Started porting the Madgqwick MARG