粒子滤波的公式和matlab

姓名：朱林富学号：08120361研究方向：粒子滤波基于粒子滤波实现的目标被动跟踪1.1被动定位系统是一个仅有角测量的系统，通常对于目标距离是不可测的。由于实时处理和计算存储量的需求，通常选用递推滤波算法来实现。由于系统本身的弱观测性，状态空间模型非线性强，导致滤波算法的收敛精度和收敛时间满足不了要求。处理这种非线性、非高斯问题，粒子滤波算法有很好的表现。粒子滤波的基本思想是：首先依据系统状态向量的经验条件分布，在状态空间抽样产生一组随机样本集合，这些样本集合称为粒子；然后根据观测值不断调整粒子的权重大小和样本位置；最后通过调整后的粒子信息修正最初的的经验条件分布，估计出系统状态和参数。该算法是一种递推滤波算法，可以用来估计任意非线性非高斯随机系统的状态和参数。粒子滤波主要有三步基本操作：采样（从不含观察值的状态空间产生新的粒子）、权值计算（根据观察值计算各个粒子的权值）、重采样（抛弃权值小的粒子，使用权值大的粒子代替），这三步构成了粒子滤波的基本算法。SIRF(SampleImportanceResamplingFilter)算法是粒子滤波的一种基本算法。1.2yx观测站目标轨迹xkyk-1xk-1ykzk-1zk被动定位系统中二维纯方位目标跟踪模型如上图所示。以观测站为原点，建立二维直角坐标系，图中标出了目标在k时刻、k-1时刻的位置，目标到观测站的距离R不可测。系统状态模型为：11,1,2,...,kkkxxukn（1.1）系统观测模型为：1tan(/)kkkkzyxv（1.2）其中[,,,]kkTkkxkyxxVyV为系统的k时刻状态值（目标在坐标系x,y方向上的位置和速度），11k-1u[,]kkTxyuu为k-1时刻x,y方向上的系统噪声，kv为k时刻的观测噪声。kz为k时刻的观测角度。1100010000110001100.500100.5为了实现方便，设定系统噪声为一零均值高斯白噪声。初始状态x描述了被跟踪目标的初始状态。传感器测量目标的角度，会产生测量误差和受到测量噪声的污染，测量方程式如1.2所示。为了简便，其中测量噪声设定为一零均值高斯白噪声vk～N（0，r）1.3在被动定位系统的二维纯方位目标跟踪中，SIRF算法的步骤如下：1）采样:根据系统状态方程式（1.1）采样得到k时刻粒子集()1,1,2,...,NikixiN11111111()()()1()()()()()1()()0.50.5kkkkkkkkkkiiiikkxxiiixxxiiiikkyyiiiyyyxxVVVyyVVV（1.3）2）权值计算：选取先验概率密度函数()1()ikkpxx为重要性函数，意味着权值更新为()()()1()iiikkkkpzx，且由观测方程式（1.2）得1kvtan(/)kkkzyx（1.4）由于观测噪声与系统状态相互独立，根据式（1.4）得()()1()()()1()()()()((tan(/)))(tan(/))iiiiiiixkkvkkvkkkkvkkkpzxpvxpzyxxpzyx假设观测噪声为一零均值高斯白噪声，则权值计算为：式1.5*()()1()()1()()21()(tan(/))exp(tan(/))2iiiiiikxkkvkkkkkkpzxpzyxzyx3）权值归一化：当各个粒子权值计算完成后进行权值归一化()*()*()1/Niiikkki4）重采样：采用RSR重采样算法，获得新粒子集()()1,Niikkix----------------重采样程序------------------------------------------------------------indr=1;indd=rN;%设置指针的初始值K=rN/W;%计算中间变量KU=rand(1,1);%随机生成一个随机阈值fori=1:rN;%主循环temp=K.*w_buffer(i,1)-U;%添加一个中间变量tempr_buffer(indr,1)=quzheng(temp);%U=r_buffer(indr,1)-temp;%更新阈值ifr_buffer(indr,1)0%i_buffer(indr,1)=i;%存储被复制粒子的地址indr=indr+1;elsei_buffer(indd,1)=i;%存储被抛弃粒子的地址indd=indd-1;end;iR=indr-1;--------------------------------------------------------------------------------------------------5）状态值输出：()()1()()1()()1()()1ˆˆˆˆˆkkkkNiikkkiNiixkxiNiikkkiNiiykyixxVVyyVV----------------状态输出程序----------------------------------------------------------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_buffer(i,1).*Vx_buffer(i,1);%计算x方向速度值Y=Y+w_buffer(i,1).*y_buffer(i,1);%计算y方位值Vy=Vy+w_buffer(i,1).*Vy_buffer(i,1);%计算y方向速度值end;rX(t,1)=X/W;%输出值归一化rVx(t,1)=Vx/W;rY(t,1)=Y/W;rVy(t,1)=Vy/W;--------------------------------------------------------------------------------------------------SIRF算法功能框图如下图所示：权值计算采样权值归一化重采样状态值输出角度输入zkxkwk*wk每输入一个观测值kz都将经过5步产生一个状态值输出ˆkx，这5步是：采样、权值计算、权值归一化、重采样、状态值输出。1.4在MATLAB中仿真目标跟踪过程，取粒子数N=1024，为实现简便设系统噪声和测量噪声均为零均值高斯白噪声。采样粒子位置和速度的初始分布分别为：00202022()(4.5,0.2)()(4,0.3)()(0.2,0.01)()(0.15,0.02)xypxNpyNpVNpVN设真实的轨迹初始位置和速度为：00004.5;4.5;0.2;0.2xyxyVV--------------------------------系统参数初始化程序--------------------------------------------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;为了便于比较，用一段程序描述目标的真实运动轨迹。-----------------------------真实运动轨迹程序--------------------------------------------------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;初始化和真实轨迹描述完成后，用1.2中讲到的SIRF算法实现被动跟踪。具体步骤分为：1）采样2）权值计算3)权值归一化4）重采样5）状态值输出。详细程序及注释见附件中particle.m文件仿真结果如下图所示，虚线为目标实际运动轨迹，实线为估计的目标运动轨迹