Implementing Madgwick IMU algorithm

Question

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

Solution

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/