pf_samples.rar_PF 导航_滤波制导_目标跟踪采样_粒子滤波_粒子滤波 图像

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 非高斯条件下粒子滤波的目标跟踪 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %说明：运用PF算法处理闪烁噪声情况下的雷达目标跟踪问题。 % 设目标作匀速直线运动，雷达位于(x0,y0)． % 状态方程为:x(k)=PHI*x(k-1)+G*w(k-1); % 雷达观测方程为:z(k)=h(x(k))+v(k); %function particlefilter clear; echo off; %初始化相关参数 %============== M=100; %采样点数 T=1; %采样间隔 N=300; %粒子数 number=100; %Monte Carlo仿真次数 x0=50000; %初始状态 y0=50000; vx=300; vy=-100; delta_w=0.1; %过程噪声标准差 delta_r=50; %闪烁噪声下观测距离标准差 delta_theta1=1*pi/180; %热噪声对应方位角标准差 delta_theta2=5*pi/180; %闪烁效应对应方位角标准差 eta=0.2; Q=delta_w^2*eye(2); %过程噪声方差阵 R1=diag([delta_r^2,delta_theta1^2]); R2=diag([delta_r^2,delta_theta2^2]); R=(1-eta)*R1+eta*R2; %测量噪声方差阵 G=[T^2/2,0; T,0; 0,T^2/2; 0,T]; resamplingScheme = 1; xmean_pf=zeros(number,M); ymean_pf=zeros(number,M); vxmean_pf=zeros(number,M); vymean_pf=zeros(number,M); %开始仿真（100次） %============== tic;% 开始计时 for j=1:number %%产生真实数据＆量测 %=================== x=zeros(4,M); z=zeros(2,M); xn=zeros(2,M); w=randn(2,M); v=randn(2,M); %w=delta_w*randn(M,2); %vr=delta_r*randn(M,1); %vtheta1=delta_theta1*randn(M,1); %vtheta2=delta_theta2*randn(M,1); x(:,1)=[x0,vx,y0,vy]';%初始状态 xn(:,1)=[x0,y0]'; for t=2:M x(:,t)=feval('sfun',x(:,t-1),T)+G*sqrtm(Q)*w(:,t);%真实数据 z(:,t)=feval('hfun',x(:,t),x0,y0)+sqrtm(R)*v(:,t); xn(:,t)=ffun(z(:,t),x0,y0);%量测 end %%粒子滤波 %========= xparticle_pf=zeros(4,M,N); xparticlePred_pf=zeros(4,M,N); zPred_pf=zeros(2,M,N); W=zeros(M,N); %初始化 for i=1:N xparticle_pf(:,1,i)=[x0,vx,y0,vy]'; end s=randn(2,M); for t=2:M %采样 for i=1:N xparticlePred_pf(:,t,i)=feval('sfun',xparticle_pf(:,t-1,i),T)+G*sqrtm(Q)*s(:,t-1); end %重要性权值计算 for i=1:N zPred_pf(:,t,i)=feval('hfun',xparticlePred_pf(:,t,i),x0,y0); W(t,i)=(1-eta)*inv(sqrt(2*pi*det(R1)))*exp(-.5*(z(:,t)-zPred_pf(:,t,i))'*inv(R1)*(z(:,t)-zPred_pf(:,t,i)))... +eta*inv(sqrt(2*pi*det(R2)))*exp(-.5*(z(:,t)-zPred_pf(:,t,i))'*inv(R2)*(z(:,t)-zPred_pf(:,t,i)))+ 1e-99;%权值计算 end W(t,:)=W(t,:)./sum(W(t,:));%归一化权值 %重新采样（可选择不同方案） if resamplingScheme==1 outIndex = residualR(1:N,W(t,:)'); % Residual resampling. elseif resamplingScheme==2 outIndex = randomR(1:N,W(t,:)'); % random resampling. elseif resamplingScheme==3 outIndex=systematicR(1:N,W(t,:)'); % Systematic resampling. else outIndex=multinomialR(1:N,W(t,:)'); % Multinomial resampling. end; xparticle_pf(:,t,:) = xparticlePred_pf(:,t,outIndex); % 获取新采样值 end %%状态估计 %========= for t=1:M xmean_pf(j,t)=mean(xparticle_pf(1,t,:)); ymean_pf(j,t)=mean(xparticle_pf(3,t,:)); vxmean_pf(j,t)=mean(xparticle_pf(2,t,:)); vymean_pf(j,t)=mean(xparticle_pf(4,t,:)); end end time=toc; xrmse_pf=zeros(1,M); yrmse_pf=zeros(1,M); vxrmse_pf=zeros(1,M); vyrmse_pf=zeros(1,M); ex_pf=zeros(1,M); ey_pf=zeros(1,M); evx_pf=zeros(1,M); evy_pf=zeros(1,M); %%状态估计均方根 %=============== for t=1:M ex_pf(1,t)=mean(xmean_pf(:,t)-x(1,t)); ey_pf(1,t)=mean(ymean_pf(:,t)-x(3,t)); evx_pf(1,t)=mean(vxmean_pf(:,t)-x(2,t)); evy_pf(1,t)=mean(vymean_pf(:,t)-x(4,t)); xstd_pf(1,t)=sqrt(sum((xmean_pf(:,t)-x(1,t)).^2)/number-ex_pf(1,t)^2); ystd_pf(1,t)=sqrt(sum((ymean_pf(:,t)-x(3,t)).^2)/number-ey_pf(1,t)^2); vxstd_pf(1,t)=sqrt(sum((vxmean_pf(:,t)-x(2,t)).^2)/number-evx_pf(1,t)^2); vystd_pf(1,t)=sqrt(sum((vymean_pf(:,t)-x(4,t)).^2)/number-evy_pf(1,t)^2); xrmse_pf(1,t)=sqrt(sum((xmean_pf(:,t)-x(1,t)).^2)/number); yrmse_pf(1,t)=sqrt(sum((ymean_pf(:,t)-x(3,t)).^2)/number); vxrmse_pf(1,t)=sqrt(sum((vxmean_pf(:,t)-x(2,t)).^2)/number); vyrmse_pf(1,t)=sqrt(sum((vymean_pf(:,t)-x(4,t)).^2)/number); end figure(1); plot(x(1,:),x(3,:),'b',xn(1,:),xn(2,:),'g',mean(xmean_pf),mean(ymean_pf),'r'); legend('真实轨迹','观测轨迹','估计轨迹'); figure(2); subplot(2,2,1); plot(1:M,ex_pf); title('x方向位置估计误差均值曲线'); subplot(2,2,2); plot(1:M,evx_pf); title('x方向速度估计误差均值曲线'); subplot(2,2,3); plot(1:M,ey_pf); title('y方向位置估计误差均值曲线'); subplot(2,2,4); plot(1:M,evy_pf); title('y方向速度估计误差均值曲线'); figure(3); subplot(2,2,1); plot(1:M,xstd_pf); title('x方向位置估计误差标准差曲线'); subplot(2,2,2); plot(1:M,vxstd_pf); title('x方向速度估计误差标准差曲线'); subplot(2,2,3); plot(1:M,ystd_pf); title('y方向位置估计误差标准差曲线'); subplot(2,2,4); plot(1:M,vystd_pf); title('y方向速度估计误差标准差曲线'); figure(4); subplot(2,2,1); plot(1:M,xrmse_pf); title('x方向位置估计误差均方根曲线'); subplot(2,2,2); plot(1:M,vxrmse_pf); title('x方向速度估计误差均方根曲线'); subplot(2,2,3); plot(1:M,yrmse_pf); title('y方向位置估计误差均方根曲线'); subplot(2,2,4); plot(1:M,vyrmse_pf); title('y方向速度估计误差均方根曲线'); disp(['computetime = ' num2str(time) 's']);