估計偏差修正擴展卡爾曼濾波新算法

鄧 兵， 孫正波， 賀 青

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

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

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

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

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

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

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

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

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

步驟2 計算動態系統狀態方程的雅可比矩陣:

(3)

步驟3 計算狀態估計和估計誤差協方差:

(6)

步驟5 計算量測預測、新息協方差矩陣和卡爾曼增益:

步驟6 計算狀態估計和估計誤差協方差:

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

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

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

(12)

(13)

根據最小二乘原理，可以得到

(14)

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

則經線性校正后的目標狀態最終估計及協方差分別為

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

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

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

4 仿真結果及分析

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

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

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

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

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

5 總 結

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

[1] 蔚婧, 文珺, 李彩彩, 等. 輔助變量純方位目標跟蹤算法[J]. 西安電子科技大學學報, 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]. 西安電子科技大學學報, 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]. 西安電子科技大學學報, 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.