李　銚　劉　文　劉朝暉

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

關鍵詞：粒子濾波器；非線性估計；重采樣；EKF；正則化粒子濾波器

中圖分類號：TP391 文獻標識碼：B 文章編號：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

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

1 基本原理

1.1 非線性貝葉斯估計

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

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

zk=hk(xk,nk)(1)

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

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

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為歸一化常數:

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

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

1.2 粒子濾波器原理與算法

1.2.1 粒子濾波器原理

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

1.2.2 粒子濾波器算法描述

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

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

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

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

其中:

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

為第i個粒子的歸一化權重。

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

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

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

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

則可以通過在已有的采樣值xi0∶k-1~Q(x0∶k-1|z1∶k-1)中增加新的狀態(tài)xik~Q(xk|x0∶k-1,z1∶k)來獲得采樣值xi0∶k~Q(x0∶k|z1∶k)。為了得出加權更新公式，后驗概率密度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)

將上兩式代入可以求得權更新公式：

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。當每一時間步都需要對P(xk|z1∶k)Ы行濾波估計時，這將特別有用。然后計算改進權重為：

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

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

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

當Ns→∞時，近似值將逼近于真實的后驗概率密度P(xk|z1∶k)。

1.2.3 粒子濾波器步驟描述

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

Particle Filter

FOR i=1∶Ns

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

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

權重：

END FOR

計算總權重： А芅si=1wik

FOR i=1∶Ns

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

END FOR

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

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

1.3 改進的正則化粒子濾波器

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

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

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

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

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

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

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

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

0(17)

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

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

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

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

2 實驗結果與分析

2.1 仿真環(huán)境

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

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

測量方程為：

yt=x2t/20+wt(21)

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

2.2 仿真結果與分析

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

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

圖2 EKF預測圖

圖3 UKF預測圖

圖4 PF預測圖

圖5 粒子數10時比較圖

圖6 粒子數100時比較圖

表1 EKF，UKF，PFD性能比較

EKFUKFPF

RMSE(50次平均)5.022 63.225 41.760 2

時間 /s(1次)0.970.721.05

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

表2 PF與RPF性能比較

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

PF102.028 30.616 2

RPF101.781 60.441 3

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

PF1001.874 55.309 6

RPF1000.835 24.504 5

3 結 語

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

參 考 文 獻

［1］劉煒,張冰.非高斯環(huán)境下基于GPF算法的目標跟蹤［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.

作者簡介 李 銚 男,1983年出生，碩士研究生。研究方向為目標識別與跟蹤。

劉 文 男，副研究員，碩士生導師。研究方向為數字圖像處理與目標識別。

劉朝暉 男，研究員。研究方向為光電探測。

注：本文中所涉及到的圖表、注解、公式等內容請以PDF格式閱讀原文。