鄧 兵, 孫正波, 賀 青
(盲信號(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á)式.
目標(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í)效性.
(12)
(13)
根據(jù)最小二乘原理,可以得到
(14)
其中,W為加權(quán)矩陣,W=(R)-1.
則經(jīng)線性校正后的目標(biāo)狀態(tài)最終估計(jì)及協(xié)方差分別為
至此可以得出偏差修正迭代擴(kuò)展卡爾曼濾波算法流程圖,如圖1所示.
圖1 偏差修正迭代擴(kuò)展卡爾曼濾波算法流程圖
圖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ì)均方根誤差
針對(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.