从零入门激光SLAM（二十）——IESKF代码实现

本文主要是介绍从零入门激光SLAM（二十）——IESKF代码实现，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

大家好呀，我是一个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;  // 是否更新biasbool update_bias_acce_ = true;  // 是否更新bias};// nominal stateSO3 R_;VecT p_ = VecT::Zero();VecT v_ = VecT::Zero();VecT bg_ = VecT::Zero();VecT ba_ = VecT::Zero();VecT g_{0, 0, -9.8};// error stateVec18T dx_ = Vec18T::Zero();// covarianceMat18T cov_ = Mat18T::Identity();// noiseMotionNoiseT 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 QQ_.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);// 投影协方差矩阵PMat18T 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();   // 计算更新后的协方差矩阵Qkdx_ = Qk * HTVr; // 计算状态增量dx_// LOG(INFO) << "iter " << iter << " dx = " << dx_.transpose() << ", dxn: " << dx_.norm();//将增量dx_合并到名义变量中Update();// 检查增量的范数是否小于终止阈值if (dx_.norm() < options_.quit_eps_) {break;}}// 更新协方差矩阵update Pcov_ = (Mat18T::Identity() - Qk * HTVH) * Pk;// 再次投影协方差矩阵PMat18T 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;
  }

详情请见...

 从零入门激光SLAM（二十）——IESKF代码解释 - 古月居 (guyuehome.com)

这篇关于从零入门激光SLAM（二十）——IESKF代码实现的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---