function [S,M]=Karlman(S_forward,M_forward,X,A,Q,C,H)
%卡爾曼濾波
%參數(shù)說明
%       X--觀測數(shù)據(jù)矢量
%       A--狀態(tài)矩陣
%       Q--驅(qū)動噪聲協(xié)方差
%       C--觀測噪聲協(xié)方差
%       H--觀測方程句柄
%       S--輸出數(shù)據(jù)矢量
%       S_foward--前次輸出矢量
%       M_forward--前次預(yù)測矩陣
global T;
%預(yù)測
S=feval(A,S_forward);  
%狀態(tài)轉(zhuǎn)換矩陣
% 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];
%最小預(yù)測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;