基于容積卡爾曼濾波的輔助粒子濾波算法

趙　彬，李　炯,曹　艷，趙丹丹

(1.空軍工程大學(xué)防空反導(dǎo)學(xué)院，陜西 西安 710051；

2.陜西師范大學(xué)計(jì)算機(jī)科學(xué)學(xué)院，陜西 西安 710062)

?

基于容積卡爾曼濾波的輔助粒子濾波算法

趙彬1，李炯1,曹艷1，趙丹丹2

(1.空軍工程大學(xué)防空反導(dǎo)學(xué)院，陜西 西安 710051；

2.陜西師范大學(xué)計(jì)算機(jī)科學(xué)學(xué)院，陜西 西安 710062)

摘要:針對(duì)非線性系統(tǒng)的狀態(tài)估計(jì)精度較低的問(wèn)題，提出基于容積卡爾曼濾波(CKF)的輔助粒子濾波(APF)算法—CAPF算法。該算法采用容積數(shù)值積分原則計(jì)算非線性函數(shù)的均值和方差，生成粒子濾波算法的重要性密度函數(shù)，獲得所需的帶權(quán)值粒子，進(jìn)而計(jì)算粒子均值，獲得系統(tǒng)狀態(tài)的最小均方誤差估計(jì)。CAPF算法由于使用最新的量測(cè)信息產(chǎn)生粒子，因而提高了對(duì)系統(tǒng)狀態(tài)估計(jì)的逼近程度。仿真結(jié)果表明，CAPF算法具有更高的濾波精度，驗(yàn)證了算法的可行性和有效性。

關(guān)鍵詞:非線性系統(tǒng)；容積卡爾曼濾波；輔助粒子濾波；重要性密度函數(shù)

0引言

自1960年，kalman提出卡爾曼濾波算法以來(lái)，各種改進(jìn)的濾波算法層出不窮，研究熱點(diǎn)也從線性高斯問(wèn)題向非線性非高斯問(wèn)題轉(zhuǎn)移。由于現(xiàn)實(shí)世界里，非線性濾波問(wèn)題更為普遍，對(duì)于該問(wèn)題的研究具有更大的應(yīng)用價(jià)值。當(dāng)前，在眾多的應(yīng)用領(lǐng)域中，目標(biāo)跟蹤一直是研究的熱點(diǎn)，基于經(jīng)典卡爾曼的改進(jìn)算法，如解決非線性濾波的擴(kuò)展卡爾曼濾波算法(EKF)[1]，無(wú)跡卡爾曼濾波(UKF)[2]，容積卡爾曼濾波算法(CKF)[3]，這些算法在視頻跟蹤和雷達(dá)跟蹤中都取得了較好的應(yīng)用效果，基于此的改進(jìn)算法還在不斷被推演，但本質(zhì)上都是卡爾曼濾波思想。另一種解決非線性濾波問(wèn)題的著名方法是序貫蒙特卡羅方法(SMC)，即粒子濾波算法(PF)，該方法是用一組離散隨機(jī)采樣點(diǎn)(粒子)來(lái)近似系統(tǒng)隨機(jī)變量的概率密度函數(shù)，使用這一近似的表示來(lái)估計(jì)非線性系統(tǒng)的狀態(tài)。對(duì)于粒子濾波的研究，主要在于有效地解決粒子退化的問(wèn)題，以期達(dá)到較好的濾波效果。文獻(xiàn)[4]提出一種基于擴(kuò)展Η∞濾波(EHF)產(chǎn)生重要性函數(shù)的粒子濾波算法，由其產(chǎn)生的重要性函數(shù)更接近于系統(tǒng)狀態(tài)的真實(shí)后驗(yàn)概率分布。文獻(xiàn)[5]對(duì)典型的四種改進(jìn)的粒子濾波算法做了比較，分析了各自的優(yōu)缺點(diǎn)，其中輔助粒子濾波算法(APF)在高誤差的影響下改善效果雖然不如不敏粒子濾波算法(UPF)，但是由于其使用兩次加權(quán)操作，產(chǎn)生的粒子權(quán)值比采樣重要再采樣(SIR)濾波產(chǎn)生的粒子權(quán)值變化更穩(wěn)定，得到的估計(jì)結(jié)果也更準(zhǔn)確。

上述算法對(duì)粒子退化只是進(jìn)行了單一方面的改善，不能較好地改善粒子退化問(wèn)題，本文針對(duì)此問(wèn)題，提出基于CKF 的APF算法-CAPF算法。

1CKF算法

CKF是一種濾波精度高，不需微分的濾波算法，采用一組等權(quán)值的Cubature點(diǎn)集來(lái)解決貝葉斯濾波的積分問(wèn)題，它使用的容積點(diǎn)的個(gè)數(shù)少，運(yùn)行時(shí)間短，比傳統(tǒng)的UKF和EKF算法具有更好的非線性逼近功能、數(shù)值精度以及濾波穩(wěn)定性[6]。

CKF濾波的具體步驟如下：

1)狀態(tài)更新

計(jì)算容積點(diǎn)：

(1)

傳播求容積點(diǎn)：

(2)

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

(3)

狀態(tài)預(yù)測(cè)協(xié)方差矩陣：

(4)

2)量測(cè)更新

計(jì)算求容積點(diǎn)：

(5)

傳播求容積點(diǎn)：

Zi,k|k-1=h(Xi,k|k-1)

(6)

計(jì)算觀測(cè)預(yù)測(cè)值：

(7)

計(jì)算觀測(cè)預(yù)測(cè)協(xié)方差矩陣：

(8)

計(jì)算互協(xié)方差矩陣：

(9)

計(jì)算卡爾曼增益：

Kk=Pxz,k|k-1P-1zz,k|k-1

(10)

狀態(tài)更新：

(11)

狀態(tài)協(xié)方差矩陣更新：

(12)

2APF算法及其改進(jìn)算法

2.1APF算法

p(Xk,i|Zk)∝p(Zk|Xk)p(Xk,i|Zk-1)=

(13)

(14)

q(X,i|Zk)=q(i|Zk)q(Xk|i,Zk)

(15)

由式(14)可得

(16)

(17)

與SIR濾波相比，APF的優(yōu)點(diǎn)在于使用兩次加權(quán)操作，使生成的粒子權(quán)值比通過(guò)SIR濾波生成的粒子權(quán)值變化更穩(wěn)定，得到的估計(jì)結(jié)果更準(zhǔn)確。

具體算法步驟[7]如下：

為了實(shí)現(xiàn)粒子濾波算法，選擇重要性密度函數(shù)是關(guān)鍵問(wèn)題之一，本文采用算法性能比UKF和EKF更好的CKF算法來(lái)產(chǎn)生重要性密度函數(shù)，獲得新的粒子濾波算法——CAPF算法。該算法的描述如下：

1)初始化(k=0)，依據(jù)先驗(yàn)條件概率抽樣:

(18)

2)對(duì)于k=1,2,…，用CKF設(shè)計(jì)的重要性函數(shù):

(19)

3)重要性采樣，當(dāng)i=1,2,…,N，用CKF更新每個(gè)粒子；重新產(chǎn)生粒子：

(20)

依據(jù)APF算法中的步驟3)計(jì)算權(quán)值并進(jìn)行歸一化處理；

5)計(jì)算狀態(tài)估計(jì)值：

(21)

(22)

6)根據(jù)得到的新粒子狀態(tài)值、方差陣和模型概率，重復(fù)以上步驟進(jìn)行下一次的濾波計(jì)算。

3仿真分析

這里采用一個(gè)廣泛的算例對(duì)CAPF算法在非線性系統(tǒng)中的應(yīng)用進(jìn)行仿真分析。其狀態(tài)模型和觀測(cè)模型為：

xk=fk(xk-1,k)+wk-1

(23)

(24)

圖1　兩種算法對(duì)目標(biāo)狀態(tài)估計(jì)Fig.1　Target state estimate of two algorithms

圖2　跟蹤位置誤差對(duì)比Fig.2　Error of tracking

圖3　APF下粒子權(quán)值概率密度Fig.3　particle weight probability density of APF

圖4　CAPF下粒子權(quán)值概率密度Fig.4　particle weight probability density of CAPF

從圖1可以看出，通過(guò)APF濾波和CAPF濾波雖然都能逼近真實(shí)狀態(tài)值，但是CAPF濾波算法的逼近程度更好一些。

為了描述本文算法的估計(jì)質(zhì)量，采用狀態(tài)估計(jì)誤差標(biāo)準(zhǔn)差(Error Standard Deviation,ESD)來(lái)評(píng)估，表達(dá)式為：

表1　兩種濾波算法的平均ESD

根據(jù)誤差理論[8]，CAPF算法相對(duì)APF算法提高的濾波精度可以計(jì)算為：

通常選取合適的重要性函數(shù)可以減小誤差，在這里，使用容積卡爾曼濾波(CKF)算法來(lái)產(chǎn)生的重要性密度函數(shù)表現(xiàn)了較好的濾波性能。

現(xiàn)有粒子濾波算法幾乎都是對(duì)所有粒子進(jìn)行的完全重采樣，這種“過(guò)渡”重采樣極易引起粒子貧化[9]。圖3和圖4分別為APF下和CAPF下粒子權(quán)值概率密度，從兩圖中可以看出，APF下的粒子退化嚴(yán)重，粒子有效權(quán)值的概率密度在重采樣后減小，而在CAPF下，由于使用了CKF產(chǎn)生的重要性函數(shù)，粒子有效權(quán)值的概率密度在重采樣后依然保持了較好的狀態(tài)分布。

4結(jié)論

本文提出了基于CKF的輔助粒子濾波算法。該算法將CKF算法與APF算法結(jié)合，運(yùn)用容積數(shù)值積分方法產(chǎn)生重要性密度函數(shù)，獲得所需的帶權(quán)粒子，結(jié)合APF的粒子權(quán)值變化的穩(wěn)定性以及估計(jì)結(jié)果的準(zhǔn)確性的特點(diǎn)。仿真驗(yàn)證表明CAPF算法具有更高的濾波精度。

參考文獻(xiàn):

[1]Doucet A, Johansen A.A tutorial on particle filtering and smoothing: Fifteen years later[M] //Crisan D, Rozovskii B eds. TheOxford Handbook of Nonliner Filtering. New York: Oxford University Press, 2011:656-704.

[2]Sarkka S. Bayesian Filtering and Smoothing[M].Cambridge: Cambridge University Press,2013.

[3]Ienkaran A, Simon H, Robert J E. Discrete-time nonlinear filtering algorithms using Gauss-Hermite quadrature[J]. Proceedings of the IEEE，2007,95(5):953-977.

[4]萬(wàn)洋,王首勇,于興偉. 一種擴(kuò)展Η∞粒子濾波方法[J].信號(hào)處理,2010,26(6):869-874.

[5]張淼,胡建旺,周云峰,等. 改進(jìn)粒子濾波算法的比較[J].電光與控制,2009,2(2):30-32.

[6]陳海，單甘霖. 基于IMMCKF的機(jī)動(dòng)目標(biāo)跟蹤算法[J].電光與控制，2011,18(10):1-5.

[7]趙梅，張三同，朱剛. 輔助粒子濾波算法及仿真舉例[J].北京交通大學(xué)學(xué)報(bào)，2006,(4):24-28.

歡迎訂閱《探測(cè)與控制學(xué)報(bào)》

《探測(cè)與控制學(xué)報(bào)》是由西安機(jī)電信息技術(shù)研究所、機(jī)電動(dòng)態(tài)控制重點(diǎn)實(shí)驗(yàn)室、中國(guó)兵工學(xué)會(huì)聯(lián)合主辦的國(guó)內(nèi)外公開(kāi)發(fā)行的學(xué)術(shù)期刊。1979年創(chuàng)刊，被中文核心期刊要目總覽、中國(guó)科技核心期刊收錄，也是中國(guó)科學(xué)引文數(shù)據(jù)庫(kù)來(lái)源刊。本刊是綜合性的學(xué)術(shù)技術(shù)刊物，旨在為引信及相關(guān)探測(cè)與控制技術(shù)領(lǐng)域內(nèi)的科研、生產(chǎn)和教學(xué)服務(wù)，重點(diǎn)刊載：機(jī)、電接觸探測(cè)，無(wú)線電、紅外、激光和磁近程探測(cè)，信號(hào)處理，超小型電源，安全與解除保險(xiǎn)裝置，測(cè)試仿真等方面的學(xué)術(shù)論文。

《探測(cè)與控制學(xué)報(bào)》為雙月刊，每?jī)?cè)15元，全年90元?？?hào)：ISSN 1008-1194；CN 61-1316/TJ，歡迎各單位及讀者訂閱。

聯(lián)系人：龐彥波電話：029-88156204郵箱：tcykz@263.net

Auxiliary Particle Filter Based on Cubature Kalman Filter

ZHAO Bin1, LI Jiong1, CAO Yan1, ZHAO Dandan2

(1.Air and Missile Defense College, Air Force Engineering University, Xi’an 710051,China;2.School of Computer Science, Shaanxi Nomal University, Xi’an 710062, China)

Abstract:A new particle filter of APF algorithm based CKF(CAPF) was proposed to improve the low state estimation accuracy for nonlinear systems. The algorithm used the cubature rule based numerical integration method to calculate the mean and covariance, to generate the important density function for the particle filter, and to obtain the required particles with weights. Then the minimum square error state estimation is obtained. CAPF algorithm generates particles using the latest measurements so that the approximation to the system state. Simulation results showed that the algorithm was of higher accuracy of CAPF, and also verified the feasibility and effectiveness of the algorithm.

Key words:nonlinear system; cubature Kalman filters; auxiliary particle filter; important density function

中圖分類號(hào):TP391

文獻(xiàn)標(biāo)志碼:A

文章編號(hào)：1008-1194(2016)01-0109-04

作者簡(jiǎn)介:趙彬(1988—)，男，河南商丘人，碩士研究生，研究方向：高速高機(jī)動(dòng)目標(biāo)跟蹤算法。E-mail：zhaobinhit@qq.com。

基金項(xiàng)目:航空科學(xué)基金項(xiàng)目資助(20130196004)

*收稿日期:2015-09-21