Bug fixes and improvements to track following.
Bug fixes and improvements to track following.

IEEERobotics/high-level,IEEERobotics/high-level,IEEERobotics/high-level,IEEERobotics/high-level,IEEERobotics/high-level