六轴融合算法

先说什么叫六轴融合？

在3Dof姿态追踪功能中，最主要的传感器就是陀螺仪（Gyroscope），它可以提供3个轴的角加速度，对时间进行积分，就可以得出物体旋转的方向角度。但是因为硬件精度等各方面原因，会产生误差，随着时间的累积，计算得到的角度误差就会越来越大，即产生漂移。

为了防止漂移，这就引入了另一个传感器，加速度计（Accelerometer）。在一般的3Dof运动中，由运动产生的加速度较少，对物体影响最大的是重力，而重力始终是垂直向下。引入Acc的本质就是利用重力来做参考，对Gyro计算的数据进行校正。

所谓六轴融合，就是Gyro三个轴+Acc三个轴，计算得到3Dof姿态。

值得注意的是：

1. 重力只能校准Roll和Pitch方向，Yaw方向依然会有误差，还要再引入地磁计（Magnetic），那就是九轴融合算法了。在Android上，SensorManager提供了注册监听的类型，其中TYPE_GAME_ROTATION_VECTOR就是用的六轴融合，TYPE_ROTATION_VECTOR用的就是九轴融合。

2. 因为硬件本身的精度，还有设备组装等方面的原因，传感器必然存在原始的误差，需要做零漂校准。也就是尽量保证设备在水平静置状态下，Gyro数据都是0，Acc只有垂直水平面的轴数据为重力值，其余为0。

（很惭愧，这两样我都没有做过）

下面是摘自网上的一个简单算法，还有好多不懂的地方，先记下来，慢慢再研究。

public class IMUTest {
    private final float Kp = 4.0f;   //Kp越大代表重力校正的权重越大，过大可能产生抖动。
    private final float Ki = 0.001f; //Ki应该是零漂校准产生的，没仔细研究过还不确定，先用个较小的值，没啥影响
    private float q0, q1, q2, q3; //依次对应四元数的w,x,y,z
    private float exInt, eyInt, ezInt;
    private long lastTime;

    public IMUTest() {
        q0 = 1.f;
        q1 = 0.f;
        q2 = 0.f;
        q3 = 0.f;
        exInt = 0.f;
        eyInt = 0.f;
        ezInt = 0.f;
    }

    public void update(float GyroX, float GyroY, float GyroZ, float AccX, float AccY, float AccZ, long timeMS) {
        long diff = timeMS - lastTime;
        lastTime = timeMS;
        float halfT = diff * 0.001f * 0.5f;//单位毫秒转为秒

        //加速度计处于自由落体状态时，不进行姿态解算，因为三个轴都为0，会产生分母无穷大的情况
        if ( Math.abs(AccX * AccY * AccZ) < 0.000001f) return;

        float norm; //矢量的模或四元数的模

        //归一化加速度计，这样变更量程也不需要修改Kp参数
        norm = (float)Math.sqrt(AccX * AccX + AccY * AccY + AccZ * AccZ);
        float ax = AccX / norm;
        float ay = AccY / norm;
        float az = AccZ / norm;
        //用当前姿态计算出重力在三个轴上的分量，
        //参考坐标n系转化到载体坐标b系，用四元数表示的方向余弦矩阵第三列即是，为啥？
        float vx = 2.f * (q1 * q3 - q0 * q2);
        float vy = 2.f * (q0 * q1 + q2 * q3);
        float vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
        //计算传感器测量重力与姿态计算的重力之间的误差，叉乘可以表示这一误差，又是为啥？
        float ex = ay * vz - az * vy;
        float ey = az * vx - ax * vz;
        float ez = ax * vy - ay * vx;
        //对误差进行积分
        exInt = exInt + ex * Ki;
        eyInt = eyInt + ey * Ki;
        ezInt = ezInt + ez * Ki;
        //将误差PI后补偿到陀螺仪，即补偿零点漂移
        //这里的水平旋转方向由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减，所以应该直接用GyroX？
        float gx = GyroX + Kp * ex + exInt;
        float gy = GyroY + Kp * ey + eyInt;
        float gz = GyroZ + Kp * ez + ezInt;

        //下面进行姿态的更新，也就是四元数微分方程的求解
        float q0temp = q0;
        float q1temp = q1;
        float q2temp = q2;
        float q3temp = q3;
        //采用一阶毕卡解法，相关知识可参见《惯性器件与惯性导航系统》P212
        q0 = q0temp + (-q1temp * gx - q2temp * gy - q3temp * gz) * halfT;
        q1 = q1temp + (q0temp * gx + q2temp * gz - q3temp * gy) * halfT;
        q2 = q2temp + (q0temp * gy - q1temp * gz + q3temp * gx) * halfT;
        q3 = q3temp + (q0temp * gz + q1temp * gy - q2temp * gx) * halfT;
        //对四元数做归一化，单位化四元数在空间旋转时不会拉伸，仅有旋转角度，这类似线性代数里的正交变换
        norm = (float)Math.sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
        q0 /= norm;
        q1 /= norm;
        q2 /= norm;
        q3 /= norm;
    }
}

参考自https://blog.csdn.net/asr9k/article/details/53258308?utm_medium=distribute.pc_relevant_t0.none-task-blog-BlogCommendFromMachineLearnPai2-1.control&dist_request_id=ccb9df40-57cb-4714-873f-727e69a96dee&depth_1-utm_source=distribute.pc_relevant_t0.none-task-blog-BlogCommendFromMachineLearnPai2-1.control