Estimation and Filters added

Estimation now works with the Madgwick algorithm.