AP_NavEKF: Tidy Up