陀螺儀和加速度計互補最後得到歐拉角（Mahony算法）

q:對一個六軸陀螺儀如何解出相對穩定的歐拉角?

互補濾波姿態估計

先附上一段代碼

/**
 * 6DOF 互補濾波姿態估計(via Mahony)
 * @param[in] halfT:狀態估計週期的一半
 */
const float Kp = 3.5, Ki = 0.05;
float exInt, eyInt, ezInt;
float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // roll,pitch,yaw 都爲 0 時對應的四元數值。
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;

    norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q0 = q0/norm;
    q1 = q1/norm;
    q2 = q2/norm;
    q3 = q3/norm;

    roll =  atan2f(2*q2*q3 + 2*q0*q1, -2*q1*q1 - 2*q2*q2 + 1)*57.3;     
    pitch =  asinf(2*q1*q3 - 2*q0*q2)*57.3;                                                          
    yaw  =  -atan2f(2*q1*q2 + 2*q0*q3, -2*q2*q2 -2*q3*q3 + 1)*57.3; 
}

這裏簡單說明一下，先不管四元數是啥，目前可以粗略的定義四元數是一個用角速度來表徵姿態的方法。

首先通過加速度計測得的三個量ax,ay,az比較準確，而由陀螺儀解算出的姿態又能反推此時三個軸理論上能夠到多少加速度。分別爲ax1,ay1,az1.那麼這兩個向量之間就存在誤差ex,ey,ez.那麼這裏定義這個誤差爲兩個向量的叉乘。也就是$模的積乘上sin\theta$由於兩個都是單位向量，所以就是$sin\theta$
然而在這個角度比較小的時候，我們可以近似的吧這兩個等價$\theta ---------sin\theta$
就可以再用這角度的偏差修正新的姿態。這樣一來，我們就巧妙地用數據相對精準的加速度計的數據來修正偏差較大的陀螺儀的數據。（這裏有個小知識點，得到了誤差後用這個誤差和比例積分兩個環節來更新姿態，ω=ω+kp∗θacc ，1、 比例增益kp控制收斂到加速度計速率 2、 積分增益ki控制陀螺儀偏差的收斂速率 ）