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

最新推荐文章于 2024-06-19 14:25:46 发布

桦树无泪

最新推荐文章于 2024-06-19 14:25:46 发布

阅读量534

点赞数 11

文章标签： SLAM c++ 自动驾驶算法

本文链接：https://blog.csdn.net/HUASHUDEYANJING/article/details/139181119

版权

大家好呀，我是一个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;
}

详情请见...

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

桦树无泪

关注

11
点赞
踩
21

收藏

觉得还不错? 一键收藏
打赏
0
评论
从零入门激光SLAM（二十）——IESKF代码实现

随着知识的越来越多，越来越细，我准备整理一个自己的激光SLAM学习笔记专栏，从0带大家快速上手激光SLAM，也方便想入门SLAM的同学和小白学习参考，相信看完会有一定的收获。如有不对的地方欢迎指出，欢迎各位大佬交流讨论，一起进步。博主创建了一个科研互助群，欢迎大家加入讨论。源码是高博大佬的，网址如下下面对代码的逻辑和主要函数进行解析。
复制链接

扫一扫