bot702hangji.m

对于仅有角度量测值的跟踪问题

%%%%本程序得到的是Beyond the kalman filter:particle filters for tracking
%%%%applications 中的 figure 6.2(图像几乎一致，只是仿真数据与书中给出的略有点变化
%%%%因为按书中数据根本得不到此图，所以做了相应改动 )

clear
T=1;N=5000;
A=[1  0  T  0
   0  1  0  T
   0  0  1  0
   0  0  0  1];q1=0.01;
B=[T^2/2   0   
    0    T^2/2
    T      0
    0      T];
H=[1  0 
   0  0];r1=1.5^2;
    %X=zeros(100,4);   Y=zeros(100,4); C=zeros(100,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(100,4);zt(1,:)=C(1,:);
    %W1=zeros(100,4);%%zt是滤波后得到的状态
   % w(:,1)=random('Normal',0.5,0.25,N,1);z1=sum(w(:,1));p(1)=N; m=0;
 for j=1:29     %%运行的时间
       v=randn(1,3);
       X(j+1,:)=X(j,:)*A'; %%目标真实的运动状态
       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     %%平台的运动状态
 end
plot(X(:,1),X(:,2),'r',Y(:,1),Y(:,2),'b');title('position ');