Skip to content

Advanced 4: Sensor Fusion

Prerequisites: Lab 4 (IMU & Heading)

Overview

In Lab 4, you used the gyroscope for heading control. But gyroscopes drift over time, and accelerometers are noisy. This module shows how to combine multiple imperfect sensors into something better.

You'll learn: - Why single sensors fail over time - Complementary filter: the practical solution - Kalman filter: the optimal solution (conceptual) - Odometry and why dead reckoning drifts

Part 1: The Sensor Fusion Problem

Every Sensor is Wrong

Sensor Good At Bad At
Gyroscope Fast changes Drifts over time
Accelerometer Absolute tilt Noisy, affected by motion
Magnetometer Absolute heading Magnetic interference
Wheel encoder Short distances Slips, accumulates error
GPS Absolute position Slow update, poor indoors

The insight: Different sensors fail in different ways. Combine them!

Short-Term vs Long-Term

Gyroscope error over time:
     Error
       │        ╱
       │       ╱
       │      ╱
       │     ╱  ← Drift accumulates!
       │    ╱
       │   ╱
       └──╱──────────────► Time

Accelerometer error over time:
     Error
       │  ╲  ╱╲  ╱╲  ╱╲
       │   ╲╱  ╲╱  ╲╱  ╲  ← Noisy but no drift
       │
       └──────────────────► Time

Strategy: - Trust gyro for short-term (fast, accurate) - Trust accelerometer for long-term (no drift)

Part 2: Complementary Filter

The Idea

Combine high-pass filtered gyro with low-pass filtered accelerometer:

\[\theta = \alpha \times (\theta_{prev} + \omega \times dt) + (1-\alpha) \times \theta_{accel}\]

Where: - \(\theta\) = estimated angle - \(\omega\) = gyroscope angular velocity - \(\theta_{accel}\) = angle from accelerometer - \(\alpha\) = filter coefficient (typically 0.95-0.98) - \(dt\) = time step

Intuition

                     ┌─────────────────┐
Gyroscope ──────────►│ High-Pass Filter├──┐
(good short-term)    └─────────────────┘  │
                                          ▼
                                        ┌───┐
                                        │ + │────► Fused Angle
                                        └───┘
                     ┌─────────────────┐  ▲
Accelerometer ──────►│ Low-Pass Filter ├──┘
(good long-term)     └─────────────────┘

Implementation

import time
import math

class ComplementaryFilter:
    def __init__(self, alpha=0.96):
        self.alpha = alpha
        self.angle = 0
        self.last_time = time.ticks_us()

    def update(self, gyro_rate, accel_angle):
        """
        Update filter with new measurements.

        Args:
            gyro_rate: Angular velocity from gyroscope (deg/s)
            accel_angle: Angle calculated from accelerometer (deg)

        Returns:
            Fused angle estimate (deg)
        """
        # Calculate time step
        now = time.ticks_us()
        dt = time.ticks_diff(now, self.last_time) / 1_000_000  # seconds
        self.last_time = now

        # Prevent huge jumps on first call or after pause
        if dt > 0.1:
            dt = 0.01

        # Complementary filter equation
        # Gyro: integrate rate to get angle change
        # Accel: direct angle measurement
        gyro_angle = self.angle + gyro_rate * dt
        self.angle = self.alpha * gyro_angle + (1 - self.alpha) * accel_angle

        return self.angle

    def reset(self, angle=0):
        self.angle = angle
        self.last_time = time.ticks_us()

Calculating Angle from Accelerometer

For pitch (tilt forward/back):

\[\theta_{pitch} = \arctan\left(\frac{a_x}{\sqrt{a_y^2 + a_z^2}}\right)\]

For roll (tilt left/right):

\[\theta_{roll} = \arctan\left(\frac{a_y}{\sqrt{a_x^2 + a_z^2}}\right)\]
def accel_to_angles(ax, ay, az):
    """Convert accelerometer readings to angles (degrees)."""
    # Avoid division by zero
    if az == 0:
        az = 0.001

    pitch = math.atan2(ax, math.sqrt(ay*ay + az*az)) * 180 / math.pi
    roll = math.atan2(ay, math.sqrt(ax*ax + az*az)) * 180 / math.pi

    return pitch, roll

Complete Example

from machine import I2C, Pin
import time
import math

# Assuming you have IMU reading functions
# from your_imu_library import read_gyro, read_accel

class IMUFusion:
    def __init__(self, i2c, alpha=0.96):
        self.i2c = i2c
        self.alpha = alpha

        # Initialize complementary filters for pitch and roll
        self.pitch_filter = ComplementaryFilter(alpha)
        self.roll_filter = ComplementaryFilter(alpha)

        # Calibration offsets (measure when stationary)
        self.gyro_offset = [0, 0, 0]
        self.calibrate()

    def calibrate(self, samples=100):
        """Calibrate gyroscope offset."""
        print("Calibrating... keep IMU still")
        sums = [0, 0, 0]

        for _ in range(samples):
            gx, gy, gz = self.read_gyro_raw()
            sums[0] += gx
            sums[1] += gy
            sums[2] += gz
            time.sleep_ms(10)

        self.gyro_offset = [s / samples for s in sums]
        print(f"Gyro offset: {self.gyro_offset}")

    def read_gyro_raw(self):
        """Read raw gyroscope (implement for your IMU)."""
        # Return gx, gy, gz in deg/s
        pass

    def read_accel_raw(self):
        """Read raw accelerometer (implement for your IMU)."""
        # Return ax, ay, az in g
        pass

    def update(self):
        """Read sensors and update fused angles."""
        # Read sensors
        gx, gy, gz = self.read_gyro_raw()
        ax, ay, az = self.read_accel_raw()

        # Apply gyro calibration
        gx -= self.gyro_offset[0]
        gy -= self.gyro_offset[1]
        gz -= self.gyro_offset[2]

        # Calculate accelerometer angles
        accel_pitch, accel_roll = accel_to_angles(ax, ay, az)

        # Update complementary filters
        pitch = self.pitch_filter.update(gy, accel_pitch)  # gy for pitch
        roll = self.roll_filter.update(gx, accel_roll)     # gx for roll

        return pitch, roll

# Usage
# imu = IMUFusion(i2c, alpha=0.96)
# while True:
#     pitch, roll = imu.update()
#     print(f"Pitch: {pitch:6.1f}  Roll: {roll:6.1f}")
#     time.sleep_ms(10)

Choosing Alpha

Alpha Trust Gyro Trust Accel Best For
0.99 99% 1% Very noisy accel, fast motions
0.96 96% 4% Typical robots (good default)
0.90 90% 10% More accel trust, slower motions
0.80 80% 20% Static applications

Rule of thumb: Higher alpha = more responsive, more drift. Lower alpha = more stable, more noise.

Part 3: Gyroscope Drift Analysis

Why Gyroscopes Drift

  1. Bias offset: Non-zero output when stationary
  2. Bias drift: Offset changes over time (temperature, aging)
  3. Integration accumulates error: Small errors add up!

Quantifying Drift

def measure_gyro_drift(duration_sec=60):
    """Measure gyroscope drift over time."""
    print(f"Measuring drift for {duration_sec} seconds...")
    print("Keep IMU completely still!")

    integrated_angle = 0
    last_time = time.ticks_us()
    samples = []

    start = time.ticks_ms()
    while time.ticks_diff(time.ticks_ms(), start) < duration_sec * 1000:
        # Read gyro
        _, _, gz = read_gyro()  # Assuming Z is yaw axis

        # Integrate
        now = time.ticks_us()
        dt = time.ticks_diff(now, last_time) / 1_000_000
        last_time = now

        integrated_angle += gz * dt

        # Record every second
        if len(samples) < time.ticks_diff(time.ticks_ms(), start) // 1000:
            samples.append(integrated_angle)
            print(f"{len(samples):3d}s: {integrated_angle:8.2f} deg")

        time.sleep_ms(10)

    drift_rate = integrated_angle / duration_sec
    print(f"\nTotal drift: {integrated_angle:.2f} deg")
    print(f"Drift rate: {drift_rate:.3f} deg/sec = {drift_rate*60:.1f} deg/min")

    return drift_rate

Typical Drift Values

IMU Typical Drift Notes
MPU6050 0.5-2 deg/sec Consumer grade
BMI160 0.1-0.5 deg/sec Better consumer
ADXRS450 0.01 deg/sec Industrial grade
Fiber optic gyro 0.001 deg/hour Navigation grade ($$$)

For your robot at 1 deg/sec drift: - After 1 minute: 60° error! - After 10 minutes: 600° error (multiple full rotations)

Part 4: Kalman Filter (Conceptual)

Why Kalman?

The complementary filter is simple but not optimal. The Kalman filter: - Adapts to changing conditions - Provides uncertainty estimates - Is mathematically optimal (under assumptions)

The Kalman Idea

Predict: Use model to predict next state Update: Correct prediction using measurement

┌────────────────────────────────────────────────┐
│                                                │
│    ┌─────────┐         ┌─────────┐            │
│    │ Predict │────────►│  State  │◄───┐       │
│    └─────────┘         └─────────┘    │       │
│         ▲                             │       │
│         │                       ┌─────┴─────┐ │
│    ┌────┴────┐                  │   Update  │ │
│    │  Model  │                  └───────────┘ │
│    └─────────┘                        ▲       │
│                                       │       │
│                               ┌───────┴─────┐ │
│                               │ Measurement │ │
│                               └─────────────┘ │
│                                               │
└───────────────────────────────────────────────┘

Simplified 1D Kalman

For angle estimation:

class SimpleKalman:
    def __init__(self, q=0.001, r=0.03):
        """
        Args:
            q: Process noise (how much we trust the model)
            r: Measurement noise (how much we trust sensors)
        """
        self.q = q  # Process variance
        self.r = r  # Measurement variance
        self.x = 0  # State estimate
        self.p = 1  # Estimate uncertainty

    def update(self, measurement, control_input=0, dt=0.01):
        """
        Update filter with measurement.

        Args:
            measurement: Sensor reading
            control_input: Known input (e.g., gyro rate)
            dt: Time step
        """
        # Predict
        self.x = self.x + control_input * dt  # Predicted state
        self.p = self.p + self.q              # Predicted uncertainty

        # Update
        k = self.p / (self.p + self.r)        # Kalman gain
        self.x = self.x + k * (measurement - self.x)  # Updated state
        self.p = (1 - k) * self.p             # Updated uncertainty

        return self.x

Full Kalman for IMU

A proper IMU Kalman filter estimates: - Angle (pitch, roll, yaw) - Angular velocity - Gyro bias (learns the drift!)

This requires matrix math - beyond our scope, but libraries exist: - MicroPython: fusion library - Arduino: Kalman library - Python: filterpy library

Complementary vs Kalman

Aspect Complementary Kalman
Simplicity Very simple Complex
Tuning One parameter (α) Multiple parameters
Adaptivity Fixed Adapts to conditions
Bias estimation No Yes (can learn drift)
Uncertainty No estimate Provides estimate
CPU usage Minimal Higher
Performance Good Optimal

For your robot: Complementary filter is usually sufficient!

Part 5: Odometry and Dead Reckoning

What is Odometry?

Estimating position from motion measurements:

Start ──[Move 10cm]──[Turn 90°]──[Move 10cm]── Where am I?

       ▲
       │ 10cm
       │
 Start ●───────► X
             10cm

The Math

Given wheel velocities \(v_L\) and \(v_R\):

def update_odometry(v_left, v_right, dt, wheelbase):
    """
    Update robot position estimate.

    Args:
        v_left, v_right: Wheel velocities (m/s)
        dt: Time step (s)
        wheelbase: Distance between wheels (m)

    Returns:
        dx, dy, dtheta: Position change
    """
    # Average velocity and angular velocity
    v = (v_left + v_right) / 2
    omega = (v_right - v_left) / wheelbase

    # Position change in robot frame
    if abs(omega) < 0.001:
        # Straight line
        dx_robot = v * dt
        dy_robot = 0
    else:
        # Arc
        radius = v / omega
        dx_robot = radius * math.sin(omega * dt)
        dy_robot = radius * (1 - math.cos(omega * dt))

    dtheta = omega * dt

    return dx_robot, dy_robot, dtheta

Why Dead Reckoning Drifts

Errors accumulate because: 1. Wheel slip: Wheels don't grip perfectly 2. Measurement noise: Encoder resolution limits 3. Systematic errors: Wheel diameter differences 4. Integration: Errors compound over time

Intended path:    Actual path (with drift):

    ●───●             ●───●
    │   │              \   \
    │   │               \   \
    ●───●                \   ●
                          \
                           ●

Reducing Odometry Drift

  1. Calibrate carefully: Measure actual wheel diameters
  2. Use encoders: Better than timing-based estimates
  3. Fuse with absolute sensors: IMU for heading, landmarks for position
  4. Periodic reset: Return to known position
class RobotOdometry:
    def __init__(self, wheelbase, wheel_radius):
        self.wheelbase = wheelbase
        self.wheel_radius = wheel_radius
        self.x = 0
        self.y = 0
        self.theta = 0  # Heading from IMU (not integrated from wheels!)

    def update(self, left_encoder, right_encoder, imu_heading, dt):
        """Update position using encoders + IMU heading."""
        # Wheel velocities from encoders
        v_left = left_encoder.velocity()
        v_right = right_encoder.velocity()

        # Use IMU for heading (more accurate than wheel-based)
        self.theta = imu_heading

        # Forward velocity
        v = (v_left + v_right) / 2

        # Update position
        self.x += v * math.cos(self.theta) * dt
        self.y += v * math.sin(self.theta) * dt

    def reset(self, x=0, y=0, theta=0):
        self.x = x
        self.y = y
        self.theta = theta

Part 6: Multi-Sensor Fusion Architecture

Typical Robot Sensor Suite

┌─────────────────────────────────────────────────────────┐
│                    Robot Sensors                        │
├─────────────┬─────────────┬─────────────┬──────────────┤
│   Encoders  │     IMU     │  Ultrasonic │ Line Sensors │
│  (velocity) │ (orientation)│  (distance) │  (position)  │
└──────┬──────┴──────┬──────┴──────┬──────┴──────┬───────┘
       │             │             │             │
       ▼             ▼             ▼             ▼
┌──────────────────────────────────────────────────────────┐
│                    Sensor Fusion                         │
│  ┌────────────┐  ┌────────────┐  ┌────────────────────┐ │
│  │ Odometry   │  │Comp. Filter│  │ Obstacle Detection │ │
│  │ (position) │  │ (heading)  │  │     (distance)     │ │
│  └─────┬──────┘  └─────┬──────┘  └─────────┬──────────┘ │
└────────┼───────────────┼───────────────────┼────────────┘
         │               │                   │
         ▼               ▼                   ▼
┌──────────────────────────────────────────────────────────┐
│                    Robot State                           │
│         x, y, theta, velocity, obstacles nearby          │
└──────────────────────────────────────────────────────────┘
         │
         ▼
┌──────────────────────────────────────────────────────────┐
│                    Control System                        │
│               (uses fused state for decisions)           │
└──────────────────────────────────────────────────────────┘

Sensor Trust Levels

Assign confidence based on conditions:

class AdaptiveFusion:
    def __init__(self):
        self.accel_trust = 1.0

    def update_trust(self, ax, ay, az):
        """Reduce accelerometer trust during high acceleration."""
        # Calculate total acceleration magnitude
        accel_mag = math.sqrt(ax*ax + ay*ay + az*az)

        # If accelerating hard, don't trust accel for angle
        # (should be ~1g when stationary)
        if abs(accel_mag - 1.0) > 0.2:
            self.accel_trust = 0.5  # Reduce trust
        else:
            self.accel_trust = 1.0  # Full trust

    def fuse(self, gyro_angle, accel_angle):
        """Fuse with adaptive trust."""
        # Adjust alpha based on trust
        effective_alpha = 0.96 + 0.03 * (1 - self.accel_trust)
        return effective_alpha * gyro_angle + (1 - effective_alpha) * accel_angle

Mini-Project: Stable Heading System

Part A: Implement Complementary Filter

  1. Implement the ComplementaryFilter class
  2. Connect to your IMU
  3. Test with manual rotation
  4. Compare raw gyro integration vs fused angle

Part B: Measure Drift

  1. Run gyro-only integration for 60 seconds
  2. Measure total drift
  3. Run complementary filter for 60 seconds
  4. Compare drift reduction

Part C: Tune Alpha

  1. Test alpha values: 0.90, 0.95, 0.98, 0.99
  2. Measure noise (standard deviation when still)
  3. Measure drift (error after 60 seconds)
  4. Find optimal alpha for your IMU

Part D: Heading-Controlled Maneuvers

  1. Implement heading control using fused angle
  2. Drive a square (90° turns)
  3. Measure final position error
  4. Compare with gyro-only heading

Deliverable

Report including: - Complementary filter implementation - Drift comparison (gyro vs fused) - Alpha tuning results table - Square maneuver accuracy data - Conclusions on sensor fusion benefits

Key Takeaways

  1. No sensor is perfect - all have different failure modes
  2. Combine short-term and long-term - gyro + accel = stable angle
  3. Complementary filter is simple and effective - great for robots
  4. Kalman is optimal but complex - use libraries if needed
  5. Dead reckoning drifts - fuse with absolute references
  6. Adapt trust to conditions - don't trust accel during acceleration

Further Reading

← Back: Control Theory | Advanced Track Overview | Next: Software Architecture →