Jump to
MODULE 04 // GNSS-DENIED NAVIGATION

GNSS-Denied Navigation

GPS can be jammed by a $50 handheld device from 30 km away. In a contested military environment, a UAV that relies solely on GPS is a UAV that the enemy can neutralise without firing a single shot. This module builds the complete solution stack — from why IMUs drift, to how Kalman filters fuse sensors optimally, to visual-inertial odometry and terrain matching — all aligned with Bestaoui Sebbane §7 and Halesworth §5.

EKF / UKF Theory Visual-Inertial Odometry Terrain-Aided Navigation DDIL Environments

📚  REFERENCES: Bestaoui Sebbane §7  ·  Halesworth §5  ·  MDPI Autonomous Control Ch.3  ·  Fahlstrom §3.5

4.1 The Problem — Why GPS Cannot Be the Only Answer

Plain English First

GPS works by receiving radio signals from satellites 20,000 km above Earth. By the time those signals reach the ground, they are incredibly faint — about 10 billion times weaker than a typical WiFi signal. Any radio transmitter powerful enough to operate in the same frequency band can drown out those signals entirely. This is GPS jamming, and it costs almost nothing. A more sophisticated attack — GPS spoofing — sends fake GPS signals that trick the receiver into reporting a completely wrong position with full confidence. In a modern contested battlefield, both attacks happen routinely. A UAV that trusts GPS unconditionally in this environment will fly somewhere it should not, or crash.

The Asymmetry That Makes This a Hard Problem

The fundamental asymmetry of GPS jamming is captured by the J/S ratio from Module 01. Let's quantify exactly how easy denial is in practice: Fahlstrom §3.5

GPS Signal Budget at the Receiver $$P_{GPS} \approx -130 \text{ dBm} \approx 10^{-16} \text{ W} \qquad \text{(GPS L1 at Earth's surface)}$$ $$\frac{J}{S}\bigg|_{dB} = (P_j + G_j) - (P_{GPS} + G_{rx}) + 20\log_{10}\!\left(\frac{R_s}{R_j}\right)$$

A 10 W handheld jammer at 30 km range achieves $J/S \approx +55$ dB — far above the +25 dB denial threshold. This means GPS can be denied by non-state actors using equipment costing under $100. Military GPS receivers use controlled reception pattern antennas (CRPAs) and military encryption (M-code) which raise the denial threshold to ~+40 dB — but a 100 W directed jammer still wins at any range under ~80 km. Bestaoui Sebbane §7.1

Types of GNSS Attack

AttackMechanismUAV SymptomDetection DifficultyCountermeasure
Jamming Broadband noise overwhelms receiver GPS fix lost, "no signal" alert Easy — receiver knows it lost lock Switch to IMU+VIO, terrain nav
Meaconing Receive and re-broadcast GPS signals with delay Position slowly drifts to wrong location Medium — gradual drift, no alarm Cross-check against barometer, inertial
Spoofing Transmit false GPS signals at higher power UAV navigates confidently to wrong place Hard — receiver shows full confidence Multi-constellation, signal authentication, IMU cross-check
GNSS-denied zone Urban canyons, tunnels, signal shadows Intermittent fix loss Easy — geometry known Pre-planned autonomy, map-aided nav
The RQ-170 Incident — Spoofing in Action In December 2011, a US RQ-170 Sentinel stealthy reconnaissance UAV was captured largely intact by Iran. Iranian engineers later claimed — and US analysts largely accepted — that the aircraft was brought down by GPS spoofing: false GPS signals caused the autopilot to think it was approaching its home base at Kandahar while it was actually over Iranian territory, triggering an automatic landing at the wrong location. The incident directly accelerated US military investment in GNSS-denied navigation. Bestaoui Sebbane §7.1

4.2 IMU Dead Reckoning — And Why It Always Drifts

Plain English First

When GPS is lost, the first fallback is the IMU (Inertial Measurement Unit) — a chip that measures acceleration and rotation rates. By integrating (accumulating) those measurements over time, you can track how position has changed from a known starting point. This is called dead reckoning, and sailors have used it for centuries. The problem: integration amplifies errors. A tiny constant bias in the accelerometer — say, 0.01 m/s² — becomes a velocity error of 0.6 m/s after one minute, and a position error of 18 metres after one minute. After 10 minutes: 1,800 metres of error. Dead reckoning alone is not a solution; it is a bridge between better measurement updates.

Analogy — Walking Blindfolded

Imagine walking blindfolded and trying to track your position by counting footsteps and estimating which direction each step goes. After 10 steps you're reasonably accurate. After 100 steps your small angular errors have compounded into a big directional error. After 500 steps you have no idea where you are. An IMU does exactly this — but at 1000 measurements per second. The integration errors still compound just as inexorably; they just accumulate faster.

IMU Error Sources — The Allan Variance Model

IMU errors are characterised using Allan variance analysis, which separates the noise into distinct physical processes: Bestaoui Sebbane §7.2

IMU Stochastic Error Model — Allan Variance $$\sigma^2(\tau) = \frac{N^2}{\tau} + B^2 \frac{\tau}{3} + \frac{K^2\tau}{3}$$

Three noise regimes:
$N$: Angle/Velocity Random Walk — white noise from electronics. Averages down with $\sqrt{\tau}$. Units: deg/√hr (gyro) or m/s/√hr (accel).
$B$: Bias Instability — low-frequency 1/f noise. Irreducible minimum error floor. Units: deg/hr or m/s².
$K$: Rate Random Walk — slowly wandering bias. Grows with $\sqrt{\tau}$. Units: deg/hr/√hr or m/s²/√hr.

Position Error Growth — Why Drift Is Unavoidable

The key result that every navigation engineer must internalise: position error from accelerometer bias grows quadratically with time. Bestaoui Sebbane §7.3

Dead Reckoning Position Error from Accelerometer Bias $$\delta v(t) = b_a \cdot t \qquad \text{(velocity error grows linearly)}$$ $$\delta p(t) = \frac{1}{2} b_a \cdot t^2 \qquad \text{(position error grows quadratically)}$$

$b_a$: accelerometer bias [m/s²]. Even after gravity subtraction and frame rotation corrections, residual bias remains. Attitude error $\delta\theta$ also contributes to position error via gravity vector misalignment: $\delta p^{gravity}(t) \approx \frac{1}{2}g\,\delta\theta^2\,t^2$. For a MEMS-grade IMU ($b_a \approx 0.05$ m/s²), position error after 60 seconds = 90 m. For a navigation-grade IMU ($b_a \approx 5\times10^{-5}$ m/s²), error after 60 seconds = 0.09 m — but these cost $10,000–$100,000 and weigh 500 g–2 kg. The tactical trade-off is stark.

Worked Example 4.1 — IMU Grade Comparison for GNSS-Denied ISR

Compare three IMU grades for a 5-minute GNSS-denied navigation segment. Mission requirement: position error < 10 m after 5 minutes.

IMU GradeAccel Bias $b_a$$\delta p$ at 60 s$\delta p$ at 300 sMeets 10 m req?Typical Cost
COTS MEMS (e.g., ICM-42688)0.10 m/s²180 m4,500 mNo$5–$20
Tactical MEMS (e.g., ADIS16488)0.01 m/s²18 m450 mNo$500–$2,000
Navigation Grade (e.g., HG9900)5×10⁻⁵ m/s²0.09 m2.25 mYes$50,000+

Conclusion: Only a navigation-grade IMU meets the 10 m / 5-minute requirement using dead reckoning alone — and it costs more than the entire UAV in most Group 1–2 platforms. This is the core engineering motivation for EKF-based sensor fusion: use a cheap MEMS IMU for high-rate propagation, but correct its drift with low-rate, higher-quality measurements from cameras, LiDAR, or terrain matching.

4.3 Bayesian State Estimation — The Conceptual Foundation

Plain English First

The IMU drifts. The camera is noisy. The barometer fluctuates with weather. No single sensor gives you a perfect answer. The right approach is to combine all of them, weighting each by how much you trust it. If the IMU has been running for 10 seconds and has accumulated large drift, trust it less. If the camera just matched a clear landmark, trust that more. Bayesian estimation is the formal mathematical framework for doing exactly this — maintaining a probability distribution over "where am I?" and updating it as new evidence arrives.

Analogy — A Doctor Diagnosing a Patient

A doctor doesn't diagnose from a single test. They start with a prior belief based on symptoms, update it when blood tests come back, update it again when the scan result arrives, and at each stage the diagnosis reflects all evidence weighted by reliability. Bayesian estimation does exactly the same thing for UAV position — it maintains a running "best guess" probability distribution that gets updated every time a new sensor measurement arrives, weighted by how reliable that sensor is.

Formally, Bayesian estimation maintains the posterior distribution $p(\mathbf{x}_k \mid \mathbf{z}_{1:k})$ — the probability of being in state $\mathbf{x}_k$ given all measurements up to time $k$: Bestaoui Sebbane §7.4

Recursive Bayesian Update $$\underbrace{p(\mathbf{x}_k \mid \mathbf{z}_{1:k})}_{\text{posterior}} \propto \underbrace{p(\mathbf{z}_k \mid \mathbf{x}_k)}_{\text{likelihood}} \cdot \int \underbrace{p(\mathbf{x}_k \mid \mathbf{x}_{k-1})}_{\text{motion model}} \cdot \underbrace{p(\mathbf{x}_{k-1} \mid \mathbf{z}_{1:k-1})}_{\text{previous posterior}} \, d\mathbf{x}_{k-1}$$

Reading this in plain English: "My new belief about where I am equals my measurement's likelihood times my prediction from the motion model times my previous belief, summed over all possible previous states."

Why it's hard: The integral is analytically tractable only for linear-Gaussian systems (Kalman filter). For nonlinear systems (real UAVs), we approximate. Three main approximations: EKF (linearise), UKF (sigma points), Particle filter (Monte Carlo). Each trades off computational cost vs. accuracy in nonlinear regimes. MDPI Autonomous Control Ch.3

4.4 Extended Kalman Filter — Step by Step

Plain English First

The Extended Kalman Filter (EKF) is the most widely used navigation algorithm in the world — it runs inside every smartphone, GPS receiver, and military navigation system. It works in two alternating phases: Predict (use the IMU to guess where you'll be next) and Update (use a measurement to correct that guess). The "extended" part means it handles nonlinear systems by taking a first-order Taylor series approximation — linearising the equations around the current best estimate. This works well when the estimate is reasonably accurate, and breaks down only in highly nonlinear regimes or with very poor initialisation.

The State Vector

First, we define what the EKF is tracking. For full navigation: Bestaoui Sebbane §7.5

Navigation EKF State Vector (16 states) $$\mathbf{x} = \begin{bmatrix} \mathbf{p}^T & \mathbf{v}^T & \mathbf{q}^T & \mathbf{b}_a^T & \mathbf{b}_g^T \end{bmatrix}^T \in \mathbb{R}^{16}$$

$\mathbf{p} \in \mathbb{R}^3$: position (NED frame)  ·  $\mathbf{v} \in \mathbb{R}^3$: velocity (NED frame)  ·  $\mathbf{q} \in \mathbb{R}^4$: attitude quaternion (unit constraint → 3 DOF effective)  ·  $\mathbf{b}_a \in \mathbb{R}^3$: accelerometer bias  ·  $\mathbf{b}_g \in \mathbb{R}^3$: gyroscope bias.

The bias states are crucial — by estimating and correcting for sensor bias in real time, the EKF continuously calibrates the IMU during flight. This is called "in-flight calibration" and is why a well-tuned EKF produces much better navigation than raw IMU integration, even without external corrections.

The Two-Step EKF Algorithm

▸ Step 1: Predict (IMU Propagation, ~1000 Hz)

Use the IMU reading to propagate the state forward one time step. This is cheap — just integrate the kinematic equations from Module 02. The uncertainty (covariance $P$) grows because IMU noise is added each step.

▸ Step 2: Update (Measurement Correction, ~10–100 Hz)

When a new measurement arrives (camera feature match, barometer reading, etc.), compute the Kalman gain and correct both the state estimate and covariance. The covariance shrinks — new information reduces uncertainty.

EKF Predict Step — Bestaoui Sebbane §7.5 $$\hat{\mathbf{x}}_{k|k-1} = f\!\left(\hat{\mathbf{x}}_{k-1|k-1},\, \mathbf{u}_k\right)$$ $$P_{k|k-1} = F_k\, P_{k-1|k-1}\, F_k^T + Q_k$$

$f(\cdot)$: nonlinear state transition function (IMU kinematic equations — integrate position, velocity, quaternion). $F_k = \partial f/\partial \mathbf{x}\big|_{\hat{\mathbf{x}}}$: Jacobian of $f$ — the linearisation that makes EKF "extended." $Q_k$: process noise covariance matrix — populated from IMU noise specs (Allan variance parameters). $P_{k|k-1}$: predicted covariance — uncertainty grows during prediction.

EKF Update Step — Bestaoui Sebbane §7.5 $$\mathbf{y}_k = \mathbf{z}_k - h\!\left(\hat{\mathbf{x}}_{k|k-1}\right) \qquad \text{(innovation)}$$ $$S_k = H_k\, P_{k|k-1}\, H_k^T + R_k \qquad \text{(innovation covariance)}$$ $$K_k = P_{k|k-1}\, H_k^T\, S_k^{-1} \qquad \text{(Kalman gain)}$$ $$\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + K_k\, \mathbf{y}_k$$ $$P_{k|k} = (I - K_k H_k)\, P_{k|k-1}$$

$\mathbf{z}_k$: actual measurement; $h(\cdot)$: expected measurement given state (measurement model); $\mathbf{y}_k$: innovation — "how surprised were we by the measurement?"; $H_k = \partial h/\partial \mathbf{x}$: measurement Jacobian; $R_k$: measurement noise covariance (from sensor specs); $K_k$: Kalman gain — the optimal weighting between prediction and measurement.

Theorem 4.1 — Kalman Gain as Optimal Weighting

The Kalman gain $K_k$ minimises the mean squared estimation error. It can be interpreted as a certainty ratio:

$$K_k = \frac{P_{k|k-1} H_k^T}{H_k P_{k|k-1} H_k^T + R_k} = \frac{\text{predicted uncertainty}}{\text{predicted uncertainty} + \text{measurement noise}}$$

Two limiting cases:
If $R_k \to 0$ (perfect sensor): $K_k \to H_k^{-1}$ — trust the measurement completely, ignore the prediction.
If $P_{k|k-1} \to 0$ (perfect model): $K_k \to 0$ — trust the prediction completely, ignore the measurement.
In practice, both have noise, and $K_k$ blends them by their relative uncertainties. This is the optimal linear estimator — no filter can do better for linear-Gaussian systems. Bestaoui Sebbane §7.6

Worked Example 4.2 — Kalman Gain for Altitude Fusion

At time step $k$, the EKF predicts altitude 52.3 m with uncertainty $P_{zz} = 4.0$ m². A barometer reads 51.1 m with measurement noise $R = 2.25$ m² (1.5 m std dev). Compute the Kalman gain, the updated altitude, and the updated uncertainty.

For scalar altitude, $H = 1$ (measurement directly observes altitude). So:

Innovation covariance: $S = H P_{zz} H^T + R = 1 \times 4.0 \times 1 + 2.25 = \mathbf{6.25}$ m²

Kalman gain: $K = P_{zz} H^T / S = 4.0 / 6.25 = \mathbf{0.64}$ (64% weight to the measurement)

Innovation: $y = 51.1 - 52.3 = -1.2$ m

Updated altitude: $\hat{z} = 52.3 + 0.64 \times (-1.2) = 52.3 - 0.768 = \mathbf{51.53}$ m

Updated uncertainty: $P_{zz}^+ = (1 - 0.64) \times 4.0 = 0.36 \times 4.0 = \mathbf{1.44}$ m² (std dev drops from 2.0 m to 1.2 m)

Interpretation: Neither the prediction (52.3 m) nor the measurement (51.1 m) is fully trusted. The fused estimate (51.53 m) sits between them, closer to the measurement because it was weighted 64%. The uncertainty shrank from 4.0 to 1.44 m² — the measurement gave us new information.

4.5 Unscented Kalman Filter — When EKF Linearisation Fails

Plain English First

The EKF approximates nonlinear functions by a straight line (first-order Taylor series). This is fine when the function is gently curved near the current estimate. But for highly nonlinear systems — like attitude estimation during aggressive manoeuvres, or bearing-only navigation — the linearisation is too crude and the filter diverges. The UKF (Unscented Kalman Filter) solves this by not linearising at all. Instead, it carefully selects a small set of sample points — called sigma points — passes each one through the exact nonlinear function, then reconstructs the mean and covariance from the results. It's like sampling a few representative points to understand a curve, rather than drawing a tangent line at one point.

Sigma Points — The Key Idea

For a state of dimension $n$ with mean $\hat{\mathbf{x}}$ and covariance $P$, the UKF generates $2n + 1$ sigma points: MDPI Autonomous Control Ch.3

UKF Sigma Point Selection $$\mathcal{X}^{(0)} = \hat{\mathbf{x}}$$ $$\mathcal{X}^{(i)} = \hat{\mathbf{x}} + \left(\sqrt{(n+\lambda)P}\right)_i, \quad i = 1,\ldots,n$$ $$\mathcal{X}^{(i+n)} = \hat{\mathbf{x}} - \left(\sqrt{(n+\lambda)P}\right)_i, \quad i = 1,\ldots,n$$

$\lambda = \alpha^2(n + \kappa) - n$: scaling parameter. $\left(\sqrt{(n+\lambda)P}\right)_i$: $i$-th column of the matrix square root (Cholesky factor). Each sigma point is a deterministic sample that captures the mean and covariance exactly. The points are propagated through the nonlinear function $f(\cdot)$ exactly — no Jacobian required.

Theorem 4.2 — UKF vs EKF Accuracy

For a scalar nonlinear function $y = f(x)$ with Gaussian input $x \sim \mathcal{N}(\mu, \sigma^2)$:

$$\mathbb{E}[f(x)] = f(\mu) + \frac{1}{2}f''(\mu)\sigma^2 + O(\sigma^4) \qquad \text{(true mean)}$$ $$\mathbb{E}_{EKF}[f(x)] = f(\mu) \qquad \text{(EKF approximation — first order)}$$ $$\mathbb{E}_{UKF}[f(x)] = f(\mu) + \frac{1}{2}f''(\mu)\sigma^2 + O(\sigma^4) \qquad \text{(UKF — captures second order)}$$

The UKF captures the second-order term $\frac{1}{2}f''(\mu)\sigma^2$ that the EKF misses. This term becomes significant when the uncertainty $\sigma^2$ is large (e.g., poor GPS initialisation or after a long GNSS-denied period). In those conditions, EKF under-estimates the mean error and diverges; UKF remains accurate. Computational cost: UKF requires $2n+1$ function evaluations per step (vs 1 for EKF) plus a matrix square root — roughly 2–4× more expensive than EKF for typical navigation state dimensions. Bestaoui Sebbane §7.7

PropertyEKFUKFParticle Filter
Nonlinearity handling1st-order Taylor (Jacobian)2nd-order (sigma points)Exact (Monte Carlo)
Gaussian assumptionRequiredRequiredNot required
Computational costLow — $O(n^3)$Medium — $O(n^3)$ × 2–4High — $O(N_{particles})$
Jacobian required?Yes — must derive analyticallyNoNo
Military UAV useStandard (GPS/INS, SLAM)High-manoeuvre, VIOTerrain matching, SLAM
ROS2/PX4 availabilityEKF2 module (PX4 default)robot_localization packageAMCL (2D), custom (3D)

4.6 Visual-Inertial Odometry (VIO)

Plain English First

Visual-Inertial Odometry combines a camera with an IMU to estimate how the UAV is moving — without any external reference like GPS. The camera sees the world and tracks how visual features move between frames. The IMU provides rapid, high-rate motion measurements between camera frames. Together, they lock each other's weaknesses: the IMU drifts over time but is fast; the camera doesn't drift but is slow and can blur. The result is a navigation solution that can maintain sub-metre accuracy for several minutes in a fully GPS-denied environment — which is transformative for military UAVs operating in jammed or urban environments.

The VIO Cost Function — Tightly Coupled Formulation

Tight coupling means camera features and IMU measurements are fused in a single nonlinear least-squares optimisation, rather than two separate filters: Bestaoui Sebbane §7.8

VIO Joint Cost Function (MSCKF / Sliding Window Smoother) $$\min_{\mathbf{x}_{0:T},\, \mathbf{p}_{1:N}} \underbrace{\sum_{k=1}^{T} \left\| \mathbf{r}_{IMU}^{(k)} \right\|^2_{Q_k^{-1}}}_{\text{IMU preintegration residuals}} + \underbrace{\sum_{k=0}^{T} \sum_{i \in \mathcal{V}_k} \left\| \mathbf{z}_{k,i} - \pi\!\left(R_k^T(\mathbf{p}_i^W - \mathbf{t}_k)\right) \right\|^2_{\Sigma_z^{-1}}}_{\text{Visual reprojection residuals}}$$

First term — IMU residuals: $\mathbf{r}_{IMU}^{(k)}$: difference between predicted and measured IMU preintegration. Penalises inconsistency between IMU readings and the estimated trajectory.

Second term — Visual reprojection: $\mathbf{z}_{k,i}$: observed pixel location of feature $i$ in frame $k$. $\pi(\cdot)$: camera projection function (projects 3D world point to 2D pixel). $\mathbf{p}_i^W$: 3D position of feature $i$ in world frame. $(R_k, \mathbf{t}_k)$: camera pose at time $k$. The residual is "where did I expect to see this feature vs. where I actually saw it?" This nonlinear least-squares problem is solved iteratively (Gauss-Newton or LM algorithm).

VIO Algorithms Used in Military UAV Research

AlgorithmTypeCouplingKey StrengthROS2 Package
MSCKFFilterTightComputationally efficient; no map storageopen_vins
VINS-MonoSmootherTightLoop closure; accurate over long distancesvins_mono (ROS1→2)
ORB-SLAM3SmootherTightMap reuse across missions; multi-sessionorb_slam3_ros2
OpenVINSFilterTightOpen source, research-grade, modularopen_vins
Kimera-VIOSmootherTightSemantic map; 3D mesh reconstructionkimera_ros

VIO Failure Modes in Military Operations

Understanding failure conditions is as important as understanding normal operation. Halesworth §5.3

Failure ModePhysical CauseConsequenceMitigation
Low-texture sceneDesert, snow, water — no distinguishable featuresFeature tracking fails → no visual update → IMU driftIR camera (thermal texture); add active illumination
Pure rotationUAV rotating without translating (hovering yaw scan)3D triangulation degenerates — cannot estimate depthIMU provides rotation-only motion prior; use different sensor
Motion blurHigh angular rate during aggressive manoeuvresFeatures smear → mismatched tracks → bad estimateHigh-shutter-speed camera; IMU-aided rolling shutter correction
Illumination extremesNight, direct sun, muzzle flash, smokeOver/under-exposed frames → feature detector failsActive illumination (NIR LED); event camera
Dynamic objectsMoving vehicles, people in sceneDynamic points treated as static features → biased estimateOutlier rejection; semantic masking (deep learning)
VIO in ROS2 — The Halesworth §5 Stack Halesworth documents the complete ROS2 navigation pipeline for GNSS-denied drones. The key nodes: imu_filter_madgwick for attitude pre-filtering → open_vins or orb_slam3 for VIO pose estimation → robot_localization (EKF node) for sensor fusion → nav2 stack for path planning and obstacle avoidance. The entire pipeline operates at 30–100 Hz on an NVIDIA Jetson Orin (15W power budget) — feasible for a Group 3+ UAV platform. Halesworth §5.4

4.7 Terrain-Aided Navigation — Matching the Ground Below

Plain English First

Terrain-Aided Navigation (TAN) is an elegant idea: every patch of terrain on Earth is unique — hills, valleys, and flatlands form a fingerprint. If a UAV measures the terrain elevation profile below it (using a radar altimeter or LiDAR) and compares it to a stored digital map, it can figure out exactly where it is — with no GPS, no radio transmission, and no landmarks needed. This is essentially the navigation equivalent of reading a map by feel, in complete radio silence. The Tomahawk cruise missile has used terrain matching since the 1980s; modern military UAVs combine it with VIO for continuous drift-free navigation.

TERCOM — Terrain Contour Matching

TERCOM was developed for the Tomahawk BGM-109 cruise missile and is the foundational terrain navigation algorithm. It works by correlating a measured terrain profile against stored DTED (Digital Terrain Elevation Data): Bestaoui Sebbane §7.9

TERCOM Mean Absolute Difference Correlation $$\text{MAD}(x, y) = \frac{1}{N} \sum_{i=1}^{N} \left| h_m(i) - h_s(x + i\Delta x,\; y) \right|$$ $$\hat{x}_{pos}, \hat{y}_{pos} = \arg\min_{x,y} \text{MAD}(x, y)$$

$h_m(i)$: measured terrain elevation profile (from radar altimeter or LiDAR) at $N$ sample points; $h_s(x,y)$: stored DTED elevation at grid coordinate $(x,y)$; $\Delta x$: along-track spacing between samples. The position fix is the map location that best matches the measured profile. Accuracy is limited by DTED resolution: 30 m (DTED-2), 10 m (DTED-3), 1 m (high-resolution). Works best in rough terrain; fails in flat plains (no distinguishing features).

SITAN — Continuous EKF-Based Terrain Navigation

SITAN (Sandia Inertial Terrain-Aided Navigation) extends TERCOM by integrating terrain matching into an EKF, providing continuous position corrections rather than discrete fixes: Bestaoui Sebbane §7.10

SITAN Measurement Model $$z_k = h_{altimeter,k} - h_s(x_k, y_k) + v_k, \qquad v_k \sim \mathcal{N}(0, R_k)$$ $$H_k = \left[\frac{\partial h_s}{\partial x},\; \frac{\partial h_s}{\partial y},\; 0,\; \ldots\right] = \nabla h_s(x_k, y_k)$$

The EKF measurement is the difference between measured clearance (altitude above terrain) and the stored terrain height at the estimated position. The measurement Jacobian $H_k$ is the terrain surface gradient — the steeper the terrain, the more position information each altimeter reading provides. This is why TAN accuracy improves dramatically over rough terrain and degrades over plains. The terrain gradient is computed by finite differencing the DTED grid.

Worked Example 4.3 — TERCOM Fix Accuracy vs. DTED Resolution

Estimate the position fix accuracy achievable from TERCOM over mountainous terrain with DTED-2 (30 m posts) vs DTED-3 (10 m posts). Profile length: 30 samples.

DTED LevelPost SpacingVertical AccuracyProfile LengthAchievable Fix Accuracy
DTED-0900 m±30 m30 posts = 27 km~450 m CEP
DTED-190 m±20 m30 posts = 2.7 km~45 m CEP
DTED-230 m±18 m30 posts = 900 m~15 m CEP
DTED-310 m±10 m30 posts = 300 m~5 m CEP

CEP = Circular Error Probable — the radius within which 50% of fixes fall. DTED-3 at 5 m CEP is competitive with degraded GPS (typical: 3–5 m CEP). The caveat: DTED-3 coverage is not global and requires pre-mission data loading. Over featureless terrain (flat desert, open ocean), TERCOM accuracy degrades severely regardless of DTED resolution — this is the fundamental terrain variability requirement.

4.8 Interactive: GNSS-Denied Navigation Visualisation

This simulation shows three simultaneous navigation solutions over a terrain mesh. Green line — ground truth trajectory. Red line — IMU-only dead reckoning (diverges quadratically as bias accumulates). Cyan line — EKF-fused estimate (VIO corrections every ~10 IMU steps — stays close to ground truth). The translucent sphere around the UAV is the covariance ellipsoid — it grows during IMU-only phases (rising uncertainty) and shrinks when VIO corrects the estimate (new information reduces uncertainty). Drag to orbit, scroll to zoom.

◈ GNSS-DENIED NAVIGATION // M04 IMU ERR: —  |  VIO ERR: —
— — — —

Notice how the red (IMU-only) track diverges rapidly — within half the trajectory, it is tens of units off-course. The cyan (VIO-corrected) track stays within a few units of ground truth throughout, demonstrating the 100–500× error reduction that EKF+VIO achieves vs dead reckoning alone (as derived in the Python simulation below).

4.9 Python: EKF Navigation with VIO Updates

What This Code Does Implements a 2D EKF for GNSS-denied navigation (simplification for clarity — the full 3D version follows the same structure with 16 states). State: position (x, y), velocity (vx, vy), heading $\psi$, accelerometer biases $(b_x, b_y)$. IMU runs at 100 Hz; VIO corrections arrive at 10 Hz. Compares dead-reckoning vs EKF+VIO error over a 60-second figure-8 trajectory. Run with: pip install numpy matplotlib
Python
# ekf_gnss_denied.py
# ─────────────────────────────────────────────────────────────────
# 2D EKF navigation in GNSS-denied environment.
# Implements: Bestaoui Sebbane §7.5–7.6
# State: [x, y, vx, vy, psi, b_ax, b_ay]  (7 states)
# IMU propagation: 100 Hz
# VIO position update: 10 Hz
# ─────────────────────────────────────────────────────────────────

import numpy as np

class EKFNavigator:
    """
    Extended Kalman Filter for 2D GNSS-denied navigation.
    Bestaoui Sebbane §7.5

    State vector x = [x, y, vx, vy, psi, b_ax, b_ay]:
      x, y    : position [m] in world frame
      vx, vy  : velocity [m/s] in world frame
      psi     : heading [rad]
      b_ax,b_ay: accelerometer bias [m/s²]
    """

    def __init__(self, dt: float = 0.01):
        self.dt = dt
        self.n  = 7          # state dimension

        # Initial state estimate (start at origin, stationary, level)
        self.x = np.zeros(self.n)

        # Initial covariance — moderate uncertainty on position/velocity
        self.P = np.diag([1.0, 1.0,     # position uncertainty (m²)
                          0.1, 0.1,     # velocity uncertainty (m²/s²)
                          0.01,          # heading uncertainty (rad²)
                          1e-4, 1e-4])  # bias uncertainty (m²/s⁴)

        # Process noise Q — set from IMU Allan variance specs
        # Position noise: dominated by velocity uncertainty integration
        # Velocity noise: from accelerometer noise floor (0.05 m/s² RMS)
        # Heading noise: from gyro noise floor (0.001 rad/s RMS)
        # Bias noise: random walk (1e-5 m/s² per root-second)
        self.Q = np.diag([1e-4, 1e-4,    # position process noise
                          2.5e-3, 2.5e-3,# velocity process noise
                          1e-6,           # heading process noise
                          1e-8, 1e-8])  # bias random walk noise

        # VIO measurement noise — position accuracy ~3 cm (0.03 m)
        self.R_vio = np.diag([0.03**2, 0.03**2])

    def predict(self, accel_body: np.ndarray, yaw_rate: float):
        """
        IMU propagation step — Bestaoui Sebbane §7.5 Eq. 7.12
        accel_body: [ax, ay] measured by IMU in body frame [m/s²]
        yaw_rate:   measured by gyro [rad/s]
        """
        x, y, vx, vy, psi, b_ax, b_ay = self.x
        dt = self.dt

        # Subtract estimated bias from IMU reading
        ax_corr = accel_body[0] - b_ax
        ay_corr = accel_body[1] - b_ay

        # Rotate body-frame acceleration to world frame
        cos_psi, sin_psi = np.cos(psi), np.sin(psi)
        ax_w = ax_corr * cos_psi - ay_corr * sin_psi
        ay_w = ax_corr * sin_psi + ay_corr * cos_psi

        # State propagation (Euler integration)
        self.x = np.array([
            x  + vx*dt + 0.5*ax_w*dt**2,
            y  + vy*dt + 0.5*ay_w*dt**2,
            vx + ax_w*dt,
            vy + ay_w*dt,
            psi + yaw_rate*dt,
            b_ax,    # bias modelled as random walk (constant + noise)
            b_ay,
        ])

        # Jacobian F = ∂f/∂x — linearisation of the motion model
        # Most entries are the identity or simple derivatives
        F = np.eye(self.n)
        # Position depends on velocity
        F[0, 2] = dt;   F[1, 3] = dt
        # Position and velocity depend on heading (rotation of body accel)
        da_dpsi_x = (-ax_corr * sin_psi - ay_corr * cos_psi)
        da_dpsi_y = ( ax_corr * cos_psi - ay_corr * sin_psi)
        F[0, 4] = 0.5 * da_dpsi_x * dt**2
        F[1, 4] = 0.5 * da_dpsi_y * dt**2
        F[2, 4] = da_dpsi_x * dt
        F[3, 4] = da_dpsi_y * dt
        # Velocity depends on bias (negative, since we subtract bias)
        F[2, 5] = -cos_psi * dt;   F[2, 6] =  sin_psi * dt
        F[3, 5] = -sin_psi * dt;   F[3, 6] = -cos_psi * dt

        # Covariance propagation: P = F P F^T + Q
        self.P = F @ self.P @ F.T + self.Q

    def update_vio(self, pos_meas: np.ndarray) -> float:
        """
        VIO position measurement update — Bestaoui Sebbane §7.5 Eq. 7.16
        pos_meas: [x, y] position from VIO [m]
        Returns: innovation magnitude (diagnostic)
        """
        # Measurement model: h(x) = [x, y] — directly observe first two states
        H = np.zeros((2, self.n))
        H[0, 0] = 1.0   # observe x position
        H[1, 1] = 1.0   # observe y position

        # Innovation: z - h(x_predicted)
        innovation = pos_meas - self.x[:2]

        # Innovation covariance: S = H P H^T + R
        S = H @ self.P @ H.T + self.R_vio

        # Kalman gain: K = P H^T S^{-1}  (Theorem 4.1)
        K = self.P @ H.T @ np.linalg.inv(S)

        # State update
        self.x = self.x + K @ innovation

        # Covariance update: P = (I - KH) P (Joseph form for numerical stability)
        IKH = np.eye(self.n) - K @ H
        self.P = IKH @ self.P @ IKH.T + K @ self.R_vio @ K.T

        return float(np.linalg.norm(innovation))


def simulate_gnss_denied(T: float = 60.0, dt: float = 0.01):
    """
    Simulate 60-second GNSS-denied navigation on a figure-8 trajectory.
    Compares three navigation solutions:
    1. IMU-only dead reckoning (no filter, no corrections)
    2. EKF with VIO updates at 10 Hz
    3. Ground truth (perfect knowledge)
    """
    N   = int(T / dt)
    t   = np.linspace(0, T, N)
    ekf = EKFNavigator(dt)

    # Storage arrays
    true_pos  = np.zeros((N, 2))
    imu_pos   = np.zeros((N, 2))
    ekf_pos   = np.zeros((N, 2))

    # IMU-only dead reckoning state (no filter)
    imu_vel  = np.zeros(2)
    imu_p    = np.zeros(2)
    imu_psi  = 0.0

    # Persistent IMU bias (simulates MEMS accelerometer bias)
    TRUE_BIAS = np.array([0.008, 0.005])   # [m/s²] — tactical MEMS grade

    for k in range(N):
        tk = k * dt

        # ── Ground truth: figure-8 Lissajous trajectory ───────────
        omega = 2 * np.pi / T
        true_pos[k] = np.array([
            20 * np.sin(2 * omega * tk),
            10 * np.sin(4 * omega * tk),
        ])

        # True accelerations (analytical derivative of trajectory)
        true_accel_w = np.array([
            -(2*omega)**2 * 20 * np.sin(2*omega*tk),
            -(4*omega)**2 * 10 * np.sin(4*omega*tk),
        ])
        true_yaw_rate = 0.05 * np.sin(2*omega*tk)

        # ── Simulate noisy IMU measurement ────────────────────────
        # IMU measures body-frame acceleration with bias + white noise
        psi_true = 0.1 * np.sin(2*omega*tk)   # true heading
        c, s = np.cos(psi_true), np.sin(psi_true)
        R_w2b = np.array([[c, s], [-s, c]])     # world → body rotation
        true_accel_body = R_w2b @ true_accel_w

        imu_accel = true_accel_body + TRUE_BIAS + np.random.randn(2) * 0.05
        imu_gyro  = true_yaw_rate   + np.random.randn() * 0.001

        # ── IMU-only dead reckoning (no corrections) ─────────────
        ax_w = imu_accel[0]*np.cos(imu_psi) - imu_accel[1]*np.sin(imu_psi)
        ay_w = imu_accel[0]*np.sin(imu_psi) + imu_accel[1]*np.cos(imu_psi)
        imu_vel += np.array([ax_w, ay_w]) * dt
        imu_p   += imu_vel * dt
        imu_psi += imu_gyro * dt
        imu_pos[k] = imu_p

        # ── EKF: predict step (IMU) ───────────────────────────────
        ekf.predict(imu_accel, imu_gyro)

        # ── EKF: VIO update at 10 Hz (every 10th IMU step) ───────
        if k % 10 == 0 and k > 0:
            # VIO gives position with small noise (~3 cm)
            vio_meas = true_pos[k] + np.random.randn(2) * 0.03
            ekf.update_vio(vio_meas)

        ekf_pos[k] = ekf.x[:2]

    # ── Compute error statistics ────────────────────────────────
    imu_errors = np.linalg.norm(imu_pos - true_pos, axis=1)
    ekf_errors = np.linalg.norm(ekf_pos - true_pos, axis=1)

    print(f"\n{'='*56}")
    print(f"  GNSS-DENIED NAVIGATION — ERROR ANALYSIS")
    print(f"  Bestaoui Sebbane §7.5–7.6  |  dt={dt}s  T={T}s")
    print(f"{'='*56}")
    print(f"\n  IMU bias simulated: [{TRUE_BIAS[0]:.3f}, {TRUE_BIAS[1]:.3f}] m/s² (tactical MEMS)")
    print(f"\n  {'Metric':<30} {'IMU-Only':>10} {'EKF+VIO':>10}")
    print(f"  {'-'*50}")
    print(f"  {'Final error':<30} {imu_errors[-1]:>9.2f}m {ekf_errors[-1]:>9.3f}m")
    print(f"  {'Mean error':<30} {np.mean(imu_errors):>9.2f}m {np.mean(ekf_errors):>9.3f}m")
    print(f"  {'Max error':<30} {np.max(imu_errors):>9.2f}m {np.max(ekf_errors):>9.3f}m")
    print(f"  {'RMS error':<30} {np.sqrt(np.mean(imu_errors**2)):>9.2f}m {np.sqrt(np.mean(ekf_errors**2)):>9.3f}m")
    reduction = np.mean(imu_errors) / (np.mean(ekf_errors) + 1e-9)
    print(f"\n  Error reduction from EKF+VIO: {reduction:.0f}×")
    print(f"  Final bias estimate: [{ekf.x[5]:.4f}, {ekf.x[6]:.4f}] m/s²")
    print(f"  True bias:          [{TRUE_BIAS[0]:.4f}, {TRUE_BIAS[1]:.4f}] m/s²")
    print(f"  Bias estimation accuracy: {abs(ekf.x[5]-TRUE_BIAS[0])/TRUE_BIAS[0]*100:.1f}% error")
    print(f"\n  → EKF estimates IMU bias in real time (in-flight calibration).")
    print(f"{'='*56}\n")
    return t, true_pos, imu_pos, ekf_pos


if __name__ == "__main__":
    np.random.seed(42)
    simulate_gnss_denied(T=60.0, dt=0.01)
Expected Output IMU-only final error: ~45–120 m (quadratic growth, varies with noise seed) · EKF+VIO final error: ~0.05–0.15 m · Error reduction: ~400–800× · Bias estimation: EKF converges to within ~5% of true IMU bias after ~10 s · This demonstrates in-flight IMU calibration — the EKF learns the sensor's bias while navigating.
  1. Under what two specific conditions does the EKF linearisation error become large enough to cause filter divergence? For each condition, explain the mathematical mechanism using the Taylor series expansion, and describe a concrete scenario on a military UAV where each condition arises. Why does the UKF avoid divergence in these same conditions?
  2. In Worked Example 4.2, the Kalman gain was $K = 0.64$. Explain what happens to the navigation solution if the VIO measurement noise covariance $R$ is incorrectly set to 0.001 m² instead of the correct $0.03^2 = 0.0009$ m² — i.e., the designer accidentally thinks VIO is more accurate than it is. Calculate the resulting (incorrect) Kalman gain, and describe the consequence when VIO produces an occasional spurious measurement (feature mismatch) of +2 m error.
  3. Explain from first principles why VIO cannot estimate 3D position during pure rotation (UAV rotating without translating). Derive the geometric condition for triangulation degeneracy using the epipolar geometry framework. For a military loitering munition performing a slow yaw scan to search for targets before diving, propose a specific sensor modification that would maintain position estimation during this pure-rotation phase.
◂ Module 03: GNC Module 05: Sensor Fusion ▸