Yaw state update fixed