# retroCorrect

Correct filter with OOSM using retrodiction

## Description

The `retroCorrect`

function corrects the state estimate and
covariance using an out-of-sequence measurement (OOSM). To use this function, 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 to the time at which the OOSM was
taken.

`[`

corrects the filter with the OOSM measurement `retroCorrState`

,`retroCorrCov`

] = retroCorrect(`filter`

,`z`

)`z`

and returns the
corrected state and state covariance. The function changes the values of
`State`

and `StateCovariance`

properties of the
filter object to `retroCorrState`

and `retroCorrCov`

,
respectively.

`___ = retroCorrect(___,`

specifies the measurement parameters for the measurement `measparams`

)`z`

.

**Caution**

You can use this syntax only when the specified `filter`

is a
`trackingEKF`

object.

## Examples

### Improve Filter Estimates Using Retrodiction

Generate a truth trajectory using the 3-D constant velocity model.

rng(2021) % For repeatable results initialState = [1; 0.4; 2; 0.3; 1; -0.2]; % [x; vx; y; vy; z; vz] dt = 1; % Time step steps = 10; sigmaQ = 0.2; % Standard deviation for process noise states = NaN(6,steps); states(:,1) = initialState; for ii = 2:steps w = sigmaQ*randn(3,1); states(:,ii) = constvel(states(:,ii-1),w,dt); end

Generate position measurements from the truths.

```
positionSelector = [1 0 0 0 0 0; 0 0 1 0 0 0; 0 0 0 0 1 0];
sigmaR = 0.2; % Standard deviation for measurement noise
positions = positionSelector*states;
measures = positions + sigmaR*randn(3,steps);
```

Show the truths and measurements in an x-y plot.

figure plot(positions(1,:),positions(2,:),"ro","DisplayName","Truths"); hold on; plot(measures(1,:),measures(2,:),"bx","DisplayName","Measures"); xlabel("x (m)") ylabel("y (m)") legend("Location","northwest")

Assume that, at the ninth step, the measurement is delayed and therefore unavailable.

delayedMeasure = measures(:,9); measures(:,9) = NaN;

Construct an extended Kalman filter (EKF) based on the constant velocity model.

estimates = NaN(6,steps); covariances = NaN(6,6,steps); estimates(:,1) = positionSelector'*measures(:,1); covariances(:,:,1) = 1*eye(6); filter = trackingEKF(@constvel,@cvmeas,... "State",estimates(:,1),... "StateCovariance",covariances(:,:,1),... "ProcessNoise",eye(6),... "MeasurementNoise",sigmaR^2*eye(3),... "MaxNumOOSMSteps",3);

Step through the EKF with the measurements.

for ii = 2:steps predict(filter); if ~any(isnan(measures(:,ii))) % Skip if unavailable correct(filter,measures(:,ii)); end estimates(:,ii) = filter.State; covariances(:,:,ii) = filter.StateCovariance; end

Show the estimated results.

plot(estimates(1,:),estimates(3,:),"gd","DisplayName","Estimates");

Retrodict to the ninth step, and correct the current estimates by using the out-of-sequence measurements at the ninth step.

[retroState,retroCov] = retrodict(filter,-1); [retroCorrState,retroCorrCov] = retroCorrect(filter,delayedMeasure);

Plot the retrodicted state for the ninth step.

plot([retroState(1);retroCorrState(1)],... [retroState(3),retroCorrState(3)],... "kd","DisplayName","Retrodicted")

You can use the determinant of the final state covariance to see the improvements made by retrodiction. A smaller covariance determinant indicates improved state estimates.

detWithoutRetrodiciton = det(covariances(:,:,end))

detWithoutRetrodiciton = 3.2694e-04

detWithRetrodiciton = det(retroCorrCov)

detWithRetrodiciton = 2.6063e-04

## Input Arguments

`filter`

— Tracking filter object

`trackingKF`

object | `trackingEKF`

object

Tracking filter object, specified as a `trackingKF`

or `trackingEKF`

object.

`z`

— Out-of-sequence measurement

*P*-by-1 real-valued vector

Out-of-sequence measurement, specified as a *P*-by-1 real-valued
vector, where *P* is the size of the measurement.

`measparams`

— Measurement parameters

structure | array of structures

Measurement parameters, specified as a structure or an array of structures. The
structure is passed into 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, |

`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` | 3-by-3 real-valued orthonormal frame rotation matrix. The direction of the rotation depends on the |

`IsParentToChild` | A logical scalar indicating whether |

`HasElevation` | A logical scalar indicating if the measurement includes elevation. For measurements
reported in a rectangular frame, if |

`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 the rectangular frame, if |

## 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-define
matrix

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

## More About

### Retrodiction and Retro-Correction

Assume the current time step of the filter is *k*. At time
*k*, the posteriori state and state covariance of the filter are
*x*(*k*|*k*) and
*P*(*k*|*k*), respectively. An
out-of-sequence measurement (OOSM) taken at time *β* now arrives at time
*k*. Find *l* such that *β* is a time
step between these two consecutive time steps:

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

where *l* is a positive integer and
*l* < *k*.

In the retrodiction step, the current state and state covariance at time *k*
are predicted 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(\beta |k)=F(\beta ,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(\beta |k)=F(\beta ,k)\left[P(k|k)+Q(k,\beta )-{P}_{xv}(\beta |k)-{P}_{xv}^{T}(\beta |k)\right]F{(\beta ,k)}^{T},$$

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

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

Here, *P*(*k*|*k-l*)
is the 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}.$$

In the second step, retro-correction, the current state and state covariance are corrected using the OOSM. The corrected state is obtained as:

$$x(k|\beta )=x(k|k)+W(k,\beta )\left[z(\beta )-z(\beta |k)\right],$$

where *z*(*β*) is the OOSM at time
*β* and
*W*(*k*,*β*), the filter gain, is
expressed as:

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

You can obtain the equivalent measurement at time *β*
based on the state estimate at the time *k*,
*z*(*β*|*k*), as

$$z(\beta |k)=H(\beta )x(\beta |k).$$

In these expressions, *R*(*β*) is the
measurement covariance matrix for the OOSM and:

$${P}_{xz}(\beta |k)=\left[P(k|k)-{P}_{xv}(\beta |k)\right]F{(\beta ,k)}^{T}H{(\beta )}^{T},$$

where *H*(*β*) is the measurement
Jacobian matrix. The corrected covariance is obtained as:

$$P(k|\beta )=P(k|k)-{P}_{xz}(\beta |k)S{(\beta )}^{-1}{P}_{xz}{(\beta |k)}^{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. https://doi.org/10.1109/TAES.2004.1292140.

## Extended Capabilities

### C/C++ Code Generation

Generate C and C++ code using MATLAB® Coder™.

In code generation, after calling the filter, you cannot change its
`MaxNumOOSMSteps`

property.

## See Also

**Introduced in R2021b**

## Open Example

You have a modified version of this example. Do you want to open this example with your edits?

## MATLAB Command

You clicked a link that corresponds to this MATLAB command:

Run the command by entering it in the MATLAB Command Window. Web browsers do not support MATLAB commands.

# Select a Web Site

Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .

You can also select a web site from the following list:

## How to Get Best Site Performance

Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.

### Americas

- América Latina (Español)
- Canada (English)
- United States (English)

### Europe

- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)

- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)