Fixing Drift Issues w/ Sensor Fusion (MadgwickAHRS) on an Arduino Uno

[define-private-public]

I'm still working on the MEGA to get access to the DMP, but in the meanwhile, I'm also interested in having a working orientation sensor using an Arduino Uno. I'm using the (MadgwickAHRS)[https://github.com/arduino-libraries/MadgwickAHRS] to perform the sensor fusion. I'm having some issues where the orientation drifts over time.

What can I do to remedy this?

So far this is my code:

// This is meant to be nothing more than a sketch that dumps data on the serial port
// for the python application to show/visualize.
//
// For the actual application:
//   - This should use the DMP on the chip for Sensor fusion instead of the Madgwick code
//     - Probably can't use Arduino then anymore, since the Sparkfun code breaks the Memory limits of the Arduino UNO chip
//   - Gyroscope sensitivity probably needs to be adjusted to be a little less sensitive (giving a greater DPS range)
//   - Should properly use the interrupt pin
//
// Communication protocol:  This print strings on the Serial port, at the rate of 115200 baud; it never reads data
//   It might print out status messages like "waiting for data"
//   Actual orientation data will follow the format:
//
//     HPR <heading/yaw deg> <pitch deg> <roll deg>
//
//   For example:
//
//     HPR 5.5 -10.07773 0.22000023
//

#include <Arduino.h>
#include <Wire.h>
#include <ICM_20948.h>
#include "MadgwickAHRS.h"

#define SERIAL_BAUD_RATE 115200
#define I2C_RATE 400000
#define IMU_TIMEOUT_MS 500                                                  // When there was a failure in talking to the IMU (or no data ready), how many milliseconds should we wait?
#define IMU_DATA_READING_WAIT_MS 25                                         // How many milliseconds to wait before attempting the next data reading
#define IMU_FUSION_FREQUENCY (1000.0f / float(IMU_DATA_READING_WAIT_MS))    // The sensor fusion algorithm needs to know how often the IMU is being read from
#define SERIAL_PRECISION 12                                                 // How many digits of the float to print to Serial

// TODO research this better
#define AD0_VAL   1     // The value of the last bit of the I2C address.
                        // On the SparkFun 9DoF IMU breakout the default is 1, and when
                        // the ADR jumper is closed the value becomes 0

// Using I2C to connect to the imu
ICM_20948_I2C imu;

// For orientation calculation
Madgwick filter;
float heading, pitch, roll;     // The orientation

void setup() {
    // Setup communications
    Serial.begin(SERIAL_BAUD_RATE);     // To computer
    Wire.begin();                       // To IMU
    Wire.setClock(I2C_RATE);

//    imu.enableDebugging();

    // Try to enable the IMU
    bool connected = false;
    while (!connected) {
        imu.begin(Wire, AD0_VAL);

        // Print status to serial
        Serial.print("Init connection to ICM 20948: ");
        Serial.println(imu.statusString());

        // If it didn't work, try again shortly
        if (imu.status != ICM_20948_Stat_Ok) {
            Serial.println("Try again in 500 ms");
            delay(IMU_TIMEOUT_MS);
        } else
            connected = true;
    }

    Serial.println("Connected to ICM 20948");

    // Setup filter
    filter.begin(IMU_FUSION_FREQUENCY);
}

void loop() {
    if (imu.dataReady()) {
        // Some data is ready
        imu.getAGMT();          // Retrieves data

        // Update the sensor fusion state
        filter.update(
            imu.gyrX(), imu.gyrY(), imu.gyrZ(),     // These units might be in degrees and not radiasn
            imu.accX(), imu.accY(), imu.accZ(),
            imu.magX(), imu.magY(), imu.magZ()
        );

        // Read data
        heading = filter.getYaw();
        pitch = filter.getPitch();
        roll = filter.getRoll();

        // Send Orientation data to the PC
        Serial.print("HPR ");                       // HPR = Heading, Pitch, Roll
        Serial.print(heading, SERIAL_PRECISION);
        Serial.print(" ");
        Serial.print(pitch, SERIAL_PRECISION);
        Serial.print(" ");
        Serial.print(roll, SERIAL_PRECISION);
        Serial.println("");     // End the line reading

        // Wait a little before the next reading
        delay(IMU_DATA_READING_WAIT_MS);
    } else {
        // Nothing, pause for a moment
        Serial.println("Waiting for data...");
        delay(IMU_TIMEOUT_MS);
    }
}

I made a 3D model of the breakout board and a Python (w/ Panda3D) visualization:

https://user-images.githubusercontent.com/4551425/118917371-be5e7f00-b8fe-11eb-9848-9c2d432bdd53.mov

The parts where the movement jerks fast is actually where I move the board. The parts where the rotation is linear is actually drift. I've before had the board completely stationary but then seen it drift slowly at the rate of about 2 deg / second, for minutes on end. I really want to eliminate this drift when using an Uno

[PaulZC]

Hi Benjamin (@define-private-public ), This is the wrong place to ask questions about other libraries. Please try asking your question in the Inertial Measurement Units Forum. Best wishes, Paul

[define-private-public]

Hi,

I've reached out on the IMU forum ( https://forum.sparkfun.com/viewtopic.php?f=83&t=55764 ), still trying to get that to work. It's better now but there are still issues. I've uploaded all of my code (Arduino & Python visualization app) here:

https://github.com/define-private-public/ICM-20948_vizualization

I have a video it in action here:

https://www.youtube.com/watch?v=OrI-3uFmiHM

I understand that this repo is strictly about the ICM-20948, but I'm still fairly a newbie when it comes to IMUs. I'm wondering if there is any other insight you might be able to give me regarding the issues I'm still experiencing.

[PaulZC]

HI Benjamin (@define-private-public ),

Sorry - I cannot give you any help with the MadgwickAHRS library, I have never used it before.

Best wishes, Paul