Adjust position reference as function of position reference, feedback of speed and position