Calibrate.
Fix acceleration vector in AHRS.
Calibrate IMU.
Clean up controller.