粒子滤波的基本原理笔记

1、粒子滤波的基本原理粒子滤波算法广泛应用在视觉跟踪领域、通信与信号处理领域、机器人、图像处理、金融经济、以及目标定位、导航、跟踪领域，其本质是利用当前和过去的观测量来估计未知量的当前值。在粒子滤波算法中使用了大量随机样本，采用蒙特卡洛仿真来完成递推贝叶斯滤波过程，其核心是使用一组具有相应权值的随机样本(粒子)来表示状态的后验分布。该方法的基本思路是选取一个重要性概率密度函数并从中进行随机抽样，得到一些带有相应权值的随机样本后，在状态观测的基础上调节权值的大小和粒子的位置，再使用这些样本来逼近状态后验分布，最后通过这组样本的加权求和作为状态的估计值。粒子滤波不受系统模型的线性和高斯假设约束，采用样

2、本形式而不是函数形式对状态概率密度进行描述，使其不需要对状态变量的概率分布作过多的约束，适用于任意非线性非高斯动态系统，是目前最适合于非线性、非高斯系统状态的滤波方法【ArulampalamMS,MaskellS,GordonN,etal.Atutorialonparticlefiltersforonlinenonlinear/non-GaussianBayesiantrackingJ.IEEETransactionsonSignalProcessing,2002,50(2):174-188.】动态系统的状态空间模型状态空间模型包括系统状态方程和观测方程，其通用的表示方法分别为【梁军.粒子滤波

3、算法及其应用研究D.哈尔滨工业大学,2009.】【黄小平,王岩，廖鹏程.粒子滤波原理及应用MATLAB仿真M.电子工业出版社.2017】TOC o 1-5 h zX=f(X,W)(1)kk-1k HYPERLINK l bookmark6 o Current Document Z=h(X,V)(2)kkk其中f()和h()为已知函数，W和V是概率密度已知的随机变量，X代表k时刻的状态量,kkkZ代表k时刻的观测量，W和V是相互独立的。kkk关于系统的状态方程和观测方程，通常也可用p(XIX)表示状态转移模型；p(ZIX)表kk-1kIk示观测似然模型；p(xJ表示初始状态的先验分布；p(xz)

4、表示系统的后验密度；00:k1:kp(xz)表示边沿后验密度，或称为后验滤波密度。卡尔曼滤波以及粒子滤波算法的本质k1:k即是利用观测序列Z对当前状态进行优化，从而得到k时刻的后验滤波密度，进而得到k时1:k刻的状态值。【MerweRVD,DoucetA,FreitasND,etal.TheunscentedparticlefilterCInternationalConferenceonNeuralInformationProcessingSystems.MITPress,2000:563-569.】贝叶斯滤波原理贝叶斯估计是利用先验知识和实际观测值构造系统的后验概率来估计系统的状态量。若系统

5、的先验概率密度为p(x)，观测值为Z，则系统的后验概率密度可表示为0:k1:kp(X0:k函：丿二p(X0丿jp(zX1)p(X)dX3)1:k0:k0:k0:k从式(3)可以看出，后验概率实际上是利用观测值对先验概率进行修正。由于进行了修正后验概率密度较先验概率密度更加接近被估计值的真实概率密度。滤波问题是为了计算后验滤波概率密度p(xz)，它实际上是p(xz)的边沿概率密度即k1:k0:kI1:kp(XZ)=J.p(XZ)dXk1:k1:k0:k1:k0:k.dX1:k0k-14)当每一个新的观测量Z到来时，即需要完成式(4)得到后验滤波概率密度。k为清晰的给出后验滤波概率密度的逐级递推形

6、式。首先，通过预测步骤得到不包含k时刻观测值的k时刻系统的先验滤波概率密度p(XkZ1:k-1其中p(XkkX)由系统状态方程决定，p(xk-1k-1)=Jp(xX)p(xk-1k-1Z)dX1:k-1k-1Z)是k-1时刻的后验滤波概率密度。然后,1:k-11k-1根据贝叶斯公式，利用k时刻的先验滤波概率密度p(x|z)推导k时刻的后验概率密度的表示形式p(XkZ)=1-kp(Z|X)p(xkI”pkZ1k-1Z)1-k-16)其中p(zkXk)由观测方程决定。p(ZIZ)p(Zk1:k-1k、kk1:k-1,1:k-1kk1:k-1k7)一般为归一化常数。因此贝叶斯滤波的递推框图如下所示图

7、1贝叶斯递推滤波框图13)贝叶斯递推过程中的主要的几个公式p（XZ）=Jp（XX）p（XZ）dX（利用上一时刻的后验滤波密度，结合状态转ki：k-1kk-1k-ri：k-1k-1移概率对这一时刻的先验滤波概率密度进行求解）p（zZ）=Jp（zX）p（xz）dx（利用先验滤波概率密度以及观测似然密度得到观k1:k-1kkk1:k-1k测概率）p(xZ)=k1-kk1-k-1利用这一时刻的先验滤波概率密度，结合观测似然概率密度对这一时刻的后验滤波概率密度进行求解）以上三个式子是贝叶斯滤波递推求解后验滤波概率密度的核心公式，称为贝叶斯递推滤波算法。这里给出贝叶斯滤波原理的一个简单例子。8）一个简单的

8、例子：假设动态系统状态空间模型中状态方程和观测方程分别为X=X+wkk-1k9）其中wN（0,1），VN（0,0.5），并给出初始状态X二0。由此，可以得到:kk0X是以概率1取值为0，即010)p(X)=8(X)00状态转移概率p(XkX)=k-1(X-X)2kk-1211)观测似然函数密度p(zX)=kk(z-X)2kk2x0.512)该系统等效于某信号经过一个N（0,1）的高斯信道进行传输，然后再利用一个测量误差为N(0,0.5)的测量设备对该信号进行测量。当从0时刻向1时刻转移时p(XZ)=Jp(XX)p(x)dXTOC o 1-5 h z1I011000=Jp(XX)8(X)dX11

9、0001(X2)=exp142k(2丿p(ZZ)=1p(ZX)p(XZ)dX101110丄exp01(z-X匕)11Iexpx0.5k/2KX212dX1丄exp2k(Z-X)2X2-11一一2x0.52v丿dX11v3kfexp2X上Z31丿x13dX114)p(xz)=1p(zx)p(xZ)11101exp、述p|Z)110(ZX)112x0.5exp(X2121exp1exp3兀(2X2Z131丿2x13丿15)此时k=1时刻的后验滤波概率密度16)p(xZ)11通过式（16）可以看出，k=1时刻的后验概率密度是利用观测方程对状态方程得到的先验概率密度的修正，因此较状态方程和观测方程的误

10、差都要小。然后将k=1时刻的后验概率密度作为k=2时刻的先验概率密度进行递推，即从1时刻向2时刻转移过程时218)p(XZ)=Jp(XX)p(XZ)dX2-1exp11=J丄exp(2、Xz131丿2x13dX1丄exp=爲exp3(X)2-(Z)2+1XZ261221丿2、X-(X+2Z)1421丿dX1=爲expr8(2、Xz231丿2x-317)也就是说p(X2|Z1)NI2Z，4（这是利用Z1对X2的估计）p(ZZ)=1p(ZX)pGZ)dX212221=J丄exp兀12(z-X)2羽2x0.5丿2y/lRexp8(X2)2-1(z1)2+2XZdX21丿=exp-(z)2-(z)2+

11、ZZJ*3311112丿(4(1丄211(22x411dX2exp,(Z)2TUTI11(2)3ZZI231丿21126=乔eXP(z)+ZZ3311112丿-3(X)-1(z)+1XZ(8261221丿i=1p(xz)=21:21:2p(zX)p(xZ)2221p(zz)1exp、汎zy2I1(z-X)2222%5丿xexp2叮2兀JU二活exp乔exp(4(1X-21112-n(z2)2-33(Z1匕+11Z1Z2丿-Z+2Z12丿丿2X1119)因此，k=2时刻的后验滤波概率密度p(X2Z)N/4(1Z+2Z4、1:2111(2i2丿1120)这样依次递推，即可得到每一时刻X2的后验概率

12、密度，从而实现对状态量的后验滤波。贝叶斯重要性采样和序贯重要性采样3.1贝叶斯重要性采样在实际问题中，求出后验概率密度后，需要根据后验密度得到某个函数的估计值。然而，后验概率密度函数与其他函数乘积的积分往往很难求解。根据蒙特卡洛仿真原理，只需要按照后验概率分布对状态量进行抽样，将每一个抽样得到的状态量称为一个“粒子”，然后将每个粒子带入函数，求函数的均值。该均值可以近似看作函数的估计值。E(g(X)L丄g(X(i)(21)0:kN0:ki=1从上式中可以看出，当N越大该近似值越接近E(g(X)。0:k然而，从后验密度中直接抽样是难以实现的。于是，引入重要性概率密度的参考分布q(XZ)，该参考分布应该是便于抽样的，利用重要性概率密度和贝叶斯公式综合运算可ok1:k得丄g(x(i)w*(X(i)E(g(X)”巴0：k丄艺w*(X(i)Nk0:ki=1=Ng(X(i)w(X(i)0:kk0:k其中w*(X(i)=pGik(k0：kqxx)p(x)0:k0:k0:k1kw*(x(i)k0:k HYPERLINK l bookmark109 o Current Document w(X(i)=k0：k兰w*(x(i)k0:ki=123)这样，引入重要性概率密度的参考分布后，将其转化为每个粒子