Added mahony algorithm for body quaternion calculation.