function [Xf Yf]=IMMF(Zx,Zy,sigmax,sigmay,sigma_ax,sigma_ay,T,N)
%交互多模算法(IMM)算法
%(Zx,Zy)觀測數據坐標
%sigmax,sigmay 測量誤差標準差
%sigma_ax,sigma_ay擾動噪聲標準差
%T 采樣周期
%N 觀測數據長度
%采用兩個模型，一個是非機動模型(CV),一個是機動模型(CA)

%%%%%模型一:CV模型參數設置 X(k+1)=AX(k)+GW(k),Z(k)=HX(k)+V(k)%%%%%%%%%%%%%
A=[1 T 0 0;
    0 1 0 0;
    0 0 1 T;
    0 0 0 1];
G=[T^2/2,0;
    T,0;
    0,T^2/2;
    0,T];
 H=[1 0 0 0;
     0 0 1 0];
R=[sigmax^2 0;
   0 sigmay^2];%測量誤差協方差矩陣
Q=zeros(2,2);%非機動模型下，擾動噪聲協方差矩陣假定為0

%%%%%模型二:CA模型參數設置 Xm(k+1)=TmXm(k)+GmWmk),Zm(k)=HmXm(k)+Vm(k)%%%%%%%%%%
Tm=[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];
Gm=[T^2/2 0;
    T    0;
    0  T^2/4;
    0   T/2;
    1  0;
    0   1];
Hm=[1 0 0 0 0 0;
   0 0 1 0 0 0];
Rm=[sigmax^2 0;
   0 sigmay^2];%測量誤差協方差矩陣
Qm=[sigma_ax^2 0;
    0 sigma_ay^2];%機動模型下系統擾動噪聲

%%%%%%%%%%%%%濾波參數設置%%%%%%%%%%%%%%%%%%%%%%
P=[0.95 0.05;
    0.05 0.95];%模型之間切換的轉移概率矩陣
N0=5;%先從開始到N0點進行非機動模型濾波
mu1=0.95;%模型一的概率
mu2=0.05;%模型二的概率
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%對于CV模型，參數變量設置
Xf=zeros(N,1);%濾波后X縱坐標值
Yf=zeros(N,1);%濾波后Y坐標值
X1kk=zeros(4,1);%模型一每次X的濾波值
X2kk=zeros(4,1);%模型一每次X的濾波值
X01k=zeros(4,1);%每次濾波的輸入值
Xx=zeros(4,N);%X的濾波值
Xk1k=zeros(4,1);%X的預測值
Pk1k=zeros(4,4);%預測誤差協方差矩陣
P1kk=zeros(4,4);%模型一濾波誤差協方差矩陣
P2kk=zeros(4,4);%模型二濾波誤差協方差矩陣
P01k=zeros(4,4);%總的誤差的協方差矩陣
Kk=zeros(4,2);%klaman增益
S=zeros(2,2);%濾波后位置估計(新息)的協方差矩陣
I=eye(4,4);%單位矩陣Xkk=zeros(4,1);%X的濾波值

%%%%%%%%%%%%%%%CV模型初始化%%%%%%%%%%%%%%%%%%%
Xf(1)=Zx(1);Xf(2)=Zx(2);
Yf(1)=Zy(1);Yf(2)=Zy(2);
X0=[Zx(2) (Zx(2)-Zx(1))/T Zy(2) (Zy(2)-Zy(1))/T]';%初始坐標
P0=[sigmax^2 sigmax^2/T 0 0 ;
    sigmax^2/T sigma_ax^2*T^2/4+2*sigmax^2/T^2 0 0;
    0 0 sigmay^2 sigmay^2/T;
    0 0 sigmay^2/T sigma_ay^2*T^2/4+2*sigmay^2/T^2];%誤差協方差矩陣初始值

%%%%%%%%%%%對于CA模型，參數變量設置%%%%%%%%%%%
X1kkm=zeros(6,1);%模型一X的濾波值
X2kkm=zeros(6,1);%模型一X的濾波值
X02k=zeros(6,1);%每次濾波模型二的輸入值
Xk1km=zeros(6,1);%X的預測值
Pk1km=zeros(6,6);%預測誤差協方差矩陣
P1kkm=zeros(6,6);%模型一誤差協方差矩陣
P2kkm=zeros(6,6);%模型二誤差協方差矩陣
P02k=zeros(6,6);%總的誤差協方差矩陣
Kkm=zeros(6,2);%klaman增益
Dm=zeros(2,1);%新息
Sm=zeros(2,2);%濾波后位置估計(新息)的協方差矩陣
Im=eye(6,6);%單位矩陣

%%%%%%%%%%%%%%%%濾波過程%%%%%%%%%%%%%%%%%%
X1kk=X0;
P1kk=P0;
for i=3:N0-1 %先從開始到N0點進行非機動模型濾波
     Xk1k=A*X1kk;%預測
     Pk1k=A*P1kk*A'+G*Q*G';%預測誤差協方差矩陣
     D=[Zx(i) Zy(i)]'-H*Xk1k;%新息
     S=(H*Pk1k*H'+R); %濾波后位置估計(新息)的協方差矩陣
     Kk=Pk1k*H'/S;%kalman濾波增益
     X1kk=Xk1k+Kk*D;% 濾波
     P1kk=(I-Kk*H)*Pk1k;%濾波誤差協方差矩陣
     Xf(i)=X1kk(1);
     Yf(i)=X1kk(3);
end
%%%%%%%%%%%CA模型初始化%%%%%%%%%%%%%%%%%%%%%%%%%%
X2kkm([5 6])=2/T^2*D;%加速度初始值
X2kkm([1 3])=[Zx(i-1) Zy(i-1)]';%位置估計
X2kkm([2 4])=X1kk([2 4])+T*X2kkm([5 6]);%速度估計
P2kkm(1,1)=R(1,1);%協方差初始估計
P2kkm(1,2)=2/T*R(1,1);
P2kkm(1,5)=2/T^2*R(1,1);
P2kkm(5,5)=4/T^4*(R(1,1)+P1kk(1,1)+2*T*P1kk(1,2)+T^2*P1kk(2,2));
P2kkm(2,2)=4/T^2*R(1,1)+4/T^2*P1kk(1,1)+P1kk(2,2)+4/T*P1kk(1,2);
P2kkm(2,5)=4/T^3*R(1,1)+4/T^3*P1kk(1,1)+2/T*P1kk(2,2)+4/T*P1kk(1,2);
P2kkm(3,3)=R(2,2);
P2kkm(4,4)=R(1,1);
P2kkm(6,6)=R(2,2);
for i=N0:N %從N0點，加入CA模型，進行交互多模濾波
    
    %%%%***輸入交互****%%%%%%%%%
    c1=P(1,1)*mu1+P(2,1)*mu2;
    c2=P(1,2)*mu1+P(2,2)*mu2;
    mu11=P(1,1)*mu1/c1;
    mu12=P(1,2)*mu1/c2;
    mu21=P(2,1)*mu2/c1;
    mu22=P(2,2)*mu2/c2;
    X2kk=X2kkm(1:4);
    X1kkm=[X1kk;2/T^2*D];
    X01k=X1kk*mu11+X2kk*mu21;
    X02k=X1kkm*mu12+X2kkm*mu22;
    P2kk=P2kkm(1:4,1:4);
    P1kkm(1:4,1:4)=P1kk;
    P1kkm(5,5)=S(1,1)*4/T^4;
    P1kkm(5,6)=S(1,2)*4/T^4;
    P1kkm(6,6)=S(2,2)*4/T^4;
    P1kkm(6,5)=S(2,1)*4/T^4;
    P01k=P1kk*mu11+P2kk*mu21+(X1kk-X01k)*(X1kk-X01k)'*mu11+(X2kk-X01k)*(X2kk-X01k)'*mu21;
    P02k=P1kkm*mu12+P2kkm*mu22+(X1kkm-X02k)*(X1kkm-X02k)'*mu12+(X2kkm-X02k)*(X2kkm-X02k)'*mu22;
    
    %%%%%%%%%%%%%%濾波過程%%%%%%%%%%
    %模型一濾波
    Xk1k=A*X01k;%預測
    Pk1k=A*P01k*A'+G*Q*G';%預測誤差協方差矩陣
    D=[Zx(i) Zy(i)]'-H*Xk1k;%新息
    S=(H*Pk1k*H'+R); %濾波后位置估計(新息)的協方差矩陣
    Kk=Pk1k*H'/S;%kalman濾波增益
    X1kk=Xk1k+Kk*D;% 濾波
    P1kk=(I-Kk*H)*Pk1k;%濾波誤差協方差矩陣
    pr1=exp(-1/2*D'*inv(S)*D)/((2*pi)*(det(S))^(1/2));
    %模型二濾波
    Xk1km=Tm*X2kkm;%預測
    Pk1km=Tm*P02k*Tm'+Gm*Qm*Gm';%預測誤差協方差矩陣
    Dm=[Zx(i) Zy(i)]'-Hm*Xk1km;%新息
    Sm=(Hm*Pk1km*Hm'+Rm); %濾波后位置估計(新息)的協方差矩陣
    Kkm=Pk1km*Hm'/Sm;%kalman濾波增益
    X2kkm=Xk1km+Kkm*Dm;% 濾波
    P2kkm=(Im-Kkm*Hm)*Pk1km;%濾波誤差協方差矩
    pr2=exp(-1/2*Dm'*inv(Sm)*Dm)/((2*pi)*(det(Sm))^(1/2));
    %模型概率更新
    c=pr1*c1+pr2*c2;
    mu1=(pr1*c1)/(c);
    mu2=(pr2*c2)/(c);
    
    %%%%%%%%%輸出交互%%%%%%%%%%%%%%%
    Xf(i)=X1kk(1)*mu1+X2kkm(1)*mu2;
    Yf(i)=X1kk(3)*mu1+X2kkm(3)*mu2;
end