利用粒子滤波算法进行的目标跟踪代码

%%%%本程序中的航迹图是Beyond the kalman filter:particle filters for tracking %%%%applications 中的 figure 6.2(图像几乎一致，只是仿真数据与书中给出的略有点变化 %%%%因为按书中数据根本得不到此图，所以做了相应改动 ) clear T=1;N=5000;M=30; %%N表示粒子数，M表示采样点个数 A=[1 0 T 0 0 1 0 T 0 0 1 0 0 0 0 1];q1=1.6*10^(-3); B=[T^2/2 0 0 T^2/2 T 0 0 T]; H=[1 0 0 0];r1=pi/sqrt(12); monte=zeros(1,M); for h=1:100 X=zeros(M,4); Y=zeros(M,4); C=zeros(M,4);%%X指的是目标真实的状态,Y指观测平台的运动状态, %%C是程序中的状态 X(1,:)=[5000,800,-130*sin(2*pi/9),-130*cos(2*pi/9) ]; %%目标的初始状态 Y(1,:)=[0,0,235*sin(pi/6),-235*cos(pi/6)];%%平台运动的初始位置 C(1,:)=X(1,:)-Y(1,:);w(:,1)=1/N*ones(N,1);c1=zeros(N+1,1);zt=zeros(M,4);zt(1,:)=X(1,:); W1=zeros(M,4);%%zt是滤波后得到的状态 w(:,1)=random('Normal',0.5,0.25,N,1);z1=sum(w(:,1));p(1)=N; m=0; for j=1:M-1 %%运行的时间 v=randn(1,3); X(j+1,:)=X(j,:)*A'+q1*v(1:2)*B'; %%目标真实的运动状态 if 12<=j||j<=16 Y(j+1,:)=Y(j,:)*A'+[-3*0.5*T^2 15*0.5*T^2 -3*T 15*T]; else Y(j+1,:)=Y(j,:)*A'; end %%平台的运动状态 C(j+1,:)=X(j+1,:)-Y(j+1,:);%%程序中的运动状态 %%C(j+1,:)=C(j,:)*A'-[-3*0.5*T^2 15*0.5*T^2 -3*T 15*T]+q1*v(1:2)*B'; g=atan(C(j+1,1)/C(j+1,2))+r1*v(3); %%传感器得到的量测值 n=0; for i=1:N z(1)=0;%%x的第一列表示的是粒子在时刻1的状态，第二列表示的是粒子在时刻2的状态， %%共i行j列N行100列，N个粒子100个跟踪时刻 if j==1 L=ones(N,1)*C(1,:); L(i,:)=normrnd((L(i,:)*A'-(Y(j+1,:)-Y(j,:)*A')),diag(q1*B*B')'); w(i,j)=w(i,1)/z1; elseif 12<=j||j<=16 L(i,:)=normrnd((L(i,:)*A'-[-3*0.5*T^2 15*0.5*T^2 -3*T 15*T]),diag(q1*B*B')'); else L(i,:)=normrnd((L(i,:)*A'),diag(q1*B*B')'); end %%%% *normpdf(x(i,:,j+1),x(i,:,j)*A'-(Y(j+1,:)-Y(j,:)*A'),diag(q1*B*B')') w(i,j+1)=w(i,j)*normpdf(g,atan(L(i,1)/L(i,2)),r1);%%w存贮的是粒子的权重，i表示粒子j表示时刻 end z=sum(w(:,j+1)); for k=1:N w(k,j+1)=w(k,j+1)/z; c1(1)=0; c1(k+1)=c1(k)+w(k,j+1)^2; %%%c是累计粒子权重的平方的变量 m=m+1; end p(j+1)=1/(c1(N+1)); %%p是判断需不需要进行重采样的变量 if p(j+1)<=N/3 [L,w(:,j+1)]=resample(L,w(:,j+1)); n=n+1 end zt(j+1,:)=w(:,j+1)'*L+Y(j+1,:);% W1(j+1,:)=(zt(j+1,:)-X(j+1,:)).^2;%%zt是滤波后得到的状态 W1是误差的平方; end monte(j+1)=monte(j+1)+(zt(j+1,1)-X(j+1,1))^2+(zt(j+1,2)-X(j+1,2))^2; end monte=(monte./100).^(1/2); w(1:10,M) p l=n t=1:1:M; subplot(2,1,1); plot(X(:,1),X(:,2),'r',Y(:,1),Y(:,2),'b');title('position '); subplot(2,1,2); %plot(zt(:,1),zt(:,2),'b',X(:,1),X(:,2),'r');title('position '); %subplot(2,2,3); plot(t,monte,'k-');title('x position error versus time step'); %subplot(2,2,4); %plot(t,W1(:,2),'r');title('y position error versus time step');