完美校准后的奇怪磁力计读数以及创建倾斜补偿航向的问题

Question

我最近尝试使用 MPU6050（陀螺仪和加速度计测量）和 QMC5883l（磁力计测量）在我的 EKF 算法中实现磁力计。没有磁力计，EKF 工作得很好，状态估计就像黄油一样流畅。然而，当添加磁力计时，事情就变得混乱了。当现实生活中偏航 = 俯仰 = 滚动 = 0 度时，状态估计显示显着的非零值。

我已经使用magneto 1.2精确校准了磁力计，这里有一些显示配置的图：

校准前的磁力计读数 校准后磁力计读数

芯片的灵敏度选择为+/-2G，这是磁电机向我展示的图片： 万磁王的修正值

当进一步调查这个问题时，我发现磁力计读数本身一定有问题。原因是，如果您想象初始四元数 ([1 0 0 0])（偏航、俯仰、滚动为零），估计的磁力计读数应为 [cos(INCLINE), 0, sin(INCLINE)]，其中 incline是局部磁偏角（局部 +1.53 度），因此，如果您始终对估计和测量进行标准化，则磁力计 z 分量应几乎为 0，而 x 分量应几乎为 1。

但是，当将磁力计平放在桌子上时，这是我得到的测量结果： 读数格式为：x, y, z

这与估计形成巨大反差，因此，卡尔曼滤波器尝试补偿这种差异，并开始显示错误的值。我已经尝试并浏览了几乎所有内容，但没有发现有类似问题的人。我现在已经测试了好几个芯片了，都是同样的问题。我不知道这么强的垂直磁场是从哪里来的，我的桌子是木头做的，而且我把其他铁磁材料放在了相当远的地方。请有人帮助弄清楚我为什么得到这些值以及如何修复它们，这是我用来读取校准值的函数的片段：

我会提到，我正在根据 NED 框架转换轴，并且我使用的是 NucleoF103RB：

void gy271_MagRead(gy271* mag)
{
    uint8_t data[6];
    int16_t x_mag;
    int16_t y_mag;
    int16_t z_mag;

    HAL_I2C_Mem_Read(&hi2c1, (DEVICE_ADDRESS_MAG << 1), REG_MAG_DATA, 1, data, 6, 100);
    x_mag = (data[1] << 8) | data[0];
    y_mag = (data[3] << 8) | data[2];
    z_mag = (data[5] << 8) | data[4];

    x_mag = x_mag - 2603.789389f;
    y_mag = y_mag - 68.478941f;
    z_mag = z_mag + 2499.499613f;

    mag->MagM[0] =    x_mag*4.332538f  - y_mag*0.139199f  + z_mag*0.131290f;
    mag->MagM[1] = -(-x_mag*0.139199f  + y_mag*4.321255f  - z_mag*0.532433f);
    mag->MagM[2] = -( x_mag*0.131290f  - y_mag*0.532433f  + z_mag*4.436754f);

    float norm = sqrtf(mag->MagM[0]*mag->MagM[0] + mag->MagM[1]*mag->MagM[1] + mag->MagM[2]*mag->MagM[2]);
    mag->MagM[0] = mag->MagM[0]/norm;
    mag->MagM[1] = mag->MagM[1]/norm;
    mag->MagM[2] = mag->MagM[2]/norm;


    printf("%f  ", mag->MagM[0]);
    printf("%f  ", mag->MagM[1]);
    printf("%f  \n", mag->MagM[2]);
/*  float yaw = atan2f(mag->MagM[1], mag->MagM[0]);
    printf("yaw is: %f \n", yaw*RAD_TO_DEGREES); */
}

欢迎任何帮助，非常感谢！

Answer 1

最后，我发现问题是我用的是赤纬而不是倾角。确实z轴值那么高。测量结果是正确的，但是我所在的地方的倾斜度是 51.2 度。倾角告诉您您所在位置的地球磁场的倾角，以预测初始磁力计值。不要混淆赤纬和倾角！