滤波算法与传感器融合

卡尔曼滤波

卡尔曼滤波主要是采用预测和更新两个过程。

预测过程主要是根据机器人运动模型，对机器人的下一个位置进行预测，其中P代表系统误差。系统误差是在更新的。例如传感器测量误差Q非常大，这样导致卡尔曼增益K值变小，这样测量值对于预测值的修正作用就会降低，因为测量值没有对预测值起到足够的修正作用，这也就导致系统误差在累计增加。同样，如果传感器足够精确，对预测量起到有效的修正，这是的系统误差就会降低。

 

拓展卡尔曼滤波主要是为了解决预测方程和测量方程可能不是线性方程，如果是非线性方程，只能使用泰勒公式进行求导

 

 

预测值其实是一个先验概率，测量值为后验概率，通过后验概率估计和确定机器人的实际位置。

 

多个传感器的融合过程其实是每个测量值与系统模型预测值进行融合，融合结果形成新的系统模型预测值，所有导航子系统各进行一次拓展卡尔曼滤波过程，则整个组合导航系统完成一次组合导航系统，系统模型的最新预测值即为整个组合导航系统的输出的最优估计结果。

上边写的是一种传感器进行串行卡尔曼融合的算法，还有一种是分多个局部卡尔曼滤波，然后通过对协方差矩阵进行简单融合，达到融合效果。

 

 

 

 

 

粒子滤波

 

粒子滤波算法的核心思想是利用一系列随机样本的加权和近似后验概率密度函数，通过求和来近似积分操作。该算法源于Monte Carlo思想，即以某事件出现的频率来指代该事件的概率。因此在滤波过程中，需要用到概率的地方，一概对变量采样，以大量采样及其相应的权值来近似表示概率密度函数。

1）预测阶段：粒子滤波首先根据状态转移函数预测生成大量的采样，这些采样就称之为粒子，利用这些粒子的加权和来逼近后验概率密度。

2）校正阶段：随着观测值的依次到达，为每个粒子计算相应的重要性权值。这个权值代表了预测的位姿取第个粒子时获得观测的概率。如此这般下来，对所有粒子都进行这样一个评价，越有可能获得观测的粒子，获得的权重越高。

3）重采样阶段：根据权值的比例重新分布采样粒子。由于近似逼近连续分布的粒子数量有限，因此这个步骤非常重要。下一轮滤波中，再将重采样过后的粒子集输入到状态转移方程中，就能够获得新的预测粒子了。

4）地图估计：对于每个采样的粒子，通过其采样的轨迹与观测计算出相应的地图估计。

 

粒子滤波用于AMCL定位过程

主要是在评分权重部分，粒子的评分权重是通过ICP进行地图匹配。一个简单的评分标准是，检查scan与地图的匹配程度，占据+占据是5分，占据+空闲是负分，空闲对空闲是1分。这样就会建立一个简单的评分标准，找到与地图匹配程度最高的粒子。

对于2D地图而言，地图特征区别度可能不高，会导致粒子滤波陷入局部最优解，这也是一种“机器人绑架”问题。为了解决机器人问题，可以通过提供一个在正确位置附近的先验过程，这样可以防止陷入局部最优解。

 

对于Gmapping中的粒子滤波建图，这个过程发生部分变化，主要是评分权重部分的区别。同样还是进行粒子采样。这个过程自己还不是很清晰。明天可能得主要解决一下这方面的问题了

 

粒子滤波器受限于空间维度，高斯类算法状态估计问题的维度还在线性和二次方之间，而粒子滤波器则是指数级的。 直接将粒子滤波器应用于SLAM问题注定会因为描述地图时所需的大量变量而失效。

在已知关联度的完全SLAM问题中，给定机器人位姿，在地图中任意两个不相交的特征集合之间， 存在着条件独立性。

如果可以得到机器人的真实路径，就可以不依赖于其他特征而直接估计所有特征位置。

 

这里的思想其实是一种采用贝叶斯后验概率的思想将机器人的定位与过程进行拆分的过程。

定位问题采用粒子滤波方法解决

地图创建问题采用扩展卡尔曼滤波方法解决

 

换句话说，如果有条神谕告诉我们真正的机器人路径，我们就可以不依赖于其它特征。 这一结构性的观察，将使得我们有可能使用一种称为Rao-Blackwellized particle filter(简称为RB滤波器)的粒子滤波器来解决SLAM问题。

RB滤波器使用粒子集合来表示一些变量的后验概率， 同时用高斯或者其它参数化的概率密度来表示剩下的所有变量。

https://gaoyichao.com/Xiaotu/?book=probabilistic_robotics&title=pr_chapter13

FastSLAM的流程是：

https://blog.csdn.net/weixin_42048023/article/details/85620544

http://www.cs.bilkent.edu.tr/~culha/ee780/

1.接收里程计信息，根据里程计误差模型产生若干粒子。

2.在每个时间周期对粒子的权值进行评估，评估的标准是与激光信息的契合程度，可以通过ICP方法来判定。

3.根据上一轮产生的带权值粒子进行权值更新与重采样，重采样是根据实际分布与建议分布之商来决定的。

4.取权值最大的粒子集合的平均值作为本轮的粒子先验，再利用激光信息更新地图。

 

 

 

 

卡尔曼滤波与粒子滤波进行比较

卡尔曼滤波与粒子滤波的模型其实是一样的

测量模型：z_t=h(x_t, v_t);

预测模型：x_t=f(x_(t-1),w_t);

粒子滤波与卡尔曼滤波的区别在于在于，卡尔曼滤波假设后验概率分布P(x_t| z_t)不再是高斯分布（高斯分布式一种单峰概率分布）。也就是卡尔曼滤波的模型必须符合高斯分布。

对于后验概率的后验条件可能有多种的情况，也就是多峰概率问题。只能是用蒙特卡洛的思想去近似得到概率密度分布函数。通过状态粒子来表示来表示，状态粒子的分布情况即是整个概率密度分布函数。

同时使用非参估计模拟任何一点的概率，这个操作过程是对所有粒子进行进行权重计算，通过归一化出来确定每一点的概率。通过确定后验概率的期望即是最后的滤波结果。

 

蒙特卡洛的思想主要应用于粒子滤波采样过程，一开始是均布，后根据概率弥补分布不同进行收敛。

 

多峰概率问题粒子：

形象一些说， 如果你发烧了，那你既有可能是嗓子发炎了，也有可能是身体其他部分发炎了，甚至有可能产生肿瘤了。那对于发烧这个现象的原因的条件概率，就会有多个峰值。

 

https://www.cnblogs.com/flyinggod/p/8767669.html