基于時頻差的高階強跟蹤UKF算法

周恭謙, 楊露菁, 劉 忠

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

0 引 言

目標跟蹤是對目標位置和速度的實時估計與預測,其被廣泛的應用于軍事領域之中。基于有源系統的目標跟蹤技術發展較為成熟,但在軍事對抗中很容易被干擾從而導致跟蹤失敗,因此基于無源系統的目標跟蹤技術受到了廣泛的關注。在無源跟蹤系統中,多觀測站可以通過接收目標信號的到達時間差(time difference of arrival, TDOA)和到達頻率差(frequency difference of arrival, FDOA)來完成對目標位置和速度的估計,由于TDOA和FDOA 構建的模型為非線性方程組,所以基于TDOA/FDOA的目標跟蹤屬于非線性濾波問題。

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

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

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

1.1 基于概率密度高階導的UT轉換

UT 變換作為一種計算隨機變量經過非線性變換后統計特性的方法, 它通過設置Sigma 樣點分布和權值來逼近樣本非線性變換參量的矩[26]。事實上為了更好的近似隨機變量經過非線性變換后的統計特性,Sigma點集應盡可能近似更高階數的中心矩[28-29]。由于多維高斯變量的問題都可以轉化為多個獨立一維標準高斯分布的問題,下面先對服從一維標準高斯分布的隨機變量x進行討論,h(x)表示關于x的非線性函數,h(x)的期望可表示為

(1)

表1 p(x)各階導數對應的極值點

(2)

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

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

(3)

則服從標準正態分布的x的高階中心距為(k-1)!=1·3·5…(k-1)

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

(4)

將σi代入式(4)即可求得對應的Wi的值,則服從一維標準高斯分布的x的sigma點集Si為

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

(5)

將一維標準正態分布拓展到n維隨機變量x中則有

(6)

1.2 基于高斯密度高階導的UKF

對離散的非線性系統

(7)

式中,Xk,Zk表示系統k時刻的狀態向量和量測向量;f(·)、h(·)表示對應的轉換函數;wk、vk為互不相關的高斯白噪聲滿足:

(8)

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

初始化

(9)

Sigma點計算

(10)

步驟1時間更新

將Sigma點進行非線性變換

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

(11)

計算預測估計值

(12)

計算預測協方差

(13)

步驟2量測更新

(14)

非線性變換sigma點

(15)

計算預測測量值

(16)

預測新息協方差

(17)

預測互協方差矩陣

(18)

計算Kalman增益

(19)

更新誤差協方差

(20)

更新狀態

(21)

2 高階強跟蹤卡爾曼濾波

2.1 改進的強跟蹤算法

(22)

強跟蹤UKF[22-24]通過將漸消因子λk引入預測協方差陣Pk|k-1,來對濾波增益矩陣Kk進行實時調整以提高系統的魯棒性與自適應能力,具體為

(23)

綜上所述,強跟蹤UKF算法在每次濾波過程中對比UKF都要多進行一次無跡變換,而這將會大大增加算法復雜度,不利于工程實踐。當非線性系統中狀態變量或者觀測變量維數較高時,這個問題將會更加突出。

而根據式(19)可知調整Kalman增益還可通過調整Pxz,k|k-1,Pzz,k|k-1來實現,可將λk分別引入Pxz,k|k-1,Pzz,k|k-1中有

(24)

(25)

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

2.2 漸消因子的計算

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

(26)

記HUKF的預測誤差與估計誤差分別為

(27)

(28)

引入未知對角陣βk使得

(29)

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

(30)

由式(30)可知

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

(31)

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

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

(32)

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

(33)

對(33)求解

(34)

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

(35)

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

(36)

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

3 實 驗

3.1 多站TDOA/FDOA跟蹤模型

第i個接收站到待測目標的距離可表示為

(37)

式中,‖·‖為求2范數運算。

待測目標到接收站i與到第一個接收站的距離差觀測量可表示為

(38)

對式(37)求導可得距離關于時間的微分方程

(39)

對式(38)求導可得距離差關于時間的微分方程

(40)

頻差觀測量可表示為

(41)

式中,f0目標信號頻率。

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

(42)

式中,wk-1為高斯白噪聲,代表方差為σk-1的對角矩陣;φk為狀態轉移矩陣;T為觀測時間間隔。

TDOA/FDOA定位測量方程可表示為

(43)

3.2 仿真實驗

4個觀測站的位置及速度如表2所示。

表2 接收站和目標的位置和速度

令目標做轉彎運動,運動軌跡如圖1所示[19,30],目標運動的起點為I,終點為F。

圖1 目標運動軌跡示意圖Fig.1 Sketch map of target motion

信號頻率f0=3×108Hz,狀態轉移矩陣φk為

(44)

(45)

(46)

(47)

按照所給的參數進行仿真實驗,圖2和圖3為本文提出的HSUKF算法和UKF、AHCKF、OCKF算法、BA-PF以及IMM-RKF位置跟蹤RMSE和速度跟蹤RMSE曲線。表3給出的是幾種算法濾波過程中計算的采樣點數、200次蒙特卡羅實驗消耗的時間以及平均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)運算時間/s采樣點個數UKF86.0632.0511.5218OCKF60.1228.7311.6416IMM-RKF48.1126.5837.2618AHCKF43.5722.8121.4199BA-PF42.1422.67180.57100HSUKF30.1916.2013.3934

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

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

3.3 實例論證

為對運動目標的跟蹤效果進行驗證,在太平寺機場開展了低速動態試驗,在一個小型航模飛機掛載UWB標簽,如圖4和圖5所示。以觀測區圓心為原點,3個定位基站按照(-50 m,0 m),(50 m,0 m),(0 m,-50 m)的正三角結構進行布設如圖6所示。此外布設一套GPS-RTK高精度定位系統,作為系統實驗結果的真值。低速動態試驗主要開展了兩大類型的飛行試驗,第一類為定向接近試驗,航模飛機攜帶UWB標簽從左前方上由遠到近接近定位基站,第二類為盤旋飛行試驗。

圖4 航模飛機Fig.4 Aircraft model aircraft

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

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

(1) 飛機左前方飛行

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

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

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

(2) 300 m處盤旋飛行

為了進一步論證目標進行曲線運動時的跟蹤性能在距原點300 m距離上進行了順時針盤旋飛行試驗,盤旋飛行試驗態勢圖如圖9所示,使用HSUKF算法和GPS對目標進行跟蹤,跟蹤曲線如圖10所示,由圖10可知算法在目標進行曲線運動時也能夠保持好的跟蹤性能,在整個過程中目標狀態發生了多個突變,HSUKF算法都能保持跟蹤性能,進一步驗證了本文算法的有效性。

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

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

4 結 論

本文提出了HSUKF算法解決UKF在系統狀態發生突變時估計精度下降的問題。通過將改進的STF算法與基于概率密度的HUKF算法相結合,在不增加計算復雜度的情況下,提高了定位精度和魯棒性。通過將HSUKF和UKF、OCKF以及AHCKF對比證明了本文算法的最優性,最后通過實例驗證了HSUKF算法的有效性。