implemented nearCurrentWaypoint, based on the gps covariance (it's a hack!)