Erik - / IMUCalc

Never actually tested in practise

This library has only been 'hand tested', it never was actually included in a quadcopter. It is now published so it might help someone, but please verify it works for you before you crash your setup (that is of course for every library you use). It is a while ago I made this, so everything that follows might be slightly different than I remember :D.

Inputs are SI units (probably), so gyro data should be in rad/s. Magnetometer and accelerometer only uses normalized vectors. You will require the following library which isn't included in this one: http://mbed.org/users/BlazeX/code/GTMath/. I am fairly certain things like normalizing a vector twice happens currently, so it can be more efficient.

Basic functionality

The library doesn't use quaternions, since they are hard, but instead two 3D vectors. Those last 2 floats aren't going to fill your memory. One vector is the in the length of the aircraft/device/etc ('heading'), the other one points up ('top'). Together they define the angle of your craft.

The currently measured vectors by the accelerometer and magnetometer are defined. The top simply calculated from the accelerometer data. For the heading the magnetometer data is used, which is moved to be at 90 degrees from the top (this is required since unless you live at the equator that won't be the case). This directly makes sure they have the 90 degree angle between them they are supposed to have.

At the same time the gyroscope offset (later more) is removed from the gyroscope data, and that is used to rotate the old heading and top vectors to new values according to your gyroscope data.

We calculate the difference between the gyroscope measurements and the accelero/magneto measurements. We call this the offset of the gyroscope. Now with a certain weight factor we combine the two measurement types into a final result, which is also used for the next gyroscope measurement. This already cancels part of the gyroscope drift.

The second part is that we average out the gyroscope offset measurements, and the result of that is used to compensate new gyroscope measurements.

[mbed] converted /Quadrocopter/IMUCalc