www.pudn.com > trackingdemos.zip > STDKALMAN.M

% STDKALMAN.M   standard discrete-time Kalman filter for the following system: 
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%         plant equation:      x(k) = F(k-1)*x(k-1) + G(k-1)*v(k-1)          % 
%%         measurment equation: z(k) = H(k)*x(k)     + I(k)*w(k)              % 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% 
% This function performs one cycle of the algorithm.   
% Note that F, G, H, and I need not be constant.   
% For example, they can be time varying and state dependent. 
% 
% function [xkk,Pkk,xkk_1,Pkk_1,Sk,Wk,zkk_1,nuk]=stdkalman(xk_1k_1,Pk_1k_1,... 
% zk,Qk_1,Rk,vmk_1,wmk,Fk_1,Gk_1,Hk,Ik) 
% 
% input parameters: 
%     xk_1k_1 ----- state estimate at time k-1 
%     Pk_1k_1 ----- covariance of the state estimate at time k-1 
%     zk      ----- measurement at time k 
%     Qk_1    ----- covariance of process noise at time k-1 
%     Rk      ----- covariance of measurement noise at time k 
%     vmk_1   ----- mean of the process noise at time k-1 
%     wmk     ----- mean of measurement noise at time k 
%     Fk_1    ----- system matrix at time k-1 
%     Gk_1    ----- process noise matrix at time k-1 
%     Hk      ----- measurement matrix at time k 
%     Ik      ----- measurement noise matrix at time k 
% output parameters: 
%     xkk     ----- state estimate at time k 
%     Pkk     ----- covariance of the state estimate at time k 
%     xkk_1   ----- state prediction of time k give k-1 
%     Pkk_1   ----- covariance of state prediction of time k given k-1 
%     Sk      ----- covariance of innovation at time k 
%     Wk      ----- filter gain at time k 
%     zkk_1   ----- measurement prediction of time k given k-1 
%     nuk     ----- innovation at time k 
% 
function [xkk,Pkk,xkk_1,Pkk_1,Sk,Wk,zkk_1,nuk]=stdkalman(xk_1k_1,Pk_1k_1,... 
    zk,Qk_1,Rk,vmk_1,wmk,Fk_1,Gk_1,Hk,Ik) 
 
%   I = eye(size(Pk_1k_1)); 
   xkk_1 = Fk_1*xk_1k_1       + Gk_1*vmk_1; 
   Pkk_1 = Fk_1*Pk_1k_1*Fk_1' + Gk_1*Qk_1*Gk_1'; 
   zkk_1 = Hk*xkk_1 + Ik*wmk; 
   nuk   = zk - zkk_1; 
   Sk    = Hk*Pkk_1*Hk' + Ik*Rk*Ik'; 
   Wk    = Pkk_1*Hk'*inv(Sk); 
   xkk   = xkk_1 + Wk*nuk; 
   Pkk   = Pkk_1 - Wk*Sk*Wk'; 
%  Pkk   = (I-Wk*Hk)*Pkk_1*(I-Wk*Hk)' + Wk*Ik*Rk*Ik'*Wk'; 
return;