added the Yaw coordinate and fixed bugs