Arduino IMU problem with calibration and accuracy

Hello!

I'm trying to use 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 Nano comes with 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? thanks

//#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);
    }
  }
}

Yaw will always drift, if you don't use a magnetometer to define North. The magnetometer also must be calibrated, or it will produce nonsense.

The Madgwick and Mahony filters won't work unless all the sensor axial definitions are made consistent, and in a right handed coordinate system. The sensor axis definitions for the accelerometer and gyro on the Nano 33 BLE do not form a right handed coordinate system, and they are not consistently defined with the magnetometer, so the sensor axes have to be properly swapped and aligned. See image below for sensor axis definitions.

I don't use that board and haven't checked whether anyone has actually done all the work needed to make the filter work properly. This thread may help.

Now it's even more confusing... user MichaelFBA is using this call to update the filter:

filter.update(gx, gy, gz, ax, ay, az, -mx, my, mz); //for all 3
[color=#24292e]


however, based on your diagrams (-y magnetic is aligned with x acceleration; as well as -x magnetic
being consistent with y acceleration), a more appropriate call should be:

[/color]

filter.update(gx, gy, gz, ax, ay, az, -my, -mx, mz);

[color]

Am I wrong?[/color]

MichaelFBA is obviously confused, because the handedness for the accelerometer and gyro is not correct. You need to correct the handedness of the accel and gyro, and swap axes so that all the directions are consistent.

My suggestion is to use the magnetometer axes as the reference coordinate system, and since the filter assumes the North East Down (NED) coordinate system, the call would be

filter.update(-gx, -gy, -gz, -ax, -ay, -az, my, mx, -mz);

That way yaw = zero should have the magnetometer Y axis pointing to magnetic North and X pointing East. However, the signs of roll, pitch and yaw depend on how the quaternion is interpreted (there is no universal definition of a positive rotation angle).

I recommend the Mahony filter. It takes less code space and is significantly faster than the Madgwick filter. Here is the code I use to convert the Mahony quaternion to conventional navigation angles, which includes a correction for my magnetic declination:

// call arguments for LSM303DLHC IMU
  MahonyQuaternionUpdate(Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2],
                         Mxyz[0], Mxyz[1], Mxyz[2], deltat);
  // Define Tait-Bryan angles.
  // In this coordinate system, the positive z-axis is down toward Earth.
  // Yaw is the angle between Sensor x-axis and Earth magnetic North
  // (or true North if corrected for local declination, looking down on the sensor
  // positive yaw is counterclockwise, which is not conventional for NED navigation.
  // Pitch is angle between sensor x-axis and Earth ground plane, toward the
  // Earth is positive, up toward the sky is negative. Roll is angle between
  // sensor y-axis and Earth ground plane, y-axis up is positive roll. These
  // arise from the definition of the homogeneous rotation matrix constructed
  // from quaternions. Tait-Bryan angles as well as Euler angles are
  // non-commutative; that is, the get the correct orientation the rotations
  // must be applied in the correct order which for this configuration is yaw,
  // pitch, and then roll.
  // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
  // which has additional links.
  qw = q[0]; qx = q[1]; qy = q[2]; qz = q[3];
  roll  = atan2(2.0 * (qw * qx + qy * qz), 1.0 - 2.0 * (qx * qx + qy * qy));
  pitch = asin(2.0 * (qw * qy - qx * qz));
  yaw   = atan2(2.0 * (qx * qy + qw * qz), 1.0 - 2.0 * ( qy * qy + qz * qz));
  // to degrees
  yaw   *= 180.0 / PI;
  pitch *= 180.0 / PI;
  roll *= 180.0 / PI;

  // http://www.ngdc.noaa.gov/geomag-web/#declination
  //conventional nav, yaw increases CW from North, corrected for local magnetic declination
  yaw = -yaw + 14.9;
  if (yaw<0.0) yaw=yaw+360.0;
  if (yaw>360.0) yaw=yaw-360.0;

Be sure to calibrate the sensors! Let me know how it goes, as I don't have that sensor.