陀螺仪和加速度计互补最后得到欧拉角（Mahony算法）

版权声明：spark https://blog.csdn.net/qq_35619728/article/details/89846633

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控制陀螺仪偏差的收敛速率 ）