Fast-lio2原理解析见链接
从零入门激光SLAM（二十一）——看不懂FAST-LIO？进来_fastlio 雷达 更改频率-CSDN博客
注释版代码完整版见
GitHub - huashu996/Fast-lio2-Supernote: Fast-lio2 code with note
本代码解析以算法流程的逻辑解析代码，一些简单的函数忽略讲解。

一、误差迭代更新

1.1 整体框架

迭代更新流程：输出修正结果、更新状态、发布里程计信息以及将特征点添加到地图

   /*** iterated state estimation ***///计算欧拉角并输出预测结果V3D ext_euler = SO3ToEuler(state_point.offset_R_L_I);fout_pre<<setw(20)<<Measures.lidar_beg_time - first_lidar_time<<" "<<euler_cur.transpose()<<" "<< state_point.pos.transpose()<<" "<<ext_euler.transpose() << " "<<state_point.offset_T_L_I.transpose()<< " " << state_point.vel.transpose() \<<" "<<state_point.bg.transpose()<<" "<<state_point.ba.transpose()<<" "<<state_point.grav<< endl; //输出预测的结果//记录更新开始时间double t_update_start = omp_get_wtime();//初始化观测矩阵协方差double solve_H_time = 0;//迭代卡尔曼滤波更新，更新地图信息kf.update_iterated_dyn_share_modified(LASER_POINT_COV, solve_H_time);state_point = kf.get_x();//获取更新后的状态值//转换欧拉角并计算激光雷达位置euler_cur = SO3ToEuler(state_point.rot);//转换欧拉角pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I;//更新四元数geoQuat.x = state_point.rot.coeffs()[0];geoQuat.y = state_point.rot.coeffs()[1];geoQuat.z = state_point.rot.coeffs()[2];geoQuat.w = state_point.rot.coeffs()[3];//记录更新结束时间double t_update_end = omp_get_wtime();/******* Publish odometry 发布里程计信息*******/publish_odometry(pubOdomAftMapped);/*** add the feature points to map kdtree将特征点添加到地图 KD 树 ***/t3 = omp_get_wtime();map_incremental();t5 = omp_get_wtime();/******* Publish points发布点云数据 *******/if (1)                         publish_path(pubPath);if (scan_pub_en || pcd_save_en)      publish_frame_world(pubLaserCloudFull);if (scan_pub_en && scan_body_pub_en) publish_frame_body(pubLaserCloudFull_body);

1.2 迭代更新

初始化动态共享数据结构，用于在迭代过程中共享数据和控制迭代过程。 初始化传播后的状态和协方差，以准备在后续的滤波步骤中使用。
定义并初始化卡尔曼增益矩阵，以用于后续的卡尔曼滤波计算。
初始化状态增量向量，用于存储新的状态增量。

  void update_iterated_dyn_share_modified(double R, double &solve_time) {1.初始化//动态共享数据结构的初始化：dyn_share_datastruct<scalar_type> dyn_share; // 定义一个名为 dyn_share 的动态共享数据结构 dyn_share.valid = true; //表示数据有效dyn_share.converge = true; //表示迭代过程已经收敛//变量初始化int t = 0; //用于记录迭代次数state x_propagated = x_; //初始化当前状态 x_，表示传播后的状态cov P_propagated = P_; //初始化当前的协方差 P_，表示传播后的协方差int dof_Measurement; //用于表示测量的自由度//卡尔曼增益矩阵的初始化Matrix<scalar_type, n, 1> K_h;Matrix<scalar_type, n, n> K_x; //定义 dx_new 并初始化为零向量，用于存储新的状态增量vectorized_state dx_new = vectorized_state::Zero();//2.迭代循环for(int i=-1; i<maximum_iter; i++){//共享数据结构的初始化和检查dyn_share.valid = true;    //观测模型h_dyn_share(x_, dyn_share);if(! dyn_share.valid){continue; }//获取观测矩阵 h_x_ 矩阵用于存储观测模型的雅可比矩阵//Matrix<scalar_type, Eigen::Dynamic, 1> h = h_dyn_share(x_, dyn_share);#ifdef USE_sparsespMt h_x_ = dyn_share.h_x.sparseView();#elseEigen::Matrix<scalar_type, Eigen::Dynamic, 12> h_x_ = dyn_share.h_x;#endif//记录时间起始点和测量自由度double solve_start = omp_get_wtime();dof_Measurement = h_x_.rows();//状态增量计算vectorized_state dx;//前状态 x_ 与传播状态 x_propagated 之间的增量 dxx_.boxminus(dx, x_propagated);dx_new = dx;//初始化协方差矩阵 P_ 为传播后的协方差 P_propagatedP_ = P_propagated;//SO3状态更新Matrix<scalar_type, 3, 3> res_temp_SO3;MTK::vect<3, scalar_type> seg_SO3;for (std::vector<std::pair<int, int> >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) {int idx = (*it).first;int dim = (*it).second;for(int i = 0; i < 3; i++){seg_SO3(i) = dx(idx+i);}res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0);for(int i = 0; i < n; i++){P_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i));    }for(int i = 0; i < n; i++){P_. template block<1, 3>(i, idx) =(P_. template block<1, 3>(i, idx)) *  res_temp_SO3.transpose();    }}//S2状态更新Matrix<scalar_type, 2, 2> res_temp_S2;MTK::vect<2, scalar_type> seg_S2;for (std::vector<std::pair<int, int> >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) {int idx = (*it).first;int dim = (*it).second;for(int i = 0; i < 2; i++){seg_S2(i) = dx(idx + i);}Eigen::Matrix<scalar_type, 2, 3> Nx;Eigen::Matrix<scalar_type, 3, 2> Mx;x_.S2_Nx_yy(Nx, idx);x_propagated.S2_Mx(Mx, seg_S2, idx);res_temp_S2 = Nx * Mx; dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0);for(int i = 0; i < n; i++){P_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i));    }for(int i = 0; i < n; i++){P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();}}//状态维数大于测量自由度时的计算卡尔曼增益if(n > dof_Measurement){//创建一个大小为dof_Measurement×n 的零矩阵h_x_curEigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x_cur = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Zero(dof_Measurement, n);//将 h_x_ 矩阵的前 12 列复制到 h_x_cur 中的左上角h_x_cur.topLeftCorner(dof_Measurement, 12) = h_x_;//计算卡尔曼增益矩阵KMatrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_ = P_ * h_x_cur.transpose() * (h_x_cur * P_ * h_x_cur.transpose()/R + Eigen::Matrix<double, Dynamic, Dynamic>::Identity(dof_Measurement, dof_Measurement)).inverse()/R;//计算增益矩阵 K_h 和 K_x//dyn_share.h 表示测量残差K_h = K_ * dyn_share.h;K_x = K_ * h_x_cur;}//状态维数小于测量自由度时的计算卡尔曼增益