M4-T15: Localization failure detection

Learning objectives

After this lesson, you should be able to:

• List the evidence of localisation health: fit residual, inlier ratio, covariance and innovation statistics, cross-checks.
• Build a monitor with thresholds and hysteresis and measure its detection delay and false alarms.
• Explain why confidence must accompany the pose.
• Connect the monitor to the navigation manager and failsafe design.

Conceptual explanation

Three families of evidence: fit quality (registration residual, inlier ratio, number of matched features or points, particle-filter effective sample size and likelihood); statistical consistency (NIS and NEES-style tests, covariance growth; lessons M3-T13, M3-T14); and independent cross-checks (odometry versus map-based pose jumps, a barometer or GNSS where available; lesson M2-T6). The monitor outputs a confidence level (lesson M2-T16), not only a binary flag.

Engineering intuition

A localiser fails in two ways: it loses (no match: the evidence disappears and is easy to see) or it locks onto the wrong place (a good match to the wrong map region: the fit is fine and only independent evidence reveals it). A pose that suddenly jumps relative to the odometry by more than the odometry could have drifted is the classic signature of the second. The monitor must therefore compare the localiser with something else.

Mathematical formulation

A simple consistency check between the map-based pose $p_{\text{map}}$ and the odometry-propagated pose $p_{\text{odo}}$, with respective covariances:

Detection design: a sliding-window score $s_{k}$ (fraction of inlier points) with enter threshold $s_{\text{in}}$ and a lower exit threshold $s_{\text{out}}$; hold time $T_{\text{hold}}$ before returning to healthy. Report the detection delay after a failure and the false-alarm rate on healthy data (lesson M3-T14).

$P_{\text{map}}$, $P_{\text{odo}}$

covariance of each pose estimate.

$d^{2}$

squared Mahalanobis distance between the two poses.

Architecture and data flow

Localiser publishes pose, covariance and diagnostics (inlier ratio, residual, $N_{\text{eff}}$) → monitor → confidence → navigation manager (modes, lesson M2-T16; degraded behaviour, lesson M8-T10). On failure the manager switches the controller’s position source to odometry only, freezes map-based corrections, and starts a relocalisation procedure (hover, spin, search) or a failsafe timer.

Real-world UAV applications

• Safe use of a prior map in GNSS-denied flight.
• Triggering relocalisation after a layout change.
• Feeding the failsafe design in module 8.
• Logging localisation health for post-flight analysis.

Implementation example

This runs as shown: a simulated flight where the localiser is right until $t=30\,\mathrm{s}$ and then locks onto a wrong place (a 6 m jump) with a good fit. The fit-only monitor misses it; the odometry cross-check detects it within a second; hysteresis avoids chatter.

import numpy as np
rng = np.random.default_rng(1)
dt, n, fail_at = 0.1, 600, 300
t = np.arange(n) * dt; truth = np.stack([3.0 * t, 0 * t], axis=1)
odo = np.zeros((n, 2)); drift = np.cumsum(0.003 * rng.standard_normal((n, 2)), axis=0)            # slow odometry error (std 0.03 m per sqrt(s) here)
odo = truth + drift
loc = truth + 0.05 * rng.standard_normal((n, 2)); loc[fail_at:] += np.array([6.0, 2.0])         # the localiser locks onto the wrong place at t = 30 s
inlier = 0.85 + 0.03 * rng.standard_normal(n)                                                       # fit quality stays good after the failure

P_map, P_odo = np.eye(2) * 0.05 ** 2, np.eye(2) * 0.4 ** 2                                         # reported covariances (odometry allowed a little drift)
chi2_999 = 13.82                                                                                    # chi-square(2) 99.9%

def monitor(use_crosscheck, hold=3.0):
    healthy, good_since, status, first_alarm, false_alarm = True, None, [], None, 0
    for k in range(n):
        d = loc[k] - odo[k]; d2 = d @ np.linalg.inv(P_map + P_odo) @ d
        bad = inlier[k] < 0.5 or (use_crosscheck and d2 > chi2_999)
        if bad:
            healthy, good_since = False, None
            if first_alarm is None and k >= fail_at: first_alarm = k
            if k < fail_at: false_alarm += 1
        elif not healthy:
            good_since = t[k] if good_since is None else good_since
            if t[k] - good_since >= hold: healthy = True
        status.append(healthy)
    return first_alarm, false_alarm, status

a_fit, fa_fit, _ = monitor(False); a_x, fa_x, st = monitor(True)
print(f"fit-only monitor: first alarm {a_fit}; with the odometry cross-check: first alarm at step {a_x} (failure at {fail_at}), false alarms before the failure: {fa_x}")
assert a_fit is None                                                                              # a good fit to the wrong place is invisible to the fit evidence
assert a_x is not None and a_x - fail_at <= 10 and fa_x == 0 and not any(st[a_x:])               # detected within 1 s, no false alarms, stays unhealthy (hysteresis)

Common engineering mistakes

Failure modes

Debugging approach

1. Replay logs with injected pose jumps and measure detection delay.
2. Plot $d^{2}$ and inlier ratio against time.
3. Measure false alarms over hours of healthy data.
4. Check that the manager reacts to every status transition.

Practical exercises

1. Choose thresholds for 1 false alarm per 100 flight hours given your $d^{2}$ distribution.
2. Add a barometer cross-check for altitude jumps.
3. Design the relocalisation procedure for a UAV that loses its map-based pose.
4. Write the log fields needed to investigate a localisation failure afterwards.

Interview questions

Write an answer in your own words, then reveal the model answer. Nothing is sent anywhere.

References and further reading

Primary papers, official documentation and standards. Each claim above that is specific to a tool or version points here.

• PX4: innovation and consistency checks in EKF2
• $Y$. Bar-Shalom, $X$. $R$. Li, $T$. Kirubarajan, Estimation with Applications to Tracking and Navigation, Wiley, 2001, chapter 5.
• C. Cadena et al., Past, present, and future of SLAM (arXiv:1606.05830)

What’s next