Robustified flight close to waypoints