retrodict

Retrodict filter to previous time step

Since R2021b

Syntax

[retroState,retroCov] = retrodict(filter,dt)

[___,retrodictStatus] = retrodict(___)

Description

The retrodict function performs retrodiction, predicting the state estimate and covariance backward to the time at which an out-of-sequence measurement (OOSM) was taken. To use this function, specify the MaxNumOOSMSteps property of the filter as a positive integer. After using this function, use the retroCorrect or retroCorrectJPDA function to update the current state estimates using the OOSM.

[retroState,retroCov] = retrodict(filter,dt) retrodicts the filter by time dt, and returns the retrodicted state and state covariance. The function also changes values of the State and StateCovariance properties of the filter object to retroState and retroCov, respectively. Additionally, if the filter is a trackingIMM, the function also changes the ModelProbabilities property of the filter.

[___,retrodictStatus] = retrodict(___) also returns the status of the retrodiction retrodictStatus as true for success and false for failure. The retrodiction process can fail if the length of the state history stored in the filter (specified by the MaxNumOOSMSteps property of the filter) does not cover the request time specified by the dt input.

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), ...
    "HasAdditiveProcessNoise",false, ...
    "ProcessNoise",eye(3), ...
    "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 = 
8.5281e-06

detWithRetrodiciton = det(retroCorrCov)

detWithRetrodiciton = 
7.9590e-06

Retrodict trackingIMM Filter

Consider a target moving with a constant velocity model. The initial position is at [100; 0 ;1] in meters. The velocity is [1; 1; 0] in meters per second.

rng(2022) % For repeatable results
initialPosition = [100; 0; 1];
velocity = [1; 1; 0];

Assume the measurement noise covariance matrix is

measureCovaraince = diag([1; 1; 0.1]);

Generate a measurement every second for a duration of five seconds.

measurements = NaN(3,5);
dt = 1;
for i =1:5
    measurements(:,i) = initialPosition + i*dt*velocity + sqrt(measureCovaraince)*randn(3,1);
end

Assume the measurement at the fourth second is out-of-sequence. It only becomes available after the fifth second.

oosm = measurements(:,4);
measurements(:,4) = NaN;

Create a trackingIMM filter with the true initial position using the initekfimm function. Set the maximum number of OOSM steps to five.

detection = objectDetection(0,initialPosition);
imm = initekfimm(detection);
imm.MaxNumOOSMSteps = 5;

Update the filter with the available measurements.

for i = 1:5
    predict(imm,dt);
    if ~isnan(measurements(:,i))
    correct(imm,measurements(:,i));
    end
end

Display the current state, diagonal of state covariance, and model probabilities.

disp("===============Before Retrodiction===============")

===============Before Retrodiction===============

disp("Current state:" + newline + num2str(imm.State'))

Current state:
106.7626       1.56623       6.15405      1.233862      1.000669    -0.1441939

disp("Diagonal elements of state covariance:" + newline + num2str(diag(imm.StateCovariance)'))

Diagonal elements of state covariance:
0.91884      1.1404     0.91861      1.2097     0.91569      1.1156

disp("Model probabities:" + newline + num2str(imm.ModelProbabilities'))

Model probabities:
0.51519   0.0016296     0.48318

Retrodict the filter and retrocorrect the filter with the OOSM.

[retroState, retroCov] = retrodict(imm,-1); 
retroCorrect(imm,oosm);

Display the results after retrodiction. From the results, the magnitude of state covariance is reduced after the OOSM is applied, showing that retrodiction using OOSM can improve the estimates.

disp("===============After Retrodiction===============")

===============After Retrodiction===============

disp("Current state:" + newline + num2str(imm.State'))

Current state:
106.6937      1.621093      6.124384      1.261032      1.117407    -0.2363415

disp("Diagonal elements of state covariance:" + newline + num2str(diag(imm.StateCovariance)'))

Diagonal elements of state covariance:
0.80678      1.0429     0.81196      1.0962     0.80353      1.0231

disp("Model probabities:" + newline + num2str(imm.ModelProbabilities'))

Model probabities:
0.5191  0.00034574     0.48055

Input Arguments

filter — Tracking filter object

trackingKF object | trackingEKF object

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

dt — Retrodiction time

nonpositive integer

Retrodiction time, in seconds, specified as a nonpositive integer. Specify retrodiction time as the time difference between the time at which the OOSM was taken and the current time.

Output Arguments

retroState — Retrodicted state

M-by-1 real-valued vector

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

retroCov — Retrodicted state covariance

M-by-M real-valued positive-definite matrix

Retrodicted state covariance, returned as an M-by-M real-valued positive-definite matrix.

retrodictStatus — Retrodiction status

true | false

Retrodiction status, returned as true indicating success and false indicating failure.

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 \tau <k-l+1,$$

where l is a positive integer and l < k.

Retrodiction

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(\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 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

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|\tau )=x(k|k)+W(k,\tau )\left[z(\tau )-z(\tau |k)\right],$$

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

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

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

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

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

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

where H(τ) is the measurement Jacobian matrix.

The corrected covariance is obtained as:

$$P(k|\tau )=P(k|k)-P_{xz}(\tau |k)S{(\tau )}^{-1}{P}_{xz}{(\tau |k)}^T.$$

where

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

IMM Retrodiction

For interactive multiple model (IMM) filter (trackingIMM), each member-filter is retrodicted to the time of OOSM in the same way as the process described above. Also, after obtaining the retrodicted state and measurement, each member-filter retro-corrects the current state of the filter as above.

Compared with a regular filter, an IMM filter needs to maintain the probability of each member-filter. In the retrodiction step, the probability of each model is first retrodicted using the probability transition matrix. Based on the OOSM, the filter can obtain the likelihood of each member-filter using the retrodicted state, filter probability, and measurement. Then, using the likelihood of each filter, the transition probability matrix, and the model probability at the current time, the filter obtains the updated model probability of each filter at the current time k. For more details, see [2].

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] Bar-shalom, Y. and Huimin Chen. “IMM Estimator with Out-of-Sequence Measurements.” IEEE Transactions on Aerospace and Electronic Systems, vol. 41, no. 1, Jan. 2005, pp. 90–98.

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.

Version History

Introduced in R2021b