Arduino IMU attempt : errors, lagging, drifting

Question

I'm trying to use an Arduino Nano 33 BLE with a screen to create something to an aircraft Attitude Indicator and Directional Gyro.

For that purpose I would need to have precise Euler angles. I found out that the Nano comes with a 9DOF sensor and I attempted the use the Madgwick library to transform the sensor data into useful angles.

However, it looks like drifting along the yaw axis is happening, and also when moving the board along pitch and yaw axis it takes the filter a long while to catch up, sometimes even a few seconds to arrive at the result.

Another solution would be to attempt to use Adafruit BNO055 that claims to provide the Euler angles directly. However, I think a more elegant solution would be to adjust my code to make it work with the sensor that is already provided on the Nano.

Ideas?

#include "Arduino_LSM6DS3.h"
#include "MadgwickAHRS.h"
#include "Arduino_LSM9DS1.h"

// initialize a Madgwick filter:
Madgwick filter;
// sensor's sample rate is fixed at 104 Hz:
const float sensorRate = 104.00;

float sax, say, saz, sgx, sgy, sgz;

void setup() {
  Serial.begin(9600);
  // attempt to start the IMU:
  if (!IMU.begin()) {
    Serial.println("Failed to initialize IMU");
    // stop here if you can't access the IMU:
    while (true);
  }
  // start the filter to run at the sample rate:
  filter.begin(sensorRate);
}

long lastPrint = 0;
long nz = 0;
float x = 0, y = 0, z = 0;

void loop() {
  // values for acceleration and rotation:
  float xAcc, yAcc, zAcc;
  float xGyro, yGyro, zGyro;

  // values for orientation:
  float roll, pitch, heading;
  // check if the IMU is ready to read:
  if (IMU.accelerationAvailable() &&
      IMU.gyroscopeAvailable()) {
    // read accelerometer &and gyrometer:
    IMU.readAcceleration(xAcc, yAcc, zAcc);
    IMU.readGyroscope(xGyro, yGyro, zGyro);
    nz++;
    if (nz < 500)   //hold the board still until nz is 500 for calibration
    {
      sgz += zGyro;
      sgx += xGyro;
      sgy += yGyro;
      x = sgx / nz;
      y = sgy / nz;
      z = sgz / nz;
    }

    // update the filter, which computes orientation:
    filter.updateIMU(xGyro - x, yGyro - y, zGyro - z, xAcc, yAcc, zAcc);

    // print the heading, pitch and roll
    roll = filter.getRoll();
    pitch = filter.getPitch();
    heading = filter.getYaw();
    long a = millis();
    if (lastPrint + 333 < millis())
    {
      lastPrint = a;
      Serial.print(nz);
      Serial.print(" Acc ");
      Serial.print(xAcc);
      Serial.print(" ");
      Serial.print(yAcc);
      Serial.print(" ");
      Serial.print(zAcc);
      Serial.print(" ");

      Serial.print("Gyr ");
      Serial.print(xGyro);
      Serial.print(" ");
      Serial.print(yGyro);
      Serial.print(" ");
      Serial.print(zGyro);
      Serial.print(" avg ");

      Serial.print(" ~~Orientation: ");
      Serial.print(heading);
      Serial.print(" ");
      Serial.print(pitch);
      Serial.print(" ");
      Serial.println(roll);
    }
  }
}

Answer 1

The yaw drift you are seeing is simply because you are not reading the LSM9DS1's magnetometer and feeding its information to the filter.

You can't compensate the gyro's yaw drift with an accelerometer; the accelerometer doesn't see yaw.

This is where the magnetometer comes in. A magnetometer is like a compass; it can see direction and changes in yaw. An accelerometer can't, so to correct the drift in yaw, you need a magnetometer.

All gyros drift and you have to compensate for that. It is not just a matter of subtracting average drift; filters (complementary, Madgwick, Kalman, etc.) are used to combine the gyro, accelerometer, and magnetometer data to calculate smooth 3D orientation data that doesn't show drift. What the drone manufacturers call "gyro" is actually a combination of gyros, accelerometers, magnetometers, and math.

There may be other things wrong in your set-up, I haven't checked, but this is fundamental: you are treating the 9DoF sensor as a 6DoF sensor, and there is nothing there to compensate the yaw drift.

The Madgwick library does have a function update() that will take information from all 3 sensors; updateIMU() can only use 2.

Also, a few things I haven't looked at closely: you are including two libraries for two different IMUs; there appears to be a calibration for the gyro in the reading loop itself; you may be updating the filter too often or not often enough.