M12-T3 — Smoothing: Using Future Measurements

Learning objectives

After this lesson, you should be able to:

• Distinguish filtering, fixed-lag smoothing and fixed-interval smoothing by what data each estimate uses and by its latency.
• Compute a fixed-lag smoothed estimate from the filter output with a backward recursion over a window.
• Explain the diminishing returns of lag and choose a lag from the filter’s memory.
• State when smoothing helps most (manoeuvres, initialisation, bias estimation) and what it costs.

The intuition

Suppose a robot travelling at 1 m/s suddenly reverses at step 60. At step 61 the filter has seen one reading that says “less far than expected”. It adjusts a little, being unsure whether this is a manoeuvre or noise, and takes about six steps (3 s) to get its velocity estimate within 0.5 m/s of the truth. A person looking at the whole record afterwards would put the turn at step 60 immediately: the readings at steps 62, 63, 64 all say “going back”.

That is what a smoother does: to estimate step $t$ it also uses readings $t+1,\dots,t+L$. The left panel measures the benefit on a constant-velocity robot. With no lag (the filter) the position error is 11.7 cm and the velocity error 6.8 cm/s. One step of lag (0.5 s) gives 9.7 cm and 6.0; two steps 8.2 and 5.3; five steps (2.5 s) 6.9 cm and 4.1. After that, nothing: ten steps give 6.8 and 3.9, twenty 6.7 and 3.9. The curve flattens at about the time the filter takes to forget: readings further ahead tell the smoother nothing it does not already know about step $t$.

The right panel shows where smoothing matters most. The reversal is a surprise to the model (it assumes a nearly constant velocity). The filter's rms velocity error in the 15 steps around it is 0.84 m/s and it needs 5.9 steps to recover; a smoother with a lag of five steps has 0.45 and needs 2.5 steps, and with ten steps 0.49 and 2.0: the same readings, interpreted with hindsight.

Smoothing buys accuracy with latency. The first few steps of lag are worth a lot, more lag is worth little, and nothing is gained for real-time control, which cannot wait.

Definitions

Filtering

The estimate of $x_t$ from $z_{1:t}$: $p(x_t\mid z_{1:t})$. Causal, no delay.

Fixed-lag smoothing

$p(x_{t-L}\mid z_{1:t})$: the estimate of the state $L$ steps ago using everything up to now. A constant delay $L\Delta t$.

Fixed-interval smoothing

$p(x_t\mid z_{1:T})$ for all $t\le T$: offline, once the whole record is available.

Fixed-point smoothing

$p(x_s\mid z_{1:t})$ for a fixed $s$ as $t$ grows: refine the estimate of one moment (an initial state, a calibration) as data arrive.

Smoother gain $C_t$

$P_{t|t}F^\top\bar P_{t+1}^{-1}$: how much the later estimate corrects the earlier one.

Derivation: why the backward pass is correct, and why lag saturates

The backward step

For the linear-Gaussian model the filter gives $x_t\mid z_{1:t}\sim\mathcal{N}(\hat x_{t|t},P_{t|t})$. The next state is $x_{t+1}=Fx_t+w$. Condition on all the readings by first writing the joint of $x_t$ and $x_{t+1}$ given $z_{1:t}$.

1. $$\begin{bmatrix}x_t\\x_{t+1}\end{bmatrix}\Big|z_{1:t}\sim\mathcal{N}\!\left(\begin{bmatrix}\hat x_{t|t}\\F\hat x_{t|t}\end{bmatrix},\begin{bmatrix}P_{t|t}&P_{t|t}F^\top\\FP_{t|t}&\bar P_{t+1}\end{bmatrix}\right)$$

   The joint of the state and its successor, with $\bar P_{t+1}=FP_{t|t}F^\top+Q$.

2. $$x_t\mid x_{t+1},z_{1:t}\sim\mathcal{N}\bigl(\hat x_{t|t}+C_t(x_{t+1}-F\hat x_{t|t}),;P_{t|t}-C_t\bar P_{t+1}C_t^\top\bigr)$$

   Conditioning a joint Gaussian (Module 4), with the gain $C_t=P_{t|t}F^\top\bar P_{t+1}^{-1}$.

3. $$\text{the later readings }z_{t+1:T}\text{ depend on }x_t\text{ only through }x_{t+1}$$

   The Markov property: once $x_{t+1}$ is known, the later readings tell nothing more about $x_t$.

4. $$\hat x_{t|T}=\hat x_{t|t}+C_t(\hat x_{t+1|T}-F\hat x_{t|t}),\quad P_{t|T}=P_{t|t}+C_t(P_{t+1|T}-\bar P_{t+1})C_t^\top$$

   Average the conditional over $x_{t+1}\mid z_{1:T}\sim\mathcal{N}(\hat x_{t+1|T},P_{t+1|T})$ (the law of total expectation and variance). This is the RTS recursion, run backward from $T$.

Why lag saturates

The correction $C_t$ multiplies how much the next state moved relative to its prediction. Each backward step multiplies by $C_t$ again: the influence of a reading $L$ steps ahead on step $t$ is roughly $\prod C$, which decays geometrically. For the constant-velocity model of the example the steady-state smoother gain matrix $C$ has eigenvalues of modulus 0.81 (computed numerically from the converged filter), so the correction carried back from a reading $n$ steps ahead shrinks like $0.81^n$: 0.35 at 5 steps, 0.12 at 10 and 0.015 at 20. That is why the table flattens between lag 5 and lag 10 and nothing is left to gain at 20.

Fixed-lag in a window

For an online system the backward pass need not go to the beginning: it runs over the window of $L$ steps and starts from the filter's estimate at the current time, so the cost per step is $O(L)$. The program does exactly this, using the filtered estimates and covariances already computed.

A smoother estimate is the filter estimate plus a correction from the future, passed back through gains that decay geometrically. A lag of a few filter time constants captures nearly all the benefit.

A worked example

The constant-velocity robot of Module 7: 0.5 s steps, acceleration noise 0.05, position readings with noise 0.2 m, a filter with correct models, 120 steps.

1. Accuracy against lag (60 runs, steps 10–100). Lag 0: position 0.1173 m, velocity 0.0679 m/s, claimed velocity std 0.0733. Lag 1: 0.0966, 0.0598, 0.0648. Lag 2: 0.0824, 0.0527, 0.0571. Lag 5: 0.0686, 0.0414, 0.0424. Lag 10: 0.0679, 0.0393, 0.0387. Lag 20: 0.0667, 0.0391, 0.0386. The claimed standard deviations track the real errors: the smoother is consistent. Lag 5 captures 96% of the position gain and 92% of the velocity gain of lag 20, for 2.5 s of delay.
2. A manoeuvre. At step 60 the velocity reverses from +1 to −1 m/s, which the motion model does not expect. The rms velocity error in steps 56 to 70 is 0.836 m/s for the filter, 0.574 with lag 2, 0.453 with lag 5 and 0.485 with lag 10; the mean time until the velocity estimate is within 0.5 m/s of the truth is 5.9, 4.2, 2.5 and 2.0 steps.
3. Reading the result. Even with hindsight the estimate is not instantaneous: the smoother knows the velocity changed only after enough readings have accumulated, and the model's low process noise resists an abrupt change (a larger Q would react faster, at the price of noise in steady motion: module 7).

import math
from robotprob import Rng, mean, kf_predict, kf_update, rts_smoother

dt, sq, r_ = 0.5, 0.05, 0.04
F = [[1.0, dt], [0.0, 1.0]]; H = [[1.0, 0.0]]
q = sq * sq
Q = [[q * dt**3 / 3, q * dt**2 / 2], [q * dt**2 / 2, q * dt]]
R = [[r_]]
T = 120

def simulate(seed, jump=False):
    rng = Rng(seed)
    pos, vel = 0.0, 1.0
    truth, zs = [], []
    for t in range(T):
        if jump and t == 60: vel = -1.0                     # a manoeuvre the model does not know about: the robot reverses
        vel += rng.gauss(0.0, sq * math.sqrt(dt)); pos += vel * dt
        truth.append((pos, vel)); zs.append(pos + rng.gauss(0.0, math.sqrt(r_)))
    return truth, zs

def filter_run(zs):
    x, P = [0.0, 1.0], [[1.0, 0.0], [0.0, 1.0]]
    xs, Ps = [], []
    for z in zs:
        x, P = kf_predict(x, P, F, Q)
        x, P, y, S, K = kf_update(x, P, [z], H, R)
        xs.append(x); Ps.append(P)
    return xs, Ps

def fixed_lag(xs, Ps, t, lag):
    """The estimate of step t using the measurements up to t + lag: the RTS backward pass over that window only."""
    end = min(len(xs) - 1, t + lag)
    sm_x, sm_P = rts_smoother(xs[t:end + 1], Ps[t:end + 1], F, Q)
    return sm_x[0], sm_P[0]

print('fixed-lag smoothing of the constant-velocity robot: 60 runs, steps 10-100')
print('lag (steps)  delay (s)   rms position error   rms velocity error   claimed velocity std')
for lag in (0, 1, 2, 5, 10, 20):
    ep, ev, cl = [], [], []
    for seed in range(1, 61):
        truth, zs = simulate(seed)
        xs, Ps = filter_run(zs)
        for t in range(10, 101):
            x, P = fixed_lag(xs, Ps, t, lag)
            ep.append((x[0] - truth[t][0]) ** 2); ev.append((x[1] - truth[t][1]) ** 2); cl.append(P[1][1])
    print(str(lag).rjust(8), str(lag * dt).rjust(11), str(round(math.sqrt(mean(ep)), 4)).rjust(16), str(round(math.sqrt(mean(ev)), 4)).rjust(20), str(round(math.sqrt(mean(cl)), 4)).rjust(20))

print('a manoeuvre the model does not know about: at step 60 the velocity changes from +1 to -1 m/s')
print('lag      rms velocity error in steps 56-70    time (steps) until the velocity estimate is within 0.5 m/s of the truth')
for lag in (0, 2, 5, 10):
    errs, reach = [], []
    for seed in range(1, 61):
        truth, zs = simulate(seed, jump=True)
        xs, Ps = filter_run(zs)
        est = [fixed_lag(xs, Ps, t, lag)[0] for t in range(50, 80)]
        for t in range(56, 71): errs.append((est[t - 50][1] - truth[t][1]) ** 2)
        k = next((t for t in range(60, 80) if abs(est[t - 50][1] - truth[t][1]) < 0.5), 80)
        reach.append(k - 60)
    print(str(lag).rjust(3), str(round(math.sqrt(mean(errs)), 3)).rjust(24), str(round(mean(reach), 1)).rjust(40))

fixed-lag smoothing of the constant-velocity robot: 60 runs, steps 10-100
lag (steps)  delay (s)   rms position error   rms velocity error   claimed velocity std
       0         0.0           0.1173               0.0679               0.0733
       1         0.5           0.0966               0.0598               0.0648
       2         1.0           0.0824               0.0527               0.0571
       5         2.5           0.0686               0.0414               0.0424
      10         5.0           0.0679               0.0393               0.0387
      20        10.0           0.0667               0.0391               0.0386
a manoeuvre the model does not know about: at step 60 the velocity changes from +1 to -1 m/s
lag      rms velocity error in steps 56-70    time (steps) until the velocity estimate is within 0.5 m/s of the truth
  0                    0.836                                      5.9
  2                    0.574                                      4.2
  5                    0.453                                      2.5
 10                    0.485                                      2.0

In robotics

Where the delay is acceptable

• Mapping and reconstruction: the map is built from the smoothed trajectory (Module 11); no one needs the pose before the scan is processed.
• Calibration and system identification: estimating a fixed bias or alignment from a whole run uses the fixed-interval smoother or the batch problem.
• Offline analysis: flight-log reconstruction, sports tracking, post-processed GNSS.
• Display and logging: a trajectory shown with a one-second delay can be much smoother than a live one.

Sliding-window smoothers in robotics

Visual–inertial odometry systems optimise the last few seconds of poses, landmarks and IMU biases together (a fixed-lag smoother), marginalising the oldest states with the Schur complement as in Module 11, lesson 3. Each new image improves the estimates of the preceding poses; the control loop reads the newest pose, which is a filter-quality estimate, and the map uses the older, better ones. The SLAM course covers these windowed estimators in its back-end lessons.

Choosing the lag

• Plot the error (or the claimed standard deviation) against the lag as in the figure and stop where the curve flattens: here at about 5 steps.
• For a nearly constant-velocity system the memory is of order $1/(1-K)$ steps (the filter's gain K sets how fast it forgets); the lag should be a few times that.
• Budget the latency: if the downstream user can wait $L\Delta t$, spend it.

Smoothing and model errors

A smoother is only as good as its model. A wrong process noise gives wrong gains $C_t$ and a delayed but biased estimate; a smoother that is told the motion is nearly constant will smooth away a real manoeuvre. Check the smoothed residuals: if the smoothed trajectory is much smoother than the readings imply, the model is too stiff.

Common misconceptions

Key takeaways

1. Filtering uses readings up to t, fixed-lag smoothing up to t + L, fixed-interval smoothing the whole record.
2. The RTS backward step corrects each state by the next smoothed state’s deviation from its prediction, through the gain C = P F ᵀ P̄⁻¹; smoothed covariances are never larger than filtered ones.
3. The gain from lag decays geometrically: lag 5 (2.5 s) gave position error 0.069 against 0.117 for the filter, lag 20 only 0.067.
4. Smoothing helps most at a surprise: after a reversal the velocity error fell from 0.84 to 0.45 m/s and the recovery from 5.9 to 2.5 steps with lag 5.
5. Choose the lag where the error curve flattens, a few times the filter’s memory, and keep the filter for control.

References and further reading

Each source is marked free or paid. Free to read does not mean free to reuse: the course resources page lists every source with its licence.

• S. Särkkä and L. Svensson, Bayesian Filtering and Smoothing, 2nd edition, 2023 , the chapters on Bayesian smoothing and the RTS smoother (including fixed-lag and fixed-point forms). Free to read as the authors’ online PDF; the book is copyrighted.
• T. D. Barfoot, State Estimation for Robotics, 2nd edition, 2024 , chapter 3, which derives the smoother as the batch solution. Free to read as the author’s PDF; the book is copyrighted.
• H. E. Rauch, F. Tung and C. T. Striebel, “Maximum Likelihood Estimates of Linear Dynamic Systems”, AIAA Journal 3 (8), 1965. Paid journal article.
• B. D. O. Anderson and J. B. Moore, Optimal Filtering, Prentice-Hall, 1979, chapter 7 (smoothing, including the fixed-lag smoother). Paid book.

Knowledge check

Three questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

The last lesson of the course goes from estimating to deciding: a POMDP is the problem of choosing actions when all you have is a belief, and it closes the loop from this module to control.

Related content

Nothing below is required to finish this lesson.

• Before
  M12-T2: The Error-State Kalman Filter

  Separate a nominal state from a small error state, write the predict, update, inject and reset steps, and say why inertial navigation uses them.

Other courses on this site treat neighbouring topics from their own angle:

• UAV M3-T6: Error-State Kalman Filter
• UAV M6-T5: PX4 state estimation and EKF2
• Physical AI: Belief under uncertainty