New Madgwick Fusion Filter

mjs513

Senior Member+
While updating my version of Fabio Versano's FreeIMU library I was looking for some of Madgwick's old papers and came across the fact the Madgwick changed what we have known as MadgwichAHRS. This occurred with the publication of his PhD thesis: AHRS algorithms and calibration solutions to facilitate new applications using low-cost MEMS and available on Github: Fusion. Its a quite a bit different than our current version of his AHRS algorithm. This is from the readME:
The algorithm calculates the orientation as the integration of the gyroscope summed with a feedback term. The feedback term is equal to the error in the current measurement of orientation as determined by the other sensors, multiplied by a gain. The algorithm therefore functions as a complementary filter that combines high-pass filtered gyroscope measurements with low-pass filtered measurements from other sensors with a corner frequency determined by the gain. A low gain will 'trust' the gyroscope more and so be more susceptible to drift. A high gain will increase the influence of other sensors and the errors that result from accelerations and magnetic distortions. A gain of zero will ignore the other sensors so that the measurement of orientation is determined by only the gyroscope.

I have a stripped-down version of the library at: https://github.com/mjs513/Fusion/tree/Fusion-Teensy. There are 2 additional examples:
1. FreeIMU_Madgwick_Fusion: Used with my FreeIMU library and
2. NXPFusion_Madgwick_Fusion: Used with NXPMotionSense library.

As before the Fusion library requires calibrated data to be supplied to the algorithm.

One thing I noticed with preliminary testing with the propshield using the Fusion algorithm or the MadgwickAHRS algorithm is that the new Fusion library has just about 0 yaw drif where the old version (using the NXPMotion example) youdo see alot of yaw drifting.

I will try to post some comparison data later.
 
Okay, thanks. Does that mean files were moved or deleted, but not edited?

Files were just deleted between the main branch and the teensy/arduino branch, i.e., here is a screen shot of the main branch:
Capture3.PNG
and my branch
Capture4.PNG

MainBranch Fusion -> TeensyBranch src
Examples -> Examples but added 2 additional branches for the propshield, 1 using the NXPMotionSense library and the other FreeIMU library which I play with alot

All other files or directories in the Main Branch were deleted in my branch.

Also in the teensy branch added a library.properties file
 
Been playing a bit with Fusion using NXPMotionSense lib and FreeIMU Lib. In both cases I started with calibrating using either MotionCal for NXPMotionSense or the FreeIMU Cal GUI:
Capture2.PNG from MotionCAL

and FreeIMU GUI
Capture.PNGCapture1.PNG

For Yaw only which always is a problem child you see that with FUSION it converges faster to a steady state solution.
NXPMotionSense:
NXPMotionSense.PNG

FreeIMU:
FreeIMU.PNG

EDIT: Attached are the NXPMotionSense examples I used for testing.
View attachment MadgwickIMU-230418a.zip

View attachment NXPFusion_Madgwick_Fusion-230418a.zip
 
Last edited:
@brtaylor

Just gave it a test spin using your old MPU-9250 library which has the calibration and FIFO functions still in it (https://github.com/mjs513/invensense-imu/tree/MPU-9250_custom) along with @DonKelly calibration sketch. The sketch I used for fusion looks like this:
Code:
#include "MPU9250.h"
#include "Fusion.h"
#include <stdbool.h>
#include <stdio.h>

#include <Wire.h>
#include "EEPROM.h"
#include "Streaming.h"

#define cout Serial

uint64_t prev_time, cur_time;
float dt;
elapsedMillis dump;

#define SAMPLE_PERIOD (0.01f) // replace this with actual sample period
float g = 9.80665;
float rads2degs = 57.296;

FusionOffset offset;
FusionAhrs ahrs;


// an MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68
MPU9250 IMU(Wire,0x68);
int status;

void setup() {
  // serial to display data
  Serial.begin(115200);
  while(!Serial) {}

  // start communication with IMU 
  status = IMU.begin();
  if (status < 0) {
    Serial.println("IMU initialization unsuccessful");
    Serial.println("Check IMU wiring or try cycling power");
    Serial.print("Status: ");
    Serial.println(status);
    while(1) {}
  }

  cout.println("IMU Connected!");
  loadCal();

  // setting the accelerometer full scale range to +/-8G 
  IMU.setAccelRange(MPU9250::ACCEL_RANGE_2G);
  // setting the gyroscope full scale range to +/-500 deg/s
  IMU.setGyroRange(MPU9250::GYRO_RANGE_2000DPS);
  // setting DLPF bandwidth to 20 Hz
  IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ);
  // setting SRD to 19 for a 50 Hz update rate
  IMU.setSrd(19);
  IMU.calibrateGyro();
  
     FusionAhrsInitialise(&ahrs);

    // Set AHRS algorithm settings
    const FusionAhrsSettings settings = {
            .convention = FusionConventionNed,
            .gain = 0.5f,
            .accelerationRejection = 10.0f,
            .magneticRejection = 20.0f,
            .rejectionTimeout = 5 * SAMPLE_PERIOD, /* 5 seconds */
    };
    FusionAhrsSetSettings(&ahrs, &settings);
  
}

void loop() {
  float val[10];

  // read the sensor
  cur_time = micros();
  IMU.readSensor(val);

        // Acquire latest sensor data
        FusionVector gyroscope = {val[3]*rads2degs, val[4]*rads2degs, val[5]*rads2degs}; // replace this with actual gyroscope data in degrees/s
        FusionVector accelerometer = {val[0]/g, val[1]/g, val[2]/g}; // replace this with actual accelerometer data in g
        FusionVector magnetometer = {val[6], val[7], val[8]}; // replace this with actual magnetometer data in arbitrary units

        gyroscope = FusionOffsetUpdate(&offset, gyroscope);
        // Calculate delta time (in seconds) to account for gyroscope sample clock error
        dt = (cur_time - prev_time)/1e6;
        FusionAhrsUpdate(&ahrs, gyroscope, accelerometer, magnetometer, dt);
        prev_time = cur_time;

        const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));
        if(dump > 1000) {
          //print_float_array(val, 10);
          //Serial.println();
          Serial.printf("Fusion (RPY): %0.1f, %0.1f, %0.1f\n", euler.angle.roll, euler.angle.pitch, euler.angle.yaw);
          dump = 0;
        }
 
}

void loadCal() {
  // EEPROM buffer and variables to load accel and mag bias
  // and scale factors from CalibrateMPU9250.ino
  uint8_t EepromBuffer[48];
  float a_xb, a_xs, a_yb, a_ys, a_zb, a_zs;
  float hxb, hxs, hyb, hys, hzb, hzs;

 // load and set accel and mag bias and scale
  // factors from CalibrateMPU9250.ino
  for (size_t i = 0; i < sizeof(EepromBuffer); i++) {
    EepromBuffer[i] = EEPROM.read(i);
  }
  memcpy(&a_xb, EepromBuffer + 0, 4);
  memcpy(&a_xs, EepromBuffer + 4, 4);
  memcpy(&a_yb, EepromBuffer + 8, 4);
  memcpy(&a_ys, EepromBuffer + 12, 4);
  memcpy(&a_zb, EepromBuffer + 16, 4);
  memcpy(&a_zs, EepromBuffer + 20, 4);
  memcpy(&hxb, EepromBuffer + 24, 4);
  memcpy(&hxs, EepromBuffer + 28, 4);
  memcpy(&hyb, EepromBuffer + 32, 4);
  memcpy(&hys, EepromBuffer + 36, 4);
  memcpy(&hzb, EepromBuffer + 40, 4);
  memcpy(&hzs, EepromBuffer + 44, 4);

  Serial.println("Accelerometer bias and scale factors:");
  cout.print( a_xb, 6); cout.print(", "); Serial.println(a_xs, 6);
  cout.print( a_yb, 6); cout.print(", "); Serial.println(a_ys, 6);
  cout.print( a_zb, 6); cout.print(", "); Serial.println(a_zs, 6);
  Serial.println(" ");
  Serial.println("Magnetometer bias and scale factors:");
  cout.print( hxb, 6); cout.print(", "); Serial.println(hxs, 6);
  cout.print( hyb, 6); cout.print(", "); Serial.println(hys, 6);
  cout.print( hzb, 6); cout.print(", "); Serial.println(hzs, 6);
  Serial.println(" ");

  IMU.setAccelCalX(a_xb, a_xs);
  IMU.setAccelCalY(a_yb, a_ys);
  IMU.setAccelCalZ(a_zb, a_zs);

  IMU.setMagCalX(hxb, hxs);
  IMU.setMagCalY(hyb, hys);
  IMU.setMagCalZ(hzb, hzs);
}

void print_float_array(float *arr, int size) {
    if (size == 0) {
        Serial.printf("[]");
    } else {     
        Serial.print('[');
        for (int i = 0; i < size - 1; i++)
            Serial.printf("%f, ", arr[i]);
        Serial.printf("%f]", arr[size - 1]);
    }
}

and got the following stable results - did change the config a bit between propshield and mpu9250
Capture4.PNG
 
Neat, that seems to be working really well! I'm excited to try it out on a couple of upcoming projects.
 
Cool let us know how it works out. Any suggestions for new MEMs IMU to play with :)

I haven't seen much in the low cost space. Most of my projects have been using the MPU-6500 since I was able to purchase a lot of them. I honestly really like that sensor - it's easy to integrate from both a hardware and software standpoint. Looks like the BMI-088 / BMI-090 is finally available, but I haven't been using it much since they have been tough to find available. I haven't done enough testing to say whether there is a measurable difference between the two. But I do have software libraries for both.

I did some work with the Honeywell HG4930 for a customer. The data was clean, as can be expected from a large, $12k USD IMU.

I'm most excited to start using the ADIS 16505-3 IMU. It's temperature calibrated and should compare well against much more expensive units from VectorNav and Honeywell. It's around $350, so not in the hobby / low-cost market, but considering that the closest competitor is the VN-100 at around $850, seems like it's at a good price point. I'm looking forward to pairing it with a uBlox ZED-F9P (or two) in an EKF. I have an eval board in house and some units to start prototyping, just need to find the time to start prototyping and spinning some boards.
 
Last edited:
The sketch I used for fusion looks like this:
Code:
#include "MPU9250.h"
#include "Fusion.h"
#include <stdbool.h>
#include <stdio.h>

#include <Wire.h>
#include "EEPROM.h"
#include "Streaming.h"

#define cout Serial

uint64_t prev_time, cur_time;
float dt;
elapsedMillis dump;

#define SAMPLE_PERIOD (0.01f) // replace this with actual sample period
float g = 9.80665;
float rads2degs = 57.296;

FusionOffset offset;
FusionAhrs ahrs;


// an MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68
MPU9250 IMU(Wire,0x68);
int status;

void setup() {
  // serial to display data
  Serial.begin(115200);
  while(!Serial) {}

  // start communication with IMU 
  status = IMU.begin();
  if (status < 0) {
    Serial.println("IMU initialization unsuccessful");
    Serial.println("Check IMU wiring or try cycling power");
    Serial.print("Status: ");
    Serial.println(status);
    while(1) {}
  }

  cout.println("IMU Connected!");
  loadCal();

  // setting the accelerometer full scale range to +/-8G 
  IMU.setAccelRange(MPU9250::ACCEL_RANGE_2G);
  // setting the gyroscope full scale range to +/-500 deg/s
  IMU.setGyroRange(MPU9250::GYRO_RANGE_2000DPS);
  // setting DLPF bandwidth to 20 Hz
  IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ);
  // setting SRD to 19 for a 50 Hz update rate
  IMU.setSrd(19);
  IMU.calibrateGyro();
  
     FusionAhrsInitialise(&ahrs);

    // Set AHRS algorithm settings
    const FusionAhrsSettings settings = {
            .convention = FusionConventionNed,
            .gain = 0.5f,
            .accelerationRejection = 10.0f,
            .magneticRejection = 20.0f,
            .rejectionTimeout = 5 * SAMPLE_PERIOD, /* 5 seconds */
    };
    FusionAhrsSetSettings(&ahrs, &settings);
  
}

void loop() {
  float val[10];

  // read the sensor
  cur_time = micros();
  IMU.readSensor(val);

        // Acquire latest sensor data
        FusionVector gyroscope = {val[3]*rads2degs, val[4]*rads2degs, val[5]*rads2degs}; // replace this with actual gyroscope data in degrees/s
        FusionVector accelerometer = {val[0]/g, val[1]/g, val[2]/g}; // replace this with actual accelerometer data in g
        FusionVector magnetometer = {val[6], val[7], val[8]}; // replace this with actual magnetometer data in arbitrary units

        gyroscope = FusionOffsetUpdate(&offset, gyroscope);
        // Calculate delta time (in seconds) to account for gyroscope sample clock error
        dt = (cur_time - prev_time)/1e6;
        FusionAhrsUpdate(&ahrs, gyroscope, accelerometer, magnetometer, dt);
        prev_time = cur_time;

        const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));
        if(dump > 1000) {
          //print_float_array(val, 10);
          //Serial.println();
          Serial.printf("Fusion (RPY): %0.1f, %0.1f, %0.1f\n", euler.angle.roll, euler.angle.pitch, euler.angle.yaw);
          dump = 0;
        }
 
}

void loadCal() {
  // EEPROM buffer and variables to load accel and mag bias
  // and scale factors from CalibrateMPU9250.ino
  uint8_t EepromBuffer[48];
  float a_xb, a_xs, a_yb, a_ys, a_zb, a_zs;
  float hxb, hxs, hyb, hys, hzb, hzs;

 // load and set accel and mag bias and scale
  // factors from CalibrateMPU9250.ino
  for (size_t i = 0; i < sizeof(EepromBuffer); i++) {
    EepromBuffer[i] = EEPROM.read(i);
  }
  memcpy(&a_xb, EepromBuffer + 0, 4);
  memcpy(&a_xs, EepromBuffer + 4, 4);
  memcpy(&a_yb, EepromBuffer + 8, 4);
  memcpy(&a_ys, EepromBuffer + 12, 4);
  memcpy(&a_zb, EepromBuffer + 16, 4);
  memcpy(&a_zs, EepromBuffer + 20, 4);
  memcpy(&hxb, EepromBuffer + 24, 4);
  memcpy(&hxs, EepromBuffer + 28, 4);
  memcpy(&hyb, EepromBuffer + 32, 4);
  memcpy(&hys, EepromBuffer + 36, 4);
  memcpy(&hzb, EepromBuffer + 40, 4);
  memcpy(&hzs, EepromBuffer + 44, 4);

  Serial.println("Accelerometer bias and scale factors:");
  cout.print( a_xb, 6); cout.print(", "); Serial.println(a_xs, 6);
  cout.print( a_yb, 6); cout.print(", "); Serial.println(a_ys, 6);
  cout.print( a_zb, 6); cout.print(", "); Serial.println(a_zs, 6);
  Serial.println(" ");
  Serial.println("Magnetometer bias and scale factors:");
  cout.print( hxb, 6); cout.print(", "); Serial.println(hxs, 6);
  cout.print( hyb, 6); cout.print(", "); Serial.println(hys, 6);
  cout.print( hzb, 6); cout.print(", "); Serial.println(hzs, 6);
  Serial.println(" ");

  IMU.setAccelCalX(a_xb, a_xs);
  IMU.setAccelCalY(a_yb, a_ys);
  IMU.setAccelCalZ(a_zb, a_zs);

  IMU.setMagCalX(hxb, hxs);
  IMU.setMagCalY(hyb, hys);
  IMU.setMagCalZ(hzb, hzs);
}

void print_float_array(float *arr, int size) {
    if (size == 0) {
        Serial.printf("[]");
    } else {     
        Serial.print('[');
        for (int i = 0; i < size - 1; i++)
            Serial.printf("%f, ", arr[i]);
        Serial.printf("%f]", arr[size - 1]);
    }
}

and got the following stable results - did change the config a bit between propshield and mpu9250
@mjs513
Thanks for the branch and the sample code. I think you are missing a call to FusionOffsetInitialise() in setup() which seems kind of important if the gyro real-time calibration is to work as designed. I need to do more testing but my motorcycle imu display seems to be working wonderfully -- and with far less lag than my current BNO085 implementation.

Also. I think your initialisation of rejectionTimeout should be .rejectionTimeout = 5 / SAMPLE_PERIOD if you want 5 seconds
 
I haven't seen much in the low cost space. Most of my projects have been using the MPU-6500 since I was able to purchase a lot of them. I honestly really like that sensor - it's easy to integrate from both a hardware and software standpoint. Looks like the BMI-088 / BMI-090 is finally available, but I haven't been using it much since they have been tough to find available. I haven't done enough testing to say whether there is a measurable difference between the two. But I do have software libraries for both.

I did some work with the Honeywell HG4930 for a customer. The data was clean, as can be expected from a large, $12k USD IMU.

I'm most excited to start using the ADIS 16505-3 IMU. It's temperature calibrated and should compare well against much more expensive units from VectorNav and Honeywell. It's around $350, so not in the hobby / low-cost market, but considering that the closest competitor is the VN-100 at around $850, seems like it's at a good price point. I'm looking forward to pairing it with a uBlox ZED-F9P (or two) in an EKF. I have an eval board in house and some units to start prototyping, just need to find the time to start prototyping and spinning some boards.

Hi Brian

And yes - still like the 6050 or the 6500 (but the 9250 grew on me a bit). Not a fan of the new ICM boards for some reason.

as for that ADIS IMU looks like the price just doubled (looked on digikey) :(

Yeah I have a couple of of the ZED-F9P boards around that I haven't done much with in a while (https://www.ardusimple.com/simplertk2b-receivers/) either - to do list when I get the energy again
 
Hi Brian

And yes - still like the 6050 or the 6500 (but the 9250 grew on me a bit). Not a fan of the new ICM boards for some reason.

as for that ADIS IMU looks like the price just doubled (looked on digikey) :(

Yeah I have a couple of of the ZED-F9P boards around that I haven't done much with in a while (https://www.ardusimple.com/simplertk2b-receivers/) either - to do list when I get the energy again

I think that must have been the breakout board that you saw, the raw chips are still around $350 - $400:
https://www.digikey.com/en/products...now&utm_medium=aggregator&utm_source=octopart

I've been doing a lot of RTK work with the ZED-F9P, let me know if you start playing with them and get stuck. I also found this video helpful:
https://youtu.be/7r__F-nyKUA

I haven't used the ICM boards yet. I have one sitting around and keep meaning to update my Invensense library to work across the range of MPU and ICM boards. Someday, when there is more time.
 
I think that must have been the breakout board that you saw, the raw chips are still around $350 - $400:
https://www.digikey.com/en/products...now&utm_medium=aggregator&utm_source=octopart

I've been doing a lot of RTK work with the ZED-F9P, let me know if you start playing with them and get stuck. I also found this video helpful:
https://youtu.be/7r__F-nyKUA

I haven't used the ICM boards yet. I have one sitting around and keep meaning to update my Invensense library to work across the range of MPU and ICM boards. Someday, when there is more time.

Yep - that was what I was looking at - the little breakout board of theirs - really not worth the money to be honest. :)

Thanks for the RTK link will have to check it out.
 
@mjs513
Thanks for the branch and the sample code. I think you are missing a call to FusionOffsetInitialise() in setup() which seems kind of important if the gyro real-time calibration is to work as designed. I need to do more testing but my motorcycle imu display seems to be working wonderfully -- and with far less lag than my current BNO085 implementation.

Also. I think your initialisation of rejectionTimeout should be .rejectionTimeout = 5 / SAMPLE_PERIOD if you want 5 seconds

@thebigg - @brtaylor

Yes you are right I forgot a copy and paste of that in the examples - didn't seem to be much of impact of leaving it out though that I could tell. Last couple of days been rewicking the example to more appropriately to use the dataready interrupt to capture the data as well as specifically setting the sample rate in the sketch (setSRD). Also saw something else that x-IO technologies uses - a x-IMU3 GUI that I saw madgwick recommend to use for debugging purposes, so implemented that as well as updating the Telemetry Viewer that we used a couple of years ago.

So now you have to specifically what you want to print (note options are toggles so turn one off before turning the other on.)
Code:
Menu Options:
	x - x-IMU3 GUI Output
	t - Telemetry Viewer Output
	s - Serial Print Output (Euler Angles)
	c - Print Calibration
	r - Print Raw Values
	h - Menu

The x-IMU3 GUI is a Windows based app (also available for linux and I think MAC) but it does need data in a specific format. It looks like this:
Capture4.jpg

You do have to go into settings and change them to get it to work for a while at least:
Settings.PNG

But if you download Telemetry Viewer v0.7 (I couldn;t get 0.8 to work on windows 10) you can get pretty much the same thing and I would recommend going this route instead of the x-IMU3 viewer:
Capture5.jpg

the settings file for that setup is:
View attachment Fusion.txt

and the updated sketch for the MPU-9250 breakout:
View attachment MPU9250_Fusion-230424a.zip

cheers and have fun.
 
Hey Mike, getting around to playing with this filter and seeing if you recall offhand what the axis orientation is? X forward, Y right, and Z down?
 
Good Morning Brian. If I remember correctly I believe its ENU - threw me off as well but if I remember correctly you can can change to NED.

Keep me posted. I have been distracted with some other projects and keep meaning to get back to this.
 
Good Morning Brian. If I remember correctly I believe its ENU - threw me off as well but if I remember correctly you can can change to NED.

Keep me posted. I have been distracted with some other projects and keep meaning to get back to this.

You're right, just had to set NED in the filter settings and it worked well with a standard aeronautics coordinate system for the sensor data.

I had to adjust the filter gain, but otherwise the performance seems good on first glance. I could probably also tune the accelerometer rejection setting a little better since linear acceleration was resulting in minor pitch changes. Wish the documentation was a little more in-depth, but it's my favorite Madgwick implementation so far.
 
@brtaylor etal

Been playing more with Fusion and found that unless I do a good cal on the magnetomer it doesn't give good yaw readings other than at 0degs. So I went back to the old invense-mup-9250 library and grabbed the accel and mag calibration routines and combined it with the old calAnalysis sketch and created a self contained sketch based on your new library that will allow you do the calibrations and save them to eeprom. Here is the new menu:
Code:
Menu Options:
========================================
	x - x-IMU3 GUI Output
	t - Telemetry Viewer Output
	s - Serial Print Output (Euler Angles)
	r - Print Values
========================================
	g - Zero Gyroscope
	a - Accel Calibration
	m - Mag Calibration
	d - Save Calibration
	l - Load Calibration
	p - Print Calibration
========================================
	h - Menu
========================================

and here is the new sketch if you are interested:
View attachment MPU9250_Fusion.zip

If you want to add it to your library enjoy - didn't put any of the normal copyright stuff at the top so you may what to add it.
 
Neat and the yaw makes sense. If anyone else comes along to use this, all my stuff here is MIT licensed.
 
@brtaylor and all interested.

I couldn't resist thetrying to see if I could get the ICM20948 working. So I updated your library with a version for the ICM20948:

https://github.com/mjs513/invensense-imu/tree/Invensense-imu-icm

Consider it still a work in progress as I haven't included Wake on Motion functionality yet but most is there including a couple new functions that are necessary. I included an example using the Fusion library and I2C interupt version. If you want give it a try and let me know if its worth including as a PR.
 
Back
Top