Added 50 knot point trajectory. Gets through entire step, but is falling backward at the end.