多传感器融合算法
多传感器数据融合算法---9轴惯性传感器 - hmmwjs的博客 - CSDN博客
加速度计模型
每个ADC模块都有一个参考电压，假设它是3.3V。要将一个10位的ADC值转成电压值，使用下列公式： VoltsRx = AdcRx * VREF / 1023（小注：8位ADC的最大值是255 = 2 ^ 8 -1，12位ADC最大值是4095 = 2 ^ 12 -1）。每个加速度计都有一个零加速度的电压值称为VzeroG，你可以在它的说明书中找到，这个电压值对应于加速度为0g，利用公式DeltaVoltsRx = VoltsRx - VzeroG 可以得到相对0g电压点的偏移量。
在得到了加速度计的电压值后，利用加速度计的灵敏度Sensitivity可将电压值转化成为单位为g（9.8m/s^2）使用公式为RX = DeltaVoltsRx /Sensitivit可以获得每个方向的加速度值。
综合上述公式，即可得到怎么读取一个ADC值并将其转换为单位为g的矢量力的分量。
Rx = (AdcRx * Vref / 1023 – VzeroG) / Sensitivity
Ry = (AdcRy * Vref / 1023 – VzeroG) / Sensitivity
Rz = (AdcRz * Vref / 1023 – VzeroG) / Sensitivity。
利用反三角函数计算向量R在每个轴上的角度：Axr = arccos(Rx/R)，Ayr = arccos(Ry/R)，Azr = arccos(Rz/R)。
陀螺仪模型
陀螺仪测试向量R与各轴之间的角度的变化率
陀螺仪输出值即AdcGyroXZ（由ADC模块得到）转换中的ADC部分（假设使用10位ADC模块，如果是8位ADC，用1023代替255，如果是12为ADC用4095代替1023），将其单位转换成为度/秒，公式为RateAxz = (AdcGyroXZ * Vref / 1023 – VzeroRate) / Sensitivity。其中VzeroRate 是零变化率电压，换句话说它是陀螺仪不受任何转动影响时的输出值，通常可以由说明书得到，需要校准，而Sensitivity为陀螺仪的灵敏度，单位为mV/deg/s,它的意思就是如果旋转速度增加1°/s,陀螺仪的输出就会增加多少mV。
例子：假设我们的ADC模块返回以下值：AdcGyroXZ = 571，AdcGyroXZ = 323
用上面的公式，在代入Acc Gyro板的参数，可得：
RateAxz = (571 * 3.3V / 1023 – 1.23V) / ( 0.002V/deg/s) =~ 306 deg/s
RateAyz = (323 * 3.3V / 1023 – 1.23V) / ( 0.002V/deg/s) =~ -94 deg/s
即此设备绕Y轴（也可以说在XZ平面内）以306°/s速度和绕X轴（或者说YZ平面内）以-94°/s的速度旋转。
加速度计和陀螺仪融合算法
1.统一坐标轴
一般将加速度计作为参考坐标
2. 确定陀螺仪的输出对应到RateAxz，RateAyz的值
3.根据陀螺仪和加速度计的位置决定是否要反转输出值
4.算法处理
由于加速度计和陀螺仪会受到许多因素的干扰，所以在使用时要利用一定的滤波算法来处理噪声。由第一部分计算得到的数据容易受到噪声的干扰，称之为Racc – 是由加速度计测量到得惯性力矢量，它可分解为下面的分量RxAcc，RyAcc，RzAcc。
这组数据是只来自于加速度计ADC的值，我们把这组数据叫做“vector”，并使用下面的符号：Racc = [RxAcc,RyAcc,RzAcc]。我们引进一个新的变量Rest = [RxEst,RyEst,RzEst]来作为算法计算的值，它是经过陀螺仪数据的修正和基于上一次估算的值。
开始时规定加速度计输出为准确值，Rest[0] = Racc[0]，每隔时间T就得到新的值即为Racc（1），Racc（2）和新的估算值Rest（1），Rest（2）等，假设我们在第n步。我们有两列已知的值可以用：Rest(n-1) – 前一个估算值，Rest(0) = Racc(0)，Racc(n) – 当前加速度计测量值，在计算Rest（n）前，我们先引进一个新的值Rgyro，它可由陀螺仪和前一个估算值得到，同样它是个矢量并由3个分量组成（Rgyro = [RxGyro,RyGyro,RzGyro]）。
首先观察陀螺仪模型中下面的关系，根据由Rz和Rxz组成的直角三角形我们能推出：tan(Axz) = Rx/Rz => Axz = atan2(Rx,Rz)，atan2函数和atan类似，但atan返回值范围是（-PI/2,PI/2），atan2返回值范围是（-PI，PI），并且有两个参数。它能将Rx，Rz值转换成360°（-PI，PI）内的角度。知道了RxEst（n-1）和RzEst（n-1）我们发现：Axz(n-1) = atan2( RxEst(n-1) , RzEst(n-1) )
陀螺仪测量的是Axz角度变化率，因此，我们可以按如下方法估算新的角度Axz（n）：Axz(n) = Axz(n-1) + RateAxz(n) * T，RateAxz可由陀螺仪ADC读取得到。
通过使用平均转速可由得到一个更准确的公式：
RateAxzAvg =（RateAxz（N）+ RateAxz（N-1））/ 2
就得到了这两个值
Axz(n) = Axz(n-1) + RateAxzAvg * T
Ayz(n) = Ayz(n-1) + RateAyz(n) * T。
现在推导RxGyro/RyGyro，将RGyro标准化为| Rgyro | = sqrt（RxGyro ^ 2 + RyGyro ^ 2 + RzGyro ^ 2），经过公式带入和简化后可得到
RxGyro = sin(Axz(n)) / SQRT (1 + cos(Axz(n))^2 * tan(Ayz(n))^2 )
RyGyro = sin(Ayz(n)) / SQRT (1 + cos(Ayz(n))^2 * tan(Axz(n))^2 )
RzGyro = Sign(RzGyro)*SQRT(1 – RxGyro^2 – RyGyro^2) 其中，当 RzGyro>=0时，Sign(RzGyro) = 1 , 当 RzGyro<0时，Sign(RzGyro) = -1 。Rz可以用作计算Axz和Ayz倾角的参考
然后采用Racc和RGyro的加权平均值来更新Rest（n）即Rest(n) = (Racc * w1/w1 + Rgyro * w2/w1 ) / (w1/w1 + w2/w1)，令w2=w1=wGyro，最后可得：
Rest(n) = (Racc + Rgyro * wGyro ) / (1 + wGyro) 其中wGyro表示我们对加速度计和陀螺仪的相信程度，这个值在5到20之间取会比较好。
得到了Rest（n）后就可以算出向量的投影算出最新的估算值了：
RxEst(n) = (RxAcc + RxGyro * wGyro ) / (1 + wGyro)
RyEst(n) = (RyAcc + RyGyro * wGyro ) / (1 + wGyro)
RzEst(n) = (RzAcc + RzGyro * wGyro ) / (1 + wGyro)
最后得到最新的标准化矢量
R = SQRT(RxEst(n) ^2 + RyEst(n)^2 + RzEst(n)^2 )
RxEst(n) = RxEst(n)/R
RyEst(n) = RyEst(n)/R
RzEst(n) = RzEst(n)/R。
