如何借助9DoF IMU提升ICM-20948罗盘航向计算的鲁棒性？

你当前仅用加速度计和磁力计计算航向的算法，核心是把磁力计数据投影到垂直于重力的平面获取北向，但一旦IMU出现俯仰或横滚，这个投影逻辑会因加速度计的微小误差（比如传感器零偏、运动加速度干扰）失效。结合陀螺仪是解决该问题的关键，具体优化方向如下：

一、引入姿态融合算法 #

陀螺仪能精准跟踪短期姿态变化（比如快速俯仰/横滚），但长期积分会产生漂移；加速度计和磁力计能提供长期稳定的姿态参考，但易受动态运动干扰。将三者数据融合，可得到更可靠的姿态角（俯仰、横滚、偏航），再基于该姿态计算航向：

• 互补滤波：实现简单，适合资源有限的平台。给不同传感器分配动态权重：陀螺仪负责短期姿态更新（积分角速度得到角度），加速度计+磁力计负责长期校准（用它们计算的姿态角修正陀螺仪漂移）。比如静止时提升加速度计+磁力计权重，运动时提升陀螺仪权重。
• 卡尔曼滤波：精度更高，适配复杂场景。通过建立状态模型，实时估计姿态误差并修正，不过实现复杂度稍高，需调试状态方程和观测方程的参数。

二、完成传感器基础校准 #

校准是提升数据可靠性的前提，能大幅降低原始数据误差：

• 加速度计校准：消除零偏和刻度误差，确保其准确输出重力方向向量。可通过将IMU放在6个不同静止姿态（每个轴朝上/朝下）采集数据，计算校准参数。
• 磁力计校准：消除硬铁（传感器自身磁场）和软铁（周围环境磁场畸变）干扰，让磁力计数据精准指向地磁北极。常用8字校准法，让IMU在空间画8字采集数据后，计算校准矩阵。

三、优化航向计算的倾斜补偿逻辑 #

基于融合后的俯仰角（pitch，弧度）和横滚角（roll，弧度），先对磁力计数据做倾斜补偿，将三维磁力计数据投影到水平面后再计算航向，示例代码逻辑如下：

// 假设已通过融合得到校准后的pitch和roll
float mag_x_compensated = mag_x * cos(pitch) + mag_z * sin(pitch);
float mag_y_compensated = mag_x * sin(roll) * sin(pitch) + mag_y * cos(roll) - mag_z * sin(roll) * cos(pitch);
// 计算航向，根据实际坐标系调整atan2参数顺序
float heading = atan2(mag_y_compensated, mag_x_compensated) * 180 / M_PI;
// 将航向转换为0-360度范围
if (heading < 0) heading += 360;

该逻辑能有效抵消俯仰、横滚对航向计算的影响，而融合陀螺仪数据正是保证pitch、roll精度的核心。

四、陀螺仪数据使用注意事项 #

• 陀螺仪输出的是角速度，需积分得到角度，但积分会随时间漂移，必须定期用加速度计（校准俯仰/横滚）和磁力计（校准偏航）修正漂移误差。
• 可给陀螺仪数据添加低通滤波，避免高频噪声影响积分结果，或在融合算法中直接处理噪声。

内容的提问来源于stack exchange，提问作者Filip Vuković