|
作者：SLAMer6+ 来源：智驾全栈与3D视觉学习星球 欢迎各位加入知识星球，获取更多技术分享，欢迎转发朋友圈分享快乐。
本篇要记录的是卡尔曼滤波与粒子滤波的拓展学习。研读理论，学习代码，上手调参，自主研发，是SLAM从理论到落地再到应用的关键步骤。为了解决实际的工程问题，举例说几个KF与PF系列算法的可行提高点，且这些点也被前辈们用各种算法和思想优化提高过，理论和结果都十分经典，其被概括为包括但不限于的以下几点：如何提高融合/定位结果的精度？举一个工程到实际上的案例：某机器人定位误差等级从0.1m提高到0.01m，下游规划控制算法结果收敛性更强，对应机器通过性增强，由此可以展望产品市场及售卖Level等； 如何削减时空消耗体量，提高效率？例如算法保持精度的同时，加快算法收敛，结果响应更好；空间消耗减少可以避免内存浪费，智能化系统运行效率得到提高。反面教材：GTA游戏中的某个史诗级bug，因为一个用处不大的上万次循环，导致了玩家卡顿十秒以上，各种吐槽，被黑客优化后，GTA官方奖励黑客1W美刀； 如何调控参数或添加约束，使得方法更加稳定，提高移植性？例如封装好后，提供SDK或API式的智能融合/定位解决方案；再者添加环境模型结构化约束，添加Flag特征进行算法行为约束等； 如何提高算法与系统和硬件的交互？算法效率不仅仅是算法代码决定的，例如加强通信方式及消息传递过程，避免未响应，错误响应等，增加Lock/Security等来提高鲁棒性。
今天的内容包括： ESKF是误差状态卡尔曼滤波（Error State Kalman Filter），KF系列算法可以适用于感知融合，定位，数据处理与关联等多种任务，而ESKF的初衷是为了完成定位这项任务的，这种考虑多状态的KF，在SLAM的惯导系统是比较多用的，在这里拉出来多谈一下，至于AKF（自适应卡尔曼滤波，Sage-Husa算法），IEKF（迭代扩展卡尔曼滤波）等，就不多说了。学习KF系列算法前首先要弄明白系统的需求是什么，比如在SLAM中到底是用它来做数据融合，还是用它来做目标定位，甚至是其他的一些后端优化。ESKF的工作基本原理：为了求解机器人/汽车的位姿，而IMU测量数据（包括加速度计，陀螺仪等硬件）带有大量噪声，直接使用IMU数据对时间积分，是会产生明显的漂移。因此，为了更好的获取结果，需要融合其他传感器数据，例如GPS，Lidar等，从而修正数据，得到收敛性更好的结果。ESKF的核心思想是把系统状态分为三类：True State（Ground Truth）：真值，指系统实际的运行状态，一般我们会在系统中假定某种输入为真值。（这一条在非线性KF系列滤波通用）； Nominal State：名义状态，指系统的变化情况或变化趋势。例如IMU的各轴向加速度对时间的积分可以认为是各轴向距离上的变化，一般在系统中我们认为它是非线性（这一条也是我们在非线性KF中的强关注点）； Error State：误差状态，真值与名义状态的差值，这个值也是非线性变化的。例如在IMU、激光、毫米波的系统中，我们可以假定激光的数据为真值，用其他两种来做融合，结果与激光做RMSE，这就可以认为是一种误差状态。
那么我们可以将任意一个状态变化过程，分割为名义变化过程和误差项，即真值为名义变化过后的结果与误差值的叠加。由此给出ESKF的步骤如下： 对IMU数据进行积分，获得名义状态X，注意这个X并没有加入额外的考虑，所以必然引入了累计误差； 利用KF算法估计Error State，包括状态更新和测量更新，这个过程是考虑了噪声的，而且由于这个误差状态的方程式近似线性的，直接使用KF就可以； 利用Error State修正Nominal State，获取“真值”； 重置Error State，等待下一次更新；
使用ESKF的优势： Error State中的参数数量与运动自由度是相等的，避免了过参数化引起的协方差矩阵奇异的风险； Error State总是接近于0，Kalman Filter工作在原点附近。因此，远离奇异值、万向节锁，并且保证了线性化的合理性和有效性； Error State总是很小，因此二阶项都可以忽略，因此雅可比矩阵的计算会很简单，很迅速； Error State的变化平缓，因此KF修正的频率不需要太高。
ESKF与KF，EKF，UKF的关系，区别这里稍微讲一下： 关系： 这一系列的算法本质上都是概率的运算，最核心的思想是贝叶斯概率，也就是先验问题与后验问题的定义与其概率计算； 各算法的推导过程和最终形态以及求解公式，都是十分相似的，离不开最基础的经典5大公式；
无论是否迭代，kF系列算法都基于马尔科夫假设，也就是只与上一帧数据进行参比计算，因此其结果精确性与稳定性相比最新的一些算法或后端图优化来说，还是有一些逊色。 区别： KF针对线性，EKF通过对目标函数的二阶泰勒式子进行截断来使非线性的内容线性化，从而完成非线性估计，但其截断误差会影响结果，持续下去也会发散；UKF通过对目标函数的分布进行特征点采样，采用高斯的 原则，选取特征点来模拟分布；PF则是与目标函数本身关系不大，使用大量的粒子对原目标函数的分布中打去，经过权重筛选和重采样，不断将模拟的结果作为分布的结果输入，获得滤波的结果；而ESKF是一种间接的滤波法，不直接对目标函数求解，而采取对系统误差状态求解。 需要一提的是，EKF由于雅克比矩阵的计算，其复杂度相对较高，时间消耗大，而UKF消耗受限于关键点的选取，PF除了受限于粒子数目外，还受限于采样方法，ESKF与KF是计算比较快的算法。
粒子滤波重采样 粒子退化问题是在做PF系列算法中比较头痛的，退化的粒子会让结果看上去变得很好，但实际离真值却又差之千里，如何在迭代中不断选取有效的粒子，即重采样是一个值得关注的问题。由此引出重采样方式的选取，粒子退化指的是粒子如果始终按照优胜劣汰的方式去选取，则有可能陷入局部最优而忽略实际中确实存在的低权重情况，甚至会直接选取错误的结果。因此为了避免粒子重复性选取导致的权重值过分“完美”，需要进行重采样，保持粒子的多样性与可参考性。重采样方式这里给出比较经典的4种，分别是随机采样，系统采样，残差采样和多项式采样。a.随机采样是一种利用分层统计思想设计出来的，将空间均匀划分，粒子打点后会产生高集中的均匀分布区，将各分布区的粒子点进行权重累计并解算（例如求平均权重），生成若干个区间权重，使用该信息进行求解。其理解起来的几何思想就是给粒子点做索引编号，对应较多的索引编号将会保留，而较少的就会被淘汰。随机采样可能会因为存在多个权重接近的均匀区而产生求解方向错误，甚至陷入局部最优。以下给出一段随机采样的matlab代码。随后几种其他重采样方法代码不再展示。function randomR_testN=10; %粒子数目A=[2,8,2,7,3,5,5,1,4,6]; %拟定的数据集，如果可以的话，可以使用随机函数生成IndexA=1:N; %记录索引W=A./sum(A);%根据不同的占比值重新分配随机层的权重%随机采样方法OutIndex=randomR(W);NewA=A(OutIndex);%第二次迭代W=NewA./sum(NewA);OutIndex=randomR(W);NewA2=NewA(OutIndex);%第三次迭代W=NewA2./sum(NewA2);OutIndex=randomR(W);NewA3=NewA2(OutIndex);% matlab绘图figuresubplot(2,1,1);plot(A,'--ro','MarkerFace','g');axis([1,N,1,N]);subplot(2,1,2);plot(NewA,'--ro','MarkerFace','g');axis([1,N,1,N]);%随机采样函数function outIndex=randomR(weight)L=length(weight);outIndex=zeros(1,L);%产生随机分布，完全均匀u=unifrnd(0,1,1,L);u=sort(u);%计算粒子的权重累计函数cdfcdf=cumsum(weight);%核心计算i=1;for j=1:Lwhile (i<=L)&(u(i)<=cdf(j))outIndex(i)=j;%粒子复制i=i+1;%迭代考察各个随机分层endend b.多项式采样是1993年提出的改进，基本解决了粒子滤波的粒子退化问题。其基本形式与推进思想与随机采样相似，不同在于多项式采样会不断记录并复制计算相似或相同结果的权重累计值，从而得到最优的发展结果。c.系统重采样是2000年提出的，这种算法与多项式采样算法较为近似，即将原有的权重空间分层，在每一层中抽取粒子，而系统重采样产生了一个随机数来供所有层使用，其结果更加均匀。d.残差重采样法是1998年提出的，它以多项式重采样法为基础，其基本思想是直接保留原有的权重大的粒子，剩下的不足部分随机选取重新赋权的粒子补充。重采样的方法比较多，这里大概说的4种比较经典，其重采样质量和算法复杂度有如下的区别：当粒子数比较少的时候，多项式重采样，系统重采样，残差重采样法是优于随机重采样的；而当粒子比较多的时候，这几种重采样的方法结果比较相似，而算法时间上，系统重采样和多项式重采样要更优一些，所以在实际的SLAM粒子滤波需求中，使用经典的系统重采样法来完成就行。那么关于KF与PF系列的学习就告一段落，具体的可以结合概念原理，以及一些案例来学习，再下载代码框架，尝试填充内容，跑通，就算是把这些东西基本上掌握了，但如果要精进，还需要多做做代码方面的工作，多实现一些技巧性功能，才能更上一层楼。另外，需要说的是，在现有的无人驾驶汽车、机器人上，KF与PF的解决方案是经典的，可用的，但依旧有很多问题，在现在的落地使用中已经在走下坡路甚至是有些落寞吃灰，最新的后端解决方案例如非线性优化思想引出的方法还需要额外学习。以上内容如有错误请留言评论，欢迎指正交流。如有侵权，请联系删除
|
