离散型kalman滤波的算法实现函数.m

这个是用于离散的状态方程的卡尔曼滤波的算法实现函数程序

%******************************************************************
%******************离散型kalman滤波的算法实现函数**************
function s = kalmanf(s)
%** 变量s为构架数组，经过滤波算法更新后，在返回
%  s.x  系统状态变量
%  s.A  一步状态转移矩阵
%  s.B  系统噪声驱动矩阵
%  s.z  观测量
%  s.H  系统观测矩阵
%  s.P  估计方差
%  s.Q  系统噪声方差阵
%  s.R  系统观测噪声方差阵

% 设置默认值:
if ~isfield(s,'x'); s.x=nan*z; end
if ~isfield(s,'P'); s.P=nan; end
if ~isfield(s,'z'); error('Observation vector missing'); end
% if ~isfield(s,'u'); s.u=0; end
if ~isfield(s,'A'); s.A=eye(length(x)); end
if ~isfield(s,'B'); s.B=0; end
if ~isfield(s,'Q'); s.Q=zeros(length(x)); end
if ~isfield(s,'R'); error('Observation covariance missing'); end
if ~isfield(s,'H'); s.H=eye(length(x)); end

if isnan(s.x)
   % initialize state estimate from first observation
   if diff(size(s.H))
      error('Observation matrix must be square and invertible for state autointialization.');
   end
   s.x = inv(s.H)*s.z;
   s.P = inv(s.H)*s.R*inv(s.H'); 
else
   
   % 离散型卡尔曼滤波算法实现:
   
   %时间更新:
   s.x = s.A*s.x;
   s.P = s.A*s.P*s.A'+s.B*s.Q*s.B';

   % 增益更新:
   K = s.P*s.H'*inv(s.H*s.P*s.H'+s.R);

   %量测更新:
   s.x = s.x + K*(s.z-s.H*s.x);
   s.P = s.P - K*s.H*s.P;  
end