Implemented the Madgwick filter taken directly from his original paper.  This option is set by setting MARG = 3