估計非線性系統狀態：Extended Kalman Filter 與 Unscented Kalman filter

當機器人的運動與觀測模型為非線性的時候，一般來說有三種方法來解非線性的情形。EKF 的方法是利用泰勒展開式線性逼近非線性的函數、Particle filter 的方法是利用蒙地卡羅法暴力地模擬出新的機率函數，而第三種方法則是用 sigmapoint，或稱為 unscented 轉換的方法。這個方法可以視為前兩種的折衷方案。

Extended Kalman Filter IEKF)

本文接續前文介紹的 Kalman filter，接著介紹當移動與觀測模型為非線性時該如何估計系統狀態。 Extended Kalman filter 為最常見的方法。EKF 的概念是用泰勒展開式，用前一輪估計結果線性近似這一輪的估計： \[ \mathbf{x}_k=f(\mathbf{x}_{k-1}, \mathbf{v}_k)+\mathbf{w}_k \\ \sim f(\mathbf{\hat{x}}_{k-1}, \mathbf{v}_k)+F_{k-1}(\mathbf{x}_{k-1}-\mathbf{\hat{x}}_{k-1})+\mathbf{w}'_k \\ \mathbf{y}_k=g(\mathbf{x}_k)+\mathbf{n}_k \\ \sim g(\mathbf{\check{x}}_k)+G_k(\mathbf{x}_k-\mathbf{\check{x}}_k)+\mathbf{n}'_k \]在 Kalman filter 中的移動與觀測模型機率密度函數也會跟著改變； \[ p(\mathbf{x}_k|\mathbf{x}_{k-1},\mathbf{v}_k)_{KF} \sim N(\mathbf{A}_{k-1}\mathbf{\hat{x}}_{k-1}+\mathbf{v}_k, \mathbf{Q}_k) \\ p(\mathbf{x}_k|\mathbf{x}_{k-1},\mathbf{v}_k)_{EKF} \sim N(\mathbf{\check{x}}_k+F_{k-1}(\mathbf{x}_{k-1}-\mathbf{\hat{x}}_{k-1}),\mathbf{Q}'_k) \\ p(\mathbf{y}_k|\mathbf{x}_k)_{KF} \sim N(\mathbf{C}_k\mathbf{\check{x}}_k,\mathbf{R}_k) \\ p(\mathbf{y}_k|\mathbf{x}_k)_{EKF} \sim N(\mathbf{\check{y}}_k+\mathbf{G}_k(\mathbf{x}_k-\mathbf{\check{x}}_k),\mathbf{R}'_k) \]因此相對於 Kalman filter，狀態轉換會變成以下的式子： \[ \mathbf{\check{x}}_k=f(\mathbf{\hat{x}}_{k-1},\mathbf{v}_k) \\ \mathbf{\check{P}}_k=\mathbf{F}_{k-1}\mathbf{\hat{P}}_{k-1}\mathbf{F}_{k-1}^T+\mathbf{Q}'_k \\ \mathbf{K}_k=\mathbf{\check{P}}_k\mathbf{G}_k^T(\mathbf{G}_k\mathbf{\check{P}}_k\mathbf{G}_k^T+\mathbf{R}'_k)^{-1} \\ \mathbf{\hat{P}}_k=(\mathbf{1}-\mathbf{K}_k\mathbf{G}_k)\mathbf{\check{P}}_k \\ \mathbf{\hat{x}}_k=\mathbf{\check{x}}_k+\mathbf{K}_k(\mathbf{y}_k-g(\mathbf{\check{x}}_k)) \]以下列出一些細節：

• \(\mathbf{y}_k-g(\mathbf{\check{x}}_k)\) 稱為 innovation
• 與 Kalman filter 不同的是其中用了非線性的運動與觀測函數來近似
• 一般來說無法保證 EKF 一定能準確地估計線性系統的狀態，主要的原因是線性近似的操作點為前一回合的狀態估計，在某些時候這微小的誤差會像滾雪球一樣越滾越大

Iterated Extended Kalman Filter (IEKF)

IEKF 與 EKF 的差別是利用迭代的方法找到一個更好的操作點，再用泰勒展開式線性估計其狀態：

1. 迭代法找出 \(\mathbf{x}_{op}\)，用 \(\mathbf{\check{x}}_{k-1}\) 當成初始值，每次的操作點為狀態估計的結果 \(\mathbf{\hat{x}}_k\)
2. 計算 \(\mathbf{y}_{op,k}=g(\mathbf{x}_{op,k})\)
3. 狀態更新式變為 \(\mathbf{\hat{x}}_k=\mathbf{\check{x}}_k+\mathbf{K}_k(\mathbf{y}_k-\mathbf{y}_{op,k}-\mathbf{G}_k(\mathbf{\check{x}}_k-\mathbf{x}_{op,k}))\)

值得一提的是 IEKF 為 MAP estimator，而 EKF 並不是。

Unscented Kalman filter (UKF)

當機器人的運動與觀測模型為非線性的時候，一般來說有三種方法來解非線性的情形。EKF 的方法是利用泰勒展開式線性逼近非線性的函數、Particle filter 的方法是利用蒙地卡羅法暴力地模擬出新的機率函數，而第三種要介紹的方法則是用 sigmapoint，或稱為 unscented 轉換的方法。這個方法可以視為前兩種的折衷方案。

1. 當狀態向量為 L 維時，找出 2L+1 個 sigmapoints。方法為將 covariance 矩陣 Cholesky 分解，讓每個 sigmapoint \(\mathbf{x}_i=\mathbf{\mu}_x+\sqrt{L+\kappa}\mathbf{L}_i\)
2. 利用非線性轉換函數轉換這些 sigmapoints
3. 算出轉換後 sigmapoints 的 mean 與 covariance

以下為一些值得一提的要點：

• 與 EKF 類似的是 UKF 也有迭代的版本 IUKF，或 ISPKF。
• IUKF 逼近的是 posterior mean，而不是 IEKF 逼近的 MAP 解；在 State Estimation for Robotis 書中的例子中的 posterior mean 的解比 MAP 準得多。