本文為 State Estimation for Robotics 書中第 5.1 節 Handling Input/Measurement Biases 的筆記。
Error Dynamics
首先我們推導在 Kalman filter 中的誤差轉換式，首先定義兩種誤差： \[ \mathbf{\check{e}}_k=\mathbf{\check{x}}_k-\mathbf{x}_k \\ \mathbf{\hat{e}}_k=\mathbf{\hat{x}}_k-\mathbf{x}_k \]利用 Kalman filter 與移動/觀測模型可以推導出以下 error dynamics： \[ \mathbf{\check{e}}_k=\mathbf{\check{x}}_k-\mathbf{x}_k =(\mathbf{A}_{k-1}\mathbf{\hat{x}}_{k-1}+\mathbf{v}_k) -(\mathbf{A}_{k-1}\mathbf{x}_{k-1}+\mathbf{v}_k+\mathbf{w}_k) \\ =\mathbf{A}_{k-1}\mathbf{\hat{e}}_{k-1}-\mathbf{w}_k \\ \mathbf{\hat{e}}_k=\mathbf{\hat{x}}_k-\mathbf{x}_k =\mathbf{\check{x}}_k+\mathbf{K}_k(\mathbf{y}_k-\mathbf{C}_k\mathbf{\check{x}}_k)-\mathbf{x}_k \\ =(\mathbf{1}-\mathbf{K}_k\mathbf{C}_k)(\mathbf{\check{x}}_k-\mathbf{x}_k)+\mathbf{K}_k\mathbf{y}_k-\mathbf{K}_k\mathbf{C}_k\mathbf{x}_k \\ =(\mathbf{1}-\mathbf{K}_k\mathbf{C}_k)\mathbf{\check{e}}_k+\mathbf{K}_k(\mathbf{y}_k-\mathbf{C}_k\mathbf{x}_k) \\ =(\mathbf{1}-\mathbf{K}_k\mathbf{C}_k)\mathbf{\check{e}}_k+\mathbf{K}_k\mathbf{n}_k \]線性的 Kalman filter 為 unbiased 可由 error dynamics 的平均與 covariance 用歸納法推導出來： \[ E[\mathbf{\check{e}}_k]=E[\mathbf{\hat{e}}_k]=\mathbf{0} \\ E[\mathbf{\check{e}}_k\mathbf{\check{e}}_k^T]=\mathbf{\check{P}}_k \\ E[\mathbf{\hat{e}}_k\mathbf{\hat{e}}_k^T]=\mathbf{\hat{P}}_k \]
存在 Bias 的 Kalman filter
當存在輸入及觀測的 bias 時，Kalman filter 的線性轉換式及 error dynamics 可以寫成以下式子： \[ \mathbf{x}_k=\mathbf{Ax}_{k-1}+\mathbf{B}(\mathbf{u}_k+\mathbf{\bar{u}})+\mathbf{w}_k \\ \mathbf{y}_k=\mathbf{Cx}_k+\mathbf{\bar{y}}+\mathbf{n}_k \\ \mathbf{\check{e}}_k=\mathbf{A\hat{e}}_{k-1}-(\mathbf{B\bar{u}}+\mathbf{w}_k) \\ \mathbf{\hat{e}}_k=(\mathbf{1}-\mathbf{K}_k\mathbf{C})\mathbf{\check{e}}_k+\mathbf{K}_k(\mathbf{\bar{y}}+\mathbf{n}_k) \]其中 \(\mathbf{\bar{u}}\) 與 \(\mathbf{\bar{y}}\) 分別為移動與觀測的 bias。
從 k=1 我們就可以看出此狀態估計的結果為 biased： \[ E[\mathbf{\check{e}}_1]=-\mathbf{B\bar{u}} \\ E[\mathbf{\hat{e}}_1]=-(\mathbf{1}-\mathbf{K}_1\mathbf{C})\mathbf{B\bar{u}}+\mathbf{K}_1\mathbf{\bar{y}} \\ \mathbf{\check{P}}_1 = E[\mathbf{\check{e}}_1\mathbf{\check{e}}_1^T]-E[\mathbf{\check{e}}_1]E[\mathbf{\check{e}}_1]^T \\ \mathbf{\hat{P}}_1 = E[\mathbf{\hat{e}}_1\mathbf{\hat{e}}_1^T]-E[\mathbf{\hat{e}}_1]E[\mathbf{\hat{e}}_1]^T \]也就是說 Kalman filter 會低估 uncertainty 的誤差。
未知的 Input Bias
實務上可以將 input bias \(\mathbf{\bar{u}}\) 加入要估計的狀態之中，跟著 \(\mathbf{x}\) 一起被估計。而根據 bias 與狀態轉換式的維度不同可能有幾種情況，主要是看以下矩陣的 rank：
\[
O'=\begin{bmatrix}
\mathbf{C'}\\
\mathbf{C'A'}\\
\vdots \\
\mathbf{C'A'}^{(N+U-1)}
\end{bmatrix}
\]
- 當 O' 的 rank 為 N 並且 U 為 0 時，此系統為 unbiased、observable 與 controllable。
- 當 O' 的 rank 等於 N + U 時，此系統為 observable 但 not-controllable。
- 當 O' 的 rank 小於 N + U 時，此系統為 unobservable。
未知的 Measurement Bias
用上述方法一樣能分析此系統是否為 observable。一個常見的例子是在 SLAM 的情況下我們不知道 measurement bias 的大小，因此當 landmark 與車子一起移動的時候我們仍然會得到相同的觀測。實務上我們用 Kalman filter 估計出來的結果會與初始值的猜測有關，或著可以說初始值的猜測就已經得將 measurement bias 考慮進去。