估計非線性系統狀態：batch discrete time estimation

本文為 State Estimation for Robotics 書中第 4.3 節 Batch Discrete-Time Estimation 的筆記。接續上一篇估計非線性系統狀態：Extended Kalman Filter 與 Unscented Kalman filter，討論當我們有所有的觀測資料時，如何做整批資料的狀態估計。與前文不同的是前文為 filter，只有當前時間及以前的資料，而本文要介紹的是 smoother，假設可以取得整個系統中的所有觀測資料。

本文與前文：估計高斯分布下的線性系統狀態：batch discrete time estimation 一樣，分別討論 MAP 以及 Bayesian inference 兩種方式來推導非線性系統下的狀態估計式。

Maximum A Posteriori 方法

此方法先定義目標函數，再用高斯牛頓法來解。

目標函數

最佳化的目標為狀態向量 \(\mathbf{x} = [\mathbf{x}_0, \mathbf{x}_1, ... \mathbf{x}_K]^T\)。而針對移動模型與觀測模型的誤差可定義為： \[ \mathbf{e}_{v,k}(\mathbf{x})=f(\mathbf{x}_{k-1},\mathbf{v}_k, \mathbf{0})-\mathbf{x}_k \\ \mathbf{e}_{y,k}(\mathbf{x})=\mathbf{y}_k-g(\mathbf{x}_k,\mathbf{0}) \]因此將它們都加起來後可得： \[ J_{v,k}(\mathbf{x})=\frac{1}{2}\mathbf{e}_{v,k}(\mathbf{x})^T\mathbf{W}^{-1}_{v,k}\mathbf{e}_{v,k}(\mathbf{x}) \\ J_{y,k}(\mathbf{x})=\frac{1}{2}\mathbf{e}_{y,k}(\mathbf{x})^T\mathbf{W}^{-1}_{y,k}\mathbf{e}_{y,k}(\mathbf{x}) \\ J(\mathbf{x})=\sum_{k=0}^{K}(J_{v,k}(\mathbf{x}) + J_{y,k}(\mathbf{x}))=\frac{1}{2}\mathbf{e}(\mathbf{x})^T\mathbf{W}^{-1}\mathbf{e}(\mathbf{x}) \]利用 Cholesky 分解 \(\mathbf{W}^{-1} = \mathbf{L}^T\mathbf{L}\) 後可把上式寫成 \(J(\mathbf{x})=\frac{1}{2}\mathbf{u}(\mathbf{x})^T\mathbf{u}(\mathbf{x})\)，而我們要最佳化的函數即為 J(x)。

最佳化的演算法

非線性最佳化演算法像是牛頓法、高斯牛頓法、以及 Levenberg–Marquardt 都是常見的選擇。要注意的是高斯牛頓法由於用 Jacobian 矩陣近似 Hessian 矩陣，並不保證能夠收斂，因此實務上會用 Levenberg–Marquardt 來幫助收斂。

高斯牛頓法的迭代式為： \[ (\frac{\partial \mathbf{u}(x)}{\partial \mathbf{x}}\mid_{\mathbf{x}_{op}})^T (\frac{\partial \mathbf{u}(x)}{\partial \mathbf{x}}\mid_{\mathbf{x}_{op}}) \delta \mathbf{x}^*= -(\frac{\partial \mathbf{u}(x)}{\partial \mathbf{x}}\mid_{\mathbf{x}_{op}})^T \mathbf{u}(\mathbf{x}_{op}) \]另外 u(x) 的 Jacobian 可以寫成：\(\frac{\partial \mathbf{u}(\mathbf{x})}{\partial \mathbf{x}}=\mathbf{L}\frac{\partial \mathbf{e}(\mathbf{x})}{\partial \mathbf{x}}\)，代換至上式可得： \[ \mathbf{H}=-\frac{\partial \mathbf{e}(\mathbf{x})}{\partial \mathbf{x}}\mid_{\mathbf{x}_{op}} \\ (\mathbf{H}^T\mathbf{W}^{-1}\mathbf{H})\delta \mathbf{x}^*=\mathbf{H}^T\mathbf{W}^{-1}\mathbf{e}(\mathbf{x}_{op}) \]因此只要能算出 Jacobian 矩陣 H (與前文 batch discrete time estimation 中的式子非常類似) 以及計算 e 向量，即可求出這次的更新 \(\delta \mathbf{x}\)。

Bayesian Inference 方法

此方法從一組初始值 \(\mathbf{x}_{op}\) 開始，寫出狀態轉換式，再找出轉換後對應的高斯分布來解高斯牛頓的迭代式。

首先我們寫出 x 的狀態轉換近似式：\[\mathbf{x}_k\approx f(\mathbf{x}_{op,k-1},\mathbf{v}_k, \mathbf{0}) + \mathbf{F}_{k-1}(\mathbf{x}_{k-1}-\mathbf{x}_{op, k-1}) + \mathbf{w}'_k\]上式可以改寫成矩陣的形式： \[ \mathbf{x}=\mathbf{F}(\mathbf{\nu} +\mathbf{w}') \\ \nu= \begin{bmatrix} \mathbf{\check{x}}_0\\ f(\mathbf{x}_{op,0},\mathbf{v}_1, \mathbf{0}) - \mathbf{F}_0\mathbf{x}_{op,0}\\ \vdots \\ f(\mathbf{x}_{op,K-1},\mathbf{v}_K, \mathbf{0}) - \mathbf{F}_{K-1}\mathbf{x}_{op,K-1} \end{bmatrix} \\ F= \begin{bmatrix} \mathbf{1} & & & & \\ \mathbf{F}_0 & \mathbf{1} & & & \\ \mathbf{F}_1 \mathbf{F}_0 & \mathbf{F}_1 & \mathbf{1} & & \\ \vdots & \vdots & \vdots & \ddots & \\ \mathbf{F}_{K-1}\cdots \mathbf{F}_0 & \mathbf{F}_{K-1}\cdots \mathbf{F}_1 & \mathbf{F}_{K-1}\cdots \mathbf{F}_2 & \mathbf{F}_{K-1} & \mathbf{1} \end{bmatrix} \]

Prior 的高斯分布

Prior 的分布可由以下式子推得： \[ \mathbf{\check{x}}=E[\mathbf{x}]=E[\mathbf{F}(\nu+\mathbf{w}')]=\mathbf{F}\nu \\ \mathbf{\check{P}}=E[(\mathbf{x}-E[\mathbf{x}])(\mathbf{x}-E[\mathbf{x}])^T]=\mathbf{F}E[\mathbf{w'w'}^T]\mathbf{F}^T=\mathbf{FQ'F}^T \]因此 prior 分布可以寫成 \(\mathbf{x} \sim N(\mathbf{F}\nu,\mathbf{FQ'F}^T)\)。

觀測模型的分布

線性近似後的觀測模型式子可寫成： \[ \mathbf{y}_k \approx \mathbf{g}(\mathbf{x}_{op,k},\mathbf{0})+\mathbf{G}_k(\mathbf{x}_{k-1}-\mathbf{x}_{op,k-1})+\mathbf{n}'_k \]寫成矩陣式為： \[ \mathbf{y}=\mathbf{y}_{op}+\mathbf{G}(\mathbf{x}-\mathbf{x}_{op})+\mathbf{n}' \\ \mathbf{y}_{op}= \begin{bmatrix} \mathbf{g}(\mathbf{x}_{op,0},\mathbf{0})\\ \vdots \\ \mathbf{g}(\mathbf{x}_{op,K},\mathbf{0}) \end{bmatrix} \\ \mathbf{G}=diag(\mathbf{G}_0,...\mathbf{G}_K) \]下一步是計算 y 的平均與 covariance： \[ E[\mathbf{y}]=\mathbf{y}_{op}+\mathbf{G}(\mathbf{\check{x}}-\mathbf{x}_{op}) \\ E[(\mathbf{y}-E[\mathbf{y}])(\mathbf{y}-E[\mathbf{y}])^T]=\mathbf{G\check{P}G}^T+\mathbf{R}' \\ E[(\mathbf{y}-E[\mathbf{y}])(\mathbf{x}-E[\mathbf{x}])^T]=\mathbf{G\check{P}} \]因此 x 與 y 的 joint distribution 就能寫成： \[ p(\mathbf{x},\mathbf{y}|\mathbf{v}) = N( \begin{bmatrix} \mathbf{\check{x}}\\ \mathbf{y}_{op}+\mathbf{G}(\mathbf{\check{x}}-\mathbf{x}_{op}) \end{bmatrix} , \begin{bmatrix} \mathbf{\check{P}} & \mathbf{\check{P}G}^T\\ \mathbf{G\check{P}} & \mathbf{G\check{P}G}^T+\mathbf{R}' \end{bmatrix} ) \]利用前文提過的 Schur Complement 即可求出 posterior 分布（細節請參考前文）。

而將 \(\mathbf{\hat{x}}-\mathbf{x}_{op}\) 寫成 \(\delta \mathbf{x}^*\)，再整理式子後將會得到與 MAP 方法完全相同的結果。

討論

• 一般來說 EKF 能比只做一回合的高斯牛頓法得到更準確的結果，但是 EKF 必須遵守馬可夫假設。
• IEKF 雖然能比 EKF 更準確，但仍然需要遵守馬可夫假設。
• Batch estimation 的高斯牛頓法是個 offline 的演算法，也不是 constant-time 的演算法，但是 EKF 為 online 與 constant-time。