深入理解如何不费吹灰之力搭建一个无人驾驶车（六）定位算法总结

六、slam中定位算法总结

6.1、概述
从本章开始介绍各种算法，大致分为定位、建图、slam体系（定位加建图），规划、路径跟踪四个方面，图片大部分引用自pythonrobotics，本系列将以pythonrobotics为基础再拓展讲很多其没提到的算法，本篇主要是讲定位系列。

6.2、高斯滤波KF系列
本人其实在我的第三章已经进行详细论述，这章算是补充，KF卡尔曼滤波属于高斯滤波系，其假设分布为一高斯分布，实际上现实中很多情况这种假设是不成立的，你需要去自己构造高斯分布，但是在许多条件下，我们的噪声是近似服从高斯分布的，这时候就可以用KF去进行定位融合，
KF也属于连续时间贝叶斯滤波，贝叶斯滤波的通式如下表达： 以下引用自我的第三章

贝叶斯滤波的第3行由里程计运动模型计算如下：（产生的高斯分布偏差即作为协方差矩阵）

其组成假设小车运动是先转弯再直行再转弯，因而由此产生了误差。

而由速度运动模型计算如下：

prob项即产生所谓的协方差矩阵，其误差包括v，w的误差及圆心偏移误差。

而KF如下表达：

其形象化表述为：


本质说白了就是让两个矮峰通过运算变成一个尖峰，为了让小学生能听懂，再次强调就是让两个分布融合，使得新分布不确定性降低，且均值（概率最高值）综合考量了两个分布的结果，而KF是强调，新分布的均值离两个观测中不确定性低的那个分布的均值最近。
为了解决预测分布的均值不能线性表示问题（即线性系数比如A，B无法计算），我们将表示函数泰勒展开取一阶项，于是得到EKF。
EKF如下表达：

其运算主要浪费在算一阶导上，即雅克比矩阵上，而为了降低EKF线性化的高阶项损失，我们还可以采用无损变换的方法，通过sigma points（关键点）无损变换得到新锋的参数（均值与协方差），而关键点必须能很好地刻画原分布（是不是想到压缩感知了？）关键是如何找关键点：UKF定义如下：

好了以上就是KF系的内容，我已经在第三章讲了。

6.3、信息滤波IF系列
KF如下：

我们做如下替换：，可以得到信息滤波的算法图如下：
（其中信息矩阵ξ与信息矩阵Ω为正则参数，其与高斯分布的矩（均值，协方差）是对偶的）

是不是一模一样？那么区别在哪：
1.设状态空间维度为n，预测步骤IF时间复杂度为O(n2.4),KF仅仅需要O(n2)，因为IF求逆运算比较费时间。
2.设状态空间维度为n，测量更新步骤IF时间复杂度为O(n2),KF却仅需要O(n2.4)，因为KF求逆运算比较费时间。
3.IF表示全局不确定性是简单的，只需让信息矩阵为0，而与之对应要让协方差矩阵变为无穷大才可以，比如传感器携带的信息是关于所有变量的严格子集时，就需要进行特殊处理。
不过信息矩阵的意义是？
其实我们在BA也谈到它，我们做一个类比，先从协方差说起，协方差对角线为其自己的方差，非对角线即为表示变量间的协方差，即变量间的联系，那么对协方差矩阵求逆得到信息矩阵，非对角线表示的就是变量间的边（或约束），我们要做的就是填信息矩阵，比如在t-1时刻我看到一个路标测量为（x1,y1），我的位姿为(a1,b1,c1),t时刻我看到同一个路标为(x2,y2),我的位姿为(a2,b2,c2),即可将约束填入信息矩阵去优化。如下：

你看，如果X3与m4未产生联系，为0即可，而协方差矩阵为0表示完全不相关，而对于不完全可观测情况，不确定并不代表不相关。但若设置为无穷大，就比如说x变化一小点，y就会成为无穷大，这显然不和逻辑。所以信息矩阵为0表示的是不参与计算，这样运算就方便了。而且信息矩阵比协方差矩阵稀疏，这样又可以通过某种方法删除一些边，让计算加快（之后讲的SEIF）（当然这里的求解与求解BA是有区别的）如下图：

对于g为非线性函数的时候我们有EIF方法。

比较下EKF：

同样是把正则参数带入EKF后的结果，

6.4、非参数滤波之直方图滤波
该法属于离散贝叶斯滤波，比起纯粹连续贝叶斯滤波其计算速度快，但是其性能降低了，不过比起连续贝叶斯滤波的高斯滤波系可以不用假设偏差是高斯分布，这点在某些情况又可以让性能提升，但纯粹用点去算连续贝叶斯滤波在计算机中是很难实现的（因为这非常耗计算量），所以有了直方图滤波，框图如下：

比较连续贝叶斯滤波：

我们发现，原本置信度bel变成了p，然后计算p估计变成了概率累加，反应在图上就是这样：

每个格子维持一个概率，而不是一个点，故我们计算时，累加就行，最后每个格子要有个观测加成，得到了t时刻每个格子的概率。其直观反映如下图

只不过把线变成了一个个区间，这个区间的大小是会影响直方图滤波的准确性的。越大效果越差，但速度越快。

6.5、非参数滤波之粒子滤波
我们说过，为了避免高斯分布的假设，我们可以试着用纯粹的点计算去做，但纯粹用点去算连续贝叶斯滤波在计算机中是很难实现的（因为这非常耗计算量），但我能不能只算部分点呢，（当然你可以用直方图滤波把空间分割成一个个格子），于是就有了粒子滤波。
引用自我第三章的图，如下：

连续时间贝叶斯滤波如下：

如何编写粒子滤波见我第三章，其实对比就会发现，粒子滤波是用粒子去近似连续贝叶斯滤波的结果，一个粒子就是一个状态量，比如粒子滤波有个思想，就是符合观测那么下一次有更高的概率被采样到，那其实就是连续贝叶斯滤波的第4项，粒子滤波巧妙地近似了。而为了防止机器人绑架问题即没有粒子无法计算问题，我们希望在粒子难以收敛的时候在地图中随机生成一些粒子，如下Augmented MCL：

而为了在粒子收敛时减少粒子数量（因为收敛时，区域粒子数量太多了，增加粒子对准确度影响不大，我们希望不显著减少粒子滤波精度的情况下降低些粒子），于是有了KLD-MCL：

这样，定位算法大致就讲完了，下次讲建图mapping算法