Debugging & Consistency — How to KNOW Your Fusion Works

Chapter 1: The Innovation — The Quantity That Tells the Truth

If we can't trust the filter's own confidence report, what can we trust? We need a quantity that the filter cannot fake — something measurable, that carries a built-in prediction of how big it should be, so we can compare what we got against what we expected. That quantity already exists inside every Kalman-family filter you've built. It is called the innovation, and learning to read it is the foundation of everything that follows.

Every recursive filter, at each step, does two things: it predicts what the next measurement should be, then it sees the actual measurement. The innovation (also called the measurement residual) is the difference between the two — the part of the measurement the filter did not see coming, the genuinely new information:

where ν (the Greek letter nu) is the innovation, z is the actual measurement we just received from the sensor, and ẑ (z-hat) is the measurement the filter predicted from its current state estimate. In a linear filter the prediction is ẑ = H·x̂, where H is the measurement matrix that maps the state estimate x̂ into the space the sensor lives in (e.g. if the state is position-and-velocity but the GPS only sees position, H picks out the position).

That word — innovation — is exact, not poetic. If the measurement were perfectly predictable, ν would be zero and the sensor told the filter nothing it didn't already know. The innovation is precisely the news: the surprise, the new content, the thing the filter must react to. The Kalman gain K multiplies this innovation to produce the correction: x̂_new = x̂ + K·ν. No innovation, no update.

The innovation comes with a prediction of its own size

Here is the magic that makes the innovation an auditing tool rather than just a number. The filter does not only produce ν; it also produces a prediction of how big ν should be — its covariance. This predicted innovation covariance is called S:

Read this as two sources of expected surprise added together. H·P·H^T is the filter's own uncertainty about the state (P), pushed forward into measurement space by H — "I'm not sure where I am, so I'm not sure what I'll measure." R is the sensor's own measurement noise — "even if I knew exactly where I am, the sensor reading would jitter by this much." Together they say: given how uncertain I am and how noisy my sensor is, my prediction error ν should have spread S.

The three signatures of a consistent filter

Here is the central theorem of filter diagnostics, and it is worth feeling why each piece must hold. If a filter is consistent — its model is correct and its covariances are honest — then its innovation sequence ν₁, ν₂, ν₃, … must have three properties:

Let's derive property 1 in one line so it doesn't feel like an assertion. The measurement is z = H·x + noise (true state x, zero-mean sensor noise). The prediction is ẑ = H·x̂. So the innovation is ν = z − ẑ = H·(x − x̂) + noise. The term (x − x̂) is the state estimation error. If the filter is unbiased, the average of (x − x̂) is zero, and the noise averages to zero, so the average of ν is zero. Bias in — bias out. A nonzero-mean innovation is a direct fingerprint of a biased estimate (a miscalibrated sensor, an unmodeled constant force, a frame offset).

Property 2 (whiteness) is the subtlest and most useful. The optimality of the Kalman filter is equivalent to its innovations being white — this is a famous result (Kailath, 1968). If today's surprise can be predicted from yesterday's surprise, then yesterday's innovation still contained information the filter did not extract, which means the filter is not optimal: its model is wrong. We'll build the whiteness test in Chapter 4. Property 3 gives us the most practical online test, NIS, which is next.

The innovation in code

Computing the innovation and its covariance is three lines — you already write them inside every Kalman update. The trick of this lesson is to log them so you can audit later:

python
import numpy as np

def innovation(z, x_hat, P, H, R):
    # z      : the measurement we just received  (vector, dim m)
    # x_hat  : current state estimate            (vector, dim n)
    # P      : current state covariance          (n x n)
    # H      : measurement matrix                (m x n)  maps state -> measurement
    # R      : sensor noise covariance           (m x m)
    z_pred = H @ x_hat                # what the filter EXPECTED to measure
    nu     = z - z_pred              # the INNOVATION: the surprise / the news
    S      = H @ P @ H.T + R         # how big the surprise SHOULD be
    return nu, S                     # log BOTH every step -> this is your audit trail

That S is the line everyone computes (it's needed for the Kalman gain anyway) and almost nobody logs. The entire consistency toolkit is built from one habit: store ν and S at every step, then test them offline or online. Everything downstream — NIS, whiteness — is a statistic of these two arrays.

Chapter 2: NIS — Auditing the Filter With No Ground Truth

We have the innovation ν and its predicted spread S. Property 3 says the real spread of ν should match S. But ν is a vector and S is a matrix — we can't eyeball "does this 3-vector have that 3×3 spread?" We need to collapse the comparison into a single number per step, scaled so that "expected" is a known value. That number is the Normalized Innovation Squared, or NIS, and it is the most important online diagnostic in all of estimation.

Read this carefully, because the S⁻¹ in the middle is the whole point. We are not just squaring the innovation; we are squaring it in units of its own predicted standard deviation. In 1D this is transparent: ε = ν²/S = (ν/√S)², which is "how many predicted standard deviations away was the measurement, squared." An innovation of 2 m when S predicted a spread of 1 m gives ε = 4 (two sigma out). The same 2 m innovation when S predicted a spread of 4 m gives ε = 0.25 (half a sigma — perfectly normal). The normalization by S is what makes NIS comparable across steps and sensors.

What value should NIS land on? Enter chi-square

Here is the beautiful result. If the filter is consistent, the innovation ν is a zero-mean Gaussian with covariance S. A standard fact of statistics: a Gaussian vector "whitened" by its own inverse-covariance and squared — exactly the form ν^TS⁻¹ν — follows a chi-square distribution (written χ²) with degrees of freedom equal to the dimension of the measurement, dim(z). So:

The expected value of a chi-square with k degrees of freedom is exactly k. So a consistent filter measuring a 2D position (dim(z) = 2) should produce NIS values averaging 2. A filter fusing a 3D LiDAR point (dim(z) = 3) should average 3. The expected NIS is just the measurement dimension — an astonishingly simple target. Persistent NIS far above dim(z) ⇒ overconfident / diverging. Persistent NIS far below ⇒ underconfident.

The window test — averaging beats single steps

One NIS value is noisy: a chi-square with 2 DOF has a healthy spread, so any single step can be 0.1 or 8 by pure chance even for a perfect filter. The fix is to average NIS over a window of N consecutive steps and test the average, which is far less noisy. The averaged statistic is itself (a scaled) chi-square: the sum of N independent χ²_k variables is χ²_Nk. So the time-averaged NIS over N steps should lie inside a confidence interval derived from a chi-square with N·dim(z) degrees of freedom, divided by N.

The two-sided 95% confidence interval for the average is [χ²_0.025(N·k) / N,  χ²_0.975(N·k) / N], where χ²_0.025 and χ²_0.975 are the lower- and upper-tail critical values of the chi-square distribution. If ε̅ falls inside this band, the filter is consistent. Outside the top ⇒ overconfident; below the bottom ⇒ underconfident.

Worked example — dim(z) = 2, N = 50, with actual bounds

Let's compute a real confidence band so the abstract formula becomes a concrete number you can check against. Suppose we fuse a 2D position measurement, so dim(z) = 2, and we average over N = 50 steps. The averaged NIS scales as a chi-square with N·dim(z) = 50×2 = 100 degrees of freedom, divided by 50.

From a chi-square table, the 2.5% and 97.5% critical values for 100 DOF are:

Divide both by N = 50 to get the bounds on the average NIS:

So the verdict rule for this setup is concrete and memorable:

Notice the band is centered near 2 (the expected value, dim(z)) but is not [1.484, 2.591] symmetric — chi-square is right-skewed. And notice the band tightens as N grows: with N = 50 we get [1.48, 2.59], a window of ~1.1; with N = 200 the band would shrink toward [1.74, 2.27]. More steps → tighter band → a more sensitive test. This is why you average over a generous window before pronouncing judgment.

NIS over a run, in code

Here is the full online monitor: compute ε each step, average over a sliding window, and compare to the chi-square band. This is the literal code you would drop into a production filter to raise an alarm:

python
import numpy as np
from scipy.stats import chi2

def nis(nu, S):
    # nu : innovation vector (dim m) ;  S : predicted innovation covariance (m x m)
    return float(nu @ np.linalg.solve(S, nu))   # nu^T S^-1 nu  (solve beats inv: stabler)

def nis_band(dim_z, N, conf=0.95):
    # two-sided confidence band on the WINDOW-AVERAGED NIS
    a = (1 - conf) / 2                        # 0.025 for 95%
    dof = N * dim_z                            # sum of N chi2_k is chi2_(N*k)
    lo = chi2.ppf(a,     dof) / N              # lower bound on the AVERAGE
    hi = chi2.ppf(1 - a, dof) / N              # upper bound on the AVERAGE
    return lo, hi

# --- run the monitor over a logged trajectory of (nu, S) pairs ---
eps   = np.array([nis(nu, S) for nu, S in log])   # NIS per step
N, dim_z = 50, 2
lo, hi = nis_band(dim_z, N)                       # e.g. (1.484, 2.591)
avg = eps[-N:].mean()                              # average over the last window

if   avg > hi: status = "OVERCONFIDENT  -> inflate Q and/or R"
elif avg < lo: status = "UNDERCONFIDENT -> shrink Q and/or R"
else:         status = "CONSISTENT"
print(f"avg NIS = {avg:.2f}  band = [{lo:.2f}, {hi:.2f}]  -> {status}")

The compact form, for when you just need the verdict inline:

python
ok = lambda eps, lo, hi: lo <= np.mean(eps) <= hi   # True == consistent

The key engineering choices: use np.linalg.solve(S, nu) not inv(S) (numerically stabler, and S can be near-singular when P collapses — itself a divergence symptom); average over a window long enough to be statistically meaningful (N ≥ 30) but short enough to react to a developing problem; and re-derive the band from the actual dim(z) and N, never hard-code [1.48, 2.59] from a different setup.

Chapter 3: NEES — The Gold-Standard Test (When You Have Ground Truth)

NIS audits the filter's measurement predictions, and it needs no ground truth, which makes it the workhorse you run online. But NIS has a blind spot: it only checks the state in the directions the sensor sees (through H). If your filter is overconfident about a quantity the sensor never measures — a hidden velocity bias, an unobservable yaw — NIS can look perfectly healthy while the actual state error is exploding in the dark. To audit the full state error directly, we need the gold-standard test, and it carries one price: it needs the truth.

The Normalized Estimation Error Squared (NEES) is NIS's sibling, but instead of normalizing the innovation by S, it normalizes the actual state error by the filter's reported state covariance P:

where η (eta) is the NEES, x is the true state, x̂ is the filter's estimate, and P is the filter's reported state covariance. The quantity (x − x̂) is the actual estimation error — the thing the filter is supposed to be small and the thing P is supposed to describe. NEES asks the most direct question possible: is the real error as big as the filter's P says it should be?

The chi-square target for NEES

The same beautiful statistics apply, with one change: NEES collapses the full state error, so its degrees of freedom equal the state dimension dim(x), not the measurement dimension:

A consistent filter tracking a 4-state vehicle (position-x, position-y, velocity-x, velocity-y — dim(x) = 4) should produce NEES values averaging 4. NEES above 4 means the real error exceeds what P claims (overconfident); below 4 means P is too fat (underconfident). Same logic as NIS, but now testing the entire state, including the parts no sensor directly sees.

Monte-Carlo averaging — the right way to use NEES

Because NEES needs simulation anyway, the standard protocol runs the filter M times with different random noise seeds (a Monte-Carlo ensemble), and at each time step averages NEES across the M runs. This average NEES over M independent runs is far less noisy than any single run, and again follows a scaled chi-square: M independent χ²_n sum to χ²_Mn, so the M-run average has band [χ²_0.025(M·n)/M, χ²_0.975(M·n)/M].

Worked example — dim(x) = 4, M = 50 runs, with bounds

Let's compute the real band, exactly as Bar-Shalom prescribes it. We Monte-Carlo a 4-state filter (dim(x) = 4) over M = 50 independent runs and average NEES at each time step. The average scales as chi-square with M·dim(x) = 50×4 = 200 degrees of freedom, divided by 50.

The 2.5% and 97.5% chi-square critical values for 200 DOF are:

Divide by M = 50:

So with 50 Monte-Carlo runs, a consistent 4-state filter's average NEES at each time step should sit inside [3.255, 4.821], centered near the expected value 4. Plot the average-NEES-over-time curve with these two horizontal lines:

The classic divergence signature on a NEES plot is unmistakable and worth memorizing: the curve starts inside the band, then climbs steadily above the upper bound and never comes back. That rising NEES is the filter's covariance P shrinking faster than its real error — overconfidence, captured numerically, before you ever ship.

NEES over a Monte-Carlo ensemble, in code

python
import numpy as np
from scipy.stats import chi2

def nees(x_true, x_hat, P):
    e = x_true - x_hat                          # the ACTUAL state error (needs truth!)
    return float(e @ np.linalg.solve(P, e))      # e^T P^-1 e

# Monte-Carlo: run the SAME filter over M seeds, store NEES per step per run.
M, T, n = 50, 200, 4                        # 50 runs, 200 steps, 4-state
NEES = np.zeros((M, T))
for m in range(M):
    filt = make_filter()                       # fresh filter, fresh noise seed
    for t in range(T):
        x_true = step_world(t)                  # simulator knows the truth
        z      = observe(x_true)
        filt.predict(); filt.update(z)
        NEES[m, t] = nees(x_true, filt.x, filt.P)

avg = NEES.mean(axis=0)                       # average across runs, per time step
lo  = chi2.ppf(0.025, M*n) / M                 # 3.255 for M=50, n=4
hi  = chi2.ppf(0.975, M*n) / M                 # 4.821
frac_in = np.mean((avg >= lo) & (avg <= hi))    # fraction of steps inside the band
print(f"band [{lo:.2f}, {hi:.2f}]  fraction consistent = {frac_in:.0%}")

If frac_in is near 100%, the filter is well-tuned and you can deploy it. If avg climbs out the top and stays there, you have caught divergence on the bench — before it cost you a robot. The whole point of NEES is to find that out here, in simulation, where the only thing that crashes is a number.

Chapter 4: The Whiteness Test — Catching a Wrong Model

NIS and NEES test whether the filter's covariance is the right size. But a filter can have the right-size covariance and still be subtly broken in a different way: its model can be wrong — it can assume the vehicle moves in straight lines when it actually turns, or that acceleration is constant when there's a sinusoidal force. This kind of bug doesn't always blow up the innovation magnitude; instead it leaves a pattern in the innovations over time. The test that catches patterns is the whiteness test, and it audits Property 2 from Chapter 1.

Recall: for a consistent filter the innovation at each step is new information, so consecutive innovations should be uncorrelated — "white" like white noise, no memory from one step to the next. If today's innovation can be predicted from yesterday's, the filter left information on the table: there was structure in the data its model failed to capture. Correlated innovations are the signature of an unmodeled dynamic.

The autocorrelation — how to see the correlation

We measure "does ν at step i predict ν at step i+τ?" with the sample autocorrelation of the innovation sequence at lag τ (tau, the time gap). For a scalar innovation sequence of length N:

where ρ(τ) (rho) is the correlation between innovations τ steps apart, normalized so that ρ(0) = 1 (everything is perfectly correlated with itself). For a white sequence, ρ(τ) is a spike at τ = 0 and approximately zero everywhere else. For a filter with an unmodeled dynamic, ρ(τ) decays slowly, oscillates, or has bumps — the leftover structure showing through.

The confidence band — how big is "approximately zero"?

The sample autocorrelation is never exactly zero at nonzero lags even for perfect white noise — finite samples wobble. The standard 95% confidence band for white noise is ±1.96/√N, where N is the number of innovations. Any ρ(τ) poking outside this band at a nonzero lag is statistically significant correlation — a red flag.

Worked number: with N = 400 innovations, the band is ±1.96/√400 = ±1.96/20 = ±0.098. So for a white sequence, you expect almost all ρ(τ) values at τ ≥ 1 to lie within ±0.098 (about 5% can poke out by chance). If you instead see ρ(1) = 0.6, ρ(2) = 0.4, ρ(3) = 0.25 — a smooth decaying tail way outside ±0.098 — that is unmistakable correlation, and your model is missing a dynamic.

Reading the diagnosis off the shape

The shape of the autocorrelation tail even hints at what is unmodeled:

This is the crucial complement to NIS: a filter can pass NIS (right-size covariance on average) yet fail whiteness (a structured pattern in the residuals). The two together pin down both kinds of error — wrong size (NIS/NEES) and wrong shape (whiteness).

Whiteness test in code

python
import numpy as np

def innovation_autocorr(nu, max_lag=30):
    nu = np.asarray(nu, float)
    nu = nu - nu.mean()                         # remove any DC (also a bias check!)
    denom = np.sum(nu * nu)                      # = rho(0) numerator -> normalizes to 1
    rho = []
    for tau in range(max_lag + 1):
        num = np.sum(nu[:len(nu) - tau] * nu[tau:])   # sum nu_i * nu_(i+tau)
        rho.append(num / denom)
    return np.array(rho)                          # rho[0] == 1.0 by construction

def is_white(nu, max_lag=30):
    rho  = innovation_autocorr(nu, max_lag)
    band = 1.96 / np.sqrt(len(nu))             # 95% white-noise band, e.g. 0.098 at N=400
    out  = np.sum(np.abs(rho[1:]) > band)        # count lags poking outside the band
    frac = out / max_lag                        # expect ~5% by chance if truly white
    return frac < 0.10, rho, band             # white if few lags breach

white, rho, band = is_white(innovations)
print("WHITE -- model OK" if white else "CORRELATED -- unmodeled dynamic!")

Two subtle but load-bearing details. First, subtracting the mean (nu - nu.mean()) does double duty — it's required for a clean autocorrelation and it surfaces any bias (a large DC component is itself the zero-mean violation from Chapter 1). Second, the test allows ~5% of lags to breach the band by chance, so we flag only when the breach fraction is high — one stray lag is not a model error, a decaying tail of them is.

Chapter 5: Tuning From the Tests — Turning Diagnoses Into Fixes

A diagnosis is only useful if it points to a repair. The whole reason we run NIS, NEES, and the whiteness test is to know which knob to turn. This chapter is the decision table that converts each test result into a specific action on the two noise covariances every Kalman-family filter exposes: Q (process noise — how much you distrust your motion model) and R (measurement noise — how much you distrust your sensor).

First, fix the two knobs in your mind, because tuning is hopeless if you confuse them. Q is the filter's admission that its prediction is imperfect — larger Q means "my motion model is rough, grow my uncertainty fast between measurements, so I'll listen harder to the next sensor reading." R is the filter's belief about sensor noise — larger R means "this sensor is noisy, don't snap to it." A too-small Q or a too-small R both cause overconfidence (the covariance collapses), but they collapse it through different doors, and the tests tell them apart.

The master tuning table

The most important subtlety — correlation is NOT a noise problem

The single most common tuning mistake is to treat every failing test by cranking Q. If NIS is high, yes, inflating Q can help. But if the whiteness test fails — correlated innovations — inflating Q is the wrong fix. Correlation means your model is missing a dynamic; no amount of noise tuning adds the missing physics. Worse, cranking Q to hide correlated residuals makes the filter believe its own motion model less everywhere, so it gets sloppy on the parts it was modeling fine. Wrong-size errors are a tuning problem (Q, R). Wrong-shape errors are a modeling problem (add states/dynamics). The tests separate the two so you fix the right thing.

Worked tuning loop — numbers

Walk a concrete iteration on a 2D tracker (dim(z) = 2, so target NIS ≈ 2, band roughly [1.48, 2.59] for N = 50):

1. Initial run. Average NIS = 6.8. Above the band → overconfident. Whiteness test: passes (flat autocorrelation). So it's a noise-size problem, not a model problem.
2. Which knob? Innovations are large at every step uniformly, including right after a measurement — that smells like the sensor being noisier than R claims. Double R.
3. Re-run. Average NIS = 3.1. Still a touch high. Nudge R up another 30%.
4. Re-run. Average NIS = 2.2, inside [1.48, 2.59]. Whiteness still passes. Consistent. Stop.

Notice the discipline: change one knob at a time, re-run, re-test. And always check whiteness alongside NIS — if whiteness had failed at step 1, no amount of R-tuning would have fixed it, and the table would have sent you to the model instead.

Chapter 6: Robustness — Surviving Outliers & Bad Associations

Consistency tests catch a filter whose noise model is wrong. But there is a second, equally lethal way fusion fails: a single gross outlier — one wild GPS multipath fix, one wrong data association (Lesson 12), one false loop closure in a pose graph (Lesson 17) — that is not noise at all, but a flat-out wrong number. The other half of debugging is making your estimator robust to these, so one bad measurement doesn't wreck the whole estimate. This chapter is the robust-cost toolkit.

Why one outlier wrecks least-squares

Every fuser in this series ultimately minimizes a sum of squared residuals — the L2 (least-squares) cost. For residuals r_i (how far each measurement is from the current estimate), the cost is:

The squaring is the problem. A residual of 10 contributes 100 to the cost; a residual of 100 contributes 10,000. So a single gross outlier dominates the entire sum, and the optimizer — trying to minimize total cost — bends the whole estimate toward the outlier to reduce that one enormous term. One bad GPS fix can drag a trajectory meters off; one wrong loop closure can fold a whole map.

Worked number: fit a constant to readings [10, 11, 9, 12, 90]. The least-squares answer is the mean = (10+11+9+12+90)/5 = 132/5 = 26.4 — a value the four good readings would never suggest, yanked up 15 units by the single outlier. The squared cost wants to sit near the 90 because the penalty for ignoring it (a residual of ~64, squared = ~4000) is gigantic. Least-squares has no defense: it assumes every residual is honest Gaussian noise, and an outlier is not.

The cure — bound the influence of large residuals

The fix is to replace the squared cost with a robust cost (an M-estimator) that grows slower than r² for large residuals, so an outlier can't dominate. The key concept is the influence function — the derivative of the cost, which is how hard a residual pulls the estimate. For L2, the influence is 2r, which grows without bound: the bigger the outlier, the harder it pulls. A robust cost bounds that influence so distant outliers pull with limited (or vanishing) force.

Huber loss — quadratic near zero, linear in the tails

The Huber loss is the gentlest robust cost and the most widely used. It behaves exactly like L2 for small residuals (so well-behaved measurements are treated optimally) but switches to a linear penalty beyond a threshold δ (delta), so large residuals pay only linearly:

The two pieces are stitched to match in value and slope at |r| = δ, so the cost is smooth. The payoff is in the influence function (the derivative): for small residuals it's r (same as least-squares, grows with the residual); but for large residuals it saturates at ±δ — a constant. No matter how huge the outlier, its pull is capped at δ. A residual of 1000 pulls exactly as hard as a residual of δ.

Worked Huber down-weighting — numbers

Take δ = 3. Compare how L2 and Huber score a normal residual vs an outlier, and — the part that matters — how hard each pulls:

The "effective weight" column is the key insight: Huber is equivalent to iteratively reweighted least squares where each residual gets weight min(1, δ/|r|). The normal residual (r=1) keeps full weight 1.0. The gross outlier (r=90) is automatically down-weighted to 3/90 = 0.033 — it contributes 3% of what least-squares would have let it contribute. The outlier is still in the sum, but its voice is throttled to a whisper. That's robustness, derived.

Dynamic Covariance Scaling (DCS) — a closed-form robust kernel

Huber down-weights but never fully rejects — a gross outlier still pulls with weight δ/|r|, small but nonzero. For SLAM back-ends drowning in bad loop closures, you want something that drives the worst outliers' influence toward zero, and does it in closed form (no iteration). Dynamic Covariance Scaling (DCS) (Agarwal et al., ICRA 2013) does exactly this. It scales each constraint's information by a factor

where Φ (Phi) is a tuning parameter (the residual scale beyond which a constraint is suspect) and r² is the squared residual. For a small residual (r² < Φ), s = 1 — full weight, ordinary least squares. For a large residual, s shrinks like 2Φ/r², so the constraint's effective information is scaled down by s² — a far steeper roll-off than Huber. Crucially DCS is a closed-form scaling: you compute s once per constraint, no iterative reweighting loop, which makes it dramatically faster than switchable constraints for large pose graphs.

Worked DCS number: take Φ = 9 (so 3σ is the suspicion threshold). For a normal residual r = 1: s = min(1, 18/(9+1)) = min(1, 1.8) = 1.0 — full weight. For an outlier r = 30 (r² = 900): s = min(1, 18/(9+900)) = 18/909 = 0.0198 — the constraint's influence is scaled to ~2%, even harder than Huber's 10% on a comparable residual. The bad loop closure is all but switched off, in one formula.

Switchable Constraints — let the optimizer turn bad edges OFF

The most explicit approach gives each (loop-closure) constraint its own switch variable s_ij ∈ [0, 1] that the optimizer is free to adjust. Switchable Constraints (Sünderhauf & Protzel, IROS 2012) augment the cost so each constraint is weighted by its switch, plus a penalty for turning a switch off:

The optimizer now jointly solves for the poses and the switches. For a good loop closure, keeping s_ij = 1 costs little (the residual is small) and avoids the penalty, so the switch stays on. For a bad loop closure, the residual r_ij is huge, so the optimizer would rather pay the penalty Λ(1−s_ij)² and drive s_ij → 0, switching the bad constraint off entirely. The optimizer disables wrong loop closures by itself — no manual outlier list. The trade is cost: switchable constraints add a variable per loop and need iteration, which is why DCS (closed-form, no extra variables) was developed as the faster sibling.

Connecting back to chi-square gating (Lesson 12)

There's a cheaper first line of defense than any robust cost: reject the outlier before it ever enters the filter. This is exactly the chi-square gate from data association (Lesson 12), and now you can see it's just NIS used as a guard. Before accepting a measurement, compute its NIS ε = ν^TS⁻¹ν and check it against a chi-square threshold (e.g. the 99% value of χ²_dim(z)):

For dim(z) = 2, χ²_0.99(2) ≈ 9.21. So any measurement whose NIS exceeds 9.21 is more than a 99% surprise — almost certainly an outlier or a wrong association — and gets rejected before it can poison the state. Gating (reject) and robust costs (down-weight) are complementary: gate the obvious outliers cheaply up front, and let a robust cost gracefully handle the borderline ones that slip through. The same NIS statistic that diagnoses the filter (Chapter 2) also protects it (here).

Robust weighting in a least-squares fuse, in code

python
import numpy as np

def huber_weight(r, delta=3.0):
    # effective IRLS weight: 1 for inliers, delta/|r| for outliers (bounded influence)
    a = np.abs(r)
    return np.where(a <= delta, 1.0, delta / a)

def dcs_weight(r, phi=9.0):
    # DCS scale s = min(1, 2*phi/(phi + r^2)); effective info weight is s^2
    s = np.minimum(1.0, 2.0 * phi / (phi + r*r))
    return s * s

def robust_fit_constant(y, kind="huber", iters=10):
    # iteratively reweighted least squares to fit a robust mean of y
    est = np.median(y)                          # robust init (median ignores outliers)
    for _ in range(iters):
        r = y - est
        w = huber_weight(r) if kind == "huber" else dcs_weight(r)
        est = np.sum(w * y) / np.sum(w)         # weighted mean -> outliers barely count
    return est

y = np.array([10, 11, 9, 12, 90])           # one gross outlier (90)
print("L2  (mean)  =", y.mean())                  # 26.40  <- wrecked by the outlier
print("Huber       =", robust_fit_constant(y, "huber"))  # ~10.5  <- ignores it
print("DCS         =", robust_fit_constant(y, "dcs"))    # ~10.5  <- ignores it harder

The compact, inline version of the Huber down-weight you can drop into any residual loop:

python
w = np.minimum(1.0, delta / np.abs(r))   # 1 for |r|<=delta, else delta/|r| (bounded pull)

The L2 mean comes out at 26.4 — dragged 15 units off by the single 90. Huber and DCS both land near 10.5, right where the four honest readings live, because the outlier's weight collapsed to ~δ/90 (Huber) or ~(2Φ/90²)² (DCS). Same data, the robust cost simply refuses to be bullied by one bad number.

Chapter 7: Showcase — Catch Overconfidence Red-Handed

This is the payoff that ties Chapters 0–5 into one live instrument. A tracking filter runs on the left; its consistency monitor runs on the right. You hold a single dial — the Q/R mis-tuning — and watch the two halves of the screen contradict each other: the estimate's covariance band tightens (the filter growing confident) while the live NIS plot climbs above its chi-square band (the filter being caught lying). Overconfidence, red-handed, exactly as it happens in a real divergence.

The guided experiment — reproduce the silent killer in four clicks

Don't just watch; falsify. Run this and confirm each prediction with your own eyes:

1. R trust = 1×, press Run. The band breathes around the truth; NIS jitters around 1 (= dim(z)), staying inside the green band. This is the honest baseline — the consistent filter.
2. Slide R trust to 5×. The covariance band visibly tightens — the filter is getting more confident. Watch NIS: its average creeps toward and then above the upper bound. The filter says it's surer; the monitor says it's lying.
3. Slide to 15×. The band is now a thin ribbon clinging to a wandering estimate; NIS spends almost all its time above the band, often far above. This is full overconfidence — the silent killer made loud by the monitor.
4. The lesson. The left half (estimate + band) looked more trustworthy as you slid the dial; the right half (NIS) screamed louder. Only the NIS monitor told the truth. A team logging only the estimate would have shipped this filter.

This is the whole capstone in one widget. The estimate's own confidence report is unreliable — it tightened precisely as the filter got worse. The NIS plot, computed from the same innovations the filter already has, caught the divergence with no ground truth, online, live. That asymmetry — the estimate looking better while NIS looks worse — is the visual signature of overconfidence, and once you've seen it you'll recognize it in every real divergence.

Chapter 8: Use Cases & Real Products — Consistency Tests in the Wild

Every lesson in this series ends with the same four chapters — Use Cases, Practical Application, Debugging, Connections — because a diagnostic you can't point at in shipped code is one you don't really own. The consistency toolkit isn't academic: NIS gates, NEES validation, and robust kernels run inside the most-deployed autonomy stacks on Earth, right now. Tour them and you'll recognize every tool from the last seven chapters.

The pattern across products

The deep case — how PX4 actually rejects a bad GPS fix

Walk one real update through PX4's EKF2, because it's the consistency toolkit in production. A GPS position arrives. The filter computes the innovation ν = z_GPS − H·x̂ and the innovation covariance S = HPH^T + R. It forms the test ratio — NIS divided by the gate threshold: ratio = νTS−1ν / gate². If ratio ≤ 1, the fix is consistent with the filter's prediction and gets fused. If ratio > 1 (a multipath fix reflected off a building, say, giving a 50 m jump), the measurement is rejected — it never touches the state. PX4 logs the innovation and the ratio so that, post-flight, you can open the log and see exactly which fixes were rejected and why. That is Chapters 1, 2, and 6 — innovation, NIS, and chi-square gating — running thousands of times per flight, on a $200 flight controller.

Chapter 9: Practical Application — The Consistency Workflow & a Divergence Runbook

You now have the tests; this recurring "now do it" chapter turns them into a workflow you can run on any fuser, plus a concrete runbook for the call every estimation engineer eventually gets: "the filter diverged in the field — figure out why." The goal is to make diagnosis mechanical, so you're never staring at a wall of numbers guessing.

The consistency workflow — the standard procedure

Choosing the robust kernel for your outlier rate

Step 6 hides a real decision. Match the kernel to how dirty your data is:

The runbook: "my filter diverged — here's how I'd diagnose it"

You get the call. The position estimate flew off into nonsense mid-mission. Don't guess — run the tests in order, and each result eliminates a class of cause:

1. Pull the innovation log and compute NIS over time. Find where it left the band. A NIS that was healthy then suddenly spiked at t = 412 s localizes the divergence to a moment — correlate it with what happened then (entered a tunnel? sensor dropout? sharp maneuver?).
2. Check the innovation mean near the divergence. A nonzero mean (Chapter 1) → a bias crept in: a sensor drifted out of calibration, or a frame/time offset appeared (Lesson 15). Zero mean but large spread → not bias, keep going.
3. Run the whiteness test on the window before divergence. Correlated innovations → model error: the vehicle did something the motion model can't represent (a turn, an acceleration). White but oversized → noise mistuning, not model.
4. Look for a single gross innovation right at the onset. One enormous ν that the filter accepted → an outlier got through (a multipath GPS, a wrong association). Fix: add/ tighten the chi-square gate, add a robust kernel.
5. If NIS looks fine but the estimate is still wrong, reach for NEES in sim. The divergence is in an unobserved state the sensor doesn't see (Chapter 5). Check observability; add a sensor or inflate Q on that state.
6. Audit covariance collapse. Plot trace(P) over time. If it shrank toward zero before the divergence, the filter was on the overconfidence death-spiral (Chapter 0) — you under-claimed Q or R. Inflate the suspect knob.

Six steps, and each test partitions the space of causes: bias vs spread, model vs noise, observed vs unobserved, inlier-noise vs outlier. By the end you don't have a guess — you have a localized cause and the matching fix from Chapter 5's table. The discipline is: never tune a knob until the tests tell you which knob.

Chapter 10: Debugging & Failure Modes — The Series-Wide Catalog

This is the chapter that ties the whole series together. Every failure mode from lessons 1–20 — overconfidence, bad associations, calibration drift, outliers, observability loss — shows up as a specific, recognizable pattern in the consistency tests you just learned. Here is the master catalog: for each failure, the symptom it presents, which test detects it, and the fix. Internalize this table and you can debug any fuser in this series from its innovation log.

The master failure-mode catalog

The most instructive entry — calibration error looks like motion-correlated innovations

This one deserves a worked symptom because it's so commonly misdiagnosed. Suppose your LiDAR is mounted with a small rotational miscalibration relative to the IMU (a 2° extrinsic error, Lesson 15), or its timestamps lag the IMU by 20 ms (a time-sync error). When the robot sits still, the two sensors agree and the innovations are tiny — the filter looks perfect. But the moment the robot moves or turns, the miscalibration projects the LiDAR's points into the wrong place, and the innovations grow in proportion to the motion. The fingerprint is unmistakable: innovations near zero at rest, large during motion, scaling with angular/linear velocity. This is why a filter that passes every static bench test can fall apart in motion — and why the shape of the innovation-vs-motion relationship points straight at calibration, not noise.

The two questions that route any divergence

When in doubt, two questions partition almost every fusion failure:

• Is the innovation mean zero? No → bias (calibration, frame offset, time-sync, unmodeled constant force). Yes → not a bias, look at spread and structure.
• Are the innovations white? No → model error (unmodeled dynamic) or motion-correlated calibration error. Yes but oversized → noise mistuning (tune Q/R). Yes and right-sized but NEES still high → observability (unobserved state diverging).

Mean and whiteness — two checks — sort a divergence into bias / model / noise / observability, and each maps to a distinct fix from the catalog. That is the entire debugging discipline of the series, compressed.

Chapter 11: Connections, References & Cheat-Sheet

You now hold the diagnostic toolkit that audits and repairs every fuser in this series. Before we name what comes next, here is everything in one place — the cheat-sheet to screenshot, the series-wide wrap-up, the motto, the real sources, and the cross-links.

The consistency toolkit — cheat-sheet

The motto

Series-wide wrap-up — this lesson monitors and repairs every fuser sf-01..sf-20

This is the capstone because the consistency toolkit is the one thing that applies to all twenty earlier lessons. Here is the map — every fuser you built, and the specific tool here that monitors or repairs it:

Every one of those twenty fusers, no matter how different, fails in the same handful of ways — and every failure is visible in the innovation. That is the unifying truth of the series: you build a fuser with the math of lessons 1–20, and you trust it with the tests of lesson 21.

Related lessons on Engineermaxxing

These go deeper on machinery this capstone audits:

• Why Fuse At All? — Lesson 1, the taxonomy this whole series builds on; the ablation/existence tests there are the conceptual ancestors of NIS/NEES.
• The Kalman Filter — where the innovation ν and covariance S are born; NIS and NEES are computed directly from its update equations.
• The Extended Kalman Filter — the nonlinear fuser whose linearization errors make consistency testing essential (crude Jacobians breed overconfidence).
• Data Association — home of the chi-square gate; the same NIS statistic that diagnoses a filter (Ch 2) also gates its measurements (Ch 6).
• INS/GNSS Coupling — where IMU-bias divergence and GNSS multipath are the textbook cases the innovation tests catch.
• LiDAR-Inertial Factor Graphs — the SLAM back-end where Huber, DCS, and switchable constraints (Ch 6) earn their keep against false loop closures.
• Robust Estimation — the deep dive on M-estimators, influence functions, and breakdown points behind Chapter 6's robust costs.

References

1. Bar-Shalom, Y., Li, X.-R., and Kirubarajan, T. Estimation with Applications to Tracking and Navigation: Theory Algorithms and Software. Wiley, 2001. The canonical reference for NEES/NIS, the chi-square consistency tests, and Monte-Carlo filter validation — Chapters 5–11 are the source for everything in Chapters 1–5 of this lesson.
2. Sünderhauf, N. and Protzel, P. "Switchable Constraints for Robust Pose Graph SLAM." IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2012. Introduces per-constraint switch variables the optimizer can turn off to reject false loop closures. doi:10.1109/IROS.2012.6385590
3. Agarwal, P., Tipaldi, G. D., Spinello, L., Stachniss, C., and Burgard, W. "Robust Map Optimization Using Dynamic Covariance Scaling." IEEE International Conference on Robotics and Automation (ICRA), 2013. The closed-form DCS robust kernel; faster than switchable constraints. doi:10.1109/ICRA.2013.6630557
4. MacTavish, K. and Barfoot, T. D. "At all Costs: A Comparison of Robust Cost Functions for Camera Correspondence Outliers." Conference on Computer and Robot Vision (CRV), 2015. A practical survey comparing Huber, Cauchy, Geman-McClure, DCS, and other M-estimators — how to pick a robust cost. doi:10.1109/CRV.2015.52
5. PX4 Autopilot. "Using the ECL EKF (EKF2)" — official documentation on innovation test ratios, measurement gates, and consistency monitoring in a production flight stack. docs.px4.io
6. Kailath, T. "An Innovations Approach to Least-Squares Estimation — Part I." IEEE Transactions on Automatic Control, vol. 13, no. 6, pp. 646–655, 1968. The foundational result that filter optimality is equivalent to white innovations. doi:10.1109/TAC.1968.1099025

Where this lesson sits in the series

This was Lesson 21 of 22 — the diagnostic capstone. Lessons 1–20 taught you to build fusers; this lesson taught you to verify and repair them with one filter-agnostic toolkit. The recurring chapter structure you've walked all series (Why → concepts → showcase → Use Cases, Practical, Debugging, Connections) closes here. One lesson remains: Lesson 22 — the Sensor Fusion Atlas, the whole map drawn at once, every relationship, filter, and diagnostic connected into a single mental model you can navigate end to end.