从贝叶斯滤波到无迹卡尔曼滤波

0 引言

在此前的文章《从贝叶斯滤波到扩展卡尔曼滤波》中，我们讲述了扩展卡尔曼滤波通过一阶泰勒级数展开将非线性高斯系统的状态转移函数 $f(x)$ 和（或）观测函数 $h(x)$ 线性化，然后采用标准卡尔曼滤波框架实现状态量的滤波过程。扩展卡尔曼滤波存在两方面的明显缺点：

• 一阶泰勒级数展开忽略了二阶及以上的高阶项，因此精度一般（通常称为一阶精度），对于高度非线性问题效果较差；
• 雅可比矩阵的计算较为繁琐，容易出错。

为解决强非线性条件下的状态估计问题，1995 年，S. J. Julier 和 J. K. Uhlmann 等人提出了无迹卡尔曼滤波（Unscented Kalman Filter，UKF）算法，并由 E. A. Wan 和 R. Vander Merwe 等人进一步完善。

无迹卡尔曼滤波基于无迹变换（Unscented Transform，UT），无迹变换研究的是如何通过确定的采样点捕获经非线性变换的高斯随机变量的后验分布的问题。通过无迹变换得到相应的统计特性后，再结合标准卡尔曼滤波框架，便得到无迹卡尔曼滤波。标准无迹卡尔曼滤波的计算量与扩展卡尔曼滤波相当，但滤波精度要优于扩展卡尔曼滤波。

1 无迹变换

1.1 什么是无迹变换

无迹变换的核心理念：

近似概率分布比近似任意的非线性函数或变换要相对容易。

无迹变换要解决的问题是：已知某随机变量（多维情形下是随机向量）的概率分布（均值和方差），求其经过某非线性函数 $g(\cdot )$ 变换后的概率分布。基于上述思想，无迹变换的主要步骤为：

(1) 根据某种规则对随机变量的概率分布进行确定性采样，并为采样点分配权重（均值权重和方差权重），采样点我们通常称之为 sigma 点；

(2) 将每一个 sigma 点进行非线性变换，得到新的 sigma 点；

(3) 对非线性变换后的新的 sigma 点进行加权求和，分别计算加权均值和加权方差，用加权均值和加权方差近似表征随机变量经非线性变换后的概率分布。

如下图所示（《概率机器人》第 3.4 节 P49）：

无迹卡尔曼滤波中使用的确定性采样方法是 sigma 点采样方法的一种具体实现，中心差分卡尔曼滤波（Central Difference Kalman Filter，CDKF）使用了 sigma 点采样方法的另一种具体实现，这类滤波算法我们统称为 sigma 点卡尔曼滤波（Sigma-Point Kalman Filter，SPKF）算法。

按照历史发展脉络来讲，无迹变换主要包括两种形式：一般形式的无迹变换和比例无迹变换（Scaled Unscented Transform，SUT）。两种无迹变换的区别主要体现在采样规则和权重计算上，下面将分别进行阐述。

1.2 一般形式的无迹变换

假设存在 $n$ 维随机向量 $X$，其服从均值 ${\mu }_x$ 和协方差 ${\Sigma }_x$ 的正态分布：

$$X\sim \mathcal{N}({\mu }_x,{\Sigma }_x)$$

将 $X$ 经过非线性函数 $g(\cdot )$ 进行变换，得到随机向量 $Y$，我们使用一般形式的无迹变换估计 $Y$ 的概率分布。一般形式的无迹变换最早由 Julier 等人提出，其主要操作流程如下所述：

步骤一：参数选择与权重计算

一般形式的无迹变换只涉及一个外部引入参数 $\kappa$，各 sigma 点（$2n+1$ 个）的权重分配如下：

$$W^{(i)}=\{\begin{array}{ll}\frac{\kappa }{n+\kappa }& i=0\\ \frac{1}{2(n+\kappa )}& i=1,\cdot \cdot \cdot ,2n\end{array}$$

其中，$\kappa \in \mathbb{R}$，表征了 sigma 点相对均值的散布程度，$\kappa$ 越大，非均值处的 sigma 点距离均值越远（参考步骤二），且所占权重越小，而均值处 sigma 点所占权重则相对越大。对于高斯问题，$n+\kappa =3$ 是一个比较好的选择，对于非高斯问题（是的，无迹变换也适用于非高斯问题），$\kappa$ 应该选择其它更恰当的值。

步骤二：确定性采样

通常情况下，siama 点位于均值处及对称分布于主轴的协方差处（每维两个）。按照如下方法采样得到 $2n+1$ 个 sigma 点，构成 $n\times (2n+1)$ 的点集矩阵 $\mathcal{X}$：

$${\mathcal{X}}^{(i)}=\{\begin{array}{ll}{\mu }_x& i=0\\ {\mu }_x+(\sqrt{(n+\kappa ){\Sigma }_x}{)}^{(i-1)}& i=1,\cdot \cdot \cdot ,n\\ {\mu }_x-(\sqrt{(n+\kappa ){\Sigma }_x}{)}^{(i-n-1)}& i=n+1,\cdot \cdot \cdot ,2n\end{array}$$

其中，$(\sqrt{(n+\kappa ){\Sigma }_x}{)}^{(i-1)}$ 表示矩阵 $(n+\kappa ){\Sigma }_x$ 作 Cholesky 分解后下三角矩阵的第 $i-1$ 列，$(\sqrt{(n+\kappa ){\Sigma }_x}{)}^{(i-n-1)}$ 同理。

步骤三：sigma 点非线性变换

将每个 sigma 点（即 $\mathcal{X}$ 的每一列）进行 $g(\cdot )$ 的非线性变换，得到变换后的新的点集矩阵 $\mathcal{Y}$：

$${\mathcal{Y}}^{(i)}=g({\mathcal{X}}^{(i)})i=0,\cdot \cdot \cdot ,2n$$

步骤四：加权计算近似均值与近似协方差

$${\mu }_y=\sum _{i=0}^{i=2n}{W}^{(i)}{\mathcal{Y}}^{(i)}$$

$${\Sigma }_y=\sum _{i=0}^{i=2n}{W}^{(i)}[{\mathcal{Y}}^{(i)}-{\mu }_y][{\mathcal{Y}}^{(i)}-{\mu }_y{]}^T$$

1.3 比例无迹变换

从 1.2 节的内容中我们可以发现，当参数 $\kappa <0$ 时，权重 $W^{(0)}=\frac{\kappa }{n+\kappa }<0$，加权算得的近似协方差可能存在非半正定的情况。为应对该问题，Julier 等人后来提出无迹变换的改进形式——比例无迹变换，并由 Merwe 等人对其进行了简化。比例无迹变换与一般形式的无迹变换的主要区别体现在参数选取与 sigma 点权重计算上，其主要操作流程如下所述：

步骤一：参数选择与权重计算

比例无迹变换引入了四个外部参数：$\alpha$、$\beta$、$\kappa$ 和 $\lambda$，各 sigma 点（$2n+1$ 个）的权重分配如下：

$$W_m^{(i)}=\{\begin{array}{ll}\frac{\lambda }{n+\lambda }& i=0\\ \frac{1}{2(n+\lambda )}& i=1,\cdot \cdot \cdot ,2n\end{array}$$

$$W_c^{(i)}=\{\begin{array}{ll}\frac{\lambda }{n+\lambda }+1-{\alpha }^2+\beta & i=0\\ \frac{1}{2(n+\lambda )}& i=1,\cdot \cdot \cdot ,2n\end{array}$$

其中，$W_m^{(i)}$ 表示计算近似均值时 sigma 点的权重，$W_c^{(i)}$ 表示计算近似协方差时 sigma 点的权重。参数 $\lambda$ 满足：

$$\lambda ={\alpha }^2(n+\kappa )-n$$

参数 $\alpha$ 和 $\kappa$ 为确定 sigma 点分布在均值多远的范围内的比例参数。$\alpha$ 满足 $10^{-4}\le \alpha \le 1$，为避免强非线性系统中的非局部效应问题，$\alpha$ 通常取一个较小值；$\kappa$ 满足 $\kappa \ge 0$，通常取 $\kappa =3-n$ 或 $\kappa =0$。

下图呈现了当 $\alpha$ 取值分别为 $0.3$ 和 $1$ 时，sigma 点的分布情况，从图中可以发现，$\alpha$ 取值越大，非均值处的 sigma 点距离均值越远。

参数 $\beta$ 用于引入随机变量概率分布的高阶矩信息，如果分布是精确的高斯分布，则 $\beta =2$ 是最优选择。

步骤二：确定性采样

通常情况下，siama 点位于均值处及对称分布于主轴的协方差处（每维两个）。按照如下方法采样得到 $2n+1$ 个 sigma 点，构成 $n\times (2n+1)$ 的点集矩阵 $\mathcal{X}$：

$${\mathcal{X}}^{(i)}=\{\begin{array}{ll}{\mu }_x& i=0\\ {\mu }_x+(\sqrt{(n+\lambda ){\Sigma }_x}{)}^{(i-1)}& i=1,\cdot \cdot \cdot ,n\\ {\mu }_x-(\sqrt{(n+\lambda ){\Sigma }_x}{)}^{(i-n-1)}& i=n+1,\cdot \cdot \cdot ,2n\end{array}$$

其中，$(\sqrt{(n+\lambda ){\Sigma }_x}{)}^{(i-1)}$ 表示矩阵 $(n+\lambda ){\Sigma }_x$ 作 Cholesky 分解后下三角矩阵的第 $i-1$ 列，$(\sqrt{(n+\lambda ){\Sigma }_x}{)}^{(i-n-1)}$ 同理。

步骤三：sigma 点非线性变换

将每个 sigma 点（即 $\mathcal{X}$ 的每一列）进行 $g(\cdot )$ 的非线性变换，得到变换后的新的点集矩阵 $\mathcal{Y}$：

$${\mathcal{Y}}^{(i)}=g({\mathcal{X}}^{(i)})i=0,\cdot \cdot \cdot ,2n$$

步骤四：加权计算近似均值与近似协方差

$${\mu }_y=\sum _{i=0}^{i=2n}{W}_m^{(i)}{\mathcal{Y}}^{(i)}$$

$${\Sigma }_y=\sum _{i=0}^{i=2n}{W}_c^{(i)}[{\mathcal{Y}}^{(i)}-{\mu }_y][{\mathcal{Y}}^{(i)}-{\mu }_y{]}^T$$

2 无迹卡尔曼滤波的假设

无迹卡尔曼滤波与扩展卡尔曼滤波具有相同的前提假设。

2.1 与卡尔曼滤波相同的假设

(1) 假设一：状态量服从正态分布
$$X\sim \mathcal{N}({\mu }_X,{\sigma }_X^2)$$

(2) 假设二：观测量服从正态分布
$$Y\sim \mathcal{N}({\mu }_Y,{\sigma }_Y^2)$$

(3) 假设三：过程噪声服从均值为 0 的正态分布
$$Q\sim \mathcal{N}(0,{\sigma }_Q^2)$$

(4) 假设四：观测噪声服从均值为 0 的正态分布
$$R\sim \mathcal{N}(0,{\sigma }_R^2)$$

2.2 与卡尔曼滤波不同的假设

(5) 假设五：状态转移函数和（或）观测函数为非线性函数

在卡尔曼滤波的前提假设中，认为状态方程中的状态转移函数 $f(x)$ 以及观测方程中的观测函数 $h(x)$ 均为线性函数。基于这种线性假设，存在常数或常矩阵 $F$，使得 $f(x)$ 可以写成卡尔曼滤波中的线性形式，存在常数或常矩阵 $H$，使得 $h(x)$ 也可以写成卡尔曼滤波中的线性形式。

不同于标准卡尔曼滤波，无迹卡尔曼滤波处理的是非线性高斯系统，假设系统的状态转移函数和（或）观测函数为非线性函数。

3 无迹卡尔曼滤波算法框架

根据噪声对系统的状态转移过程和观测过程的影响是线性可加的还是非线性不可加的，无迹卡尔曼滤波算法有两种形式：可加性噪声条件下的无迹卡尔曼滤波算法和非可加性噪声条件下的无迹卡尔曼滤波算法。

3.1 可加性噪声条件下的无迹卡尔曼滤波

在此前的文章《从概率到贝叶斯滤波》中，我们曾经这样描述系统的状态方程和观测方程：

$$\{\begin{array}{ll}{X}_k=f(X_{k-1})+Q_k& \Rightarrow 状态方程\\ Y_k=h(X_k)+R_k& \Rightarrow 观测方程\end{array}$$

从上式中我们可以发现，过程噪声 $Q_k$ 和观测噪声 $R_k$ 是以线性可加项的形式存在于系统状态方程和观测方程中的，这种形式的表述基于这样的前提假设：过程噪声 $Q_k$ 对系统状态转移过程的影响是线性的，观测噪声 $R_k$ 对系统观测过程的影响也是线性的。很明显，此种情况下，$Q_k$ 与 $X_k$ 是同维的，$R_k$ 与 $Y_k$ 是同维的。

由此引申出可加性噪声条件下的无迹卡尔曼滤波算法，《概率机器人》第 3.4 节 P53 对算法流程有着很好的描述。假设有如下已知条件：

• 高斯的状态量随机向量 $X$，$n_X$ 维，记 $n_X=n$
• $k-1$ 时刻 $X$ 的后验均值与后验协方差：${\mu }_{X_{k-1}}^{+}$，${\Sigma }_{X_{k-1}}^{+}$
• $k$ 时刻的外部控制量输入 $u_k$
• 高斯的过程噪声随机向量：$Q\sim \mathcal{N}(0,{\Sigma }_Q)$
• 状态转移函数：$f(X_{k-1},u_k)$
• 高斯的观测量随机向量 $Z$，$n$