smooth

Backward smooth state estimates of tracking filter

Since R2021a

Syntax

[smoothX,smoothP] = smooth(filter)

[smoothX,smoothP] = smooth(filter,numBackSteps)

Description

[smoothX,smoothP] = smooth(filter) runs a backward recursion to obtain smoothed states and covariances at the previous steps for a tracking filter, filter. The function determines the number of backward steps based on the number of executed forward steps F and the maximum number of backward steps MB specified by the MaxNumSmoothingSteps property of the filter. If F < MB, the number of backward steps is F – 1. Otherwise, the number of backward steps is MB.

The number of forward steps is equal to the number of calls to the predict object function of the filter. The backward steps do not include the current time step of the filter.

example

[smoothX,smoothP] = smooth(filter,numBackSteps) specifies the number of backward smoothing steps numBackSteps. The value of numBackSteps must be less than or equal to the smaller of F – 1 and MB, where F is the number of executed forward steps and MB is the maximum number of backward steps specified by the MaxNumSmoothingSteps property of the filter.

Note

Use this function for tracking filters other than trackingIMM. To run backward recursion to obtain smoothed states for trackingIMM filter, see smooth.

Examples

collapse all

Fixed-Interval Smoothing for `trackingEKF`

Open Live Script

Create a truth trajectory based on a constant turn motion model and generate 2-D position measurements.

rng(2020); % For repeatable results
% Initialization
dt = 1;
simTime = 50;
tspan = 0:dt:simTime;
trueInitialState = [0; 1; 0; 1; 5]; % [x;vx;y;vy;omega]
processNoise = diag([0.5; 0.5; 0.1]); % process noise matrix
measureNoise = diag([4 4 1]); % measurement noise matrix

numSteps = length(tspan);
trueStates = NaN(5,numSteps);
trueStates(:,1) = trueInitialState;


% Propagate the constant turn model and generate the measurements with
% noise. 
for i = 2:length(tspan)
    trueStates(:,i) = constturn(trueStates(:,i-1),chol(processNoise)*randn(3,1),dt);    
end
measurements = ctmeas(trueStates) + chol(measureNoise)*randn(3,numSteps);

Plot the truth trajectory and the measurements.

figure
plot(trueStates(1,1),trueStates(3,1),'r*','HandleVisibility','off')
hold on
plot(trueStates(1,:),trueStates(3,:),'r','DisplayName','Truth')
plot(measurements(1,:),measurements(2,:),'ko','DisplayName','Measurements')
xlabel('x (m)')
ylabel('y (m)')
axis image

Create a trackingEKF filter object based on the constant turn motion model.

initialGuess = [measurements(1,1);- 1; measurements(2,1); -1; 0];
filter = trackingEKF(@constturn,@ctmeas,initialGuess, ...
    'StateCovariance', diag([1,1,1,1,10]), ...
    'StateTransitionJacobianFcn',@constturnjac, ...
    'MeasurementNoise',measureNoise, ...
    'MeasurementJacobianFcn',@ctmeasjac, ...
    'EnableSmoothing',true, ...
    'MaxNumSmoothingSteps',numSteps);

estimateStates = NaN(size(trueStates));
estimateStates(:,1) = filter.State;

Propagate the filter and update the estimated state with the measurements.

for i=2:length(tspan)
    predict(filter,dt)
    estimateStates(:,i) = correct(filter,measurements(:,i));
end

Visualize the estimated results.

plot(estimateStates(1,:),estimateStates(3,:),'b','DisplayName','Forward filtering')

Backward smooth the estimated states.

smoothStates = smooth(filter);

Visualize the smoothed trajectory.

plot(smoothStates(1,:),smoothStates(3,:),'g','DisplayName','Backward smoothing')
legend('Location','best')

Figure contains an axes object. The axes object with xlabel x (m), ylabel y (m) contains 4 objects of type line. One or more of the lines displays its values using only markers These objects represent Truth, Measurements, Forward filtering, Backward smoothing.

Obtain the estimation errors.

forwardError = abs(estimateStates - trueStates);
smoothError = abs(smoothStates - trueStates(:,1:end-1));
rmsForward = sqrt(mean(forwardError'.^2))

rmsForward = 1×5

    1.6492    1.2326    1.6138    1.1619    5.0195

rmsSmooth = sqrt(mean(smoothError'.^2))

rmsSmooth = 1×5

    0.9201    0.6587    1.2122    0.6139    2.2426

Visualize the estimation errors. From the results, the smoothing process reduces the estimation errors.

figure
subplot(2,1,1)
plot(tspan,forwardError(1,:),'b')
hold on;
plot(tspan(1:end-1),smoothError(1,:),'g')
title('Position Errors')
legend('Forward filtering','Backward smoothing')
ylabel('x error (m)')
subplot(2,1,2)
plot(tspan,forwardError(3,:),'b')
hold on
plot(tspan(1:end-1),smoothError(3,:),'g')
xlabel('time (sec)')
ylabel('y error (m)')

Figure contains 2 axes objects. Axes object 1 with title Position Errors, ylabel x error (m) contains 2 objects of type line. These objects represent Forward filtering, Backward smoothing. Axes object 2 with xlabel time (sec), ylabel y error (m) contains 2 objects of type line.

figure
subplot(3,1,1)
plot(tspan,forwardError(2,:),'b')
hold on;
plot(tspan(1:end-1),smoothError(2,:),'g')
title('Velocity Errors')
legend('Forward filtering','Backward smoothing')
ylabel('v_x error (m/s)')
subplot(3,1,2)
plot(tspan,forwardError(4,:),'b')
hold on;
plot(tspan(1:end-1),smoothError(4,:),'g')
xlabel('time (sec)')
ylabel('v_y error (m/s)')
subplot(3,1,3)
plot(tspan,forwardError(5,:),'b')
hold on;
plot(tspan(1:end-1),smoothError(5,:),'g')
xlabel('time (sec)')
ylabel('\omega-error (deg/s)')

$Figure contains 3 axes objects. Axes object 1 with title Velocity Errors, ylabel v_x error (m/s) contains 2 objects of type line. These objects represent Forward filtering, Backward smoothing. Axes object 2 with xlabel time (sec), ylabel v_y error (m/s) contains 2 objects of type line. Axes object 3 with xlabel time (sec), ylabel \omega-error (deg/s) contains 2 objects of type line.$

Fixed-Lag Smoothing of `trackingEKF`

Open Live Script

Create a truth trajectory based on a constant turn motion model and generate 2-D position measurements.

rng(2020); % For repeatable results
% Initialization
dt = 1;
simTime = 50;
tspan = 0:dt:simTime;
trueInitialState = [0; 1; 0; 1; 5]; % [x;vx;y;vy;omega]
processNoise = diag([0.5; 0.5; 0.1]); % process noise matrix
measureNoise = diag([4 4 1]); % measurement noise matrix

numSteps = length(tspan);
trueStates = NaN(5,numSteps);
trueStates(:,1) = trueInitialState;


% Propagate the constant turn model and generate the measurements with
% noise. 
for i = 2:length(tspan)
    trueStates(:,i) = constturn(trueStates(:,i-1),chol(processNoise)*randn(3,1),dt);    
end
measurements = ctmeas(trueStates) + chol(measureNoise)*randn(3,numSteps);

Plot the truth trajectory and the measurements.

figure
plot(trueStates(1,1),trueStates(3,1),'r*','HandleVisibility','off')
hold on;
plot(trueStates(1,:),trueStates(3,:),'r','DisplayName','Truth')
plot(measurements(1,:),measurements(2,:),'ko','DisplayName','Measurements')
xlabel('x (m)')
ylabel('y (m)')
axis image;

Create a trackingEKF filter object based on the constant turn motion model. Set the smoothing lag to three steps.

initialGuess = [measurements(1,1);-1;measurements(2,1);-1;0];
filter = trackingEKF(@constturn,@ctmeas,initialGuess,...
    'StateCovariance', diag([1,1,1,1,10]),...
    'StateTransitionJacobianFcn',@constturnjac,...
    'MeasurementNoise', measureNoise,...
    'MeasurementJacobianFcn',@ctmeasjac,...
    'EnableSmoothing',true,...
    'MaxNumSmoothingSteps',4); 

estimateStates = NaN(size(trueStates));
estimateStates(:,1) = filter.State;
stepLag = 3; % Smoothing lag steps
smoothStates = NaN(5,numSteps-stepLag);

Propagate the filter and update the estimated state with the measurements.

for i = 2:length(tspan)
    predict(filter,dt);
    estimateStates(:,i) = correct(filter,measurements(:,i));
    if i > 3
        smoothSegment = smooth(filter,stepLag);
        smoothStates(:,i-3) = smoothSegment(:,1);
    end
end

Visualize the forward estimated and the smoothed trajectories.

plot(estimateStates(1,:),estimateStates(3,:),'b','DisplayName','Forward filtering')
plot(smoothStates(1,:),smoothStates(3,:),'g','DisplayName','Backward smoothing')
legend('Location','best')

Obtain the estimation errors.

forwardError = abs(estimateStates - trueStates);
smoothError = abs(smoothStates - trueStates(:,1:end-stepLag));
rmsForward = sqrt(mean(forwardError'.^2))

rmsForward = 1×5

    1.6492    1.2326    1.6138    1.1619    5.0195

Visualize the estimation errors. From the results, the smoothing process reduces the estimation errors.

figure
subplot(2,1,1)
plot(tspan,forwardError(1,:),'b')
hold on
plot(tspan(1:end-stepLag),smoothError(1,:),'g')
title('Position Errors')
legend('Forward filtering','Backward smoothing')
ylabel('x error (m)')
subplot(2,1,2)
plot(tspan,forwardError(3,:),'b')
hold on
plot(tspan(1:end-stepLag),smoothError(3,:),'g')
xlabel('time (sec)')
ylabel('y error (m)')

figure()
subplot(3,1,1)
plot(tspan,forwardError(2,:),'b')
hold on;
plot(tspan(1:end-stepLag),smoothError(2,:),'g')
title('Velocity Errors')
legend('Forward filtering','Backward smoothing')
ylabel('v_x error (m/s)')
subplot(3,1,2)
plot(tspan,forwardError(4,:),'b')
hold on;
plot(tspan(1:end-stepLag),smoothError(4,:),'g')
xlabel('time (sec)')
ylabel('v_y error (m/s)')
subplot(3,1,3)
plot(tspan,forwardError(5,:),'b')
hold on;
plot(tspan(1:end-stepLag),smoothError(5,:),'g')
xlabel('time (sec)')
ylabel('\omega-error (deg/s)')

Input Arguments

collapse all

`filter` — Filter for object tracking
tracking filter object

Filter for object tracking, specified as one of these objects:

trackingABF — Alpha-beta filter
trackingKF — Linear Kalman filter
trackingEKF — Extended Kalman filter
trackingUKF — Unscented Kalman filter
trackingCKF — Cubature Kalman filter
trackingMSCEKF — Extended Kalman filter using modified spherical coordinates (MSC)

`numBackSteps` — Number of backward steps
positive integer

Number of backward steps, specified as a positive integer. The value must be less than or equal to the smaller of F – 1 and MB, where F is the number of executed forward steps and MB is the maximum number of backward steps specified by the MaxNumSmoothingSteps property of the filter.

Output Arguments

collapse all

`smoothX` — Smoothed states
N-by-K matrix

Smoothed states, returned as an N-by-K matrix. N is the state dimension and K is the number of backward steps. The first column represents the earliest state in the time interval of smoothing, which is the end state of the backward recursion. The last column represents the latest state in the time interval of smoothing, which is the state at the beginning of backward recursion.

Data Types: single | double

`smoothP` — Smoothed covariances
N-by-N-by-K array

Smoothed covariances, returned as an N-by-N-by-K array. N is the state dimension and K is the number of backward steps. Each page (an N-by-N matrix) of the array is the smoothed covariance matrix for the corresponding smoothed state in the smoothX output.

Data Types: single | double

References

[1] SÄrkkÄ, Simo. “Unscented Rauch--Tung--Striebel Smoother.” IEEE Transactions on Automatic Control, 53, no. 3 (April 2008): 845–49. https://doi.org/10.1109/TAC.2008.919531.

[2] Rauch, H. E., F. Tung, and C. T. Striebel. “Maximum Likelihood Estimates of Linear Dynamic Systems.” AIAA Journal 3, no. 8 (August 1965): 1445–50. https://doi.org/10.2514/3.3166.

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

In code generation, after cloning the filter, you cannot change its EnableSmoothing property.

Version History

Introduced in R2021a

smooth

Syntax

Description

Examples

Fixed-Interval Smoothing for trackingEKF

Fixed-Lag Smoothing of trackingEKF

Input Arguments

filter — Filter for object tracking tracking filter object

numBackSteps — Number of backward steps positive integer

Output Arguments

smoothX — Smoothed states N-by-K matrix

smoothP — Smoothed covariances N-by-N-by-K array

References

Extended Capabilities

C/C++ Code Generation Generate C and C++ code using MATLAB® Coder™.

Version History

Fixed-Interval Smoothing for `trackingEKF`

Fixed-Lag Smoothing of `trackingEKF`

`filter` — Filter for object tracking
tracking filter object

`numBackSteps` — Number of backward steps
positive integer

`smoothX` — Smoothed states
N-by-K matrix

`smoothP` — Smoothed covariances
N-by-N-by-K array

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.