Sensor Fusion

Sensor fusion combines data from multiple sensors to produce a better estimate than any single sensor alone. Each sensor has different strengths and weaknesses — fusion exploits their complementary properties.

Why It Matters

No sensor is perfect. Accelerometers are noisy but don’t drift. Gyroscopes are smooth but drift over time. GPS is absolute but slow and noisy. Fusing them gives you the best of each: accurate, low-noise, drift-free estimates. Every drone, phone, and autonomous vehicle depends on sensor fusion.

The Problem

Sensor	Measures	Strength	Weakness
Accelerometer	Tilt (from gravity vector)	No drift	Noisy, vibration-sensitive
Gyroscope	Angular rate (integrate → angle)	Smooth, fast	Drifts over time
Magnetometer	Heading (Earth’s magnetic field)	Absolute reference	Distorted by metal, motors
GPS	Position, velocity	Absolute, no drift	Slow (1-10 Hz), noisy (±2m)
Barometer	Altitude (from pressure)	Stable	Affected by weather, doors

No single sensor gives good attitude/position alone. Fusion combines them.

Complementary Filter (Simple, Effective)

The key insight: gyroscope is good for short-term (high frequency), accelerometer is good for long-term (low frequency). A complementary filter blends them:

angle = α · (angle + gyro_rate · dt) + (1 - α) · accel_angle
         ────── high-pass ──────       ──── low-pass ────

alpha = 0.98  # trust gyro 98% for fast changes, accel 2% for drift correction
dt = 0.01     # 100 Hz
 
angle = 0.0
angles = []
for accel_angle, gyro_rate in sensor_data:
    angle = alpha * (angle + gyro_rate * dt) + (1 - alpha) * accel_angle
    angles.append(angle)

How to choose α: higher → more gyro trust → smoother but more drift. Lower → more accel trust → noisier but less drift. α = 0.96-0.99 works well for most IMU applications at 100+ Hz.

Why It Works

In the frequency domain:

• α · (angle + gyro·dt) is a first-order high-pass filter on the gyro
• (1-α) · accel_angle is a first-order low-pass filter on the accelerometer
• The two filters sum to 1 at all frequencies → “complementary”

Gyro drift is low-frequency → filtered out by high-pass. Accelerometer noise is high-frequency → filtered out by low-pass.

Kalman Filter (Optimal, More Complex)

The Kalman Filter is the optimal linear estimator for Gaussian noise. It uses a system model to predict, then corrects with measurements, weighted by how much it trusts each.

Aspect	Complementary Filter	Kalman Filter
Complexity	One line of code	~20 lines + matrix math
Tuning	One parameter (α)	Q and R matrices
Optimality	Good enough	Optimal for linear + Gaussian
Multi-sensor	Awkward beyond 2 sensors	Natural — just add measurements
Computational cost	Trivial	Matrix multiplications per step
Nonlinear systems	No support	EKF/UKF handle it

Use complementary filter when: you have 2 sensors, real-time is critical, and “good enough” is fine. Use Kalman when: multiple sensors, you need optimal estimates, or the system is nonlinear.

Extended Kalman Filter (EKF)

Standard Kalman assumes linear dynamics. Rotation is nonlinear (sin/cos in equations). The EKF linearizes around the current estimate at each step:

1. Predict using the nonlinear model: x̂⁻ = f(x̂, u)
2. Linearize: compute Jacobian F = ∂f/∂x at current estimate
3. Update covariance: P⁻ = F·P·Fᵀ + Q
4. Standard Kalman update step with measurement

Used in: drone flight controllers (attitude estimation), GPS/INS navigation, robot localization.

Madgwick Filter (Practical Alternative)

Gradient descent approach — computationally lighter than EKF, better than complementary for 9-DOF IMU (accel + gyro + mag):

# Madgwick filter update (simplified concept)
# Uses quaternion representation to avoid gimbal lock
q = quaternion_from_gyro(q, gyro, dt)           # integrate gyro
q_correction = gradient_descent(q, accel, mag)   # correct toward gravity + north
q = normalize(q + beta * q_correction)           # blend

Used in: many open-source IMU libraries, robotics, and motion tracking. Parameter beta controls correction strength (similar to 1-α in complementary filter).

Practical: IMU Fusion for Drone Attitude

import numpy as np
 
class ComplementaryFilter:
    def __init__(self, alpha=0.98, dt=0.004):  # 250 Hz
        self.alpha = alpha
        self.dt = dt
        self.roll = 0.0
        self.pitch = 0.0
 
    def update(self, accel, gyro):
        # Accel-based angles (only valid when not accelerating)
        accel_roll = np.arctan2(accel[1], accel[2])
        accel_pitch = np.arctan2(-accel[0], np.sqrt(accel[1]**2 + accel[2]**2))
 
        # Complementary fusion
        self.roll = self.alpha * (self.roll + gyro[0] * self.dt) + \
                    (1 - self.alpha) * accel_roll
        self.pitch = self.alpha * (self.pitch + gyro[1] * self.dt) + \
                     (1 - self.alpha) * accel_pitch
 
        return self.roll, self.pitch

Note: yaw cannot be estimated from accelerometer alone (gravity doesn’t help with heading). Use magnetometer or GPS heading for yaw fusion.

Related

• Kalman Filter — optimal estimation theory and implementation
• IMU and Sensors — sensor hardware and noise characteristics
• Drone Flight Dynamics — attitude estimation feeds into flight control
• PID Controller — uses fused estimates as feedback input
• Signal Processing — filtering concepts (low-pass, high-pass)