Some tunings:
-yaw does not affect motor speed so much
-change scale of PID constants KD