Wrote MPC ode.  Finished guidance thread