I'm using your version of the Madgwick AHRS library (https://github.com/PaulStoffregen/MadgwickAHRS) and after some tweaks it seems to work very well, at least as good as the Freescale Kalman filters. However I have seen some articles (dated 2017) on various websites that there we some errors in in the algorithm and that there was a new version but I have not found any update.

I thought that when I saw this library that it had fixed the known problem with the matrix arithmetic. Has it?

I tried someone's supposed update and it didn't work for me so I abandoned it!

I have modified the Madgwick filter initialisation to make it lock on to the current orientation at start-up. It now locks on immediately rather 10+s and takes around 1mS to process a sample and compared with 14mS for the Kalman filter using a 48MHz processor.