Kalman filter tilt angle with an MPU6050 on an Arduino Uno

// Kalman filter tilt angle: an MPU6050's accelerometer and gyroscope fused
// with TKJ Electronics' Kalman filter library into a roll and a pitch angle
// that are both smooth and drift-free, plotted beside the raw estimates on
// the Serial Plotter, with a bubble-level pair of LEDs.
//
//   MPU6050  SDA -> A4   SCL -> A5   VCC -> 5V   GND -> GND
//   LEDs     green on pin 6, red on pin 7, each through 220 ohms to GND
//
// TWO SENSORS, TWO FLAWS. Held still, the accelerometer measures only
// gravity, so atan2 of its axes gives the tilt; but every bump and vibration
// is acceleration too, so the angle is noisy. The gyroscope measures how
// fast the board turns; adding that up gives a smooth angle, but every
// small error adds up with it and the angle drifts away.
//
// THE KALMAN FILTER keeps two numbers, the angle and the gyro's bias (its
// reading when it is not turning), and how sure it is of each. Every sample
// it predicts the new angle from the gyro rate less the bias, then corrects
// the prediction toward the accelerometer's angle, by more when it is less
// sure of the prediction. Q_angle, Q_bias and R_measure (0.001, 0.003 and
// 0.03 by default) say how much to trust each source.
//
// THE COMPLEMENTARY FILTER beside it is the one-line alternative most
// balancing robots use: 93% gyro and 7% accelerometer every sample.
//
// Open the Serial Plotter at 115200 baud to see all four lines for each
// axis. Things to try: drag the MPU6050 to tilt it and watch the gyro-only
// line wander from the others while the Kalman line follows the tilt. Hold
// it level within 2 degrees and the green LED lights; tilt it and the red one does.

#include <Wire.h>
#include <Kalman.h>

const uint8_t MPU_ADDR = 0x68;
const byte GREEN_LED = 6;
const byte RED_LED = 7;
const float LEVEL_WITHIN = 2.0;            // degrees either way counts as level
const unsigned long SAMPLE_US = 10000;     // 100 samples a second

Kalman kalmanRoll;
Kalman kalmanPitch;

float gyroRoll, gyroPitch;   // the gyro alone, added up
float compRoll, compPitch;   // the complementary filter
float kalRoll, kalPitch;     // the Kalman filter
unsigned long lastSample;
byte printEvery = 0;

int16_t accX, accY, accZ, gyroX, gyroY;

void writeRegister(uint8_t reg, uint8_t value) {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(reg);
  Wire.write(value);
  Wire.endTransmission();
}

bool readSensor() {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);  // ACCEL_XOUT_H: accelerometer, temperature, gyroscope
  if (Wire.endTransmission(false) != 0) return false;
  if (Wire.requestFrom(MPU_ADDR, (uint8_t)14) != 14) return false;
  uint8_t data[14];
  for (uint8_t i = 0; i < 14; i++) data[i] = Wire.read();
  // Each value is two bytes, high byte first. Bytes 6 and 7 are the
  // temperature and 12 and 13 the gyro's Z axis, not used here.
  accX = data[0] << 8 | data[1];
  accY = data[2] << 8 | data[3];
  accZ = data[4] << 8 | data[5];
  gyroX = data[8] << 8 | data[9];
  gyroY = data[10] << 8 | data[11];
  return true;
}

// The accelerometer's angles in degrees. Pitch is kept to -90..90 so the
// two angles never fight each other near upside down.
float accelRoll() { return atan2(accY, accZ) * RAD_TO_DEG; }
float accelPitch() { return atan(-accX / sqrt((float)accY * accY + (float)accZ * accZ)) * RAD_TO_DEG; }

void setup() {
  Serial.begin(115200);
  pinMode(GREEN_LED, OUTPUT);
  pinMode(RED_LED, OUTPUT);
  Wire.begin();
  Wire.setClock(400000L);

  writeRegister(0x6B, 0x01);  // PWR_MGMT_1: wake up, clock from the X gyro's PLL
  writeRegister(0x19, 7);     // SMPLRT_DIV: 8 kHz / (7 + 1) = 1 kHz
  writeRegister(0x1A, 0x00);  // CONFIG: no low-pass filter
  writeRegister(0x1B, 0x00);  // GYRO_CONFIG: +-250 deg/s, 131 counts per deg/s
  writeRegister(0x1C, 0x00);  // ACCEL_CONFIG: +-2 g
  delay(100);

  while (!readSensor()) {
    Serial.println(F("No MPU6050 at 0x68: check SDA, SCL and power"));
    delay(1000);
  }
  // Start every estimate at the accelerometer's angle.
  float roll = accelRoll(), pitch = accelPitch();
  kalmanRoll.setAngle(roll);
  kalmanPitch.setAngle(pitch);
  gyroRoll = compRoll = kalRoll = roll;
  gyroPitch = compPitch = kalPitch = pitch;
  Serial.println(F("Kalman tilt: roll and pitch, accel gyro comp kalman"));
  lastSample = micros();
}

void loop() {
  if (micros() - lastSample < SAMPLE_US) return;
  float dt = (micros() - lastSample) / 1000000.0;
  lastSample = micros();
  if (!readSensor()) return;

  float roll = accelRoll();
  float pitch = accelPitch();
  float rollRate = gyroX / 131.0;   // deg/s
  float pitchRate = gyroY / 131.0;

  // Roll runs -180..180; when it wraps round, start the filters again there
  // rather than let them swing the long way.
  if ((roll < -90 && kalRoll > 90) || (roll > 90 && kalRoll < -90)) {
    kalmanRoll.setAngle(roll);
    compRoll = kalRoll = gyroRoll = roll;
  } else {
    kalRoll = kalmanRoll.getAngle(roll, rollRate, dt);
  }
  kalPitch = kalmanPitch.getAngle(pitch, pitchRate, dt);

  gyroRoll += rollRate * dt;
  gyroPitch += pitchRate * dt;
  compRoll = 0.93 * (compRoll + rollRate * dt) + 0.07 * roll;
  compPitch = 0.93 * (compPitch + pitchRate * dt) + 0.07 * pitch;

  bool level = fabs(kalRoll) <= LEVEL_WITHIN && fabs(kalPitch) <= LEVEL_WITHIN;
  digitalWrite(GREEN_LED, level ? HIGH : LOW);
  digitalWrite(RED_LED, level ? LOW : HIGH);

  // Every fifth sample, 20 lines a second, for the Serial Plotter.
  if (++printEvery < 5) return;
  printEvery = 0;
  Serial.print(F("accRoll:"));
  Serial.print(roll);
  Serial.print(F(" gyroRoll:"));
  Serial.print(gyroRoll);
  Serial.print(F(" compRoll:"));
  Serial.print(compRoll);
  Serial.print(F(" kalRoll:"));
  Serial.print(kalRoll);
  Serial.print(F(" accPitch:"));
  Serial.print(pitch);
  Serial.print(F(" gyroPitch:"));
  Serial.print(gyroPitch);
  Serial.print(F(" compPitch:"));
  Serial.print(compPitch);
  Serial.print(F(" kalPitch:"));
  Serial.println(kalPitch);
}