Calculating Angles from MPU6050

Question

I've been playing with a GY521 breakout recently with the MPU6050 chip on it, though i've hit a bit of a problem that I can't quite work out

I know when I rotated the board I rotated ~90 degrees about the Y axis, and using the sensitivity numbers from the datasheet (131) and integrating i've managed to get something that looks believable from the gyroscope, but am having problems with the accelerometer.

From a number of sources, this being one of them, I've managed to get the three equations seen on here, however when I apply them to either the raw or scaled data (as they're scalers is shouldn't make a difference?) I get a set of data that varies from 0 to 90, but only on the X and Z axes, while nothing happens to the Y.

Updated to include code:

#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"

MPU6050 accelgyro;

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

double timeStep, time, timePrev;
double arx, ary, arz, grx, gry, grz, gsx, gsy, gsz, rx, ry, rz;

int i;
double gyroScale = 131;

void setup() {

  Wire.begin();
  Serial.begin(9600);
  accelgyro.initialize();

  time = millis();

  i = 1;

}

void loop() {

  // set up time for integration
  timePrev = time;
  time = millis();
  timeStep = (time - timePrev) / 1000; // time-step in s

  // collect readings
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // apply gyro scale from datasheet
  gsx = gx/gyroScale;   gsy = gy/gyroScale;   gsz = gz/gyroScale;

  // calculate accelerometer angles
  arx = (180/3.141592) * atan(ax / sqrt(square(ay) + square(az))); 
  ary = (180/3.141592) * atan(ay / sqrt(square(ax) + square(az)));
  arz = (180/3.141592) * atan(sqrt(square(ay) + square(ax)) / az);

  // set initial values equal to accel values
  if (i == 1) {
    grx = arx;
    gry = ary;
    grz = arz;
  }
  // integrate to find the gyro angle
  else{
    grx = grx + (timeStep * gsx);
    gry = gry + (timeStep * gsy);
    grz = grz + (timeStep * gsz);
  }  

  // apply filter
  rx = (0.1 * arx) + (0.9 * grx);
  ry = (0.1 * ary) + (0.9 * gry);
  rz = (0.1 * arz) + (0.9 * grz);

  // print result
  Serial.print(i);   Serial.print("\t");
  Serial.print(timePrev);   Serial.print("\t");
  Serial.print(time);   Serial.print("\t");
  Serial.print(timeStep, 5);   Serial.print("\t\t");
  Serial.print(ax);   Serial.print("\t");
  Serial.print(ay);   Serial.print("\t");
  Serial.print(az);   Serial.print("\t\t");
  Serial.print(gx);   Serial.print("\t");
  Serial.print(gy);   Serial.print("\t");
  Serial.print(gz);   Serial.print("\t\t");
  Serial.print(arx);   Serial.print("\t");
  Serial.print(ary);   Serial.print("\t");
  Serial.print(arz);   Serial.print("\t\t");
  Serial.print(grx);   Serial.print("\t");
  Serial.print(gry);   Serial.print("\t");
  Serial.print(grz);   Serial.print("\t\t");
  Serial.print(rx);   Serial.print("\t");
  Serial.print(ry);   Serial.print("\t");
  Serial.println(rz);

  i = i + 1;
  delay(50);

}

Results:

Strikes me as a little odd, as I was expecting only a rotational change in Y. Any suggestions?

Answer 1

If I understand your question, the main issues are that the axis don't match up, and z from the accel is not accurate?

with respect to the axis: just swap the values. Rotation math is self-consistent and "correct" in many different permutations. What makes it correct is the set of equations, not any single equation; So, copying equations from one reference frame to another will yield incorrect results even if they were correct originally.

your link addresses this difference:

"Interpreting the diagram at left is a little tricky, since the tilt of the X-axis actually shows rotation around the Y-axis, and the angling of the Y-axis shows (negative) rotation around the X-axis."

  ary = -(180/3.141592) * atan(ax / sqrt(square(ay) + square(az))); 
  arx =  (180/3.141592) * atan(ay / sqrt(square(ax) + square(az)));

should work better for you

On the second point: accelerometers can't measure yaw, the "z" equation can safely be ignored. that isn't yaw it calculates. Calculating z from the gyro alone should fix your second issue.

Other considerations:

• The gyroscope tracking can be made more accurate by using the midpoint rule in your integration
• Your code is not integrating the gyroscope's signal with the best estimate of its last position (rx, ry, rz), instead it integrates the gyro with separate numbers (grx, gry, grz). In effect information from the accelerometer is discarded each loop and gyro drift is not corrected for.
• Thanks for including those graphs with your code samples =] rotation math is extremely tricky, and graphs help a lot