踩坑/Robotics Stack Exchange·2016-05-29

航向接近 -pi/pi 时卡尔曼估计失真

航向角跨越 -pi/pi 边界导致估计异常，通过将角度和残差归一化到 [-pi, pi] 解决。

AI 整理

• 本体 SDK 与控制
• 感知与导航

本文由 AI 根据公开内容整理，可能有误，请以原文为准：Regarding Kalman filter and estimating heading with magnetic compass（Robotics Stack Exchange #9636）

状态：已解决

问题

差分机器人静止且航向接近 -pi/pi 时，融合陀螺、里程计和磁罗盘的航向估计变得无效。

环境

差分机器人，带 IMU、轮编码器、磁罗盘；互补滤波融合 gyroZ 和里程计，再用卡尔曼滤波校正。

解决办法

1. 在预测后检查角度：若 angle > pi，令 angle = angle - 2*pi；若 angle < -pi，令 angle = angle + 2*pi。
2. 若可能出现超过 2*pi 的尖峰，用 while (abs(angle) > pi) 循环重复上述归一化。
3. 对磁罗盘测量角与预测角的差值也执行同样的 [-pi, pi] 归一化，再用于校正。
4. 校正后的估计值再次执行同样检查。

原问题作者：erltot；采纳答案作者：trudesagen。原内容依 CC BY-SA 4.0 授权，本页摘要同样依 CC BY-SA 4.0 授权。

来源

本页事实对应的公开页面，以原文为准。

1. Robotics Stack Exchange #9636：Regarding Kalman filter and estimating heading with magnetic compass访问 2026-10-22