鄧 兵， 孫正波， 賀 青

(盲信號處理重點實驗室，四川 成都 610041)

長期以來，目標跟蹤作為一個經(jīng)典問題，在雷達、通信、聲納、遙測遙控、無線網(wǎng)絡(luò)等多個領(lǐng)域受到廣泛關(guān)注[1-4]．典型的無源定位跟蹤系統(tǒng)，通過測量目標與傳感器之間在時域、頻域、空域或能量域的相對關(guān)系[5]，應(yīng)用卡爾曼濾波(Kalman Filter，KF)算法[1-5]、粒子濾波(Particle Filter，PF)算法[6]等獲取目標狀態(tài)估計．而在實際當中，由于測量系統(tǒng)往往是非線性模型，需要采用非線性濾波方法來提高濾波精度．

擴展卡爾曼濾波器(Extended Kalman Filter，EKF)作為一種應(yīng)用最為廣泛的非線性濾波器[2]，其通過對非線性函數(shù)進行一階泰勒級數(shù)展開并舍棄高級誤差項，將濾波過程中的非線性目標狀態(tài)估計問題轉(zhuǎn)化為線性估計問題，再利用最優(yōu)卡爾曼濾波進行狀態(tài)估計，是一種次優(yōu)濾波方法．獲得一次狀態(tài)估計以后，在更新的狀態(tài)估計值附近對原非線性函數(shù)重新進行泰勒展開，再次利用擴展卡爾曼濾波器對重新展開的線性函數(shù)進行估計，可能會獲得更好的估計值，重復(fù)迭代這一過程直至收斂，即為迭代擴展卡爾曼濾波器(Iterated Extended Kalman Filter，IEKF)[3-5]．此外，通過計算非線性測量方程的修正函數(shù)還發(fā)展出了修正增益擴展卡爾曼濾波器(Modified Gain Extended Kalman Filter，MGEKF)[5]，但是修正函數(shù)不是任何場景下均存在，影響了此類算法的推廣應(yīng)用．近年來，將粒子濾波器與標準擴展卡爾曼濾波器相結(jié)合，利用擴展卡爾曼濾波器產(chǎn)生重要性密度函數(shù)，從而形成了擴展卡爾曼粒子濾波器(Extended Kalman Particle Filter，PF-EKF)[6]．但是以上方法均沒有克服掉擴展卡爾曼濾波器固有的缺陷，當系統(tǒng)的非線性比較強時，采用局部線性化處理非線性系統(tǒng)估計問題會帶來較大的誤差，嚴重時甚至?xí)?dǎo)致濾波器的發(fā)散．文獻[7-8]中提出了一種線性校正時差定位算法，通過線性校正非線性定位方程的初始定位結(jié)果來提升定位精度．因此，若能夠采用線性校正思想對狀態(tài)濾波結(jié)果進行偏差修正，抑制局部線性化所帶來的誤差影響，將有利于提升目標跟蹤精度．

筆者主要考慮在非線性目標跟蹤濾波中，利用文獻[7-8]中的線性校正思想，結(jié)合擴展卡爾曼濾波器濾波算法的特點，通過對擴展卡爾曼濾波器濾波結(jié)果偏差進行估計校正來提升跟蹤的有效性．首先給出標準擴展卡爾曼濾波的基本流程，在此基礎(chǔ)上提出偏差修正擴展卡爾曼濾波算法；然后在時差頻差跟蹤場景下給出具體的表達式．

1 標準擴展卡爾曼濾波算法基本流程

目標跟蹤主要是在目標運動建模的基礎(chǔ)上建立狀態(tài)方程與測量方程，再利用濾波跟蹤算法求得目標的狀態(tài)信息．卡爾曼濾波是線性高斯條件下的狀態(tài)最優(yōu)估計解，然而在實際系統(tǒng)中，動態(tài)過程和量測過程通常是非線性的，不能直接使用卡爾曼濾波算法．通過泰勒級數(shù)展開的方法可以獲得非線性系統(tǒng)的近似，進而采用卡爾曼濾波處理非線性系統(tǒng)的濾波問題，這就是擴展卡爾曼濾波．由于采用局部線性化的方法處理非線性濾波問題，擴展卡爾曼濾波算法只有在模型線性化誤差較小時才能夠獲得優(yōu)良的濾波結(jié)果，否則線性化截斷誤差將會嚴重影響目標狀態(tài)的估計精度，甚至使得跟蹤結(jié)果不收斂．

考慮如下方程所描述的離散時間非線性系統(tǒng)，假定系統(tǒng)中的過程噪聲和量測噪聲都是加性噪聲:

其中，xk∈Rn，表示k時刻的狀態(tài)向量;zk∈Rn，表示k時刻的量測向量; 非線性函數(shù)f(·)和h(·)分別表示動態(tài)模型函數(shù)和量測模型函數(shù);wk～N(0，Qk)和vk～N(0，Rk)，分別為系統(tǒng)服從零均值高斯分布的過程噪聲和量測噪聲，其協(xié)方差分別為Qk和Rk，兩者互不相關(guān)．

擴展卡爾曼濾波算法遞推步驟描述如下[4]．

步驟2 計算動態(tài)系統(tǒng)狀態(tài)方程的雅可比矩陣:

(3)

步驟3 計算狀態(tài)估計和估計誤差協(xié)方差:

(6)

步驟5 計算量測預(yù)測、新息協(xié)方差矩陣和卡爾曼增益:

步驟6 計算狀態(tài)估計和估計誤差協(xié)方差:

相對于其他非線性濾波方法，擴展卡爾曼濾波的優(yōu)點是使用較為簡便，用簡單的算法即可獲得較好的濾波效果．而且在工程實踐上，線性化近似是解決非線性系統(tǒng)問題的常用手段，易于理解和使用，因此可以廣泛應(yīng)用于大多數(shù)實際問題中．然而，其缺點在于擴展卡爾曼濾波在非線性度較高的系統(tǒng)中無法獲得滿意的濾波效果，線性化截斷誤差對目標狀態(tài)估計結(jié)果影響明顯，嚴重時甚至導(dǎo)致跟蹤發(fā)散．

迭代卡爾曼濾波[5]通過將式(10)獲得的目標狀態(tài)估計帶回式(6)重新計算Hk，并重復(fù)步驟5至步驟6直至收斂．實際當中往往限制迭代次數(shù)以增強算法的時效性．

2 線性校正擴展卡爾曼濾波算法

(12)

(13)

根據(jù)最小二乘原理，可以得到

(14)

其中，W為加權(quán)矩陣，W=(R)-1．

則經(jīng)線性校正后的目標狀態(tài)最終估計及協(xié)方差分別為

至此可以得出偏差修正迭代擴展卡爾曼濾波算法流程圖，如圖1所示．

圖1 偏差修正迭代擴展卡爾曼濾波算法流程圖

3 時頻差場景下偏差修正迭代擴展卡爾曼濾波算法

4 仿真結(jié)果及分析

圖2和圖3分別仿真分析了擴展卡爾曼濾波器、迭代擴展卡爾曼濾波器、修正增益擴展卡爾曼濾波器算法與筆者所提線性校正迭代擴展卡爾曼濾波算法的位置與速度均方根誤差．仿真中迭代擴展卡爾曼濾波器及筆者所提算法迭代次數(shù)均為3次．從仿真結(jié)果可以看出，擴展卡爾曼濾波器由于局部線性化所帶來的舍入誤差會隨著跟蹤時間的增加而逐漸增加，嚴重時會引起濾波發(fā)散；迭代擴展卡爾曼濾波器由于增加了迭代過程，能夠減小局部線性化誤差所帶來的影響，但是也不能完全消除；修正增益擴展卡爾曼濾波器由于修正了每次的濾波增益，較擴展卡爾曼濾波器、迭代擴展卡爾曼濾波器算法跟蹤性能更優(yōu)；而筆者所提算法由于在每次濾波后進行了線性校正，較好地克服掉了大部分線性化誤差對濾波結(jié)果所帶來的影響，跟蹤收斂性最好．

圖2 不同算法位置估計均方根誤差圖3 不同算法速度估計均方根誤差

圖4和圖5為筆者所提線性校正擴展卡爾曼濾波算法在不同的迭代次數(shù)下的跟蹤效果．從結(jié)果可以看出，隨著迭代次數(shù)的增加，筆者提出算法的性能也得到增強．

此外，相比于迭代擴展卡爾曼濾波器算法，文中增加了線性校正過程，算法的復(fù)雜度有所增加，但增加不多．為驗證上述分析，在MATLAB R2009b仿真條件下，經(jīng)統(tǒng)計，在 60 s 數(shù)據(jù)長度下，運算200次擴展卡爾曼濾波器、迭代擴展卡爾曼濾波器(迭代次數(shù)為3次)、修正增益擴展卡爾曼濾波器以及筆者所提算法(迭代次數(shù)為3次)的計算總時間分別為 2.213 4 s、5.962 6 s、3.235 2 s、6.531 8 s，符合預(yù)期．

圖4 不同迭代次數(shù)下位置估計均方根誤差圖5 不同迭代次數(shù)下速度估計均方根誤差

5 總 結(jié)

針對實際當中非線性目標跟蹤濾波問題，根據(jù)擴展卡爾曼濾波器算法的自身特點，通過增加對濾波結(jié)果的偏差修正，利用加權(quán)最小二乘算法估計出目標狀態(tài)估計偏差，對原觀測矩陣進行更新，并再次濾波，最終得到目標狀態(tài)估計．仿真分析表明，筆者所提算法相比于擴展卡爾曼濾波器、迭代擴展卡爾曼濾波器以及修正增益擴展卡爾曼濾波器算法具有較好的跟蹤性能，能夠更好地適應(yīng)非線性濾波問題．筆者所提的偏差修正濾波思想可以進一步推廣到其他非線性濾波算法中，下一步將開展相應(yīng)的研究工作．

[1] 蔚婧, 文珺, 李彩彩, 等. 輔助變量純方位目標跟蹤算法[J]. 西安電子科技大學(xué)學(xué)報, 2016, 43(1): 167-172.

YU Jing, WEN Jun, LI Caicai, et al. Modified Instrumental Variable Method for Bearings-only Target Tracking[J]. Journal of Xidian University, 2016, 43(1): 167-172.

[2] LIU Y H, LI T, YANG Y Y, et al. Estimation of Tire-road Friction Coefficient Based on Combined APF-IEKF and Iteration Algorithm[J]. Mechanical Systems and Signal Processing, 2017, 88: 25-35.

[3] YANG Y J, FAN X G, ZHOU Z F, et al. Amended Kalman Filter for Maneuvering Target Tracking[J]. Chinese Journal of Electronics, 2016, 25(6): 1166-1171.

[4] CHEN X Y, XU Y, LI Q H, et al. Improving Ultrasonic-based Seamless Navigation for Indoor Mobile Robots Utilizing EKF and LS-SVM [J]. Measurement, 2016, 92: 243-251.

[5] TIAN Y, CHEN Z, YIN F L. Distributed Iterated Extended Kalman Filter for Speaker Tracking in Microphone Array Networks [J]. Applied Acoustics, 2017, 118: 50-57.

[6] YU H B, WANG G H, CAO Q, et al. A Fusion Based Particle Filter TBD Algorithm for Dim Targets[J]. Chinese Journal of Electronics, 2015, 24(3): 590-595.

[7] 朱國輝, 馮大政, 周延, 等. 一種線性校正TOA定位算法 [J]. 西安電子科技大學(xué)學(xué)報, 2015, 42(3): 22-25.

ZHU Guohui, FENG Dazheng, ZHOU Yan, et al. TOA Localization Algorithm Using the Linear-correction Technique [J]. Journal of Xidian University, 2015, 42(3): 22-25.

[8] 鄧兵, 孫正波, 楊樂, 等. 存在站址誤差時的線性校正TDOA定位算法[J]. 西安電子科技大學(xué)學(xué)報, 2017, 44(4): 252-258.

DENG Bing, SUN Zhengbo, YANG Le, et al. TDOA Localization with Linear-correction in the Presence of Sensor Position Errors[J]. Journal of Xidian University, 2017, 44(4): 252-258.

[9] ZHU G H, FENG D Z. Bi-iterative Method for Moving Source Localisation Using TDOA and FDOA Measurements[J]. Electronics Letters, 2015, 51(1): 8-10.

[10] HU D X, HUANG Z, CHEN X, et al. A Moving Source Localization Method Using TDOA, FDOA and Doppler Rate Measurements[J]. IEICE Transactions on Communications, 2016, E99B(3): 758-766.

[11] DENG B, SUN Z B, YANG L, et al. A Linear-correction TDOA and FDOA Method for Moving Source Localization[J]. IEICE Transactions on Fundamentals, 2017, E100A(4): 1066-1069.