GNSS-inertial fusion

"The satellites go quiet the instant the tunnel begins. The accelerometers never stop talking, they just start lying more with every passing second. My job is to know exactly how much to trust each one."

A Dead-Reckoning AI Agent

This section builds directly on two earlier chapters. The inertial mechanization, biases, and noise densities come from Chapter 23, and the strapdown integration and drift growth were the subject of Chapter 24. The estimator itself is the Kalman family from Chapter 9, applied here to an error state rather than the full state. GNSS error sources (Section 25.1) set the measurement noise you will feed the update step.

Why fuse: the complementary error spectra

The cleanest way to see why fusion works is in the frequency domain. Inertial dead reckoning is excellent at high frequency: sample-to-sample it captures every bump, turn, and acceleration with millisecond latency. Its error lives at low frequency, a slow ramp as bias and gravity-projection mistakes integrate twice into position. GNSS is the mirror image: its error is high-frequency (a few meters of independent noise per fix, plus multipath jitter) but it has no long-term drift, because each fix is an independent absolute measurement tied to the satellites. Fusion is therefore a complementary filter in spirit: trust the IMU for the fast changes, trust GNSS for the slow truth, and let the estimator find the crossover. The Kalman formulation makes that crossover optimal and, critically, adaptive to how good each sensor currently is.

Loosely, tightly, and deeply coupled architectures

Fusion systems are classified by where in the GNSS processing chain the inertial data enters. In loosely coupled fusion, the GNSS receiver computes its own position-velocity solution and the filter treats that solution as a measurement. It is simple, modular, and works with any receiver that outputs a fix, which is why it dominates consumer devices. Its weakness is that the receiver needs four or more satellites to produce any solution at all, so in a deep urban canyon where only two or three are visible, a loosely coupled filter gets nothing.

Tightly coupled fusion instead feeds the raw pseudoranges (the individual satellite-to-receiver distance measurements from Section 25.1) directly into the filter, one measurement per satellite. Now even a single visible satellite contributes a constraint, so the system degrades gracefully instead of falling off a cliff when the count drops below four. The cost is complexity: the filter must model satellite geometry, clock bias, and per-range noise. Deeply (ultra-tightly) coupled systems close the loop even further, feeding the inertial-predicted motion back into the receiver's signal tracking loops so they can hold a lock through brief dropouts. That last tier lives in aviation and defense hardware; for the mobile and automotive systems this book targets, loosely and tightly coupled cover the ground.

The error-state Kalman filter

The idea that makes GNSS-inertial fusion tractable is to estimate the error in the navigation solution rather than the solution itself. A strapdown mechanization (Chapter 24) integrates the IMU at full rate to produce a nominal position, velocity, and attitude. Alongside it, an error-state Kalman filter (ESKF, also called an indirect or complementary filter) tracks a small state vector of the corrections to that nominal trajectory:

These are, in order, the position error, velocity error, a small-angle attitude error, and the accelerometer and gyroscope biases. Working in the error state buys two things. First, the error dynamics are close to linear even when the true attitude is not, because the errors themselves stay small, so an extended Kalman filter linearization is accurate. Second, the fast nonlinear integration runs outside the filter at IMU rate, while the filter runs a low-dimensional linear model. The error-state prediction propagates the covariance with the linearized dynamics and driving noise:

where \(\mathbf{Q}_k\) encodes the IMU noise densities and the bias random-walk. When a GNSS fix arrives, its disagreement with the nominal position (the innovation) drives the update. The filter computes the Kalman gain, estimates the current error state, and then injects that correction back into the nominal trajectory and resets the error state to zero. The bias estimates persist, and that is the machinery that keeps the dead reckoning honest between fixes.

import numpy as np

# Loosely coupled error-state KF, 1D for clarity: state = [pos_err, vel_err, accel_bias].
# The nominal trajectory is integrated from the IMU outside the filter;
# here we track its error and correct it whenever a GNSS fix arrives.

dt = 0.01                                  # 100 Hz IMU
F = np.array([[1, dt, -0.5*dt*dt],         # pos_err += vel_err*dt - 0.5*bias*dt^2
              [0,  1,       -dt],          # vel_err += -bias*dt
              [0,  0,         1]])          # bias is a slow random walk
Q = np.diag([0.0, (0.02*dt)**2, (1e-4)**2])   # accel noise, bias random-walk
H = np.array([[1.0, 0.0, 0.0]])            # GNSS observes position only
R = np.array([[5.0**2]])                   # 5 m GNSS noise

P = np.diag([10.0**2, 1.0**2, 0.1**2])     # initial uncertainty
dx = np.zeros(3)                           # error state, reset to 0 after each update

def predict():
    global dx, P
    dx = F @ dx
    P = F @ P @ F.T + Q

def gnss_update(nominal_pos, gnss_pos):
    global dx, P
    innov = np.array([gnss_pos - nominal_pos]) - H @ dx   # fix vs. nominal+error
    S = H @ P @ H.T + R
    K = P @ H.T @ np.linalg.inv(S)
    dx = dx + (K @ innov)
    P = (np.eye(3) - K @ H) @ P
    correction = dx.copy()                 # inject into nominal, keep bias estimate
    dx[:2] = 0.0                            # reset pos/vel error; bias carries forward
    return correction                      # apply to nominal pos, vel, and bias

The code above is the loosely coupled skeleton in one dimension. A production system replaces the scalar position with a full 15-state model in a local navigation frame, uses quaternions for attitude with the small-angle \(\delta\boldsymbol{\theta}\) as the error, and swaps the position-only measurement for a position-plus-velocity fix (GNSS Doppler gives an excellent velocity observation that pins down the accelerometer bias fast). To go tightly coupled you change only \(H\) and the update: each satellite contributes one pseudorange row, and \(H\) becomes the line-of-sight geometry to that satellite. The prediction step is untouched, which is exactly why the error-state design is so durable.

Practical necessities: initialization, ZUPT, and outlier rejection

Three details separate a filter that works on paper from one that works on a vehicle. Initialization matters because the error-state linearization assumes small errors: the filter needs a reasonable initial heading, which on a moving platform comes from GNSS course-over-ground, and at rest from a magnetometer or a dual-antenna GNSS baseline. A bad initial yaw is the classic reason a fresh filter diverges. Zero-velocity updates (ZUPT) exploit moments when the platform is provably stationary (detected from low IMU variance): feeding a synthetic "velocity is zero" measurement during those windows is a nearly free, extremely informative observation that crushes velocity drift and sharpens the bias estimates, and it is the single most effective trick in pedestrian and industrial dead reckoning. Outlier rejection guards the update: a multipath-corrupted GNSS fix in an urban canyon can be tens of meters wrong, and if it enters the filter unchallenged it corrupts both position and the precious bias states. Gating each fix by its normalized innovation (the chi-square test on the innovation covariance \(\mathbf{S}\)) rejects fixes that disagree with the prediction far beyond what the noise model allows, and it connects directly to the anomaly-detection machinery of Chapter 12.

When the batch smoother beats the filter

A Kalman filter is causal: at each instant it uses only past and present data. For live navigation that is mandatory. But for offline reconstruction (mapping runs, survey trajectories, post-drive analytics) you have the whole log, and a batch smoother that revisits every timestep with both past and future measurements is strictly more accurate. Modern high-accuracy GNSS-inertial reconstruction is increasingly posed as a factor graph, where IMU measurements between GNSS fixes become preintegrated motion factors linking pose nodes, and each fix becomes a unary factor. Optimizing the whole graph is the smoothing problem of Chapter 11, and it is the same formulation that underlies visual-inertial odometry and SLAM. The filter and the smoother are two views of one estimation problem, and the fusion principles in this section carry over directly into the general multi-sensor fusion of Chapter 49.

What's Next

In Section 25.6, we turn from building estimators to judging them: the metrics that quantify localization quality (horizontal and along-track error, CEP, availability, and time-to-first-fix) and the leakage-safe evaluation protocols that keep a fusion system honest about how well it will actually perform in the wild.