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. Levenberg-Marquardt fixes this by adding a damping term, solving \((\Lambda + \lambda\,\mathrm{diag}(\Lambda))\,\Delta = -J^\top r\). Large \(\lambda\) shortens the step toward a cautious gradient-descent move; small \(\lambda\) recovers the aggressive Gauss-Newton step. The solver raises \(\lambda\) when a step makes the error worse and lowers it when a step succeeds, adapting its trust in the linear model automatically. This robustness is why it is the default in most practical solvers.

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 is more expensive 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 is the one that minimizes noise-weighted squared error, and you find it 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.