AP_NavEKF : fixed bug in pos and vel reset when in static mode