EKF3
SENSOR FUSION

The Extended Kalman Filter is the single most important algorithm in autonomous drone flight. It answers the two fundamental questions: "Where am I?" and "Which way am I pointing?" This page explains what it does, why it matters, and how it works in a GPS-denied EW environment.

What EKF3 Does — Plain Language

Imagine you're blindfolded in a car. You can FEEL the car turning (that's the gyroscope). You can FEEL acceleration and braking (that's the accelerometer). Someone tells you the air pressure (that's the barometer — lower pressure means higher altitude). And you have a compass.

Each of these senses gives you a clue about where you are and what you're doing. But each one is imperfect. The gyroscope slowly drifts — after 5 minutes, it thinks you've turned 3° more than you actually have. The accelerometer picks up vibrations from the engine. The compass gets confused near metal objects. The barometer changes with weather.

EKF3 is the algorithm that says: "I know each sensor is lying a little. I know HOW MUCH each sensor typically lies. I will take all their readings, weight them by how reliable each one is, and calculate the BEST POSSIBLE ESTIMATE of where I am and how I'm moving." It does this 400 times per second.

This means: if the gyroscope says you've turned 30° but the compass says 27°, and the EKF3 knows the gyro drifts but the compass is accurate in this area, it will estimate 27.5° — trusting the compass more. If the compass is near metal and giving crazy readings, EKF3 reduces its trust in the compass automatically and relies more on the gyroscope.

When GPS Is Jammed — AHRS Mode

In a GPS-denied environment (enemy jamming all satellite signals), EKF3 loses its best position sensor. It switches to AHRS mode (Attitude and Heading Reference System). In this mode it knows the drone's attitude — roll, pitch, and yaw — from the IMU (gyroscope + accelerometer) and the barometer. It knows altitude from the barometer. But it does NOT know lateral position (north/south/east/west).

What this means for operations: the drone can still fly level, hold altitude, and maintain heading. The pilot flies by visual reference through FPV goggles. The AI can still detect targets and calculate RELATIVE position from the detection math (see §6.1). But absolute map coordinates require either terrain matching against pre-loaded orthophotos or manual operator estimation.

Adding an optical flow sensor (PMW3901, €8) restores velocity estimation relative to the ground. The drone won't drift uncontrollably — it can hold position. But the position estimate drifts over time without GPS corrections. Expect ~1-3m/minute of drift in a stable hover.

The Sensors — What Each One Measures

The Math — For Engineers

EKF3 runs a prediction-correction cycle 400 times per second. In plain terms, this means: "Based on what the gyro and accelerometer tell me, I PREDICT where the drone should be now. Then I CHECK that prediction against the barometer, compass, and any other available sensor. If my prediction was wrong, I CORRECT it. The amount I correct depends on how much I trust each sensor right now."

The core equation (state prediction):

# State prediction (runs every 2.5ms)
x_predicted = F @ x_previous + B @ u
# F = state transition matrix (physics model)
# x = state vector [position, velocity, attitude, gyro_bias, accel_bias]
# u = IMU readings [accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z]

# Covariance prediction (how uncertain are we?)
P_predicted = F @ P_previous @ F.T + Q
# Q = process noise (how much we expect the model to be wrong)

# Measurement update (when baro/compass/GPS data arrives)
K = P_predicted @ H.T @ inv(H @ P_predicted @ H.T + R)
# K = Kalman gain (how much to trust this measurement)
# H = measurement matrix (maps state to sensor reading)
# R = measurement noise (how much this sensor typically lies)

x_corrected = x_predicted + K @ (measurement - H @ x_predicted)
P_corrected = (I - K @ H) @ P_predicted

What this means in plain language: the Kalman gain K is the "trust factor." When K is high, the system trusts the sensor more than the prediction. When K is low, it trusts the prediction more. K automatically adjusts based on how consistent each sensor has been recently. If the compass starts giving erratic readings (near aurora, for example), K for compass drops toward zero and the system relies on gyro + accelerometer instead.

Mathematical Derivation — Why The Kalman Gain Is Optimal

This section derives the Kalman gain step-by-step and shows why K = P·Hᵀ·(H·P·Hᵀ + R)⁻¹ is the optimal weighting — not a heuristic. The derivation follows Kalman's original 1960 paper and the standard textbook treatment (Beard & McLain, Small Unmanned Aircraft, Princeton 2012, ch. 8; Simon Optimal State Estimation, Wiley 2006). This page's contribution is specializing the general theorem to Fischer 26's sensor set and showing why each sensor's R-matrix entry has the value it does.

Step 1 — The linear measurement model

Each sensor measurement z is modeled as a linear function of the state x plus Gaussian noise v:

z = H · x + v,   where v ~ N(0, R)

Example sensor models for EKF3 on Fischer 26:
  Barometer:    z_baro  = altitude  → H_baro  = [0,0,1,0,...,0]     (selects Z component)
  Magnetometer: z_mag   = yaw angle → H_mag   = [0,...,1,...,0]     (yaw is state[9])
  Optical flow: z_flow  = V_north/H → H_flow  = [V_n/H, V_e/H,...]  (nonlinear, see Step 5)

R is the sensor noise covariance — typically diagonal with entries equal to the square of the sensor's 1σ noise. For Fischer 26:

R_baro  = 0.5² = 0.25 m²        (BMP390 1σ relative accuracy)
R_mag   = (5°)² = 0.00762 rad²   (IST8310 typical error after calibration)
R_flow  = (0.1 m/s)² = 0.01 m²/s²  (PMW3901 datasheet)

Step 2 — The minimum-variance update

Given a prediction x̂⁻ with covariance P⁻ and a new measurement z, the best linear unbiased estimator (BLUE) of x̂⁺ is a weighted sum:

x̂⁺ = (I − K·H) · x̂⁻ + K · z      (convex combination parameterized by K)

Minimize trace(P⁺) over K:
  P⁺ = (I − K·H) · P⁻ · (I − K·H)ᵀ + K · R · Kᵀ
  
∂trace(P⁺)/∂K = 0
  → −2·(I − K·H)·P⁻·Hᵀ + 2·K·R = 0
  → K·(H·P⁻·Hᵀ + R) = P⁻·Hᵀ
  → K = P⁻·Hᵀ · (H·P⁻·Hᵀ + R)⁻¹

So the Kalman gain is the unique choice that minimizes posterior covariance. It is not a heuristic — it is the optimal linear estimator given Gaussian noise assumptions. Every other weighting produces a strictly worse estimate (higher variance).

Step 3 — Intuition from the scalar case

When the state is a single scalar (e.g., altitude only) and we have one sensor (barometer), the gain reduces to:

K = P⁻ / (P⁻ + R)

Consider three regimes:
  P⁻ → 0 (very confident prediction):    K → 0  → ignore new measurement
  P⁻ = R (equal trust):                   K = 0.5 → average prediction and sensor
  P⁻ → ∞ (no prior info):                 K → 1  → trust sensor fully

Post-update variance:
  P⁺ = (1 - K) · P⁻  =  P⁻ · R / (P⁻ + R)
     = harmonic mean of prior and sensor variance
  
Always: P⁺ < min(P⁻, R)  — the posterior is STRICTLY MORE CERTAIN than either source alone.

This is the mathematical reason why multi-sensor fusion beats single-sensor navigation: each independent measurement strictly reduces position uncertainty. The Dempster-Shafer confidence-fusion derivation on the threat-fusion page has an analogous structure for discrete evidence.

Step 4 — EKF3's specific gain evolution over time

In steady-state cruise with all sensors healthy, Fischer 26's EKF3 converges to stable K values for each sensor. Published ArduPilot logs show typical steady-state gains for each measurement:

Sensor         Nominal K     Interpretation
─────────────────────────────────────────────────────────────
Barometer      0.05-0.15     "Trust baro a little; mostly trust IMU integration"
Magnetometer   0.02-0.08     "Lower trust; slow-drifting yaw reference"
Optical flow   0.25-0.50     "Trust flow significantly when altitude < 30 m"
GPS (if avail) 0.70-0.90     "Trust GPS heavily; it is the most accurate position source"

When a sensor degrades (aurora disrupting compass, vibration blinding optical flow):
  Innovation (z - H·x̂⁻) grows beyond 3σ of H·P⁻·Hᵀ + R.
  ArduPilot's innovation-gate test rejects the measurement (sets K effectively to 0).
  EKF3 falls back to other sensors.
  
This is why EKF3 does not crash when a sensor fails — bad measurements are statistically
detected and excluded by the innovation test, not by the gain calculation itself.

Step 5 — Nonlinear extension (the "Extended" in EKF3)

The derivation above assumes linear H. For nonlinear measurements (optical flow depends on altitude AND velocity, GPS depends on drone's WGS84 position via ECEF transform), EKF3 uses first-order linearization:

For nonlinear z = h(x) + v:
  Replace H with the Jacobian Hⱼ = ∂h/∂x evaluated at current x̂⁻
  Propagate covariance: P⁺ = (I − K·Hⱼ) · P⁻
  Innovation: y = z − h(x̂⁻)  (full nonlinear residual, not linear approximation)
  
Caveat: linearization error grows with uncertainty. If P⁻ is large (high uncertainty),
the linearization around x̂⁻ may be poor. EKF3 can diverge in this regime.
This is why ArduPilot requires initial alignment (~30 s of stationary convergence)
before takeoff — to ensure P⁻ is small enough that linearization is valid.

Worked Example 1 — Barometer-only altitude update

Fischer 26 is in AHRS mode (no GPS). Prior altitude estimate from IMU integration: 118.2 m with 1σ uncertainty 2.0 m (accumulated over 10 s without correction). Barometer reading arrives: 120.1 m with sensor 1σ = 0.5 m.

Prior:        x̂⁻ = 118.2 m, P⁻ = 4.0 m²
Sensor:       z = 120.1 m, R = 0.25 m²

Scalar Kalman gain:
  K = P⁻ / (P⁻ + R) = 4.0 / (4.0 + 0.25) = 0.941

Post-update estimate:
  x̂⁺ = 118.2 + 0.941 × (120.1 - 118.2) = 118.2 + 1.79 = 119.99 m

Post-update uncertainty:
  P⁺ = (1 - 0.941) × 4.0 = 0.235 m²  (σ = 0.485 m)

Interpretation: the prediction was 2 m uncertain; the barometer is 0.5 m uncertain;
K = 0.94 because the sensor is 16× more certain than the prediction, so we mostly
trust the sensor. After update we are 0.485 m uncertain — slightly better than
the sensor alone because the prediction still contributed information.

Worked Example 2 — Innovation gate rejects a bad measurement

Same prior (118.2 m, P⁻ = 4.0 m²). Now a spurious barometer reading of 85 m arrives (sensor experienced pressure pulse from prop wash during aggressive maneuver).

Innovation:           y = 85 - 118.2 = -33.2 m
Innovation variance:  S = P⁻ + R = 4.25 m², σ_inno = 2.06 m
Innovation σ-ratio:   |y| / σ_inno = 33.2 / 2.06 = 16.1σ

ArduPilot innovation gate threshold: typically 3-5σ.
  16.1σ >> 5σ  → REJECT measurement. Do not update.
  
Consequence: state estimate x̂⁺ = x̂⁻ = 118.2 m (unchanged).
  Uncertainty grows slightly from prediction step: P⁺ ~ P⁻ + Q_baro
  
If the spurious readings persist >2 seconds, EKF3 marks the sensor as failed
and reduces H_baro to zero matrix (excludes the sensor from future updates).
Operator sees warning on GCS: "EK3 IMU0 baro inconsistent".

This is how EKF3 survives sensor faults: statistical gating catches outliers before they corrupt the state estimate. The math is the same Kalman framework — just augmented with a χ² test on the innovation before accepting the update.

Verification Code — Reproducing the Worked Examples

def scalar_kalman_update(x_prior, P_prior, z, R, innovation_gate_sigma=5.0):
    """Single-measurement scalar Kalman update with innovation gating.
    Returns (x_post, P_post, accepted).
    """
    # Innovation
    y = z - x_prior
    S = P_prior + R              # innovation variance
    sigma_inno = S ** 0.5
    
    # Innovation gate
    if abs(y) / sigma_inno > innovation_gate_sigma:
        return x_prior, P_prior, False   # rejected, no update
    
    # Kalman gain and update
    K = P_prior / (P_prior + R)
    x_post = x_prior + K * y
    P_post = (1 - K) * P_prior
    return x_post, P_post, True

# Reproduce Worked Example 1 (barometer accepted)
x, P, ok = scalar_kalman_update(x_prior=118.2, P_prior=4.0, z=120.1, R=0.25)
print(f"Example 1: x={x:.3f} m, P={P:.3f} m², accepted={ok}")
# Expected: x=119.988 m, P=0.235 m², accepted=True

# Reproduce Worked Example 2 (spurious baro rejected)
x, P, ok = scalar_kalman_update(x_prior=118.2, P_prior=4.0, z=85.0, R=0.25)
print(f"Example 2: x={x:.3f} m, P={P:.3f} m², accepted={ok}")
# Expected: x=118.200 m, P=4.000 m², accepted=False (16.1σ >> 5σ gate)

# Sensitivity: how does K change with prior uncertainty?
for P_prior in [0.1, 0.5, 1.0, 4.0, 100.0]:
    K = P_prior / (P_prior + 0.25)
    print(f"P_prior={P_prior:6.1f} m² → K={K:.3f}")
# Shows: K approaches 1 as prior uncertainty grows — system trusts sensor more.

# Verify optimality: any other K gives higher variance
import math
P_prior, R = 4.0, 0.25
K_optimal = P_prior / (P_prior + R)
for K_test in [0.0, 0.5, K_optimal, 1.0]:
    P_post_test = (1 - K_test)**2 * P_prior + K_test**2 * R
    print(f"K={K_test:.3f}: P_post={P_post_test:.4f}")
# Shows: K_optimal (0.941) gives minimum P_post.

Why This Derivation Matters Operationally

Four operational decisions depend on the Kalman derivation being correct. If we get this wrong the consequences are direct: Fischer 26 either diverges into an unstable state estimate mid-flight, or it silently absorbs bad sensor data and flies toward a position that does not exist. Both failure modes are fatal to a GPS-denied mission.

First, sensor procurement: the R-matrix entries (baro 0.25 m², mag 0.008 rad², flow 0.01 m²/s²) come directly from datasheet specs of specific parts. Substituting a cheaper sensor with 3× the noise changes R by 9× and degrades the Kalman gain in a predictable way — the planner can compute the resulting position uncertainty increase before deciding whether to save €30 per airframe. The derivation makes the link between sensor spec and flight performance quantitative rather than intuitive.

Second, the optical flow recommendation: PMW3901 at €8 reduces position drift in hover from 200 m/10 min (IMU-only) to perhaps 10 m/10 min (IMU + flow). Without the Kalman derivation this is just a claim. With it, the reason is visible: flow's R (0.01 m²/s² for velocity) is small compared to IMU's process noise Q (velocity-growth rate ~10× higher under vibration), so K_flow stabilizes near 0.4 and the velocity estimate stays bounded rather than drifting. If we get this wrong the consequences are that hover becomes unusable in GPS-denied mode.

Third, innovation gating discipline: Worked Example 2 shows that without the 5σ innovation gate a single spurious barometer pulse would inject a 33 m error into the altitude estimate. The gate is not paranoia — it is the line between "robust fusion" and "sensor-fault catastrophic failure." ArduPilot's published default gate threshold is 3σ; FSG-A operational experience with prop-wash pressure pulses suggests 5σ is safer for Fischer 26's fixed-wing dynamics.

Fourth, the initial-alignment requirement: Step 5 shows that EKF3 can diverge if P⁻ is too large when linearization is applied. This is why Fischer 26 must be stationary for 30 s before arming — to let the filter converge to small enough P that the nonlinear extension is valid. A commander who bypasses this alignment for "faster deployment" ships an airframe that may never converge and will drift uncontrollably after takeoff. The derivation makes explicit why the procedure exists rather than leaving it as an opaque checklist item.

The Kalman update formula is validated in provable_claims.py under EKF_SCALAR_UPDATE (single-measurement scalar case). Multi-dimensional EKF3 behavior is validated only by ArduPilot's own regression tests, not independently by FSG-A — the provable_claims test covers the core mathematics but not the 24-dimensional state vector's full interaction with all sensors.

ArduPilot Configuration for GPS-Denied

Sources

Parameter sources. BMI270 IMU characteristics (400 Hz, ~0.5°/min drift) — Bosch Sensortec datasheet 2022. BMP390 barometer characteristics (±0.5 m, 50 Hz) — Bosch 2023 datasheet. PMW3901 optical flow specifications (±0.1 m/s, works below 30 m AGL, 80 Hz) — PixArt 2023 datasheet. IST8310 magnetometer specifications — iSentek datasheet. VL53L1X rangefinder specifications (0–4 m range, ±2 cm accuracy) — STMicroelectronics datasheet. ArduPilot parameters (AHRS_EKF_TYPE, EK3_SRC1_*, FLOW_TYPE, RNGFND1_TYPE) — ArduPilot documentation.

Mathematical sources. EKF3 24-dimensional state vector — defined in ArduPilot code (libraries/AP_NavEKF3). Prediction equations x = F·x + B·u and covariance P = F·P·Fᵀ + Q — standard Extended Kalman Filter. Kalman gain K = P·Hᵀ·(H·P·Hᵀ + R)⁻¹ — standard derivation.

Operational estimates — not validated by FSG-A in the field. Position drift of 1–3 m/min in hover without GPS — typical EKF3 performance with optical flow from public reports, not measured by FSG-A on the drone platform. 50–200 m drift over a 10-minute flight — scaling from per-minute rate, depends on maneuver stability. GPS-loss detection time <2 seconds — published ArduPilot value for fallback transition. All these figures should be revalidated before operational use.

External standards and references. ArduPilot EKF3 documentation (ardupilot.org/dev/docs/ekf3, 2024). "Extended Kalman Filtering for Battery and GPS-Denied Navigation" — Beard & McLain, Small Unmanned Aircraft (Princeton, 2012). BMI270 IMU datasheet (Bosch, 2022). BMP390 barometer datasheet (Bosch, 2023). PMW3901 optical flow datasheet (PixArt, 2023). FOI Memo 8336 on autonomous UAS systems (2024).

Related Chapters