A motor control system includes a shaft configured to be rotationally driven by a motor. A rotor position sensor is configured to detect rotation of the shaft and output a rotor position signal. A controller is in communication with the motor and the rotor position sensor. The controller converts the rotor position signal to an angle with error, tracks the angle with error to provide an angle that retains mechanical dynamics as stationary reference frame signals, transforms the stationary reference frame signals and the angle with mechanical dynamics to rotating reference frame signals, filters the rotating reference frame signals, transforms the filtered rotating reference frame signals to provide filtered stationary frame signals, and takes an arctangent of the filtered stationary reference frame signals to create a corrected motor control angle. The controller commands the motor based upon the corrected motor control angle.