Madgwick changes, added correct parameters and fixed euler angle init bug
Merged with master_merge