AP_NavEKF: Improved use of static mode