still debugging why navigation mpc stops for no reason. almost done with...

still debugging why navigation mpc stops for no reason. almost done with cartesian path following - with correctly formulated QP it solves underactuation. did some nice refactoring of path-following mpcs