Properly set BODY_TO_IMU values