kfilter.m

姿态定位完整的matlab算法

function [Xk, Pk] = kfilter(Ft, Xk_1, Qt, Hk, Zk, Rk, Pk_1, Tkf, n)
%卡尔曼滤波
    In = eye(n);
    Fikk_1=In +Tkf*Ft +Tkf^2/2*Ft^2 +Tkf^3/6*Ft^3 +Tkf^4/24*Ft^4 +Tkf^5/120*Ft^5; 
    M1=Qt; M2=Ft*M1+(Ft*M1)'; M3=Ft*M2+(Ft*M2)'; M4=Ft*M3+(Ft*M3)'; M5=Ft*M4+(Ft*M4)';
    Qk=M1*Tkf +M2*Tkf^2/2 +M3*Tkf^3/6 +M4*Tkf^4/24 +M5*Tkf^5/120;
    
    Pkk_1=Fikk_1*Pk_1*Fikk_1'+Qk;                    %Qk
    Kk=Pkk_1*Hk'*(Hk*Pkk_1*Hk'+Rk)^-1;               %Rk
    Pk=(In-Kk*Hk)*Pkk_1;
    
    Xkk_1=Fikk_1*Xk_1;    
    Xk=Xkk_1+Kk*(Zk-Hk*Xkk_1);                       %Zk