Fixed the trajectory following code by adding proportional gain. Also added a differential gain.