Added a new path-following algorith, which works better