retroCorrectJPDA

Correct tracking filter with OOSMs using JPDA-based algorithm

Since R2022a

Syntax

[retroCorrState,retroCorrCov] = retroCorrectJPDA(filter,z,jpdacoeffs)

[___] = retroCorrectJPDA(___,measparams)

Description

The 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 retrodict function to successfully retrodict the current state of the filter to the time at which the OOSMs were taken.

[retroCorrState,retroCorrCov] = retroCorrectJPDA(filter,z,jpdacoeffs) corrects the filter using the OOSM measurements z 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 State and StateCovariance properties of the filter object to retroCorrState and retroCorrCov, respectively. If the filter is a trackingIMM object, the function also changes the ModelProbabilities property of the filter.

[___] = retroCorrectJPDA(___,measparams) specifies the measurement parameters for the out-of-sequence measurements z, in addition to all arguments from the previous syntax.

Note

You can use this syntax only when filter is a trackingEKF or trackingIMM object.

Examples

Correct trackingEKF Estimation Using retroCorrectJPDA

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

Initialize a 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, ...
    HasAdditiveProcessNoise=false, ...
    ProcessNoise = eye(3))

filter = 
  trackingEKF with properties:

                          State: [6×1 double]
                StateCovariance: [6×6 double]

             StateTransitionFcn: @constvel
     StateTransitionJacobianFcn: []
                   ProcessNoise: [3×3 double]
        HasAdditiveProcessNoise: 0

                 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.2582

traceCovariance = trace(P)

traceCovariance = 
14.5817

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 = 
-17.4344

retroTraceCovariance = trace(PRetro)

retroTraceCovariance = 
10.7589

Input Arguments

filter — Tracking filter object

trackingKF object | trackingEKF object | trackingIMM

Tracking filter object, specified as a trackingKF, trackingEKF, or trackingIMM object.

z — Out-of-sequence measurements

M-by-N matrix

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.

Data Types: single | double

jpdacoeffs — Joint probabilistic data association coefficients

(N+1)-element vector

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 1.

Data Types: single | double

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:

Field	Description
Frame	Enumerated type indicating the frame used to report measurements. When detections are reported using a rectangular coordinate system, set Frame to 'rectangular'. When detections are reported in spherical coordinates, set Frame to 'spherical' for the first structure.
OriginPosition	Position offset of the origin of the child frame relative to the parent frame, represented as a 3-by-1 vector.
OriginVelocity	Velocity offset of the origin of the child frame relative to the parent frame, represented as a 3-by-1 vector.
Orientation	Frame orientation, specified as a 3-by-3 real-valued orthonormal frame rotation matrix. The direction of the rotation depends on the IsParentTochild field.
IsParentToChild	A logical scalar indicating whether Orientation performs a frame rotation from the parent coordinate frame to the child coordinate frame. If false, Orientation performs a frame rotation from the child coordinate frame to the parent coordinate frame instead.
HasElevation	A logical scalar indicating if the measurement includes elevation. For measurements reported in a rectangular frame, if HasElevation is false, measurement function reports all measurements with 0 degrees of elevation.
HasAzimuth	A logical scalar indicating if the measurement includes azimuth.
HasRange	A logical scalar indicating if the measurement includes range.
HasVelocity	A logical scalar indicating if the reported detections include velocity measurements. For measurements reported in a rectangular frame, if HasVelocity is false, the measurement function reports measurements as [x y z]. If HasVelocity is true, the measurement function reports measurements as [x y z vx vy vz].

Output Arguments

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 matrix

State covariance corrected by retrodiction, returned as an M-by-M real-valued positive-definite matrix.

More About

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:

$$k-l\le \tau <k-l+1,$$

where l is a positive integer and l < k.

Retrodiction

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:

$$x(\tau |k)=F(\tau ,k)x(k|k),$$

where F(τ,k) is the backward state transition matrix from time step k to time step τ. The retrodicted covariance is obtained as:

$$P(\tau |k)=F(\tau ,k)\left[P(k|k)+Q(k,\tau )-P_{xv}(\tau |k)-P_{xv}^T(\tau |k)\right]F{(\tau ,k)}^T,$$

where Q(k,τ) is the covariance matrix for the process noise and

$$P_{xv}(\tau |k)=Q(k,\tau )-P(k|k-l)S^{*}{(k)}^{-1}Q(k,\tau ).$$

Here, P(k|k-l) is the a priori state covariance at time k, predicted from the covariance information at time k–l, and

$$S^{*}{(k)}^{-1}=P{(k|k-l)}^{-1}-P{(k|k-l)}^{-1}P(k|k)P{(k|k-l)}^{-1}.$$

Retro-Correction with JPDA

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:

$$W(k,\tau )=P_{xz}(\tau |k){\left[H(\tau )P(\tau |k)H^T(\tau )+R(\tau )\right]}^{-1}.$$

$$S(\tau )=H(\tau )P(\tau |k)H{(\tau )}^T+R(\tau )$$

where H(τ) is the measurement Jacobian matrix, and R(τ) is the covariance matrix for the OOSMs.

The corrected state is obtained as:

$$x(k|\tau )=x(\tau |k)+W(k,\tau )\delta z(\tau ),$$

where δz(τ):

$$\delta z(\tau )={\displaystyle \sum _{j=1}^m{\beta }_j\delta z_j}.$$

Here, m is the number of OOSMs at time step τ, β_j is the corresponding JPDA coefficient for the out-of-sequence measurement z_j, and:

$$\delta z_j=z_j-h(x_{\tau |k}),$$

in which h(x_τ|k) is the predicted measurement using the retrodicted state x_τ|k.

The corrected covariance is:

$$P_{k|\tau }=P_{\tau |k}-(1-{\beta }_0)W(k,\tau )S(\tau )W{(k,\tau )}^T+W(k,\tau )\left[{\displaystyle \sum _{j=1}^m{\beta }_j{\tilde{z}}_j{\tilde{z}}_j^T-\delta z(\tau )\delta z{(\tau )}^T}\right]W{(k,\tau )}^T$$

References

[1] 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.

[2] 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.

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2022a