www.pudn.com > trackingdemos.zip > prekalman.m

% PREKALMAN.M   standard discrete-time filter prediction 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_1,Pkk_1]=prekalman(xk_1k_1,Pk_1k_1,... 
% vmk_1,Qk_1,Fk_1,Gk_1) 
% 
% input parameters: 
%     xk_1k_1 ----- state estimate at time k-1 
%     Pk_1k_1 ----- covariance of the state estimate at time k-1 
%     vmk_1   ----- mean of the process noise at time k-1 
%     Qk_1    ----- covariance of process noise at time k-1 
%     Fk_1    ----- system matrix at time k-1 
%     Gk_1    ----- process noise matrix at time k-1 
% output parameters: 
%     xkk_1   ----- state prediction of time k give k-1 
%     Pkk_1   ----- covariance of state prediction of time k given k-1 
% 
function [xkk_1,Pkk_1]=prekalman(xk_1k_1,Pk_1k_1,... 
    vmk_1,Qk_1,Fk_1,Gk_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'; 
return;