MPU-6050 drift problem

Hi all,
Currently we are trying to make a balancing robot (segway robot) with Mindstorms NXT and the MPU-6050. After quite some effort we managed to connect the MPU-6050 with the NXT, so now we can read the gyro x, y and z axis values properly.

To balance the robot needs both the angle and the angular speed. When we calculate the angular speed,
angularspeed = gyromeasurment - gyroError
the angular speed (when the gyro is stationary) oscillates slightly around 0, so that is good.

And when we calculate the angle with the angular speed (while the sensor is stationary),
Angle = Previous angle + angular speed (* deltaT)
the angle starts at zero and very slowly gets higher or lower, so thats normal too (right?)!

But here is the problem:
When we rotate the sensor randomly (left, right, left) and then try to rotate it back to zero degrees, we notice that the reference point (zero degrees position) has drifted way to the right. The more and faster we move the sensor, the faster the reference point drifts to the right. And when you keep the sensor motionless again , the reference point remains at the same spot (as it should be).

So to us it seems a motion-related drift problem: when you move the sensor, the gyro starts drifting. And not a little, no, when you rotate the sensor left and right for about two seconds, the reference point has rotated left for about 360 degrees (and always to the right!).

Although we are very unexperienced with gyroscopes, we are starting to think the MPU-6050 is faulty. But maybe there is something wrong with the code I wrote, maybe it's something else. We hope this is sufficient information to find the problem.

Thanks in advance
Beuktank

...

readResult = I2C_gyrosensor.getData(0x45, readData, len);
if (readResult>=0) {


     //measure gyroError
     for(int i=1; i<1000; i++){

     I2C_gyrosensor.getData(0x45, readData, len);
     gyroY = ((readData[0]<<8) | readData[1]);
     buffer += gyroY;
     }

     gyroError = buffer/1000;


     while(!Button.ESCAPE.isDown()){

          //read ryro
          I2C_gyrosensor.getData(0x45, readData, len);
          gyroValue = ((readData[0]<<8) | readData[1]);

          //calculate angular speed
          trueGyro = gyroValue-gyroError;
          //calculate angle
          angle += trueGyro;

          LCD.drawString("Angle = " + angle + " ", 0, 0);
     }
} 

else {
     // display error message...
     LCD.drawString("ERROR: " + readResult, 0, 1); 
     Thread.sleep(2000);
}

The gyro drifts.
The accelerometer is sensivite for vibrations.
The magnetometer is disturbed by magnetic fields of devices.
The barometer is the only one without problems, it measures the baromic pressure.

But if you combine them all, you can use it to stabelize a quadcopter.

The ideal gyro would not drift, but MEMS sensors are not ideal.

Erdin thanks for your reply.
I know drifting is normal for a gyroscope, but this drift is so strong that the reference point drifts about 90 degrees in only a few seconds. (again, not when stationary) I just don't know how to adjust for this kind of drift. To be honest I am happy when our robot only balances for 20 seconds only using the gyroscope....

Do you know any examples which use only a gyro ?
I agree that your gyro seems to drift more than normal, but I don't know what the maximum allowable drift is.

This a good guide to start, Arduino Forum

Mr Grotkasten here:
http://robin-grotkasten.jimdo.com/elektronik-projekte/lego-nxt-standalone-mpu6050-nxc/
uses only a gyroscope and his robot balances pretty good for a pretty long time. He has also the MPU-6050 but only with an other breakout-board. So it should be possible for me too to make a balancing robot with only a gyroscope.
I think that you cant even use a gyro with that much drift in a kalmanfilter with accelerometer. (?)

Thanks for the link, that is indeed with only gyro.

I don't understand the code, is the buffer capable to contain the total of 1000 gyro samples ?

Did you test all three axis (x,y,z) for drift ? Perhaps you can rotate the sensor and use another axis.

When I don't move the sensor the calculated angular speed stabilizes at around zero, so that means the gyroError is calculated correctly. Besides that I have the same problem when I take 100 samples.

I tested the x, y, and z axis and the same problem occurs. The code is written in Lejos (Java) by the way.

I tested my MPU-6050, and the drift is not bad at all. Using only the gyro should work for a simple balancing robot.

Perhaps the problem is in the code.
My question is still the same: is the buffer capable to contain the total of 100 gyro samples ?

The buffer is an int.
The int data type is a 32-bit signed two's complement integer.
Is that what you mean by capable ?

Yes.
32-bit should be okay.
I don't know what could be wrong.

Neither do I...
I am thinking to get an other MPU-6050, hopefully that one doesn't have the same problem.
thanks for the effort anyway

It's probably not possible to ever get a good result with only the gyroscope. It WILL drift!

Combining the accelerometer and the gyroscope data can be very simple through the use of a complementary filter as described in this article: Loading.... It's only a few lines of code ...

You're right, the gyro will drift. But I have made some changes witch provide way better results. So if there's anyone with the same problem here's what I did:
So I bought an other MPU-6050 but it showed exactly the same results as the other MPU did. I decided to change the program language to NXC instead of JAVA. And I might be wrong but I have the idea that NXC is a lot faster and better when it comes to programming robots.
Next thing I did was multiply everything in the code by 10, and at the end I divided the results by 10 again. I did this because now you can enter for example 265 for the GyroError instead of only 27. The precision you gain with this adjustment is really showing of in the results! Of course when you can use floating point you can simply enter decimal numbers instead. But for me this little more precision provided a lot of change in the gyrodrift!
Finally my segway is standing up straight!

Pieter-Jan5000 thanks for the link that's what I was looking for. I'm trying to combine the gyro and accelerometer now.

Here's the the code I used (basically the code from Robin Grotkasten):

#define PORT S2
#define Adresse (0x68<<1) //MPU6050 I2C-Address

//Initialization of the MPU6050 is based on "MultiWii"
//http://code.google.com/p/multiwii/

byte init1[] = {Adresse, 0x6B, 0x80}; // reset all registers to default values
byte init2[] = {Adresse, 0x6B, 0x03}; // set z axis as clock source
byte init3[] = {Adresse, 0x1A, 0x00}; // disables FSYNC, configures DLPF
byte init4[] = {Adresse, 0x1B, 0x18}; // set range to 2000 */2

// In register 0x43 the measurement of the X-rotation is written
byte read_gyro[] = {Adresse, 0x43};

byte nbytes;
byte data[];

int angle_speed0= 255;            // Drift (to be adjusted) 273
int angle_speed=0;                   // Gyro-value
int true_angle_speed=0;
int angle_absolute=0;               // Angle of the segway
int angle_absolute_a=0;
int true_angle_speed_a = 0;
int route = 0;
int last_route_error = 0;
 int I = 0;
int angle_P_factor=12;              //PD-to be ajusted
int angle_D_factor=100;
int last_angle_error=0;

int route_P_factor=1;                //PD-to be ajusted
int route_D_factor=250;


task main(){
    ResetRotationCount(OUT_B);
    SetSensorLowspeed(PORT);

    I2CWrite(PORT, 0, init1);
    while(I2CStatus(PORT, nbytes)>0);

    Wait(5);

    I2CWrite(PORT, 0, init2);
    while(I2CStatus(PORT, nbytes)>0);

    I2CWrite(PORT, 0, init3);
    while(I2CStatus(PORT, nbytes)>0);

    I2CWrite(PORT, 0, init4);
    while(I2CStatus(PORT, nbytes)>0);

    TextOut(1, LCD_LINE1, "DONE!");
    Wait(2000);
    ClearLine(LCD_LINE1);


    while (1) {
       I2CWrite(PORT, 6, read_gyro);
       while(I2CStatus(PORT, nbytes)>0);
       if (nbytes == 6){
          I2CRead(PORT, 6, data); // burst read of 6 registers:
          // data[0] = X_gyro_H (not used in this code)
          // data[1] = X_gyro_L (not used in this code)
          // data[2] = Y_gyro_H
          // data[3] = Y_gyro_L
          // data[4] = Z_gyro_H (not used in this code)
          // data[5] = Z_gyro_L (not used in this code)

          // convert the 8-bit values of X_gyro_H and X_gyro_L into one
          // X gyro value (integer value of 16 bit):
          angle_speed = (((data[2]<<8) | data[3])) *10;
          true_angle_speed = angle_speed + angle_speed0;
          true_angle_speed /= 500;

       }


       else{
          TextOut( 0, LCD_LINE1, "Error");
       }

       angle_absolute += true_angle_speed;
       ClearLine (LCD_LINE1);
       ClearLine (LCD_LINE2);
       NumOut(0, LCD_LINE1, angle_absolute_a);
       NumOut(0, LCD_LINE2, true_angle_speed_a);


       int error_angle = 0 - angle_absolute; //PD-Regelung: Winkel
       int angle_PTerm = error_angle * angle_P_factor;
       int angle_DTerm = true_angle_speed * angle_D_factor;

       route = (MotorRotationCount(OUT_B)); //PD-Regelung: Position
       int error_route = 0 - route;
       int route_PTerm = error_route * route_P_factor;
       int route_DTerm = (error_route - last_route_error) * route_D_factor;
       last_route_error = error_route;

       int PD = angle_PTerm - angle_DTerm - route_PTerm - route_DTerm;
       PD /= 10;


       ClearLine (LCD_LINE3);
       NumOut(0, LCD_LINE3, PD);

       if (PD >100) {
          PD = 100;
       } // limit the load on the motor to the maximum value
       if (PD < -100){
          PD = -100;
       }

       OnFwd(OUT_BC, PD);
    }
}

Just 2 question:
How can I implement the atan2 function in an arduino sketch?

Is mindstrom or NXC ATMega related? ( just curiousity )

@Pieter-jan5000

I'm very impressed by your complement filter.
Would it be possible to show us a graph from two sensors? ( one with your filter and another with a kalman filter, both doing the same 'move' )

Would be great if we couldt use the complement filter for angles > 90°. My quadcopter can do loops and flips. Hard to imagine, what would happen when I'd use your filter.

You need to have some stable angle measurment to keep the gyro from drifting. Even if you calibrate the gyro there will be always a tine mine error present in the measurment which will integrate by time to a huge error angle. So I dont think u can have a robust angle calculation untill u combine it with accelrometer. Here is a very good example http://hobbylogs.me.pn/?p=47 there is a library to measuring the gyro and accel.

Hi, I am using MPU6050 for self balancing unicycle. I am using single axis of sensor. I am getting my tilt angle from following code.

double accXangle=(atan2(AcY,AcZ)+PI)RAD_TO_DEG;
// Convert gyro raw vlues to degrees!
double gyroXrate=(double)GyX/131.0;
// Complementary Filter!
double compAngleX = (0.97 * (compAngleX + (gyroXrate * dt))) + (0.03 * accXangle);
//Serial.println("AcX = ");
float a=31.142
(compAngleX-5.35);
float ang=a-90;
return ang;

But the problem is that due to sudden movements of sensor, it gives random values which have nothing to do with sensor movement. For example when sensor is suddenly moved in positive direction of angle measurement, i gives -ve values for angle which then effect on pwm and thus direction of motor.

Can any one tell me where i am going wrong or why this is happening.

Do not triple post, or hijack other people's threads.

HELP PLEASE

I am working on an independent ultrasonic transmitter and receiver
I am able to generate the required pulse in the transmitter but not able to synch and retrieve data from the receiver side
I am using ZIGBEE s2 model for time synch

Thanks
regards
vj

Hi,
I kinda have the same problem.
I use a GY80 module... The accelerometer angle seems alright, but the gyroscope values drift a lot...
When I tried this a few days back, although the gyro values drifted, it still gave a reading close to that of acclerometer (+/- 5 degrees) but I don't know what happened coz the gyro now drifts to more than 20 degrees when brought back to the initial position.
Is it a problem with the refresh rate of Arduino, or what could the reason be? Please help me fix this...

Iam working on self balancing robot. The value from mpu6050 is stoping when connected to motor, that is when the motor starts rotating the reading from mpu6050 is stoping and the motor is working at last pwm value. I am using seperate power supplies for arduino and motors still it occures.