fixed yaw/ideal yaw problem correctly