這一章將介紹卡爾曼濾波、擴展卡爾曼濾波以及無跡卡爾曼濾波，並從貝葉斯濾波的角度來進行分析並完成數學推導。如果您對貝葉斯濾波不了解，可以查閱相關書籍或閱讀 【概率機器人 2 遞歸狀態估計】。
這三種濾波方式都假設狀態變量 $\mathbf{x}_t$ 的置信度 $\mathrm{bel}(\mathbf{x}_t)$ 為正態分布，這樣在計算 $\mathbf{x}_t$ 的置信度時，只需要計算出其分布的均值與方差即可。下面將分別介紹這三種濾波算法。
1.卡爾曼濾波
首先，回顧一下貝葉斯濾波。其目標是求解狀態變量 $\mathbf{x}_t$ 的置信度 $ \mathrm{bel}(\mathbf{x}_t)$ ，求解思想是以遞歸的方式（根據 $ \mathrm{bel}(\mathbf{x}_{t-1})$ 計算 $ \mathrm{bel}(\mathbf{x}_t)$ ），具體實現分為兩個步驟：
(a) 預測：利用 $ \mathrm{bel}(\mathbf{x}_{t-1})$ 計算 $ \overline{\mathrm{bel}}(\mathbf{x}_t)$ ，即
$$ \overline{\mathrm{bel}}(\mathbf{x}_t) = \int p(\mathbf{x}_t | \mathbf{x}_{t-1}, \mathbf{u}_t) \mathrm{bel}(\mathbf{x}_{t-1}) \mathrm{d}\mathbf{x}_{t-1} \tag{3.1} $$
(b) 測量更新：利用測量值 $\mathbf{z}_t$ 和 $ \overline{\mathrm{bel}}(\mathbf{x}_t)$ 計算 $ \mathrm{bel}(\mathbf{x}_{t})$，即
$$ \mathrm{bel}(\mathbf{x}_t) = \eta p(\mathbf{z}_t | \mathbf{x}_t) \overline{\mathrm{bel}}(\mathbf{x}_t) \tag{3.2} $$
卡爾曼濾波是通過合理的假設以方便實現以上的計算，具體而言就是假設 $p(\mathbf{x}_t | \mathbf{x}_{t-1}, \mathbf{u}_t) \mathrm{bel}(\mathbf{x}_{t-1})$ 和 $p(\mathbf{z}_t | \mathbf{x}_t)$ 為高斯分布，這樣就可以用均值 $\boldsymbol{\mu}_t$ 和方差 $\mathbf{\Sigma}_t$ 來表示置信度 $ \mathrm{bel}(\mathbf{x}_t)$。具體假設如下：
(1) 狀態轉移概率 $p(\mathbf{x}_t | \mathbf{x}_{t-1}, \mathbf{u}_t) $ 是帶有隨機高斯噪聲的線性函數，可由下式表示：
$$ \mathbf{x}_t = \mathbf{A}_t \mathbf{x}_{t-1} + \mathbf{B}_t\mathbf{u}_t + \boldsymbol{\varepsilon}_t \tag{3.3} $$
其中，$\boldsymbol{\varepsilon}_t$ 表示高斯隨機噪聲，均值為0，方差為 $\mathbf{R}_t$。
這樣，狀態轉移概率 $p(\mathbf{x}_t | \mathbf{x}_{t-1}, \mathbf{u}_t) $ 可由式(3.3)得出：
$$p(\mathbf{x}_t | \mathbf{x}_{t-1}, \mathbf{u}_t) = \mathrm{det} (2\pi \mathbf{R}_t)^{-\frac{1}{2}} \exp \left \{ -\frac{1}{2} ( \mathbf{x}_t - \mathbf{A}_t\mathbf{x}_{t-1} - \mathbf{B}_t\mathbf{u}_t )^T \mathbf{R}_t^{-1} ( \mathbf{x}_t - \mathbf{A}_t\mathbf{x}_{t-1} - \mathbf{B}_t\mathbf{u}_t ) \right \} \tag{3.4} $$
(2) 測量概率 $p(\mathbf{z}_t | \mathbf{x}_t )$ 也是與帶有高斯噪聲的自變量呈線性關系：
$$ \mathbf{z}_t = \mathbf{C}_t \mathbf{x}_t + \boldsymbol{\delta}_t \tag{3.5} $$
其中，$\boldsymbol{\delta}_t$ 為測量噪聲，是均值為0，方差為 $\mathbf{Q}_t$ 的高斯隨機分布。
因此，測量概率可由式(3.5)得出：
$$ p(\mathbf{z}_t | \mathbf{x}_t ) = \mathrm{det} (2\pi \mathbf{Q}_t)^{-\frac{1}{2}} \exp \left \{ -\frac{1}{2} ( \mathbf{x}_t - \mathbf{C}_t\mathbf{x}_t )^T \mathbf{Q}_t^{-1}( \mathbf{x}_t - \mathbf{C}_t\mathbf{x}_t ) \right \} \tag{3.6} $$
(3) 初始置信度 $ \mathrm{bel}(\mathbf{x}_0)$ 是正態分布，其均值為 $\boldsymbol{\mu}_0$ ，方差為 $\mathbf{\Sigma}_0$：
$$ \mathrm{bel}(\mathbf{x}_t) = p(\mathbf{x}_0) = \mathrm{det}(2\pi \mathbf{\Sigma}_0)^{-\frac{1}{2}} \exp \left\{ -\frac{1}{2} (\mathbf{x}_0 - \boldsymbol{\mu}_0)^T \mathbf{\Sigma}_0^{-1} (\mathbf{x}_0 - \boldsymbol{\mu}_0) \right\} \tag{3.7} $$
以上三個假設保證了 $ \mathrm{bel}(\mathbf{x}_t)$ 在任何時刻 $t$ 總符合高斯分布，這樣我們求解其均值與方差即可。
1.1 卡爾曼濾波算法
程序3.1描述了KF算法（Kalman filter algorithm），時刻 $t$ 的置信度 $ \mathrm{bel}(\mathbf{x}_t)$ 用 均值 $\boldsymbol{\mu}_t$ 、方差 $\mathbf{\Sigma}_t$ 表示。
| 程序3.1 卡爾曼濾波算法 | |
| 1: | Algorithm Kalman_filter( $\boldsymbol{\mu}_{t-1}$, $\mathbf{\Sigma}_{t-1}$, $\mathbf{u}_t$, $\mathbf{z}_t$ ): |
| 2: | $ \overline{\boldsymbol{\mu}}_t = \mathbf{A}_t \boldsymbol{\mu}_{t-1} + \mathbf{B}_t\mathbf{u}_t $ |
| 3: | $ \overline{\mathbf{\Sigma}}_t = \mathbf{A}_t \mathbf{\Sigma}_{t-1} \mathbf{A}_t^T + \mathbf{R}_t $ |
| 4: | $ \mathbf{K}_t = \overline{\mathbf{\Sigma}}_t \mathbf{C}_t^T (\mathbf{C}_t \overline{\mathbf{\Sigma}}_t \mathbf{C}_t^T + \mathbf{Q}_t)^{-1} $ |
| 5: | $ \boldsymbol{\mu}_t = \overline{\boldsymbol{\mu}}_t + \mathbf{K}_t (\mathbf{z}_t - \mathbf{C}_t \overline{\boldsymbol{\mu}}_t ) $ |
| 6: | $ \mathbf{\Sigma}_t = (\mathbf{I} - \mathbf{K}_t \mathbf{C}_t) \overline{\mathbf{\Sigma}}_t $ |
| 7: | return $\boldsymbol{\mu}_t$, $\mathbf{\Sigma}_t$ |
程序3.1中的第2、3行為預測階段，第4、5、6行為測量更新階段。其中第4行計算的變量矩陣 $\mathbf{K}_t $ 叫做卡爾曼增益矩陣，它明確了測量綜合到新的狀態估計的程度。
1.2 卡爾曼濾波的數學推導
(1) 預測階段：
程序3.1中的第2、3行為預測階段，其目標是根據時刻 $t-1$ 的置信度 $ \mathrm{bel}(\mathbf{x}_{t-1})$ 來計算 時刻 $t$ 的 $ \overline{\mathrm{bel}}(\mathbf{x}_t)$。
在這里， 置信度 $ \mathrm{bel}(\mathbf{x}_{t-1})$ 由均值 $\boldsymbol{\mu}_{t-1}$ 和方差 $\mathbf{\Sigma}_{t-1}$ 表達，而 $ \overline{\mathrm{bel}}(\mathbf{x}_t)$ 由 均值 $\overline{\boldsymbol{\mu}}_{t-1}$ 和方差 $\overline{\mathbf{\Sigma}}_{t-1}$ 表達。
根據 式(3.1)(3.4)，可得
$$ \begin{eqnarray*} \overline{\mathrm{bel}}(\mathbf{x}_t) &=& \int \underbrace{p(\mathbf{x}_t | \mathbf{x}_{t-1}, \mathbf{u}_t)}_{N(\mathbf{x}_t;\, \mathbf{A}_t \mathbf{x}_{t-1}\, +\mathbf{B}_t\mathbf{u}_t, \mathbf{R}_t)} \underbrace{ \mathrm{bel}(\mathbf{x}_{t-1}) }_{\; N(\mathbf{x}_{t-1}\,; \boldsymbol{\mu}_{t-1} \, , \mathbf{\Sigma}_{t-1} ) } \mathrm{d}\mathbf{x}_{t-1} \\ &=& \eta \int \exp \left\{ -\frac{1}{2} ( \mathbf{x}_t - \mathbf{A}_t\mathbf{x}_{t-1} - \mathbf{B}_t\mathbf{u}_t )^T \mathbf{R}_t^{-1} ( \mathbf{x}_t - \mathbf{A}_t\mathbf{x}_{t-1} - \mathbf{B}_t\mathbf{u}_t ) \right\} \\ & & \exp \left\{ -\frac{1}{2} (\mathbf{x}_{t-1} - \boldsymbol{\mu}_{t-1})^T \mathbf{\Sigma}_{t-1}^{-1} (\mathbf{x}_{t-1} - \boldsymbol{\mu}_{t-1}) \right\} \mathrm{d}\mathbf{x}_{t-1} \end{eqnarray*} \tag{3.8} $$
式(3.8)包含了一個對 $\mathbf{x}_{t-1}$ 的積分，我們首先嘗試來消除這個積分。定義
$$\begin{eqnarray*} L_t =& \frac{1}{2}( \mathbf{x}_t - \mathbf{A}_t \mathbf{x}_{t-1} -\mathbf{B}_t\mathbf{u}_t )^T \mathbf{R}_t^{-1} ( \mathbf{x}_t - \mathbf{A}_t \mathbf{x}_{t-1} -\mathbf{B}_t\mathbf{u}_t ) + \\ & \frac{1}{2} (\mathbf{x}_{t-1} - \boldsymbol{\mu}_{t-1})^T \mathbf{\Sigma}_{t-1}^{-1} (\mathbf{x}_{t-1} - \boldsymbol{\mu}_{t-1}) \end{eqnarray*} \tag{3.9}$$
這樣，式(3.8)簡化為
$$ \overline{\mathrm{bel}}(\mathbf{x}_t) = \eta \int \exp \{ -L_t \} \mathrm{d}\mathbf{x}_{t-1} \tag{3.10} $$
為了能夠消除式(3.8)中的積分，我們從 $L_t$ 的形式入手。由於 $L_t$ 是$\mathbf{x}_{t-1}$的二次型，也是$\mathbf{x}_{t}$的二次型，因此我們首先從 $L_t$ 分解出 $\mathbf{x}_{t-1}$ 的二次型項。即 $L_t$ 可分解為：
$$ L_t = L_t(\mathbf{x}_t) + \frac{1}{2}(\mathbf{x}_{t-1} - \Delta)^T \mathbf{\Psi} ^{-1}(\mathbf{x}_{t-1} -\Delta) \tag{3.11} $$
這樣，式(3.11)中的第一項 $L_t(\mathbf{x}_t)$ 可以從積分中提取出來，而第二項可以利用“正態分布的積分為1”來消去。
現在我們來求出式(3.11)中的 $\Delta$ 和 $\mathbf{\Psi}$，根據式(3.9)計算 $L_t$ 對 $\mathbf{x}_{t-1}$ 的一二階導數：
$$ \frac{\partial L_t}{\partial \mathbf{x}_{t-1}} = - \mathbf{A}_t^T \mathbf{R}_t^{-1} ( \mathbf{x}_t - \mathbf{A}_t\mathbf{x}_{t-1} - \mathbf{B}_t\mathbf{u}_t) + \mathbf{\Sigma}_{t-1}^{-1} (\mathbf{x}_{t-1}-\boldsymbol{\mu}_{t-1}) \tag{3.12} $$
$$ \frac{\partial^2 L_t}{\partial \mathbf{x}_{t-1}^2} = \mathbf{A}_t^T \mathbf{R}_t^{-1} \mathbf{A} + \mathbf{\Sigma}_{t-1}^{-1} \tag{3.13} $$
令 $L_t$ 的一階導為 0 ，得 $L_t$ 關於 $\mathbf{x}_{t-1}$ 的極值點為
$$ \mathbf{x}_{t-1} = (\mathbf{A}_t^T \mathbf{R}_t^{-1} \mathbf{A} + \mathbf{\Sigma}_{t-1}^{-1} ) ^{-1}[ \mathbf{A}_t^T \mathbf{R}_t^{-1} (\mathbf{x}_t - \mathbf{B}_t\mathbf{u}_t) +\mathbf{\Sigma}_{t-1}^{-1}\boldsymbol{\mu}_{t-1} ] \tag{3.14} $$
觀察式(3.11)，可知 $\Delta$ 即為 $L_t$ 對 $\mathbf{x}_{t-1}$ 的極小值點， $\mathbf{\Psi}^{-1}$即為 $L_t$對$\mathbf{x}_{t-1}$的二階導。因此可取
$$ \Delta = \mathbf{\Psi} [ \mathbf{A}_t^T \mathbf{R}_t^{-1} (\mathbf{x}_t - \mathbf{B}_t\mathbf{u}_t) +\mathbf{\Sigma}_{t-1}^{-1}\boldsymbol{\mu}_{t-1} ] \tag{3.15} $$
$$ \mathbf{\Psi}^{-1} = \mathbf{A}_t^T \mathbf{R}_t^{-1} \mathbf{A} + \mathbf{\Sigma}_{t-1}^{-1} \tag{3.16} $$
因此，$L_t(\mathbf{x}_t) $ 為
$$ \begin{eqnarray*} L_t(\mathbf{x}_t) &=& L_t - \frac{1}{2}(\mathbf{x}_{t-1} - \Delta)^T \mathbf{\Psi} ^{-1}(\mathbf{x}_{t-1} -\Delta) \\ &=& +\frac{1}{2} (\mathbf{x}_t - \mathbf{B}_t\mathbf{u}_t)^T \mathbf{R}_t^{-1} (\mathbf{x}_t - \mathbf{B}_t\mathbf{u}_t) + \frac{1}{2} \boldsymbol{\mu}_{t-1}^T \mathbf{\Sigma}_{t-1}^{-1}\boldsymbol{\mu}_{t-1} - \frac{1}{2} [ \mathbf{A}_t^T \mathbf{R}_t^{-1} (\mathbf{x}_t - \mathbf{B}_t\mathbf{u}_t) \\ & & +\mathbf{\Sigma}_{t-1}^{-1}\boldsymbol{\mu}_{t-1} ]^T (\mathbf{A}_t^T \mathbf{R}_t^{-1} \mathbf{A} + \mathbf{\Sigma}_{t-1}^{-1})^{-1} [ \mathbf{A}_t^T \mathbf{R}_t^{-1} (\mathbf{x}_t - \mathbf{B}_t\mathbf{u}_t) +\mathbf{\Sigma}_{t-1}^{-1}\boldsymbol{\mu}_{t-1} ] \end{eqnarray*} \tag{3.17} $$
式(3.17)的詳細推導過程就不給出了。此時，式(3.10)可以進一步化簡為
$$ \begin{eqnarray*} \overline{\mathrm{bel}}(\mathbf{x}_t) &=& \eta \int \exp \left\{ -L_t(\mathbf{x}_t) - \frac{1}{2}(\mathbf{x}_{t-1} - \Delta)^T \mathbf{\Psi} ^{-1}(\mathbf{x}_{t-1} -\Delta) \right\} \mathrm{d}\mathbf{x}_{t-1} \\ &=& \eta \exp \{ -L_t(\mathbf{x}_t) \} \int \exp \left\{ - \frac{1}{2}(\mathbf{x}_{t-1} - \Delta)^T \mathbf{\Psi} ^{-1}(\mathbf{x}_{t-1} -\Delta) \right\} \mathrm{d}\mathbf{x}_{t-1} \end{eqnarray*} \tag{3.18} $$
根據“正態分布的的積分為1”，可知 $\int \exp \left\{ - \frac{1}{2}(\mathbf{x}_{t-1} - \Delta)^T \mathbf{\Psi} ^{-1}(\mathbf{x}_{t-1} -\Delta) \right\} \mathrm{d}\mathbf{x}_{t-1} = \det(2\pi \mathbf{\Psi})^{ \frac{1}{2}}$，則
$$ \overline{\mathrm{bel}}(\mathbf{x}_t) = \eta \exp \{ -L_t(\mathbf{x}_t) \} \tag{3.19} $$
式(3.19)將積分值 $\det(2\pi \mathbf{\Psi})^{ \frac{1}{2}}$ 歸入到歸一化因子 $\eta$ 中。這里，由於 $L_t(\mathbf{x}_t)$ 是 $mathbf{x}_t$ 的二次型，故 $ \overline{\mathrm{bel}}(\mathbf{x}_t) $ 服從正態分布，這也正好符合我們前面的假設。
根據式(3.19)，計算 $ \overline{\mathrm{bel}}(\mathbf{x}_t) $ 對 $\mathbf{x}_t$ 的一二階導數，即可求出其分布的均值 $\overline{\boldsymbol{\mu}}_{t}$ 和方差 $\overline{\mathbf{\Sigma}}_{t}$。即
$$ \begin{eqnarray*} \frac{\partial L_t(\mathbf{x}_t)}{\partial \mathbf{x}_{t}} &=& \mathbf{R}_t^{-1}(\mathbf{x}_t - \mathbf{B}_t \mathbf{u}_t) - \mathbf{R}_t^{-1}\mathbf{A}_t (\mathbf{A}_t^T \mathbf{R}_t^{-1} \mathbf{A} + \mathbf{\Sigma}_{t-1}^{-1} )^{-1}[ \mathbf{A}_t^T \mathbf{R}_t^{-1} (\mathbf{x}_t - \mathbf{B}_t\mathbf{u}_t) +\mathbf{\Sigma}_{t-1}^{-1}\boldsymbol{\mu}_{t-1} ] \\ &=& \left[ \mathbf{R}_t^{-1} - \mathbf{R}_t^{-1}\mathbf{A}_t (\mathbf{A}_t^T \mathbf{R}_t^{-1} \mathbf{A} + \mathbf{\Sigma}_{t-1}^{-1} )^{-1} \mathbf{A}_t^T \mathbf{R}_t^{-1} \right ] (\mathbf{x}_t - \mathbf{B}_t\mathbf{u}_t) - \\ & & \mathbf{R}_t^{-1}\mathbf{A}_t (\mathbf{A}_t^T \mathbf{R}_t^{-1} \mathbf{A} + \mathbf{\Sigma}_{t-1}^{-1} )^{-1} \mathbf{\Sigma}_{t-1}^{-1}\boldsymbol{\mu}_{t-1} \end{eqnarray*} \tag{3.20}$$
利用求逆引理，有
$$ \mathbf{R}_t^{-1} - \mathbf{R}_t^{-1}\mathbf{A}_t (\mathbf{A}_t^T \mathbf{R}_t^{-1} \mathbf{A} + \mathbf{\Sigma}_{t-1}^{-1} )^{-1} \mathbf{A}_t^T \mathbf{R}_t^{-1} = ( \mathbf{R}_t + \mathbf{A}_t \mathbf{\Sigma}_{t-1} \mathbf{A}_t^T )^{-1} \tag{3.21} $$
將(3.21)代入到(3.20)，可得
$$ \frac{\partial L_t(\mathbf{x}_t)}{\partial \mathbf{x}_{t}} = ( \mathbf{R}_t + \mathbf{A}_t \mathbf{\Sigma}_{t-1} \mathbf{A}_t^T )^{-1} (\mathbf{x}_t - \mathbf{B}_t\mathbf{u}_t) - \mathbf{R}_t^{-1}\mathbf{A}_t (\mathbf{A}_t^T \mathbf{R}_t^{-1} \mathbf{A} + \mathbf{\Sigma}_{t-1}^{-1} )^{-1} \mathbf{\Sigma}_{t-1}^{-1}\boldsymbol{\mu}_{t-1} \tag{3.22} $$
當一階導為零時得到 $L_t(\mathbf{x}_t) $ 的極小值點
$$ \begin{eqnarray*} \mathbf{x}_t &=& \mathbf{B}_t\mathbf{u}_t + \underbrace{ ( \mathbf{R}_t + \mathbf{A}_t \mathbf{\Sigma}_{t-1} \mathbf{A}_t^T )^{-1} \mathbf{R}_t^{-1}\mathbf{A}_t }_{ \mathbf{A}_t + \mathbf{A}_t \mathbf{\Sigma}_{t-1} \mathbf{A}_t^T \mathbf{R}_t^{-1}\mathbf{A}_t } \underbrace{ (\mathbf{A}_t^T \mathbf{R}_t^{-1} \mathbf{A} + \mathbf{\Sigma}_{t-1}^{-1} )^{-1} \mathbf{\Sigma}_{t-1}^{-1} }_{ (\mathbf{\Sigma}_{t-1}^{-1} \mathbf{A}_t^T \mathbf{R}_t^{-1} \mathbf{A} + \mathbf{I} )^{-1} } \boldsymbol{\mu}_{t-1} \\ &=& \mathbf{B}_t\mathbf{u}_t + \mathbf{A}_t ( \mathbf{I} + \mathbf{\Sigma}_{t-1}^{-1} \mathbf{A}_t^T \mathbf{R}_t^{-1} \mathbf{A} ) (\mathbf{\Sigma}_{t-1}^{-1} \mathbf{A}_t^T \mathbf{R}_t^{-1} \mathbf{A} + \mathbf{I} )^{-1} \boldsymbol{\mu}_{t-1} \\ &=& \mathbf{B}_t\mathbf{u}_t + \mathbf{A}_t \boldsymbol{\mu}_{t-1} \end{eqnarray*} \tag{3.23}$$
故 $ \overline{\mathrm{bel}}(\mathbf{x}_t) $ 的均值 $\overline{\boldsymbol{\mu}}_{t}$ 為 $ \mathbf{B}_t\mathbf{u}_t + \mathbf{A}_t \boldsymbol{\mu}_{t-1} $。這證明了程序3.1卡爾曼濾波算法中第2行的正確性。
根據式(3.22)，得到
$$ \frac{\partial ^2 L_t(\mathbf{x}_t)}{\partial \mathbf{x}_{t}^2} = ( \mathbf{R}_t + \mathbf{A}_t \mathbf{\Sigma}_{t-1} \mathbf{A}_t^T )^{-1} \tag{3.24} $$
其逆矩陣即為 $ \overline{\mathrm{bel}}(\mathbf{x}_t) $ 的協方差 $\overline{\mathbf{\Sigma}}_{t}$ 。這證明了程序3.1中第3行的正確性。
(2) 測量更新階段
程序3.1第4~6行為測量更新階段，其目標是利用測量值 $\mathbf{z}_t$ 和 $ \overline{\mathrm{bel}}(\mathbf{x}_t)$ 計算 $ \mathrm{bel}(\mathbf{x}_{t})$， 即求解 $\boldsymbol{\mu}_t$、$\mathbf{\Sigma}_t$。
根據式(3.2)，得
$$ \mathrm{bel}(\mathbf{x}_{t}) = \eta \underbrace{ p(\mathbf{z}_t | \mathbf{x}_t)}_{N(\mathbf{z}_t ; \mathbf{C}_t \mathbf{x}_t , \mathbf{Q}_t )} \underbrace{ \overline{\mathrm{bel}}(\mathbf{x}_t) }_{ \;\;\; N(\mathbf{x}_t; \overline{\boldsymbol{\mu}}_{t} , \overline{\mathbf{\Sigma}}_{t} )} \tag{3.25} $$
為方便分析，定義
$$ J_t = \frac{1}{2} (\mathbf{z}_t - \mathbf{C}_t \mathbf{x}_t )^T \mathbf{Q}_t ^{-1} (\mathbf{z}_t - \mathbf{C}_t \mathbf{x}_t ) + \frac{1}{2} ( \mathbf{x}_t - \overline{\boldsymbol{\mu}}_{t} )^T \overline{\mathbf{\Sigma}}\,\!_{t}^{-1} ( \mathbf{x}_t - \overline{\boldsymbol{\mu}}_{t} ) \tag{3.26} $$
則式(3.27)簡化為
$$ \mathrm{bel}(\mathbf{x}_{t}) = \eta \exp \{ - J_t\} \tag{3.27}$$
要求解 $\boldsymbol{\mu}_t$、$\mathbf{\Sigma}_t$，需計算 $J_t$ 關於 $ \mathbf{x}_{t}$ 的一二節導數：
$$ \frac{\partial J_t}{\partial \mathbf{x}_{t}} = - \mathbf{C}_t^T \mathbf{Q}_t ^{-1} (\mathbf{z}_t - \mathbf{C}_t \mathbf{x}_t ) + \overline{\mathbf{\Sigma}}\,\!_{t}^{-1} ( \mathbf{x}_t - \overline{\boldsymbol{\mu}}_{t} ) \tag{3.28} $$
$$ \frac{\partial^2 J_t}{\partial \mathbf{x}_{t}^2} = \mathbf{C}_t^T \mathbf{Q}_t ^{-1}\mathbf{C}_t + \overline{\mathbf{\Sigma}}\,\!_{t}^{-1} \tag{3.29} $$
根據，式(3.29)，可得 $ \mathrm{bel}(\mathbf{x}_{t})$的協方差為
$$ \mathbf{\Sigma}_t = (\mathbf{C}_t^T \mathbf{Q}_t ^{-1}\mathbf{C}_t + \overline{\mathbf{\Sigma}}\,_{t}^{-1} )^{-1} \tag{3.30}$$
當 $J_t$ 的一階導為零時得到極小值點為
$$ \begin{eqnarray*} \mathbf{x}_t &=& \overline{\boldsymbol{\mu}}_{t} + \underbrace{ ( \mathbf{C}_t^T \mathbf{Q}_t ^{-1} \mathbf{C}_t + \overline{\mathbf{\Sigma}}\,\!_{t}^{-1} )^{-1} }_{ \mathbf{\Sigma}_t } \mathbf{C}_t^T \mathbf{Q}_t ^{-1} ( \mathbf{z}_t - \mathbf{C}_t \overline{\boldsymbol{\mu}}_{t} ) \\ &=& \overline{\boldsymbol{\mu}}_{t} + \mathbf{\Sigma}_t \mathbf{C}_t^T \mathbf{Q}_t ^{-1} ( \mathbf{z}_t - \mathbf{C}_t \overline{\boldsymbol{\mu}}_{t} ) \end{eqnarray*} \tag{3.31}$$
上式的值即為 $\boldsymbol{\mu}_t$，現在定義卡爾曼增益為
$$ \mathbf{K}_t = \mathbf{\Sigma}_t \mathbf{C}_t^T \mathbf{Q}_t ^{-1} \tag{3.32} $$
則得到
$$ \boldsymbol{\mu}_t = \overline{\boldsymbol{\mu}}_{t} + \mathbf{K}_t ( \mathbf{z}_t - \mathbf{C}_t \overline{\boldsymbol{\mu}}_{t} ) \tag{3.33} $$
由於式(3.32)等式右邊包含 $\mathbf{\Sigma}_t$ ，故對其變形
$$ \begin{eqnarray*} \mathbf{K}_t &=& \mathbf{\Sigma}_t \mathbf{C}_t^T \mathbf{Q}_t ^{-1} \\ &=& \mathbf{\Sigma}_t \mathbf{C}_t^T \mathbf{Q}_t ^{-1} \underbrace{ ( \mathbf{C}_t \overline{\mathbf{\Sigma}}_{t} \mathbf{C}_t^T +\mathbf{Q}_t )( \mathbf{C}_t \overline{\mathbf{\Sigma}}_{t} \mathbf{C}_t^T +\mathbf{Q}_t)^{-1} }_{\mathbf{I}} \\ &=& \mathbf{\Sigma}_t ( \mathbf{C}_t^T \mathbf{Q}_t ^{-1} \mathbf{C}_t \overline{\mathbf{\Sigma}}_{t} \mathbf{C}_t^T + \mathbf{C}_t^T \mathbf{Q}_t ^{-1} \mathbf{Q}_t ) ( \mathbf{C}_t \overline{\mathbf{\Sigma}}_{t} \mathbf{C}_t^T +\mathbf{Q}_t)^{-1} \\ &=& \mathbf{\Sigma}_t ( \mathbf{C}_t^T \mathbf{Q}_t ^{-1} \mathbf{C}_t \overline{\mathbf{\Sigma}}_{t} \mathbf{C}_t^T + \overline{\mathbf{\Sigma}}\,_t^{-1} \overline{\mathbf{\Sigma}}_{t} \mathbf{C}_t^T ) ( \mathbf{C}_t \overline{\mathbf{\Sigma}}_{t} \mathbf{C}_t^T +\mathbf{Q}_t)^{-1} \\ &=& \mathbf{\Sigma}_t \underbrace{ ( \mathbf{C}_t^T \mathbf{Q}_t ^{-1} \mathbf{C}_t + \overline{\mathbf{\Sigma}}\,_t^{-1} ) }_{ \mathbf{\Sigma}_t^{-1} } \overline{\mathbf{\Sigma}}_{t} \mathbf{C}_t^T ( \mathbf{C}_t \overline{\mathbf{\Sigma}}_{t} \mathbf{C}_t^T +\mathbf{Q}_t)^{-1} \\ &=& \overline{\mathbf{\Sigma}}_{t} \mathbf{C}_t^T ( \mathbf{C}_t \overline{\mathbf{\Sigma}}_{t} \mathbf{C}_t^T +\mathbf{Q}_t)^{-1} \end{eqnarray*} \tag{3.34} $$
利用求逆引理，式(3.30)可以化簡為
$$ \begin{eqnarray*} \mathbf{\Sigma}_t &=& (\mathbf{C}_t^T \mathbf{Q}_t ^{-1}\mathbf{C}_t + \overline{\mathbf{\Sigma}}\,_{t}^{-1} )^{-1} \\ &=& \overline{\mathbf{\Sigma}}_t - \overline{\mathbf{\Sigma}}_t \mathbf{C}_t ^T ( \mathbf{Q}_t + \mathbf{C}_t \overline{\mathbf{\Sigma}}_t \mathbf{Q}_t^T )^{-1} \mathbf{C}_t \overline{\mathbf{\Sigma}}_t \\ &=& [\, \mathbf{I} - \underbrace{ \overline{\mathbf{\Sigma}}_t \mathbf{C}_t ^T ( \mathbf{Q}_t + \mathbf{C}_t \overline{\mathbf{\Sigma}}_t \mathbf{Q}_t^T )^{-1} }_{ \mathbf{K}_t } \mathbf{C}_t \,] \overline{\mathbf{\Sigma}}_t \\ &=& (\mathbf{I} - \mathbf{K}_t \mathbf{C}_t ) \overline{\mathbf{\Sigma}}_t \end{eqnarray*} \tag{3.35} $$
式(3.34)(3.33)(3.35)與程序3.1的3~5行相對應，即證畢。$\square$
2.擴展卡爾曼濾波（EKF）
擴展卡爾曼濾波是卡爾曼濾波在非線性情形下的近似推廣，具體而言就是 放寬了關於式(3.3)(3.5)的假設，這里假設狀態轉移概率和測量概率分別由非線性函數 $g(\cdot)$ 和 $h(\cdot)$ 控制：
$$ \mathbf{x}_t = g(\mathbf{u}_t, \mathbf{x}_{t-1}) + \boldsymbol{\varepsilon}_t \tag{3.36} $$
$$ \mathbf{z}_t = h(\mathbf{x}_t) + \boldsymbol{\delta}_t \tag{3.37} $$
如果直接按照前面的數學推導來計算，在計算式(3.12)時將會出現問題，因為需要確定 $\frac{ \partial g(\mathbf{u}_t, \mathbf{x}_{t-1}) }{ \partial \mathbf{x}_{t-1} }$ ，這也將導致式(3.13)的二階導變得十分復雜。
為了方便推導，EKF利用一階泰勒展開的方式將非線性函數線性化，即
$$ \begin{eqnarray*} g(\mathbf{u}_t, \mathbf{x}_{t-1}) & \approx & g(\mathbf{u}_t, \boldsymbol{\mu}_{t-1}) + g'( \mathbf{u}_t, \boldsymbol{\mu}_{t-1} ) ( \mathbf{x}_{t-1} - \boldsymbol{\mu}_{t-1} ) \\ & = & g(\mathbf{u}_t, \boldsymbol{\mu}_{t-1}) + \mathbf{G}_t ( \mathbf{x}_{t-1} - \boldsymbol{\mu}_{t-1} ) \end{eqnarray*} \tag{3.38} $$
$$ \begin{eqnarray*} h( \mathbf{x}_t ) & \approx & h( \overline{ \boldsymbol{\mu} }_t ) + h'( \overline{ \boldsymbol{\mu} }_t ) ( \mathbf{x}_t - \overline{ \boldsymbol{\mu} }_t ) \\ & = & h( \overline{ \boldsymbol{\mu} }_t ) + \mathbf{H}_t ( \mathbf{x}_t - \overline{ \boldsymbol{\mu} }_t ) \end{eqnarray*} \tag{3.39} $$
其中， $ \mathbf{G}_t = \frac{ \partial g(\mathbf{u}_t, \mathbf{x}_{t-1} \,) }{ \partial \mathbf{x}_{t-1} } |_{ \mathbf{x}_{t-1} \,=\, \boldsymbol{\mu}_{t-1} } $、$ \mathbf{H}_t = \frac{ \partial h(\mathbf{x}_{t}) }{ \partial \mathbf{x}_{t} } |_{ \mathbf{x}_{t} = \overline{ \boldsymbol{\mu} }_t } $
由此得到擴展卡爾曼濾波算法，如程序3.2。
| 程序3.2 擴展卡爾曼(EKF)濾波算法 | |
| 1: | Algorithm Extended_Kalman_filter($\boldsymbol{\mu}_{t-1}$, $\mathbf{\Sigma}_{t-1}$, $\mathbf{u}_t$, $\mathbf{z}_t$) |
| 2: | $ \overline{\boldsymbol{\mu}}_t = g(\mathbf{u}_t, \boldsymbol{\mu}_{t-1}) $ |
| 3: | $ \overline{\mathbf{\Sigma}}_t = \mathbf{G}_t \mathbf{\Sigma}_{t-1} \mathbf{G}_t^T + \mathbf{R}_t $ |
| 4: | $ \mathbf{K}_t = \overline{\mathbf{\Sigma}}_t \mathbf{H}_t^T (\mathbf{H}_t \overline{\mathbf{\Sigma}}_t \mathbf{H}_t^T + \mathbf{Q}_t)^{-1} $ |
| 5: | $ \boldsymbol{\mu}_t = \overline{\boldsymbol{\mu}}_t + \mathbf{K}_t (\mathbf{z}_t - h(\overline{\boldsymbol{\mu}}_t ) ) $ |
| 6: | $ \mathbf{\Sigma}_t = (\mathbf{I} - \mathbf{K}_t \mathbf{H}_t) \overline{\mathbf{\Sigma}}_t $ |
| 7: | return $\boldsymbol{\mu}_t$, $\mathbf{\Sigma}_t$ |
可以發現，程序3.2與程序3.1非常類似，不同的是 1）在求 $\overline{\boldsymbol{\mu}}_t$、$\boldsymbol{\mu}_t$ 是直接使用非線性函數$g(\cdot)$、$h(\cdot)$計算；2）其余部分只是做了簡單替換$ \mathbf{A}_t \rightarrow \mathbf{G}_t $、$ \mathbf{C}_t \rightarrow \mathbf{H}_t $。在按照式(3.38)(3.39)的近似后，擴展卡爾曼的數學推導與卡爾曼的推導相同。
3.無跡卡爾曼濾波（UKF）
無跡卡爾曼濾波的思想是挑選幾個關鍵點，通過加權統計來計算均值與方差，以實現濾波過程。
| 程序3.3 無跡卡爾曼(UKF)濾波算法 | |
| 1: | Algorithm Unscented_Kalman_filter($\boldsymbol{\mu}_{t-1}$, $\mathbf{\Sigma}_{t-1}$, $\mathbf{u}_t$, $\mathbf{z}_t$) |
| 2: | $ \boldsymbol{\chi }_{t-1} = ( \boldsymbol{\mu}_{t-1} \;\;\;\; \boldsymbol{\mu}_{t-1}+\gamma \sqrt{ \mathbf{\Sigma}_{t-1} } \;\;\;\; \boldsymbol{\mu}_{t-1} - \gamma \sqrt{ \mathbf{\Sigma}_{t-1} } ) $ |
| 3: | $ \bar{\boldsymbol{\chi }}_{t}^{\ast } = g( \mathbf{u}_t , \boldsymbol{\chi }_{t-1} ) $ |
| 4: | $ \bar{\boldsymbol{\mu}}_t = \sum_{i=0}^{2n} w_m^{[i]} \bar{\boldsymbol{\chi }}\,\!_{t}^{\ast [i] } $ |
| 5: | $ \overline{\mathbf{\Sigma}}_t = \sum_{i=0}^{2n} w_c^{[i]} ( \bar{\boldsymbol{\chi }}\,\!_{t}^{\ast [i] } - \bar{\boldsymbol{\mu}}_t ) ( \bar{\boldsymbol{\chi }}\,\!_{t}^{\ast [i] } - \bar{\boldsymbol{\mu}}_t ) ^T + \mathbf{R}_t $ |
| 6: | $ \overline{\boldsymbol{\chi }}_t = ( \bar{\boldsymbol{\mu}}_t \;\;\;\; \bar{\boldsymbol{\mu}}_t + \gamma \sqrt{ \bar{\mathbf{\Sigma}}_t } \;\;\;\; \bar{\boldsymbol{\mu}}_t - \gamma \sqrt{ \bar{\mathbf{\Sigma}}_t } ) $ |
| 7: | $ \overline{\mathbf{Z}}_t = h (\bar{\boldsymbol{\chi }}_t) $ |
| 8: | $ \hat{\mathbf{z}}_t = \sum_{i=0}^{2n} w_m^{[i]} \overline{\mathbf{Z}}\,\!_t^{[i]} $ |
| 9: | $ \mathbf{S}_t = \sum_{i=0}^{2n} w_c^{[i]} ( \overline{\mathbf{Z}}\,\!_t^{[i]} - \hat{\mathbf{z}}_t) ( \overline{\mathbf{Z}}\,\!_t^{[i]} - \hat{\mathbf{z}}_t)^T + \mathbf{Q}_t $ |
| 10: | $ \overline{\mathbf{\Sigma}}\,\!_t^{x,z} = \sum_{i=0}^{2n} w_c^{[i]} ( \bar{\boldsymbol{\chi }}\,\!_t^{[i]} - \bar{\boldsymbol{\mu}}_t ) ( \overline{\mathbf{Z}}\,\!_t^{[i]} - \hat{\mathbf{z}}_t)^T $ |
| 11: | $ \mathbf{K}_t = \overline{\mathbf{\Sigma}}\,\!_t^{x,z} \mathbf{S}_t^{-1} $ |
| 12: | $ \boldsymbol{\mu}_t = \bar{\boldsymbol{\mu}}_t + \mathbf{K}_t ( \mathbf{z}_t - \hat{\mathbf{z}}_t)$ |
| 13: | $ \mathbf{\Sigma}_t = \overline{\mathbf{\Sigma}}_t - \mathbf{K}_t \mathbf{S}_t \mathbf{K}_t^T $ |
| 14: | return $ \boldsymbol{\mu}_t $, $\mathbf{\Sigma}_t$ |