Revert "Major update to implement Madgwicks Gradient Descent MARG implementation"

This reverts commit 747b9137786ef5fbb3ecd6866c106402d9838f58.
Revert "Revert "Major update to implement Madgwicks Gradient Descent MARG implementation""

This reverts commit 0b59d1a59a82b5c3613c52c596fefe77a0c93c93.