本篇文章主要內容是簡介 Probabilistic Robotics 書中的 3.2 章：The Kalman Filter。本文假設讀者已經讀過了談狀態估計的前文，使用的數學工具或符號都與前文相同。
利用高斯分布實作狀態轉換的估計
前文提到了我們用 \(bel(x_t)\) 表示在 狀態 \(x_t\) 的可信度：
\[
bel(x_t) = p(x_t \mid z_{1:t}, u_{1:t})
\]並且在 t 時間的可信度 \(bel(x_t)\) 可以由 t-1 時間的可信度 \(bel(x_{t-1})\) 計算而來：
\[
\overline{bel}(x_t) = \int p(x_t \mid x_{t-1}, u_{t})\ bel(x_{t-1}) dx_{t-1}
\\
bel(x_t) = \eta\ p(z_t \mid x_t)\ \overline{bel}(x_t)
\]
Kalman Filter 就是利用高斯分布來描述上式的所有機率分布。我們首先寫出以下三個機率分布 \(p(x_t \mid x_{t-1}, u_{t})\) 、 \(p(z_t \mid x_t)\) 以及 \(bel(x_0)\) 的形式：
狀態轉移機率 \(p(x_t \mid x_{t-1}, u_{t})\)
Kalman Filter 的假設是狀態的轉移是個線性系統，也就是從 t-1 時間的狀態 \(x_{t-1}\) 轉換至 t 時間的狀態 \(x_t\) 可以用以下轉換式來描述：
\[
x_t = A_t x_{t-1} + B_t u_t + \varepsilon_t
\]
上式中狀態 \(x_t\) 為 n 維的向量，A 為 \(n \times n\) 的矩陣；\(u_t\) 是控制資訊，為 m 維的向量，因此 B 為 \(n \times m\) 的矩陣；\(\varepsilon_t\) 指的是在 t 時間的雜訊，是一個平均為 0 ，covariance 為 \(R_t\) 的高斯分布。
在寫出狀態轉移式之後，我們就可以把 \(p(x_t \mid x_{t-1}, u_{t})\) 用一個平均為 \(A_t x_{t-1} + B_t u_t\)，covariance 為 \(R_t\) 的高斯分布，也就是：
\[
p(x_t \mid x_{t-1}, u_{t})=det(2 \pi R_t)^{-\frac{1}{2}}exp\{-\frac{1}{2}(x_t - A_t x_{t-1} - B_t u_t)^T R_t^{-1} (x_t - A_t x_{t-1} - B_t u_t)\}
\]
測量機率 \(p(z_t \mid x_t)\)
一樣用線性系統描述從 t 時間的狀態 \(x_t\) 到感測資料 \(z_t\) 的關係：
\[
z_t = C_t x_t + \delta_t
\]
上式中狀態 \(x_t\) 為 n 維的向量，感測資料 \(z_t\) 為 k 維的向量，C 為 \(k \times n\) 的矩陣；\(\delta_t\) 指的是在 t 時間的感測雜訊，是一個平均為 0 ，covariance 為 \(Q_t\) 的高斯分布，因此 \(p(z_t \mid x_t)\) 可以寫成：
\[
p(z_t \mid x_t) = det(2 \pi Q_t)^{-\frac{1}{2}}exp\{-\frac{1}{2}(z_t - C_t x_t)^T Q_t^{-1} (z_t - C_t x_t)\}
\]
初始可信度 \(bel(x_0)\)
初始可信度 \(bel(x_0)\) 是一個平均為 \(\mu_0\)，covariance為 \(\Sigma_0\) 的高斯分布：
\[
bel(x_0)=p(x_0)= det(2 \pi \Sigma_0)^{-\frac{1}{2}}exp\{-\frac{1}{2}(x_0 - \mu_0)^T \Sigma_0^{-1} (x_0 - \mu_0)\}
\]
Kalman Filter 的轉換式推導
原書中的轉換式推導是從 \(\overline{bel}(x_t)\) 開始，把上面的式子都代進去來推導，整個過程非常複雜。在這裡我從另一個角度切入會讓推導的過程簡單許多，但是無論是哪一種推導方法都會得到一樣的結果。
在從 \(bel(x_{t-1})\) 推導至 \(bel(x_t)\) 時，我們想要找的其實是一組最好的 \(x_t\) 來求出最大的 \(bel(x_t)\) 。參考本文一開始的 \(bel(x_t)\) 式子：
\[
bel(x_t) = \eta\ p(z_t \mid x_t)\int p(x_t \mid x_{t-1}, u_{t})\ bel(x_{t-1}) dx_{t-1}
\]
這個最好的 \(x_t\) 為：
\[
arg\ \underset{x_t}{max}\ bel(x_t) = arg\ \underset{x_t}{max}\ \eta\ p(z_t \mid x_t)\int p(x_t \mid x_{t-1}, u_{t})\ bel(x_{t-1}) dx_{t-1}
\\
= arg\ \underset{x_t}{max}\ p(z_t \mid x_t)p(x_t \mid x_{t-1}, u_t)
\]
這個式子的形式跟本文一開始的 \(bel(x_t)\) 有一點點不同，但是可以想像在轉換的過程中，我們已經知道了 \(bel(x_{t-1})\) 的機率分布，因此可以寫成上述的形式。
下一步便是把前面介紹的高斯分布：狀態轉移機率與測量機率代進去，並且等號兩邊加上 log，可以得到的式子是：
\[
arg\ \underset{x_t}{max} (z_t - C_t x_t)^T Q_t^{-1} (z_t - C_t x_t) + (x_t - A_t x_{t-1} - B_t u_t)^T R_t^{-1} (x_t - A_t x_{t-1} - B_t u_t)
\]
對 \(x_t\) 偏微分，讓導數為零求極值：
\[
C_t^T Q_t^{-1} C_t x_t + R_t^{-1} x_t = (z_t^T Q_t^{-1} C_t)^T + R_t^{-1}(A_t x_{t-1} + B_t u_t)
\\
x_t = ( C_t^T Q_t^{-1} C_t + R_t^{-1})^{-1}(C_t^T Q_t^{-1}z_t + R_t^{-1}(A_t x_{t-1} + B_t u_t))
\]
這裡我們必須引用 Inversion Lemma 來解上式，Inversion Lemma 的式子如下：
\[
(R + PQP^T)^{-1}=R^{-1}-R^{-1}P(Q^{-1}+P^T R^{-1} P)^{-1}P^T R^{-1}
\]
上式的證明請參考 wikipedia。
接下來把 Inversion Lemma 代換：
\[
x_t = ( C_t^T Q_t^{-1} C_t + R_t)^{-1}(C_t^T Q_t^{-1}z_t + R_t^{-1}(A_t x_{t-1} + B_t u_t))
\\
= (R_t - R_t C_t^T(Q_t + C_t R_t C_t^T)^{-1}C_t R_t)(C_t^T Q_t^{-1}z_t + R_t^{-1}(A_t x_{t-1} + B_t u_t))
\]
上式中的 \(R_t C_t^T(Q_t + C_t R_t C_t^T)^{-1}\) 又稱為 Kalman Gain，接下來用 \(K_t\) 來表示：
\[
x_t = (R_t - K_t C_t R_t)(C_t^T Q_t^{-1}z_t + R_t^{-1}(A_t x_{t-1} + B_t u_t))
\\
= R_t C_t^T Q_t^{-1}z_t + (A_t x_{t-1} + B_t u_t) - K_t C_t R_t C_t^T Q_t^{-1}z_t - K_t C_t (A_t x_{t-1} + B_t u_t)
\\
= (A_t x_{t-1} + B_t u_t) - K_t C_t (A_t x_{t-1} + B_t u_t) + (I - K_t C_t)R_t C_t^T Q_t^{-1}z_t
\]
快要到最後一步了！現在要來證明 \((I - K_t C_t)R_t C_t^T Q_t^{-1} = K_t\)，證明的步驟是想辦法讓等號兩邊相等：
\[
(I - K_t C_t)R_t C_t^T Q_t^{-1} = R_t C_t^T Q_t^{-1} - K_t C_t R_t C_t^T Q_t^{-1} = K_t
\\
R_t C_t^T Q_t^{-1} = K(I + C_t R_t C_t^T Q_t^{-1}) = R_t C_t^T(Q_t + C_t R_t C_t^T)^{-1} (I + C_t R_t C_t^T Q_t^{-1})
\\
R_t C_t^T Q_t^{-1}Q_t = R_t C_t^T(Q_t + C_t R_t C_t^T)^{-1} (I + C_t R_t C_t^T Q_t^{-1}) Q_t
\\
R_t C_t^T = R_t C_t^T(Q_t + C_t R_t C_t^T)^{-1}(Q + C_t R_t C_t^T) = R_t C_t^T
\] 因此可以得到更乾淨的轉換式：
\[
x_t = (A_t x_{t-1} + B_t u_t) - K_t C_t (A_t x_{t-1} + B_t u_t) + K_t z_t
\\
= (A_t x_{t-1} + B_t u_t) + K_t(z_t - C_t (A_t x_{t-1} + B_t u_t))
\] 意思是當從 t-1 時間轉換至 t 時間時，高斯分布的平均會從 \(\mu_{t-1}\) 變成 \( (A_t \mu_{t-1} + B_t u_t) + K_t(z_t - C_t (A_t \mu_{t-1} + B_t u_t))\)。
總結 Kalman Filter 演算法
演算法的輸入是時間 t-1 狀態 \(x_{t-1}\) 的高斯分布參數 \(\mu_{t-1}, \Sigma_{t-1}\)，以及 t 時間的控制資訊 \(u_t\)、感測資料 \(z_t\)，而輸出是時間 t 狀態 \(x_{t}\) 的高斯分布參數 \(\mu_{t}, \Sigma_{t}\)：
\[
\bar{\mu}_t = A_t \mu_{t-1} + B_t u_t
\\
\bar{\Sigma}_t = A_t \Sigma_{t-1} A_t^T + R_t
\\
K_t = \bar{\Sigma}_t C_t^T(C_t \bar{\Sigma}_t C_t^T + Q_t)^{-1}
\\
\mu_t = \bar{\mu}_t + K_t(z_t - C_t \bar{\mu}_t)
\\
\Sigma_t = (I - K_t C_t)\bar{\Sigma}_t
\]
（註：本文略過了推導 \(\Sigma_t\) 的過程，有興趣的讀者可以參考原書中的證明。）