基于時(shí)頻差的高階強(qiáng)跟蹤UKF算法

周恭謙, 楊露菁, 劉 忠

(海軍工程大學(xué)電子工程學(xué)院, 湖北 武漢 430033)

0 引 言

目標(biāo)跟蹤是對(duì)目標(biāo)位置和速度的實(shí)時(shí)估計(jì)與預(yù)測(cè),其被廣泛的應(yīng)用于軍事領(lǐng)域之中?；谟性聪到y(tǒng)的目標(biāo)跟蹤技術(shù)發(fā)展較為成熟,但在軍事對(duì)抗中很容易被干擾從而導(dǎo)致跟蹤失敗,因此基于無源系統(tǒng)的目標(biāo)跟蹤技術(shù)受到了廣泛的關(guān)注。在無源跟蹤系統(tǒng)中,多觀測(cè)站可以通過接收目標(biāo)信號(hào)的到達(dá)時(shí)間差(time difference of arrival, TDOA)和到達(dá)頻率差(frequency difference of arrival, FDOA)來完成對(duì)目標(biāo)位置和速度的估計(jì),由于TDOA和FDOA 構(gòu)建的模型為非線性方程組,所以基于TDOA/FDOA的目標(biāo)跟蹤屬于非線性濾波問題。

針對(duì)非線性濾波問題粒子濾波(particle filter, PF)[1-5]隨機(jī)變量不要求高斯分布,估計(jì)精度較高受到廣泛的關(guān)注,然而其計(jì)算復(fù)雜度較高,消耗時(shí)間過長(zhǎng)。針對(duì)此文獻(xiàn)[6]提出一種基于蝙蝠算法的粒子濾波(bat algorithm optimized particle filter, BA-PF),在原有算法引入飛行策略,降低了狀態(tài)預(yù)測(cè)所需的粒子數(shù)量和提高了濾波的定位精度,但算法的復(fù)雜度仍然較高無法滿足跟蹤的實(shí)時(shí)性要求,而文獻(xiàn)[7]提出一種交互式多模型(interactive multiple model, IMM)[8-10]與秩濾波(rank Kalman filter,RKF)[11]結(jié)合的算法在一定程度上提高了定位精度,但由于采用多個(gè)模型進(jìn)行濾波并行和交互提好了計(jì)算復(fù)雜度,需要運(yùn)行較長(zhǎng)的時(shí)間。

無跡卡爾曼濾波(unscented Kalman filter, UKF)和容積卡爾曼濾波(cubature Kalman filter,CKF)作為新興非線性濾波算法[12-16]被研究應(yīng)用到多個(gè)領(lǐng)域,UKF算法的核心思想是通過無跡變換(unscented Transform, UT)去近似非線性系統(tǒng)狀態(tài)的后驗(yàn)概率密度函數(shù)。而CKF的采用三階球面-相徑容積規(guī)則來近似非線性函數(shù)傳遞的后驗(yàn)均值和方差,歸根到底其是UKF算法中UT變換的特殊形式[17]。為了提高CKF算法和UKF算法的估計(jì)精度:文獻(xiàn)[18-19]提出了高階球面-相徑容積的采樣方法來獲取高于CKF階數(shù)的估計(jì)精度。文獻(xiàn)[20]提出一種正交容積卡爾曼濾波跟蹤算法(orthogonal cubature Kalman filter,OCKF),通過引入一個(gè)特定的正交矩陣改進(jìn)容積采樣方法來減小標(biāo)準(zhǔn)容積采樣方法的舍棄誤差。文獻(xiàn)[21]提出一種高斯和高階無跡卡爾曼濾波算法,針對(duì)UKF提出了一種高階無跡變換來提高非線性變換的近似精度。盡管這些算法一定程度上提高了UKF和CKF算法的估計(jì)精度,但當(dāng)目標(biāo)運(yùn)動(dòng)狀態(tài)發(fā)生突變時(shí) UKF和CKF算法出現(xiàn)的狀態(tài)估計(jì)精度急劇下降的問題卻無法解決。而強(qiáng)跟蹤濾波(strong tracking filter, STF)[22-23]的引入能夠解決UKF和CKF算法的魯棒性問題,強(qiáng)跟蹤濾波通過將次優(yōu)漸消因子引入預(yù)測(cè)協(xié)方差陣,實(shí)時(shí)調(diào)整卡爾曼增益矩陣來保持對(duì)目標(biāo)突變時(shí)良好的跟蹤能力。近年來出現(xiàn)了很多STF結(jié)合UKF和CKF形成的新型的非線性濾波方法:基于新息正交原理的抗差UKF[24],基于多漸消因子強(qiáng)跟蹤UKF[25-26],自適應(yīng)高階容積卡爾曼濾波(adaptive high-degree cubature Kalman filter, AHCKF)[27]。但這些算法需要進(jìn)行在原算法的基礎(chǔ)上增加了一次UT變換,提高了計(jì)算的復(fù)雜度,且對(duì)跟蹤的估計(jì)精度提高有限。為了在不增加算法復(fù)雜度的情況下,提高算法對(duì)目標(biāo)跟蹤的精度和對(duì)突變的適應(yīng)能力,本文提出一種高階強(qiáng)跟蹤卡爾曼濾波方法(high order strong tracking UKF, HSUKF):首先利用高斯概率密度函數(shù)高階導(dǎo)數(shù)的極值作為Sigma樣點(diǎn)進(jìn)行UT轉(zhuǎn)換,通過樣本點(diǎn)捕捉更高階的中心矩來提高非線性變換近似精度。然后將改進(jìn)的STF算法引入到HUKF中在不增加計(jì)算復(fù)雜度的情況下進(jìn)一步提高了算法的魯棒性,通過仿真與UKF、OCKF、AHCKF、BA-PF以及IMM-RKF對(duì)比表明了該算法在狀態(tài)突變時(shí)有更高的估計(jì)性能,最后通過實(shí)例論證了算法的有效性。

1 基于概率密度高階無跡卡爾曼濾波

1.1 基于概率密度高階導(dǎo)的UT轉(zhuǎn)換

UT 變換作為一種計(jì)算隨機(jī)變量經(jīng)過非線性變換后統(tǒng)計(jì)特性的方法, 它通過設(shè)置Sigma 樣點(diǎn)分布和權(quán)值來逼近樣本非線性變換參量的矩[26]。事實(shí)上為了更好的近似隨機(jī)變量經(jīng)過非線性變換后的統(tǒng)計(jì)特性,Sigma點(diǎn)集應(yīng)盡可能近似更高階數(shù)的中心矩[28-29]。由于多維高斯變量的問題都可以轉(zhuǎn)化為多個(gè)獨(dú)立一維標(biāo)準(zhǔn)高斯分布的問題,下面先對(duì)服從一維標(biāo)準(zhǔn)高斯分布的隨機(jī)變量x進(jìn)行討論,h(x)表示關(guān)于x的非線性函數(shù),h(x)的期望可表示為

(1)

表1 p(x)各階導(dǎo)數(shù)對(duì)應(yīng)的極值點(diǎn)

(2)

式中,[k]x表示x的高階中心距。

定理1如果變量x服從均值為μ、方差為σ2的高斯分布,[k]x可以由μ和σ表示

(3)

則服從標(biāo)準(zhǔn)正態(tài)分布的x的高階中心距為(k-1)!=1·3·5…(k-1)

綜合式(2)、式(3)考慮sigma點(diǎn)對(duì)稱分布的特性可得到矩陣方程式(4)：

(4)

將σi代入式(4)即可求得對(duì)應(yīng)的Wi的值,則服從一維標(biāo)準(zhǔn)高斯分布的x的sigma點(diǎn)集Si為

Si={(0,W0),(±σ1,W1),…,(±σL,WL)}

(5)

將一維標(biāo)準(zhǔn)正態(tài)分布拓展到n維隨機(jī)變量x中則有

(6)

1.2 基于高斯密度高階導(dǎo)的UKF

對(duì)離散的非線性系統(tǒng)

(7)

式中,Xk,Zk表示系統(tǒng)k時(shí)刻的狀態(tài)向量和量測(cè)向量;f(·)、h(·)表示對(duì)應(yīng)的轉(zhuǎn)換函數(shù);wk、vk為互不相關(guān)的高斯白噪聲滿足:

(8)

式中,Qk為非負(fù)定矩陣;Rk為正定矩陣,δkj為Kronecker-δ函數(shù)。

初始化

(9)

Sigma點(diǎn)計(jì)算

(10)

步驟1時(shí)間更新

將Sigma點(diǎn)進(jìn)行非線性變換

εi,k|k-1=f(εi,k-1)

(11)

計(jì)算預(yù)測(cè)估計(jì)值

(12)

計(jì)算預(yù)測(cè)協(xié)方差

(13)

步驟2量測(cè)更新

(14)

非線性變換sigma點(diǎn)

(15)

計(jì)算預(yù)測(cè)測(cè)量值

(16)

預(yù)測(cè)新息協(xié)方差

(17)

預(yù)測(cè)互協(xié)方差矩陣

(18)

計(jì)算Kalman增益

(19)

更新誤差協(xié)方差

(20)

更新狀態(tài)

(21)

2 高階強(qiáng)跟蹤卡爾曼濾波

2.1 改進(jìn)的強(qiáng)跟蹤算法

(22)

強(qiáng)跟蹤UKF[22-24]通過將漸消因子λk引入預(yù)測(cè)協(xié)方差陣Pk|k-1,來對(duì)濾波增益矩陣Kk進(jìn)行實(shí)時(shí)調(diào)整以提高系統(tǒng)的魯棒性與自適應(yīng)能力,具體為

(23)

綜上所述,強(qiáng)跟蹤UKF算法在每次濾波過程中對(duì)比UKF都要多進(jìn)行一次無跡變換,而這將會(huì)大大增加算法復(fù)雜度,不利于工程實(shí)踐。當(dāng)非線性系統(tǒng)中狀態(tài)變量或者觀測(cè)變量維數(shù)較高時(shí),這個(gè)問題將會(huì)更加突出。

而根據(jù)式(19)可知調(diào)整Kalman增益還可通過調(diào)整Pxz,k|k-1,Pzz,k|k-1來實(shí)現(xiàn),可將λk分別引入Pxz,k|k-1,Pzz,k|k-1中有

(24)

(25)

而將λk引入式(24)、式(25),則無需重新求解Pxz,k|k-1,Pzz,k|k-1,只需要兩次UT變換就能實(shí)現(xiàn)濾波過程,而且新方法可以同時(shí)控制預(yù)測(cè)新息協(xié)方差和預(yù)測(cè)互協(xié)方差,比將λk引入Pk|k-1具有更強(qiáng)的自適應(yīng)性。將式(24)、式(25)對(duì)式(17)、式(18)替換即是完整的高階強(qiáng)跟蹤卡爾曼濾波過程。

2.2 漸消因子的計(jì)算

式(24)、式(25)中漸消因子的計(jì)算可由式(26)計(jì)算得到

(26)

記HUKF的預(yù)測(cè)誤差與估計(jì)誤差分別為

(27)

(28)

引入未知對(duì)角陣βk使得

(29)

結(jié)合式(1)、式(22)、式(26)、式(29)有

(30)

由式(30)可知

Pxz,k|k-1-KkVγ,k=0

(31)

結(jié)合式(19)、式(31)有

Kk(Pzz,k|k-1-Vγ,k)=0

(32)

結(jié)合式(17)、式(32)有

(33)

對(duì)(33)求解

(34)

式(34)的解λk可能小于1,為避免這種情況出現(xiàn),最終漸消因子可定義為

(35)

式(34)中Vγ,k可由式(36)計(jì)算：

(36)

式中,0<ρ≤1為遺忘因子,通常選取為0.95。

3 實(shí) 驗(yàn)

3.1 多站TDOA/FDOA跟蹤模型

第i個(gè)接收站到待測(cè)目標(biāo)的距離可表示為

(37)

式中,‖·‖為求2范數(shù)運(yùn)算。

待測(cè)目標(biāo)到接收站i與到第一個(gè)接收站的距離差觀測(cè)量可表示為

(38)

對(duì)式(37)求導(dǎo)可得距離關(guān)于時(shí)間的微分方程

(39)

對(duì)式(38)求導(dǎo)可得距離差關(guān)于時(shí)間的微分方程

(40)

頻差觀測(cè)量可表示為

(41)

式中,f0目標(biāo)信號(hào)頻率。

由上述可知基于TDOA/FDOA定位的狀態(tài)方程可表示為

(42)

式中,wk-1為高斯白噪聲,代表方差為σk-1的對(duì)角矩陣;φk為狀態(tài)轉(zhuǎn)移矩陣;T為觀測(cè)時(shí)間間隔。

TDOA/FDOA定位測(cè)量方程可表示為

(43)

3.2 仿真實(shí)驗(yàn)

4個(gè)觀測(cè)站的位置及速度如表2所示。

表2 接收站和目標(biāo)的位置和速度

令目標(biāo)做轉(zhuǎn)彎運(yùn)動(dòng),運(yùn)動(dòng)軌跡如圖1所示[19,30],目標(biāo)運(yùn)動(dòng)的起點(diǎn)為I,終點(diǎn)為F。

圖1 目標(biāo)運(yùn)動(dòng)軌跡示意圖Fig.1 Sketch map of target motion

信號(hào)頻率f0=3×108Hz,狀態(tài)轉(zhuǎn)移矩陣φk為

(44)

(45)

(46)

(47)

按照所給的參數(shù)進(jìn)行仿真實(shí)驗(yàn),圖2和圖3為本文提出的HSUKF算法和UKF、AHCKF、OCKF算法、BA-PF以及IMM-RKF位置跟蹤RMSE和速度跟蹤RMSE曲線。表3給出的是幾種算法濾波過程中計(jì)算的采樣點(diǎn)數(shù)、200次蒙特卡羅實(shí)驗(yàn)消耗的時(shí)間以及平均RMSE和速度RMSE的值。

圖2 位置跟蹤RMSE曲線Fig.2 RMSE curve of Position tracking

圖3 速度跟蹤RMSE曲線Fig.3 RMSE curve of velocity tracking

算法位置RMSE均值/m速度RMSE均值/(m/s)運(yùn)算時(shí)間/s采樣點(diǎn)個(gè)數(shù)UKF86.0632.0511.5218OCKF60.1228.7311.6416IMM-RKF48.1126.5837.2618AHCKF43.5722.8121.4199BA-PF42.1422.67180.57100HSUKF30.1916.2013.3934

從圖2、圖3可以看出在整個(gè)跟蹤階段HSUKF算法在跟蹤過程中相對(duì)其他濾波算法RMSE最小, 這是因?yàn)镠SUKF算法通過sigma點(diǎn)的選取近似非線性變換的8階中心矩比其他算法獲得了更高的精度,而BA-PF由于機(jī)動(dòng)增量的引入增加了定位誤差,仍保持著較高的定位精度,AHCKF提出了一種高階球面-相徑容積規(guī)則采樣能近似5階中心矩精度其定位精度僅次于HSUKF和BA-PF。OCKF通過引入一個(gè)特定的正交矩陣改進(jìn)容積采樣方法來使舍棄誤差更小但其歸根到底還是三階濾波算法因而跟蹤精度只比UKF算法高。當(dāng)對(duì)目標(biāo)進(jìn)行加強(qiáng)機(jī)動(dòng)后,UKF、OCKF以及IMM-RKF位置和速度RMSE迅速增大,且收斂較為緩慢,原因在于當(dāng)目標(biāo)狀態(tài)突變時(shí),系統(tǒng)模型不能正確的描述目標(biāo)的真實(shí)運(yùn)動(dòng),而依然根據(jù)模型進(jìn)行狀態(tài)更新會(huì)產(chǎn)生較大誤差。而HSUKF算法和AHCKF的RMSE增大幅度遠(yuǎn)遠(yuǎn)小于上述3種方法,且濾波收斂速度均比上述2種方法快,說明HSUKF算法和AHCKF對(duì)于狀態(tài)突變具有較強(qiáng)的跟蹤能力。這是因?yàn)楫?dāng)目標(biāo)狀態(tài)突變時(shí),強(qiáng)跟蹤算法的引入能夠充分提取殘差序列中的有效信息,使輸出的殘差序列正交,通過漸消因子λk的引入實(shí)時(shí)調(diào)整增益矩陣Kk,從而明顯的提高了跟蹤精度。

從表3可以看出,HSUKF算法的位置RMSE均值和速度RMSE均值最小,進(jìn)一步證明了算法的最優(yōu)性。BA-PF算法雖然有著較高的定位精度,且BA-PF較傳統(tǒng)PF算法降低了狀態(tài)預(yù)測(cè)所需的粒子數(shù)目,但計(jì)算復(fù)雜度仍較高消耗時(shí)間最長(zhǎng)。IMM-RKF消耗時(shí)間僅次于BA-PF算法這是由于采用多個(gè)模型進(jìn)行濾波并行和交互從而增加了計(jì)算量。而其他4種算法的運(yùn)算時(shí)間的長(zhǎng)短與4種算法濾波過程中計(jì)算的采樣點(diǎn)個(gè)數(shù)相關(guān),由于UKF進(jìn)行兩次無跡變換,而一次無跡變換sigma采樣點(diǎn)的數(shù)目為2n+1(n為狀態(tài)向量的維數(shù)本文取4),由此計(jì)算濾波過程中UKF采樣點(diǎn)總數(shù)為18,相對(duì)其他算法復(fù)雜度最低,因此運(yùn)算時(shí)間最短,而OCKF算法只在CKF采樣過程中引入了正交矩陣,并沒有增加額外的計(jì)算量,,由于OCKF算法一次無跡變換sigma采樣點(diǎn)的數(shù)目為2n,計(jì)算出OCKF算法的采樣點(diǎn)總數(shù)為16,因此消耗的時(shí)間和UKF幾乎相同。而AHCKF算法進(jìn)行一次無跡變換采樣點(diǎn)個(gè)數(shù)為2n2+1,而強(qiáng)跟蹤算法的引入需經(jīng)歷3次無跡變換,所以AHCKF算法總的采樣點(diǎn)個(gè)數(shù)為99,盡管AHCKF定位精度和適應(yīng)狀態(tài)突變能力有所增強(qiáng),但算法復(fù)雜度明顯提高,運(yùn)算的時(shí)間也大大增加。而HSUKF算法一次無跡變換sigma采樣點(diǎn)數(shù)為4n+1,而改進(jìn)強(qiáng)跟蹤算法引入只需兩次無跡變換,因此HSUKF算法總的采樣點(diǎn)個(gè)數(shù)為34,算法復(fù)雜度相對(duì)AHCKF大大的降低。而通過運(yùn)行時(shí)間的對(duì)比HSUKF算法平均運(yùn)行一次的時(shí)間相對(duì)于UKF來說只增加了0.01 s,證明了HSUKF算法能夠兼顧計(jì)算復(fù)雜度和跟蹤精度。

3.3 實(shí)例論證

為對(duì)運(yùn)動(dòng)目標(biāo)的跟蹤效果進(jìn)行驗(yàn)證,在太平寺機(jī)場(chǎng)開展了低速動(dòng)態(tài)試驗(yàn),在一個(gè)小型航模飛機(jī)掛載UWB標(biāo)簽,如圖4和圖5所示。以觀測(cè)區(qū)圓心為原點(diǎn),3個(gè)定位基站按照(-50 m,0 m),(50 m,0 m),(0 m,-50 m)的正三角結(jié)構(gòu)進(jìn)行布設(shè)如圖6所示。此外布設(shè)一套GPS-RTK高精度定位系統(tǒng),作為系統(tǒng)實(shí)驗(yàn)結(jié)果的真值。低速動(dòng)態(tài)試驗(yàn)主要開展了兩大類型的飛行試驗(yàn),第一類為定向接近試驗(yàn),航模飛機(jī)攜帶UWB標(biāo)簽從左前方上由遠(yuǎn)到近接近定位基站,第二類為盤旋飛行試驗(yàn)。

圖4 航模飛機(jī)Fig.4 Aircraft model aircraft

圖5 安裝于航模底部的UWB標(biāo)簽Fig.5 UWB label installed at the bottom of the model aircraft

圖6 基站布設(shè)示意圖Fig.6 Schematic map of the base station

(1) 飛機(jī)左前方飛行

飛機(jī)從左前方方位角約為-14°的角度由遠(yuǎn)到近駛?cè)?飛行的飛行態(tài)勢(shì)如圖7所示,使用HSUKF算法和GPS輔助跟蹤定位曲線圖如圖8所示,由于飛機(jī)航模由人操作,在沒有參照物情況下,飛行軌跡很難保持直線,狀態(tài)容易發(fā)生由于突變,正好檢驗(yàn)算法對(duì)突變情況的適應(yīng)能力,從圖8可以看出HSUKF算法在狀態(tài)發(fā)生突變時(shí)也能保持良好的跟蹤效果。

圖7 左前方飛行示意圖Fig.7 Schematic of left front flight

圖8 HSUKF左前方飛行跟蹤示意圖Fig.8 HSUKF left forward flight tracking diagram

(2) 300 m處盤旋飛行

為了進(jìn)一步論證目標(biāo)進(jìn)行曲線運(yùn)動(dòng)時(shí)的跟蹤性能在距原點(diǎn)300 m距離上進(jìn)行了順時(shí)針盤旋飛行試驗(yàn),盤旋飛行試驗(yàn)態(tài)勢(shì)圖如圖9所示,使用HSUKF算法和GPS對(duì)目標(biāo)進(jìn)行跟蹤,跟蹤曲線如圖10所示,由圖10可知算法在目標(biāo)進(jìn)行曲線運(yùn)動(dòng)時(shí)也能夠保持好的跟蹤性能,在整個(gè)過程中目標(biāo)狀態(tài)發(fā)生了多個(gè)突變,HSUKF算法都能保持跟蹤性能,進(jìn)一步驗(yàn)證了本文算法的有效性。

圖9 盤旋飛行示意圖Fig.9 Schematic diagram of circling flight

圖10 HSUKF盤旋飛行跟蹤示意圖Fig.10 HSUKF circling flight tracking diagram

4 結(jié) 論

本文提出了HSUKF算法解決UKF在系統(tǒng)狀態(tài)發(fā)生突變時(shí)估計(jì)精度下降的問題。通過將改進(jìn)的STF算法與基于概率密度的HUKF算法相結(jié)合,在不增加計(jì)算復(fù)雜度的情況下,提高了定位精度和魯棒性。通過將HSUKF和UKF、OCKF以及AHCKF對(duì)比證明了本文算法的最優(yōu)性,最后通過實(shí)例驗(yàn)證了HSUKF算法的有效性。