lr_kalman.m

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

% LR_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,...
% zk,Qk_1,Rk,vmk_1,wmk,Fk_1,Gk_1,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
%     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
%     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,...
    zk,Qk_1,Rk,vmk_1,wmk,Fk_1,Gk_1,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;
   Sk    = Hk*Pkk_1*Hk' + Ik*Rk*Ik';
   LR = gausspdf(zk, zkk_1, Sk)*Pd/(1-Pd)/lambda;
   if LR < 1e-10
       LR = log(1e-10);
   else
       LR = log(LR);
   end
return;