一種改進的容積卡爾曼濾波目標(biāo)跟蹤算法

張曉春，馬新斐，劉志宏，杜國宏，楊 耀

(1.成都信息工程大學(xué)電子工程學(xué)院，四川 成都610225;2.重慶光電技術(shù)研究所，重慶400060)

0 引言

運動目標(biāo)跟蹤是模式識別、圖像處理、計算機視覺等領(lǐng)域的重要課題目標(biāo)跟蹤算法是紅外圖像末制導(dǎo)的核心算法，核心算法精確與否直接關(guān)系到導(dǎo)彈的命中精度，在軍事及民用領(lǐng)域都有很重要的研究價值和實用意義。目前主要包括粒子濾波、MeanShiift模型、CamShift模型、魔板匹配跟蹤、感知哈希算法、MAD和ABS等，跟蹤一方面從像素值之間關(guān)系出發(fā)進行處理，另一方面可通過顏色相關(guān)圖統(tǒng)計進行實現(xiàn)。在非線性處理算法中，擴展卡爾曼濾波(EKF)將非線性系統(tǒng)線性化是通過截斷泰勒級數(shù)展開式實現(xiàn)的，因此對于非線性程度較高的系統(tǒng)，EKF的濾波精度受到限制。無跡卡爾曼濾波(UKF)利用UT變換后的采樣點的方式近似非線性函數(shù)的概率分布，對非線性程度較高的系統(tǒng)也有很強的適應(yīng)性，但是隨著系統(tǒng)狀態(tài)向量維數(shù)的增加，特別是當(dāng)維數(shù)時，系統(tǒng)狀態(tài)協(xié)方差的半正定性很難保證，因此會UKF對系統(tǒng)狀態(tài)的估計精度造成不良影響。針對高維情況下非線性濾波估計，容積卡爾曼濾波(cubature kalman filter，CKF)應(yīng)運而生，CKF 是 Arasaratnam 等[1]于2009年提出的一種新型非線性濾波算法，算法使用基于球面－徑向積分準(zhǔn)則的數(shù)值積分方法計算非線性系統(tǒng)非線性變換的隨機變量后的均值和協(xié)方差，結(jié)構(gòu)簡單，實現(xiàn)方便，且濾波精度高。

對于非線性濾波算法而言，其濾波精度都是建立在模型準(zhǔn)確的基礎(chǔ)之上，包括EKF、UKF以及CKF，當(dāng)模型不準(zhǔn)確時，則濾波算法對該系統(tǒng)的非線性估計精度下降，甚至導(dǎo)致濾波發(fā)散。但在實際應(yīng)用當(dāng)中，非線性系統(tǒng)復(fù)雜程度不同，所建立的模型往往不能準(zhǔn)確描述系統(tǒng)狀態(tài)。為解決在系統(tǒng)模型不準(zhǔn)確時常規(guī)濾波算法估計精度不高的問題，專家學(xué)者提出很多自適應(yīng)濾波算法以及魯棒濾波算法，取得不錯的濾波效果[2－3]，其中強跟蹤濾波算法(strong tracking filter，STF)對狀態(tài)突變有較強自適應(yīng)跟蹤能力，在各領(lǐng)域也得到廣泛應(yīng)用[4]。強跟蹤算法最初是在線性系統(tǒng)中提出的，周東華等[5－6]將該算法推廣到EKF中，王小旭等[7]將其推廣到UKF及CDKF當(dāng)中，都取得較好的效果。

目前強跟蹤擴展卡爾曼濾波器能夠適用在非線性系統(tǒng)，但是EKF的算法框架致使該算法在強非線性系統(tǒng)中濾波精度不高，甚至引起發(fā)散，且非線性系統(tǒng)需要計算其雅克比矩陣，實現(xiàn)復(fù)雜。CKF算法采用球面-徑向容積準(zhǔn)則，具有泰勒級數(shù)展開的三階精度，而且強跟蹤算法能夠克服CKF對于狀態(tài)突變自適應(yīng)跟蹤能力差的問題，因此，建立基于CKF的強跟蹤濾波算法具有重要的現(xiàn)實意義。

提出一種改進的強跟蹤算法，針對強機動目標(biāo)的狀態(tài)估計問題，在分析擴展卡爾曼濾波(EKF)以及無跡卡爾曼濾波(UKF)對非線性、高維系統(tǒng)狀態(tài)估計性能不足的前提下，使用容積卡爾曼濾波(cubature kalman filter，CKF)解決非線性、高維系統(tǒng)對高機動目標(biāo)的狀態(tài)估計問題。

1 改進的強跟蹤算法

強跟蹤濾波對狀態(tài)估計過于依賴量測值，雖然理論上可以得到最優(yōu)估計，但由于殘差協(xié)方差矩陣對真實的殘差方差近似不夠準(zhǔn)確，致使對判定濾波異常的閾值設(shè)置較小，漸消因子的產(chǎn)生出現(xiàn)異常，使得濾波增益達不到最優(yōu)，且其估計值也不夠平滑。

文獻[8]將判定濾波異常的閾值調(diào)整至 βC0，k+1，當(dāng)β=3時，若不考慮V0，k的影響，如式(1)所示

即判定異常的概率為1.5%，式中:C0，k+1為殘差方差V0，k+1的理論值，即式(2)所示

若考慮V0，k，由于V0，k＜C0，k+1，假定二者相等，式(2)變?yōu)?/p>

由式(3)可知，將閾值提高到3C0，k+1時，濾波器在正常濾波的情況對濾波發(fā)散進行誤判的概率為1.5%～2.6%，大大降低了產(chǎn)生漸消因子的概率，使強跟蹤的估計精度進一步提高。

因此，提高閾值后，殘差方差為

通過式(4)推導(dǎo)可得

由式(3)～(5)可知，改進的強跟蹤算法(ISTF)的漸消因子取法如式(6)～(9)所示

其中，弱化因子與量測向量的維數(shù)n的關(guān)系表示如式(10)所示

由式(10)可知，弱化因子β取值越小，強跟蹤算法的魯棒性越強，同時誤判概率增大，估計平滑度降低;取值越大，則對濾波發(fā)散的誤判概率越小，同時系統(tǒng)狀態(tài)估計的魯棒性減弱，甚至使該算法退化為一般濾波器。

2 改進的強跟蹤CKF算法

在計算強跟蹤濾波算法漸消因子時，需要計算系統(tǒng)方程和量測方程的雅可比矩陣，但是CKF算法中，采用的是近似非線性函數(shù)的高斯權(quán)值積分，在濾波方程中不存在Fk和Hk+1，而且對于某些不可微的非線性系統(tǒng)，其雅克比矩陣也難以求取，因此需要加以改進，建立強跟蹤CKF算法。

對于系統(tǒng)狀態(tài)及量測的一步預(yù)測值和，其求解如式(11)和(12)所示

式中:

為求取漸消因子，使C0，k+1與經(jīng)Pk+1|k求得的量測方差矩陣相等，即得式(16)為

當(dāng)量測函數(shù)h(xk+1)為非線性時，經(jīng)其傳遞后的為關(guān)于λ 的非線性函數(shù)，因此漸消因子不易直k+1接求得，因此，現(xiàn)將近似為C0，k+1的線性函數(shù)，可得如下形式

根據(jù)式(18)則有

令式(19)中，V0，k+1=βC0，k+1，即可得

則可得到如下公式

根據(jù)式(21)～(24)，可得強跟蹤CKF算法的濾波流程為:

首先，求取系統(tǒng)狀態(tài)向量相應(yīng)的容積點，并計算其經(jīng)f(Xk+1)傳播后的容積點，進而計算狀態(tài)預(yù)測估計以及預(yù)測誤差協(xié)方差，隨后求解量測向量相應(yīng)的容積點，并計算其經(jīng)h(Xk+1)傳播后的容積點，進而計算量測預(yù)測估計、新息協(xié)方差以及交叉協(xié)方差。

其次，由式(19)～(24)計算出強跟蹤漸消因子λk+1，結(jié)合式(15)求出加入漸消因子后的預(yù)測誤差協(xié)方差Pk+1|k，如式(25)所示

再次，由加入漸消因子后的預(yù)測誤差協(xié)方差Pk+1|k，求解量測向量相應(yīng)的容積點，并計算其經(jīng)h(Xk+1)傳播后的容積點，進而計算量測預(yù)測估計、新息協(xié)方差Pzz，k+1|k以及交叉協(xié)方差Pxz，k+1|k。

最后，計算系統(tǒng)濾波增益Wk、狀態(tài)更新方程以及協(xié)方差更新方程，求出狀態(tài)估計以及Pk+1。

3 結(jié)果與分析

通過對坦克車輛的階躍機動和圓機動進行仿真驗證，并與CKF和UKF算法進行比較，驗證機動目標(biāo)跟蹤算法的有效性。首先設(shè)定直角坐標(biāo)系二維平面，X軸和Y軸上的量測噪聲均是均值為0、標(biāo)準(zhǔn)偏差為1 m的高斯序列，掃描周期為0.25 s，機動頻率為0.05。

統(tǒng)計模型算法中，最大機動加速度設(shè)為amax=5 m/s2，仿真次數(shù)為100次。目標(biāo)運動軌跡設(shè)定為:目標(biāo)初始在X軸做勻速運動，初速8 m/s，隨后以加速度2 m/s2減速為0;在第40～52個掃描區(qū)間，以加速度4 m/s2沿Y軸先加速至12 m/s再勻減速為零;第64～72個掃描區(qū)間沿X軸先以加速度4 m/s2加速至8 m/s，再以加速度2 m/s2加速至12m/s，隨后保持勻速運動;第100～138個掃描區(qū)間，以向心加速度4 m/s2做180°勻速圓機動;第138～162個掃描區(qū)間，沿X軸保持勻速運動;第162～170個掃描區(qū)間，在X軸上保持勻速，Y軸上以加速度4 m/s2加速至8 m/s;第170～178個掃描區(qū)間，在X軸上以加速度4 m/s2加速至20 m/s，Y軸上以加速度4 m/s2減速至0;隨后沿X軸以速度20 m/s保持勻速運動。其運動軌跡如圖1所示。

圖1 目標(biāo)運動軌跡圖

為克服初始狀態(tài)對濾波精度的影響，統(tǒng)一假定濾波初始狀態(tài)為

這里，系統(tǒng)噪聲wk、過程噪聲vk符合高斯分布，且wk～N(0，Qk)，vk～N(0，Rk)，初始狀態(tài)估計取為0，并且初始協(xié)方差矩陣取為P0|0=diag{10，20，0.04，10，2，0.04}。

為公平比較各非線性濾波算法，進行100次獨立仿真取其平均。將文中改進的強跟蹤容積卡爾曼濾波算法(ISTCKF)與CKF、UKF進行比較。圖2表示3種濾波算法的跟蹤軌跡對比。

圖2 3種濾波算法的跟蹤軌跡對比

由圖2可見，提出的ICKF跟蹤比CKF和UKF更穩(wěn)定，跟蹤誤差更小，在目標(biāo)非機動和弱機動時，ICKF保持了較好的跟蹤性能，與CKF相當(dāng)，優(yōu)于UKF，尤其是在狀態(tài)突變的時刻，ICKF跟蹤穩(wěn)定，收斂速度更快。同時CKF跟蹤性能略優(yōu)于UKF。

為定量比較各非線性濾波算法性能，定義均方根誤差(RMSE)為

式(26)中:的濾波估計值。

表1列出了各濾波算法x，y，和軸的狀態(tài)估計RMSE均值。

表1 狀態(tài)估計均方誤差

通過表1可知，文中的ICKF算法在各坐標(biāo)方向的濾波誤差明顯小于其他非線性濾波器，尤其表現(xiàn)在目標(biāo)狀態(tài)突變處。CKF的均方誤差性能要好于UKF。同時，同時也發(fā)現(xiàn)ICKF對運動目標(biāo)，尤其是階躍機動目標(biāo)位置和速度具有最強的跟蹤能力，能自適應(yīng)地跟蹤狀態(tài)較大變化，CKF的跟蹤能力與UKF相差不大，CKF略優(yōu)于UKF。

4 結(jié)束語

針對強機動目標(biāo)的狀態(tài)估計問題，在分析擴展卡爾曼濾波(EKF)以及無跡卡爾曼濾波(UKF)對非線性、高維系統(tǒng)狀態(tài)估計性能不足的前提下，使用容積卡爾曼濾波(Cubature Kalman Filter，CKF)解決非線性、高維系統(tǒng)對高機動目標(biāo)的狀態(tài)估計問題。針對CKF對于狀態(tài)突變自適應(yīng)跟蹤能力差的問題，采用改進的強跟蹤算法與CKF結(jié)合形成強跟蹤CKF算法，仿真結(jié)果表明，該算法能很好地適應(yīng)目標(biāo)機動的變化，尤其對機動狀態(tài)突變有良好的跟蹤性能，具有一定的實際應(yīng)用價值。

[1] Arasaratnam I，Haykin S.Cubature Kalman Filters[J].IEEE Trans.on Automatic Control，2009，54(6):1254－1269.

[2] 楊廷梧，黨懷義，蘇明.基于信息偏差的觀測奇異值自適應(yīng)濾波算法[J].測控技術(shù)，2010，29(10):16－19.

[3] 徐進寶，封錫盛，任申真.濾波在純方位跟蹤中的應(yīng)用[J].計算機仿真，2012，29(7):388－392.

[4] Liu Z B，Shi Z Y，Xu W L.Multi－ component Information－equalized Extended Strong Tracking Filter for Global Localization:A Scheme Robust to Kidnapping and Symmetrical Environments[J].Robotics and Autonmous Systems，2010，58(5):465－487.

[5] 周東華，席裕庚，張鐘俊.非線性系統(tǒng)帶次優(yōu)漸消因子的擴展卡爾曼濾波[J].控制與決策，1990，23(5):1－6.

[6] 周東華，席裕庚，張鐘俊.一種帶多重次優(yōu)漸消因子的擴展卡爾曼濾波[J].自動化學(xué)報，1991，17(6):689－695.

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

[8] 葛磊.容積卡爾曼濾波算法研究及其在導(dǎo)航中的應(yīng)用[D].哈爾濱:哈爾濱工程大學(xué)博士學(xué)位論文，2013.