lr_3d_kalman.m

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

% LR_3d_KALMAN.M   calculate the log likelihood ratio using
% 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 LR = lr_dkalman(xk_1k_1,Pk_1k_1,...
% z1k,R1k,z2k,R2k,Qk_1,vmk_1,wmk,Fk_1,Gk_1,Fk,Gk,Hk,Ik,Pd,lambda)
%
% input parameters:
%     xk_1k_1 ----- state estimate at time k-1
%     Pk_1k_1 ----- covariance of the state estimate at time k-1
%     z1k     ----- measurement at time k
%     R1k     ----- covariance of measurement noise at time k
%     z2k     ----- measurement at time k+1
%     R2k     ----- covariance of measurement noise at time k+1
%     Qk_1    ----- covariance of process noise at time k-1
%     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
%     Fk      ----- system matrix at time k
%     Gk      ----- process noise matrix at time k
%     Hk      ----- measurement matrix at time k
%     Ik      ----- measurement noise matrix at time k
%     Pd      ----- probability of target detection
%     lambda  ----- spatial density of false alarm
% output parameters:
%     LR      ----- the log likelihood ratio (used as a cost for assignment) of having
%                   measurement zk as target originated vs. false alarm
%
function LR = lr_kalman(xk_1k_1,Pk_1k_1,...
    z1k,R1k,z2k,R2k,Qk_1,vmk_1,wmk,Fk_1,Gk_1,Fk,Gk,Hk,Ik,Pd,lambda)

   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   = z1k - zkk_1;
   Sk    = Hk*Pkk_1*Hk' + Ik*R1k*Ik';
   Wk    = Pkk_1*Hk'*inv(Sk);
   xk_1k_1   = xkk_1 + Wk*nuk;
   Pk_1k_1   = Pkk_1 - Wk*Sk*Wk';   
   LR1 = gausspdf(z1k, zkk_1, Sk)*Pd/(1-Pd)/lambda;
   xkk_1 = Fk*xk_1k_1       + Gk*vmk_1;
   Pkk_1 = Fk*Pk_1k_1*Fk' + Gk*Qk_1*Gk';
   zkk_1 = Hk*xkk_1 + Ik*wmk;
   Sk    = Hk*Pkk_1*Hk' + Ik*R2k*Ik';
   LR2 = gausspdf(z2k, zkk_1, Sk)*Pd/(1-Pd)/lambda;
   LR = LR1*LR2;
   if LR < 1e-10
       LR = log(1e-10);
   else
       LR = log(LR);
   end
return;