Madgwick IMU/AHRS and Fast Inverse Square Root

Hi Everyone,

I'm currently using Madgwick's popular filter for my own AHRS project. After some experimentation with sensors and adjusting the beta gain, I've noticed some unpredictable behavior of the filter. For example, euler angles computed from the quaternion changed unexpectedly to 20 degrees off the reference angle (in a static scenario!).

After setting the magnetometer input to (0,0,0) the problem stayed the same. Then I removed the gyroscope inputs (0,0,0) and the problem stayed the same. My conclusion was that it must have been related to the accelerometer inputs.

After experimenting with real sensors I moved to artificial ACC input data and set up a test bed for Madgwick's algorithm (MadgwickTests on GitHub). I've figured out that in Madgwick's algorithm the fast inverse square root leads to huge instabilities when noisy measurements are applied. As an example, consider the pitch and roll angles shown in the following image:

The input for the filter was: gyro = (0,0,0), acc = (2.5, 2.5, 9.15) +- 0.1 uniform noise (see test.c for details). As you can see, the pitch and roll angles (red/green, in degrees) of the original implementation show unacceptable variations.

Remarkably more stable and accurate pitch/roll angles (blue/magenta) were achieved by exchanging the inverse square root implementation. I used the following code from Accurate and Fast InvSqrt for computing the inverse square root of float x:

unsigned int i = 0x5F1F1412 - (*(unsigned int*)&x >> 1);
float tmp = *(float*)&i;
float y = tmp * (1.69000231f - 0.714158168f * x * tmp * tmp);

Please see the GitHub commit in which I've added some code for switching between the original (0), the proposed (1) and the reference (2) inverse square root implementations.

I hope that my investigations are helpful to improve the accuracy of Madgwick's C filter implementation.

Cheers, Tobias

Replies

Tim C May 16, 2014 at 8:29am

Hi, I noticed the error builds up under continuous external vibrations(<0.3g) in either pitch or roll axis. The error is around 1 deg per second. Is there anyway to mitigate this? I tried to set beta to improve dynamic performance but
- Tobias Simon > Tim C May 16, 2014 at 8:42am
  
  Hi, 0.3g should not be a problem. We have acceleration around +-3g on our UAV; we have beta set to 0.025 and no error builds up. Is everying alright with your sensor data?
  - Tim C > Tobias Simon May 16, 2014 at 8:52am
    
    The static error on accelerometer is +-0.02g, on gyro is +-0.4deg/s, on magetometer is +-0.01guass. My sampling rate is around 160HZ. Maybe it's too low? I checked Madgwick's paper and it looks like there is no big difference once the sampling rate is above 100HZ. When I just lay everything flat on the table and slide it continuously in all directions, the pitch and roll errors build up but converge to 0 slowly(in 3sec) once I stop sliding.
    
    Tim
    - Tobias Simon > Tim C May 23, 2014 at 1:58pm
      
      The sampling rate should be no problem. What is your set-up?
      
      Are you using a 32 bit or a 64 bit processor?
      
      Which kind of sensors are you using? Is the axis alignment correct?
      
      Did you calibrate the gyro upon start-up?
      - Harry > Tobias Simon June 11, 2014 at 12:30am
        
        I'm playing with a board that has an mpu60X0 and shoehorned the Madgwick algorithm onto it with a lot of trial and error. You guys are talking about overshoot and I have undershoot/lag. I increased the integration time and it almost looks right. It takes a lot of memory to run it too.
        
        All my playing and I haven't found anything that improves on the DIYdrones DCM.
        
        Harry > Harry June 11, 2014 at 1:51pm
        
        I have to retract that a bit, FreeIMU uses the Madgwick AHRSupdate and is just as good as DCM, if not better and is about the same size in memory. I'm testing it now and it has none of the molasses effect that my attempt did. It's also much smoother than the DCM. My implementation is screwy because I'm a beginner programmer. If I tweak the sampling frequency it might be ok, but it will still be 6k bigger than FreeIMU.
Tobias Simon March 26, 2014 at 1:37am

OK, the problem was my 64 bit machine. Long is not 4 bytes wide and thus, the following code line fails:

https://github.com/TobiasSimon/MadgwickTests/blob/master/MadgwickAH...

The portable solution for this is:

float halfx = 0.5f * x;

float y = x;

int32_t i = *(int32_t *)&y;

i = 0x5f3759df - (i>>1);

y = *(float*)&i;

y = y * (1.5f - (halfx * y * y));

return y;

TobiasSimon/MadgwickTests

Madgwick AHRS Numeric Stability Tests. Contribute to TobiasSimon/MadgwickTests development by creating an account on GitHub.
neoraptor November 22, 2012 at 6:56am

Thank you for this optimisation. I will test it this evening.

I would have some question about using this algorithm.

I plan to use it with an atmel board (xmega-a1 or xmega-b1) and atavrsbin1 9dof board (http://www.atmel.com/Images/doc8354.pdf) :

* How fast should I read the sensor and make the DCM update?
* Should I do this using interrupt based code?
* How to choose the "right" parameters for the filter?

Thank your for your help.

of 6

This reply was deleted.

Discussions

Madgwick IMU/AHRS and Fast Inverse Square Root

Replies

Activity