高斯滤波——KF、EKF 与 UKF

高斯滤波——KF、EKF 与 UKF

写在前面

本文是本人学习卡尔曼滤波及其拓展算法时整理的笔记。由于本人的知识体系尚不完整，对许多基本且经典的理论没有充分的了解，所以本文也会对其中一些理论进行解释。

高斯线性系统

卡尔曼滤波就是在线性高斯假设的基础上建立起来的。用高斯函数来表示后验具有很好的影响，因为它是单峰的，有单一的极大值。高斯滤波中的参数的均值和方差称为矩参数，分别为一阶矩和二阶矩。

所以首先让我们来看看后验满足高斯分布需要满足什么条件：

1. 满足贝叶斯滤波的马尔可夫假设
2. 状态转移概率 $P(x_t|u_t,x_{t-1})$ 必须是带有随机高斯噪声的参数的线性函数。公式表示为：

$$x_t=A_t{x}_{t-1}+B_t{u}_t+{\epsilon }_t$$
其中 $x_t$ 为状态向量，$u_t$ 为t时刻的控制向量；$A_t$ 为 $n\ast n$ 的方阵，$n$ 为状态向量 $x$ 的维数；$B_t$ 为 $n\ast m$ 的矩阵，$m$ 为控制向量 $u$ 的维数；$\epsilon$ 为高斯随机向量，由状态转移的不确定性引入，其维数与状态向量维数相同，均值为 0，协方差用 $R_t$ 表示。

3. 测量概率 $P(Z_t|x_t)$ 也与带有高斯噪声的自变量呈线性关系。公式表示为：

$$z_t=C_t{x}_t+{\delta }_t$$

$C_t$ 为 $k\ast n$ 的矩阵，$k$ 为测量向量 $Z$ 的维数，向量 $\delta$ 为测量噪声，分布为均值是 0，方差为 $Q$ 的多变量高斯分布

4. 初始置信度 $bel(x_0)$ 必须是正态分布的。多维联合分布的表达式为：

b e l ( x 0 ) = p ( x 0 ) = d e t ( 2 π Σ 0 ) − 1 2 e x p { − 1 2 ( x 0 − μ 0 ) T Σ 0 − 1 ( x 0 − μ 0 ) } bel(x_0)=p(x_0)=det(2\pi\Sigma_0)^{-\frac{1}{2}}exp\{-\frac{1}{2}(x_0-\mu_0)^T\Sigma_0^{-1}(x_0-\mu_0)\} bel(x0​)=p(x0​)=det(2πΣ0​)−21​exp{ −21​(x0​−μ0​)TΣ0−1​(x0​−μ0​)}

下面来对第一个条件作出一点解释。

贝叶斯滤波与马尔可夫假设

贝叶斯滤波

大多数计算置信度的通用算法都是由贝叶斯滤波算法给出的。贝叶斯滤波算法根据上一时刻置信度、测量和控制数据计算置信度分布。

算法伪代码表示为：

其中 $p(x_t|u_t,x_{t-1})$ 为上一时刻的置信度 ，$bel(x_{t-1})$ 为此时刻的状态转移概率，$\eta$ 为归一化变量，$p(z_t|x_t)$ 为测量概率，$\overline{bel}(x_t)$ 为预测置信度。

贝叶斯滤波的性质：

1. 贝叶斯滤波算法是递归的，时刻 t 的置信度由上一时刻的置信度、最近的控制、最近的测量数据三者求得。
2. 算法分为两个过程：预测（控制更新）与更新。
3. 贝叶斯滤波的预测过程在控制之后、测量之前，上一时刻的置信度与此时刻的状态转移概率乘积的积分。
4. 贝叶斯滤波的更新在测量数据之后，测量概率与预测置信度的乘积，再乘以归一化变量。

马尔科夫假设

马尔可夫假设也可以说是完整状态假设。即如果知道当前状态 $x_t$，则马尔可夫假设认为，过去和未来数据都是独立的。

卡尔曼滤波（KF）

线性卡尔曼滤波主要分为两部分：预测与更新修正。这里先摆出算法伪代码：

其中上图中的 2、3 两式为预测方程，3、4、5式为更新修正方程。下面解释一下方程中参数的含义：

1. 预测方程部分，$\bar{\mu }$ 为状态量，$A$ 为预测矩阵，$Bu$ 为控制信号，$u$ 为测量噪声，$\bar{\Sigma }$ 为预测误差，矩阵 $R$ 是过程噪声的协方差。
2. 更新修正方程部分，$K$ 为卡尔曼增益，$C$ 为测量矩阵，$Q$ 也是噪声协方差，具体为测量噪声的均值，$\mu$ 为修正后的状态量，$z$ 为观测量。

拓展卡尔曼滤波（EKF）

拓展卡尔曼滤波与线性卡尔曼滤波相比采用了泰勒级数进行函数线性化。这里同样先摆出算法伪代码：

区别主要有以下几点：

1. 卡尔曼滤波中状态值迭代的公式由原先的线性函数变为了非线性的 $g(u_t,{\mu }_{t-1})$，一般 $g(\cdot )$ 为一个运动模型的状态转移函数。
2. 因为扩展卡尔曼滤波使用线性变换来近似非线性线性变换，具体来说，EKF使用多元一阶泰勒展式（雅可比矩阵）来进行线性化，所以预测误差 $\bar{\Sigma }$ 的由常矩阵 $A$ 变为了 $G$，即对各个元素求偏导数。
3. 参照预测过程，对于更新修正过程我们也需要求解测量空间的多元一阶泰勒展式，即将 $C$ 改为了雅可比矩阵 $H$

无迹卡尔曼滤波（UKF）

无迹卡尔曼滤波通过使用加权统计线性回归过程实现随机线性化。这种线性化的方法叫做无损变换（unscented transformation）这一技术主要通过 n 个在先验分布中采集的点（sigma points）的线性回归来线性化随机变量的非线性函数。由于考虑的是随机变量的扩展，这种线性化要比泰勒级数线性化（EKF所使用的策略）更准确。算法伪代码如下：

相对于卡尔曼滤波，无迹卡尔曼滤波的区别主要有

1. 首先要计算出各个 sigma 点的权重 $\omega$，然后基于每个 sigma 点的权重去求新的分布的均值 $\bar{\mu }$ 和方差 $\bar{\Sigma }$
2. 预测矩阵直接使用预测出来的 sigma 点集 $\bar{\mathcal{Z}}$，并忽略掉处理噪声部分
3. 因为测量噪声的协方差 $Q$ 对系统没有非线性影响，故直接加到测量协方差上
4. 更新方程同样需要计算出 sigma 点集在状态空间和测量空间的互相关函数 ${\bar{\Sigma }}^{x,z}$，后面按照卡尔曼滤波的更新步骤计算即可

总结

本文直接从公式角度分别对 KF、EKF 和 UKF 进行了一点分析，并比较了其中的区别。

本人以后会发布一些关于机器学习模型算法，自动控制算法的其他文章，也会聊一聊自己做的一些小项目，希望读者朋友们能够喜欢。