加速计陀螺仪6轴数据融合算法解析

  四旋翼和云台都离不开加速计和陀螺仪。加速计提供实时的重力加速度方向，进而可以计算出机体姿态角度。陀螺仪提供实时的旋转角度，通过积分也能换算成机体姿态角度。加速计对机体振动非常敏感，稍有扰动，加速度方向就会偏移（外力与重力的合力），但是长期运行时，经过平滑滤波算法之后的加速计数据就比较可信。相比而言，陀螺仪则对外部扰动不敏感，反而是长时间积分后会出现角度漂移。因此实际应用中，都是将二者采集的数据进行融合，相互取长补短。

  下面这段代码是网上比较常见的6轴融合算法，原作者只添加了几句简单的英文注释，几处关键段落阅读起来非常费解。而网上的大多数引用页面对这段代码也都没有做进一步的分析，我怀疑大家也都是一知半解，拿来就用，用得顺手便没有再去深究。这个周末我仔细研究了一下，对各关键公式进行了理论推导，供大家参考。三个关键段落的推导过程详见后文Note1，Note2，Note3。

void IMUupdate() {  
        float norm;  
        float vx, vy, vz;  
        float ex, ey, ez;  
        float ax,ay,az,gx,gy,gz;  
        float q0,q1,q2,q3;  
        float q0temp,q1temp,q2temp,q3temp;  
         
        ax=Data.pSensor->accel[0];  
        ay=Data.pSenso