cartographer 算法中提供了一个位置和姿态估计器，其主要作用是在前端匹配时作为预测位置的输入，即可认为是匹配的初始位置和姿态。在前面已经详细讲解了前端scan math的三种方法，其中前端的优化匹配和相关匹配一定程度上依赖于初始位姿，因此获取预测位置十分重要。
获取预测的位置方式有许多种，比如其他slam算法中常见的odometry转换、IMU积分、上刻位置近似等。而cartographer提供了一个3d预测估计器实现（包含2d实现），可自动融合历史位置、odometer和IMU数据。其调用接口目录为：
cartographer/mapping/pose_extrapolator_interface.h
接口内部为虚函数，具体有两种实现方式，目前2d使用一种PoseExtrapolator，其位姿估计器具体实现代码目录为：
cartographer/mapping/pose_extrapolator.h

pose_extrapolator类定义

  //时间参数，两次预测估计最小时间差
  const common::Duration pose_queue_duration_;
  // 带时间戳的位置结构体
  struct TimedPose {
    common::Time time;
    transform::Rigid3d pose;
  };
  // 带时间戳的位置队列
  std::deque<TimedPose> timed_pose_queue_;
  // 从位置队列估计的线速度和角速度
  Eigen::Vector3d linear_velocity_from_poses_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d angular_velocity_from_poses_ = Eigen::Vector3d::Zero();

  // 重力加速度常数
  const double gravity_time_constant_;
  // Imu 原始data 队列，一般仅保留两个或最新预测时间之后的所有序列
  std::deque<sensor::ImuData> imu_data_;

  // 全局的航向推算器
  std::unique_ptr<ImuTracker> imu_tracker_;
  // 共享临时航向角推算器
  std::unique_ptr<ImuTracker> odometry_imu_tracker_;
  std::unique_ptr<ImuTracker> extrapolation_imu_tracker_;

  // 推算新的位置缓存
  TimedPose cached_extrapolated_pose_;

  // 里程计队列信息，一般仅保留两个或最新预测时间之后的所有序列
  std::deque<sensor::OdometryData> odometry_data_;
  // 通过里程计估计线速度和角速度
  Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d angular_velocity_from_odometry_ = Eigen::Vector3d::Zero();

pose_extrapolator定义了预测估计器所使用所有变量和参数；其中主要对外方法接口为三种传感器数据的插入和获取预测位置和姿态。

  // 添加传感器数据
  void AddPose(common::Time time, const transform::Rigid3d& pose) override;
  void AddImuData(const sensor::ImuData& imu_data) override;
  void AddOdometryData(const sensor::OdometryData& odometry_data) override;
  
  // 推算出估计位置
  transform::Rigid3d ExtrapolatePose(common::Time time) override;

插入IMU数据 AddImuData（）

// 添加imu data
void PoseExtrapolator::AddImuData(const sensor::ImuData& imu_data) {
  CHECK(timed_pose_queue_.empty() ||
        imu_data.time >= timed_pose_queue_.back().time);
  imu_data_.push_back(imu_data);
  TrimImuData();
}

由于IMU可直接提供角速度信息，因此无需进行一定预处理，即已获取imu的角速度信息；

插入里程计数据 AddOdometryData（）

// 添加 odometrydata
void PoseExtrapolator::AddOdometryData(
    const sensor::OdometryData& odometry_data) {
  CHECK(timed_pose_queue_.empty() ||
        odometry_data.time >= timed_pose_queue_.back().time);
  odometry_data_.push_back(odometry_data);
  TrimOdometryData();
  if (odometry_data_.size() < 2) {
    return;
  }
  // TODO(whess): Improve by using more than just the last two odometry poses.
  // Compute extrapolation in the tracking frame.
  const sensor::OdometryData& odometry_data_oldest = odometry_data_.front();
  const sensor::OdometryData& odometry_data_newest = odometry_data_.back();
  // 两次时间间隔
  const double odometry_time_delta =
      common::ToSeconds(odometry_data_oldest.time - odometry_data_newest.time);
  // 两次位置转移矩阵
  const transform::Rigid3d odometry_pose_delta =
      odometry_data_newest.pose.inverse() * odometry_data_oldest.pose;

  // 获取旋转速度
  angular_velocity_from_odometry_ =
      transform::RotationQuaternionToAngleAxisVector(
          odometry_pose_delta.rotation()) /
      odometry_time_delta;

  if (timed_pose_queue_.empty()) {
    return;
  }

  // 获取根据里程计转换直接获取最新线速度
  const Eigen::Vector3d
      linear_velocity_in_tracking_frame_at_newest_odometry_time =
          odometry_pose_delta.translation() / odometry_time_delta;

  // 提取里程计和当前真实位置的旋转转移矩阵
  // 其中odometry_imu_tracker_ 与 与刚添加历史位置时的imu_tracker_信息一致
  const Eigen::Quaterniond orientation_at_newest_odometry_time =
      timed_pose_queue_.back().pose.rotation() *
      ExtrapolateRotation(odometry_data_newest.time,
                          odometry_imu_tracker_.get());
  // 提取当前真实线速度                        
  linear_velocity_from_odometry_ =
      orientation_at_newest_odometry_time *
      linear_velocity_in_tracking_frame_at_newest_odometry_time;
}

仅根据odometer估计当前线速度和角速度信息；

UpdateVelocitiesFromPoses（）

此函数则是根据历史最近的两帧位姿信息进行微分，获取线速度和角速度。

// 从历史位置估计当前的速度
void PoseExtrapolator::UpdateVelocitiesFromPoses() {
  if (timed_pose_queue_.size() < 2) {
    // We need two poses to estimate velocities.
    return;
  }
  CHECK(!timed_pose_queue_.empty());
  const TimedPose& newest_timed_pose = timed_pose_queue_.back();
  const auto newest_time = newest_timed_pose.time;
  const TimedPose& oldest_timed_pose = timed_pose_queue_.front();
  const auto oldest_time = oldest_timed_pose.time;
  // 计算最近两次位置的时间差
  const double queue_delta = common::ToSeconds(newest_time - oldest_time);
  // 时间间隔需满足超出一定阈值
  if (queue_delta < common::ToSeconds(pose_queue_duration_)) {
    LOG(WARNING) << "Queue too short for velocity estimation. Queue duration: "
                 << queue_delta << " s";
    return;
  }
  // 获取最近两次的位置
  const transform::Rigid3d& newest_pose = newest_timed_pose.pose;
  const transform::Rigid3d& oldest_pose = oldest_timed_pose.pose;
  // 计算线速度
  linear_velocity_from_poses_ =
      (newest_pose.translation() - oldest_pose.translation()) / queue_delta;
  // 计算角速度
  angular_velocity_from_poses_ =
      transform::RotationQuaternionToAngleAxisVector(
          oldest_pose.rotation().inverse() * newest_pose.rotation()) /
      queue_delta;
}

插入历史位置AddPose（）

// 添加历史位置
void PoseExtrapolator::AddPose(const common::Time time,
                               const transform::Rigid3d& pose) {
  // 2d slam imu_tracker_ 没有初始化
  // 第一次加入历史位置时，需初始化
  if (imu_tracker_ == nullptr) {
    // 当前时间为初始时间
    common::Time tracker_start = time;
    // 如果有imu数据的话， 取imu和当前时间最早时间
    if (!imu_data_.empty()) {
      tracker_start = std::min(tracker_start, imu_data_.front().time);
    }
    // 初始化 角度推算器
    imu_tracker_ =
        absl::make_unique<ImuTracker>(gravity_time_constant_, tracker_start);
  }
  // 记录对应时间和位置，放入一个队列存储
  timed_pose_queue_.push_back(TimedPose{time, pose});
  // 确保仅保留两个，同时最新位置的间隔不超出参数阈值
  while (timed_pose_queue_.size() > 2 &&
         timed_pose_queue_[1].time <= time - pose_queue_duration_) {
    timed_pose_queue_.pop_front();
  }
  // 从位置中更新速度信息
  UpdateVelocitiesFromPoses();
  //将全局imu推算器执行推算，主要推算估计的航向信息
  AdvanceImuTracker(time, imu_tracker_.get());
  TrimImuData();
  TrimOdometryData();
  // 智能指针创建，并共享内容，无需delete
  // 即每次添加新的pose时同时开辟两个tracker
  odometry_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
  extrapolation_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
}

插入历史位置进行更新估计为主要和必要方法，即使没有odometer和imu插入数据也可正常工作，因此AddPose（）可为主要函数，即目的按照时间顺序插入历史真实位置（实际上为scan-match后较为准确的位置）。然后进行估计线速度和角速度。通过角速度再次进行积分获取航向变换信息。

航向角积分实现AdvanceImuTracker（）

//imu 估计器更新，即迭代更新当前估计位置的航向信息， 估计器采用指针可调用不同具体估计器
// 
void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
                                         ImuTracker* const imu_tracker) const {
  CHECK_GE(time, imu_tracker->time());
  // 当没有imu数据或者，更新的当前时间比imu数据时间早
  if (imu_data_.empty() || time < imu_data_.front().time) {
    // There is no IMU data until 'time', so we advance the ImuTracker and use
    // the angular velocities from poses and fake gravity to help 2D stability.
    // 计算出当前估计的朝向信息和重力加速度信息
    imu_tracker->Advance(time);
    // 无imu，则无线性加速度信息
    imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());
    // 更新推算器中目前采用的角速度
    // 如果有里程计信息，显然更相信里程计信息
    imu_tracker->AddImuAngularVelocityObservation(
        odometry_data_.size() < 2 ? angular_velocity_from_poses_
                                  : angular_velocity_from_odometry_);
    return;
  }
  // 按照最新传感器时间进行更新航向角度信息
  if (imu_tracker->time() < imu_data_.front().time) {
    // Advance to the beginning of 'imu_data_'.
    imu_tracker->Advance(imu_data_.front().time);
  }
  auto it = std::lower_bound(
      imu_data_.begin(), imu_data_.end(), imu_tracker->time(),
      [](const sensor::ImuData& imu_data, const common::Time& time) {
        return imu_data.time < time;
      });
  // 将imudata中每一时刻进行估计更新
  while (it != imu_data_.end() && it->time < time) {
    imu_tracker->Advance(it->time);
    imu_tracker->AddImuLinearAccelerationObservation(it->linear_acceleration);
    imu_tracker->AddImuAngularVelocityObservation(it->angular_velocity);
    ++it;
  }
  // 最新更新
  imu_tracker->Advance(time);
}

由于已估计出目前的线速度和角速度，为获取估计姿态则需要对角速度进行积分，然后获取角度旋转矩阵。由于航向积分并非像平移一样线性加减即可，因此cartographer专门提供了一个用于角度积分的ImuTracker类。

ImuTracker类定义

代码目录如下：
cartographer/mapping/imu_tracker.h
类定义如下：

  const double imu_gravity_time_constant_;      // 重力加速度时间更新参数
  common::Time time_;                           // 积分最新时间戳
  common::Time last_linear_acceleration_time_;  // 上时刻更新重力加速度时间戳
  Eigen::Quaterniond orientation_;              // 积分器自身维持的航向值
  Eigen::Vector3d gravity_vector_;              // 重力加速度向量
  Eigen::Vector3d imu_angular_velocity_;        // 用于积分的角速度

其核心的积分方法为void Advance(common::Time time);其具体实现如下：

// 积分计算出当前估计的朝向信息
void ImuTracker::Advance(const common::Time time) {
  CHECK_LE(time_, time);
  // 当前时刻与上次时刻差
  const double delta_t = common::ToSeconds(time - time_);
  // 根据旋转速度计算旋转矩阵
  const Eigen::Quaterniond rotation =
      transform::AngleAxisVectorToRotationQuaternion(
          Eigen::Vector3d(imu_angular_velocity_ * delta_t));
  // 更新当前定位方向信息
  orientation_ = (orientation_ * rotation).normalized();
  // 根据旋转信息更新重力加速度方向
  // conjugate()表示共轭
  gravity_vector_ = rotation.conjugate() * gravity_vector_;
  time_ = time;
}

其中角度积分其出现关于重力加速度信息的，对于2d slam作用不是太大。其主要作用是判断激光雷达是否为真实水平，若有一定夹角（即重力加速度向量gravity_vector_），则需进行投影。即具体更新过程不再详细描述。

获取预测位置ExtrapolatePose(const common::Time time)

//输出当前时间的估计的位置
transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) {
  // 获取最新的即上次的位置和姿态信息
  const TimedPose& newest_timed_pose = timed_pose_queue_.back();
  // 当前时间更晚
  CHECK_GE(time, newest_timed_pose.time);
  // 和上次更新的时间一致，则可直接返回上次获取估计的位置
  // 否则进行换算
  if (cached_extrapolated_pose_.time != time) {
    // 获取估计的平移矩阵
    // 然后对上次的位置进行平移，获取估计位置
    const Eigen::Vector3d translation =
        ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();
    // 获取估计旋转矩阵
    // 然后对上次的航向进行旋转，获取估计的航向
    // extrapolation_imu_tracker_ 与最新加入历史位置时的imu_tracker_一致
    const Eigen::Quaterniond rotation =
        newest_timed_pose.pose.rotation() *
        ExtrapolateRotation(time, extrapolation_imu_tracker_.get());
    cached_extrapolated_pose_ =
        TimedPose{time, transform::Rigid3d{translation, rotation}};
  }
  return cached_extrapolated_pose_.pose;
}

以上最终输出预测值接口，在上次真实位置基础上进行积分，即获取平移矩阵和旋转矩阵，进行转换获取最终估计位置和姿态；

其中位置平移矩阵积分较为简单，可参考代码即可。

//获取当前时间与插入最新位置的平移矩阵
Eigen::Vector3d PoseExtrapolator::ExtrapolateTranslation(common::Time time) {
  // 最新插入的位置信息
  const TimedPose& newest_timed_pose = timed_pose_queue_.back();
  // 计算时间差
  const double extrapolation_delta =
      common::ToSeconds(time - newest_timed_pose.time);

  // 根据线速度直接计算
  if (odometry_data_.size() < 2) {
    return extrapolation_delta * linear_velocity_from_poses_;
  }
  return extrapolation_delta * linear_velocity_from_odometry_;
}

而旋转转换矩阵，参考代码如下。由于专门ImuTracker角度积分器自己内部维持了个角度积分值，因此需要提取出最新的航向与上次航向的过程旋转矩阵。

// 获取当前时间与插入最新位置的旋转变换矩阵
Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation(
    const common::Time time, ImuTracker* const imu_tracker) const {
  CHECK_GE(time, imu_tracker->time());
  // 更新当前的航向信息
  AdvanceImuTracker(time, imu_tracker);
  // 获取全局imu_tracker_ 朝向，由于未进行更新，即为上一时刻的朝向
  const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
  // 求出上次到最新航向矩阵的转换矩阵
  return last_orientation.inverse() * imu_tracker->orientation();
}

思想总结

位姿估计器解决的是在实现前端scan-match之前的预测值，即激光新的一帧匹配前获取当前预测定位信息。同时假设传感器数据之间，slam本体以恒定线速度和恒定角速度移动。根据上次的scan-match的准确位置后进行积分，获取预测位置和姿态。
其中估计的恒定速度主要两种方式组成：采用两次scan-match位置进行微分获得；另一种通过里程计进行微分获得；由于里程计的更新速度更快，故里程计可计算最新结果时，优先使用；
估计的角速度则有三种方式组成：1.采用两次scan-match的航向进行微分获得，频率最低；2.采用里程计进行微分；3.采用IMU直接获取，优先级最高；

cartographer针对imu和odometer是否使用可进行配置，根据配置信息和时间序列进行选择具体方式，最后根据估计的线速度和角速度进行实时积分，获取预测估计位置。
最基本的配置odometry和imu均无，则估计器实际上仅是根据历史位置进行推算出线速度和角速度，进行估计位置。