[小应用]GD32F103+MPU9150 四轴飞行器第一步：9DOF姿态融合

const float g_kp=2.0f;//加速度权重，越大则向加速度测量值收敛越快

const float g_ki=0.005f;//误差积分增益

const float g_period=0.04f;//单位秒

float q0=1,q1=0,q2=0,q3=0;

float exInt=0,eyInt=0,ezInt=0;//scaled integral error

void CCCOMDealDlg::Quaternion_CF(FLOAT_SENSORDATA stSensor, float *roll, float *pitch, float *yaw)

{

        float norm;

        float vx, vy, vz;

        float ex, ey, ez;

        float ax,ay,az;

        float gx,gy,gz;



        ax = stSensor.fAx;//加速度

        ay = stSensor.fAy;

        az = stSensor.fAz;

        gx = stSensor.fTx;//角速度

        gy = stSensor.fTy;

        gz = stSensor.fTz;



        // normalise the measurements(加计的三维向量转成单位向量)

        norm = sqrt(ax*ax + ay*ay + az*az);

        ax = ax / norm;

        ay = ay / norm;

        az = az / norm;



        // estimated direction of gravity(提取四元数的等效余弦矩阵中的重力分量)

        vx = 2*(q1*q3 - q0*q2);

        vy = 2*(q0*q1 + q2*q3);

        vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;





        // error is sum of cross product between reference direction of field and direction measured by sensor

        // 向量叉积得出姿态误差

        ex = (ay*vz - az*vy);

        ey = (az*vx - ax*vz);

        ez = (ax*vy - ay*vx);

        

        // integral error scaled integral gain

        // 对误差进行积分

        exInt = exInt + ex*g_ki;

        eyInt = eyInt + ey*g_ki;

        ezInt = ezInt + ez*g_ki;



        // adjusted gyroscope measurements

        // 互补滤波，姿态误差补偿到角速度上，修正角速度积分漂移

        gx = gx + g_kp*ex + exInt;

        gy = gy + g_kp*ey + eyInt;

        gz = gz + g_kp*ez + ezInt;



        // integrate quaternion rate and normalise

        // 一阶龙格库塔法更新四元数

        q0 = q0 + (-q1*gx - q2*gy - q3*gz)*g_period/2;

        q1 = q1 + (q0*gx + q2*gz - q3*gy)*g_period/2;

        q2 = q2 + (q0*gy - q1*gz + q3*gx)*g_period/2;

        q3 = q3 + (q0*gz + q1*gy - q2*gx)*g_period/2; 



        // normalise quaternion

        // 四元数归一化

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

        q0 = q0 / norm;

        q1 = q1 / norm;

        q2 = q2 / norm;

        q3 = q3 / norm;



        //TRACE("%.2f %.2f %.2f %.2f\n", q0, q1, q2, q3);



        float radian;

        //四元数转欧拉角

    if(roll)

        {

                //radian = atan2f(2.0f*(q0*q1 + q2*q3), 1 - 2.0f*(q1*q1 + q2*q2));

                radian = atan2f(-2.0f*(q1*q3 - q0*q2),1 - 2.0f*(q1*q1 + q2*q2));

        *roll = radian*180/3.14f;

    }

    if(pitch)//俯仰角

        {

        // 使用safe_asin()来处理pitch接近90/-90时的奇点

                //radian = asin(2.0f*(q0*q2 - q1*q3));

                radian = asin(2.0f*(q2*q3 + q0*q1));

        *pitch = radian*180/3.14f;

    }

    if(yaw)//航向角

        {

                //radian = atan2f(2.0f*(q1*q2 - q0*q3), 2.0f*(q0*q0 + q1*q1) - 1);

                radian = atan2f(2.0f*(q1*q2 - q0*q3),  1- 2.0f*(q1*q1 + q3*q3));

                *yaw = radian*180/3.14f;

    }

        //TRACE("%.2f\t%.2f\t%.2f\n", *roll, *pitch, *yaw);

}