iOS 上 Madgwick 的传感器融合算法 [英] Madgwick's sensor fusion algorithm on iOS

问题描述

我正在尝试运行 但这也不起作用.所以你有提示吗?如果 Madgwick 的 alogithm 输出可以与 CoreMotion 输出 (CMAttitudeReferenceFrameXMagneticNorthZVertical) 在同一个系统中，那就完美了.

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

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

解决方案

我不知道如何在目标 c 中编写它，但这是我如何在 vanilla c 中完成坐标转换的方法.我还想旋转方向，使 +y 为北.这种翻译也体现在下面的方法中.

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

void madgeq_to_openglq(float *fMadgQ, float *fRetQ) {浮动 fTmpQ[4];//绕Z轴旋转90度:float fXYRotationQ[4] = { sqrt(0.5), 0, 0, -1.0*sqrt(0.5) };//反转旋转向量以适应惯用手问题:fTmpQ[0] = fMadgQ[0];fTmpQ[1] = fMadgQ[1] * -1.0f;fTmpQ[2] = fMadgQ[2];fTmpQ[3] = fMadgQ[3] * -1.0f;//然后将转换后的 Rotation 存储到 ret 中:quatMult((float *) &fTmpQ, (float *) &fXYRotationQ, fRetQ);}//四元数乘法运算符.期望其 wxyz 顺序的 4 元素数组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]]);返回;}

希望有帮助！

i'm trying to run Madgwick's sensor fusion algorithm on iOS. Since the code is open source i already included it in my project and call the methods with the provided sensor values.

But it seems, that the algorithm expects the sensor measurements in a different coordinate system. The Apple CoreMotion Sensor System is given on the right side, Madgewick's on the left. Here is the picture of the different coordinate systems. Both systems follow the right hand rule. For me it seems like there is a 90 degree rotation around the z axis. But this didn't work.

I also tried to flip x and y (and invert z) axis as suggested by other stackoverflow posts for WP but this didn't work also. So do you have a hint? Would be perfect if Madgwick's alogithm output could be in the same system as the CoreMotion output (CMAttitudeReferenceFrameXMagneticNorthZVertical).

Furthermore I'm looking for a good working value for betaDef on the iPhone. betaDef is kind of the proportional gain and is currently set to 0.1f.

Any help on how to achieve the goal would be appreciated.

解决方案

I'm not sure how to write this in objective c, but here's how I accomplished the coordinate transformations in vanilla c. I also wanted to rotate the orientation so that +y is north. This translation is also reflected in the below method.

This method expects a 4 element quaternion in the form of wxyz, and returns a translated quaternion in the same format:

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;
}

Hope that helps!

这篇关于iOS 上 Madgwick 的传感器融合算法的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！