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

> 原文链接：https://robotics.hellobike.com/pitfalls/2016-05-29-robotics-se-9636
> 频道：pitfalls | 更新日期：2016-05-29 | 标签：auto, stackexchange, mobile-robot, kalman-filter, compass
> 核心摘要：航向角跨越 -pi/pi 边界导致估计异常，通过将角度和残差归一化到 [-pi, pi] 解决。

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

::: v-pre
> 本文由 AI 根据公开内容整理，可能有误，请以原文为准：[Regarding Kalman filter and estimating heading with magnetic compass](https://robotics.stackexchange.com/questions/9636/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 授权。
:::

## 原始参考来源 (Sources)

- [Robotics Stack Exchange #9636：Regarding Kalman filter and estimating heading with magnetic compass](https://robotics.stackexchange.com/questions/9636/regarding-kalman-filter-and-estimating-heading-with-magnetic-compass)