大家好呀，我是一個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)
人臉識別系統)
