AP_NavEKF : Improvements to staticMode robustness