AP_NavEKF: Improved initialisation and setting of static mode