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