Finished implementing optimal control test. Currently works for two step lookahead. test_differential_equation_planner_main.cpp has been modified to run this test; will generalize in the future.