State estimation and EKF

Example 1 Height from an accelerometer and a barometer

A drone hovers and then climbs 4 m between seconds 5 and 9. The accelerometer has a 0.05 m/s² bias and 0.3 m/s² noise; the barometer has 0.5 m noise (simulated data).

import numpy as np

rng = np.random.default_rng(3)
dt, T = 0.01, 30.0
n = int(T / dt)
t = np.arange(n) * dt
true_a = np.where((t > 5) & (t < 7), 1.0, 0.0) - np.where((t > 7) & (t < 9), 1.0, 0.0)
true_v = np.cumsum(true_a) * dt
true_h = np.cumsum(true_v) * dt
acc = true_a + 0.05 + rng.normal(0, 0.3, n)    # m/s² with bias
baro = true_h + rng.normal(0, 0.5, n)          # m

A = np.array([[1, dt], [0, 1]])                # state [height, velocity]
B = np.array([0.5 * dt * dt, dt])
H = np.array([[1.0, 0.0]])
Q = np.diag([1e-5, 1e-3])
R = np.array([[0.25]])
x, P, est = np.zeros(2), np.eye(2), []
for k in range(n):
    x = A @ x + B * acc[k]                     # predict
    P = A @ P @ A.T + Q
    S = H @ P @ H.T + R                        # update
    K = P @ H.T @ np.linalg.inv(S)
    x = x + (K @ (baro[k] - H @ x)).ravel()
    P = (np.eye(2) - K @ H) @ P
    est.append(x[0])

rms = lambda e: float(np.sqrt(np.mean(e ** 2)))
drift = (np.cumsum(np.cumsum(acc) * dt) * dt)[-1] - true_h[-1]
print(f"true height at end {true_h[-1]:.2f} m")
print(f"accelerometer only: error at end {drift:.1f} m")
print(f"barometer only: RMS error {rms(baro - true_h):.2f} m")
print(f"Kalman filter: RMS error {rms(np.array(est) - true_h):.2f} m")

true height at end 3.98 m
accelerometer only: error at end 26.7 m
barometer only: RMS error 0.50 m
Kalman filter: RMS error 0.08 m

The accelerometer alone drifts by tens of metres in 30 seconds because a small bias is integrated twice. The barometer alone jitters by about half a metre. The Kalman filter’s error is several times smaller than either, because it uses acceleration for short-term changes and the barometer to pull the estimate back over the long term.

Example 2 Which GNSS positions are rejected?

The position innovation variance is 4 m² (2 m standard deviation) and the gate is 500 (simulated data).

import math

GATE, S = 500, 4.0                             # EK3_POS_I_GATE, m²
innovations = [0.8, -1.5, 3.9, 9.2, 12.0]      # m
for nu in innovations:
    ratio = nu ** 2 / (S * (GATE / 100) ** 2)
    logged = math.sqrt(ratio)                  # value seen in XKF4.SP
    print(f"innovation {nu:+5.1f} m: test ratio {ratio:.3f}, logged {logged:.2f} -> {'used' if ratio < 1 else 'REJECTED'}")
print(f"largest accepted innovation: {GATE / 100 * math.sqrt(S):.1f} m")

innovation  +0.8 m: test ratio 0.006, logged 0.08 -> used
innovation  -1.5 m: test ratio 0.022, logged 0.15 -> used
innovation  +3.9 m: test ratio 0.152, logged 0.39 -> used
innovation  +9.2 m: test ratio 0.846, logged 0.92 -> used
innovation +12.0 m: test ratio 1.440, logged 1.20 -> REJECTED
largest accepted innovation: 10.0 m

Values more than 5 standard deviations from the prediction are rejected, which protects the EKF from GNSS jumps caused by multipath. But if data is rejected for a long time, the EKF drifts on the accelerometer and may reset to the new GNSS value. Widening the gate to “stop the warnings” is therefore not a fix; find out why the values disagree.