前言:
在SLAM中,状态估计问题一般采用滤波器或者非线性优化的方法来解决,例如在LIO-SAM中进行状态估计时,使用了ICP的方法进行状态估计,其中ICP的求解采用了高斯牛顿的非线性优化方法;又例如在FAST-LIO中使用了滤波器迭代卡尔曼滤波的方法进行状态估计。
1.非线性优化和EKF的区别:
在优化问题中,每迭代一次,状态估计发生变化之后,我们会重新对新的估计点做泰勒展开,而不像EKF那样只在固定点上做一次泰勒展开,可以粗略的认为EKF仅是优化中的一次迭代。
非线性优化-高斯牛顿法的优化过程可参考以下文章:
EKF相较于KF的通常做法是,在某个点附近考虑运动方程及观测方程的一阶泰勒展开,只保留一阶项,即线性的部分,然后按照线性系统进行推导。
KF的理解可以参考以下文章: