1

I am trying to write a simple sketch for the MPU6050.

The problem is that the sensor is pretty much solid when stationary or changing the angle steadily at slow to mid angular velocities but if I try to change the angle too fast there's a huge drift in the readings by pretty much tens of degrees in either direction as shown in the below images from the serial plotter.

On slow to mid changes in the pitch angle the gyro is tracking the accelerometer pitch almost identically (it drifts a little over time but it's normal and acceptable for the complementary filter).

On fast changes however the pitch drifts instantly by tens of degrees from the accelerometer. The huge drift makes the complementary filter pretty much useless in correcting the output

It's my first try with the sensor so I don't have any experience with its parameters. But the problem seems to be in the lines of integrating the new angular velocities in roll and pitch.

  • I got lazy and used the default sensitivity scale factors; does this have anything to do with this behaviour?

  • The dt measured for the Arduino is 20 ms between readings; is this too slow for the fast changes or something?

  • The datasheet also mentioned a programmable output range. Does this need to be programmed for more bandwidth and accurate results?

EDIT:

I tried to read a bit about the sensitivity scale factor. It seemed that increasing the angular velocity beyond 250 degrees per second is not measured when selecting the default configuration.

After some testing I realized that 1000 deg/s is a good configuration for me; the drift certainly is much better but still not what I hoped for. It still drifts by +- 10 degrees in two or three seconds of moving it around.

Slow to Mid changes output:

Slow to Mid speed changes in the pitch

Fast changes output:

Fast changes in the pitch

#include <Wire.h>
#define CAL_AVERAGE 2000

const int MPU = 0x68; //I2C address of MPU6050
const float alpha = 0.95;

double ax, ay, az;
double gx, gy, gz;

//float accAngleX, accAngleY, accAngleZ;
//float gyroAgnleX, gyroAngleY, gyroAngleZ;

double accErrorX, accErrorY, accErrorZ;
double gyroErrorx, gyroErrorY, gyroErrorZ;

double accRoll, accPitch;
double gyroRoll, gyroPitch, gyroYaw;

double roll, pitch, yaw;

double dt, currentTime, previousTime;


void setup(){
  previousTime = 0;

  Serial.begin(115200);
  Wire.begin();     //init the communication

  //reset the mpu
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission(true);

  //calibrate here
  calculateError();
}

void loop(){
  readAcc();
  readGyro();

  //get roll and pitch of accelerometer
  accRoll = atan2(ay, az) * 180 / PI;
  accPitch = atan2(ax, az) * 180 / PI;
    
  currentTime = millis();
  //set gyro roll and pitch for accelerometer roll and pitch on first reading only
  if (previousTime != 0) { 
    //get roll, pitch and yaw of gyro
    dt = currentTime - previousTime;
    dt /= 1000;
    gyroRoll += gx * dt;
    gyroPitch += -gy * dt;
    gyroYaw += gz * dt;
  } else {
    gyroRoll = accRoll;
    gyroPitch = accPitch;
    gyroYaw = 0;
  }

  previousTime =  currentTime;

  roll = alpha*gyroRoll + (1 - alpha)*accRoll;
  pitch = alpha*gyroPitch + (1 - alpha)*accPitch;
  yaw = gyroYaw;

  //finally print the values collected
  printData();
}

void readAcc(){
  //first read accelerometer data
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true);

  //accelerometer raw data
  ax = ( (Wire.read() << 8) | Wire.read() ) / 16384.0;
  ay = ( (Wire.read() << 8) | Wire.read() ) / 16384.0;
  az = ( (Wire.read() << 8) | Wire.read() ) / 16384.0;

  ax -= accErrorX;
  ay -= accErrorY;
  az -= accErrorZ;
}

void readGyro(){
  //second read gyro
  Wire.beginTransmission(MPU);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true);

  //gyro raw data
  gx = ( (Wire.read() << 8) | Wire.read() ) / 131.0;
  gy = ( (Wire.read() << 8) | Wire.read() ) / 131.0;
  gz = ( (Wire.read() << 8) | Wire.read() ) / 131.0;

  gx -= gyroErrorx;
  gy -= gyroErrorY;
  gz -= gyroErrorZ;
}

void calculateError(){
  float axErr = 0, ayErr = 0, azErr = 0;
  float gxErr = 0, gyErr = 0, gzErr = 0;

  for (int i = 0; i < CAL_AVERAGE; i++) {
    readAcc();
    az = az - 1;

    axErr += ax;
    ayErr += ay;
    azErr += az;

    readGyro();

    gxErr += gx;
    gyErr += gy;
    gzErr += gz;
  }

  accErrorX = axErr/CAL_AVERAGE;
  accErrorY = ayErr/CAL_AVERAGE;
  accErrorZ = azErr/CAL_AVERAGE;

  gyroErrorx = gxErr/CAL_AVERAGE;
  gyroErrorY = gyErr/CAL_AVERAGE;
  gyroErrorZ = gzErr/CAL_AVERAGE;
}

void printData() {
  Serial.print(ax);
  Serial.print(",     ");
  Serial.print(ay);
  Serial.print(",     ");
  Serial.print(az);

  Serial.print(",               ");

  Serial.print(gx);
  Serial.print(",     ");
  Serial.print(gy);
  Serial.print(",     ");
  Serial.print(gz);

  Serial.print(",               ");

  Serial.print(accPitch);
  Serial.print(",     ");
  Serial.print(accRoll);

  Serial.print(",               ");

  Serial.print(gyroPitch);
  Serial.print(",     ");
  Serial.print(gyroRoll);
  Serial.print(",     ");
  Serial.print(gyroYaw);

  Serial.print(",               ");

  Serial.print(pitch);
  Serial.print(",     ");
  Serial.print(roll);
  Serial.print(",     ");
  Serial.print(yaw);

  Serial.print(",     ");
  Serial.println(dt);
}

2 Answers 2

2

In his answer, st2000 recommends you use quaternions, rather than Euler angles, for tracking the orientation of your vehicle. I will give the exact same recommendation, but for a different reason: the formulas you use, namely

gyroRoll += gx * dt;
gyroPitch += -gy * dt;
gyroYaw += gz * dt;

are just plain wrong. They are correct if rotations only ever happen along a single axis. They are also “kinda good” (correct as a first-order approximation) if the angles involved are very small. But in the general case, they are wrong. A easy way to convince yourself of this fact is to notice that the final (yaw, pitch, roll) being the sum of many small contributions, they do not depend on the order in which these contributions were added. In other words, these formulas imply that the composition of rotations is commutative, which I hope you know is not the case.

The only reasonable ways of composing many rotations (which is what this attitude-tracking code tries to do) are rotation matrices and quaternions, the latter being more efficient.

Now, a few remarks:

  1. You should not track (gyroRoll, gyroPitch and gyroYaw) independently from (roll, pitch, yaw), otherwise your complementary filter becomes inefficient at canceling gyro drift. You should instead merge those variable into a single triplet.

  2. The line

    ( (Wire.read() << 8) | Wire.read() ) / 16384.0;
    

    gets you the two bytes from the MPU in an unspecified order. They may be in the right order with this particular version of this particular compiler when run with this particular set of options, but you should not count on this order being reliable. You should instead read the two bytes in two separate statements.

  3. If you do not care about the yaw, you may just track the gravity vector instead of the full attitude. This should save you the calls to atan2() and possibly simplify the math further. You may not even need the quaternions after all.

1

Consider what might happen if the code or hardware dead reckoning capabilities were too slow to accurately sample short events. Such as a sudden stop. Robbed of the opportunity to integrate a fast deceleration event, the code and hardware might continue to report motion.

To speed up code execution, consider switching from trigonometry based calculations to quaternion calculations. In general this replaces time consuming repetitive summation algorithms with simple math calculations more suitable for embedded processors.

To speed up the hardware, consider an Arduino platform with a faster processor.

Also consider more creative solutions suitable for your application:

  • If angular momentum is more important than linear motion sample the accelerometer only every other time through your main loop.
  • Or, if motion is totally random, and the device is on Earth, consider zero-calibrating the gyroscope when there is no movement perceived by the accelerometer. Specifically, when there is a 9.8m/s*s vector sensed by the accelerometer and the pitch, roll and yaw of the accelerometer is not changing.

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.