基于輔助模型的改進(jìn)粒子濾波算法

何 健,李亞安,易 鋒,李國(guó)輝,2

(1.西北工業(yè)大學(xué)航海學(xué)院,陜西西安 710072;2.西安郵電學(xué)院電信系,陜西西安 710061)

0 引言

非線(xiàn)性系統(tǒng)的估計(jì)問(wèn)題廣泛存在于許多領(lǐng)域,包括統(tǒng)計(jì)信號(hào)處理、經(jīng)濟(jì)學(xué)、雷達(dá)跟蹤、飛行器導(dǎo)航、聲納等,其中最主要的問(wèn)題就是對(duì)非線(xiàn)性狀態(tài)的后驗(yàn)概率密度函數(shù)(PDF)進(jìn)行估計(jì),但在實(shí)際中很難直接對(duì)PDF進(jìn)行表示,在非線(xiàn)性高斯條件下,傳統(tǒng)的解決方法是利用 EKF(Extended Kalman Filter)[1]和 UKF(Unscented Kalman Filter)[2],其中EKF是結(jié)合卡爾曼濾波算法對(duì)非線(xiàn)性部分進(jìn)行線(xiàn)性化處理的一種次優(yōu)估計(jì)算法。而 UKF避免了EKF線(xiàn)性化等近似方法,無(wú)需計(jì)算Jacobian矩陣,提高了估計(jì)精度。Rudolph Vander Merwe和 Eric A.Wan在 UKF的基礎(chǔ)上給出了SRUKF(Square Root Unscented Kalman Filter)[3]算法。UKF每次更新中都要對(duì)Sigma點(diǎn)花費(fèi)大量的計(jì)算,并要求計(jì)算狀態(tài)協(xié)方差矩陣的平方根;在SRUKF中,平方根被直接記錄下來(lái),避免了每次采樣時(shí)的重新因數(shù)分解,并且得到的協(xié)方差陣一定是非負(fù)的,因而確保了算法的穩(wěn)定性。近幾年發(fā)展起來(lái)的粒子濾波PF(Particle Filter)[4]是一種基于蒙特卡羅的貝葉斯?fàn)顟B(tài)估計(jì)算法,它通過(guò)一組歸一化權(quán)值的粒子來(lái)近似表示PDF,適用于非線(xiàn)性、非高斯條件下的目標(biāo)跟蹤。在粒子濾波器中,普遍存在的問(wèn)題是粒子退化現(xiàn)象,即經(jīng)過(guò)幾次迭代,除一個(gè)粒子外,其他粒子都減少到可以忽略的程度。降低該現(xiàn)象影響的最有效方法是選擇建議分布函數(shù)和重采樣方法。UPF(Unscented Particle Filter)[5]采用 UKF來(lái)計(jì)算建議分布,粒子的產(chǎn)生充分利用了新的測(cè)量信息,可以使其建議分布較好地逼近后驗(yàn)概率密度,如果測(cè)量噪聲過(guò)大,UKF很容易產(chǎn)生目標(biāo)失捕問(wèn)題[6]。SPPF(Sigma Point Particle Filter)[7]則是利用SRUKF來(lái)計(jì)算建議分布,可以使建議分布更好地逼近后驗(yàn)概率密度。

針對(duì)SPPF算法在非線(xiàn)性、非高斯系統(tǒng)的目標(biāo)跟蹤精確度不太高這一問(wèn)題,本文提出了一種改進(jìn)Sigma粒子濾波算法MSPPF(Modified Sigma Point Particle Filter)。

1 SPPF濾波算法

1.1 貝葉斯估計(jì)原理

假定動(dòng)態(tài)時(shí)變系統(tǒng)可描述為

系統(tǒng)方程:

量測(cè)方程:

式中,f和h為系統(tǒng)狀態(tài)和量測(cè)的非線(xiàn)性變換,xk,yk分別是系統(tǒng)k時(shí)刻的狀態(tài)值和量測(cè)值,wk,v k分別是系統(tǒng)k時(shí)刻的狀態(tài)噪聲和測(cè)量噪聲。則狀態(tài)預(yù)測(cè)方程和狀態(tài)更新方程分別為:

1.2 粒子濾波算法

PF是通過(guò)蒙特卡羅方法實(shí)現(xiàn)貝葉斯遞歸估計(jì)。從待估計(jì)的后驗(yàn)分布p(x0:k|y0:k)中抽樣出N個(gè)獨(dú)立同分布的粒子和相應(yīng)的歸一化權(quán)值(x0:k),則分布p(x0:k|y 0:k)的一個(gè)估計(jì)表示為:

當(dāng)無(wú)法從后驗(yàn)分布p(x0:k|y0:k)中直接采樣時(shí),可以找一個(gè)容易采樣的密度分布函數(shù)q(x0:k|y0:k)(建議分布)中采樣。

令其中w(x 0:k)為重要性權(quán)值:

PF的實(shí)現(xiàn)步驟:

1)初始化k=0

從p(x0)采樣粒子{xi0:k,i=1,…,N}

2)重要性權(quán)值的計(jì)算

粒子權(quán)值更新方程:

4)重采樣

從xik中根據(jù)重要性權(quán)值重新采樣得到新的N個(gè)粒子~xik,并重新分配權(quán)值 ~wik=1/N。

隨著迭代次數(shù)的增加,重要性權(quán)值的分布變得越來(lái)越傾斜,有可能出現(xiàn)粒子匱乏現(xiàn)象,為了避免粒子匱乏,Goden[8]等提出了重采樣方法,主要思想就是去掉那些權(quán)值小的粒子,復(fù)制權(quán)值大的粒子。

1.3 SPPF算法

在粒子濾波器中,關(guān)鍵問(wèn)題就是對(duì)建議分布的選擇。當(dāng)q(x k|:k-1,y0:k)=p(xk|:k-1,y 0:k)時(shí),重要性權(quán)值條件方差最小,為最優(yōu)重要性函數(shù),實(shí)際上,很難對(duì)它進(jìn)行采樣。在應(yīng)用中更常使用先驗(yàn)概率密度來(lái)作為建議分布:

由于函數(shù)沒(méi)有新的測(cè)量信息,它的性能主要依靠觀測(cè)噪聲的方差,當(dāng)觀測(cè)噪聲的方差很小時(shí),它的性能就很差。而SPPF是利用SRUKF來(lái)更新粒子均值和方差,從而用高斯近似的方法作為建議分布密度函數(shù)∈q(xk|-1,y0:k)=,),其中 N(?)表示高斯函數(shù)。

2 基于SRUKF和輔助模型的改進(jìn)Sigma粒子濾波算法

先稱(chēng)(1)、(2)為主模型,這里主要介紹一種輔助模型并使用SRUKF和它共同來(lái)產(chǎn)生建議分布。

2.1 輔助模型

定義輔助模型[9]為:

這里mk是一個(gè)小方差高斯噪聲,觀測(cè)噪聲nk和(2)中的vk認(rèn)為是一樣的。如果h(?)是線(xiàn)性的,nk是高斯噪聲,卡爾曼濾波就可以對(duì)rk進(jìn)行最優(yōu)估計(jì)。在實(shí)際環(huán)境中,h(?)往往是非線(xiàn)性的,就可以使用SRUKF來(lái)對(duì)r k進(jìn)行估計(jì)。

2.2 改進(jìn)Sigma粒子濾波算法(MSPPF)

在時(shí)刻k,從由SRUKF產(chǎn)生的建議分布密度中采樣第一個(gè)粒子,然后把和輔助模型結(jié)合并使用SRUKF產(chǎn)生新的建議分布,從中采樣第二個(gè)粒子。重復(fù)這個(gè)過(guò)程,遞歸的獲得,…,N表示的粒子的總數(shù)。當(dāng)觀測(cè)噪聲很小時(shí),除了第一個(gè)粒子是從主模型中獲得外,其他的粒子都是從第一個(gè)粒子和輔助模型遞歸得到的,這N-1個(gè)粒子充分利用了新信息yk,因此MSPPF能更好地近似后驗(yàn)概率密度。

MSPPF的算法步驟:

2)重要性采樣:

3)重要性權(quán)值:

當(dāng)i=2,…,N時(shí),

4)重采樣:

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

為了證明MSPPF的性能,考慮一個(gè)非線(xiàn)性非高斯模型[1,5]。

這里w=4e-2,uk服從Gamma(3,2)分布,觀測(cè)噪聲v k服從高斯分布N(0,10-5),目標(biāo)的初始狀態(tài)x0=1,經(jīng)過(guò)100次蒙特卡羅仿真,每次仿真時(shí)間是60 s,采樣間隔為1 s。1次獨(dú)立試驗(yàn)的均方根誤差定義為:

其中 PF、UPF、SPPF、MSSPF的粒子數(shù)分別為1 000,200,200,20。圖1、圖2表示各個(gè)濾波算法對(duì)目標(biāo)狀態(tài)的估計(jì)曲線(xiàn)和均方根誤差曲線(xiàn)圖。

圖1 不同濾波器的狀態(tài)估計(jì)Fig.1 State estimation for different filters

圖2 100次蒙特卡羅仿真的RMSEFig.2 RMSE for 100 times Monte-Carlo simulations

從上圖可以看出,在非高斯環(huán)境中,由于PF中沒(méi)有包含最新的測(cè)量信息,所以它受噪聲的干擾比較大,算法不穩(wěn)定,跟蹤性能不好。UPF和SPPF各自生成的建議分布中有效地利用了測(cè)量信息,在粒子數(shù)比較少的情況下,具有較高的精確度。而本文提出的MSSPF由輔助模型和SRUKF共同來(lái)產(chǎn)生建議分布,它比其他的算法更能有效地利用測(cè)量信息,所以只需要20個(gè)粒子就可以達(dá)到很高的精度。

表1給出了重復(fù)進(jìn)行100次蒙特卡羅實(shí)驗(yàn)得到的 MSPPF 與 EKF、UKF、PF 、UPF、SPPF 性能對(duì)比統(tǒng)計(jì)結(jié)果。可以看出它們對(duì)狀態(tài)估計(jì)的均方根誤差(RMSE)均值和方差的變化。

表1 RMSE的均值和方差Tab.1 The mean and variance of RMSE

4 結(jié)論

對(duì)于非線(xiàn)性非高斯問(wèn)題,粒子濾波在迭代過(guò)程中經(jīng)常會(huì)出現(xiàn)粒子退化現(xiàn)象,本文提出了一種改進(jìn)Sigma粒子濾波算法MSPPF。該算法采用了輔助模型與SRUKF來(lái)構(gòu)造建議分布,有效地利用了新的測(cè)量信息,使建議分布能更好地逼近后驗(yàn)概率密度。仿真結(jié)果表明,MSPPF不僅具有很好的穩(wěn)定性,而且用很少的粒子就可以達(dá)到很高的跟蹤精度。MSSPF是一種效率高、性能好的跟蹤算法。

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

[2]Julier SJ,Uhlmann JK.Unscented filtering and nonlinear estimation[J].Proceeding of the IEEE,2004,92(3):401-422.

[3]Merwe R V,Wan E A.The Squre-root Unscented Kalman Filter for State and Parametere-estimation[J].International Conferenceon Acoustics,and Speech signal Processing,2001:3 461-3 464.

[4]Arulampalam S,Maskell S,Gordon N.A tutorial on particle filters for online nonlinear/non-Gaussian bayesian tracking[J].IEEE Trans Signal Processing,2002,50(2):174-188.

[5]Van der Merwe R,Doucet A,Nando de Freitas,et al.The Unscented Particle Filter[R].GB:Cambridge University Engineering Department,2000.

[6]肖延國(guó),余志軍,魏建明.一種改進(jìn)的無(wú)跡粒子濾波器在目標(biāo)跟蹤中的應(yīng)用[J].中國(guó)科學(xué)技術(shù)大學(xué)學(xué)報(bào),2008,38(2):183-188.XIAO Yanguo,YU Zhijun,WEI Jianming.An improved unscented particle filter for target tracking in sensor networks[J].Journal of University of Science and Technology of China,2008,38(2):183-188.

[7]Guo Wen Y,Han CZ,Lei M.Improved Unscented Particle Filter for Nonlinear Bayesian Estimation[J].Information Fusion,2007(10):1-6.

[8]Doucet A,Godsill S,Andrieu C.On sequential Monte Carlo methods for Bayesian filtering[J].Statistics and Computing,2000,10(3):197-208.

[9]Chen Qi,Bondon P.A new unscented particle filter[J].International Conference on Acoustics,Speech and Signal Processing,2008:3 417-3 420.