... or at least closer to it.
We were hard-resetting the yaw to zero when the vehicle was upright. That makes for huge simulated gyro rates, and that means the differences between the gyros can be huge sample-to-sample, so we can get gyros-inconsistent errors.
Fix things so we don't reset yaw at the same time as pitch, and also twist the vehicle to point North again when disarmed.
The hard-reset of all state variables means we feed the EKF inconsistent gyro data. Attempt to upright the vehicle with a simple p-gain when it is disarmed, as if someone is pushing the thing back upright