Arduino Nano 33 BLE orientation computation using LSM9DS1 and Madgwick Filter

[niklasoswald]

Hey,

I'm trying to compute the orientation of my Arduino Nano 33 BLE using it's on-chip LSM9DS1 IMU. I try to use the Arduino Madgwick Filter Library. I update the filter using the values which I get from the accelerometer, gyroscope and magnetometer using the Arduino LSM9DS1 Library.
However, when rotating the Nano (e.g. 45° pitch) it takes a pretty long time until the measured euler angles from the Madgwick Filter settle at the given angle.
So generally speaking, I'm searching for a way to improve the response time of the Madgwick Filter or to fix my implementation in case there is something I'm totally doing wrong here.
Here is my code:

#include <MadgwickAHRS.h>
#include <Arduino_LSM9DS1.h>

float gForceX, gForceY, gForceZ; 
float rateRoll, ratePitch, rateYaw;
float mX, mY, mZ;
float gyroXOffset, gyroYOffset, gyroZOffset;
float accXOffset, accYOffset, accZOffset;
float magXOffset, magYOffset, magZOffset;

Madgwick filter;
unsigned long microsPerReading, microsPrevious;
unsigned long microsNow;

void calibrate();

void setup() {
  Serial.begin(9600);
  while (!Serial);

  gyroXOffset = gyroYOffset = gyroZOffset = 0;
  accXOffset = accYOffset = accZOffset = 0;
  magXOffset = magYOffset = magZOffset = 0;

  if (!IMU.begin()) {
    Serial.println("Failed to initialize IMU.");
    while (true);   // Stop execution
  }

  calibrate();

  filter.begin(119);
  Serial.println(IMU.accelerationSampleRate());
  Serial.println(IMU.gyroscopeSampleRate());
  Serial.println(IMU.magneticFieldSampleRate());

  microsPerReading = 1000000 / 119;
  microsPrevious = micros();
}

void loop() {
  microsNow = micros();
  if (microsNow - microsPrevious >= microsPerReading) {
  // put your main code here, to run repeatedly:
      IMU.readAcceleration(gForceX, gForceY, gForceZ);
      IMU.readGyroscope(rateRoll, ratePitch, rateYaw);
      IMU.readMagneticField(mX, mY, mZ);

      gForceX -= accXOffset;
      gForceY -= accYOffset;
      gForceZ -= accZOffset;
      rateRoll -= gyroXOffset;
      ratePitch -= gyroYOffset;
      rateYaw -= gyroZOffset;
      mX -= magXOffset;
      mY -= magYOffset;
      mZ -= magZOffset;

      filter.update(rateRoll, ratePitch, rateYaw, gForceX, gForceY, gForceZ, mX, mY, mZ);

      Serial.print(filter.getRoll());
      Serial.print(",");
      Serial.print(filter.getPitch());
      Serial.print(",");
      Serial.println(filter.getYaw());
    
      microsPrevious = microsPrevious + microsPerReading;
  }
}

void calibrate() {
  Serial.println("Calibrating Gyroscope ...");

    int numSamples = 2000;

    for (int i = 0; i < numSamples; i++) {
        IMU.readGyroscope(rateRoll, ratePitch, rateYaw);
        gyroXOffset += rateRoll;
        gyroYOffset += ratePitch;
        gyroZOffset += rateYaw;
        delay(1);
    }

    gyroXOffset /= numSamples;
    gyroYOffset /= numSamples;
    gyroZOffset /= numSamples;

    Serial.println("Gyroscope Calibration completed.");

    Serial.println("Calibrating Accelerometer ...");

    for (int j = 0; j < numSamples; j++) {
        IMU.readAcceleration(gForceX, gForceY, gForceZ);
        accXOffset += gForceX;
        accYOffset += gForceY;
        accZOffset += gForceZ;
        delay(1);
    }

    accXOffset /= numSamples;
    accYOffset /= numSamples;
    accZOffset = (accZOffset / numSamples) - 1;

    Serial.println("Accelerometer Calibration completed.");

    Serial.println("Calibrating Magnetometer ...");

    numSamples = 5000;
    float mMinX, mMinY, mMinZ;
    float mMaxX, mMaxY, mMaxZ;

    mMinX = mMinY = mMinZ = 1e6;
    mMaxX = mMaxY = mMaxZ = -1e6;

    for (int k = 0; k < numSamples; k++) {
        IMU.readMagneticField(mX, mY, mZ);

        if (mX < mMinX) mMinX = mX;
        if (mY < mMinY) mMinY = mY;
        if (mZ < mMinZ) mMinZ = mZ;

        if (mX > mMaxX) mMaxX = mX;
        if (mY > mMaxY) mMaxY = mY;
        if (mZ > mMaxZ) mMaxZ = mZ;

        delay(1);
    }

    magXOffset = (mMinX + mMaxX) / 2;
    magYOffset = (mMinY + mMaxY) / 2;
    magZOffset = (mMinZ + mMaxZ) / 2;

    Serial.println("Magnetometer calibration completed.");
}

[jremington]

For any AHRS fusion filter to work, the gyro, magnetometer and the accelerometer have to use the same, right handed coordinate system, and be properly calibrated.

The manufacturer-assigned coordinate systems of the LSM9DS1 are inconsistent and need to be corrected.

After fixing that, I suggest to learn about better ways of calibrating the sensors. Working examples can be found at GitHub - jremington/LSM9DS1-AHRS: Mahony 3D fusion filter and tilt compensated compass, with sensor calibration code

[sonofcy]

@niklasoswald

pretty long time

With an 8 sec delay I don't doubt it.

[niklasoswald]

@sonofcy

With an 8 sec delay I don't doubt it.

Not quite. Note that I'm using micros and not millis.
This means that I'm reading the values every
8403 microseconds = 8.4 milliseconds = 0.0084 seconds.
This should be fine, I guess.

Still thanks for your reply

[jremington]

FYI, from the LSM9DS1 data sheet. Note that the manufacturer-assigned accel/gyro axes form a left handed coordinate system.

This means that I'm reading the values every
8403 microseconds = 8.4 milliseconds = 0.0084 seconds.

Probably not. At a UART serial baud rate of 9600(*), it takes one millisecond to print each character. Work out how long it takes to print the following, then again for a more reasonable serial baud rate of 115200.

      Serial.print(filter.getRoll());
      Serial.print(",");
      Serial.print(filter.getPitch());
      Serial.print(",");
      Serial.println(filter.getYaw());

Since the AHRS update loop needs to run very quickly for accurate integration, calculate and use or print out Euler angles only when necessary.

(*) it may be that the Nano 33 BLE uses native USB for the PC Serial port, in which case baud rates are irrelevant.

[niklasoswald]

Hey jremington,

thanks for already helping me out with the Madgwick-Filter.
However, I'm simply not achieving the destined behavior.
That's why I decided to try out your implementation of the Mahony filter (https://github.com/jremington/LSM9DS1-AHRS/blob/main/Mahony_AHRS/MahonyUW_AHRS.ino). Here's my implementation:

#include <Arduino_LSM9DS1.h>

float S_mag[3][3] = {
  {1.281967, 0.049886, -0.017155},
  {0.049886, 1.427994, -0.008333},
  {-0.017155, -0.008333, 1.252374}
};

float h_mag[3] = {
  8.570474, 47.349507, 52.219895
};

float S_acc[3][3] = {
  {1.002586, -0.001156, 0.002170},
  {-0.001156, 1.002056, -0.000748},
  {0.002170, -0.000748, 1.001003}
};

float h_acc[3] = {
  -0.031949, -0.003886, -0.006204
};

// These are the free parameters in the Mahony filter and fusion scheme,
// Kp for proportional feedback, Ki for integral
// Kp is not yet optimized. Ki is not used.
#define Kp 50.0
#define Ki 0.0

unsigned long now = 0, last = 0; //micros() timers for AHRS loop
float deltat = 0;  //loop time in seconds

#define PRINT_SPEED 300 // ms between angle prints
unsigned long lastPrint = 0; // Keep track of print time

// Vector to hold quaternion
static float q[4] = {1.0, 0.0, 0.0, 0.0};
static float yaw, pitch, roll; //Euler angle output

float gForceX, gForceY, gForceZ; 
float rateRoll, ratePitch, rateYaw;
float mX, mY, mZ;
float gyroXOffset, gyroYOffset, gyroZOffset;

void scaleIMU();
void calibrate();
void setupFilter();
void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat);

void setup() {
  Serial.begin(115200);
  while (!Serial);

  gyroXOffset = gyroYOffset = gyroZOffset = 0;

  if (!IMU.begin()) {
    Serial.println("Failed to initialize IMU.");
    while (true);   // Stop execution
  }
  calibrate();
}

void loop() {
  if (IMU.accelerationAvailable() && IMU.gyroscopeAvailable() && IMU.magneticFieldAvailable()) {
    IMU.readAcceleration(gForceX, gForceY, gForceZ);
    IMU.readGyroscope(rateRoll, ratePitch, rateYaw);
    IMU.readMagneticField(mX, mY, mZ);

    scaleIMU();

    // correct accel/gyro handedness
    // Note: the illustration in the LSM9DS1 data sheet implies that the magnetometer
    // X and Y axes are rotated with respect to the accel/gyro X and Y, but this is 
    // not the case if Acc X axis is flipped to fix the handedness

    gForceX = -gForceX; //fix accel/gyro handedness
    rateRoll = -rateRoll; //must be done after offsets & scales applied to raw data
    
    now = micros();
    deltat = (now - last) * 1.0e-6; // seconds since last update
    last = now;

    MahonyQuaternionUpdate(gForceX, gForceY, gForceZ, rateRoll, ratePitch, rateYaw, mX, mY, mZ, deltat);

    if (millis() - lastPrint > PRINT_SPEED) {
      roll  = atan2((q[0] * q[1] + q[2] * q[3]), 0.5 - (q[1] * q[1] + q[2] * q[2]));
      pitch = asin(2.0 * (q[0] * q[2] - q[1] * q[3]));
      yaw   = atan2((q[1] * q[2] + q[0] * q[3]), 0.5 - ( q[2] * q[2] + q[3] * q[3]));

      // to degrees
      yaw   *= 180.0 / PI;
      pitch *= 180.0 / PI;
      roll *= 180.0 / PI;

      if (yaw < 0) yaw += 360.0;
      if (yaw >= 360.0) yaw -= 360.0;

      Serial.print(roll);
      Serial.print(",");
      Serial.print(pitch);
      Serial.print(",");
      Serial.println(yaw);

      lastPrint = millis();
    }
  }
}

void scaleIMU() {
  // Subtract accelerometer and gyroscope offsets
  rateRoll -= gyroXOffset;
  ratePitch -= gyroYOffset;
  rateYaw -= gyroZOffset;

  float temp[3] = {gForceX - h_acc[0], gForceY - h_acc[1], gForceZ - h_acc[2]};
  gForceX = S_acc[0][0] * temp[0] + S_acc[0][1] * temp[1] + S_acc[0][2] * temp[2];
  gForceY = S_acc[1][0] * temp[0] + S_acc[1][1] * temp[1] + S_acc[1][2] * temp[2];
  gForceZ = S_acc[2][0] * temp[0] + S_acc[2][1] * temp[1] + S_acc[2][2] * temp[2];

  // normalize accelerometer values
  float mag = sqrt(gForceX*gForceX + gForceY*gForceY + gForceZ*gForceZ);
  gForceX /= mag;
  gForceY /= mag;
  gForceZ /= mag;

  // apply hard and soft iron offsets
  temp[0] = mX - h_mag[0];
  temp[1] = mY - h_mag[1];
  temp[2] = mZ - h_mag[2];
  mX = S_mag[0][0] * temp[0] + S_mag[0][1] * temp[1] + S_mag[0][2] * temp[2];
  mY = S_mag[1][0] * temp[0] + S_mag[1][1] * temp[1] + S_mag[1][2] * temp[2];
  mZ = S_mag[2][0] * temp[0] + S_mag[2][1] * temp[1] + S_mag[2][2] * temp[2];

  // normalize magnetometer values
  mag = sqrt(mX*mX + mY*mY + mZ*mZ);
  mX /= mag;
  mY /= mag;
  mZ /= mag;
}

void calibrate() {
  Serial.println("Calibrating Gyroscope ...");

    int numSamples = 2000;

    for (int i = 0; i < numSamples; i++) {
        IMU.readGyroscope(rateRoll, ratePitch, rateYaw);
        gyroXOffset += rateRoll;
        gyroYOffset += ratePitch;
        gyroZOffset += rateYaw;
        delay(1);
    }

    gyroXOffset /= numSamples;
    gyroYOffset /= numSamples;
    gyroZOffset /= numSamples;

    Serial.println("Gyroscope Calibration completed.");
}

// Mahony orientation filter, assumed World Frame NWU (xNorth, yWest, zUp)
// Modified from Madgwick version to remove Z component of magnetometer:
// The two reference vectors are now Up (Z, Acc) and West (Acc cross Mag)
// sjr 3/2021
// input vectors ax, ay, az and mx, my, mz MUST be normalized!
// gx, gy, gz must be in units of radians/second
//
void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat)
{
  // Vector to hold integral error for Mahony method
  static float eInt[3] = {0.0, 0.0, 0.0};
    // short name local variable for readability
  float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];
  float norm;
  float hx, hy, hz;  //observed West horizon vector W = AxM
  float ux, uy, uz, wx, wy, wz; //calculated A (Up) and W in body frame
  float ex, ey, ez;
  float pa, pb, pc;

  // Auxiliary variables to avoid repeated arithmetic
  float q1q1 = q1 * q1;
  float q1q2 = q1 * q2;
  float q1q3 = q1 * q3;
  float q1q4 = q1 * q4;
  float q2q2 = q2 * q2;
  float q2q3 = q2 * q3;
  float q2q4 = q2 * q4;
  float q3q3 = q3 * q3;
  float q3q4 = q3 * q4;
  float q4q4 = q4 * q4;

  // Measured horizon vector = a x m (in body frame)
  hx = ay * mz - az * my;
  hy = az * mx - ax * mz;
  hz = ax * my - ay * mx;
  // Normalise horizon vector
  norm = sqrt(hx * hx + hy * hy + hz * hz);
  if (norm == 0.0f) return; // Handle div by zero

  norm = 1.0f / norm;
  hx *= norm;
  hy *= norm;
  hz *= norm;

  // Estimated direction of Up reference vector
  ux = 2.0f * (q2q4 - q1q3);
  uy = 2.0f * (q1q2 + q3q4);
  uz = q1q1 - q2q2 - q3q3 + q4q4;

  // estimated direction of horizon (West) reference vector
  wx = 2.0f * (q2q3 + q1q4);
  wy = q1q1 - q2q2 + q3q3 - q4q4;
  wz = 2.0f * (q3q4 - q1q2);

  // Error is the summed cross products of estimated and measured directions of the reference vectors
  // It is assumed small, so sin(theta) ~ theta IS the angle required to correct the orientation error.

  ex = (ay * uz - az * uy) + (hy * wz - hz * wy);
  ey = (az * ux - ax * uz) + (hz * wx - hx * wz);
  ez = (ax * uy - ay * ux) + (hx * wy - hy * wx);
 
  if (Ki > 0.0f)
  {
    eInt[0] += ex;      // accumulate integral error
    eInt[1] += ey;
    eInt[2] += ez;
    // Apply I feedback
    gx += Ki * eInt[0];
    gy += Ki * eInt[1];
    gz += Ki * eInt[2];
  }


  // Apply P feedback
  gx = gx + Kp * ex;
  gy = gy + Kp * ey;
  gz = gz + Kp * ez;

 //update quaternion with integrated contribution
 // small correction 1/11/2022, see https://github.com/kriswiner/MPU9250/issues/447
  gx = gx * (0.5*deltat); // pre-multiply common factors
  gy = gy * (0.5*deltat);
  gz = gz * (0.5*deltat);
  float qa = q1;
  float qb = q2;
  float qc = q3;
  q1 += (-qb * gx - qc * gy - q4 * gz);
  q2 += (qa * gx + qc * gz - q4 * gy);
  q3 += (qa * gy - qb * gz + q4 * gx);
  q4 += (qa * gz + qb * gy - qc * gx);

  // Normalise quaternion
  norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
  norm = 1.0f / norm;
  q[0] = q1 * norm;
  q[1] = q2 * norm;
  q[2] = q3 * norm;
  q[3] = q4 * norm;
}

Unfortunately, I'm getting absolute non-sense values.
I'm calibrating my accelerometer and magnetometer using magneto, just like you recommended. Here are two graphics displaying the calibration results.

According to this, I'd say that the problem is not due to lacking calibration.
I'd highly appreciate you if you could help me out.
Note, that I'm not using the Ardafruit LSM9DS1 Breakout Board but the LSM9DS1 mounted on the Arduino Nano 33 BLE.

Thanks in advance!

Cheers,
Nik

[jremington]

The code is a bit difficult to follow, given the arbitrary renaming of so many variables, but nothing jumps out at me.

Check that the axial assignments for the magnetometer and accelerometer are correct by printing out the calibrated, normalized and corrected values for each axis, when that axis is aligned along the direction of maximum field strength.

The reaction force to gravity points straight up (positive), and the Earth's magnetic field points North and down in the northern hemisphere, North and up in the southern hemisphere. The value should be about 1.0 when the axis is properly aligned.

Remember that after flipping the acc/gyro X axes, the positive X direction will be opposite the direction indicated in the manufacturer's illustration (post #6).