Sensor Fusion Basics

Learning Goals 5 min

The accelerometer is noisy. The gyroscope drifts. Combined cleverly, you get a clean tilt angle in real time — "sensor fusion". The simplest fusion is the complementary filter: one line of code, perfectly good for self-balancing robots. By the end of this lesson you will:

1. Implement a complementary filter to fuse accel + gyro into a tilt estimate.
2. Understand why this is "low-pass on accel + high-pass on gyro" intuitively.
3. Compare against the noisier (accel only) and drifty (gyro only) baselines.

Warm-Up 10 min

Hardware: same MPU6050 from yesterday + USB to laptop.

The complementary filter idea

Two estimates of the same angle:

• Accel angle: noisy under motion, but always correct on average (gravity doesn't drift).
• Gyro angle: smooth in the short term, but drifts long term.

The trick: use the gyro for fast changes, the accel for the slow truth. Mathematically:

angle = α × (angle + gyroRate × dt) + (1 − α) × accelAngle

α is typically 0.98. Most of the new estimate comes from integrating the gyro (smooth, short-term); a small portion is pulled towards the accel measurement (corrects long-term drift).

New Concept · The complementary filter 25 min

The maths in one line

float alpha = 0.98;
float angle = 0.0;
unsigned long lastT = 0;

void updateAngle() {
  sensors_event_t a, g, t;
  mpu.getEvent(&a, &g, &t);

  unsigned long now = millis();
  float dt = (now - lastT) / 1000.0;
  if (lastT == 0) dt = 0;
  lastT = now;

  // Accelerometer pitch angle (degrees)
  float accelAngle = atan2(a.acceleration.x,
                           sqrt(a.acceleration.y * a.acceleration.y +
                                a.acceleration.z * a.acceleration.z))
                     * 180.0 / PI;

  // Gyro rate (deg/s) — apply your calibrated offsets first
  float gyroRate = (g.gyro.y - gyroOffsetY) * 180.0 / PI;

  // Fuse
  angle = alpha * (angle + gyroRate * dt) + (1 - alpha) * accelAngle;
}

The fused angle is smooth AND drift-free.

Tuning α

Adjust based on sample rate and noise level. 100 Hz update rate with α = 0.98 is standard.

Why it works (intuitively)

Mathematically equivalent to: low-pass filter on the accel angle + high-pass filter on the gyro integral. Each sensor contributes the frequencies it's good at. Vibrations on the accel are filtered out (low-pass kills high freq); drift on the gyro is filtered out (high-pass kills DC).

Kalman vs complementary

The Kalman filter is a more rigorous fusion technique with optimal weighting based on noise covariance. For 6-axis IMU pitch, the complementary filter gives 95% of the benefit at 5% of the maths. Use Kalman if you need maximum precision (drone autopilot) or want to learn for its own sake.

Worked Example · Compare three angle estimates 25 min

The instrument sketch

Print three values per loop: accel-only angle, gyro-integrated angle, fused angle. Plot in Serial Plotter.

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

Adafruit_MPU6050 mpu;

float fusedAngle = 0.0;
float gyroAngle  = 0.0;     // pure integration baseline
float gyroOffsetY = 0.0;
const float alpha = 0.98;
unsigned long lastT = 0;
const unsigned long PERIOD_MS = 10;

void calibrateGyro() {
  long sum = 0;
  const int N = 500;
  for (int i = 0; i < N; i++) {
    sensors_event_t a, g, t;
    mpu.getEvent(&a, &g, &t);
    sum += g.gyro.y * 1000.0;
    delay(2);
  }
  gyroOffsetY = sum / 1000.0 / N;
  Serial.print("# gyro Y offset = "); Serial.println(gyroOffsetY);
}

void setup() {
  Serial.begin(115200);
  Wire.begin();
  if (!mpu.begin()) while (true);
  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);
  calibrateGyro();

  // Init fused angle to the first accel reading
  sensors_event_t a, g, t;
  mpu.getEvent(&a, &g, &t);
  fusedAngle = atan2(a.acceleration.x,
                     sqrt(a.acceleration.y * a.acceleration.y +
                          a.acceleration.z * a.acceleration.z))
               * 180.0 / PI;
  gyroAngle = fusedAngle;
  lastT = millis();
}

void loop() {
  unsigned long now = millis();
  if (now - lastT < PERIOD_MS) return;
  float dt = (now - lastT) / 1000.0;
  lastT = now;

  sensors_event_t a, g, t;
  mpu.getEvent(&a, &g, &t);

  float accelAngle = atan2(a.acceleration.x,
                           sqrt(a.acceleration.y * a.acceleration.y +
                                a.acceleration.z * a.acceleration.z))
                     * 180.0 / PI;

  float gyroRate = (g.gyro.y - gyroOffsetY) * 180.0 / PI;
  gyroAngle += gyroRate * dt;
  fusedAngle = alpha * (fusedAngle + gyroRate * dt) + (1 - alpha) * accelAngle;

  Serial.print(accelAngle); Serial.print(" ");
  Serial.print(gyroAngle);  Serial.print(" ");
  Serial.println(fusedAngle);
}

Test results to expect

• Hold still → all three roughly equal.
• Tap the table → accelAngle spikes; gyroAngle stays steady; fusedAngle stays mostly steady (small bump).
• Leave for 60 s without moving → accelAngle and fusedAngle stay close to true angle; gyroAngle slowly drifts away (visibly off after a minute).
• Tilt slowly to 30° and hold → all three converge on 30°.

The fused signal has the best of both: smooth like the gyro, drift-free like the accel.

Try It Yourself 15 min

Mini-Challenge · Predict tomorrow's build 10 min

Tomorrow you build a self-balancing robot. It needs:

• A fused tilt angle (this lesson's output).
• A PID controller (L04-24).
• Two motors with encoders (L04-27).
• An L298N motor driver.

Sketch on paper: what does the loop look like? Pseudocode it. Tomorrow we'll see if your prediction matches the real sketch.

Recap 5 min

Complementary filter = 1 line of code; combines accel (smooth, drift-free in the long run) with gyro (responsive, but drifts). α ≈ 0.98 is the standard. The output is the "real" tilt angle suitable for closed-loop control. Tomorrow we use it to balance.

Sensor fusion

Combining multiple sensor readings into a better estimate than any single one alone.

Complementary filter

Simple fusion: angle = α × (gyro-integrated) + (1−α) × accel-measured. Cheap, effective.

Low-pass / high-pass equivalence

The complementary filter is mathematically a low-pass on the accel + a high-pass on the gyro integral.

α (alpha)

The weighting coefficient. 0.98 means 98% gyro, 2% accel per sample.

Kalman filter

Optimal fusion based on noise covariance. More accurate but more complex.

Drift

Slow accumulation of integration error from a gyro's residual bias.

Sample period (dt)

Time between filter updates. Must be consistent for the integration to work properly.

Homework 5 min

1. Build the §4 three-line plot. Watch the fused vs gyro vs accel comparison.
2. Save the fused-angle code as a reusable function for tomorrow.
3. Read ahead to ARD-L04-30 (Self-Balancing Robot v1).