pdakalman.m

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

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