fkalmancv.m

IMM滤波的 matlab程序

function [Xkk,Pkk1]=fkalmancv(Xk1,Pk1,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);  
    Xkk=Fkcv*Xk ;  % 一步预测
    I=eye(3,3);
    Pk=(I-Kk*H)*(1.0*Pkk1);