Clean up of compass startup and inertial ekf