一種改進的容積卡爾曼濾波目標跟蹤算法

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

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

0 引言

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

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

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

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

1 改進的強跟蹤算法

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

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

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

若考慮V0，k，由于V0，k＜C0，k+1，假定二者相等，式(2)變為

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

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

通過式(4)推導可得

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

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

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

2 改進的強跟蹤CKF算法

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

對于系統狀態及量測的一步預測值和，其求解如式(11)和(12)所示

式中:

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

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

根據式(18)則有

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

則可得到如下公式

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

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

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

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

最后，計算系統濾波增益Wk、狀態更新方程以及協方差更新方程，求出狀態估計以及Pk+1。

3 結果與分析

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

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

圖1 目標運動軌跡圖

為克服初始狀態對濾波精度的影響，統一假定濾波初始狀態為

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

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

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

由圖2可見，提出的ICKF跟蹤比CKF和UKF更穩定，跟蹤誤差更小，在目標非機動和弱機動時，ICKF保持了較好的跟蹤性能，與CKF相當，優于UKF，尤其是在狀態突變的時刻，ICKF跟蹤穩定，收斂速度更快。同時CKF跟蹤性能略優于UKF。

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

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

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

表1 狀態估計均方誤差

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

4 結束語

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

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

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

[3] 徐進寶，封錫盛，任申真.濾波在純方位跟蹤中的應用[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] 周東華，席裕庚，張鐘俊.非線性系統帶次優漸消因子的擴展卡爾曼濾波[J].控制與決策，1990，23(5):1－6.

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

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

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