这一章将介绍卡尔曼滤波、扩展卡尔曼滤波以及无迹卡尔曼滤波，并从贝叶斯滤波的角度来进行分析并完成数学推导。如果您对贝叶斯滤波不了解，可以查阅相关书籍或阅读 【概率机器人 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$