AP_NavEKF : improved  static and on-ground mode selection logic