INS/GNSS Coupling — Bounding Inertial Drift

Chapter 1: Coupling Architectures — Loose, Tight, and Deep

We've agreed to fuse the INS and GNSS. The next question is brutally practical: at what point in the GNSS receiver's processing chain do we tap in? A GNSS receiver is itself a little pipeline. Raw radio signals from the satellites become pseudoranges (the apparent distance to each satellite, computed from signal travel time) and Doppler measurements (range-rate, from frequency shift), and then a least-squares or filtering step turns four-or-more pseudoranges into a single position-velocity-time solution. We can fuse with the INS at either end of that chain — and the choice has enormous consequences. The three classic levels are loosely coupled, tightly coupled, and deeply (ultra-tightly) coupled.

Loosely coupled — fuse the GNSS solution

The simplest architecture taps in at the end of the GNSS chain. The receiver does its own thing, computes a position-velocity solution, and hands that solution to the navigation filter as a measurement. The filter's job is just "the INS says I'm here, GNSS says I'm there, reconcile them." This is loosely coupled integration: two black boxes (an INS filter and a GNSS receiver) connected by a thin wire carrying a position/velocity fix.

Tightly coupled — fuse the raw pseudoranges

The tightly coupled architecture taps in earlier, before the receiver collapses everything into a solution. Instead of fusing a position fix, you fuse the raw pseudoranges and Doppler for each individual satellite. The navigation filter holds the satellites' known positions (from the broadcast ephemeris) and, for each satellite, predicts what the pseudorange should be given the INS's current position estimate. The measurement model maps state → predicted pseudorange:

where ρ̂_i is the predicted pseudorange to satellite i, p_sat,i is that satellite's known position, p_INS is the INS position estimate, c is the speed of light, and δt_clk is the receiver clock bias (now carried as an extra state). The innovation is (measured pseudorange − predicted pseudorange), per satellite.

The cost of tight coupling is complexity and intimacy. You need access to the receiver's raw measurements (not all consumer receivers expose them), you must model each satellite's clock and atmospheric delays, and you must carry the receiver clock bias and drift as filter states. It is more code, more states, and more knowledge of GNSS internals — but it buys robustness exactly where loose coupling fails.

Deeply / ultra-tightly coupled — the INS aids the receiver

The deepest architecture reverses part of the flow. In deeply coupled (also ultra-tightly coupled) integration, the INS doesn't just receive from the GNSS chain — it feeds back into the receiver's signal-tracking loops. A GNSS receiver tracks each satellite's carrier and code with phase-locked and delay-locked loops; under heavy vibration or in a deep fade those loops can lose lock. If the INS tells the tracking loops "the platform just accelerated this way, so expect the Doppler to shift by this much," the loops can stay locked through dynamics and jamming that would otherwise break them. Conceptually: the INS stabilizes the receiver's front end, and the receiver stabilizes the INS's drift — a mutual aiding loop. Deep coupling lives in high-end military and aerospace systems; for this lesson it's enough to know it exists and points the opposite direction (nav → receiver).

Below, watch loose vs tight under a shrinking satellite count. As you drag the satellite slider down, the loosely coupled solution holds firm until it hits four, then collapses to "no fix." The tightly coupled solution keeps constraining the estimate all the way down to one satellite — weaker, but never nothing.

Chapter 2: The Error-State Kalman Filter — Filter the Mistake, Not the Position

We know what to fuse and where to tap in. Now the central machine: the filter that actually does the fusing. The naive idea — run a Kalman filter directly on the full navigation state (position, velocity, attitude) — turns out to be a bad idea, and the reason it is bad reveals the most elegant trick in all of navigation: the error-state Kalman filter (ESKF), also called the indirect Kalman filter.

Why filtering the full state directly is painful

Imagine a Kalman filter whose state is the full navigation state — call it the total state x: position (meters, spanning the whole Earth), velocity (up to hundreds of m/s), attitude (a full 3-D orientation). Three problems pile up:

• Huge dynamic range. Position can be millions of meters; the corrections the filter makes are centimeters. Packing both into one linear filter is numerically awkward.
• Strong nonlinearity, fast. The strapdown equations (Lesson 13) are nonlinear and must run at the IMU rate (hundreds of Hz). A direct Kalman filter would have to linearize those fast nonlinear dynamics every tick — expensive and fragile.
• Attitude doesn't live in a vector space. Orientation lives on a curved manifold (rotations, quaternions with a unit-norm constraint), not in flat R³. A linear filter that adds a correction to a quaternion breaks the unit-norm constraint. (We fix this properly in Chapter 4.)

So the full state is large, nonlinear, fast, and partly non-Euclidean. A Kalman filter wants the opposite: small, linear, slow, Euclidean. The error-state trick gives the filter exactly the state it wants.

The decomposition — nominal ⊕ error

Here is the move. Split the true state into two pieces: a nominal state and a small error state. The nominal state is what the high-rate strapdown mechanization computes by integrating the IMU (Lesson 13) — a smooth, full-dynamic-range trajectory that is almost right but slowly drifting. The error state, written δx, is the small difference between that nominal trajectory and the truth:

Read it as: truth = (what the strapdown thinks) plus (a small correction). The symbol ⊕ is "plus" for ordinary states like position and velocity, and a slightly fancier composition for attitude (Chapter 4) — but the spirit is just addition. The Kalman filter's entire job is now reframed: don't estimate the position — estimate the position error. Don't filter the full nonlinear state; filter the tiny correction to it.

Why the error dynamics ARE linear — the justification

Why does filtering the error work when filtering the full state was so painful? Because the error is small, and small quantities propagate linearly. Take any nonlinear dynamics and look at how a tiny perturbation evolves: to first order, the perturbation obeys a linear differential equation — the Jacobian of the dynamics evaluated along the nominal trajectory. This is just calculus: a smooth function looks linear if you zoom in close enough, and the error is a close zoom-in on the truth.

Concretely, the error-state dynamics come out as a linear system:

where δẋ is the rate of change of the error, F is the (time-varying) error-dynamics matrix built from the nominal trajectory, w is the IMU's white noise (driving the error), and G maps that noise into the error. Crucially, F is the Jacobian of the nav equations about the nominal — and because the error is small, that first-order linear model is an excellent approximation. The error state is also Gaussian to first order: it is driven by Gaussian IMU noise through a linear system, and a linear function of a Gaussian is Gaussian. Linear dynamics + Gaussian noise = the exact home turf of the Kalman filter. That is why the ESKF is valid where a direct filter struggles: we moved the nonlinearity out of the filter (into the high-rate strapdown integrator) and left the filter a clean linear-Gaussian problem.

Predict, update, inject, reset — the four-beat cycle

The ESKF runs a four-beat loop. Two beats happen at IMU rate (fast), two at GNSS rate (slow):

That reset in beat 4 is the signature of the error-state filter and trips up everyone the first time. After you inject the estimated error into the nominal state, the nominal state is the corrected estimate — so the remaining error is, by construction, back to (unknown, small, mean-zero). You zero the error-state mean and keep its covariance. The error then re-grows from zero as the IMU drifts, until the next GNSS fix lets you estimate and inject it again. This produces a characteristic sawtooth: the error climbs as the IMU coasts, then drops to zero at each fix. We'll animate exactly that sawtooth below.

Closed-loop (feedback) vs feedforward

When you inject the correction back into the strapdown integrator so the nominal state itself is corrected (beat 4 above), that's closed-loop or feedback mode. The alternative, feedforward, leaves the strapdown running free and only adds the estimated error at the output. Feedback is almost always preferred because it keeps the nominal trajectory close to truth, which keeps the small-error linearization valid — if you let the strapdown drift far without correcting it, the error stops being "small" and the linear F stops being accurate. Feedback continuously re-centers the linearization point. We'll return to this in the Practical chapter.

Chapter 3: The 15 States — What the Filter Actually Tracks

We said the error state δx is "small." Now let's open it up and see what's inside. The standard INS/GNSS error state has fifteen elements, in five blocks of three. Memorize these five blocks — they are the canonical state vector you will see in every navigation paper, in PX4's EKF2, in NovAtel's SPAN, in textbook after textbook:

Three plus three plus three plus three plus three: fifteen states. (Tightly coupled filters add two more for the receiver clock bias and drift, making seventeen; some systems add scale-factor errors or a lever-arm estimate.) Let's walk each block and feel why it earns its place.

Position and velocity error — the cascade

Position error δp is the obvious one: it is the gap between where the strapdown thinks the vehicle is and where it actually is — the very drift we're fighting. GNSS measures position, so a GNSS fix speaks directly to δp. But position error doesn't appear from nowhere; it accumulates from velocity error. If your velocity estimate is off by 0.1 m/s, then after 10 seconds your position is off by 1 m, purely from integration. So δv is in the state because it is the source of position drift. The filter watches velocity error so it can predict where position error is heading — and a GNSS velocity (Doppler) measurement speaks directly to δv.

The bias states — the secret to long dropouts

Here is the deepest reason the 15-state filter beats a naive position-only filter: the accelerometer bias b_a and gyro bias b_g. These are slowly-varying offsets baked into every IMU — the accelerometer reads, say, 0.02 m/s² too high even when perfectly still; the gyro reads 0.01°/s of rotation when motionless. A position-only filter ignores them, so during a GNSS dropout the uncorrected biases integrate straight into runaway drift. The 15-state filter estimates the biases from the GNSS fixes (the geometry of how the error grows reveals the bias driving it) and subtracts them from the IMU. With the bias removed, the IMU coasts dramatically longer before drift matters.

This is the difference between a tunnel you survive and one you don't. A 15-state ESKF that has been observing GNSS for a few minutes has learned its IMU's biases tightly; when GNSS drops, it removes those biases and the residual drift is far smaller. The bias states are not bookkeeping — they are the mechanism that turns a short coast into a long one.

Below, toggle the bias states on and off, then run a GNSS dropout. With bias estimation on, the coast through the dropout is tight; with it off, the same dropout sends the position error sprinting away — the visible payoff of those six extra states.

Worked example — one error-state injection step, by hand

Let's do an actual injection with real numbers, the way the filter does it at a GNSS fix. We'll work the east position channel to keep it scalar and clear. Setup at the moment a GNSS fix arrives:

• Nominal east position from the strapdown: x_nom = 412.30 m
• GNSS measured east position: z = 409.80 m
• Prior error covariance (variance of east position error): P = 9.0 m² (so σ = 3 m — the strapdown has drifted, uncertainty grew)
• GNSS measurement noise variance: R = 4.0 m² (σ = 2 m)
• Measurement maps error straight to east position, so H = 1

Step 1 — innovation. The measurement model predicts the GNSS reading from the nominal state. With H = 1, the predicted east position is just x_nom, so the innovation (measured − predicted) is:

GNSS says we are 2.5 m west of where the strapdown thinks — that's the drift, made visible.

Step 2 — innovation covariance and Kalman gain. The innovation covariance is S = H P Hᵀ + R = P + R = 9 + 4 = 13. The Kalman gain is the trust slider from Lesson 5:

Because the prior (P = 9) is larger than the measurement noise (R = 4), the filter trusts GNSS more than the strapdown — K is above 0.5, leaning toward the measurement, exactly as inverse-variance weighting demands.

Step 3 — estimate the error. The error state starts at zero (we reset it last time). The update sets:

The filter's best estimate of the east position error is −1.73 m — the nominal is about 1.73 m too far east.

Step 4 — inject. Fold the error into the nominal state:

The corrected east position is 410.57 m — pulled most of the way from the strapdown's 412.30 toward GNSS's 409.80, but not all the way, because GNSS is noisy too. The fused answer sits between them, weighted by trust.

Step 5 — update covariance and reset. The posterior covariance shrinks: P ← (1 − K H) P = (1 − 0.692) × 9 = 0.308 × 9 = 2.77 m² (so σ dropped from 3 m to about 1.66 m — the fix sharpened us, and 2.77 is smaller than both the prior 9 and the measurement's 4, the inverse-variance bonus). Finally, reset the error-state mean to zero: the −1.73 m has been applied, so the remaining error is once again unknown-and-small. The covariance P = 2.77 carries forward; the mean restarts at zero and re-grows as the IMU drifts until the next fix.

Chapter 4: Attitude Error & the Multiplicative EKF

Four of the five error blocks — position, velocity, accel bias, gyro bias — live happily in flat R³, where "plus" means ordinary vector addition. The fifth block, attitude error δθ, does not. Orientation is the one part of the navigation state that lives on a curved manifold, and naively adding a correction to it breaks things. The fix — representing attitude error multiplicatively — is the multiplicative extended Kalman filter (MEKF), and it is one of the most beautiful ideas in estimation.

Why attitude resists ordinary addition

The cleanest way to store an orientation is a quaternion q — four numbers with a hard constraint: they must have unit length, ||q|| = 1. (Lesson 13 and the dedicated quaternion lessons cover why; here we just need the constraint.) Now try to run a Kalman filter that carries the quaternion in its state and corrects it by adding a small vector, the way it corrects position. Two things break:

• The norm constraint shatters. Add a small 4-vector to a unit quaternion and the result is no longer unit length — it's no longer a valid rotation. You'd have to re-normalize every step, and the filter's covariance doesn't know about the constraint, so it allocates probability to impossible orientations.
• The dimension mismatch. A quaternion has four numbers but a rotation has only three degrees of freedom (the norm constraint eats one). A Kalman filter with a 4-D attitude state has a singular covariance — it tracks uncertainty in a direction (the norm) that has no real freedom. That singularity wrecks the matrix math.

The multiplicative fix — error as a small rotation

The MEKF sidesteps both problems with one decision: keep the full attitude as a unit quaternion in the nominal state, but represent the error as a small rotation multiplied onto the nominal, not added. The true orientation is the nominal orientation composed with a tiny error rotation:

where ⊗ is quaternion multiplication and δq is a small error quaternion. Because the error rotation is tiny, its quaternion is, to first order:

The scalar part is essentially 1 (a tiny rotation barely rotates), and the vector part is half the small-angle error vector δθ — three numbers. That three-number vector δθ is what the Kalman filter carries in its state — not the four-number quaternion. The filter's attitude error is a clean 3-vector in R³, with a well-behaved 3×3 covariance and no constraint to violate. The full unit quaternion lives safely in the nominal state, untouched by the filter's linear algebra.

Inject and reset, the attitude way

The injection step for attitude is the only place where ⊕ is fancier than plain addition. After the filter estimates the error angles δθ̂, it builds the small error quaternion from them and multiplies it onto the nominal:

The estimated tilt is rotated into the nominal orientation, and the error angles reset to zero — the same inject-and-reset rhythm as position, but with rotation composition instead of addition. This keeps the quaternion exactly unit-length at all times (because we only ever multiply unit quaternions), and keeps the filter's covariance honest (it only ever describes a 3-D tilt). Position, velocity, and biases inject additively; attitude injects multiplicatively. That single distinction is the whole content of "multiplicative" EKF.

Why attitude error is the most dangerous of all

It's worth dwelling on why attitude gets this special treatment beyond mathematical tidiness: a tiny attitude error does outsized damage. The accelerometer measures specific force, which includes gravity (about 9.81 m/s²). If your attitude estimate is tilted by even a small angle θ, the strapdown mis-projects gravity, leaking a horizontal acceleration of roughly g·sinθ into your velocity. A 0.1° tilt (0.00175 rad) leaks 9.81 × 0.00175 ≈ 0.017 m/s² of phantom horizontal acceleration — which integrates to velocity and then position drift. Attitude error is the dominant upstream driver of the whole error cascade, which is exactly why getting its representation right (and estimating gyro bias to keep it small) matters so much.

Below, watch a small attitude tilt leak gravity into horizontal acceleration. Drag the tilt and see the phantom acceleration — and the position drift it produces — grow.

Chapter 5: The Lever Arm — The Antenna Isn't Where the IMU Is

Here is an error that is pure geometry, costs nothing to fix, and silently poisons countless first-attempt navigation filters: the lever arm. The IMU sits somewhere on the vehicle — bolted near the center of mass, say. The GNSS antenna sits somewhere else — up on the roof, out on a wing, on a mast. They are not at the same point. The filter estimates the position of the IMU, but GNSS measures the position of the antenna. If you fuse the antenna's position as if it were the IMU's, the filter sees a constant discrepancy and spends its life fighting a phantom error that isn't an error at all.

The fix — predict the antenna, not the IMU

The cure is to make the measurement model honest. The antenna's position is the IMU's position plus the lever arm r (the fixed offset vector from IMU to antenna), rotated into the navigation frame by the current attitude. So the predicted GNSS position is:

where p_IMU is the IMU position (the filter's state), r is the lever arm measured in the body frame, and R_nav is the rotation matrix from body to navigation frame (from the attitude). The filter compares GNSS to this prediction, so the constant offset is accounted for and the innovation reflects real drift, not the mounting geometry.

For velocity there's a second, subtler term. When the vehicle rotates, a point offset from the rotation center moves even if the center is still — think of a kid on the end of a spinning merry-go-round. So the antenna's velocity is the IMU's velocity plus a rotational term ω × r (angular velocity crossed with the lever arm), rotated into the nav frame:

where ω is the body's angular rate (straight from the gyros). Skip this ω×r term and your filter will see a phantom velocity error every time the vehicle turns — a heading-dependent error that's maddening to diagnose because it only appears in turns.

Worked example — the lever-arm correction, by hand

Let's compute an actual lever-arm correction with real numbers. A survey van has its IMU at the floor near the rear axle and its GNSS antenna on a mast. The lever arm from IMU to antenna, measured in the body frame (x = forward, y = right, z = down), is:

That is: the antenna is 1.5 m forward of the IMU and 2.0 m up (z is down, so −2.0 means up). Take the simple case where the van faces due north and is level, so the body frame aligns with the nav frame (R_nav = identity).

Position correction. With R_nav = identity, the predicted antenna position is just the IMU position plus r. Say the filter's IMU position is p_IMU = [North 200.0, East 50.0, Down 0.0] m. Then:

GNSS will read the antenna at roughly [201.5, 50.0, −2.0] (2 m up, 1.5 m north of the IMU). If we'd predicted the bare IMU position [200.0, 50.0, 0.0], the innovation would show a phantom 1.5 m north error and a 2.0 m altitude error forever — pure mounting geometry the filter would waste effort chasing.

Now rotate the van. Turn it to face east (90° clockwise from north). The "forward" 1.5 m of lever arm now points east, not north. The rotation matrix swaps the contribution: the predicted antenna offset becomes [North 0.0, East 1.5, Down −2.0]. So now:

The same physical lever arm produces a 1.5 m offset to the east now instead of north — the correction is heading-dependent, which is exactly why a filter that ignores it can't compensate with a single constant: the phantom error rotates with the vehicle. Only the full R_nav·r term tracks it.

Velocity correction in a turn. Suppose the van is turning at ω = [0, 0, 0.2] rad/s (yawing, z-axis down, about 11°/s). The rotational velocity of the antenna is ω × r:

So during this turn the antenna is moving 0.3 m/s to the right (east, in body frame) relative to the IMU, purely from the rotation. GNSS Doppler will see that 0.3 m/s; if the filter doesn't add the ω×r term, it mistakes it for a real 0.3 m/s velocity error — but only while turning. Add the term and the phantom vanishes.

Below, rotate the body and watch the antenna swing around the IMU on its lever arm. The phantom error shows what GNSS would appear to report if you forgot the R_nav·r correction — notice it rotates with the body, so no constant offset can cancel it.

Chapter 6: Showcase — A Vehicle, a Tunnel, and the Fused Estimate

Everything comes together here. A vehicle drives a 2-D loop. You can watch three trajectories at once: the INS-only estimate sliding ever further from truth, the GNSS fixes scattering near truth, and the fused ESKF estimate — smooth, bounded, and glued to the dashed truth. Then trigger a GNSS dropout (a tunnel) and watch the fused estimate coast on the INS, drift a little, and snap back the instant satellites return. The shaded ellipse is the filter's position-error covariance — it swells during the dropout (uncertainty grows when coasting) and shrinks on reacquisition (a fix sharpens it).

Play with it until the architecture feels inevitable. Run with the tunnel off and notice the fused line is smoother than the GNSS dots and far more bounded than the INS line — it is better than either input. Hit the tunnel and watch the fused estimate calmly coast where GNSS-only would simply stop and INS-only would sprint away; the covariance ellipse grows to honestly report rising uncertainty. Drop the IMU quality to the floor and the coast gets ugly fast (a cheap IMU drifts quickly through the dropout); raise it and the vehicle can cross a long tunnel barely noticing. That single trade — IMU quality buys dropout endurance — is the entire economics of navigation-grade vs consumer-grade inertial sensors.

Chapter 7: Use Cases & Real Products

INS/GNSS coupling via an error-state filter is not an academic curiosity — it is the silent backbone of nearly everything that knows where it is. Here is where it actually runs, and the names of the stacks that run it.

Automotive dead-reckoning — through the tunnel

Every modern car navigation system fuses a low-cost MEMS IMU (and often wheel-speed and steering-angle sensors) with GNSS in a loosely or tightly coupled filter. When you drive into a tunnel or a multi-level parking garage and your phone's blue dot keeps moving instead of freezing, that's INS/GNSS dead-reckoning coasting on the IMU. Automotive suppliers (Bosch, Continental, u-blox with its on-chip dead-reckoning) ship exactly this. Add wheel odometry and the coast through a long tunnel becomes remarkably tight — the wheels measure distance traveled, which directly bounds along-track drift.

Aircraft & UAV navigation

Commercial and military aircraft carry navigation-grade INS fused with GNSS as the primary position-velocity-attitude source. On the drone side, the open-source autopilots PX4 and ArduPilot both run an error-state navigation filter — PX4's is literally called EKF2, an error-state EKF fusing IMU, GNSS, magnetometer, barometer, and optical flow. It is the single most-deployed ESKF in the world by unit count, flying millions of drones. The 15-state structure from Chapter 3, the multiplicative attitude from Chapter 4, the lever arm from Chapter 5 — all of it is in EKF2.

Surveying & mapping — RTK + INS

High-accuracy mobile mapping (the vans that build street-view and HD maps, airborne LiDAR survey, agricultural field mapping) demands centimeter-level position and precise attitude to georeference every sensor sample. These systems fuse RTK GNSS (real-time kinematic, carrier-phase GNSS good to centimeters) with a high-grade IMU in a tightly coupled ESKF. The commercial leaders are NovAtel SPAN (their GNSS+INS product line) and Applanix POS (the standard for airborne and marine mapping). Tight coupling matters here because survey routes run under bridges and tree canopy where satellites drop below four.

Smartphone fused location

Your phone's "fused location provider" blends GNSS, the MEMS IMU, Wi-Fi, and cell-tower positioning. The inertial part does pedestrian dead-reckoning (step detection plus heading) fused with GNSS so the blue dot stays smooth and survives brief urban-canyon dropouts. It's a lighter-weight cousin of the full ESKF, but the same complementary logic: high-rate inertial for smoothness, bounded GNSS to kill drift.

Agriculture & marine

Precision-agriculture auto-steer (tractors driving centimeter-accurate rows) uses RTK-GNSS+INS so the tractor holds its line even as terrain pitches and rolls the antenna — the INS provides the attitude that georeferences the RTK fix and bridges momentary GNSS gaps behind a tree line. Marine survey and dynamic-positioning systems on ships fuse GNSS with INS (and sometimes Doppler velocity logs) to hold station and georeference sonar, riding out the slow heave and roll that would smear an unfused GNSS track.

Chapter 8: Practical Application — Building One That Works

You understand the architecture. Now the engineering decisions that separate a filter that works in the lab from one that survives the road. These are the dials and choices you'll actually turn.

Choosing loose vs tight

Decide by your environment and your access. Go loose if you mostly operate in open sky, you bought a sealed receiver that only outputs a solution, and you value modularity (swap receivers freely). Go tight if you routinely drop below four satellites (cities, forests, canyons), you can get raw pseudoranges from your receiver, and you need graceful degradation. The single most common mistake is staying loose in a satellite-starved environment and watching the filter go blind every time the count dips — when two visible satellites could have kept it constrained.

Tuning process noise for the bias states

The bias states need a small process noise (the Q matrix entries) that models them as a slow random walk. Too small and the filter believes the bias is frozen — it stops tracking the real drift, and the estimate ages out. Too large and the filter lets the bias wander to absorb every wiggle, including ones that are really measurement noise or unmodeled dynamics, making it twitchy and stealing observability from the true states. The right value comes from the IMU's datasheet (bias instability, often quoted as an Allan-variance number) tuned up a bit on real data. Rule of thumb: start from the datasheet bias-instability, then increase Q until the bias estimate tracks slow temperature drift without chasing noise.

ZUPT and NHC — free aiding for ground vehicles

Ground vehicles get two powerful, sensor-free aiding measurements that dramatically extend dropout endurance:

• ZUPT (Zero-velocity UPdaTe). When you detect the vehicle is stationary (no wheel motion, no IMU vibration above threshold), you feed the filter a measurement: velocity = 0. This is an almost perfect, free observation that snaps the velocity error to zero and lets the filter re-estimate accel bias precisely — because at a standstill, any nonzero measured acceleration is bias. Pedestrian and stop-and-go vehicle navigation lives on ZUPTs.
• NHC (Non-Holonomic Constraint). A wheeled vehicle can't move sideways or instantly jump vertically — its velocity in the body's lateral and vertical axes is essentially zero (it only goes forward and yaws). Feeding the filter "lateral and vertical body velocity = 0" as a measurement bounds two of the three velocity-error components for free, between every GNSS fix. NHC is why a car can coast through a long tunnel far better than a free-floating drone.

RTK and PPP — upgrading the GNSS source

Standard GNSS gives meter-level position. Two upgrades give centimeters, and they slot straight into the same ESKF as a better (lower-R) position measurement:

• RTK (Real-Time Kinematic) uses carrier-phase measurements plus a nearby base station (or network) to reach centimeter accuracy — the survey and auto-steer standard. It needs the base-station correction link and a clear-ish sky to fix the phase ambiguities.
• PPP (Precise Point Positioning) reaches decimeter-to-centimeter accuracy globally without a local base, using precise satellite orbit/clock products — but takes minutes to converge. Good for open-ocean or remote work where no base station exists.

Both are just lower-noise position (and sometimes attitude, via dual-antenna) measurements to the filter; the architecture doesn't change, only R shrinks and the fused estimate sharpens.

Initialization & alignment

The ESKF assumes the error is small — which means you must start close to truth, or the linearization is invalid from tick one. Position and velocity init from the first GNSS fixes. Attitude is harder: at rest, the accelerometers sense gravity, which fixes roll and pitch (you can see "down"), and a magnetometer or dual-antenna GNSS or a brief drive (course-over-ground from GNSS velocity) fixes heading. This alignment phase — coarse then fine — must finish before you trust the filter. A bad initial heading is the classic "filter slowly diverges for the first minute" bug.

Chapter 9: Debugging & Failure Modes

INS/GNSS filters fail in a small set of characteristic, recognizable ways. Learn the signature of each and you'll diagnose in minutes what otherwise costs days. Here is the field guide.

1. Uncompensated lever arm

Symptom: a constant position offset that rotates with heading (north when facing north, east when facing east), plus velocity errors that appear only in turns. Detect: log the GNSS innovation and correlate it with heading — if the position innovation's direction tracks the vehicle's heading, it's the lever arm. Fix: measure r and add R_nav·r (and ω×r for velocity) to the measurement model. The most common silent bug, from Chapter 5.

2. GNSS multipath & spoofing — the poisoned fix

In urban canyons, GNSS signals bounce off buildings before reaching the antenna, arriving late and producing a multipath error — a confident, plausible, wrong fix, sometimes tens of meters off. Spoofing is the malicious version: a transmitter feeds the receiver fake satellite signals. A naive filter ingests the bad fix and corrupts its whole state — one poisoned measurement can ruin minutes of careful estimation. Detect & defend: innovation gating (a.k.a. the chi-squared / NIS test). Before applying any measurement, check whether its innovation is consistent with the filter's predicted innovation covariance S. If the innovation is implausibly large (say, more than a few sigma), reject the measurement — the filter trusts its own physics over a wild outlier. This is the competitive-fusion idea from Lesson 1 (reject the liar) applied to GNSS.

3. Time-sync & latency between IMU and GNSS

Symptom: velocity-dependent errors — small when slow, large when fast — that worsen with speed. Cause: the IMU and GNSS timestamps are misaligned, so a GNSS fix from time t gets applied to the INS state at time t+δ. At 30 m/s, a 20 ms timing error is 0.6 m of position error. Detect: errors that scale with velocity and vanish at rest point straight at time-sync. Fix: hardware-timestamp both sensors against a common clock (PPS from the GNSS receiver is the gold standard), and handle GNSS latency explicitly (apply the fix to the past state it corresponds to, then re-propagate — an out-of-sequence-measurement problem).

4. Filter overconfidence after a long outage

Symptom: after a long GNSS dropout, the filter rejects the first good fix on reacquisition (gates it out) or jumps violently. Cause: during a long coast the true error grew large, but if the covariance didn't grow enough (under-tuned process noise) the filter thinks it's still confident — so the now-distant-but-correct GNSS fix looks like an outlier and gets gated. Fix: make sure the predicted covariance grows realistically during coasting (tune Q so P swells honestly), and consider relaxing the gate right after a long outage so the filter can recapture. The covariance ellipse from the showcase swelling during the tunnel is the healthy behavior; a filter whose ellipse stays small while it coasts is lying to itself.

5. Attitude unobservable when stationary

Symptom: heading drifts or won't converge while the vehicle sits still or drives in a straight line, then suddenly snaps right after the first turn. Cause: some states are unobservable without excitation. Heading (yaw) is not observable from GNSS position/velocity when you're moving in a straight line — you have to turn (or have a second antenna, or a magnetometer) for GNSS to constrain heading. Likewise, biases are weakly observable without dynamics. Detect: heading variance stays large and only collapses after maneuvers. Fix: excite the system (turns, accelerations) during alignment, add a heading source (dual-antenna, magnetometer), and don't expect full observability at rest. This is why aircraft do S-turns during alignment.

6. Consistency checks — NEES & NIS

How do you know your filter is even honest — that its reported covariance matches its real errors? Two standard tests:

• NIS (Normalized Innovation Squared): for each measurement, compute yᵀS⁻¹y. Averaged over time it should sit inside a chi-squared confidence band. Too high → the filter is over-confident (R or Q too small); too low → over-conservative. NIS uses only measurements, so you can run it live in the field.
• NEES (Normalized Estimation Error Squared): the same idea against the true error (needs ground truth, so it's a simulation/bench tool): eᵀP⁻¹e should match the state dimension on average. If NEES blows up, your covariance is too small — the filter claims more confidence than it has.

A filter that passes NIS in the field and NEES on the bench is consistent; one that fails them is lying about its uncertainty, and every downstream consumer (the gate, the path planner) inherits the lie.

Chapter 10: Connections, References & Cheat-Sheet

You've built, from the inside, the workhorse architecture of real navigation: an error-state Kalman filter fusing a high-rate IMU with bounded GNSS, with multiplicative attitude, bias estimation, and lever-arm compensation. Let's connect it to the rest of the series and leave you a one-page reference.

The complementary-filter connection — ESKF is the optimal generalization

Cast your mind back to Lesson 6, the complementary filter. There, you fused a drifting-but-smooth inertial source with a bounded-but-noisy reference by high-passing the inertial and low-passing the reference, with a fixed crossover frequency you tuned by hand. The error-state Kalman filter is exactly that idea, made statistically optimal. It high-passes the IMU (trusts it on short time-scales, where its drift hasn't accumulated) and low-passes GNSS (trusts it on long time-scales, where its bounded error wins) — but instead of a hand-tuned crossover, the Kalman gain sets the crossover automatically and optimally, tick by tick, from each source's tracked variance. The complementary filter is the ESKF with a frozen, hand-picked gain; the ESKF is the complementary filter that computes its own gain. Same physics, optimal weighting.

The whole ESKF update in code

Here is the verbose, commented numpy ESKF measurement update — predict the error covariance, fold in a GNSS position fix, then inject and reset. This is the matrix version of the scalar worked example from Chapter 3:

python
import numpy as np

# --- state layout (15): [dp(3) dv(3) dth(3) ba(3) bg(3)] ---
# nominal state lives OUTSIDE the filter (strapdown). The filter only
# ever carries the ERROR covariance P and a (usually-zero) error mean.

def predict_error_cov(P, Phi, Q):
    # Phi = discrete error-state transition (I + F*dt), built from the
    # nominal trajectory; Q = process noise (IMU noise + bias random walk).
    return Phi @ P @ Phi.T + Q          # the drift/uncertainty GROWS here

def gnss_position_update(nominal, P, z_gps, R, lever_r):
    # --- 1. predicted measurement: IMU position + rotated lever arm ---
    R_nav = nominal['R_nav']                 # body->nav rotation (from attitude)
    p_imu = nominal['p']
    p_pred = p_imu + R_nav @ lever_r        # predict the ANTENNA, not the IMU

    # --- 2. innovation: how far GNSS disagrees with our prediction ---
    y = z_gps - p_pred                      # (3,) residual

    # --- 3. measurement Jacobian H: dz/d(error). Position error maps
    #       straight through (I on the dp block); zeros elsewhere. ---
    H = np.zeros((3, 15))
    H[:, 0:3] = np.eye(3)            # dp block

    # --- 4. innovation covariance + chi-squared GATE (reject bad fixes) ---
    S = H @ P @ H.T + R
    nis = y @ np.linalg.solve(S, y)         # normalized innovation squared
    if nis > 16.0:                          # ~3-sigma gate for 3 dof
        return nominal, P, False            # multipath/spoof? coast on IMU

    # --- 5. Kalman gain + error estimate ---
    K  = P @ H.T @ np.linalg.inv(S)         # (15,3) trust weights
    dx = K @ y                              # (15,) ESTIMATED error state

    # --- 6. covariance update (Joseph form is more stable in practice) ---
    I = np.eye(15)
    P = (I - K @ H) @ P @ (I - K @ H).T + K @ R @ K.T

    # --- 7. INJECT the error into the nominal, then RESET it to zero ---
    nominal = inject(nominal, dx)         # dp,dv,ba,bg add; dth multiplies (MEKF)
    # error mean is now zero by construction; only P carries forward.
    return nominal, P, True

def inject(nominal, dx):
    nominal['p'] += dx[0:3]               # position: additive
    nominal['v'] += dx[3:6]               # velocity: additive
    nominal['q'] = quat_mul(small_quat(dx[6:9]), nominal['q'])  # attitude: MULTIPLICATIVE
    nominal['ba'] += dx[9:12]             # accel bias: additive
    nominal['bg'] += dx[12:15]            # gyro bias: additive
    return nominal

And the same idea, compressed — the four conceptual beats, stripped to their essence:

python
# compact ESKF: nominal lives outside; filter handles only the error
def eskf_step(nominal, P, Phi, Q, z, R, H, lever_r):
    P = Phi @ P @ Phi.T + Q                                  # 1. predict (error grows)
    y = z - (nominal['p'] + nominal['R_nav'] @ lever_r)      # 2. innovation (lever-arm aware)
    S = H @ P @ H.T + R
    if y @ np.linalg.solve(S, y) > 16.0: return nominal, P    # gate: drop outliers
    K = P @ H.T @ np.linalg.inv(S)
    nominal = inject(nominal, K @ y)                          # 3+4. inject & (mean) reset
    P = (np.eye(15) - K @ H) @ P
    return nominal, P

Where this sits, and what's next

This lesson fused inertial with a radio absolute reference (GNSS). The very next conceptual step is to replace — or augment — the radio reference with a visual one. When GNSS is unavailable entirely (indoors, underground, on Mars, in a GPS-denied warzone), a camera can play GNSS's role: it provides bounded, drift-correcting observations of the world that leash the IMU's drift. That is visual-inertial odometry (VIO) — the same error-state, IMU-as-spine philosophy, with image features instead of satellites as the aiding source. Lesson 16: Visual-Inertial Odometry is the vision-aided analog of everything you just learned: the IMU still drifts, something bounded still corrects it, and an error-state filter (or its optimization cousin) still does the fusing. If you understood this lesson, VIO will feel like coming home with a new aiding sensor.

Cheat-sheet

Related lessons on Engineermaxxing

• The Kalman Filter — the optimal recursive fuser whose gain is the trust slider; the ESKF is a Kalman filter run on the error state.
• Complementary Filters & AHRS — the fixed-gain, hand-tuned version of exactly this high-pass-inertial / low-pass-aiding idea; the ESKF is its optimal generalization.
• The Extended Kalman Filter — how to fuse nonlinear sensors by linearizing; the ESKF moves the nonlinearity into the strapdown and keeps the filter linear.
• Kalman Filter (core lesson) and EKF (core lesson) and Bayes Filter — deeper standalone treatments of the estimation machinery underneath.

Lesson 13, IMU & Inertial Navigation, is the strapdown mechanization this lesson sits on top of — the source of the nominal state and the drift we spend this whole lesson bounding. Lesson 16, Visual-Inertial Odometry, is the vision-aided sequel.

References

1. Groves, P. D. Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems, 2nd ed. Artech House, 2013. The definitive engineering reference for loose/tight/deep coupling, the error-state filter, and lever-arm modeling.
2. Solà, J. "Quaternion kinematics for the error-state Kalman filter." 2017. The clearest modern derivation of the ESKF with multiplicative attitude error. arXiv:1711.02508
3. Markley, F. L. and Crassidis, J. L. Fundamentals of Spacecraft Attitude Determination and Control. Springer, 2014. The canonical treatment of the multiplicative EKF (MEKF) and small-angle attitude error.
4. Farrell, J. A. Aided Navigation: GPS with High Rate Sensors. McGraw-Hill, 2008. A rigorous, implementation-oriented account of INS aiding, observability, and consistency.