rsasaki0109/kalman_filter_localization_ros2

解决的问题

本项目为机器人和车辆提供高精度定位系统，解决了实时估计车辆位置、速度和姿态（朝向）的问题。它专门应对传感器噪声、GNSS 中断（GPS 信号丢失）以及 IMU 偏置漂移等挑战，确保即使在单个传感器失效或不准确时，车辆也能知道自己的位置。

工作原理

该系统使用误差状态扩展卡尔曼滤波器 (ESEKF) 来融合来自多个传感器的数据：GNSS（位置和速度）、IMU（加速度和角速度）以及轮式里程计。

关键技术机制包括：

• Measurement Replay：一个回溯并重放的引擎，允许滤波器在实际的传感器时间戳处融合测量值，从而修正延迟。
• Robust Loss Functions：使用 Huber 和 Cauchy 损失函数来忽略可能会扭曲位置估计的“离群值”测量值。
• 应对中断：集成非完整约束 (NHC) 和零速更新 (ZUPT)，以在 GNSS 中断期间保持准确性。
• 传播模型：提供多种积分方法（fast、exact 或 legacy），用于计算传感器更新之间的状态。
• 平滑处理：可选的固定滞后 RTS 平滑器可以使用未来数据重新处理过去的状态，从而创建高度精确的参考轨迹。

适用对象

• 开发自动驾驶车辆或地面机器人的机器人工程师。
• 专注于传感器融合和定位算法的研究人员。
• 在导航栈中使用 ROS 2 的开发人员。

亮点

• 多传感器融合：结合了 GNSS、IMU 和轮速数据。
• 故障隔离：自动隔离产生非有限值或持续被拒绝样本的传感器。
• 偏置估计：持续估计并校正陀螺仪和加速度计的偏置。
• 诊断工具：包括用于传感器配置诊断的 kf_doctor 以及用于计算 APE 和 RPE 误差的全面评估套件。
• UrbanNav 性能：证明了在长时间 GNSS 中断（长达 81.8 秒）期间，能将 3D RMSE 从 600 多米降低到 3.15 米的能力。