卡爾曼濾波（Kalman Filter）

定義

卡爾曼濾波（Kalman Filter）是一種遞迴的狀態估計演算法：在系統與感測器都帶有高斯雜訊的假設下，結合「運動模型的預測」與「感測器的量測」，算出變異數最小的狀態估計。

直覺理解

機器人用輪速計推算自己往前走了 1 公尺（預測），GPS 卻說走了 1.3 公尺（量測）。兩者都不完全可信，卡爾曼濾波依照各自的不確定度給出加權平均——誰比較可信就聽誰多一點，並同時更新「現在有多不確定」。

數學表示

預測：

x^k∣k−1=Fx^k−1+Buk,Pk∣k−1=FPk−1FT+Q\hat{x}_{k|k-1} = F\hat{x}_{k-1} + Bu_k, \qquad P_{k|k-1} = FP_{k-1}F^{T} + Q

更新：

Kk=Pk∣k−1HT(HPk∣k−1HT+R)−1K_k = P_{k|k-1}H^{T}\left(HP_{k|k-1}H^{T} + R\right)^{-1} x^k=x^k∣k−1+Kk(zk−Hx^k∣k−1),Pk=(I−KkH)Pk∣k−1\hat{x}_k = \hat{x}_{k|k-1} + K_k\left(z_k - H\hat{x}_{k|k-1}\right), \qquad P_k = (I - K_kH)P_{k|k-1}

KkK_k 稱為卡爾曼增益，決定要相信量測多少。

使用情境

  • 感測融合：融合 IMU、輪速計、GPS（ROS 2 的 robot_localization 套件即以 EKF/UKF 實作）。
  • EKF-SLAM：把機器人位姿與地標位置放進同一個狀態向量一起估計。
  • 目標追蹤：追蹤行人、車輛的位置與速度。

常見誤解

  • 非線性系統能直接用嗎？ 標準 KF 只適用線性系統，機器人運動多半是非線性的，實務上幾乎都用 EKF 或 UKF。
  • 多峰分佈怎麼辦？ 卡爾曼濾波只能表示單一高斯分佈，遇到「可能在 A 也可能在 B」的情況要改用粒子濾波。

延伸閱讀

常見問題

faq_01.log
卡爾曼濾波、EKF、UKF 有什麼差別？
標準卡爾曼濾波假設系統是線性的；擴展卡爾曼濾波（EKF）把非線性模型在目前估計點做一階泰勒展開線性化；無跡卡爾曼濾波（UKF）改用一組 sigma 點傳遞分佈，對強非線性系統通常更準。
faq_02.log
Q 和 R 矩陣要怎麼調？
Q 是過程雜訊（你多不相信運動模型），R 是量測雜訊（你多不相信感測器）。R 可以從感測器規格或靜止時的量測變異數估計，Q 通常需要實驗調整：Q 調大，估計更快跟上量測但更抖；Q 調小，估計更平滑但反應較慢。

相關文章

相關術語

粒子濾波（Particle Filter）SLAM（同時定位與建圖）視覺里程計（Visual Odometry）