这篇文章将向大家介绍LIO部分的基于ESKF(error state kalman filter)的误差状态卡尔曼滤波算法来进行位姿优化的一些细节内容。
LIO的ESKF实现的运动方程参考文献[4]，其采用基于误差状态卡尔曼滤波器的原理，将系统的运行状态分解为非线性传播的名义状态和线性传播的误差状态；其中名义状态保持非线性状态的传播特性（IMU预积分的过程的imu pose的积分计算），而误差状态通过一阶泰勒展开将非线性问题局部线性化。通过线性模型将误差状态进行传播（参见预积分过程中state.cov协方差矩阵的传播计算，详细说明见文献[4]中）。
VoxelMapManager类的StateEstimation函数则实现了基于lidar和imu紧耦合的ESKF位姿优化算法，其中误差状态传播方程基于IMU的预积分结果，残差测量方程基于Lidar点云的多种误差因素（如基于李代数的旋转扰动量等），这里比较特殊的地方表现在：1、基于误差状态的传播计算，具体的关于误差状态的传播计算方式参考[1],主要在于基于IMU的误差状态传播协方差F_x的定义及误差传播协方差的计算实现[2]；2、基于测量误差方法的特殊性（根据名义状态和误差状态的定义，以及基于李代数的扰动模型等便于求导的方法进行的状态定义），点云的测量残差方程的定义基于点云点到匹配平面的距离残差，其雅可比以及ESKF的计算公式推导请参考[5]。
References
- [1]、LIO的ESKF实现的误差状态传播方程和代码对照分析
- [2]、误差状态传播协方差矩阵的计算实现： https://gitee.com/kindlytree/fast-livo2-comments/blob/main/src/IMU_Processing.cpp#L431
- [3]、在LIO(lidar和imu融合的背景下)，ESKF算法误差测量方程如何定义？
- [4]、点云点误差测量协方差矩阵的计算实现： https://gitee.com/kindlytree/fast-livo2-comments/blob/main/src/voxel_map.cpp#L470
- [5]、https://gitee.com/kindlytree/fast-livo2-comments/blob/main/LIO.md#误差观测方程–hmathbfx–及其雅可比矩阵推导
Leave a Reply