MPU6050 Z-Axis Gyro Drift Problem

Hello all, I am having a problem with gyro drift on my MPU6050 on a Sparkfun SEN11028 board. I am using Jeff Rowberg's excellent I2C lib. I am using the MPU6050 to determine the orientation of an object in 3D and displaying it in Processing. However the orientation tends to drift around the Z-Axis (direction of gravity vector). When I rotate the sensor around the Z-axis the error is magnified. Rotating the object around some arbitrary angle, and then rotating it back will not return the angular position to 0.

I have scoured the forums but the most relevant problem was one where the user had been using int data types and had a problem with precision. I am using floats. I am also using quaternions as well and converting them to axis-angle in Processing.

I have some specific items I'm hoping I can get help with.

  1. Is the quaternion output from the MPU6050 derived from sensor fusion? I know that both the accelerometer and gyro onboard suffer from noise and drift respectively. However the senser fusion is supposed to take care of that problem. I think it does because there is little noise and the motion seems very smooth. However I am getting gyro drift and specifically only about the Z-axis.

  2. Is there initial calibration that should be implemented first with the mpu6050? I have found no documentation stating that as a requirement.

  3. I am not doing any sort of filtering. I had hoped by using the quaternion output of the mpu6050 that I would be able to avoid Kalman filtering etc.

  4. The orientation of my object does not match the orientation of the mpu6050, so what I've done in Processing is to reassign the axes. I do not think this would make a difference, but I am not sure. The gyro drift occurs on every axis of the MPU6050 when it is rotated to coincide with gravity, thus there isn't anything special about the z-axis specifically marked on the sensor.

That is all I can think of at the moment. Thanks in advance.

Usually a magnetic sensor is added to eliminate drift Problems around the gravity vector. But indoors they are less reliable. They need to be adjusted. You have to tell the sensor where it is.

Here i am attached my code for stabilizing 3-axis platform using MPU6050.....but i didn't get proper control signals which are to be given three servo motor to control the platform..
Please anyone check my code and tell me where i have to modify....please help me..

(CODE TAGS PLEASE!)

#include <kalman.h>
#include <Wire.h>
#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter
#include <LiquidCrystal.h>

// initialize the library with the numbers of the interface pins
LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
Kalman kalmanX;
Kalman kalmanY;
Kalman kalmanZ;
const uint8_t IMUAddress = 0x68;
int pwmpin1= 8;
double out1= 0;
int pwmpin2= 9;
double out2 =0;
int pwmpin3= 10;
double out3 =0;

/* IMU Data */
int16_t accX;
int16_t accY;
int16_t accZ;
int16_t tempRaw;
int16_t gyroX;
int16_t gyroY;
int16_t gyroZ;

double accXangle; // Angle calculate using the accelerometer
double accYangle;
double accZangle;
double temp;
double gyroXangle = 180; // Angle calculate using the gyro
double gyroYangle = 180;
double gyroZangle = 180;
double compAngleX = 180; // Calculate the angle using a Kalman filter
double compAngleY = 180;
double compAngleZ = 180;
double kalAngleX; // Calculate the angle using a Kalman filter
double kalAngleY;
double kalAngleZ;
double a;
double b;
double c;
uint32_t timer;

void setup() {  
  Serial.begin(115200);
  Wire.begin();  
  i2cWrite(0x6B,0x00); // Disable sleep mode  
  if(i2cRead(0x75,1)[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("MPU-6050 with address 0x"));
    Serial.print(IMUAddress,HEX);
    Serial.println(F(" is not connected"));
    while(1);
  }      
  kalmanX.setAngle(180); // Set starting angle
  kalmanY.setAngle(180);
  kalmanZ.setAngle(0);
  timer = micros();
   // declare pin 9 to be an output:
  pinMode(8, OUTPUT);
  analogWrite(8, 50);
  pinMode(9, OUTPUT);  
  analogWrite(9, 50);   
  pinMode(10, OUTPUT);  
  analogWrite(10, 50); 
  // set up the LCD's number of columns and rows: 
  lcd.begin(16, 4);
 // Print a message to the LCD.
  lcd.print("  GYRO STABLE ");
  delay(1000);
  lcd.setCursor(0, 0);
  //lcd.print("                ");
  
}

void loop() {
  /* Update all the values */
 // lcd.setCursor(0, 0);
  //lcd.print("GYRO STABLE ");
  //lcd.setCursor(0, 1);
//lcd.print("X-AXIS ");
  //lcd.setCursor(0, 1);
  //lcd.print(kalAngleX);
  //delay(1);
 //lcd.setCursor(7, 1);
 // lcd.print(kalAngleY);
 lcd.setCursor(0, 1);
    lcd.print("X-");
    lcd.setCursor(2, 1);
    lcd.print(kalAngleX);
    lcd.setCursor(-4, 2);
    lcd.print("Y-");
    lcd.setCursor(-2, 2);
    lcd.print(kalAngleY);
    lcd.setCursor(-4, 3);
    lcd.print("Z-");
    lcd.setCursor(-2, 3);
    lcd.print(kalAngleZ);
    lcd.setCursor(9, 1);
    lcd.print("ER");
    lcd.setCursor(12, 1);
    lcd.print(a);
    lcd.setCursor(5, 2);
    lcd.print("ER");
    lcd.setCursor(8, 2);
    lcd.print(b);
    lcd.setCursor(5, 3);
    lcd.print("ER");
    lcd.setCursor(7, 3);
    lcd.print(c);
  delay(100);
  
  uint8_t* data = i2cRead(0x3B,14);  
  accX = ((data[0] << 8) | data[1]);
  accY = ((data[2] << 8) | data[3]);
  accZ = ((data[4] << 8) | data[5]);  
  tempRaw = ((data[6] << 8) | data[7]);  
  gyroX = ((data[8] << 8) | data[9]);
  gyroY = ((data[10] << 8) | data[11]);
  gyroZ = ((data[12] << 8) | data[13]);
  
  /* Calculate the angls based on the different sensors and algorithm */
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
  accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
  accYangle = (atan2(accX,accY)+PI)*RAD_TO_DEG;  
  
  double gyroXrate = (double)gyroX/131.0;
  double gyroYrate = -((double)gyroY/131.0);
  double gyroZrate = -(double)gyroZ/131.0;
  gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); // Calculate gyro angle without any filter  
  gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);
  gyroZangle += gyroZrate*((double)(micros()-timer)/1000000);
  //gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate
  //gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);
  
  compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle); // Calculate the angle using a Complimentary filter
  compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle);  
  compAngleZ = (0.93*(compAngleZ+(gyroZrate*(double)(micros()-timer)/1000000)))+(0.07*accZangle);
  
  kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter
  kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
  kalAngleZ = kalmanZ.getAngle(accZangle, gyroZrate, (double)(micros()-timer)/1000000);
  timer = micros();
  a = (180 -  kalAngleX);
  b = (180 -  kalAngleY);
  c=  (0 - kalAngleZ);
  temp = ((double)tempRaw + 12412.0) / 340.0;


  /* Print Data */   
  /*
  Serial.print(accX);Serial.print("\t");
  Serial.print(accY);Serial.print("\t");  
  Serial.print(accZ);Serial.print("\t");    
  
  Serial.print(gyroX);Serial.print("\t");  
  Serial.print(gyroY); Serial.print("\t");   
  Serial.print(gyroZ);Serial.print("\t");  
  */
/*  Serial.print(accXangle);Serial.print("\t");
  Serial.print(accYangle);Serial.print("\t"); 
    
  Serial.print(gyroXangle);Serial.print("\t");
  Serial.print(gyroYangle);Serial.print("\t");
  
  Serial.print(compAngleX);Serial.print("\t");
  Serial.print(compAngleY); Serial.print("\t");
  
  Serial.print(kalAngleX);Serial.print("\t");
  Serial.print(kalAngleY);Serial.print("\t");
  

  //Serial.print(temp);Serial.print("\t");
   
  Serial.print("\n");*/
  
 out1 = (int) 255.0*((float)kalAngleX)/ 360.0;
 // out=kalAngleX-41;
 
 if(( out1 > 128) && ( out1 < 140))
 {
   analogWrite(pwmpin1,(out1 + 11)); 
 }
 else if(( out1 < 126) && ( out1 > 115))
 {
   analogWrite( pwmpin1,(out1 - 11) );
 }
 //else if(out < 99)
 //{
 //  Serial.print(out);
 //  analogWrite( pwmpin, 127);
 //}
 //else if(out > 155){
 //  Serial.print(out);
 //  analogWrite( pwmpin, 127);
// }
 else{
    analogWrite( pwmpin1,out1);
 }
 
 
  delay(1); // The accelerometer's maximum samples rate is 1kHz
out2 = (int) 255.0*((float)kalAngleY)/ 360.0;
 // out=kalAngleX-41;
 
 if(( out2 > 128) && ( out2 < 140))
 {
   analogWrite(pwmpin2,(out2 + 11)); 
 }
 else if(( out2 < 126) && ( out2 > 115))
 {
   analogWrite( pwmpin2,(out2 - 11) );
 }
 //else if(out < 99)
 //{
 //  Serial.print(out);
 //  analogWrite( pwmpin, 127);
 //}
 //else if(out > 155){
 //  Serial.print(out);
 //  analogWrite( pwmpin, 127);
// }
 else{
    analogWrite( pwmpin2,out2);
 }
 
 delay(1); // The accelerometer's maximum samples rate is 1kHz
out3 = (int) 255.0*((float)kalAngleZ)/ 360.0;
 // out=kalAngleX-41;
 
 if(( out3 > 128) && ( out3 < 140))
 {
   analogWrite(pwmpin3,(out3 + 11)); 
 }
 else if(( out3 < 126) && ( out3 > 115))
 {
   analogWrite( pwmpin3,(out3 - 11) );
 }
 //else if(out < 99)
 //{
 //  Serial.print(out);
 //  analogWrite( pwmpin, 127);
 //}
 //else if(out > 155){
 //  Serial.print(out);
 //  analogWrite( pwmpin, 127);
// }
 else{
    analogWrite( pwmpin3,out3);
 }
  
}
void i2cWrite(uint8_t registerAddress, uint8_t data){
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.write(data);
  Wire.endTransmission(); // Send stop
}
uint8_t* i2cRead(uint8_t registerAddress, uint8_t nbytes) {
  uint8_t data[nbytes];  
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.endTransmission(false); // Don't release the bus
  Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading
  for(uint8_t i = 0; i < nbytes; i++)
    data[i] = Wire.read();
  return data;
}