fix dt position update bug, stepwise yaw controlling