基于二次預(yù)測的粒子濾波算法

武 勇，王 俊，曹運合，張培川

（西安電子科技大學(xué) 雷達信號處理國家重點實驗室，西安710071）

0 引 言

非線性濾波問題廣泛存在于信號處理、雷達探測、目標(biāo)跟蹤、衛(wèi)星導(dǎo)航等諸多領(lǐng)域［1－2］，這類問題可歸納為存在觀測噪聲時非線性系統(tǒng)的狀態(tài)估計問題。貝葉斯概率統(tǒng)計理論為這種狀態(tài)估計問題提供了一套完整的數(shù)學(xué)解決方案，其基本思想是將狀態(tài)估計問題轉(zhuǎn)化為后驗概率密度函數(shù)或后驗分布的逼近問題，并通過遞歸的方式實現(xiàn)對狀態(tài)空間的迭代估計。對于線性高斯模型來說，傳統(tǒng)的卡爾曼濾波被公認為是最優(yōu)的貝葉斯估計，其通過遞推式更新有限維統(tǒng)計量來精確計算狀態(tài)空間的后驗分布［3－4］。然而對于大部分非線性、非高斯的情況，狀態(tài)空間后驗分布的閉式解不存在或難以求解［5］，這就需要對后驗分布進行近似計算。文獻［6－7］提出的粒子濾波算法是一種通用的非線性濾波算法，其采用一系列帶有權(quán)值的樣本點對狀態(tài)的后驗概率分布進行近似，這種算法本質(zhì)上是基于狀態(tài)搜索進行的。首先通過重要性密度函數(shù)隨機生成若干個粒子，每個粒子代表一個可能的狀態(tài)，然后在狀態(tài)空間對它們進行傳播，使用似然函數(shù)選出最有可能生成當(dāng)前觀測值的若干個粒子點，并賦予一個較大的權(quán)值，最后通過保留下來的粒子對狀態(tài)的后驗分布進行近似。由于在狀態(tài)逼近的過程中使用了大量的粒子，因此該算法具有較高的計算復(fù)雜度。粒子濾波因優(yōu)良的性能，一經(jīng)被提出便獲得了廣泛的應(yīng)用［8－10］。但在粒子濾波中存在著兩個主要的問題：①當(dāng)粒子采樣不準(zhǔn)確時，如采樣得到的粒子位于實際后驗分布的拖尾區(qū)域，經(jīng)過狀態(tài)搜索后，絕大部分的粒子權(quán)值都會趨于0。這會為后驗分布的近似帶來很大的誤差，甚至有可能引起濾波發(fā)散。因此，采樣粒子的質(zhì)量直接決定著對后驗分布近似的精度。從最優(yōu)重要性密度函數(shù)的表達式可以看出，在采樣粒子時，應(yīng)考慮最新的觀測信息［7］?；诖?，相繼出現(xiàn)了輔助粒子濾波［11］、無跡粒子濾波［12－13］、擴展卡爾曼粒子濾波［14］以及多種卡爾曼相結(jié)合的粒子濾波［15］等方法，這幾種方法都是將當(dāng)前時刻的觀測值引入到粒子的采樣過程中，以提高所選粒子的質(zhì)量。②在對狀態(tài)進行迭代估計過程中，會出現(xiàn)粒子退化以及粒子貧化的現(xiàn)象［16］，減少了狀態(tài)估計中可用粒子的數(shù)量和種類，增大了狀態(tài)估計誤差，一般通過重采樣技術(shù)來緩解這種現(xiàn)象的發(fā)生，即在每次迭代后對粒子進行重新采樣，根據(jù)重采樣的策略不同，狀態(tài)估計的精度也各不相同，如差分演化粒子濾波及其變種［17］。

綜合以上的分析，對粒子濾波存在問題的改進主要從如何有效利用觀測信息提高采樣粒子的質(zhì)量和重采樣策略兩個方面進行。本文從第一個方面出發(fā)，提出了一種基于二次預(yù)測的粒子濾波算法，稱為二次預(yù)測粒子濾波（SP－PF）。算法首先使用狀態(tài)轉(zhuǎn)移密度函數(shù)對粒子進行初采樣，獲得預(yù)測粒子，然后在預(yù)測粒子的基礎(chǔ)上，通過求解一個最小二乘問題，將當(dāng)前時刻的觀測信息引入到粒子的二次預(yù)測中，并使用似然函數(shù)計算新粒子的權(quán)值，最后通過粒子加權(quán)對當(dāng)前的狀態(tài)進行估計，估計完后再對粒子進行重采樣。SP－PF 算法的優(yōu)勢在于：將當(dāng)前時刻的觀測信息融合在粒子采樣階段，充分利用了可信的觀測信息，避免了傳統(tǒng)粒子濾波中觀測信息僅用來評判粒子好壞的問題，提高了位于真實后驗分布附近采樣粒子的數(shù)量，緩解了迭代過程中出現(xiàn)的粒子退化問題，進而改善了狀態(tài)估計的精度。

1 二次預(yù)測粒子濾波算法

假設(shè)非線性系統(tǒng)的狀態(tài)模型和觀測模型分別為：

式中：xk∈Rm為k 時刻的m 維狀態(tài)向量；yk∈Rn為k 時刻的n 維觀測向量；f（·）、h（·）為已知的函數(shù)；uk、vk分別為分布已知的系統(tǒng)噪聲和觀測噪聲，它們之間相互獨立。

濾波的目的就是利用當(dāng)前時刻的觀測值和上一時刻的狀態(tài)估計值，對當(dāng)前時刻的狀態(tài)（后驗概率密度函數(shù)）進行迭代更新。

SP－PF算法是在經(jīng)典粒子濾波的框架下發(fā)展的，通過蒙特卡洛積分的方法對狀態(tài)的后驗概率密度函數(shù)進行估計，其估計的數(shù)學(xué)表達式為：

式中：N 為采樣的粒子數(shù)；y1：k 為到k 時刻所有觀測量 構(gòu) 成 的 觀 測 序 列y1：k ＝｛y1，y2，…，yk｝；為從重要性密度函數(shù)q（xk｜xk－1，y1：k）采樣得到的粒子序列為對應(yīng)于每個粒子的歸一化重要性權(quán)重，滿足

歸一化前，粒子的重要性權(quán)重通過下式獲得：

通常選擇狀態(tài)轉(zhuǎn)移密度函數(shù)作為重要性密度函數(shù)，即：

通過式（5）得到的預(yù)測粒子沒有包含當(dāng)前時刻的觀測信息，僅是對當(dāng)前狀態(tài)的先驗估計。為了使采樣粒子包含當(dāng)前時刻的觀測信息，構(gòu)建如下最小二乘估計問題：

上式是一個無約束極小化問題，其代價函數(shù)為：

式（6）所述問題可轉(zhuǎn)換為代價函數(shù)求導(dǎo)等于0的問題，即：

利用狀態(tài)的先驗估計xk｜k－1，通過一階泰勒展開式對觀測函數(shù)h（xk）進行近似，即：

式中：Hk＝?xkh（xk）｜xk＝xk｜k－1，將 式（9）代 入J（xk）的表達式易得：

g（yk，xk｜k－1）＝y(tǒng)k－h(huán)（xk｜k－1）＋Hkxk｜k－1

對J（xk）求導(dǎo)后式（8）可以轉(zhuǎn)化為：

求解式（11），經(jīng)整理變形后可得當(dāng)前狀態(tài)xk的最新估計為：

將從式（5）采樣得到的預(yù)測粒子分別代入式（12），即可獲得新的預(yù)測粒子。在新粒子中，不僅包含了對狀態(tài)的預(yù)測值（先驗信息），同時也包含了當(dāng)前時刻的觀測值（后驗信息），即：

通過式（3）計算新粒子的重要性權(quán)值，易得：

對重要性權(quán)值進行歸一化處理：

利用最新采樣的粒子和相應(yīng)的權(quán)值對當(dāng)前的狀態(tài)進行更新：

最后，按文獻［6］中的方法進行粒子的重采樣，重采樣后各粒子的權(quán)值為1／N。

本文提出的基于二次預(yù)測的粒子濾波算法可歸納為如下步驟：

初始化：從先驗概率分布p（x0）采樣得到粒子

（1）根據(jù)式（4）選擇重要性密度函數(shù)q（xk｜xk－1，y1：k）。

（2）根據(jù)q（xk｜xk－1，y1：k）對粒子進行初采樣，得到預(yù)測粒子

（3）計算觀測函數(shù)h（xk）的雅克比矩陣Hk。

（4）根據(jù)式（13）進行二次采樣，得到包含觀測信息的新粒子。

（7）根據(jù)式（16）對狀態(tài)進行后驗估計，得到xk。

（8）對粒子進行重采樣。

（9）k＝k＋1，轉(zhuǎn)到步驟（2）。

2 算法討論

通過傳統(tǒng)采樣獲得的粒子僅是對當(dāng)前狀態(tài)的先驗估計，這種粒子的生存能力不強，容易出現(xiàn)粒子分布嚴重偏離真實分布的情況，而式（12）恰好可以將觀測信息和狀態(tài)的先驗信息有機結(jié)合在一起，充分利用觀測信息來提高采樣粒子的穩(wěn)健性。另外，SP－PF算法需要計算非線性觀測方程的雅克比矩陣，為狀態(tài)估計過程增加了額外的運算量。由于對每個粒子的處理是相互獨立的，因此可通過并行技術(shù)（如GPU 高性能并行計算）來提高算法的處理效率。

通過式（12）得到的新粒子不僅包含了當(dāng)前時刻的觀測信息，而且也是當(dāng)前狀態(tài)的無偏估計，證明如下：

將式（9）代入式（1）中的觀測方程，可得：

將式（17）代入式（12），化解易得：

3 仿真實驗

對于可信觀測的非線性濾波問題，本文所提出的SP－PF 算法具有良好的狀態(tài)估計精度。為了驗證SP－PF算法的有效性，本文將SP－PF與基于序貫重要性采樣的標(biāo)準(zhǔn)粒子濾波（SPF）、輔助粒子濾波（APF）以及無跡粒子濾波（UPF）進行了對比。實驗使用的狀態(tài)方程、觀測方程分別為：

式中：系統(tǒng)噪聲uk服從Gamma（3，2）分布、觀測噪聲vk服從高斯分布N（0，0.0001），狀態(tài)初值為1，觀測周期為1s，觀測時間為50s，粒子數(shù)量為50。如無特別說明，以上參數(shù)均為默認值。

實驗采用均方根誤差（RMSE）作為性能評價指標(biāo)：

式中：M 為蒙特卡洛仿真實驗次數(shù)，默認取值為500；xm為估計值為真值。

圖1為一次仿真實驗中4種濾波算法對狀態(tài)的估計結(jié)果。從圖中可以看出：UPF和SP－PF都可以很好地對狀態(tài)進行逼近，其中，SP－PF的濾波過程更加穩(wěn)定，與真值的擬合度更高。相比于UPF和SP－PF，SPF 和APF 的狀態(tài)估計過程不夠穩(wěn)定，在部分采樣點上出現(xiàn)了輕微的濾波發(fā)散現(xiàn)象。在本次實驗中，從與真值的擬合度和濾波的穩(wěn)定度兩方面考慮，SP－PF優(yōu)于其他3種算法。

圖1 不同濾波算法的狀態(tài)估計結(jié)果Fig.1 State estimation results of different filters

圖2 給出了經(jīng)過500次蒙特卡洛實驗后SPPF與其他3 種算法的RMSE 對比圖。由圖可知：SP－PF 的RMSE 明 顯 低 于SPF、APF 和UPF，這主要是由于SP－PF的每個粒子融入了最新的觀測信息，與UPF 相比，SP－PF 沒有對狀態(tài)的后驗分布作任何形式的假設(shè)。表1分別統(tǒng)計了圖2中4種算法的單次平均運行時間、RMSE 均值和標(biāo)準(zhǔn)差。RMSE 的均值和標(biāo)準(zhǔn)差表征了算法的估計精度和穩(wěn)定性。從表中可以清楚地看出：與SPF、APF 和UPF 相比，SP－PF 的RMSE均值和標(biāo)準(zhǔn)差是最小的，說明了SP－PF 具有更高的估計精度和更強的穩(wěn)健性。在算法的平均運行時間方 面，SPF 最 短，SP－PF 和APF 次 之，UPF最長。由于SP－PF 需要計算觀測方程的雅克比矩陣，為狀態(tài)估計過程引入了額外的運算，因而導(dǎo)致其運行時間略高于SPF。APF 在每次迭代中需要對粒子進行兩次采樣，兩次權(quán)值計算和兩次重采樣，且每次的處理過程類似，因此APF 的平均運行時間約為SPF的兩倍。由于UPF 將無跡卡爾曼濾波器（UKF）作為粒子產(chǎn)生器，每個粒子均需要通過運行UKF 來產(chǎn)生，因此UPF 的運行效率最低。

圖2 不同濾波算法的RMSE對比圖Fig.2 Comparison of the RMSE of different filters

表1 不同濾波算法的RMSE均值、標(biāo)準(zhǔn)差和平均運行時間Table 1 RMSE mean，RMSE standard deviation and average running time of different filters

圖3 給出了粒子數(shù)量取不同值時，SPF、APF、UPF和SP－PF 算法的RMSE 均值變化情況。顯然，增加粒子數(shù)對改善算法估計精度是有益的，隨著粒子數(shù)的增加，4種濾波算法的估計精度逐漸提高，并趨于平穩(wěn)。此外，在粒子數(shù)量取不同值的情況下，SP－PF 算法的誤差均值均低于其他3 種算法，且SP－PF 算法可以使用更少的粒子，獲得更高的性能，進一步說明了該算法的性能優(yōu)于其他3種濾波算法。

圖3 粒子數(shù)量對算法性能的影響Fig.3 Effects of particle number on the performance of different filters

針對粒子濾波算法在實際應(yīng)用中存在的運算量大、處理效率低的缺點，以SP－PF 算法為例，在英偉達型號為Tesla K20的圖形處理器（GPU）上對其進行了并行實現(xiàn)，并與CPU 的處理時間（以Matlab處理時間為代表）進行了對比，結(jié)果如表2所示。由表2可知：GPU 實現(xiàn)的處理時間均達到了毫秒級，且隨粒子數(shù)的增加變化緩慢，表現(xiàn)出了較強的數(shù)據(jù)擴展性，與CPU 的處理時間相比，獲得了最高144倍的加速性能，極大地提高了SPPF算法的處理效率，這主要得益于GPU 中的上千個并行運算核。因此，GPU 為實現(xiàn)粒子濾波算法的在線處理提供了一條有效途徑。

表2 SP－PF在CPU 和GPU 上實現(xiàn)的處理時間對比Table 2 Comparison of the processing time of the SP－PFimplemented on CPU and GPU

4 結(jié)束語

傳統(tǒng)粒子濾波在處理非線性濾波問題時所用的粒子是通過狀態(tài)轉(zhuǎn)移函數(shù)從上一時刻的粒子中采樣得到的，由于在采樣粒子中沒有任何關(guān)于當(dāng)前觀測值的信息，容易出現(xiàn)粒子分布嚴重偏離真實后驗分布或粒子退化的情況，進而影響了狀態(tài)估計的精度。針對這種情況，本文提出了一種基于二次預(yù)測的粒子濾波算法，其基本思想是將粒子采樣分為兩個階段進行。首先采樣得到預(yù)測粒子，然后通過最小二乘估計，將觀測信息用于對預(yù)測粒子的采樣中，得到包含觀測信息的新的預(yù)測粒子，最后利用這些新粒子對狀態(tài)進行估計。由于在粒子采樣中融入了觀測量信息，且所得到的新粒子都是對當(dāng)前狀態(tài)的無偏估計，因此促進了采樣粒子向高似然區(qū)域的移動，提高了各個粒子與真實狀態(tài)之間的關(guān)聯(lián)性，降低了粒子分布嚴重偏離真實后驗分布的風(fēng)險，改善了狀態(tài)估計的精度。通過計算機仿真，將該算法與標(biāo)準(zhǔn)粒子濾波、輔助粒子濾波和無跡卡爾曼粒子濾波進行了對比，結(jié)果顯示該算法的估計精度更高、穩(wěn)定性更好。由于該算法在二次預(yù)測中需要計算觀測函數(shù)的雅克比矩陣，接下來的工作可從降低算法運算量方面進行深入研究。

［1］Sedai S，Bennamoum M，Huynh D Q.A Gausian process guided particle filter for tracking 3Dhuman pose in video［J］.IEEE Transactions on Image Processing，2013，22（11）：4286－4300.

［2］Yardim C，Gerstoft P，Hodgkiss W S.Tracking refractivity from clutter using Kalman and particle filters［J］.IEEE Transactions on Antennas and Propagation，2008，56（4）：1060－1069.

［3］夏楠，邱天爽，李景春，等.一種卡爾曼濾波與粒子濾波相結(jié)合的非線性濾波算法［J］.電子學(xué)報，2013，41（1）：148－152.Xia Nan，Qiu Tian－shuang，Li Jing－chun，et al.A nonlinear filter algorithm combining the Kalman filter and the particle filter［J］.Acta Electronica Sinica，2013，41（1）：148－152.

［4］Kalman R E.A new approach to linear filtering and prediction problems［J］.Transactions of the AMSE Journal of Basic Engineering，1960，82（1）：35－45.

［5］Kotecha J H，Djuric P M.Gaussian particle filter［J］.IEEE Transactions on Signal Processing，2003，51（10）：2591－2601.

［6］Li Hong－wei，Wang Jun.Particle filter for manoeuvring target tracking via passive radar measurements with glint noise［J］.IET Radar Sonar and Navigation，2012，6（3）：180－189.

［7］Arulampalam M S，Maskell S，Gordon N，et al.A tutorial on particle filters for online nonlinear／non－Gaussian Bayesian tracking［J］.IEEE Transactions on Signal Processing，2002，50（2）：174－188.

［8］Georgy Jacques，Korenberg Michael J，Bayoumi Mohamed M.Low－cost three－dimensional navigation solution for RISS／GPS integration using mixture particle filter［J］.IEEE Transactions on Vehicular Technology，2010，59（2）：599－614.

［9］戴連君，唐濤，蔡伯根.基于自適應(yīng)粒子濾波的北斗衛(wèi)星信號周跳探測［J］.吉林大學(xué)學(xué)報：工學(xué)版，2013，43（4）：1146－1152.Dai Lian－jun，Tang Tao，Cai Bo－gen.Cycle slip detection for BeiDou satellite based on adaptive particle filter［J］.Journal of Jilin University（Engineering and Technology Edition），2013，43（4）：1146－1152.

［10］Wang Ya－feng，Sun Fu－chun，Zhang You－an，et al.Central difference particle filter applied to transfer alignment for SINS on missiles［J］.IEEE Transactions on Aerospace and Electronic Systems，2012，48（1）：375－387.

［11］Pitt M K，Shephard N.Filtering via simulation：auxiliary particle filters［J］.Journal of the American Statistical Association，1999，94（446）：590－599.

［12］Zhan Rong－h(huán)ui，Xin Qin，Wan Jian－wei.Modified unscented particle filter for nonlinear Bayesian tracking［J］.Journal of Systems Engineering and Electronics，2008，19（1）：7－14.

［13］Li Hong－wei，Wang Jun，Su Hong－tao.Improved particle filter based on differential evolution［J］.Electronics Letters，2011，47（19）：1078－1079.

［14］侯靜，景占榮，羊彥.遠距離干擾環(huán)境下目標(biāo)跟蹤的擴展卡爾曼粒子濾波算法［J］.電子與信息學(xué)報，2013，35（7）：1587－1592.Hou Jing，Jing Zhan－rong，Yang Yan.Extended Kalman particle filter algorithm for target tracking in stand－off jammer［J］.Journal of Electronics ＆Information Technology，2013，35（7）：1587－1592.

［15］于洪波，王國宏，孫 蕓，等.一種融合UKF和EKF的粒子濾波狀態(tài)估計算法［J］.系統(tǒng)工程與電子技術(shù)，2013，35（7）：1375－1379.Yu Hong－bo，Wang Guo－h(huán)ong，Sun Yun，et al.Particle filtering algorithm of state estimation on fusion of UKF and EKF［J］.Systems Engineering and Electronics，2013，35（7）：1375－1379.

［16］何友，修建娟，關(guān)欣，等.雷達數(shù)據(jù)處理及應(yīng)用［M］.北京：電子工業(yè)出版社，2013：76－78.

［17］李紅偉，王俊，王海濤.一種基于差分演化的粒子濾波算法［J］.電子與信息學(xué)報，2011，33（7）：1639－1643.Li Hong－wei，Wang Jun，Wang Hai－tao.A new particle filter based on differential evolution method［J］.Journal of Electronics ＆Information on Technology，2011，33（7）：1639－1643.