c - 倾斜磁力计输出 - 俯仰和滚动倾斜补偿 - 我迷路了

Question

我在这个具体问题上浪费了两天(两夜)，并且在倾斜补偿我的磁力计输出方面完全失败。
我尝试了在谷歌和开源示例中可以读到的所有内容，但没有任何东西指导我对两者(滚动和俯仰)进行适当的倾斜补偿。 .

设置: 我使用校准值，但不使用统一值。
我有一个融合的重力 vector ，它工作正常且准确。
该传感器是 9dof BMX055 ( http://www.mouser.com/ds/2/621/BST-BMX055-DS000-01-274824.pdf ) 磁力计的最小值/最大值在每个轴上均为 +- 512(差异很小，但所有轴均归零)。 硬件是Sam3X8e(Cortex M3)，全部用C编写。
浮点和三角函数可以很快完成，所以这没有问题。 BMX055 芯片已对齐，使引脚 20、19、18 指向前面。
数据表第 159-161 页显示了方向。 当我抬起前部时，音调会升高。
当我升起左侧时，滚动会升起。

示例值:
当水平水平时，指向我的算法称为 305deg 的方向

Pitch: 0  , Roll 0   : MAG cal:  x 132   y  93  z   -364  
Pitch: +24, Roll 0   : MAG cal:  x -109   y  93  z   -397  
Pitch: +46, Roll 0   : MAG cal:  x -303   y  89  z   -351  
Pitch: 0  , Roll -44 : MAG cal:  x 151   y  352  z   -235  
Pitch: 0  , Roll +36 : MAG cal:  x 130   y  -140  z   -328  
Pitch: 78 , Roll -2  : MAG cal:  x -503   y  93  z   -199  
Pitch: 7  , Roll -53 : MAG cal:  x 135   y  424  z   -180

对齐应该始终在 305 度左右(尽我所能)，也许 +- 5 度。

公式:(到处都使用相同的公式)

uint16_t compass_tilt_compensation(float roll_radians, float pitch_radians,float mag_y, float mag_x, float mag_z) 
{

  float tilt_compensated_heading;
  float MAG_X;
  float MAG_Y;
  float cos_roll;
  float sin_roll;
  float cos_pitch;
  float sin_pitch;
  int16_t heading;
  //pitch_radians =roll_radians;
  //roll_radians *= -1;
  //mag_x *= -1;
  //roll_radians=0.0f;
  //pitch_radians=0;v
  //pitch_radians *=-1;

  cos_roll = cos(roll_radians);
  sin_roll = sin(roll_radians);
  cos_pitch = cos(pitch_radians);
  sin_pitch = sin(pitch_radians);


#if 0
  MAG_X = mag_x*cos_pitch+mag_y*sin_roll*sin_pitch+mag_z*cos_roll*sin_pitch;
  MAG_Y = mag_y*cos_roll-mag_z*sin_roll;
  tilt_compensated_heading = atan2f(-MAG_Y,MAG_X); 
#else
    MAG_X = mag_x * cos_pitch + mag_z * sin_pitch;
    MAG_Y = mag_x * sin_roll * sin_pitch + mag_y * cos_roll - mag_z * sin_roll * cos_pitch;
    tilt_compensated_heading = atan2f(-MAG_Y,MAG_X);
    //tilt_compensated_heading = atan2f(-mag_y,mag_x); // works fine when leveled
#endif

//convert to degree from 0-3599
    heading = tilt_compensated_heading * RAD_TO_DEG * 10;
     if ( heading < 0 ) 
     {
         heading += 3600;
     }

  return heading;
}

我尝试了各种组合，我尝试只固定一个轴并将一个轴始终保留为 0，在任何输入上交换 X/Y 俯仰/滚动，*-1。

结果总是完全错误的。 有时(取决于我尝试通过试验/错误反转或不反转值的位置)这些值似乎接近线性。有时，一根轴会在正区域得到补偿。
然而，滚动和俯仰总是会导致“随机”跳跃和航向变化。
数学从来都不是我最喜欢的，现在我后悔了。

我个人的猜测是，公式在普林西比是正确的，杂志在普林西比工作(毕竟我在升级时获得了适当的学位)，但我不知何故喂了一些错误的东西。 (比如Y和X需要交换，z*-1，pitch需要*-1)

擅长这个主题的人可以看一下并指导我如何获得正确的标题吗？
今晚如果能睡几个小时就不用再梦见功能失调的算法了，那就太好了:)

更新: 这里的倾斜补偿适用于指向 305 度航向时的负滚转。 此处也使用:http://www.emcu.it/MEMS/Compass/MEMS_Compass_A_RTM1v3.pdf

Answer 1

三天的工作，终于找到了我遇到的问题，倾斜补偿开始起作用了!
我在各个论坛上看到很多人遇到了这样的问题，所以这就是我所做的:

我一步步解释我做了什么以及为什么这样做。也许有类似问题的人会通过这种方式找到解决方案。

1. 调整这些值会有所帮助(也许只需反转俯仰或滚动，必须交换 X 或 Y)。
   然而，如果您的问题不仅仅是一个小问题，则值相当多，并且组合的数量太多。

2. 发布的公式工作正常(位于事件 #if 分支中的公式。
   如果您有磁力计，并且 atan2(-y,x) 在平整时为您提供正确的航向，那么这个公式也适用于您。

3. 我所做的是从 I2C 二进制逻辑(和数据表)开始彻底检查所有传感器和 vector 。
   在这种特殊情况 (BMX055) 中很重要，数据表方向页面错误!
   关于轴 (x,y) 方向以及旋转为正或负时存在多个错误。有时适用右手定则，有时不适用，并且附图具有误导性(错误)。博世在记录该芯片(以及前一个芯片)方面做得很糟糕。 他们似乎不希望人们正确理解它，他们多次编写了融合 API，其中包含优化的定点算术和高级校准，但它不向公众开放。

我需要做的是建立一个合适的 body 引用系统。 自己决定 X 的位置，然后让所有传感器在向同一方向(正/负)倾斜/滚动时正确更改 X 轴。 对俯仰、横滚和重力/磁场执行此操作。

一旦他们都玩得很好，我就重新开始。
航向公式仍然无法正常工作，但现在我第一次信任了 vector 机。
我添加了 vector 旋转矩阵函数，并使用横滚和俯仰以及偏航 = 0 将磁 vector 旋转回来。 惊喜:磁 vector 经过倾斜补偿。

现在我知道这是可以做到的:)

我回到原来的公式，用Y替换X(因为我已经交换了它们以匹配 body 引用系统(陀螺仪/磁力的X和Y)。
现在倾斜补偿适用于俯仰，但不适用于横滚。
所以我反转了roll_radians，突然间它得到了完美的倾斜补偿。

我现在有两个解决方案。一种是旋转矩阵解决方案，另一种是每个人都在使用的标准解决方案。我会看看其中一个是否表现更好，如果结果值得的话，也许会在这里给出最后的更新。