从陀螺仪数据中找到四元数？

Question

我一直在尝试构建一个过滤器，它可以成功地将罗盘、地磁和陀螺数据结合起来，从而产生一种平滑的增强现实体验。在阅读了这的帖子后，经过大量的讨论，我终于找到了一个很好的算法来校正我的传感器数据。我读过的大多数例子都展示了如何用陀螺仪校正加速度计，而不是用陀螺仪校正罗盘+加速度计数据。这是我已经确定的算法，它的工作原理很好，但是如果我不朝北看的话，我会碰到万向节锁。这个算法是平衡过滤器，而不是只在3D中实现。

初始化步骤：

• 使用(噪声)加速度计和罗盘传感器数据初始化一个世界旋转矩阵(这已经由Android提供)

更新步骤：

• 集成每个轴(x，y，z)的陀螺仪读数(time_delta *读数)
• 使用积分提供的欧拉角旋转世界旋转矩阵
• 从新旋转矩阵中找出四元数
• 从未经过滤的加速度计+指南针数据中找到旋转矩阵(使用OS提供的功能，我认为它使用角度/轴计算)
• 从上一步生成的矩阵中获取四元数。
• 在步骤2中产生的四元数(从陀螺仪产生的)和加速度计数据之间的斜度，使用一个基于实验魔术的系数。
• 转换回一个矩阵，并使用它来绘制场景。

我的问题是，当我面对北方，然后试图往南看的时候，整个东西都爆炸了，似乎是万向节锁。经过几个万向节锁，整个过滤器处于一个未定义的状态。在四处搜索时，我听到每个人都说“只使用四元数”，但恐怕这并不是那么简单(至少对我来说不是这样)，而且我知道我只是缺少了一些东西。任何帮助都将不胜感激。

Answer 1

使用四元数的最大原因是避免欧拉角的奇异性问题。你可以用陀螺数据直接旋转四元数。

Answer 2

许多假设-如果信息被延迟或没有具体用途，但可能对其他人有用-正如我在一些研究之后发现的：：

a.使用kalman (线性或非线性)滤波器进行以下操作：

陀螺仪用来积分三角角，而加速度计则告诉你外部极限。

b.欧拉速率与陀螺角度变化率不同，因此需要四元数或欧拉表示：

四元数不是平凡的，但是有两个主要步骤

1. For Roll, pitch,yaw   you get three quaternions as cos(w) +sin(v) where w is scalar part and v is vector part (or when coding just another variable) 
 Then simply multiply all 3 quat. to get a delta quaternion
 i.e   quatDelta[0] =c1c2*c3 - s1s2*s3;
  quatDelta[1] =c1c2*s3 + s1s2*c3;
  quatDelta[2] =s1*c2*c3 + c1*s2*s3;
  quatDelta[3] =c1*s2*c3 - s1*c2*s3;
where c1,c2,c3 are cos of roll,pitch,yaw and s stands for sin of the same actually half of those gyro pre integrated angles.

2. Then just multiply by old quaternion you had

  newQuat[0]=(quaternion[0]*quatDelta[0] - quaternion[1]*quatDelta[1] - quaternion[2]*quatDelta[2] - quaternion[3]*quatDelta[3]);
  newQuat[1]=(quaternion[0]*quatDelta[1] + quaternion[1]*quatDelta[0] + quaternion[2]*quatDelta[3] - quaternion[3]*quatDelta[2]);
  newQuat[2]=(quaternion[0]*quatDelta[2] - quaternion[1]*quatDelta[3] + quaternion[2]*quatDelta[0] + quaternion[3]*quatDelta[1]);
  newQuat[3]=(quaternion[0]*quatDelta[3] + quaternion[1]*quatDelta[2] - quaternion[2]*quatDelta[1] + quaternion[3]*quatDelta[0]);

在循环代码时，它会被更新，因此只有quatenion是全局变量，而不是其他变量。

3. Lastly if you want Euler angles from them then do the following:


`euler[2]=atan2(2.0*(quaternion[0]*quaternion[1]+quaternion[2]*quaternion[3]), 1-2.0*(quaternion[1]*quaternion[1]+quaternion[2]*quaternion[2]))euler[1]=safe_asin(2.0*(quaternion[0]*quaternion[2] - quaternion[3]*quaternion[1]))euler[0]=atan2(2.0*(quaternion[0]*quaternion[3]+quaternion[1]*quaternion[2]), 1-2.0*(quaternion[2] *quaternion[2]+quaternion[3]*quaternion[3]))`

euler[1] is pitch and so on.. 

我只想概述四元数实现的一般步骤。可能有一些小的错误，但我自己尝试了，它的工作。请注意，当转换为欧拉角时，你会得到奇点，也称为"Gimbal锁“。

这里的一个重要提示是，这不是我的工作，但我在网上找到了它，并想感谢谁做过这个无价的code...Cheers。