Pitch and Roll angles with MPU6050 and Complimentary filter

Hi,

I'm using an MPU6050 (3 axis accelerometer, 3 axis gyroscope) and a complimentary filter to create 3d rotations of a box in processing without yaw. However when testing the filtered roll angles or filtered pitch angles separately there seems to be an unnecessary jump. When rotating from 0 to pi or 0 to -pi in pitch or roll it seems to work fine, but when I go beyond pi or -pi it seems to make an unnecessary 2pi turn and then continue correctly. Below is what I see from pitch angles in Serial Plotter. Arduino code and processing code are also attached.

Arduino Code:

#include <Wire.h>

const int MPU6050_address = 0x68;
int ACCEL_XOUT_H, ACCEL_XOUT_L, ACCEL_YOUT_H, ACCEL_YOUT_L, ACCEL_ZOUT_H, ACCEL_ZOUT_L;
int GYRO_XOUT_H, GYRO_XOUT_L, GYRO_YOUT_H, GYRO_YOUT_L, GYRO_ZOUT_H, GYRO_ZOUT_L;
double ACCEL_XOUT, ACCEL_YOUT, ACCEL_ZOUT, GYRO_XOUT, GYRO_YOUT, GYRO_ZOUT, dt, start, finish; 
double gx, gy, gz, elapsed, angle_X, angle_Y, angle_Z, X, Y, Z;
const double pi = 3.14159265, alpha = 0.96;

void setup() 
{  
  Wire.begin();
  Serial.begin(9600);
  
  Wire.beginTransmission(MPU6050_address);
  Wire.write(0x6B);
  Wire.write(B00000000);  //enables wakeup mode
  Wire.endTransmission();
}

void loop() 
{
  start = millis();
  
  Wire.beginTransmission(MPU6050_address);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_address,2);
  ACCEL_XOUT_H = Wire.read();
  ACCEL_XOUT_L = Wire.read();

  ACCEL_XOUT_H = ACCEL_XOUT_H << 8;
  ACCEL_XOUT = ((ACCEL_XOUT_H + ACCEL_XOUT_L) / 16384.00) - 0.02; //Last value is offset value


  Wire.beginTransmission(MPU6050_address);
  Wire.write(0x3D);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_address,2);
  ACCEL_YOUT_H = Wire.read();
  ACCEL_YOUT_L = Wire.read();

  ACCEL_YOUT_H = ACCEL_YOUT_H << 8;
  ACCEL_YOUT = ((ACCEL_YOUT_H + ACCEL_YOUT_L) / 16384.00) - 0.025; //Last value is offset value


  Wire.beginTransmission(MPU6050_address);
  Wire.write(0x3F);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_address,2);
  ACCEL_ZOUT_H = Wire.read();
  ACCEL_ZOUT_L = Wire.read();

  ACCEL_ZOUT_H = ACCEL_ZOUT_H << 8;
  ACCEL_ZOUT = ((ACCEL_ZOUT_H + ACCEL_ZOUT_L) / 16384.00) - 0.005; //Last value is offset value


  Wire.beginTransmission(MPU6050_address);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_address,2);
  GYRO_XOUT_H = Wire.read();
  GYRO_XOUT_L = Wire.read();

  GYRO_XOUT_H = GYRO_XOUT_H << 8;
  GYRO_XOUT = ((GYRO_XOUT_H + GYRO_XOUT_L) / 131.00) - 0.791; //Last value is offset value
  gx = (GYRO_XOUT * dt) * pi/180.00;
  

  Wire.beginTransmission(MPU6050_address);
  Wire.write(0x45);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_address,2);
  GYRO_YOUT_H = Wire.read();
  GYRO_YOUT_L = Wire.read();

  GYRO_YOUT_H = GYRO_YOUT_H << 8;
  GYRO_YOUT = ((GYRO_YOUT_H + GYRO_YOUT_L) / 131.00) + 0.291; //Last value is offset value
  gy = (-1 * (GYRO_YOUT) * dt) * pi/180.00;


  Wire.beginTransmission(MPU6050_address);
  Wire.write(0x47);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_address,2);
  GYRO_ZOUT_H = Wire.read();
  GYRO_ZOUT_L = Wire.read();

  GYRO_ZOUT_H = GYRO_ZOUT_H << 8;
  GYRO_ZOUT = ((GYRO_ZOUT_H + GYRO_ZOUT_L) / 131.00) - 0.7913; //Last value is offset value
  gz = (GYRO_ZOUT * dt) * pi/180.00;

  angle_X = atan2(ACCEL_YOUT, ACCEL_ZOUT);
  angle_Y = atan2(ACCEL_XOUT,ACCEL_ZOUT);
  //angle_Z = atan2(ACCEL_YOUT, ACCEL_XOUT);
  
  X = alpha * (X + gx) + (1.00 - alpha) * angle_X; // pitch angles seem incorrect
  Y = alpha * (Y + gy) + (1.00 - alpha) * angle_Y; // roll angles seem incorrect
  //Z = (Z + gz);

  Serial.print(X);
  Serial.print(" ");
  Serial.print(0);
  Serial.print(" ");
  Serial.println(0);
  
  finish = millis();
  elapsed = finish - start;
  dt = elapsed / 1000.00;
  start = 0.00;
  elapsed = 0.00;
}

Processing Code:

import processing.serial.*;

Serial myPort;
String myString = null;
float X, Y, Z;

void setup(){
  String portName = Serial.list()[1];
  myPort = new Serial(this, portName, 9600);
  size(500,400,P3D);
  myPort.bufferUntil(10);
}

void draw()
{
  
  background(0);
  smooth();
  lights();
  
  fill(150);
  stroke(100);
  pushMatrix();
  translate(250,200,0);
  rotateX(X);
  rotateZ(Y);
  rotateY(Z);
  box(100,50,50);
  popMatrix();
}

void serialEvent(Serial myPort)
{
 while(myPort.available() > 0)
 {
  myString = myPort.readStringUntil(10);
  if(myString != null)
  {
     myString = trim(myString);
     float[] val = float(split(myString, " "));
      if(val.length == 3)
      {
       X = val[0];
       Y = val[1];
       Z = val[2];
      }
      
  }
 }
}

Serial Plotter.pdf (29.7 KB)

The problem you are having is unavoidable when working with Euler angles, because they do not uniquely or continuously represent body orientations. The problem is known as "gimbal lock" and is the reason that most developers now use a quaternion to represent body orientation.

By the way, one of these two equations is wrong, depending on which order and which direction the rotations are applied.

  angle_X = atan2(ACCEL_YOUT, ACCEL_ZOUT);
  angle_Y = atan2(ACCEL_XOUT,ACCEL_ZOUT);

See How_to_Use_a_Three-Axis_Accelerometer_for_Tilt_Sensing-DFRobot and the referenced literature for the correct equations.

The Serial plot attachment shows continous rotation around the x axis in only one dimension. I'm confused on why there is a jump between -pi to pi in the filtered angle. The equations for pitch and roll from the accelerometers calculated from the link yield similar results.

I'm confused on why there is a jump between -pi to pi in the filtered angle

There is no jump. Those are two different representations of the same angle.

The output range of trigonometric functions is restricted, which is clearly stated in the function documentation.