www.pudn.com > strong-tracking-filter.rar > kalmanfilter.m, change:2010-10-09,size:789b


function [x1,P1,r1,S1]=kalmanfilter(x0,P0,F,G,Q,H,R,z) 
% kalman filtering 
%  输入:x0:当前时刻状态; 
%        P0:当前时刻协方差; 
%        F:状态转移矩阵; 
%        G:过程噪声分布矩阵; 
%        Q:过程噪声V(k)(零均值高斯)的正定协方差矩阵; 
%        H:量测矩阵; 
%        R:量测噪声W(k)(零均值高斯)的正定协方差矩阵; 
%        z:量测值 
%  输出:x1:下一时刻状态(滤波); 
%        P1:下一时刻协方差(滤波); 
%        r1:新息; 
%        S1:增益 
x1_pro=F*x0;             % 状态一步预估值 
z1_pro=H*x1_pro;         % 量测的预测 
P1_pro=F*P0*F'+G*Q*G';   % 协方差一步预估值 
r1=z-z1_pro;             % 残差(新息) 
S1=H*P1_pro*H'+R;        % 新息协方差 
K1=P1_pro*H'*inv(S1);    % 增益 
x1=x1_pro+K1*r1;         % 状态更新方程 
P1=P1_pro-K1*S1*K1';     % 协方差更新方程