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

利用粒子滤波算法进行的目标跟踪代码，对学习目标跟踪的同学有所帮助

%%%%本程序中的航迹图是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（1M）;
 for  h=1:100
   X=zeros（M4）;   Y=zeros（M4）; C=zeros（M4）;%%X指的是目标真实的状态Y指观测平台的运动状态
    %%C是程序中的状态
    X（1:）=[5000800-130*sin（2*pi/9）-130*cos（2*pi/9） ];  %%目标的初始状态
    Y（1:）=[00235*sin（pi/6）-235*cos（pi/6）];%%平台运动的初始位置
    C（1:）=X（1:）-Y（1:）;w（:1）=1/N*ones（N1）;c1=zeros（N+11）;zt=zeros（M4）;zt（1:）=X（1:）;
    W1=zeros（M4）;%%zt是滤波后得到的状态
   w（:1）=random（‘Normal‘0.50.25N1）;z1=sum（w（:1））;p（1）=N; m=0;
   for j=1:M-1     %%运行的时间
       v=randn（13）;
       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+11）/C（j+12））+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（N1）*C（1:）;
                  L（i:）=normrnd（（L（i:）*A‘-（Y（j+1:）-Y（j:）*A‘））diag（q1*B*B‘）‘）;
                  w（ij）=w（i1）/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（ij+1）=w（ij）*normpdf（gatan（L（i1）/L（i2））r1）;%%w存贮的是粒子的权重，i表示粒子j表示时刻
       end
       z=sum（w（:j+1））;
       for  k=1:N
           w（kj+1）=w（kj+1）/z; 
           c1（1）=0;
           c1（k+1）=c1（k）+w（kj+1）^2; %%%c是累计粒子权重的平方的变量
           m=m+1;
       end
       p（j+1）=1/（c1（N+1））; %%p是判断需不需要进行重采样的变量
       if  p（j+1）<=N/3
           [Lw（:j+1）]=resample（Lw（: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+11）-X（j+11））^2+（zt（j+12）-X（j+12））^2;
 end
 monte=（monte./100）.^（1/2）;
 w（1:10M）
 p
 l=n
  t=1:1:M;
subplot（211）;
plot（X（:1）X（:2）‘r‘Y（:1）Y（:2）‘b‘）;title（‘position ‘）;
subplot（212）;
%plot（zt（:1）zt（:2）‘b‘X（:1）X（:2）‘r‘）;title（‘position ‘）;
%subplot（223）;
plot（tmonte‘k-‘）;title（‘x position  error versus time step‘）;
%subplot（224）;
%plot（tW1（:2）‘r‘）;title（‘y position  error versus time step‘）;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       3194  2008-07-03 21:18  bot703.m

----------- ---------  ---------- -----  ----

                 3412                    2