0. 简介

上一节我们将while内部的IKD-Tree部分全部讲完,下面将是最后一部分,关于后端优化更新的部分。

1. 迭代更新

最后一块主要做的就是,拿当前帧与IKD-Tree建立的地图算出的残差,然后去计算更新自己的位置,并将更新后的结果通过map_incremental函数插入到由ikd-Tree表示的映射中。

// 外参,旋转矩阵转欧拉角
            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; //输出预测的结果

            if (0) // If you need to see map point, change to "if(1)"
            {
                // 释放PCL_Storage的内存
                PointVector().swap(ikdtree.PCL_Storage);
                // 把树展平用于展示
                ikdtree.flatten(ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD);
                featsFromMap->clear();
                featsFromMap->points = ikdtree.PCL_Storage;
            }

            pointSearchInd_surf.resize(feats_down_size); //搜索索引
            Nearest_Points.resize(feats_down_size);      //将降采样处理后的点云用于搜索最近点
            int rematch_num = 0;
            bool nearest_search_en = true; //

            t2 = omp_get_wtime();

            /*** 迭代状态估计 ***/
            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(pubOdomAftMapped);

            /*** 向映射kdtree添加特性点 ***/
            t3 = omp_get_wtime();
            map_incremental();
            t5 = omp_get_wtime();

            /******* 发布轨迹和点 *******/
            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);
                publish_frame_lidar(pubLaserCloudFull_lidar);
            }
            // publish_effect_world(pubLaserCloudEffect);
            // publish_map(pubLaserCloudMap);

            /*** Debug 参数 ***/
            if (runtime_pos_log)
            {
                frame_num++;
                kdtree_size_end = ikdtree.size();
                aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t5 - t0) / frame_num;
                aver_time_icp = aver_time_icp * (frame_num - 1) / frame_num + (t_update_end - t_update_start) / frame_num;
                aver_time_match = aver_time_match * (frame_num - 1) / frame_num + (match_time) / frame_num;
                aver_time_incre = aver_time_incre * (frame_num - 1) / frame_num + (kdtree_incremental_time) / frame_num;
                aver_time_solve = aver_time_solve * (frame_num - 1) / frame_num + (solve_time + solve_H_time) / frame_num;
                aver_time_const_H_time = aver_time_const_H_time * (frame_num - 1) / frame_num + solve_time / frame_num;
                T1[time_log_counter] = Measures.lidar_beg_time;
                s_plot[time_log_counter] = t5 - t0;                         //整个流程总时间
                s_plot2[time_log_counter] = feats_undistort->points.size(); //特征点数量
                s_plot3[time_log_counter] = kdtree_incremental_time;        // kdtree增量时间
                s_plot4[time_log_counter] = kdtree_search_time;             // kdtree搜索耗时
                s_plot5[time_log_counter] = kdtree_delete_counter;          // kdtree删除点数量
                s_plot6[time_log_counter] = kdtree_delete_time;             // kdtree删除耗时
                s_plot7[time_log_counter] = kdtree_size_st;                 // kdtree初始大小
                s_plot8[time_log_counter] = kdtree_size_end;                // kdtree结束大小
                s_plot9[time_log_counter] = aver_time_consu;                //平均消耗时间
                s_plot10[time_log_counter] = add_point_size;                //添加点数量
                time_log_counter++;
                printf("[ mapping ]: time: IMU + Map + Input Downsample: %0.6f ave match: %0.6f ave solve: %0.6f  ave ICP: %0.6f  map incre: %0.6f ave total: %0.6f icp: %0.6f construct H: %0.6f \n", t1 - t0, aver_time_match, aver_time_solve, t3 - t1, t5 - t3, aver_time_consu, aver_time_icp, aver_time_const_H_time);
                ext_euler = SO3ToEuler(state_point.offset_R_L_I);
                fout_out << 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 << " " << feats_undistort->points.size() << endl;
                dump_lio_state_to_log(fp);
            }
        }

        status = ros::ok();
        rate.sleep();
    }

    /**************** save map ****************/
    /* 1. make sure you have enough memories
    /* 2. pcd save will largely influence the real-time performences **/
    if (pcl_wait_save->size() > 0 && pcd_save_en)
    {
        string file_name = string("scans.pcd");
        string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name);
        pcl::PCDWriter pcd_writer;
        cout << "current scan saved to /PCD/" << file_name << endl;
        pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);
    }

    fout_out.close();
    fout_pre.close();

    if (runtime_pos_log)
    {
        vector<double> t, s_vec, s_vec2, s_vec3, s_vec4, s_vec5, s_vec6, s_vec7;
        FILE *fp2;
        string log_dir = root_dir + "/Log/fast_lio_time_log.csv";
        fp2 = fopen(log_dir.c_str(), "w");
        fprintf(fp2, "time_stamp, total time, scan point size, incremental time, search time, delete size, delete time, tree size st, tree size end, add point size, preprocess time\n");
        for (int i = 0; i < time_log_counter; i++)
        {
            fprintf(fp2, "%0.8f,%0.8f,%d,%0.8f,%0.8f,%d,%0.8f,%d,%d,%d,%0.8f\n", T1[i], s_plot[i], int(s_plot2[i]), s_plot3[i], s_plot4[i], int(s_plot5[i]), s_plot6[i], int(s_plot7[i]), int(s_plot8[i]), int(s_plot10[i]), s_plot11[i]);
            t.push_back(T1[i]);
            s_vec.push_back(s_plot9[i]);
            s_vec2.push_back(s_plot3[i] + s_plot6[i]);
            s_vec3.push_back(s_plot4[i]);
            s_vec5.push_back(s_plot[i]);
        }
        fclose(fp2);
    }

    return 0;
}

前面获取的传播状态\hat{x_k}和协方差\hat{P_k}对未知状态x_k施加了一个先验高斯分布。具体来说,\hat{P_k}表示以下误差状态的协方差:
在这里插入图片描述
式中,Jκ为( \widehat {x}_ {k}^ {\pi } 田 \widehat {x}_ {k}^ {\kappa } )日 \widehat {x} k\hat{x}^k_k=0处的偏微分。
在这里插入图片描述
式中,A(.)^{-1} 定义于公式(6),如下。
在这里插入图片描述
如下的δ^Gθ_{I_k}δ^Iθ{L_k}分别为IMU的姿态和转动的误差状态。

在这里插入图片描述
对于第一次迭代(以拓展的卡尔曼滤波器为例),有 \hat{x^κ_k} =x_k, J_κ =I

除了先验分布外,我们也有一个源于测量(8)的状态分布:
在这里插入图片描述
结合(10)的先验分布和(12)的测量模型,可得到状态x_k的后验分布,其等价表示为x^k_k及其最大后验估计:
在这里插入图片描述
式中,有||x||^2_M = x^TM^{−1} x。该最大后验估计问题可由下面的迭代卡尔曼滤波器解决:
在这里插入图片描述
需注意,卡尔曼增益K计算需要对状态维数矩阵求逆,而不是在之前的工作中使用的测量维数矩阵。上述过程将重复进行,直到收敛(即||x_κ^{(k+1)} 日 x^κ_k||<无穷小)。收敛后的最优状态和协方差估计为:
在这里插入图片描述
通过状态更新x_k,第k次扫描中的每个LiDAR点(Lpj)将通过(16)被转换到全局框架:
在这里插入图片描述
转换后的LiDAR点{Gp¯j}将被插入到由ikd-Tree表示的映射中。我们的状态估计在算法1中进行了总结。

在这里插入图片描述

        // 迭代错误状态EKF更新修改为一个特定的系统
        void update_iterated_dyn_share_modified(double R, double &solve_time)
        {

            dyn_share_datastruct<scalar_type> dyn_share;
            dyn_share.valid = true;
            dyn_share.converge = true;
            int t = 0;
            //获取上一次的状态和协方差矩阵
            state x_propagated = x_;
            cov P_propagated = P_;
            int dof_Measurement;

            Matrix<scalar_type, n, 1> K_h;
            Matrix<scalar_type, n, n> K_x;

            vectorized_state dx_new = vectorized_state::Zero();
            // 最多进行maximum_iter次迭代优化
            for (int i = -1; i < maximum_iter; i++)
            {
                dyn_share.valid = true;
                // 计算测量模型方程的雅克比,也就是点面残差的导数 H(代码里是h_x)
                h_dyn_share(x_, dyn_share);
                // 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
                // 获取测量模型的雅克比d(pos, rot, 0, 0)/dx
                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(); // 观测方程个数m
                vectorized_state dx;           // 定义误差状态
                x_.boxminus(dx, x_propagated); //获取误差dx
                dx_new = dx;                   // 用于迭代的误差状态

                if (!dyn_share.valid)
                {
                    continue;
                }
                // 预测得到的误差状态协方差矩阵
                //协方差矩阵在迭代过程中不会代入下一次迭代,直到最后一次退出时更新,在迭代过程中更新的只是先验
                P_ = P_propagated;
                // 这一大段都在求协方差的先验更新,大致上是P=(J^-1)*P*(J^-T)如论文式16~18
                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();
                    }
                }

                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();
                    }
                }
                // Matrix<scalar_type, n, Eigen::Dynamic> K_;
                // Matrix<scalar_type, n, 1> K_h;
                // Matrix<scalar_type, n, n> K_x;

                /*
                if(n > dof_Measurement)
                {
                    K_= P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose()/R + Eigen::Matrix<double, Dynamic, Dynamic>::Identity(dof_Measurement, dof_Measurement)).inverse()/R;
                }
                else
                {
                    K_= (h_x_.transpose() * h_x_ + (P_/R).inverse()).inverse()*h_x_.transpose();
                }
                */
                // 状态维度 n > 测量维度 dof_Measurement
                // 如果状态量维度大于观测方程 n > m,不满秩
                if (n > dof_Measurement)
                {
                    //#ifdef USE_sparse
                    // Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_temp = h_x * P_ * h_x.transpose();
                    // spMt R_temp = h_v * R_ * h_v.transpose();
                    // K_temp += R_temp;
                    Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x_cur = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Zero(dof_Measurement, n);
                    //每一次迭代将重新计算增益K,即论文式18
                    h_x_cur.topLeftCorner(dof_Measurement, 12) = h_x_;
                    /*
                    h_x_cur.col(0) = h_x_.col(0);
                    h_x_cur.col(1) = h_x_.col(1);
                    h_x_cur.col(2) = h_x_.col(2);
                    h_x_cur.col(3) = h_x_.col(3);
                    h_x_cur.col(4) = h_x_.col(4);
                    h_x_cur.col(5) = h_x_.col(5);
                    h_x_cur.col(6) = h_x_.col(6);
                    h_x_cur.col(7) = h_x_.col(7);
                    h_x_cur.col(8) = h_x_.col(8);
                    h_x_cur.col(9) = h_x_.col(9);
                    h_x_cur.col(10) = h_x_.col(10);
                    h_x_cur.col(11) = h_x_.col(11);
                    */
                    // 重新计算增益矩阵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_ * dyn_share.h;
                    K_x = K_ * h_x_cur;
                    //#else
                    //    K_= P_ * h_x.transpose() * (h_x * P_ * h_x.transpose() + h_v * R * h_v.transpose()).inverse();
                    //#endif
                }
                else
                {
                    //避免求逆矩阵,K按稀疏矩阵分解的方法如论文式20
#ifdef USE_sparse
                    // Eigen::Matrix<scalar_type, n, n> b = Eigen::Matrix<scalar_type, n, n>::Identity();
                    // Eigen::SparseQR<Eigen::SparseMatrix<scalar_type>, Eigen::COLAMDOrdering<int>> solver;
                    spMt A = h_x_.transpose() * h_x_;
                    cov P_temp = (P_ / R).inverse();
                    P_temp.template block<12, 12>(0, 0) += A;
                    P_temp = P_temp.inverse();
                    /*
                    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_cur.col(0) = h_x_.col(0);
                    h_x_cur.col(1) = h_x_.col(1);
                    h_x_cur.col(2) = h_x_.col(2);
                    h_x_cur.col(3) = h_x_.col(3);
                    h_x_cur.col(4) = h_x_.col(4);
                    h_x_cur.col(5) = h_x_.col(5);
                    h_x_cur.col(6) = h_x_.col(6);
                    h_x_cur.col(7) = h_x_.col(7);
                    h_x_cur.col(8) = h_x_.col(8);
                    h_x_cur.col(9) = h_x_.col(9);
                    h_x_cur.col(10) = h_x_.col(10);
                    h_x_cur.col(11) = h_x_.col(11);
                    */
                    K_ = P_temp.template block<n, 12>(0, 0) * h_x_.transpose();
                    K_x = cov::Zero();
                    K_x.template block<n, 12>(0, 0) = P_inv.template block<n, 12>(0, 0) * HTH;
                    /*
                    solver.compute(R_);
                    Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R_in_temp = solver.solve(b);
                    spMt R_in =R_in_temp.sparseView();
                    spMt K_temp = h_x.transpose() * R_in * h_x;
                    cov P_temp = P_.inverse();
                    P_temp += K_temp;
                    K_ = P_temp.inverse() * h_x.transpose() * R_in;
                    */
#else
                    cov P_temp = (P_ / R).inverse();
                    // Eigen::Matrix<scalar_type, 12, Eigen::Dynamic> h_T = h_x_.transpose();
                    Eigen::Matrix<scalar_type, 12, 12> HTH = h_x_.transpose() * h_x_;
                    P_temp.template block<12, 12>(0, 0) += HTH;
                    /*
                    Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x_cur = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Zero(dof_Measurement, n);
                    //std::cout << "line 1767" << std::endl;
                    h_x_cur.col(0) = h_x_.col(0);
                    h_x_cur.col(1) = h_x_.col(1);
                    h_x_cur.col(2) = h_x_.col(2);
                    h_x_cur.col(3) = h_x_.col(3);
                    h_x_cur.col(4) = h_x_.col(4);
                    h_x_cur.col(5) = h_x_.col(5);
                    h_x_cur.col(6) = h_x_.col(6);
                    h_x_cur.col(7) = h_x_.col(7);
                    h_x_cur.col(8) = h_x_.col(8);
                    h_x_cur.col(9) = h_x_.col(9);
                    h_x_cur.col(10) = h_x_.col(10);
                    h_x_cur.col(11) = h_x_.col(11);
                    */
                    cov P_inv = P_temp.inverse();
                    // std::cout << "line 1781" << std::endl;
                    K_h = P_inv.template block<n, 12>(0, 0) * h_x_.transpose() * dyn_share.h; // (H_T_H + P^-1)^-1 * H^T * h(残差) = K * h
                    // std::cout << "line 1780" << std::endl;
                    // cov HTH_cur = cov::Zero();
                    // HTH_cur. template block<12, 12>(0, 0) = HTH;
                    K_x.setZero(); // = cov::Zero();

                    K_x.template block<n, 12>(0, 0) = P_inv.template block<n, 12>(0, 0) * HTH; //(H_T_H + P^-1)^-1 * H_T_H = KH

                    // K_= (h_x_.transpose() * h_x_ + (P_/R).inverse()).inverse()*h_x_.transpose();
#endif
                }

                // K_x = K_ * h_x_;
                //由于是误差迭代KF,得到的是误差的最优估计!
                Matrix<scalar_type, n, 1> dx_ = K_h + (K_x - Matrix<scalar_type, n, n>::Identity()) * dx_new; //误差增量后验 K*h + (K*H - I) dx

                state x_before = x_; // 加上校正后的误差状态dx_
                x_.boxplus(dx_);     //根据计算得到的误差增量后验,更新状态量
                // 判断迭代是否发散
                dyn_share.converge = true;
                //判断已收敛的条件是误差的估计值小于阈值
                for (int i = 0; i < n; i++)
                {
                    if (std::fabs(dx_[i]) > limit[i])
                    {
                        dyn_share.converge = false;
                        break;
                    }
                }
                if (dyn_share.converge)
                    t++;

                if (!t && i == maximum_iter - 2)
                {
                    dyn_share.converge = true;
                }
                // 迭代完成后更新误差状态协方差矩阵
                //结束迭代后,更新协方差矩阵的后验值,大致上是P=(I-K*H)*P,如论文式19
                if (t > 1 || i == maximum_iter - 1)
                {
                    L_ = P_;
                    // std::cout << "iteration time" << t << "," << i << std::endl;
                    Matrix<scalar_type, 3, 3> res_temp_SO3;
                    MTK::vect<3, scalar_type> seg_SO3;
                    for (typename std::vector<std::pair<int, int>>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)
                    {
                        int idx = (*it).first;
                        for (int i = 0; i < 3; i++)
                        {
                            seg_SO3(i) = dx_(i + idx);
                        }
                        res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
                        for (int i = 0; i < n; i++)
                        {
                            L_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));
                        }
                        // if(n > dof_Measurement)
                        // {
                        //     for(int i = 0; i < dof_Measurement; i++){
                        //         K_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i));
                        //     }
                        // }
                        // else
                        // {
                        for (int i = 0; i < 12; i++)
                        {
                            K_x.template block<3, 1>(idx, i) = res_temp_SO3 * (K_x.template block<3, 1>(idx, i));
                        }
                        //}
                        for (int i = 0; i < n; i++)
                        {
                            L_.template block<1, 3>(i, idx) = (L_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
                            P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
                        }
                    }

                    Matrix<scalar_type, 2, 2> res_temp_S2;
                    MTK::vect<2, scalar_type> seg_S2;
                    for (typename std::vector<std::pair<int, int>>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)
                    {
                        int idx = (*it).first;

                        for (int i = 0; i < 2; i++)
                        {
                            seg_S2(i) = dx_(i + idx);
                        }

                        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;
                        for (int i = 0; i < n; i++)
                        {
                            L_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));
                        }
                        // if(n > dof_Measurement)
                        // {
                        //     for(int i = 0; i < dof_Measurement; i++){
                        //         K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i));
                        //     }
                        // }
                        // else
                        // {
                        for (int i = 0; i < 12; i++)
                        {
                            K_x.template block<2, 1>(idx, i) = res_temp_S2 * (K_x.template block<2, 1>(idx, i));
                        }
                        //}
                        for (int i = 0; i < n; i++)
                        {
                            L_.template block<1, 2>(i, idx) = (L_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
                            P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
                        }
                    }

                    // if(n > dof_Measurement)
                    // {
                    //     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_cur.topLeftCorner(dof_Measurement, 12) = h_x_;
                    //     /*
                    //     h_x_cur.col(0) = h_x_.col(0);
                    //     h_x_cur.col(1) = h_x_.col(1);
                    //     h_x_cur.col(2) = h_x_.col(2);
                    //     h_x_cur.col(3) = h_x_.col(3);
                    //     h_x_cur.col(4) = h_x_.col(4);
                    //     h_x_cur.col(5) = h_x_.col(5);
                    //     h_x_cur.col(6) = h_x_.col(6);
                    //     h_x_cur.col(7) = h_x_.col(7);
                    //     h_x_cur.col(8) = h_x_.col(8);
                    //     h_x_cur.col(9) = h_x_.col(9);
                    //     h_x_cur.col(10) = h_x_.col(10);
                    //     h_x_cur.col(11) = h_x_.col(11);
                    //     */
                    //     P_ = L_ - K_*h_x_cur * P_;
                    // }
                    // else
                    //{
                    P_ = L_ - K_x.template block<n, 12>(0, 0) * P_.template block<12, n>(0, 0);
                    //}
                    solve_time += omp_get_wtime() - solve_start;
                    return;
                }
                solve_time += omp_get_wtime() - solve_start;
            }
        }

2. 参考链接

https://blog.csdn.net/MWY123_/article/details/124110021
https://zhuanlan.zhihu.com/p/471876531
https://zhuanlan.zhihu.com/p/471876061
https://zhuanlan.zhihu.com/p/476286576