Fast-Lio2代码及论文解析2

目录

一、系统框架

二、状态方程

三、向前传播

四、反向传播

五、观测模型

 六、迭代更新

---

一、系统框架

• 系统输入为IMU测量数据和LiDAR点云数据，分别用作于系统的状态预测与更新阶段；
• 点云数据频率较低而IMU为高频数据，IMU测量数据到来后，系统状态根据IMU运动微分方程不断向前传播；
• 由于载体处于运动状态，在一帧点云积累完毕发出后，并非所有点云都是在数据发出时刻采集到的，此时我们可以根据IMU向前传播的计算结果，将所有点云都校正到点云数据发出的时刻；
• 随后，根据流形卡尔曼滤波（ESKF），将观测模型线性化后得到误差观测方程，计算残差与对应的雅可比矩阵，在先验与观测的基础上采用最大后验估计进行迭代卡尔曼滤波(IEKF)，直到满足迭代终止条件后输出后验估计，并更新地图，关于ikd-tree与地图管理模块本文将不做讨论。