快速邊緣粒子濾波在無序量測問題中的應用*

袁 丁，胡建旺，吉 兵，顧 峰

（1.軍械工程學院 信息工程系，河北 石家莊 050003;2.73903部隊，福建 廈門 361000）

0 引言

在目標跟蹤系統中，由于所用傳感器具有不同的采樣速率，以及傳輸中延遲的不同，測量同步很難被保證，從而產生了異步融合問題。更進一步，會出現來自同一目標的較早的量測在較晚的量測之后到達融合中心，即無序量測（out-of-sequence measurement，OOSM）現象［1］。傳統的卡爾曼濾波（KF）算法不能直接處理這種“負時間量測更新”問題，需要研究相應的濾波算法。

目前，最適合對無序量測（OOSM）作實時處理的濾波思想是直接更新法。直接更新法是直接利用OOSM和已存儲的目標狀態充分估計量，對當前狀態估計直接進行再更新，以得到新的狀態估計及其估計誤差協方差矩陣［2］。直接更新法存儲量和計算量較小，且輸出沒有滯后，在這一濾波思想下，學者提出了諸如A1，B1，AA1，FPFD等線性系統下的最優或次優濾波算法。

對于弱非線性高斯系統，可以通過泰勒級數展開將非線性問題轉化為線性，但在應用擴展卡爾曼濾波（EKF）時，線性化過程會導致較大的濾波誤差［2］。文獻［3］提出了基于無跡卡爾曼濾波（UKF）的OOSM算法，通過UT避免了求解非線性量測方程的雅可比矩陣或海塞矩陣。對于強非線性高斯系統，Ortan推導得到了加入OOSM后的后驗密度，提出了OOSM 粒子濾波（OOSM-PF）算法［4］，此后又有學者提出了使用高斯粒子濾波（GPF）、Uscented粒子濾波（UPF）、有效性存儲粒子濾波（SEPF）的 OOSM 濾波算法［5～7］。但以PF為基礎的算法需要解決計算量和存儲量大的問題。

對此，本文提出以快速邊緣粒子濾波（fast marginalized PF，FMPF）為基礎的新的非線性OOSM處理算法。通過結合FMPF算法與前向預測濾波思想，算法可有效處理一步或多步OOSM問題，且降低了計算復雜度與存儲量，實時性更好。

1 問題描述

考慮非線性離散時間系統［2］

其中，xk為k時刻目標狀態向量，fk，k-1（·）為離散時間非線性狀態轉移函數，wk，k-1為從時刻k-1到時刻k的累積過程噪聲。zk為k時刻量測向量，hk（·）為非線性量測函數，vk為k時刻量測噪聲。假設初始時刻，目標初始狀態，過程噪聲與量測噪聲之間互不相關。

在時刻k，使用貝葉斯濾波方程可以得到后驗分布p（xk|z1∶k），繼而得到k時刻的目標狀態估計值和誤差協方差矩陣Pk|k。隨后，來自較早時刻d的量測zd，在狀態估計被計算出后到達融合中心。這里假定zd為l步滯后，即有tk-1＜td＜tk。其中，1≤l≤s，s為延遲量測到達融合中心的最大滯后時間。

需要解決的問題是:用延遲量測zd來更新后驗分布p（xk|z1∶k），以便獲得最后的后驗分布 p（xk|z1∶k，zd），繼而得到最新的狀態估計。

2 FMPF

FMPF在處理時，將目標跟蹤模型視為含有線性子結構的非線性狀態空間模型。其中，位置信息是強非線性信息，而速度信息是線性信息。此時，系統方程式（1）、式（2）可分解為

由此，可以將濾波過程分為并行的兩部分，采用PF處理非線性部分，采用KF處理線性部分。而FMPF其實是在MPF的基礎上，對線性部分的處理作了簡化，僅用一個KF對線性部分進行狀態估計，進一步節約計算資源。FMPF處理流程如圖1所示，具體實現過程可參見相關文獻［8～11］。

3 算法設計

新算法采用前向預測方法來處理OOSM問題。作為直接更新法的一種，前向預測法在使用時需要解決2個問題，即如何構建包含延遲量測的重構航跡和如何實現重構航跡與已有航跡的融合。在FMPF框架下，算法將狀態變量分成非線性與線性兩部分，并分別采用不同方式解決上述問題，現介紹如下:

圖1 FMPF處理流程圖Fig 1 Flow chart of FMPF processing

1）非線性部分處理

在此，借鑒現有的OOSM—PF算法處理延遲量測。

假設根據量測時戳，可以找到延遲量測zd在量測序列z1∶k中的位置。此時，還需要td-1時刻的狀態估計-1|d-1和估計誤差協方差矩陣。算法具體步驟為:

a.初始化

在延遲量測zd到達融合中心以后，由p（）產生粒子群，粒子對應的權值為

b.利用延遲量測zd，完成更新

首先，各粒子一步預測為

進而，更新粒子權值為

并歸一化為

其中，i=1，2，…，N。

由此，可得到狀態估計及其協方差矩陣為

2）線性部分

a.完成由td-1→td的更新

由于td-1時刻的狀態估計及其對應的誤差協方差矩陣已知，此時有

b.完成由td→tk的更新

由td→tk的更新可參照式（11）～式（15）進行，在此不再贅述。由此，可以得到和，完成了重構航跡的構建。

c.融合重構估計與已有估計

由此，得到了基于OOSM-FMPF的處理算法。算法在FMPF框架下，結合前向預測濾波思想處理OOSM。在非線性部分，算法將已有估計作為量測值，再次進行濾波。在線性部分，則借鑒航跡融合方法，通過加權等融合算法實現再更新。與現有算法相比，新算法實時性更好，這主要是因為:

1）算法采用FMPF框架，減小了非線性狀態向量的維數，使算法的計算復雜度降低。而FMPF與MPF相比，則進一步減小了線性部分的計算量。

2）新算法采用前向預測濾波，在非線性部分處理時，不需要存儲大量的粒子及其權重;而對于線性部分，也不需要存儲過去的量測或更新;從而減小了算法的存儲量。

此外，在FMPF框架下，可以采用一些改進的更優算法，如采用OOSM-UPF或OOSM-GPF算法代替OOSM-PF處理非線性部分從而進一步提高算法性能。

4 仿真實驗

仿真實驗中，采用二維空間中的運動模型為

取狀態變量為xk=［xkyk］T，其中，位置信息［xk，yk］T為強非線性信息，速度信息［］T為線性信息。目標運動航跡如圖2所示。

圖2 目標運動軌跡Fig 2 Target trajectory

實際量測過程中得到的是目標的距離r和偏轉角θ。量測方程為

將式（16）改寫為式（3）～式（4）所表示的MPF模型結構。模型中各參數為

假設傳感器獲得10個量測，在傳輸過程中，延遲量測在最后一個采樣間隔到達。仿真時，考慮單步延遲和兩步延遲的情形。同時，分別采用正常時序處理、OOSM-FMPF算法以及丟棄滯后法等3種方式處理這組量測，以驗證算法性能。進行50次Monte Carlo仿真，圖3、圖4給出了單步延遲情形下使用3種算法所得X軸向與Y軸向的估計，表1給出了單步延遲和兩步延遲的情形下算法在X軸向估計的RMSE。

表1 不同延遲步數下X方向估計的RMSE（m）Tab 1 X-direction estimated RMSE with different delay step number

從表1和圖2、圖3的結果可以看出:本文提出的OOSM-FMPF在單步滯后與兩步滯后的情形下，濾波精度都要高于丟棄滯后法，且接近正常時序處理結果，說明算法可以有效處理OOSM問題。

5 結論

本文針對非線性條件下的OOSM問題，提出了基于FMPF新處理算法。新算法保留FMPF算法框架，并采用前向預測方法處理OOSM。算法將狀態變量分為線性和非線性兩部分，分別采用相應的無序估計算法處理。新算法計算量與存儲量更小，實時性更好。仿真實驗也驗證了算法性能。今后，可以考慮使用GPF等改進PF算法，代替本文算法中的標準粒子算法，進一步提高估計精度。

圖3 一步延遲下X軸向估計Fig 3 X estimated with 1-step-lag

圖4 一步延遲下Y軸向估計Fig 4 Y direction estimate with 1-step-lag

［1］韓崇昭，朱紅艷，段戰勝，等.多源信息融合［M］.2版.北京:清華大學出版社，2010:368-379.

［2］王 煒，黃心漢，王 敏.無序量測濾波更新算法綜述［J］.控制與決策，2012，27（1）:1-5.

［3］陳金廣，李 潔，高新波.基于UT變換的單步滯后無序量測算法［J］.中國科學:信息科學，2011，41（5）:638-648.

［4］Mahendra M，Kirubarajan T，Sanjeev A.Out-of-sequence measurement processing for tracking ground target using particle filters［C］//Proceedings of 2002 IEEE Aerospace Conference，2002:1809-1818.

［5］Wang W，Wang G B，Huang X H.et al.Gaussian particle filtering algorithm for out-of-sequence measurement problem［C］//Proceedings of 2011 International Conference on Electric Information and Control Engineering，2011:3297-3300.

［6］周建中，王樹宗.一種基于粒子濾波的分布式跟蹤算法［J］.武漢理工大學學報:交通科學與工程版，2010，34（5）:924-927.

［7］Karl B，Karl-Erik A，Anders R.Storage efficient particle filters with multiple out-of-sequence measurements［C］//Proceedings of 2012 15th International Conference on Information Fusion，Singapore，2012:471-478.

［8］朱志宇.粒子濾波算法及其應用［M］.北京:科學出版社，2010:132-134.

［9］薛 鋒，劉 忠，石章松.快速Marginalized粒子濾波器在純方位跟蹤中的應用［J］.火力與指揮控制，2008，33（1）:66-72.

［10］陳金廣，馬麗麗，陳 亮.基于邊緣粒子濾波的目標跟蹤算法研究［J］.計算機工程與應用，2010，40（28）:128-131.

［11］鄒衛軍，陳 益，薄煜明.相似采樣粒子濾波在混合系統估計中的應用［J］.電光與控制，2009，16（11）:55-59.