Madgwick IMU Lag Issues

Hello, I have a Nano 33 BLE and I am using the Madgwick IMU filter to get pitch and roll data (I don’t care about yaw for this so I am only using a gyro and accelerometer). I have tried a few different pieces of code and libraries but I can never solve my issue. My issue is that there is lag in the readings. For example, I’ll have my board flat at a pitch of 0° and then I’ll move it to 90°; but the reading slowly works up to 90° and after a second or more, it will finally settle at 90°. Same thing with roll. Here is my code and a link to a video of my serial plotter so you can see the lag. Does anyone know how to resolve this issue? Thank you.

// based off of Tom Igoe's version here :

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

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

void setup() {
  // 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:

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);

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

    // print the pitch and roll
    roll = filter.getRoll();
    pitch = filter.getPitch();
    Serial.print("Orientation: ");
    Serial.print(" ");

It is in the nature of digital filters that they have a delay. How much depends on the filter and its parameters e.g. order and the implementation.

How much do you know about digital filters?

Can you provide a link to the filter source code e.g. GitHub?

Do you have an idea about how much/little delay you would like to have for your application?

Thank you for the response. I know the basics of digital filters - I’ve had my run ins with Mr. Nyquist before - but I’m certainly not an expert. I’ve used Labview before to implement different high/low pass filters digitally rather than include the components on my bread board and I know it will slow it down. However, I am surprised how slow this IMU filter is moving.

Here is the link to library on GitHub: GitHub - arduino-libraries/MadgwickAHRS: Arduino implementation of the MadgwickAHRS algorithm and here is the algorithm’s creator’s page: Open source IMU and AHRS algorithms – x-io Technologies

I found my old project with a nano and an MPU6050 IMU. It uses a differrent library, but I’m pretty sure it’s still the Madgwick IMU algorithm (I’ll have to double check this). It’s updating its position with almost no lag (e.g. as soon as I flip it to 90 degrees, it’s showing 90 degrees). For my new project, I would definitely need the correct values to be displayed in less than a half second.

I’ll keep poking around the forums, and if I can’t get this to work, I’ll try a basic complementary filter or a kalman filter and just deal with possible gimbal lock.

I played a little with the example. I do not think there is a issue with lag. I can move the board and see real-time change in the Serial Plotter window. I can draw nice sine waves for two axis.

What I can see, is that when the board stays static in a vertical orientation the values will drift away and it will take a while to drift back to zero when I place the board back in a horizontal orientation. Maybe this is intentional and part of the magnetic distortion and bias drift compensation.

I would recommend you read the madgwick_internal_report.pdf to learn about the filter. The document looks interesting. If I have some time I will give it a go in the next couple of days.

Thanks for checking it out yourself. Yeah, I was mainly testing it by moving between a vertical and horizontal position so maybe that's what I'm seeing. I'll look more carefully at more orientations and I'll read up more on the report and bias drift compensation and see what I can learn.

This topic was automatically closed 120 days after the last reply. New replies are no longer allowed.