Add DCM Mahony motion algorith just in case madgwick had a poor accurance on air