That is much better. We can probably come up with another initialization algorithm that will first calculate approximate yaw from mag and accelerometer and use it as initial value.
If you don’t mind I would really like to add your changes to our repository. Could you send a pull request on GitHub? Let me know if I could somehow help with that.
Thanks for making it work with magnetometer!