www.pudn.com > IMM.rar > test.m

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