Control systems and methods for an autonomous vehicle. A control method includes obtaining data indicative of a motion plan for the autonomous vehicle, upsampling the data indicative of the motion plan to determine a desired vehicle state, obtaining data indicative of a current vehicle state, and determining a control command for the autonomous vehicle based at least in part on the desired vehicle state and the current vehicle state.