基于粒子滤波实现的目标被动跟踪

姓名：朱林富 学号：08120361 研究方向：粒子滤波基于粒子滤波实现的目标被动跟踪1.1被动定位系统是一个仅有角测量的系统，通常对于目标距离是不可测的。

由于实时处理和计算存储量的需求，通常选用递推滤波算法来实现。

由于系统本身的弱观测性，状态空间模型非线性强，导致滤波算法的收敛精度和收敛时间满足不了要求。

处理这种非线性、非高斯问题，粒子滤波算法有很好的表现。

粒子滤波的基本思想是：首先依据系统状态向量的经验条件分布，在状态空间抽样产生一组随机样本集合，这些样本集合称为粒子；然后根据观测值不断调整粒子的权重大小和样本位置；最后通过调整后的粒子信息修正最初的的经验条件分布，估计出系统状态和参数。

该算法是一种递推滤波算法，可以用来估计任意非线性非高斯随机系统的状态和参数。

粒子滤波主要有三步基本操作：采样（从不含观察值的状态空间产生新的粒子）、权值计算（根据观察值计算各个粒子的权值）、重采样（抛弃权值小的粒子，使用权值大的粒子代替），这三步构成了粒子滤波的基本算法。

SIRF(Sample Importance Resampling Filter)算法是粒子滤波的一种基本算法。

1.2被动定位系统中二维纯方位目标跟踪模型如上图所示。

以观测站为原点，建立二维直角坐标系，图中标出了目标在k 时刻、k-1时刻的位置，目标到观测站的距离R 不可测。

系统状态模型为：11,1,2,...,k k k x x u k n --=Φ+Γ= （1.1） 系统观测模型为：1tan (/)k k k k z y x v -=+ （1.2）其中[,,,]k k Tk k x k y x x V y V =为系统的k 时刻状态值（目标在坐标系x,y 方向上的位置和速度），11k-1u [,]k k Tx y u u --=为k-1时刻x,y 方向上的系统噪声，k v 为k 时刻的观测噪声。

k z 为k 时刻的观测角度。

1100010000110001⎡⎤⎢⎥⎢⎥Φ=⎢⎥⎢⎥⎣⎦ 100.500100.5⎡⎤⎢⎥⎢⎥Γ=⎢⎥⎢⎥⎣⎦为了实现方便，设定系统噪声为一零均值高斯白噪声。

初始状态x 描述了被跟踪目标的初始状态。

传感器测量目标的角度，会产生测量误差和受到测量噪声的污染，测量方程式如1.2所示。

为了简便，其中测量噪声设定为一零均值高斯白噪声v k ～N （0，r ）1.3在被动定位系统的二维纯方位目标跟踪中，SIRF 算法的步骤如下：1）采样:根据系统状态方程式（1.1）采样得到k 时刻粒子集{}()1,1,2,...,N i k i x i N ==11111111()()()1()()()()()1()()0.50.5k k k k k k k k k k i i i i k k x x i i i x x x i i i i kk y y i i i y y y x x V V V yyVV V --------()-()()-()=++μ=+μ=++μ=+μ（1.3）2）权值计算：选取先验概率密度函数()1()i k k p x x -|为重要性函数，意味着权值更新为()()1()i i ik k k k p z x -ω∝ω|，且由观测方程式（ 1.2）得1k v t a n (/)k k k z y x -=- （1.4）由于观测噪声与系统状态相互独立，根据式（1.4）得()()1()()()1()()()()((tan (/)))(tan (/))i i i i i i i x k k v k k v k k k k v k k k p z x p v x p z y x x p z y x --|=|=-|=- 假设观测噪声为一零均值高斯白噪声，则权值计算为：式1.5*()()1()()1()()21()(tan (/))exp (tan (/))2i i i i i i k x k k v k k k k k k p z x p z y x z y x --2⎡⎤ω=|=-=--⎢⎥δ⎣⎦ 3）权值归一化：当各个粒子权值计算完成后进行权值归一化()*()*()1/Ni i i k kk i =ω=ωω∑ 4）重采样：采用RSR 重采样算法，获得新粒子集{}()()1,Ni i k k i x =ω----------------重采样程序------------------------------------------------------------indr=1;indd=rN; %设置指针的初始值 K=rN/W; %计算中间变量KU=rand(1,1); %随机生成一个随机阈值 for i=1:rN; %主循环temp=K.*w_buffer(i,1)-U; %添加一个中间变量temp r_buffer(indr,1)=quzheng(temp); %U=r_buffer(indr,1)-temp; %更新阈值 if r_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ˆˆˆˆˆk kk kNi i k k k i Ni i x k x i Ni i k k k i Ni i y k y i xx V V yy V V =====ω=ω=ω=ω∑∑∑∑----------------状态输出程序---------------------------------------------------------- X=0;Vx=0;Y=0;Vy=0; for i=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 算法功能框图如下图所示：每输入一个观测值k z 都将经过5步产生一个状态值输出ˆk x，这5步是：采样、权值计算、权值归一化、重采样、状态值输出。

1.4在MATLAB 中仿真目标跟踪过程，取粒子数N=1024，为实现简便设系统噪声和测量噪声均为零均值高斯白噪声。

采样粒子位置和速度的初始分布分别为：00202022()(4.5,0.2)()(4,0.3)()(0.2,0.01)()(0.15,0.02)x y p x N p y N p V N p V N ====设真实的轨迹初始位置和速度为：00004.5; 4.5;0.2;0.2x y x y V V ====--------------------------------系统参数初始化程序--------------------------------------------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.015625 UzMEAN=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); %初始测量角for t=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文件仿真结果如下图所示，虚线为目标实际运动轨迹，实线为估计的目标运动轨迹。