Binary Bayes Filter 的例子

本文將延續前文的 Bayes Filter 式子推導，用 Probabilistic Robotics 書中 2.4.2 的例子來介紹 Bayes Filter 的原理。

複習一些觀念

•  Filter 問題在 state estimation 之中想要解的是找出最合理的狀態估計 \(x_t\)。
• 初始值 \(x_0\) 可以假設成 uniform distribution，或任何合理的機率分布。

例子

在本例子中，我們假設有一個能夠開關門的機器人，它有一個感測器可以偵測門是否為開的。在此模型中，狀態 \(x\) 為門的狀態，也就是開或關兩種可能。

一開始機器人對於門的狀態一無所知，因此假設兩種可能的機率皆為 0.5，也就是： \[ bel(x_0 = open) = 0.5 \\ bel(x_0 = closed) = 0.5 \] 此外假設機器人的感測器不是相當準確，因此感測到的結果可能與實際狀態不同： \[ p(z_t = sense\_open | x_t = is\_open) = 0.6 \\ p(z_t = sense\_closed | x_t = is\_open) = 0.4 \\ p(z_t = sense\_open | x_t = is\_closed) = 0.2 \\ p(z_t = sense\_closed | x_t = is\_closed) = 0.8 \] 也就是說當門是開著的情況下感測器有 40% 出錯的機率，而門關著的情況下有 20% 出錯的機率。 

接下來此機器人有兩種動作可以控制：開門或是閒置。當然開門這個動作可能會失敗，因此狀態轉換機率如下： \[ p(x_t = is\_open | u_t = push, x_{t-1} = is\_open) = 1 \\ p(x_t = is\_closed | u_t = push, x_{t-1} = is\_open) = 0 \\ p(x_t = is\_open | u_t = push, x_{t-1} = is\_closed) = 0.8 \\ p(x_t = is\_closed | u_t = push, x_{t-1} = is\_closed) = 0.2 \\ \] 也就是說推開門的動作成功的機率為 80%。 

下一步要來計算大概的估計 \(\overline{bel}(x_1)\) 了。我們假設在 \(t = 1\) 時機器人什麼都不做，並且感測到門是開的： \[ \overline{bel}(x_1 = is\_open) \\ = p(x_1 = is\_open | u_1 = do\_nothing, x_0 = is\_open)bel(x_0 = open) \\ + p(x_1 = is\_open | u_1 = do\_nothing, x_0 = is\_closed)bel(x_0 = closed)\\ = 1.0 * 0.5 + 0 * 0.5 = 0.5 \] \[ \overline{bel}(x_1 = is\_closed) \\ = p(x_1 = is\_closed | u_1 = do\_nothing, x_0 = is\_open)bel(x_0 = open) \\ + p(x_1 = is\_closed | u_1 = do\_nothing, x_0 = is\_closed)bel(x_0 = closed)\\ = 0 * 0.5 + 1.0 * 0.5 = 0.5 \] 

接下來我們要從 \(\overline{bel}(x_1)\) 來算 \(bel(x_1)\)： \[ bel(x_1 = is\_open) = \eta \ p(z_1 = sense\_open | x_1 = is\_open) \overline{bel}(x_1 = is\_open) \\ = \eta \ 0.6 * 0.5 = \eta \ 0.3\\ bel(x_1 = is\_closed) = \eta \ p(z_1 = sense\_open | x_1 = is\_closed) \overline{bel}(x_1 = is\_closed) \\ = \eta \ 0.2 * 0.5 = \eta \ 0.1 \\ \] Normalization 後得到： \[ bel(x_1 = is\_open) = 0.75 \\ bel(x_1 = is\_closed) = 0.25 \] 這就是在 \(t=1\) 時的狀態估計結果。假如 \(t=2\) 時機器人推開了門，並且感測器也認為門是開的則會得到以下結果： \[ bel(x_2 = is\_open) = 0.983 \\ bel(x_2 = is\_closed) = 0.017 \]

參考資料

[1] Statistical Techniques in Robotics: Bayes Filtering