Mahony算法进行姿态解算。