基于概率核函數的改進粒子濾波算法研究

張恒浩 唐 超 王小錠 魏 明 張 霞 吳勝寶

中國運載火箭技術研究院研發中心，北京100076

針對粒子濾波算法在組合導航系統應用中出現的粒子衰減退化問題［1-2］，引入基于概率核函數的改進粒子濾波算法。這種改進算法在設計過程中引入基于概率統計的核函數，通過改進粒子濾波算法中的重要性函數來獲得全面的系統信息;然后設計出核函數和對應的交互核函數，并將其應用在粒子濾波算法的重采樣計算中;最后利用取得的粒子集合和對應的權值生成新的粒子集。這樣改進的粒子濾波算法在阻止粒子集合退化現象發生的同時還可以保持粒子的多樣性，從而可以達到提高系統導航精度的目的。仿真結果表明，引入改進粒子濾波算法的組合導航系統可以有效降低系統的誤差，提高系統導航精度。

1 重要性采樣函數設計

可以看出，改進粒子濾波算法在計算重要性采樣時考慮了已知量測值條件下狀態估計概率和前一狀態值條件下的狀態估計概率。通過這2種狀態估計概率值的比例，最大程度上反映了重要性采樣函數表示的系統信息，避免了可能因為傳感器精度的限制而出現的采樣集合丟失。這樣的設計可以有效地提高系統粒子濾波的性能。

2 基于隨機概率統計的核函數設計

核函數算法主要是用來提供一種處理非線性問題的方法。粒子濾波算法中引入核函數的主要目的是希望通過核函數算法來解決粒子濾波算法在計算過程中發生的粒子退化現象。核函數的設計要與粒子濾波算法中得到的粒子集合的特征相對應。另外在設計核函數的過程中還要考慮粒子濾波算法的計算量和計算復雜度，避免在計算過程中出現獲得非線性積分數據的困難［8］。

因此，針對應用在組合導航系統中的粒子濾波算法改進這一特定問題，選擇什么樣的核函數是至關重要的。而且即使選擇了某一核函數，相應參數選取的不同也會影響改進的粒子濾波算法的計算精度。所以改進粒子濾波算法中引入的核函數算法必須考慮重采樣粒子的概率特征，通過概率特征來設計核函數的概率密度函數和分布函數，然后再通過重采樣計算得到新的粒子集。下面給出基于概率統計理論的核函數推導。

假設X作為一個隨機變量，則將函數式(5)所表示的函數稱為隨機變量X的概率分布函數［9］。

根據上述概率密度函數的定義，核函數設計的核心是要從粒子濾波得到的重要性采樣樣本數據中建立一個概率密度函數［11］，然后將這一概率密度函數引入重采樣計算中。

為了設計核函數，必須考慮粒子濾波中重采樣得到的粒子集合的特征，然后才能根據特征設計有針對性的核函數。下面分析原來典型的粒子濾波重采樣算法得到的粒子集合的性質。

重采樣粒子具有隨機性，所以它們彼此之間具有獨立性，相互之間不影響對方的采樣數值。粒子重采樣還是基于系統狀態方程的估計進行采樣的，這些粒子同系統狀態方程的分布有相同的分布，所以具有同分布性。同時滿足同分布性和隨機性的粒子集合可以近似看做是隨機抽樣樣本［12］。經過上面描述可知，粒子濾波過程中的重采樣得到的新的粒子集合滿足上述條件，在引入核函數改進粒子濾波算法的過程中可以將重采樣過程得到的粒子集合看做是隨機采樣數據集合。在設計核函數的時候利用隨機抽樣集合的特征來建立核函數方程。

假設粒子濾波計算過程中一共有n個重采樣粒子，可以看做是n個樣本估計。設對應的第i個粒子對應得到的數據為k(xi，x)。由樣本估計原理計算得到粒子集合整體的概率密度估計表達式為:

定義由重采樣計算得到的第i個粒子對應的函數表達式k(xi，x)為交互核函數，對于在粒子濾波算法中的核函數則定義其表達式為K(xi，x)。

因為交互核函數k(xi，x)實際上是重采樣算法得到的粒子中第i個粒子的概率密度函數值，所以對于任意的x，交互核函數k(xi，x)必須滿足條件:

核函數K(xi，x)表示的含義是在粒子濾波重采樣中得到的第i個粒子的分布函數值。核函數和交互核函數之間的關系如下:

為了在計算中方便求解，一般在設計交互核函數時要盡可能地將已知量xi的函數表達式和未知量x的函數表達式分開。這樣在計算過程中可以更方便地求出核函數，這種設計方法可以使粒子濾波算法減小計算量，有利于滿足系統的實時性要求。

根據這一思路，設計的交互核函數一般將其改進為:

式(12)中，因為設計的交互核函數k(xi，x)已將關于xi的函數表達式和x的函數表達式分開，在求解核函數K(xi，x)時只對x做積分運算。所以在計算過程中可以將函數ψi(xi)看做常數。設改進后的交互核函數ψi(xi)φi(x)對x求積分得到的核函數為:

設計出核函數K(xi，x)和交互核函數k(xi，x)后，將它們引入粒子濾波算法中的重采樣計算中，利用取得的粒子集合和對應的權值生成新的粒子集，在阻止粒子集合退化現象發生的同時保持粒子的多樣性，這樣可以提高粒子濾波算法的計算精度。

3 引入核函數的改進粒子濾波算法設計

因為傳統的粒子濾波算法所依據的信息主要是離散的樣本粒子及其在后驗概率分布中占有比重權值的粒子。在傳統的粒子濾波算法中引入重采樣方法的主要目的就是利用粒子權值來區別對待所獲得的采樣粒子。它通過保留或者復制粒子權值較大的粒子、放棄刪除粒子權值較小的粒子來解決粒子退化問題。由于大量的粒子在迭代過程中逐漸由相同的大權值粒子復制而來，這樣的結果必然導致粒子樣本的多樣性喪失，樣本粒子逐漸枯竭。

改進粒子濾波算法根據前面推導得到的樣本粒子集合以及對應的權值，通過設計的核函數來模擬擬合離散分布，估計出后驗概率密度的連續模型分布函數，再根據這個模型對粒子濾波算法進行重采樣，這樣可以得到更具多樣性的粒子，從而保證了粒子集合的多樣性，并能有效地抑制粒子退化現象。

因此，推導改進的核函數粒子濾波算法的計算步驟為:

步驟5:狀態輸出估計:當系統的改進粒子濾波在步驟4中進行重采樣計算后，則狀態輸出估計表達式為:

步驟6:設k=k+1，并返回步驟2繼續計算。

這6個步驟給出了引入核函數的粒子濾波改進算法的計算推導過程。在這一改進算法中，首先，對重要性函數設計所作的改進會使算法能通過采樣粒子更全面地反映系統狀態，從而提高粒子濾波器的工作性能;其次，在重采樣過程中引入基于概率統計的核函數算法，計算生成新的粒子集合。核函數的引入可以使粒子濾波算法在計算過程中保持粒子集合的多樣性，不因粒子濾波算法的迭代而使粒子性質變得單一;最后由于核函數計算過程中引入了隨機抽樣算法的平均計算思路，這樣如果計算過程中出現粒子失真現象時可通過加權平均的思想將其變得穩定。所以在計算粒子濾波過程中，核函數解算可使粒子濾波中得到的重采樣粒子集合變得平滑。

4 仿真驗證

為驗證改進粒子濾波算法的性能，在組合導航系統中應用改進的粒子濾波核函數算法和傳統的粒子濾波算法進行組合導航系統的濾波設計，通過兩者結果的比較來驗證改進的粒子濾波算法的有效性和可行性。

組合導航系統由慣性導航系統和GPS衛星定位系統組成，組合導航系統的狀態方程和觀測方程表達式如式(24)和(25)所示:

組合導航系統工作時的初始參數如下:

初始狀態的速度誤差和位置誤差均為0，計算過程中的迭代時間為300s;

粒子算法中計算的粒子數目設為40，重采樣的粒子限制值Nth設為20。

在計算過程中通過對組合導航系統內部的狀態誤差進行估計，并得到計算結果。通過比對2種算法的計算結果來判斷改進算法的優劣。

組合導航系統在應用改進的粒子濾波核函數算法時需要設計核函數，根據推導核函數時給出的核函數相關信息，選擇核函數K(xi，x)的表達式為:

為了簡化計算，在核函數式(27)中設參數λ的值為1。

這樣將設置好參數的核函數和交互核函數引入改進粒子濾波算法的重采樣算法中，分別將改進的粒子濾波核函數算法和原來的經典粒子濾波算法應用到組合導航系統中。這2種算法都是通過粒子解算估計出組合導航狀態X的估計值，再通過濾波算法得到下一狀態的推測值。對比2種算法得到的誤差結果，從而判斷改進的粒子濾波算法是否能起到改善組合導航系統性能的要求。

圖1～5為2種算法的結果比較。圖中的實線表示引進核函數的改進粒子濾波算法得到的結果，虛線表示傳統的粒子濾波算法得到的結果。

從仿真結果可以看出，當粒子集合退化嚴重，粒子集合向單一性發展的時候，傳統的粒子濾波算法會產生較大的導航誤差，并且誤差有發散趨勢。引入核函數的改進粒子濾波算法在工作中可以降低由粒子集合退化產生的組合導航系統誤差，并且使系統誤差的變化趨于平穩。組合導航系統在工作時誤差的變化率變得平穩，這樣有利于系統在工作中達到穩定狀態。

圖1 正北方向位置誤差比較

圖2 正東方向位置誤差比較

圖3 高度位置誤差比較

圖4 北向速度誤差比較

圖5 東向速度誤差比較

5 結論

針對傳統粒子濾波算法中出現的粒子衰減退化問題，引入了基于概率核函數設計的改進粒子濾波算法。并將這一算法應用在組合導航系統中。該算法在粒子濾波算法出現粒子衰減退化現象時能夠有效抑制粒子衰減并保持粒子的多樣性。仿真結果表明，這種基于概率核函數設計的改進粒子濾波算法可以很好地解決粒子濾波算法在計算過程中出現的粒子衰減退化問題，提高組合導航系統的計算精度。

［1］ Julier S，Unlmann J.Unscented Filtering and Nonlinear Estimation［J］.Proceedings of IEEE，2004，192(3):401-422.

［2］ Rudolph V M，Arnaud D.The Unscented Particle Filter［R］.CU ED/F-Infeng/Tr380.Cambridge University Engineering Department，2001.

［3］ Erik B，Peter J A，Nils C，John M S.Monte Caulo filter for Non-linear State Estimation［J］.Automatica，2001，37(2):177-183.

［4］ 莊澤森，張建秋，尹建君.Rao—Blackwellizes粒子概率假設密度濾波算法［J］.航空學報，2009，30(4):698-705.(ZHUANG Zesen， ZHANG Jianqiu， YIN Jianjun.Rao-Blackwellized Particle Probability Hypothesis Density Filter［J］.Acta Aeronautica ET Astronautica Sinica，2009，30(4):698-705.)

［5］ 張磊.一種基于高斯混合模型粒子濾波的故障預測算法［J］.航空學報，2009，30(2):319-324.(ZHANG Lei.A Fault Prognostic Based on Gaussian Mixture Model Particle Filter.Acta Aeronautica ET Astronautica Sinica，2009，30(2):319-324.

［6］ Petovello M G，Lachapelle G.Comparison of Vectorbased Software Receiver Implementations with Application to Ultra-tight GPS/INS Integration［C］.ION GNSS 2006.Fort Worth TX，2006:1-10.

［7］ Rankin J.An Errormodel for Sensor Simulation GPS and Differential GPS［C］.IEEE Position Location and Navigation Symposium，1994:260-266.

［8］ Sumit Roy，Ronald A.Iltis Decentralized Linear Estimation in Correlated Measurement Noise［J］.IEEE Transaction on Aerospace and Electronic Systems，1999，27(6):939-941.

［9］ 葛哲學，楊擁民.非高斯噪聲下基于Unscented粒子濾波器的非線性系統故障診斷方法［J］.兵工學報，2007，28(3):332-335.(GE Zhemin，YANG Yongmin.Unscented Particle Filter-based Fault Diagnosis of Nonlinear System with Non-Gaussian Noises［J］.Actaarmamentarll，2007，28(3):332-335.)

［10］ 于飛，唐小勇，潘洪悅.改進粒子群算法在三維水下導航規劃中的應用［J］.北京理工大學學報，2010，30(9):1059-1064.(YU Fei，TANG Xiaoyong，PAN Hongyue.The Application of an Improved PSO to the Submersible Path-Planning［J］.Transactions of Beijing Institute of Technology，2010，30(9):1059-1064.)

［11］ Thierry Chateau.M2SIR:A Multi Modal Sequential Importance Resampling Algorithm for Particle Filters［C］.IEEE ICIP2009:4073-4076.

［12］ Wataru Sawabe.Application of Particle Filter to Autonomous Navigation System for Outdoor Environment［C］.SICE Annual Conference 2008:93-96.