修正的強跟蹤有限差分濾波算法

巫春玲，巨永鋒，段晨東，劉盼芝

（長安大學 陜西 西安 710064）

對于再入階段的彈道目標跟蹤問題，由于目標運動模型內在的非線性，需要使用非線性的濾波技術來估計目標的狀態。文獻[1-3]等使用擴展卡爾曼濾波（EKF）[3-4]，無味卡爾曼濾波（UKF）[5]，粒子濾波（PF）[3]等算法來跟蹤彈道目標。 EKF算法雖然簡單，但其需要計算Jacobia矩陣和/或Hessians矩陣；UKF效果較好，但對高維情況，也較復雜；PF算法精度很高，但計算量相當大；由于再入彈道目標速度很快，留待跟蹤的時間很短，所以應該采用計算量小又精度高的算法。本文提出一種強跟蹤有限差分近似[6-7]的濾波算法來跟蹤再入階段的彈道目標。強跟蹤濾波[8]具有較強的魯棒性，極強的跟蹤能力，適中的計算復雜性。在有限差分擴展卡爾曼濾波算法中，引入強跟蹤的次優漸消因子，從而得到本文的強跟蹤有限差分擴展卡爾曼濾波算法（STFDEKF），仿真實驗表明新算法是比較有效的針對再入彈道目標跟蹤的濾波算法。

1 彈道目標動力學模型和雷達量測模型

1.1 目標運動模型

所用模型為一再入彈道目標的跟蹤問題，參見文獻[3]。設k是一個非負整數，T是連續兩個雷達量測的時間間隔。為簡單起見，假設地球是平的，通過以下離散時間非線性動態系統狀態等式定義目標運動

式中：xk為k時刻的目標狀態向量；ψk為狀態轉移函數；β為目標彈道系數；wk為過程噪聲。

假定過程噪聲序列是零均值高斯白噪聲的，協方差矩陣給定如下：

其中，q是過程噪聲強度參數。過程噪聲說明了模型中沒有考慮到的所有力以及模型與現實的偏離的影響。

式（1）中，非線性函數 ψk（xk，β）給定如下：

其中，g=9.8 m/s2是引力加速度。矩陣Φ和G分別為：

f（xk，β）代表氣動阻力的影響，其表達式為：

其中，ρ（y）是空氣密度函數，滿足：ρ（y）=1.21907·e－y/9146.64

1.2 雷達量測模型

雷達觀測量有兩個，斜距r和俯仰角ε；斜距和俯仰角的量測誤差標準離差分別為 σr（偽距）和 σε（俯仰角）；雷達坐標始終為xR=0，yR=0；將雷達量測轉換到笛卡爾坐標系下為橫坐標d=r cosε和縱坐標h=r sinε，因此量測的線性等式為

其中，

另外，dk，hk分別是雷達量測轉換到笛卡爾坐標系下的橫坐標和縱坐標；Hk是觀測矩陣；vk是笛卡爾坐標系中的量測噪聲，與過程噪聲和初始狀態獨立，為零均值的高斯白噪聲，其協方差矩陣為Rk。

2 修正的強跟蹤濾波器

在原強跟蹤濾波器（STF）[8]基礎上，做出以下修正：

第一，增加狀態噪聲協方差的權重

在P（k+1|k）的遞推計算過程中，增加協方差 Q（k）的權重。這樣的話，P（k+1|k）可如下定義：

第二，為了使狀態的估計更光滑，采用平方根函數的特性，那么漸消因子的計算被修正為

其中，

其中，V0（k+1）是殘差方差矩陣，如下計算：

其中，r（k+1）為殘差，r（1）是初始殘差，tr[·]表示對矩陣求秩運算。ρ為遺忘因子，其范圍是0＜ρ≤1，引入遺忘因子的目的是為了進一步對過去的老數據漸消從而突出最新殘差向量的影響，進一步提高強跟蹤濾波器的快速跟蹤能力，通常選擇ρ=0.95。β是弱化因子，用來改進狀態估計的光滑性。這個值通常通過計算機仿真，憑經驗決定。

第三，保證估計誤差協方差矩陣的對稱性和半正定性，改用以下的數值上更加魯棒的等式來遞推：

通過以上修正，強跟蹤濾波器的數值特性及濾波精度在一定程度上得以改進。

3 強跟蹤有限差分擴展卡爾曼濾波器

強跟蹤濾波具有較強的魯棒性，極強的跟蹤能力，適中的計算復雜性。在有限差分擴展卡爾曼濾波算法中，引入以上修正的強跟蹤次優漸消因子，從而得到本文的強跟蹤有限差分擴展卡爾曼濾波算法（STFDEKF）。具體如下：

考慮如下的非線性系統狀態方程和量測方程：

其中，xk和yk分別表示k時刻的狀態向量和觀測向量；uk為控制輸入向量；wk和vk分別表示滿足某種分布的狀態噪聲和觀測噪聲；并且通常假定噪聲序列{wk}和{vk}是相互獨立的。狀態噪聲和觀測噪聲假定是不相關的白噪聲過程，均值和方差分別為：

則在FDEKF算法基礎上，在預測誤差協方差矩陣中引入上述修正的強跟蹤算法，得到STFDEKF算法如下：

1）引入以下Cholesky分解：

2）得到如下4個矩陣

3）計算狀態一步預測和預測誤差協方差矩陣

λ（k+1）為前述被修正了的強跟蹤的漸消因子。

4）量測預測，增益，狀態估計及估計誤差協方差矩陣

接下來，通過仿真實驗對提出的算法進行性能分析。

4 實驗及結果分析

4.1 實驗場景

這一部分，通過Monte Carlo仿真比較了3種算法的性能。仿真場景設為：掃描周期T=2 s，過程噪聲強度為q=1，雷達量測的偽距誤差標準離差為σr=200 m，俯仰角誤差標準離差為 σε=0.05 rad，目標彈道系數設為已知，β=40 000 kgm-1s-2。初始狀態x0為高斯隨機向量，其均值和方差已知，均值為m0=[232000 2290cos（190°） 88000 2290sin（190°）]，其中，距離的單位為m，速度的單位為m/s；方差為∑=diag（[1000220210002202]），仿真目標跟蹤步長為 120步，運行 500次 Monte Carlo仿真，并假定目標檢測概率為1，虛警概率為0。

4.2 實驗結果

比較的標準為各個算法的位置均方根誤差，速度均方根誤差，濾波可靠性，以及各算法的執行時間等。

用平均標準化估計誤差平方 （Average Normalized Estimation Error Square，ANEES）比較 3種算法的濾波可靠性。ANEES的定義如下[3]：

式中：n——狀態維數；M——Monte-Carlo仿真次數；xi——第i次Monte Carlo運行時的真實狀態；x^i——第i次Monte Carlo運行時的狀態估計；Pi——第i次Monte Carlo運行時的狀態估計誤差協方差矩陣。

ANEES曲線越接近于1，表示濾波可靠性越高。

下面，將STFDEKF算法分別與FDEKF、EKF和UKF算法相比較，比較的準則是均方根誤差、濾波可靠性及計算復雜性。然后在圖1至圖4中給出各算法的位置和速度均方根誤差及濾波可靠性比較，最后在表1中給出各算法的計算復雜性比較。

圖1 位置均方根誤差Fig.1 Positon RMSE

在圖1至圖4中，圖1是對圖的尾部的放大，以便可以清楚地看到各個算法的濾波誤差曲線。通過觀察，可以看出，FDEKF算法無論是在位置RMSE還是速度RMSE上，均很明顯的比EKF的小，其跟蹤精度明顯高于EKF的精度。另外，其濾波可靠性也明顯提高。而STFDEKF的估計精度與UKF很接近，而且其可靠性明顯高于FDEKF和EKF，與UKF很接近。

圖2 位置均方根誤差（對圖尾部的放大）Fig.2 Positon RMSE（enlarge Fig.1）

圖3 速度均方根誤差Fig.3 Velocity RMSE

圖4 ANEES比較Fig.4 ANEEScomparision

在計算復雜性上，各種算法的500次Monte Carlo仿真的計算時間如表 1所示。從表 1中可見，FDEKF和EKF的計算量相當，STFDEKF算法計算量適中，而UKF耗時最多。

5 結束語

在擴展卡爾曼濾波器基礎上，結合有限差分及強跟蹤濾波的優點，提出一種改進的強跟蹤有限差分擴展卡爾曼濾波算法用于再入階段的彈道目標跟蹤。算法采用有限差分運算進行非線性函數的近似，避免了非線性函數的求導運算。使得算法實現簡單。另外，算法中引入了強跟蹤的次優漸消因子，可以實時調整狀態預測誤差協方差矩陣。算法改進了跟蹤精度，擴大了應用范圍，增強了濾波收斂性。仿真實驗就估計精度，濾波可靠性及計算復雜性等方面對新算法與FDEKF、EKF和UKF算法進行了比較，結果表明，STFDEKF算法計算量適中，而且精度較高，對于再入階段的彈道目標跟蹤是個很好的選擇。

表1 500次Monte Carlo仿真的計算時間比較Tab.1 Computation Time Comparison of 500 Monte Carlo simulation

[1]Bar－Shalom Y，Li X R，Kirubarajan T.Estimation with Applications to Tracking and Navigation:Theory， Algorithms，and Software[M].Wiley， New York，2001.

[2]Li X R，Jilkov V P.A Survey of Maneuvering Target Tracking.Part II:Ballistic Target Models[C]//In Proc.2001 SPIE Conf.on Signal and Data Processing of Small Targets，San Diego， CA， USA， July－Aug.2001，4473:559－581.

[3]Farina， D.Benvenuti， B.Ristic.Tracking a ballistic target:comparison of several nonlinear filters[J].IEEE Trans.Aerospace andElectronic Systems，2002， AES－38:854－867.

[4]Costa P.Adaptive model architecture and extended Kalman－Bucy filters[J].IEEE Trans.Aerospace and Electronic Systems，1994 AES－30:525－533.

[5]Julier S J，Uhlmann J.Unscented Filtering and Nonlinear Estimation.Proceedings of the IEEE，2004，92（3）:401－422.

[6]Schei T S.A Finite－Difference Method for Linearization in Nonlinear Estimation Algorithms[J].Automatica，1997，33（11）:2053－2058.

[7]R.van der Merwe，E.A.Wan.Efficient derivative －free Kalman filters for online learning[C]//in Proc.Eur.Symp.on Artificial Neural Networks （ESANN）， Bruges， Belgium ～Apr.2001.

[8]Zhou D H，Frank P M.Strong tracking Kalman filtering of nonlinear time－varying stochastic systems with coloured noise:Application to parameter estimation and empirical robustness analysis[J].Int.J.Control，1996，65（5）:295－307.