IMU, complementary filter to estimate roll angle

semogenius · March 20, 2024, 6:43am

Hello, I am using MPU 5060 IMU to find the roll angle (x direction) by implementing Complementary filter. I have included calibration for accelomter abd gyro but When I run my code , it works but doesn’t feel right, the angle as I rotates starts to drift then starting to stable after few seconds.and using goniometer there is a about 9 degrees difference .

Can you explain why this happens and what could be the solution to make it accurate ?

Thanks

Code:

#include <Wire.h>

float Ax, Ay, Az; // Accelerometer readings
float KneeAngleA, TwistAngleA; // Angles calculated from accelerometer
float GyroX, GyroY, GyroZ; // Gyroscope readings
float AngularX, AngularY, AngularZ; // Angular velocities
float CalibrationX, CalibrationY, CalibrationZ; // Gyroscope calibration values
int CalibrationValue; // calibration average
uint32_t Duration; // Duration since start
const float alpha = 0.89; // Complementary filter weight
const long T = 10; // Sample time in microseconds
float Theta; // Angle

void Config() {
// I2C communication and Low Pass Filter
Wire.beginTransmission(0x68);
Wire.write(0x1A);
Wire.write(0x05);
Wire.endTransmission();

// Accelerometer Data Output
Wire.beginTransmission(0x68);
Wire.write(0x1C);
Wire.write(0x10);
Wire.endTransmission();

// Accelerometer Measurements Output
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();

// Sensitivity of the Accelerometer
Wire.requestFrom(0x68, 6);
int16_t AxLSB = Wire.read() << 8 | Wire.read();
int16_t AyLSB = Wire.read() << 8 | Wire.read();
int16_t AzLSB = Wire.read() << 8 | Wire.read();

// Gyroscope Measurements Output
Wire.beginTransmission(0x68);
Wire.write(0x43);
Wire.endTransmission();
Wire.requestFrom(0x68, 6);
int16_t GyroX = Wire.read() << 8 | Wire.read();
int16_t GyroY = Wire.read() << 8 | Wire.read();
int16_t GyroZ = Wire.read() << 8 | Wire.read();

// Converting the Accelerometer Measurements to m/s^2
Ax = ((float)AxLSB / 4096 + 0.03) * 9.81; // Convert from g to m/s^2
Ay = (float)AyLSB / 4096 * 9.81; // Convert from g to m/s^2
Az = ((float)AzLSB / 4096 - 0.02) * 9.81; // Convert from g to m/s^2

// Convert Gyroscope Measurements to radians/s
AngularX = ((float)GyroX / 65.5) * (PI / 180); // Convert from degrees/s to radians/s
AngularY = ((float)GyroY / 65.5) * (PI / 180); // Convert from degrees/s to radians/s
AngularZ = ((float)GyroZ / 65.5) * (PI / 180); // Convert from degrees/s to radians/s

// Calculate the Angle from Accelerometer
KneeAngleA = atan(Ay / sqrt(Ax * Ax + Az * Az)) * (180 / PI); // Convert from radians to degrees
TwistAngleA = -atan(Ax / sqrt(Ay * Ay + Az * Az)) * (180 / PI); // Convert from radians to degrees
}

void setup() {
Serial.begin(57600);
Wire.setClock(400000);
Wire.begin();
delay(200);
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x00);
Wire.endTransmission();

for (CalibrationValue = 0; CalibrationValue < 3000; CalibrationValue++) {
Config();
CalibrationX += AngularX;
CalibrationY += AngularY;
CalibrationZ += AngularZ;
delayMicroseconds(500);
}

CalibrationX /= 3000;
CalibrationY /= 3000;
CalibrationZ /= 3000;
Duration = micros();
}

void loop() {
Config();
AngularX -= CalibrationX;
AngularY -= CalibrationY;
AngularZ -= CalibrationZ;

// Complementary filter to calculate The Angle
Theta = alpha * (Theta + T * AngularX) + (1 - alpha) * KneeAngleA;

Serial.print("Theta [°]= ");
Serial.println((int)Theta); // Convert to integer for printing
delay(90);
}

jremington · March 20, 2024, 2:28pm

Please use code tags when posting code. This code uses one of the correct approaches to calculate pitch and roll angles, when the sensor is stationary:

// minimal MPU-6050 tilt and roll. Works with MPU-9250 too.
// works perfectly with GY-521, pitch and roll signs agree with arrows on sensor module 7/2019
//
// Tested with 3.3V eBay Pro Mini with no external pullups on I2C bus (worked with internal pullups)
// Add 4.7K pullup resistors to 3.3V if required. A4 = SDA, A5 = SCL

#include<Wire.h>
const int MPU_addr1 = 0x68;
float xa, ya, za, roll, pitch;

void setup() {

  Wire.begin();                                      //begin the wire communication
  Wire.beginTransmission(MPU_addr1);                 //begin, send the slave adress (in this case 68)
  Wire.write(0x6B);                                  //make the reset (place a 0 into the 6B register)
  Wire.write(0);
  Wire.endTransmission(true);                        //end the transmission
  Serial.begin(9600);
}

void loop() {

  Wire.beginTransmission(MPU_addr1);
  Wire.write(0x3B);  //send starting register address, accelerometer high byte
  Wire.endTransmission(false); //restart for read
  Wire.requestFrom(MPU_addr1, 6); //get six bytes accelerometer data
  int t = Wire.read();
  xa = (t << 8) | Wire.read();
  t = Wire.read();
  ya = (t << 8) | Wire.read();
  t = Wire.read();
  za = (t << 8) | Wire.read();
// formula from https://wiki.dfrobot.com/How_to_Use_a_Three-Axis_Accelerometer_for_Tilt_Sensing
  roll = atan2(ya , za) * 180.0 / PI;
  pitch = atan2(-xa , sqrt(ya * ya + za * za)) * 180.0 / PI; //account for roll already applied

  Serial.print("roll = ");
  Serial.print(roll,1);
  Serial.print(", pitch = ");
  Serial.println(pitch,1);
  delay(400);
}

semogenius · March 20, 2024, 7:20pm

Thanks for your response.

Can you explain more, please? because the code that you provided is just to calculate the roll and pitch angle using accelerometer readings, and my topic is using both accelerometer and gyro readings to get angles and then using it in a complementary filter to get accurate readings in real-time.

jremington · March 20, 2024, 7:33pm

Portions of the code you posted are simply incorrect.

This, for example, is nonsense, because 3D rotation angles are not independent variables.

KneeAngleA = atan(Ay / sqrt(Ax * Ax + Az * Az)) * (180 / PI); // Convert from radians to degrees
TwistAngleA = -atan(Ax / sqrt(Ay * Ay + Az * Az)) * (180 / PI); // Convert from radians to degrees

semogenius · March 20, 2024, 8:02pm

I got you, thanks brother

Can you point out the mistakes I made in my code please, and what can be the solutions? except in the accelerometer angles mistake.

Appreciate it

jremington · March 20, 2024, 8:09pm

The complementary filter has largely been abandoned, since the use of quaternions to represent 3D orientation works so much better, and avoids problems with singular Euler angles.

Moving forward, there are plenty of examples of Mahony, Madgwick (and for higher accuracy, but much slower updates) Kalman fusion filters available.

You should also move away from the long obsolete MPU-6050, as the more recent IMUs substantially outperform it. You probably have some sort of imitation or clone, and there is no reason to believe it even meets the original manufacturer's specifications.

If you want to try a Mahony filter for the MPU-6050, here is one example. Yaw is relative to startup, since there is no North reference, and will drift. The ISM330 6DOF sensor works MUCH better.

semogenius · March 21, 2024, 6:34am

Thanks very much, Really appreciate this.

I have updated my code and tried to be more simple to make to easier.

what is your comment on it ?

#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;

const float alpha = 0.95;  // Complementary filter Weight
const float dt = 0.01;     // Sample time in seconds

float roll = 0.0;
float pitch = 0.0;

void setup() {
  Serial.begin(57600);

  // Initialize MPU6050
  Wire.begin();
  mpu.initialize();
}

void loop() {
  // Read accelerometer and gyroscope data
  int16_t ax, ay, az;
  int16_t gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Convert raw values to physical units
  float accelScale = 9.81 / 16384.0;  // Convert raw accelerometer values to m/s^2
  float gyroScale = 250.0 / 32767.0;  // Convert raw gyroscope values to degrees/s
  float ax_mps2 = ax * accelScale;
  float ay_mps2 = ay * accelScale;
  float az_mps2 = az * accelScale;
  float gx_degps = gx * gyroScale;
  float gy_degps = gy * gyroScale;
  float gz_degps = gz * gyroScale;

  // Calculate roll and pitch angles from accelerometer data
  float acc_roll = atan2(ay_mps2, az_mps2) * 180.0 / PI;
  float acc_pitch = atan2(-ax_mps2, sqrt(ay_mps2 * ay_mps2 + az_mps2 * az_mps2)) * 180.0 / PI;

  // Calculate gyro angles
  float gyro_roll = gx_degps * dt;   // Integration of gyroscope data
  float gyro_pitch = gy_degps * dt;  // Integration of gyroscope data

  // Complementary filter to combine accelerometer and gyro data
  roll = alpha * (roll + gyro_roll) + (1 - alpha) * acc_roll;
  pitch = alpha * (pitch + gyro_pitch) + (1 - alpha) * acc_pitch;

  // Print roll and pitch angles
  Serial.print(roll);
  Serial.print(",");
  Serial.println(pitch);

  delay(10);  
}

system · September 17, 2024, 6:35am

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.

Topic		Replies	Views
Wrong angles using MPU6500 Sensors	8	132	January 25, 2026
Problem Implementing Complementary Filter Sensors	15	882	February 13, 2024
MPU6050 with arduino complementary filter Programming	8	8857	May 5, 2021
Complementary filter with the MPU6050 General Guidance	28	62340	May 5, 2021
Problem with Accelerometer-Readings Sensors	11	667	May 18, 2024

IMU, complementary filter to estimate roll angle

Related topics