-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