MPU 6050 - Angle calculation from gyro sensor very inaccurate

Hello,

I am trying to use a GY-521 MPU 6050 to measure an angle from an inverted pendulum, that I want to build.
I know that there is a certain drift in the angle calculation due to the integration of the angular velocity. However I noticed, that apart from the small constant shift over time there is a huge offset when I shake/move the sensor around and then go back to my original position. In that case the calculated angle suddenly gets an offset of 20-30 or sometimes up to 90 degrees. And this obviously can¬īt be corrected with the additional measurements from the accelerometer.

Here is the code that I basically use. It¬īs from this website

/*
  Arduino and MPU6050 Accelerometer and Gyroscope Sensor Tutorial
  by Dejan, https://howtomechatronics.com
*/
#include <Wire.h>
const int MPU = 0x68; // MPU6050 I2C address
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float roll, pitch, yaw;
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
float elapsedTime, currentTime, previousTime;
int c = 0;
void setup() {
 Serial.begin(19200);
 Wire.begin();                      // Initialize comunication
 Wire.beginTransmission(MPU);       // Start communication with MPU6050 // MPU=0x68
 Wire.write(0x6B);                  // Talk to the register 6B
 Wire.write(0x00);                  // Make reset - place a 0 into the 6B register
 Wire.endTransmission(true);        //end the transmission
 /*
 // Configure Accelerometer Sensitivity - Full Scale Range (default +/- 2g)
 Wire.beginTransmission(MPU);
 Wire.write(0x1C);                  //Talk to the ACCEL_CONFIG register (1C hex)
 Wire.write(0x10);                  //Set the register bits as 00010000 (+/- 8g full scale range)
 Wire.endTransmission(true);
 // Configure Gyro Sensitivity - Full Scale Range (default +/- 250deg/s)
 Wire.beginTransmission(MPU);
 Wire.write(0x1B);                   // Talk to the GYRO_CONFIG register (1B hex)
 Wire.write(0x10);                   // Set the register bits as 00010000 (1000deg/s full scale)
 Wire.endTransmission(true);
 delay(20);
 */
 // Call this function if you need to get the IMU error values for your module
 calculate_IMU_error();
 delay(20);
}
void loop() {
 // === Read acceleromter data === //
 Wire.beginTransmission(MPU);
 Wire.write(0x3B); // Start with register 0x3B (ACCEL_XOUT_H)
 Wire.endTransmission(false);
 Wire.requestFrom(MPU, 6, true); // Read 6 registers total, each axis value is stored in 2 registers
 //For a range of +-2g, we need to divide the raw values by 16384, according to the datasheet
 AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // X-axis value
 AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Y-axis value
 AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Z-axis value
 // Calculating Roll and Pitch from the accelerometer data
 accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - AccErrorX ; // AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more details
 accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) - AccErrorY ; // AccErrorY ~(-1.58)
 // === Read gyroscope data === //
 previousTime = currentTime;        // Previous time is stored before the actual time read
 currentTime = millis();            // Current time actual time read
 elapsedTime = (currentTime - previousTime) / 1000; // Divide by 1000 to get seconds
 Wire.beginTransmission(MPU);
 Wire.write(0x43); // Gyro data first register address 0x43
 Wire.endTransmission(false);
 Wire.requestFrom(MPU, 6, true); // Read 4 registers total, each axis value is stored in 2 registers
 GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet
 GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
 GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
 // Correct the outputs with the calculated error values
 GyroX = GyroX - GyroErrorX ; // GyroErrorX ~(-0.56)
 GyroY = GyroY - GyroErrorY ; // GyroErrorY ~(2)
 GyroZ = GyroZ - GyroErrorZ ; // GyroErrorZ ~ (-0.8)
 // Currently the raw values are in degrees per seconds, deg/s, so we need to multiply by sendonds (s) to get the angle in degrees
 gyroAngleX = gyroAngleX + GyroX * elapsedTime; // deg/s * s = deg
 gyroAngleY = gyroAngleY + GyroY * elapsedTime;
 yaw =  yaw + GyroZ * elapsedTime;
 // Complementary filter - combine acceleromter and gyro angle values
 roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
 pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;
 
 // Print the values on the serial monitor
 Serial.print(roll);
 Serial.print("/");
 Serial.print(pitch);
 Serial.print("/");
 Serial.println(yaw);
}
void calculate_IMU_error() {
 // We can call this funtion in the setup section to calculate the accelerometer and gyro data error. From here we will get the error values used in the above equations printed on the Serial Monitor.
 // Note that we should place the IMU flat in order to get the proper values, so that we then can the correct values
 // Read accelerometer values 200 times
 while (c < 200) {
   Wire.beginTransmission(MPU);
   Wire.write(0x3B);
   Wire.endTransmission(false);
   Wire.requestFrom(MPU, 6, true);
   AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
   AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
   AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
   // Sum all readings
   AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI));
   AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 / PI));
   c++;
 }
 //Divide the sum by 200 to get the error value
 AccErrorX = AccErrorX / 200;
 AccErrorY = AccErrorY / 200;
 c = 0;
 // Read gyro values 200 times
 while (c < 200) {
   Wire.beginTransmission(MPU);
   Wire.write(0x43);
   Wire.endTransmission(false);
   Wire.requestFrom(MPU, 6, true);
   GyroX = Wire.read() << 8 | Wire.read();
   GyroY = Wire.read() << 8 | Wire.read();
   GyroZ = Wire.read() << 8 | Wire.read();
   // Sum all readings
   GyroErrorX = GyroErrorX + (GyroX / 131.0);
   GyroErrorY = GyroErrorY + (GyroY / 131.0);
   GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);
   c++;
 }
 //Divide the sum by 200 to get the error value
 GyroErrorX = GyroErrorX / 200;
 GyroErrorY = GyroErrorY / 200;
 GyroErrorZ = GyroErrorZ / 200;
 // Print the error values on the Serial Monitor
 Serial.print("AccErrorX: ");
 Serial.println(AccErrorX);
 Serial.print("AccErrorY: ");
 Serial.println(AccErrorY);
 Serial.print("GyroErrorX: ");
 Serial.println(GyroErrorX);
 Serial.print("GyroErrorY: ");
 Serial.println(GyroErrorY);
 Serial.print("GyroErrorZ: ");
 Serial.println(GyroErrorZ);
}

What's your problem actually?

Well as I have described in my post, the angle measurements are all over the place as soon as I move my sensor around. When I move the sensor around and then go back to the original position the angle measurement should be the same as in the beginning, right? But this is not the case. Instead the moving/shaking of the sensor adds a lot of offset and I get totally wrong measurements...

When I move the sensor around and then go back to the original position the angle measurement should be the same as in the beginning, right?

Why?

ut this is not the case. Instead the moving/shaking of the sensor adds a lot of offset and I get totally wrong measurements...

That's exactly what I would expect if you don't use the acceleration sensor. Why do you want to get the angle from the gyrometer? You cannot integrate the values fast enough to get accurate results.

Well, this is what is proposed on many websites, to use sensor fusion of the acceleration sensor and the (integrated) gyro data, to get more accurate angle measurements.

Have you considered using your internet search thingy on the words "mpu6050 complementary filter"?

Even better look up the words "mpu6050 complementary low pass filter".

This is the code that's attempting to keep track of orientation from the rate gyro outputs:

  gyroAngleX = gyroAngleX + GyroX * elapsedTime; // deg/s * s = deg

gyroAngleY = gyroAngleY + GyroY * elapsedTime;
  yaw =  yaw + GyroZ * elapsedTime;

Alas this is mathematically completely wrong and doesn't work at all.

Rotation in 3 dimensions doesn't work like this, its not commutative.

You need to use quarternions or DCMs to integrate rotation in 3D. The three axes are not
independent for rotation.

Or put another way your code assumes a fixed reference frame, yet the rate gyro data is
always relative to the current, varying, orientation.

Interesting, trying to use the angle of platform precession from a rate gyro. They’d be better off measuring the accelerometer output and converting that to an angle. One might look up what is meant by rate gyro. Oh and one could, also, look up the formula on the internet of how to get an angle measurement from accelerometer.

My experience, for a hobbyist the MPU, the BNO, the LSM, and similar hobbyist modules, will give a fair angle output, from the accelerometers, to keep a X/Y platform fairly level. I found that when I wanted to do much more then a few parlor tricks, using an automotive IMU/MMU was quite an improvement. The down side is one will have to figure out how to communicate with the device on one’s own.

There is a replacement for the MPU9250/55; ICM-20948. The improvements are not that much over the MPU9250/55. See https://invensense.tdk.com/wp-content/uploads/2018/10/AN-000146-v2.0-TDK_Migration_MPU_9250toICM-20948.pdf page 3. For those wanting a faster data output in a hobbyist IMU thingy the SPI speed increase from 1 MHz to 7 MHz might be of interest.

You can try out the Mahony fusion filter. Here is my version for the MPU-6050. It is essential that you accurately determine and subtract the gyro offset, individually for each sensor.

Of course, yaw is always relative to the starting orientation.

//
// MPU-6050 Mahony IMU  (yaw angle is relative to starting orientation)
// last update 7/9/2020 
//

#include "Wire.h"

// AD0 low = 0x68 (default for Sparkfun module)
// AD0 high = 0x69
int MPU_addr = 0x68;

// vvvvvvvvvvvvvvvvvv  VERY VERY IMPORTANT vvvvvvvvvvvvvvvvvvvvvvvvvvvvv
//These are the previously determined offsets and scale factors for accelerometer and gyro for
// a particular example of an MPU-6050. They are not correct for other examples.
//The IMU code will NOT work well or at all if these are not correct

float A_cal[6] = {265.0, -80.0, -700.0, 0.994, 1.000, 1.014}; // 0..2 offset xyz, 3..5 scale xyz
float G_off[3] = { -499.5, -17.7, -82.0}; //raw offsets, determined for gyro at rest
#define gscale ((250./32768.0)*(PI/180.0))  //gyro default 250 LSB per d/s -> rad/s

// ^^^^^^^^^^^^^^^^^^^ VERY VERY IMPORTANT ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

// GLOBALLY DECLARED, required for Mahony filter
// vector to hold quaternion
float q[4] = {1.0, 0.0, 0.0, 0.0};

// Free parameters in the Mahony filter and fusion scheme,
// Kp for proportional feedback, Ki for integral
float Kp = 30.0;
float Ki = 0.0;

// with MPU-9250, angles start oscillating at Kp=40. Ki does not seem to help and is not required.
// with MPU-6050, some instability observed at Kp=100 Now set to 30.

// char s[60]; //snprintf buffer, if needed

// globals for AHRS loop timing
unsigned long now_ms, last_ms = 0; //millis() timers

// print interval
unsigned long print_ms = 200; //print angles every "print_ms" milliseconds
float yaw, pitch, roll; //Euler angle output

void setup() {

  Wire.begin();
  Serial.begin(9600);
  Serial.println("starting");

  // initialize sensor
  // defaults for gyro and accel sensitivity are 250 dps and +/- 2 g
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);

}

// AHRS loop

void loop()
{
  static int i = 0;
  static float deltat = 0;  //loop time in seconds
  static unsigned long now = 0, last = 0; //micros() timers

  //raw data
  int16_t ax, ay, az;
  int16_t gx, gy, gz;
  int16_t Tmp; //temperature

  //scaled data as vector
  float Axyz[3];
  float Gxyz[3];


  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers
  int t = Wire.read() << 8;
  ax = t | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  t = Wire.read() << 8;
  ay = t | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  t = Wire.read() << 8;
  az = t | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  t = Wire.read() << 8;
  Tmp = t | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  t = Wire.read() << 8;
  gx = t | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  t = Wire.read() << 8;
  gy = t | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  t = Wire.read() << 8;
  gz = t | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

  Axyz[0] = (float) ax;
  Axyz[1] = (float) ay;
  Axyz[2] = (float) az;

  //apply offsets and scale factors from Magneto
  for (int i = 0; i < 3; i++) Axyz[i] = (Axyz[i] - A_cal[i]) * A_cal[i + 3];

  Gxyz[0] = ((float) gx - G_off[0]) * gscale; //250 LSB(d/s) default to radians/s
  Gxyz[1] = ((float) gy - G_off[1]) * gscale;
  Gxyz[2] = ((float) gz - G_off[2]) * gscale;

  //  snprintf(s,sizeof(s),"mpu raw %d,%d,%d,%d,%d,%d",ax,ay,az,gx,gy,gz);
  //  Serial.println(s);

  now = micros();
  deltat = (now - last) * 1.0e-6; //seconds since last update
  last = now;

  Mahony_update(Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], deltat);

  // Compute Tait-Bryan angles.
  // In this coordinate system, the positive z-axis is down toward Earth.
  // Yaw is the angle between Sensor x-axis and Earth magnetic North
  // (or true North if corrected for local declination, looking down on the sensor
  // positive yaw is counterclockwise, which is not conventional for NED navigation.
  // Pitch is angle between sensor x-axis and Earth ground plane, toward the
  // Earth is positive, up toward the sky is negative. Roll is angle between
  // sensor y-axis and Earth ground plane, y-axis up is positive roll. These
  // arise from the definition of the homogeneous rotation matrix constructed
  // from quaternions. Tait-Bryan angles as well as Euler angles are
  // non-commutative; that is, the get the correct orientation the rotations
  // must be applied in the correct order which for this configuration is yaw,
  // pitch, and then roll.
  // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
  // which has additional links.

  roll  = atan2((q[0] * q[1] + q[2] * q[3]), 0.5 - (q[1] * q[1] + q[2] * q[2]));
  pitch = asin(2.0 * (q[0] * q[2] - q[1] * q[3]));
  //conventional yaw increases clockwise from North. Not that the MPU-6050 knows where North is.
  yaw   = -atan2((q[1] * q[2] + q[0] * q[3]), 0.5 - (q[2] * q[2] + q[3] * q[3]));
  // to degrees
  yaw   *= 180.0 / PI;
  if (yaw < 0) yaw += 360.0; //compass circle
  pitch *= 180.0 / PI;
  roll *= 180.0 / PI;

  now_ms = millis(); //time to print?
  if (now_ms - last_ms >= print_ms) {
    last_ms = now_ms;
    // print angles for serial plotter...
    //  Serial.print("ypr ");
    Serial.print(yaw, 0);
    Serial.print(", ");
    Serial.print(pitch, 0);
    Serial.print(", ");
    Serial.println(roll, 0);
  }
}
//--------------------------------------------------------------------------------------------------
// Mahony scheme uses proportional and integral filtering on
// the error between estimated reference vector (gravity) and measured one.
// Madgwick's implementation of Mayhony's AHRS algorithm.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date      Author      Notes
// 29/09/2011 SOH Madgwick    Initial release
// 02/10/2011 SOH Madgwick  Optimised for reduced CPU load
// last update 07/09/2020 SJR minor edits
//--------------------------------------------------------------------------------------------------
// IMU algorithm update

void Mahony_update(float ax, float ay, float az, float gx, float gy, float gz, float deltat) {
  float recipNorm;
  float vx, vy, vz;
  float ex, ey, ez;  //error terms
  float qa, qb, qc;
  static float ix = 0.0, iy = 0.0, iz = 0.0;  //integral feedback terms
  float tmp;

  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
  tmp = ax * ax + ay * ay + az * az;
  if (tmp > 0.0)
  {

    // Normalise accelerometer (assumed to measure the direction of gravity in body frame)
    recipNorm = 1.0 / sqrt(tmp);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;

    // Estimated direction of gravity in the body frame (factor of two divided out)
    vx = q[1] * q[3] - q[0] * q[2];
    vy = q[0] * q[1] + q[2] * q[3];
    vz = q[0] * q[0] - 0.5f + q[3] * q[3];

    // Error is cross product between estimated and measured direction of gravity in body frame
    // (half the actual magnitude)
    ex = (ay * vz - az * vy);
    ey = (az * vx - ax * vz);
    ez = (ax * vy - ay * vx);

    // Compute and apply to gyro term the integral feedback, if enabled
    if (Ki > 0.0f) {
      ix += Ki * ex * deltat;  // integral error scaled by Ki
      iy += Ki * ey * deltat;
      iz += Ki * ez * deltat;
      gx += ix;  // apply integral feedback
      gy += iy;
      gz += iz;
    }

    // Apply proportional feedback to gyro term
    gx += Kp * ex;
    gy += Kp * ey;
    gz += Kp * ez;
  }

  // Integrate rate of change of quaternion, q cross gyro term
  deltat = 0.5 * deltat;
  gx *= deltat;   // pre-multiply common factors
  gy *= deltat;
  gz *= deltat;
  qa = q[0];
  qb = q[1];
  qc = q[2];
  q[0] += (-qb * gx - qc * gy - q[3] * gz);
  q[1] += (qa * gx + qc * gz - q[3] * gy);
  q[2] += (qa * gy - qb * gz + q[3] * gx);
  q[3] += (qa * gz + qb * gy - qc * gx);

  // renormalise quaternion
  recipNorm = 1.0 / sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
  q[0] = q[0] * recipNorm;
  q[1] = q[1] * recipNorm;
  q[2] = q[2] * recipNorm;
  q[3] = q[3] * recipNorm;
}
1 Like

Hey thanks for sharing your code!
I think I¬īll read up on quaternions the next couple of days and then have a closer look at your code:)