Checkpoint: NMPC First run. TODO: (1)Adjust state transition distance (2) Determine minimum horizon for specific distance. (3)Determine horizon-computation time relationship. (4) Determine maximum change in angle between waypoints
 Checkpoint: NMPC in matlab. Port code to C++
 Checkpoint: Made some corrections to SQP including hessian and merit adjustments. TODO: Implement state rotation
MPC done. Need to tune weighting.  PathPlanner started
MPC done. Need to tune weighting.  PathPlanner started
Creating GUI for planner
Path planner main algorithm initial testing is successful. TODO: pathplanner code coverage
LMPC is working
Path planning done. Need to find spline tool to divide path
Path Following working. Need to do explicit path following
Path Following working. Need to do explicit path following
Computing EMPC using GPU
Computing EMPC using GPU