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

本文為 State Estimation for Robotics 書中第 3.2 節 Recursive Discrete-Time Smoothing 的筆記，目的是改進前文 batch discrete time estimation 的方法讓計算變快，並且在推導過程中引出 Kalman Filter 的式子。

Cholesky Smoother

前文要解的是 \((H^T W^{-1} H)\mathbf{\hat{x}}=H^T W^{-1}\mathbf{z}\)，而矩陣 \(H^T W^{-1} H\) 剛好為稀疏矩陣，因此能夠使用 Cholesky Decomposition 來分解成 \(H^T W^{-1} H = LL^T\)，另外只要令 \(L\mathbf{d}=H^T W^{-1}\mathbf{z}\) 並求解 \(\mathbf{d}\) 以後，我們想要解的 \(\mathbf{\hat{x}}\) 即可用 \(L^T \mathbf{\hat{x}} = \mathbf{d}\) 來解出來。

• 書中 3.61 式列出了如何從 \(L_0 L_0^T\) 開始解起，利用遞迴式子一層一層解出 \(LL^T\) 矩陣的其他子矩陣。
• 書中 3.63 式列出了如何從 \(\mathbf{d}_0\) 開始一路解出所有的 \(\mathbf{d}\)。
• 書中 3.65 式列出了 backward 從 \(L^T \mathbf{d}\) 至解出所有 \(\mathbf{\hat{x}}\) 的過程。
• 書中 3.66 式列出了整個 Cholesky Smoother 的計算過程，包含如何計算初始值，一步步 forward 求出矩陣 I (information matrix) 與 \(\mathbf{d}\)，再用 backward 計算 \(\mathbf{\hat{x}}\)。

Ranch-Tung-Striebel Smoother

Ranch-Tung-Striebel Smoother 使用了一些 SMW identity 代換 Cholesky 中的式子，在代換後的解的形式會變得更簡單。我們將分開討論 covariance、mean 以及 backward state 的式子。

Covariance

1. 將書中 3.66a 與 3.66c 代入 3.66d，可得到一串很長的 \(I_k\) 式子，再用 SMW identity (2.75b) 代換後可得到 \(I_k=(A_{k-1}I^{-1}_{k-1}A^T_{k-1}+Q_k)^{-1}+C^T_k R^{-1}_k C_k\)。
2. 用 \(\check{P}_{k,f}\) 代換 \(I_k^{-1}\)。\(\check{P}_{k,f}\) 稱為 predicted covariance，而 \(\hat{P}_{k,f}\) 稱為 corrected covariance。代換後上式會變成以下兩個步驟： \[ \check{P}_{k,f}=A_{k-1}\hat{P}_{k-1}A^T_{k-1}+Q_k \\ \hat{P}_k^{-1}=\check{P}_{k,f}^{-1}+C^T_kR^{-1}_kC_k \]
3. 定義矩陣 \(K_k\) (Kalman gain matrix)：\(K_k=\hat{P}_{k,f}C^T_kR^{-1}_k\)。利用 SMW identity (2.75b) 可改寫成以下形式：\(K_k=\check{P}_{k,f}C^T_k(C_k\check{P}_{k,f}C^T_k+R_k)^{-1}\)
4. 利用 SMW identity (2.75b) 代換： \[ \check{P}_{k,f}^{-1}=\hat{P}_{k,f}^{-1}-C^T_kR^{-1}_kC_k=\hat{P}_{k,f}^{-1}(\mathbf{1}-\hat{P}_{k,f}C^T_kR^{-1}_kC_k) \\ =\hat{P}_{k,f}^{-1}(\mathbf{1}-K_kC_k) \]
5. 因此 \(\hat{P}_{k,f}=(\mathbf{1}-K_kC_k)\check{P}_{k,f}\)

步驟 2 與 5 即為 covariance 的 forward 轉換式。

Mean

1. 代換 \(q_k\)，並且利用此關係式 \(\hat{P}^{-1}_{k,f}\hat{\mathbf{x}}_{k,f}=\mathbf{q}_k\)
2. 會變成以下兩個步驟，其中 \(\check{\mathbf{x}}_{k,f}\) 為 predicted mean，\(\hat{\mathbf{x}}_{k,f}\) 為 corrected mean： \[ \check{\mathbf{x}}_{k,f}=A_{k-1} \hat{\mathbf{x}}_{k-1,f}+\mathbf{v}_k \\ \hat{P}_{k,f}^{-1}\hat{\mathbf{x}}_{k,f}=\check{P}_{k,f}^{-1}\check{\mathbf{x}}_{k,f}+C_k^T R^{-1} \mathbf{y}_k \]
3. 整理後得到以下式子： \[ \hat{\mathbf{x}}_{k,f}=\hat{P}_{k,f}\check{P}^{-1}_{k,f}\check{\mathbf{x}}_{k,f}+\hat{P}_{k,f}C^T_kR^{-1}_k \mathbf{y}_k \\ \hat{P}_{k,f}\check{P}^{-1}_{k,f} = 1-K_kC_k \\ \hat{P}_{k,f}C^T_kR^{-1}_k = K_k \]

步驟 3 即為 mean 的 forward 轉換式。

Backward state

用類似的方法求解，可得以下轉換式：\(\hat{\mathbf{x}}_{k-1}=\hat{\mathbf{x}}_{k-1,f}+\hat{P}_{k-1,f}A^T_{k-1}\check{P}^{-1}_{k,f}(\mathbf{\hat{x}}_k-\check{\mathbf{x}}_{k,f})\)

整個 Ranch-Tung-Striebel Smoother 的流程即為將 covariance 的步驟 2, 3, 5、mean 的步驟 3、及 backward state 的式子湊起來。而這些 forward 的式子又被稱為 Kalman filter。要注意的是 smoother 指得是再看過所有的資料（包含未來的）後來估計狀態，而 filter 是只靠過去與現在的資料來估計現在的狀態。而 RTS smoother 與 batch 演算法是完全等價的，並不是近似解。