going to switch to trying to constrain the beginning of the trajectory (for MPC)
preliminary mpc working. add noise next
mpc lookahead version framework setup. deal with end case. note: approximate hessian is causing the problems
shifting Hess forward in time works better than setting it to identity every time. however, still approx_merit_improve < 0 problems
working on slam-state params
Merge branch 'gkahn'