大家好呀，我是一个SLAM方向的在读博士，深知SLAM学习过程一路走来的坎坷，也十分感谢各位大佬的优质文章和源码。随着知识的越来越多，越来越细，我准备整理一个自己的激光SLAM学习笔记专栏，从0带大家快速上手激光SLAM，也方便想入门SLAM的同学和小白学习参考，相信看完会有一定的收获。如有不对的地方欢迎指出，欢迎各位大佬交流讨论，一起进步。博主创建了一个科研互助群Q：772356582，欢迎大家加入讨论。
源码是高博大佬的，网址如下
slam_in_autonomous_driving/src/ch8 at master · gaoxiang12/slam_in_autonomous_driving · GitHub
下面对代码的逻辑和主要函数进行解析
一、IESKF结构
参数配置/设置状态变量
struct Options { Options() = default; /// IEKF配置 int num_iterations_ = 3; // 迭代次数 double quit_eps_ = 1e-3; // 终止迭代的dx大小 /// IMU 测量与零偏参数 double imu_dt_ = 0.01; // IMU测量间隔 double gyro_var_ = 1e-5; // 陀螺测量标准差 double acce_var_ = 1e-2; // 加计测量标准差 double bias_gyro_var_ = 1e-6; // 陀螺零偏游走标准差 double bias_acce_var_ = 1e-4; // 加计零偏游走标准差 /// RTK 观测参数 double gnss_pos_noise_ = 0.1; // GNSS位置噪声 double gnss_height_noise_ = 0.1; // GNSS高度噪声 double gnss_ang_noise_ = 1.0 * math::kDEG2RAD; // GNSS旋转噪声 /// 其他配置 bool update_bias_gyro_ = true; // 是否更新bias bool update_bias_acce_ = true; // 是否更新bias}; // nominal state SO3 R_; VecT p_ = VecT::Zero(); VecT v_ = VecT::Zero(); VecT bg_ = VecT::Zero(); VecT ba_ = VecT::Zero(); VecT g_{0, 0, -9.8}; // error state Vec18T dx_ = Vec18T::Zero(); // covariance Mat18T cov_ = Mat18T::Identity(); // noise MotionNoiseT Q_ = MotionNoiseT::Zero(); GnssNoiseT gnss_noise_ = GnssNoiseT::Zero(); Options options_;- 设置初始条件
void SetInitialConditions(Options options, const VecT& init_bg, const VecT& init_ba, const VecT& gravity = VecT(0, 0, -9.8)) { BuildNoise(options); options_ = options; bg_ = init_bg; ba_ = init_ba; g_ = gravity; cov_ = 1e-4 * Mat18T::Identity(); cov_.template block<3, 3>(6, 6) = 0.1 * math::kDEG2RAD * Mat3T::Identity(); } //构建噪声模型 void BuildNoise(const Options& options) { double ev = options.acce_var_; double et = options.gyro_var_; double eg = options.bias_gyro_var_; double ea = options.bias_acce_var_; double ev2 = ev; // * ev; double et2 = et; // * et; double eg2 = eg; // * eg; double ea2 = ea; // * ea; // set Q Q_.diagonal() << 0, 0, 0, ev2, ev2, ev2, et2, et2, et2, eg2, eg2, eg2, ea2, ea2, ea2, 0, 0, 0; double gp2 = options.gnss_pos_noise_ * options.gnss_pos_noise_; double gh2 = options.gnss_height_noise_ * options.gnss_height_noise_; double ga2 = options.gnss_ang_noise_ * options.gnss_ang_noise_; gnss_noise_.diagonal() << gp2, gp2, gh2, ga2, ga2, ga2; } 使用IMU预测
bool IESKF<S>::Predict(const IMU& imu) { /// Predict 部分与ESKF完全一样，不再解释 assert(imu.timestamp_ >= current_time_); double dt = imu.timestamp_ - current_time_; if (dt > (5 * options_.imu_dt_) || dt < 0) { LOG(INFO) << "skip this imu because dt_ = " << dt; current_time_ = imu.timestamp_; return false; } VecT new_p = p_ + v_ * dt + 0.5 * (R_ * (imu.acce_ - ba_)) * dt * dt + 0.5 * g_ * dt * dt; VecT new_v = v_ + R_ * (imu.acce_ - ba_) * dt + g_ * dt; SO3 new_R = R_ * SO3::exp((imu.gyro_ - bg_) * dt); R_ = new_R; v_ = new_v; p_ = new_p; Mat18T F = Mat18T::Identity(); F.template block<3, 3>(0, 3) = Mat3T::Identity() * dt; F.template block<3, 3>(3, 6) = -R_.matrix() * SO3::hat(imu.acce_ - ba_) * dt; F.template block<3, 3>(3, 12) = -R_.matrix() * dt; F.template block<3, 3>(3, 15) = Mat3T::Identity() * dt; F.template block<3, 3>(6, 6) = SO3::exp(-(imu.gyro_ - bg_) * dt).matrix(); F.template block<3, 3>(6, 9) = -Mat3T::Identity() * dt; cov_ = F * cov_ * F.transpose() + Q_; current_time_ = imu.timestamp_; return true; }迭代观测模型
using CustomObsFunc = std::function<void(const SE3& input_pose, Eigen::Matrix<S, 18, 18>& HT_Vinv_H, Eigen::Matrix<S, 18, 1>& HT_Vinv_r)>; // 使用自定义观测函数更新滤波器 bool IESKF<S>::UpdateUsingCustomObserve(IESKF::CustomObsFunc obs) { // H阵由用户给定 // 保存当前的旋转矩阵 SO3 start_R = R_; Eigen::Matrix<S, 18, 1> HTVr; Eigen::Matrix<S, 18, 18> HTVH; Eigen::Matrix<S, 18, Eigen::Dynamic> K; Mat18T Pk, Qk; //进入残差更新循环 for (int iter = 0; iter < options_.num_iterations_; ++iter) { // 调用用户提供的观测函数 //GetNominalSE3()获取当前名义状态位姿 //SE3 GetNominalSE3() const { return SE3(R_, p_); } //HTVH卡尔曼更新步骤中用于修正预测的协方差矩阵 //HTVr卡尔曼更新步骤中用于修正预测的状态变量 obs(GetNominalSE3(), HTVH, HTVr); // 投影协方差矩阵P Mat18T J = Mat18T::Identity(); J.template block<3, 3>(6, 6) = Mat3T::Identity() - 0.5 * SO3::hat((R_.inverse() * start_R).log()); Pk = J * cov_ * J.transpose(); // 卡尔曼更新 Qk = (Pk.inverse() + HTVH).inverse(); // 计算更新后的协方差矩阵Qk dx_ = Qk * HTVr; // 计算状态增量dx_ // LOG(INFO) << "iter " << iter << " dx = " << dx_.transpose() << ", dxn: " << dx_.norm(); //将增量dx_合并到名义变量中 Update(); // 检查增量的范数是否小于终止阈值 if (dx_.norm() < options_.quit_eps_) { break; } } // 更新协方差矩阵update P cov_ = (Mat18T::Identity() - Qk * HTVH) * Pk; // 再次投影协方差矩阵P Mat18T J = Mat18T::Identity(); Vec3d dtheta = (R_.inverse() * start_R).log(); J.template block<3, 3>(6, 6) = Mat3T::Identity() - 0.5 * SO3::hat(dtheta); cov_ = J * cov_ * J.inverse(); // 重置状态增量 dx_.setZero(); return true; }
详情请见...
