Remove smoothing while I work on gyroscope integration...
Implement quaternion to rotation matrix

Nice blog on the maths for this here:
http://www.camelsoftware.com/firetail/blog/c/imu-maths/