- Drastically increase nonlinear constraint violation cost (I think)
- Make state & input costs much smaller than constraint costs
- Add condition to stop simulation if nonlinear constraint
is not satisfied (doesn't seem to work for going off track)
- Create separate nonlinear constraints for each state to check
if it is on track and if it doesn't intersect each obstacle
- Change indexing of nonlinear constraints
- Decrease `sim_stop_idx` to 1500
- Add some debugging messages for when simulation hits
limit or when `fmincon` can't satisfy constraints
- Add more `fmincon` outputs for future debugging
- Add obstacle information to `getTrajectoryInfo` call
in `part2_test_controller.m` to see when car crashes
- Add parameter to stop simulation when specified
index in reference trajectory is reached
- Modify `forwardIntegrate` to return `Xobs` for plotting
- Plot obstacles in part2 testing figure
- Change some line plotting styles for clarity
- Add 1/2 coefficient to cost function
- Move `MPC_Class.m` to "Deliverables" folder
- Modify `part2_test_controller.m` plotting style a bit
- Rename `part2_MPC_controller.m` to `sravan_MPC_controller.m`
