Removing gravity from MPU6050 Data

mouuaadd · April 4, 2022, 3:29pm

Hello,

I have been hard stuck in an IMU Data processing problem. I am using MPU6050 which is a 6DOF Sensor (Accelerometer & gyroscope) with Adafruit_MPU6050.h library.

The problem lies in the gravity vector being a part of the accelerometer data (magnitude of 9.8 m/s^2). My goal is to retrieve the linear acceleration of a person and therefore the velocity, but as the sensor rotates, the xyz components of the gravity are divided between the 3 axis.

After some research, on the forum and inspiration from this topic, (Richard Tech's website and Github were taken down), I wrote an algorithm that:

Takes the acceleration at rest and defines that as the gravity acceleration.
Uses the rotation matrix to bring the acceleration readings back into earth's frame.
Subtracts the gravity from the rotated acceleration.

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <math.h>

Adafruit_MPU6050 mpu;
float prev = 0;
float RadX = 0;
float RadY = 0;
float RadZ = 0;
float AccX = 0;
float AccY = 0;
float AccZ = 0;
int flag = 0;
float scale = 0.01;
gx_offset = 0, gy_offset = 0, gz_offset = 0;
float gravity [3] = {0,0,0};



void setup(void) {
  Serial.begin(115200);
  while (!Serial)
    delay(10); // will pause Zero, Leonardo, etc until serial console opens

  Serial.println("Adafruit MPU6050 test!");

  // Try to initialize!
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  Serial.print("Accelerometer range set to: ");
  switch (mpu.getAccelerometerRange()) {
  case MPU6050_RANGE_2_G:
    Serial.println("+-2G");
    break;
  case MPU6050_RANGE_4_G:
    Serial.println("+-4G");
    break;
  case MPU6050_RANGE_8_G:
    Serial.println("+-8G");
    break;
  case MPU6050_RANGE_16_G:
    Serial.println("+-16G");
    break;
  }
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  Serial.print("Gyro range set to: ");
  switch (mpu.getGyroRange()) {
  case MPU6050_RANGE_250_DEG:
    Serial.println("+- 250 deg/s");
    break;
  case MPU6050_RANGE_500_DEG:
    Serial.println("+- 500 deg/s");
    break;
  case MPU6050_RANGE_1000_DEG:
    Serial.println("+- 1000 deg/s");
    break;
  case MPU6050_RANGE_2000_DEG:
    Serial.println("+- 2000 deg/s");
    break;
  }

  mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);
  Serial.print("Filter bandwidth set to: ");
  switch (mpu.getFilterBandwidth()) {
  case MPU6050_BAND_260_HZ:
    Serial.println("260 Hz");
    break;
  case MPU6050_BAND_184_HZ:
    Serial.println("184 Hz");
    break;
  case MPU6050_BAND_94_HZ:
    Serial.println("94 Hz");
    break;
  case MPU6050_BAND_44_HZ:
    Serial.println("44 Hz");
    break;
  case MPU6050_BAND_21_HZ:
    Serial.println("21 Hz");
    break;
  case MPU6050_BAND_10_HZ:
    Serial.println("10 Hz");
    break;
  case MPU6050_BAND_5_HZ:
    Serial.println("5 Hz");
    break;
  }

  Serial.println("");
  Serial.println("Calibrating...");
  
 sensors_event_t a, g, temp;
 mpu.getEvent(&a, &g, &temp);
 

 gravity[0] = a.acceleration.x;
 gravity[1] = a.acceleration.y;
 gravity[2] = a.acceleration.z;
 gx_offset += g.gyro.x;
 gy_offset += g.gyro.y;
 gz_offset += g.gyro.z;
 
  delay(2000);
}


//****************************************************************************************************************
//*********************************************************************************************************
//***********************************************************************************************
//****************************************************************************************
//*********************************************************************************




void loop() {
  /* Get new sensor events with the readings */
  
  // accel = Body Frame Acceleration
  // gravity = acceleration at the start
  //rG = Rotated Gravity into body frame
  //mA = modified acceleration = rotated Acceleration - gravity
  //rA = Rotated acceleration back into earth frame
  
  float rG[3], rA[3];
  float mA[3];
  float now = millis();
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  float elapsed = (now-prev)/1000;
  g.gyro.x = (int)((g.gyro.x-gx_offset) / scale) * scale;
  g.gyro.y = (int)((g.gyro.y-gy_offset) / scale) * scale;
  g.gyro.z = (int)((g.gyro.z-gz_offset) / scale) * scale;
  RadX += (g.gyro.x*elapsed);
  RadY += (g.gyro.y*elapsed);
  RadZ += (g.gyro.z*elapsed);
  float  DegX = RadX*57.2958;
  float  DegY = RadY*57.2958;
  float  DegZ = RadZ*57.2958;

//Defining acceleration and removing the offset
  float accel[3]={(int)((a.acceleration.x) / scale) * scale,(int)((a.acceleration.y) / scale) * scale,(int)((a.acceleration.z) / scale) * scale};// data will come from the accelerometers

//Body frame to Inertial frame matrix
  float R[3][3] =
{
  { cos(RadZ)*cos(RadY) , cos(RadZ)*sin(RadY)*sin(RadX) - sin(RadZ)*cos(RadX) , cos(RadZ)*sin(RadY)*cos(RadX) + sin(RadZ)*sin(RadX)},
  { sin(RadZ)*cos(RadY) , sin(RadZ)*sin(RadY)*sin(RadX) + cos(RadZ)*cos(RadX) , sin(RadZ)*sin(RadY)*cos(RadX) - cos(RadZ)*sin(RadX)},
  {     -1* sin(RadY)    ,                  cos(RadY) * sin(RadX)                 ,               cos(RadY) * cos(RadX)                   }
};
//


//Rotated Acceleration back to earth frame
rA[0]= accel[0]*R[0][0] + accel[1]*R[0][1] + accel[2]*R[0][2] ;
rA[1]= accel[0]*R[1][0] + accel[1]*R[1][1] + accel[2]*R[1][2] ;
rA[2]= accel[0]*R[2][0] + accel[1]*R[2][1] + accel[2]*R[2][2] ;

//subtract gravity from earth frame acceleration 
mA[0]=rA[0]-gravity[0];
mA[1]=rA[1]-gravity[1];
mA[2]=rA[2]-gravity[2];


 //Final Calculations
double Acc = sqrt(pow(accel[0],2)+pow(accel[1],2)+pow(accel[2],2));
double RAcc = sqrt(pow(rA[1],2)+pow(rA[2],2));
double RGAcc = sqrt(pow(mA[0],2)+pow(mA[1],2)+pow(mA[2],2));
double angle = atan(mA[2]/mA[1])*57.2958;


  /* Print out the values */
//  Serial.print("Temperature: ");
//  Serial.print(temp.temperature);
//  Serial.print(" RadC  |");

  Serial.print("  rA: ");
  Serial.print(mA[0]);
  Serial.print(", Y: ");
  Serial.print(mA[1]);
  Serial.print(", Z: ");
  Serial.print(mA[2]);
// Serial.print(", M: ");
//  Serial.print(RAcc);
  Serial.print(" m/s^2  |");
//  Serial.print(" angle: ");
//  Serial.print(angle);


//  Serial.print("Rotation X: ");
//  Serial.print(g.gyro.x,4);
//  Serial.print(", Y: ");
//  Serial.print(g.gyro.y,4);
//  Serial.print(", Z: ");
//  Serial.print(g.gyro.z,4);
//  Serial.print(" rad/s  |");
//  Serial.print("  Degrees X: ");
//  Serial.print(DegX);
//  Serial.print(", Y: ");
//  Serial.print(DegY);
//  Serial.print(", Z: ");
//  Serial.print(DegZ);
//  Serial.print("  |");

//  Serial.print("Elapsed:");
//  Serial.print(elapsed);

  Serial.println("");
  prev = now;
  
}

However, the results are not convincing. I'm getting readings that are close to 0 m/s^2 at rest and far from 0 when rotated slowly to 90 degrees (Sample rate is 0.01s). I need to have data that at least approximates real life values but these readings will result in high inaccuracy when derived vs. time.

Rotated 90 degrees in Y:
Y_Rotation

I'm not sure where the issue is from. Has anyone done this before and succeeded?

jremington · April 4, 2022, 3:46pm

Not with any useful long term accuracy. You can read about one reason for the failure of this approach here:

https://web.archive.org/web/20201022151347/chrobotics.com/library

Scroll down to "using accelerometers to estimate ..."

DrDiettrich · April 4, 2022, 3:47pm

It may be poor offset compensation. Look for good offset determination code and follow the procedures. Another issue may be a slight misalignment of the sensor within its case or on board that results in fake offsets.

mouuaadd · April 4, 2022, 5:40pm

I stumbled upon this interesting website during my research. I figured the approach would work for my application which consists of 30 seconds long trials MAX.

Is it doable? or do you recommend dropping the MPU6050 and using other technology to determine the kinematics of a person i.e Computer Vision?

jremington · April 4, 2022, 5:43pm

The above assumes that there is no acceleration other than that due to gravity, and is guaranteed to fail if the assumption is not reasonable, or the orientation changes.

Many people have tried to do what you want, and have had little to no success.

mouuaadd · April 4, 2022, 5:58pm

All my trials have been done on a rested breadboard, with the X axis of the IMU initially pointing up. So I believe that the assumption stands.

So can we say that the accelerometer has to be in a flat position (not rotating) in order to obtain linear acceleration?

jremington · April 4, 2022, 6:27pm

No.

To subtract the acceleration due to gravity accurately, you need to know the 3D (Earth frame) sensor orientation to about 0.1 degree accuracy, or better.

Commercial IMUs are available to do this, at cost $50000 and up.

system · October 1, 2022, 6:27pm

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

Topic		Replies	Views
MPU6050 Library Mistake in Removal of gravity vector from accelerometer? Sensors	5	1075	September 30, 2022
Inconsistent data about gravity vector using mpu6050 Programming Questions	6	1963	November 24, 2021
MPU-6050's Linear Acceleration is Way Off Sensors	9	8136	May 6, 2021
Need help with imu Project Guidance	2	436	May 5, 2021
Remove gravity acceleration on the MPU6050 Programming Questions	4	1936	May 6, 2021

Removing gravity from MPU6050 Data

Related Topics