MPU6050 GyroScope Readings Are Drifting

Using Arduino Sensors
1

Hey all, I am currenly using the gyroscope as a tool to make sure my robot is making sharp 90 degree turns and being able to follow a line without going off course. However, the gyroscope angle readings drift by itself, and it is completely offsetting my readings. Is this any way I can fix this issue?
Thanks

Here is my code:

#include <MPU6050_tockn.h>
#include <Wire.h>
MPU6050 mpu6050(Wire);

const int MPU = 0x68;

void setup() {
  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(false);
  
  mpu6050.begin();
  mpu6050.calcGyroOffsets();
  mpu6050.update();

//For some reason, the readings are spiked when I start so I have to skip over them
  for (int i = 0; i < 200; i++) {
    gyroReading();
    delay(1);
  }
}

double gyroReading() {
  mpu6050.update();
  angleX = mpu6050.getAngleX() - start_angleX;
  angleY = mpu6050.getAngleY() - start_angleY;
  angleZ = mpu6050.getAngleZ() - start_angleZ;

  Serial.print(angleX);
  Serial.print("    ");
  Serial.print(angleY);
  Serial.print("    ");
  Serial.println(angleZ);
  return angleZ;
}
2

I have moved your topic to an appropriate forum category @winstonn.

In the future, please take some time to pick the forum category that best suits the subject of your topic. There is an "About the _____ category" topic at the top of each category that explains its purpose.

This is an essential part of responsible forum usage, as explained in the "How to get the best out of this forum" guide. It contains a lot of other useful information. Please read it.

Thanks in advance for your cooperation.

3

I don't use that library, and suspect that the gyro offset estimation is not optimal.

In any case, even if accurate offsets are calculated and applied, the temperature dependence of the offsets, measurement errors and inaccurate integration inevitably lead to incorrect 3D orientation and drift.

With the MPU-6050, "gyro only" Mahony filter integration works reasonably well for a few minutes. Example here: GitHub - jremington/MPU-6050-Fusion: Mahony 3D filter for accel/gyro and gyro-only integration

4

Thank you so much for the quick response! The code works much better than the previous library I was using. However, I have two questions. Firstly, every 90-degree turn is read in multiples of 45, so if I start from angle 0 and turn 90 degrees right, it outputs 45. This assumes that a full rotation would then result in 180 degrees, not 360. But when I turn the sensor left from 0, it then jumps to 360, not 180. Is this intended, because for me it's very counter-intuitive? Also, there are still increments but in an orderly pattern, so is it possible if I can manually change the rate-of-change offset? Since I'm a beginner at this, I am struggling to find the line of code for this. Thank you so much for your consideration!

5

Please post the code you are using, with code tags. The Github code was tested and working properly some years ago, and I no longer have an MPU-6050 to test, but perhaps I can spot a mistake.

The MPU-6050 has long been discontinued. You probably have a clone or counterfeit, which may not meet the original specifications.

A more modern 6DOF sensor, like the ISM330, will perform much better.

6

You have to apply kalman filtering. Otherwise, these gyros give unrealistic data output.

7

No filter can fix a systematic error, like gyro drift.

8

This is the code:

#include "Wire.h"

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

int cal_gyro = 1;  //set to zero to use gyro calibration offsets below.

// 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 AHRS will NOT work well or at all if these are not correct

float A_cal[6] = {0.0, 0.0, 0.0, 1.000, 1.000, 1.000}; // 0..2 offset xyz, 3..5 scale xyz
float G_off[3] = { 0., 0., 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;

// Notes: 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, Kp=30 works well

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

// 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 unsigned int i = 0; //loop counter
  static float deltat = 0;  //loop time in seconds
  static unsigned long now = 0, last = 0; //micros() timers
  static long gsum[3] = {0};
  //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); // 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)

  // calibrate gyro upon startup. SENSOR MUST BE HELD STILL (a few seconds)
  i++;
  if (cal_gyro) {

    gsum[0] += gx; gsum[1] += gy; gsum[2] += gz;
    if (i == 500) {
      cal_gyro = 0;  //turn off calibration and print results

      for (char k = 0; k < 3; k++) G_off[k] = ((float) gsum[k]) / 500.0;

      Serial.print("G_Off: ");
      Serial.print(G_off[0]);
      Serial.print(", ");
      Serial.print(G_off[1]);
      Serial.print(", ");
      Serial.print(G_off[2]);
      Serial.println();
    }
  }

  // normal AHRS calculations

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

    //apply offsets and scale factors from Magneto
    for (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. Strictly valid only for approximately level movement
    
    // 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.
       
      // WARNING: This angular conversion is for DEMONSTRATION PURPOSES ONLY. It WILL
      // MALFUNCTION for certain combinations of angles! See https://en.wikipedia.org/wiki/Gimbal_lock

    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
    //ccrrect for local magnetic declination here
    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
// 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;
   tmp = 0.0; //IGNORE ACCELEROMETER

  // ignore accelerometer if false (tested OK, SJR)
  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, given by gyro term
  // rate of change = current orientation quaternion (qmult) gyro rate

  deltat = 0.5 * deltat;
  gx *= deltat;   // pre-multiply common factors
  gy *= deltat;
  gz *= deltat;
  qa = q[0];
  qb = q[1];
  qc = q[2];

  //add qmult*delta_t to current orientation
  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);

  // Normalise 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;
}
9

I see no problems with the code, if that is what you are actually using. Please post examples of the output.

But when I turn the sensor left from 0, it then jumps to 360, not 180. Is this intended, because for me it's very counter-intuitive?

That is the expected behavior for the yaw angle, which by conventional navigation is rotation about the vertical (Z) axis. Yaw is 0 with X pointing due North and increases to 90 when X points East, 180 S, 270 W, etc.

However, the MPU-6050 has no means of detecting "North" so yaw = 0 is assumed at startup, when the sensor is level with Z pointing up.

Test the sensor by setting it level on a table, with Z up, and rotate it slowly about Z, so that the X axis reorients by 90 degrees.

It should track that motion reasonably accurately. However, if you rotate the sensor too quickly, the angle integration becomes inaccurate.

10

Ok, so I'm now trying to return the yaw value so I can actually use it t align my robot to make consistent turns, but it is returning "ovf". Could you take a look to see what's wrong with my code?

For the reading angle part, this is the only portion I changed, x is a parameter the function takes in to know what value to return.

    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 ");
      if (x == 2){ 
      Serial.print(yaw, 0);
      Serial.print(", ");
      Serial.print(pitch, 0);
      Serial.print(", ");
      Serial.println(roll, 0);
      }
    if (x == 0)return yaw;
    else return roll;
    }
  }
}

Here is where I call the function:

void adjustment(String lastTurn) {
  //back(100);
  //delay(200);
  fullyStop();
  delay(200);

  double curAngle = gyroReading(0);
  double counter = 0.0;
  double refAngle = 1.0;
  int initialAngle = curAngle;

  boolean found = false;
  for (int i = 0; i < 1000; i++){
    gyroReading(1);
  }
  while (not found) {
    refAngle = 90.0 * counter;
    if (abs(curAngle - refAngle) < 45.0) {
      found = true;
      break;
    }
    else {
      Serial.println(abs(curAngle - refAngle));
      curAngle = gyroReading(0);
      counter+=1.0;
    }
  }
11

Did you perform the test mentioned, and does yaw track as expected?

For questions about code, post ALL the code. I can't make much sense of the snippets.

1 Like
12

Nevermind it's all good, thanks for your help! The error was just some delay in the gyroReading() function, I just deleted it.

13

Im glad i found this, im trying to attempt to use the MPU-6050 to control mouse cursor movements on the horizontal axis, within bounds, for use on flight sim for the xbox. just trying to hack together some frankenstein combination of my glasses, the leonardo, or probably my pro micro, and the MPU-6050 sensor, wont ever look fast at anything , probably just for slow looking around as you would if flying a real 2 seater or something alike that, so far, i have managed to map quaternions to the mouse, but my god, i am having a buggar of a time with either the mapping, or something in the filter. one part i read here mentioned that the z axis needs to face up

BlockquoteHowever, the MPU-6050 has no means of detecting "North" so yaw = 0 is assumed at startup, when the sensor is level with Z pointing up.

Blockquote

Test the sensor by setting it level on a table, with Z up, and rotate it slowly about Z, so that the X axis reorients by 90 degrees.

Blockquote

Now, is that Z- up, or Z+ up...

14

I apologize for being an idiot with the formatting of that.. sorry guys.

15

i wanted to build hand gesture controlled robotic arm of 4 servo motors and mpu6050 gyro.Im facing an issue with exact angle measurements especially with yaw(z-axis).The connections are fine please do look at the following code.

#include<Wire.h> //I2C Wire Library

#include<Servo.h> //Servo Motor Library

Servo servo_1;

Servo servo_2;

Servo servo_3;

Servo servo_4;

const int MPU_addr=0x68; //MPU6050 I2C Address

int16_t axis_X,axis_Y,axis_Z;

int minVal=265;

int maxVal=402;

double x;

double y;

double z;

void setup()

{

Serial.begin(9600);

Wire.begin(); //Initilize I2C Communication

Wire.beginTransmission(MPU_addr); //Start communication with MPU6050

Wire.write(0x6B); //Writes to Register 6B

Wire.write(0); //Writes 0 into 6B Register to Reset

Wire.endTransmission(true); //Ends I2C transmission

servo_1.attach(2); // Forward/Reverse_Motor

servo_2.attach(3); // Up/Down_Motor

servo_3.attach(4); // Forward/Reverse_Motor

servo_4.attach(5); // Left/Right_Motor

}

void loop()

{

Wire.beginTransmission(MPU_addr);

Wire.write(0x3B); //Start with regsiter 0x3B

Wire.endTransmission(false);

Wire.requestFrom(MPU_addr,14,true); //Read 14 Registers

axis_X=Wire.read()<<8|Wire.read(); //Reads the MPU6050 X,Y,Z AXIS Value

axis_Y=Wire.read()<<8|Wire.read();

axis_Z=Wire.read()<<8|Wire.read();

int xAng = map(axis_X,minVal,maxVal,-90,90); // Maps axis values in terms of -90 to +90

int yAng = map(axis_Y,minVal,maxVal,-90,90);

int zAng = map(axis_Z,minVal,maxVal,-90,90);

x= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI); //Formula to convert into degree

y= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);

z= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);

//forward backward using 2 mid servo(m1 m3)-ROLL

if(x >=0 && x <= 60)

{

 int mov1 = map(x,0,60,0,90);

 Serial.print("Movement in up/down = ");

 Serial.print(mov1);

 Serial.println((char)176);


   servo_1.write(mov1),servo_3.write(mov1);

}

else if(x >=300 && x <= 360)

{

 int mov1 = map(x,360,250,0,180);

 Serial.print("Movement in Up/Down = ");

 Serial.print(mov1);

 Serial.println((char)176);
 
 servo_1.write(mov1),servo_3.write(mov1);

}

//up and down using m2-PITCH

if(y >=0 && y <= 60)

{

 int mov2 = map(y,0,60,0,90);

 Serial.print("Movement in F/R = ");

 Serial.print(mov2);

 Serial.println((char)176);

 servo_4.write(mov2);

}

else if(y >=300 && y <= 360)

{

 int mov2 = map(y,360,250,0,180);

 Serial.print("Movement in F/R = ");

 Serial.print(mov2);

 Serial.println((char)176);
 
 servo_4.write(mov2);

}

//left right using bottom servo(m4)-YAW

if(z >=0 && z <= 60)

{

 int mov3 = map(z,0,60,90,180);

 Serial.print("Movement in Left = ");

 Serial.print(mov3);

 Serial.println((char)176);

 servo_2.write(mov3);

}

else if(z >=300 && z <= 360)

{

 int mov3 = map(z,360,300,90,0);

 Serial.print("Movement in Right = ");

 Serial.print(mov3);

 Serial.println((char)176);

 servo_2.write(mov3);

}

}

request for possible solutions

16

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.