function [XXE,YYE]=immkf(XX,YY,T,D)
%概率轉(zhuǎn)移矩陣
P=[0.95 0.025 0.025
   0.025 0.95 0.025
   0.025 0.025 0.95];

%模型1參數(shù)(非機(jī)動(dòng))
q1=0;
Fai1=[1 T 0 0
      0 1 0 0
      0 0 1 T
      0 0 0 1];
G1=[T/2 0
    1  0
    0  T/2
    0  1];
H1=[1 0 0 0
   0 0 1 0];

%模型2參數(shù)(機(jī)動(dòng))
q2=0.001;
Fai2=[1 T 0 0 T*T/2 0
      0 1 0 0  T    0
      0 0 1 T  0  T*T/2
      0 0 0 1  0    T
      0 0 0 0  1    0
      0 0 0 0  0    1];                                                                                                                        
G2=[T*T/4 0
    T/2 0
    0 T*T/4
    0  T/2
    1  0
    0  1];
H2=[1 0 0 0 0 0
    0 0 1 0 0 0];

%模型3參數(shù)(機(jī)動(dòng))
q3=0.0114;
Fai3=Fai2;
G3=G2;
H3=H2;

%初始化
Z(1,:)=XX;
Z(2,:)=YY;
XE1=[XX(3),(XX(3)-XX(2))/T,YY(3),(YY(3)-YY(2))/T]';
PE1=[D  D/T   0   0
     D/T q1/4+2*D/(T*T) 0 0
     0   0    D  D/T
     0   0    D/T q1/4+2*D/(T*T)];
 
XE2=[XX(3),(XX(3)-XX(2))/T,YY(3),(YY(3)-YY(2))/T,(XX(3)+XX(1)-2*XX(2))/T*T,(YY(3)+YY(1)-2*YY(2))/T*T]';
PE2=[D  D/T   0   0    0      0
     D/T q2*T*T/16+2*D/(T*T) 0 0 q2*T/4 0
     0   0    D  D/T   0      0
     0   0  D/T q2*T*T/16+2*D/(T*T) 0  q2*T/4
     0  q2*T/4 0   0   2*q2    0
     0   0    0   q2*T/4  0 2*q2];
 
 XE3=XE2;
 PE3=[D  D/T   0 0 0 0
     D/T q3*T*T/16+2*D/(T*T) 0 0 q3*T/4 0
     0   0    D  D/T   0      0
     0   0  D/T q3*T*T/16+2*D/(T*T) 0  q3*T/4
     0  q3*T/4 0   0   2*q3    0
     0   0    0   q3*T/4  0 2*q3];
  U=[1 0 0];
  
  N=length(XX);
%循環(huán)
  for k=3:N
%輸入交互
for j=1:3
    c(j)=dot(P(:,j),U);
end
for i=1:3
    for j=1:3
     UU(i,j)=P(i,j)*U(i)/c(j);
    end
end
X01=(XE1(1:4))*UU(1,1)+(XE2(1:4))*UU(2,1)+(XE3(1:4))*UU(3,1);
XE1(5)=0;XE1(6)=0;
X02=XE1*UU(1,2)+XE2*UU(2,2)+XE3*UU(3,2);   
X03=XE1*UU(1,3)+XE2*UU(2,3)+XE3*UU(3,3);

P01=(PE1+(XE1(1:4)-X01)*(XE1(1:4)-X01)')*UU(1,1)+(PE2(1:4,1:4)+(XE2(1:4)-X01)*(XE2(1:4)-X01)')*UU(2,1)...
    +(PE3(1:4,1:4)+(XE3(1:4)-X01)*(XE3(1:4)-X01)')*UU(3,1);
for i=5:6
    for j=1:6
        PE1(i,j)=0;
    end
end
for i=5:6
    for j=1:6
        PE1(j,i)=0;
    end
end
P02=(PE1+(XE1-X02)*(XE1-X02)')*UU(1,2)+(PE2+(XE2-X02)*(XE2-X02)')*UU(2,2)...
    +(PE3+(XE3-X02)*(XE3-X02)')*UU(3,2);
P03=(PE1+(XE1-X03)*(XE1-X03)')*UU(1,3)+(PE2+(XE2-X03)*(XE2-X03)')*UU(2,3)...
    +(PE3+(XE3-X03)*(XE3-X03)')*UU(3,3);
%模型條件濾波
%模型1
%X01=X01(1:4);
XP1=Fai1*X01;
PP1=Fai1*P01*Fai1';
K1=PP1*H1'*inv(H1*PP1*H1'+D*eye(2));
XE1=XP1+(K1*(Z(:,k)-H1*(XP1)));
PE1=(eye(4)-K1*H1)*PP1;

miu1=Z(:,k)-H1*(XP1);
S1=H1*PP1*H1'+D*eye(2);
A(1)=exp(-miu1'*inv(S1)*miu1/2)/((2*pi)*sqrt(det(S1)));

%模型2
XP2=Fai2*X02;
PP2=Fai2*P02*Fai2'+G2*q2*eye(2)*G2';
K2=PP2*H2'*inv(H2*PP2*H2'+D*eye(2));
XE2=XP2+K2*(Z(:,k)-H2*XP2);
PE2=(eye(6)-K2*H2)*PP2;

miu2=Z(:,k)-H2*XP2;
S2=H2*PP2*H2'+D*eye(2);
A(2)=exp(-miu2'*inv(S2)*miu2/2)/((2*pi)*sqrt(det(S2)));

%模型3
XP3=Fai3*X03;
PP3=Fai3*P03*Fai3'+G3*q3*eye(2)*G3';
K3=PP3*H3'*inv(H3*PP3*H3'+D*eye(2));
XE3=XP3+K3*(Z(:,k)-H3*XP3);
PE3=(eye(6)-K3*H3)*PP3;

miu3=Z(:,k)-H3*XP3;
S3=H3*PP3*H3'+D*eye(2);
A(3)=exp(-miu3'*inv(S3)*miu3/2)/((2*pi)*sqrt(det(S3)));

%求歸一化常數(shù)
C=dot(A,c);

%求模型概率
for i=1:3
 U(i)=A(i)*c(i)/C;
end

%輸出交互
XXE(k)=XE1(1)*U(1)+XE2(1)*U(2)+XE3(1)*U(3);
YYE(k)=XE1(3)*U(1)+XE2(3)*U(2)+XE3(3)*U(3);
end%循環(huán)結(jié)束