MPU6050 IMU for Robots: Tilt Angle with a Complementary Filter

A self-balancing robot, a drone or a walking robot must know its own tilt many times per second. The MPU6050, usually sold on the GY-521 breakout board, is the most common low-cost sensor for this. It combines a three-axis accelerometer and a three-axis gyroscope on one chip and talks to a microcontroller over I2C.

Neither sensor alone gives a good angle. The accelerometer is noisy and is disturbed by every bump and acceleration, while the gyroscope is smooth but drifts. In this tutorial you will read the raw registers with the standard Wire library, convert them to physical units, calibrate the gyroscope offset, and combine both sensors with a complementary filter to produce a clean tilt angle ready for the balancing robot in the next tutorial.

What the Accelerometer and Gyroscope Measure

The accelerometer measures acceleration including gravity. When the sensor is still, the gravity vector tells you which way is down, so the tilt can be computed with atan2. When the robot accelerates or vibrates, those forces add to gravity and the computed angle jumps around.

The gyroscope measures angular rate in degrees per second. Integrating the rate over time gives the change in angle, and that result is smooth and unaffected by linear acceleration. However, any small constant offset in the rate is integrated too, so the angle slowly drifts away even when the sensor is still.

Setting (default)	Full-scale range	Sensitivity
ACCEL_CONFIG AFS_SEL = 0	plus or minus 2 g	16384 LSB per g
GYRO_CONFIG FS_SEL = 0	plus or minus 250 degrees/s	131 LSB per degree/s

Wiring and Key Registers

The GY-521 board has a 3.3 V regulator and I2C pull-up resistors, so it is normally powered from the Arduino's 5 V pin. Connect SDA to A4 and SCL to A5 on an Uno. With AD0 low or unconnected the I2C address is 0x68; with AD0 high it is 0x69.

Register	Address	Use in this tutorial
PWR_MGMT_1	0x6B	Write 0x00 to wake the chip from sleep mode
CONFIG	0x1A	Digital low-pass filter; value 3 gives about 44 Hz accel and 42 Hz gyro bandwidth
GYRO_CONFIG	0x1B	Write 0x00 for plus or minus 250 degrees/s
ACCEL_CONFIG	0x1C	Write 0x00 for plus or minus 2 g
ACCEL_XOUT_H	0x3B	Start of 14 data bytes: accel XYZ, temperature, gyro XYZ
WHO_AM_I	0x75	Returns 0x68 on a genuine MPU6050

The Complementary Filter

The complementary filter trusts the gyroscope for fast changes and the accelerometer for the long-term average. Each loop it integrates the gyro rate onto the previous angle, then nudges the result slightly towards the accelerometer angle.

angle = alpha * (angle + gyroRate * dt) + (1 - alpha) * accelAngle

alpha = tau / (tau + dt)
Example: dt = 0.005 s (200 Hz), alpha = 0.98 -> tau = 0.98*0.005/0.02 = 0.245 s
Gyro dominates changes faster than about 0.25 s; accelerometer corrects slower drift

Choosing alpha is a trade-off. A value closer to 1, such as 0.995, rejects more vibration from the motors but takes longer to remove gyro drift and to recover from a wrong starting angle. A smaller value, such as 0.95, corrects drift quickly but lets more accelerometer noise through. Because alpha and the loop period together set tau, keep the loop rate fixed; if you change the rate, recompute alpha so that tau stays the same. A Kalman filter can do slightly better, but for a balancing robot the complementary filter is simpler to understand and tune, and usually good enough.

Arduino Code

This sketch measures tilt about the sensor's X axis, using the Y and Z accelerometer readings and the X gyro rate. If your board is mounted differently, change the axes as described in the comments.

MPU6050 register-level reading, gyro calibration and complementary filter at 200 Hz

#include <Wire.h>

const uint8_t MPU = 0x68;      // 0x69 if AD0 is high
const float ALPHA = 0.98;
const unsigned long LOOP_US = 5000;  // 200 Hz

float gyroOffsetX = 0;
float angle = 0;
unsigned long lastUs;

void writeReg(uint8_t reg, uint8_t val) {
  Wire.beginTransmission(MPU);
  Wire.write(reg);
  Wire.write(val);
  Wire.endTransmission();
}

// Read accel Y, Z and gyro X as raw 16-bit signed values
void readRaw(int16_t &ay, int16_t &az, int16_t &gx) {
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);                       // ACCEL_XOUT_H
  Wire.endTransmission(false);            // repeated start
  Wire.requestFrom(MPU, (uint8_t)14);
  int16_t d[7];
  for (int i = 0; i < 7; i++) {
    uint8_t hi = Wire.read();             // high byte comes first
    uint8_t lo = Wire.read();             // read separately: C++ does not fix operand order
    d[i] = (int16_t)((hi << 8) | lo);
  }
  // d[0..2] = accel X,Y,Z  d[3] = temperature  d[4..6] = gyro X,Y,Z
  ay = d[1]; az = d[2]; gx = d[4];
}

void setup() {
  Serial.begin(115200);
  Wire.begin();
  Wire.setClock(400000);                  // 400 kHz fast-mode I2C

  writeReg(0x6B, 0x00);                   // wake up
  writeReg(0x1A, 0x03);                   // DLPF ~44/42 Hz
  writeReg(0x1B, 0x00);                   // gyro +-250 dps
  writeReg(0x1C, 0x00);                   // accel +-2 g

  Wire.beginTransmission(MPU); Wire.write(0x75); Wire.endTransmission(false);
  Wire.requestFrom(MPU, (uint8_t)1);
  Serial.print("WHO_AM_I = 0x"); Serial.println(Wire.read(), HEX);

  // Gyro calibration: keep the sensor perfectly still for ~2 s
  long sum = 0;
  int16_t ay, az, gx;
  for (int i = 0; i < 1000; i++) {
    readRaw(ay, az, gx);
    sum += gx;
    delay(2);
  }
  gyroOffsetX = sum / 1000.0;

  readRaw(ay, az, gx);
  angle = atan2((float)ay, (float)az) * 180.0 / PI;  // start from accel angle
  lastUs = micros();
}

void loop() {
  if (micros() - lastUs < LOOP_US) return;
  unsigned long now = micros();
  float dt = (now - lastUs) / 1000000.0;
  lastUs = now;

  int16_t ay, az, gx;
  readRaw(ay, az, gx);

  float accAngle = atan2((float)ay, (float)az) * 180.0 / PI;  // degrees
  float gyroRate = (gx - gyroOffsetX) / 131.0;                // degrees per second

  angle = ALPHA * (angle + gyroRate * dt) + (1.0 - ALPHA) * accAngle;

  static int n = 0;
  if (++n >= 20) {            // print at 10 Hz so Serial does not slow the loop
    n = 0;
    Serial.print(accAngle, 1); Serial.print("  ");
    Serial.println(angle, 2);
  }
}

Testing and What to Look For

Open the Serial Plotter at 115200 baud. The first trace is the raw accelerometer angle and the second is the filtered angle. Tap the table next to the sensor: the accelerometer trace spikes while the filtered trace barely moves. Tilt the board slowly to 45 degrees and hold: both traces settle on the same value, showing that the gyro drift is being corrected.

All readings zero or -1: wrong wiring or address; run an I2C scanner sketch to confirm 0x68.
Angle drifts steadily: the board moved during calibration; restart with it still.
Angle has the wrong sign or axis: swap to accel X/Z with gyro Y, or negate the angle, to match your mounting.
Reading freezes after motors start: I2C wires picking up noise; keep them short and away from motor leads.

Summary

The MPU6050 gives raw accelerometer and gyroscope data over I2C after waking it through PWR_MGMT_1. Convert with 16384 LSB per g and 131 LSB per degree per second, subtract the measured gyro offset, and fuse the two angles with a complementary filter running at a fixed rate. The result is a fast, stable tilt angle, the essential input for a self-balancing robot.