关于九轴陀螺仪的数据融合问题

Question

最近在弄九轴陀螺仪的数据融合，用的AHRS算法，获取数据后，如果不加磁力计数据，roll轴和pitch轴数据都还正常，yaw轴会一直偏移，可是加上磁力计后数据就不正常了，会响应很慢，而且还各个轴输出数据都不准确。用的就是网上开源的数据融合代码。想问问可能是哪些方面的问题？

/* Feed the sensor data in NED(North East Down) reference frame. Rotation can be added. */
int AHRS_Update2(fp32 QUAT[4], fp32 ACC[3], fp32 GYRO[3], fp32 MAG[3]) 
{   
    float recipNorm;
    float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;  
    float hx, hy, hz, bx, bz;
    float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
    float halfex, halfey, halfez;
    float qa, qb, qc;

    if (MAG == NULL)
        return AHRS_UpdateIMU(QUAT, ACC, GYRO);


    float mx = MAG[0];
    float my = MAG[1];
    float mz = MAG[2];

    // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
    if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
        return AHRS_UpdateIMU(QUAT, ACC, GYRO);
    }

    float ax = ACC[0];
    float ay = ACC[1];
    float az = ACC[2];

    float gx = GYRO[0];
    float gy = GYRO[1];
    float gz = GYRO[2];

// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
    // 只在加速度计数据有效时才进行运算
    if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

        // Normalise accelerometer measurement
        // 将加速度计得到的实际重力加速度向量v单位化
        recipNorm = invSqrt(ax * ax + ay * ay + az * az);
        ax *= recipNorm;
        ay *= recipNorm;
        az *= recipNorm;     

        // Normalise magnetometer measurement
        // 将磁力计得到的实际磁场向量m单位化
        recipNorm = invSqrt(mx * mx + my * my + mz * mz);
        mx *= recipNorm;
        my *= recipNorm;
        mz *= recipNorm;   

        // Auxiliary variables to avoid repeated arithmetic
        // 辅助变量，以避免重复运算
        q0q0 = QUAT[0] * QUAT[0];
        q0q1 = QUAT[0] * QUAT[1];
        q0q2 = QUAT[0] * QUAT[2];
        q0q3 = QUAT[0] * QUAT[3];
        q1q1 = QUAT[1] * QUAT[1];
        q1q2 = QUAT[1] * QUAT[2];
        q1q3 = QUAT[1] * QUAT[3];
        q2q2 = QUAT[2] * QUAT[2];
        q2q3 = QUAT[2] * QUAT[3];
        q3q3 = QUAT[3] * QUAT[3];

        // Reference direction of Earth's magnetic field
        // 通过磁力计数据与坐标转换矩阵得到理论磁场向量
        // 公式（5）
        hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
        hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
        hz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
        // 公式（6）
        bx = sqrt(hx * hx + hy * hy);
        bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));

        // Estimated direction of gravity and magnetic field
        // 将理论重力加速度向量与理论磁场向量通过坐标转换矩阵转换至机体坐标系
        // 注意，这里实际上是矩阵*1/2，在开头对Kp Ki的宏定义均为2*增益
        // 至于这么设计程序的原因笔者也没有弄清，猜测可能是为减少乘法运算量？
        // 笔记（一）中内容
        halfvx = q1q3 - q0q2;
        halfvy = q0q1 + q2q3;
        halfvz = q0q0 - 0.5f + q3q3;
        // 公式（7）
        halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
        halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
        halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);  

        // Error is sum of cross product between estimated direction and measured direction of field vectors
        // 通过向量外积得到重力加速度向量和磁场向量的实际值与测量值直接误差
        // 公式（8）（9）（10）
        halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
        halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
        halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);

        // Compute and apply integral feedback if enabled
        // 在误差补偿PI控制器中积分项使能情况下计算并应用积分项
        if(twoKi > 0.0f) {
            // integral error scaled by Ki
            // 积分过程
            integralFBx += twoKi * halfex * (1.0f / sampleFreq);    
            integralFBy += twoKi * halfey * (1.0f / sampleFreq);
            integralFBz += twoKi * halfez * (1.0f / sampleFreq);

            // apply integral feedback
            // 应用误差补偿中的积分项
            gx += integralFBx;  
            gy += integralFBy;
            gz += integralFBz;
        }
        else {
            // prevent integral windup
            // 避免为负值的Ki时积分异常饱和
            integralFBx = 0.0f; 
            integralFBy = 0.0f;
            integralFBz = 0.0f;
        }

        // Apply proportional feedback
        // 应用误差补偿中的比例项
        gx += twoKp * halfex;
        gy += twoKp * halfey;
        gz += twoKp * halfez;
    }

    // Integrate rate of change of quaternion
    //一阶龙格库塔法迭代四元数
    gx *= (0.5f * (1.0f / sampleFreq));     // pre-multiply common factors
    gy *= (0.5f * (1.0f / sampleFreq));
    gz *= (0.5f * (1.0f / sampleFreq));
    qa = QUAT[0];
    qb = QUAT[1];
    qc = QUAT[2];
    QUAT[0] += (-qb * gx - qc * gy - QUAT[3] * gz);
    QUAT[1] += (qa * gx + qc * gz - QUAT[3] * gy);
    QUAT[2] += (qa * gy - qb * gz + QUAT[3] * gx);
    QUAT[3] += (qa * gz + qb * gy - qc * gx); 

    // Normalise quaternion
    // 单位化四元数 保证四元数在迭代过程中保持单位性质
    recipNorm = invSqrt(QUAT[0] * QUAT[0] + QUAT[1] * QUAT[1] + QUAT[2] * QUAT[2] + QUAT[3] * QUAT[3]);
    QUAT[0] *= recipNorm;
    QUAT[1] *= recipNorm;
    QUAT[2] *= recipNorm;
    QUAT[3] *= recipNorm;

    return 0;
}

Answer 1

https://blog.csdn.net/weixin_43862847/article/details/87100953