added matlab interface to do waypoint following