test.m

来自「根据《多源信息融合》一书中的IMM多模型跟踪算法所写的仿真程序」· M 代码 · 共 46 行

function    test=test(u)
%========================================================================
%首先产生仿真用运动数据(虚拟测量)
%========================================================================
%匀速运动模型阶段,运动时间为10s
%-------------------------------------------------------------------
T=0.1;
A=[ 1, 0, T, 0; 0, 1, 0, T; 0, 0, 1, 0; 0, 0, 0, 1];     %匀速运动的状态转移矩阵
X0=[10 10 10 20];                                         %初始化    
Xvk(:,:,1)=X0;
for i=1:100
    Xvk(:,:,i+1)=A*Xvk(:,:,i)';
end
for i=1:101
    x(i)=Xvk(1,1,i);
    y(i)=Xvk(1,2,i);
end
%-------------------------------------------------------------------
%匀加速运动模型阶段，运动时间为7s
%-------------------------------------------------------------------
A=[ 1, 0, T, 0, T*T/2, 0; 0, 1, 0, T, 0, T*T/2; 0, 0, 1, 0, T, 0; 0, 0, 0, 1, 0 ,T;0, 0, 0, 0, 1, 0;0, 0, 0, 0, 0, 1];
                                                        %匀加速运动的状态转移矩阵
X0=[Xvk(:,:,100),5,3];                                  %匀速运动转匀加速运动初始化
Xak(:,:,1)=X0;
for i=1:70
    Xak(:,:,i+1)=A*Xak(:,:,i)';
    x(100+i+1)=Xak(1,1,i+1);
    y(100+i+1)=Xak(1,2,i+1);
end
%-------------------------------------------------------------------
%给仿真用数据加噪
%-------------------------------------------------------------------
noise_d=u;
noise=noise_d*randn(2,171);
for i=2:171
    xx(i)=x(i);
    yy(i)=y(i);
    x(i)=x(i)+noise(1,i);
    y(i)=y(i)+noise(2,i);
    test(:,:,i)=[x(i),y(i),xx(i),yy(i)];
end
for i=1:171
    d(i)=sqrt(x(i)*x(i)+y(i)*y(i));
end