快速強跟蹤UKF算法及其在機動目標跟蹤中的應用

鮑水達, 張 安, 畢文豪

(1. 西北工業大學電子信息學院, 陜西 西安 710129;2. 西北工業大學航空學院, 陜西 西安 710072)

0 引 言

過去幾十年間,卡爾曼濾波(Kalman filter, KF)作為一種經典高效的濾波方法,廣泛應用于信號處理、目標跟蹤、故障檢測、捷聯導航等領域。傳統的KF只能處理線性時不變系統,無法處理非線性系統。為此,學者們基于KF提出擴展卡爾曼濾波(extended Kalman filter, EKF)、無跡卡爾曼濾波(unscented Kalman filter, UKF)等新的非線性濾波算法。EKF通過Taylor展開,將非線性問題局部線性化,從而解決非線性濾波問題。但EKF對非線性函數近似精度為一階,濾波精度方面并不如人意,且要計算雅可比矩陣,一定程度上限制它的應用。UKF基于無跡變換(unscented transform, UT),采用確定性采樣方法近似非線性系統的后驗概率密度函數[1]。UKF不用求解雅可比矩陣,對非線性系統狀態轉移函數和量測函數沒有可導性要求,無需知道函數的具體表達形式,也可對黑盒系統進行處理;UKF至少能夠達到三階精度,明顯優于EKF,而計算量同階,獲得更多的關注和更廣泛的應用[2-5]。

無論是KF、EKF或UKF,它們都有一個共同的基本假設:系統模型是準確的。當系統模型不能正確描述真實系統時,將導致濾波精度下降,甚至濾波發散。為此,學者們提出了各式各樣的自適應算法,用于提高KF及相關算法的魯棒性,保證濾波精度,降低對系統模型準確性的要求[6-13]。自適應濾波算法可分為貝葉斯方法、最大似然函數、相關性和協方差匹配四大類[9]。前兩種方法多應用于多模型系統,但計算量很大的不足阻礙了它們的實際應用[11-12]。相關性方法,是將一組方程將函數與未知參數關聯起來,主要用于時變系統[13]。協方差匹配方法通過比較理論協方差與實際協方差之間的差異來判斷濾波優劣,進而對濾波進行在線自適應調整,是一種廣泛應用的自適應方法[6-7]。文獻[6]基于狀態向量估計殘差協方差匹配,提出一種自適應漸消UKF(adaptive fading UKF, AFUKF),來克服模型不確定性,然而,與UKF相比,計算量增加較多,且要額外計算雅各比矩陣。文獻[7]提出的強跟蹤算法(strong tracking filter, STF)是一種基于新息協方差匹配的自適應濾波方法。STF通過引入次優漸消因子,調整預測狀態協方差,進而在線調整濾波增益K,達到自適應的目的。將STF引入UKF,建立強跟蹤UKF,能極大提高UKF魯棒性,降低對系統模型依賴[14-18]。

文獻[14]推導了漸消因子引入UKF的過程,并給出了在UKF形式下,次優漸消因子的等價計算方法。強跟蹤UKF以其良好的濾波性能及魯棒性,已經廣泛應用在諸多領域中。文獻[15]采用一種改良的平方根分解方法,提出改進的強跟蹤平方根UKF,克服衛星自主導航中不確定性干擾和噪聲的影響。文獻[16]應用假設檢驗理論提出改進的強跟蹤UKF,并實際應用于無人機動態導航。文獻[17]提出強跟蹤平方根無跡 Kalman 神經網絡來提高能耗模型的精度和對電解槽突變狀態的實時跟蹤能力。文獻[18]提出改進的強跟蹤UKF算法,用于捷聯慣性導航系統(strap-down inertial navigation system, SINS)大方位失準角初始對準中。然而,強跟蹤UKF的UT變換變為3次,極大地增加算法計算量,限制它在一些工程中的應用,如板載小型衛星[6]。基于以上分析,本文采用Taylor展開分析漸消因子機理,建立漸消因子近似引入方法,減少UT變換次數,提出快速強跟蹤UKF,并定性分析快速強跟蹤UKF計算量增長。

1 強跟蹤UKF算法

1.1 強跟蹤UKF

考慮非線性離散系統

xk=f(xk-1)+wk-1

(1)

zk=h(xk)+vk

(2)

式中,xk∈Rn為系統狀態向量;zk∈Rm為量測向量;n和m為對應維數;f(·)和h(·)分別為非線性系統的狀態轉移函數和量測函數;wk-1∈Rn為系統噪聲,vk∈Rm為量測噪聲,二者均為高斯白噪聲,且互不相關,協方差矩陣為Qk和Rk。

強跟蹤UKF將漸消因子引入UKF,兼具有STF魯棒性強和UKF變換精度高,有效克服了UKF在系統模型不準確時魯棒性差的問題。針對式(1)和式(2)描述的非線性系統,強跟蹤UKF由4個步驟組成[12]。

步驟1選擇采樣策略

本文采用對稱采樣策略

(3)

式中,κ為可設參數;i=1,2,…,n。對應權值為

(4)

步驟2時間更新

χj=f(Xj)

(5)

(6)

(7)

式中,上標s表示λk引入前的變量。

步驟3計算漸消因子λk

(8)

(9)

(10)

(11)

λk計算公式為

(12)

(13)

新的預測狀態協方差Pk/k-1為

(14)

步驟4量測更新

(15)

(16)

(17)

強跟蹤UKF在時間更新、計算漸消因子λk和量測更新3個步驟共進行3次UT變換,比傳統UKF多使用一次UT,額外使用UT變換會極大地增加算法計算量。可見,現有的引入漸消因子方法是以增加算法計算量為代價,來克服UKF在系統模型不準確時魯棒性差的不足,仍需進一步完善改進。

1.2 漸消因子引入

STF通過引入次優漸消因子,調整狀態協方差,進而在線調整增益K,迫使殘差正交,即

(18)

式中,Pk-1是k-1時刻的狀態協方差;λk是k時刻的漸消因子;上標s表示漸消因子引入前的變量。此時,預測狀態協方差Pk/k-1為

(19)

式中,Fk為雅可比矩陣。式(19)為強跟蹤UKF采用的漸消因子引入方法。

另一種漸消因子引入方法[18]為

(20)

對于EKF而言,采用式(19)或者式(20)不會產生顯著的差異,因為EKF本身要使用雅可比矩陣F和H。但對于UKF等不用計算雅可比矩陣的濾波方法,基于式(20)的漸消因子引入方法,不用額外計算雅可比矩陣,故式(20)比式(19)更加合適。本文的分析和推導將基于式(20)。

根據式(20),漸消因子引入UKF后為

(21)

對應次優漸消因子計算方法[17]為

(22)

式中,Vk的估算公式為式(13);β為可調參數,β≥1。

2 快速強跟蹤UKF算法

2.1 漸消因子近似引入方法

根據式(21),本文提出一種適用于UKF的漸消因子近似引入方法,即

(23)

(24)

(25)

(26)

在近似方法中,漸消因子直接作用于相關變量上,不再間接地通過UT變換引入UKF, UT變換使用次數將降低為2次。

漸消因子近似引入方法推導如下。

(27)

(28)

(29)

(30)

更新后的狀態協方差Pk為

(31)

(32)

將式(20)和式(32)代入式(28),有

(33)

(34)

(35)

(36)

將式(36)代入式(10),得

(37)

(38)

將式(21)代入式(31)得

(39)

推導完畢。

2.2 算法流程

根據傳統UKF算法流程以及式(1)和式(2)表示的非線性系統,基于近似引入方法的快速強跟蹤UKF算法流程如下:

步驟1選擇采樣策略

本文采用對稱采樣策略,如式(3)和式(4)。

步驟2時間更新

步驟3計算漸消因子λk

根據式(22)計算得k時刻λk,并代入式(21)求得更新后的Pk/k-1。

步驟4量測更新

與強跟蹤UKF相比,快速強跟蹤UKF僅需要兩次UT變換,在步驟3中直接套用求解公式即可求解得λk,過程簡潔,基本與傳統UKF一致。

3 計算量分析和收斂性分析

3.1 計算量分析

本節將利用統計浮點運算次數(flops)的方法,分析傳統UKF、強跟蹤UKF與快速強跟蹤UKF的計算量,說明兩強跟蹤UKF相對于傳統UKF在不同情況下的計算量增長情況,假設采樣策略為對稱采樣,矩陣平方根算法為Cholesky分解。

強跟蹤UKF和傳統UKF均為O(n3)的算法,以最高冪次的系數(即n3的系數)為對比標準,粗略分析得計算量變化。一次flops定義為兩個浮點數進行一次加、減、乘、除運算。UKF中計算量為n3的矩陣操作如表1所示[19]。對于矩陣乘法為A·B,A∈RN×M,B∈RM×L。

表1 各矩陣操作的計算量

UKF的計算量主要由n、m、f(·)和h(·)決定。先忽略f(·)和h(·)計算量的影響,討論僅考慮n、m的影響。經統計,3種算法的計算量如表2所示。快速強跟蹤UKF并沒額外增加計算量為n3的操作,故不考慮f(·)和h(·)計算量時,其計算量與傳統UKF相當,且不受采樣策略與平方根求解算法影響。強跟蹤UKF由于要多使用一次UT變換,且在漸消因子計算中有矩陣乘法,求逆等操作,計算量明顯高于傳統UKF。

表2 各算法的計算量

在快速強跟蹤UKF中,僅在式(23)中多調用了1次h(·)。而強跟蹤UKF中,額外使用UT變換導致h(·)被多調用2n次。對于f(·)的調用,3種算法并沒有區別。

綜上,快速強跟蹤UKF的計算量增長明顯低于強跟蹤UKF,與傳統UKF相近,且隨著狀態向量維數和h(·)計算量的增加,快速強跟蹤UKF的計算量會越來越接近傳統UKF。相反,如果f(·)的計算量變大,將會削弱快速強跟蹤UKF在計算量增長方面的優勢。

3.2 收斂性分析

本節采用新息序列的特性[20],分析快速強跟蹤UKF的收斂性。采用的濾波收斂性判據為

(40)

式中,r為儲備系數,r≥1。r=1為最嚴格的收斂性判據,當r足夠大時,即可判定濾波發散。式(40)右側是理論新息協方差,左側是k時刻的實際新息協方差,在快速強跟蹤UKF中即為Vk,對式(22)進行變換,有

β·tr(Rk)

(41)

(1) 漸消因子λk=1

當漸消因子λk=1,快速強跟蹤UKF退化為標準UKF,保留UKF的優良性能。根據式(22),Ck≤1,代入式(41),有

β·tr(Rk)

(42)

快速強跟蹤UFK中理論新息協方差為

(43)

對式(43)進行矩陣求跡,有

r·tr(Pzz,k)=

(44)

β·tr(Rk)≤r0·tr(Pzz,k)

(45)

(2) 漸消因子λk>1

λk>1表示快速強跟蹤UKF檢測到濾波錯誤,需要進行修正。實際新息協方差為

β·tr(Rk)

(46)

修正后,快速強跟蹤UFK中新的理論新息協方差為

(47)

與式(1)中分析類似,必然存在一個略大于1的r0,使得以下不等式成立,滿足收斂性判據:

β·tr(Rk)≤r0·tr(Pzz,k)

(48)

綜上,無論λk取何值,快速強跟蹤UKF均滿足收斂性判據,保持濾波穩定性。

4 仿真實例

4.1 一維仿真實例

仿真實例采用強非線性模型[21](univariate nonstationary growth model,UNGM)

(49)

式中,α、β、μ是可設參數;cos(1.2(n-1))可視為時變噪聲;c1和c2為調整參數,通過調整c1和c2來模擬系統模型相對于真實系統的失調程度。初始條件為x0=10,P0=10,Q=0.01,R=1,α=0.5,β=2.5,ν=8。

選擇傳統UKF,強跟蹤UKF、文獻[6]中的AFUKF和快速強跟蹤UKF來對比驗證本文算法魯棒性、濾波性能、計算量。

首先,取c1=c2=1,即系統模型與真實模型一致,4種算法的濾波結果如圖1所示。在模型準確時,新息理論協方差與新息實際協方差基本一致,漸消因子始終為1,強跟蹤UKF理論上會退化為傳統UKF算法。從圖1可得,4種算法濾波結果基本一致,3種自適應UKF均正確地退化為傳統UKF,與傳統UKF濾波結果一致。

圖1 模型準確時4種算法濾波結果Fig.1 Results of four algorithms in accurate model

算法運行時間可作為判斷算法計算量的指標。在CPU為Intel i3 550,內存為2 GB的筆記本上,仿真時間N=60,蒙特卡羅仿真M=100,4種算法在Matlab 2014a上的運行時間和相對傳統UKF增長的如表3所示。快速強跟蹤UKF避免額外使用UT變換,運行時間增長明顯小于強跟蹤UKF,降低了約64.26%,計算量增長遠小于強跟蹤UKF,但需額外計算λk,計算量變大,故運行時間大于傳統UKF,在本例中增長了約20.46%。AFUKF采用假設檢驗來判斷是否進行修正,本例中不存在模型不確定性,故運行時間僅增長21.53%。

表3 一維模型時各算法運行時間

然后,對比4種算法在模型不準確時的性能差異。在真實模型中,取c1=0.6,c2=0.75;在系統模型中,仍然取c1=c2=1,即相比真實模型,系統模型在建模過程中α和γ取值偏大。圖2為4種算法的濾波結果,圖3為4種算法濾波誤差,表4為均方根誤差(root mean square error, RMSE),表5為對應運行時間。

表4 一維模型不準確時各算法額運行時間

圖2 模型不準確時4種算法濾波結果Fig.2 Result of four algorithms in inaccurate model

對比4種算法的濾波結果,3種自適應UKF的性能明顯優于傳統UKF。AFUKF的性能介于傳統UKF和強跟蹤UKF之間。漸消因子提高UKF的魯棒性,增強UKF適應模型失調的能力,使得2種強跟蹤UKF在模型失調時仍能給出較好的濾波結果。由圖3可知,傳統UKF中大部分誤差大的點,均被2種強跟蹤UKF修正。快速強跟蹤UKF作為強跟蹤UKF的近似方法,性能差異極小,RMSE僅相差0.060 3。模型不確定性使得AFUKF進行修正,導致運行時間大大增加,與強跟蹤UKF相近,比快速強跟蹤UKF多增長了約51.28%。

圖3 模型不準確時4種算法濾波誤差Fig.3 Filtering error of four algorithms in inaccurate model

算法時間/s增長傳統UKF1.4511強跟蹤UKF2.4971.720 9AFUKF2.4341.677 5快速強跟蹤UKF1.7191.184 7

4.2 機動目標跟蹤

以一個典型的機動目標跟蹤問題來驗證算法的有效性。一個地面雷達連續瞄準和跟蹤一個飛行器,飛行器以未知的轉速下在水平面中執行機動轉彎。飛行器在水平面中機動轉彎的運動學模型[22]為

xk-1+wk-1

(50)

(51)

式中,T是雷達的采樣間隔,T=1;φ1和φ2為調整參數,且φ1=1 m2/s3,φ2=1.75×10-3rad2/s3。

雷達安置在原點(0,0)處,獲取與飛行器的距離σ和方位角θ

(52)

式中,arctan是求反正切函數;vk是相互獨立的高斯白噪聲,且均值為0,協方差為

R=diag[1 000 m2100 m·rad2]

(53)

初始條件為

x0=[1 000 m 300 m/s 1 000 m 0 m/s -3°/s]T

P0=

diag[100 m210 m2/s2100 m210 m2/s2100 mrad2/s2]T

(54)

k=15時,模擬飛行器發生位置突變，突變量Δxk,Δyk為

(55)

圖4 狀態突變時和Ωk的RMSEFig.4 RMSE of and Ωk in mutation of state

由圖4可知,突變發生前,4種算法濾波結果基本一致,說明當模型準確時,2種強跟蹤UKF算法均能正確地退化為UKF算法,保持了UKF算法的優良性能。突變發生時,4種算法在該時刻均無法處理該情況,濾波結果不理想。突變發生后,3種自適應UKF在幾個時刻內迅速收斂,濾波誤差下降到正常范圍,不再受突變點影響。傳統UKF則不然,突變點的影響持續了將近20個時刻,濾波誤差較大。圖5與以上分析相符,突變后,傳統UKF的軌跡開始在參考值附近來回波動一段時間才慢慢穩定到參考值附近。漸消因子的引入增強了UKF應對狀態突變的能力。AFUKF也具備一定應對狀態突變能力,但穩定性明顯不如兩種強跟蹤UKF。

圖5 狀態突變時4種算法的濾波結果Fig.5 Results of four algorithms in mutation of state

4種算法的運行時間如表6所示,運行計算機配置與仿真實例1相同。在該實例中,快速強跟蹤UKF的運行時間已經非常接近傳統UKF。這也驗證了第3.1節計算量分析的結果,隨著狀態向量維數的增加,快速強跟蹤UKF和傳統UKF的計算量會越來越接近。

表6 4種算法的運行時間

5 結 論

漸消因子引入UKF,能有效提高UKF魯棒性,增強適應模型失調、狀態突變的能力,但要求額外使用UT變換,導致計算量急劇提高。本文利用Taylor展開,分析漸消因子在UKF中的作用機理。在UKF中漸消因子不僅在線調整濾波增益,還調整量測預測值,通過同時調整兩者來達到充分提取新息序列中信息的目的。然后,本文建立一種適用于UKF的漸消因子近似引入方法,將漸消因子直接作用于相關的協方差和量測預測值上,提出快速強跟蹤UKF。將快速強跟蹤UKF用于機動目標跟蹤問題進行仿真,表明快速強跟蹤UKF能有效克服狀態突變,保持濾波的穩定性,增強了UKF的魯棒性,性能與強跟蹤UKF相差無幾。同時,快速強跟蹤UKF計算量與UKF相近,遠低于強跟蹤UKF。

參考文獻：

[1] JULIER S J, UHLMANN J K, DURRANT-WHYTE H F. A new approach for filtering nonlinear systems[C]∥Proc.of the American Control Conference, 1995: 1628-1632.

[2] JUNG K, KIM J, KIM J, et al. Positioning accuracy improvement of laser navigation using UKF and FIS[J]. Robotics and Autonomous Systems, 2014, 62(9): 1241-1247.

[3] SHAN B N, WANG J, DENG B, et al. UKF-based closed loop iterative learning control of epileptiform wave in a neural mass model[J]. Cognitive Neurodynamics, 2015, 9(1): 31-40.

[4] 許杰,戚大偉.基于改進加權質心和UKF的移動目標定位算法[J].吉林大學學報(工學版),2016,46(4):1354-1359.

XU J, QI D W. Moving target localization based on improved weighted centroid and UKF algorithm[J]. Journal of Jilin University (Engineering and Technology Edition), 2016, 46(4):1354-1359.

[5] HU G G, GAO S S, ZHONG Y M. A derivative UKF for tightly coupled INS/GPS integrated navigation[J]. ISA Transactions, 2015, 56: 135-144.

[6] SOKEN H E, HAJIYEV C. Adaptive fading UKF with Q-adaptation: application to picosatellite attitude estimation[J]. Journal of Aerospace Engineering, 2011, 26(3): 628-636.

[7] 周東華,席裕康.非線性系統帶次優漸消因子的擴展卡爾曼濾波[J].控制與決策,1990,5(5): 1-6.

ZHOU D H, XI Y K. Suboptimal fading extended Kalman filtering for nonlinear systems[J].Control and Decision,1990,5(5): 1-6.

[8] ISHIHARA S. Robust Kalman filtering for nonlinear systems with parameter uncertainties[C]∥Proc.of the SICE Annual Conference, 2014: 1986-1991.

[9] HAJIYEV C, SOKEN H E. Robust adaptive unscented Kalman filter for attitude estimation of pico satellites[J]. International Journal of Adaptive Control and Signal Processing,2014,28(2): 107-120.

[10] LI W L, SUN S H, JIA Y M, et al. Robust unscented Kalman filter with adaptation of process and measurement noise covariances[J]. Digital Signal Processing, 2016, 48(C): 93-103.

[11] GAO B B, GAO S S, GAO L, et al. An adaptive UKF for nonlinear state estimation via maximum likelihood principle[C]∥Proc.of the 6th International Conference on Electronics Information and Emergency Communication, 2016: 117-120.

[12] MOCHNAC J, MARCHEVSKY S, KOCAN P. Bayesian filtering techniques: Kalman and extended Kalman filter basics[C]∥Proc.of the 19th International Conference Radioelektronika, 2009: 119-122.

[13] GROTHE O. A higher order correlation unscented Kalman filter[J].Applied Mathematics and Computation,2013,219(17):9033-9042.

[14] 王小旭,趙琳,夏全喜.基于Unscented變換的強跟蹤濾波器[J].控制與決策,2010,25(7):1063-1068.

WANG X X, ZHAO L, XIA Q X. Strong tracking filter based on unscented transformation[J]. Control and Decision, 2010, 25(7): 1063-1068.

[15] 李敏,王松艷,張迎春,等.改進的強跟蹤平方根UKF在衛星導航中應用[J].系統工程與電子技術,2015,37(8):1858-1865.

LI M, WANG S Y, ZHANG Y C, et al. Satellite autonomous navigation filtering algorithm based on improved strong tracking square-root UKF[J]. Systems Engineering and Electronics, 2015, 37(8): 1858-1865.

[16] HU G G, GAO S S, ZHONG Y M, et al. Modified strong tracking unscented Kalman filter for nonlinear state estimation with process model uncertainty[J]. International Journal of Adaptive Control and Signal Processing, 2015, 29(12): 1561-1577.

[17] 李太福,姚立忠,易軍,等.強跟蹤平方根UKFNN的鋁電解槽工耗動態演化模型[J].自動化學報,2014,40(3): 522-530.

LI T F, YAO L Z, YI J, et al. An improved UKFNN based on square root filter and strong tracking filter for dynamic evolutionary modeling of aluminum reduction cell[J]. Acta Automatica Sinica, 2014, 40(3): 522-530.

[18] 郭澤,繆玲娟,趙洪松.一種改進的強跟蹤UKF算法及其在SINS大方位失準角初始對準中的應用[J].航空學報,2014,35(1): 203-214.

GUO Z, MIAO L J, ZHAO H S. An improved strong tracking UKF algorithm and its application in sins initial alignment under large azimuth misalignment angles[J]. Acta Aeronautica et Astronautica Sinica, 2014, 35(1): 203-214.

[19] GREWAL M S, ANDREWS A P. Kalman filtering: theory and practice using matlab[M]. 3rd ed. New Jersey: Wiley, 2008.

[20] 徐樹生,林孝工,趙大威,等.強跟蹤SRCKF及其在船舶動力定位中的應用[J].儀器儀表學報,2013,34(6): 1266-1272.

XU S S, LIN X G, ZHAO D W, et al. Strong tracking SRCKF and its application in vessel dynamic positioning[J]. Chinese Journal of Scientific Instrument, 2013, 34(6): 1266-1272.

[21] KOTECHA J H, DJURIC P M. Gaussian particle filtering[J]. IEEE Trans.on Signal Processing,2003,51(10):2592-2601.

[22] ZHAO L Q, WANG J L, YU T, et al. Design of adaptive robust square-root cubature Kalman filter with noise statistic estimator[J].Applied Mathematics and Computation,2015, 256(C): 352-367.