Main Content

smooth

Backward smooth state estimates of tracking filter

Since R2021a

collapse all in page

Syntax

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

Description

example

[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.

[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

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')

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
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)')

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 for object tracking, specified as one of these objects:

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

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

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

Version History

Introduced in R2021a