Robust cost functions

"You trusted every measurement equally, so one lidar return off a mirror convinced me the wall was two meters closer. I did exactly what the squared cost told me to. Next time, let me stop believing the liars."

A Chastened AI Agent

This section builds directly on the nonlinear least squares machinery of Section 11.2 and the sensor noise models of Chapter 2. It assumes you understand residuals, Jacobians, and the Gauss-Newton loop. The failure modes it defends against, multipath and non-line-of-sight returns, are the same ones catalogued for positioning in Chapter 25.

Why one outlier wins

Recall the whitened residual \(e_i = \lVert h_i(X_i) - z_i \rVert_{\Sigma_i}\), a scalar in units of standard deviations. The least squares objective sums \(\rho(e_i) = \tfrac12 e_i^2\). What a solver actually feels is not the cost but its derivative, the influence function \(\psi(e) = \rho'(e)\), which is the gradient each factor pushes with. For the squared cost, \(\psi(e) = e\): influence grows without bound. A residual at 30 standard deviations pushes thirty times harder than one at the noise floor, so it commandeers the solution. Under a Gaussian model a 30-sigma event should never happen, but a mismatched feature produces them routinely. The cure is to choose a \(\rho\) whose influence bounds or even redescends to zero, so that beyond some point a residual stops pulling harder the more wrong it is. Estimators built this way are called M-estimators, and the function \(\rho\) is a robust kernel or loss.

Three kernels span the useful range. The Huber loss is quadratic within a threshold \(\delta\) and linear beyond it, so influence saturates at a constant: outliers keep a fixed, bounded pull. It is convex, so it never introduces new local minima, which makes it the safe default. The Cauchy (Lorentzian) loss, \(\rho(e) = \tfrac{c^2}{2}\log(1 + e^2/c^2)\), is redescending: its influence rises, peaks, then decays toward zero, so a wild outlier is nearly ignored. Geman-McClure and truncated (Tukey) losses redescend even more aggressively, driving influence to exactly zero past a cutoff, which is the closest a smooth kernel gets to deleting a measurement. Redescending kernels reject outliers harder but are non-convex, so they need a good initial guess or a continuation strategy to avoid settling into a bad basin.

Iteratively reweighted least squares in code

The reweighting is easiest to trust when you watch it act. Below, a 1D estimate of a constant offset is measured many times; most readings are clean but a handful are gross outliers. Plain least squares averages everything and is pulled off; Huber IRLS down-weights the liars and recovers the true value. The weights are the whole story, so we print them.

import numpy as np

rng = np.random.default_rng(1)
truth = 5.0
z = truth + rng.normal(0, 0.3, 40)          # 40 clean readings
z[:4] = [18.0, -7.0, 22.0, 15.0]            # 4 gross outliers
sigma = 0.3
delta = 1.345                                # Huber threshold, in sigmas

def huber_weights(e):                        # e = whitened residuals
    a = np.abs(e)
    return np.where(a <= delta, 1.0, delta / a)

x = np.median(z)                             # robust-ish start
for k in range(8):
    e = (x - z) / sigma                      # whitened residuals
    w = huber_weights(e)                     # per-factor weights
    x = np.sum(w * z) / np.sum(w)            # weighted normal equation
print("least squares mean:", z.mean().round(3))
print("Huber IRLS estimate:", round(x, 3), " truth:", truth)
print("weights on the 4 outliers:", huber_weights((x - z[:4]) / sigma).round(3))

The mechanism generalizes without modification: in a real factor graph each \(w_i\) scales that factor's rows in the Jacobian and residual, and the sparse solve proceeds exactly as before. Note the deliberate median initialization. Robust kernels care where they start, because a redescending loss can lock onto whatever majority the initial guess happens to sit near.

Gating, switchable constraints, and graduated non-convexity

Robust kernels are the workhorse, but three companions matter in practice. First, chi-squared gating: because a whitened residual squared is chi-squared distributed under the Gaussian model, you can pre-reject any factor whose \(e_i^2\) exceeds a confidence threshold (for one degree of freedom, \(e_i^2 > 5.99\) is the 95% cutoff). This is a hard, cheap outlier test used to screen data associations before they ever enter the solve. Second, switchable constraints and dynamic covariance scaling attach a latent on/off weight to each suspect factor, typically a loop closure, and let the optimizer itself decide whether to trust it, jointly estimating the weights alongside the poses. This is the standard defense for the false loop closures introduced in Section 11.4, where a single wrong closure can shatter an entire map.

Choosing a kernel and its scale

Two knobs decide behavior: which kernel, and its threshold. Prefer Huber when outliers are moderate and you value a convex, initialization-free solve; prefer Cauchy or Geman-McClure when outliers are gross and you already have a decent estimate (from a filter, from odometry, or from GNC's continuation). The threshold is set in units of the noise sigma, which is exactly why the whitening of Section 11.2 matters: a mis-scaled \(\Sigma_i\) makes every residual look like an outlier or none of them do. If your covariances are honest, the kernel scale becomes a portable constant rather than a per-dataset tuning chore. The cost of robustness is small and worth naming: influence weights make the effective information matrix slightly smaller, so the uncertainty you read out in Section 11.6 must be computed from the reweighted system, not the nominal one, or you will report yourself more certain than you are.

What's Next

In Section 11.6, we turn from the point estimate to its uncertainty. The same information matrix \(J^\top J\) that the solver builds to take a step also encodes how confident the estimate is: its inverse is the covariance of the whole trajectory. We will read marginal uncertainties for individual poses straight out of the sparse factorization, see why robust reweighting shrinks that information, and connect the result back to the calibration discipline the book carries throughout.