基于高斯粒子濾波算法的改進及應用

孫翌晨 李軍

摘 要：針對粒子濾波存在的樣本貧化現象，提出了一種優化重選樣本粒子的粒子濾波算法。這種方法在引入最新量測后將狀態后驗概率密度逼近為一個高斯分布，在粒子貧化問題逐漸凸顯后，通過該分布重新采集粒子后再進行運算，有效緩解了傳統方法在粒子貧化后出現的濾波精度下降的問題。仿真結果表明，新的粒子濾波算法有更高的濾波精度和運行效率。

關鍵詞：粒子濾波；后驗概率；粒子貧化；重新選取

近年來，粒子濾波在目標跟蹤領域得到了越來越廣泛的應用。常見的非線性濾波方法，如擴展卡爾曼濾波（Extended Kalman Filter，EKF），無跡卡爾曼濾波（Unscented Kalman Filter，UKF）都是針對非線性系統的線性卡爾曼濾波方法的變形與改進，因此使用條件也受到卡爾曼濾波算法的條件限制[1]。而粒子濾波算法通過蒙特卡羅仿真手段產生大量粒子，隨著采樣粒子數不斷增大，其散布情況將逐漸逼近狀態的后驗概率密度。粒子濾波在解決非高斯分布系統問題上具有明顯的優勢，可以說它是目前非高斯非線性系統狀態估計的“最優”濾波器[2]。但是，隨著時間的遞推，會出現粒子的退化問題。通常，有兩種方法可以減輕粒子退化問題：一是增加重采樣環節；二是選擇合適的重要密度函數進行更有效的采樣[3-5]。常規的重采樣方法隨著迭代次數的增加，會出現粒子貧化問題，為此，人們提出了許多不同的方法來解決這個問題，如高斯粒子濾波算法（Gaussian particle filter），重采樣粒子移動算法（Resample-Move Alogrithm）[6]，增加馬爾可夫鏈蒙特卡羅（Markov Chain Monte Carlo）移動步驟[7-9]，對粒子進行正則（Regula—rization）重采樣[10]。

筆者將標準粒子濾波算法和高斯粒子濾波相結合，引入一個重新選擇粒子的過程，即粒子優化重選粒子濾波（Optimized Repicking Particle Filter）。當粒子貧化問題出現時，通過由當前濾波值和方差生成的高斯密度函數來逼近后驗概率分布，重新采樣后再進行運算。由于引入粒子優化重選的過程，緩解了粒子的退化與貧化問題，所以濾波精度要高于標準算法。最后的仿真結果也證明了這一點。

1 高斯粒子濾波算法

GPF通過由高斯密度函數來逼近后驗概率分布，其基本思想是利用描述k時刻目標狀態xk的后驗概率分布 ，是對應權值為的粒子集，其中是0到k時刻的狀態集。權值被歸一化為 。由此，k時刻的后驗概率密度可表示為

根據文獻[11]，選擇=為IDF可使權值的方差最小化，但通常情況下很難求得的表達式，因此一種簡單常用的替代方案是選擇先驗作為重要密度函數。因此，可將重要性權值寫為：

1.1 量測更新

當接受到第k個觀測值zk之后，可以利用樣本及其權值來計算濾波值和方差pk，將狀態后驗概率密度逼近為一個高斯分布，可表示為

其中

1.2 時間更新

由于已經將近似為高斯函數，故可將狀態預測概率密度近似為：

其蒙特卡羅逼近為

式中，是從 采樣得到的粒子，狀態預測概率密度函數的均值和方差可表示為

2 樣本優化重選粒子濾波

與一般的粒子濾波相比，GPF不需要對粒子進行重采樣，降低了計算量和復雜度，但是濾波精度較之一般的粒子濾波稍差，因而考慮將兩者結合。

由此，引入樣本優化重選粒子濾波算法ORPF。在粒子貧化問題出現后，引入高斯粒子濾波的思想將后驗概率密度逼近為一個高斯分布。由通過蒙特卡羅仿真得到粒子集各個粒子的權值為1/N。進而，在的基礎上通過狀態方程產生新的粒子集 進行K+1時刻的計算。這樣，后一時刻的粒子根據前一時刻的新采樣的粒子集更新而來，這樣就可以保留粒子的多樣性。所以，當在出現粒子貧化問題時，可以采用這種方法來解決這個問題。

新算法的描述如下：

（1）由p（x0）得到N個采樣點 。

（2）計算權值 并對其歸一化，得到

（3）輸出K時刻的狀態估計為：

方差為

（4）判斷是否需要重采樣：計算的值，如果Neff

（5）進行重采樣，將原來的帶權樣本映射為等權樣本

（6）判斷是否要重新采集粒子，設重采樣后被復制次最多的粒子繁殖出了a個子代，當a>0.2N時則認為粒子多樣性喪失，此時進入步驟（7），否則，跳入步驟（8）。

（7）根據通過蒙特卡羅采樣得到粒子集

（8）通過狀態方程產生新粒子集

（9）重復步驟（2）～（8）。

3 實驗仿真結果及分析

3.1 仿真一

選用單變量非靜態增長模型（UNGM），仿真對象的過程模型和量測模型如下。

過程模型：

量測模型：

式中，ω（t）和v（t）為零均值高斯噪聲。仿真取噪聲方差Q為10，量測噪聲方差R為1。分別用增加馬爾可夫鏈蒙特卡羅粒子濾波（MCMCPF）和樣本優化重選粒子濾波算法（ORPF）來對這樣的非線性系統進行狀態估計和跟蹤。時間步長50次。

其中均方根誤差

具體數據如表1所示：

狀態估計曲線如圖1、2所示：

如表1所示，過程噪聲方差Q=10、量測噪聲方差R=1時。在N=100，MCMCPF的誤差均值為3.6946，而ORPF為3.5602；MCMCPF的RMSE為5.2191，而ORPF為4.0066；在狀態估計時間上，MCMCPF為0.009502s，而ORPF為0.010143s。在N=500時，MCMCPF的誤差均值為2.6765，而ORPF為2.4154；MCMCPF的RMSE為3.6747，而ORPF為3.2293；在時間估計上，MCMCPF為0.085130s，而ORPF為0.028229s。由此，說明ORPF的估計精度比MCMCPF略高，在粒子數較少時，兩種算法的估計時間相差不大，但隨著粒子數的增加，MCMCPF的估計時間急劇上升，遠超ORPF。

如圖1、2、3、4所示，ORPF算法的估計誤差是略小于MCMCPF的，濾波精度是略優于MCMCPF的。

綜上，可以得出，在高度非線性模型下，ORPF在濾波精度上略優于MCMCPF，在實時性上要優于MCMCPF。

3.2 仿真二

選用被動定位系統中二維純方位目標跟蹤模型，仿真對象的過程模型和量測模型如下：

過程模型：

量測模型：

其中為系統k時刻的狀態值，即目標在x和y軸上的位置和速度，為k-1時刻x，y方向的系統噪聲，為k時刻的觀測噪聲。為k時刻的觀測角度。

如圖5、6所示，ORPF在X軸方向和Y軸方向上的估計誤差都小于PF的估計誤差，說明ORPF的估計精度高于PF。圖7展示了2種算法的狀態估計曲線，顯然ORPF的跟蹤精度要好。

4 結束語

本文提出了一種基于GPF的加入了粒子優化重選步驟的新的粒子濾波。傳統的重采樣方法中權值較大的粒子會被多次復制，造成粒子的貧化。在貧化問題出現時，利用當前時刻的估計值和方差構建高斯密度函數來逼近后驗概率分布，從中重新采樣并更新粒子。該方法能夠有效的緩解粒子貧化帶來的問題。仿真結果表明，提出的ORPF在估計性能上要好于傳統的PF。

[參考文獻]

[1]占榮輝，張軍.非線性濾波理論與目標跟蹤應用[M].北京：國防工業出版社，2013：108-167.

[2]朱志宇.粒子濾波算法及其應用[M].北京：科學出版社，2010.

[3]胡士強，敬忠良.粒子濾波算法綜述[J].2005，20（4）：361-365.

[4]楊小軍，潘泉，王睿，等.粒子濾波進展與展望[J].控制理論與應用， 2006，23（2）：261-267.

[5]張俊根.粒子濾波及其在目標跟蹤中的應用研究[D].西安電子科技大學，2011.

[6]Gilks W R，Berzuini C.Following a moving target—Monte Carlo inference for dynamic Bayesian models[J].Journal of the Royal Statistical Society：Series B （Statistical Methodology）， 2001，63（1）：127-146.

[7]Fearnhead P.Markov chain Monte Carlo，sufficient statistics， and particle filters[J].Journal of Computational and Graphical Statistics，2002，11（4）：848-862.

[8]Carlin B P，Polson N G，Stoffer D S.A Monte Carlo approach to nonnormal and nonlinear state-space modeling[J].Journal of the American Statistical Association，1992，87（418）：493-500.

[9]Berzuini C，Best N G，Gilks W R，et al.Dynamic conditional independence models and Markov chain Monte Carlo methods[J]. Journal of the American Statistical Association，1997， 92（440）： 1403-1412.

[10]Sarkar P.Sequential Monte Carlo Methods in Practice[J]. Technometrics，2003，45（1）：106-106.

[11]Sanjeev Arulampalam M，Maskell S，Gordon N，et al.A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking[J].Signal Processing，IEEE Transactions on， 2002，50（2）：174-188.