概率机器人 第三章高斯滤波（正态分布）

高斯滤波也就是在将贝叶斯滤波里面的所有置信度都用正态分布来表示。高斯分布具有单峰，这是符合机器人学中很多的追踪问题都是单峰的，并且后验都是以小的不确定性聚集在真实状态的周围。本章讨论了两种参数的绿滤波，基于矩参数的滤波和基于正则参数的滤波。矩参数是指均值和方差，因为均值和方差是概率分布的一阶矩和二阶矩。正则参数由信息矩阵和信息向量组成。通过矩阵求逆，可以从一个参数得到另外的一个参数，这两种方法对偶的。
对于这两种滤波方式都必须要满足三个基本要求：1.初始置信度必须满足高斯分布。2.状态转移方程是一个包含独立高斯噪声，且是与参数呈线性关系的函数。3.测量方程也是一个包含独立高斯噪声，且与参数呈线性关系的函数。
两种滤波的方式都可以扩展到非线性系统。可以通过泰勒展开用近似替代的方法得到一个近似线性的函数，这样的方法叫做扩展卡尔曼滤波和扩展信息滤波。
卡尔曼滤波还可以采取不同的线性近似方法得到无迹卡尔曼滤波，通过将函数选定点进行无迹变换得到线性化。扩展卡尔曼滤波核无迹卡尔曼滤波都是通过将非线性函数近似线性化来处理得到的。泰勒级数展开和无迹变换的精度性取决于两个因素：系统的非线性程度和不确定程度。如果能够较为准确的知道系统的状态，那么由线性化所造成的误差就相对较小。
高斯向多峰后验的扩展被称为多假设卡尔曼滤波，它用高斯混合分布表示后验，高斯混合分布就是多个高斯分布的加权和，在更新该滤波的时候需要分裂和融合或者修剪单个高斯分布。
卡尔曼滤波算法
卡尔曼滤波实现了连续状态的置信度计算，但是它不适用与离散或者混合的状态空间。它用矩参数表示置信度：在t时刻，置信度用均值μ和方差Σ_t表示。如果它满足三个条件和马尔科夫性那么它的后验就是高斯的。三个条件在上面已经提到了，状态转移方程的具体形式如下：

其中x_t和x_(t-1)分别表示两个时刻的状态，u_t为t时刻的控制向量。A_t和B_t分别是n×n和n×m的矩阵，其中m是控制向量的维数，n是状态的维数。ε_t是一个满足高斯分布的噪声，均值为0，方差为R_t。
测量方程的具体形式如下：

C_t是k×n的矩阵，k的测量向量的维数，n是状态的维数。σ_t是满足高斯分布的测量噪声，均值为0，方差为Q_t。
初始置信度满足正态分布，初始置信度的均值为μ_0，方差为Σ_0。

整个算法如下：

前两行分别计算预测的均值和方差，这两个表示了在测量更新前的预测置信度(bel) ̅(x_t),这只是仅仅考虑了控制所引起的改变。第三行是计算的卡尔曼增益，具体实现过程可以在网上百度，有很多介绍。第四行和第五行是考虑了测量之后的对预测的置信度进行的更新。卡尔曼滤波由于是贝叶斯滤波的一种，所以它也包含贝叶斯滤波的两个步骤，预测和更新。
扩展卡尔曼滤波
由于卡尔曼滤波算法有三个要求，其中观测方程和测量方程都必须满足线性关系，但是很多实际情况下这两个方程可能都是非线性的，所以卡尔曼滤波不能很好的满足这个需求。但是有很多办法可以将非线性的近似为线性处理，扩展卡尔曼滤波通过泰勒展开的办法来将非线性函数近似线性化。
状态转移方程和观测方程如下：

g(u_t,x_(t-1) )和h(x_t )都是非线性函数。ε_t是一个满足高斯分布的噪声，均值为0，方差为R_t。σ_t是满足高斯分布的测量噪声，均值为0，方差为Q_t。
将两个非线性函数泰勒展开：

既然是近似，那么肯定要选取一个认为最有可能的值来近似，这样近似的效果才会好，所以这里用μ(t-1)去近似x(t-1),用t是课的预测均值((μt ) ̅ )去近似真实的状态值(x_t )。那么状态转移置信度的均值就变成了[x_t-g(u_t,μ(t-1) )+G_t (x_(t-1)-μ_(t-1) )]，观测置信度的均值变为了[z_t-h((μ_t ̅) )+H_t (x_t-(μ_t ̅) )]，两个的方差都没有发生改变。这样我们就可以得到扩展卡尔曼滤波算法了。
整个算法如下：

无迹卡尔曼滤波算法
扩展卡尔曼滤波是采用的泰勒级数展开得到的，但是为了得到更好的结果，我们还可以采用另外一种方法，通过加权统计线性回归过程实现随机线性化，这种方法通常能够得到更好的近似，被称为无迹卡尔曼滤波算法。
这些点的具体选取过程如下：通过选取位于均值处的点和对称分布于主轴协方差处的两个点（每一维），然后将其进行非线性变换。具体的选取规则如下：

在这其中λ=α^2 (n+κ)-n,这里的α和κ是确定选取点分布在均值范围的一个比例参数。每一个选取的点都有两个相关的权值，一个是w_m^i,它是计算均值的时候使用的，另外一个是w_c^i,它是计算高斯协方差的时候使用的。

参数β是考虑了附加的高斯噪声所产生的影响。如果分布是精确的高斯模型，通常取值为2.
选取的点经过非线性变换得到对应的点

经过变换后的点服从的高斯分布均值和方差计算如下：

整个算法如下：

信息滤波
卡尔曼滤波的对偶算法就是信息滤波，他们两个有着相同的假设，都假设置信度用高斯分布表示。两者主要的不同在于对高斯置信度的表示不同，卡尔曼滤波是用他们的矩（一阶矩和二阶矩），而信息滤波是用他们的正则参数来表示的。由于参数的不同也导致了不同的更新方程，但是思想都是一样的。具体而言就是对于卡尔曼滤波而言可能参数计算复杂，那么信息滤波可能会简单点，但是信息滤波计算复杂的地方，卡尔曼滤波计算又要简单一些。
正则参数由矩阵Ω和向量ξ组成。他们可以有一阶矩和二阶矩推导出来。

这两个参数是通过高斯变换简单的处理而得到的。

把只含有x的项提取出来，其余都是常量，得到：

把矩阵Ω和向量ξ带入可以得到由正则参数表示的高斯分布

信息滤波算法：
对于信息滤波而言，同样需要满足三个要求，状态转移方程和观测方程都是线性的，且都有一个均值为0的高斯噪声，初始状态满足高斯分布,观测方程和状态转移方程都如卡尔曼滤波一样。
整个算法如下：

信息滤波如同卡尔曼滤波一样，都是分为两个步骤，先预测然后更新。可以看到信息滤波再预测的时候需要求一个n*n矩阵的逆矩阵，这将加大运算难度。而在卡尔曼滤波中就不会存在这个问题。但是在预测的时候卡尔曼滤波需要求卡尔曼增益，这个卡尔曼增益会有一个求逆矩阵，所以这两个是对偶的。
扩展信息滤波
同样的，信息滤波也可以近似非线性的情况，称为扩展信息滤波算法,状态转移方程和观测方程都和扩展卡尔曼滤波是一样的，符号意义也相同。
整个算法如下：