修正的強(qiáng)跟蹤有限差分濾波算法

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

（長(zhǎng)安大學(xué) 陜西 西安 710064）

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

1 彈道目標(biāo)動(dòng)力學(xué)模型和雷達(dá)量測(cè)模型

1.1 目標(biāo)運(yùn)動(dòng)模型

所用模型為一再入彈道目標(biāo)的跟蹤問(wèn)題，參見(jiàn)文獻(xiàn)[3]。設(shè)k是一個(gè)非負(fù)整數(shù)，T是連續(xù)兩個(gè)雷達(dá)量測(cè)的時(shí)間間隔。為簡(jiǎn)單起見(jiàn)，假設(shè)地球是平的，通過(guò)以下離散時(shí)間非線性動(dòng)態(tài)系統(tǒng)狀態(tài)等式定義目標(biāo)運(yùn)動(dòng)

式中：xk為k時(shí)刻的目標(biāo)狀態(tài)向量；ψk為狀態(tài)轉(zhuǎn)移函數(shù)；β為目標(biāo)彈道系數(shù)；wk為過(guò)程噪聲。

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

其中，q是過(guò)程噪聲強(qiáng)度參數(shù)。過(guò)程噪聲說(shuō)明了模型中沒(méi)有考慮到的所有力以及模型與現(xiàn)實(shí)的偏離的影響。

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

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

f（xk，β）代表氣動(dòng)阻力的影響，其表達(dá)式為：

其中，ρ（y）是空氣密度函數(shù)，滿(mǎn)足：ρ（y）=1.21907·e－y/9146.64

1.2 雷達(dá)量測(cè)模型

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

其中，

另外，dk，hk分別是雷達(dá)量測(cè)轉(zhuǎn)換到笛卡爾坐標(biāo)系下的橫坐標(biāo)和縱坐標(biāo)；Hk是觀測(cè)矩陣；vk是笛卡爾坐標(biāo)系中的量測(cè)噪聲，與過(guò)程噪聲和初始狀態(tài)獨(dú)立，為零均值的高斯白噪聲，其協(xié)方差矩陣為Rk。

2 修正的強(qiáng)跟蹤濾波器

在原強(qiáng)跟蹤濾波器（STF）[8]基礎(chǔ)上，做出以下修正：

第一，增加狀態(tài)噪聲協(xié)方差的權(quán)重

在P（k+1|k）的遞推計(jì)算過(guò)程中，增加協(xié)方差 Q（k）的權(quán)重。這樣的話(huà)，P（k+1|k）可如下定義：

第二，為了使?fàn)顟B(tài)的估計(jì)更光滑，采用平方根函數(shù)的特性，那么漸消因子的計(jì)算被修正為

其中，

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

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

第三，保證估計(jì)誤差協(xié)方差矩陣的對(duì)稱(chēng)性和半正定性，改用以下的數(shù)值上更加魯棒的等式來(lái)遞推：

通過(guò)以上修正，強(qiáng)跟蹤濾波器的數(shù)值特性及濾波精度在一定程度上得以改進(jìn)。

3 強(qiáng)跟蹤有限差分?jǐn)U展卡爾曼濾波器

強(qiáng)跟蹤濾波具有較強(qiáng)的魯棒性，極強(qiáng)的跟蹤能力，適中的計(jì)算復(fù)雜性。在有限差分?jǐn)U展卡爾曼濾波算法中，引入以上修正的強(qiáng)跟蹤次優(yōu)漸消因子，從而得到本文的強(qiáng)跟蹤有限差分?jǐn)U展卡爾曼濾波算法（STFDEKF）。具體如下：

考慮如下的非線性系統(tǒng)狀態(tài)方程和量測(cè)方程：

其中，xk和yk分別表示k時(shí)刻的狀態(tài)向量和觀測(cè)向量；uk為控制輸入向量；wk和vk分別表示滿(mǎn)足某種分布的狀態(tài)噪聲和觀測(cè)噪聲；并且通常假定噪聲序列{wk}和{vk}是相互獨(dú)立的。狀態(tài)噪聲和觀測(cè)噪聲假定是不相關(guān)的白噪聲過(guò)程，均值和方差分別為：

則在FDEKF算法基礎(chǔ)上，在預(yù)測(cè)誤差協(xié)方差矩陣中引入上述修正的強(qiáng)跟蹤算法，得到STFDEKF算法如下：

1）引入以下Cholesky分解：

2）得到如下4個(gè)矩陣

3）計(jì)算狀態(tài)一步預(yù)測(cè)和預(yù)測(cè)誤差協(xié)方差矩陣

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

4）量測(cè)預(yù)測(cè)，增益，狀態(tài)估計(jì)及估計(jì)誤差協(xié)方差矩陣

接下來(lái)，通過(guò)仿真實(shí)驗(yàn)對(duì)提出的算法進(jìn)行性能分析。

4 實(shí)驗(yàn)及結(jié)果分析

4.1 實(shí)驗(yàn)場(chǎng)景

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

4.2 實(shí)驗(yàn)結(jié)果

比較的標(biāo)準(zhǔn)為各個(gè)算法的位置均方根誤差，速度均方根誤差，濾波可靠性，以及各算法的執(zhí)行時(shí)間等。

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

式中：n——狀態(tài)維數(shù)；M——Monte-Carlo仿真次數(shù)；xi——第i次Monte Carlo運(yùn)行時(shí)的真實(shí)狀態(tài)；x^i——第i次Monte Carlo運(yùn)行時(shí)的狀態(tài)估計(jì)；Pi——第i次Monte Carlo運(yùn)行時(shí)的狀態(tài)估計(jì)誤差協(xié)方差矩陣。

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

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

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

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

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

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

圖4 ANEES比較Fig.4 ANEEScomparision

在計(jì)算復(fù)雜性上，各種算法的500次Monte Carlo仿真的計(jì)算時(shí)間如表 1所示。從表 1中可見(jiàn)，F(xiàn)DEKF和EKF的計(jì)算量相當(dāng)，STFDEKF算法計(jì)算量適中，而UKF耗時(shí)最多。

5 結(jié)束語(yǔ)

在擴(kuò)展卡爾曼濾波器基礎(chǔ)上，結(jié)合有限差分及強(qiáng)跟蹤濾波的優(yōu)點(diǎn)，提出一種改進(jìn)的強(qiáng)跟蹤有限差分?jǐn)U展卡爾曼濾波算法用于再入階段的彈道目標(biāo)跟蹤。算法采用有限差分運(yùn)算進(jìn)行非線性函數(shù)的近似，避免了非線性函數(shù)的求導(dǎo)運(yùn)算。使得算法實(shí)現(xiàn)簡(jiǎn)單。另外，算法中引入了強(qiáng)跟蹤的次優(yōu)漸消因子，可以實(shí)時(shí)調(diào)整狀態(tài)預(yù)測(cè)誤差協(xié)方差矩陣。算法改進(jìn)了跟蹤精度，擴(kuò)大了應(yīng)用范圍，增強(qiáng)了濾波收斂性。仿真實(shí)驗(yàn)就估計(jì)精度，濾波可靠性及計(jì)算復(fù)雜性等方面對(duì)新算法與FDEKF、EKF和UKF算法進(jìn)行了比較，結(jié)果表明，STFDEKF算法計(jì)算量適中，而且精度較高，對(duì)于再入階段的彈道目標(biāo)跟蹤是個(gè)很好的選擇。

表1 500次Monte Carlo仿真的計(jì)算時(shí)間比較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，F(xiàn)rank 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.