快速邊緣粒子濾波在無(wú)序量測(cè)問(wèn)題中的應(yīng)用*

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

（1.軍械工程學(xué)院 信息工程系，河北 石家莊 050003;2.73903部隊(duì)，福建 廈門(mén) 361000）

0 引言

在目標(biāo)跟蹤系統(tǒng)中，由于所用傳感器具有不同的采樣速率，以及傳輸中延遲的不同，測(cè)量同步很難被保證，從而產(chǎn)生了異步融合問(wèn)題。更進(jìn)一步，會(huì)出現(xiàn)來(lái)自同一目標(biāo)的較早的量測(cè)在較晚的量測(cè)之后到達(dá)融合中心，即無(wú)序量測(cè)（out-of-sequence measurement，OOSM）現(xiàn)象［1］。傳統(tǒng)的卡爾曼濾波（KF）算法不能直接處理這種“負(fù)時(shí)間量測(cè)更新”問(wèn)題，需要研究相應(yīng)的濾波算法。

目前，最適合對(duì)無(wú)序量測(cè)（OOSM）作實(shí)時(shí)處理的濾波思想是直接更新法。直接更新法是直接利用OOSM和已存儲(chǔ)的目標(biāo)狀態(tài)充分估計(jì)量，對(duì)當(dāng)前狀態(tài)估計(jì)直接進(jìn)行再更新，以得到新的狀態(tài)估計(jì)及其估計(jì)誤差協(xié)方差矩陣［2］。直接更新法存儲(chǔ)量和計(jì)算量較小，且輸出沒(méi)有滯后，在這一濾波思想下，學(xué)者提出了諸如A1，B1，AA1，F(xiàn)PFD等線(xiàn)性系統(tǒng)下的最優(yōu)或次優(yōu)濾波算法。

對(duì)于弱非線(xiàn)性高斯系統(tǒng)，可以通過(guò)泰勒級(jí)數(shù)展開(kāi)將非線(xiàn)性問(wèn)題轉(zhuǎn)化為線(xiàn)性，但在應(yīng)用擴(kuò)展卡爾曼濾波（EKF）時(shí)，線(xiàn)性化過(guò)程會(huì)導(dǎo)致較大的濾波誤差［2］。文獻(xiàn)［3］提出了基于無(wú)跡卡爾曼濾波（UKF）的OOSM算法，通過(guò)UT避免了求解非線(xiàn)性量測(cè)方程的雅可比矩陣或海塞矩陣。對(duì)于強(qiáng)非線(xiàn)性高斯系統(tǒng)，Ortan推導(dǎo)得到了加入OOSM后的后驗(yàn)密度，提出了OOSM 粒子濾波（OOSM-PF）算法［4］，此后又有學(xué)者提出了使用高斯粒子濾波（GPF）、Uscented粒子濾波（UPF）、有效性存儲(chǔ)粒子濾波（SEPF）的 OOSM 濾波算法［5～7］。但以PF為基礎(chǔ)的算法需要解決計(jì)算量和存儲(chǔ)量大的問(wèn)題。

對(duì)此，本文提出以快速邊緣粒子濾波（fast marginalized PF，F(xiàn)MPF）為基礎(chǔ)的新的非線(xiàn)性O(shè)OSM處理算法。通過(guò)結(jié)合FMPF算法與前向預(yù)測(cè)濾波思想，算法可有效處理一步或多步OOSM問(wèn)題，且降低了計(jì)算復(fù)雜度與存儲(chǔ)量，實(shí)時(shí)性更好。

1 問(wèn)題描述

考慮非線(xiàn)性離散時(shí)間系統(tǒng)［2］

其中，xk為k時(shí)刻目標(biāo)狀態(tài)向量，fk，k-1（·）為離散時(shí)間非線(xiàn)性狀態(tài)轉(zhuǎn)移函數(shù)，wk，k-1為從時(shí)刻k-1到時(shí)刻k的累積過(guò)程噪聲。zk為k時(shí)刻量測(cè)向量，hk（·）為非線(xiàn)性量測(cè)函數(shù)，vk為k時(shí)刻量測(cè)噪聲。假設(shè)初始時(shí)刻，目標(biāo)初始狀態(tài)，過(guò)程噪聲與量測(cè)噪聲之間互不相關(guān)。

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

需要解決的問(wèn)題是:用延遲量測(cè)zd來(lái)更新后驗(yàn)分布p（xk|z1∶k），以便獲得最后的后驗(yàn)分布 p（xk|z1∶k，zd），繼而得到最新的狀態(tài)估計(jì)。

2 FMPF

FMPF在處理時(shí)，將目標(biāo)跟蹤模型視為含有線(xiàn)性子結(jié)構(gòu)的非線(xiàn)性狀態(tài)空間模型。其中，位置信息是強(qiáng)非線(xiàn)性信息，而速度信息是線(xiàn)性信息。此時(shí)，系統(tǒng)方程式（1）、式（2）可分解為

由此，可以將濾波過(guò)程分為并行的兩部分，采用PF處理非線(xiàn)性部分，采用KF處理線(xiàn)性部分。而FMPF其實(shí)是在MPF的基礎(chǔ)上，對(duì)線(xiàn)性部分的處理作了簡(jiǎn)化，僅用一個(gè)KF對(duì)線(xiàn)性部分進(jìn)行狀態(tài)估計(jì)，進(jìn)一步節(jié)約計(jì)算資源。FMPF處理流程如圖1所示，具體實(shí)現(xiàn)過(guò)程可參見(jiàn)相關(guān)文獻(xiàn)［8～11］。

3 算法設(shè)計(jì)

新算法采用前向預(yù)測(cè)方法來(lái)處理OOSM問(wèn)題。作為直接更新法的一種，前向預(yù)測(cè)法在使用時(shí)需要解決2個(gè)問(wèn)題，即如何構(gòu)建包含延遲量測(cè)的重構(gòu)航跡和如何實(shí)現(xiàn)重構(gòu)航跡與已有航跡的融合。在FMPF框架下，算法將狀態(tài)變量分成非線(xiàn)性與線(xiàn)性?xún)刹糠?，并分別采用不同方式解決上述問(wèn)題，現(xiàn)介紹如下:

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

1）非線(xiàn)性部分處理

在此，借鑒現(xiàn)有的OOSM—PF算法處理延遲量測(cè)。

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

a.初始化

在延遲量測(cè)zd到達(dá)融合中心以后，由p（）產(chǎn)生粒子群，粒子對(duì)應(yīng)的權(quán)值為

b.利用延遲量測(cè)zd，完成更新

首先，各粒子一步預(yù)測(cè)為

進(jìn)而，更新粒子權(quán)值為

并歸一化為

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

由此，可得到狀態(tài)估計(jì)及其協(xié)方差矩陣為

2）線(xiàn)性部分

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

由于td-1時(shí)刻的狀態(tài)估計(jì)及其對(duì)應(yīng)的誤差協(xié)方差矩陣已知，此時(shí)有

b.完成由td→tk的更新

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

c.融合重構(gòu)估計(jì)與已有估計(jì)

由此，得到了基于OOSM-FMPF的處理算法。算法在FMPF框架下，結(jié)合前向預(yù)測(cè)濾波思想處理OOSM。在非線(xiàn)性部分，算法將已有估計(jì)作為量測(cè)值，再次進(jìn)行濾波。在線(xiàn)性部分，則借鑒航跡融合方法，通過(guò)加權(quán)等融合算法實(shí)現(xiàn)再更新。與現(xiàn)有算法相比，新算法實(shí)時(shí)性更好，這主要是因?yàn)?

1）算法采用FMPF框架，減小了非線(xiàn)性狀態(tài)向量的維數(shù)，使算法的計(jì)算復(fù)雜度降低。而FMPF與MPF相比，則進(jìn)一步減小了線(xiàn)性部分的計(jì)算量。

2）新算法采用前向預(yù)測(cè)濾波，在非線(xiàn)性部分處理時(shí)，不需要存儲(chǔ)大量的粒子及其權(quán)重;而對(duì)于線(xiàn)性部分，也不需要存儲(chǔ)過(guò)去的量測(cè)或更新;從而減小了算法的存儲(chǔ)量。

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

4 仿真實(shí)驗(yàn)

仿真實(shí)驗(yàn)中，采用二維空間中的運(yùn)動(dòng)模型為

取狀態(tài)變量為xk=［xkyk］T，其中，位置信息［xk，yk］T為強(qiáng)非線(xiàn)性信息，速度信息［］T為線(xiàn)性信息。目標(biāo)運(yùn)動(dòng)航跡如圖2所示。

圖2 目標(biāo)運(yùn)動(dòng)軌跡Fig 2 Target trajectory

實(shí)際量測(cè)過(guò)程中得到的是目標(biāo)的距離r和偏轉(zhuǎn)角θ。量測(cè)方程為

將式（16）改寫(xiě)為式（3）～式（4）所表示的MPF模型結(jié)構(gòu)。模型中各參數(shù)為

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

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

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

5 結(jié)論

本文針對(duì)非線(xiàn)性條件下的OOSM問(wèn)題，提出了基于FMPF新處理算法。新算法保留FMPF算法框架，并采用前向預(yù)測(cè)方法處理OOSM。算法將狀態(tài)變量分為線(xiàn)性和非線(xiàn)性?xún)刹糠郑謩e采用相應(yīng)的無(wú)序估計(jì)算法處理。新算法計(jì)算量與存儲(chǔ)量更小，實(shí)時(shí)性更好。仿真實(shí)驗(yàn)也驗(yàn)證了算法性能。今后，可以考慮使用GPF等改進(jìn)PF算法，代替本文算法中的標(biāo)準(zhǔn)粒子算法，進(jìn)一步提高估計(jì)精度。

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

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

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

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

［3］陳金廣，李 潔，高新波.基于UT變換的單步滯后無(wú)序量測(cè)算法［J］.中國(guó)科學(xué):信息科學(xué)，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］周建中，王樹(shù)宗.一種基于粒子濾波的分布式跟蹤算法［J］.武漢理工大學(xué)學(xué)報(bào):交通科學(xué)與工程版，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］朱志宇.粒子濾波算法及其應(yīng)用［M］.北京:科學(xué)出版社，2010:132-134.

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

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

［11］鄒衛(wèi)軍，陳 益，薄煜明.相似采樣粒子濾波在混合系統(tǒng)估計(jì)中的應(yīng)用［J］.電光與控制，2009，16（11）:55-59.