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.

[retroCorrState,retroCorrCov] = retroCorrect(filter,z) corrects the filter with the OOSM measurement 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.

example

___ = retroCorrect(___,measparams) specifies the measurement parameters for the measurement z.

Caution

You can use this syntax only when the specified filter is a trackingEKF object.

Examples

collapse all

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

collapse all

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

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

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, Frame is set to 'rectangular'. When detections are reported in spherical coordinates, Frame is set 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 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, the measurements are reported assuming 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 the rectangular frame, if HasVelocity is false, the measurements are reported as [x y z]. If HasVelocity is true, measurements are reported 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

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

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\left(\beta |k\right)=F\left(\beta ,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(\beta |k\right)=F\left(\beta ,k\right)\left[P\left(k|k\right)+Q\left(k,\beta \right)-{P}_{xv}\left(\beta |k\right)-{P}_{xv}^{T}\left(\beta |k\right)\right]F{\left(\beta ,k\right)}^{T},$

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

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

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

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

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

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

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

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

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

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

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

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

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