迭代容積卡爾曼粒子濾波算法

王華劍 景占榮 鄭文泉 屈保平

(西北工業大學電子信息學院，西安 710072)

近年來，在自動控制、目標跟蹤、人工智能、故障檢測、數字通信和金融統計等復雜系統中廣泛存在著非線性/非高斯的狀態估計問題.基于序貫Monte Carlo的粒子濾波(particle filter，PF)利用數值逼近的方式較好地解決了非線性/非高斯系統的參數估計和狀態濾波問題.粒子濾波的基本思想是采用隨機Monte Carlo對先驗分布在狀態空間產生一組隨機樣本集，構建重要性密度函數，遞推地用這些隨機樣本集和其權重來近似于狀態變量真實的后驗概率密度函數，用樣本均值來代替積分運算，從而獲得狀態的最小方差估計過程［1-2］.

但是由于標準粒子濾波算法選擇先驗概率密度作為重要性密度函數，沒有考慮當前的量測值，從重要性密度函數中取樣得到的樣本與從真實后驗概率密度采樣得到的樣本有很大偏差，尤其當似然函數位于系統狀態轉移概率密度的尾部或似然函數呈尖峰狀態時，粒子濾波器可能失敗［2-3］.為此，國內外學者對如何選取重要性密度函數做了大量的研究.文獻［4-7］分別提出了用擴展卡爾曼算法(EKF)、迭代擴展卡爾曼算法(IEKF)、無跡卡爾曼算法(UKF)和迭代無跡卡爾曼算法(IUKF)構建粒子算法的重要性密度函數，雖然均考慮了當前的觀測信息，提高了濾波精度，并且IEKF和IUKF算法基于高斯-牛頓迭代思想，利用觀測更新得到的狀態量對非線性量測方程重新進行線性化，從而減小線性化誤差，使得產生的重要性密度函數更加符合狀態變量的實際后驗概率分布，但是擴展卡爾曼粒子濾波(算法PF-EKF)和迭代擴展卡爾曼粒子濾波算法(PF-IEKF)需要計算雅克比矩陣，這不可避免會引入截斷誤差，使得狀態估計性能提高不大［4-5］，另外無跡卡爾曼粒子濾波算法(PF-UKF)和迭代無跡卡爾曼粒子濾波算法(PF-IUKF)只有在Sigma點集設計合理時，才能得到較好的濾波精度，而在狀態高維時若參數選擇不當將會引起自協方差非正定，導致濾波性能會降低甚至發散［8-11］.最近，提出了一種基于容積卡爾曼(CKF)［12-13］的粒子濾波算法——容積卡爾曼粒子濾波算法(CPF).該算法使用容積數值積分原則計算非線性隨機變量的均值和協方差，避免了求導運算，產生了粒子濾波算法的重要性密度函數，融入了當前的量測信息，使得重要性密度函數更加逼近后驗概率函數，其濾波性能得到較好改善，并且實時性較強［14-15］.

本文在CPF的基礎上提出一種基于Gauss-Newton迭代思想的粒子濾波算法.該算法利用當前量測信息，通過Gauss-Newton迭代方法對CKF算法進行改進，減小線性化誤差，以此來產生粒子濾波算法的重要性密度函數，使得迭代CKF產生的重要性密度函數更接近于真實后驗概率分布.仿真結果表明，迭代CKF粒子濾波具有更高的估計精度.

1 容積卡爾曼粒子濾波

容積卡爾曼濾波算法(CKF)采用基于容積原則以數值方式來解決高斯域貝葉斯濾波的積分問

式中，xk，yk分別為系統的狀態向量和量測向量;F(·)，H(·)分別為系統狀態轉移函數和系統量測函數;wk，vk分別為相互獨立的過程噪聲和量測噪聲.

容積粒子濾波算法的基本步驟可參考文獻［14-15］.題.它同UKF一樣，是一種確定性采樣高斯濾波方法，但是CKF相對UKF計算量小，能得到更加精確的濾波效果和穩定性能，且更容易實現［11］.

容積卡爾曼粒子濾波算法是將CKF應用于粒子濾波框架中，使用CKF來產生重要性密度函數，由于融入了當前的量測信息，使得從重要性密度函數中取樣得到的樣本更加逼近真實后驗概率密度采樣得到的樣本，提高了估計濾波的精度.

考慮如下的離散非線性系統

2 迭代容積卡爾曼粒子濾波

CPF的核心思想是利用CKF產生重要性密度函數，即基于容積原則通過以一組確定的點集和相應的權值來近似得到非線性函數的均值和協方差，以產生CPF的重要性密度函數，但由于量測方程一般為非線性的系統，基于此的做法可能造成量測值得不到正確的利用，從而引入誤差.基于Gauss-Newton的量測迭代算法可較好地改善這一問題.

Gauss-Newton迭代算法主要應用于求解非線性最小二乘問題，同時由于CKF是一種非線性次優高斯濾波器，即各個隨機量符合高斯分布，則狀態量測的最大后驗概率更新密度可表達為

求解上式的最大似然估計即是利用Gauss-Newton對下式“費用函數”的進行最小化求解:

結合Gauss-Newton迭代算法，可得到迭代迭代容積卡爾曼粒子濾波算法的基本形式，其具體步驟如下:

步驟1 初始化.從p(x0)中采樣粒子{=1，2，…，N}.

步驟2 每一時刻用迭代CKF算法更新采樣粒子，n=1，2，…，N，i=1，2，…，NIter.

步驟3 產生重要性密度函數

步驟4 計算歸一化權重

步驟6 輸出k時刻的狀態和方差估值

3 仿真結果及分析

對迭代容積卡爾曼粒子濾波在非線性非高斯系統中進行仿真驗證，并與PF算法和CPF算法進行分析比較.設系統模型的狀態方程和觀測方程:

式中，過程噪聲服從Gamma分布，wk～Gamma(3，2)，觀測噪聲服從正態分布vk～ N(0，0.0001)，初始狀態x0=1，粒子數N=200，重采樣部分采用殘差采樣法，仿真時間T=60.定義狀態的均方根誤差為

圖1給出了PF，CPF和迭代容積卡爾曼粒子濾波器的狀態估計曲線，圖2為3種算法的均方根估計誤差，表1為這3種算法的均方根誤差均值和方差及平均運行時間對比.

圖1 各濾波器的狀態估計曲線圖

圖2 各濾波器均方根估計誤差

表1 狀態均方根誤差的均值和方差

通過表1和圖2可以看出，由于在ICPF算法中其構造重要性密度函數時，將量測更新后的狀態估計值為參考點，對量測方程進行線性迭代處理，使得估計值更加準確，所以ICPF的估計性能明顯優于PF和CPF算法.而運行時間上僅僅略高于CPF算法，但跟蹤精度得到明顯的改善.

4 結論

基于Gauss-Newton迭代思想，提出了迭代容積卡爾曼粒子濾波算法.該算法利用當前量測信息，通過Gauss-Newton迭代方法對CKF算法進行改進，減小線性化誤差，以此來產生粒子濾波算法的重要性密度函數，使得迭代CKF產生的重要性密度函數更接近于真實后驗概率分布.仿真結果表明ICPF估計效能優于CPF，是一種有效的非線性估計算法.

References)

［1］Gordon N J，Salmond D J，Smith A F M.Novel approach to nonlinear/non-Gaussian Bayesian state estimation［J］.IEE Proceedings F(Radar and Signal Processing)，1993，140(2):107-113.

［2］Arulampalam M S，Maskell S，Gordon N.A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking［J］.IEEE Transactions on Signal Processing，2002，50(2):174-188.

［3］Doucet A，Godsilli S J，Abdrueu C.On sequential Monte Carlo sampling methods for Bayesian filtering［J］.Statistics and Computing，2000，10(3):197-208.

［4］De Freitas N，Niranjan M，Gee A H，et al.Sequential Monte Carlo methods to train neural network models［J］.Neural Computation，2000，12(4):955-993.

［5］李良群，姬紅兵，羅軍輝.迭代擴展卡爾曼粒子濾波器［J］.西安電子科技大學學報，2007，34(2):233-238.Li Liangqun，Ji Hongbing，Luo Junhui.Iterated extended Kalman particle filtering［J］.Journal of Xidian University，2007，34(2):233-238.(in Chinese)

［6］Van der Merwe R，Douce A，de Freitas N，et al.The unscented particle filter［R］.Cambridge，UK:Engineering Department，Cambridge University，2000:1-45.

［7］郭文艷，韓崇昭，雷明.迭代無跡Kalman粒子濾波的建議分布［J］.清華大學學報:自然科學版，2007，41(S2):1866-1869.Guo Wenyan，Han Chongzhao，Lei Ming.Particle distribution control for an iterated unscented Kalman particle filter［J］.Journal of Tsinghua University:Science and Technology，2007，41(S2):1866-1869.(in Chinese)

［8］Bell B M，Cathey F W.The iterated Kalman filter update as a Gauss-Newton method［J］.IEEE Transactions on Automatic Control，1993，38(2):294-297.

［9］Zhang Jungen，Ji Hongbing.IMM iterated extended Kalman particle filter based target tracking［J］.Journal of Electronics and Information Technology，2010，32(5):1116-1120.

［10］Arasaratnam I，Haykin S.A numerical integration perspective on Gaussian filters［J］.IEEE Transactions on Automatic Control，2009，54(8):1254-1269.

［11］王小旭，潘泉，黃鶴，等.非線性系統確定采樣型濾波算法綜述［J］.控制與決策，2012，27(6):801-812.Wang Xiaoxu，Pan Quan，Huang He，et al.Overview of deterministic sampling filtering algorithms for nonlinear system［J］.Control and Decision，2012，27(6):801-812.(in Chinese)

［12］Arasaratnam I，Haykin S.Cubature Kalman filters［J］.IEEE Transactions on Automatic Control，2009，54(6):1254-1269.

［13］Arasaratnam I，Haykin S，Hurd T R.Cubature Kalman filtering for continuous-discrete systems:theory and simulations［J］.IEEE Transactions on Signal Processing，2010，58(10):4977-4993.

［14］孫楓，唐李軍.Cubature粒子濾波［J］.系統工程與電子技術，2011，31(11):2554-2557.Sun Feng，Tang Lijun.Cubature particle filter［J］.Systems Engineering and Electronics，2011，31(11):2554-2557.(in Chinese)

［15］穆靜，蔡遠利，張俊敏.容積粒子濾波算法及其應用［J］.西安交通大學學報，2011，45(8):13-17.Mu Jing，Cai Yuanli，Zhang Junmin.Cubature particle filter and its application［J］.Journal of Xi'an Jiao Tong University，2011，45(8):13-17.(in Chinese)