采用Madgwick算法实现姿态解算,加速度计和磁强计采用梯度下降法用来补偿陀螺仪的误差。