AP_NavEKF : Fix bug that leaves height unconstrained in static mode