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