karlman.m

来自「本程序是基于机动目标跟踪课题的整个算法程序」· M 代码 · 共 39 行

function [S,M]=Karlman(S_forward,M_forward,X,A,Q,C,H)
%卡尔曼滤波
%参数说明
%       X--观测数据矢量
%       A--状态矩阵
%       Q--驱动噪声协方差
%       C--观测噪声协方差
%       H--观测方程句柄
%       S--输出数据矢量
%       S_foward--前次输出矢量
%       M_forward--前次预测矩阵
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)/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];
%最小预测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;