http://mbed.org/users/aberk/code/IMUfilter/#8a920397b510