karlman.asv

来自「利用Kalman Filter进行机动目标跟踪的例子」· ASV 代码 · 共 36 行

function [s,M]=Karlman(s_forward,M_forward,X,a,Q,C,H)
%卡尔曼滤波
%@author:fantasy
%@date:2006.5.15
%参数说明
%       X--观测数据矢量
%       A--状态矩阵

%       Q--驱动噪声协方差
%       C--观测噪声协方差
%       h--观测方程句柄
%       s--输出数据矢量
%       s_foward--前次输出矢量
%       M--前次预测矩阵
%预测
s=feval(a,s_forward);
   
%状态转换矩阵

A=[1,0,-s(4)*sin(s(3))*T,0,0;...
   0,1,s(4)*cos(s(3))*T,0,0;...
   0,0,1,-s(5)*T/(s(4))^2,
%最小预测MSE矩阵
M=M_forward;
M=A*M*A'+Q;

%卡尔曼增益矩阵
K=M*H'*inv(C+H*M*H');

%修正
s=s+K*(X-H*s);

%最小MSE矩阵
M=M-K*H*M;