《卡尔曼滤波原理及应用-MATLAB仿真》程序-6.2.4

《卡尔曼滤波原理及应用-MATLAB仿真》程序-6.2.4

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   交互多模Kalman滤波在目标跟踪中的应用
%   详细原理介绍及中文注释请参考：
%  《卡尔曼滤波原理及应用-MATLAB仿真》，电子工业出版社，黄小平著。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function ImmKalman
clear;
T=2;
M=50;
N=900/T;
N1=400/T;
N2=600/T;
N3=610/T;
N4=660/T;
Delta=100;
Rx=zeros(N,1);
Ry=zeros(N,1);
Zx=zeros(N,MM);  % 此处有错，请修改为书中P126一致即可
Zy=zeros(N,M);
t=2:T:400;
x0=2000+0*t';
y0=10000-15*t';
t=402:T:600;
x1=x0(N1)+0.075*((t'-400).^2)/2;
y1=y0(N1)-15*(t'-400)+0.075*((t'-400).^2)/2;
t=602:T:610;
vx=0.075*(600-400);
x2=x1(N2-N1)+vx*(t'-600);
y2=y1(N2-N1)+0*t';
t=612:T:660;
x3=x2(N3-N2)+(vx*(t'-610)-0.3*((t'-610).^2)/2);
y3=y2(N3-N2)-0.3*((t'-610).^2)/2;
t=662:T:900;
vy=-0.3*(660-610);