M7-T4 — Consistency, Observability and Divergence

Learning objectives

After this lesson, you should be able to:

• Define NEES, state its expected value for a consistent filter, and test it against a chi-square band over Monte Carlo runs.
• Test the observability of a linear model with the observability matrix, and explain what an unobservable direction does to the covariance.
• Describe how overconfidence makes a filter diverge, and list practical remedies.
• Distinguish the NEES (needs ground truth) from the NIS (does not) and use each where it applies.

The intuition

There are three ways a Kalman filter can silently go wrong, and each has its own test.

• Inconsistency: the covariance it reports is not the covariance of its real errors. In simulation, where the truth is known, you test it with the normalised estimation error squared (NEES): the squared Mahalanobis size of the real error, using the filter's own P. Averaged over many runs it should equal the state dimension.
• Unobservability: the sensors cannot determine part of the state, however long you wait. A filter does not complain: the covariance of that part simply never shrinks, and the estimate there is the prior. You detect it by examining the model before running anything.
• Divergence: the filter's gain falls to almost zero because it believes it is certain, and it stops listening to the sensor while the truth drifts away. Overconfidence feeds itself.

The left panel is the NEES version of the sweep in the last lesson. With the correct q the mean NEES is about 1.8, near the expected 2 (the simulation's noise is discretised a little differently from the Q matrix, which accounts for the small gap). With a q ten times too small it climbs without limit, 88 at step 40: the real errors are about 6 times larger than the filter's covariance says. With q too large the mean is about 1: the filter is conservative. The right panel shows the extreme case: a filter given q = 0, which at step 100 has a claimed standard deviation of 2 cm while its error is 64 cm.

Overconfidence is the dangerous failure: the filter keeps reporting a small covariance, so everything downstream (planning, gating, control) trusts an estimate that is wrong.

Definitions

NEES $\epsilon_t$

$(\mathbf{x}_t - \hat{\mathbf{x}}_t)^\top P_t^{-1}(\mathbf{x}_t - \hat{\mathbf{x}}_t)$: the squared Mahalanobis size of the true error. It needs the true state, so it is used in simulation or with a reference system.

Consistent filter

For a consistent filter the NEES follows $\chi^2_n$ with $n$ the state dimension, and its average over $N$ runs is $n$.

Observability matrix $\mathcal{O}$

$\begin{bmatrix}H\\HF\\\vdots\\HF^{n-1}\end{bmatrix}$: a linear time-invariant system is observable if and only if it has rank $n$.

Unobservable direction

A direction $\mathbf{v}$ of the state with $\mathcal{O}\mathbf{v} = 0$: changing the state along $\mathbf{v}$ changes no measurement.

Divergence

The estimation error grows while the reported covariance shrinks or stays small.

Derivation: why the NEES is χ², and what unobservable means

The NEES

1. $$\mathbf{e}_t = \mathbf{x}_t - \hat{\mathbf{x}}_t\sim\mathcal{N}(0, P_t)$$

   For a consistent filter the error of the estimate is zero-mean Gaussian with the covariance the filter reports. This is the content of “consistent”.

2. $$\mathbf{w} = P_t^{-1/2}\mathbf{e}_t\sim\mathcal{N}(0, I_n)$$

   Whiten the error with the Cholesky factor of P (the Mahalanobis lesson of module 4): n independent standard normal components.

3. $$\epsilon_t = \mathbf{e}_t^\top P_t^{-1}\mathbf{e}_t = \mathbf{w}^\top\mathbf{w}\sim\chi^2_n,\qquad \mathbb{E}[\epsilon_t]=n$$

   A sum of n squared standard normals is χ² with n degrees of freedom, whose mean is n.

The statistic from one run is noisy (χ² with 2 degrees of freedom has a long tail), so the test averages over runs, which narrows the band: with 300 runs of a 2-state filter, the 95% band for the average is 1.78 to 2.23.

Observability

Without noise, the measurements are $\mathbf{z}_k = HF^k\mathbf{x}_0$. Stacking the first n of them gives $\mathbf{z}_{0:n-1} = \mathcal{O}\mathbf{x}_0$, which can be solved for $\mathbf{x}_0$ if and only if $\mathcal{O}$ has rank n (the Cayley–Hamilton theorem means later rows add nothing).

1. $$\text{position, velocity; position sensor:}\quad \mathcal{O} = \begin{bmatrix}1&0\\1&\Delta t\end{bmatrix},\quad \det\mathcal{O} = \Delta t = 0.5$$

   Rank 2: observable. Two successive positions give the velocity.

2. $$\text{two constants } a,b;; z = a + b:\quad \mathcal{O} = \begin{bmatrix}1&1\\1&1\end{bmatrix},\quad \det\mathcal{O} = 0$$

   Rank 1: not observable. The direction (1, −1) changes no measurement.

In the second case the filter can learn the sum but never the difference, and its covariance records exactly that: the variance of $a+b$ falls without limit, and the variance of $a-b$ stays at its prior value however many readings arrive. The estimate of each of a and b separately is just the prior split, and is wrong.

A filter never complains about an unobservable state. It reports the prior uncertainty there, which is the only honest thing to do. The warning is in the covariance, and in the observability matrix, not in the error.

A worked example

Three experiments with the programs of the last two lessons.

1. Consistency. Over 300 runs, the 95% band for the mean NEES is 1.78 to 2.23. At steps 10, 20 and 40 the correct filter has 1.83, 1.82 and 1.67, near 2 (slightly low because of the discretisation noted above, and consistent on average over steps 20 to 40). A q 10 times too small (as a standard deviation) gives 6.1, 42.7 and 88.0: overconfident, and it gets worse with time. A q 4 times too large gives about 1.0: underconfident.
2. Observability. For two constants a = 2 and b = 1 seen only through their sum, $\det\mathcal{O} = 0$. After 200 readings the standard deviation of $a+b$ is 0.014, but that of $a-b$ is still 1.41, its prior value. The estimates are 1.507 and 1.507, the prior split of the measured sum of 3. For the robot with a position sensor $\det\mathcal{O} = 0.5$: observable.
3. Divergence. A scalar filter with q = 0 (certain of its motion model) on the robot of lesson 1: its gain falls from 0.1 at step 10 to 0.02 at step 50 and 0.005 at step 200, and its claimed standard deviation to 1.4 cm. Its error at step 100 is 0.64 m, more than 30 times the claim. With q = 0.01 the gain stays at 0.39 and the error stays near the claim.

import math
from robotprob import Rng, kf_predict, kf_update, nees, mean, chi2_ppf

# 1. NEES of the constant-velocity filter, correct Q and a Q that is too small
dt, sq, r = 0.5, 0.05, 0.04
F = [[1.0, dt], [0.0, 1.0]]; H = [[1.0, 0.0]]; R = [[r]]
def Qm(s):
    q = s * s
    return [[q * dt**3 / 3, q * dt**2 / 2], [q * dt**2 / 2, q * dt]]
def run(seed, s_filter, steps=40):
    rng = Rng(seed)
    pos, vel = 0.0, 1.0
    x, P = [0.0, 0.0], [[1.0, 0.0], [0.0, 1.0]]
    Q = Qm(s_filter)
    out = []
    for t in range(steps):
        vel += rng.gauss(0.0, sq * math.sqrt(dt)); pos += vel * dt
        z = pos + rng.gauss(0.0, math.sqrt(r))
        x, P = kf_predict(x, P, F, Q)
        x, P, y, S, K = kf_update(x, P, [z], H, R)
        out.append(nees([x[0] - pos, x[1] - vel], P))   # the real error, measured with the filter's own P
    return out
runs = 300
lo, hi = chi2_ppf(0.025, 2 * runs) / runs, chi2_ppf(0.975, 2 * runs) / runs
print('two-sided 95% bounds for the mean NEES over', runs, 'runs:', round(lo, 3), 'to', round(hi, 3))
for name, s in [('correct q', 0.05), ('q 10x too small (std)', 0.005), ('q 4x too large (std)', 0.2)]:
    A = [run(seed, s) for seed in range(1, runs + 1)]
    avg = [mean([a[t] for a in A]) for t in range(40)]
    print(name.ljust(22), 'mean NEES at steps 10, 20, 40:', [round(avg[i], 2) for i in (9, 19, 39)], ' verdict', 'consistent' if lo <= mean(avg[19:]) <= hi else ('overconfident' if mean(avg[19:]) > hi else 'underconfident'))

# 2. observability: two unknown constants a, b measured only through their sum
Hs = [[1.0, 1.0]]; Fs = [[1.0, 0.0], [0.0, 1.0]]
O = [[1.0, 1.0], [1.0, 1.0]]                      # [H; H F]
print('observability matrix determinant (a, b measured through a + b):', O[0][0]*O[1][1]-O[0][1]*O[1][0])
Ho = [[1.0, 0.0]]; Fo = [[1.0, dt], [0.0, 1.0]]
O2 = [Ho[0], [Ho[0][0]*Fo[0][0]+Ho[0][1]*Fo[1][0], Ho[0][0]*Fo[0][1]+Ho[0][1]*Fo[1][1]]]
print('observability matrix determinant (position, velocity, position sensor):', O2[0][0]*O2[1][1]-O2[0][1]*O2[1][0])
x, P = [0.0, 0.0], [[1.0, 0.0], [0.0, 1.0]]
rng = Rng(3)
a, b = 2.0, 1.0
for t in range(200):
    z = a + b + rng.gauss(0.0, 0.2)
    x, P = kf_predict(x, P, Fs, [[0.0, 0.0], [0.0, 0.0]])
    x, P, y, S, K = kf_update(x, P, [z], Hs, [[0.04]])
sd_sum = math.sqrt(P[0][0] + 2*P[0][1] + P[1][1]); sd_diff = math.sqrt(P[0][0] - 2*P[0][1] + P[1][1])
print('after 200 readings: std of a+b', round(sd_sum, 4), ' std of a-b', round(sd_diff, 4), ' estimates', [round(v, 3) for v in x], ' truth', (a, b))

# 3. divergence: q set to zero for a robot whose motion really is noisy
def diverge(q_filter, seed=2, steps=200):
    rng = Rng(seed); p, mu, var = 0.0, 0.0, 1.0
    rows = []
    for t in range(steps):
        p += 0.5 + rng.gauss(0.0, 0.1)
        z = p + rng.gauss(0.0, 0.2)
        mu += 0.5; var += q_filter
        k = var / (var + 0.04); mu += k * (z - mu); var *= 1 - k
        rows.append((t + 1, p, mu, math.sqrt(var), k))
    return rows
for qf in (0.0, 0.01):
    rows = diverge(qf)
    print('q =', qf, [(t, round(mu - p, 3), round(sd, 3), round(k, 3)) for t, p, mu, sd, k in rows if t in (10, 50, 100, 200)])

two-sided 95% bounds for the mean NEES over 300 runs: 1.78 to 2.233
correct q              mean NEES at steps 10, 20, 40: [1.83, 1.82, 1.67]  verdict consistent
q 10x too small (std)  mean NEES at steps 10, 20, 40: [6.07, 42.68, 87.98]  verdict overconfident
q 4x too large (std)   mean NEES at steps 10, 20, 40: [1.06, 1.04, 1.03]  verdict underconfident
observability matrix determinant (a, b measured through a + b): 0.0
observability matrix determinant (position, velocity, position sensor): 0.5
after 200 readings: std of a+b 0.0141  std of a-b 1.4142  estimates [1.507, 1.507]  truth (2.0, 1.0)
q = 0.0 [(10, -0.224, 0.063, 0.1), (50, 0.009, 0.028, 0.02), (100, 0.636, 0.02, 0.01), (200, -0.205, 0.014, 0.005)]
q = 0.01 [(10, -0.144, 0.125, 0.39), (50, -0.095, 0.125, 0.39), (100, 0.107, 0.125, 0.39), (200, -0.062, 0.125, 0.39)]

In robotics

Validating a filter before it flies

Before trusting a filter on a real robot, simulate it with a truth model: run a few hundred Monte Carlo runs and plot the mean NEES against its band. On real data the truth is usually missing, so use the NIS (last lesson) and, where a reference exists (motion capture, RTK GPS), the NEES against it.

Observability in robot design

Observability decides which sensors you need. A robot with a gyro and an accelerometer can estimate roll and pitch (gravity observes them) but not heading (nothing observes it): without a compass or a map the heading covariance grows without bound, and a filter should say so. In SLAM, the absolute position and heading of the whole map are unobservable with relative measurements alone; the covariance of the first pose is the anchor, and the gauge freedom reappears in M11-T4: Nonlinear Least Squares and Pose Graph Optimization.

Remedies for divergence

• Inflate Q (a little “fudge” noise), the first and simplest remedy. Never set Q to zero for a real system.
• Use the Joseph form and symmetrise P, so rounding cannot make P lose positive definiteness.
• Gate outliers (module 4) so that one bad reading does not corrupt the state, but not so tightly that a filter that is already off never recovers: a rejected-reading counter that triggers a reset is common practice.
• Model the missing effect (a bias state, a drag term) when the innovations are correlated.

Linear observability is not enough for nonlinear systems

The rank test applies to linear systems. Nonlinear systems can be observable in some configurations and not in others (a bearing-only sensor tracking a target that moves straight has an unobservable range). Linearisation, the next module, can also invent observability that the true system does not have.

Common misconceptions

Key takeaways

1. The NEES, eᵀP⁻¹e, has an expected value equal to the state dimension for a consistent filter. Test the average over many runs against a χ² band.
2. Above the band: overconfident, the dangerous case. Below: underconfident, safe but wasteful.
3. A linear system is observable if the observability matrix [H; HF; …] has full rank. Unobservable directions keep their prior covariance for ever.
4. A filter that is overconfident gives a small gain, stops correcting, and diverges silently. Never use Q = 0; inflate Q and gate carefully.
5. Use the NEES in simulation, the NIS on real data, and look at the whole of P, not one entry.

References and further reading

Each source is marked free or paid. Free to read does not mean free to reuse: the course resources page lists every source with its licence.

• Y. Bar-Shalom, X. R. Li and T. Kirubarajan, Estimation with Applications to Tracking and Navigation, Wiley, 2001, section 5.4 (NEES and NIS consistency tests) and chapter 3 (linear estimation). Paid book.
• T. D. Barfoot, State Estimation for Robotics, 2nd edition, 2024 , the chapters on linear-Gaussian estimation and on nonlinear estimation discuss consistency and observability. Free to read as the author’s PDF; the book is copyrighted.
• R. Labbe, Kalman and Bayesian Filters in Python (online book) , has chapters on performance and on filter divergence. Free to read; licence not checked.
• S. Thrun, W. Burgard and D. Fox, Probabilistic Robotics, MIT Press, 2005, chapter 3 (Gaussian filters). Paid book.

Knowledge check

Three questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

Module 7 has built the linear Kalman filter and the habits of checking it. The world of robots is rarely linear: the next module linearises the models around the current estimate (the extended Kalman filter) and then replaces the linearisation with sigma points (the unscented transform).

Related content

Nothing below is required to finish this lesson.

• Before
  M7-T3: Innovation, Gain and Tuning Q and R

  Explain the innovation and its covariance, predict the effect of changing Q and R, and tune a filter from its residuals.

• Lab
  M7-L1: 1D Kalman Filter

  Rover drives along a rail with a noisy range sensor and slipping wheels. Implement predict and update, tune Q and R, and prove with the innovation that the filter is consistent.

Other courses on this site treat neighbouring topics from their own angle:

• UAV M3-T4: Kalman Filter
• UAV M3-L1: Implement a Kalman Filter in Python
• Perception P7-T4: Simple Filtering and Kalman Filter Intuition
• UAV M3-T15: Normalized Estimation Error Squared