一種用于解決非線性濾波問題的新型粒子濾波算法

1、一種用于解決非線性濾波問題的新型粒子濾波算法王法勝 趙清杰（北京理工大學計算機科學技術學院 北京 100081）摘 要 粒子濾波算法受到許多領域的研究人員的重視，該算法的主要思想是使用一個帶有權值的粒子集合來表示系統(tǒng)的后驗概率密度。在擴展卡爾曼濾波和Unscented卡爾曼濾波算法的基礎上，本文提出一種新型粒子濾波算法。首先用Unscented卡爾曼濾波器產(chǎn)生系統(tǒng)的狀態(tài)估計，然后用擴展卡爾曼濾波器重復這一過程并產(chǎn)生系統(tǒng)在k時刻的最終狀態(tài)估計。在實驗中，針對非線性程度不同的兩種系統(tǒng)，分別采用五種粒子濾波算法進行實驗。結果證明，本文所提出算法的各方面性能都明顯優(yōu)于其他四種粒子濾波算法。關鍵字 非線

2、性濾波;擴展卡爾曼濾波器;Unscented卡爾曼濾波器;粒子濾波器;MKPF1 引言本課題主要受北京理工大學基礎基金(200501F4210)及計算機科學技術基金資助,部分受到國家自然科學基金(60503050)資助.王法勝,男,1983年生,碩士研究生,主要研究方向為粒子濾波技術及其應用. Email: wangfashion. 趙清杰,女,1966年生,博士,副教授,碩士研究生導師.主要研究方向為智能控制技術,機器視覺,非線性濾波技術,醫(yī)學圖像處理等. Email: qingjie.zhao.眾所周知，卡爾曼濾波器1-2是解決線性高斯問題的最優(yōu)濾波方法，但是在現(xiàn)實世界中，人們所面臨的問題

3、大都是非線性非高斯的，因此非線性濾波問題是極為普遍的，許多領域都涉及到，其中包括統(tǒng)計信號處理、經(jīng)濟學、生物統(tǒng)計學，以及工程領域中的通信3、雷達跟蹤4、目標跟蹤5,6、汽車定位7、導航7-8、機器人定位9-12等等。 解決非線性濾波問題最為普遍的方法是擴展卡爾曼濾波器(extended Kalman filter, EKF)。但是該方法只適用于弱非線性的系統(tǒng)，對于強非線性系統(tǒng)，很容易導致發(fā)散。最近，研究人員提出一種新的用于解決非線性濾波問題的濾波器，它是基于這樣一種考慮：近似一種高斯分布要比近似任何一種非線性方程容易的多。他們將這種濾波器稱為Unscented卡爾曼濾波器(UKF)13-15。實

4、驗證明UKF給出的估計結果比EKF更準確，尤其是它能給出更精確的系統(tǒng)狀態(tài)方差估計。然而，UKF的使用具有一定的限制，它不適用于一般的非高斯分布的模型。解決非線性濾波問題的一種更新的方法是粒子濾波器(particle filter, PF)，其基本思想是用一組帶有權值的粒子集合來表示解決問題時需要的后驗概率密度16，然后用這一近似的表示來計算系統(tǒng)的狀態(tài)估計。在過去幾年里，粒子濾波器在許多領域取得了成功的應用。自粒子濾波器被第一次提出以來，經(jīng)過幾年的發(fā)展，現(xiàn)在已經(jīng)出現(xiàn)多種粒子濾波器，例如擴展卡爾曼粒子濾波器(EKPF) 13、Unscented粒子濾波器(UPF) 13、輔助粒子濾波器(Auxil

5、iary particle filter) 17、高斯粒子濾波器(Gaussian particle filter)18、高斯加和粒子濾波器(Gaussian sum particle filter)19、PARZEN粒子濾波器20，以及由我國的李良群提出的迭代擴展卡爾曼粒子濾波器(Iterated EKPF)21等等。在EKF和UKF的基礎上，本文提出一種新型粒子濾波算法，稱之為混合卡爾曼粒子濾波器(mixed Kalman particle filter, MKPF)。它將EKF、UKF一起作為建議分布。在時刻k，首先用Unscented卡爾曼濾波器產(chǎn)生系統(tǒng)的狀態(tài)估計，然后用擴展卡爾曼濾波

6、器重復這一過程并產(chǎn)生系統(tǒng)在k時刻的最終狀態(tài)估計。本文第2節(jié)介紹了PF的基本原理；第3節(jié)介紹了EKF和UKF；第4節(jié)結合EKF和UKF，提出了新的粒子濾波算法；第5節(jié)給出比較實驗結果；第6節(jié)為結論。2 粒子濾波器假設動態(tài)系統(tǒng)的狀態(tài)空間模型為：(1)(2)表示系統(tǒng)在k時刻所處的狀態(tài)，表示k時刻的測量向量，兩個函數(shù)和分別表示系統(tǒng)的狀態(tài)轉移函數(shù)和測量函數(shù)，分別表示系統(tǒng)的過程噪聲以及測量噪聲。粒子濾波算法最先由Gordon在文獻22中提出，它為離散時間的遞歸濾波問題提供了一種近似的貝葉斯解決方法，其基本思想是構造一個基于樣本的后驗概率密度函數(shù)。用表示系統(tǒng)后驗概率密度函數(shù)的粒子集合，其中, i=1,N是支

7、持樣本集，相應的權值為，且滿足，而表示到時刻k系統(tǒng)所有狀態(tài)的集合，所以時刻k的后驗密度可以近似表示為：(3)于是就有了一種表示真實后驗密度的離散帶權近似表示，而那些關于數(shù)學期望的復雜計算（通常帶有復雜的積分運算）就可以簡化為和運算了，如：(4)可以近似為：(5)許多粒子濾波器依賴于重要采樣技術，粒子權值就是根據(jù)重要采樣技術來選擇的23-24，因此，建議分布的設計就顯得非常重要。如果根據(jù)重要密度選擇粒子，那么粒子的權值可以定義為，(6)在時刻k-1，如果已經(jīng)得到k-1時刻后驗密度的近似表示的粒子集合，下一步就是用一個新的粒子集合來近似表示k時刻的后驗密度。為了得到一種遞歸的表示方法，可以將選擇的

8、重要密度函數(shù)因式分解為，(7)然后，通過將獲得的新狀態(tài)加入到已知的粒子集合中，得到新的粒子集合。根據(jù)貝葉斯規(guī)則，可以得到權值更新方程如下：=(8)將（7）和（8）代人（6），得到權值更新方程如下：(9)為了得到一種更為簡單的形式，假設（即假設方程(1)所描述的是一個一階馬爾可夫過程），這就意味著重要密度只取決于和，因此，修正的權值為，(10)基本粒子濾波算法的一個主要問題是退化問題，即經(jīng)過幾步迭代以后，除了極少數(shù)粒子外，其他的粒子權值小到可以忽略不計的程度。減少退化現(xiàn)象影響的方法一般有兩種，一是選擇好的重要密度函數(shù)，另一種是使用再采樣技術13,22-24。再采樣方法就是去除那些權值較小的粒子，

9、而復制權值較大的粒子。目前存在多種再采樣算法，如殘差采樣、最小方差采樣、多項式采樣等。本文使用殘差采樣算法。文獻13給出了標準粒子濾波器算法。3 擴展卡爾曼濾波器與Unscented卡爾曼濾波器3.1 擴展卡爾曼濾波器擴展卡爾曼濾波器中系統(tǒng)的狀態(tài)分布用高斯隨機變量（GRV）來表示。在某一時刻，EKF方法將系統(tǒng)的非線性方程在當前關于系統(tǒng)狀態(tài)x的估計處，展開成一階泰勒展式。EKF的具體算法參見文獻1,2。以EKF作為建議分布就得到了擴展卡爾曼粒子濾波器（EKPF）。由于EKF用泰勒展式將系統(tǒng)的非線性方程進行線性化，使得系統(tǒng)的非線性性質(zhì)不能得到很好的描述，該算法只能達到一階的精度。EKPF的具體算法

10、參見文獻13。3.2 Unscented卡爾曼濾波器(UKF)Unscented卡爾曼濾波器(UKF)也是一種遞歸式貝葉斯估計方法，它利用Unscented變換 (Unscented transformation，UT)方法，用一組確定的取樣點來近似后驗概率。但是UKF不必線性化非線性狀態(tài)方程和觀測方程，它直接利用非線性狀態(tài)方程來估算狀態(tài)向量的概率密度函數(shù)。UKF規(guī)定一組確定的取樣點，當狀態(tài)向量的概率密度函數(shù)是高斯的，利用這組取樣點能獲取高斯密度函數(shù)的均值和協(xié)方差。當高斯狀態(tài)向量經(jīng)由非線性系統(tǒng)進行傳遞時，對任何一種非線性系統(tǒng)，利用這組取樣點能獲取精確到三階矩的后驗均值和協(xié)方差。 Unscent

11、ed變換Unscented變換是計算進行非線性傳遞的隨機向量概率的一種方法15，它是基于這樣一種考慮：近似一種概率分布比近似一種任意的非線性方程或者非線性變換要容易的多14,15。設x是維的隨機向量，是一非線性函數(shù)z=g(x)，考慮將x通過非線性函數(shù)g傳遞，假定x的均值和協(xié)方差分別為和。為了計算關于z的統(tǒng)計量，我們首先選擇2+1個帶有權值的樣本點（也稱SIGMA點）,，使其能夠完全獲取隨機變量x的真實的均值和協(xié)方差。SIGMA點的選擇以及權值的確定是根據(jù)以下方程，(11)(12)其中是一個尺度調(diào)節(jié)因子，決定了我們選擇的SIGMA點在其均值附近的分布情況，通常將設置為一個很小的正值（如0.001

12、）。是次級尺度調(diào)節(jié)因子，通常設置為0，是用來結合關于x的分布的先驗知識（對于高斯分布，的最佳取值為2）。是矩陣平方根的第i行，表示第i個SIGMA點的權值，且滿足Wi=1。是用來計算均值(mean)的權值，是用來計算協(xié)方差(covariance)的權值，二者除在初始情況下不同外，在時都是一樣的。將每個SIGMA點通過非線性函數(shù)向前傳遞，(13)通過計算可以得到z的均值和協(xié)方差的估計，(14)(15) Unscented卡爾曼濾波器UKF是UT的直接擴展，系統(tǒng)的狀態(tài)分布仍然用一個高斯隨機變量來表示，用一個經(jīng)過選擇的SIGMA點的集合來具體表示，但是狀態(tài)隨機變量被重新定義為原始狀態(tài)和噪聲變量的擴張

13、向量(augmented)，上標T表示轉秩。方程(11)用來獲得相應的SIGMA矩陣。通常，EKF和UKF都使用高斯近似來表示先驗以及后驗密度。但是，UKF同樣能夠準確的獲得在先驗和后驗密度上的離散度。關于UKF算法的實現(xiàn)，請參考文獻13-15。在粒子濾波器的框架內(nèi)，以UKF作為建議分布，就得到了Unscented粒子濾波器，詳細的UPF算法請參考文獻13。最近，李良群21等人提出了一種迭代擴展卡爾曼粒子濾波器（IEKPF），該算法在仿真實驗中得到的效果要比標準例子濾波器以及EKPF和UPF的效果要好。在該算法中，將測量更新過程進行迭代，以獲取更加“真實”的估計量。本文提出的MKPF的效果比I

14、EKPF更好，下面進行詳細介紹。4 混合卡爾曼粒子濾波器MKPF采用混合的卡爾曼粒子濾波器作為建議分布，仿真實驗中得到的估計結果優(yōu)于其它幾種濾波算法，包括UPF和IEKPF。在時刻k，首先用UKF更新粒子，以獲得相應的狀態(tài)估計值，然后，分別計算系統(tǒng)模型與測量模型的雅可比矩陣，用EKF更新粒子，此時使用UKF已經(jīng)得到的狀態(tài)估計值作為k-1時刻的狀態(tài)估計，即，令。經(jīng)過計算得到k時刻最終的狀態(tài)及其相應的協(xié)方差的估計值和。從而可以從建議分布中抽取粒子。假設k-1時刻的狀態(tài)及相應協(xié)方差的估計分別為和，在下一時刻k，先使用UKF更新粒子。該過程中用到的SIGMA點的選擇根據(jù)方程：=(16)此后將SIGMA

15、點分別通過系統(tǒng)模型與測量模型向前傳遞，得到狀態(tài)及協(xié)方差的預測值：, (17)(18)(19)其中和是第j個SIGMA點的權值，。所以，預測的測量值的均值可以按以下方程計算得到：(20)得到新的測量值之后，更新預測狀態(tài)估計量如下：(21)其中為卡爾曼增益，按下面公式計算得到，(22)(23)這樣就獲得了狀態(tài)估計值。然后用EKF執(zhí)行粒子更新過程。首先預測狀態(tài)及協(xié)方差：f()(24)(25)據(jù)此求取卡爾曼增益：(26)修正預測量得到最終所需的估計量，如下：(27)(28)其中Q為系統(tǒng)噪聲的協(xié)方差，R為測量噪聲的協(xié)方差，和以及和分別為系統(tǒng)模型和測量模型的雅可比矩陣，最終求得的和就是我們要求的k時刻的估

16、計量。綜上所述，MKPF算法可以表示如下：算法 1: MKPF算法(1) 初始化: k=0For i=1N, 從初始先驗密度中抽取粒子，并設： (29)(2) FOR k=1,2,1）. FOR i=1,N:- 根據(jù)UKF更新粒子： 根據(jù)方程(16)計算所需的SIGMA點. 傳遞SIGMA點并計算處一步預測估計值：方程 (17) (20) 獲取新的測量值，根據(jù)方程(21)修正一步預測估計值，獲得修正的狀態(tài)估計。- 根據(jù)EKF更新粒子： 令=，據(jù)方程(24)(25)計算狀態(tài)及相應協(xié)方差的一步預測值。 分別求取系統(tǒng)模型及觀測模型的雅可比矩陣 和。 計算修正的狀態(tài)及協(xié)方差估計值：方程(27) (28

17、) 令，最終求得k時刻所需的估計量。- 得到近似服從高斯分布的建議分布，抽取樣本（粒子）- 根據(jù)方程(10)為每個樣本賦以權值。ENDFOR2）. FOR i=1,N 歸一化權值：。 ENDFOR3）.再采樣過程：a) 消除權值較小的粒子，復制權值較大的粒子，獲得N個隨機樣本，近似服從分布.b) 為每個再采樣之后的樣本粒子賦以相同的權值FOR i=1,N , =1/N. ENDFOR5 實驗結果這一部分給出本文提出的MKPF算法的實驗結果，并與其他幾種非線性濾波算法進行比較。共進行兩次實驗，分別采用不同的系統(tǒng)狀態(tài)空間模型。實驗一 假設非線性系統(tǒng)的狀態(tài)空間模型為：(30) (31)其中服從伽馬分

18、布，表示系統(tǒng)噪聲，測量噪聲服從高斯分布。實驗中使用的粒子數(shù)目為N=200個，觀測時間為T60，進行100次獨立實驗。UT參數(shù)設置為=1，=0，=2。算法的輸出為粒子集合的均值，計算公式如下： (32)一次獨立實驗的均方誤差為： (33)圖2給出了不同粒子濾波器進行一次獨立實驗所產(chǎn)生的狀態(tài)估計結果，可以看出，在某些時刻標準粒子PF與EKPF產(chǎn)生的估計偏離真實值較為嚴重，但是UPF、IEKPF以及MKPF所估計的狀態(tài)能較好地與真實狀態(tài)吻合，而MKPF算法的效果最為理想。表1給出了經(jīng)過100次獨立實驗、不同非線性濾波算法的均方誤差的均值以及方差，可以非常明顯地看出：MKPF算法的估計精度優(yōu)于其他幾種

19、粒子濾波算法。表1. 100次獨立實驗后不同非線性濾波算法產(chǎn)生的均方誤差的均值與方差（實驗一）AlgorithmMSEmeanvarEKF0.369690.0161216UKF0.268120.0107PF0.190890.041927EKPF0.290280.015819UPF0.0494930.0045229IEKPF0.0439650.0010825MKPF0.0156540.0004159圖3給出了100次獨立實驗之后每個時刻產(chǎn)生的均方誤差的曲線，同樣表明了MKPF算法的精度優(yōu)于其他幾種粒子濾波算法。圖2. 不同非線性濾波算法產(chǎn)生的狀態(tài)估計曲線（實驗一）圖3. 不同粒子濾波器產(chǎn)生的均方

20、誤差曲線（實驗一）實驗二 假設非線性系統(tǒng)的狀態(tài)空間模型為：(34)(35)可以看出，該系統(tǒng)具有更強的非線性。系統(tǒng)噪聲與測量噪聲的分布與實驗一相同，其他參數(shù)設置同實驗一。與實驗一對應，圖4為獨立運行一次產(chǎn)生的狀態(tài)估計的均值與真實狀態(tài)的對比?？梢钥吹?，在幾種粒子濾波器中，EKPF對于非線性系統(tǒng)的適應性是最差的。以上幾種非線性濾波算法估計性能比較表如表2所示。經(jīng)過100次獨立運行實驗后，各粒子濾波器產(chǎn)生的均方誤差曲線如圖5，可以看到MKPF算法的均方誤差曲線除在局部產(chǎn)生波動外，其總體誤差小于其它算法，這同樣反應在表2中。同樣可以看出MKPF算法得出的估計結果優(yōu)于其它幾類非線性濾波算法。表2. 100

21、次獨立實驗后不同非線性濾波算法產(chǎn)生的均方誤差的均值與方差（實驗二）AlgorithmMSEmeanvarEKF0.551510.024951UKF0.421190.021457PF0.364450.069802EKPF0.480430.02831UPF0.119940.015088IEKPF0.056510.0015319MKPF0.0356260.0033575圖4. 不同非線性濾波算法產(chǎn)生的狀態(tài)估計曲線（實驗二）圖5. 不同粒子濾波器產(chǎn)生的均方誤差曲線（實驗二）6 結論粒子濾波器在解決非線性非高斯濾波問題中取得了非常好的效果。綜合以上論述與實驗，本文提出的新型粒子濾波器MKPF，該算法采用

22、EKF與UKF混合作為建議分布，得到一種更接近真實分布的近似表達方式。實驗結果表明，新算法明顯優(yōu)于其它幾種粒子濾波算法。MKPF算法的提出為非線性濾波領域中的問題提供了一種新的解決方法。在下一步工作中，我們將用MKPF算法解決目標跟綜問題、以及非線性系統(tǒng)的參數(shù)估計問題。參考文獻：1. Anderson B. D.O., Moore J. B. Optimal filtering. Prentice-Hall, 1979.2. Welch G., Bishop G. An introduction to the kalman filer. University of North Carolina

23、 at Chapel Hill: Technical Report, TR 95-041, 2004.3. Djuric P. M., Kotecha J. H., et al. Particle filtering. IEEE Signal Processing Magazine, Sep. 2003, 20(5):1938.4. Herman S.C., Moulin P. A particle filtering approach to joint radar tracking and automatic target recognition. In: Proceedings of IE

24、EE Aerospace Conference, Big Sky, Montana, 2002, 4:17891808.5. Deng Xiaolong, Xie Jianying, Guo Weizhong. Bayesian target tracking based on particle filter. Journal of Systems Engineering and Electronics, 2005, 16(3):545549.6. Chang Cheng, Ansari Rashid. Kernel particle filter for visual tracking. I

25、EEE Signal Processing Letters, 2005, 12(3): 242245.7. Gustafsson F., Gunnarsson F., et al. Particle filters for positioning, navigation and tracking. IEEE Transactions on Signal Processing, 2002, 50(2):425437.8. Kalsson Rickard. Particle filtering for positioning and tracking applications D. Departm

26、ent of Electrical Engineering, Linköpings Universitet, 2005.9. Fox D., Hightower J., Liao L., Schulz D., Borriello G. Bayesian filtering for location estimation. IEEE Pervasive Computing, 2003, 2(3):2433.10. Rekleitis I. M. A particle filter tutorial for mobile robot localization. Montreal, Que

27、bec, Canada: Centre for Intelligent Machines, McGill University, Technical Report: TM-CIM-04-02, 2004.11. Thrun S., Fox D., Burgard W., Dellaert F. Robust monte carlo localization for mobile robots. Artificial Intelligence, 2001, 99141.12. Kwok C., Fox D., Meila M. Real-Time particle filters. Procee

28、dings of the IEEE, 2004, 92(3):469484.13. Merwe R., Doucet A., Freitas Nando de, Wan Eric. The unscented particle filter. Cambridge University, engineering department: Technical report, CUED/ F-INFENG/ TR 380, 2000.14. Julier S. J., Uhlmann J. K. Unscented filtering and nonlinear estimation. Proceed

29、ings of the IEEE, 2004, 92(3):401422.15. Wan Eric A., Merwe R. The unscented kalman filter for nonlinear estimation. In: Proceedings of International Symposium on Adaptive Systems for Signal Processing, Communications and Control, Alberta, Canada, 2000, 153158.16. YUAN Ze-jian, ZHENG Nan-ning, JIA X

30、in-chun. The Gauss-Hermite Particle Filter, Acta Electronica Sinica, 2003, 31(7):970973. (in Chinese)袁澤劍,鄭南寧,賈新春. 高斯-厄米特粒子濾波器. 電子學報, 2003, 31(7):970973.17. Pitt M. K., Shephard N. Filtering via simulation: auxiliary particle filters. Journal of the American Statistical Association, 1999, 94(2):59059

31、9.18. Kotecha Jayesh H., Djuric Petar M. Gaussian particle filtering. IEEE Transactions on signal processing, Oct. 2003, 51(10): 25922601.19. Jayesh H. Kotecha, Petar M. Djuric. Gaussian sum particle filtering. IEEE Transactions on signal processing, Oct. 2003, 51(10): 26022611.20. Lehn-Schiler T.,

32、Erdogmus D., Principe J. C. Parzen particle filters. In: Proceedings of International Conference on Acoustics, Speech, and Signal Processing, Montreal, Canada, 2004, V: 781784.21. Li Liang-qun, Ji Hong-bing, Luo Jun-hui. The iterated extended kalman particle filter. In: Proceedings of International

33、Symposium on Communications and Information Technologies, Beijing, China, 2005, 11721175.22. Gordon N.J., Salmond D.J., Smith . Novel approach to nonlinear/non-gaussian bayesian state estimation. IEE Proceedings on Radar and Signal Processing, 1993. 140(2):107113.23. Arulampalam M. S., Maskell S., G

34、ordon N., Clapp T. A tutorial on particle filters for on-line nonlinear/non-gaussian bayesian tracking. IEEE Transactions on Signal Processing, 2002, 50(2):174188.24. Doucet A. On sequential simulation-based methods for bayesian filtering. University of Cambridge: Technical Report, CUED/F-INFENG/TR.

35、310, 1998.A New Particle Filter for Nonlinear Filtering ProblemsWANG Fa-Sheng, ZHAO Qing-Jie(School of Computer Science & Technology, Beijing Institute of Technology, Beijing 100081)Abstract Particle filters have gained special attention by researchers in various fields. The key idea of this tec

36、hnique is to represent the posterior density by sets of weighed samples. In this paper, we propose a new particle filter which is based on the extended Kalman filter and the unscented Kalman filter. It first uses the former to generate an estimate of the state at time k, and then uses the latter to

37、repeat the process and to gain the final estimate of the state and corresponding covariance at time k. In the experiments, we test five different particle filters on two different nonlinear systems. The experimental results indicate that the proposed particle filter shows much better performance tha

38、n the other four particle filters do.Keywords nonlinear filtering; extended Kalman filter; unscented Kalman filter; mixed Kalman particle filter作者簡介：王法勝,男,1983年生,碩士研究生,主要研究方向為粒子濾波技術及其應用. Email: 趙清杰,女,1966年生,博士,副教授,碩士研究生導師.主要研究方向為智能控制技術,機器視覺,非線性濾波技術,醫(yī)學圖像處理等. . 電話:ANG Fa-Sheng, born in 1

39、983, Master. His research interests mainly focus on particle filtering technique and corresponding application in computer vision.ZHAO Qing-Jie, born in 1966, Ph. D., associate professor. Her research interests include intelligent controlling technique, machine vision, nonlinear filtering technique, medical image processing, etc.Background Nonlinear filtering problems generally exist in many fields such as robot, artificial intelligence, biomedicine, computer vision, statistics, industrial control and sig