Uncertainty, Least Squares
& Robust Costs

Chapter 1: Covariance, Propagation & Mahalanobis Distance

A LiDAR returns range and bearing. The map wants Cartesian. Two questions drive this whole chapter: what is the covariance of that point in the robot's Cartesian frame — and is this next return the same landmark?

A covariance is a shape

A covariance is not an error bar. An error bar is a number per axis; a covariance matrix is a shape. It encodes, for every direction in the state space at once, how far the truth is likely to be from your estimate along that direction. The diagonal gives you the per-axis spread, and the off-diagonal gives you the thing that per-axis logging throws away: whether being wrong in x predicts being wrong in y.

Read that outer product literally. Each entry Σ_ij is the average of (x_i − μ_i)(x_j − μ_j). The diagonal is the average of a square, so it is non-negative and is the variance. The off-diagonal is an average of a product, so its sign says whether the two errors tend to move together.

Worked example 1 — a 2×2 covariance from five samples, every step visible. Five repeated observations of the same landmark, in metres:

Step 1 — the means. μ_x = (2 + 4 + 6 + 8 + 10) / 5 = 30 / 5 = 6. μ_y = (3 + 7 + 7 + 11 + 12) / 5 = 40 / 5 = 8.

Step 2 — centre the data. dx = (−4, −2, 0, +2, +4), dy = (−5, −1, −1, +3, +4).

Step 3 — the three sums, divided by N − 1 = 4 (Bessel's correction, because we estimated the mean from the same data):

Step 4 — read the shape. The correlation is Σ_xy / √(Σ_xxΣ_yy) = 11 / √130 = 11 / 11.402 = 0.965. Nearly perfectly correlated: the landmark is well localised along one line and badly localised across it. That is the classic range-bearing signature, and the marginal standard deviations (√10 = 3.16 and √13 = 3.61) tell you almost none of it.

Step 5 — the eigenvalues, by hand. For a 2×2, the characteristic polynomial is λ² − (trace)λ + det = 0, with trace = 10 + 13 = 23 and det = 9:

The 1σ ellipse semi-axes are the square roots: √22.602 = 4.754 m along the major axis and √0.398 = 0.631 m along the minor. A ratio of 7.5 in length, 56.8 in variance — that ratio is the condition number of Σ, and it is the number that decides whether your solver will behave.

Step 6 — the orientation. The major eigenvector satisfies (Σ − λ₁I)v = 0, so (10 − 22.602)v_x + 11 v_y = 0, i.e. −12.602 v_x + 11 v_y = 0, so v_y/v_x = 12.602/11 = 1.1456. The angle is arctan(1.1456) = 48.9° from the x axis.

Propagation through a nonlinear function

Your sensor does not measure the quantity you want. A LiDAR measures range and bearing; the map wants x and y. A camera measures pixels; the planner wants metres. Every time you change coordinates you must carry the uncertainty with you, and the mechanism is one line of first-year calculus.

The derivation, which you should be able to produce in twenty seconds. Let y = f(x) with x = μ + δ, where δ is the (small) error with covariance Σ. Take the first-order Taylor expansion about μ:

So the error in y is approximately Jδ, and its covariance is

That is the whole derivation. The constant f(μ) drops out because covariance is about deviations from the mean, and J comes out of the expectation because it is a fixed matrix evaluated at a fixed point. Where does the Jacobian come from? It is the linear map that the error rides through, and nothing more.

Worked example 2 — polar to Cartesian, every number. A LiDAR return at range r = 10.0 m, bearing θ = 30°, with σ_r = 0.10 m and σ_θ = 2°.

Step 1 — convert the bearing sigma to radians, because every trigonometric derivative assumes radians: σ_θ = 2 × π/180 = 0.034907 rad, so σ_θ² = 0.00121847.

Step 2 — the mean point. x = r cosθ = 10 × 0.866025 = 8.6603 m, y = r sinθ = 10 × 0.5 = 5.0000 m.

Step 3 — the Jacobian of (x, y) = (r cosθ, r sinθ) with respect to (r, θ):

Notice the units already: the second column has an r multiplying it, so a bearing error of one radian becomes a position error of r metres. Angular uncertainty is amplified by range. That single observation explains most of what a LiDAR-based map looks like.

Step 4 — multiply, in two stages. First JΣ (scaling column 1 by 0.01 and column 2 by 0.00121847):

Then (JΣ)J^T, where J^T = [[0.866025, 0.500000], [−5.000000, 8.660254]]:

Step 5 — sanity-check the answer against physics before you move on. σ_x = √0.037962 = 0.1948 m, σ_y = √0.093885 = 0.3064 m. Is that plausible? The along-range uncertainty is 0.10 m and the cross-range uncertainty is rσ_θ = 10 × 0.034907 = 0.3491 m — three and a half times larger. The ellipse is a thin sliver perpendicular to the beam, tilted at 30°, and its projections onto x and y are 0.195 and 0.306. Consistent.

Where the linearisation stops being true

JΣJ^T is an approximation, and the question that matters is always "when does it break?" The honest answer has a closed form for this particular function, which is why it is such a good example.

For Gaussian θ, the exact mean of r cosθ is r cosθ̄ · e^−σ_θ2/2. The exponential is the bearing-shrinkage bias: averaging a cosine over an angular spread always pulls the result toward the origin, and the linearisation, which is blind to curvature, never sees it. Here is that error made numerical against a 200,000-sample Monte Carlo push-through:

At 2° the linearisation is essentially exact — a 6 mm mean error against a 19 cm standard deviation is nothing. At 25° the mean is 90 cm wrong, which is three times the standard deviation of the estimate. Your EKF is not slightly imprecise at that point; it is confidently reporting a point that the true distribution almost never visits. The shape has also stopped being an ellipse — it is the classic banana, curved along the arc of possible bearings, and no symmetric ellipse can represent it.

See the EKF lesson for the filtering machinery and the UKF lesson for the sigma-point alternative; this lesson only cares about the covariance that comes out and whether you can defend it.

Mahalanobis distance: the only distance that matters

Now the second half of the opening question — is this next return the same landmark? Euclidean distance cannot answer that, because "close" means something different along the beam than across it.

The Mahalanobis distance measures a vector in units of the local standard deviation, in every direction at once:

The cleanest way to see what it does is to factor S = LL^T (a Cholesky decomposition, L lower-triangular). Then

So Mahalanobis distance is ordinary Euclidean distance after whitening — after applying the linear map that turns the uncertainty ellipse into a unit circle. That sentence is the whole idea, and it also tells you how to implement it: a triangular solve, never a matrix inverse.

Worked example 3 — two residuals of identical length, opposite verdicts. Use the Σ from example 1: S = [[10, 11], [11, 13]], det = 9. Its inverse, again by the 2×2 adjugate rule:

Residual A: y = (3, 2). Euclidean length √13 = 3.606.

Residual B: y = (3, −2). The same Euclidean length, √13 = 3.606.

The 95% gate for a 2-DOF measurement is chi-square at 5.991. Residual A (d² = 2.778) is accepted. Residual B (d² = 32.111) is rejected — it sits at 5.7 standard deviations. Identical Euclidean length; an 11.6× difference in the statistic that decides the association. Residual A points along the elongated axis that S declares uncertain; residual B points across it.

Because d² is a sum of squares of independent unit-variance Gaussians (that is exactly what whitening produced), it is chi-square distributed with m degrees of freedom. That is what turns a distance into a decision:

Where this lives in a real system

Here is the data-association path on a mid-size warehouse AMR, with the shapes and the clock:

The cost that is not free: carrying P. This is where EKF-SLAM died, and the arithmetic is worth having ready.

The last row is the punchline, and it is the answer to "why did the field move from filtering to optimisation?" It is not that the filter is wrong — it is that a filter maintains a dense covariance over everything it has ever seen, while a factor graph maintains a sparse information matrix and marginalises what it no longer needs. Same statistics, different data structure, three orders of magnitude in cost. The factor-graph lesson has the sparsity structure in detail.

Store the square root, not the covariance. Every serious back-end (√SAM, GTSAM, the square-root information filter in an INS) carries a Cholesky or QR factor rather than P itself, for one reason: the condition number of L is the square root of the condition number of P. If κ(P) = 10¹² then in double precision (about 16 significant digits) you have four digits of signal left after the worst cancellation. In square-root form κ(L) = 10⁶ and you keep ten. That is the difference between a filter that survives a long mission and one that produces a non-positive-definite P at minute forty.

The code: propagate, gate, update

From scratch first — the version you should be able to write from nothing:

python
import numpy as np

def polar_to_xy(r, th):
    return np.array([r * np.cos(th), r * np.sin(th)])

def jacobian_polar(r, th):
    # rows = outputs (x, y), cols = inputs (r, theta)
    return np.array([[np.cos(th), -r * np.sin(th)],
                     [np.sin(th),  r * np.cos(th)]])

def propagate(Sigma, J):
    """First-order covariance transport:  Sigma_y = J Sigma J^T."""
    return J @ Sigma @ J.T

def mahalanobis2(y, S):
    """Squared Mahalanobis distance, via Cholesky. NEVER np.linalg.inv(S):
    forming the inverse squares the condition number and costs 3x more."""
    L = np.linalg.cholesky(S)              # S = L L^T, raises if S is not PD
    w = np.linalg.solve(L, y)              # whiten: w = L^-1 y
    return float(w @ w)                    # ||w||^2

def ellipse_axes(Sigma, conf=0.95, chi2_2dof=5.991):
    """Semi-axis lengths and orientation of the confidence ellipse."""
    vals, vecs = np.linalg.eigh(Sigma)     # eigh: symmetric, ascending, stable
    k = np.sqrt(chi2_2dof)                 # 2.448 for 95% in 2-D, NOT 1.96
    order = np.argsort(vals)[::-1]
    vals, vecs = vals[order], vecs[:, order]
    axes = k * np.sqrt(np.maximum(vals, 0.0))
    angle = np.arctan2(vecs[1, 0], vecs[0, 0])
    return axes, angle

# The worked example from above, verified
r, th = 10.0, np.radians(30.0)
S_polar = np.diag([0.10**2, np.radians(2.0)**2])
S_xy = propagate(S_polar, jacobian_polar(r, th))
# S_xy = [[ 0.037962, -0.048431], [-0.048431,  0.093885]]
# sigma_x = 0.1948 m,  sigma_y = 0.3064 m
print(mahalanobis2(np.array([0.3, 0.2]), S_xy))   # 12.95 -> outside the 5.991 gate

Now the production form. Three changes, and each one is a question you should be ready for:

python
from scipy.linalg import cho_factor, cho_solve, solve_triangular

def gate_batch(Y, S, thresh=5.991):
    """Y: (N, m) innovations sharing one S. Returns (d2, keep).
    One factorisation amortised over all N rows -- this is the difference
    between 15 flops and 15N flops on the hot path."""
    L = np.linalg.cholesky(S)                       # O(m^3/3), once
    Wt = solve_triangular(L, Y.T, lower=True)      # (m, N), O(N m^2)
    d2 = np.einsum('ij,ij->j', Wt, Wt)              # column norms, no temporaries
    return d2, d2 < thresh

def joseph_update(P, K, H, R):
    """Symmetric, positive-definite-preserving covariance update.
    Costs ~2x the naive (I-KH)P but is the standard in flight software."""
    I = np.eye(P.shape[0])
    A = I - K @ H
    P = A @ P @ A.T + K @ R @ K.T
    return 0.5 * (P + P.T)                          # re-symmetrise every step

The three questions those changes invite: (1) Why never inv? Forming S⁻¹ costs about 3× a Cholesky solve and squares the condition number, so you lose twice as many significant digits for no benefit. (2) Why factor once for a batch? Because S depends on the landmark, not the candidate — hoisting the factorisation out of the inner loop is where the real speed-up lives. (3) Why Joseph? Because (I − KH)P is a difference of two nearly equal matrices; catastrophic cancellation can push an eigenvalue negative. Joseph form is a sum of two PSD terms, so it cannot.

Debugging: three failure modes

Failure mode 1 — loss of positive definiteness.

• Symptom: numpy.linalg.LinAlgError: Matrix is not positive definite from the gate, thirty to sixty minutes into a mission — never at startup. Or, more insidiously, no exception at all: the Kalman gain quietly goes to zero and the filter stops responding to measurements while continuing to report a shrinking covariance.
• Why: the naive update P ← (I − KH)P subtracts two nearly equal matrices. With κ(P) around 10¹² and double precision, cancellation eats twelve of your sixteen digits, and the smallest eigenvalue — which is genuinely tiny in an elongated covariance — can land on the wrong side of zero.
• The metric that reveals it: log min(eigvalsh(P)) and the asymmetry norm(P - P.T, 'fro') / norm(P, 'fro') on every update. Healthy: minimum eigenvalue stays several orders above machine epsilon times the maximum eigenvalue; asymmetry stays below 10⁻¹². When asymmetry starts climbing through 10⁻⁸, you have hours, not days.
• The fix, in order of cost: re-symmetrise every step (free); Joseph form (2× the update cost); square-root form (a rewrite, and the only permanent answer).

Failure mode 2 — degrees where radians belong. This one deserves its own bullet because the signature is so distinctive.

• Symptom: the gate accepts everything, or rejects everything, from the first frame. No drift, no slow degradation — broken immediately and completely.
• The number: (180/π)² = 3282.8. Putting a bearing variance in square degrees where the Jacobian expects square radians inflates that term by 3283×; the other direction deflates it by the same factor.
• The metric that reveals it: average NIS. If it comes back within a factor of two of 3283 (or of 1/3283 ≈ 0.0003), stop debugging the algorithm and grep for deg. A NIS that is off by roughly three thousand is never a modelling subtlety.

Failure mode 3 — the Jacobian evaluated at a stale point.

• Symptom: the filter is consistent when the robot moves slowly and inconsistent when it moves fast, with the transition happening at a repeatable speed.
• The metric that reveals it: scatter average NIS against angular rate over a recorded bag. A flat cloud means your model is fine; a clear positive slope means linearisation error, and the x-intercept of the trend tells you the rate at which your approximation stops holding — which is exactly the number to put in the system's operating envelope.
• The fix: re-linearise about the posterior (the iterated EKF, typically two or three inner iterations), or switch to sigma points. Note the cost honestly: the IEKF multiplies the update cost by the iteration count; the UKF costs 2n+1 propagations of the full nonlinear model.

The frontier: invariant filters and learned noise

The invariant EKF makes the linearisation error state-independent. Barrau and Bonnabel, "The Invariant Extended Kalman Filter as a Stable Observer" (IEEE Transactions on Automatic Control, 2017), showed that if you define the estimation error as a group element rather than a vector difference — η = X̂⁻¹X on SE(3) or SE₂(3) — then for a class of systems called group-affine, the error dynamics are exactly log-linear. The Jacobian no longer depends on the estimate, so the two great EKF sins — the covariance depending on the (wrong) current estimate, and spurious observability of unobservable directions — simply vanish. Hartley et al., "Contact-Aided Invariant EKF for Legged Robot State Estimation" (IJRR 2020), put this on a real biped and it is now the default formulation in legged locomotion. If the question is "what would you do differently today", this is the answer that shows you read past the textbook.

Learned covariance. Brossard, Barrau and Bonnabel, "AI-IMU Dead-Reckoning" (IEEE Transactions on Intelligent Vehicles, 2020), train a small convolutional network that looks at raw IMU windows and outputs the measurement covariance for a set of pseudo-measurements — not the state, only the noise model. On KITTI it reaches around 1% translational drift using an IMU alone. The idea generalises: the estimator stays classical and auditable, and learning is confined to the one component nobody can hand-tune well. Liu et al., "Deep Inference for Covariance Estimation" (ICRA 2018), makes the same argument for visual measurements, predicting a Cholesky-parameterised covariance so the output is positive definite by construction.

The non-learned alternative is still competitive. Sigma-point transforms (the UKF) match the propagated mean and covariance to third order for Gaussian inputs at a cost of 2n+1 evaluations of the true nonlinear function — no Jacobian, no derivation, no chain rule bug. For n = 15 that is 31 propagations per step, which at 200 Hz is entirely affordable on modern hardware and was not in 2001. A large part of "the EKF is standard" is inertia from a compute budget that no longer applies.

Try it: the propagation bench

What to look for. At σ_bearing = 2° the cloud fills the ellipse neatly and the mean shift readout is under a centimetre — the EKF is telling the truth. Take it past 10° and two things go wrong at once: the cloud curves out of the ellipse at the ends, and the true mean slides toward the sensor. Only the second one is a bias, and a bias is far more damaging than an inflated variance, because averaging more measurements does not remove it. This is the plot that explains why the UKF exists.

Chapter 2: Least Squares → Weighted → MAP

Derive least squares. It sounds like a warm-up. It is not. Writing down x̂ = (H^TH)⁻¹H^Tz immediately and stopping is what memorising a formula looks like. Starting from a probability distribution is what understanding looks like — and it is where all the interesting structure lives.

One derivation, five estimators

Here is the single most valuable structural fact in estimation, and it is worth stating before you write anything:

Start where the physics is. You have a measurement model: the sensor reports z, the world state is x, and they are related linearly through a known matrix H plus additive noise.

H is the observation matrix (m×n): row i says which combination of state elements measurement i sees. R is the measurement noise covariance (m×m): it says how wrong each sensor is and whether their errors are related. Nothing about squares yet.

The probability of seeing z given a hypothesised x is the Gaussian density evaluated at the residual:

Maximum likelihood means picking the x that makes the data least surprising. Logarithms are monotone, and the normalising constants do not depend on x, so maximising the density is the same as minimising the exponent:

Differentiate and set to zero. Expand the quadratic: J = ½(z^TR⁻¹z − 2x^TH^TR⁻¹z + x^TH^TR⁻¹Hx). Using ∂(x^TAx)/∂x = 2Ax for symmetric A and ∂(x^Tb)/∂x = b:

The matrix on the left, Λ = H^TR⁻¹H, is the information matrix (also called the Fisher information for this linear-Gaussian model — see Fisher Information & the CRLB). Its inverse is the covariance of the estimate. Two facts fall out of that, and both carry weight:

• Information is additive. Each measurement contributes one rank-1-ish term h_ih_i^T/σ_i² to the sum. That additivity is the entire reason factor graphs and incremental solvers work: adding a measurement means adding a term, not redoing the problem.
• The covariance of the estimate is (H^TR⁻¹H)⁻¹, and for a correctly specified model this attains the Cramér–Rao bound — it is the best any unbiased estimator can do.

Now set R = σ²I. Then R⁻¹ = I/σ², the scalar cancels from both sides, and the normal equations collapse to H^THx̂ = H^Tz — ordinary least squares. OLS is not a different method. It is this method with the assumption that every sensor is equally good and none of their errors are related. Say it that way and you have also answered "when should I not use OLS?"

Worked example 1 — three range measurements, three estimators

Three sensors measure the same distance to a beacon. Two are good laser rangefinders, one is a cheap ultrasonic:

OLS — every sensor equally trusted. H is a column of ones, so H^TH = 3 and H^Tz = 10.20 + 9.80 + 12.00 = 32.00:

WLS — trust in proportion to precision. Now H^TR⁻¹H = ∑w_i and H^TR⁻¹z = ∑w_iz_i:

Look at what happened. OLS put the estimate at 10.667 — 66 cm away from where the two precise sensors agree, because it let a sensor with ten times the noise cast an equal vote. WLS lands at 10.010, one centimetre from the laser average, and reports a standard deviation of 7 cm — better than either laser alone, because information added.

MAP — add what you already believed. Suppose a prior from the previous timestep says x ~ N(9.50, 0.20²), so the prior information is 1/0.04 = 25. The MAP normal equation adds the prior as one more information term:

The prior pulled the estimate down by 5.6 cm and shrank the reported uncertainty from 7.05 cm to 6.65 cm. Both effects are correct: a prior is information, and information both moves the estimate and tightens it. This is also exactly a Kalman update — the prior is the predicted state, the measurements are the innovation, and the weighting is the Kalman gain in disguise.

Worked example 2 — a two-parameter fit, by hand, twice

Scalars hide the matrix structure, so do a line fit. Three points, model y = a + bx, unknowns θ = (a, b):

Step 1 — assemble H^TH by accumulating over rows, because that is how you would actually build it in code and it makes the sparsity obvious. Each row contributes h_ih_i^T:

Step 2 — solve the 2×2. det = 3×5 − 3×3 = 15 − 9 = 6, so (H^TH)⁻¹ = (1/6)[[5, −3], [−3, 3]]:

Step 3 — residuals, and the check that costs nothing. Predictions at x = 0, 1, 2 are 1.1667, 2.6667, 4.1667, so r = (−0.1667, +0.3333, −0.1667). Their sum is exactly zero — guaranteed whenever the model has an intercept, because the first normal equation is the statement ∑r_i = 0. If your residuals do not sum to zero on a model with a constant term, you have a bug, full stop.

Step 4 — the parameter covariance. SSR = 0.02778 + 0.11111 + 0.02778 = 0.16667. With n − p = 3 − 2 = 1 degree of freedom, σ̂² = 0.16667, and

So σ_a = √0.13889 = 0.373 and σ_b = √0.08333 = 0.289, with a negative correlation of −0.0833/(0.373×0.289) = −0.77. Intercept and slope are anti-correlated, which is physically obvious: raise the line at x = 0 and you must tilt it down to keep passing through the data. That correlation is why you must never quote slope and intercept uncertainty independently.

Now down-weight the third point with w = (1, 1, 0.01) — the same machinery, one extra factor per row:

The fit swung from (1.167, 1.500) to (1.010, 1.971) — essentially the exact line through the first two points, y = 1 + 2x. One weight changed from 1 to 0.01 and the slope moved by 31%. Remember that sensitivity number. It is what makes Chapter 3 necessary: if a weight can do that when you set it deliberately, an outlier can do it when you do not.

The design: what a real back-end assembles

Every optimisation-based estimator you will meet in robotics is this cost function, assembled from many small terms. The engineering question is: what does assembling and solving it actually cost?

Take a representative sliding-window visual-inertial system — the shape of VINS-Mono, OKVIS, or a modern VIO back-end:

The factor-graph lesson has the sparsity structure and the elimination ordering in full. What matters here is the mapping: every factor in the graph is one term in the sum you just derived, its information matrix is that term's R⁻¹, and the graph's edges are exactly the non-zero blocks of H.

Where R actually comes from, in descending order of honesty:

The numerical decision every back-end makes: do you form the normal equations or factor the Jacobian directly?

The conditioning row is the one to remember. Forming H^TH squares the condition number, so if κ(H) = 10⁶ you lose twelve significant digits instead of six. In double precision (about sixteen) that is survivable; in single precision (about seven) it is fatal, which is precisely why square-root formulations exist and why an embedded team running float32 will care.

The code: one function, four estimators

python
import numpy as np

def wls(H, z, R=None, x0=None, P0=None):
    """One function, four estimators.
        R  = None, no prior  -> OLS
        R  given, no prior   -> WLS / GLS / MLE
        R and prior given    -> MAP == the Kalman update in information form
    Returns (x_hat, P_hat)."""
    H = np.atleast_2d(H); m, n = H.shape
    Rinv = np.eye(m) if R is None else np.linalg.inv(np.atleast_2d(R))

    Lam = H.T @ Rinv @ H          # information matrix  (n, n)
    eta = H.T @ Rinv @ z          # information vector  (n,)

    if x0 is not None:                # fold the prior in -- it is just one more term
        P0inv = np.linalg.inv(P0)
        Lam = Lam + P0inv
        eta = eta + P0inv @ x0

    L = np.linalg.cholesky(Lam)   # raises if the problem is rank deficient
    x_hat = np.linalg.solve(L.T, np.linalg.solve(L, eta))
    P_hat = np.linalg.inv(Lam)    # only if a caller actually needs the covariance
    return x_hat, P_hat

# The worked example, all three estimators from one call
H = np.ones((3, 1)); z = np.array([10.2, 9.8, 12.0])
R = np.diag([0.1**2, 0.1**2, 1.0**2])
print(wls(H, z)[0])                              # [10.667]  OLS
print(wls(H, z, R)[0])                           # [10.010]  WLS,  sigma 0.0705
print(wls(H, z, R, np.array([9.5]), np.array([[0.04]]))[0])   # [9.954], sigma 0.0665

That version is deliberately naive so the structure is visible. Here is what you write when it has to run:

python
from scipy.linalg import cho_factor, cho_solve, solve_triangular

def wls_sqrt(H, z, sigma, x0=None, sigma0=None):
    """Square-root form: whiten the rows, then let QR do the work.
    Never forms H^T H, so the condition number is kappa(H), not kappa(H)^2.
    `sigma` is the per-row standard deviation (a diagonal R)."""
    A = H / sigma[:, None]                 # whiten:  R^-1/2 H
    b = z / sigma
    if x0 is not None:                    # a prior is just extra pseudo-measurements
        A = np.vstack([A, np.diag(1.0 / sigma0)])
        b = np.concatenate([b, x0 / sigma0])
    Q, Rr = np.linalg.qr(A)                 # A = Q R,  R upper triangular
    x_hat = solve_triangular(Rr, Q.T @ b)   # one triangular solve
    # Cov = (A^T A)^-1 = R^-1 R^-T -- one more triangular solve, no inverse of A^T A
    Rinv = solve_triangular(Rr, np.eye(Rr.shape[0]))
    return x_hat, Rinv @ Rinv.T

Debugging: three failure modes of a solver

Failure mode 1 — rank deficiency and gauge freedom.

• Symptom: the solver reports convergence, the residuals look fine, and the reported covariance contains eigenvalues around 10¹². Or the Cholesky throws. Or the answer changes every run because Levenberg damping is picking an arbitrary point in a flat valley.
• Why: a monocular VIO problem has four unobservable directions — global position (3) and yaw about gravity (1). Roll and pitch are observable because gravity is a measurable direction. Nothing in the data distinguishes solutions that differ along those four, so Λ is singular by construction, not by accident.
• The metric that reveals it: count eigenvalues of Λ below (10⁻⁹ × λ_max). Healthy monocular VIO: exactly 4. Add a scale ambiguity (no IMU) and it becomes 7. If the count is larger than the gauge dimension you expect, you have a structural problem — a landmark seen from only one view, a keyframe with no connections — and no amount of damping will fix it.
• The fix: gauge-fix (hold the first pose constant), or add a weak prior on the gauge directions, or use Levenberg–Marquardt damping and accept that the answer inside the null space is arbitrary. All three are correct; say which one you would pick and why.

Failure mode 2 — units in R, the highest-frequency real bug in this whole area.

• Symptom: the optimiser converges in one iteration and the answer equals the initial guess. Or it takes 400 iterations and lands on nonsense. Both are the same bug seen from opposite sides.
• Why: a reprojection residual computed in normalised image coordinates but weighted with a covariance specified in pixels is off by f_x². At f_x = 500 that is 250,000×. The prior, correctly specified in metres, then outweighs every camera measurement combined, and the optimiser politely reports that the initial guess was optimal.
• The metric that reveals it: the final cost against its chi-square expectation. Ceres prints Final cost = ½∑r² over whitened residuals; for a correctly weighted problem the expectation is (M − N)/2. With M = 2,231 residuals and N = 750 parameters, that is 740. If Ceres reports 1.4×10⁵, the ratio is 189 — either your weights are ~189× too tight or you have outliers, and Chapter 3 tells you how to tell those two apart.
• The second metric: per-residual-block cost. Ceres and GTSAM will both report it. If one block type carries 99% of the final cost, that block is either mis-weighted or is where the model is wrong.

Failure mode 3 — the normal equations eating your precision.

• Symptom: the solution changes when you reorder the residual blocks, or differs between float32 and float64 by more than the reported uncertainty. The optimiser "converges" to slightly different answers on identical inputs across machines.
• The metric that reveals it: compare np.linalg.cond(H) with np.linalg.cond(H.T @ H). The second should be the square of the first; when it comes back much larger than that, the product has already lost digits. Rule of thumb: you have about 16 − log₁₀κ(H^TH) reliable digits in double precision. When that number drops below about 4, switch to QR.
• The common structural cause: parameters with wildly different scales in the same vector — metres next to radians next to unitless inverse depths. Rescale the columns (Jacobian scaling / Ceres's jacobi preconditioner) before blaming the solver.

The frontier: incremental, square-root, differentiable, certifiable

Incremental solving. Kaess et al., "iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree" (IJRR 2012), is the reason optimisation replaced filtering rather than merely competing with it. It keeps a factorisation in a data structure (the Bayes tree) that supports adding a factor and re-solving only the part of the tree that changed, so the cost of an update depends on how much the new measurement disturbs the graph, not on the graph's size. Loop closure is the expensive case, and it is expensive exactly when it should be.

Square-root everything. Demmel, Sommer, Cremers and Usenko, "Square Root Bundle Adjustment for Large-Scale Reconstruction" (CVPR 2021), and the sliding-window follow-up "Square Root Marginalization for Sliding-Window Bundle Adjustment" (ICCV 2021), do landmark elimination by nullspace projection on the square-root Jacobian rather than by Schur complement on the Hessian. The payoff is the conditioning argument above made concrete: their solver runs in single precision at accuracy the Schur-complement version cannot reach in single precision, which matters enormously on embedded and mobile GPUs.

Differentiable least squares. Pineda et al., "Theseus: A Library for Differentiable Nonlinear Optimization" (NeurIPS 2022), makes the whole Gauss–Newton solve a differentiable layer in PyTorch. The consequence for this chapter is direct: R stops being something you tune and becomes something you learn, by backpropagating a downstream task loss through the solver into the weights. The estimator stays a least-squares estimator — interpretable, with a residual you can inspect — and only the noise model is learned.

Certifiable global optimality. Rosen et al., "SE-Sync: A certifiably correct algorithm for synchronization over the special Euclidean group" (IJRR 2019), solves pose-graph SLAM through a semidefinite relaxation that returns a certificate that the answer is the global optimum, not just a local one. This kills the oldest objection to optimisation-based SLAM — "how do you know you are not in a local minimum?" — for the pose-graph case. It is a good thing to have an opinion about, because the honest limitation is that the certificate holds only under bounded noise and the relaxation is not tight once outliers appear.

Try it: the information stacking bench

Three things to try. (1) In OLS, drag the ultrasonic σ anywhere — the answer does not move, because OLS cannot hear you. That is the failure. (2) Switch to WLS and drag it from 1.00 to 0.10: the estimate walks from 10.01 up toward 10.67 as the bad sensor earns an equal vote. (3) Switch to MAP and drag the prior σ down to 0.05: the prior's information is 400, twice the two lasers combined, and the posterior collapses onto the prior mean. That last one is what an over-tight marginalisation prior does to a VIO window, and it is why "the estimate never moves from the initial guess" is a weighting bug.

Chapter 3: Outliers & Robust Cost Functions

Your feature matcher returns one wrong correspondence in forty. What does that do to your bundle adjustment — and how do you fix it?

Why one outlier can own the fit

Chapter 2 derived the quadratic cost from the Gaussian likelihood. That derivation is exactly correct — and exactly the problem, because your residuals are not Gaussian. A Gaussian says a 10σ residual happens once in 10²³ trials. A feature matcher on a repetitive warehouse shelf produces one every forty matches. The cost function does not know that, so it treats the 10σ residual as a catastrophe to be fixed at any price.

Worked example 1 — one outlier in four points flips the sign of the slope. The truth is y = 1 + 2x. Three points are clean, and the fourth has a bad association:

Assemble by rows, exactly as in Chapter 2. H^TH accumulates [[1, x], [x, x²]] over x = 0, 1, 2, 3:

The true slope is +2. Least squares reports −0.1. One outlier in four points did not degrade the fit; it inverted it. And the reported covariance will be a perfectly respectable-looking matrix, because nothing in the normal equations has any concept of a measurement being wrong.

Now look at the residuals, because this is the part nobody expects. At θ̂ = (2.4, −0.1) the predictions are 2.4, 2.3, 2.2, 2.1:

The three functions that describe any cost

To reason about this rather than guess, describe a cost by three related functions of the (whitened) residual r:

Least squares: ρ = ½r², ψ = r, w = 1. The influence function is a straight line through the origin with no upper bound. A residual ten times larger pulls ten times harder, forever. That single sentence is the entire diagnosis, and it is what you should say before proposing any fix.

Huber (Huber, 1964) keeps the quadratic near zero and switches to linear beyond a threshold δ:

The δ/2 in the linear branch is not decoration: it is exactly what makes ρ continuous at |r| = δ, since ½δ² = δ(δ − δ/2). Deriving that on the spot is the difference between understanding the function and recalling it.

Cauchy (also called the Lorentzian) never goes linear — it goes logarithmic:

Worked example 2 — the three costs at three residuals, every number. Take δ = 1 and c = 1:

Read the ψ columns down. At r = 10 the L2 measurement pulls with force 10. Huber pulls with force 1 — capped, but still pulling, and pulling as hard as a measurement at r = 1, forever. Cauchy pulls with force 0.099, which is less than a measurement at r = 0.5. Check the arithmetic on that Cauchy entry: ψ = 10/(1 + 100) = 10/101 = 0.09901, and ρ = 0.5×log(101) = 0.5×4.6151 = 2.3076.

IRLS: why a robust cost is still least squares

Here is the derivation that makes robust estimation implementable, and it is three lines. You are minimising ∑_i ρ(r_i) with r_i = z_i − h_i^Tθ. Set the gradient to zero:

Now use the definition w(r) = ψ(r)/r, so ψ(r_i) = w(r_i) r_i:

Worked example 3 — IRLS from a least-squares start, and why it stalls. Continue the four-point problem. Residuals at the OLS solution are r = (−1.4, +0.7, +2.8, −2.1). Take Huber with δ = 1:

Accumulate H^TWH by rows, w_i[[1, x_i], [x_i, x_i²]]:

The slope moved from −0.100 to −0.075. It is still negative. Five more iterations bring it to (2.067, −0.050) — converging, slowly, to a fit that is still completely wrong. Huber from a least-squares start, on a masked problem, does not recover. That is not a bug in Huber; it is the direct consequence of bounded-but-non-zero influence plus a starting point where the outlier already won.

And the standard scale estimate makes it worse, not better. The textbook says to set δ from a robust scale estimate. The median absolute deviation is the usual choice:

The 1.4826 is 1/Φ⁻¹(0.75) = 1/0.6745, the factor that makes MAD agree with the standard deviation for genuinely Gaussian data. Compute it on r = (−1.4, 0.7, 2.8, −2.1): sorted, (−2.1, −1.4, 0.7, 2.8), so median(r) = (−1.4 + 0.7)/2 = −0.35. The absolute deviations are |−1.4 + 0.35| = 1.05, |0.7 + 0.35| = 1.05, |2.8 + 0.35| = 3.15, |−2.1 + 0.35| = 1.75. Sorted: (1.05, 1.05, 1.75, 3.15), median = (1.05 + 1.75)/2 = 1.40.

Every residual is at or below 2.8, so with δ = 2.792 every weight is essentially 1 and the robust fit is the least-squares fit. The scale estimate was computed from a contaminated fit, so it inherited the contamination and then licensed it. This is masking closing the loop on itself.

Worked example 4 — the same problem, started from a consensus estimate. Suppose RANSAC picked the two points (0, 1) and (1, 3) and reported θ₀ = (1, 2). Residuals are then (0, 0, 0, −7): the outlier finally stands alone. Use Cauchy with c = 1, so w = 1/(1 + r²):

The first three rows contribute the clean [[3, 3], [3, 5]] and (9, 13) from Chapter 2's identical accumulation; the fourth contributes 0.02×[[1, 3], [3, 9]] = [[0.02, 0.06], [0.06, 0.18]] and 0.02×0×(1, 3) = (0, 0):

One reweight, and the slope is 1.866 against a truth of 2.000. A second iteration gives (1.0986, 1.8511) and it settles near (1.100, 1.848). Compare the three:

Four points is a deliberately brutal case — with 25% contamination and a high-leverage outlier at the end of the x range, no estimator can be comfortable. The lesson generalises: the initialisation matters more than the kernel. With forty points and 15% outliers, plain Huber IRLS from an OLS start recovers cleanly, which is exactly what the code lab below demonstrates.

The loss-functions lesson covers this family from the machine-learning side, where the same shapes appear as smooth L1, Charbonnier and Geman–McClure. The estimation vocabulary is different; the functions are identical.

The design: which factors get a kernel

In a real back-end a robust kernel is one line wrapped around a residual block. The design questions are which blocks get one, what δ means, and what it costs.

c++
// Ceres: the loss function wraps a cost function, per residual block.
problem.AddResidualBlock(reproj_cost, new ceres::HuberLoss(1.345), pose, point);
problem.AddResidualBlock(imu_cost,    nullptr,  pose_i, vel_i, bias_i, pose_j);
problem.AddResidualBlock(prior_cost,  nullptr,  pose_0);
// GTSAM equivalent:
//   noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345),
//                              noiseModel::Isotropic::Sigma(2, 1.0));

Note which blocks got nullptr. That is the design decision, and it is the one worth being able to defend:

The tuning constants, and where they come from. Each is chosen so that if the data really is Gaussian, the robust estimator retains 95% of the efficiency of plain least squares — you pay 5% more variance as insurance:

What it costs. Less than people fear, and the cost is in iterations rather than in flops:

The gate and the kernel are the same idea at different hardness. Chi-square gating from Chapter 1 is a truncated least-squares cost — weight 1 inside the gate, weight 0 outside. A robust kernel is the soft version. Production systems use both: gate first to remove obvious garbage cheaply, then a robust kernel to handle what survives. The gate is a decision you cannot take back; the kernel is one the optimiser revises on the next iteration.

The code: IRLS in twenty lines

python
import numpy as np

def huber_weight(r, delta):
    return np.minimum(1.0, delta / np.maximum(np.abs(r), 1e-12))

def cauchy_weight(r, c):
    return 1.0 / (1.0 + (r / c)**2)

def mad_sigma(r):
    """Robust scale. 1.4826 = 1/Phi^-1(0.75), so this agrees with the
    standard deviation on clean Gaussian data."""
    return 1.4826 * np.median(np.abs(r - np.median(r)))

def irls(H, z, weight_fn, theta0=None, iters=30, tune=1.345, tol=1e-10):
    """Iteratively reweighted least squares.
    theta0 SHOULD come from RANSAC or LMedS -- an OLS start on a masked
    problem converges to the wrong basin (see the worked example)."""
    theta = np.linalg.lstsq(H, z, rcond=None)[0] if theta0 is None else np.array(theta0, float)
    for _ in range(iters):
        r = z - H @ theta
        s = max(mad_sigma(r), 1e-9)          # re-estimate scale every iteration
        w = weight_fn(r, tune * s)
        Hw = H * w[:, None]                   # row scaling; never form a dense diag(w)
        new = np.linalg.solve(H.T @ Hw, Hw.T @ z)
        if np.max(np.abs(new - theta)) < tol:
            theta = new; break
        theta = new
    return theta, w

# Diagnostics to print every single time -- see DEBUG below
def weight_health(w, expected_outlier_rate=0.15):
    n_eff = w.sum()**2 / (w**2).sum()      # effective sample size
    frac_down = (w < 0.5).mean()
    return dict(n_eff_frac=n_eff / len(w),
                frac_downweighted=frac_down,
                looks_right=abs(frac_down - expected_outlier_rate) < 0.10)

The production one-liners, and what each one hides:

python
from scipy.optimize import least_squares

# f_scale IS delta, and it applies to the residual you return -- so return
# WHITENED residuals or f_scale means nothing.
sol = least_squares(residual_fn, theta0, loss='huber', f_scale=1.345, jac=jac_fn)
# loss: 'linear' (L2) | 'soft_l1' (pseudo-Huber) | 'huber' | 'cauchy' | 'arctan'

# statsmodels: the classical M-estimator with scale handling built in
#   sm.RLM(z, H, M=sm.robust.norms.HuberT(t=1.345)).fit(scale_est='mad')

# OpenCV, when the model is geometric: CONSENSUS first, polish after
#   cv2.findFundamentalMat(p1, p2, cv2.USAC_MAGSAC, 1.0, 0.999)
#   then re-fit on the inlier set with a robust kernel

Debugging: three ways a robust kernel goes wrong

Failure mode 1 — masking: the robust fit equals the least-squares fit.

• Symptom: you turned on HuberLoss, the final cost dropped slightly, and the estimate barely moved. Everyone concludes "there were no outliers".
• Why: the scale was estimated from a contaminated fit, so δ came out far too large and nothing was down-weighted — exactly the δ = 2.79 result in worked example 3, where every weight came back at 0.994 or above.
• The metric that reveals it: the weight histogram. A healthy robust fit shows a spike at w = 1.0 plus a thin tail, and the fraction with w < 0.5 should be within a few points of the outlier rate you believe you have. If every weight is above 0.9, the kernel never engaged. Second check: refit with LMedS or RANSAC. If the two fits differ by more than the parameter standard deviation, IRLS is in the wrong basin.
• The fix: initialise from a consensus method, and consider fixing δ from prior knowledge of the sensor noise rather than re-estimating it from contaminated residuals.

Failure mode 2 — δ too small: everything becomes an outlier.

• Symptom: the reported covariance inflates run over run, the estimate becomes jittery, and the solver takes many more iterations than usual.
• Why: with δ well below the true noise scale, ordinary Gaussian residuals get down-weighted too, so you have thrown away most of your data.
• The metric that reveals it: effective sample size, n_eff = (∑w)² / ∑w². With all weights 1 this is exactly n. Report n_eff/n; on a healthy problem with 15% outliers it sits around 0.85–0.95. Below 0.5 the kernel is eating inliers, and the extra variance shows up directly in the covariance.

Failure mode 3 — a robust kernel on a factor that is not an outlier source.

• Symptom: a sliding-window VIO that drifts slowly over minutes despite healthy reprojection error, healthy NIS on the visual factors, and no tracking loss. Restarting the estimator makes it briefly better.
• Why: someone wrapped the marginalisation prior in a robust kernel "for safety". When the window's estimate moves, the prior's residual grows, the kernel down-weights it, and the window forgets its own history — a slow, silent amnesia.
• The metric that reveals it: log the applied weight of every non-association factor. The marginalisation prior, the gauge prior and the IMU factors must all read exactly 1.000 at every iteration. Any value below 1 on those blocks is a bug, and it is invisible in every other diagnostic you have.

The frontier: GNC, adaptive kernels, certificates

Graduated non-convexity puts the convexity trade back on the table. Yang, Antonante, Tzoumas and Carlone, "Graduated Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers to Global Outlier Rejection" (IEEE RA-L, 2020), start with the kernel's scale parameter so large that the cost is effectively convex, solve, then shrink the scale and re-solve, three to five times. You get the outlier rejection of a redescending kernel with something close to the initialisation-independence of a convex one, and — the practical payoff — no RANSAC stage at all for many problems. To "Huber or Cauchy?", GNC is the answer that recognises the question as a false dichotomy.

Learn the shape of the kernel instead of picking one. Barron, "A General and Adaptive Robust Loss Function" (CVPR 2019), defines a single two-parameter family whose shape parameter α recovers L2 (α = 2), Charbonnier (α = 1), Cauchy (α = 0), Geman–McClure (α = −2) and Welsch (α → −∞) as special cases. Because the family is differentiable in α, you can make α a free parameter and let the optimiser decide how robust to be — per problem, or per residual type. It became the default loss in a great deal of neural depth and flow work for exactly this reason.

Certifiable robustness at extreme outlier rates. Yang, Shi and Carlone, "TEASER: Fast and Certifiable Point Cloud Registration" (IEEE T-RO, 2021), combines truncated least squares with a decoupled estimation of scale, rotation and translation, and returns a certificate of global optimality. It handles outlier rates above 95% on point-cloud registration — a regime where RANSAC's required iteration count is astronomical. Worth knowing as the existence proof that "robust" and "certified" are not contradictory.

Treat the outlier decision as a variable. Sünderhauf and Protzel, "Switchable Constraints for Robust Pose Graph SLAM" (IROS 2012), attach a continuous switch to every loop closure and let the optimiser turn it off, with a prior penalising the switch. Agarwal et al.'s Dynamic Covariance Scaling (ICRA 2013) then showed this is equivalent to a particular robust kernel with a closed-form weight — a nice unification: the switch is w(r). Knowing those two lines of work are the same idea is a good thing to have in your pocket.

Try it: influence functions side by side

Chapter 4: Showcase — The Fitting Bench

Everything in the previous three chapters exists so that you can answer one question with your hands instead of your mouth: what happens to my estimate when one measurement is wrong?

Below is a live fitting bench. Fourteen points sit near the line y = 1 + 2x. Three estimators fit them simultaneously — least squares, Huber and Cauchy — and the panel on the right draws each estimator's influence function ψ(r) with a dot showing where the point you are dragging sits on it.

That right-hand panel is the entire lesson. When you drag a point away from the line, watch the dot on the L2 curve climb forever while the Huber dot flattens onto its ceiling and the Cauchy dot turns around and comes back down. The lines on the left move because the dots on the right moved. Cause and effect, on one screen.

Five experiments, and the number to read in each

Do these in order. Each one isolates a claim from an earlier chapter, and each has a number you should be able to predict before you look.

Experiment 1 — clean data: all three agree. Press Reset. The three lines lie on top of one another and the fitted slope is within a few hundredths of 2.000. This is the point of the 95%-efficiency tuning constants from Chapter 3: a robust estimator costs you almost nothing when there is nothing to be robust against. If the robust fits were visibly worse here, δ and c would be too small.

Experiment 2 — one outlier, dragged slowly. Grab a middle point and pull it straight down. Watch three things at once:

• The red line follows immediately and keeps following, without limit.
• The teal line follows at first, then stops moving once the residual passes δ — it has hit the influence ceiling. It stays displaced, but the displacement stops growing.
• The purple line follows, reaches a maximum displacement around |r| ≈ c, and then returns to the clean fit as you keep pulling. That non-monotonic behaviour is the redescending property, and it is genuinely surprising the first time you see it.

Now read the share-of-pull bars under the influence panel, because that is where the surprise is. On clean data every point owns about 1/14 = 7.1% of the total pull, and the marked point reads 6.9%. Drag it down and the three bars do three different things — and only one of them does what the picture in your head predicts.

The L2 bar does not climb forever. It locks onto exactly 50.0% and stays there. Pull the point to the bottom of the plot, to a residual of −17.5, and the red bar reads 50.0%. Set benPts[6].y = -1000 in the console, so the residual is −937, and it still reads 50.0%. The reason is the two-line proof further down this chapter: at the least-squares optimum ∑r_i = 0, so if the outlier is the lone opposite-sign residual then ∑|r_i| = 2|r_k| identically, and share^a_k = 1/2 for every configuration. One half is not a big number the outlier happens to reach; it is a ceiling that no configuration can exceed, and the moment the outlier becomes the lone opposite-sign residual it is also a floor. Anyone who tells you a single point can own 60% or 80% of a least-squares gradient has never printed the number.

Here is the whole drag, measured on the bench's own solver. Every cell is what the bars print, to four decimals instead of the one the canvas has room for:

The Huber bar locks too, at exactly 18.7564%, and that is the more surprising of the two because nobody teaches it. Read down the Huber column: from y = 9.2 to y = −1000 the number does not move in the fourth decimal. Neither does the fit. Print it and you get a = 1.06419040, b = 1.98972574 at a residual of −13, and a = 1.06419040, b = 1.98972574 at a residual of 10⁶. Identical to eight decimals. The teal line in Experiment 2 does not "stop moving much" — past δ it stops moving at all, and there is a clean reason.

Watch what the outlier contributes to the weighted normal equations once its weight is w_k = δ/|r_k|. Its four accumulations are w_k, w_kx_k, w_ky_k and w_kx_ky_k. As |y_k| grows, w_k decays like 1/|y_k|, so the first two go to zero — the outlier drops out of the normal matrix entirely — while

Those are constants. So in the limit the estimator is solving the thirteen-inlier least-squares problem with a fixed extra term on the right-hand side, and the displacement it causes is exactly

Do it by hand for this dataset. The thirteen inliers give s₁₁ = 13, s₁₂ = 65.3846, s₂₂ = 463.3136, so det = 13·463.3136 − 65.3846² = 6023.077 − 4275.148 = 1747.929. The outlier sits at x_k = 4.6154. Then

which is [0.092417, −0.003081], so at δ = 1 the predicted displacement is Δa = −0.092417, Δb = +0.003081. Check it against the solver: the thirteen inliers alone fit y = 1.156607 + 1.986645x; the converged Huber fit on all fourteen is y = 1.064190 + 1.989726x. Subtract: −0.092417 and +0.003081. Exact to six decimals, from arithmetic you can do on a whiteboard.

Only the Cauchy bar is non-monotonic, and it is the only one that behaves the way the influence panel's shape leads you to expect. It rises to a peak of 13.31% — nearly double an even share — at r = −1.046, which is c to within the resolution of a drag, because that is where ψ_Cauchy peaks. Then it turns around and falls: 10.13%, 6.21%, 4.39%, 2.14% at the preset, 1.53% at the drag floor. Past that the tail has a closed form. For |r| ≫ c the numerator ψ ≈ c²/|r| while the denominator is nearly all inliers and settles at ∑|ψ| → 3.4750, so

Test it against the table: at r = −18.797 that predicts 1.531% and the solver prints 1.527%; at −40.300 it predicts 0.714% against 0.717%; at −1010.303 it predicts 0.0285% against 0.0287%. The Cauchy share decays like 1/|r| with no floor at all, and a two-term approximation nails it to three significant figures over two decades of residual. That is the difference between "the outlier gets ignored" as a slogan and as a rate.

One more reading, easy to miss and worth the ten seconds: the 50% lock has a visible switch-on point. Above y = 1.378 the L2 bar is at 49.9996% and rising; below it, exactly 50.0000% forever. What happens at that y is that the last inlier still sharing the outlier's sign — point i = 8, at x = 6.154 — has its residual dragged through zero, so from there down the outlier is the lone negative residual and the proof's hypothesis is satisfied. The theorem's precondition is a thing you can watch happen on the screen, which is the best possible way to remember that it has one.

Experiment 3 — the redescending trap, and why you cannot trip it by dragging. With the point pulled far out, bring it back toward the line slowly and watch the purple curve. The folklore says that somewhere around |r| ≈ c the Cauchy fit should lurch — two nearly-equal minima, the solver flipping between them. Do it and nothing lurches. The purple line slides back smoothly the whole way, and that is not a bug in the demo: that is the finding.

The reason is one line of the bench's source. It chains its solves — L2 seeds Huber, Huber seeds Cauchy, var tC = benchSolve(wCau, c, tH) — so the Cauchy solve never gets a cold start. It always begins at an answer that has already survived a bounded-influence kernel, and from there it simply cannot reach the second basin. Formally: I re-ran the same solver from four sensible starts — the OLS fit, the Huber fit, θ = (0, 0), and the ground truth (1, 2) — at c = 0.2, 0.3, 0.4, 0.5, 0.7, 1.0, 1.2 and 1.5. At every single c the four starts agree to a relative cost gap under 10⁻¹⁴, which is machine precision on a sum of fourteen logarithms. On this data, from any reasonable start, the non-convexity never bites.

So the bench gives you a button to aim at it deliberately. Press Cauchy start: Huber (warm) and it flips to OUTLIER LINE (cold): instead of seeding from the Huber fit, the Cauchy solve now starts on the exact line through the marked point and its neighbour — two points, one line, sitting right on top of the outlier. It is the worst legal initialisation and it is exactly what a minimal-sample hypothesis would hand you if the sample were contaminated. Now the lurch is something you can produce on demand:

1. Press Throw one outlier, then turn the cold start on. Set c = 1.0. Nothing happens — the purple line is still the good fit. One basin.
2. Walk c down one step at a time. At c = 0.9, 0.8, 0.7, 0.6: still nothing. Same answer, same cost.
3. Step from c = 0.6 to c = 0.5 and the purple line snaps to a slope of 7.7, and the readout under the canvas flips its basin gap from < 1e-3 to 5.85. Step to 0.4, 0.3, 0.2 and it swings to a slope of about +18.2 — a line drawn essentially through the outlier and its neighbour, ignoring the other twelve points entirely.
4. Turn the cold start back off at c = 0.2. The purple line jumps back to slope 2.008. Same data, same kernel, same c, same solver — two answers, and the only difference is where you started. That is what non-convexity costs you, made pressable.

The readout under the canvas now prints both costs on every frame, so you never have to take this on trust: J from the Huber start, J from the outlier-line start, and the basin gap |J_a − J_b| / min(J_a, J_b) between them. It is green when the surface has one basin and red when it has two, and it goes red at exactly the c where the folklore says it should. That readout is the failure-mode section's second metric, wired live to a slider — and note it is printed whether or not the cold start is switched on, because a detector you have to remember to enable is a detector you will forget.

Now look at the Cauchy share bar while you are in the wrong basin. At c = 0.2 with the cold start on, it reads 41.4% — nearly six times an even share, and twenty times what the same bar reads in the good basin. Sit with that for a second, because it is the most vivid thing on the screen: the estimator has not failed to reject the outlier, it has adopted it. In that basin the fitted line passes through the bad point, so the bad point's residual is tiny and its Cauchy weight is near 1, while the twelve honest measurements are hundreds of units away, get weights near zero, and are the ones being discarded. A redescending kernel in the wrong basin does not degrade gracefully; it inverts, and it inverts with high confidence — small residuals, small cost, clean convergence, every internal check green. That is the exact shape of failure the failure-mode section's second-solve test exists to catch, and it is why "the cost looked fine" is never evidence.

Read the bottom rows first. At c = 1.0 the kernel is wide enough that the cost still has one basin, and even the malicious start gets pulled back to the right answer — identical to six decimals. At c = 0.2 it does not: the bad start ends at a slope of 18.573 against a truth of 2.000, sitting in a basin whose cost is 5.4× worse. Narrowing c is what manufactures the second basin. The redescending trap is not a property of the Cauchy kernel; it is a property of the Cauchy kernel tuned tight, and c is the knob that decides whether you have one basin or several. That distinction is what separates someone who has read about non-convexity from someone who has run it.

And it is worth knowing the threshold as a number, because you can bisect for it in about twenty lines. Run the basin-gap test at a given c, and binary-search on c for the point where it stops firing: on this dataset the second basin survives up to c = 0.5364 and is gone by c = 0.5365. Since the slider moves in steps of 0.1, that lands the transition between the c = 0.5 and c = 0.6 detents, which is exactly where you felt it snap. "My redescending kernel is safe above c = 0.54 on this data, and I found that by bisection on a two-start cost comparison" is a sentence with a method inside it. "Cauchy is non-convex so be careful" is not.

One further thing the button reveals that the folklore never mentions: it is not two basins, it is at least three. Seed the solve on the line through the outlier and its left neighbour instead of its right one — slope −15.65 rather than +18.68 — and at c = 0.2 the solver converges to a third distinct minimum at slope −15.551 with J = 2.6389, close to but measurably different from the +18.572 basin's J = 2.5996. Three local minima on fourteen points and two parameters. This matters for how you phrase the fix: "restart from a second initialisation and keep the lower cost" is a detector, not a solution — with k basins you would need to have sampled the right one. The actual solution is to remove the basins, which is what graduated non-convexity does by starting at a c so wide the surface is provably single-basin and annealing down while tracking the minimum. The bench's c = 1.0 row is the first step of that schedule.

Experiment 4 — leverage. Press Outlier at the far end, which puts the bad point at the largest x. Compare with dragging a point in the middle by the same amount. The far-end outlier does far more damage for the same residual, because its arm on the slope is longer. This is the leverage effect from Chapter 3: a robust cost bounds the influence of a large residual, but a leverage point can bend the model toward itself and thereby keep its own residual small. That is the case Huber cannot save you from.

Here is the trap in the demo itself, and you should catch it: the bars on screen cannot show you this. They plot the pull on the intercept, and leverage lives entirely in the arm x that the intercept share throws away. The derivation is two sections down; the number you should compute by hand while running this experiment is the slope share, |x_kψ_k| / ∑_i|x_iψ_i|. For the far-end point at x = 10 it is roughly double what the bar reads:

Now compare that L2 slope error, 0.570, against the 0.025 the same estimator posted with the outlier in the middle. Same magnitude of corruption, same single bad point, 23× the slope damage, purely because x moved from 4.6 to 10. That factor of 23 is the whole content of the word "leverage", and it is a number, not an adjective.

Experiment 5 — δ too small. Slide Huber's δ down to 0.2 on clean data. The teal line starts jittering as you nudge points, and the fit becomes noticeably noisier than the red one. You are now down-weighting ordinary Gaussian residuals, throwing away real information for no reason — failure mode 2 from Chapter 3, visible to the eye.

"Noticeably noisier" is an eyeball judgement, and you cannot alarm on an eyeball. The quantity to log is the mean IRLS weight on data you believe is clean, ∑w_i/N, evaluated at the converged fit. On the clean dataset — the fourteen points on your screen right now, after pressing Reset, seed 7, before you throw any outlier — it reads:

Write down which dataset that reading is on, because it matters and it is where this chapter can mislead you. 0.677 is this dataset's number — one particular draw of fourteen Gaussian residuals. Draw fourteen fresh residuals and you get a different number: over 400 independent noise draws the same quantity at δ = 0.20 averages 0.789, with a spread wide enough that individual draws land anywhere from 0.61 to 0.94. The DEBUG section below reports the 400-draw ensemble figures, because a threshold you intend to alarm on must not be calibrated against one lucky draw. So when you see 0.677 here and 0.789 there, they are not two measurements of one thing that disagree — they are one solve versus an ensemble of solves, and the distance between them is itself the reason the DEBUG section exists. The full symptom → metric → threshold triple, and what that spread does to a naive alarm, is below.

Reading the share-of-pull bars quantitatively — and what they leave out

The bars are not decoration, but they are also not the whole story, and the fastest way to see why is to actually differentiate the cost instead of gesturing at it. Write the model as r_i(θ) = y_i − (a + b x_i) with θ = (a, b), and the total cost as J(θ) = ∑_i ρ(r_i(θ)). By the chain rule:

Now the two partials, which is where the whole leverage story hides. Differentiating r_i = y_i − a − b x_i:

So the gradient is a pair of sums, not one sum:

Read that carefully, because it says the demo is only telling you half the truth. The intercept arm weights every residual identically. The slope arm weights each residual by its own x. A point therefore owns two different fractions of the pull, and they are not the same number:

Divide one share by the other and the leverage factor falls out in closed form. Let x̄_ψ = ∑_i|x_iψ_i| / ∑_i|ψ_i| be the influence-weighted mean arm. Then:

Check it on both datasets, using the Huber fit in each. Both checks close to four significant figures, which is what tells you the identity is exact and not a fit:

The first row is the quiet one: a mid-range outlier sits almost exactly at the crowd's average arm, so its slope share and intercept share are the same number and the bar you can see is telling you the truth. The second row is the one that bites. A point's extra grip on the slope is exactly its arm divided by the crowd's average arm — 1.76× here, and it grows without bound as you push the point further out along x, because x̄_ψ is set by the crowd and x_k is not. That single line is the most compact statement of leverage you can put on a whiteboard, and it is why "bounded influence" and "high breakdown" are different guarantees.

With that established, here is the default state of the bench, worked out so you can check the numbers on screen against arithmetic. (The Share column is the intercept share, matching what the bars draw.)

Check the L2 row by hand. Its ψ is the residual itself, so the outlier contributes 12.382. The other thirteen points have a mean absolute residual of 0.952 against the L2 fit — inflated, because the fit has been dragged — so they contribute 13 × 0.952 = 12.38. Total 24.76, and 12.382 / 24.764 = 50.0%. One point in fourteen holds half the vote.

That 50.0% is not a rounding coincidence, and the two-line proof is worth having ready, because "why exactly half?" is the natural next question. At the least-squares optimum both gradient components vanish, which by the derivation above means ∑_i r_i = 0 and ∑_i x_ir_i = 0 exactly. Suppose one residual r_k has the opposite sign to all the others — which is precisely what "one outlier pulled down through a clean line" means. Then ∑_i≠k r_i = −r_k, and because those thirteen all share a sign, ∑_i≠k|r_i| = |∑_i≠k r_i| = |r_k|. Therefore ∑_i|r_i| = 2|r_k| and:

Run the identical argument on ∑x_ir_i = 0 (all x_i ≥ 0 here) and the slope share is exactly 1/2 too — the solver reports 114.294 for ∑|x_ir_i| against 57.147 for the outlier alone, and 57.147 × 2 = 114.294. So under least squares, a lone opposite-sign outlier owns exactly half of both gradient components no matter how far out you drag it. Drag it to a residual of 10⁶ and it still owns 50%, because it drags the fit until it does. That is unbounded influence stated as a theorem rather than a picture.

Check the Huber row. Past δ the influence is clamped at exactly δ = 1.000, no matter how far out the point goes. The thirteen inliers now sit at a mean absolute residual of 0.333 (the fit is nearly correct again), all inside δ, so each contributes its own residual: 13 × 0.333 = 4.33. Total 5.33, share 1.000 / 5.332 = 18.8%. Still five times an even share, but bounded.

The Huber and L2 checks both used the same shortcut — take the mean residual, push it through ψ, multiply by 13 — and both are exact. They are exact for a reason that does not survive to the next row, so notice it now. For L2, ψ(r) = r is linear in r; for Huber inside δ, ψ(r) = r is also linear. Whenever ψ is linear over the range you are averaging, mean(ψ(r)) = ψ(mean r) and the shortcut is an identity. Move to any redescending kernel and it stops being one.

Check the Cauchy row — and watch the shortcut break. The outlier first: ψ = r/(1 + r²) at r = −13.319 gives 13.319 / (1 + 177.40) = 13.319 / 178.40 = 0.0747. Now the thirteen inliers. Their mean absolute residual is 0.33191, and the tempting move is to evaluate ψ there: 0.33191 / (1 + 0.11016) = 0.29897. That is the wrong number. ψ_Cauchy is concave over the inlier range, so by Jensen's inequality the mean of ψ sits below ψ of the mean. The quantity you actually need is mean(|ψ(r_i)|) = 0.26251, a full 12% lower. Use it and everything closes: 13 × 0.26251 = 3.4126, plus the outlier's 0.0747 gives ∑|ψ| = 3.4873, and 0.0747 / 3.4873 = 2.14% — the table's 2.1%, and the bar on screen. Take the shortcut instead and you would have computed 13 × 0.29897 + 0.0747 = 3.9613 and reported a share of 1.89%, which matches nothing.

Because a claim like "trust the mean of ψ, not ψ of the mean" is exactly the kind of thing that should be checkable rather than asserted, here are all thirteen inliers term by term, against the converged Cauchy fit y = 1.0649 + 1.9997x. Add the last column over the thirteen inlier rows yourself; it comes to 3.4127:

Two things jump out of that table that no summary statistic would have shown you. First, the outlier's weight is 0.0056 against a typical inlier's 0.90, so it enters the normal equations at about 0.6% of an ordinary measurement — the column sums to ∑w = 11.43, meaning Cauchy is solving a 14-point problem with roughly 11.4 points' worth of weight and has spent almost none of it on the bad one. Second, look at row 0: a perfectly good point with r = +0.9009 is the single largest inlier contributor at 0.4973, six times the outlier's. The most influential measurement in this fit is a clean one, which is the state you want and the exact opposite of what the L2 column shows. Share 2.14% is below the even share of 7.1%: the outlier has been demoted beneath an ordinary measurement, by arithmetic you just checked by hand.

Rebuild the bench's solver from nothing

You have now dragged points around a solver you have never seen. That is the wrong way round — what you cannot build, you do not yet own. The bench's entire estimator is twenty lines, and it is the same twenty lines you would write in any editor. Here it is, with the fourteen points frozen so you can run it and get the digits in the table above.

The one idea that makes it work is iteratively reweighted least squares. The robust gradient is ∑ψ(r_i) ∂r_i/∂θ. Define a weight w(r) = ψ(r)/r and that becomes ∑w(r_i) r_i ∂r_i/∂θ, which is exactly the gradient of a plain weighted least-squares problem. So: freeze the weights at the current residuals, solve a weighted LS problem in closed form, recompute the weights, repeat. Each sweep is ordinary linear algebra; the robustness lives entirely in the weight function.

python
import numpy as np

# ── the bench's fourteen points, frozen ────────────────────────────────
# This is exactly what "Throw one outlier" gives you: seed 7, index 6
# pulled down by 13.0 from the true line y = 1 + 2x.
X = np.array([0.0000, 0.7692, 1.5385, 2.3077, 3.0769, 3.8462, 4.6154,
              5.3846, 6.1538, 6.9231, 7.6923, 8.4615, 9.2308, 10.0000])
Y = np.array([1.9658, 2.5146, 3.7458, 5.6427, 7.1057, 9.0134, -3.0243,
              11.3434, 12.7725, 15.3868, 16.8053, 18.3178, 19.2894, 21.0286])

def w_huber(r, d):
    """The IRLS weight, w = psi(r)/r. Inside delta psi(r) = r so w = 1;
    outside, psi(r) = delta*sign(r) so w = delta/|r| and decays like 1/|r|.
    Clamp the divisor: r == 0 is legal and must give w = 1, not a NaN."""
    a = np.abs(r)
    return np.where(a <= d, 1.0, d / np.maximum(a, 1e-12))

def solve_2x2(x, y, w):
    """One weighted normal-equation solve, in closed form.

        [ s11  s12 ] [a]   [b1]      s11 = sum w        b1 = sum w*y
        [ s12  s22 ] [b] = [b2]      s12 = sum w*x      b2 = sum w*x*y
                                     s22 = sum w*x*x

    No lstsq, no np.linalg.solve, no forming an inverse: for a 2x2 the
    closed form is exact, branch-free and about twenty flops. Say out loud
    while writing it that det > 0 as long as two x values differ and the
    weights are positive -- that is the observability condition, and it is
    the same statement as 'you cannot fit a line to one point'."""
    s11 = w.sum();          s12 = (w * x).sum();      s22 = (w * x * x).sum()
    b1  = (w * y).sum();    b2  = (w * x * y).sum()
    det = s11 * s22 - s12 * s12
    return (s22 * b1 - s12 * b2) / det, (s11 * b2 - s12 * b1) / det

def irls(x, y, d, sweeps=40, tol=1e-12):
    a, b = solve_2x2(x, y, np.ones_like(x))     # sweep -1: a plain OLS start
    for k in range(sweeps):
        w      = w_huber(y - (a + b * x), d)    # 1. reweight at current theta
        na, nb = solve_2x2(x, y, w)             # 2. re-solve with those weights
        if abs(na - a) < tol and abs(nb - b) < tol:
            return na, nb, k                    # 3. stop when nothing moves
        a, b = na, nb
    return a, b, sweeps

a, b, k = irls(X, Y, d=1.0)
print(a, b, k)      # 1.064177  1.989727  converged at sweep 7

Two details in there deserve attention. The np.maximum(a, 1e-12) guard exists because a residual of exactly zero is a perfectly ordinary event (a point that lands on the line) and the naive d/abs(r) would hand you an inf weight and a NaN fit. And the closed-form 2×2 is not laziness — the general form of this same solve is a Cholesky of J^TWJ, which is what Chapter 1 argued for and what every real solver does; at n = 2 the Cholesky is those five sums.

Now instrument the loop and print the sweeps. This is the trace that connects the code to the table three sections up:

The ∑w column is the most informative thing in that table, and it is free. At convergence it reads 13.0753, which factors by hand: thirteen inliers at weight exactly 1, plus the outlier at δ/|r| = 1/13.2719 = 0.0753. Thirteen plus 0.0753. The weight vector is a per-measurement verdict, and you can read the estimator's opinion of every point straight off it without any extra instrumentation.

Now read sweep 0, because it is masking caught in the act. ∑w = 11.8278, not 13.08 — six points are below weight 1. One is the outlier (w = 1/12.3818 = 0.081). The other five are honest measurements at i = 0, 5, 9, 10 and 11, whose residuals against the contaminated OLS line exceed δ = 1 — 1.954, 1.213, 1.357, 1.218, 1.173 — even though their residuals against the true line y = 1 + 2x are only 0.966, 0.321, 0.541, 0.421 and 0.395. Every one of them is an ordinary draw from the 0.35-sigma noise, and every one of them is being penalised for the outlier's crime. On the very first sweep the estimator is punishing five good points and barely touching the bad one. It recovers by sweep 1 here; on the far-end dataset, or with a tighter δ, it does not. That is the whole argument for a consensus initialisation in one column of numbers, and "I print ∑w per sweep" is a far better answer to "how would you debug your robust solver?" than "I'd look at the residuals".

And now the library form — but only now, because being handed this line before you have written the loop teaches you nothing:

python
from scipy.optimize import least_squares

res = least_squares(lambda t: Y - (t[0] + t[1] * X), x0=[0.0, 0.0],
                    loss='huber', f_scale=1.0)     # f_scale IS delta
print(res.x)     # [1.064177  1.989727]  -- identical to the loop, to 6 decimals

res = least_squares(lambda t: Y - (t[0] + t[1] * X), x0=[0.0, 0.0],
                    loss='cauchy', f_scale=1.0)    # f_scale IS c
print(res.x)     # [1.064873  1.999739]  -- the purple line, to 4 decimals

The trap in that one-liner is f_scale. SciPy does not take δ directly; it evaluates ρ on the scaled square z = (f/f_scale)² and multiplies the result back by f_scale², which for loss='huber' works out to exactly the Huber kernel with δ = f_scale — hence the six-decimal agreement above. Leave f_scale at its default of 1.0 while your residuals are in pixels and you have silently declared every measurement past one pixel an outlier. The units of f_scale are the units of your residual, and forgetting that is the single most common way this call goes wrong. In C++ the same knob is new ceres::HuberLoss(delta) passed as the loss argument to AddResidualBlock; in GTSAM it is noiseModel::Robust::Create(mEstimator::Huber::Create(k), base), where the base noise model carries the covariance and the robust wrapper carries only the kernel — two separate objects, and mixing up which one holds the scale is the GTSAM version of the same bug.

CODE — the twenty lines that turn the bench into an instrument

The solver above produces an answer. Everything this chapter actually reads — both share arms, the frozen-fit displacement, the basin gap — is instrumentation wrapped around it, and each piece is short enough to write while talking. Here they are, in the order they appear in the chapter, each returning a number you can check against a table above.

python
def shares(x, y, theta, psi):
    """Both gradient arms, because the canvas only draws the first one.

    grad J = ( -sum psi_i , -sum x_i psi_i ).  A point therefore owns TWO
    fractions and they differ by exactly |x_k| / xbar_psi -- the leverage
    multiplier. Returning one of them and calling it 'influence' is the bug
    the bench itself ships with, and naming it is worth more than hiding it."""
    r  = y - (theta[0] + theta[1] * x)
    p  = np.abs(psi(r))
    return p / p.sum(), (np.abs(x) * p) / (np.abs(x) * p).sum()

sa, sb = shares(X, Y, huber_fit, lambda r: np.clip(r, -1.0, 1.0))
print(sa[6], sb[6])   # 0.18756  0.18483   -- mid outlier: the arms agree
# On the far-end dataset the same call prints 0.16313 and 0.28706: the
# slope arm is 1.76x the bar you can see. That ratio is |x_k| / xbar_psi.

def frozen_offset(x, y, k, delta):
    """Where the Huber fit lands once |r_k| > delta, WITHOUT solving for it.

    Past delta the outlier's weight is delta/|r|, so its normal-matrix terms
    decay to zero while its right-hand-side terms tend to the constants
    -delta and -delta*x_k. It has stopped being a measurement and become a
    constant force. The displacement is therefore closed-form and does not
    depend on the residual at all -- which is what 'bounded influence'
    means when you write it down instead of gesturing at it."""
    m  = np.ones(len(x), bool); m[k] = False          # inliers only
    A  = np.column_stack([np.ones(m.sum()), x[m]])
    H  = A.T @ A                                       # the 2x2 normal matrix
    return -delta * np.linalg.solve(H, np.array([1.0, x[k]]))

print(frozen_offset(X, Y, 6, 1.0))   # [-0.092417  0.003081]
# Check: inliers alone fit (1.156607, 1.986645); IRLS on all 14 with
# delta=1 converges to (1.064190, 1.989726). The difference is exactly the
# two numbers above, and it is LINEAR in delta -- halve delta, halve both.

def basin_gap(x, y, c, k):
    """Non-convexity has no internal tell, so buy one for a second solve.

    Start A: the Huber fit -- what any sane pipeline would hand you.
    Start B: the exact line through point k and its neighbour -- what a
    CONTAMINATED minimal sample would hand you. If the two converge to
    different costs, c is tight enough to have carved the outliers their
    own minimum. Note what this does NOT prove: with three or more basins,
    two starts give a lower bound on the global minimum, not a certificate."""
    th_a = irls_cauchy(x, y, c, init=huber_fit(x, y, 1.0))
    b    = (y[k + 1] - y[k]) / (x[k + 1] - x[k])
    th_b = irls_cauchy(x, y, c, init=np.array([y[k] - b * x[k], b]))
    Ja, Jb = cauchy_cost(th_a, c), cauchy_cost(th_b, c)
    return abs(Ja - Jb) / min(Ja, Jb), th_a, th_b

# Bisect for the width at which the second basin appears -- 20 lines and
# it converts "Cauchy is non-convex, be careful" into a number you own.
lo, hi = 0.2, 1.5
for _ in range(50):
    mid = 0.5 * (lo + hi)
    lo, hi = (mid, hi) if basin_gap(X, Y, mid, 6)[0] > 1e-3 else (lo, mid)
print(lo)        # 0.5364 -- safe above, two basins below

All three functions are on the canvas right now. The share arms are the bars (first arm only — that is the instrument's stated limitation). The frozen offset is why the teal line stops moving. The basin gap is the coloured number in the readout, recomputed on every frame from two full IRLS solves, which is affordable precisely because an IRLS solve is a fixed number of 2×2 accumulations. If you can write these three functions from memory you can rebuild every claim in this chapter from the data, which is a materially different position from remembering that the claims were made.

The sixty-second whiteboard version

You will not always have this bench. Sometimes you have a marker and a colleague to convince. Here is the same demonstration in six strokes, in the order to draw them:

1. Draw six dots roughly on a line, and one dot well below it. Say: "one bad association in seven."
2. Draw the least-squares line, visibly tilted toward the bad dot. Say: "the fit is a compromise nobody asked for."
3. Draw a vertical stick from each dot to the line. Point at the good point with the biggest stick. Say: "and now the largest residual is on a good measurement — that is masking, and it is why 'delete the biggest residual' deletes the wrong one."
4. To the right, draw axes and the line ψ = r. Say: "the pull on the solution is the derivative of the cost. For a quadratic, the derivative is the residual, so a residual ten times bigger pulls ten times harder, with no ceiling."
5. Draw the flat top of Huber on the same axes. Say: "bounded influence — capped at δ, but still pulling forever."
6. Draw Cauchy coming back down to zero. Say: "redescending — a gross outlier is ignored, at the price of a non-convex cost that needs a good initialisation."

Six strokes, ninety seconds, and you have covered the mechanism, the trap, and the tradeoff. The right-hand plot is the one that earns the follow-up question, because it is the one almost nobody draws.

Two failure modes with the metric you would actually alarm on

"The teal line looks jittery" and "the Cauchy fit lurches" are observations you can make on a fourteen-point toy with your eyes. Neither survives contact with a robot, where the fit is invisible, the data is a bag replay from a customer site, and the only thing you have is whatever you thought to log. A failure mode without a logged metric and a threshold is not a failure mode you can find. So here are both of them written the way an on-call runbook writes them: symptom → metric → healthy reading → broken reading.

Failure mode 1 — δ is too tight (Experiment 5). The symptom is a robust fit that is noisier than plain least squares on data with no outliers in it, which sounds absurd until you realise you are throwing away good measurements. The metric is the one quantity IRLS hands you for free every sweep: the mean applied weight on data you have independent reason to believe is clean, ∑w_i/N. Log it per solve; it costs one accumulator.

Here is that metric, alongside the thing it is a proxy for — the standard deviation of the fitted slope over 400 independent noise draws. The last column is the classical efficiency: the variance least squares achieves divided by the variance you achieved, which is exactly the quantity the 95%-efficiency tuning constant from Chapter 3 is named after.

Two of these columns are single-solve measurements and four are ensemble measurements, and the table says which is which, because conflating them is the exact mistake this section is about. Columns 2 and 3 are the bench's own clean dataset — the fourteen points on your screen, the numbers Experiment 5 quotes, reproducible by pressing Reset and moving the δ slider. Columns 4 to 7 are averages over 400 fresh noise draws of the same generator. A metric you alarm on is always the second kind.

Three things to take from that table, all of them quotable. First, the tuning constant is validated, not asserted: δ = 1.345σ buys 96.7% efficiency here, against the textbook asymptotic figure of 95% — and the small excess is honest, because 95% is the asymptotic result for a location estimate and this is fourteen points and two parameters. When someone asks "where does 1.345 come from and does it actually work?", that is the answer with a measurement attached.

Second, the healthy and broken readings are separated by a threshold you can write down. Healthy: mean weight > 0.95 on believed-clean data, which corresponds to a variance penalty under 4%. Broken: mean weight 0.789 at δ = 0.20 (the 400-draw figure; this particular dataset reads 0.677), with two thirds of the good points down-weighted and 22% more variance than doing nothing at all. Alarm at mean_weight < 0.9 and you catch it, because 0.9 sits in the gap between the 0.923 you get at a merely aggressive δ and the 0.789 you get at a broken one.

And now break that threshold, because as stated it does not survive contact with a single solve. Every number in the ensemble column is a mean, and a mean tells you nothing about whether one reading crosses a line. The bench's own dataset already proves the point: at δ = 0.35 the ensemble says 0.9227, comfortably healthy, but this dataset reads 0.8869 — already below the 0.9 alarm, on data with no outlier in it at all. Press Reset, set δ = 0.35, and you are looking at a false alarm.

So measure the spread, not just the mean. Here is the same sweep re-run at 4,000 draws — the means land within 0.004 of the 400-draw column above, so nothing has changed except that the tails are now resolved — reporting what fraction of individual 14-point solves would trip an alarm at 0.9:

Read the middle rows. On one 14-point solve the 0.9 alarm has a 28.9% false-alarm rate at a δ that is fine, and a 3.6% miss rate at a δ that is broken. Ship that and you will either be paged every third keyframe or you will teach the team to ignore the page, which is worse. The metric was never wrong; the sampling was. Average it over a window of solves first and the same threshold becomes usable: at 20 solves the false-alarm rate at δ = 0.35 falls to 1.0% and detection at δ ≤ 0.25 is 100%. The window shrinks the standard error by √M — 0.0414/√20 = 0.0093 — so the healthy mean 0.9209 sits (0.9209 − 0.9)/0.0093 = 2.3 standard errors above the line, and the broken mean 0.7850 sits (0.9 − 0.7850)/(0.0658/√20) = 7.8 standard errors below it. Those two numbers are the design, and note they are not symmetric: the alarm is deliberately placed close to the healthy side and far from the broken side, because missing a broken kernel costs you a silently degraded estimate for as long as it runs, while a 1% false-alarm rate costs you one glance at a dashboard.

That is the general lesson, and it transfers to every metric in this chapter. A threshold is not defined by the gap between two means. It is defined by the gap between two means measured in standard errors of the statistic you actually alarm on, and if the statistic is one small solve you may not have any gap at all. The fix is almost always free: keep a ring buffer, alarm on the window.

python
# Failure mode 1, instrumented the way you would actually ship it.
# One accumulator in the solver; one ring buffer in the monitor.
import collections

class WeightMonitor:
    """Alarms on the WINDOWED mean IRLS weight, never on a single solve.

    On 14-residual solves the per-solve statistic has sd ~ 0.04, so a 0.9
    threshold false-alarms on 29% of perfectly healthy frames. Averaging 20
    solves divides that sd by sqrt(20) = 4.5 and the false-alarm rate goes
    to 1%. WINDOW is therefore not a smoothing nicety -- it is the parameter
    that makes the threshold mean anything."""
    WINDOW = 20
    LO = 0.90      # below: delta too tight, you are discarding good data
    HI = 0.9999    # pinned at 1: delta too loose, kernel is inert

    def __init__(self):
        self.buf = collections.deque(maxlen=self.WINDOW)

    def push(self, w):                 # w = the final sweep's weight vector
        self.buf.append(float(w.mean()))
        if len(self.buf) < self.WINDOW:
            return None                  # never judge on a partial window
        m = sum(self.buf) / self.WINDOW
        if m < self.LO:
            return ("kernel too tight", m)  # failure mode 2 of Chapter 3
        if m > self.HI:
            return ("kernel inert", m)      # failure mode 1: plain LS with extra steps
        return None

Two details in that class are the ones worth defending out loud. WINDOW = 20 is not a taste parameter — it is the smallest window for which the healthy mean clears the threshold by more than two standard errors (2.3) while the broken mean misses it by nearly eight, and if your residual count per solve were 400 instead of 14 the per-solve sd would be roughly √(400/14) ≈ 5× smaller and you could shrink the window to one or two solves. The window length is a function of how many residuals each solve has, and being able to say that sentence is the difference between choosing 20 and guessing 20. And the upper bound matters as much as the lower one, which is the third reading of this table.

Third, and this is the one that catches people out on this very bench: at the default δ = 1.0 the mean weight is 1.0000. Not 0.99 — exactly 1. The largest clean residual in the dataset is 0.839, so nothing ever crosses δ and the kernel is completely inert. That is why Experiment 1 shows all three lines on top of each other, and it is also the diagnostic for the opposite failure: a mean weight pinned at exactly 1.0 on data you know contains outliers means your δ is too loose and you are running plain least squares with extra steps. The metric catches both ends. Chapter 3's failure mode 1 and failure mode 2 are the same number read in two directions.

Failure mode 2 — a redescending kernel in a second basin (Experiment 3). The symptom is a fit that is confidently wrong: the solver converges, the reported cost is small, the covariance looks fine, and the answer is nonsense. Nothing internal to a single solve reveals it, because a local minimum satisfies every convergence test a local minimum is asked to satisfy. The metric therefore has to come from outside the solve: run it twice from two different initialisations and compare the final costs.

python
# Non-convexity has no internal tell. Buy one for the price of a second solve.
th_a = irls_cauchy(X, Y, c, init=ols_fit(X, Y))          # cheap, sensible start
th_b = irls_cauchy(X, Y, c, init=minimal_sample_fit(X, Y))  # independent start

J_a, J_b = cauchy_cost(th_a, c), cauchy_cost(th_b, c)
basin_gap = abs(J_a - J_b) / min(J_a, J_b)

if basin_gap > 1e-3:
    # Two basins. Keep the lower-cost answer, and log it: this is the
    # signal that c is too tight for your data, NOT that the solver broke.
    log.warn("cauchy multi-basin: J=%.4f vs %.4f, dslope=%.3f",
             J_a, J_b, abs(th_a[1] - th_b[1]))

This exact metric is running live under the canvas. The bench computes both solves on every frame — Cauchy from the Huber fit and Cauchy from the outlier line — and prints both costs and the basin_gap between them in the readout, green under 10⁻³ and red above it. Move the c slider and watch it flip. You are not reading a table about a diagnostic; you are driving the diagnostic.

Run it against sensible pairs of starts and it reports clean — OLS versus Huber versus zero versus truth all give basin_gap under 10⁻¹⁴ at every c from 0.2 to 1.5, which is the honest finding from Experiment 3. Point the second start at the outlier and it fires immediately:

Notice what basin_gap does not do: it does not grow monotonically as c tightens. It peaks at 6.92 near c = 0.4 and then falls back to 4.42 at c = 0.2, because both costs are shrinking as the kernel narrows and the ratio is not a stable ordering of severity. Use it as a detector, never as a severity score. The quantity that does track severity is the last column, Δslope, which climbs to 16.6 — and slope error is the thing your downstream consumer feels. If you log one number, log the parameter disagreement; if you alarm on one number, alarm on the cost gap.

The same button works on the leverage dataset, and the numbers there are worth a look because the second basin is wider. Press Outlier at the far end, turn the cold start on, and the two-point start is now the line through the last two points, slope −17.24. That basin survives all the way to c = 0.6 — at c = 0.6 the warm solve reports J = 1.9458 and the cold one J = 9.6609 — where on the mid-outlier dataset c = 0.6 was already safe. A high-leverage outlier does not just corrupt the answer; it enlarges the region of kernel widths in which a bad initialisation can trap you. That is a second, independent reason leverage points deserve a consensus stage in front of the optimiser rather than a tighter kernel behind it.

The tell to carry away, and it is transferable: a robust solve that converges cleanly but whose cost depends on where you started it is not a solver bug, it is a tuning bug — c is tight enough to have carved the outliers their own minimum. The fix is not a different solver or a tighter tolerance; it is either a wider c, or a consensus initialisation, or graduated non-convexity, which is precisely the idea of starting at a c so wide the cost is convex and annealing it down while tracking the minimum. And the second solve that buys you the whole diagnostic costs one extra IRLS run — on the 400-residual problem sized in the next section, under 60 microseconds of arithmetic.

One honest caveat on the detector, and it should be named up front: two starts detect two basins, and this cost has at least three. A restart policy that samples k initialisations gives you the best of the k you happened to try, and nothing more — it is a lower bound on the global minimum with no certificate attached. That is precisely the gap graduated non-convexity closes: instead of sampling basins it removes them, by beginning at a width where the surrogate cost provably has one minimum and annealing down while the minimiser tracks it continuously. The bench's c = 1.0 row is the first step of that schedule, and the c = 0.5364 boundary is the point below which the schedule stops being optional.

What changes at 400 residuals instead of 14

The bench is deliberately small so that a single point can dominate visibly. Production problems are usually larger, and the fair question is whether any of this still matters. It does, but differently:

The shapes, the rate, and the budget

"400 residuals" is still a count, and counts do not decide architectures. The sentence "I would ship Huber because the solve time is bounded" is the central design claim of this chapter, and up to this point it has been an assertion with no number behind it. Here is the number.

Make the problem concrete: a monocular VIO front end, keyframe rate 20 Hz, 400 tracked landmarks, refining the current camera pose only (motion-only bundle adjustment — the map points are held fixed, which is what a front end does). Then:

At 20 Hz the whole frame budget is 50 ms. Detection, tracking and matching eat most of it; a realistic allocation gives the optimiser about 10 ms. Now count what one IRLS sweep costs, which is the entire point — it is a fixed count, computable in advance:

At a conservative 2 GMAC/s on one embedded core that is about 58 µs of arithmetic, plus the 4 000 residual-and-Jacobian evaluations that dominate the real cost (each a projection and a 2×6 analytic Jacobian). Call it a few hundred microseconds wall clock. Against a 10 ms allocation that is a margin of roughly 30×, and — this is the part that matters — the margin does not depend on the data. k sweeps of a fixed-size accumulation is a worst-case execution time you can put in a table and defend to a safety reviewer. That is what "bounded" means; it was never about the constant.

Now price the alternative on the same problem. RANSAC's iteration count is the Fischler–Bolles formula N = log(1 − p) / log(1 − w^s), with p = 0.99 confidence, w the inlier ratio and s the minimal-sample size — s = 2 for a line, s = 3 for P3P pose. Every iteration also needs a consensus pass over all 400 landmarks:

That table is the design argument, and it is not the one people expect. RANSAC is not "slower" — at 90% inliers it is cheaper than the IRLS loop. The problem is the shape of the curve: N depends on w, w is a property of the scene, and you do not know w until you have run the thing. Cost blows up by 115× exactly when matching degrades — motion blur, low texture, a wet road at night, a repeated facade — which is precisely the frame where you least want the optimiser to overrun and drop a control cycle. A cost that is cheap on the easy frames and unbounded on the hard ones is the wrong shape for a fixed-rate loop, no matter how good its average is.

What the bench does not show you, and should

Be honest about the demo's limits, because they are real:

• It is one-dimensional and linear. A real reprojection residual is 2-D, and the model is nonlinear in the pose, so each Gauss–Newton step is a linearisation and the "fit" is a local one. The influence-function story survives intact — it is a statement about the cost, not the model — but the convergence story gets harder.
• The scale is fixed by a slider, not estimated. In production δ comes from a robust scale estimate that is itself computed from contaminated residuals, which is where masking enters. The bench deliberately removes that loop so you can see the kernel in isolation.
• Fourteen points is a small sample. With 400 reprojection residuals in a keyframe, a single outlier holds far less of the total pull, and Huber is usually enough. The regimes where this demo's drama is realistic are the ones with few, high-leverage measurements — loop closures, GPS fixes, plane fits on sparse LiDAR returns. Those are exactly the places robust costs matter most.
• There is no gate. A real pipeline would have chi-square gated most of these outliers before the optimiser ever saw them. The kernel is the second line of defence, not the first.

Where this came from, and where it went

Every idea you just dragged around on that canvas has an author and a date, and naming them is worth doing for a reason beyond politeness: it keeps clear which parts are settled and which parts are still moving. Four of them are load-bearing in this chapter alone.

Two more worth knowing because they answer the obvious follow-up, "and how do you pick δ and c on real data?" — the question the bench dodges with a slider. Barron, "A General and Adaptive Robust Loss Function" (CVPR 2019), builds a one-parameter family whose shape parameter α interpolates L2, Huber-like, Cauchy and Geman–McClure, and — the actual contribution — derives the normalising constant that makes it a proper probability distribution, so α and the scale can be learned by gradient descent alongside the model instead of tuned. Chebrolu, Läbe, Vysotska, Behley & Stachniss, "Adaptive Robust Kernels for Non-Linear Least Squares Problems" (IEEE RA-L 6(2), 2021), takes that family into the robotics setting and estimates the shape per-problem via expectation-maximisation inside the solver, which is the version you would actually wire into GTSAM.

Practice: ten minutes, closed book, pen only

Reading this chapter is not the same as being able to produce it under a whiteboard marker while someone waits. Close the page. Set a timer for ten minutes. Every answer below is a number you can check against something earlier in this chapter, so there is no ambiguity about whether you got it.

If you got eight of eleven inside ten minutes, this topic is yours. The ones people miss are 4, 8, 9 and 10: drill 4 because the visualisation actively trains the wrong intuition, drill 8 because "run it twice" feels too cheap to be the professional answer and is, and drills 9 and 10 because everyone has been taught "L2 influence is unbounded" as a slogan and almost nobody has watched what the share and the fit actually do while the residual runs away.

Chapter 5: The Field Guide

Here is the question this whole lesson has been building toward: "Our AMR reports two-centimetre localisation uncertainty and it clips a rack about once a week. Where do you look first?" There is no partial credit for knowing what a covariance is. You need the whole chain loaded — what the number claims, what would prove it false, what it costs to check, and what you would change on Monday — and you need it in one breath.

Everything below is that load-out, in the order you will need it. Read section 1 first; do the derivations in section 1b out loud until they are muscle memory; run section 3 with a timer; keep section 4 for the day a bug like these lands on you; use section 7 whenever you need the numbers back at your fingertips.

1 · The cheat sheet

1b · Derive it cold — four things you must never merely recall

A cheat sheet you can only recite is a liability, because the follow-up to every row above is "where does that come from?" and a memorised constant has no follow-up. Each derivation below takes under a minute at a whiteboard. Rehearse them until you can talk and write at the same time; that is the actual skill being tested.

(a) Why the 2-D 95% ellipse is 2.448σ and not 1.96σ. Start from the Gaussian density. Every point on a constant-density contour has the same exponent, so the contour is the set

which is an ellipse. Now whiten: define u = Σ^−½(x − μ). The condition becomes u^Tu = c — a circle of radius √c — and u is standard normal with independent components. So c is a sum of m squared standard normals, which is the definition of a chi-square with m degrees of freedom. That is the entire link between "ellipse" and "chi-square", and it is two lines.

For m = 2 the chi-square CDF has a closed form you can quote: P(χ²₂ ≤ c) = 1 − e^−c/2. Set it to 0.95:

And now the punchline, which is the part that actually matters. 1.96 is the one-dimensional number, from 2(1 − Φ(1.96)) = 0.05. Feed it into the 2-D formula and see what you really drew: c = 1.96² = 3.8416, so the enclosed mass is 1 − e^−1.9208 = 1 − 0.1465 = 0.854. An engineer who plots a "95% ellipse" at 1.96σ is plotting an 85.4% ellipse and labelling it 95%. On a planner that inflates obstacles by that contour, one pass in seven is outside the region you told it was safe.

Read the third column down: the "number of sigmas" that buys you 95% grows with dimension, because you are asking a larger set of directions to all behave at once. The two numbers you actually need at your fingertips are 5.991 (2-D gate) and 12.592 (6-DOF pose gate); everything else you can regenerate from −2 ln(1−p) when m = 2, or admit you would look up.

(b) Why Σ_y = JΣJ^T, in three lines and one honest caveat. Let y = f(x) and expand about the mean x̄:

Then δy = y − ȳ ≈ J δx, and

The whole proof is the third equality: J is evaluated at the fixed point x̄, so it is a constant matrix and comes outside the expectation. Say that sentence out loud — it is what distinguishes someone who derived the formula from someone who copied it, and it immediately tells you the failure mode: the instant J is re-evaluated at a random point (an iterated EKF, a relinearisation), that step is no longer free.

The caveat is the term you threw away, ½ δx^TH δx. Two consequences, and you should volunteer both. First, the neglected curvature matters only when f bends appreciably over one sigma — make it concrete: for polar→Cartesian at r = 10 m with σ_θ = 5 mrad, the arc sags below its chord by r(1 − cos σ_θ) = 10 × 1.25×10⁻⁵ = 0.125 mm, against a 5 cm cross-range spread — 0.25%, so linearise without apology. Second, the surviving second-order term is a bias: E[y] ≈ f(x̄) + ½ tr(HΣ). Averaging more measurements shrinks variance as 1/N and does nothing at all to that bias, which is why a bad linearisation shows up as a confident wrong answer rather than a noisy one.

(c) Why "least squares" has squares in it at all. Take z = Hx + v with v ~ N(0, R). The likelihood is

Maximising a likelihood is minimising its negative logarithm, and the logarithm of an exponential of a quadratic is a quadratic. The squares come from the Gaussian's exponent and from nowhere else — there is no separate "principle of least squares" to appeal to. Differentiate and set to zero:

The immediate corollary is worth having ready: change the noise model and you change the cost. Laplace noise gives you L1 and the median; a Gaussian core with heavy tails gives you Huber. "Robust cost" is not a hack bolted onto least squares — it is least squares under a different, more honest likelihood.

(d) Why MAP is the Kalman update — with a two-line numerical check. Multiply the likelihood by a Gaussian prior p(x) ∝ exp(−½(x − x̄)^TP⁻¹(x − x̄)). Negative logs add, so the cost gains one more quadratic, and the stationary point becomes

Two independent quadratics added means two information matrices added — Λ = ∑_i Λ_i. That additivity is the whole reason factor graphs exist: a new measurement is a new term, never a rebuild.

Worked example 1 — MAP and Kalman agree, every digit. Scalar state, H = 1. Prior x̄ = 0 with σ₀ = 1, so P = 1. One measurement z = 4 with σ = 0.5, so R = 0.25. Information form first:

Now the Kalman route, which must give the same answer or one of us is wrong:

2 · System-design patterns

Prompt A — "Design the uncertainty story for a warehouse AMR's localisation stack."

• Frame it by consumer first. "Who reads the covariance?" The planner inflates obstacles by 3σ; the manipulation gate refuses to reach when trace(P) exceeds a threshold; the fleet dashboard logs it. That makes P a safety-relevant output, not diagnostics.
• Then sources. IMU noise density from a twelve-hour Allan-variance log, not the datasheet. LiDAR σ_range and σ_bearing from the datasheet, propagated to Cartesian with the Jacobian. Wheel odometry empirically, because slip is on no datasheet.
• Then the pipeline, with the arithmetic done in the open. Every arrow in a design carries a rate, a message type and a byte count; do the multiplication explicitly rather than quoting a round number you cannot defend.
  Worked example 2 — the bandwidth budget, digit by digit.
  Propagate at IMU rate: 200 Hz, 15-state (position, velocity, attitude, accel bias, gyro bias — 3 each). P is 15 × 15 = 225 doubles → 225 × 8 B = 1,800 B = 1.8 kB. If you published P every tick that would be 1,800 × 200 = 360,000 B/s = 360 kB/s on a topic nobody consumes at that rate — which is precisely why you do not.
  Gate at scan rate: 10 Hz, innovations are 2-D (range, bearing in the ground plane), so S is 2 × 2 and you accept while d² < 5.991 (derived in 1a, not memorised).
  Publish at 50 Hz. nav_msgs/Odometry carries two covariance blocks — pose and twist — each 6 × 6 = 36 doubles: 2 × 36 = 72 doubles × 8 B = 576 B per message, and 576 × 50 = 28,800 B/s = 28.8 kB/s. If you only ever fill the pose block, halve it: 36 × 8 = 288 B and 288 × 50 = 14,400 B/s = 14.4 kB/s.
  Know which half you meant. The fastest way to sink your own credibility is to say "288 bytes at 50 Hz, so 28.8 kB/s" — anyone listening multiplies, gets 14.4, and now every other number you say is suspect.
• Then the guardrail. Average NIS over a fifty-sample window, published as a diagnostic topic, alarmed outside its chi-square band, and asserted in CI against three recorded bags. Size the window explicitly: the average of N draws from χ²_m has mean m and standard deviation √(2m/N), so at m = 2 and N = 50 that is √(4/50) = 0.283, and a ±3σ alarm band is 2 ± 0.85, i.e. [1.15, 2.85]. A filter reporting an average NIS of 400 is not near that band; it is 1,400 standard deviations outside it. "The estimator's own honesty becomes a tested property, with a threshold I can derive rather than tune."
• Close with the failure you are protecting against, and price it.
  Worked example 3 — what 20× overconfidence actually costs, both tails computed.
  Suppose the true lateral error is Gaussian with σ_true = 0.40 m while the filter reports σ_rep = 0.02 m. That is 20× too tight in standard deviation, 400× in variance.
  Honest stack. The planner clears racks by 3σ = 3 × 0.40 = 1.20 m. The chance of exceeding that on a pass is P(|e| > 3σ) = 2(1 − Φ(3)) = 2 × 0.001350 = 2.7 × 10⁻³ — about one pass in 370. Do the fleet arithmetic too: at 500 aisle passes a shift that is 500 × 0.0027 = 1.4 margin breaches per shift, which is exactly why a real fleet clears by more than 3σ and keeps a reactive layer underneath the planner. Quoting 3σ as if it were safe is its own small tell.
  Overconfident stack. Same planner, same rule, but it inflates by 3 × 0.02 = 0.06 m. Measured in true sigmas that clearance is z = 0.06 / 0.40 = 0.15, so P(|e| > 0.06 m) = 2(1 − Φ(0.15)) = 2 × 0.4404 = 0.88.
  Nine passes in ten are outside the margin the planner believed was safe. The rack is not hit nine times in ten because a 6 cm excursion mostly still fits inside the physical aisle — but the safety argument is void on 88% of passes, and there is no evidence in any log that it happened.
  Reconcile this with Chapter 0, because the two computations look contradictory at first glance. Chapter 0 asked the same question against a fixed 0.25 m aisle clearance rather than a 3σ inflation: claimed risk 2Φ(−0.25/0.02) = 2Φ(−12.5) = 7.5 × 10⁻³⁶, true risk 2Φ(−0.25/0.40) = 2Φ(−0.625) = 0.53 — one pass in two. Same lie about σ, different definition of the margin, and the answers differ by thirty orders of magnitude. That sensitivity is the lesson: because the Gaussian tail is exponential in σ, a linear error in the covariance becomes an astronomically nonlinear error in risk, and the size of the blow-up depends on where the margin sits. Know which margin your planner actually uses before you quote a number.
  The sentence to land: "The same planner, the same hardware, the same code path. One over-optimistic number in a YAML file moved the fraction of passes with no valid clearance argument from 0.27% to 88%, and nothing in the system reported it. That is the bug this whole design exists to make visible."

Prompt B — "We are moving from an EKF to a factor graph. Convince me it is worth the rewrite."

• Concede first. The statistics are identical for a linear-Gaussian model; a Kalman filter is exactly MAP on a chain-structured factor graph with every past state marginalised out. Leading with that buys you credibility for the rest.
• Then the two real arguments. Relinearisation: a filter linearises once and lives with it — and by part (b) of section 1b, the discarded second-order term is a bias, so no amount of subsequent data averages it away; a smoother revisits past states when new evidence arrives and re-evaluates J there. Sparsity: a filter carries a dense n×n covariance, and you should size it explicitly rather than saying "it gets big".
  Worked example 4 — why the dense filter dies at 500 landmarks.
  State: 500 landmarks × 3 coordinates = 1,500, plus the 15-state nav block → n = 1,515.
  Storage: P has 1,515² = 2,295,225 entries × 8 B = 18,361,800 B = 18.4 MB. That is one copy; the naive update needs two or three live at once, so budget ~50 MB of L2-hostile traffic per step.
  Compute: the covariance propagation F P F^T is two dense n×n matrix products, O(n³) ≈ 1,515³ = 3.5 × 10⁹ multiply–adds per step. At a generous 10 GFLOP/s on an embedded core that is > 0.3 s per propagation step — on a loop you wanted to run at 200 Hz, i.e. 5 ms. You are 70× over budget, and the number grows as the cube.
  The graph's information matrix for the same problem has O(n) non-zeros, because a landmark is only connected to the handful of keyframes that saw it.
• Give the number, and show the cube. Schur-complement elimination on a ten-keyframe, 200-landmark window: poses are 10 × 15 = 150 variables, landmarks are 200 × 3 = 600, so the full Hessian is 750 × 750. Eliminate the landmark block first and you are left with a 150 × 150 solve. Dense factorisation costs about n³/3, so the ratio is 750³/150³ = 5³ = 125× fewer flops — 1.4 × 10⁸ against 1.1 × 10⁶. Then be honest about what the ratio hides: forming the complement is not free, but the landmark block is block-diagonal in 3×3 blocks (a landmark shares no factor with another landmark), so inverting it is 200 independent 3×3 inversions — linear in landmark count, and genuinely negligible. Naming why the block is diagonal is the part that shows you have implemented one.
• Name the cost honestly. You now own an elimination ordering, a marginalisation policy, and a subtler class of bug — a robust kernel on the marginalisation prior produces silent amnesia that no other diagnostic shows. Budget for that.

Prompt C — "Would you let a neural network output your measurement covariance?"

• Yes, with three guard rails. Output a Cholesky factor so the result is positive definite by construction; clamp the eigenvalues to a physically derived range so a hallucination cannot claim micrometre precision; keep NIS monitoring live so out-of-distribution drift is caught within about fifty samples.
• Say why it is the safe place to put learning. The estimator stays classical, so the residual is still inspectable and the failure still debuggable. Only the one component nobody hand-tunes well becomes learned.
• Cite the precedent. Brossard, Barrau & Bonnabel, AI-IMU Dead-Reckoning (IEEE T-IV 2020), reaches roughly 1% translational drift on KITTI with an IMU alone by learning exactly this and nothing else.
• Name the fallback. When the consistency monitor trips, revert to the hand-tuned prior and log the window for offline triage. A learned component you cannot fall back from is not shippable.

Prompt D — "Our loop closures occasionally weld two aisles together. Design the defence."

• Layer it. Appearance retrieval → geometric verification with RANSAC → chi-square gate on the relative pose (6 DOF, 12.592 at 95%) → robust kernel on the surviving loop factor → a group-consistency check across candidate closures.
• Explain why the last layer exists. A single false closure can be perfectly self-consistent geometrically. A group check asks whether a set of closures agrees with each other, which a lone impostor cannot fake.
• Give the asymmetry. A missed loop closure costs you drift you would have had anyway; a false one corrupts the map irreversibly. Tune the whole chain conservatively and accept the recall loss.
• Cite the mechanism. Sünderhauf & Protzel's switchable constraints (IROS 2012) make the accept/reject decision a variable the optimiser owns, and Agarwal et al.'s dynamic covariance scaling (ICRA 2013) shows that switch is algebraically a robust weight.

3 · Coding drills

A drill is not finished when the code is right. It is finished when you can state, without looking, the shape and dtype of every array on every line, and hand-evaluate one row of the output. That is what the follow-ups are made of, so every drill below ships a shapes header and one fully worked instantiation.

Drill 1 — "Gate a batch of innovations." The key lines, and what to narrate while typing:

python
# shapes: S (2,2) float64 innovation covariance, symmetric positive definite
#         Y (N,2) float64 stacked innovations, one per row, N = 300
L  = np.linalg.cholesky(S)             # L: (2,2) lower, S = L @ L.T -- factor once, outside the loop
Wt = np.linalg.solve(L, Y.T)           # Y.T: (2,300) -> Wt: (2,300)  "whiten -- this is why it is Mahalanobis"
d2 = np.einsum('ij,ij->j', Wt, Wt)     # d2: (300,) float64 -- column norms, no temporaries
keep = d2 < 5.991                      # keep: (300,) bool -- chi-square, 2 DOF, 95%

Worked example 5 — hand-evaluate one column before you claim it works. Take a lidar innovation covariance of σ = 20 cm along track and 10 cm across, uncorrelated:

For one innovation y = [0.30, 0.05] the whitened vector is the triangular solve L u = y, which here is just an elementwise divide: u = [0.30/0.2, 0.05/0.1] = [1.5, 0.5]. Then

Say what 2.50 means: √2.50 = 1.58, so that measurement is 1.58 sigma out in the whitened metric — comfortably inside the gate even though its along-track component is 30 cm, which sounds alarming in metres and is only 1.5σ in the units that matter. Now push it to y = [0.60, 0.05]: u = [3.0, 0.5], d² = 9.00 + 0.25 = 9.25 > 5.991, rejected. Being able to move one number and flip the decision, live, is the whole point of the drill.

What this drill is really checking: that you did not call np.linalg.inv(S); that 5.991 is a chi-square quantile you can derive (section 1b(a)) and not a magic number; and that the factorisation is hoisted out of the loop.

Expect these three follow-ups, in this order.

• "What shape is Y?" — the highest-value question in the drill, and the reason the header comment exists. Y is (N, 2): innovations stacked as rows, so Y.T is (2, N) and the solve treats each innovation as a column of the right-hand side. If Y arrives as (2, N) instead, Y.T is (N, 2) and np.linalg.solve raises a shape error for every N ≠ 2 — and silently returns garbage when N = 2, which is exactly the size of the fixture in your unit test. Assert Y.shape[1] == S.shape[0] at the top of the function and say so unprompted.
• "What if S is not positive definite?" — the Cholesky raises LinAlgError, and that is a feature, not a nuisance to be caught and swallowed. It means the covariance has lost positive definiteness and you want to know at that instant, not forty minutes into the mission when it finally trips something else (see debug row 2).
• "Why einsum and not (Wt**2).sum(0)?" — be honest: at N = 300 they are indistinguishable; einsum avoids materialising the (2, N) squared temporary, which starts to matter at N in the tens of thousands, e.g. a full lidar sweep gated at once. Do not oversell a micro-optimisation — claiming a 300-element speedup is a worse signal than admitting it is a wash.

Drill 2 — "Weighted least squares with a prior, and no matrix inverse."

python
# shapes: H (N,p) design, z (N,) measurements, sig (N,) per-measurement sigma
#         x0 (p,) prior mean, sig0 (p,) prior sigma.  N = 4 rows, p = 2 params
A = np.vstack([H / sig[:, None], np.diag(1.0 / sig0)])   # A: (N+p, p) = (6,2) -- prior = extra rows
b = np.concatenate([z / sig, x0 / sig0])                 # b: (N+p,) = (6,)
Q, Rr = np.linalg.qr(A)                                  # Q: (6,2), Rr: (2,2) -- kappa(H), not kappa(H)^2
x = solve_triangular(Rr, Q.T @ b)                        # Q.T @ b: (2,) -> x: (2,) float64

Worked example 6 — make "the prior is worth k measurements" a number. Fit a line, so p = 2, from N = 4 points all with σ = 0.1 m. Start with a deliberately weak prior x₀ = [0, 0], σ₀ = [10, 10]. The four measurement rows each carry weight 1/0.1 = 10; the two prior rows carry 1/10 = 0.1. What lands on the intercept diagonal of A^TA is:

The prior is a 1-in-40,000 nudge — that is what "weak prior" means numerically, and it is a far better answer than "it barely matters". Now tighten σ₀ to 0.05: the prior row carries 1/0.05 = 20, contributing 20² = 400, exactly matching all four measurements combined. At σ₀ = 0.05 the prior is worth four measurements, and you can say that in a sentence with the arithmetic behind it. The general rule falls straight out: a prior of σ₀ is worth (σ/σ₀)² measurements of noise σ. Converting a knob into a count is the kind of sentence people remember.

What to say while typing: "A prior is just extra pseudo-measurements — stacking the square-root information as rows is algebraically identical to adding P⁻¹ to the normal equations (section 1b(d)), and it keeps the condition number at κ(H) instead of squaring it."

Put a number on the conditioning too, because "squaring the condition number" is a phrase people repeat without pricing. Float64 has ε ≈ 2.2 × 10⁻¹⁶, so you carry about 16 decimal digits. A merely awkward Jacobian with κ(H) = 10⁴ gives normal equations at κ = 10⁸: you lose 8 digits and keep 8, which is fine. In float32 (ε ≈ 1.2 × 10⁻⁷, ~7 digits) the same problem has κ(H^TH) > 1/ε and the solve returns noise. That is the actual argument for square-root methods on a mobile GPU, and it is why the √BA paper exists.

Expect "so when would you form the normal equations anyway?" — when the problem is large and sparse, because the sparsity win is three orders of magnitude, double precision absorbs the conditioning cost, and QR on a sparse matrix suffers fill-in that Cholesky with a good ordering avoids. The honest answer names the trade rather than declaring one method better.

Drill 3 — "IRLS with a Huber weight."

python
# shapes: H (N,p) design, z (N,) measurements, th (p,) parameters
#         r (N,) residuals, s () scalar robust scale, w (N,) weights in [0,1]
for _ in range(30):
    r = z - H @ th                                     # r: (N,) float64
    s = 1.4826 * np.median(np.abs(r - np.median(r)))   # robust scale, scalar
    w = np.minimum(1.0, 1.345 * s / np.maximum(np.abs(r), 1e-12))
    th = np.linalg.solve(H.T @ (w[:, None] * H), H.T @ (w * z))   # (p,p) solve -> th: (p,)

Both constants must be defensible, because both will be asked about. 1.4826 = 1/Φ⁻¹(0.75): for clean Gaussian data the median absolute deviation converges to 0.6745σ, so dividing by 0.6745 — i.e. multiplying by 1.4826 — makes the robust scale agree with σ. 1.345 is the Huber tuning constant giving 95% asymptotic efficiency at the Gaussian: you accept a 5% larger variance than ordinary least squares on clean data in exchange for bounded influence on dirty data. Neither is a magic number; both are a stated price.

Worked example 7 — masking, computed twice so you can see it happen. Five residuals at the current iterate, one of them an outlier:

Route 1 — robust scale (what the code does). Sorted, r is (−0.20, −0.05, 0.10, 0.15, 3.00), so median(r) = 0.10. Then |r − 0.10| = (0, 0.30, 0.05, 0.15, 2.90); sorted that is (0, 0.05, 0.15, 0.30, 2.90), so MAD = 0.15. Hence

The four clean points keep full weight; the outlier is down-weighted 10×. The kernel engaged.

Route 2 — sample standard deviation (the bug). mean(r) = 3.00/5 = 0.60. Deviations (−0.50, −0.80, −0.45, −0.65, 2.40), squares (0.2500, 0.6400, 0.2025, 0.4225, 5.7600), sum 7.2750, divided by N−1 = 4 gives 1.8188, so σ̂ = 1.3486. Then δ = 1.345 × 1.3486 = 1.8139 and

What to say: "The gradient of any ρ is ∑ψ(r)h, and writing ψ(r) = w(r)·r turns it straight back into the weighted normal equations — so a robust cost reuses the WLS solver unchanged." Then, unprompted: "I would initialise from RANSAC rather than least squares, because on a masked problem an OLS start converges in the wrong basin, and I would log the weight histogram so I can prove the kernel actually engaged." That unprompted sentence is the drill — the code was never the hard part.

Drill 4 — "Propagate a covariance through a nonlinear function, and validate it."

python
# units and magnitudes -- state them before you type, or the assert is unfalsifiable:
#   r  = 10.0   m      range to the landmark
#   th = 0.5    rad    bearing (~28.6 deg)
#   sr = 0.02   m      range sigma  (2 cm, a good lidar)
#   st = 0.005  rad    bearing sigma (5 mrad, ~0.29 deg)
# shapes: Sx (2,2) = diag(sr**2, st**2) polar; J (2,2); Sy (2,2) Cartesian
J  = np.array([[np.cos(th), -r*np.sin(th)],
               [np.sin(th),  r*np.cos(th)]])          # J: (2,2) float64, evaluated at (r, th)
Sy = J @ Sx @ J.T                                     # Sy: (2,2), symmetric by construction
# validate -- this is the part people skip:
rs = r + sr*rng.standard_normal(20000)             # rs: (20000,)
ts = th + st*rng.standard_normal(20000)            # ts: (20000,)
Smc = np.cov(np.stack([rs*np.cos(ts), rs*np.sin(ts)]))   # stack: (2,20000) -> Smc: (2,2)
assert np.abs(Sy - Smc).max() / np.abs(Smc).max() < 0.05

Worked example 8 — every entry of that Jacobian and the sanity check that needs no computer. With cos(0.5) = 0.87758 and sin(0.5) = 0.47943:

Multiply out the top-left entry by hand so you can show the mechanism: Σ_y,11 = 0.8776² × 4.0×10⁻⁴ + 4.7943² × 2.5×10⁻⁵ = 3.081×10⁻⁴ + 5.746×10⁻⁴ = 8.827×10⁻⁴. Note which term dominates: the bearing contribution is nearly twice the range contribution even though σ_θ is only 5 mrad, because it arrives multiplied by r². The full result:

Now the check you can do in your head, and should say before anyone asks. A rotation cannot change two independent standard deviations, only their orientation — so the ellipse axes must come out as exactly the along-beam and cross-beam spreads:

Verify without an eigensolver, using the two rotation invariants:

Two multiplications and a subtraction proved a 2×2 eigen-decomposition. Trace and determinant are invariant under rotation, so if your propagated covariance has the right trace and determinant it has the right axes — and if it does not, you have a bug in J, not in your intuition. This check is what someone does who has debugged the code rather than read about it.

Does the assert actually hold? Quote the two error sources separately rather than hoping. Monte Carlo sampling error on a covariance entry from N samples is about √(2/N) = √(2/20000) = 1.0%, so the 5% threshold has a 5× margin against noise alone. The linearisation error is far smaller: over one bearing sigma the arc sags below its chord by r(1 − cos σ_θ) = 10 × 1.25×10⁻⁵ = 0.125 mm against the 5 cm cross-range spread, i.e. 0.25%. Run with a fixed seed and the observed relative discrepancy at N = 20,000 is about 1.5% — sampling-dominated, as predicted, and comfortably inside the assert. If you cannot say which of the two errors your tolerance is protecting against, the tolerance is a wish, not a test — and a hoped-for tolerance is exactly the kind of flaky assert that gets a CI job disabled six weeks later.

What to say: "The Jacobian carries an r in its second column, so bearing error is amplified by range — the ellipse is a sliver perpendicular to the beam with cross-range spread rσ_θ, 5 cm here against 2 cm in range." Then validate against Monte Carlo without being asked, and close on the thing that actually kills systems: the mean shift, not the covariance error. The bias is rσ_θ²/2 — 0.125 mm at 10 m, 0.75 mm at 60 m, growing linearly in range. But divide it by the cross-range spread rσ_θ and the ratio is σ_θ/2, independent of range. That inversion is worth stating explicitly, because most people assume long range is what breaks an EKF: it is not, it is coarse bearing. A 5 mrad lidar is 0.25% biased at any range; a 2° radar (35 mrad) is 1.75%; a 17° bearing estimate is 15% and the linearisation is finished. And whichever regime you are in, averaging kills variance as 1/N and does nothing whatsoever to the bias — a confident wrong answer, not a noisy one.

4 · Debugging scenarios

A debug hypothesis is only as good as the metric that separates it from its nearest decoy, with the value that distinguishes them. "I would look at the innovations" settles nothing. "I would compute average NIS; if it is near 1 the covariance is honest and my hypothesis is wrong" is a claim you can falsify. Every row below names that metric and the number.

5 · Classical vs modern — and when to use which

6 · Recommended reading

The one book. Timothy Barfoot, State Estimation for Robotics (Cambridge, 2nd edition 2024). It is the only text that carries covariance, least squares, MAP and Lie-group state together in one consistent notation, and its treatment of what "the mean" even means on a manifold is the thing most engineers are missing. If you read one thing on estimation, read the first three chapters. Two runners-up worth owning: Bar-Shalom, Li & Kirubarajan, Estimation with Applications to Tracking and Navigation (2001), which is the source of the consistency-testing material behind Chapter 0; and Huber & Ronchetti, Robust Statistics (2nd ed., 2009) for the influence-function theory behind Chapter 3.

Five papers, and why each one.

1. Barrau & Bonnabel, "The Invariant Extended Kalman Filter as a Stable Observer" (IEEE TAC, 2017). Read it for the idea that the definition of the error is a design choice, and that choosing it well makes the linearisation error state-independent. It is the deepest available fix to "the covariance lies", and it costs nothing at runtime.
2. Kaess, Johannsson, Roberts, Ila, Leonard & Dellaert, "iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree" (IJRR, 2012). Read it for why the field left filtering: the update cost depends on how much a new measurement disturbs the graph, not on the graph's size.
3. Yang, Antonante, Tzoumas & Carlone, "Graduated Non-Convexity for Robust Spatial Perception" (IEEE RA-L, 2020). Read it for the resolution of the Huber-versus-Cauchy dilemma — anneal the scale from convex to redescending and get both properties, often with no RANSAC stage at all.
4. Barron, "A General and Adaptive Robust Loss Function" (CVPR, 2019). Read it for the unification: one family containing L2, Charbonnier, Cauchy, Geman–McClure and Welsch, differentiable in its shape parameter, so robustness itself becomes learnable.
5. Demmel, Sommer, Cremers & Usenko, "Square Root Bundle Adjustment for Large-Scale Reconstruction" (CVPR, 2021). Read it for the conditioning argument made concrete: nullspace marginalisation on the square-root Jacobian delivers single-precision accuracy the Schur-complement route cannot reach.

One more if you have the time: Pineda et al., "Theseus: A Library for Differentiable Nonlinear Optimization" (NeurIPS, 2022) — the cleanest statement of what changes when the solver becomes a differentiable layer and R becomes something you learn rather than tune.

Five repositories, and exactly what to look at.

1. ceres-solver/ceres-solver — open include/ceres/loss_function.h. Every kernel in this lesson is there in about ten lines each, plus the corrector that converts ρ and ψ into the rescaled residual and Jacobian a Gauss–Newton step needs. It is the best short reference implementation of Chapter 3 anywhere.
2. borglab/gtsam — gtsam/linear/NoiseModel.h shows how a robust model composes with a base noise model, which is exactly what makes "δ is in whitened units" true rather than a convention. Then read gtsam/nonlinear/ISAM2.cpp for the incremental machinery.
3. HKUST-Aerial-Robotics/VINS-Mono — vins_estimator/src/factor/marginalization_factor.cpp. This is where "the marginalisation prior is the P⁻¹x̄ term" stops being abstract and becomes several hundred lines of real code.
4. MIT-SPARK/TEASER-plusplus — the graduated non-convexity loop and the truncated-least-squares scale solver. Small, readable, and the fastest way to understand what "certifiable" actually buys you.
5. facebookresearch/theseus — the differentiable Gauss–Newton and Levenberg–Marquardt layers plus the implicit-function-theorem backward pass. Read the backward pass; it is the part that makes a learned R possible.

7 · The numbers drill, and the rehearsal protocol

Everything above is knowledge. This section is practice — the part that decides whether the knowledge arrives on time. Fluency is mostly retrieval speed on a small set of numbers, and it is trainable.

The order-of-magnitude drill. Cover the right two columns. Answer each to one significant figure, out loud, in under five seconds. If you need a calculator for any row, you do not know that row yet.

Three of these — 5.991, 1.4826, 1.345 — are the constants most often quoted without provenance, and each has a one-line origin in the table's third column. If someone asks "where does 1.345 come from?" and you say "it is the standard value", you have just told them how you treat every other constant in your code.

The rehearsal protocol. Reading this chapter is not practising it. Three passes, in this order, and the third is the one that matters:

1. Whiteboard, silent, no notes (25 min). Reproduce the four derivations in section 1b from a blank surface: the chi-square ellipse, JΣJ^T, least squares as MLE, MAP as the Kalman update. Anything you cannot reproduce, you do not know — go back to the chapter that derives it (0, 1, 2 or 3) rather than re-reading the cheat sheet, which will only reinforce the recall you already have.
2. Timer, typing, no autocomplete (30 min). Type all four drills from section 3 into a blank file. Before running anything, write the shape and dtype of every intermediate as a comment. Then run and check the worked instantiations reproduce: d² = 2.50; the prior worth exactly four measurements at σ₀ = 0.05; the outlier weight 0.0997 robust versus 0.605 masked; trace 2.900×10⁻³ and determinant 1.00×10⁻⁶. A drill you have run is worth ten you have read.
3. Out loud, to a wall, with a stopwatch (20 min). Answer the chapter's opening question in ninety seconds: two-centimetre reported uncertainty, one rack strike a week, where do you look. Then the four system-design prompts in section 2, three minutes each, doing every multiplication audibly. This pass feels absurd and is the only one that transfers, because the failure mode under pressure is never "I had not heard of NIS" — it is "I knew it and could not assemble the sentence when it mattered."

"It is better to be roughly right than precisely wrong."
— attributed to Carveth Read, and the whole of Chapter 3 in nine words

Want to pressure-test all of this? The Studio button at the top runs a timed practice session on exactly this material.

Bridges from this lesson: Bayesian Estimation · SLAM: Factor Graphs · Loss Functions · Fisher Information & the CRLB · EKF · UKF