IMU library or functions for Tilt, roll, yaw

Hi guys,

I'm working on a project which includes a Wemos D1 Mini + LSM6DS3(accelerometer and gyroscope).

I'm able to read the values from the LSM6DS3 but now comes the hard part (at least for me )

How can I translate the sensor values to real life values for yaw, tilt and roll?
This chip doesn't have a DMP so I need to do that part via programming.

I found some explanations and formula's around the internet. But I couldn't find a clear example which also includes sensor fusion to compensate the sensor drift.

Are there any universal functions or a library which can be used? or at least some working code example?

Would be nice to get some help. :slight_smile:

An oldie but goodie that might give you a bit of direction https://os.mbed.com/users/onehorse/

You need a magnetometer to measure yaw. The yaw value derived from a 6DOF sensor will be relative, and drift.

There are several examples of the Madgwick or Mahony (better) filters for the 6DOF MPU6050 which could be adapted to your sensor.

Thanks, I will have a look but at first glance it still seems complicated.

it’s quite strange to me that there isn’t a universal library. There are many 6Dof sensors and all the developers have the same challenge.

I’ve choosen the LSM6DS3 because it’s dirt cheap (70-80 cents :slight_smile: ) so hopefully i’m able to get it working.

MPU 6050 starting at 0,50€ :wink: plus shipping... ok granted it has a 6 to 8 week shipping time from china, but guess where the other chips are coming from :wink:

i am working on a MPU 6050 project... but if you just need the math you could try to read through this library and modify as required.

maybe that helps... if not, sorry for wasting your time

Well, the MPU 6050 and 6500 are stated End of life by Invensense and their advice is not to use it for new designs. Also via more official sources (mouser, LCSC, etc..) the 6050 is not available for € 0,5,- but the LSM6D3S is. (70 cents)

This is for now a hobby project but may end up in a serie production so I want to take costs into account.

To which library you're referring, Arne?

Not sure if this works for ESP32.

Thanks for the library Pagus.

Sadly I don't see any functions for real life tilt + roll values.

After some reading....and reading... :slight_smile:

As far as I can see this are the steps that I need to take to get pitch, roll from the output of the IMU:

  1. Calibrate Accelerometer + Gyro for noise reduction (determine average noise)
  2. Get ACC values and determine ACC angle via:

rollAcc = atan2(ya , za) * 180.0 / PI;
pitchAcc = atan2(-xa , sqrt(ya * ya + za * za)) * 180.0 / PI;

  1. Get Gyro values and determine Gyro angle

  2. Calculate temperature offset for accelerometer + gyro (OPTIONAL)

  3. Combine accelerometer and gyro angle values with a complementary filter like:

roll = 0.98 * gyroAngleX + 0.02 * accAngleX;
pitch = 0.98 * gyroAngleY + 0.02 * accAngleY;

Are these the right steps?

That is one possible approach. The already mentioned Mahony fusion algorithm is another.

MasterBlaster:
After some reading....and reading... :slight_smile:

Are these the right steps?

No, you have to work in 3D throughout to combine accelerometer and rate-gyro. You need to derive
an orientation in 3D, fusing the gyro's short term rotation data with the accelerometers long term
gravity vector. (and maybe a magnetometer too).

Only once this is done can you get pitch or roll from the quarternion or DCM (two ways to encode 3D orientation).

Rotations about the different axes in 3D are not independent.

Hi All,

Now I'm a little confused. :slight_smile:

The steps in my previous post are used in multiple tutorials/articles. Are these not correct?
I understand that using a mahony (for 6DOF) or madgwick (9DOF) fusion method is better then standard complementary sensor fusion and maybe the way to go after all.

Having said that, I think I don't need absolute coordination because I don't use it for a Drone but for some basic balancing/tilt sensing. Yaw is nice to have but doesn't need to be very precise for the long term. A magnetometer would probably be overkill.

I have step 1+2 implemented but, before I spend more time on it, I would like to know if the steps are BS or not. :slight_smile:

Hi MasterBlaster,

The issue is that roll, pitch and yaw are not independent variables. For example if you pitch up 45° then yaw left 90°, you’re now in a roll position at 45°. The gyroscope cannot sense that a change in yaw also affects the roll and pitch. (The accelerometer can, but it’s so heavily filtered by the complementary filter that it takes too long to take effect). Therefore the relationship between roll, pitch and yaw has to be tracked by your microcontroller’s software.

Using the approach you detailed, it’s possible to additionally cross couple the yaw motion into the roll and pitch. This works provided your application doesn’t require full (roll and pitch) rotation in 3D space, just say up to 60° from the horizontal:

gyroRollAngle = gyroRollRate * dt + lastFilteredRollAngle -                // Integrate the gyro roll rate and add the last computed roll angle
lastFilteredPitchAngle * DEGREES_TO_RADIANS * gyroYawRate * dt;          // This line adds yaw coupling - using small angle approximation
//lastFilteredPitchAngle * sin(DEGREES_TO_RADIANS * gyroYawRate * dt);     // Formula without small angle approximaion
gyroPitchAngle = gyroPitchRate * dt + lastFilteredPitchAngle +             // Integrate the gyro pitch rate and add the last computed pitch angle
lastFilteredRollAngle * DEGREES_TO_RADIANS * gyroYawRate * dt;           // This line adds yaw coupling - using small angle approximation
//lastFilteredRollAngle * sin(DEGREES_TO_RADIANS * gyroYawRate * dt);      // Formula without small angle approximation

This is detailed in Shane Colton’s blog about the complementary filter here: Shane Colton: Fun with the Complementary Filter / MultiWii

3D rotational kinematic approaches use either the Direction Cosine Matrix (DCM) or Quaternions.

Thanks for the explanation Martin.

I understand now what the issue is.

Probably I could get away with the complementary approach as I don’t really need yaw. However, 3d approach would be cooler…hmm…

Need to think about it. :slight_smile:

Combine accelerometer and gyro angle values with a complementary filter

Better yet a low-pass complementary filter, for the Delta_T use micros instead of millis.

Hi,

Last couple of weeks I was fiddeling arround with some code and I got it working more or less.
But to be honest, I find the results disapointing.

So I could use some help.

  • Even with a noise offset (averaging) the results are very unstable.
  • Also if I turn one axis, there are unrealistic peak values (fast turning) or even sometimes lower values.
  • The accelerator values seem more stable and accurate then Gyro or combined, which is strange to my mind
  • i’m not sure how to use some settings like filter.begin(value). it only works if the value is 10 but I don’t know why.
    .

Below you can see the unstability and turning 90 degrees which triggers a jump to 152 degrees and also some negative values. :o

15:54:39.470 -> 166.64,  0.12,  0.14
15:54:39.604 -> 165.84,  0.16,  0.18
15:54:39.748 -> 165.61,  0.19,  -0.01
15:54:39.850 -> 165.39,  0.09,  0.28
15:54:39.990 -> 165.10,  0.11,  0.14
15:54:40.103 -> 164.96,  -0.09,  0.15
15:54:40.240 -> 164.69,  -0.04,  0.50
15:54:40.379 -> 166.23,  0.11,  0.44
15:54:40.491 -> 166.10,  -0.18,  0.58
15:54:40.631 -> 167.93,  -0.00,  0.70
15:54:40.738 -> 171.72,  -0.06,  1.27
15:54:40.878 -> 172.08,  0.05,  2.90
15:54:40.983 -> 173.87,  -1.19,  29.22
15:54:41.123 -> 174.39,  0.25,  67.94
15:54:41.262 -> 175.58,  4.68,  -31.40
15:54:41.374 -> 175.29,  5.02,  -28.89
15:54:41.518 -> 175.23,  3.16,  135.99
15:54:41.624 -> 175.25,  4.03,  143.96
15:54:41.732 -> 178.51,  8.71,  38.23
15:54:41.874 -> 174.76,  11.29,  -38.32
15:54:41.986 -> 177.18,  2.32,  152.33
15:54:42.122 -> 171.24,  9.95,  29.65
15:54:42.269 -> 165.19,  0.69,  74.43
15:54:42.377 -> 164.98,  1.48,  106.26
15:54:42.490 -> 163.39,  0.54,  94.08
15:54:42.638 -> 162.76,  0.43,  94.56
15:54:42.744 -> 162.53,  0.58,  94.01
15:54:42.881 -> 162.08,  0.59,  93.44
15:54:43.015 -> 162.00,  0.56,  94.03
15:54:43.121 -> 161.57,  0.56,  93.46
15:54:43.258 -> 161.16,  0.53,  93.76

Here’s my code: (i’m not a professional so be kind :slight_smile: )

#include "SparkFunLSM6DS3.h"
#include "Wire.h"
//#include "SPI.h"
#include "MahonyAHRS.h"
#include "MadgwickAHRS.h"

LSM6DS3 myIMU; //Default constructor is I2C, addr 0x6B

Mahony filter; // Choose a filter
//Madgwick filter;

//int16_t accOffsetX,accOffsetY,accOffsetZ,gyroOffsetX,gyroOffsetY,gyroOffsetZ;
//int16_t accX,accY,accZ,gyroX,gyroY,gyroZ;
float accOffsetX,accOffsetY,accOffsetZ,gyroOffsetX,gyroOffsetY,gyroOffsetZ = 0;
float accX,accY,accZ,gyroX,gyroY,gyroZ;

float x, y, z, aRoll, aPitch, aHeading;                 //three axis acceleration data
double roll = 0.00, pitch = 0.00, heading = 0.00;       //Roll & Pitch are the angles which rotate by the axis X and y

  

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);//(9600);
  delay(500); //relax...
  Serial.println("Processor came out of reset.\n");

  Wire.setClock(400000); // 400KHz
  
  //Call .begin() to configure the IMU
  myIMU.begin();

  //myIMU.settings.gyroSampleRate = 13;   //Hz.  Can be: 13, 26, 52, 104, 208, 416, 833, 1666
  //myIMU.settings.gyroBandWidth = 200;  //Hz.  Can be: 50, 100, 200, 400;
  //myIMU.settings.accelSampleRate = 13;  //Hz.  Can be: 13, 26, 52, 104, 208, 416, 833, 1666, 3332, 6664, 13330
  //myIMU.settings.accelBandWidth = 200;  //Hz.  Can be: 50, 100, 200, 400;

  filter.begin(10);

  CalcImuNoiseFloat();
  //CalcImuNoiseRaw();

  delay(500);
}


void loop()
{
  
   /*

  accX = myIMU.readRawAccelX() - accOffsetX;
  accY = myIMU.readRawAccelY() - accOffsetY;
  accZ = myIMU.readRawAccelZ() - accOffsetZ;
  gyroX = myIMU.readRawGyroX() - gyroOffsetX;
  gyroY = myIMU.readRawGyroY() - gyroOffsetY;
  gyroZ = myIMU.readRawGyroZ() - gyroOffsetZ;
  */
  accX = myIMU.readFloatAccelX() - accOffsetX;
  accY = myIMU.readFloatAccelY() - accOffsetY;
  accZ = myIMU.readFloatAccelZ() - accOffsetZ;
  gyroX = myIMU.readFloatGyroX() - gyroOffsetX;
  gyroY = myIMU.readFloatGyroY() - gyroOffsetY;
  gyroZ = myIMU.readFloatGyroZ() - gyroOffsetZ;  


  //calculatePR();

  //filter.update(accX, accY, accZ, gyroX, gyroY, gyroZ, 0, 0, 0);
  filter.updateIMU(gyroX, gyroY, gyroZ, accX, accY, accZ );

  //x = filter.getPitch();
  //y = filter.getRoll();
  //z = filter.getYaw();
  /*
  Serial.println("***** filter output *****");
  Serial.print(" roll = ");
  Serial.println(y, 2);
  Serial.print(" pitch = ");
  Serial.println(x, 2);
  Serial.print(" Yaw = ");
  Serial.println(z, 2);
  Serial.println("********************");
*/

  if (readyToPrint()) {
    // print the heading, pitch and roll
    roll = filter.getRoll();
    pitch = filter.getPitch();
    heading = filter.getYaw();
    Serial.print(heading);
    Serial.print(",  ");
    Serial.print(pitch);
    Serial.print(",  ");
    Serial.println(roll);
    //Serial.println("*******");
    //Serial.print(" aRoll = ");
    //Serial.println(aRoll, 2);
    //Serial.print(" aPitch = ");
    //Serial.println(aPitch, 2);
  }
}

void CalcImuNoiseFloat(){
  Serial.print(" **** Function: CalcImuNoiseFloat()");

  float xa, ya, za, xg, yg, zg; 
  int iterations = 200;
  
  xa = 0;
  ya = 0;
  za = 0;

    // ----------- Acc offset
    for (int i = 0; i < iterations; i++) {
      
      xa = xa + myIMU.readFloatAccelX();
      ya = ya + myIMU.readFloatAccelY();
      za = za + myIMU.readFloatAccelZ(); 
    }
    
    // ----------- Gyro offset
    for (int i = 0; i < iterations; i++) {
  
      xg = xg + myIMU.readFloatGyroX();
      yg = yg + myIMU.readFloatGyroY();
      zg = zg + myIMU.readFloatGyroZ();
    } 

  accOffsetX = xa/iterations;
  accOffsetY = ya/iterations;
  accOffsetZ = za/iterations - 1;
  gyroOffsetX = xg/iterations;
  gyroOffsetY = yg/iterations;
  gyroOffsetZ = zg/iterations;
    
  Serial.print(" accOffsetX: ");
  Serial.print(accOffsetX,4);
  Serial.print(" accOffsetY: ");
  Serial.print(accOffsetY,4);
  Serial.print(" accOffsetZ: ");
  Serial.print(accOffsetZ,4);
  Serial.println();
  // ------------------
  Serial.print("*** Gyro offSet ***");
  Serial.print(" gyroOffsetX: ");
  Serial.print(gyroOffsetX,4);
  Serial.print(" gyroOffsetY: ");
  Serial.print(gyroOffsetY,4);
  Serial.print(" gyroOffsetZ: ");
  Serial.print(gyroOffsetZ,4);
  Serial.println();
}

void CalcImuNoiseRaw(){
  Serial.print(" **** Function: CalcImuNoiseRaw()");

  int16_t xa, ya, za, xg, yg, zg; 
  int iterations = 200;
  xa = 0;
  ya = 0;
  za = 0;

    // ----------- Acc offset
    for (int i = 0; i < iterations; i++) {
      
      xa = xa + myIMU.readRawAccelX();
      ya = ya + myIMU.readRawAccelY();
      za = za + myIMU.readRawAccelZ(); 
    }
    
    // ----------- Gyro offset
    for (int i = 0; i < iterations; i++) {
  
      xg = xg + myIMU.readRawGyroX();
      yg = yg + myIMU.readRawGyroY();
      zg = zg + myIMU.readRawGyroZ();
    } 

  accOffsetX = xa/iterations;
  accOffsetY = ya/iterations;
  accOffsetZ = za/iterations; // - 1;
  gyroOffsetX = xg/iterations;
  gyroOffsetY = yg/iterations;
  gyroOffsetZ = zg/iterations;
    
  Serial.print(" accOffsetX: ");
  Serial.print(accOffsetX,4);
  Serial.print(" accOffsetY: ");
  Serial.print(accOffsetY,4);
  Serial.print(" accOffsetZ: ");
  Serial.print(accOffsetZ,4);
  Serial.println();
  // ------------------
  Serial.print("*** Gyro offSet ***");
  Serial.print(" gyroOffsetX: ");
  Serial.print(gyroOffsetX,4);
  Serial.print(" gyroOffsetY: ");
  Serial.print(gyroOffsetY,4);
  Serial.print(" gyroOffsetZ: ");
  Serial.print(gyroOffsetZ,4);
  Serial.println();
}

void calculatePR(){
  double x_Buff = float(accX);
  double y_Buff = float(accY);
  double z_Buff = float(accZ);
  aRoll = atan2(y_Buff , z_Buff) * 57.295779513082321;
  aPitch = atan2((- x_Buff) , sqrt(y_Buff * y_Buff + z_Buff * z_Buff)) * 57.295779513082321;
}

// Decide when to print
bool readyToPrint() {
  static unsigned long nowMillis;
  static unsigned long thenMillis;

  // If the Processing visualization sketch is sending "s"
  // then send new data each time it wants to redraw
  while (Serial.available()) {
    int val = Serial.read();
    if (val == 's') {
      thenMillis = millis();
      return true;
    }
  }
  // Otherwise, print 8 times per second, for viewing as
  // scrolling numbers in the Arduino Serial Monitor
  nowMillis = millis();
  if (nowMillis - thenMillis > 125) {
    thenMillis = nowMillis;
    return true;
  }
  return false;
}

The LSM6DS3 is a 6DOF sensor, so yaw will be relative, and unstable.

You need a properly calibrated 9DOF sensor in order to estimate absolute 3D orientations.

jremington:
The LSM6DS3 is a 6DOF sensor, so yaw will be relative, and unstable.

You need a properly calibrated 9DOF sensor in order to estimate absolute 3D orientations.

Hi Jremington,

Thanks for your reply.

I know it's a 6DOF sensor, I'm not really interrested in the yaw but i'm hoping to get decent pitch and roll.
I turned the IMU 90 degrees and if you look at the results, the roll (last row) has strange values. it over-and undershoots the values. Something is off but I can't figure out what.

Any gues?

With serial plotter running, could you post a screen shot of your data?

Your description reads like somewhere along the line there is a unit8_t. If using a uint8_t instead of a int8_t can make a difference in roll over.

When the a value is sitting at +180 degrees and rolls over to -180 a uint8_t will give wonky readings.

Will this help?

  1. The sensor is horizontal.
  2. Then I turned it slowly 90 degrees.
  3. Back to horizontal.
  4. 90 degrees other way.
    5 Horizontal again