Using the algorithm of imuFilter with gyroscope(l3g20h) and accelerometer(lsm303) sensors

[brightproject]

Hello @RCmags I am rewriting the code to assemble it under the stm32f411ceu6 or BlackPill in the Arduino ecosystem.

I receive raw data, process it using the calibration function

calibrateSensor(gyroBias, accelBias);

I normalize the axes like this: ax = ((aix * ACCEL_RANGE) / ACCEL_SCALE); // 1G and

gx = ((gix * GYRO_RANGE) / GYRO_SCALE); // DPS
gx *= DEG_TO_RAD; // radians per second

After the filter, I multiply the orientation angles by 57.32 to get the angles(ROLL, PITCH, YAW) in degrees/second again. I output port data to the serial in a format that is understandable to the visualizer for processing. And most importantly, as a result, I do not see the “magic” of the Kalman filter. Orientation angles (yaw generally drifts probably half a degree per second, even the Majwick filter didn’t work that bad), roll also goes away quickly. Even with a coefficient

define GAIN 1.0

The algorithm doesn't want to calculate orientation angles based only on the accelerometer, it still uses a gyroscope, otherwise I can't explain the drift I see.

https://github.com/RCmags/imuFilter/assets/1788098/d650af09-ba42-42bd-a604-5146ab450850

and

https://github.com/RCmags/imuFilter/assets/1788098/072de865-9257-4229-832d-11f172a151d9

Code below

/*
 This sketch shows to initialize the filter and update it with the imu output. It also shows how to get the euler angles of the estimated heading orientation.
*/

/* NOTE: The accelerometer MUST be calibrated for the fusion to work. Adjust the 
   AX, AY, AND AZ offsets until the sensor reads (0,0,0) at rest. 
*/

#define AHRS_VERSION 13.1

#define PROCESSING
// #define DEBUG

#include <Wire.h>
// library Adafruit LSM303
#include <Adafruit_LSM303.h>
// library  L3DG20
#include <L3G.h>

// #include <basicMPU6050.h>       // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
    #include <imuFilter.h>            // https://github.com/RCmags/imuFilter/

/* // Gyro settings:
#define         LP_FILTER   3           // Low pass filter.                    Value from 0 to 6
#define         GYRO_SENS   0           // Gyro sensitivity.                   Value from 0 to 3
#define         ACCEL_SENS  0           // Accelerometer sensitivity.          Value from 0 to 3
#define         ADDRESS_A0  LOW         // I2C address from state of A0 pin.   A0 -> GND : ADDRESS_A0 = LOW
                                        //                                     A0 -> 5v  : ADDRESS_A0 = HIGH
// Accelerometer offset:
constexpr int   AX_OFFSET = 0;          // Use these values to calibrate the accelerometer. The sensor should output 1.0g if held level. 
constexpr int   AY_OFFSET = 0;          // These values are unlikely to be zero.
constexpr int   AZ_OFFSET = 0;

//-- Set the template parameters:

basicMPU6050<LP_FILTER,  GYRO_SENS,  ACCEL_SENS, ADDRESS_A0,
             AX_OFFSET,  AY_OFFSET,  AZ_OFFSET
            >imu; */

// =========== Settings ===========
imuFilter fusion;

#define G (9.80665)
#define DEG_TO_RAD (M_PI/180)
#define RAD_TO_DEG (180/M_PI)

#define GYRO_RANGE 2000.0f
// #define GYRO_SCALE 28571.4f
#define GYRO_SCALE 32768.0f
#define ACCEL_RANGE 8.0f
#define ACCEL_SCALE 32768.0f

#define STATUS_LED PC13
#define SERIAL_BAUD (115200)

#define GAIN          0.9    /* Fusion gain, value between 0 and 1 - Determines orientation correction with respect to gravity vector.If set to 1 the gyroscope is dissabled. If set to 0 the accelerometer is dissabled (equivant to gyro-only) */

#define SD_ACCEL      0.1     /* Standard deviation of acceleration. Accelerations relative to (0,0,1)g outside of this band are suppresed. Accelerations within this band are used to update the orientation. [Measured in g-force] */                          

#define FUSION        true    /* Enable sensor fusion. Setting to "true" enables gravity correction */

int16_t aix, aiy, aiz, gix, giy, giz;// raw
float ax, ay, az, gx, gy, gz, mx, my, mz;// G | deg/s | uGauss
float axX, ayY, azZ, gxX, gyY, gzZ, mxX, myY, mzZ;// m/s^2 | rad/s | microtesla (uT)

float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer

Adafruit_LSM303 lsm303;
// Adafruit_LSM303 lsm303(&Wire1);
L3G gyro;

void setup() {
  Serial.begin(SERIAL_BAUD);
  Wire.begin();

  Serial.print("AHRS ver: "); Serial.println(AHRS_VERSION, 1);

  pinMode(STATUS_LED, OUTPUT);  // Status LED
  digitalWrite(STATUS_LED, HIGH); // stm32f411 led OFF

  // Calibrate imu
  // imu.setup();
  // imu.setBias();

  // Adafruit LSM303
    initAccelMag();
    // L3DG20
    initGyro();

  // read accel + mag data
  lsm303.read();
  // read gyro data
  gyro.read();

  calibrateSensor(gyroBias, accelBias);

  #ifdef DEBUG

  Serial.print("gyroBias[0]: ");
  Serial.println(gyroBias[0]);
  Serial.print("gyroBias[1]: ");
  Serial.println(gyroBias[1]);
  Serial.print("gyroBias[2]: ");
  Serial.println(gyroBias[2]);

  Serial.print("accelBias[0]: ");
  Serial.println(accelBias[0]);
  Serial.print("accelBias[1]: ");
  Serial.println(accelBias[1]);
  Serial.print("accelBias[2]: ");
  Serial.println(accelBias[2]);

  delay(2000);

  #endif

  aix = lsm303.accelData.x - accelBias[0]; // RAW X
aiy = lsm303.accelData.y - accelBias[1]; // RAW Y
aiz = lsm303.accelData.z + accelBias[2]; // RAW Z

  ax = ((aix * ACCEL_RANGE) / ACCEL_SCALE); // G
  // ax *= G; // in 9.8 m/s/s
  ay = ((aiy * ACCEL_RANGE) / ACCEL_SCALE); // G
  // ay *= G; // in 9.8 m/s/s
  az = ((aiz * ACCEL_RANGE) / ACCEL_SCALE); // G
  // az *= G; // in 9.8 m/s/s

  #if FUSION
    // Set quaternion with gravity vector
    // fusion.setup( imu.ax(), imu.ay(), imu.az() );     
    fusion.setup( ax, -az, ay );     
  #else 
    // Just use gyro
    fusion.setup();                                   
  #endif
}

void loop() {

  // read accel + mag data
  lsm303.read();
  // read gyro data
  gyro.read();

  aix = lsm303.accelData.x - accelBias[0]; // RAW
    aiy = lsm303.accelData.y - accelBias[1]; // RAW
    aiz = lsm303.accelData.z + accelBias[2]; // RAW

  ax = ((aix * ACCEL_RANGE) / ACCEL_SCALE); // 1G
  // ax *= G; // in 9.8 m/s/s
  ay = ((aiy * ACCEL_RANGE) / ACCEL_SCALE); // 1G
  // ay *= G; // in 9.8 m/s/s
  az = ((aiz * ACCEL_RANGE) / ACCEL_SCALE); // 1G
  // az *= G; // in 9.8 m/s/s

  // Serial.print("ax: ");
  // Serial.println(ax);
  // Serial.print("ay: ");
  // Serial.println(ay);
  // Serial.print("az: ");
  // Serial.println(az);

    gix = gyro.g.x - gyroBias[0];// RAW
    giy = gyro.g.y - gyroBias[1];// RAW
    giz = gyro.g.z - gyroBias[2];// RAW

  gx = ((gix * GYRO_RANGE) / GYRO_SCALE); // DPS
  gx *=  DEG_TO_RAD; // radians per second
  gy = ((giy * GYRO_RANGE) / GYRO_SCALE); // DPS
  gy *=  DEG_TO_RAD; // radians per second
  gz = ((giz * GYRO_RANGE) / GYRO_SCALE); // DPS
  gz *=  DEG_TO_RAD; // radians per second

  #ifdef DEBUG
  Serial.print("aix: ");
  Serial.println(aix);
  Serial.print("aiy: ");
  Serial.println(aiy);
  Serial.print("aiz: ");
  Serial.println(aiz);
  // Serial.print("ax: ");
  // Serial.println(ax);
  // Serial.print("ay: ");
  // Serial.println(ay);
  // Serial.print("az: ");
  // Serial.println(az);
  Serial.print("gix: ");
  Serial.println(gix);
  Serial.print("giy: ");
  Serial.println(giy);
  Serial.print("giz: ");
  Serial.println(giz);
  // Serial.print("gx: ");
  // Serial.println(gx);
  // Serial.print("gy: ")
  // Serial.println(gy);
  // Serial.print("gz: ");
  // Serial.println(gz);
  delay(200);
  #endif

  // Update filter:

  #if FUSION
    /* NOTE: GAIN and SD_ACCEL are optional parameters */
    fusion.update( gx, -gz, -gy, ax, az, ay, GAIN, SD_ACCEL );  
  #else
    // Only use gyroscope
    fusion.update( imu.gx(), imu.gy(), imu.gz() );
  #endif

/*   // Display angles:
  Serial.print( fusion.pitch() );
  Serial.print( " " );
  Serial.print( fusion.yaw() );
  Serial.print( " " );
  Serial.print( fusion.roll() );

  // timestep
  Serial.print( " " );
  Serial.print( fusion.timeStep(), 6 );

  Serial.println();  */

    #ifdef PROCESSING

    Serial.print("Orientation: ");
    Serial.print(fusion.pitch() * RAD_TO_DEG); // roll
    Serial.print(" ");
    Serial.print(fusion.roll() * RAD_TO_DEG); // pitch
    Serial.print(" ");
    Serial.println(fusion.yaw() * RAD_TO_DEG); // yaw

    #endif

}

bool initAccelMag()
// void initAccelMag()
{

    bool successAccelMag = false;

    if (!lsm303.begin())
    {
    successAccelMag = false;
    #ifdef DEBUG
    Serial.println("Failed to autodetect accel + mag LSM303!");
    #endif
    while (1);
    } else {
    successAccelMag = true;
    #ifdef DEBUG
    Serial.println("Accel + mag LSM303 FOUND!");
    #endif
    }

  lsm303.enable_temperatureSensor();
  // tempRaw = lsm303.get_temperatureData();
    // lsm303.setMagGain(Adafruit_LSM303::LSM303_MAGGAIN_1_3);
    lsm303.setMagGain(Adafruit_LSM303::LSM303_MAGGAIN_8_1);
    // 0x08 = 0b00001000 2g
    // lsm303.write8(LSM303_ADDRESS_ACCEL, Adafruit_LSM303::LSM303_REGISTER_ACCEL_CTRL_REG4_A, 0x08);   
    // 0x18 = 0b00011000 4g
    // lsm303.write8(LSM303_ADDRESS_ACCEL, Adafruit_LSM303::LSM303_REGISTER_ACCEL_CTRL_REG4_A, 0x18); 
    // 0x28 = 0b00101000 8g
    lsm303.write8(LSM303_ADDRESS_ACCEL, Adafruit_LSM303::LSM303_REGISTER_ACCEL_CTRL_REG4_A, 0x28);
    // 0x38 = 0b00111000 16g
    // lsm303.write8(LSM303_ADDRESS_ACCEL, Adafruit_LSM303::LSM303_REGISTER_ACCEL_CTRL_REG4_A, 0x38);

    // 0 Boot - 00 HP-Filter - 0 filtered data selection - 0 HP filter enabled for int2 source - 0 HP filter enabled for int1 source -
    // 00 HP Filter Cut off Frequ
    // 0b00000000
    // lsm303.write8(LSM303_ADDRESS_ACCEL, Adafruit_LSM303::LSM303_REGISTER_ACCEL_CTRL_REG2_A, 0x00);
    // 0b10000100 - 0x84
    // lsm303.write8(LSM303_ADDRESS_ACCEL, Adafruit_LSM303::LSM303_REGISTER_ACCEL_CTRL_REG2_A, 0x84);
    // 0b11111111 - 0xF
    // lsm303.write8(LSM303_ADDRESS_ACCEL, Adafruit_LSM303::LSM303_REGISTER_ACCEL_CTRL_REG2_A, 0xFF);

    lsm303.enable_temperatureSensor();

    return successAccelMag;

}

bool initGyro()
// void initGyro()
{

    bool successGyro = false;

    if (!gyro.init())
    {
    successGyro = false;
    #ifdef DEBUG
    Serial.println("Failed to autodetect gyro L3GD20!");
    #endif
    while (1);
    } else {
    successGyro = true;
    #ifdef DEBUG
    Serial.println("Gyro L3DG20 FOUND!"); 
    #endif
    }
    // enableDefault
    // 0x5F = 0b01011111
    // DR = 01 (190 Hz ODR); BW = 01 (25 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)    
    gyro.enableDefault();
    // gyro.writeReg(L3G::CTRL_REG4, 0x00); // 250 dps full scale 0b0000000
    // gyro.writeReg(L3G::CTRL_REG4, 0x10); // 500 dps full scale 0b00100000
    gyro.writeReg(L3G::CTRL_REG4, 0x20); // 2000 dps full scale 0b00110000
    // gyro.writeReg(L3G::CTRL_REG1, 0x0F); // normal power mode, all axes enabled, ODR 95Hz, Cut-Off 12.5 Hz - 0b00001111

    // 0x6F = 0b01101111
    // DR = 01 (190 Hz ODR); BW = 10 (50 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
    // gyro.writeReg(L3G::CTRL_REG1, 0x6F);
    // 0xAF = 0b10101111
    // DR = 10 (380 Hz ODR); BW = 10 (50 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)  
    // gyro.writeReg(L3G::CTRL_REG1, 0xAF);
    // 0xBF = 0b10111111
    // DR = 10 (380 Hz ODR); BW = 11 (100 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
    gyro.writeReg(L3G::CTRL_REG1, 0xBF);

    return successGyro;

}

// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
void calibrateSensor(float * dest1, float * dest2)
{  
  uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
  uint16_t ii, packet_count, fifo_count;
  int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};

  // uint16_t  gyrosensitivity  = 131;   // = 131 LSB/degrees/sec
  // uint16_t  accelsensitivity = 16384;  // = 16384 LSB/g
  uint16_t  gyrosensitivity  = GYRO_RANGE;
  uint16_t  accelsensitivity = ACCEL_RANGE;

  fifo_count = 100;
  packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging

  for (ii = 0; ii < packet_count; ii++) {
    int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};

    // Form signed 16-bit integer for each sample in FIFO
    accel_temp[0] = (int16_t) lsm303.accelData.x;
    accel_temp[1] = (int16_t) lsm303.accelData.y;
    accel_temp[2] = (int16_t) lsm303.accelData.z;    
    gyro_temp[0]  = (int16_t) gyro.g.x;
    gyro_temp[1]  = (int16_t) gyro.g.y;
    gyro_temp[2]  = (int16_t) gyro.g.z;

    // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
    accel_bias[0] += (int32_t) accel_temp[0];
    accel_bias[1] += (int32_t) accel_temp[1];
    accel_bias[2] += (int32_t) accel_temp[2];
    gyro_bias[0]  += (int32_t) gyro_temp[0];
    gyro_bias[1]  += (int32_t) gyro_temp[1];
    gyro_bias[2]  += (int32_t) gyro_temp[2];

}
    // Normalize sums to get average count biases
    accel_bias[0] /= (int32_t) packet_count;
    accel_bias[1] /= (int32_t) packet_count;
    accel_bias[2] /= (int32_t) packet_count;
    gyro_bias[0]  /= (int32_t) packet_count;
    gyro_bias[1]  /= (int32_t) packet_count;
    gyro_bias[2]  /= (int32_t) packet_count;

  if(accel_bias[1] > 0L) {accel_bias[1] -= (int32_t) accelsensitivity;}  // Remove gravity from the z-axis accelerometer bias calculation
  else {accel_bias[1] += (int32_t) accelsensitivity;}

// Push gyro biases to hardware registers

  dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; // construct gyro bias in deg/s for later manual subtraction
  dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
  dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity;

// Output scaled accelerometer biases for manual subtraction in the main program
  dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; 
  dest2[1] = (float)accel_bias[1]/(float)accelsensitivity;
  dest2[2] = (float)accel_bias[2]/(float)accelsensitivity;
}

It would be great if you tried to run this code with similar sensors or others, for example icm20948 or bno055. Let me know if you need a graphic attitude indicator in the processing language, I will send it to you in an archive. I also plan to test the algorithm based on raw data from the gyroscope and accelerometer from the flight gear simulator, as I did here

https://forum.flightgear.org/viewtopic.php?f=36&t=42308

I would be glad for a little help.

[RCmags]

@brightproject Hi bright, I'm happy to help with the conversion of the code. Unfortunately, I don't have the hardware to help you develop the converted code, so at best I can only provide suggestions that might be helpful.

Upon reading your explanation, It's not clear to me whether you've been able to obtain clean, almost drift free outputs for the gyroscope and accelerometer. Please note that for the fusion algorithm to work, the angular velocity must be measured in radians/second, and the acceleration must be measured in g-units (dimensionless unit). In the case of acceleration, it must read |g| = 1.0 (magnitude of gravity) when the sensor is held perfectly level.

Moreover, the fusion algorithm will not correct for an uncalibrated angular velocity and acceleration. These must be calibrated manually before being fed through the filter.

Also (and very importantly), sometimes the orientation the IMU when it is held level will not correspond to the acceleration vector (0,0,1) which the fusion algorithm expects. You may have to swap the acceleration components to a different coordinate system to orient the vector properly. For example: (Ax, Ay, Az) -> (-Ax, Az, Ay)

Another important thing is the GAIN variable. This value should ideally not be very large. Think of it like the alpha parameter of an exponential moving average. If GAIN=1 the fusion output will just be the instantaneous acceleration vector.

---

Moving forward: For now, lets just focus on the output of your IMU to see if its compatible with the form expected by imuFilter.

[brightproject]

Collected data in the setup and loop section below

Accel + mag LSM303 FOUND!
Gyro L3DG20 FOUND!
ax: -64.00 ay: 4112.00 az: -432.00
gx: 108 gy: -35 gz: 62
gyroBias[0]: 0.05 gyroBias[1]: -0.02 gyroBias[2]: 0.03
accelBias[0]: -8.00 accelBias[1]: 513.00 accelBias[2]: -54.00
aix: -72 aiy: 3647 aiz: -454
ax: -0.02 ay: 0.89 az: -0.11
gix: 2 giy: 2 giz: -3
gx: 0.00 gy: 0.00 gz: -0.00
aix: -40 aiy: 3663 aiz: -454
ax: -0.01 ay: 0.89 az: -0.11
gix: -4 giy: -2 giz: 3
gx: -0.00 gy: -0.00 gz: 0.00
aix: -56 aiy: 3663 aiz: -406
ax: -0.01 ay: 0.89 az: -0.10
gix: -13 giy: 7 giz: 0
gx: -0.01 gy: 0.01 gz: 0.00
aix: -24 aiy: 3615 aiz: -470
ax: -0.01 ay: 0.88 az: -0.11
gix: 7 giy: -7 giz: 6
gx: 0.01 gy: -0.01 gz: 0.01
aix: -40 aiy: 3631 aiz: -470
ax: -0.01 ay: 0.89 az: -0.11
gix: -2 giy: 0 giz: 3
gx: -0.00 gy: 0.00 gz: 0.00
aix: -72 aiy: 3631 aiz: -438
ax: -0.02 ay: 0.89 az: -0.11
gix: 5 giy: -3 giz: -9
gx: 0.01 gy: -0.00 gz: -0.01
aix: -56 aiy: 3615 aiz: -422
ax: -0.01 ay: 0.88 az: -0.10
gix: -8 giy: -7 giz: -2
gx: -0.01 gy: -0.01 gz: -0.00

Acceleration values ​​are both in raw values ​​and in units of G. Angular velocities are also in raw values ​​and in rad/sec.

define GYRO_RANGE 2000.0f

define ACCEL_RANGE 8.0f

You wrote that gyroscopes should be set in degrees per second - is this probably a mistake?

[RCmags]

Ouff! Sorry!!!! Here I am trying to help and I made a mistake. Yes, I meant radians/second!!!

---

That embarrassment aside, those accelerometer values seem to be miscalibrated. The magnitude of the vector ~(ax: -0.01 ay: 0.89 az: -0.11) is approximately ~0.9.

This filter requires the resting acceleration be exactly 1 in magnitude, as it uses any deviation from this value Error = Accel_Mag - 1 to determine whether to apply a correction or not. In this case, you should manually calibrate the accelerometer to achieve 1g in magnitude indifferent of how its rotated when the sensor is at rest.

The SD_ACCEL parameter of fusion.update( gx, -gz, -gy, ax, az, ay, GAIN, SD_ACCEL ); determines whether the acceleration vector will update the heading, if Error < SD_ACCEL. Notice how if the acceleration is not calibrated and normalized, the error will be larger than the allowed tolerance.

---

Next step: After calibrating the accelerometer, how does the fusion algorithm react to the gyroscope and accelerometer values in isolation? try:

fusion.update( 0, 0, 0, ax, az, ay, GAIN, SD_ACCEL );

and

fusion.update( gx, -gz, -gy );

If the accelerometer and gyroscope axes are aligned properly, the acceleration should slowly converge towards the heading estimated by the gyroscope. If the acceleration by itself causes the heading output to diverge then swap the sign of one of the acceleration components.