估計高斯分布下的線性系統狀態：batch discrete time estimation

本文為 State Estimation for Robotics 書中第 3.1 節 Batch Discrete-Time Estimation 的筆記，目的是先將機器人學中的線性系統狀態估計問題定義清楚，並且先用最直觀（但是無法 real-time）的方法來求解。

高斯分布下的線性系統狀態估計問題

這篇與前文：狀態估計 State Estimation 與狀態轉換式相關。首先假設機器人的移動與觀測模型：

• 移動模型：\(\mathbf{x}_k=\mathbf{A}_{k-1}\mathbf{x}_{k-1}+\mathbf{v}_k+\mathbf{w}_k,\ \ k = 1\cdots K\)
• 觀測模型：\(\mathbf{y}_k=\mathbf{C}_k\mathbf{x}_k+\mathbf{n}_k,\ \ k=0\cdots K\)

其中 x 為整個系統的狀態，為 N 維的向量。v 為移動模型的輸入，w 為噪音。y 為觀測到的資料，為 M 維的向量，而 n 為觀測的噪音。其中噪音皆為高斯分布，其 covariance 分別為 Q 以及 R。

此問題的目標是要找到所有時間的系統狀態 x。利用 batch 有兩種方法可以解決此問題：

• Maximum a posteriori (MAP)：利用最佳化的方法找出最有可能的 posterior 狀態
  
• Bayesian inference：利用狀態轉換式解出 posterior 高斯分布

由於目前的問題假設是線性系統，兩種方法得到的解會相同；但當問題假設為非線性系統時就會得到不同的解了。

Maximum a posteriori (MAP) 方法

我們想要解的是 x 的 posterior 分布，並經過貝氏定理轉換後可得到： \[ \hat{\mathbf{x}}=arg\ \underset{\mathbf{x}}{max}\ p(\mathbf{x}|\mathbf{v}, \mathbf{y}) \\ p(\mathbf{x}|\mathbf{v}, \mathbf{y}) = \frac{p(\mathbf{x}, \mathbf{y}, \mathbf{v})}{p(\mathbf{v}, \mathbf{y})} =p(\mathbf{x}, \mathbf{y}, \mathbf{v}) \frac{p(\mathbf{x}|\mathbf{v})}{p(\mathbf{y}|\mathbf{v})p(\mathbf{x},\mathbf{v})} \\ =p(\mathbf{y}|\mathbf{x},\mathbf{v})\frac{p(\mathbf{x}|\mathbf{v})}{p(\mathbf{y}|\mathbf{v})} =p(\mathbf{y}|\mathbf{x})p(\mathbf{x}|\mathbf{v}) \]這幾個機率分布可以分別拆解成： \[ p(\mathbf{y}|\mathbf{x})=\prod_{k=0}^Kp(\mathbf{y}_k|\mathbf{x}_k) \\ p(\mathbf{x}|\mathbf{v})=p(\mathbf{x}_0|\mathbf{\check{x}}_0)\prod_{k=1}^{K}p(\mathbf{x}_k|\mathbf{x}_{k-1},\mathbf{v}_k) \]將他們取對數後會得到以下式子： \[ ln\ p(\mathbf{x}_0|\mathbf{\check{x}}_0)=-\frac{1}{2}(\mathbf{x}_0-\mathbf{\check{x}}_0)^T\mathbf{\check{P}}_0^{-1}(\mathbf{x}_0-\mathbf{\check{x}}_0)-C_1 \\ ln\ p(\mathbf{x}_k|\mathbf{x}_{k-1},\mathbf{v}_k)=\frac{1}{2}(\mathbf{x}_k-\mathbf{A}_{k-1}\mathbf{x}_{k-1}-\mathbf{v}_k)^T \mathbf{Q}_k^{-1}(\mathbf{x}_k-\mathbf{A}_{k-1}\mathbf{x}_{k-1}-\mathbf{v}_k)-C_2 \\ ln\ p(\mathbf{y}_k|\mathbf{x}_k)=-\frac{1}{2}(\mathbf{y}_k-\mathbf{C}_k\mathbf{x}_k)^T\mathbf{R}_k^{-1}(\mathbf{y}_k-\mathbf{C}_k\mathbf{x}_k)-C_3 \]將它們都相加起來後就得到此目標函數 \(\mathbf{\hat{x}} = arg\ \underset{\mathbf{x}}{max}\ J(\mathbf{x})\)。

將此長式子轉換成矩陣後可以得到 \(J(\mathbf{x})=\frac{1}{2}(\mathbf{\mathbf{z}}-\mathbf{Hx})^T \mathbf{W}^{-1}(\mathbf{z}-\mathbf{Hx})\) 的形式： \[ \mathbf{z}=\begin{bmatrix} \mathbf{\check{x}}_0\\ \mathbf{v}_1\\ \vdots\\ \mathbf{v}_k\\ \mathbf{y}_0\\ \vdots\\ \mathbf{y}_K \end{bmatrix} , \mathbf{x}=\begin{bmatrix} \mathbf{x}_1\\ \vdots\\ \mathbf{x}_K \end{bmatrix} \\ \mathbf{H}=\begin{bmatrix} \mathbf{1} & & & \\ -\mathbf{A}_0 & \mathbf{1} & & \\ & \ddots & \ddots & \\ & & -\mathbf{A}_{K-1} & \mathbf{1} \\ \mathbf{C}_0 & & & \\ & \mathbf{C}_1 & & \\ & & \ddots & \\ & & & \mathbf{C}_K \end{bmatrix} \\ \mathbf{W}= \begin{bmatrix} \mathbf{\check{P}}_0 & & & & & & \\ & \mathbf{Q}_1 & & & & & \\ & & \ddots & & & & \\ & & & \mathbf{Q}_K & & & \\ & & & & \mathbf{R}_0 & & \\ & & & & & \ddots & \\ & & & & & & \mathbf{R}_K \end{bmatrix} \]最後用 least square 解可得到：\((\mathbf{H}^T \mathbf{W}^{-1}\mathbf{H})\mathbf{\hat{x}}=\mathbf{H}^T \mathbf{W}^{-1}\mathbf{z}\)。

Bayesian inference 方法

當我們將移動模型寫成遞迴式時，可以改寫成以下的矩陣式 \(\mathbf{x}=\mathbf{A}(\mathbf{v}+\mathbf{w})\)： \[ \mathbf{A}= \begin{bmatrix} \mathbf{1} & & & & \\ \mathbf{A}_0 & \mathbf{1} & & & \\ \mathbf{A}_1\mathbf{A}_0 & \mathbf{A}_1 & \mathbf{1} & & \\ \vdots & \vdots & \vdots & \ddots & \\ \mathbf{A}_{K-1}\cdots \mathbf{A}_0 & \mathbf{A}_{K-1}\cdots \mathbf{A}_1 & \cdots & \mathbf{A}_{K-1} & \mathbf{1} \end{bmatrix} \]而 x 的期望值為 \(\mathbf{\check{x}}=E[\mathbf{x}]=E[\mathbf{A}(\mathbf{v}+\mathbf{w})]=\mathbf{Av}\)；Covariance 為 \(\mathbf{\check{P}}=\mathbf{AQA}^T\)。因此 \(p(\mathbf{x}|\mathbf{v})\) 是平均為 \(\mathbf{Av}\)，covariance 為 \(\mathbf{AQA}^T\) 的高斯分布。

另一方面，觀測模型可以寫成 \(\mathbf{y}= \mathbf{Cx}+\mathbf{n}\) 的形式，\(\mathbf{C}=diag(\mathbf{C}_0,\mathbf{C}_1,\cdots, \mathbf{C}_K)\)。\(p(\mathbf{x},\mathbf{y}|\mathbf{v})\) 可以因此寫成以下高斯分布： \[ N(\begin{bmatrix} \mathbf{\check{x}}\\ \mathbf{C\check{x}} \end{bmatrix} , \begin{bmatrix} \mathbf{\check{P}} & \mathbf{\check{P}C}^T\\ \mathbf{C\check{P}} & \mathbf{C\check{P}C}^T+\mathbf{R} \end{bmatrix} ) \]\(p(\mathbf{x},\mathbf{y}|\mathbf{v})\) 可以分解成 \(p(\mathbf{x}|\mathbf{v},\mathbf{y})p(\mathbf{y}|\mathbf{v})\)，而我們只對第一項的 posterior 有興趣。接下來的一小節要介紹如何從 \(p(\mathbf{x},\mathbf{y}|\mathbf{v})\) 的高斯分布推導至 \(p(\mathbf{x}|\mathbf{v},\mathbf{y})\)。

Schur Complement

Schur complement 可以用來解決子矩陣的反矩陣問題： \[ M=\begin{bmatrix} A & B\\ C & D \end{bmatrix} \\ M^{-1} = \begin{bmatrix} I & 0\\ -D^{-1}C & I \end{bmatrix} \begin{bmatrix} (A-BD^{-1}C)^{-1} & 0\\ 0 & D^{-1} \end{bmatrix} \begin{bmatrix} I & -BD^{-1}\\ 0 & 0 \end{bmatrix} \]其中 \(A-BD^{-1}C\) 稱為 Schur Complement。

我們利用此性質可以用來分解此高斯分布 \(p(\mathbf{x},\mathbf{y}) = N( \begin{bmatrix} \mathbf{\mu}_x\\ \mathbf{\mu}_y \end{bmatrix} , \begin{bmatrix} \Sigma_{xx} & \Sigma_{xy}\\ \Sigma_{yx} & \Sigma_{yy} \end{bmatrix} )\)：\[ (x-\mu)^T \Sigma (x-\mu) \\= (x-\mu)^T \begin{bmatrix} I & 0\\ -\Sigma_{yy}^{-1}\Sigma_{yx} & I \end{bmatrix} \begin{bmatrix} (\Sigma_{xx}-\Sigma_{xy}\Sigma_{yy}^{-1}\Sigma_{yx})^{-1} & 0\\ 0 & \Sigma^{-1}_{yy} \end{bmatrix} \\ \begin{bmatrix} I & -\Sigma_{xy}\Sigma_{yy}^{-1}\\ 0 & I \end{bmatrix} (x-\mu) \\ =(x-\mu_x-\Sigma^{-1}_{yy}\Sigma_{yx}(y-\mu_y))^T(\Sigma_{xx}-\Sigma_{xy}\Sigma_{yy}^{-1})(x-\mu_x-\Sigma^{-1}_{yy}\Sigma_{yx}(y-\mu_y)) \\+(y-\mu_y)^T \Sigma_{yy}^{-1}(y-\mu_y) \]也就是說： \[ p(\mathbf{x},\mathbf{y})=p(\mathbf{x}|\mathbf{y})p(\mathbf{y}) \\ p(\mathbf{x}|\mathbf{y}) = N(\mu_x+\Sigma_{xy}\Sigma^{-1}_{yy}(y-\mu_y),\Sigma_{xx}-\Sigma_{xy}\Sigma^{-1}_{yy}\Sigma_{yx}) \]

利用 Schur Complement 解 posterior

帶回原式後可得： \[ p(\mathbf{x}|\mathbf{v},\mathbf{y}) =N(\mathbf{\check{x}}+\mathbf{\check{P}}\mathbf{C}^T(\mathbf{C\check{P}C}^T+\mathbf{R})^{-1}(\mathbf{y}-\mathbf{C\check{x}}), \\ \mathbf{\check{P}}-\mathbf{\check{P}C}^T(\mathbf{C\check{P}C}^T+\mathbf{R})^{-1}\mathbf{C\check{P}} ) \]接下來要利用 Sherman-Morrison-Woodbury 公式，也有人稱為 matrix inversion lemma 來簡化式子：

1. \(\mathbf{(A^{-1}+BD^{-1}C)^{-1}\equiv A-AB(D+CAB)^{-1}CA}\)
2. \(\mathbf{(D+CAB)^{-1}\equiv D^{-1}-D^{-1}C(A^{-1}+BD^{-1}C)^{-1}BD^{-1}}\)
3. \(\mathbf{AB(D+CAB)^{-1}\equiv (A^{-1}+BD^{-1}C)^{-1}BD^{-1}}\)
4. \(\mathbf{(D+CAB)^{-1}CA \equiv D^{-1}C(A^{-1}+BD^{-1}C)^{-1}}\) 

利用前兩個式子可將 \(p(\mathbf{x}|\mathbf{v},\mathbf{y})\) 轉化成： \[ N( (\mathbf{\check{P}}^{-1}+\mathbf{C}^T\mathbf{R}^{-1}\mathbf{C})^{-1}(\mathbf{\check{P}}^{-1}\mathbf{\check{x}}+\mathbf{C}^T\mathbf{R}^{-1}\mathbf{y}) , (\mathbf{\check{P}}^{-1}+\mathbf{C}^T\mathbf{R}^{-1}\mathbf{C})^{-1} ) =N(\mathbf{\hat{x}},\mathbf{\hat{P}}) \]將式子整理過後可得到： \[ (\mathbf{\check{P}}^{-1}+\mathbf{C}^T\mathbf{R}^{-1}\mathbf{C})\mathbf{\hat{x}}=\mathbf{\check{P}}^{-1}\mathbf{\check{x}}+\mathbf{C}^T\mathbf{R}^{-1}\mathbf{y} \\ \mathbf{\check{x}}=\mathbf{Av} \\ \mathbf{\check{P}}^{-1}=(\mathbf{AQA}^T)^{-1}=\mathbf{A}^{T}\mathbf{Q}^{-1}\mathbf{A}^{-1} \\ (\mathbf{A}^{T}\mathbf{Q}^{-1}\mathbf{A}^{-1} + \mathbf{C}^T\mathbf{R}^{-1}\mathbf{C})\mathbf{\hat{x}}=\mathbf{A}^{T}\mathbf{Q}^{-1}\mathbf{v}+\mathbf{C}^T\mathbf{R}^{-1}\mathbf{y} \]此結果與 MAP 方法得到的完全相同。