fimm.m

IMM滤波的 matlab程序

function [Xkk,Pkk,Pkk1,S,K,Xkk2,M]=fimm(i,Rmeasure,X0,Pkk,MU,T,Z,temp)
%T is sampling interval 
% if temp==1
% Sigma0=0.001;
% Sigma1=0.0006;
% Sigma2=0.0003;
% % Sigma0=0.001;
% % Sigma1=0.0006;
% % Sigma2=0.0003;
% end
% 
% if temp==2
% Sigma0=0.001;
% Sigma1=0.0006;
% Sigma2=0.0003;
% end

if temp==1
Sigma0=0.2;
Sigma1=8;
Sigma2=10;

end

if temp==2
Sigma0=0.0001;
Sigma1=0.0006;
Sigma2=0.0003;
end

T=1;
Fkcv=[1,T,0;
      0,1,0;
      0,0,0];
  
Qkcv=[ T*T*T*T/4,T*T*T/2,0;
       T*T*T/2,T*T,0;
       0,0,0]*Sigma0*Sigma0; 
Fkca=[1,T,0.5*T*T;
      0,1,T;
      0,0,1];    
Q1kca=[T*T*T*T/4,T*T*T/2,T*T/2;
       T*T*T/2,T*T,T;
       T*T/2,T,1]*Sigma1*Sigma1;
   
Q2kca=[T*T*T*T/4,T*T*T/2,T*T/2;
       T*T*T/2,T*T,T;
       T*T/2,T,1]*Sigma2*Sigma2;
C=[1,0,0];
% X0k(:,1)=[x02+12;y02+16;-v2;0;0;0]; 
% X1k(:,1)=[x02+12;y02+16;-v2;0;0;0]; 
% X2k(:,1)=[x02+12;y02+16;-v2;0;0;0]; 

X0k(:,i-1)=X0; 
X1k(:,i-1)=X0; 
X2k(:,i-1)=X0;
P0k(:,:,1)=[1,0,0;
            0,1,0;
            0,0,1]*10000;
        
P1k(:,:,1)=[1,0,0;
            0,1,0;
            0,0,1]*10000;
        
P2k(:,:,1)=[1,0,0;
            0,1,0;
            0,0,1]*10000;
        


%     MU(1)=MU(1,i-1);
%     MU(2)=MU(2,i-1);
%     MU(3)=MU(3,i-1);
 
    P0k(:,:,i-1)=Pkk;
    P1k(:,:,i-1)=Pkk;
    P2k(:,:,i-1)=Pkk;
% the model transitional probability matrix denote H
H=[0.92,0.02,0.06;
   0.02,0.9,0.08;
   0.1,0.2,0.7];
% k=i;
% for i=(k+1):time
    % calculate the Xxk and Ppk (the state and covariance after interacting or mixing)
    for ii=1:3
        for jj=1:3
            mu(ii,jj)=(H(ii,jj)*MU(ii))/(H(1,jj)*MU(1)+H(2,jj)*MU(2)+H(3,jj)*MU(3));
        end
    end
        
    Xx0k(:,i-1)=X0k(:,i-1)*mu(1,1)+X1k(:,i-1)*mu(2,1)+X2k(:,i-1)*mu(3,1);
    Xx1k(:,i-1)=X0k(:,i-1)*mu(1,2)+X1k(:,i-1)*mu(2,2)+X2k(:,i-1)*mu(3,2);
    Xx2k(:,i-1)=X0k(:,i-1)*mu(1,3)+X1k(:,i-1)*mu(2,3)+X2k(:,i-1)*mu(3,3);
    
    Pp0k(:,:,i-1)=mu(1,1)*(P0k(:,:,i-1)+(X0k(:,i-1)-Xx0k(:,i-1))*(X0k(:,i-1)-Xx0k(:,i-1))')...
                  +mu(2,1)*(P1k(:,:,i-1)+(X1k(:,i-1)-Xx0k(:,i-1))*(X1k(:,i-1)-Xx0k(:,i-1))')...
                  +mu(3,1)*(P2k(:,:,i-1)+(X2k(:,i-1)-Xx0k(:,i-1))*(X2k(:,i-1)-Xx0k(:,i-1))');
    Pp1k(:,:,i-1)=mu(1,2)*(P0k(:,:,i-1)+(X0k(:,i-1)-Xx1k(:,i-1))*(X0k(:,i-1)-Xx1k(:,i-1))')...
                  +mu(2,2)*(P1k(:,:,i-1)+(X1k(:,i-1)-Xx1k(:,i-1))*(X1k(:,i-1)-Xx1k(:,i-1))')...
                  +mu(3,2)*(P2k(:,:,i-1)+(X2k(:,i-1)-Xx1k(:,i-1))*(X2k(:,i-1)-Xx1k(:,i-1))');
    Pp2k(:,:,i-1)=mu(1,3)*(P0k(:,:,i-1)+(X0k(:,i-1)-Xx2k(:,i-1))*(X0k(:,i-1)-Xx2k(:,i-1))')...
                  +mu(2,3)*(P1k(:,:,i-1)+(X1k(:,i-1)-Xx2k(:,i-1))*(X1k(:,i-1)-Xx2k(:,i-1))')...
                  +mu(3,3)*(P2k(:,:,i-1)+(X2k(:,i-1)-Xx2k(:,i-1))*(X2k(:,i-1)-Xx2k(:,i-1))');  
          
    Rk0=Rmeasure;          
    Pk0=Pp0k(:,:,i-1);
    Xk0=Xx0k(:,i-1);

    [Xk0,Pk0,Pkk10,S0,K0,Xkk20,lamta0]=fkalmancvp(Pk0,Xk0,Fkcv,Qkcv,Rk0,C,Z);
    X0k(:,i)=Xk0;
    P0k(:,:,i)=Pk0;
    Pkk10(:,:,i)=Pkk10;
    S0(:,i)=S0;
    K0(:,i)=K0;
    Xkk20(:,i)=Xkk20;
    lamta(1)=lamta0;
 
    Rk1=Rmeasure;
    Pk1=Pp1k(:,:,i-1);
    Xk1=Xx1k(:,i-1);
    [Xk1,Pk1,Pkk11,S1,K1,Xkk21,lamta1]=fkalmancap(Pk1,Xk1,Fkca,Q1kca,Rk1,C,Z);
    X1k(:,i)=Xk1;
    P1k(:,:,i)=Pk1;
    Pkk11(:,:,i)=Pkk11;
    S1(:,i)=S1;
    K1(:,i)=K1;
    Xkk21(:,i)=Xkk21;
    lamta(2)=lamta1;
   
    Rk2=Rmeasure;
    Pk2=Pp2k(:,:,i-1);
    Xk2=Xx2k(:,i-1);
    [Xk2,Pk2,Pkk12,S2,K2,Xkk22,lamta2]=fkalmancap(Pk2,Xk2,Fkca,Q2kca,Rk2,C,Z);
    X2k(:,i)=Xk2;
    P2k(:,:,i)=Pk2;
    Pkk12(:,:,i)=Pkk12;
    S2(:,i)=S2;
    K2(:,i)=K2;
    Xkk22(:,i)=Xkk22;
    lamta(3)=lamta2;
 
    for kk=1:3
        c(kk)=H(1,kk)*MU(1)+H(2,kk)*MU(2)+H(3,kk)*MU(3);  
    end
    
    for kk=1:3
        MU(kk)=lamta(kk)*c(kk)/(lamta(1)*c(1)+lamta(2)*c(2)+lamta(3)*c(3));
    end
%     aaa1(i)=MU(1);
%     aaa2(i)=MU(2);
%     aaa3(i)=MU(3);
  
 % state estimate
    Xkk(:,i)=X0k(:,i)*MU(1)+X1k(:,i)*MU(2)+X2k(:,i)*MU(3);
    Xkk=Xkk(:,i);
    Pkk(:,:,i)=P0k(:,:,i)*MU(1)+P1k(:,:,i)*MU(2)+P2k(:,:,i)*MU(3);
    Pkk=Pkk(:,:,i);
    Pkk1(:,:,i)=Pkk10(:,:,i)*MU(1)+Pkk11(:,:,i)*MU(2)+Pkk12(:,:,i)*MU(3);
    Pkk1= Pkk1(:,:,i);
    S(:,i)= S0(:,i)*MU(1)+ S1(:,i)*MU(2)+S2(:,i)*MU(3);
    S=S(:,i);
    K(:,i)=K0(:,i)*MU(1)+K1(:,i)*MU(2)+K2(:,i)*MU(3); 
    K=K(:,i);
    Xkk2(:,i)=Xkk20(:,i)*MU(1)+Xkk21(:,i)*MU(2)+Xkk22(:,i)*MU(3);
    Xkk2=Xkk2(:,i);
% 模型概率更新    
    MU(:,i)=[MU(1);MU(2);MU(3)];
    M=MU(:,i);