Implement Stanley controller

-Implement control algorithms proposed in "Autonomous Automobile Trajectory Tracking for Off-Road Driving: Controller Design, Experimental Validation and Racing" written by G. M. Hoffmann, C. J. Tomlin, M. MOntemerlo, and S. Thrun. -Only implement the lateral control part (steering angle) since the longitudinal control is being done by ssc interface modules.