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:
- Implement a complementary filter to fuse accel + gyro into a tilt estimate.
- Understand why this is "low-pass on accel + high-pass on gyro" intuitively.
- 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 α
| α | Behaviour |
|---|---|
| 0.99 | Mostly gyro. Smooth but slow to correct drift. |
| 0.98 | Default for balancing robots. |
| 0.95 | More accel weighting. Faster correction but noisier. |
| 0.90 | Half accel, half gyro. Noisy under motion. |
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.
Basic 5 min
Goal: Try α = 0.90 and α = 0.99. Compare the noise / responsiveness trade-off.
1. Think first (on paper, no typing yet):
- Predict: with α = 0.90, is the output closer to the accelerometer or the gyro? Will it be noisier or smoother?
- And with 0.99?
- What three tests will you do with each value so the comparison is fair?
2. Code it. In the instrument sketch, change only alpha. Upload, test, then repeat for the other value.
3. Upload and observe. In the Plotter: hold the board still, then tilt it quickly to 30°, then tap the table.
It works if:
- For each α you have noted how noisy it is when still, how quickly it reaches 30°, and how big the tap spike is.
- You can say which α you would choose for a balancing robot, and why.
Challenge 1 5 min
Goal: Drive a servo's angle from the fused tilt. The servo arm follows your hand-held IMU's tilt smoothly.
1. Think first (on paper, no typing yet):
- A servo takes 0 to 180°. Your angle is roughly −90 to +90. How do you convert one to the other?
- What should happen if the angle goes past the servo's range?
- Should the servo follow the raw accelerometer angle or the fused one? Why?
2. Code it. Add a servo on pin 9 to the instrument sketch. Keep the 10 ms filter timing as it is.
3. Upload and observe. Hold the IMU in your hand. Tilt it slowly, then quickly, then tap it.
It works if:
- Flat: the servo arm sits at 90°.
- A 30° tilt moves the arm about 30° the matching way.
- The arm moves smoothly, barely twitches on a tap, and never slams into its end stop.
Challenge 2 5 min
Goal: Install a proper Kalman filter library (e.g. "SimpleKalmanFilter"). Compare its output to the complementary filter. Note: more accurate but harder to tune.
1. Think first (on paper, no typing yet):
- This kind of filter smooths one noisy number. Which number will you feed it?
- It has a few settings that trade smoothness against speed. Which one would you change to make it smoother?
- What will you print so the Plotter shows the two filters side by side?
2. Code it. Install the library from the Library Manager. Open its example first to see how it is used, then add it to your instrument sketch next to the complementary filter.
3. Upload and observe. Plot three lines: raw accelerometer angle, complementary filter, Kalman filter. Hold still, tilt, then tap.
It works if:
- Three lines on one plot.
- You can describe one moment where the Kalman filter looks better, and one where it lags or is harder to tune.
Challenge 3 · Fix the broken filter 10 min
A classmate wrote a complementary filter from memory. It uploads, but the angle is useless. Held still at 30°, it jumps about like the raw accelerometer. A tap on the table makes it spike. There are four mistakes.
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
Adafruit_MPU6050 mpu;
const float ALPHA = 0.98;
const unsigned long PERIOD_MS = 10;
float gyroOffsetY = 0.0;
unsigned long lastT = 0;
void setup() {
Serial.begin(115200);
Wire.begin();
mpu.begin();
lastT = millis();
}
void loop() {
float fusedAngle = 0.0;
unsigned long now = millis();
if (now - lastT < PERIOD_MS) return;
float dt = now - lastT;
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;
fusedAngle = (1 - ALPHA) * (fusedAngle + gyroRate * dt) + ALPHA * accelAngle;
Serial.println(fusedAngle);
}For each mistake, write the line, what is wrong, and the corrected line. Then upload your fixed sketch and plot it.
It works if the angle sits steady at 30° when you hold the board there. A tap on the table should barely move the line.
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.
Extra Mission 5 min
Part 1 — Plan the balancing robot's brain
Next lesson you build a self-balancing robot. It needs a fused tilt angle (this lesson), a PID controller (ARD-L04-24), two motors with encoders (ARD-L04-27) and an L298N. On paper, plan its loop() in pseudocode before you see the real sketch.
Your plan must include:
- The order of the steps inside one pass of
loop(). - What the PID controller takes in and what it gives out.
- How fast the loop must run, in Hz, and how you keep it steady.
- A safety rule: when should the motors cut out?
Part 2 — Make it
Write the first half of that brain as a real sketch, with no motors yet. Turn your fused-angle code into a function called readFusedAngle(). Every 10 ms, print the angle and a pretend motor command of 8 × angle. Keep the command between −255 and 255. If the angle passes ±45°, print FALLEN and send a command of 0.
Bring back next class: your uploaded sketch and a Serial Plotter screenshot of the angle and the motor command while you tilt the board. Next lesson, check your pseudocode against the real sketch.