Mpc controller cannot handle huge jumps in state
If a trajectory has 100 points, the current reference is 0, the planning horizon is 30, and the next state is at e.g. 31 or more, then the planner will break, specifically at this line:
This should be fixed by guarding this with a check.