Sensor fusion

Why this matters

An IMU measures acceleration hundreds of times a second, but integrating acceleration into position lets small errors accumulate until the position drifts far away. GNSS gives position without drift, but only a few times a second, with metre-level noise, and sometimes jumps wrongly when signals reflect off buildings. Sensor fusion combines the strengths of both, like walking through a dark room counting steps and touching the wall now and then to correct the count.

The Kalman filter

A Kalman filter estimates a state (such as position and velocity) together with its uncertainty. Following Welch and Bishop, it loops through two steps:

1. Predict: use a motion model and the IMU to move the state forward; the uncertainty $P$ grows every time
2. Update: when a measurement such as GNSS arrives, compute the innovation $y$, the measurement minus the prediction, then move the state towards the measurement weighted by the Kalman gain $K$. The more the measurement is trusted (small R), the larger $K$

Before using a measurement, the system checks whether the innovation is larger than the uncertainty can explain, using the test ratio $y^2/(g^{2}S)$, where $g$ is the gate in standard deviations. Above 1, the measurement is rejected. PX4 EKF2 uses the same principle; its GNSS position gate (EKF2_GPS_P_GATE) defaults to 5 standard deviations.

import numpy as np

rng = np.random.default_rng(348)
dt = 0.01
t = np.arange(0, 20.0, dt)
true_acc = 0.6 * np.sin(0.5 * t)
true_pos = np.cumsum(np.cumsum(true_acc) * dt) * dt
imu_acc = true_acc + 0.05 + rng.normal(0, 0.2, t.size)
gnss = true_pos + rng.normal(0, 1.5, t.size)
gnss[600] += 25.0                                   # bad GNSS fix at t = 6 s

x, P = np.zeros(2), np.diag([4.0, 1.0])             # state [position, velocity]
F, B = np.array([[1, dt], [0, 1]]), np.array([0.5 * dt**2, dt])
Q, R, H, gate = np.diag([1e-4, 4e-4]), 1.5**2, np.array([1.0, 0.0]), 5.0
est, dead, vel, pos = np.zeros(t.size), np.zeros(t.size), 0.0, 0.0
for k in range(t.size):
    x, P = F @ x + B * imu_acc[k], F @ P @ F.T + Q          # predict
    vel += imu_acc[k] * dt
    pos += vel * dt
    dead[k] = pos                                             # IMU only
    if k % 20 == 0:                                           # GNSS every 0.2 s
        y = gnss[k] - H @ x
        S = H @ P @ H + R
        ratio = y**2 / (gate**2 * S)
        if ratio > 1:
            print(f"t = {t[k]:.1f} s: GNSS rejected, test ratio {ratio:.2f}")
        else:
            K = P @ H / S
            x, P = x + K * y, (np.eye(2) - np.outer(K, H)) @ P
    est[k] = x[0]
g = np.arange(0, t.size, 20)
rmse = lambda a, idx=slice(None): np.sqrt(np.mean((a[idx] - true_pos[idx]) ** 2))
print(f"RMSE  IMU only {rmse(dead):.2f} m | GNSS only {rmse(gnss, g):.2f} m | fused {rmse(est):.2f} m")

t = 6.0 s: GNSS rejected, test ratio 8.06
RMSE  IMU only 4.82 m | GNSS only 2.94 m | fused 0.69 m

The EKF in the flight controller

Real systems have dozens of states (position, velocity, attitude, sensor biases, magnetic field, wind) and nonlinear equations, so they use an Extended Kalman Filter (EKF) that linearises around the current estimate. Probabilistic Robotics explains the principle in detail.

• PX4 EKF2 fuses the IMU, magnetometer, GNSS, barometer, range finder, optical flow, external vision and airspeed; each source has its own test ratio, visible in the log
• ArduPilot EKF3 is ArduPilot’s default estimator
• Fusing position from a vision system (such as VIO) into EKF2 is enabled with EKF2_EV_CTRL, a bitmask selecting horizontal position, vertical position, velocity or yaw; it is off by default

VIO (visual-inertial odometry), such as VINS-Mono and OpenVINS, estimates motion from a camera and an IMU and can replace GNSS indoors, but the camera and IMU must be calibrated well first.

Calibrating camera and IMU

Calibration has three parts: intrinsics (the camera model, such as focal length and distortion), extrinsics (the rotation and translation between camera and IMU) and time offset (the timing difference between the two sensors). Tools such as Kalibr report quality as reprojection error: the distance on the image between detected points and points projected from the model.

residuals = [(3, 4), (-1, 2), (0, -2), (2, 1)]      # (dx, dy) in pixels
n = len(residuals)
sq = sum(dx * dx + dy * dy for dx, dy in residuals)
print(f"RMSE per point {(sq / n) ** 0.5:.2f} px, per component {(sq / (2 * n)) ** 0.5:.2f} px")

RMSE per point 3.12 px, per component 2.21 px

The same residuals give different numbers depending on the definition; OpenCV’s calibrateCamera divides by the number of points, so always state the definition. A small value on the calibration set does not confirm quality on new images; check with a separate data set, and record image resolution, lens, distortion model and target size with it.

Module lab

Common mistakes

Summary

• The Kalman filter predicts with a model and the IMU, then updates with measurements weighted by uncertainty
• An innovation gate uses the test ratio to reject outliers; above 1 is rejected
• PX4 EKF2 and ArduPilot EKF3 fuse many sources and log their test ratios
• Camera–IMU calibration covers intrinsics, extrinsics and time offset, and RMSE must be reported with its definition

Check your understanding

1. Why does integrating IMU acceleration alone make position drift?
2. Innovation $y=6$ m, $S=1$ m², gate $g=5$. What is the test ratio, and is the measurement used?
3. If R (measurement variance) increases, does the Kalman gain rise or fall?
4. Two residuals are (3, 4) and (0, 0) px. What is the reprojection RMSE per point?
5. Which PX4 parameter enables fusing position from a vision system?