Updated to use goal yaw