深蓝学院高翔《自动驾驶与机器人中的SLAM技术》学习记录2 ESKF

深蓝学院高翔《自动驾驶与机器人中的SLAM技术》学习记录2 ESKF"/>

深蓝学院高翔《自动驾驶与机器人中的SLAM技术》学习记录2 ESKF

ESKF

IMU 进行递推
RTK / Odom 进行误差修正
预测 -> RTK/Odom修正误差

    io.SetIMUProcessFunc([&](const sad::IMU& imu) {/// IMU 处理函数if (!imu_init.InitSuccess()) {imu_init.AddIMU(imu);return;}/// 需要IMU初始化if (!imu_inited) {// 读取初始零偏，设置ESKFsad::ESKFD::Options options;// 噪声由初始化器估计options.gyro_var_ = sqrt(imu_init.GetCovGyro()[0]);options.acce_var_ = sqrt(imu_init.GetCovAcce()[0]);eskf.SetInitialConditions(options, imu_init.GetInitBg(), imu_init.GetInitBa(), imu_init.GetGravity());imu_inited = true;return;}if (!gnss_inited) {/// 等待有效的RTK数据return;}/// GNSS 也接收到之后，再开始进行预测eskf.Predict(imu);/// predict就会更新ESKF，所以此时就可以发送数据auto state = eskf.GetNominalState();if (ui) {ui->UpdateNavState(state);}/// 记录数据以供绘图save_result(fout, state);usleep(1e3);}).SetGNSSProcessFunc([&](const sad::GNSS& gnss) {/// GNSS 处理函数if (!imu_inited) {return;}sad::GNSS gnss_convert = gnss;// 方位角无效，不使用，返回if (!sad::ConvertGps2UTM(gnss_convert, antenna_pos, FLAGS_antenna_angle) || !gnss_convert.heading_valid_) {return;}/// 去掉原点if (!first_gnss_set) {origin = gnss_convert.utm_pose_.translation();first_gnss_set = true;}gnss_convert.utm_pose_.translation() -= origin;// 要求RTK heading有效，才能合入ESKFeskf.ObserveGps(gnss_convert);auto state = eskf.GetNominalState();if (ui) {ui->UpdateNavState(state);}save_result(fout, state);gnss_inited = true;}).SetOdomProcessFunc([&](const sad::Odom& odom) {/// Odom 处理函数，本章Odom只给初始化使用imu_init.AddOdom(odom);if (FLAGS_with_odom && imu_inited && gnss_inited) {eskf.ObserveWheelSpeed(odom);}}).Go();

• 首先设定三个传感器数据的回调函数，返回值为TxtIO对象的引用，进行链式调用，一次性定义三个回调函数
• 通过Go()函数对文本文件数据进行遍历，遍历到哪一行哪种类型的数据便对其赋值，并调用回调函数进行处理
• 本次数据第一条均为Odom数据 时间 左轮 右轮
• 如果FLAGS_with_odom设置为true，即为开启轮速修正
• 初始时刻IMU并未初始化，init_success_ = false

bool StaticIMUInit::AddOdom(const Odom& odom) {// 判断车辆是否静止if (init_success_) {return true;}if (odom.left_pulse_ < options_.static_odom_pulse_ && odom.right_pulse_ < options_.static_odom_pulse_) {is_static_ = true;} else {is_static_ = false;}current_time_ = odom.timestamp_;return true;
}

• 返回车辆是否静止的判断 is_static_ = true
• 当读取到IMU数据行时首先对IMU进行初始化,初始化时间要超过设定值 init_time_seconds_ = 10.0 意为车辆静止10s后开始初始化。使用书上对IMU的初始化方法进行tryinit()

bool StaticIMUInit::AddIMU(const IMU& imu) {if (init_success_) {return true;}if (options_.use_speed_for_static_checking_ && !is_static_) {LOG(WARNING) << "等待车辆静止";init_imu_deque_.clear();return false;}if (init_imu_deque_.empty()) {// 记录初始静止时间init_start_time_ = imu.timestamp_;}// 记入初始化队列init_imu_deque_.push_back(imu);double init_time = imu.timestamp_ - init_start_time_;  // 初始化经过时间// 初始化时间要超过设定值 init_time_seconds_ = 10.0 意为车辆静止10s后开始初始化if (init_time > options_.init_time_seconds_) {// 尝试初始化逻辑TryInit();}// 维持初始化队列长度while (init_imu_deque_.size() > options_.init_imu_queue_max_size_) {init_imu_deque_.pop_front();}current_time_ = imu.timestamp_;return false;
}bool StaticIMUInit::TryInit() {// 初始化用的数据不够  双端队列容器if (init_imu_deque_.size() < 10) {return false;}// 计算均值和方差Vec3d mean_gyro, mean_acce;math::ComputeMeanAndCovDiag(init_imu_deque_, mean_gyro, cov_gyro_, [](const IMU& imu) { return imu.gyro_; });math::ComputeMeanAndCovDiag(init_imu_deque_, mean_acce, cov_acce_, [this](const IMU& imu) { return imu.acce_; });// 以acce均值为方向，取9.8长度为重力  norm()求模长LOG(INFO) << "mean acce: " << mean_acce.transpose();gravity_ = -mean_acce / mean_acce.norm() * options_.gravity_norm_;// 重新计算加计的协方差math::ComputeMeanAndCovDiag(init_imu_deque_, mean_acce, cov_acce_,[this](const IMU& imu) { return imu.acce_ + gravity_; });// 检查IMU噪声if (cov_gyro_.norm() > options_.max_static_gyro_var) {LOG(ERROR) << "陀螺仪测量噪声太大" << cov_gyro_.norm() << " > " << options_.max_static_gyro_var;return false;}if (cov_acce_.norm() > options_.max_static_acce_var) {LOG(ERROR) << "加计测量噪声太大" << cov_acce_.norm() << " > " << options_.max_static_acce_var;return false;}// 估计测量噪声和零偏init_bg_ = mean_gyro;init_ba_ = mean_acce;LOG(INFO) << "IMU 初始化成功，初始化时间= " << current_time_ - init_start_time_ << ", bg = " << init_bg_.transpose()<< ", ba = " << init_ba_.transpose() << ", gyro sq = " << cov_gyro_.transpose()<< ", acce sq = " << cov_acce_.transpose() << ", grav = " << gravity_.transpose()<< ", norm: " << gravity_.norm();LOG(INFO) << "mean gyro: " << mean_gyro.transpose() << " acce: " << mean_acce.transpose();init_success_ = true;return true;
}

• 然后使用IMU数据对eskf进行设定初始参数
• 遍历到RTK数据时没有使用IMU对eskf进行初始化，跳过这条数据，并且如果方位角无效，即GUSS数据最后一个值为0，跳过该数据。

template <typename S>
bool ESKF<S>::ObserveGps(const GNSS& gnss) {/// GNSS 观测的修正assert(gnss.unix_time_ >= current_time_);if (first_gnss_) {R_ = gnss.utm_pose_.so3();p_ = gnss.utm_pose_.translation();first_gnss_ = false;current_time_ = gnss.unix_time_;return true;}assert(gnss.heading_valid_);ObserveSE3(gnss.utm_pose_, options_.gnss_pos_noise_, options_.gnss_ang_noise_);current_time_ = gnss.unix_time_;return true;
}template <typename S>
bool ESKF<S>::ObserveSE3(const SE3& pose, double trans_noise, double ang_noise) {/// 既有旋转，也有平移/// 观测状态变量中的p, R，H为6x18，其余为零Eigen::Matrix<S, 6, 18> H = Eigen::Matrix<S, 6, 18>::Zero();H.template block<3, 3>(0, 0) = Mat3T::Identity();  // P部分H.template block<3, 3>(3, 6) = Mat3T::Identity();  // R部分（3.66)// 卡尔曼增益和更新过程Vec6d noise_vec;noise_vec << trans_noise, trans_noise, trans_noise, ang_noise, ang_noise, ang_noise;Mat6d V = noise_vec.asDiagonal();Eigen::Matrix<S, 18, 6> K = cov_ * H.transpose() * (H * cov_ * H.transpose() + V).inverse();// 更新x和covVec6d innov = Vec6d::Zero();innov.template head<3>() = (pose.translation() - p_);          // 平移部分innov.template tail<3>() = (R_.inverse() * pose.so3()).log();  // 旋转部分(3.67)dx_ = K * innov;cov_ = (Mat18T::Identity() - K * H) * cov_;UpdateAndReset();return true;
}/// 更新名义状态变量，重置error statevoid UpdateAndReset() {p_ += dx_.template block<3, 1>(0, 0);v_ += dx_.template block<3, 1>(3, 0);R_ = R_ * SO3::exp(dx_.template block<3, 1>(6, 0));if (options_.update_bias_gyro_) {bg_ += dx_.template block<3, 1>(9, 0);}if (options_.update_bias_acce_) {ba_ += dx_.template block<3, 1>(12, 0);}g_ += dx_.template block<3, 1>(15, 0);// 以上为均值部分的误差更新，更新后可以简单的实现为dx_=0// 下面为协方差的更新，需要考虑重置后的切空间投影ProjectCov();dx_.setZero();}

• 观测修正过程同书，之后更新名义状态变量，重置error state
• 均值部分的误差更新，更新后可以简单的实现为dx_=0
• 协方差的更新，需要考虑重置后的切空间投影，同书

    void ProjectCov() {Mat18T J = Mat18T::Identity();J.template block<3, 3>(6, 6) = Mat3T::Identity() - 0.5 * SO3::hat(dx_.template block<3, 1>(6, 0));cov_ = J * cov_ * J.transpose();}

• 轮速预测修正

bool ESKF<S>::ObserveWheelSpeed(const Odom& odom) {assert(odom.timestamp_ >= current_time_);// odom 修正以及雅可比// 使用三维的轮速观测，H为3x18，大部分为零Eigen::Matrix<S, 3, 18> H = Eigen::Matrix<S, 3, 18>::Zero();H.template block<3, 3>(0, 3) = Mat3T::Identity();// 卡尔曼增益Eigen::Matrix<S, 18, 3> K = cov_ * H.transpose() * (H * cov_ * H.transpose() + odom_noise_).inverse();// velocity obsdouble velo_l = options_.wheel_radius_ * odom.left_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;double velo_r =options_.wheel_radius_ * odom.right_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;double average_vel = 0.5 * (velo_l + velo_r);VecT vel_odom(average_vel, 0.0, 0.0);VecT vel_world = R_ * vel_odom;dx_ = K * (vel_world - v_);// update covcov_ = (Mat18T::Identity() - K * H) * cov_;UpdateAndReset();return true;
}