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

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