先说什么叫六轴融合？
在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
