Smoothing Algorithms Tutorial

This tutorial demonstrates state smoothing: using the entire measurement sequence, including future measurements, to improve estimates at each past time step – something a causal filter cannot do.

Topics covered:

  • Forward Kalman filtering as the baseline

  • The Rauch-Tung-Striebel (RTS) backward smoothing pass

  • Comparing filtered vs. smoothed position/velocity RMSE

  • How smoothing shrinks estimation uncertainty relative to filtering alone

System and True Trajectory

The tutorial uses a constant-velocity model with a hidden acceleration phase in the middle of the trajectory (so the filter has to “catch up” when it starts, and lags after it ends):

import numpy as np

np.random.seed(42)
dt = 0.1
n_steps = 100

F = np.array([[1, dt], [0, 1]])
H = np.array([[1, 0]])
Q = np.eye(2) * 0.01
R = np.array([[0.1]])

x_true = np.zeros((n_steps, 2))
x_true[0] = [0.0, 1.0]
for k in range(1, n_steps):
    x_true[k] = F @ x_true[k - 1]
    if 30 < k < 70:  # hidden acceleration phase
        x_true[k] += np.array([0, 0.1])

z_all = np.zeros((n_steps, 1))
for k in range(n_steps):
    z_all[k] = H @ x_true[k] + np.random.randn() * np.sqrt(R[0, 0])

Forward Kalman Filter

x_filt = np.zeros((n_steps, 2))
P_filt = np.zeros((n_steps, 2, 2))
x_filt[0], P_filt[0] = [0.0, 1.0], np.eye(2)

for k in range(1, n_steps):
    x_pred = F @ x_filt[k - 1]
    P_pred = F @ P_filt[k - 1] @ F.T + Q

    innovation = z_all[k] - H @ x_pred
    S = H @ P_pred @ H.T + R
    K = P_pred @ H.T / S[0, 0]

    x_filt[k] = x_pred + K.flatten() * innovation[0]
    P_filt[k] = (np.eye(2) - K @ H) @ P_pred

rmse_filt = np.sqrt(np.mean((x_filt - x_true) ** 2))

RTS Backward Smoother

The RTS pass walks backward from the last filtered estimate, correcting each step using the smoothed estimate one step ahead of it:

x_smooth = np.zeros_like(x_filt)
P_smooth = np.zeros_like(P_filt)
x_smooth[-1], P_smooth[-1] = x_filt[-1], P_filt[-1]

for k in range(n_steps - 2, -1, -1):
    x_pred_next = F @ x_filt[k]
    P_pred_next = F @ P_filt[k] @ F.T + Q

    A = P_filt[k] @ F.T @ np.linalg.inv(P_pred_next)

    x_smooth[k] = x_filt[k] + A @ (x_smooth[k + 1] - x_pred_next)
    P_smooth[k] = P_filt[k] + A @ (P_smooth[k + 1] - P_pred_next) @ A.T

rmse_smooth = np.sqrt(np.mean((x_smooth - x_true) ** 2))
print(f"Filter RMSE: {rmse_filt:.4f}")
print(f"RTS Smoother RMSE: {rmse_smooth:.4f}")

Because the smoother incorporates information from both directions, its uncertainty (P_smooth) is uniformly at or below the filter’s (P_filt) at every time step, and its RMSE against the true trajectory is lower – most visibly during the hidden-acceleration phase, where the causal filter has no way to anticipate the change.

Next Steps