Hello.
I am sorry you are having trouble with the AHRS code on your AltIMU. The behavior you are describing with the yaw axis sounds like an issue from poorly calibrating your magnetometer. You might try calibrating again to try and get a more accurate set of minimum and maximum calibration constants. In general, when calibrating, it might help imagine that the AltIMU is the tip of a paintbrush and while the calibration routine is running, your goal is to cover the entire inside of a sphere with paint.
If that does not improve the yaw issues enough, you might consider trying other more involved calibration methods, like what Jim Remington mentions in this thread.
-Jon