四元数姿态解算及多传感器融合详细解析

void AHRS_update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {  

    float recipNorm;  

    float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;    

    float hx, hy, bx, bz; /*地理坐标系下的磁强度值*/

    float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;/*根据陀螺仪更新的四元数估计出的机体坐标系下的加速度值和磁传感器值*/

    float halfex, halfey, halfez;  /*根据实际测得的加速度值和磁传感器值和估计得出的上述值叉乘得到的估计误差*/

    float qa, qb, qc;  /*四元数*/

  

    // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)  

    if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {  

        MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az);  /*如果测量的磁传感器值为0，则只需要用加速度值对陀螺数据进行补偿*/

        return;  

    }  

  

    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)  

    if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {  

  

        /* 对重力加速度进行归一化*/

        recipNorm = invSqrt(ax * ax + ay * ay + az * az);  

        ax *= recipNorm;  

        ay *= recipNorm;  

        az *= recipNorm;       

         /*归一化结束*/

 

        /* 对磁传感器进行归一化*/

        recipNorm = invSqrt(mx * mx + my * my + mz * mz);  

        mx *= recipNorm;  

        my *= recipNorm;  

        mz *= recipNorm;     

         /*归一化结束*/

 

      /*提前进行四元数的相关运算，为后面的矩阵旋转做准备，更便于与推导矩阵风格统一

        q0q0 = q0 * q0;  

        q0q1 = q0 * q1;  

        q0q2 = q0 * q2;  

        q0q3 = q0 * q3;  

        q1q1 = q1 * q1;  

        q1q2 = q1 * q2;  

        q1q3 = q1 * q3;  

        q2q2 = q2 * q2;  

        q2q3 = q2 * q3;  

        q3q3 = q3 * q3;