帶異步相關(guān)噪聲的戰(zhàn)斗機(jī)蛇形機(jī)動(dòng)跟蹤算法

盧春光，周中良，劉宏強(qiáng)，寇添，楊遠(yuǎn)志

空軍工程大學(xué) 航空工程學(xué)院，西安 710038

蛇形機(jī)動(dòng)是戰(zhàn)斗機(jī)飛行員在隱蔽接敵、機(jī)動(dòng)規(guī)避及協(xié)同探測(cè)等戰(zhàn)術(shù)動(dòng)作中經(jīng)常采用的戰(zhàn)術(shù)機(jī)動(dòng)類型。在一定的空戰(zhàn)態(tài)勢(shì)下，飛行員進(jìn)行蛇形機(jī)動(dòng)反映了其戰(zhàn)術(shù)意圖，因此在空戰(zhàn)中如何快速而又準(zhǔn)確地識(shí)別出目標(biāo)的蛇形機(jī)動(dòng)模式對(duì)于明確其戰(zhàn)術(shù)意圖以及評(píng)估當(dāng)前的戰(zhàn)場(chǎng)態(tài)勢(shì)具有十分重要的意義。目前，機(jī)動(dòng)模式的識(shí)別主要從機(jī)動(dòng)模式的幾何特征(宏觀)以及運(yùn)動(dòng)參量特征(微觀)兩個(gè)方面著手：文獻(xiàn)[1]通過將戰(zhàn)術(shù)機(jī)動(dòng)進(jìn)行分割并按時(shí)間序列進(jìn)行編碼，采用隱馬爾科夫模型進(jìn)行訓(xùn)練、識(shí)別；文獻(xiàn)[2]通過提取目標(biāo)航向角變化率、高度變化率等運(yùn)動(dòng)參量特征，設(shè)計(jì)了一個(gè)兩級(jí)識(shí)別方案，采用模糊推理和時(shí)間自動(dòng)機(jī)方法實(shí)現(xiàn)了機(jī)動(dòng)模式的識(shí)別。對(duì)于蛇形機(jī)動(dòng)目標(biāo)而言，轉(zhuǎn)彎角速度是其最為關(guān)鍵的運(yùn)動(dòng)參量，因此通過載機(jī)雷達(dá)的量測(cè)數(shù)據(jù)精確辨識(shí)出轉(zhuǎn)彎角速度對(duì)于提高戰(zhàn)斗機(jī)蛇形機(jī)動(dòng)模式的識(shí)別率具有重要意義。

目前，機(jī)動(dòng)目標(biāo)轉(zhuǎn)彎角速度辨識(shí)的問題已經(jīng)得到了許多學(xué)者的關(guān)注。Roth[3]、Arasaratnam[4]、Jia等[5]、Huang[6]等將轉(zhuǎn)彎角速度作為額外的狀態(tài)變量，從而對(duì)狀態(tài)進(jìn)行擴(kuò)維處理，分別使用擴(kuò)展卡爾曼濾波/無跡卡爾曼濾波、容積卡爾曼濾波、基于5階球徑容積規(guī)則的高階容積卡爾曼濾波、高階魯棒容積卡爾曼濾波等非線性濾波方法辨識(shí)角速度和估計(jì)狀態(tài)變量。缺點(diǎn)是角速度估計(jì)的精度依賴于目標(biāo)初始狀態(tài)及協(xié)方差、過程噪聲和量測(cè)噪聲等因素；黃偉平等[7]、Zhu和Cheng[8]、Efe和Atherton[9]分別根據(jù)轉(zhuǎn)彎角速度與目標(biāo)速度方向角、速度、加速度之間的物理關(guān)系，首先采用非線性濾波算法估計(jì)出目標(biāo)速度方向角、速度、加速度等狀態(tài)量，而后間接辨識(shí)出轉(zhuǎn)彎角速度。Yuan等[10]通過引入距離變化率量測(cè)，采用Cmin方法[11]并結(jié)合航向角的變化估計(jì)出兩個(gè)可能的轉(zhuǎn)彎角速度的值，基于此設(shè)計(jì)包含勻速模型和兩個(gè)勻速轉(zhuǎn)彎模型的交互多模型算法進(jìn)行狀態(tài)估計(jì)。Frencl等[12]通過引入距離變化率量測(cè)估計(jì)轉(zhuǎn)彎角速度，并根據(jù)速度矢量在距離方向上的投影與距離變化率估計(jì)量之間的差異，對(duì)轉(zhuǎn)彎角速度進(jìn)行進(jìn)一步修正以提高估計(jì)精度，然后依據(jù)最優(yōu)線性無偏估計(jì)準(zhǔn)則，利用粒子濾波進(jìn)行狀態(tài)估計(jì)。缺點(diǎn)是沒有考慮角速度與目標(biāo)狀態(tài)之間相互耦合關(guān)系，增大了辨識(shí)風(fēng)險(xiǎn)；Bar等[13]通過采用勻速模型和擴(kuò)維的勻速轉(zhuǎn)彎模型設(shè)計(jì)了交互多模型算法，用于估計(jì)轉(zhuǎn)彎角速度。缺點(diǎn)是辨識(shí)與估計(jì)精度依賴于模型切換概率與轉(zhuǎn)移概率的設(shè)置；Yuan等[14]提出了基于期望最大化(EM)算法的轉(zhuǎn)彎角速度辨識(shí)算法，該算法采取的是“先辨識(shí)-后估計(jì)”的策略，通過辨識(shí)出的轉(zhuǎn)彎角速度進(jìn)而估計(jì)目標(biāo)的運(yùn)動(dòng)狀態(tài)，缺點(diǎn)是依賴于初始值以及機(jī)動(dòng)初始概率的設(shè)置。

在實(shí)際空戰(zhàn)中，非合作目標(biāo)的轉(zhuǎn)彎角速度是未知且時(shí)變的，并且與目標(biāo)狀態(tài)相互耦合、彼此影響，一方面目標(biāo)狀態(tài)估計(jì)性能的提升有利于轉(zhuǎn)彎角速度的辨識(shí)，另一方面轉(zhuǎn)彎角速度辨識(shí)性能的提升有利于目標(biāo)狀態(tài)估計(jì)精度的提高。上述文獻(xiàn)中所提到的轉(zhuǎn)彎角速度辨識(shí)方法均沒有考慮轉(zhuǎn)彎角速度與目標(biāo)狀態(tài)之間的耦合關(guān)系以及這種耦合關(guān)系所帶來的影響，并且均采取的是開環(huán)序貫處理方式，沒有形成閉環(huán)反饋。而且上述文獻(xiàn)所提到的狀態(tài)估計(jì)與轉(zhuǎn)彎角速度辨識(shí)方法中，都需要假設(shè)量測(cè)噪聲與過程噪聲是相互獨(dú)立的，但是在實(shí)際跟蹤過程中，這種假設(shè)條件往往難以滿足，比如雷達(dá)在跟蹤目標(biāo)時(shí)，受目標(biāo)運(yùn)動(dòng)速度以及觀測(cè)角度的影響，導(dǎo)致過程噪聲與量測(cè)噪聲之間的相關(guān)性不能被忽略[15-16]；在異步多傳感器信息融合過程中，由于量測(cè)值作為輸出反饋傳遞給網(wǎng)絡(luò)系統(tǒng)，同樣有可能使得量測(cè)噪聲與過程噪聲具有異步相關(guān)性[16-17]；在自動(dòng)目標(biāo)識(shí)別系統(tǒng)和自動(dòng)威脅識(shí)別系統(tǒng)中，當(dāng)量測(cè)噪聲和過程噪聲同時(shí)依賴于系統(tǒng)狀態(tài)時(shí)，傳感器噪聲與過程噪聲之間也可能會(huì)存在相關(guān)性[18]；同時(shí)由于連續(xù)系統(tǒng)離散化過程中，系統(tǒng)狀態(tài)離散化采樣時(shí)將引入額外的噪聲到量測(cè)模型中，導(dǎo)致量測(cè)噪聲與過程噪聲之間存在一定的時(shí)空相關(guān)性[19]。因此，在聯(lián)合狀態(tài)估計(jì)和角速度辨識(shí)的過程中必須考慮這種相關(guān)性。噪聲相關(guān)條件下的估計(jì)問題可以分為兩大類：同步相關(guān)和異步相關(guān)。針對(duì)噪聲同步相關(guān)條件下的狀態(tài)估計(jì)主要有以下3種框架：噪聲解耦框架、高斯近似遞歸濾波框架和相關(guān)高斯近似濾波框架。這3種框架對(duì)線性系統(tǒng)或者在最小均方誤差意義下的非線性系統(tǒng)而言是等價(jià)的[20]。Bar[13]、Chang[21]、Hu[22]等采用重構(gòu)偽狀態(tài)方程的方式，來達(dá)到相關(guān)噪聲解耦的目的，并分別設(shè)計(jì)了擴(kuò)展卡爾曼濾波、邊緣化的無跡卡爾曼濾波和無跡卡爾曼濾波進(jìn)行狀態(tài)估計(jì)；Wang等[23]通過計(jì)算兩步狀態(tài)后驗(yàn)預(yù)測(cè)概率密度函數(shù)和一步量測(cè)后驗(yàn)預(yù)測(cè)概率密度函數(shù)，基于二階Stirling插值，設(shè)計(jì)了新的中心差分濾波器；Huang等[24]通過分析上述兩種框架的缺點(diǎn)，設(shè)計(jì)了一種新的相關(guān)高斯近似濾波框架。針對(duì)噪聲異步相關(guān)條件下的狀態(tài)估計(jì)的研究主要有：于浛等[17]針對(duì)隨機(jī)時(shí)滯和異步相關(guān)噪聲情況，提出了一種改進(jìn)的高斯近似濾波算法；Souto和Ishihara[15]針對(duì)異步相關(guān)噪聲條件下的估計(jì)問題設(shè)計(jì)了一種魯棒擴(kuò)展卡爾曼濾波算法。

針對(duì)上述問題，本文通過將狀態(tài)估計(jì)與轉(zhuǎn)彎角速度辨識(shí)聯(lián)合考慮，基于EM算法框架提出一種帶異步相關(guān)噪聲的聯(lián)合估計(jì)與辨識(shí)算法,該算法通過解除目標(biāo)狀態(tài)與轉(zhuǎn)彎角速度之間的耦合關(guān)系，進(jìn)而獲得轉(zhuǎn)彎角速度閉環(huán)形式的解析解，并引入滑窗思想，以提高辨識(shí)的實(shí)時(shí)性，主要包括E-step和M-step 2個(gè)部分：E-step基于當(dāng)前估計(jì)的角速度并利用帶異步相關(guān)噪聲的高斯近似濾波器與平滑器獲得的后驗(yàn)平滑概率密度和聯(lián)合分布密度，近似計(jì)算完整的對(duì)數(shù)函數(shù)似然函數(shù)的期望；M-step通過使期望最大化更新獲得下一次迭代的角速度估計(jì)量。

1 問題描述

蛇形機(jī)動(dòng)可以分解成若干個(gè)具有不同轉(zhuǎn)彎角速度的圓弧轉(zhuǎn)彎?rùn)C(jī)動(dòng)，因此蛇形機(jī)動(dòng)目標(biāo)轉(zhuǎn)彎角速度的辨識(shí)問題相應(yīng)地轉(zhuǎn)化為圓弧轉(zhuǎn)彎?rùn)C(jī)動(dòng)的角速度辨識(shí)問題。在帶異步相關(guān)噪聲背景下，假設(shè)目標(biāo)在二維平面中運(yùn)動(dòng)，轉(zhuǎn)彎?rùn)C(jī)動(dòng)模型狀態(tài)方程及量測(cè)方程為

xk+1=fk(xk,Ωk)+wk

(1)

zk+1=hk+1(xk+1)+vk+1

(2)

式中：

fk(xk,Ωk)=

1.1 基于系統(tǒng)重構(gòu)的角速度解耦策略

由于蛇形機(jī)動(dòng)目標(biāo)的轉(zhuǎn)彎角速度Ωk這一未知參數(shù)非線性耦合在狀態(tài)轉(zhuǎn)移矩陣之中，傳統(tǒng)的辨識(shí)算法僅能獲得轉(zhuǎn)彎角速度的近似解，辨識(shí)效果不佳，因此為了獲取轉(zhuǎn)彎角速度的解析解，就需要將轉(zhuǎn)彎角速度與狀態(tài)轉(zhuǎn)移矩陣之間的非線性耦合關(guān)系解除，即在原有系統(tǒng)模型的基礎(chǔ)之上，通過模型的轉(zhuǎn)換，重構(gòu)一個(gè)新的狀態(tài)方程，將轉(zhuǎn)彎角速度從狀態(tài)轉(zhuǎn)移矩陣中提取出來，從而解除這種耦合性，如式(3)所示：

xk+ 1=F(xk)θ+xk+wk

(3)

式中：

θ3=1-cos(ΩkT)

θ4=sin(ΩkT)

1.2 基于量測(cè)重構(gòu)的異步相關(guān)噪聲解耦策略

由于在上述異步相關(guān)噪聲條件下，傳統(tǒng)的高斯近似濾波器性能不佳，甚至發(fā)散，進(jìn)而影響EM算法的辨識(shí)效果，所以為了實(shí)現(xiàn)狀態(tài)的精確估計(jì)和轉(zhuǎn)彎角速度的準(zhǔn)確辨識(shí)，本節(jié)通過采用“去相關(guān)框架”[25]，基于原有的量測(cè)模型，對(duì)量測(cè)模型進(jìn)行轉(zhuǎn)換，從而重構(gòu)一個(gè)偽量測(cè)方程，使得重構(gòu)的偽量測(cè)方程滿足過程噪聲與量測(cè)噪聲之間的相關(guān)性解除。在重構(gòu)的偽量測(cè)基礎(chǔ)之上實(shí)現(xiàn)濾波和平滑，獲得后驗(yàn)平滑概率密度和聯(lián)合分布密度，進(jìn)而實(shí)現(xiàn)轉(zhuǎn)彎角速度辨識(shí)與狀態(tài)估計(jì)。

在量測(cè)方程式(2)右邊形式地加上一等于零的項(xiàng)：

fk(xk,Ωk)-wk)

(4)

式中：Gk+1為待定矩陣。

記

fk(xk,Ωk))

(5)

(6)

則量測(cè)方程轉(zhuǎn)換成為

(7)

(8)

E[(vk+1-Gk+1wk)(vk+1-Gk+1wk)Τ]=

(9)

(10)

可得

(11)

(12)

2 基于HCKS-EM的聯(lián)合估計(jì)與辨識(shí)算法

基于極大似然估計(jì)準(zhǔn)則[26-27]，本文提出了一種帶異步相關(guān)噪聲的聯(lián)合狀態(tài)估計(jì)與角速度辨識(shí)算法，具體的算法框架如圖1所示。在第t次迭代時(shí)，利用帶異步相關(guān)噪聲的高斯近似濾波器與平滑器，獲得目標(biāo)狀態(tài)的平滑估計(jì)量，并采用EM算法進(jìn)行參數(shù)θ的辨識(shí)，辨識(shí)后的參數(shù)θ用于第t+1次的狀態(tài)估計(jì)，不斷迭代直到滿足設(shè)定的要求為止。

圖1 聯(lián)合估計(jì)與辨識(shí)Fig.1 Joint estimation and identification

(13)

(14)

式中：

(15)

(16)

(17)

當(dāng)l=k-1時(shí)，I1是一個(gè)常數(shù)，與待辨識(shí)量θ無關(guān)；當(dāng)0≤l

pθ(xk-i|xk-i-1)～N(xk-i;F(xk-i-1)θ+xk-i-1,Q)

(18)

(19)

2.1 E-step

跟據(jù)貝葉斯準(zhǔn)則得到xk-i和xk-i-1的聯(lián)合概率密度分布：

(20)

采用帶異步相關(guān)噪聲的HCKS平滑器得到平滑概率密度時(shí)，將基于以下假設(shè)：

服從高斯分布[29]。

(21)

式中：

則

(22)

(23)

(24)

(25)

將式(21)和式(23)代入(20)的聯(lián)合分布，有

(26)

到k-i-1時(shí)刻的平滑分布為

(27)

(28)

(29)

為了求解I2，首先定義如下兩種積分函數(shù)：

(30)

Δ(xk-i,xk-i-1)=?F(xk-i-1)TQ-1(xk-i-xk-i-1)·

(31)

然后分別將F(xi)、xi按xi的分量進(jìn)行分解：

(32)

(33)

(34)

(35)

則

(36)

(37)

2.2 五階球面-徑向容積規(guī)則

本文采用Jia等提出的五階球面徑向容積規(guī)則[5]解決帶異步相關(guān)噪聲的高斯近似濾波器和平滑器中的非線性高斯積分的計(jì)算問題。基于容積規(guī)則的高斯積分求解可抽象成為

(38)

(39)

式中:ej為空間Rn中的單位向量，其第j個(gè)元素為1；權(quán)值ws1和ws2分別為

(40)

(41)

由矩匹配法可知，五階徑向規(guī)則當(dāng)中，容積點(diǎn)與權(quán)重需要滿足：

(42)

(43)

(44)

將式(39)、式(43)和式(44)代入式(38)可得

(45)

由式(45)可知五階容積規(guī)則的采樣點(diǎn)數(shù)量為

2n2+1。

2.3 M-step

M-step的主要任務(wù)是解決I2的極大化問題，即求解使I2滿足極大值時(shí)所對(duì)應(yīng)的θt+1值，用于EM算法的下一次迭代更新：

(46)

當(dāng)I2取得極大值時(shí)滿足：

(47)

則可求得參數(shù)θ的迭代表達(dá)式為

(48)

本文采用的是滯后窗口，即辨識(shí)k-l時(shí)刻的參數(shù)θ的值需要使用[k-l,k]時(shí)刻的量測(cè)，然后通過反解參數(shù)θ與參數(shù)Ω之間的函數(shù)關(guān)系，求得k-l時(shí)刻目標(biāo)的轉(zhuǎn)彎角速度的辨識(shí)結(jié)果。

本文算法總結(jié)如表1所示。

表1 基于HCKS-EM的聯(lián)合估計(jì)與辨識(shí)算法

2.4 收斂性分析

基于HCKS-EM的聯(lián)合估計(jì)與辨識(shí)算法的收斂性受帶異步相關(guān)噪聲濾波算法的收斂性以及EM算法的收斂性兩個(gè)方面的影響。

對(duì)于EM算法的收斂性而言，為了更好地說明該算法的收斂性，需要給出以下定理：

?t=1,2,…

(49)

(50)

令

(51)

(52)

(53)

(54)

(55)

對(duì)于式(54)右邊第二項(xiàng)而言，已知當(dāng)x≥0時(shí),滿足:lnx≤x-1，則

(56)

同樣從式(54)～式(56)可以看出

(57)

3 仿真分析

本文利用水平方向上的轉(zhuǎn)彎?rùn)C(jī)動(dòng)非線性動(dòng)態(tài)模型，仿真出一條蛇形機(jī)動(dòng)軌跡。假設(shè)機(jī)動(dòng)目標(biāo)在1～120 s以Ω1=-2 (°)·s-1作轉(zhuǎn)彎運(yùn)動(dòng)，在k=121 s時(shí)轉(zhuǎn)彎角速度突變?yōu)棣?=3 (°)·s-1作轉(zhuǎn)彎運(yùn)動(dòng)，在k=241 s時(shí)轉(zhuǎn)彎角速度突變?yōu)棣?=-2 (°)·s-1作轉(zhuǎn)彎運(yùn)動(dòng)，并持續(xù)到300 s。設(shè)置轉(zhuǎn)彎角速度初始值為Ω0=-1 (°)·s-1，采樣周期T=1，q1=0.1 m2/s3，wk為零均值高斯白噪聲，其協(xié)方差為Q。

通過載機(jī)雷達(dá)可以獲得目標(biāo)與載機(jī)之間的相對(duì)距離r、方向角φ的信息，則可獲得系統(tǒng)的非線性量測(cè)方程為

本文提出的聯(lián)合估計(jì)與辨識(shí)算法和擴(kuò)維法的初始狀態(tài)以及協(xié)方差分別設(shè)置為

初始狀態(tài)：

初始協(xié)方差：

P0=

diag(100 m2，10 m2·s-2，100 m2，10 m2·s-2)

擴(kuò)維法初始狀態(tài)：

擴(kuò)維法協(xié)方差：

100 mrad/s)

為了評(píng)估分析本文提出算法的性能，首先將本文算法與傳統(tǒng)的擴(kuò)維法以及基于UKF的交多模型算法(IMM-UKF)[32]進(jìn)行對(duì)比分析。IMM-UKF算法采用了標(biāo)準(zhǔn)維納過程速度模型和擴(kuò)維的勻速轉(zhuǎn)彎模型兩種機(jī)動(dòng)模型，并且兩種機(jī)動(dòng)模型的狀態(tài)和協(xié)方差的初始值以及量測(cè)噪聲和過程噪聲設(shè)置與上文保持一致。維納過程速度模型的狀態(tài)轉(zhuǎn)移矩陣為F，量測(cè)矩陣為H，表達(dá)式分別為

IMM-UKF算法中模型之間的轉(zhuǎn)移概率矩陣為Π，初始模型概率矩陣為μ，表達(dá)式分別為

本文提出的聯(lián)合估計(jì)與辨識(shí)算法采用滑窗機(jī)制，窗口設(shè)置為5，最大迭代次數(shù)為5次，各執(zhí)行100次蒙特卡羅仿真。圖2和圖3分別為上述3種 算法對(duì)角速度辨識(shí)和目標(biāo)狀態(tài)估計(jì)的效果，從圖2和圖3中可以看出，本文提出的聯(lián)合估計(jì)與辨識(shí)算法在角速度辨識(shí)和目標(biāo)狀態(tài)估計(jì)上比傳統(tǒng)的擴(kuò)維法以及IMM-UKF算法誤差小，精度高，這主要是因?yàn)楸疚奶岢龅乃惴ń獬私撬俣扰c狀態(tài)方程之間的耦合關(guān)系，從而便于獲得角速度辨識(shí)的解析解，并且，采用了閉環(huán)反饋的處理方式，通過反復(fù)的迭代不斷修正角速度辨識(shí)與目標(biāo)狀態(tài)估計(jì)的誤差，從而提高了辨識(shí)與估計(jì)的精度。而且從圖2和圖3中發(fā)現(xiàn)，基于IMM-UKF算法的角速度辨識(shí)和目標(biāo)狀態(tài)估計(jì)的均方根誤差要比傳統(tǒng)的擴(kuò)維法大，這主要是因?yàn)榻换ザ嗄Ｐ退惴ㄌ幚淼氖悄繕?biāo)跟蹤時(shí)發(fā)生的模型不匹配問題，但是本文設(shè)計(jì)的蛇形機(jī)動(dòng)仿真采用的是單一的勻速轉(zhuǎn)彎模型，所以交互多模型算法的優(yōu)勢(shì)并未體現(xiàn)出來，產(chǎn)生的辨識(shí)與估計(jì)效果不佳。從圖中可進(jìn)一步看出，在異步相關(guān)噪聲背景下，帶異步相關(guān)噪聲的高斯近似濾波器的估計(jì)與辨識(shí)效果優(yōu)于傳統(tǒng)的標(biāo)準(zhǔn)的高斯近似濾波器，這主要是因?yàn)椋疚乃岬降膸М惒较嚓P(guān)噪聲的高斯近似濾波與平滑算法采用了“去相關(guān)”框架，通過重構(gòu)偽量測(cè)方程，解除了量測(cè)噪聲與過程噪聲之間的相關(guān)性，在目標(biāo)狀態(tài)與量測(cè)相互獨(dú)立的基礎(chǔ)上設(shè)計(jì)的濾波算法顯然性能優(yōu)于傳統(tǒng)的濾波算法，并從圖中可看出高階容積卡爾曼算法估計(jì)與辨識(shí)效果優(yōu)于容積卡爾曼算法和無跡卡爾曼算法，尤其是在量測(cè)噪聲和過程噪聲增大時(shí)，這種優(yōu)越性就越明顯，但是相應(yīng)的計(jì)算量就會(huì)增加，計(jì)算時(shí)間增大，主要是因?yàn)楦唠A容積卡爾曼算法的采樣點(diǎn)數(shù)量高于容積卡爾曼算法和無跡卡爾曼算法。

圖2 角速度辨識(shí)均方根誤差Fig.2 RMSE of turn rate identification

圖3 位置和速度估計(jì)均方根誤差Fig.3 RMSE of position and velocity estimation

其次，從該算法的本身結(jié)構(gòu)著手，對(duì)該算法進(jìn)行評(píng)估分析。主要從窗長(zhǎng)和迭代次數(shù)兩個(gè)方面分析。采取滑動(dòng)滯后窗口策略，窗口長(zhǎng)度l分別設(shè)置為2、3、5、10，最大迭代次數(shù)均為5次，各執(zhí)行100次蒙特卡羅仿真。從圖4和圖5中可以看出，隨著窗口長(zhǎng)度的增大，該算法收斂于真實(shí)值的時(shí)刻就越早，精度越高，當(dāng)角速度發(fā)生突變時(shí)，對(duì)于突變的角速度反應(yīng)也越快。并且從圖6和表2可以看出，窗口長(zhǎng)度越大，該算法估計(jì)的目標(biāo)狀態(tài)整體精度就越高，但是消耗的時(shí)間越長(zhǎng)，這顯然是時(shí)間與精度之間的“博弈”問題，從圖中還可以進(jìn)一步看出，當(dāng)窗長(zhǎng)大于5時(shí)，由窗長(zhǎng)帶來的精度效益不太明顯，相反時(shí)間消耗問題更加突出。

圖4 不同窗口長(zhǎng)度下角速度辨識(shí)結(jié)果Fig.4 Identification result of turn rate with different window length

對(duì)于迭代次數(shù)而言，將窗口長(zhǎng)度l設(shè)置為5，最大迭代次數(shù)titer分別設(shè)置為2、3、5、10，各執(zhí)行100次蒙特卡羅仿真。從圖7和圖8中可以看出，隨著迭代次數(shù)的增加，該算法在收斂于真實(shí)值的時(shí)刻就越早，并且對(duì)于角速度突變反應(yīng)的也比較靈敏，角速度辨識(shí)的精度也越高。

圖5 不同窗口長(zhǎng)度下角速度辨識(shí)均方根誤差Fig.5 RMSE of turn rate identification with different window length

圖6 不同窗口下位置和速度估計(jì)均方根誤差Fig.6 RMSE of position and velocity estimation with different window length

表2 計(jì)算k=37 s時(shí)的角速度和狀態(tài)所耗費(fèi)的時(shí)間

Table 2 Time cost for calculating turnrate and states at k=37 s

滑動(dòng)窗口長(zhǎng)度l=2l=3l=5l=10時(shí)間/s0.013 60.019 30.032 40.056 2

圖7 不同迭代次數(shù)下角速度辨識(shí)結(jié)果Fig.7 Identification result of turn rate with different iterations

圖8 不同迭代次數(shù)下角速度辨識(shí)的均方根誤差Fig.8 RMSE of turn rate identification with different iterations

表3 計(jì)算k=47 s時(shí)的角速度和狀態(tài)所耗費(fèi)的時(shí)間

Table 3 Time cost for calculating turnrate and states at k=47 s

最大迭代次數(shù)titer=2titer=3titer=5titer=10時(shí)間/s0.010 30.016 30.026 40.054 2

圖9 不同迭代次數(shù)下位置和速度估計(jì)均方根誤差Fig.9 RMSE of position and velocity estimation with different iterations

從圖9目標(biāo)狀態(tài)4個(gè)分量的RMSE可以看出，迭代次數(shù)越大，該算法估計(jì)的目標(biāo)狀態(tài)整體精度就越高，但是從表3顯示的不同迭代次數(shù)下計(jì)算k=47 s時(shí)的角速度和狀態(tài)所耗費(fèi)的時(shí)間越大，性價(jià)比不高。尤其是當(dāng)?shù)螖?shù)大于5時(shí)，由迭代次數(shù)帶來的精度效益遠(yuǎn)遠(yuǎn)比不上時(shí)間帶來的損耗，降低該算法的實(shí)時(shí)性。

4 結(jié) 論

1) 在E-Step，采用系統(tǒng)重構(gòu)角速度解耦策略解除了轉(zhuǎn)彎角速度與狀態(tài)轉(zhuǎn)移矩陣之間的耦合特性，在此基礎(chǔ)上將完備似然函數(shù)進(jìn)行分解，從而將其轉(zhuǎn)換成帶參數(shù)θ的解析表達(dá)式，降低了由于狀態(tài)擴(kuò)維法擴(kuò)維所帶來的復(fù)雜性；通過采用量測(cè)重構(gòu)噪聲相關(guān)性解耦策略，解除過程噪聲與量測(cè)噪聲之間的異步相關(guān)耦合特性，基于貝葉斯框架，設(shè)計(jì)了帶異步相關(guān)噪聲的高斯近似濾波和平滑算法，并基于該算法獲得了目標(biāo)狀態(tài)的平滑估計(jì)。

2) 在M-Step中，通過極大化完備似然函數(shù)求得轉(zhuǎn)彎角速度閉環(huán)形式的解析解，降低了辨識(shí)風(fēng)險(xiǎn)，提高了狀態(tài)估計(jì)精度。

3) 仿真實(shí)驗(yàn)表明，本文提出的聯(lián)合估計(jì)與辨識(shí)算法性能優(yōu)于傳統(tǒng)的擴(kuò)維法以及交互多模型算法。