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 公尺的能力。