M2-T16: Multi-sensor GNSS-denied navigation architectures

Learning objectives

After this lesson, you should be able to:

• Explain the complementary failure modes of GNSS, vision, LiDAR and inertial sensing.
• Draw a layered architecture with an IMU-driven core, monitored aiding sources and a navigation manager.
• Define navigation confidence levels and the modes they trigger.
• Choose between loosely and tightly coupled integration for a given design.

Conceptual explanation

A layered GNSS-denied architecture has four parts: (1) an IMU-driven core estimator that always runs and provides continuous, smooth state; (2) aiding sources (GNSS, visual-inertial odometry, LiDAR-inertial odometry) that correct its drift; (3) monitors attached to each source, which publish health and covariance; (4) a navigation manager that turns monitor outputs into a confidence level and a mode.

Engineering intuition

Choose the aiding sources so that a condition that kills one rarely kills the other: a dark corridor defeats vision but gives LiDAR strong structure on both walls and floor; a long featureless corridor can defeat LiDAR geometry but a lit, textured wall helps vision. Diversity beats redundancy of the same type. And what matters as much as the sensors is knowing when each is lying: the manager must see health and covariance, and the estimator must inflate uncertainty honestly when a source degrades, or the confidence metric is meaningless.

Mathematical formulation

Loose versus tight coupling

Loosely coupled: each odometry subsystem produces its own pose (and covariance); the core filter fuses those poses as measurements. Simple and modular, but the subsystem’s internal uncertainty is approximated by a single covariance and correlations are lost. Tightly coupled: the filter or optimiser processes raw features or scan residuals together with IMU, so it can still extract information when too few features exist for a standalone pose.

Confidence

One workable definition combines the estimator’s covariance and the source health:

$\sigma _{\text{pos}}$

one-sigma position uncertainty from the estimator’s covariance (m).

$s_{g}$, $s_{a}$

thresholds derived from the mission requirement (for example allowed error against the smallest clearance) and verified in test.

Thresholds are requirements, not defaults: derive them from the smallest margin the mission must respect (module 8, lesson M8-T1).

Mode logic

Architecture and data flow

Monitors publish a small health record per source: timestamp, validity, covariance (or inflation factor), and specific indicators (feature count, inlier ratio, registration residual, satellite count, NIS). The estimator publishes state and covariance. The manager consumes both and publishes mode and confidence to the planner and mission layer; the planner uses them to bound speed and choose conservative paths. This record format is an interface worth writing in the ICD (lesson M7-T17).

Real-world UAV applications

• Warehouse, tunnel and under-canopy inspection where GNSS is unavailable.
• Urban delivery where GNSS degrades between buildings.
• Search and rescue in collapsed structures with dust and darkness (LiDAR and radar over vision).
• Defence and critical infrastructure where jamming and spoofing are expected.

Implementation example

This runs as shown: a small, testable navigation manager. Feed it monitor outputs and it returns the confidence level and mode, with a failsafe timer.

from dataclasses import dataclass

@dataclass
class Health:
    gnss_ok: bool
    vio_ok: bool
    lio_ok: bool
    sigma_pos: float        # estimator 1-sigma position uncertainty (m)

class NavManager:
    def __init__(self, s_green=3.0, s_amber=10.0, red_timeout=8.0):
        self.s_green, self.s_amber, self.red_timeout = s_green, s_amber, red_timeout
        self.red_since = None; self.failsafe = False

    def update(self, t, h: Health):
        odo = h.vio_ok or h.lio_ok
        if h.sigma_pos < self.s_green and h.gnss_ok and odo:
            conf = "GREEN"                      # GNSS and odometry agree and the estimate is tight
        elif h.sigma_pos < self.s_amber and (odo or h.gnss_ok):
            conf = "AMBER"                      # one kind of aiding left, uncertainty bounded
        else:
            conf = "RED"
        if conf == "RED":
            self.red_since = t if self.red_since is None else self.red_since
            if t - self.red_since > self.red_timeout: self.failsafe = True
        else:
            self.red_since = None
        mode = "FAILSAFE" if self.failsafe else {"GREEN": "NOMINAL", "AMBER": "DEGRADED", "RED": "DEAD_RECKONING"}[conf]
        return conf, mode

m = NavManager()
assert m.update(0.0, Health(True, True, False, 0.4)) == ("GREEN", "NOMINAL")
assert m.update(1.0, Health(False, True, False, 0.8)) == ("AMBER", "DEGRADED")        # GNSS lost, vision aids
assert m.update(2.0, Health(False, False, False, 2.5))[0] == "RED"                    # nothing aiding
assert m.update(9.0, Health(False, False, False, 6.0)) == ("RED", "DEAD_RECKONING")   # 7 s: still within the timeout
assert m.update(11.0, Health(False, False, False, 9.0)) == ("RED", "FAILSAFE")        # 9 s of RED: failsafe latches
print("navigation manager behaves as specified")

Common engineering mistakes

Failure modes

Debugging approach

1. Replay logged data with each sensor removed in turn and watch confidence and mode; any case that stays GREEN is a gap.
2. Compare reported $\sigma$ to actual error against an external reference: the error should rarely exceed $3\sigma$.
3. Check hysteresis by sweeping a monitored value up and down across a threshold.
4. Review the log for the latency from fault onset to mode change.

Practical exercises

1. Choose sensors for a mine-shaft inspection UAV and justify the diversity.
2. Add hysteresis to the manager above so that it returns to GREEN only after 3 s of good health.
3. Derive a RED timeout from a drift rate of 0.2 m/s and a 2 m clearance margin.
4. In the GNSS-Denied Failure Simulator, find a fault pair that defeats the architecture without triggering the failsafe in time.

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.

• $P$. $D$. Groves, Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems, 2nd ed., Artech House, 2013 (integration architectures and integrity).
• T. Qin et al., “A general optimization-based framework for local odometry estimation with multiple sensors”, arXiv:1901.03638 (the VINS-Fusion local odometry framework)
• PX4: external position estimation

What’s next