In one embodiment, a lateral drifting error is determined based on at least a current location of an ADV. The lateral drifting error is segmented into a first drifting error and a second drifting error using a predetermined segmentation algorithm. A planning module plans a path or trajectory for a current driving cycle (e.g., planning cycle) to drive the ADV from the current location for a predetermined period of time. The planning module performs a first drifting error correction on the trajectory by modifying at least a starting point of the trajectory based on the first drifting error to generate a modified trajectory. A control module controls the ADV to drive according to the modified trajectory, including performing a second drifting error correction based on the second drifting error.