Working integration of Madgwick IMU filter
Code cleanup
IMU implementation