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);
Cheers, Tobias
Replies
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:
My Gyro is 16 bits and I set 2000 dps - so I do this:
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
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?
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);
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
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.
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...
Tobias,
I am using this one:
https://www.sparkfun.com/products/10724
and this C implementation:
http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
-
1
-
2
-
3
-
4
-
5
of 6 Next