概率机器人中的高斯滤波

---

高斯滤波
常用的高斯滤波有卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波、信息滤波。
连续空间。
置信度用多元正态分布表示。
高斯是单峰的，具有单一的极大值。
高斯滤波的缺点：单峰高斯分布方面的局限。

矩参数：高斯滤波中参数均值和方差称为矩参数，均值是概率分布的一阶矩阵，方差是概率分布的二阶矩阵，正态分布其他矩都是0。
正则参数：也叫本质参数，可通过矩参数转化。

---

卡尔曼滤波：（KF）

适用于：线性高斯系统
作用：对连续状态的置信度计算。不适用于离散或混合状态空间。
使用KF的四个条件：
（1）满足贝叶斯滤波的马尔可夫假设
（2）状态转移概率，必须是带有随机高斯噪声的参数的线性函数
（3）测量概率，也与带有高斯噪声的自变量呈线性关系。
（4）初始置信度，必须是正态分布
满足上面的四个条件，在任何时刻t总是高斯分布。

卡尔曼滤波算法：
KF表示均值为μ_t、方差为Σ_t在时刻t的置信度bel(x_t)。
KF算法伪代码如下图所示：

KF算法的输入：
（1）t-1时刻的置信度，其均值和方差分别用μ_t-1、Σ_t-1表示。
（2）t时刻的控制向量u_t和测量向量z_t

KF算法的输出：
输出t时刻的置信度，用用μ_t、Σ_t表示。然后用多元正态分布公式轻松的就计算出来了。

KF算法的流程：
（1） 通过u_t和上一时刻的置信度，求预测置信度，第2-3行
（2） 计算卡尔曼增益矩阵，反映了测量数据在新的状态估计的程度。第4行
（3） 通过卡尔曼增益矩阵、真实的测量向量z_t、预测置信度来更新该时刻的置信度。称为更新，第5-6行

KF算法的计算效率：
矩阵求逆的计算复杂度为O(d^2.4),这里指维数为dxd的矩阵
KF算法的每一次迭代下界近似为O(k^2.4)，这里k是测量向量zt的维数。源于第4行矩阵求逆。
第6行乘法，复杂度至少为O(n²),n为状态空间的维数。

KF算法的局限性：
只适用于高斯线性系统。

---

扩展卡尔曼滤波：(EKF)

卡尔曼滤波虽然很好，但是不幸的是，实际状态转移和测量很少是线性的。所以扩展卡尔曼滤波就出现了。
扩展卡尔曼滤波放宽了使用条件，假设状态转移概率和测量概率分别由非线性函数g和h控制。
对于任意函数g和h，置信度不再是一个高斯分布。
实际上EKF计算的置信度是真实置信度的高斯近似值。EKF继承KF表示t时刻置信度的表示，但是有不同。

EKF的目标：从EK精确的计算后验概率，转变为有效地估计均值和方差。

EKF近似的思想：通过一阶泰勒展开的线性化

线性化通过一个在高斯函数的均值处与非线性函数g相切的线性函数去近似g。
蒙特卡洛对高斯的估计是将500000点进行g变换，然后计算出均值和方差。与EKF的存在误差。

EKF的线性化：
（1） 泰勒展开根据g的值和斜率构造一个函数g的线性近似函数。

对于高斯函数，最有可能的状态就是后验的均值μ_t-1。

写成高斯形式，状态转移概率可由下式近似：

G_t为n x n矩阵，n代表状态的维数。该矩阵称为雅克比矩阵，不同时刻是不同的。
（2） 将函数h线性化

写成高斯函数，测量概率为：

扩展卡尔曼滤波算法：
算法伪代码如下：

算法流程：与KF一致，只是其中内容被替换了，核心就是将非线性的状态转移和测量线性化后进行与KF一样的预测、更新流程。
雅克比矩阵G_t对应A_t和B_t，H_t对应C_t。

EKF的时间复杂度：
O(k^2.4+n²),其中，k是测量向量z_t的维数，n为状态向量x_t的维数。

EKF的局限性：
如果非线性函数在估计均值处近似为线性，那么EKF能够以足够高的精度近似后验置信度。
若是较高的不确定性通常会导致结果随机变量的均值和方差估计更不精确。

---

无迹卡尔曼滤波：（UKF）

UKF通过使用加权统计线性回归过程实现随机线性化。

线性化方式：
不是采用泰勒级数展开的方式，通过取的σ点直接经过函数g进行变换。若是n维高斯分布，结果有2n+1个σ点。σ点位于均值处及对称分布于主轴的协方差处（每维两个）。
每个σ点都有两个与之相关的权值，一个权值计算均值时使用，另一个权值计算高斯的协方差时使用。

UKF的精度高于EKF

无迹卡尔曼滤波算法：
UKF算法的渐近复杂性与EKF的相同，UKF的一个优点是不需要计算雅克比矩阵，因此UKF也被称为免求导滤波器。

UKF的局限性：
如果置信度是高度非高斯的，UKF表示就具有很大局限性，滤波实现就会很差。

---

信息滤波：（IF）

KF的对偶滤波算法就是信息滤波IF。IF用高斯分布表示置信度，标准的IF和KF有相同的假设。
KF算法的高斯分布由它们的矩（均值和方差）表示。
IF算法的高斯分布由正则参数表示。该正则参数由一个信息矩阵和信息向量组成。

正则参数：

信息滤波算法：
信息滤波的伪代码如下：

IF算法和KF的算法流程一样，式子可以也通过矩和本质参数的转化，化为一致。
需要满足KF满足的所有条件，也只适用于线性高斯系统。
输入和输出变成了本质参数，也是预测和更新过程。
IF算法的复杂度正好和KF的顺序相反。