%******************************************************************
%******************離散型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