ios - Madgwick 在 iOS 上的传感器融合算法

Question

我正在尝试运行 Madgwick's sensor fusion algorithm在 iOS 上。由于代码是开源的，我已经将它包含在我的项目中，并使用提供的传感器值调用方法。

但似乎该算法期望传感器测量值在不同的坐标系中。 Apple CoreMotion 传感器系统位于右侧，Madgewick 位于左侧。这是different coordinate systems的图片.两个系统都遵循右手法则。
对我来说，似乎围绕 z 轴旋转了 90 度。但这没有用。



我还尝试按照 other stackoverflow posts 的建议翻转 x 和 y(并反转 z)轴对于 WP，但这也不起作用。那你有提示吗？
如果 Madgwick 的算法输出可以与 CoreMotion 输出 (CMAttitudeReferenceFrameXMagneticNorthZVertical) 在同一个系统中，那将是完美的。

此外，我正在寻找 的良好工作值(value)。 betaDef 在 iPhone 上。 betaDef 是一种比例增益，目前设置为 0.1f。

任何有关如何实现目标的帮助将不胜感激。

Answer 1

我不确定如何在 objective-c 中编写此代码，但这是我在 vanilla c 中完成坐标转换的方法。我还想旋转方向，使 +y 为北。这种翻译也反射(reflect)在下面的方法中。

此方法需要一个 wxyz 形式的 4 元素四元数，并以相同格式返回翻译后的四元数:

void madgeq_to_openglq(float *fMadgQ, float *fRetQ) {
  float fTmpQ[4];
  // Rotate around Z-axis, 90 degres:
  float fXYRotationQ[4] = { sqrt(0.5), 0, 0, -1.0*sqrt(0.5) };

  // Inverse the rotation vectors to accomodate handedness-issues:
  fTmpQ[0] = fMadgQ[0];
  fTmpQ[1] = fMadgQ[1] * -1.0f;
  fTmpQ[2] = fMadgQ[2];
  fTmpQ[3] = fMadgQ[3] * -1.0f;

  // And then store the translated Rotation into ret:
  quatMult((float *) &fTmpQ, (float *) &fXYRotationQ, fRetQ);
}

// Quaternion Multiplication operator. Expects its 4-element arrays in wxyz order 
void quatMult(float *a, float *b, float *ret) {
  ret[0] = (b[0] * a[0]) - (b[1] * a[1]) - (b[2] * a[2]) - (b[3] * a[3]);
  ret[1] = (b[0] * a[1]) + (b[1] * a[0]) + (b[2] * a[3]) - (b[3] * a[2]);
  ret[2] = (b[0] * a[2]) + (b[2] * a[0]) + (b[3] * a[1]) - (b[1] * a[3]);
  ret[3] = (b[0] * a[3]) + (b[3] * a[0]) + (b[1] * a[2]) - (b[2] * a[1]);

  return;
}

希望有帮助!