bot703.m

目标跟踪中的粒子滤波器进行了100次Monte carlo 仿真（输出航迹图和误差曲线）

%%%%本程序中的航迹图是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');