karlman.m

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

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--前次预测矩阵
global T;
%预测
s=feval(a,s_forward);
  
%状态转换矩阵

% A=[1,0,-s(4)/2*sin(s(3)/2)*T,cos(s(3)/2)*T,0;...
%    0,1,-s(4)/2*cos(s(3)/2)*T,-sin(s(3)/2)*T,0;...
%    0,0,1,-s(5)*T/(s(4))^2,T/s(4);...
%    0,0,0,1,0;...
%    0,0,0,0,1];

A=[1,0,-s(4)*sin(s(3))*T,cos(s(3))*T,0;...
   0,1,-s(4)*cos(s(3))*T,-sin(s(3))*T,0;...
   0,0,1,-s(5)*T/(s(4))^2,T/s(4);...
   0,0,0,1,0;...
   0,0,0,0,1];

%最小预测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;