IMU and attitude estimation

To keep itself level, a drone must know which way is up, and it must know it hundreds of times per second. There is no single sensor that tells it. Instead the flight controller combines two imperfect ones, a gyroscope and an accelerometer, whose errors happen to be opposites. That combination is called attitude estimation, and the simplest useful version of it fits in two lines of code.

What the IMU measures

An IMU (inertial measurement unit) is a chip, usually an MPU6050, ICM-42688 or similar, that packs a 3-axis gyroscope and a 3-axis accelerometer.

The gyroscope measures angular rate in degrees per second (deg/s) around each axis. To get an angle you integrate the rate over time:

angle_k = angle_(k-1) + gyro_k * dt

At 250 Hz, dt = 0.004 s. If the drone rolls at 50 deg/s for one loop, the angle changes by 50 * 0.004 = 0.2 deg. The gyro is smooth, fast and nearly immune to vibration, but it has a small constant offset called bias. Suppose the bias is only 0.5 deg/s. Integrating it for one minute gives:

error = 0.5 deg/s * 60 s = 30 deg

That is drift: the estimate wanders away from reality without bound, so the gyro alone cannot hold a hover.

The accelerometer measures specific force in units of g. When the drone is not accelerating, the only force it senses is gravity, which points straight down, so the tilt can be computed directly from where gravity appears in the three axes:

roll = atan2(ay, az) and pitch = atan2(-ax, sqrt(ay^2 + az^2))

For example, with ay = 0.17 g and az = 0.98 g: roll = atan2(0.17, 0.98) = 9.8 deg. This angle never drifts, but it has two weaknesses. First, it is noisy: motor vibration adds sharp spikes. Second, it is fooled by real acceleration. If the drone accelerates forward at 0.3 g while level, the accelerometer sees atan(0.3) = 16.7 deg of false tilt.

The gyro is trustworthy for short times, the accelerometer for long times. So combine them.

The complementary filter

Idea: trust the gyro in the short term and let the accelerometer slowly pull the estimate back. Start from a low-pass filter, which is a first-order lag with time constant tau that smooths a signal a:

tau * dy/dt + y = a

Discretise with step dt, replacing dy/dt by (y_k - y_(k-1)) / dt:

tau * (y_k - y_(k-1)) / dt + y_k = a_k

Solve for y_k:

y_k = tau / (tau + dt) * y_(k-1) + dt / (tau + dt) * a_k

Define alpha = tau / (tau + dt). Then dt / (tau + dt) = 1 - alpha, and:

y_k = alpha * y_(k-1) + (1 - alpha) * a_k

That filters the accelerometer alone. The final step: instead of using the old estimate y_(k-1) as the prediction, use the old estimate advanced by the gyro, y_(k-1) + gyro_k * dt. The result is the complementary filter:

angle_k = alpha * (angle_(k-1) + gyro_k * dt) + (1 - alpha) * accel_angle_k

With tau = 0.5 s and dt = 0.004 s:

alpha = 0.5 / 0.504 = 0.992

So each step takes 99.2 percent from the gyro-propagated angle and 0.8 percent from the accelerometer. Fast motion comes from the gyro, while accelerometer noise is averaged over roughly tau seconds. The crossover frequency is 1 / (2 * pi * tau) = 0.32 Hz: slower than that, the accelerometer wins.

What happens to a gyro bias b? In steady state angle_k = angle_(k-1) = theta, so theta = alpha * (theta + b * dt) + (1 - alpha) * a. Solving gives theta = a + b * tau. The constant error is only:

0.5 deg/s * 0.5 s = 0.25 deg

instead of 30 degrees and growing. This is the whole point of the filter.

Code

On an Arduino with an MPU6050, calibrating the gyro bias at start-up removes most of the drift before the filter even runs. Keep the drone still during setup().

#include <Wire.h>

const uint8_t MPU = 0x68;
const float ACC_SCALE  = 16384.0;  // LSB per g at +-2 g
const float GYRO_SCALE = 131.0;    // LSB per deg/s at +-250 deg/s
const float TAU = 0.5;             // filter time constant, s
const float DT  = 0.004;           // 250 Hz loop, s

float roll = 0, pitch = 0;
float gxBias = 0, gyBias = 0;
unsigned long nextTick;
int counter = 0;

int16_t read16() {
  int16_t hi = Wire.read();
  int16_t lo = Wire.read();
  return (hi << 8) | lo;
}

void readIMU(float &ax, float &ay, float &az, float &gx, float &gy) {
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);                        // first accel register
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, (uint8_t)14, (uint8_t)true);
  ax = read16() / ACC_SCALE;
  ay = read16() / ACC_SCALE;
  az = read16() / ACC_SCALE;
  read16();                                // skip temperature
  gx = read16() / GYRO_SCALE;
  gy = read16() / GYRO_SCALE;
  read16();                                // skip gyro z
}

void setup() {
  Serial.begin(115200);
  Wire.begin();
  Wire.setClock(400000);
  Wire.beginTransmission(MPU);
  Wire.write(0x6B); Wire.write(0);         // wake the chip up
  Wire.endTransmission(true);

  float ax, ay, az, gx, gy;
  for (int i = 0; i < 500; i++) {          // average 500 samples while still
    readIMU(ax, ay, az, gx, gy);
    gxBias += gx / 500.0;
    gyBias += gy / 500.0;
    delay(2);
  }
  nextTick = micros();
}

void loop() {
  float ax, ay, az, gx, gy;
  readIMU(ax, ay, az, gx, gy);
  gx -= gxBias;
  gy -= gyBias;

  float accRoll  = atan2(ay, az) * RAD_TO_DEG;
  float accPitch = atan2(-ax, sqrt(ay * ay + az * az)) * RAD_TO_DEG;

  const float alpha = TAU / (TAU + DT);
  roll  = alpha * (roll  + gx * DT) + (1.0 - alpha) * accRoll;
  pitch = alpha * (pitch + gy * DT) + (1.0 - alpha) * accPitch;

  if (++counter >= 50) {                   // print at 5 Hz
    counter = 0;
    Serial.print(roll); Serial.print("  "); Serial.println(pitch);
  }
  nextTick += 4000;                        // hold a fixed 250 Hz loop
  while (micros() < nextTick) {}
}

Axis signs depend on how the chip is mounted. Tilt the board and check that gyro rate and accelerometer angle move in the same direction, otherwise flip one sign.

The same filter in Python, tested on synthetic data so you can see the drift disappear:

import random
random.seed(1)

dt, tau = 0.004, 0.5
alpha = tau / (tau + dt)

true_angle = 10.0                 # the drone sits tilted by 10 deg
bias = 0.5                        # deg/s gyro bias
gyro_only = fused = true_angle

for _ in range(int(60 / dt)):     # 60 seconds
    gyro = bias + random.gauss(0, 0.3)               # true rate is 0
    accel_angle = true_angle + random.gauss(0, 3.0)  # noisy accelerometer
    gyro_only += gyro * dt
    fused = alpha * (fused + gyro * dt) + (1 - alpha) * accel_angle

print(f"gyro only error: {gyro_only - true_angle:5.2f} deg")   # about 30
print(f"fused error:     {fused - true_angle:5.2f} deg")       # about 0.25 plus jitter

Beyond the complementary filter

Real flight controllers use more elaborate estimators with the same goal. The Madgwick and Mahony filters represent orientation as a quaternion (four numbers that avoid the gimbal lock of roll-pitch-yaw angles) and correct it with a single gain, much like our 1 - alpha. They are cheap enough for small microcontrollers. A Kalman filter (in drones, usually an extended Kalman filter or EKF) goes further: it keeps an explicit estimate of the gyro bias and weighs each sensor by its known noise variance. PX4 and ArduPilot use EKFs to fuse the IMU with GPS, barometer and compass. None of them can measure yaw from gravity alone, because gravity does not change when you turn around the vertical axis. Yaw needs a magnetometer or GPS.