# 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: 43261

### Replies to This Discussion

Hello!

I'm using free-running IMU device based on LSM6DS3 (gyro and accelerometer) and LIS3MDL (magnetometer).

LSM6DS3 are configured for 2000 °/s (gyro) and 16G (accerometer). LIS3MDL are configured for 4 gauss range.

Magnetometer are calibrated using Magneto algorithm.

Data from chips are collected and saved to flash memory with high speed (up to 250 samples/s). After data reading I calculate trajectory using some Madgwick algorithm (MadgwickAHRSupdate (gyro, accelerometer and magnetometer) or MadgwickAHRSupdateIMU (gyro, accelerometer only). For all cases I'm using for factor Beta=0.041 value.

Below I apply the screenshots for same device trajectory.

The first trajectory path was built usung gyro and accelerometer data.

The second trajectory path was built using gyro, accelerometer and magnetometer data with using magnetometer calibration correction.

The third trajectory path was built using gyro, accelerometer and magnetometer data without magnetometer correction.

The device path are created on the small place inside a tower building (concrete, steel and glass construction) on high floor.

I have done the experiment some times and have got the similar results. I confused this results and I can't understand why Madgwick algorithm using magnetometer data (MadgwickAHRSupdate) works so bad.

I have applied all last changes in the algorithm except listed below (_bx and _bz are single not a half). Hovever I have tried both variants (my variant are better fit).

_bx = sqrt(hx * hx + hy * hy);
_bz  = -_2q0mx * q.y + _2q0my * q.x + m1.z * q0q0 + _2q1mx * q.z - m1.z * q1q1 + _2q2 * m1.y * q.z - m1.z * q2q2 + m1.z * q3q3;

I'm simply an engineer not a scientist, and I can't understand Mangwick original article algorihm for checking.

Could anybody help me or explain at least where I'm do wrong. I have checked the formulas some times in the source code and not found any differens (except notation above).

PS: The calibration magnetometer sphere (after correction using matrice calulated using Magneto program) posted below.

SY, Serge B.

Serge,

I can't give the details or the data, as I'm under an NDA---but suffice to say, in our testing here at PAS we encountered similar scaling defects to the positional data (due to accelerometer scaling, perhaps?), which was unable to be resolved from either open-source nor from manufacturer (Interstil)-given calibration instructions. Please feel free to message me directly to discuss this in more detail; we are still at something of a loss, and are considering a custom hardware-software solution.

Hi Thomas:

I have fixed my error by changing the part where I calculate the orientation angles. It turns out I was estimating dT for accelerometer and magnetometer while it should only have been applied to gyroscope. I'll leave it at that for now...

Thomas Guldborg said:

Hello Paven Chinta

1

2

3

4

5

6

7

8

9

10

## Groups

277 members

3163 members

1 member

1123 members

• ### ArduBoat User Group

258 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