估計(jì)偏差修正擴(kuò)展卡爾曼濾波新算法

鄧 兵， 孫正波， 賀 青

(盲信號(hào)處理重點(diǎn)實(shí)驗(yàn)室，四川 成都 610041)

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

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

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

1 標(biāo)準(zhǔn)擴(kuò)展卡爾曼濾波算法基本流程

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

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

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

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

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

(3)

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

(6)

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

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

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

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

2 線性校正擴(kuò)展卡爾曼濾波算法

(12)

(13)

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

(14)

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

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

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

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

3 時(shí)頻差場(chǎng)景下偏差修正迭代擴(kuò)展卡爾曼濾波算法

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

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

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

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

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

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

5 總 結(jié)

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

[1] 蔚婧, 文珺, 李彩彩, 等. 輔助變量純方位目標(biāo)跟蹤算法[J]. 西安電子科技大學(xué)學(xué)報(bào), 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] 朱國(guó)輝, 馮大政, 周延, 等. 一種線性校正TOA定位算法 [J]. 西安電子科技大學(xué)學(xué)報(bào), 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] 鄧兵, 孫正波, 楊樂, 等. 存在站址誤差時(shí)的線性校正TDOA定位算法[J]. 西安電子科技大學(xué)學(xué)報(bào), 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.