AP_NavEKF : staticMode bugfix and robustness improvements