Do you have motors tilted in their respective yaw direction? Tilting motors reduces the need for rpm change when yawing, and the FC handles yaw much easier when only small rpm changes are needed to yaw. It also eliminates problems that occur if one motor is tilted slightly wrong by mistake.
I...