粒子滤波的多目标检测前跟踪程序_MPF_TBD_matlab

clc clear all T=1; Dx=1; Dy=1; nx=50; my=50; v1=2; v2=1; L=0.7; q1=0.001; q2=0.01; sigma=1;%量测复高斯噪声标准差 SNR1=12; %%%信噪比 SNR2=12; %%%信噪比 P1=2*sigma^2*(10^(SNR1/10));%目标信号功率 P2=2*sigma^2*(10^(SNR2/10)); Ak1=sqrt(P1); Ak2=sqrt(P2); I1=Ak1/2; I2=2*Ak2; pai=[0.9 0.1 0.0 0.1 0.8 0.1 0.0 0.1 0.9];%%马尔科夫状态转移模型 Q1=[q1*T^3/3 q1*T^2/2 0 q1*T^2/2 q1*T 0]; Q2=[0 0;0 0]; Q=[Q1 Q2 Q2 Q1 0 0 0 0 q2*T];%%过程噪声协方差 F=[1 T 0 0 0 0 1 0 0 0 0 0 1 T 0 0 0 0 1 0 0 0 0 0 1]; X01=[10,1,25,0.5,Ak1]';%%目标初始状态 X02=[15,1,8,0.5,Ak2]';%%目标初始状态 N=5000; Nth=1000; K=25; X1=zeros(5,K);%真实状态 X2=zeros(5,K);%真实状态 Z=zeros(nx,my);%真实量测 V=zeros(1,1);%真实量测噪声 x=zeros(5,K,N); %粒子状态 e=zeros(K,N); %存在状态 errorX1=zeros(K); errorY1=zeros(K); errorX2=zeros(K); errorY2=zeros(K); p=3; lp=5; % %------------真实状态----------------- for k=1:K if k==4 X1(:,k)=X01; elseif k>4&&k<=16 X1(:,k)=F*X1(:,k-1)+sqrt(Q)*randn(5,1);%%目标1在4-16帧存在 end end for k=1:K if k==10 X2(:,k)=X02; elseif k>10&&k<=22 X2(:,k)=F*X2(:,k-1)+sqrt(Q)*randn(5,1);%%目标1在10-22帧存在 end end % figure(2) % plot(X1(1,:),X1(3,:),'r-o'); % axis([0 50 0 50]) % hold on; % plot(X2(1,:),X2(3,:),'b-o'); for k=1:K for l=1:lp % %------------生成量测----------------- for i=1:nx for j=1:my H=X1(5,k)*exp(-((i*Dx-X1(1,k))^2+(j*Dy-X1(3,k))^2)*L/2)+X2(5,k)*exp(-((i*Dx-X2(1,k))^2+(j*Dy-X2(3,k))^2)*L/2); Zmeasure=H+sigma*(randn+1j*randn); Z(i,j)=(abs(Zmeasure))^2; end end % surf(Z); % %------------粒子状态----------------- for t=1:N%%%对于每一个粒子 if k==1&&rand<=pai(1,2) e(k,t)=1;%%粒子初始状态设为1，表示存在 x(:,k,t)=shengcheng5D(nx,my,v1,v2,I1,I2); elseif k>=2 if e(k-1,t)==0&&rand<=pai(1,2)%新生粒子 e(k,t)=1; x(:,k,t)=shengcheng5D(nx,my,v1,v2,I1,I2); elseif e(k-1,t)==1&&rand<=pai(2,2)%继续粒子 e(k,t)=1; x(:,k,t)=F*x(:,k-1,t)+sqrt(Q)*randn(5,1); elseif e(k-1,t)==1&&rand>=(1-pai(2,3))%新生粒子 e(k,t)=2; x(:,k,t)=shengcheng5D(nx,my,v1,v2,I1,I2); elseif e(k-1,t)==2&&rand<=pai(3,3)%继续粒子 e(k,t)=2; x(:,k,t)=F*x(:,k-1,t)+sqrt(Q)*randn(5,1); end end X_rap(:,t)=[x(:,k,t);e(k,t)]; % %------------粒子权值----------------- q(t)=1;%%所有粒子初始权重为1 if e(k,t)%%如果该粒子存在 % for i=1:nx % for j=1:my ix=round(x(1,k,t));%%x方向上的位置单元最近边界 jy=round(x(3,k,t));%%y方向上的位置单元最近边界 for i=max(1,ix-p):min(nx,ix+p) for j=max(1,jy-p):min(my,jy+p) h=X1(5,k).^2*exp(-((i*Dx-X1(1,k))^2+(j*Dy-X1(3,k))^2)*L)+X2(5,k).^2*exp(-((i*Dx-X2(1,k))^2+(j*Dy-X2(3,k))^2)*L)+2*sigma^2; L1=(1/h)*exp((-1/h)*Z(i,j)); L2=(1/(2*sigma^2))*exp(-(1/(2*sigma^2))*Z(i,j)); q(t)=q(t)*L1/L2; end end end end % %------------重采样----------------- q=q/sum(q);%权重归一化 W=cumsum(q); u=rand(N+1,1); u=u/sum(u); U=cumsum(u); X_ra=zeros(6,N); %重采样之后的粒子状态 Neff=1/(sum(q.^2)); if Neff<Nth i=1; j=1; while j<=N&&i<=N if W(j)>=U(i) X_ra(:,i)=X_rap(:,j); i=i+1; else j=j+1; end end else X_ra=X_rap; end % %------------更新状态----------------- Xf1=zeros(4,1); Xf2=zeros(4,1); effect1=0; effect2=0; for t=1:N e(k,t)=X_ra(6,t); x(:,k,t)=X_ra(1:5,t); if e(k,t)==1 %第一个目标的粒子 effect1=effect1+1; Xf1=Xf1+X_ra(1:4,t); elseif e(k,t)==2 %第一个目标的粒子 effect2=effect2+1; Xf2=Xf2+X_ra(1:4,t); end end if effect1 Xi1(:,k)=Xf1/effect1; else Xi1(:,k)=Xf1/N; end if effect2 Xi2(:,k)=Xf2/effect2; else Xi2(:,k)=Xf2/N; end errorXX1(k)=(Xi1(1,k)-X1(1,k)).^2; errorYY1(k)=(Xi1(3,k)-X1(3,k)).^2; errorXX2(k)=(Xi2(1,k)-X2(1,k)).^2; errorYY2(k)=(Xi2(3,k)-X2(3,k)).^2; errorX1(k)=errorX1(k)+errorXX1(k); errorY1(k)=errorY1(k)+errorYY1(k); errorX2(k)=errorX2(k)+errorXX2(k); errorY2(k)=errorY2(k)+errorYY2(k); end rmseX1(k)=sqrt(1/lp*errorX1(k)); rmseX2(k)=sqrt(1/lp*errorX2(k)); rmseY1(k)=sqrt(1/lp*errorY1(k)); rmseY2(k)=sqrt(1/lp*errorY2(k)); end % %------------画粒子云-----------------Ak1 % figure(1) % plot(X_ra(1,:),X_ra(3,:),'r*'); % %------------------------------------ figure(3) plot(Xi1(1,4:16),Xi1(3,4:16),'r-p',Xi2(1,10:22),Xi2(3,10:22),'k-p'); hold on; plot(X1(1,4:16),X1(3,4:16),'b-*',X2(1,10:22),X2(3,10:22),'b-*'); legend('第1个目标的跟踪效果','第2个目标的跟踪效果') figure(4) plot(rmseX1(1:K),'r'); hold on plot(rmseY1(1:K),'g'); legend('X方向误差','Y方向误差'); title('第一个目标跟踪误差') figure(5) plot(rmseX2(1:K),'r'); hold on plot(rmseY2(1:K),'g'); legend('X方向误差','Y方向误差'); title('第二个目标跟踪误差')