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

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

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

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

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

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

1 容積卡爾曼粒子濾波

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

式中，xk，yk分別為系統(tǒng)的狀態(tài)向量和量測向量;F(·)，H(·)分別為系統(tǒng)狀態(tài)轉(zhuǎn)移函數(shù)和系統(tǒng)量測函數(shù);wk，vk分別為相互獨(dú)立的過程噪聲和量測噪聲.

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

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

考慮如下的離散非線性系統(tǒng)

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

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

Gauss-Newton迭代算法主要應(yīng)用于求解非線性最小二乘問題，同時(shí)由于CKF是一種非線性次優(yōu)高斯濾波器，即各個(gè)隨機(jī)量符合高斯分布，則狀態(tài)量測的最大后驗(yàn)概率更新密度可表達(dá)為

求解上式的最大似然估計(jì)即是利用Gauss-Newton對下式“費(fèi)用函數(shù)”的進(jìn)行最小化求解:

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

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

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

步驟3 產(chǎn)生重要性密度函數(shù)

步驟4 計(jì)算歸一化權(quán)重

步驟6 輸出k時(shí)刻的狀態(tài)和方差估值

3 仿真結(jié)果及分析

對迭代容積卡爾曼粒子濾波在非線性非高斯系統(tǒng)中進(jìn)行仿真驗(yàn)證，并與PF算法和CPF算法進(jìn)行分析比較.設(shè)系統(tǒng)模型的狀態(tài)方程和觀測方程:

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

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

圖1 各濾波器的狀態(tài)估計(jì)曲線圖

圖2 各濾波器均方根估計(jì)誤差

表1 狀態(tài)均方根誤差的均值和方差

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

4 結(jié)論

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

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］李良群，姬紅兵，羅軍輝.迭代擴(kuò)展卡爾曼粒子濾波器［J］.西安電子科技大學(xué)學(xué)報(bào)，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］.清華大學(xué)學(xué)報(bào):自然科學(xué)版，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］王小旭，潘泉，黃鶴，等.非線性系統(tǒng)確定采樣型濾波算法綜述［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］.系統(tǒng)工程與電子技術(shù)，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］穆靜，蔡遠(yuǎn)利，張俊敏.容積粒子濾波算法及其應(yīng)用［J］.西安交通大學(xué)學(xué)報(bào)，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)