I’ve been running your AHRS code with the MinIMU 9 v5 at 400kHz I2C speeds on an Arduino Mega2560 with no issues, but I recently switched over to an Arduino MKR1000 where, at the fast speed, the yaw spins at about 1 rotation every 3-5 seconds when the IMU is stationary. If I reduce the I2C speed to 100kHz, things are fine again. Have you seen this before?
FYI, I shortened the wires to the MCU to 3" from about 7" as a part of the transition to the MKR1000, and to test if that caused the problem I fell back to the Mega2560, but it still ran properly at the fast speed. I also begin each test with compass calibration and give the DCM algorithm 5 seconds to settle down. Could it be the shift to 3.3V? Or the additional 4.7k I2C pullups on the MKR1000?