prekalman.m

一个目标跟踪系统的MATLAB 源程序包

% 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;