fast-lio2添加wheel在发散时轨迹约束

fast-lio2主要算法为迭代误差卡尔曼滤波算法，imu作为预测，点云更新，当点云发散时输出位姿异常，漂移很大，后端在融合出现崩溃情况，加入轮速计约束发散时位姿。

1.订阅wheel话题

2.发散检测，检测到发散将点云协方差设置最大。

3.打滑检测，打滑的时候，限制轮速计的协方差。

4.收敛时，增加轮子的协方差，轮子的观测值误差较大会影响融合结果。

fast-lio2中状态量为pos rot offsetR offsetT vel bg ba g，速度的观测量为x方向，imu坐标系为右前上，将轮速计转化到imu坐标系为y方向，后轮驱动，imu在前方为，Imu的实际速度与车的长度有关。

   if (use_wheel_ && (v_odom.size() > 0)) {
            try {
                auto kf_cov_ = kf_state.get_P();
                Eigen::Matrix<double, 3, 23> H_WHEEL = Eigen::Matrix<double, 3, 23>::Zero();
                H_WHEEL.template block<3, 3>(0, 12) = Eigen::Matrix<double, 3, 3>::Identity(); //p rot R_l_i T_l_i vel bg ba g
                Eigen::Matrix<double, 3, 3> odom_noise_ = Eigen::Matrix<double, 3, 3>::Zero();
                if (flag_degen_)
                    if (slip_cov_[1] < 1)
                        odom_noise_.diagonal() << cov_wheel_, cov_wheel_, cov_wheel_;
                    else
                        odom_noise_.diagonal() << 3.0 * cov_wheel_, 3.0 * cov_wheel_, 3.0 * cov_wheel_;
                else
                    odom_noise_.diagonal() << 10.0 * cov_wheel_, 10.0 * cov_wheel_, 10.0 * cov_wheel_;
                Eigen::Matrix<double, 23, 3> K_WHEEL = kf_cov_ * H_WHEEL.transpose() * (H_WHEEL * kf_cov_ * H_WHEEL.transpose() + odom_noise_).inverse();
                Eigen::Matrix<double, 3, 1> vel_odom(0, sqrt(pow(v_odom[wheel_num]->twist.linear.x, 2) + pow(angvel_last[2] * Wheel_T_wrt_IMU[1], 2)), 0.0); // + pow(angvel_last[3] * 0.8164, 2)
                Eigen::Matrix<double, 3, 1> vel_world = imu_state.rot * vel_odom;
                Eigen::Matrix<double, 23, 1> dx_ = K_WHEEL * (vel_world - imu_state.vel);
                //std::cerr << "dx_" << dx_ << std::endl;
                kf_cov_ = (Eigen::Matrix<double, 23, 23>::Identity() - K_WHEEL * H_WHEEL) * kf_cov_;
                // update cov
                kf_state.change_P(kf_cov_);
                imu_state.pos += dx_.template block<3, 1>(0, 0);
                imu_state.vel += dx_.template block<3, 1>(12, 0);
                kf_state.change_x(imu_state);
                //R_ = R_ * SO3::exp(dx_.template block<3, 1>(6, 0));
                //Eigen::Matrix<double, 23, 23> J = Eigen::Matrix<double, 23, 23>::Identity();
                //J.template block<3, 3>(6, 6) = Eigen::Matrix<double, 3, 3>::Identity() - 0.5 * SO3::hat(dx_.template block<3, 1>(6, 0));
                //kf_state.change_P(J * kf_state.get_P() * J.transpose());
                wheel_slip_vel_.push_back(v_odom[wheel_num]->twist.linear.x); //0-x,1-y,2-z
                wheel_slip_gyr_.push_back(v_odom[wheel_num]->twist.angular.z);
            } catch (exception &e) {
                std::cerr << "Standard exception: " << e.what() << std::endl;
            }
        }

 左边为未添加轮速计的轨迹，中间为实际轨迹，右侧为添加轮子后的轨迹。发散后由于imu精度低存在角度评估不准的情况。持续优化中。