Learning Objectives
- 說明為什麼兩個都不準的資料，合起來可以比任何一個都準
- 用卡爾曼增益解釋濾波器比較相信誰
- 寫出一維卡爾曼濾波的預測與更新
- 用里程計與光達估計到牆的距離，並說明濾波器在哪裡會出錯
里程計和光達都不準，要相信誰
機器人沿著走廊往前走，想知道自己走了多遠。手上有兩個資料：
| 感測器 | 短時間內 | 長時間下來 |
|---|---|---|
| 里程計（看輪子轉多少） | 很平順，差異很小 | 誤差一直累積，只會越來越偏 |
| 光達（量到牆的距離） | 每一筆都抖動 | 不會累積，平均起來是對的 |
只相信里程計，走久了就偏；只相信光達，數字一直跳，拿去控制機器人也會跟著抖。
直覺的作法是取平均，但兩個資料的可信度不同，應該給比較可信的那個比較大的權重。問題是權重要給多少，而且可信度還會隨時間改變：剛開機時完全不知道自己在哪裡，走了一段又量了很多次之後就有把握多了。
卡爾曼濾波（Kalman filter）回答的就是這件事。它除了記住「我猜現在的值是多少」，也記住「我對這個猜測有多沒把握」，再依照這個把握度決定新資料要算多少。它出自 Rudolf Kalman 1960 年發表於 Journal of Basic Engineering 的論文（doi:10.1115/1.3662552）。
這一課只處理一維：一個數字（位置），和它的變異數（σ²，標準差的平方，數字越大越沒把握）。
合起來的公式
剛才綠線的公式只有三行：
K ＝ σ1² ÷ (σ1² ＋ σ2²)
μ ＝ μ1 ＋ K × (μ2 − μ1)
σ² ＝ (1 − K) × σ1²
K 是卡爾曼增益（Kalman gain），範圍 0 到 1，代表這次要往量測移動多少：
- 預測很不確定（σ1 大）：K 接近 1，幾乎照量測走。
- 量測很不確定（σ2 大）：K 接近 0，幾乎不理量測。
用數字走一次：預測 2.0（σ1 ＝ 0.2），量測 2.6（σ2 ＝ 0.4）。
K ＝ 0.04 ÷ (0.04 ＋ 0.16) ＝ 0.2
μ ＝ 2.0 ＋ 0.2 × 0.6 ＝ 2.12
σ² ＝ 0.8 × 0.04 ＝ 0.032，σ ≈ 0.179
結果 0.179 比 0.2 和 0.4 都小，所以多一個資料一定讓濾波器更有把握。這個結論成立的前提是雜訊確實是常態分布，而且兩個感測器的誤差彼此無關。同一個原因造成的誤差（例如同一片玻璃讓兩個感測器都量錯）合起來不會變準，濾波器還會誤以為變準了。
一步一步看預測與更新
機器人要做的事是把剛才的公式放進迴圈。注意 P（變異數）的變化：預測之後變大，更新之後變小。一開始 P 是 1.0，表示完全沒把握，所以第一筆量測的 K 很接近 1。
預測讓人變沒把握，量測讓人變有把握
整個濾波器就是兩步反覆：
預測（有里程計時）：x⁻ ＝ x ＋ d P⁻ ＝ P ＋ Q
更新（有量測時）： K ＝ P⁻ ÷ (P⁻ ＋ R)
x ＝ x⁻ ＋ K(z − x⁻) P ＝ (1 − K)P⁻
- Q 是濾波器認為每一步預測會增加多少不確定（里程計的雜訊變異數）。
- R 是濾波器認為量測的雜訊變異數。
Q 和 R 是你告訴濾波器的，不是它自己量到的。告訴它的數字和真實感測器不一樣，它不會發現，只會算出一個看起來很有信心的錯誤答案。下面的練習會讓你親眼看到這件事。
接到 ROS 2：/odom 預測、/scan 更新
放到機器人上，預測和更新由兩個不同的回呼觸發，頻率也不同：這個模擬世界裡 /odom 每秒 20 筆、/scan 每秒 5 筆。卡爾曼濾波不需要它們一樣快，也不需要一一配對：有里程計就預測，有光達就更新。
這裡的狀態是「到前方牆的距離」。兩件事要注意：
/odom給的是累積的位置，預測要用這一筆和上一筆的差。- 模擬世界的里程計雜訊沒有偏差，真實輪子會打滑、輪徑會有誤差。下面這個練習裡，真正讓里程計出問題的是另一件事：一開始猜錯了。
下面的程式先猜到牆的距離是 6 公尺，實際上是 4.45 公尺，並且只有在估計值超過 1 公尺時才往前走。只有預測、沒有更新時，它會多走 1.55 公尺，超過牆的位置。
這是模擬環境的結果：時間是虛擬的，雜訊是 noise 參數設定的常態分布，沒有輪子打滑。
實作：一維卡爾曼濾波
寫出 kalman_step：先預測，有量測才更新。z 是 None 代表這一步光達沒有資料（例如被遮住），只做預測。
卡爾曼濾波在哪裡會出錯
| 假設 | 什麼時候不成立 | 會發生什麼 |
|---|---|---|
| 系統是線性的 | 機器人會轉彎，位置和角度的關係有 sin、cos | 要用延伸版本，例如 EKF（extended Kalman filter，在目前的估計點附近做線性近似） |
| 雜訊是常態分布 | 光達打到玻璃，偶爾量出差很遠的值 | 離譜的量測照樣算進去，估計被拉走 |
| 雜訊彼此獨立 | 兩個感測器被同一個原因影響 | 濾波器高估自己的把握 |
| 只有一個可能的答案 | 長得一樣的兩條走廊，可能在任一條 | 一個高斯分布表達不了「兩個都有可能」，第 17 課的粒子濾波處理這個情況 |
實務上常見的保護：
- 量測先過濾：量測和預測差太遠就不用。常見的判斷是差值超過幾倍標準差。
- 把不確定性拿來做決定：σ 超過某個值，就降速或停下來，而不是繼續相信估計值。光達被遮住時 σ 會一直變大，這正是停下來的依據。
- Q 和 R 要量：讓機器人靜止，記錄一段感測器讀數，算出的變異數是 R 的起點。
確認一下
預測的變異數是 0.09，量測的變異數是 0.01。卡爾曼增益 K 是多少？這次會比較靠近誰？
光達被遮住 8 秒，這段時間濾波器只有預測。估計的變異數 P 會怎樣？
光達打到玻璃，有一筆量到 4.8 公尺，預測是 1.2 公尺。一般的卡爾曼濾波會怎麼處理這一筆？
把 R 設得比光達真實的雜訊小很多，會發生什麼？
兩個感測器都因為陽光而量錯同一個方向，把它們丟進卡爾曼濾波合起來，結果會？
重點整理
- 卡爾曼濾波同時記住估計值和變異數，用變異數決定新資料要算多少。
K ＝ P ÷ (P ＋ R)：預測沒把握就聽量測，量測沒把握就聽預測。- 預測讓變異數變大，更新讓它變小。感測器一停，變異數就一直變大，可以拿來當作停下來的依據。
- Q 和 R 是人給的。給錯了濾波器不會發現，還會回報很有信心的結果。
- 假設是線性、常態、獨立。離譜的量測要先過濾，非線性要用 EKF，多個可能的位置要用粒子濾波。
- 模擬世界的里程計雜訊沒有偏差，也沒有輪子打滑；真實機器人的 Q 和 R 要實際量。
下一課換一個感測器：相機。它能告訴你「那是什麼」，但給你的只是一大堆數字。