Control & trajectory planning optimization

A controller which calculates in real time the optimal trajectory for the vehicle and outputs the commands for the vehicle in order to drive along the trajectory.