Discussions

Madgwick IMU/AHRS and Fast Inverse Square Root

Posted by Tobias Simon on November 2, 2012 at 3:48am

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:

results.png?width=579The 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

You need to be a member of diydrones to add comments!

Join diydrones

Tags: ahrs, imu, madgwick
Email me when people reply –
Follow

Replies

    • Jan > Long Le August 2, 2015 at 12:14pm

      Hi, 

      I am new here and want to use Madgwick library for my drone. I use CORTEX M4, and as IMU have a STEVAL-MKI124V1.

      I am not sure :

      In which units I have to put a data to do function MadgwickAHRSupdateIMU? (Accel, Gyro, Magnet).

      Do I have to callibrate all the sensors? (I am sure that with callibration I will have the best results, but for start, is it necessary?)

      Also MY accelerometer is 16 bit I set +-2g - so I do this with RAW data from ACC:

       ACCE.X=(float)((AKCE.X/65536)*4);

      My Gyro is 16 bits and I set 2000 dps - so I do this:

      GYRO.X=(float)(GYRO.X/(65536))*2000;

      I am doing something wrong, because those euler angles I am obtaining (I am not moving with sensors!!):

      -->in attachment

      Can you help me please?

      Thank you very much.

      mad.png

      https://storage.ning.com/topology/rest/1.0/file/get/3702584221?profile=original
  • Roman Hazanski September 18, 2014 at 7:49am

    Hello everyone! First of all I'd like to say THANK YOU VERY VERY MUCH to Jeroen van de Mortel for his formulae fix!

    I had a problem with madgwick filter: when I gave my sensor more than 90 degrees rotation over roll or pitch axis (i.e. sensor z axis starts to point downwards) - resulting quaternion have become really shaky and constantly flipping +-180 degrees (q0 and q3 were changing signs constantly).

    Also setting bigger and bigger values of beta parameter was making quaternion more and more stable (which is sort of the contrary of what is expected).

    With described fix to gradient decent step flipping have stopped! That's great! I'm actually interested how does original code work on all those youtube videos I've seen.. hmm. Maybe drones just don't very much roll or pitch more than 30 degrees.

    Anyway, now I have two more interesting problems.

    Without magnetometer data - everything seems fine; I'm actually okay with not using magnetometer. But I'm still curious.

    However, if I use magnetometer and roll or pitch angle approach 90 degrees - unwanted (but smooth) yaw rotation on 180 starts to happen. When I rotate sensor upside down over one axis I also get yaw shift of 180 degrees.
    I'm using LSM9DS0 sensor; magnetometer z axis is inverse of accel and gyro. If I inverse magno z measurements on filter's input - this strange yaw rotation still occurs but in the other direction.

    It's not supposed to be like that, is it?

  • Paraglider August 16, 2014 at 11:00pm
    Jeroen, is it possible to get your working Madgwick code including the corrections you made?
    • Jeroen van de Mortel > Paraglider August 19, 2014 at 1:12am

      These lines where changed in the code, sebastian emailed me that the code on the website was old and soon will be replaced by a new version.

      // Reference direction of Earth's magnetic field
      hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
      hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
      _2bx = sqrt(hx * hx + hy * hy);
      _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
      _4bx = 2.0f * _2bx;
      _4bz = 2.0f * _2bz;
      _8bx = 2.0f * _4bx;
      _8bz = 2.0f * _4bz;

      // Gradient decent algorithm corrective step
      s0= -_2q2*(2.0f*(q1q3 - q0q2) - ax)    +   _2q1*(2.0f*(q0q1 + q2q3) - ay)   +  -_4bz*q2*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)   +   (-_4bx*q3+_4bz*q1)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)    +   _4bx*q2*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
      s1= _2q3*(2.0f*(q1q3 - q0q2) - ax) +   _2q0*(2.0f*(q0q1 + q2q3) - ay) +   -4.0f*q1*(2.0f*(0.5 - q1q1 - q2q2) - az)    +   _4bz*q3*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)   + (_4bx*q2+_4bz*q0)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)   +   (_4bx*q3-_8bz*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);             
      s2= -_2q0*(2.0f*(q1q3 - q0q2) - ax)    +     _2q3*(2.0f*(q0q1 + q2q3) - ay)   +   (-4.0f*q2)*(2.0f*(0.5 - q1q1 - q2q2) - az) +   (-_8bx*q2-_4bz*q0)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(_4bx*q1+_4bz*q3)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q0-_8bz*q2)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
      s3= _2q1*(2.0f*(q1q3 - q0q2) - ax) +   _2q2*(2.0f*(q0q1 + q2q3) - ay)+(-_8bx*q3+_4bz*q1)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(-_4bx*q0+_4bz*q2)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);

      • Tobias Simon > Jeroen van de Mortel August 19, 2014 at 11:48am

        Hi Jeroen, I've tested the code finally - looks good! However, I have to adjust the Euler angle outputs as follows to get comparable results:

           pitch *= -1.0;
           yaw *= -1.0;
           if (roll < 0)
              roll += M_PI;
           else
              roll -= M_PI;

        Cheers,

        Tobias

        • Long Le > Tobias Simon November 23, 2014 at 11:25pm

          Hi Tobias,

          It seems that you have lots of experience in using Madgwick filter. Thank you for your sharing in this post. 

          Currently, I am applying Madgwick filter in Android. But the result is not correct. I think the problem is the difference in coordinate system.What is the using coordinate system in sensor frame? What is the output world coordinate system? Could you please share some information? I want to apply Madgwick filter in Android. But I do not know the original coordinate system which Madgwick used to feed the input of update function from Android.

          I am appreciate for your reply.

      • Paraglider > Jeroen van de Mortel August 19, 2014 at 6:26am
        Ok, thanks for the information. I will try out the new code once Sebastian has uploaded the new version.
  • Tim C July 13, 2014 at 10:34am

    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

This reply was deleted.

Activity