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

Posted 2023-03-05

【中文标题】Madgwick 在 iOS 上的传感器融合算法

【英文标题】：Madgwick's sensor fusion algorithm on iOS

【发布时间】：2013-07-22 12:49:45

【问题描述】：

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

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

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

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

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

【问题讨论】：

你能完成任务吗？

不，抱歉。不再拥有 Mac。

感谢您的快速回复

请问什么已经有效了？ Madgwick 滤波器的工作原理如下： 1) 使用当前姿态以角轴形式计算加速度计/磁力计姿态误差 2) 将这些误差中的一部分减去陀螺仪轴角形式 3) 更新姿态四元数积分校正轴 -角度值。您有这些步骤中的任何一个单独工作吗？ Madgwick 的过滤器应在任何参考系中工作，但生成的四元数将在输入数据的参考系中。我相信你可能还有另一个问题。如果按原样应用过滤器会得到什么？

嗨弗利克斯。你解决问题了吗？

【参考方案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;

希望有帮助！