# 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

Views: 42514

### Replies to This Discussion

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.

Hi Tobias,

I have been using this algorithm for quiet a while now, but I found some problems with it

when using the MARG algorithm instead off the IMU version it seems very slow and having some overshoot.

In the picture below you can see the X as turning using MARG and IMU. The real motion is the same.

Did you had any problems with that ?

Hi Jeroen, it looks like you are repeating the same sensor movement twice; one with the AHRS and one with IMU filter.

Maybe it would be better to first acquire the inout data once and then feed it into the two different algorithms for comparison..

We had issues with overshoot/undershoot when the accelerometers or magnetic field sensor measurements were used without calibration.

Tobias,

I'm wondering what conventions you are using when you feed into Madgwick's update function.

I use the right-hand coordinate system and x,y and z directions are consistent for all three sensors (acc,gyro,mag).

For some reason I need to feed negative gyro values, and upside down z value of magnetometer into the algorithm.

For example, AHRSupdate(-gx,-gy,-gz,ax,ay,az,mx,my,-mz);

Also, what's your QuaternionToEuler() function?

This is mine:

Yaw = Atan2(2.0f * (Quaternion[1] * Quaternion[2] - Quaternion[3] * Quaternion[0]), 2 * q0square + 2 * q1square - 1) * 180 / PI;
Pitch = -Asin(2.0f * (Quaternion[1] * Quaternion[3] + Quaternion[2] * Quaternion[0])) * 180 / PI;
Roll = Atan2(2.0f * (Quaternion[2] * Quaternion[3] - Quaternion[1] * Quaternion[0]), 2 * q0square + 2 * q3square - 1) * 180 / PI;

There are other versions using different conventions. What I don't understand is why I need to feed upside down magnetometer value.

Tim

Hi Tim,

I use the following IMU sensor cluster with the indicated right-handed coordinate system on the back: http://www.drotek.fr/shop/62-575-thickbox_dm/imu-10dof-mpu6050-hmc5...

The MPU6050 and HMC5883L axes are already aligned on the sensor board; Thus, I do not need to flip sensor readings. Which sensor cluster are you using?

I use this quat2euler function here: https://github.com/PenguPilot/PenguPilot/blob/master/autopilot/serv...

https://github.com/ptrbrtz/razor-9dof-ahrs/blob/master/Arduino/Razo...

According to this, try the Madgwick filter using this sensor input order:

-gx, -gy, -gz, ay, ax, az, mx, -my, -mz

Do you mean sensor outputs without swapping axis? It looks like

-gx, -gy, -gz, ay, ax, az, mx, -my, -mz

works. However, your quatToEuler() breaks when I use it with IMUUpdate(-gx,-gy,-gz,ax,ay,az).

Pitch and Roll won't go over 45deg.....

I am so confused now but I will give this a try. https://github.com/TobiasSimon/MadgwickTests

Basically I am just gonna use the same update() and quatToEuler().

May I ask what do you feed into update()?

Also, gyro and acc on my 9dof IMU are aligned in all three axis, so according to right-hand rule, the gyro should give correct data. Why does Madgwick's algorithm need inverted values? I tested with positive values and it integrates to an angle(theta) but converges to -theta when not moving. It's definitely integrating in the wrong direction....weird....

That is maybe a good idea doing it in matlab. I have calibrated the compass. I also saw differences when the IMU was mounted upsidedown.

Using the algorithm without an calibrated ACC should not be a problem, it should not cause an overshoot. Or you must have a really bad ACC.

You are right, gyro and acc are correctly aligned. Maybe it's a good starting point to look only into the IMU update function first.

In our case, gyro and acc are also correctly aligned (mpu6050). We feed the following in the update function: gx, gy, gz, ax, ay, az (nothing is swapped or inverted). Could you please check the behavior in this case?

Beta =0.05

Red= IMU (Gyro+Acc), Blue= MARG(Gyro+Acc+Mag)

On the Y-axis there is some difference, the MARG seems to be slower.

Red= IMU (Gyro+Acc), Blue= MARG(Gyro+Acc+Mag)

Every sensor upside up (pointing to the sky)

Red= IMU (Gyro+Acc), Blue= MARG(Gyro+Acc+Mag)

Every sensor upside down (pointing to earth)

Especially the last one is strange, on the Y-axis the IMU goes negative and the MARG positive.

And with the yaw the same and it is has al large overshoot and take almost 50 seconds to stabilize. The slow startup is for both orientations. The quad is in the last two pictures completely static.

Graph were made with Sebastians his Matlab scripts.

Did you had the same overshoot ?

Hi Jeroen,

right after filter initialization, we can observe a similar behavior of the filter.

I guess that the different convergence paths in your last figure depends on the gradient descent algorithm itself. However, this shouldn't be a problem.

We use a decreasing beta gain in order to have quick convergence in the beginning.

This is the slow version with fixed beta gain; convergence approximately at step 6000:

This is the fast version with decreasing beta gain; convergence approximately at step 300:

1

2

3

4

5

6

7

8

9

10

## Groups

1072 members

1122 members

52 members

249 members

• ### BBBmini

76 members

Season Two of the Trust Time Trial (T3) Contest
A list of all T3 contests is here. The current round, the Vertical Horizontal one, is here