Adjust Poses to Obtain Accurate Trajectory