基于粒子濾波器的非線性估計(jì)方法

李　銚　劉　文　劉朝暉

摘 要：介紹基于粒子濾波器的非線性估計(jì)方法。采用正則化粒子濾波器來(lái)緩解粒子濾波器重采樣造成的問(wèn)題，改進(jìn)了粒子濾波器的性能。在一種典型的非靜態(tài)增長(zhǎng)模型下比較EKF,UKF,PF和RPF的濾波性能差異。仿真結(jié)果表明，PF在濾波精度方面優(yōu)于EKF和UKF，而RPF在精度和計(jì)算復(fù)雜度等方面均優(yōu)于PF。

關(guān)鍵詞：粒子濾波器；非線性估計(jì)；重采樣；EKF；正則化粒子濾波器

中圖分類號(hào)：TP391 文獻(xiàn)標(biāo)識(shí)碼：B 文章編號(hào)：1004－373X(2009)04－141－04

Nonlinear Estimation Method Based on Particle Filter

LI Yao1,2 ，LIU Wen1,2，LIU Zhaohui1

(1.Xi′an Institute of Optics and Precision Mechanics,Chinese Academy of Sciences,Xi′an,710119,China;

2.Graduate School,Chinese Academy of Sciences,Beijing,100039,China)

Abstract：Nonlinear estimation methods based on Particle Filter(PF) are proposed.Regularized Particle Filter(RPF) is emphasized to relieve the problems caused by resampling of PF,and improve the performance of PF.The comparison of filtering performance among EKF,UKF,PF and RPF is made in a typical nonstatic model.The simulation results show that PF is better than EKF and UKF in the performance of accuracy,and the performance of RPF is better than PF in both filtering accuracy and calculating complexity.

Keywords：particle filter;nonlinear estimation;resampling;EKF;regularized particle filter

貝葉斯方法為動(dòng)態(tài)系統(tǒng)的估計(jì)問(wèn)題提供了一類典型的解決框架，從中可以得到系統(tǒng)狀態(tài)估計(jì)的最優(yōu)貝葉斯解。對(duì)于線性系統(tǒng)，卡爾曼濾波器被公認(rèn)為是最好的解決方法；對(duì)于非線性系統(tǒng)的估計(jì)問(wèn)題，最經(jīng)典并得到廣泛應(yīng)用的方法是擴(kuò)展卡爾曼濾波(EKF)算法，但其缺點(diǎn)是僅利用了非線性函數(shù)泰勒展開式的一階偏導(dǎo)部分，忽略了高階項(xiàng)，常會(huì)導(dǎo)致狀態(tài)后驗(yàn)分布估計(jì)時(shí)產(chǎn)生較大誤差，影響到濾波算法的性能乃至整個(gè)非線性系統(tǒng)的估計(jì)性能。為了改善這種狀況，近年來(lái)，粒子濾波器(Particle Filter)以其處理非線性、非高斯信號(hào)能力強(qiáng)，而成為非線性估計(jì)領(lǐng)域的一個(gè)熱門研究方向。

1 基本原理

1.1 非線性貝葉斯估計(jì)

非線性貝葉斯估計(jì)是粒子濾波器的基礎(chǔ)，粒子濾波器的目的即為近似求取狀態(tài)的最優(yōu)貝葉斯解。對(duì)于如下非線性隨機(jī)狀態(tài)空間模型：

xk=fk(xk-1,vk-1)

zk=hk(xk,nk)(1)

式中:fk:Rnx?xRnv→Rnx為狀態(tài)xk-1的非線性函數(shù)；zk為系統(tǒng)狀態(tài)xk的觀測(cè)；vk∈Rnx,nk∈Rnv分別表示過(guò)程噪聲和觀測(cè)噪聲；nx,nv分別表示狀態(tài)和過(guò)程噪聲的維數(shù)；fk:Rnx?Rnv→Rnx和hk:Rnx?Rnv→Rnx為有界的非線性映射。

從Bayes方法的角度而言，k時(shí)刻的狀態(tài)xk需要通過(guò)遞推運(yùn)算得出。假設(shè)已知狀態(tài)xk的初始概率分布P(x0|z0)=P(x0)，就能通過(guò)預(yù)測(cè)和更新的遞推方式估計(jì)出系統(tǒng)狀態(tài)的概率分布(PDF)P(xk|z1:k)和預(yù)測(cè)概率分布P(xk+1|z1:k)，其預(yù)測(cè)方程和更新方程分別為：

P(xk|z1∶k-1)=∫P(xk|xk-1)P(xk-1|z1∶k-1)dxk-1(2)

P(xk|z1∶k)=P(zk|xk)P(xk|z1∶k-1)/Ck(3)

其中，Ck為歸一化常數(shù):

Ck=∫P(xk|z1∶k-1)P(zk|xk)dxk(4)

式（2），式（3）為非線性貝葉斯估計(jì)的基本思想。通常不能對(duì)它進(jìn)行精確的分析。在滿足一定的條件下，可以得到最優(yōu)貝葉斯解。

1.2 粒子濾波器原理與算法

1.2.1 粒子濾波器原理

粒子濾波器是通過(guò)預(yù)測(cè)和更新狀態(tài)概率密度函數(shù)采樣集的方法來(lái)近似非線性系統(tǒng)的隨機(jī)貝葉斯估計(jì)，即將狀態(tài)概率密度函數(shù)作為一組隨機(jī)采樣進(jìn)行迭代運(yùn)算，以樣本均值代替積分運(yùn)算，從而獲得狀態(tài)最小方差估計(jì)的過(guò)程，這些樣本即所謂的粒子。隨著粒子數(shù)目的增加，粒子的概率密度函數(shù)逐漸逼近狀態(tài)的概率密度函數(shù)，粒子濾波估計(jì)即達(dá)到了最優(yōu)貝葉斯估計(jì)的效果。

1.2.2 粒子濾波器算法描述

為了表示粒子濾波算法細(xì)節(jié)，假設(shè){xi0∶k,wik}Nsi=1表示后驗(yàn)概率密度函數(shù)(PDF)為P(x0∶k|z1∶k)的隨機(jī)粒子，其中{xi0∶0,i=0,1,…,Ns}是關(guān)聯(lián)權(quán)重為{wik,i=1,2,…,Ns}的點(diǎn)集，x0∶k={xj,j=0,1,…,k}是一直到時(shí)刻k的所有狀態(tài)集合。權(quán)值wik經(jīng)過(guò)歸一化處理后，在時(shí)刻kУ暮笱楦怕拭芏瓤梢越似表示為：

P(x0∶k|z1∶k)臁芅si=1wikδ(x0∶k-xi0∶k)(5)

因此對(duì)真實(shí)的后驗(yàn)概率P(x0∶k|z1∶k)有了一個(gè)合適的離散加權(quán)近似，利用重要度采樣原理對(duì)權(quán)值wik進(jìn)行選擇，假設(shè)P(x)∞π(x)為概率密度,但是因?yàn)棣?x)可被估計(jì)，所以從中很難得出x的采樣值。于是令xi~Q(x),i=1,2,…,Ns為能從重要密度函數(shù)Q(?)中產(chǎn)生的采樣值，然后對(duì)密度P(?)Ы行加權(quán)近似可知：

P(x)臁芅si=1wiδ(x-xi)(6)

其中:

Wi∞π(xi)Q(xi)(7)

為第i個(gè)粒子的歸一化權(quán)重。

這樣，如果樣本xi0∶k來(lái)自重要性密度函數(shù)q(xi0∶k|z1∶k)，則式中的權(quán)值wikЭ梢遠(yuǎn)ㄒ邐：

Wik∞P(xi0∶k)|z1∶k)/Q(xi0∶k|z1∶k)(8)

對(duì)于每一次迭代，都需要通過(guò)采樣產(chǎn)生P(x0∶k-1|z1∶k-1)的近似值并需要新的采樣集來(lái)近似P(x0∶k|z1∶k)。如果重要性密度函數(shù)做如下分解：

Q(x0∶k|z1∶k)=Q(xk|x0∶k-1,z1∶k)Q(x0∶k|z1∶k-1)(9)

則可以通過(guò)在已有的采樣值xi0∶k-1~Q(x0∶k-1|z1∶k-1)中增加新的狀態(tài)xik~Q(xk|x0∶k-1,z1∶k)來(lái)獲得采樣值xi0∶k~Q(x0∶k|z1∶k)。為了得出加權(quán)更新公式，后驗(yàn)概率密度P(x0∶k|z1∶k)Э梢員硎疚：

P(x0∶k|z1∶k)=P(zk|x0∶k,z1∶k-1)P(x0∶k|z1∶k-1)P(xk|z1∶k-1)=

P(zk|x0∶k|z1∶k-1)P(xk|x0∶k-1|z1∶k-1)P(x0∶k-1|z1∶k-1)P(zk|z1∶k-1)∞

P(zk|xk)P(xk|xk-1)P(xk|xk-1)P(x0∶k-1|z1∶k-1)(10)

將上兩式代入可以求得權(quán)更新公式：

wik∞P(zk|xik)P(xik|xik-1)P(xi0∶k-1|z1∶k-1)Q(xik|xi0∶k-1,z1∶k)Q(xik-1|z1∶k-1)＝

wik-1P(zk|xik)P(xik|xik-1)Q(xik|xi0∶k-1,z1∶k)(11)

此外，如果Q(xk|x0∶k-1,z1∶k)=Q(xk|xk-1,zk)，則重要性密度只取決于xk-1和zk。當(dāng)每一時(shí)間步都需要對(duì)P(xk|z1∶k)Ы行濾波估計(jì)時(shí)，這將特別有用。然后計(jì)算改進(jìn)權(quán)重為：

wik∞wik-1P(zk|xik)P(xik|xik-1)Q(x1k|xik-1,zk)(12)

這樣，后驗(yàn)概率密P(xk|z1∶k)度可近似為:

P(xk|z1∶k)臁芅si=1wikδ(xk-xik)(13)

當(dāng)Ns→∞時(shí)，近似值將逼近于真實(shí)的后驗(yàn)概率密度P(xk|z1∶k)。

1.2.3 粒子濾波器步驟描述

算法步驟用偽代碼描述如下：

Particle Filter

FOR i=1∶Ns

從xik~Q(xk|xik-1,zk)中抽取樣本值

根據(jù)wik∞wik-1P(zk|xik)P(xik|xik-1)Q(xik|xik-1,zk)Ъ撲闃匾性

權(quán)重：

END FOR

計(jì)算總權(quán)重： А芅si=1wik

FOR i=1∶Ns

歸一化： wik=wik/∑Nsi=1wik

END FOR

1.2.4 粒子濾波器優(yōu)缺點(diǎn)

粒子濾波算法簡(jiǎn)單實(shí)用，擺脫了解決非線性估計(jì)問(wèn)題時(shí)隨機(jī)量必須滿足高斯分布的制約條件，并在一定程度上解決了粒子數(shù)樣本匱乏問(wèn)題，因此近年來(lái)該算法在許多領(lǐng)域得到成功應(yīng)用。但是由于粒子濾波在重采樣時(shí)，樣本是從離散分布而不是連續(xù)分布中抽樣的，造成了計(jì)算復(fù)雜度增大，很多時(shí)間被耗費(fèi)在重采樣上，而且引入新的問(wèn)題粒子多樣性匱乏，嚴(yán)重時(shí)會(huì)造成“粒子崩潰”。因此需要在此進(jìn)行進(jìn)一步的改進(jìn)。

1.3 改進(jìn)的正則化粒子濾波器

為了有效緩解粒子濾波在重采樣過(guò)程造成的粒子多樣性匱乏問(wèn)題，提出采用正則化粒子濾波器(Regularized Particle Filter，RPF)方法，與一般粒子濾波算法相比，正則化粒子濾波在重采樣過(guò)程中有所區(qū)別，一般粒子濾波采用的離散形式計(jì)算P(xk|z1∶k)，而在RPF中，樣本值是從以下近似中抽取的：

P(xk|z1∶k)臁芅i=1wikKh(xk-xik)(14)

式中：Kh(x)為核密度K（?）的調(diào)節(jié)值，Kh(x)=1/hnx(x/h)；h＞0為Kernel帶寬系數(shù)；nx為狀態(tài)向量x的維數(shù)；wik為歸一化權(quán)值。Kernel密度函數(shù)是一個(gè)如下形式對(duì)稱分布的概率密度函數(shù)：

∫xK(x)dx=0, ∫‖x‖2K(x)dx＜∞(15)

K(?)和hУ氖實(shí)毖∪∈溝謎媸島笱楦怕拭芏群駝則化經(jīng)驗(yàn)表示積分均方誤差均值最小，該值被定義為：

MISE()=E(xk|z1∶k)-P(xk|z1∶k)〗2dxk〗(16)

式中：(xk|z1∶k)代表在條件下對(duì)P(xk|z1∶k)У慕似。在特殊情況下所有的采樣值有相同的權(quán)重，核的一個(gè)最佳選擇為Epanechnikov kernel：

Kopt=nx+12cnx(1-‖x‖2),if‖x‖＜1

0(17)

式中：cnx是RnxУ牡ノ磺蛺寤。進(jìn)一步說(shuō)，當(dāng)潛在的密度函數(shù)是單位協(xié)方差矩陣的高斯分布，帶寬的最佳選擇為：

hopt=AN1/(nx+4)s(18)

A=π)nx〗1/(nx+4)(19)

正則化粒子濾波算法步驟與粒子濾波基本一致，不同在于最后一步是采用式中連續(xù)形式計(jì)算后驗(yàn)概率密度的。

2 實(shí)驗(yàn)結(jié)果與分析

2.1 仿真環(huán)境

仿真環(huán)境為Matlab 6.5,采用模型為一種典型非靜態(tài)增長(zhǎng)的非線性模型，其狀態(tài)方程為：

Xt=xt-1/2+25xt-1/(1+x2t-1)+8cos(1.2T)+Vt(20)

測(cè)量方程為：

yt=x2t/20+wt(21)

式中：vt和wt分別為過(guò)程噪聲和測(cè)量噪聲，vt~N(0,Qt),wt~N(0,Rt)；狀態(tài)變量xt為一維變量。

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

仿真過(guò)程中，根據(jù)假設(shè)的初始狀態(tài)值，通過(guò)模型方程可以得到系統(tǒng)真實(shí)測(cè)量值，仿真時(shí)間50 s，采樣間隔1 s，設(shè)初始狀態(tài) 為0.1，取Qk-1=1和Rk=1，分布采用EKF，UKF，PF和RPF進(jìn)行仿真。仿真中UKF的sigma點(diǎn)取3，相應(yīng)的狀態(tài)和觀測(cè)如圖1所示，圖2為EKF預(yù)測(cè)圖，圖3為UKF預(yù)測(cè)圖，圖4為PF(粒子數(shù)取100)預(yù)測(cè)圖，圖5、圖6分布為PF與RPF在粒子數(shù)取10和100時(shí)的比較圖。表1為EKF，UKF，PF性能比較，表2為PF與RPF性能比較。

圖1 狀態(tài)與觀測(cè)圖

圖2 EKF預(yù)測(cè)圖

圖3 UKF預(yù)測(cè)圖

圖4 PF預(yù)測(cè)圖

圖5 粒子數(shù)10時(shí)比較圖

圖6 粒子數(shù)100時(shí)比較圖

表1 EKF，UKF，PFD性能比較

EKFUKFPF

RMSE(50次平均)5.022 63.225 41.760 2

時(shí)間 /s(1次)0.970.721.05

由圖1~圖4以及表1可以看出，使用EKF跟蹤與真實(shí)的情況偏離較遠(yuǎn)，改用UKF進(jìn)行跟蹤預(yù)測(cè)后，明顯可以看出UKF在目標(biāo)狀態(tài)有一定趨勢(shì)能夠很快地逼近真實(shí)結(jié)果，表現(xiàn)要遠(yuǎn)遠(yuǎn)好于EKF。使用PF后，不僅在目標(biāo)狀態(tài)上有一定的趨勢(shì)能夠很快地逼近真實(shí)結(jié)果，而且與真實(shí)狀態(tài)的差距相對(duì)前面的兩種方法要小得多。圖5、圖6以及表2可以看出，隨著粒子數(shù)目增加，PF和RPF的精度也不斷提升。粒子數(shù)較少時(shí)，RPF較PF更能逼近真實(shí)值。在計(jì)算負(fù)荷上RPF低于PF，在精度和實(shí)時(shí)性上RPF高于PF。符合改進(jìn)的需求。

表2 PF與RPF性能比較

濾波器粒子數(shù)(M)RMSE(50次平均)時(shí)間/s(1次)

PF102.028 30.616 2

RPF101.781 60.441 3

濾波器粒子數(shù)(M)RMSE(50次平均)時(shí)間/s(1次)

PF1001.874 55.309 6

RPF1000.835 24.504 5

3 結(jié) 語(yǔ)

對(duì)于非線性估計(jì)問(wèn)題，粒子濾波器較EKF和UKF等算法更能逼近真實(shí)值，獲得高精度。對(duì)于粒子濾波的重采樣問(wèn)題，采用的正則化粒子濾波方法緩解了粒子多樣性匱乏問(wèn)題，同時(shí)提高了濾波精度和實(shí)時(shí)性。

參 考 文 獻(xiàn)

［1］劉煒,張冰.非高斯環(huán)境下基于GPF算法的目標(biāo)跟蹤［J］.火力與指揮控制,2008,33(2):69－72.

［2］Doucet A,Godsill S,Andrieu C.On Sequential Monte Carlo Sampling Methods for Bayesian Filtering［J］.Statistics and Computing,2000,10(3):197－208.

［3］Arulampalam M Sanjeev，Simon Maskell，Neil Gordon,et al.A Tutorial on Particle Filters for Online Nonlinear/Non－Gaussian Bayesian Tracking［J］.IEEE Trans.on Signal Processing，2002，50(2)：174－188.

［4］Doucet A，Gordon N.Sequential Monte Carlo Methods in Practice.New York:Springer－Verlag，2001.

［5］Kitagawa G.Monte Carlo Filter and Smoother for Non－Gaussian Nonlinear State Space Models［J］.Journal of Computational and Graphical Statistics，1996，5(1)：1－25.

［6］Erzuini C，Best N.Dynamic Conditional Independence Models and Markov Chain Monte Carlo Methods.Journal of the American Statistical Association，1997,92(5)：1 403－1 412.

［7］Gordon N J,Salmond D J.Novel Approach to Non－linear and Non－Gaussian Bayesian State Estimation［J］.Proc.of Institute Engineering，1993，140(2)：107－113.

［8］Julier S J,Uhlmann J K.Unscented Filtering and Nonlinear Estimation ［J］.Proceedings of the IEEE，2004，92(3)：401－422.

［9］Julier S J，Uhlmann J K，Durrant Whyten H F.A New Approach for Filtering Nolinear System［A］.The Proceedings of the American Control Conference.Seattle,Washington，ACC，1995：1 628－1 632.

［10］Handschin J E.Monte Carlo Techniques for Prediction and Filtering of Non－linear Stochastic Processes［J］.Automatica，1970，6(3)：555－563.

作者簡(jiǎn)介 李 銚 男,1983年出生，碩士研究生。研究方向?yàn)槟繕?biāo)識(shí)別與跟蹤。

劉 文 男，副研究員，碩士生導(dǎo)師。研究方向?yàn)閿?shù)字圖像處理與目標(biāo)識(shí)別。

劉朝暉 男，研究員。研究方向?yàn)楣怆娞綔y(cè)。

注：本文中所涉及到的圖表、注解、公式等內(nèi)容請(qǐng)以PDF格式閱讀原文。