ekf_mcda_update_sa.m

基于RBMCDA (Rao-Blackwellized Monte Carlo Data Association)方法的多目标追踪程序

%EKF_MCDA_UPDATE  EKF Monte Carlo Data Association Update
%
% Syntax:
%   [S,C] = EKF_MCDA_UPDATE(S,Y,H,R,IM,V,TP,CP,CD,param)
%
% In:
%   S  - 1xN cell array containing particle structures
%   Y  - Measurement as Dx1 matrix
%   H  - Derivative of h() with respect to state as matrix,
%        inline function or name of function in
%        form H(x [,P1,P2,...])
%   R  - Measurement noise covariance.
%   IM - Mean prediction (innovation) as vector,
%        inline function or name of function in
%        form h(x [,P1,P2,...]).           (optional, for default see EKF_UPDATE)
%   V  - Derivative of h() with respect to noise as matrix,
%        inline function or name of function in
%        form V(x [,P1,P2,...]).           (optional, for default see EKF_UPDATE)
%   TP - Tx1 vector of prior probabilities for measurements
%        hitting each of the targets.                 (optional, default uniform)
%   CP - Prior probability of a measurement being due
%        to clutter.                                  (optional, default zero)
%   CD - Probability density of clutter measurements,
%        which could be for example 1/V, where V is
%        the volume of clutter measurement space.     (optional, default 0.01)
%   param - Parameters of H,h, and V.
%
% Out:
%   S   - 1xN cell array containing struct arrays of updated particles
%   C   - 1xN vector of contact indices 0...T, where 0 means clutter
%
% Description:
%   Perform update step for a Monte Carlo Data Association
%   filter which uses Extended Kalman Filter as estimator
%   for the subproblem, where associations are known. Filter
%   can be used for tracking multiple objects and modeling
%   of clutter measurements.
%   
%   The model is
%
%         { c_d,                 if clutter
%     y ~ { N(y | h(x{i}), R)  , if measurement from target i
%
%   Resampling is NOT performed.
%
% See also:
%   EFF_WEIGHTS, RESAMPLE, EKF_UPDATE, KF_UPDATE

% History:
%   18.02.2003 SS  Added missing prior to weight update
%   39.01.2003 SS  Resampling removed
%    3.12.2002 SS  The first implementation
%   27.11.2002 SS  Help texts created.
%
% Copyright (C) 2002, 2003 Simo S鋜kk