第三章：高斯滤波器

3.1 介绍

本章介绍一族重要的迭代状态估计器，它们叫做高斯滤波器Gaussian Filter。它们是有史以来连续空间下最早的可以轻松实现的贝叶斯滤波器。虽然它们有很多不足之处，但也依然是目前最流行的技术。

所有这些高斯类的技术都有一个共同的思想，就是用多变量正态分布表示置信度。我们已经在公式2.4中定义了多变量正态分布，这里重写如下： $$ \begin{equation}\label{f1} p(x) = \det(2\pi \Sigma)^{-\frac{1}{2}} \exp\left\{-\frac{1}{2}(\boldsymbol{x-μ})^T\Sigma^{-1}(\boldsymbol{x-μ})\right\} \end{equation} $$ 这个关于变量$x$的概率分布有两个参数：均值$μ$和协方差矩阵$\Sigma$。均值$μ$是一个与$x$相同维度的向量。协方差矩阵是一个半正定的对称矩阵，其参数数量是$x$维度的平方。

用高斯函数描述后验概率有很多重要的结论。最重要的是，高斯函数是单峰分布的，它只有一个极大值。这样的后验概率是机器人学中很多跟踪问题的特点，以很小的不确定性聚焦在真实状态周围。在全局优化问题中，往往存在多个不同的假设，而每一种假设在后验概率中都有一个模态，因而高斯型的后验概率就很难适用了。

高斯函数用均值和方差进行参数化的方法称为矩参数化moments parameterization。均值和方差分别是概率分布的一阶和二阶中心矩。正态分布的其它矩都为零。在本章中，我们将要讨论另一种参数化方法，称为正则参数化carnonical parameterization或者自然表达法natural parameterization。矩和正则这两种参数化方法，在功能上是等价的，存在一种双射关系可以从一种形式转换到另一种形式。但是，这两种形式的滤波器具有不同的计算特点，在后续的内容中，我们会看到贝叶斯滤波器中的两个更新运算在一种形式下容易计算在另一种形势下就变得很复杂。

本章主要介绍两种基本的高斯滤波器算法：

在3.2节描述了卡尔曼滤波器(Kalman Filter)，它用矩参数化的形式实现了贝叶斯滤波器，适用于线性的状态方程和测量方程。
在3.3节中将卡尔曼滤波器扩展到非线性的领域中，称为扩展卡尔曼滤波器(extented Kalman filter)。
在3.4节中介绍另一种非线性的卡尔曼滤波器，称为无迹卡尔曼滤波器(unscented Kalman filter)。
在3.5节中描述了信息滤波器(information filter)，卡尔曼滤波器的对偶形式，采用的是正则参数化方法描述的高斯函数。

3.2 卡尔曼滤波器

3.2.1 线性高斯系统

卡尔曼滤波器(Kalman filter, KF)可能是研究最充分的贝叶斯滤波器实现。它是由Swerling(1958)和Kalman(1960)提出的，是一种对线性系统的滤波和预测的技术。卡尔曼滤波器实现了连续状态的置信度计算。它不能用于离散的或者混合状态空间中。

卡尔曼滤波器用矩参数的形式表示置信度：$t$时刻的置信度用均值$μ_t$和协方差$\Sigma_t$来表示。如果后验概率满足以下三个特性，以及贝叶斯滤波器的马尔可夫假设，那么它就服从高斯分布。

状态转移概率$p(x_t | u_t, x_{t-1})$中的状态变换必须是状态和控制量的线性函数与高斯噪声的和，可以写成下式(译者按:在以后的译文中我们称之为状态方程)： $$ \begin{equation}\label{f2} x_t = A_t x_{t-1} + B_t u_t + \varepsilon_t \end{equation} $$ 其中，$x_t, x_{t-1}$是状态，$u_t$是$t$时刻的控制量。在我们的符号体系中，它们都是列向量，具有如下的形式： $$ \begin{equation}\label{f3} x_t = \begin{bmatrix} x_{1,t} \\ x_{2,t} \\ \vdots \\ x_{n,t} \end{bmatrix} u_t = \begin{bmatrix} u_{1,t} \\ u_{2,t} \\ \vdots \\ u_{m,t} \end{bmatrix} \end{equation} $$ $A_t$是一个$n \times n$的方阵，其中$n$是状态向量$x_t$的维度。$B_t$是一个$n \times m$的矩阵，其中$m$是控制向量$u_t$的维度。随机变量$\varepsilon_t$是一个服从高斯分布的随机向量，用来对状态转移过程中的随机因素建模，与状态向量的维度相同。 $\varepsilon_t$的均值为0，协方差矩阵被表示为$R_t$。公式($\ref{f2}$)中描述的状态转移概率称为线性高斯函数linear Gaussian。理论上，在公式($\ref{f2}$)中可以有一个常数项，因为它在本文中没有什么作用所以就省略了。

将公式($\ref{f2}$)代入公式($\ref{f1}$)就得到了状态转移概率密度函数$p(x_t | u_t, x_{t-1})$。转移之后的状态均值为$A_tx_{t-1} + B_tu_t$，方差为$R_t$： $$ \begin{equation}\label{f4} p(x_t | u_t, x_{t-1}) = \det(2\pi R_t)^{-\frac{1}{2}} \exp \left\{ -\frac{1}{2} (x_t - A_tx_{t-1} - B_tu_t)^T R_t^{-1} (x_t - A_tx_{t-1} - B_tu_t)\right\} \end{equation} $$
测量概率密度$p(z_t | x_t)$也必须是线性的，并加上了高斯噪声，如下式所示(译者按:在以后的译文中我们称之为测量方程): $$ \begin{equation}\label{f5} z_t = C_t x_t + \delta_t \end{equation} $$ 其中$C_t$是一个$k \times n$的矩阵，$k$是测量向量$z_t$的维度。向量$\delta_t$描述了测量噪声，其概率密度是一个均值为0协方差矩阵为$Q_t$的多变量高斯函数。测量概率密度可以表示为如下的多变量正态分布： $$ \begin{equation}\label{f6} p(z_t | x_t) = \det(2\pi Q_t)^{-\frac{1}{2}} \exp \left\{ -\frac{1}{2} (z_t - C_tx_t)^T Q_t^{-1} (z_t - C_tx_t)\right\} \end{equation} $$
初始置信度$bel(x_0)$必须是正态分布。令$μ_0$为置信度的均值，$\Sigma_0$为协方差矩阵，有： $$ \begin{equation}\label{f7} bel(x_0) = p(x_0) = \det(2\pi \Sigma_0)^{-\frac{1}{2}} \exp \left\{ -\frac{1}{2} (x_0 - μ_0)^T \Sigma_0^{-1} (x_0 - μ_0)\right\} \end{equation} $$

这三个假设是在任意时刻$t$后验置信度$bel(x_t)$服从高斯分布的必要条件。我们将在3.2.4节给出数学证明。

3.2.2 卡尔曼滤波器

算法3.1中列出了卡尔曼滤波器的伪代码。卡尔曼滤波器用均值$μ_t$和协方差矩阵$\Sigma_t$描述$t$时刻的置信度$bel(x_t)$。它的输入是$t-1$时刻的置信度，用$μ_{t-1}$和$\Sigma_{t-1}$来描述。为了更新矩参数，我们还需要知道控制量$u_t$和测量值$z_t$。卡尔曼滤波器的输出就是用$μ_t$和$\Sigma_t$表示的$t$时刻置信度。

算法3.1. 针对线性高斯状态方程和测量方程的卡尔曼滤波器

在其第2和第3行中，计算$\barμ$和$\bar\Sigma$得到融合测量数据$z_t$之前的预测置信度$\overline{bel}(x_t)$。这个置信度的计算考虑了控制量$u_t$。将均值$μ_{t-1}$代入公式($\ref{f2}$)更新均值。更新协方差矩阵时，需要考虑到状态方程中的矩阵$A_t$，因为协方差矩阵是一个二次型矩阵，所以$A_t$和它的转置分别乘到了协方差矩阵$\Sigma_{t-1}$的左右两侧。

在第4到第6行，滤波器根据测量值$z_t$和预测置信度$\overline{bel}(x_t)$计算校正置信度$bel(x_t)$。其中，第4行所计算的变量$K_t$就是所谓的卡尔曼增益Kalman gain，它描述了测量值中蕴含的新状态的信息量。在第5行中，对比测量值$z_t$与测量方程所得到的估计值，并将其偏差乘以卡尔曼增益$K_t$，来校正状态的均值向量。其核心概念就是更新量innovation——实际测量值$z_t$与估计测量值$C_t\bar{μ}_t$之间的偏差。最后，在第6行中用卡尔曼增益$K_t$更新了协方差矩阵$\Sigma_t$。

卡尔曼滤波器具有很高的计算效率。现如今最快的求$d \times d$矩阵逆的算法的时间复杂度也是$O(d^{2.4})$。所以计算卡尔曼增益的下界都接近$O(k^{2.4})$，其中$k$是测量向量$z_t$的维度。这个接近立方的复杂度主要来自于第四行中的矩阵求逆的运算。第六行的矩阵乘法运算(矩阵$K_tC_t$可能是稀疏的)最低的时间复杂度也是$O(n^2)$，其中$n$是状态空间的维度。在很多的应用中，比如说后续章节中要讨论的机器人建图的应用中，测量空间的维度要远低于状态空间的维度，更新过程的复杂度是$O(n^2)$。

3.2.3 例子

图3.2中给出了一个简单的一维定位场景下的卡尔曼滤波算法的例子。假设机器人沿着水平的轴线运动。机器人的位置的服从正态分布如图(a)所示。机器人通过传感器测量它的位置(比如说，一个GPS系统)，然后返回了一个测量值，服从高斯分布如图(b)所示。通过算法中的第4行到第6行的过程，机器人融合了初始位置概率密度和测量概率密度，如图(c)所示。这个置信度的均值介于两个原始的高斯分布的均值之间，而且它的方差要小于两个原始的方差。这种不确定度比原来的高斯分布小的事实看起来是有悖直觉的, 其实它是卡尔曼滤波器收集信息的一般特征。


(a)	(b)	(c)

(d)	(e)	(f)
图 3.2. 卡尔曼滤波器示例。(a)初始置信度, (b)具有一定不确定性的测量值(粗线), (c)用卡尔曼滤波器算法集成了测量值的置信度,(d)向右移动后的置信度(引入了不确定性),(e)一次具有不确定性的测量,(f)最终的置信度

接着，假设机器人向右运动。因为转移到的下一个状态是随机的，所以其不确定度增加了。图(d)中的实现显示卡尔曼滤波器算法中的第2和3行计算后的高斯分布。这个高斯分布的峰随着机器人的移动而向右移动了，而且方差也增大了。接着如图(e)中所示的那样，机器人进行了第二次测量，最终计算出了如图(f)所示的结果。

正如这个例子所展示的那样，卡尔曼滤波器一直在测量更新操作(measurement update step)和预测操作(prediction step)之间切换。测量更新操作就是算法3.1中的第5-7行，将传感器数据融合到当前的置信度中，降低了机器人的不确定性。第2到4行则是预测操作，将控制量融合到置信度中，增加了不确定性。

3.2.4 卡尔曼滤波器的数学推导

本节推导卡尔曼滤波器算法。在初次阅读的时候可以先跳过本部分。

好的，就先跳过。回头再来。

3.3 扩展卡尔曼滤波器

3.3.1 为什么线性化?

状态方程和测量方程的线性的，这两个假设对卡尔曼滤波器正确性至关重要。高斯分布的随机变量经过线性变换后仍然服从高斯分布，这一特性在推导卡尔曼滤波器算法过程中起到了重要作用。卡尔曼滤波器的效率得益于对其参数的这种闭合形式的运算。

本章和下一章中，我们将列举一些由一维高斯随机变量变换得到的不同概率密度函数表达形式的特性。图3.3(a)中举例说明了随机变量的线性变换。图中右下角的曲线表示随机变量$X \sim \mathcal{N}(x; μ, \delta^2)$的概率密度函数。$X$通过如右上角所示的线性函数$y = ax + b$变换，得到随机变量$Y$。 $Y$服从均值为$aμ + b$方差为$a^2\delta^2$的高斯分布，如左上角的阴影区域所示。令$X = x_{t-1}, Y = x_t$，可以发现这个例子与公式($\ref{f2}$)很像，只是没有噪声项。


(a)	(b)
图 3.3. 高斯随机变量的线性变换与非线性变换。(a)线性变换，(b)非线性变换。两幅图中右下角描绘了原始的随机变量$X$的概率密度。右上角的曲线表示状态变换方程，其中均值的变换过程用点画线表示。左上角的曲线则是变换后的随机变量$Y$的分布。

不幸的是，实际的状态方程和测量方程很少有线性的。比如说，对于一个以不变的线速度和角速度沿着圆形的轨道运动的机器人，就不能用线性的状态方程来描述。此外，高斯函数使得置信度变成了一个单峰分布，这样的滤波器我们称之为朴素卡尔曼滤波器plain Kalman Filter，它在很多简单的机器人问题中都不适用。

扩展卡尔曼滤波器extended Kalman filter, EKF放松了朴素卡尔曼滤波器的线性假设。它用函数$g$和$h$来分别表示状态方程和测量方程，这两个函数可以是非线性的。 $$ \begin{equation}\label{f48} x_t = g(u_t, x_{t-1}) + \varepsilon_t \end{equation} $$ $$ \begin{equation}\label{f49} z_t = h(x_t) + \delta_t \end{equation} $$ 这个模型是对公式($\ref{f2},\ref{f5}$)所描述的卡尔曼滤波器线性高斯模型的推广。函数$g$代替了公式($\ref{f2}$)中的矩阵$A_t$和$B_t$， $h$代替了公式($\ref{f5}$)中的矩阵$C_t$。由于函数$g$和$h$的非线性特点，最终的置信度将不再是高斯函数了。实际上，通过非线性函数$g$和$h$更新置信度通常是不可能的，而且贝叶斯滤波器的变换也不再具有闭合的特性了。

图3.3(b)中示例了高斯随机变量的非线性变换过程。其中右下角和右上角的曲线分别描述了随机变量$X$的概率密度函数和非线性函数$g$。变换后的随机变量$Y = g(X)$的密度函数有左上角的阴影区域表示。因为这一密度函数不能够通过闭合的形式计算得到，图中的曲线是对$p(x)$进行了500,000次采样，代入函数$g$后画出的直方图(译者按:这就是蒙特卡洛估计)。可以看出，因为$g$的非线性特性，$Y$不再服从高斯分布了。

扩展开尔曼滤波器用一个高斯函数来近似表示真实的置信度。图3.3(b)中左上角的曲线显示了用于近似$Y$的密度函数的高斯函数。相应的，EKF用一个均值$μ_t$和协方差矩阵$\Sigma_t$来表示$t$时刻的置信度$bel(x_t)$。因此，EKF继承了卡尔曼滤波器的基本置信度表达形式，所不同的是这一置信度只是一个近似。EKF这样做的目的是将计算确切的后验概率转换为高效的估计均值和方差的计算。但是，由于这种统计方法不能以闭合的形式计算， EKF不得不做出近似操作。

3.3.1 基于泰勒展开的线性化

如图3.4所示，EKF的核心思想就是线性化linearization。如图中右上角中虚线所示，这一线性化近似过程通过非线性函数$g$在高斯分布的均值上的正切来实现。左上角中的虚线表示由这一线性化近似的函数映射得到的高斯分布。左上角的实现表示蒙特卡洛近似的均值和方差，这两个高斯分布之间的偏差就是对函数$g$线性近似的误差。

图 3.4. EKF的线性化示例EKF中并不是用非线性函数$g$对高斯分布进行变换，而是使用了$g$的一个线性化近似。这个近似是函数$g$在原始高斯分布的均值上的正切，变换后的高斯函数如左上角的虚线所示。线性化带来的近似误差由线性化高斯分布(虚线)与高精度蒙特卡洛估计得到的高斯分布(实线)之间的差异来表示。

线性化的核心优势就是计算效率。这里蒙特卡洛估计出的高斯函数是对500,000个样本经过函数$g$映射之后计算的均值和协方差。EKF所使用的线性化方法，只需要确定线性近似函数，然后以闭合形式映射高斯分布就可以了。实际上，一旦线性化了$g$，EKF的置信度更新机制就与卡尔曼滤波器的一样了。这一线性化方法也适用于测量方程$h$，也就是用$h$的正切来近似。这样就可以保持后验置信度服从高斯分布。

有很多种线性化的方法。EKF使用的是一阶泰勒展开(Taylor expansion)的方法，通过函数$g$的值和斜率构造了一个线性近似。斜率可以通过偏导获得： $$ \begin{equation}\label{f50} g'(u_t, x_{t-1}) := \frac{\partial{g(u_t, x_{t-1})}}{\partial{x_{t-1}}} \end{equation} $$ 显然，$g$的函数值以及它的斜率值都取决于自变量。比较合理的自变量就是选择当前最可能的状态作为泰勒展开的基点。对于高斯函数，最可能的状态就是其均值$μ_{t-1}$。也就是说，用$g$在$(μ_{t-1},u_t)$上展开，用其在该点的函数值和梯度值近似: $$ \begin{align}\label{f51} g(u_t, x_{t-1}) & \approx g(u_t, μ_{t-1}) + g'(u_t, μ_{t-1})(x_{t-1} - μ_{t-1}) \\ & = g(u_t, μ_{t-1}) + G_t(x_{t-1} - μ_{t-1}) \\ G_t & := g'(u_t, μ_{t-1}) \end{align} $$ 状态转移后的概率密度函数可以由下面的高斯函数近似表示: $$ \begin{equation}\label{f52} p(x_t | u_t, x_{t-1}) \approx \det(2\pi R_t)^{-\frac{1}{2}} \exp\left\{-\frac{1}{2} [x_t - g(u_t, μ_{t-1}) - G_t(x_{t-1} - μ_{t-1})]^T R_t^{-1} [x_t - g(u_t, μ_{t-1}) - G_t(x_{t-1} - μ_{t-1})] \right\} \end{equation} $$ 需要注意$G_t$是一个$n \times n$的矩阵，其中$n$是状态的维度。它通常被称为是雅可比矩阵(Jacobian)，其值取决于$u_t$和$μ_{t-1}$，因此在不同的时刻它们的值都不同。

对于测量方程$h$，EKF进行相同的线性化操作，在$\bar{μ}_t$处泰勒展开： $$ \begin{align}\label{f53} h(x_t) & \approx h(\bar{μ}_t) + h'(\bar{u}_t)(x_t - \bar{μ}_{t}) \\ & = h(\bar{μ}_t) + H_t(x_t - \bar{μ}_{t}) \\ H_t & := h'(\bar{u}_t) \end{align} $$ 其中$h'(x_t) = \frac{\partial{h(x_t)}}{\partial{x_t}}$。写成高斯函数的形式，有： $$ \begin{equation}\label{f54} p(z_t | x_t) = \det(2\pi Q_t)^{-\frac{1}{2}} \exp\left\{-\frac{1}{2} [z_t - h(\bar{μ}_t) - H_t(x_t - \bar{μ}_{t})]^T Q_t^{-1} [z_t - h(\bar{μ}_t) - H_t(x_t - \bar{μ}_{t})] \right\} \end{equation} $$

3.3.3 EKF算法

算法3.3中描述了EKF算法。该算法在很大程度上与算法3.1中描述的卡尔曼滤波器算法类似。

算法3.3. 扩展卡尔曼滤波器

EKF与卡尔曼滤波器的主要不同点总结如下，EKF用非线性的生成规则替代了卡尔曼滤波器的线性预测函数。此外，EKF用雅可比矩阵$G_t$代替了状态方程中的矩阵$A_t, B_t$，用$H_t$替代了测量方程中的矩阵$C_t$。我们将在第7章给出一个详细的扩展卡尔曼滤波的例子。

	卡尔曼滤波器	EKF
状态方程(第2行)	$A_t μ_{t-1} + B_tu_t$	$g(u_t, μ_{t-1})$
测量方程(第5行)	$C_t \bar{μ}_t$	$h(\bar{μ}_t)$

3.3.4 EKF的数学推导

暂略。

3.3.5 一些实际的考虑

EKF已经称为机器人学中状态估计的最流行工具。它的优势在于形式简单、计算效率高。与卡尔曼滤波器一样，更新过程的时间复杂度为$O(k^{2.4}+n^2)$，其中$k$是测量向量$z_t$的维度，$n$是状态向量$x_t$的维度。而其它的算法，比如说后面要讨论的粒子滤波器，可能需要$n$的指数级复杂度。

EKF的计算效率得益于多变量高斯分布表达的置信度。高斯函数是一种单峰的分布，可以看作是一次考虑了不确定度的猜测。在很多实际的问题中，高斯函数是一个鲁棒的估计器。本书的后续章节中将讨论1000维甚至更高维状态空间下的卡尔曼滤波器的应用。 EKF已经在很多状态估计问题(that violate the underlying assumptions)中得到了很成功的应用。

使用线性泰勒展开对状态方程和测量方程做出近似的操作是EKF算法的一个重要限制。基本上所有的机器人学问题中的状态方程和测量方程都是非线性的。 EKF的这种近似方法的可行性依赖于两个主要因素。首先，它依赖与需要近似的函数的非线性度和不确定度。图3.5中的两幅图形象的说明了不确定度对线性化质量的影响。图中的两个高斯随机变量经过相同的非线性函数映射，而且这两个高斯分布具有相同的均值，只是图(a)中分布比图(b)的具有较大的协方差。左上角的阴影部分描述了用蒙特卡洛方法估计的映射后的随机变量的分布，比较胖的高斯分布的变形程度要比瘦的那个大很多。图中的实线是通过蒙特卡洛估计的高斯分布，虚线是线性化后得到的高斯分布。对比两者可以看出，较高的不确定度往往导致对随机变量的估计具有更低的准确度。


(a)	(b)
图 3.5. 不确定度对线性化质量的影响。两幅图的右下角显示了高斯分布，它们具有相同的均值，但是协方差不同。右上角为非线性函数，在两幅图中具有相同的配置。如左上角的图像所示，具有更大的不确定度的高斯函数所产生的随机变量的密度函数更不规则，图中实线是蒙特卡洛估计的高斯函数，虚线是EKF得到的高斯函数。

第二个影响线性高斯近似质量的因素就是如图3.6所示的函数$g$的局部非线性度。图中两个高斯函数具有相同的方差，并通过相同的非线性函数映射。在图(a)中的高斯均值所对应的$g$的局部非线性度比图(b)中所示的严重。对比线性化后映射高斯函数和蒙特卡洛估计的高斯函数，可以得出更高非线性度将导致更差的线性化效果的结论。


(a)	(b)
图 3.6. 局部非线性度对线性化质量的影响。两幅图的右下角显示了高斯分布，它们具有相同的协方差，但是均值不同。右上角为非线性函数，在两幅图中具有相同的配置。如左上角的图像所示，具有更大非线性度的映射后的高斯函数畸变更大，图中实线是蒙特卡洛估计的高斯函数，虚线是EKF得到的高斯函数。

有时，我们可能想要做出多目标的假设。比如说，一个机器人可能对于自身的位置有两个不同的假设，此时就需要用到多模态(multi-modal representation)形式的后验置信度了。但是，这里描述的EKF是不能够表示这样的多峰置信度的。通常会对EKF进行扩展，用混合高斯函数来表示后验概率，其形式如下： $$ \begin{equation}\label{f65} bel(x) = \frac{1}{\Sigma_l \psi_{t,l}} \sum_l {\psi_{t,l} \det(2\pi \Sigma_{t,l})^{-\frac{1}{2}} \exp \left\{ -\frac{1}{2}(x_t - μ_{t,l})^T\Sigma_{t,l}^{-1}(x_t - μ_{t,l}) \right\} } \end{equation} $$ 其中$\psi_{t,l} ≥ 0$称为混合参数，它们是各个混合要素的权重，通过对以相应高斯分布为条件的观测值的似然估计。这种使用这种混合高斯模型的EKF称为多假设扩展卡尔曼滤波器(multi-hypothesis (extended) Kalman filters, MHEKF)。

如果非线性函数在估计的均值附近近似线性，那么EKF就可以取得较好的近似效果，进而得到更准确的后验置信度。此外，机器人越不确定系统的状态，其高斯置信度就越胖，对状态方程和测量方程的线性化效果就越差。因此，在实际应用EKF的时候应当保证状态的不确定度在一个很小的范围内。

3.4 无迹卡尔曼滤波器

EKF中使用的泰勒级数展开只是一种高斯分布变换的线性化方法。研究发现另外两种方法具有更好的效果。一种方法称为矩匹配moments matching，通过保留后验分布中真实的均值和方差来进行线性化，使用这种方法的滤波器称为密度假设滤波器assumed density filter, ADF。无迹卡尔曼滤波器unscented Kalman filter, UKF使用的是另一种线性化方法，通过加权随机线性回归过程进行随机线性化。我们现在讨论UKF算法，不给出数学推导了。读者可以通过阅读参考文献中的资料了解更多细节。

3.4.1 通过无迹变换的线性化

图3.7中示例了UKF使用的线性化方法，称之为无迹变换unscented transform。这种方法并不对函数$g$进行泰勒技术展开， UKF从高斯分布中抽取了所谓的$\sigma$点，并将之通过$g$映射。一般情况下，这些$\sigma$点都在均值附近沿着协方差主轴对称的选取，每个维度上选择两个点。对于均值为$μ$协方差为$\Sigma$的$n$维高斯分布，根据如下法则选取$2n+1$个$\sigma$点$\mathcal{X}^{[i]}$: $$ \begin{equation}\label{f66} \begin{array}{lcl} \mathcal{X}^{[0]} & = & μ & \\ \mathcal{X}^{[i]} & = & μ + \left( \sqrt{(n+\lambda) \Sigma} \right)_i & for\ i = 1, \cdots, n \\ \mathcal{X}^{[i]} & = & μ - \left( \sqrt{(n+\lambda) \Sigma} \right)_{i-n} & for\ i = n+1, \cdots, 2n \\ \end{array} \end{equation} $$ 其中$\lambda = \alpha^2(n+k)-n$，$\alpha,k$分别是两个标量参数用于确定$\sigma$点的分布距离均值的距离。每一个$\sigma$点$\mathcal{X}^{[i]}$都有两个权值， $w_m^{[i]}$用于计算均值，$w_c^{[i]}$用于计算高斯函数的协方差: $$ \begin{equation}\label{f67} \begin{array}{lcl} & & w_m^{[0]} & = & \frac{\lambda}{n + \lambda} \\ & & w_c^{[0]} & = & \frac{\lambda}{n + \lambda} + (1 - \alpha^2 + \beta) \\ w_m^{[i]} & = & w_c^{[i]} & = & \frac{1}{2(n + \lambda)}\ for\ i = 1, \cdots, 2n \\ \end{array} \end{equation} $$ 参数$\beta$可以记录高斯形式的目标分布的高阶信息。如果目标分布就是一个高斯分布，那么$\beta = 2$就是最优选择。

图 3.7. UKF使用的线性化方法示例。滤波器首先从$n$维的高斯分布中抽取$2n+1$个加权$\sigma$点，此处$n=1$。这些$\sigma$点将通过非线性函数$g$映射。如右上角中的小圆圈所示，从映射后的$\sigma$点抽象出线性化的高斯分布。与EKF一样，这种线性化方法也存在近似误差，通过将左上角虚线所示的线性化高斯分布与实线所示的蒙特卡洛估计的高斯分布对比，以它们的差异来量化。

然后采样的$\sigma$点经过函数$g$的映射，就获得了畸变之后的高斯分布探针: $$ \begin{equation}\label{f68} \mathcal{Y}^{[i]} = g(\mathcal{X}^{[i]}) \end{equation} $$ 接着，根据下式从探针中提取出参数$\begin{pmatrix} μ' & \Sigma' \end{pmatrix}$: $$ \begin{equation}\label{f69} \begin{array}{lcl} μ' & = & \sum_{i=0}^{2n} w_m^{[i]} \mathcal{Y}^{[i]} \\ \Sigma' & = & \sum_{i=0}^{2n} w_m^{[i]} (\mathcal{Y}^{[i]} - μ')(\mathcal{Y}^{[i]} - μ')^T \\ \end{array} \end{equation} $$


(a)	(b)
图 3.8. 不确定度对UKF线性化方法的影响。为了方便对比，这里也给出了相同设置下EKF的线性化效果。对比虚线和实线标记的高斯函数，可以看到无迹变换下两者更接近，所以带来的近似误差更小。

图3.8描述了不确定度对UKF线性化方法的影响。为了方便对比，图中还给出了使用EKF泰勒级数展开形式的线性化结果。图3.9中显示了函数$g$的局部非线性度对于UKF和EKF的线性化方法的影响对比。我们可以看到无迹变换比一阶泰勒级数展开的线性化结果更准确。实际上，无迹变换的准确度与二阶泰勒展开的准确度相当，而EKF的线性化方法只是一阶的。需要注意的是，EKF和UKF都可以扩展成更高阶的形式。


(a)	(b)
图 3.9. 原始高斯分布的均值对UKF线性化方法的影响。为了方便对比，这里也给出了相同设置下EKF的线性化效果。对比虚线和实线标记的高斯函数，可以看到无迹变换下两者更接近，所以带来的近似误差更小。

3.4.2 UKF算法

如算法3.4所示，UKF算法使用无迹变换进行线性化。其输入和输出与EKF算法一样。第2行根据公式($\ref{f66}$)计算了上一时刻的$\sigma$点，其中$\gamma = \sqrt{n + \lambda}$。第3行中通过无噪声状态预测计算$\bar{\mathcal{X}}_t^*$。然后在第4和第5行中根据这些$\sigma$点预测均值和协方差。第5行中将$R_t$加到了$\sigma$点的协方差上，从而对预测噪声的不确定度建模(相当于算法3.3中的第3行)。这里假设预测噪声$R_t$是一个附加项。在第7章中，我们将给出一个能够更精确地状态方程和测量方程的噪声项。

在第6行中，从预测的高斯分布中提取了新的$\sigma$点集合。此时的$\sigma$点集$\bar{\mathcal{X}}_t$中蕴含了预测阶段后的所有不确定度。在第7行中，对于每一个$\sigma$点都计算一个预测观察值。这些观察$\sigma$点$\bar{\mathcal{Z}}_t$将用于计算预测观察值$\hat{z}_t$以及它的不确定度$S_t$。矩阵$Q_t$是附加测量噪声的协方差矩阵。可以发现，$S_t$的含义与算法3.3中EKF算法第4行描述的不确定度$H_t \bar{\Sigma}_t H_t^T + Q_t$是一样的。第10行是要确定状态和观测值之间的互协方差，它们将在第11行中用于计算卡尔曼增益$K_t$。这个互协方差$\bar{\Sigma}_t^{x,z}$对应于EKF算法中第4行的$\bar{\Sigma}_tH_t^T$。很容易理解，第12和13行描述的估计更新操作与EKF算法的实现形式是等价的。

和EKF一样，UKF的更新操作和预测操作的复杂度是不对称的。实际上EKF要比UKF快一点。但是UKF和EKF的计算复杂度的量级是一样的，UKF只是多了一个常数项。此外，得益于无迹变换出色的线性化方法，UKF具有不俗的表现。对于纯粹的线性系统，UKF的估计效果与卡尔曼滤波器的效果一样。对于非线性系统UKF与EKF的效果相当甚至由于EKF，因为它在非线性度和后验状态不确定度的负面影响做出了改进。在很多实际应用中，EKF和UKF的差异可以忽略不计。

UKF的另一个优势就是它不需要计算雅可比矩阵。雅可比矩阵在一些情况下很难计算。因而UKF也称为免求导滤波器derivative-free filter。

无迹变换与粒子滤波器所采用的基于采样的表示方法类似。但是它们的关键不同点在于无迹变换中的$\sigma$点的选取是确定性的，而粒子滤波器则完全是随机选择样本。这一点具有很重要的意义。如果置信度近似为高斯分布，那么UKF远比粒子滤波器更有效率。另一方面，如果置信度的分布与高斯分布相差甚远，那么UKF就具有很大的局限性，还不如随机选择样本。

3.5 信息滤波器

卡尔曼滤波器的对偶实现就是信息滤波器(Information Filter, IF)。与KF和它的非线性版本EKF、UKF类似，IF也是使用高斯函数表达置信度的。因此，标准的信息滤波器具有和卡尔曼滤波器一样的假设。IF和KF的关键不同之处在于高斯置信度的表示形式上。 KF系列的算法用矩参数的形式(moments, 均值和方差)表示高斯函数，IF则用正则参数形式表示高斯函数。正则参数形式的高斯函数由一个信息矩阵和一个信息向量构成。不同的表达形式对应的更新方程也不相同，以至于一种形式下的复杂的计算在另一种形式下就会变得简单，反过来也是一样。因此我们说正则参数形式的IF与矩参数形式的KF是互为对偶(dual to each other)的实现。

3.5.1 正则参数化

用正则参数用一个矩阵$\Omega$和一个向量$\xi$表示多变量高斯函数。其中矩阵$\Omega$是协方差矩阵的逆: $$ \begin{equation}\label{f70} \Omega = \Sigma^{-1} \end{equation} $$ $\Omega$被称为是信息矩阵information matrix，有时也叫做精度矩阵precision matrix。向量$\xi$则被称为是信息向量$information vector$，定义为: $$ \begin{equation}\label{f71} \xi = \Sigma^{-1} μ \end{equation} $$ 显然$\Omega$和$\xi$完全是高斯函数的表示参数。通过公式($\ref{f70}$)和公式($\ref{f71}$)的逆运算，可以很容易计算出高斯函数的均值和方差: $$ \begin{equation}\label{f72} \Sigma = \Omega^{-1} μ \end{equation} $$ $$ \begin{equation}\label{f73} μ = \Omega^{-1} \xi \end{equation} $$ 我们可以从公式($\ref{f1}$)所示的指数形式的多变量高斯函数中导出正则参数，公式($\ref{f1}$)重写如下： $$ \begin{equation}\label{f74} p(x) = \det(2\pi \Sigma)^{-\frac{1}{2}} \exp\left\{-\frac{1}{2}(\boldsymbol{x-μ})^T\Sigma^{-1}(\boldsymbol{x-μ})\right\} \end{equation} $$ 直接对上式展开，得到： $$ \begin{equation}\label{f75} \begin{array}{lcl} p(x) & = & \det(2\pi \Sigma)^{-\frac{1}{2}} \exp\left\{-\frac{1}{2}x^T\Sigma^{-1}x + x^T\Sigma^{-1}μ - \frac{1}{2}μ^T\Sigma^{-1}μ\right\} \\ & = & \begin{matrix} \underbrace{det(2\pi\Sigma)^{-\frac{1}{2}} \exp\left\{-\frac{1}{2}μ^T\Sigma^{-1}μ\right\}} & \exp\left\{-\frac{1}{2}x^T\Sigma^{-1}x + x^T\Sigma^{-1}μ \right\} \\ const. & \end{matrix} \end{array} \end{equation} $$ 其中用"const"标记的部分并不依赖于变量$x$。因此，我们将之写入归一化算子$\eta$： $$ \begin{equation}\label{f76} p(x) = \eta \exp\left\{-\frac{1}{2}x^T\Sigma^{-1}x + x^T\Sigma^{-1}μ \right\} \end{equation} $$ 代入正则参数$\Omega$和$\xi$的定义有： $$ \begin{equation}\label{f77} p(x) = \eta \exp\left\{-\frac{1}{2}x^T\Omega x + x^T\xi \right\} \end{equation} $$ 很多时候，正则参数比矩参数更有用。尤其是对高斯函数求负对数的时候，它就是一个以$\Omega$和$\xi$为参数关于变量$x$的二次函数： $$ \begin{equation}\label{f78} -\log {p(x)} = const. + \frac{1}{2}x^T \Omega x - x^T \xi \end{equation} $$ 这里的$const.$就是一个常数。注意我们不能用符号$\eta$来表示这个常数，概率的负对数是不能归一化的。对的概率分布$p(x)$求负对数得到的是$x$的二次函数，其中$\Omega$和$\eta$分别是其二次项和一次项的参数。实际上，对于高斯函数矩阵$\Omega$必须是半正定的，因此$-\log{p(x)}$就是一个均值为$μ = \Omega^{-1}\xi$的二次距离函数。通过对公式($\ref{f78}$)求偏导，并令之等于0，可以很容易证明： $$ \begin{equation}\label{f79} \frac{\partial{\left[-\log{p(x)}\right]}}{\partial x} = 0 \Leftrightarrow \Omega x - \xi = 0 \Leftrightarrow x = \Omega^{-1} \xi \end{equation} $$ 其中矩阵$\Omega$确定了距离函数在变量$x$的不同维度上增长的斜率。由矩阵$\Omega$加权的二次距离称为马氏距离Mahalanobis distance。

3.5.2 信息滤波器算法

算法3.5是信息滤波器的伪代码。它的输入是$t-1$时刻由正交参数$\xi_{t-1}$和$\Omega_{t-1}$表示的高斯函数。与所有的贝叶斯滤波器一样，它还需要控制量$u_t$和测量值$z_t$作为输入参数。其输出是更新之后的高斯参数$\xi_t$和$\Omega_t$。

算法3.5. 信息滤波器算法

更新过程涉及到了3.2节中定义的矩阵$A_t, B_t, C_t, R_t, Q_t$。信息滤波器中状态方程和测量方程也如公式($\ref{f2}$,$\ref{f5}$)中定义的那样是线性的： $$ \begin{equation}\label{f80} x_t = A_t x_{t-1} + B_t u_t + \varepsilon_t \end{equation} $$ $$ \begin{equation}\label{f81} z_t = C_t x_t + \delta_t \end{equation} $$ $R_t$和$Q_t$分别是零均值噪声变量$\varepsilon_t$和$\delta_t$的协方差矩阵。

与卡尔曼滤波器类似，信息滤波器的更新过程也是由预测操作和测量更新操作两个步骤完成的。算法3.5的第2、3行描述了预测操作的实现，用参数$\bar{\xi}_t$和$\bar{\Omega}_t$记录融合了控制量$u_t$后$x_t$的高斯分布。接着在第4、5行中实现根据测量值$z_t$更新置信度的测量更新操作。

这两个操作的计算复杂度差异很大，尤其是当状态空间的维度很大的时候，这种差异很明显。预测操作中涉及到两个$n \times n$矩阵求逆的过程，其中$n$是状态空间的维度。矩阵的逆运算基本上需要$O(n^{2.4}$)的时间复杂度。在卡尔曼滤波器中，这一过程是增量形式的，需要$O(n^2)$的时长，如果控制量只改变部分变量，或者这些变量的变换是相对独立的，那么它就需要的时间更少。而在信息滤波器中测量更新过程是增量形式的，时间复杂度为$O(n^2)$，而且如果测量值只含有部分状态变量的信息，它还可以更快。在卡尔曼滤波器中测量更新过程却是一个更复杂过程，需要对矩阵求逆，时间复杂度为$O(n^{2.4}$。这就是我们称卡尔曼滤波器和信息滤波器是对偶实现的原因。

3.5.3 信息滤波器的数学推导

暂略。

3.5.4 扩展信息滤波器

以与EKF极为相似的线性化方法，扩展信息滤波器extended information filter, EIF，对IF做出了非线性扩展。算法3.6中给出了扩展信息滤波器的伪代码。在第2到4行实现了预测操作，第5到7行实现了测量更新。

算法3.6. 扩展信息滤波器EIF算法

这些更新过程与线性信息滤波器十分类似，只是用函数$g$和$h$(以及它们的雅可比矩阵$G_t$和$H_t$)替代了线性模型中的矩阵$A_t, B_t, C_t$。和EKF一样，$g$和$h$分别描述了状态方程和测量方程中的非线性关系，如公式($\ref{f48},\ref{f49}$)定义的那样： $$ \begin{equation}\label{f93} \begin{cases} x_t & = & g(u_t, x_{t-1}) + \varepsilon_t \\ z_t & = & h(x_t) + \delta_t \end{cases} \end{equation} $$ 不幸的是，$g$和$h$都需要一个状态作为输入。这需要从正交参数中恢复状态估计量$μ$。如算法3.6第2行所示，其可以很容易通过$\Omega_{t-1}$和$\xi_{t-1}$计算得到状态$μ_{t-1}$。第5行中计算估计状态$\bar{μ}$。恢复状态估计则做法似乎与使用正交参数表示正交分布的思想相违背。我们将在介绍使用扩展信息滤波器进行机器人建图时重新回来讨论这一问题。

3.5.5 扩展信息滤波器的数学推导

暂略。

3.5.6 一些实际的考虑

应用于实际的机器人问题的时候，信息滤波器具有几个卡尔曼滤波器所不具备的优点。比如说，信息滤波器描述全局不确定度的形式就简单很多，只需要令$\Omega = 0$即可。而使用矩参数的时候，这样的全局不确定度表示意味着协方差矩阵为无穷大。当传感器的测量值所携带的信息只是状态变量的一个子集时，这种问题尤其突出，而这一现象在机器人学中十分常见。为了处理这种情况，EKFs必须做出一些特殊的处理才行。在本书后续章节中讨论的很多问题上，信息滤波器都比卡尔曼滤波器在数值上更稳定。

在本书的后续章节中，我们可以看到信息滤波器和它的一些扩展可以让机器人不用立即将信息融合进概率密度中就可以实现信息融合。在涉及到几百个甚至几百万个变量的复杂的估计问题中，这就是一个很大的优势。对于如此庞大的问题，因为新信息需要通过一个庞大系统的变量来传播，所以卡尔曼滤波器会带来一些计算问题。通过适当的修改，信息滤波器可以把新信息局部地融合到系统中，从而规避了这一问题。但是这并不是本章中所讨论的朴素信息滤波器的特性，我们将在第12章中介绍这样的滤波器。

信息滤波器相比于卡尔曼滤波器的另一个优势就是，它天生就支持多机器人问题(multi-robot problems)。多机器人问题通常涉及到对分布式的传感器数据进行融合的问题。这种融合通常通过贝叶斯公式实现。当使用对数形式来表达，贝叶斯法则就是一个加法运算，而正则参数形式的信息滤波器就是用对数形式表示的概率模型。因此，信息滤波器通过对多个机器人的信息求和就可以将分散的信息融合起来。此外，加法运算是支持交换律的，因此信息滤波器可以以任何顺序，在任意时间进行信息融合，是一种极其松散的实现方式。当然用矩参数的形式也可以做到类似的效果，但是表达相同的信息，它需要更大的花销。除了这一优势，信息滤波器在多机器人系统中的应用潜力还很大，在第12章中我们将深入讨论多机器人问题。

尽管信息滤波器有这些优势，但也会被一些限制所掩盖。EIF的一个主要劣势就是在更新过程中还需要恢复状态估计，这一操作需要对信息矩阵求逆。接着为了执行预测操作还需要再进行一次矩阵逆运算。在很多机器人问题中，EKF不需要这么多的矩阵逆运算。对于高维度的状态空间，信息滤波器的计算效率要比卡尔曼滤波器低。这也是目前EKF比EIF流行的一个原因。

我们将在本书的后续章节中看到，当信息矩阵具有一定结构就不存在这些限制。在很多机器人问题中，状态变量的交互是局部的，因此信息矩阵可能是稀疏的。这种稀疏特性不意味着协方差矩阵也是稀疏的。

信息滤波器可以看作是图，在信息矩阵中除了对角线，其他位置上的非零元素，对应的行索引和列索引所指示的状态变量是有关联的。稀疏的信息矩阵对应着稀疏的图，这种图通常被称为高斯马尔可夫随机随机场(Gaussian Markov random field)。有很多算法能够高效的再这样的场中完成基本的更新和估计操作，它们被称为循环置信度传播算法"loopy belief propagation"。在本书中讨论的建图问题的信息矩阵就是稀疏的，信息滤波器及其扩展形式的算法就比卡尔曼滤波器和非稀疏信息滤波器要更有效。

3.6 总结

在本部分中介绍了用多变量高斯函数表示后验概率的高效的贝叶斯滤波器算法。我们注意到：

高斯函数有矩参数和正则参数两种不同的形式。矩参数形式用均值(一阶中心矩)和协方差矩阵(二阶中心矩)来描述高斯函数。正则参数形式则用信息矩阵和信息向量来描述。这两种形式是互补的，相互之间可以通过矩阵逆运算来获得。
这两种形式都可以用来实现贝叶斯滤波器。用矩参数的形式时，就是卡尔曼滤波器。其互补的实现就是信息滤波器，用正则参数形式实现。根据控制量进行预测对于卡尔曼滤波器算法而言计算简单，但是测量更新过程相对复杂。信息滤波器则是相反，测量更新过程简单，预测过程复杂。
这两种滤波器都建立在三个假设上。首先，初始置信度必须是高斯分布的。其次，状态方程必须是线性的并附加了一个独立的高斯噪声。最后，测量方程也需要是线性的并附加了一个独立的高斯噪声。满足这三个假设的系统称为线性高斯系统。
这两种形式都可以扩展到非线性系统上。本章中描述的方法计算非线性方程的正切，进行线性化，将滤波器扩展到非线性系统上。计算正切的方法称为泰勒展开，在特定点计算函数值及其一阶偏导值，得到雅可比矩阵(Jacobian)。这样的滤波器就是扩展滤波器。
无迹卡尔曼滤波器使用的是另一种称为无迹变换的线性化技术。它在一些选定点插入探针，并通过这些探针的映射结果对状态方程和测量方程做出线性近似。这种滤波器的实现不需要计算雅可比矩阵，因此也被称为免求导滤波器。对于线性系统UKF和EKF是等价的，但在非线性系统中UKF具有更好的效果，其计算复杂度与EKF一致。
泰勒级数扩展的准确性依赖于两个因素：系统的非线性程度，和后验概率的“肥胖”程度。如果对系统状态估计具有相对高的确定度，协方差就小，扩展的滤波器效果就越好。越大的不确定性，线性化的误差就越大。
高斯滤波器的一个主要优势就是计算效率高：更新过程具有状态空间维度的多项式复杂度。这不是后面章节中介绍的技术能够达到的。高斯滤波器的主要的劣势就是高斯分布是单峰的。
多假设卡尔曼滤波器用混合高斯模型描述后验概率。所谓的混合高斯模型，就是对多个高斯函数加权求和而已。这种卡尔曼滤波器扩展形式的更新机制需要对单个高斯分布进行生成、合并、剪枝操作。多假设卡尔曼滤波器更适合处理离散关联数据的问题，这也是一个机器人学中常见的问题。
在多变量高斯分布的世界里，卡尔曼滤波器和信息滤波器有互补的优势和缺点。但是卡尔曼滤波器和它的非线性扩展形式要比信息滤波器更流行。

本章选择的材料都是目前机器人学中最流行的技术。还有很多高斯滤波器的变形和扩展来应付各种不同的限制和缺点。本书中很多算法都是基于高斯滤波器的。很多机器人问题都需要以后验概率中的稀疏结构和因式分解形式对这些算法进行扩展。

3.7 参考文献

卡尔曼滤波器是由Swerling(1958)和Kalman(1960)提出的。一般情况下，卡尔曼滤波器都是用作最小二乘假设下的最优估计器，很少用来计算后验概率，虽然两种观点在合适的假设下是一致的。有很多出色的介绍卡尔曼滤波器和信息滤波器的教材，包括Maybeck(1990)和Jazwinsky(1970)的著作。同期还有Bar-Shalom和Fortmann(1988),Bar-Shalom和Li(1998)研究用于数据关联的卡尔曼滤波器。

inversion lemma可以在Golub和Loan(1986)中找到。根据Coppersmith和Winograd (1990)可以找到时间复杂度为$O(n^{2.376})$的矩阵求逆算法，这是目前较新的对$O(n^3)$复杂度的消元算法的改进。这些工作起源于Strassen’s (1969)的开创性论文，所提出的复杂度为$O(n^{2.807})$的算法。 Cover和Thomas (1991)提供了一个针对离散系统的信息论综述。无迹卡尔曼滤波器是由Julier和Uhlmann (1997)提出的。van der Merwe (2004)在各种不同的状态估计问题中对比了UKF和EKF。 Minka (2001)提供了一个混合高斯的矩匹配方法。

3.8 练习

暂略。

	卡尔曼滤波器	EKF
状态方程(第2行)	\(A_t μ_{t-1} + B_tu_t\)	\(g(u_t, μ_{t-1})\)
测量方程(第5行)	\(C_t \bar{μ}_t\)	\(h(\bar{μ}_t)\)