M11-T2 — MAP Estimation and Weighted Least Squares

Learning objectives

After this lesson, you should be able to:

• Write the MAP estimate as the minimiser of a negative log-posterior: likelihood terms plus prior terms.
• Show how a Gaussian prior acts as regularisation, and predict when MAP beats maximum likelihood and when it does not.
• Formulate a filtering problem (prior, motion model, measurements) as one weighted least-squares problem over the whole path.
• State the equivalences: filter = last step of the batch MAP; smoother = the whole batch MAP.

The intuition

Maximum likelihood asks: which state makes the readings most probable? MAP asks: which state is most probable given the readings and what I knew before? By Bayes' rule the posterior is likelihood times prior, so the log-posterior is the log-likelihood plus the log-prior. In least-squares terms a Gaussian prior is just one more squared residual, $(\theta-\mu_0)^\top P_0^{-1}(\theta-\mu_0)$, pulling the estimate towards what we expected, with a force that depends on how sure we were.

The left panel quantifies the pull. When the quantity really comes from the prior, MAP wins: with one reading its error is 0.70 against 0.995 for ML, a 30% reduction; with three, 0.51 against 0.585; with ten the advantage almost disappears (0.30 against 0.31), because ten readings outweigh the prior. When the prior is wrong (the truth is three prior standard deviations away) MAP loses: 1.66 against 0.995 with one reading, and still 0.40 against 0.31 with ten. The prior is a bet: it pays when it is right and costs when it is wrong, and the more readings there are, the less either matters.

The right panel is the statement that ties the modules together. Take a robot with a constant-velocity model and position readings for 20 steps. The Kalman filter processes them one at a time and its velocity uncertainty falls from 0.90 to 0.073 m/s. The smoother, which uses every reading to estimate every step, is much better in the middle (0.039) and better at the start (0.073 at step 1, where the filter has 0.90), because readings from the future say how fast the robot was going at the beginning. At the last step the two coincide. Solved as one large least-squares problem over all 40 unknowns, the MAP path is identical to the smoother's, to the last digit printed.

The batch MAP path is the smoother. The filter is the same problem, solved one step at a time, and reporting only the newest state: its answer equals the batch answer at the last step and no other.

Definitions

Prior $p(\theta)$

What is believed before the readings. For a Gaussian prior $\mathcal{N}(\mu_0,P_0)$.

Posterior

$p(\theta\mid\mathbf{z})\propto p(\mathbf{z}\mid\theta)\,p(\theta)$.

MAP estimate

$\hat\theta_\mathrm{MAP}=\arg\max_\theta\,p(\theta\mid\mathbf{z})=\arg\min_\theta\,[-\log p(\mathbf{z}\mid\theta)-\log p(\theta)]$.

Regularisation

A term in the objective that penalises solutions far from a preferred one. A Gaussian prior is a quadratic penalty (ridge).

Batch estimation

Estimating all the states of a trajectory together from all the readings.

Smoother

The posterior of every state given all readings, past and future. For linear-Gaussian models, the RTS smoother computes it by a backward pass.

Derivation: MAP from Bayes’ rule, shrinkage, and filter = recursive MAP

MAP is least squares plus a prior term

1. $$p(\theta\mid\mathbf{z})=\frac{p(\mathbf{z}\mid\theta)\,p(\theta)}{p(\mathbf{z})}$$

   Bayes’ rule. The denominator does not depend on θ.

2. $$-\log p(\theta\mid\mathbf{z})=\tfrac12\sum_i\lVert\mathbf{r}_i\rVert^2_{R_i^{-1}}+\tfrac12\lVert\theta-\mu_0\rVert^2_{P_0^{-1}}+\text{const}$$

   With Gaussian likelihood and Gaussian prior, both negative logs are quadratic. The sum is the MAP objective.

3. $$\bigl(P_0^{-1}+J^\top R^{-1}J\bigr)\Delta=J^\top R^{-1}\mathbf{r}-P_0^{-1}(\theta-\mu_0)$$

   The Gauss–Newton step for the MAP problem. The prior adds its information $P_0^{-1}$ to the information matrix and a restoring force to the right-hand side.

Shrinkage

For a mean with a prior $\mathcal{N}(0,\sigma_0^2)$ and $n$ readings of variance $\sigma^2$, the MAP is the sample mean multiplied by $\dfrac{n/\sigma^2}{n/\sigma^2+1/\sigma_0^2}$: shrunk towards the prior mean. Its error has two parts.

1. $$\hat\mu_\mathrm{MAP}-\mu=\frac{n\sigma_0^2}{n\sigma_0^2+\sigma^2}(\bar z-\mu)-\frac{\sigma^2}{n\sigma_0^2+\sigma^2}\mu$$

   Write the estimate in terms of the true value $\mu$. The first term is the reduced noise of the sample mean, the second the bias from shrinking.

2. $$\mathbb{E}\bigl[(\hat\mu_\mathrm{MAP}-\mu)^2\bigr]=\frac{n\sigma^2\sigma_0^4+\sigma^4\mu^2}{(n\sigma_0^2+\sigma^2)^2}$$

   The mean squared error for a given true μ: variance plus bias squared.

3. $$\text{if }\mu\sim\mathcal{N}(0,\sigma_0^2):\quad \mathbb{E}[\mathrm{MSE}]=\frac{\sigma^2\sigma_0^2}{n\sigma_0^2+\sigma^2}<\frac{\sigma^2}{n}$$

   Averaged over the prior the MAP error is always below that of ML: with σ = σ₀ = 1, n = 1 it is 0.5 against 1 (rms 0.71 against 1.0: the 0.703 measured).

4. $$\text{if }\mu\text{ is far from }0:\quad \mathrm{MSE}\approx\frac{\sigma^4\mu^2}{(n\sigma_0^2+\sigma^2)^2}\ \text{dominates}$$

   The bias term grows with the square of the distance of the truth from the prior mean. A wrong prior costs more than a right one saves.

Filter = last step of the batch MAP, smoother = all of it

The MAP objective for the path is a sum of terms, each involving at most two consecutive states: a chain. The normal equations are block-tridiagonal. Minimising over all states except the last by eliminating them one at a time from the oldest (a forward pass) leaves a quadratic in $x_T$ alone, whose minimiser and curvature are the filter's mean and information at $T$: this elimination is the Kalman filter, and it needs no future data. Back-substituting from $x_T$ to $x_1$ (a backward pass) gives every other state of the batch solution: this is the RTS smoother.

1. $$\text{eliminate }x_1,\dots,x_{T-1};\Rightarrow;\min_{x_T}\tfrac12\lVert x_T-\mu_T\rVert^2_{P_T^{-1}}$$

   The marginalisation of the earlier states leaves exactly the Kalman filter posterior at T (Gaussian). Marginalising a Gaussian is the Schur complement, treated in lesson 3.

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

   The RTS backward recursion: each state is corrected by how much the next one moved relative to its prediction.

Filtering, smoothing and batch estimation are one problem at three levels of use. The filter is cheap and causal; the smoother and the batch are better because they use the future.

A worked example

Two programs.

1. ML against MAP. A quantity with prior N(0, 1) and readings with noise 1; 20 000 trials per row. Prior right: the rms error of ML and MAP is 0.995 and 0.703 for n = 1, 0.585 and 0.508 for n = 3, 0.313 and 0.299 for n = 10. Prior wrong (true value from N(3, 1)): 0.995 and 1.655 for n = 1, 0.585 and 0.910 for n = 3, 0.313 and 0.404 for n = 10.
2. Filter, smoother and batch. The constant-velocity robot of Module 7 (0.5 s steps, 20 steps, position readings with noise 0.2 m), a prior of identity covariance on (0, 0). The batch problem has 40 unknowns, assembled block by block: a term for the prior, 19 motion terms and 20 reading terms. Its solution matches the Kalman filter at the last step to 9 decimals and the RTS smoother at every step to 9 decimals; it differs from the filter at earlier steps, by up to 0.213 m in position and 0.698 m/s in velocity.
3. What the future adds. The filter's velocity at step 1 is 0.28 m/s (the truth is about 1.0) with a standard deviation of 0.898; the smoother has 0.979 with a standard deviation of 0.073. In the middle of the run the smoother's velocity standard deviation, 0.039, is nearly half the filter's 0.073.

import math
from robotprob import Rng, mean, solve, inverse, transpose, matmul, matvec, madd, mscale, kf_predict, kf_update, rts_smoother

# 1. ML against MAP for a quantity with a prior N(0, 1), from n readings with noise std 1
print('the true value is drawn from      n    rms error ML    rms error MAP')
for label, draw in (('the prior N(0, 1)  ', lambda r: r.gauss(0.0, 1.0)), ('far from it N(3, 1)', lambda r: r.gauss(3.0, 1.0))):
    for n in (1, 3, 10):
        rng = Rng(100 + n)
        e_ml, e_map = [], []
        for _ in range(20000):
            truth = draw(rng)
            z = [truth + rng.gauss(0.0, 1.0) for _ in range(n)]
            ml = sum(z) / n
            mp = sum(z) / (n + 1.0)                    # prior precision 1, data precision n: the MAP is the sum divided by (n + 1)
            e_ml.append((ml - truth) ** 2); e_map.append((mp - truth) ** 2)
        print(label.ljust(24), str(n).rjust(3), str(round(math.sqrt(mean(e_ml)), 3)).rjust(12), str(round(math.sqrt(mean(e_map)), 3)).rjust(14))

# 2. the Kalman filter, the smoother and one batch MAP problem over the whole path give the same answers
dt, sq, r_, T = 0.5, 0.05, 0.04, 20
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_]]
rng = Rng(5)
pos, vel = 0.0, 1.0
zs = []
for t in range(T):
    vel += rng.gauss(0.0, sq * math.sqrt(dt)); pos += vel * dt
    zs.append(pos + rng.gauss(0.0, math.sqrt(r_)))
x0, P0 = [0.0, 0.0], [[1.0, 0.0], [0.0, 1.0]]

xs, Ps, xp, Pp = [], [], [], []
x, P = list(x0), P0
for z in zs:
    x, P = kf_predict(x, P, F, Q); xp.append(x); Pp.append(P)
    x, P, y, S, K = kf_update(x, P, [z], H, R)
    xs.append(x); Ps.append(P)
sm_x, sm_P = rts_smoother(xs, Ps, F, Q)

# the same problem as one least squares problem over the 2T unknowns x_1 ... x_T: a term for the prior, one for every motion step, one for every reading
n = 2 * T
A = [[0.0] * n for _ in range(n)]; b = [0.0] * n
def add_block(i, j, M):                                # add the 2x2 block M at block row i, block column j
    for p in range(2):
        for c in range(2): A[2 * i + p][2 * j + c] += M[p][c]
Qinv = inverse(Q)
FT = transpose(F)
C1 = madd(Q, matmul(matmul(F, P0), FT))                # x_1 given the prior on x_0: covariance Q + F P0 F^T
C1inv = inverse(C1)
add_block(0, 0, C1inv)
m0 = matvec(F, x0)
for p, v in enumerate(matvec(C1inv, m0)): b[p] += v
for t in range(T - 1):                                 # motion terms (x_{t+1} - F x_t)^T Q^-1 (x_{t+1} - F x_t)
    add_block(t + 1, t + 1, Qinv)
    add_block(t, t, matmul(matmul(FT, Qinv), F))
    add_block(t + 1, t, mscale(matmul(Qinv, F), -1.0))
    add_block(t, t + 1, mscale(matmul(FT, Qinv), -1.0))
for t in range(T):                                     # reading terms (z - x_pos)^2 / r
    A[2 * t][2 * t] += 1.0 / r_; b[2 * t] += zs[t] / r_
sol = solve(A, b)
batch = [[sol[2 * t], sol[2 * t + 1]] for t in range(T)]

mx = lambda U, V, k: max(abs(u[k] - v[k]) for u, v in zip(U, V))
print('largest difference between the batch MAP path and the Kalman filter, positions and velocities, steps 1-20:', round(mx(batch, xs, 0), 3), round(mx(batch, xs, 1), 3))
print('last step only:            batch vs filter', round(abs(batch[-1][0] - xs[-1][0]), 9), round(abs(batch[-1][1] - xs[-1][1]), 9))
print('largest difference between the batch MAP path and the RTS smoother:', round(mx(batch, sm_x, 0), 9), round(mx(batch, sm_x, 1), 9))
print('velocity standard deviation after each step 1..20')
print(' filter  :', [round(math.sqrt(P[1][1]), 3) for P in Ps])
print(' smoother:', [round(math.sqrt(P[1][1]), 3) for P in sm_P])
print('velocity estimate at step 1 (the true velocity is about 1.0 m/s): filter', round(xs[0][1], 3), ' smoother', round(sm_x[0][1], 3))

the true value is drawn from      n    rms error ML    rms error MAP
the prior N(0, 1)          1        0.995          0.703
the prior N(0, 1)          3        0.585          0.508
the prior N(0, 1)         10        0.313          0.299
far from it N(3, 1)        1        0.995          1.655
far from it N(3, 1)        3        0.585           0.91
far from it N(3, 1)       10        0.313          0.404
largest difference between the batch MAP path and the Kalman filter, positions and velocities, steps 1-20: 0.213 0.698
last step only:            batch vs filter 0.0 0.0
largest difference between the batch MAP path and the RTS smoother: 0.0 0.0
velocity standard deviation after each step 1..20
 filter  : [0.898, 0.463, 0.265, 0.176, 0.13, 0.105, 0.091, 0.082, 0.078, 0.075, 0.074, 0.074, 0.073, 0.073, 0.073, 0.073, 0.073, 0.073, 0.073, 0.073]
 smoother: [0.073, 0.064, 0.057, 0.05, 0.046, 0.042, 0.04, 0.04, 0.039, 0.039, 0.039, 0.039, 0.04, 0.041, 0.042, 0.046, 0.051, 0.057, 0.065, 0.073]
velocity estimate at step 1 (the true velocity is about 1.0 m/s): filter 0.28  smoother 0.979

In robotics

Priors in robotics

• Initial pose: the prior on x₀ anchors the problem; without it a SLAM problem is gauge-free (lesson 4).
• Motion model: each odometry measurement is a prior-like constraint between consecutive poses.
• Map priors: a known floor plan, smoothness of a surface, gravity direction for an IMU.
• Regularisation in calibration: a weak prior on parameters that the data constrain poorly stabilises the solution.

When to use which

Choosing priors responsibly

A prior is justified when it comes from knowledge (a datasheet, a calibration, physics). A prior used just to make a problem numerically solvable should be weak, so that it does not bias the answer, and the effect of changing it should be tested, as the right-and-wrong experiment above does: if the answer moves a lot when the prior moves a little, the data are not saying much.

Cost

The batch problem has a block-tridiagonal structure and can be solved in time linear in the number of steps, not cubic, by exactly the forward and backward pass described above. A dense 40 × 40 solve as in the program is for illustration; real solvers exploit the sparsity (lesson 4).

Common misconceptions

Key takeaways

1. MAP = ML plus a prior: minimise the weighted squared residuals plus (θ − μ₀)ᵀP₀⁻¹(θ − μ₀).
2. A Gaussian prior shrinks the estimate towards the prior mean. Averaged over the prior it always helps (0.703 against 0.995 for one reading); with a wrong prior it hurts (1.655 against 0.995).
3. Filtering as a batch problem: the prior on x₀, one term per motion step and one per reading; the Gauss–Newton matrix is block-tridiagonal.
4. The Kalman filter equals the batch MAP at the last step, and the RTS smoother equals it at every step (9 decimals in the experiment).
5. The smoother is much more certain than the filter wherever there are future readings: the velocity at step 1 had a standard deviation of 0.073 instead of 0.898.

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.

• T. D. Barfoot, State Estimation for Robotics, 2nd edition, 2024 , chapter 3, which derives the Kalman filter and the RTS smoother from the batch MAP problem for exactly this model. Free to read as the author’s PDF; the book is copyrighted.
• S. Särkkä and L. Svensson, Bayesian Filtering and Smoothing, 2nd edition, 2023 , the chapters on batch estimation, Kalman filtering and the RTS smoother. Free to read as the authors’ online PDF; the book is copyrighted.
• S. Thrun, W. Burgard and D. Fox, Probabilistic Robotics, MIT Press, 2005, section 3.2.4 (the information filter) and chapter 11 (the graph SLAM view of the same objective). Paid book.
• H. E. Rauch, F. Tung and C. T. Striebel, “Maximum Likelihood Estimates of Linear Dynamic Systems”, AIAA Journal 3 (8), 1965. Paid journal article.

Knowledge check

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

What’s next

The batch view scales to problems where states are not a chain: poses linked by loop closures, landmarks seen from many poses. The next lesson describes such problems as factor graphs and shows how to eliminate variables one at a time, which is marginalisation by the Schur complement.

Related content

Nothing below is required to finish this lesson.

• Before
  M11-T1: Maximum Likelihood Estimation

  Write a likelihood function, maximise its logarithm, and derive the maximum likelihood estimates of a Gaussian mean and variance.

• Lab
  M11-L1: Occupancy Grid and Pose Graph SLAM

  The map of the lab has a doubled wall where the loop should close. Build the grid in log odds, find why it smears, and correct the trajectory with a pose graph before mapping again.

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

• SLAM M4-T2: Pose Graphs
• SLAM M4-L1: Visualize a Pose Graph
• UAV M4-T12: Pose graph optimization
• SLAM: Robust Optimization