作者:SLAMer6+
来源:智驾全栈与3D视觉学习星球
欢迎各位加入知识星球,获取更多技术分享,欢迎转发朋友圈分享快乐。
内容介绍
如何提高融合/定位结果的精度?举一个工程到实际上的案例:某机器人定位误差等级从0.1m提高到0.01m,下游规划控制算法结果收敛性更强,对应机器通过性增强,由此可以展望产品市场及售卖Level等;
如何削减时空消耗体量,提高效率?例如算法保持精度的同时,加快算法收敛,结果响应更好;空间消耗减少可以避免内存浪费,智能化系统运行效率得到提高。反面教材:GTA游戏中的某个史诗级bug,因为一个用处不大的上万次循环,导致了玩家卡顿十秒以上,各种吐槽,被黑客优化后,GTA官方奖励黑客1W美刀;
如何调控参数或添加约束,使得方法更加稳定,提高移植性?例如封装好后,提供SDK或API式的智能融合/定位解决方案;再者添加环境模型结构化约束,添加Flag特征进行算法行为约束等;
如何提高算法与系统和硬件的交互?算法效率不仅仅是算法代码决定的,例如加强通信方式及消息传递过程,避免未响应,错误响应等,增加Lock/Security等来提高鲁棒性。
今天的内容包括:
了解ESKF
了解粒子滤波PF的重采样
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是计算比较快的算法。
粒子滤波重采样
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
以上内容如有错误请留言评论,欢迎指正交流。如有侵权,请联系删除
联系客服