%************交互多模算法*********************
%IMM算法
%Radar
%N 
%*********************************************
%function IMM(N)
N=100;
T=2;
r=10000;
R=[r 0;0 r];
A1=[1 T 0 0; 0 1 0 0;0 0 1 T;0 0 0 1];
H1=[1 0 0 0; 0 0 1 0];
G1=[T/2 0;1 0;0 T/2;0 1];
A2=[1 T 0 0 T^2/2 0; 0 1 0 0 T 0
    0 0 1 T 0 T ^2/2; 0 0 0 1 0 T
    0 0 0 0 1 0; 0 0 0 0 0 1];
H2=[1 0 0 0 0 0 ;0 0 1 0 0 0 ];
G2=[T^2/4 0; T^2/2 0 
    0 T^2/4;0 T^2/2
    1 0;0 1];
q1=0;q2=0.05;q3=0.02;
Q1=[q1 0; 0 q1];
Q2=[q2 0; 0 q2];
Q3=[q3 0; 0 q3];
P=[0.95 0.025 0.025   %模型狀態轉移矩陣
    0.025 0.95 0.025
    0.025 0.025 0.95];
U=[1 0 0]';
%模擬目標真實運動軌跡
x0=2000;y0=10000;v0=-15;
alx=0.075;aly=0.075;a2x=-0.3;a2y=-0.3;
t1=400;K1=t1/T;
t2=600;K2=(t2-t1)/T;
t3=610;K3=(t3-t2)/T;
t4=660;K4=(t4-t3)/T;
t5=1500;K5=(t5-t4)/T;
% 0----400
k1=1:K1;
x1=x0*ones(1,length(k1));
y1=y0+v0*k1*T;
%400 ----600
k2=1:K2;
x2=x1(K1)+alx*(k2*T).^2/2;
y2=y1(K1)+v0*k2*T+aly*(k2*T).^2/2;
%600------610
k3=1:K3;
x3=x2(K2)+alx*(t2-t1)*(k3*T);
y3=y2(K2)*ones(1,length(k3));
%610--------660
k4=1:K4;
x4=x3(K3)+alx*(t2-t1)*(k4*T)+alx*(k4*T).^2/2;
y4=y3(K3)+a2y*(k4*T).^2/2;
%660----1500
k5=1:K5;
x5=x4(K4)*ones(1,length(k5));
y5=y4(K4)+a2y*(t4-t3)*(k5*T);
x=[x1 x2 x3 x4 x5];
y=[y1 y2 y3 y4 y5];
K=K1+K2+K3+K4+K5;
xx=zeros(1,K);yy=zeros(1,K);
ex1=zeros(1,K);ey1=zeros(1,K);
ex2=zeros(1,K);ey2=zeros(1,K);
%
for i=1:N
    zx=randn(1,K)*100+x;
    zy=randn(1,K)*100+y;
    Z=[zx;zy];
    %
    LX=[zx(2) (zx(2)-zx(1))/T zy(2) (zy(2)-zy(1))/T]';
    LP=[r r/T 0 0
        r/T 2*r/(T*T) 0 0
        0 0 r r/T
        0 0 r/T 2*r/(T*T)];
    xx(1)=xx(1)+zx(1);yy(1)=yy(1)+zy(1);
    ex1(1)=ex1(1)+x(1)-zx(1);ex2(1)=ex2(1)+(x(1)-zx(1))^2;
    ey1(1)=ey1(1)+y(1)-zy(1);ey2(1)=ey2(1)+(y(1)-zy(1))^2;
    %
    LX1=[LX',0,0]';
    LX2=[LX',0,0]';
    LX3=[LX',0,0]';
    LP1=[LP,zeros(4,2);zeros(2,6)];
    LP2=[LP,zeros(4,2);zeros(2,6)];
    LP3=[LP,zeros(4,2);zeros(2,6)];
    for k=2:K
        xx(k)=xx(k)+LX(1);
        yy(k)=yy(k)+LX(3);
        ex1(k)=ex1(k)+x(k)-LX(1);
        ex2(k)=ex2(k)+(x(k)-LX(1))^2;
        ey1(k)=ey1(k)+y(k)-LX(3);
        ey2(k)=ey2(k)+(y(k)-LX(3))^2;
        %
        C_=P'*U;
        U=P.*(U*ones(1,3))./(ones(3,1)*C_');
        X01=LX1*U(1,1)+LX2*U(2,1)+LX3*U(3,1);
        X02=LX1*U(1,2)+LX2*U(2,2)+LX3*U(3,2);
        X03=LX1*U(1,3)+LX2*U(2,3)+LX3*U(3,3);
        P01=U(1,1)*(LP1+(LX1-X01)*(LX1-X01)')+U(2,1)*(LP2+(LX2-X01)*(LX2-X01)')+U(3,1)*(LP3+(LX3-X01)*(LX3-X01)');
        P02=U(1,2)*(LP1+(LX1-X02)*(LX1-X02)')+U(2,2)*(LP2+(LX2-X02)*(LX2-X02)')+U(3,2)*(LP3+(LX3-X02)*(LX3-X02)');
        P03=U(1,3)*(LP1+(LX1-X03)*(LX1-X03)')+U(2,3)*(LP2+(LX2-X03)*(LX2-X03)')+U(3,3)*(LP3+(LX3-X03)*(LX3-X03)');
        %
        X01=X01(1:4);
        P01=P01(1:4,1:4);
        [LX1,LP1,E1]=kfilter(A1,H1,G1,Q1,R,X01,P01,Z,k);
        LX1=[LX1',0,0]';
        LP1=[LP1,zeros(4,2);zeros(2,6)];
        [LX2,LP2,E2]=kfilter(A2,H2,G2,Q2,R,X02,P02,Z,k);
        [LX3,LP3,E3]=kfilter(A2,H2,G2,Q3,R,X03,P03,Z,k);
        E=[E1 E2 E3];
        C=E*C_;
        U=(E'.*C_)/C;
        %
        LX=LX1*U(1)+LX2*U(2)+LX3*U(3);
        LP=U(1)*(LP1+(LX1-LX)*(LX1-LX)')+U(2)*(LP2+(LX2-LX)*(LX2-LX)')+U(3)*(LP3+(LX3-LX)*(LX3-LX)');
    end
end
%
for k=1:K
    ex_(k)=ex1(k)/N;
    ex(k)=sqrt(ex2(k)/N-ex_(k)^2);
    ey_(k)=ex1(k)/N;
    ey(k)=sqrt(ey2(k)/N-ey_(k)^2);
    xx(k)=xx(k)/N;
    yy(k)=yy(k)/N;
end
     figure(1);
     plot(x,y,'K-',zx,zy,'g:',xx,yy,'r-');
     legend('真實軌跡','觀測樣本','估計軌跡');
     
     figure(2);
     plot(ex_);
     legend('X 方向平均誤差');
     figure(3);
     plot(ex);
     legend('X 方向濾波曲線的標差曲線');