AP_NavEKF : Improved behaviour on entry to and exit from static mode