fkalmancvp.m

IMM滤波的 matlab程序

function [Xk,Pk,Pkk1,S,Kk,Xkk2,lamta]=fkalmancvp(Pk1,Xk1,Fkcv,Qkcv,Rk,H,Z)
% this function is a kalman filter algorithm based on cv model       
    Pkk1=Fkcv*Pk1*Fkcv'+Qkcv;

    S=H*Pkk1*H'+Rk;
    Kk=Pkk1*H'*inv(S); 
    Xkk1=Fkcv*Xk1;

    
%V is innovation of ch model
    V=(Z-H*Xkk1);
    Xk=Xkk1+Kk*(V);  
    I=eye(3,3);
    Pk=(I-Kk*H)*(1.0*Pkk1);
    Xkk2=Fkcv*Xk;
  lamta=(((2*pi)^(-1))*(det(S))^(-1/2))*exp(-0.005*V'*inv(S)*V);