AP_NavEKF : Position and Velocity reset bugfix