粒子滤波,程序

粒子滤波(ParticleFilter，PF)，又称为序贯蒙特卡罗算法，是一种基于蒙特卡罗方法的贝叶斯滤波技术。粒子滤波的基本原理是寻找一组在状态空间传播的随机粒子（样本）描述系统的状态，通过蒙特卡罗方法处理贝叶斯估计中的积分运算，从而得到系统状态的最小均方差估计。当粒子数量区域无穷时可以逼近服从任意概率分布的系统状态。与其他滤波技术相比，粒子滤波不需要对系统状态做任何先验性假设，原则上可以应用于任何能用状态空间模型描述的随机系统。一、贝叶斯估计贝叶斯定理是贝叶斯估计方法的理论基础。贝叶斯定理表达如下：(|)()(|)()fyxfxfxyfy其中，x为待估计参数，y为样本观测值信息，即样本信息，f(x)是待估计参数x的先验分布密度函数，f(x|y)是x的后验分布密度函数，f(y)和f(y|x)是y的密度函数。因此通过上式可以看出，后验信息正比于样本信息与先验信息的乘积。可以通过样本信息对先验信息进行修正来得到更准确的后验信息。得到后验分布的密度函数后，就可以此为基础进行参数的点估计、区间轨迹和假设检验。二、序贯重要性采样方法序贯重要性采样方法的核心思想是利用一系列随机样本的加权和所需的验后概率密度得到状态的估计值。当样本点的数量无穷多时，蒙特卡罗特性与验后概率密度的函数表达等价，序贯重要性采样滤波器近似于贝叶斯滤波器。对于如下的非线性系统：(1)[(),()]()[(),()]xkfxkwkzkhxkvk式中，f(·)和h(·)是非线性函数，w(k)和v(k)是系统的状态噪声和观测噪声。设001[,,,]kkxxxx为从0~k时刻所有状态向量的集合，112[,,,]kkzzzz为1~k时刻所有观测向量的集合。滤波过程中利用01kkxz和获得最优的xk+1，即1{[()]}[()][()|]()kEfxkfxkpxkzdxk一般而言，1|kpxkz是多变量且非高斯的很难直接采样，可以用与其近似的分布1[()|]kxkz代替它进行采样，则1111111111[x(k)][()][()|]()[()|][()][()|]()[()|][|()][()][()][()|]()[][()|][x(k)][()][()|]()[]kkkkkkkkkkEffxkpxkzdxkpxkzfxkxkzdxkxkzpzxkpxkfxkxkzdxkpzxkzwfxkxkzdxkpz式中1[()|]kxkz称为重要性函数，而11[|()][()][()][()|]kkpzxkpxkwxkxkz称为重要性权值。考虑到状态演变和观测条件的相对独立性，通常认为状态0kx具有马尔可夫性，z(k)仅和x(k)有关，那么1111111[()|][()|(1),][(1)|][|()][()|()][|(1)]kkkkkxkzxkxkzxkzpzxkpzkxkpzxk将上式代入重要性权值公式，则w[x(k)]可以写成11111111[|()][()][()][()|][()|()][|(1)][()|(1)][(1)][()|(1),][(1)|][()|()][()|(1)][(1)][()|(1),]kkkkkkpzxkpxkwxkxkzpzkxkpzxkpxkxkpxkxkxkzxkzpzkxkpxkxkwxkxkxkz在适当选取重要性函数1[()|(1),z]kxkxk的基础上，可通过贝叶斯体系中的时间更新和观测更新步骤，以序贯更新重要性权值。对于重要性函数1[()|(1),z]kxkxk，如果能够从目标验后概率密度1[()|]kpxkz采样出N个粒子[xi(k)],i=1,2,···，N，则可以用经验概率分布以近似验后概率密度11[()|z]()[()()]Nkiiixkwkxkxk式中，()为狄拉克函数。三、基本粒子滤波器步骤基本粒子滤波包括样本采样和再采样，结合贝叶斯滤波体系的时间更新和观测更新两步骤进行，具体算法步骤如下：(1)、初始化：t=0。①从i=1,2,···,N,按照初始重要性函数[(0)]x选取初始粒子群1,2,[(0)]iiNx。②从i=1,2,···,N，估计初始粒子额重要性权值i[(0)|(0)][(0)][(0)][(0)|(0)]ipzxpxwxxz③从i=1,2,···,N，归一化初始重要性权值1[(0)](0)[(0)]iiNiiwxwwx(2)、时间更新：t=k-1,k1。从i=1,2,···,N，按照重要性函数11[()|(1),z]kiixkxk选取更新状态后的粒子群1,2,[()]iiNxk，且()[(1)]iixkfxk。(3)、观测更新：t=k。①从i=1,2,···,N，在已获得z(k)的情况下，估计重要性权值系数1[()|()][()|(1)][()][(1)][()|(1),]iiiiikiipzkxkpxkxkwxkwxkxkxkz②从i=1,2,···,N，归一化重要性权值1[()]()[()]iiNiiwxkwkwxk(4)、重采样①从i=1,2,···,N，根据权值()iwk，分别复制高权值粒子，舍弃低权值粒子，从而中心产生N个粒子集1,2,[()]iiNxk。②从i=1,2,···,N，归一化权值1()iwkN(5)、输出结果111()()()[()|]()[()()]NNkiiiiiixkwkxkxkzwkxkxk步骤(2)至(5)循环进行。四、粒子滤波的仿真在上图中，蓝色的点代表状态的真值，绿色折线为粒子滤波的估计。从图中可以看出粒子滤波具有较好的滤波效果。程序：x=0.1;%初始状态Q=1;%模型噪声W的方差R=1;%测量噪声V的方差tf=50;%仿真长度N=100;%粒子滤波的粒子数xhat=x;P=2;%状态的方差xhatPart=x;%初始化粒子滤波fori=1:Nxpart(i)=x+sqrt(P)*randn;%状态值采样endxArr=[x];xhatPartArr=[xhatPart];fork=1:tf%tf为时间长度，k可以理解为时间轴上的k时刻%x的数据为时刻k的真实状态值x=0.5*x+25*x/(1+x^2)+8*cos(1.2*(k-1))+sqrt(Q)*randn;%状态方程(1)05101520253035404550-20-15-10-505101520timestepstate真值粒子滤波估计y=x^2/20+sqrt(R)*randn;%观测方程(2)(真值)%Particlefilter生成100个粒子并根据预测和观测差值计算各个粒子的权重fori=1:Nxpartminus(i)=0.5*xpart(i)+25*xpart(i)/(1+xpart(i)^2)+8*cos(1.2*(k-1))+sqrt(Q)*randn;ypart=xpartminus(i)^2/20;%(预测值)vhat=y-ypart;%观测和预测的差q(i)=(1/sqrt(R)/sqrt(2*pi))*exp(-vhat^2/2/R);%根据差值给出权重end%Normalizethelikelihoodofeachaprioriestimate.qsum=sum(q);fori=1:Nq(i)=q(i)/qsum;%归一化权重end%类似与遗传算法进行选择fori=1:Nu=rand;%uniformrandomnumberbetween0and1qtempsum=0;forj=1:Nqtempsum=qtempsum+q(j);ifqtempsum=u%重采样低权重进行剔除，同时保留高权重，防止退化方法xpart(i)=xpartminus(j);break;endendend%Theparticlefilterestimateisthemeanoftheparticles.xhatPart=mean(xpart);%经过粒子滤波处理后的初值%SavedatainarraysforlaterplottingxArr=[xArrx];xhatPartArr=[xhatPartArrxhatPart];endt=0:tf;figure;plot(t,xArr,'b.',t,xhatPartArr,'g');xlabel('timestep');ylabel('state');legend('真值',''粒子滤波估计);