simple_pdaf_update.m

IMM PDAF跟踪滤波的程序

function [xnew, Pnew] = Simple_PDAF_Update(F, H, Q, R, y, x, P, initial)
% Simple_PDAF_Update Do a one step update of the PDAF 
% [xnew, Pnew] = Simple_PDAF_Update(F, H, Q, R, y, x, P, initial)
%
% Given
%  x(:) =   E[ X | Y(1:t-1) ] and
%  P(:,:) = Var[ X(t-1) | Y(1:t-1) ],
% compute 
%  xnew(:) =   E[ X | Y(1:t-1) ] and
%  Pnew(:,:) = Var[ X(t) | Y(1:t) ],
% using
%  y(:,ClutPointNum)   - the observations at time t
%  F - the system matrix
%  H - the observation matrix
%  Q - the system covariance
%  R - the observation covariance
%
% If initial=true, x and V are taken as the initial conditions (and F and Q are ignored).
% If there is no observation vector, set K = zeros(ss).

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Parameters
PG = .99; % probabiliry of gating
PD = .9;  % probability of detection
gamma = 9.2; % in 2D- case 99% - equals g^2
%gamma = 9;   % n 2D- case 98.9 %
%gamma = 4;   % n 2D- case 86.5 %


if nargin < 8, initial = 0; end

[ObsDim,ClutPointNum] = size(y);

if initial
  xpred = x;
  Ppred = P;
else
  xpred = F*x;
  Ppred = F*P*F' + Q; % Vpred
end
e       = y - H*xpred(:,ones(1,ClutPointNum)); % error (innovation)
S       = H*Ppred*H' + R;
loglik  = gaussian_prob(e, zeros(ObsDim,1), S, 2); % computes only the volume

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% find validated points
validp_ind  = find(loglik < gamma);
vlen        = length(validp_ind);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% check if the number = 0
if vlen == 0,
    xnew = xpred;
    Pnew = Ppred;
    disp('No points in Validation region');
    return;
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% compute association probabilities
betta         = zeros(1,vlen+1); % the last one is zero
betta(1:vlen) = exp(-.5*loglik(validp_ind)); % a
%betta(vlen+1) = (1-PG*PD)/PD*2*vlen/gamma;
betta(vlen+1) = (1-PG*PD)/PD*2*vlen/gamma*sqrt(det(S)); % by Tracking and Data Association
%betta(vlen+1) = vlen*(1-PG*PD)/PD/(pi*gamma*sqrt(det(S))); % by Multitarget-Multisensor Tracking
betta         = betta./sum(betta);

%vlen

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% update
Sinv    = inv(S);
W       = Ppred*H'*Sinv; % Kalman gain matrix
etot    = e(:,validp_ind)*betta(1:vlen)';
xnew    = xpred + W*etot;
Pc      = (eye(size(F,1)) - W*H)*Ppred;
Pgag    = W*((e(:,validp_ind).*betta(ones(ObsDim,1),1:vlen))*e(:,validp_ind)' - etot*etot')*W';
Pnew    = betta(vlen+1)*Ppred + (1-betta(vlen+1))*Pc + Pgag;