Correct filter with OOSM using retrodiction
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
function to successfully retrodict the current state to the time at which the OOSM was
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.
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
z— Out-of-sequence measurement
Out-of-sequence measurement, specified as a P-by-1 real-valued vector, where P is the size of the measurement.
measparams— Measurement parameters
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:
Enumerated type indicating the frame used to report measurements. When detections are
reported using a rectangular coordinate system,
Position offset of the origin of the child frame relative to the parent frame, represented as a 3-by-1 vector.
Velocity offset of the origin of the child frame relative to the parent frame, represented as a 3-by-1 vector.
3-by-3 real-valued orthonormal frame rotation matrix. The direction of the rotation depends on the
A logical scalar indicating whether
A logical scalar indicating if the measurement includes elevation. For measurements
reported in a rectangular frame, if
|A logical scalar indicating if the measurement includes azimuth.|
|A logical scalar indicating if the measurement includes range.|
A logical scalar indicating if the reported detections include velocity measurements. For measurements reported in the rectangular frame, if
retroCorrState— State corrected by retrodiction
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
State covariance corrected by retrodiction, returned as an M-by-M real-valued positive-definite matrix.
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:
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:
where F(β,k) is the backward state transition matrix from time step k to time step β. The retrodicted covariance is obtained as:
where Q(k,β) is the covariance matrix for the process noise and,
Here, P(k|k-l) is the priori state covariance at time k, predicted from the covariance information at time k–l, and
In the second step, retro-correction, the current state and state covariance are corrected using the OOSM. The corrected state is obtained as:
where z(β) is the OOSM at time β and W(k,β), the filter gain, is expressed as:
You can obtain the equivalent measurement at time β based on the state estimate at the time k, z(β|k), as
In these expressions, R(β) is the measurement covariance matrix for the OOSM and:
where H(β) is the measurement Jacobian matrix. The corrected covariance is obtained as:
 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.
In code generation, after calling the filter, you cannot change its