估計高斯分布下的線性系統狀態：Recursive Discrete-Time Filtering

本文為 State Estimation for Robotics 書中第 3.2 節 Recursive Discrete-Time Filtering 的筆記，目的是推導出 filter 的關係式讓線上的情形也能估計當前的系統狀態。本文將會先介紹如何分解 batch 方法得到的解，再用 MAP 與貝氏推論的方法推導出 Kalman filter 的式子。

分解 Batch 方法的解

從前文的 batch 方法開始，\((H^TW^{-1}H)x = H^TW^{-1}z\) 的式子中我們可以先把 H 分解成 12 個區塊： \[ \mathbf{H}= \begin{bmatrix} \mathbf{H}_{11} & & \\ \mathbf{H}_{21} & \mathbf{H}_{22} & \\ & \mathbf{H}_{32} & \mathbf{H}_{33} \\ & & \mathbf{H}_{43} \end{bmatrix} \]四個 row 分別表示 0 至 k-1 的資訊、k 的資訊、k+1 的資訊、k+2 至 K 的資訊；同理 W 矩陣也能分成四個主要的區塊。利用這些區塊改寫 \((H^TW^{-1}H)x = H^TW^{-1}z\) 的式子可以得到以下結果： \[ \mathbf{H}^T\mathbf{W}^{-1}\mathbf{Hx}= \begin{bmatrix} \mathbf{L}_{11} &\mathbf{L}_{12} & \\ \mathbf{L}_{12}^T & \mathbf{L}_{22} & \mathbf{L}_{32}^T\\ & & \mathbf{L}_{33} \end{bmatrix} \begin{bmatrix} \mathbf{\hat{x}}_{0:k-1}\\ \mathbf{\hat{x}}_k\\ \mathbf{\hat{x}}_{k+1:K} \end{bmatrix} =\begin{bmatrix} \mathbf{r}_1\\ \mathbf{r}_2\\ \mathbf{r}_3 \end{bmatrix} \]經由高斯消去法整理以後可以得到 \(\mathbf{\hat{x}}_k\) 的關係式： \[ (\mathbf{L}_{22}-\mathbf{L}_{12}^T\mathbf{L}_{11}^{-1}\mathbf{L}_{12}-\mathbf{L}_{32}^T\mathbf{L}_{33}^{-1}\mathbf{L}_{32}) \mathbf{\hat{x}}_k= (\mathbf{r}_2-\mathbf{L}_{12}^T\mathbf{L}_{11}^{-1}\mathbf{r}_1-\mathbf{L}_{32}^T\mathbf{L}_{33}^{-1} \mathbf{r}_3) \\ \mathbf{\hat{P}}_k^{-1}\mathbf{\hat{x}}_k=\mathbf{q}_k \] 在簡化、整理過後會成為以下幾個式子： \[ \mathbf{\hat{P}}_k^{-1}=\mathbf{H}_{22}^T\mathbf{\hat{P}}_{k,f}^{-1}\mathbf{H}_{22}+\mathbf{H}_{32}^T\mathbf{\hat{P}}^{-1}_{k,b}\mathbf{H}_{32} \\ \mathbf{q}_k=\mathbf{H}^T_{22}\mathbf{q}_{k,f}+\mathbf{H}^T_{32}\mathbf{q}_{k,b} \]可以看出第一項是 forward，第二項是 backward。另外再定義 forward 及 backward 的狀態估計結果 \(\mathbf{\hat{x}}_{k,f}\) 與 \(\mathbf{\hat{x}}_{k,b}\) 以後可以再改寫成以下式子： \[ \mathbf{\hat{P}}^{-1}_{k,f}\mathbf{\hat{x}}_{k,f}=\mathbf{q}_{k,f} \\ \mathbf{\hat{P}}^{-1}_{k,b}\mathbf{\hat{x}}_{k,b}=\mathbf{q}_{k,b} \\ \mathbf{\hat{x}}_k=\mathbf{\hat{P}}_k(\mathbf{H}_{22}^T\mathbf{\hat{P}}_{k,f}^{-1}\mathbf{\hat{x}}_{k,f} +\mathbf{H}_{32}^T\mathbf{\hat{P}}_{k,b}^{-1}\mathbf{\hat{x}}_{k,b} ) \]到這裡我們總算將整個 batch 方法的解拆解成 forward 及 backward 的高斯分布了：

• Batch 方法的解 \(p(\mathbf{x}_k|\mathbf{v},\mathbf{y})\rightarrow N(\mathbf{\hat{x}}_k,\mathbf{\hat{P}}_k)\)
• Forward 的高斯分布：\(p(\mathbf{x}_k|\mathbf{\check{x}}_0,\mathbf{v}_{1:k},\mathbf{y}_{0:k})\rightarrow N(\mathbf{\hat{x}}_{k,f},\mathbf{\hat{P}}_{k,f})\)
• Backward 的高斯分布：\(p(\mathbf{x}_k|\mathbf{v}_{k+1:K},\mathbf{y}_{k+1:K})\rightarrow N(\mathbf{\hat{x}}_{k,b},\mathbf{\hat{P}}_{k,b})\)

既然我們知道了 batch 方法能夠分解成 forward 及 backward，接下來就是要將 forward 解轉換為 recursive filter。

用 MAP 推導 Kalman filter

從這節開始為了簡化記號，將用 \(\mathbf{\hat{x}}_k\) 代替 \(\mathbf{\hat{x}}_{k,f}\)、\(\mathbf{\hat{P}}_k\) 代替 \(\mathbf{\hat{P}}_{k,f}\)。MAP 推導的精神是將 \(\mathbf{\hat{x}}\) 拆解成 \(p(y|x)\) 與 \(p(x|v)\)，而在 recursive filter 的條件下要解決的問題是給定 \(\mathbf{\hat{x}}_{k-1},\mathbf{\hat{P}}_{k-1},\mathbf{v}_k,\mathbf{y}_k\)，估計出最好的 \(\mathbf{\hat{x}}_k,\mathbf{\hat{P}}_k\)。

接下來運用前文得到的結果定義以下幾個矩陣： \[ \mathbf{z}= \begin{bmatrix} \mathbf{\hat{x}}_{k-1}\\ \mathbf{v}_k\\ \mathbf{y}_k \end{bmatrix} \\ \mathbf{H}= \begin{bmatrix} \mathbf{1} & \\ -\mathbf{A}_{k-1} & \mathbf{1}\\ & \mathbf{C}_k \end{bmatrix} \\ \mathbf{W}= \begin{bmatrix} \mathbf{\hat{P}}_{k-1} & & \\ & \mathbf{Q}_k & \\ & & \mathbf{R}_k \end{bmatrix} \]將這些式子都代入 \((\mathbf{H}^T\mathbf{W}^{-1}\mathbf{H})\mathbf{\hat{x}}=\mathbf{H}^T\mathbf{W}^{-1}\mathbf{z}\) 後會得到一串很長的 \(\mathbf{\hat{x}}\) 式子： \[ (\mathbf{Q}_k^{-1}-\mathbf{Q}_k^{-1}\mathbf{A}_{k-1} (\mathbf{\hat{P}}_{k-1}^{-1}+\mathbf{A}^T_{k-1}\mathbf{Q}_k^{-1}\mathbf{A}_{k-1} )^{-1}\mathbf{A}^T_{k-1}\mathbf{Q}_k^{-1} +\mathbf{C}^T_k\mathbf{R}_k^{-1}\mathbf{C}_k)\mathbf{\hat{x}}_k \\ =\mathbf{Q}_k^{-1}\mathbf{A}_{k-1}(\mathbf{\hat{P}}_{k-1}^{-1}+\mathbf{A}^T_{k-1}\mathbf{Q}_k^{-1}\mathbf{A}_{k-1})^{-1} (\mathbf{\hat{P}}^{-1}_{k-1}\mathbf{\hat{x}}_{k-1}-\mathbf{A}^T_{k-1}\mathbf{Q}_k^{-1}\mathbf{v}_k) \\ +\mathbf{Q}^{-1}_k\mathbf{v}_k+\mathbf{C}^T_k\mathbf{R}^{-1}_k\mathbf{y}_k \]

利用 SMW identity 代換並定義以下三個式子後可以更進一步簡化式子： \[ \mathbf{\check{P}}_k=\mathbf{Q}_k+\mathbf{A}_{k-1}\mathbf{\hat{P}}_{k-1}\mathbf{A}_{k-1}^T \\ \mathbf{\hat{P}}_k=(\mathbf{\check{P}}_k^{-1}+\mathbf{C}^T_k\mathbf{R}^{-1}_k\mathbf{C}_k)^{-1} \\ \mathbf{\check{x}}_k=\mathbf{A}_{k-1}\mathbf{\hat{x}}_{k-1}+\mathbf{v}_k \\ \mathbf{\hat{P}}_k^{-1}\mathbf{\hat{x}}_k=\mathbf{\check{P}}_k^{-1}\mathbf{\check{x}}_k+\mathbf{C}^T_k\mathbf{R}_k^{-1}\mathbf{y}_k \]上面的四個式子又稱為 Kalman filter 的 inverse covariance form 或 information form，也就是從 \(N(\hat{x}_{k-1},\hat{P}_{k-1})\) 利用 predictor 更新至高斯分布 \(N(\check{x}_k,\check{P}_k)\)，再用 measurement 的高斯分布 \(N(y_k,R_k)\) 當作 corrector 更新新的 estimate 高斯分布 \(N(\hat{x}_k,\hat{P}_k)\)。

定義 Kalman gain \(\mathbf{K}_k\) 以後即可再代換成以下的 recursive 式子： \[ \mathbf{K}_k=\mathbf{\check{P}}_k\mathbf{C}^T_k(\mathbf{C}_k\mathbf{\check{P}}_k\mathbf{C}^T_k+\mathbf{R}_k)^{-1} \\ P_k=(\mathbf{1}-\mathbf{K}_k\mathbf{C}_k)\mathbf{\check{P}}_k \\ \mathbf{\hat{x}}_k=\mathbf{\check{x}}_k+\mathbf{K}_k(\mathbf{y}_k-\mathbf{C}_k\mathbf{\check{x}}_k) \]到這為止我們將 Kalman filter 用 MAP 方法推導出 recursive 的式子了。以下有兩點值得一提：

• \(\mathbf{y}_k-\mathbf{C}_k\mathbf{\check{x}}_k\) 稱為 innovation，意義為期望的 measurement 與實際的 measurement 的差
• Kalman gain 的意義是將 innovation 與 predictor 的狀態估計取一個加權平均

用 Bayesian Inference 推導 Kalman filter

Bayesian inference 的方法比較單純，與前文中的推導過程非常類似：

1. 利用期望值公式推出 \(\mathbf{\check{x}}_k=E[\mathbf{x}_k]=\mathbf{A}_{k-1}\mathbf{\hat{x}}_{k-1}+\mathbf{v}_k\)
2. 一樣利用期望值公式推導 covariance：\(\mathbf{\check{P}}_k=E[(\mathbf{x}_k-E[\mathbf{x}_k])(\mathbf{x}_k-E[\mathbf{x}_k])^T]=\mathbf{A}_{k-1}\mathbf{\hat{P}}_{k-1}\mathbf{A}^T_{k-1}+\mathbf{Q}_k\)
3. Correction step 利用 Bayesian inference 寫出 posterior 高斯分布： \( p(\mathbf{x}_k|\mathbf{\check{x}}_0,\mathbf{v}_{1:k},\mathbf{y}_{0:k}) = N(\mathbf{\mu}_x+\mathbf{\Sigma}_{xy}\mathbf{\Sigma}^{-1}_{yy}(\mathbf{y}_k-\mathbf{\mu}_y), \mathbf{\Sigma}_{xx}-\mathbf{\Sigma}_{xy}\mathbf{\Sigma}^{-1}_{yy}\mathbf{\Sigma}_{yx} ) \)
4. 前一步驟的高斯分布的 mean 與 covariance 即為時間 k 的狀態估計結果。

要再強調一次的是：由於目前的模型為線性的，因此 MAP 與 Bayesian inference 會得到相同結果，但在非線性的情況下則否。