stdkalman.m

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

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