第三部分 将它们综合起来。融合加速度计和陀螺仪的数据。
如果你在阅读这篇文章你可能已经有了或准备购买一个IMU设备，或者你准备用独立的加速度计和陀螺仪搭建一个。
在使用整合了加速度计和陀螺仪的IMU设备时，首先要做的就是统一它们的坐标系。最简单的办法就是将加速度计作为参考坐标系。大多数的加速度计技术说明书都会指出对应于物理芯片或设备的XZY轴方向。例如，下面就是Acc Gyro板的说明书中给出的XYZ轴方向：
接下来的步骤是：
- 确定陀螺仪的输出对应到上述讨论的RateAxz，RateAyz值。
- 根据陀螺仪和加速度计的位置决定是否要反转输出值 不要设想陀螺仪陀的输出有XY，它会适应加速度计坐标系里的任何轴，尽管这个输出是IMU模块的一部分。最好的办法就是测试。
接下来的示例用来确定哪个陀螺仪的输出对应RateAxz。
- 首先将设备保持水平。加速度计的XY轴输出会是零加速度电压（Acc Gyro板的值是1.65V）
- 接下来将设备绕Y轴旋转，换句话说就是将设备在XZ平面内旋转，所以X、Z的加速度输出值会变化而Y轴保持不变。
-当以匀速旋转设备的时候，注意陀螺仪的哪个通道输出值变化了，其他输出应该保持不变。
- 在陀螺仪绕Y轴旋转（在XZ平面内旋转）的时候输出值变化的就是AdcGyroXZ，用于计算RateAxz
-最后一步，确认旋转的方向是否和我们的模型对应，因为陀螺仪和加速度的位置关系，有时候你可能要把RateAxz值反向
-重复上面的测试，将设备绕Y轴旋转，这次查看加速度计的X轴输出（也就是AdcRx）。如果AdcRx增大（从水平位置开始旋转的第一个90°），那AdcGyroXZ应当减小。这是因为我们观察的是重力矢量，当设备朝一个方向旋转时矢量会朝相反的方向旋转（相对坐标系运动）。所以，如果你不想反转RateAxz，你可以在公式3中引入正负号来解决这个问题： RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 – VzeroRate) / Sensitivity ,其中InvertAxz= 1 或-1
同样的方法可以用来测试RateAyz，将设备绕X轴旋转，你就能测出陀螺仪的哪个输出对应于RateAyz，以及它是否需要反转。一旦你确定了InvertAyz，你就能可以用下面的公式来计算RateAyz：
RateAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 – VzeroRate) / Sensitivity
如果对Acc Gyro板进行这些测试，你会得到下面的这些结果：
- RateAxz的输出管脚是GX4，InvertAxz = 1
- RateAyz输出管脚是GY4，InvertAyz = 1
从现在开始我们认为你已经设置好了IMU模块并能计算出正确的Axr，Ayr，Azr值（在第一部分加速度计中定义）以及RateAyz，RateAyz（在第二部分陀螺仪中）。下一步，我们分析这些值之间的关系并得到更准确的设备和地平面之间的倾角。
你可能会问自己一个问题，如果加速度计已经告诉我们Axr，Ayr，Azr的倾角，为什么还要费事去得到陀螺仪的数据？答案很简单：加速度计的数据不是100%准确的。有几个原因，还记加速度计测量的是惯性力，这个力可以由重力引起（理想情况只受重力影响），当也可能由设备的加速度（运动）引起。因此，就算加速度计处于一个相对比较平稳的状态，它对一般的震动和机械噪声很敏感。这就是为什么大部分的IMU系统都需要陀螺仪来使加速度计的输出更平滑。但是怎么办到这点呢？陀螺仪不受噪声影响吗？
陀螺仪也会有噪声，但由于它检测的是旋转，因此对线性机械运动没那么敏感，不过陀螺仪有另外一种问题，比如漂移（当选择停止的时候电压不会回到零速率电压）。然而，通过计算加速度计和陀螺仪的平均值我们能得到一个相对更准确的当前设备的倾角值，这比单独使用加速度计更好。
接下来的步骤我会介绍一种算法，算法受卡尔曼滤波中的一些思想启发，但是它更简单并且更容易在嵌入式设备中实现。在此之前，让我们先看看我们需要算法计算什么值。所要算的就是重力矢量R=[Rx,Ry,Rz]，它可由其他值推导出来，如Axr，Ayr，Azr或者cosX，cosY，cosZ，由这些值我们能得到设备相对地平面的倾角值，这些关系我们在第一部分已经讨论过。有人可能会说-根据第一部分的
公式2我们不是已经得到Rx，Ry，Rz的值了吗？是的，
但是记住，这些值只是由加速度计数据推导出来的，如果你直接将它们用于你的程序你会得到难以忍受的噪声。为了避免进一步的混乱，我们重新定义加速度计的测量值： Racc – 是由加速度计测量到得惯性力矢量，它可分解为下面的分量（在XYZ轴上的投影）：
RxAcc = (AdcRx * Vref / 1023 – VzeroG) / Sensitivity
RyAcc = (AdcRy * Vref / 1023 – VzeroG) / Sensitivity
RzAcc = (AdcRz * Vref / 1023 – VzeroG) / Sensitivity
现在我们得到了一组只来自于加速度计ADC的值。我们把这组数据叫做“vector”，并使用下面的符号：
Racc = [RxAcc,RyAcc,RzAcc]
因为这些Racc的分量可由加速度计数据得到，我们可以把它当做算法的输入。 请注意Racc测量的是重力，如果你得到的矢量长度约等于1g那么你就是正确的：
|Racc| = SQRT(RxAcc^2 +RyAcc^2 + RzAcc^2)
但是请确定把矢量转换成下面的矢量非常重要：
Racc(normalized) = [RxAcc/|Racc| , RyAcc/|Racc| , RzAcc/|Racc|].
这可以确保标准化Racc始终是1。
接来下我们引进一个新的向量： Rest = [RxEst,RyEst,RzEst]
这就是算法的输出值，它经过陀螺仪数据的修正和基于上一次估算的值。 这是算法所做的事：
-加速度计告诉我们：“你现在的位置是Racc” 我们回答：“谢谢，但让我确认一下”
-然后根据陀螺仪的数据和上一次的Rest值修正这个值并输出新的估算值Rest。 -我们认为Rest是当前设备姿态的“最佳值”。 让我们看看它是怎么实现的。
数列的开始，我们先认为加速度值正确并赋值： Rest(0) = Racc(0)
Rest和Racc是向量，所以上面的式子可以用3个简单的式子代替，注意别重复了：
RxEst（0）= RxAcc（0）
RyEst（0）= RyAcc（0）
RzEst（0）= RzAcc（0）
接下来我们在每个等时间间隔T秒做一次测量，得到新的测量值，并定义为Racc（1），Racc（2），Racc（3）等等。同时，在每个时间间隔我们也计算出新的估算值Rest（1）,Rest（2），Rest（3），等等。
假设我们在第n步。我们有两列已知的值可以用：
Rest(n-1) – 前一个估算值，Rest(0) = Racc(0) Racc(n) – 当前加速度计测量值
在计算Rest（n）前，我们先引进一个新的值，它可由陀螺仪和前一个估算值得到。 叫做Rgyro，同样它是个矢量并由3个分量组成： Rgyro = [RxGyro,RyGyro,RzGyro]
我们分别计算这个矢量的分量，从RxGyro开始。
首先观察陀螺仪模型中下面的关系，根据由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）内的角度。更多信息请阅读 atan2.
所以，知道了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
好了，现在我们有了Axz（n），Ayz（n）。现在我们如何推导出RxGyro/RyGyro?根据
公
式1我们可以把Rgyro长度写成下式：
| Rgyro | = SQRT（RxGyro ^ 2 + RyGyro ^ 2 + RzGyro ^ 2）
同时，因为我们已经将Racc标准化，我们可以认为它的长度是1并且旋转后保持不变，所以写成下面的方式相对比较安全： | Rgyro | = 1
我们暂时采用更短的符号进行下面的计算： x =RxGyro , y=RyGyro, z=RzGyro
根据上面的关系可得：
x = x / 1 = x / SQRT(x^2+y^2+z^2) 分子分母同除以SQRT（X ^ 2 + Z ^ 2）
x = ( x / SQRT(x^2 + z^2) ) / SQRT( (x^2 + y^2 + z^2) / (x^2 + z^2) )
注意x / SQRT(x^2 + z^2) = sin(Axz),
所以: x = sin(Axz) / SQRT (1 + y^2 / (x^2 + z^2) )
将SQRT内部分式的分子分母同乘以z^2
x = sin(Axz) / SQRT (1 + y^2 * z ^2 / (z^2 * (x^2 + z^2)) )
注意 z / SQRT(x^2 + z^2) = cos(Axz)， y / z = tan(Ayz),
所以最后可得: x = sin(Axz) / SQRT (1 + cos(Axz)^2 * tan(Ayz)^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 )
提示：这个公式还可以更进一步简化。分式两边同除以sin（axz（你））可得：
RxGyro = 1 / SQRT (1/ sin(Axz(n))^2 + cos(Axz(n))^2 / sin(Axz(n))^2 * tan(Ayz(n))^2 )
RxGyro = 1 / SQRT (1/ sin(Axz(n))^2 + cot(Axz(n))^2 * sin(Ayz(n))^2 / cos(Ayz(n))^2 )
现在加减 cos(Axz(n))^2/sin(Axz(n))^2 = cot(Axz(n))^2
RxGyro = 1 / SQRT (1/ sin(Axz(n))^2 - cos(Axz(n))^2/sin(Axz(n))^2 + cot(Axz(n))^2 * sin(Ayz(n))^2 / cos(Ayz(n))^2 + cot(Axz(n))^2 )
综合条件1、2和3、4可得：
RxGyro = 1 / SQRT (1 + cot(Axz(n))^2 * sec(Ayz(n))^2 ), 其中 cot(x) = 1 / tan(x) ， sec(x) = 1 / cos(x)
这个公式只用了2个三角函数并且计算量更低。如果你有Mathematica程序，通过使用 FullSimplify [Sin[A]^2/ ( 1 + Cos[A]^2 * Tan[B]^2)]你可以验证这个公式。
现在我们发现：
RzGyro = Sign(RzGyro)*SQRT(1 – RxGyro^2 – RyGyro^2).
其中，当 RzGyro>=0时，Sign(RzGyro) = 1 , 当 RzGyro<0时，Sign(RzGyro) = -1 。 一个简单的估算方法：
Sign(RzGyro) = Sign(RzEst(n-1))
在实际应用中，当心RzEst（n-1）趋近于0。这时候你可以跳过整个陀螺仪阶段并赋值：Rgyro=Rest（n-1）。Rz可以用作计算Axz和Ayz倾角的参考，当它趋近于0时，它可能会溢出并引发不好的后果。这时你会得到很大的浮点数据，并且tan()/atan()函数得到的结果会缺乏精度。
现在我们回顾一下已经得到的结果，我们在算法中的第n步，并计算出了下面的值： Racc – 加速度计读取的当前值
Rgyro –根据Rest(-1)和当前陀螺仪读取值所得
我们根据哪个值来更新Rest（n）呢？你可能已经猜到，两者都采用。我们会用一个加权平均值，得：
Rest(n) = (Racc * w1 + Rgyro * w2 ) / (w1 + w2) 分子分母同除以w1，公式可简化成：
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之间会得到一个很好的结果。
此算法和卡尔曼滤波最主要的差别是它的权重是相对固定的，而卡尔曼滤波中的权重会随着加速度计读取的噪声而改变。卡尔曼滤波注重给你一个“最好”的理论结果，而此算法给你的是实际项目中“够用”的结果。你可以实现一个算法，它能根据测量的噪声而改变wGyro值，但对大部分应用来说固定的权重也能工作的很好。 现在得到最新的估算值还差一步：
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
现在，可以再次进行下一轮循环了。
This guide originally appeared onstarlino.com, I've made a few light edits and re-posted it with permission. Thanks Starlino!
