added madgwick algorithm