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

Question

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

但似乎该算法需要在不同的坐标系中进行传感器测量。 Apple CoreMotion传感器系统位于右侧，Madgewick位于左侧。这是different coordinate systems的图片。两个系统都遵循右手规则。 对我来说，似乎绕z轴旋转90度。但这没效果。

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

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

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

Answer 1

我不确定如何在目标c中写这个，但这是我如何在vanilla c中完成坐标转换。我也想旋转方向，使+ y为北。此翻译也反映在以下方法中。

此方法需要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;
}

希望有所帮助！