从零入门激光SLAM（二十二）——Fast-lio2代码详解（三） 迭代误差更新

大家好呀，我是一个SLAM方向的在读博士，深知SLAM学习过程一路走来的坎坷，也十分感谢各位大佬的优质文章和源码。随着知识的越来越多，越来越细，我准备整理一个自己的激光SLAM学习笔记专栏，从0带大家快速上手激光SLAM，也方便想入门SLAM的同学和小白学习参考，相信看完会有一定的收获。如有不对的地方欢迎指出，欢迎各位大佬交流讨论，一起进步。博主创建了一个科研互助群Q：951026257，欢迎大家加入讨论。

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_sparse
                spMt h_x_ = dyn_share.h_x.sparseView();
            #else
                Eigen::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 之间的增量 dx
            x_.boxminus(dx, x_propagated);
            dx_new = dx;
            //初始化协方差矩阵 P_ 为传播后的协方差 P_propagated
            P_ = 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_cur
                Eigen::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_;
                //计算卡尔曼增益矩阵K
                Matrix<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;
            }
            //状态维数小于测量自由度时的计算卡尔曼增益

详情请见。。。

从零入门激光SLAM（二十二）——Fast-lio2代码详解（三） 迭代误差更新 - 古月居 (guyuehome.com)