校准后磁力计读数异常及倾斜补偿航向问题排查

我正在将QMC5883l磁力计集成到基于MPU6050（提供陀螺仪与加速度计数据）的EKF（扩展卡尔曼滤波）算法中。无磁力计时，EKF的状态估计效果极佳；但加入磁力计后，当实际偏航、俯仰、横滚角均为0°时，状态估计值却显示明显非零数值。

我已使用Magneto 1.2对磁力计完成精准校准：

• 校准前：磁力计读数呈不规则散点分布
• 校准后：读数接近标准球形分布
  芯片灵敏度设置为±2G，Magneto给出的校正值包含偏移量与缩放矩阵参数。

进一步排查后发现问题根源在于磁力计读数本身：当初始四元数为[1 0 0 0]（即姿态角均为0°）时，归一化后的磁力计估计读数应为[cos(1.53°), 0, sin(1.53°)]（本地磁偏角为+1.53°），因此z分量应接近0，x分量应接近1。但将磁力计平放于远离铁磁材料的木质桌面时，实际读数与预期差异极大（读数格式为x,y,z）。

这种差异导致卡尔曼滤波进行错误补偿，进而输出错误的姿态估计值。我已排查多种可能，更换多颗芯片后问题依旧，无法确定强垂直磁场来源。以下是读取校准后数值的代码片段（基于NucleoF103RB，采用NED坐标系转换）：

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); */
}

内容的提问来源于stack exchange，提问作者Nxyoutou