www.pudn.com > un_IMM_PDAF.zip > Simple_PDAF_Update.m

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;