基于二次預測的粒子濾波算法

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

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

0 引 言

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

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

1 二次預測粒子濾波算法

假設非線性系統的狀態模型和觀測模型分別為：

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

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

SP－PF算法是在經典粒子濾波的框架下發展的，通過蒙特卡洛積分的方法對狀態的后驗概率密度函數進行估計，其估計的數學表達式為：

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

歸一化前，粒子的重要性權重通過下式獲得：

通常選擇狀態轉移密度函數作為重要性密度函數，即：

通過式（5）得到的預測粒子沒有包含當前時刻的觀測信息，僅是對當前狀態的先驗估計。為了使采樣粒子包含當前時刻的觀測信息，構建如下最小二乘估計問題：

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

式（6）所述問題可轉換為代價函數求導等于0的問題，即：

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

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

g（yk，xk｜k－1）＝yk－h（xk｜k－1）＋Hkxk｜k－1

對J（xk）求導后式（8）可以轉化為：

求解式（11），經整理變形后可得當前狀態xk的最新估計為：

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

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

對重要性權值進行歸一化處理：

利用最新采樣的粒子和相應的權值對當前的狀態進行更新：

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

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

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

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

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

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

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

（7）根據式（16）對狀態進行后驗估計，得到xk。

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

（9）k＝k＋1，轉到步驟（2）。

2 算法討論

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

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

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

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

3 仿真實驗

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

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

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

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

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

圖1 不同濾波算法的狀態估計結果Fig.1 State estimation results of different filters

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

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

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

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

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

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

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

4 結束語

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

［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］夏楠，邱天爽，李景春，等.一種卡爾曼濾波與粒子濾波相結合的非線性濾波算法［J］.電子學報，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］戴連君，唐濤，蔡伯根.基于自適應粒子濾波的北斗衛星信號周跳探測［J］.吉林大學學報：工學版，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－hui，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］侯靜，景占榮，羊彥.遠距離干擾環境下目標跟蹤的擴展卡爾曼粒子濾波算法［J］.電子與信息學報，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的粒子濾波狀態估計算法［J］.系統工程與電子技術，2013，35（7）：1375－1379.Yu Hong－bo，Wang Guo－hong，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］何友，修建娟，關欣，等.雷達數據處理及應用［M］.北京：電子工業出版社，2013：76－78.

［17］李紅偉，王俊，王海濤.一種基于差分演化的粒子濾波算法［J］.電子與信息學報，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.