-O3 for trajMPC messes things up. without it for trajMPC, down to 1000ms for all waypoint belief planning, but 400 ms still for state space
pushing to run on rll2
fixed angle merit cost bug
can choose timestep for traj, working on for state
added slam-state-mpc
working on MPC loop. bug with replanning (look at initialization)
not sure whether loops should be T_MPC or T in slam-state-mpc. committing and switching to T
about to merge with Michael's
fixed f bug in parameter