Correct tracking filter with OOSMs using JPDA-based algorithm
retroCorrectJPDA function corrects the state estimate and
covariance of a tracking filter using out-of-sequence measurements (OOSMs) based on a joint
probabilistic data association (JPDA) algorithm. To use this function, you must specify the
MaxNumOOSMSteps property of the
filter as a
positive integer. Before using this function, you must use the
function to successfully retrodict the current state of the filter to the time at which the
OOSMs were taken.
retroCorrCov] = retroCorrectJPDA(
filter using the OOSM measurements
and its corresponding joint probabilistic data association coefficients
jpdacoeffs. The function returns the corrected state and state
covariance. The function changes the values of the
StateCovariance properties of the filter object to
filter is a
object, the function also changes the
ModelProbabilities property of
trackingEKF Estimation Using
Assume a platform is moving in 3-D with a constant velocity of 10 m/s in the x-direction. Each discrete time step of the system is 1 second. The entire simulation lasts for 3 seconds.
vel = 10; % x-direction velocity dt = 1; % truePositions = [1*vel 0 0; 2*vel 0 0; 3*vel 0 0]';
The system contains two sensors that obtain 3-D position measurements of the platform. The second sensor has a bias of 0.5 meters in its x-direction measurement.
bias = 0.5;
Specify JPDA coefficients for the two sensors as 0.6 and 0.3, respectively.
jpdacoeffs = [0.6 0.3 0.1];
trackingEKF filter object for 3-D motion estimation. Specify the
MaxNumOOSMSteps property as
3 to enable retrodiction in the filter.
filter = trackingEKF(State=[0; 0; 0; 0; 0; 0], ... StateTransitionFcn=@constvel, ... MeasurementFcn=@cvmeas, ... MaxNumOOSMSteps=3)
filter = trackingEKF with properties: State: [6x1 double] StateCovariance: [6x6 double] StateTransitionFcn: @constvel StateTransitionJacobianFcn:  ProcessNoise: [6x6 double] HasAdditiveProcessNoise: 1 MeasurementFcn: @cvmeas MeasurementJacobianFcn:  HasMeasurementWrapping: 0 MeasurementNoise: 1 HasAdditiveMeasurementNoise: 1 MaxNumOOSMSteps: 3 EnableSmoothing: 0
Predict the filter and correct it with measurements. At t = 2 seconds, assume the measurements from the sensors do not arrive at the filter and thus became out-of-sequence measurements.
for j = 1:3 predict(filter,dt); meas1 = truePositions(:,1); meas2 = truePositions(:,1) + [bias 0 0]'; if j ~= 2 [x,P] = correctjpda(filter,[meas1 meas2],jpdacoeffs); else OOSMs = [meas1 meas2]; end end
Show the estimation error in the x-direction and the estimate error covariance matrix trace. The matrix trace indicates the uncertainties in the state estimate.
xError = x(1) - truePositions(1,end)
xError = -19.6654
traceCovariance = trace(P)
traceCovariance = 14.6598
Assume the OOSMs become available after t = 3 seconds. Retrodict the filter by 1 second and retro-correct the filter with the OOSMs.
retrodict(filter,-1); [xRetro,PRetro] = retroCorrectJPDA(filter,OOSMs,jpdacoeffs);
Show the covariance matrix trace after retro-correction. The reduced estimate error and covariance trace show improvement of the state estimates.
retroXError = xRetro(1) - truePositions(1,end)
retroXError = -18.4098
retroTraceCovariance = trace(PRetro)
retroTraceCovariance = 11.6873
z — Out-of-sequence measurements
Out-of-sequence measurements, specified as an M-by-N matrix, where M is the dimension of a single measurement, and N is the number of measurements.
jpdacoeffs — Joint probabilistic data association coefficients
Joint probabilistic data association coefficients, specified as an
(N+1)-element vector. The ith
(i = 1, …, N) element of
jpdacoeffs is the joint probability that the
ith measurement in
z is associated with the
filter. The last element of
jpdacoeffs is the probability that no
measurement is associated with the filter. The sum of all elements of
jpdacoeffs must equal
measparams — Measurement parameters
structure | array of structures
Measurement parameters, specified as a structure or an array of structures. The
function passes this structure to the measurement function specified by the
MeasurementFcn property of the tracking
filter. The structure can optionally contain these fields:
Enumerated type indicating the frame used to report measurements. When detections are
reported using a rectangular coordinate system, set
Position offset of the origin of the child frame relative to the parent frame, represented as a 3-by-1 vector.
Velocity offset of the origin of the child frame relative to the parent frame, represented as a 3-by-1 vector.
Frame orientation, specified as a 3-by-3 real-valued orthonormal frame rotation matrix.
The direction of the rotation depends on the
A logical scalar indicating whether
A logical scalar indicating if the measurement includes elevation. For measurements
reported in a rectangular frame, if
|A logical scalar indicating if the measurement includes azimuth.|
|A logical scalar indicating if the measurement includes range.|
A logical scalar indicating if the reported detections include velocity measurements.
For measurements reported in a rectangular frame, if
retroCorrState — State corrected by retrodiction
M-by-1 real-valued vector
State corrected by retrodiction, returned as an M-by-1 real-valued vector, where M is the size of the filter state.
retroCorrCov — State covariance corrected by retrodiction
M-by-M real-valued positive-definite
State covariance corrected by retrodiction, returned as an M-by-M real-valued positive-definite matrix.
JPDA-Based Retrodiction and Retro-Correction
Assume the current time step of the filter is k. At time k, the a posteriori state and state covariance of the filter are x(k|k) and P(k|k), respectively. Out-of-sequence measurements (OOSMs) taken at time τ now arrive at time k. Find l such that τ is a time step between these two consecutive time steps:
where l is a positive integer and l < k.
In the retrodiction step, predict the current state and state covariance at time k back to the time of the OOSM. You can obtain the retrodicted state by propagating the state transition function backward in time. For a linear state transition function, the retrodicted state is expressed as:
where F(τ,k) is the backward state transition matrix from time step k to time step τ. The retrodicted covariance is obtained as:
where Q(k,τ) is the covariance matrix for the process noise and
Here, P(k|k-l) is the a priori state covariance at time k, predicted from the covariance information at time k–l, and
In the second step, retro-correction, correct the current state and state covariance using the OOSMs with a joint probabilistic data association algorithm. First, obtain the Kalman gain matrix and the innovation matrix at time τ as:
where H(τ) is the measurement Jacobian matrix, and R(τ) is the covariance matrix for the OOSMs.
The corrected state is obtained as:
Here, m is the number of OOSMs at time step τ, βj is the corresponding JPDA coefficient for the out-of-sequence measurement zj, and:
in which h(xτ|k) is the predicted measurement using the retrodicted state xτ|k.
The corrected covariance is:
 Bar-Shalom, Y., Huimin Chen, and M. Mallick. “One-Step Solution for the Multistep out-of-Sequence-Measurement Problem in Tracking.” IEEE Transactions on Aerospace and Electronic Systems 40, no. 1 (January 2004): 27–37.
 Muntzinger, Marc M., et al. “Tracking in a Cluttered Environment with Out-of-Sequence Measurements.” 2009 IEEE International Conference on Vehicular Electronics and Safety (ICVES), IEEE, 2009, pp. 56–61.