navigation bugfix: correct yaw in last pose of path