qiangjidongmubiaogenzong.m

强机动目标跟踪的基本模型仿真

 %目标跟踪算法仿真—_几维Cv模型跟踪匀速直线运动
        %部分分量初始化设置
          clear all;
          clc;
        T= 1;%采样周期
        hits=300;%采样点数
        MCNum=200;%Monte Carlo仿真次数
%模型的构造以及初始参数的设置
x0=0;v_x=100;a_x=0;%初始位置以及初始速度
y0=0;v_y=200;a_y=0;
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];%系统转移矩阵
q=[T^4/4 T^3/2;T^3/2 T^2];
Qq=0.001;%模拟加速度的过程噪声方差
O2=zeros(2,2);
Q=Qq*[q,O2;O2,q];%噪声的协方差阵
H=[1,0,0,0;0,0,1,0];%观测矩阵
R_X=20; % X方向观测误差标准差
R_Y=20; % Y方向观测误差标准差
R=[R_X^2,0;0,R_Y^2];,%观测噪声协方差矩阵
PO=[R_X^2,0,0,0;0,1,0,0;0,0,R_Y^2,0;0,0,0,1];%估计误差协方差阵初值
XO=[x0,v_x,y0,v_y]';%状态向量初值
DX_Average=zeros(1,hits); % Monte Carlo仿真的需要—位置误差计算的需要
DY_Average=zeros(1,hits);
%目标运动的真实轨迹(不叠加噪声)
for t=1:T:hits
    if(t==1)
        X(t)=x0;
        Y(t)=y0;
      else
        v_x=v_x+a_x;
          v_y=v_y+a_y;
        X(t)=X(t-1)+v_x;
        Y(t)=Y(t-1)+v_y;
      end
end
%Monte Carlo仿真的开始
for i=1:1:MCNum
    %叠加噪声的轨迹构造
    noise_x=R_X*randn(1,hits);
    Z_X=X+noise_x; %X方向加噪声
    noise_y=R_Y*randn(1,hits);
    Z_Y=Y+noise_y; %X方向加噪声
    Z=[Z_X;Z_Y];
%注意每次循环都要给XO和PO赋初值
XO=[x0,v_x,y0,v_y]';
Xk=zeros(4,hits);
PO=[R_X^2,0,0,0;0,1,0,0;0,0,R_Y^2,0;0,0,0,1];
for t=2:T:hits;
    %KF滤波方程
    Xk_predict=F*XO;
    Pk_predict=F*PO*F'+Q;%预测误差协方差阵
    S=H*Pk_predict*H'+R;%信息协方差阵
    K=Pk_predict*H'*inv(S);%增益矩阵
    Xk(:,t)=Xk_predict+K*(Z(:,t)-H*Xk_predict);%估计矩阵(最后的输出值)
    Pk=(eye(4)-K*H)*Pk_predict;%估计误差协方差阵
    PO=Pk;%估计误差协方差阵
    XO=Xk(:,t);%估计矩阵更新
end
DX=(X-Xk(1,:)).^2; %X方向的滤波误差
DX_Average=DX_Average+DX;
DY=(Y-Xk(3,:)).^2; %Y方向的滤波误差
DY_Average=DY_Average+DY;
end
DX_Average=sqrt(DX_Average/MCNum)%均方根误差(RMSE)的结果
DY_Average=sqrt(DY_Average/MCNum)
        %输出结果
        t=1:hits;
        subplot(2,1,1);
        plot(X(t),Y(t),'-*',Xk(1,t),Xk(3,t), '-');%,DXXes Average,DYYse Average,'y');
        grid on
      xlabel('X轴(米)');
      ylabel('Y轴(米)');
      title('原始轨迹及其跟踪');
      legend('原始轨迹','跟踪轨迹');
        subplot(2,1,2);
        plot(t,DX_Average(t),'-r',t,DY_Average(t),'-b');
        grid on;
        xlabel('采样次数');
        ylabel('跟踪误差(米)');
        title('X及Y方向的滤波误差');
        legend('X轴误差','Y轴误差');