MAP estimation and nonlinear least squares

"You asked me for the most probable trajectory. I quietly turned that into a pile of squared residuals, because that is the only shape of problem I actually know how to minimize."

A Pragmatic AI Agent

This section assumes you are comfortable with priors, likelihoods, and posteriors from Chapter 4, with the measurement-model and sensor-noise language of Chapter 2, and with the factor-graph vocabulary of Section 11.1. Unlike the recursive filters of Chapter 9, which touch one state at a time, MAP estimation optimizes the whole trajectory jointly.

From posterior to a sum of squares

Collect every state into one stacked vector \(X = \{x_1, \dots, x_n\}\) and write \(Z\) for all measurements. The factor graph says the posterior factors into a product over the graph's factors,

where each \(\phi_i\) touches only its local subset of variables \(X_i\). The MAP estimate is the maximizer, \(X^\star = \arg\max_X \prod_i \phi_i(X_i)\). Products are awkward to optimize, so we take the negative logarithm, which turns the product into a sum and the maximization into a minimization. When each factor is Gaussian, \(\phi_i(X_i) \propto \exp\!\big(-\tfrac12 \lVert h_i(X_i) - z_i \rVert^2_{\Sigma_i}\big)\), the negative log of each factor is precisely a squared, covariance-weighted error term. The whole problem becomes

Here \(h_i\) is the measurement function (what the sensor would read if the state were \(X_i\)), \(z_i\) is what it actually read, and the residual \(e_i = h_i(X_i) - z_i\) is the disagreement. The norm \(\lVert \cdot \rVert_\Sigma\) is the Mahalanobis distance: it divides each residual by its uncertainty, so a millimeter of disagreement from a precise lidar counts far more than a meter from a noisy GNSS fix. This is where sensor noise models from Chapter 2 earn their keep: \(\Sigma_i\) is the currency in which competing sensors bid for influence.

Linearize, solve, repeat: Gauss-Newton

The catch is that \(h_i\) is almost never linear. A range sensor involves a square root; a camera projection divides by depth; a rotation is trigonometric. Stack all residuals into one vector \(r(X)\), weighted so the objective is simply \(\tfrac12 \lVert r(X) \rVert^2\). We cannot minimize this in closed form, but we can linearize it around the current estimate \(X\). A first-order Taylor expansion replaces the residual with \(r(X + \Delta) \approx r(X) + J\,\Delta\), where \(J = \partial r / \partial X\) is the Jacobian stacking every factor's local derivative. Substituting and setting the gradient to zero gives the normal equations:

Solve this linear system for the update \(\Delta\), take the step \(X \leftarrow X + \Delta\), relinearize at the new point, and repeat until \(\Delta\) is negligible. That is Gauss-Newton. The matrix \(\Lambda = J^\top J\) is the information matrix, and its sparsity is the whole reason smoothing scales: because each factor touches only a few variables, \(\Lambda\) is mostly zeros with structure that mirrors the graph. A sparse Cholesky factorization solves a problem with thousands of poses in milliseconds. That same \(\Lambda\) also delivers the estimate's covariance, the topic of Section 11.6.

Gauss-Newton is fast near a good guess but can overshoot and diverge when the linearization is poor, because the quadratic model it trusts is accurate only close to the current estimate. Levenberg-Marquardt fixes this with a damping term, solving \((\Lambda + \lambda\,\mathrm{diag}(\Lambda))\,\Delta = -J^\top r\). Large \(\lambda\) shrinks the step toward a cautious gradient-descent move; small \(\lambda\) recovers the aggressive Gauss-Newton step. Damping with \(\mathrm{diag}(\Lambda)\) rather than the identity matters: it scales the penalty to each variable's own curvature, so a poorly constrained direction, a beacon geometry that barely pins down one axis, is damped in proportion to how little the data already determines it, rather than every state being treated as equally uncertain. The solver raises \(\lambda\) when a step worsens the error and lowers it when a step succeeds: a trust-region rule, shrinking its trust in the linear model after being misled and expanding it while things go well. That is why Levenberg-Marquardt is the default whenever the initial guess is rough, a pose graph seeded from raw, drifting odometry, while plain Gauss-Newton suffices once the estimate is already close.

Watching Gauss-Newton run

The mechanics are clearest on a problem small enough to print. Below, a static receiver at unknown 2D position estimates its location from noisy range measurements to four beacons at known coordinates, a stripped-down version of the positioning problem in Chapter 25. Each range is one nonlinear factor; we build the residual vector and its Jacobian by hand and iterate the normal equations.

import numpy as np

beacons = np.array([[0., 0.], [10., 0.], [0., 10.], [10., 10.]])
true_xy = np.array([3.0, 4.0])
rng = np.random.default_rng(0)
sigma = 0.20                                     # range noise std (meters)
z = np.linalg.norm(beacons - true_xy, axis=1) + rng.normal(0, sigma, 4)

def residual_and_jacobian(x):
    d = x - beacons                              # vectors from each beacon
    rng_pred = np.linalg.norm(d, axis=1)         # predicted ranges h(x)
    r = (rng_pred - z) / sigma                   # whitened residuals
    J = (d / rng_pred[:, None]) / sigma          # d h / d x, also whitened
    return r, J

x = np.array([1.0, 1.0])                         # deliberately poor start
for k in range(6):
    r, J = residual_and_jacobian(x)
    delta = np.linalg.solve(J.T @ J, -J.T @ r)   # normal equations
    x = x + delta
    print(f"iter {k}: x={x.round(3)}  cost={0.5*r@r:8.3f}  step={np.linalg.norm(delta):.4f}")
print("estimate:", x.round(3), " truth:", true_xy)

The loop is the entire section in eight lines: form residuals, build the Jacobian, solve \(J^\top J\,\Delta = -J^\top r\), step. The information matrix \(J^\top J\) here is a dense \(2\times 2\) because every factor touches the one variable, but swap in a trajectory of a thousand poses and that matrix becomes banded and sparse, and only the solver call changes.

What MAP gains, and what it costs

Why prefer this batch optimization to a recursive filter? Two reasons. First, relinearization: an extended Kalman filter linearizes each nonlinearity once, at the instant the measurement arrives, and can never revisit that choice; a smoother relinearizes every factor at every iteration around the improving estimate, so an early bad linearization gets corrected as later evidence arrives. This is why smoothing is markedly more accurate than filtering on strongly nonlinear problems, a limitation first met in Chapter 10. Second, joint consistency: MAP finds the trajectory that best explains all measurements simultaneously, so a loop closure can correct poses hundreds of steps in the past, which no forward filter can do.

The costs are equally concrete. Solving over the whole trajectory costs more than a single filter update, which is why Section 11.3 introduces incremental smoothing to reuse work between solves. And least squares assumes Gaussian noise: a single gross outlier, a mismatched loop closure, a lidar return off a glass wall, enters as a squared term and can drag the entire estimate off; Section 11.5 replaces the squared cost with robust kernels to defend against exactly that. For now, hold the core equivalence: the most probable trajectory minimizes noise-weighted squared error, found by linearizing and solving sparse linear systems until it stops moving.

What's Next

In Section 11.3, we make this loop incremental. Re-solving the entire trajectory from scratch every time a new measurement lands is wasteful when only a few variables actually change, so iSAM2 and the GTSAM library reuse the previous factorization, updating just the affected part of the sparse system. That turns the batch optimizer of this section into a smoother fast enough to run online, on a robot, in real time.