# retroCorrectJPDA

Correct tracking filter with OOSMs using JPDA-based algorithm

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

example

````[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. NoteYou can use this syntax only when `filter` is a `trackingEKF` or `trackingIMM` object . ```

## Examples

collapse all

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: [6x1 double] StateCovariance: [6x6 double] StateTransitionFcn: @constvel StateTransitionJacobianFcn: [] ProcessNoise: [3x3 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

collapse all

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

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`

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`

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

collapse all

State corrected by retrodiction, returned as an M-by-1 real-valued vector, where M is the size of the filter state.

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

collapse all

### 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 `

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\left(\tau |k\right)=F\left(\tau ,k\right)x\left(k|k\right),$`

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

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

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

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

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}^{*}{\left(k\right)}^{-1}=P{\left(k|k-l\right)}^{-1}-P{\left(k|k-l\right)}^{-1}P\left(k|k\right)P{\left(k|k-l\right)}^{-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\left(k,\tau \right)={P}_{xz}\left(\tau |k\right){\left[H\left(\tau \right)P\left(\tau |k\right){H}^{T}\left(\tau \right)+R\left(\tau \right)\right]}^{-1}.$`

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

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

The corrected state is obtained as:

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

where δz(τ):

`$\delta z\left(\tau \right)=\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 zj, and:

`$\delta {z}_{j}={z}_{j}-h\left({x}_{\tau |k}\right),$`

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}-\left(1-{\beta }_{0}\right)W\left(k,\tau \right)S\left(\tau \right)W{\left(k,\tau \right)}^{T}+W\left(k,\tau \right)\left[\sum _{j=1}^{m}{\beta }_{j}{\stackrel{˜}{z}}_{j}{\stackrel{˜}{z}}_{j}^{T}-\delta z\left(\tau \right)\delta z{\left(\tau \right)}^{T}\right]W{\left(k,\tau \right)}^{T}$`

 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.