Implementing Madgwick IMU algorithm

algorithm filtering pitch
jrandj picture jrandj · May 12, 2014 · Viewed 13.3k times · Source

I'm trying to use Sebastian Madgwick's IMU algorithms to obtain roll, pitch and yaw from my sensor, which is an MPU-9150. The folder I downloaded came with some sample data, and I just replaced this data with my own, making sure to keep units/conventions the same etc. The problem is that the output angles from the algorithm with my data do not make much sense and are very noisy, especially the yaw angle. The roll and pitch seem plausible but I can get better just from my accelerometer/gyroscope data. I've attached a plot of his example data (which works), my data, and my output.

Is there some extra filtering I have to do with my data before I try and use these algorithms? So far I've just been playing around with median filtering, and that didn't seem to make a difference.

Thanks

Example data Example data output My data My data output

Answer

c0redumb picture c0redumb · Apr 28, 2015

I happened to found some note on the instability issue of the Magdwick's algorithm by Tobias Simon as detailed here

http://diydrones.com/forum/topics/madgwick-imu-ahrs-and-fast-inverse-square-root

The issue was with the fast inverse square root used in the Magdwick implementation. I was not clear about where the issue was. Maybe it is with the precision of the algorithm, or maybe it is because that the original code did not work on a 64-bit machine. Replacing the implementation with a different (better?) one achieves amazingly better and more stable AHRS calculation. (Or making some small change to the code for 64-bit machine also solved the problem? Please read the comments of the original note of Tobias Simon.)

A better inverse square root implementation can be found here

https://pizer.wordpress.com/2008/10/12/fast-inverse-square-root/

Related questions

String similarity algorithms?

I need to compare 2 strings and calculate their similarity, to filter down a list of the most similar strings. Eg. searching for "dog" would return dog doggone bog fog foggy Eg. searching for "crack" would return crack wisecrack rack jack …

Read More
How do I check if an array includes a value in JavaScript?

What is the most concise and efficient way to find out if a JavaScript array contains a value? This is the only way I know to do it: function contains(a, obj) { for (var i = 0; i < a.length; i++) { …

Read More
Removing duplicates in the lists

Pretty much I need to write a program that checks if a list has any duplicates and if it does it removes them and returns a new list with the items that weren't duplicated/removed. This is what I have …

Read More