fkalmancap.m

来自「IMM滤波的 matlab程序」· M 代码 · 共 17 行

function [Xk,Pk,Pkk1,S,Kk,Xkk2,lamta]=fkalmancap(Pk1,Xk1,Fkca,Qkca,Rkca,H,Z)
% this function is extented kalman filter algorithm

    Pkk1=Fkca*Pk1*Fkca'+Qkca;

    Xkk1=Fkca*Xk1;
    S=H*Pkk1*H'+Rkca;
    Kk=Pkk1*H'*inv(S);

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