MPC cost (objective) function options: veldes horizon, output damping

[avikde]

Options:

• Only final goal point
• A trajectory with cost to each point
• If trajectory tracking is really important (like maybe somersault??) have a running cost: quadratic cost multiplied by dt. Intuitively like an integral control term?

Both of these were already implemented while working on #10 but the latter doesn't really work well. Could debug the failure of the latter more by seeing if an open-loop definitely-feasible trajectory has the same problem. Come back to this after #24

• in somersault MPC compare trajectory options #32 going back to over after spinning caused instability. fix
• veldes horizon beneficial?