粒子滤波算法简介与matlab程序

粒子滤波作者-niewei120——nuaaBayes法则：贝叶斯定理由英国数学家贝叶斯(ThomasBayes1702-1763)发展，用来描述两个条件概率之间的关系，比如P(A|B)和P(B|A)。按照乘法法则：P(A∩B)=P(A)*P(B|A)=P(B)*P(A|B)，可以立刻导出贝叶斯定理公式：P(A|B)=P(B|A)*P(A)/P(B)。通常，事件A在事件B(发生)的条件下的概率，与事件B在事件A的条件下的概率是不一样的；然而，这两者是有确定的关系,贝叶斯法则就是这种关系的陈述。Pr(A)是A的先验概率或边缘概率。之所以称为先验是因为它不考虑任何B方面的因素。Pr(A|B)是已知B发生后A的条件概率，也由于得自B的取值而被称作A的后验概率。Pr(B|A)是已知A发生后B的条件概率，也由于得自A的取值而被称作B的后验概率。Pr(B)是B的先验概率或边缘概率，也作标准化常量（normalizedconstant）。先验概率的计算比较简单，没有使用贝叶斯公式；而后验概率的计算，要使用贝叶斯公式。若用Pr(B|A)/Pr(B)表示标准似然度，则后验概率=标准似然度*先验概率。例子：一座别墅在过去的20年里一共发生过2次被盗，别墅的主人有一条狗，狗平均每周晚上叫3次，在盗贼入侵时狗叫的概率被估计为0.9，问题是：在狗叫的时候发生入侵的概率是多少？我们假设A事件为狗在晚上叫，B为盗贼入侵，则P(A)=3/7，P(B)=2/(20·365)=2/7300，P(A|B)=0.9，按照公式很容易得出结果：P(B|A)=0.9*(2/7300)/(3/7)=0.00058贝叶斯决策理论方法基本思想是：1、已知类条件概率密度参数表达式和先验概率。2、利用贝叶斯公式转换成后验概率。3、根据后验概率大小进行决策分类。贝叶斯滤波的核心思想就是利用已知的信息来判断状态变量的后验概率，在目标跟踪中也就是对所有观测值Z1：Zk={Z1,Z2…Zk}已知的情况下，计算出后验概率P(Xk|Z1:k)，其计算的方法具体分为预测和更新。预测：在系统还没有获得k时刻观测值Zk情况下，利用k-1时刻的先验概率P(Xk-1|Z1:k-1)及k时刻的先验概率P(Xk|Z1:k-1)得到预测值。更新：更新实际上是对预测的一个修正过程，利用最新的观测值Zk和前面求得到P(Xk|Z1:k-1)得到k时刻的后验概率P(Xk|Z1:k)。蒙特卡洛方法蒙特卡洛(MonteCarlo)方法或者称为随机采样方法，由于现实中的很多问题无法直接分析求解，所以在统计上就引入了随机模拟方法，利用随机过程来做统计检验的算法。其基本思想为：当所求解问题是某种随机事件出现的概率，或者是某个随机变量的期望值，或者是某个随机变量的函数的期望值时，通过某种“实验”的方法，以这种事件出现的频率估计这一随机事件的概率，或者得到这个随机变量的某些数字特征，并将其作为问题的解。有一个例子可以使你比较直观地了解蒙特卡洛方法：假设我们要计算一个不规则图形的面积，那么图形的不规则程度和分析性计算（比如，积分）的复杂程度是成正比的。蒙特卡洛方法是怎么计算的呢？假想你有一袋豆子，把豆子均匀地朝这个图形上撒，然后数这个图形之中有多少颗豆子，这个豆子的数目就是图形的面积。当你的豆子越小，撒的越多的时候，结果就越精确。在这里我们要假定豆子都在一个平面上，相互之间没有重叠。在解决实际问题的时候应用蒙特卡洛方法主要有两部分工作：用蒙特卡洛方法模拟某一过程时，需要产生各种概率分布的随机变量。用统计方法把模型的数字特征估计出来，从而得到实际问题的数值解。粒子滤波的方法1.蒙特卡洛方法的求积分首先利用蒙特卡洛方法的求解积分过程如下：加入要对进行积分运算，上式为一个基于X变量的积分，p(X)为X的概率分布，f(X)是p(X)的可积函数，独立抽取N个随机样本服从分布p(X),且独立同分布，那么概率分布p(X)可以近似地写为：后者为狄拉克δ函数，则积分过程可以近似表示为：2.利用蒙特卡洛方法的求期望由于期望是对函数和函数概率的积分，了更加容易实现积分，引入了重要性采样的思想，通过从引入的一个密度函数采样，从而避开了十分困难的采样环节，能够对问题进行求解。对于期望有E[g(X)]=∫g(X)p(X)dX，由于p(X)在理论上求解很困难，因此利用重要性采样把这个困难的问题转换成一个可知的密度函数q(X)，从中采样，再求得g(X)的期望。因此对于求g(X)的数学期望转换成了通过已知概率密度函数q(X)，求解的期望的问题。在采样点足够多的时候有：从q(X)中采样得到样本X(i)，因此对于个g(x)的数学期望中，是多出来的，把这部分称为重要性即权重，作为样本即粒子的估计的权重。3.利用蒙特卡洛方法的求贝叶斯滤波中的函数f(X0:k)的条件期望令重要性为，故而有：带入上式有：利用近似方法有：为减小计算量，将重要性的权重值，转化为递推方法表示，这样能较好的降低迭代的计算量的问题。假设状态估计的过程是最优估计，那么选区的作为参考的概率密度函数就只会依赖于Xk-1和Zk，则有：则之后的权重可以表示为：4.粒子滤波的流程粒子滤波的matlab实现系统状态模型为：11,1,2,...,kkkxxukn（1.1）系统观测模型为：1tan(/)kkkkzyxv（1.2）假设观测噪声为一零均值高斯白噪声，则权值计算为：式1.5*()()1()()1()()21()(tan(/))exp(tan(/))2iiiiiikxkkvkkkkkkpzxpzyxzyxclear;echooff;%-----------------Parameter--------------------------xMEAN=4.5;xSIGMA=0.2;%初始化采样粒子的位置和速度yMEAN=4;ySIGMA=0.3;VxMEAN=0.2;VxSIGMA=0.01;VyMEAN=0.15;VySIGMA=0.02;UxMEAN=0;UxSIGMA=0.015625;%初始化x方向和y方向噪声，均为零均值高斯白噪声UyMEAN=0;UySIGMA=0.015625;%标准差为0.015625UzMEAN=0;UzSIGMA=0.001;rSTEP=30;%采样30个时刻wk=-1/(2*UzSIGMA);%粒子权重常数wj=1/sqrt(2*pi*UzSIGMA);%粒子权重常数1/sqr(2*pi)rN=8000;%粒子数为1024w_buffer=zeros(rN,1);%存储权值的空间r_buffer=zeros(rN,1);%存储复制次数的空间i_buffer=zeros(rN,1);%存储粒子指针的空间tX=zeros(rSTEP,1);tY=zeros(rSTEP,1);tVx=zeros(rSTEP,1);tVy=zeros(rSTEP,1);tZ=zeros(rSTEP,1);%每一时刻状态的估计值rX=zeros(rSTEP,1);rY=zeros(rSTEP,1);rVx=zeros(rSTEP,1);rVy=zeros(rSTEP,1);%每一时刻状态的真实值x0=4.5;y0=4.5;Vx0=0.2;Vy0=0.2;%初始化真实状态的初值%-----------------真实状态值和观测角度值-----------------------------tX(1,1)=x0;tY(1,1)=y0;tVx(1,1)=Vx0;tVy(1,1)=Vy0;w0=gauss(UxMEAN,UxSIGMA,rSTEP);%噪声w1=gauss(UyMEAN,UySIGMA,rSTEP);w2=gauss(UzMEAN,UzSIGMA,rSTEP);tZ(1,1)=atan(tY(1,1)./tX(1,1))+w2(1,1);%初始测量角fort=2:rSTEPtX(t,1)=tX(t-1,1)+tVx(t-1,1)+w0(t,1);%tX(t,1)用来存储目标在x方向上的真实坐标值tY(t,1)=tY(t-1,1)+tVy(t-1,1)+w1(t,1);%tY(t,1)用来存储目标在y方向上的真实坐标值tVx(t,1)=tVx(t-1,1)+0.5.*w0(t,1);%tVx(t,1)用来存储目标在x方向上的真实速度值tVy(t,1)=tVy(t-1,1)+0.5.*w1(t,1);%tVy(t,1)用来存储目标在y方向上的真实速度值tZ(t,1)=atan(tY(t,1)./tX(t,1))+w2(t,1);%tZ(t,1)用来存储每一时刻的测量角end;%---------------初始化----------------------------x_buffer=gauss(xMEAN,xSIGMA,rN);Vx_buffer=gauss(VxMEAN,VxSIGMA,rN);y_buffer=gauss(yMEAN,ySIGMA,rN);Vy_buffer=gauss(VyMEAN,VySIGMA,rN);fori=1:rNw_buffer(i,1)=1/rN;%初始权重值r_buffer(i,1)=1;%初始复制次数i_buffer(i,1)=i;%初始粒子指针end;iR=rN;%需重采样粒子数初始为rN%------------------粒子滤波-------------------------------fort=1:rSTEP%----------------采样----------------------------indr=1;indd=rN;w0=gauss(UxMEAN,UxSIGMA,rN);w1=gauss(UyMEAN,UySIGMA,rN);reg=zeros(1,4);forindr=1:iRx=i_buffer(indr,1);%粒子指针reg(1,1)=x_buffer(x,1);%reg用来存储每一时刻目标x,y方向上的估计坐标值和x,y方向上的的估计速度值reg(1,2)=Vx_buffer(x,1);%reg(1,3)=y_buffer(x,1);reg(1,4)=Vy_buffer(x,1);x_buffer(x,1)=reg(1,1)+reg(1,2)+w0(indr,1);%Vx_buffer(x,1)=reg(1,2)+0.5.*w0(indr,1);y_buffer(x,1)=reg(1,3)+reg(1,4)+w1(indr,1);Vy_buffer(x,1)=reg(1,4)+0.5.*w1(indr,1);fork=r_buffer(indr,1)-1:-1:1x_buffer(i_buffer(indd,1),1)=reg(1,1)+reg(1,2)+w0(indd,1);%复制粒子Vx_buffer(i_buffer(indd,1),1)=reg(1,2)+0.5.*w0(indd,1);y_buffer(i_buffer(indd,1),1)=reg(1,3)+reg(1,4)+w1(indd,1);Vy_buffer(i_buffer(indd,1),1)=reg(1,4)+0.5.*w1(indd,1);indd=indd-1;end;end;%----------------权值计算---------------------W=0;fori=1:rN;x=tZ(t,1)-atan(y_buffer(i,1)./x_buffer(i,1));%计算公式见报告中式1.5w_buffer(i,1)=exp(wk*x*x);%第i个粒子的权值W=W+w_buffer(i,1);%权值的累加和end;%----------------状态输出---------------------------X=0;Vx=0;Y=0;Vy=0;fori=1:rN;X=X+w_buffer(i,1).*x_buffer(i,1);%计算x方位值Vx=Vx+w_b