AP_NavEKF: fixed merge error
AP_NavEKF: bug fixes and robustness enhancements