基于粒子滤波器的非线性估计方法

李　铫　刘　文　刘朝晖

摘 要：介绍基于粒子滤波器的非线性估计方法。采用正则化粒子滤波器来缓解粒子滤波器重采样造成的问题，改进了粒子滤波器的性能。在一种典型的非静态增长模型下比较EKF,UKF,PF和RPF的滤波性能差异。仿真结果表明，PF在滤波精度方面优于EKF和UKF，而RPF在精度和计算复杂度等方面均优于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

贝叶斯方法为动态系统的估计问题提供了一类典型的解决框架，从中可以得到系统状态估计的最优贝叶斯解。对于线性系统，卡尔曼滤波器被公认为是最好的解决方法；对于非线性系统的估计问题，最经典并得到广泛应用的方法是扩展卡尔曼滤波(EKF)算法，但其缺点是仅利用了非线性函数泰勒展开式的一阶偏导部分，忽略了高阶项，常会导致状态后验分布估计时产生较大误差，影响到滤波算法的性能乃至整个非线性系统的估计性能。为了改善这种状况，近年来，粒子滤波器(Particle Filter)以其处理非线性、非高斯信号能力强，而成为非线性估计领域的一个热门研究方向。

1 基本原理

1.1 非线性贝叶斯估计

非线性贝叶斯估计是粒子滤波器的基础，粒子滤波器的目的即为近似求取状态的最优贝叶斯解。对于如下非线性随机状态空间模型：

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

zk=hk(xk,nk)(1)

式中:fk:Rnx•xRnv→Rnx为状态xk-1的非线性函数；zk为系统状态xk的观测；vk∈Rnx,nk∈Rnv分别表示过程噪声和观测噪声；nx,nv分别表示状态和过程噪声的维数；fk:Rnx•Rnv→Rnx和hk:Rnx•Rnv→Rnx为有界的非线性映射。

从Bayes方法的角度而言，k时刻的状态xk需要通过递推运算得出。假设已知状态xk的初始概率分布P(x0|z0)=P(x0)，就能通过预测和更新的递推方式估计出系统状态的概率分布(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）为非线性贝叶斯估计的基本思想。通常不能对它进行精确的分析。在满足一定的条件下，可以得到最优贝叶斯解。

1.2 粒子滤波器原理与算法

1.2.1 粒子滤波器原理

粒子滤波器是通过预测和更新状态概率密度函数采样集的方法来近似非线性系统的随机贝叶斯估计，即将状态概率密度函数作为一组随机采样进行迭代运算，以样本均值代替积分运算，从而获得状态最小方差估计的过程，这些样本即所谓的粒子。随着粒子数目的增加，粒子的概率密度函数逐渐逼近状态的概率密度函数，粒子滤波估计即达到了最优贝叶斯估计的效果。

1.2.2 粒子滤波器算法描述

为了表示粒子滤波算法细节，假设{xi0∶k,wik}Nsi=1表示后验概率密度函数(PDF)为P(x0∶k|z1∶k)的随机粒子，其中{xi0∶0,i=0,1,…,Ns}是关联权重为{wik,i=1,2,…,Ns}的点集，x0∶k={xj,j=0,1,…,k}是一直到时刻k的所有状态集合。权值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)中增加新的状态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 粒子滤波器优缺点

粒子滤波算法简单实用，摆脱了解决非线性估计问题时随机量必须满足高斯分布的制约条件，并在一定程度上解决了粒子数样本匮乏问题，因此近年来该算法在许多领域得到成功应用。但是由于粒子滤波在重采样时，样本是从离散分布而不是连续分布中抽样的，造成了计算复杂度增大，很多时间被耗费在重采样上，而且引入新的问题粒子多样性匮乏，严重时会造成“粒子崩溃”。因此需要在此进行进一步的改进。

1.3 改进的正则化粒子滤波器

为了有效缓解粒子滤波在重采样过程造成的粒子多样性匮乏问题，提出采用正则化粒子滤波器(Regularized Particle Filter，RPF)方法，与一般粒子滤波算法相比，正则化粒子滤波在重采样过程中有所区别，一般粒子滤波采用的离散形式计算P(xk|z1∶k)，而在RPF中，样本值是从以下近似中抽取的：

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

式中：Kh(x)为核密度K（•）的调节值，Kh(x)=1/hnx(x/h)；h＞0为Kernel带宽系数；nx为状态向量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У牡ノ磺蛱寤。进一步说，当潜在的密度函数是单位协方差矩阵的高斯分布，带宽的最佳选择为：

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

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

正则化粒子滤波算法步骤与粒子滤波基本一致，不同在于最后一步是采用式中连续形式计算后验概率密度的。

2 实验结果与分析

2.1 仿真环境

仿真环境为Matlab 6.5,采用模型为一种典型非静态增长的非线性模型，其状态方程为：

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)；状态变量xt为一维变量。

2.2 仿真结果与分析

仿真过程中，根据假设的初始状态值，通过模型方程可以得到系统真实测量值，仿真时间50 s，采样间隔1 s，设初始状态 为0.1，取Qk-1=1和Rk=1，分布采用EKF，UKF，PF和RPF进行仿真。仿真中UKF的sigma点取3，相应的状态和观测如图1所示，图2为EKF预测图，图3为UKF预测图，图4为PF(粒子数取100)预测图，图5、图6分布为PF与RPF在粒子数取10和100时的比较图。表1为EKF，UKF，PF性能比较，表2为PF与RPF性能比较。

图1 状态与观测图

图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在目标状态有一定趋势能够很快地逼近真实结果，表现要远远好于EKF。使用PF后，不仅在目标状态上有一定的趋势能够很快地逼近真实结果，而且与真实状态的差距相对前面的两种方法要小得多。图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］刘炜,张冰.非高斯环境下基于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格式阅读原文。