擴(kuò)展卡爾曼濾波與粒子濾波性能對(duì)比

薛長(zhǎng)虎，聶桂根，汪　晶

(武漢大學(xué)衛(wèi)星導(dǎo)航定位技術(shù)研究中心，湖北 武漢 430079)

?

擴(kuò)展卡爾曼濾波與粒子濾波性能對(duì)比

薛長(zhǎng)虎，聶桂根，汪晶

(武漢大學(xué)衛(wèi)星導(dǎo)航定位技術(shù)研究中心，湖北 武漢 430079)

Comparison of Performance between Extended Kalman Filter and Particle Filter

XUE Changhu,NIE Guigen,WANG Jing

摘要：在非線性濾波方法中，粒子濾波以其精度均勻且不受模型復(fù)雜程度的影響而受到廣泛關(guān)注。與傳統(tǒng)的非線性濾波方法——擴(kuò)展卡爾曼濾波相比，粒子濾波有著無(wú)可替代的優(yōu)勢(shì)，但也存在著一些缺陷。本文對(duì)這兩種非線性濾波算法分別用MATLAB和VC++編程進(jìn)行模擬仿真，并進(jìn)行了一系列的比較，從算法原理、計(jì)算精度和穩(wěn)定性、計(jì)算速度等方面說(shuō)明了各自的優(yōu)缺點(diǎn)和具體的差別。試驗(yàn)結(jié)果表明，粒子濾波計(jì)算消耗的時(shí)間遠(yuǎn)遠(yuǎn)大于擴(kuò)展卡爾曼濾波消耗的時(shí)間，并且粒子濾波計(jì)算時(shí)間隨粒子數(shù)的增加呈近似級(jí)數(shù)增長(zhǎng)。

關(guān)鍵詞：非線性系統(tǒng)；擴(kuò)展卡爾曼濾波；粒子濾波

近年來(lái)，對(duì)濾波方法的研究是科學(xué)界的一個(gè)熱門(mén)課題。在系統(tǒng)的數(shù)學(xué)模型比較簡(jiǎn)單且預(yù)測(cè)值服從高斯分布的情況下，卡爾曼濾波提供了一種幾乎完美的求解最優(yōu)估計(jì)的算法，并且在航空航天、導(dǎo)航與制導(dǎo)、目標(biāo)識(shí)別與跟蹤、測(cè)繪、金融等領(lǐng)域得到了非常廣泛的應(yīng)用。但在實(shí)際情況中，我們所遇到的系統(tǒng)通常會(huì)復(fù)雜得多，這些系統(tǒng)的數(shù)學(xué)模型往往具有非線性和模型不確定性等特點(diǎn)[1]。非線性濾波的研究已經(jīng)成為一個(gè)熱點(diǎn)話題，先后出現(xiàn)了擴(kuò)展卡爾曼濾波、無(wú)味卡爾曼濾波、粒子濾波(又稱自舉濾波)等方法。其中，擴(kuò)展卡爾曼濾波(extended Kalman filter, EKF)和粒子濾波(particle filter, PF)是目前研究最多、應(yīng)用最廣的非線性濾波算法。本文將從算法(包括系統(tǒng)模型、原理)和性能(包括計(jì)算精度、運(yùn)算速度)等方面對(duì)這兩種濾波算法進(jìn)行對(duì)比。

一、擴(kuò)展卡爾曼濾波和粒子濾波算法對(duì)

1. 系統(tǒng)模型

一個(gè)非線性離散系統(tǒng)的數(shù)學(xué)模型的一般形式可以表示為

(1)其中，上面為系統(tǒng)的狀態(tài)更新方程，xk為第k時(shí)刻系統(tǒng)的狀態(tài)向量；wk-1為第k-1時(shí)刻系統(tǒng)的過(guò)程的平穩(wěn)噪聲序列；fk為系統(tǒng)狀態(tài)轉(zhuǎn)移函數(shù)，是狀態(tài)xk-1的非線性(或線性)函數(shù)。下面為系統(tǒng)的觀測(cè)方程，zk為系統(tǒng)的觀測(cè)向量；vk為觀測(cè)的平穩(wěn)噪聲序列；hk為系統(tǒng)觀測(cè)函數(shù)，是狀態(tài)xk的非線性(或線性)函數(shù)。

從式(1)可以看出，狀態(tài)轉(zhuǎn)移函數(shù)和觀測(cè)函數(shù)沒(méi)有做出線性或非線性的假設(shè)條件，過(guò)程噪聲和觀測(cè)噪聲也沒(méi)有假設(shè)為高斯白噪聲序列。本文將在以下章節(jié)分別對(duì)擴(kuò)展卡爾曼濾波和粒子濾波對(duì)模型的處理過(guò)程進(jìn)行總結(jié)。

2. 擴(kuò)展卡爾曼濾波模型與算法

(2)

式中

(3)

再根據(jù)傳統(tǒng)的卡爾曼濾波理論，可得到擴(kuò)展卡爾曼濾波算法遞推公式

(4)

擴(kuò)展卡爾曼濾波實(shí)際上是利用上一時(shí)刻的狀態(tài)估計(jì)值來(lái)預(yù)測(cè)當(dāng)前時(shí)刻的狀態(tài)，利用觀測(cè)值進(jìn)行修正。其算法簡(jiǎn)單，計(jì)算量小，易于在計(jì)算機(jī)上進(jìn)行模擬試驗(yàn)。但由于擴(kuò)展卡爾曼濾波使用了泰勒級(jí)數(shù)展開(kāi)，只保留了一次項(xiàng)，因此該算法得到的結(jié)果并不是最優(yōu)估計(jì)，總會(huì)存在著一些系統(tǒng)誤差，并且更新方程和觀測(cè)方程的非線性越強(qiáng)，圖像彎曲程度越大，預(yù)測(cè)結(jié)果的誤差就會(huì)越大。另外，隨機(jī)模型的構(gòu)建也非常關(guān)鍵，如果構(gòu)建的隨機(jī)模型與準(zhǔn)確性較差會(huì)直接影響濾波結(jié)果的質(zhì)量。

3. 粒子濾波的模型與算法

粒子濾波算法是一種基于貝葉斯采樣估計(jì)的序貫重要性采樣濾波方法，其核心是用一組帶有權(quán)值的隨機(jī)樣本來(lái)近似表征后驗(yàn)概率密度函數(shù)，從而求得狀態(tài)的最小方差估計(jì)[5-7]。

狀態(tài)預(yù)測(cè)方程

(5)

狀態(tài)更新方程

(6)

式中

(7)

然而該過(guò)程計(jì)算非常復(fù)雜，特別是式中存在的積分運(yùn)算，使得該過(guò)程難以實(shí)現(xiàn)。1993年，Gordon和Salmond提出基于序貫重要性采樣(SIS)思想的自舉濾波算法，借助蒙特卡洛方法，將積分運(yùn)算離散化，轉(zhuǎn)變?yōu)閷?duì)帶權(quán)樣本求和的運(yùn)算過(guò)程[2,9]。

那么狀態(tài)的后驗(yàn)期望為

(8)

但隨著時(shí)間的增加，大多數(shù)粒子的權(quán)值逐漸減小并趨于零，這就會(huì)出現(xiàn)粒子退化問(wèn)題，即重要性權(quán)值會(huì)集中在極少數(shù)的粒子上，僅靠這些粒子難以有效表達(dá)出后驗(yàn)概率密度函數(shù)，而權(quán)值小的粒子經(jīng)過(guò)同樣復(fù)雜的計(jì)算過(guò)程，其對(duì)計(jì)算結(jié)果的影響微乎其微，甚至可以忽略不計(jì)。為了解決這個(gè)問(wèn)題，Gordon等提出了重采樣方法。目前常用的處理方式是計(jì)算有效粒子數(shù)Neff，定義為

(9)

如果Neff

粒子濾波實(shí)質(zhì)上是將連續(xù)的概率密度函數(shù)離散化，通過(guò)計(jì)算離散樣本的加權(quán)和來(lái)近似計(jì)算狀態(tài)后驗(yàn)估計(jì)值的過(guò)程。粒子濾波的優(yōu)點(diǎn)在于它可以適用于任何系統(tǒng)，并沒(méi)有線性或高斯分布的前提條件，精度與模型和分布也無(wú)關(guān)。因此與擴(kuò)展卡爾曼濾波相比，粒子濾波的適應(yīng)性、可靠性更強(qiáng)，精度更均勻。但是粒子濾波的數(shù)學(xué)過(guò)程比較復(fù)雜，計(jì)算困難，并且算法的精度與所選取的重要性函數(shù)和粒子數(shù)量有關(guān)。故粒子濾波在近年來(lái)計(jì)算機(jī)技術(shù)迅速發(fā)展的時(shí)代才得到一些實(shí)際應(yīng)用。

二、擴(kuò)展卡爾曼濾波和粒子濾波仿真試驗(yàn)與性能對(duì)比

設(shè)有如下非線性模型

(10)

式中，wk和vk為零均值高斯白噪聲，方差分別為10.0和1.0。該狀態(tài)更新方程和觀測(cè)方程都是高度非線性的，下面分別使用擴(kuò)展卡爾曼濾波和粒子濾波算法對(duì)該模型進(jìn)行50步估計(jì)，并從計(jì)算精度和速度方面對(duì)兩種方法的性能進(jìn)行比較。

1. 計(jì)算精度的比較

試驗(yàn)選用最新版Matlab軟件(Matlab 2014a)進(jìn)行模擬，取初值x0=0.1，粒子數(shù)N=500。圖1為擴(kuò)展卡爾曼濾波和粒子濾波估計(jì)結(jié)果，狀態(tài)真實(shí)值大概介于-20～+20之間。從圖中可以看出，粒子濾波的估計(jì)結(jié)果與真實(shí)值更為接近，除了少數(shù)幾個(gè)偏差較大的點(diǎn)，絕大多數(shù)估計(jì)值都很接近真實(shí)值，而擴(kuò)展卡爾曼濾波在很多時(shí)刻都會(huì)出現(xiàn)偏離真實(shí)值較大的情況，并且偏離的幅度也比粒子濾波大得多。為了更直觀地顯示出二者的差異繪制了擴(kuò)展卡爾曼濾波和粒子濾波的估計(jì)結(jié)果與真實(shí)值的誤差折線圖，如圖2所示。可以看出，粒子濾波計(jì)算結(jié)果的誤差大小和方差明顯優(yōu)于擴(kuò)展卡爾曼濾波。在系統(tǒng)非線性較強(qiáng)的時(shí)刻，擴(kuò)展卡爾曼濾波算法得到的結(jié)果誤差大小能達(dá)到粒子濾波誤差的幾倍甚至十幾倍。

圖1　擴(kuò)展卡爾曼濾波與粒子濾波估計(jì)結(jié)果

圖2　擴(kuò)展卡爾曼濾波與粒子濾波估計(jì)值與真實(shí)值的差

為了使試驗(yàn)結(jié)果更有說(shuō)服性，筆者用Visual C++語(yǔ)言進(jìn)行了同樣的仿真過(guò)程，使用的編譯器為微軟發(fā)布的Visual Studio 2010，設(shè)置同樣的初始條件，將計(jì)算結(jié)果導(dǎo)入到數(shù)據(jù)文件中，再借助Matlab繪圖功能繪出狀態(tài)估計(jì)折線圖(如圖3所示)和誤差折線圖(如圖4所示)，可以得到與Matlab仿真類似的結(jié)果?？梢钥闯?，在高度非線性的系統(tǒng)中，粒子濾波算法的精度和穩(wěn)定性明顯優(yōu)于擴(kuò)展卡爾曼濾波。

2. 計(jì)算速度對(duì)比

將擴(kuò)展卡爾曼濾波與粒子濾波的仿真代碼用兩個(gè)腳本分別實(shí)現(xiàn)，運(yùn)行并計(jì)時(shí)，比較二者的計(jì)算時(shí)間。筆者使用的計(jì)算機(jī)CPU型號(hào)為Intel Core i5雙核處理器，主頻2.60 GHz，內(nèi)存大小為8 GB，Windows 8操作系統(tǒng)。將擴(kuò)展卡爾曼濾波和粒子濾波仿真代碼分別運(yùn)行10次，并記下每次運(yùn)行的時(shí)間。粒子數(shù)不是很多時(shí)，VC++運(yùn)行兩種算法時(shí)間差別不大，而Matlab運(yùn)行時(shí)間差異比較明顯，見(jiàn)表1。

圖3　用VC++實(shí)現(xiàn)的狀態(tài)估計(jì)結(jié)果

圖4　用VC++實(shí)現(xiàn)的計(jì)算結(jié)果誤差

s

進(jìn)行同樣的50步估計(jì)過(guò)程，卡爾曼濾波只需將每一步的結(jié)果進(jìn)行迭代計(jì)算來(lái)得到下一步的先驗(yàn)預(yù)測(cè)，再用觀測(cè)值進(jìn)行修正即可；而粒子濾波需要將每一步的每個(gè)粒子進(jìn)行迭代計(jì)算，并進(jìn)行重采樣，最后將所有的粒子取加權(quán)和得到最終結(jié)果，實(shí)際上每一個(gè)粒子的計(jì)算過(guò)程相當(dāng)于一個(gè)小的卡爾曼濾波過(guò)程，粒子濾波的計(jì)算量遠(yuǎn)遠(yuǎn)大于擴(kuò)展卡爾曼濾波。因此，粒子濾波計(jì)算用的時(shí)間與擴(kuò)展卡爾曼濾波有著比較明顯的差距。

從粒子濾波的運(yùn)算過(guò)程中可以看出，粒子濾波的計(jì)算量與所采樣的粒子數(shù)量密切相關(guān)。筆者統(tǒng)計(jì)了仿真試驗(yàn)過(guò)程中粒子數(shù)量對(duì)粒子濾波程序運(yùn)行時(shí)間的影響，見(jiàn)表2。

表2　粒子濾波不同粒子數(shù)運(yùn)算時(shí)間統(tǒng)計(jì)　s

可見(jiàn)，采樣的粒子數(shù)量對(duì)粒子濾波運(yùn)算速度的影響是十分顯著的。當(dāng)粒子數(shù)較少時(shí)(如100或200)可以較快地得到計(jì)算結(jié)果，Matlab仿真模擬可以在1 s內(nèi)完成計(jì)算和繪圖工作，VC++計(jì)算時(shí)間幾乎看不出有差異；隨著粒子數(shù)的逐漸增長(zhǎng)，Matlab程序和VC++程序的運(yùn)行時(shí)間都會(huì)有比較明顯的增加；當(dāng)粒子數(shù)達(dá)到5000時(shí)，Matlab程序的計(jì)算時(shí)間能達(dá)到近3 min，VC++程序運(yùn)算時(shí)間也達(dá)到了幾秒。圖5和圖6分別顯示了Matlab程序和VC++程序計(jì)算時(shí)間隨粒子數(shù)的變化趨勢(shì)，可見(jiàn)程序計(jì)算時(shí)間與粒子數(shù)量并不是呈線性變化的，而是呈近似拋物線的變化趨勢(shì)。

圖5　Matlab程序計(jì)算時(shí)間隨粒子數(shù)的變化

圖6　VC++程序計(jì)算時(shí)間隨粒子數(shù)的變化

針對(duì)以上試驗(yàn)結(jié)論，筆者另外選取了一組非線性模型進(jìn)行了驗(yàn)證。系統(tǒng)模型[17]為

對(duì)該系統(tǒng)進(jìn)行同樣過(guò)程的模擬試驗(yàn)，濾波結(jié)果對(duì)比、誤差對(duì)比及粒子濾波時(shí)間變化等得到與上述仿真試驗(yàn)相似的結(jié)果(如圖7—圖9所示)，由此可以驗(yàn)證上述結(jié)論。

圖7　兩種濾波算法估計(jì)結(jié)果對(duì)比

圖8　兩種濾波算法結(jié)果誤差對(duì)比

然而在實(shí)際工程應(yīng)用中，系統(tǒng)模型是極其復(fù)雜的，并且絕大多數(shù)系統(tǒng)是多維的，狀態(tài)量和觀測(cè)量都是多維向量，為了使最后計(jì)算結(jié)果達(dá)到精度和穩(wěn)定性等方面的要求，通常需要采集幾萬(wàn)個(gè)粒子來(lái)計(jì)算，其計(jì)算量是非常龐大的，特別是在需要實(shí)時(shí)計(jì)算數(shù)據(jù)的領(lǐng)域，如組合導(dǎo)航、目標(biāo)跟蹤等，普通計(jì)算機(jī)的計(jì)算速度是遠(yuǎn)遠(yuǎn)達(dá)不到要求的，因此粒子濾波的實(shí)用性就降低了很多。有學(xué)者提出了很多相關(guān)的改進(jìn)算法來(lái)改進(jìn)粒子濾波的計(jì)算精度和速度，如Freitas等提出擴(kuò)展卡爾曼粒子濾波算法的思想[9,12]，Merwe等提出無(wú)味粒子濾波算法[13]，Pitt等提出輔助粒子濾波算法[14]，蔣蔚博士等提出了支持向量機(jī)概率密度估計(jì)粒子濾波和支持向量回歸機(jī)粒子濾波等方法[15-16]。這些方法都在一定程度上改善了粒子濾波的性能和精度，但仍然沒(méi)有使粒子濾波廣泛地應(yīng)用于實(shí)時(shí)計(jì)算的領(lǐng)域。

圖9　Matlab程序計(jì)算時(shí)間隨粒子數(shù)的變化

三、結(jié)束語(yǔ)

粒子濾波具有不受系統(tǒng)模型的限制、對(duì)被估計(jì)狀態(tài)的后驗(yàn)分布不做任何假設(shè)及在非線性系統(tǒng)濾波中精度均勻、穩(wěn)定性好等特性，因此具有其他估計(jì)方法不具備的優(yōu)勢(shì)，但因其計(jì)算過(guò)程中重要性采樣、粒子退化和計(jì)算量等問(wèn)題導(dǎo)致其實(shí)現(xiàn)起來(lái)比較困難、實(shí)時(shí)性差，因此粒子濾波的適用領(lǐng)域非常有限。試驗(yàn)表明，同樣的非線性動(dòng)態(tài)系統(tǒng)，粒子濾波的精度和穩(wěn)定性優(yōu)于擴(kuò)展卡爾曼濾波，但其運(yùn)行消耗的時(shí)間要遠(yuǎn)遠(yuǎn)大于擴(kuò)展卡爾曼濾波；并且，粒子濾波的運(yùn)行時(shí)間隨粒子數(shù)量的增加呈近似拋物線增長(zhǎng)，粒子數(shù)越多，消耗的時(shí)間增加越快。

目前大部分領(lǐng)域在非線性濾波中都還使用算法簡(jiǎn)單、計(jì)算方便的擴(kuò)展卡爾曼濾波方法，然而隨著各領(lǐng)域?qū)V波計(jì)算精度和穩(wěn)定性等方面的要求越來(lái)越高，粒子濾波算法將會(huì)逐漸改進(jìn)和完善，并將應(yīng)用于各種科研和工程領(lǐng)域中。

參考文獻(xiàn)：

[1]趙琳, 王小旭, 丁繼成，等. 組合導(dǎo)航系統(tǒng)非線性濾波算法綜述[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2009(1):46-52.

[2]GORDON N J, SALMOND D J, SMITH A F M. Novel Approach to Nonlinear/Non-Gaussian Bayesian State Estimation[C]∥IEE Proceedings F (Radar and Signal Processing). [S.l.]: IET Digital Library, 1993.

[3]毛克誠(chéng), 孫付平. 擴(kuò)展卡爾曼濾波與采樣卡爾曼濾波性能比較[J]. 海洋測(cè)繪, 2006(5):4-6.

[4]HAYKIN S E. Kalman Filtering and Neural Networks [M]. New York: John Wiley & Sons, 2001.

[5]胡士強(qiáng), 敬忠良. 粒子濾波原理及其應(yīng)用[M]. 北京: 科學(xué)出版社, 2010: 21-25.

[6]于金霞,劉文靜,湯永利. 粒子濾波重采樣算法研究[J]. 微計(jì)算機(jī)信息, 2010(16)：44-45.

[7]胡士強(qiáng), 敬忠良. 粒子濾波算法綜述[J]. 控制與決策, 2005(4)：361-371.

[8]CHEN Z. Bayesian Filtering: From Kalman Filters to Particle Filters, and Beyond [J]. Statistics, 2003, 182(1): 1-6.

[9]DOUCET A, GODSILL S, ANDRIEU C. On Sequential Monte Carlo Sampling Methods for Bayesian Filtering [J]. Statistics and Computing, 2000, 10(3): 197-208.

[10]程水英, 張劍云. 粒子濾波評(píng)述[J]. 宇航學(xué)報(bào), 2008(4)：1099-1111.

[11]ARULAMPALAM M S, 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.

[12]FREITAS de GOMES,FERDINAN DO J. Bayesian Methods for Neural Networks[D]. [S.l.]: University of Cambridge, 2003.

[13]MERWE der R V,FREITAS de N,DOUCET A, et al. The Unscented Particle Filter[J].NIPS, 2001:13: 584-590.

[14]PITT M K, SHEPHARD N. Filtering via Simulation: Auxiliary Particle Filters [J]. Journal of the American Statistical Association, 1999, 94(446): 590-599.

[15]JIANG W, YI G, ZENG Q. Application of Proximal Support Vector Regression to Particle Filter[C]∥Intelligent Computing and Intelligent Systems, 2009. [S.l.]：IEEE, 2009.

[16]蔣蔚. 粒子濾波改進(jìn)算法研究與應(yīng)用[D].哈爾濱：哈爾濱工業(yè)大學(xué), 2010.

[17]杜航原, 郝燕玲, 趙玉新. 基于集合卡爾曼濾波的改進(jìn)粒子濾波算法[J]. 系統(tǒng)工程與電子技術(shù), 2011, 33(7): 1653-1657.

中圖分類號(hào)：P228

文獻(xiàn)標(biāo)識(shí)碼：B

文章編號(hào)：0494-0911(2016)04-0010-05

通信作者：聶桂根

作者簡(jiǎn)介：薛長(zhǎng)虎(1992—)，男，碩士生，主要研究方向?yàn)槎嘣从^測(cè)數(shù)據(jù)與滑坡機(jī)理模型同化理論與方法。E-mail：xch073423@163.com

基金項(xiàng)目：國(guó)家重點(diǎn)基礎(chǔ)研究發(fā)展計(jì)劃(2013CB733205)

收稿日期：2015-05-29

引文格式： 薛長(zhǎng)虎，聶桂根，汪晶. 擴(kuò)展卡爾曼濾波與粒子濾波性能對(duì)比[J].測(cè)繪通報(bào)，2016(4)：10-14.DOI:10.13474/j.cnki.11-2246.2016.0111.