Lidar第一帧作为基坐标
1、lidar原始数据预处理默认不提取特征，对原始数据间隔式（间隔3个点）降采样提取。
2、imu初始化、惯性解算、误差分析、状态、协方差预测
3、Lidar与imu时间状态对齐
4、Lidar去畸变
5、构建局部地图，提供最近点索引，最小二乘方法拟合平面，构建点面残差
6、误差卡尔曼滤波
一、IMU
IMU初始化：计算加速度、角速度均值，利用均值计算方差。
IMU惯性解算、误差分析：状态、协方差预测
惯性解算：
利用相邻两个时刻的imu数据，采用离散中值法作为运动方程输入的加速度和角速度，并去除零偏。
Fast-lio认为加速度计方向是准确的，将已知当地的重力大小做先验，利用比例和先验重力微调重力大小。3自由度变为2个自由度。
计算旋转、加速度（转全局坐标系下并去重力和零偏）、位置传播、速度传播进行状态预测。
两种传播情况：当imu的head和tail在lidar帧前时，imu从head传播到tail，当lidar帧出现在imu两帧之间时，imu积分传播从lidar帧开始。
误差分析：
构建误差微分方程，得到状态转移矩阵，进行协方差传递。
二、lidar与Imu时间和状态对齐
计算lidar与imu最后时刻的时间差
通过Imu的加速度与角速度预估lidar最后时刻的速度、旋转和位置，此时仍是在imu坐标系下
通过外参计算出Lidar最后时刻世界坐标系下的状态。imu积分结果给lidar位姿提供初值。
三、Lidar点云去畸变
目的：将一帧中每个激光点云投影到最后时刻
获得每个激光点云的位姿，两帧imu之间会有许多lidar点，通过插值法，即得lidar与imu头帧的时间差，积分得lidar在imu下的位姿，再积分指lidar末尾，再通过外参转至lidar系下。
四、构建局部地图
以lidar帧最后一个状态构建立方体，立方体长宽高为48
地图管理：在ikdtree上保留当前lidar位置周围得局部地图，根据去畸变后的点云构造三维体素栅格，对体素栅格下采样达到滤波得目的（一个体素用中心点来近似，减少点的数量，保持点云形状）。
第一帧lidar数据用来构造ikdtree，其他帧加入树进行索引变换构建局部地图，当前点利用最近邻搜索找最近点5个（最远距离小于5m），最小二乘法将最近点拟合平面，获得平面方程系数，检验计算平面的有效性：将最近点集合带入平面方程得残差，残差与阈值（0.05）做比较，误差超过阈值，拟合得平面有问题，小于阈值，拟合得平面有效。计算点到平面的残差（当前点带入平面方程），构造观测方程和雅可比矩阵（残差关于优化变量求导）得到H阵。链式法则求偏导，观测方程先对状态求导，状态量对误差状态量求导。
五、误差卡尔曼滤波
名义值变量（8个、23维）--位置、姿态、旋转外参、平移外参、速度、加速度零偏、角速度零偏、重力
P阵：状态协方差矩阵（23*23维）
过程噪声维数Q（4个、12维）--加速度、角速度白噪声、加速度角速度零偏白噪声。斜对角阵
状态转移矩阵Fx（24*23维），噪声转移矩阵Fw（24*12维）。
R阵：测量噪声协方差矩阵（n*n维）一帧中有n个点云，斜对角阵
观测方程的雅可比矩阵H阵（n*12维）一帧中有n个点云，残差与位置、姿态、旋转外参、平移外参这四个优化变量有关，共12维。
计算卡尔曼滤波增益：如果量测维度小于状态维度，用传统公式（n*n维），否则用新公式（12*12维）
误差值会随着迭代次数增加而减小，当相邻两次增量之差小于一定阈值就认为是收敛，或者达到最大迭代次数也将停止迭代。用误差的后验去更新位姿并且更新P阵（状态协方差矩阵）
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
