2020年6月29日 星期一

Kalman Filter─高斯分布與狀態估計(機器人學簡介)

本篇文章主要內容是簡介 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\) 的過程,有興趣的讀者可以參考原書中的證明。)

沒有留言:

張貼留言