MPU6050 - Integrate euler rates to pitch,roll,yaw angle

[forthe24]

Hello, I'm trying to create a code that displays the roll, pitch, and yaw angle of an mpu6050 sensor by using the raw gyroscope data.

My purpose in writing this code was to show the gyro drift being created when your only using the gyroscope. The mpu6050 sensor is just laying at the table therefore I was expecting that the angle produce will start somewhere around 0 and will slowly increase or drift. The problem I'm having is that the angles I'm getting is all over the place. As of now I can't seem to determine the problem.

OUTPUT

Code

float gx,gy,gz;
float rollAngle=0.0f;
float pitchAngle=0.0f;
float yawAngle=0.0f;
float phiDot,thetaDot,psiDot;
float dt;
unsigned long startTime = 0;
const float rad2deg = 180.0f / PI;

//gyro measurements (rad/sec)
  gx = mpu6050.getGyroX();
  gy = mpu6050.getGyroY();
  gz = mpu6050.getGyroZ();
  
  unsigned long currentTime = micros();
  unsigned long delta = currentTime - startTime;
  dt = delta/1000000;

//body rate to euler rate
  phiDot = gx + tanf(pitchAngle) * (sinf(rollAngle) * gy + cosf(rollAngle) * gz);
  thetaDot = cosf(rollAngle) * gy - sinf(rollAngle) * gz;
  psiDot = cosf(pitchAngle) * (sinf(rollAngle) * gy + cosf(rollAngle) * gz);
  
//integrate euler rate to determine the estimated roll and pitch angle
  pitchAngle = pitchAngle + dt * phiDot;
  rollAngle = rollAngle + dt * thetaDot;

Body rate to euler rate

[jremington]

To cancel the drift (at least for short times) you need to measure the rate gyro offsets while it is still, and subtract those from subsequent measurements. Most people average a few hundred readings during startup.

The Mahony AHRS filter can be used to integrate the rates and obtain a reasonable estimate of the 3D orientation, by ignoring the accelerometer data. Example here. The orientation will be relative to the starting orientation, and it will always drift slowly, as the gyro offsets are very temperature dependent.

[forthe24]

Yes, I already did measure the gyro offsets using the built in function. But the angles are still all over the place.

  Wire.begin();
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);

[Koepel]

Can you get a newer and better sensor ?
The MPU-6050 is old, outdated, no longer manufactured, noisy, and it might be counterfeit.

The MPU-6050 (the real one from years ago) was noisy, but the drift was not so bad. Just laying on the table, it only drifts a little over a minute. Can you print the raw values to decide if your sensor is any good ?

This is the minimum sketch to get the raw values.

[forthe24]

sadly we can't use a newer sensor since this is what we have propose to our professor.

this is the built in function to get the raw gyro data in rad/sec

raw values (gx,gy and gz)

I did notice that my raw values are changing constantly even though the sensor is not moving. So I was assuming that the value I am getting from getGyro() was the problem.

[Koepel]

Please try the minimal sketch and tell which library you use.

It seems that the values are low and the Yaw uses the noise to determine the angle.

[forthe24]

Library: MPU6050_tockn

using minimal sketch

[ibuildrobots]

Couple of things I would be interested in double checking based on what I see in the code snippet and latest output sample that you have included in your above posts:

• Are you sure that you are calculating dt correctly?
  • First you are using an integer division when I would have expected you to want a floating point division based on the fact that it should be a fraction of a second. If you divide by 1000000.0f instead of 1000000 then you would get an integer division.
  • Do you update startTime to currentTime after calculating delta? If not then delta will always just be equal to currentTime.
• I don't know what MPU6050 library you are using but I would double check that the getGyro*() calls return radians/sec instead of degrees/sec. I have seen a preference for degrees/sec in gyro data sheets and documentation so the library might just go along with those. Also the noise you show in your latest sample output isn't as bad if the values are in degrees/sec instead of radians/sec.

[forthe24]

I'm using the MPU6050_tockn as my library. I'll double-check my code again based on your suggestions.

[Koepel]

The "MPU6050_tockn" seems to be in the Library Manager, it is this one: https://github.com/tockn/MPU6050_tockn.

The raw data seems alright. It is noisy, but I think that those are normal numbers. The filter should filter most out the noise.
If you have a 9DOF sensor (accelerometer, gyro, magnetometer) then the problems of one sensor are reduced by the other two.

Some MPU-6050 modules have the wrong capacitor at a pin, and by replacing or by adding a capacitor it becomes less noisy: https://forum.arduino.cc/t/mpu6050-breakout-board-gy-521-wrong-capacitor-results-in-erratic-data/380339

I think the library has a filter that is not good enough.
Could you test jremington's Mahony AHRS filter ?

[jremington]

@forthe24

But the angles are still all over the place.

The math must be wrong. Those are completely nonsensical angles. Did you forget that the trig functions use radians? And, as already pointed out, this certainly won't work:
dt = delta/1000000;

The code at the link I posted works fine.

[forthe24]

Thank you for all the suggestions, I had fixed the problem. It was indeed the calculation for dt, I also use another code for getting the raw angular velocities and everything works fine.

[ibuildrobots]

@forthe24

I had fixed the problem.

Excellent!

One experiment you can try that has helped me with gyro drift in the past is to log a moving average of the gyro drift for all axis along with the sensor temperature (a lot of the gyros I have used in the past do have a temperature measurement option as well). I cool the sensor down in the freezer, take it out, run the logging process and then graph the gyro drift vs temperature as the sensor warms up. On most gyros this has been a linear relationship. Fitting a linear equation to the graph could then be used to calibrate out the drift even more accurately in the code and the effect of gyro drift further reduced in the integration of the Euler angle measurements.