Ported implementation of Seb Madgwick's MARG algorithm given to me by someone I met online (Jonny Barker) to be compatible with our platform and to use Kevin's Vector/Quaternion Library