www.pudn.com > trackingdemos.zip > PDAKALMAN.M

% PDAKALMAN.M   discrete-time Kalman filter with probabilistic data association 
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%         plant equation:      x(k) = F(k-1)*x(k-1) + G(k-1)*v(k-1)          % 
%%         the i-th measurment: z(k, i) = H(k)*x(k)     + I(k)*w(k)           % 
%%                   with prob: p(i+1)                                        % 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% 
% 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]=pdakalman(xk_1k_1,Pk_1k_1,... 
% zk,Rk,p,Qk_1,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      ----- measurements at time k, zk(:,i) is the i-th measurement 
%     Rk      ----- covariances of measurement noise at time k, Rk(:,:,i) is the i-th cov. matrix 
%     p       ----- association probabilities, p(i+1) is the probability that zk(:,i) originates from the target 
%                   p(1) is the probability that none of the measurements are target originated 
%     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 
%     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 
% 
function [xkk,Pkk]=pdakalman(xk_1k_1,Pk_1k_1,... 
    zk,Rk,p,Qk_1,vmk_1,wmk,Fk_1,Gk_1,Hk,Ik) 
 
   num = length(zk(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'; 
   xkk = xkk_1; 
   Pkk = p(1).*Pkk_1; 
   nu = zeros(1, length(zk(:,1))); 
   P_tilde = p(1).*xkk*xkk'; 
   for i=1:num 
       zkk_1 = Hk*xkk_1 + Ik*wmk; 
       nuk   = zk(:,i) - zkk_1; 
       Sk    = Hk*Pkk_1*Hk' + Ik*Rk(:,:,i)*Ik'; 
       Wk    = Pkk_1*Hk'*inv(Sk); 
       xkk   = xkk + p(i+1).*Wk*nuk; 
       P_tilde = P_tilde + p(i+1).*(xkk_1+Wk*nuk)*(xkk_1+Wk*nuk)'; 
       Pkk   = Pkk + p(i+1).*(Pkk_1 - Wk*Sk*Wk'); 
   end 
   Pkk = Pkk + P_tilde - xkk*xkk'; 
return;