kalmanfilter.m

来自「自编Kalman滤波算法仿真示例程序Matlab版」· M 代码 · 共 15 行

function [Xkk,Pkk]=KalmanFilter(Xk_1k_1,Zk,Pk_1k_1,Phikk_1,Hk,Qk_1,Rk)
%标准卡尔曼滤波算法
%系统方程
% X(k)=Phi(k,k-1)X(k-1)+Gamma(k-1)W(k-1)
% Z(k)=H(x)X(k)+V(k)
%输入量--Xk_1k_1,Zk,Pk_1k_1,Phikk_1,Hk,Qk_1,Rk
%输出量--Xkk,Pkk

Xkk_1=Phikk_1*Xk_1k_1;
Pkk_1=Phikk_1*Pk_1k_1*Phikk_1'+Qk_1';

Kk=Pkk_1*Hk'*(Hk*Pkk_1*Hk'+Rk)^(-1);

Xkk=Xkk_1+Kk*(Zk-Hk*Xkk_1);
Pkk=(eye(size(Pkk_1))-Kk*Hk)*Pkk_1;