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

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