fixed casadi bug (deltaMatrix)
now tune parameters since casadi is fixed
slam-state works well
crashing on mpc on top path
still unable to get slam-state-mpc to run. usually crashes on the upper path where approximate merit function is often < 0. not sure if tweaking parameters at this point will solve it. my gut says it fails because satisfying the loose waypoint angle constraint directly conflicts going to the landmark
Merge branch 'gkahn'