test.m

来自「多模型和概率数据关联结合后的IMMPDA算法」· M 代码 · 共 45 行

function    test=test(u,T)
%========================================================================
%首先产生仿真用运动数据(虚拟测量)
%========================================================================
%匀速运动模型阶段,运动时间为10s
%-------------------------------------------------------------------

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),50,3];                                  %匀速运动转匀加速运动初始化
Xak(:,:,1)=X0;
for i=1:100
    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
%-------------------------------------------------------------------
%给仿真用数据加噪
%-------------------------------------------------------------------
test=[];
noise_d=u;
noise=noise_d*randn(2,201);
for i=2:201
    xx(i)=x(i);
    yy(i)=y(i);
    x(i)=x(i)+noise(1,i);
    y(i)=y(i)+noise(2,i);
    test=[test [x(i),y(i),xx(i),yy(i)]'];
end