AP_NavEKF: Cleaned up on-ground state and covariance update logic