交互多模型Kalman滤波在目标跟踪中的应用

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   交互多模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（N1）;
Ry=zeros（N1）;
Zx=zeros（NM）;  % 此处有错，请修改为书中P126一致即可
Zy=zeros（NM）;
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）;
x4=x3（N4-N3）+0*t‘;
y4=y3（N4-N3）+vy*（t‘-660）;
Rx=[x0;x1;x2;x3;x4];
Ry=[y0;y1;y2;y3;y4];
Mt_Est_Px=zeros（MN）;
Mt_Est_Py=zeros（MN）;

nx=randn（NM）*Delta;
ny=randn（NM）*Delta;
Zx=Rx*ones（1M）+nx;
Zy=Ry*ones（1M）+ny;
for m=1:M
    Mt_Est_Px（m1）=Zx（1m）;
    Mt_Est_Py（m1）=Zx（2m）;
    xn（1）=Zx（1m）;
    xn（2）=Zx（2m）;
    yn（1）=Zy（1m）;
    yn（2）=Zy（2m）;
    phi=[1T00;0100;001T;0001];
    h=[1000;0010];
    g=[T/20;10;0T/2;01];
    q=[Delta^20;0Delta^2];
    vx=（Zx（2）-Zx（1m））/2;
    vy=（Zy（2）-Zy（1m））/2;
    x_est=[Zx（2m）;vx;Zy（2m）;vy];
    p_est=[Delta^2Delta^2/T00;Delta^2/T2*Delta^2/（T^2）00;
        00Delta^2Delta^2/T;00Delta^2/T2*Delta^2/（T^2）];
    Mt_Est_Px（m2）=x_est（1）;
    Mt_Est_Py（m2）=x_est（3）;
    for r=3:N
        z=[Zx（rm）;Zy（rm）];
        if r<20
            x_pre=phi*x_est;
            p_pre=phi*p_est*phi‘;
            k=p_pre*h‘*inv（h*p_pre*h‘+q）;
            x_est=x_pre+k*（z-h*x_pre）;
            p_est=（eye（4）-k*h）*p_pre;
            xn（r）=x_est（1）;
            yn（r）=x_est（3）;
            Mt_Est_Px（mr）=x_est（1）;
            Mt_Est_Py（mr）=x_est（3）;
        else
            if r==20
                X_est=[x_est;0;0];
                P_est=p_est;
                P_est（66）=0;
                for i=1:3
                    Xn_est{i1}=X_est;
                    Pn_est{i1}=P_est;
                end
                u=[0.80.10.1];
            end
            [X_estP_estXn_estPn_estu]=IMM（Xn_estPn_estTzDeltau）;
            xn（r）=X_est（1）;
            yn（r）=X_est（3）;
            Mt_Est_Px（mr）=X_est（1）;
            Mt_Est_Py（mr）=X_est（3）;
        end
    end
end
err_x=zeros（N1）;
err_y=zeros（N1