基于迭代容積濾波的無源跟蹤算法

章 靜，王社陽，魏喜慶

（1.上海航天技術(shù)研究院，上海 201109；2.上海機(jī)電工程研究所，上海 201109）

0 引言

導(dǎo)彈對(duì)目標(biāo)的跟蹤時(shí)，傳統(tǒng)主動(dòng)雷達(dá)制導(dǎo)方法易受敵方電子干擾，當(dāng)應(yīng)用紅外等被動(dòng)傳感器對(duì)目標(biāo)進(jìn)行跟蹤，可較好地解決抗干擾問題。隨著被動(dòng)測(cè)量技術(shù)在跟蹤和制導(dǎo)中的廣泛應(yīng)用，在僅有角度和距離的無源測(cè)量條件下對(duì)目標(biāo)的狀態(tài)進(jìn)行估計(jì)引起了廣泛關(guān)注。數(shù)學(xué)上，可在直角坐標(biāo)系中用一個(gè)線性動(dòng)態(tài)模型和一個(gè)非線性觀測(cè)模型描述該問題，也可在極坐標(biāo)系中用一個(gè)非線性動(dòng)態(tài)模型和一個(gè)線性觀測(cè)模型描述［1－4］。被動(dòng)傳感器獲取目標(biāo)的方位角和距離信息，當(dāng)跟蹤坐標(biāo)系為直角坐標(biāo)系時(shí)，因測(cè)量方程為非線性，故無法直接采用線性濾波方法。一種最直接的方法是將非線性模型進(jìn)行近似線性化，對(duì)線性化的系統(tǒng)采用卡爾曼濾波框架，即擴(kuò)展卡爾曼濾波（EKF）［5］。但經(jīng)一階線性化近似后，EKF忽略了模型部分非線性特性，當(dāng)初始誤差較大時(shí)，存在估計(jì)效果急劇下降和濾波收斂速度緩慢的問題。為進(jìn)一步改善非線性系統(tǒng)估計(jì)性能，文獻(xiàn)［6］根據(jù)對(duì)隨機(jī)變量的概率分布進(jìn)行逼近較對(duì)非線性函數(shù)進(jìn)行逼近更容易的方法，提出了無跡卡爾曼濾波（UKF）［6］。UKF通過經(jīng)無跡變換后的采樣點(diǎn)集逼近非線性函數(shù)概率分布，并繼承了卡爾曼濾波框架，其非線性估計(jì)性能優(yōu)于EKF。UKF無需計(jì)算非線性系統(tǒng)的雅可比矩陣，因其良好適應(yīng)性受到了廣泛關(guān)注。用高斯埃爾米特積分公式得到的高斯埃爾米特濾波器，具有較UKF更好的估計(jì)精度和數(shù)值穩(wěn)定性，但其計(jì)算量隨狀態(tài)維數(shù)增加而呈指數(shù)級(jí)增長，龐大的運(yùn)算量導(dǎo)致其只能適于低維系統(tǒng)。文獻(xiàn)［7］通過三階容積法則的數(shù)值積分方法，近似高斯加權(quán)積分推導(dǎo)出了容積濾波（CKF），認(rèn)為CKF是UKF在特殊參數(shù)選取情形下的一個(gè)特例，但具有更好的濾波精度和數(shù)值穩(wěn)定性。

為此，本文提出了一種二維目標(biāo)的相對(duì)狀態(tài)估計(jì)算法。

1 純方位測(cè)量問題描述

本文研究在二維平面內(nèi)對(duì)目標(biāo)的無源跟蹤，數(shù)學(xué)表達(dá)式為

假設(shè)觀測(cè)雷達(dá)位于坐標(biāo)原點(diǎn)，則觀測(cè)值信息

式中：xk，θk分別為距離和角度測(cè)量值，vk∈N（0，Rk）為測(cè)量噪聲。該測(cè)量模型具有典型的非線性特性［8］。

2 容積濾波

具加性噪聲的非線性系統(tǒng)狀態(tài)方程和量測(cè)方程可表示為

非線性高斯濾波的核心是求解多變量非線性函數(shù)與高斯密度函數(shù)乘積的積分。文獻(xiàn)［7］通過三階容積積分法則，利用2n個(gè)容積點(diǎn)加權(quán)求和替代加權(quán)高斯積分。對(duì)函數(shù)f（x）的加權(quán)高斯積分

a）時(shí)間預(yù)測(cè)

計(jì)算容積點(diǎn)

容積點(diǎn)傳播

估計(jì)預(yù)測(cè)均值和協(xié)方差陣

式中：i＝1，…，2n。

b）量測(cè)更新

計(jì)算容積點(diǎn)

容積點(diǎn)傳播

c）量測(cè)預(yù)測(cè)值、新息方差和協(xié)方差矩陣計(jì)算

d）計(jì)算量測(cè)更新

3 迭代容積濾波

無源測(cè)量模型具強(qiáng)非線性特性，為獲得良好的濾波效果，本文用LM算法改進(jìn)CKF的量測(cè)更新過程。與加權(quán)最小二乘算法類似，定義時(shí)刻k的代價(jià)函數(shù)

式中：為狀態(tài)估值。設(shè)時(shí)刻k第j步的狀態(tài)迭代預(yù)測(cè)值已知，則未知狀態(tài)與當(dāng)前狀態(tài)和修正值Δxk，j的關(guān)系可表示為

定義第j步迭代估計(jì)時(shí)的殘差值

第j＋1步迭代的殘差值

將式（22）代入式（17），代價(jià)函數(shù)變?yōu)?/p>

確定修正估計(jì)值Δxk，j的原則是其滿足代價(jià)函數(shù)Jk，j＋1最小。最小化Jk，j＋1的問題與求解加權(quán)最小二乘問題等價(jià)，易求得

式中：Πk，j為方差陣，且Πk，j＝ （（Hk，j）T（Rk）－1＋ηIn×n）－1。此處：η為加權(quán)系數(shù)；In×n為n維單位陣。當(dāng)η趨于0時(shí)，式（24）等價(jià)于高斯最小二乘微分修正算法（GLSDC）；當(dāng)η趨于無窮時(shí)，式（24）等價(jià)于最速下降法。

LM算法克服了高斯最小二乘微分修正算法在先驗(yàn)估值與真值相差較大時(shí)收斂速度較慢的不足，以及在估值接近最優(yōu)值時(shí)最速下降法收斂速度變慢的缺點(diǎn)?；贚M算法的改進(jìn)CKF算法在進(jìn)一步提高了CKF估計(jì)精度的同時(shí)，具有較快的收斂速度。將狀態(tài)量測(cè)更新過程用LM算法替代為

式中：取為計(jì)算得到的預(yù)測(cè)狀態(tài)值。由于線性系統(tǒng)的互協(xié)方差陣的值為

代入式（25）

定義偽測(cè)量矩陣

式中：Pk，0＝Pk；Pxz，k，j＝Pxz，k［14］。則狀態(tài)量測(cè)更新

定義迭代卡爾曼增益

則式（29）可簡化為

協(xié)方差矩陣

當(dāng)代價(jià)函數(shù)Jk，j隨迭代次數(shù)的增加而變化較小時(shí)，滿足迭代終止條件

按以下準(zhǔn)則選取LM算法中的η：η初值取一個(gè)較大的值（通常取 （Hk，j）T（Rk）－1Hk，j范數(shù)10倍以上）。將式（31）迭代計(jì)算的狀態(tài)估計(jì)值代入式（23）中，若Jk，j＋1≥Jk，j，則舍去這次的狀態(tài)迭代值并將η變?yōu)?η；若Jk，j＋1＜Jk，j，則保留狀態(tài)估計(jì)值并將η變?yōu)棣牵?。

適當(dāng)選取式（33）迭代終止條件的系數(shù)ε，本算法能在達(dá)到良好估計(jì)精度時(shí)及時(shí)跳出迭代，一定程度保證了算法的實(shí)時(shí)性。

4 迭代容積濾波在無源跟蹤中應(yīng)用

目標(biāo)進(jìn)行無源跟蹤仿真時(shí)，設(shè)仿真步長Δt為1s；Rk＝diag［100m210rad2］；仿真時(shí)長T＝100s；狀態(tài)初值及其估值分別為

x1＝ ［0m 0m／s 0.4m －0.05m／s 0rad］T，相應(yīng)的協(xié)方差陣P0對(duì)角線元素分別為100m2，10m2／s2，100mrad2／s2。此處：x01＝0.05m；x02＝0.01m／s；x03＝0.7m；x04＝－0.055m／s；x05＝0rad。定義橫向位置均方根誤差γpx、縱向位置均方根誤差γpy、位置均方根誤差γpos、速度均方根誤差γvel和角速度均方根誤差γtum－rate分別為

進(jìn)行了100次蒙特卡洛仿真，目標(biāo)的跟蹤軌跡與真實(shí)飛行軌跡如圖1所示。由圖可知：因測(cè)量信息較少導(dǎo)致EKF的精度較低，在時(shí)間大于50s后出現(xiàn)了嚴(yán)重的濾波發(fā)散；UKF，CKF的估計(jì)精度較高，估計(jì)曲線與真實(shí)軌跡幾乎重合。因EKF估計(jì)結(jié)果發(fā)散，UKF，CKF的均方根誤差分別如圖2～4所示。由圖可知：CKF，UKF的速度和角速度估計(jì)精度非常相似，CKF的位置估計(jì)精度略優(yōu)于UKF。

圖1 飛行軌跡Fig.1 RMSE trajectory

位置、速度與角速度的RTAMSE值見表1。由表可知：CKF的精度略高于UKF，而EKF的跟蹤結(jié)果發(fā)散，無法實(shí)現(xiàn)有效。

表1 RTAMSE表Tab.1 RTAMSE table

圖2 位置均方根誤差Fig.2 RMSE in position

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

圖4 角速度均方根誤差Fig.4 RMSE in turn rate

5 結(jié)束語

本文對(duì)僅有角度和距離測(cè)量條件下的一種無源目標(biāo)跟蹤算法進(jìn)行了研究。采用容積濾波作為跟蹤算法，提出基于龍貝格－馬爾塔（LM）算法改進(jìn)的迭代容積無源跟蹤算法以提高跟蹤精度。仿真結(jié)果驗(yàn)證了迭代容積濾波算法有良好的跟蹤精度。

［1］ BALAKRISHNAN S N.Extention to modified polar coordinates and application with passive measurements［J］.Journal of Guidance，Control，and Dynamics，1989，12（6）：906－912.

［2］ SONG，T L SPEYER J.A stochastic analysis of a modified gain extended Kalman filter with applications to estimation with bearings only measurements［J］.IEEE Transactions on Automatic Control，1985，30（10）：940－949.

［3］ SONG T L.Observability of target tracking with bearing－only measurements［J］.IEEE Trans on Aerosp Electron Syst，1996，32（4）：1468－1471.

［4］ CADRE J P，JAUFFRET C.Discrete－time observability and estimability analysis for bearings－only target motion analysis［J］.IEEE Trans on Aerosp Elec－tron Syst，1997，33（1）：178－201.

［5］ 孫仲康，郭福成，馮道旺，等.單站無源定位跟蹤技術(shù)［M］.北京：國防工業(yè)出版社，2008：186－199.

［6］ CRASSIDIS J，JUNKINS J.Optimal estimation of dynamic systems［M］.Boca Raton：Chapman ＆Hall，2004：419－433.

［7］ ARASARATNAM I，HAYKIN S.Cubature kalman filters［J］.IEEE Transactions on Automatic Control，2009，54（6）：1254－1269.

［8］ 周 荻，胡振坤，胡恒章.自適應(yīng)推廣Kalman濾波應(yīng)用于導(dǎo)彈的被動(dòng)制導(dǎo)問題［J］.宇航學(xué)報(bào)，1997，18（4）：31－36.