Major update to implement Madgwicks Gradient Descent MARG implementation