EEPW论坛

参考【讨论帖】姿态解算相关知识储备http://forum.eepw.com.cn/thread/250804/1 中的算法，我是否可以理解为：

下面代码中的ax是X轴包含了重力加速度的加速度值！！！

                     ex是X轴的重力加速度的值！！！

用  ax - ex 所得的值就是X轴上面除去重力加速度后的真实加速度值？？？

请大家多多指教！谢谢！

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) {
        float norm;
        float vx, vy, vz;
        float ex, ey, ez;        
      
        // 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;

这是把四元数换算成《方向余弦矩阵》中的第三列的三个元素。
根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。
所以这里的vx\y\z，其实就是当前的欧拉角（即四元数）的机体坐标参照系上，换算出来的重力单位向量。

        // 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);