IMU学名惯性测量单元，所有的运动都可以分解为一个直线运动和一个旋转运动，故这个惯性测量单元就是测量这两种运动，直线运动通过加速度计可以测量，旋转运动则通过陀螺。
void IMUupdate(float gx, float gy, float gz, float ax,float ay, float az)
{
float norm;
float vx, vy, vz;
float ex, ey, ez;
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q1q1 = q1*q1;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
if(ax*ay*az==0)
return;
// 第一步：对加速度数据进行归一化
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
// 第二步：DCM矩阵旋转
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;
// 第三步：在机体坐标系下做向量叉积得到补偿数据
ex = ay*vz - az*vy ;
ey = az*vx - ax*vz ;
ez = ax*vy - ay*vx ;
// 第四步：对误差进行PI计算，补偿角速度
exInt = exInt + ex * Ki;
eyInt = eyInt + ey * Ki;
ezInt = ezInt + ez * Ki;
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
// 第五步：按照四元数微分公式进行四元数更新
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
}1. 数据规范化
在使用一个算法之前首先要弄清楚数据数据的单位是什么，在互补滤波算法中，角速度的单位是“弧度每秒”，加速度是数据是归一化后的数据，无单位。
2. DCM旋转矩阵
通过旋转矩阵计算标准加速度在当前姿态下的理论加速度分量，其中旋转矩阵通过四元素得到。（四元数，欧拉角，和旋转矩阵之间可以相互转换）。
3. 通过向量叉积得到误差
这个误差指的是两个向量的不平行程度，和夹角的正弦成正比，叉乘=a*b*sin(角度),二两个向量又归一化了，所以就和sin(角度成正比)，进而反应了陀螺仪估计的姿态和加速度计测量的姿态误差。Mahony那篇论文用叉乘来表示误差是有其物理意义的。一般来说向量的误差不是用叉乘来表示，直接作减法会更合适。而单位向量的向量叉积在物理意义上在小角度意义就是轴角，轴角本身就刻画了旋转，也就是叉乘的结果可以看成是旋转误差，在这个物理意义下比任何误差表示都要合理和切合实际系统。
4. PID调节
5. 四元素微分公式
利用PID补偿后的角速度对四元数进行更新。