Magwick Library - is it current?

Status
Not open for further replies.
I'm using your version of the Madgwick AHRS library (https://github.com/PaulStoffregen/MadgwickAHRS) and after some tweaks it seems to work very well, at least as good as the Freescale Kalman filters. However I have seen some articles (dated 2017) on various websites that there we some errors in in the algorithm and that there was a new version but I have not found any update.

I thought that when I saw this library that it had fixed the known problem with the matrix arithmetic. Has it?

I tried someone's supposed update and it didn't work for me so I abandoned it!

I have modified the Madgwick filter initialisation to make it lock on to the current orientation at start-up. It now locks on immediately rather 10+s and takes around 1mS to process a sample and compared with 14mS for the Kalman filter using a 48MHz processor.
 
Please open a github issue on that repository. Or multiple issues, if there is more that 1 distinct problem.

When you do write the issue report on github, try to include specific info. Mentioning "some articles (dated 2017) on various website" without links to the specific articles is not helpful. I will close that issue without any investigation if you don't give the links.

Likewise on improvements, that's what github pull requests are meant to do. When there's a bug or performance problem, a test case and specific instructions to quickly reproduce the problem are essential. If you don't give that sort of info, you can expect your contribution to sit unused for a very long time. Be specific about the problems and give test cases so I and others can verify.

Also please understand we just released Teensy 4.0 only 5 days ago. I'm going to be crazy busy with T4 stuff for at least the next month. Maintaining open source libs always comes at a lower priority than work that sustains PJRC. But I do (usually) get around to it. Ping me again in a month or two, but in the meantime, please do open issues or pull requests on github.
 
Thanks Paul I understand the pressure. I have contacted Seb Madgwick in the hope that he can throw some light on the issue.
 
I'm using your version of the Madgwick AHRS library (https://github.com/PaulStoffregen/MadgwickAHRS) and after some tweaks it seems to work very well, at least as good as the Freescale Kalman filters. However I have seen some articles (dated 2017) on various websites that there we some errors in in the algorithm and that there was a new version but I have not found any update.

I thought that when I saw this library that it had fixed the known problem with the matrix arithmetic. Has it?

I tried someone's supposed update and it didn't work for me so I abandoned it!

I have modified the Madgwick filter initialisation to make it lock on to the current orientation at start-up. It now locks on immediately rather 10+s and takes around 1mS to process a sample and compared with 14mS for the Kalman filter using a 48MHz processor.

Can you reference the websites that mention the errors. I know one issue was the srt function and think there was another one that I saw on one of the drone forums? Been a while since I worked on this.
 
Yep - I have seen that before - don't think those changes were incorporated into the PRJC madwick implementation that is included in the lib. I've incorporated it into my version and it does work no problem. The original discussion I have seen is from here: //diydrones.com/xn/detail/705844:Comment:1755084 But it states:
Code:
Permalink(https://diydrones.com/xn/detail/705844:Comment:1755084)  Reply by Jeroen van de Mortel on 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);

EDIT: Forgot. It should be a drop in replacement for the code in MadgwickAHRS.cpp if you want to test. The change should start at about line 109 in the file (https://github.com/arduino-libraries/MadgwickAHRS/blob/master/src/MadgwickAHRS.cpp). The original code was never updated and still isn't

If you notice in the post that he already contacted Seb Madgwick about it.
 
mjs513. I tried the updated algorithm and it worked better than last time but not as good as my version of the original Arduino library. So I do not recommend updating the library yet.

Can you tell me what platform are you testing the code on?

I'm using a Adafruit M0+ or a Teensy 3.2 interfacing to and Adafruit FXOS8700 and FXAS2100C shield. I have a calibration sketch that performs a sensor offset/scaling test for the accelerometer, and a magnetometer test that put the hard and soft iron parameters into EEPROM. The Madgwick sketch also initialises the quaternion using a 6DOF NED compass

Regards, Mike
 
I have tested on the whole family of T3.x's as well as the Uno and Mega. One of the things you may have to do when moving from platform to platform is tweak the "K" values. Noticed that they may change going from processor to processor.

Not sure what you mean when you say doesn't work as good? Also, have you tried the Madgwick's Mahony algorithm?
 
The 3D representation suffers from serious jitter although the heading is reasonable. By the 'K' value I assume mean the gain?

I shall use the original library version as it works well until we have the latest algorithm proven.

I have contacted Seb Madgwick and he seems to have abandoned the code for the original and 'updated' algorithm I've just tried. He has approached the design in a more systematic way using functions to perform that matrix arithmetic that probably reduces the risk of errors. I have a friend and colleague who is a linear algebra mathematician who will look into it, try it out and hopefully create an Arduino library.
 
Status
Not open for further replies.
Back
Top