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

章 靜，王社陽，魏喜慶

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

0 引言

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

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

1 純方位測量問題描述

本文研究在二維平面內(nèi)對目標的無源跟蹤，數(shù)學表達式為

假設觀測雷達位于坐標原點，則觀測值信息

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

2 容積濾波

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

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

a）時間預測

計算容積點

容積點傳播

估計預測均值和協(xié)方差陣

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

b）量測更新

計算容積點

容積點傳播

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

d）計算量測更新

3 迭代容積濾波

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

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

定義第j步迭代估計時的殘差值

第j＋1步迭代的殘差值

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

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

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

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

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

代入式（25）

定義偽測量矩陣

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

定義迭代卡爾曼增益

則式（29）可簡化為

協(xié)方差矩陣

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

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

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

4 迭代容積濾波在無源跟蹤中應用

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

x1＝ ［0m 0m／s 0.4m －0.05m／s 0rad］T，相應的協(xié)方差陣P0對角線元素分別為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分別為

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

圖1 飛行軌跡Fig.1 RMSE trajectory

位置、速度與角速度的RTAMSE值見表1。由表可知：CKF的精度略高于UKF，而EKF的跟蹤結(jié)果發(fā)散，無法實現(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é)束語

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

［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］ 周 荻，胡振坤，胡恒章.自適應推廣Kalman濾波應用于導彈的被動制導問題［J］.宇航學報，1997，18（4）：31－36.