【概率机器人】3.1 卡尔曼滤波、扩展卡尔曼滤波和无迹卡尔曼滤波

最新推荐文章于 2024-09-21 07:18:21 发布

dayanbu5856

最新推荐文章于 2024-09-21 07:18:21 发布

阅读量521

点赞数

原文链接：http://www.cnblogs.com/zengzhen94/p/8627070.html

版权

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