PoseExtrapolator 2. ImuTracker

IMU Tracker

imu_tracker_: 存放由IMU数据得到的信息，用一帧一帧IMU来推断当前时刻的机器人位姿
odometry_imu_tracker_: 存放由里程计得到的信息，表示AddPose到AddOdometryData之间（即最新优化的位姿时刻到最新里程计数据时刻之间的时间段）的姿态变化。在AddOdometryData用于辅助计算线速度
extrapolation_imu_tracker_: 存放经过数据融合后的结果，表示AddPose到ExtrapolatePose之间（即最新优化的位姿时刻到最新估计位姿时刻之间的时间段）的姿态变化。在ExtrapolatePose用于前端辅助实现激光运动畸变矫正。

重点看IMU Tracker。

一般IMU要安装在被测物体的重心上。imu的角速度测量精度往往比线速度好的多，因此在工程上，经常采用imu的角速度，角速度与时间积分计算角度，得到的角度变化量与初始角度相加，就得到目标角度，其中积分时间 $\Delta t$ 越小输出的角度越精确。但积分的累积误差会随着时间的而增加，所以陀螺仪只能工作在相对较短的时间内。

所以要得到较为真实的姿态角，就要利用一阶低通滤波扬长避短，在短时间内增加陀螺仪的权值，在更长的时间增加加速度的权值，这样系统输出角度就接近真实值了。

ImuTracker是使用角速度和线加速度(就是来自IMU数据) 跟踪 朝向和重力向量。平均化后的线加速度(假设移动很慢)，是直接测量为g倍数的，roll/pitch不会漂移，但是重力加速度会漂移，也就是说用IMU测量得到的话题/IMU/linear_acceleration/z不是固定的9.8

更新姿态 void Advance(common::Time time)

将姿态更新到时刻time。通过计算time和上次的时间差，imu的角速度乘时间差得到角位移，原来的姿态右乘角位移得到新的姿态；同时更新时间和重力向量gravity_vector_

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_ 初始为 Eigen::Quaterniond::Identity()
  orientation_ = (orientation_ * rotation).normalized();

  /*  车体的旋转方向，跟重力向量的旋转方向是反向的，比如车体向上仰a角度，
  重力向量是往反方向即俯方向旋转a角度，
  conjugate 返回当前四元数的共轭四元数，就是逆旋转 */
  // gravity_vector_ 初始为 Eigen::Vector3d::UnitZ()  向量(0,0,1)
  gravity_vector_ = rotation.conjugate() * gravity_vector_;
  time_ = time;
}

// 因为imu直接测得角速度，所以直接更新
void ImuTracker::AddImuAngularVelocityObservation(
    const Eigen::Vector3d& imu_angular_velocity)
{
  imu_angular_velocity_ = imu_angular_velocity;
}

更新重力向量

使用线加速度不断更新重力向量gravity_vector_，再根据重力向量更新旋转矩阵和方位角。后面还会对齐点云方向

// 使用线加速度更新重力向量，使用了一阶低通滤波， 防止加速度跳动剧烈
void ImuTracker::AddImuLinearAccelerationObservation(
    const Eigen::Vector3d& imu_linear_acceleration) 
{
  // 当前时刻离上一次时刻的差， 初值是inf
  const double delta_t =
      last_linear_acceleration_time_ > common::Time::min()
          ? common::ToSeconds(time_ - last_linear_acceleration_time_)
          : std::numeric_limits<double>::infinity();
  last_linear_acceleration_time_ = time_;
  // 计算权重, alpha为滤波系数, delta_t越大, 当前的权重越大. delta为inf时，权重为1
  // imu_gravity_time_constant_为滤波器常数， 默认为10
  const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_);
  // 一阶低通滤波， gravity_vector_ 为滤波输出值， imu_linear_acceleration为采样值
  // 初始的输出值是 imu_linear_acceleration，如果没有IMU就是 (0,0,1)
  gravity_vector_ =
      (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;

  // 根据新的重力向量，更新对应的旋转矩阵, FromTwoVectors返回一个表示从向量a旋转到向量b的四元数
      // rotation 看做线加速度对上次线加速度的校正
    // orientation_.conjugate() * Eigen::Vector3d::UnitZ()就是把这个四元数转换成向量的形式
    // 在这里我们只关注重力方向上的值，即z轴上的值
  const Eigen::Quaterniond rotation = FromTwoVectors(
      gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ() );
  // 更新方位角
  orientation_ = (orientation_ * rotation).normalized();
  // 如果之前的计算正确，二者的乘积应当是（0,0,1）
  CHECK_GT((orientation_ * gravity_vector_).z(),  0.);
  CHECK_GT((orientation_ * gravity_vector_).normalized().z(),  0.99);
}

一阶低通滤波，实现了硬件上的RC低通滤波器的功能，滤掉IMU噪声。算法公式为：

$Y_n = \alpha X_n+(1-\alpha)Y_{n-1}$

这里的X就是线加速度，Y是重力向量，滤波系数 $\alpha$ 通常远小于1，d为滤波器的常数10

从公式可以看出，时间差越大， $\alpha$ (当前线加速度的权重 )越大，当前线加速度的权重越大，上一次的重力向量的权重越小，应该是考虑到长时间内的重力向量数据不可靠

使用我的参数和IMU，发现alpha经常保持很精确的数值0.0009995，原因当然是delta_t

没看出这里如何避免传统一阶滤波的缺点，常说的动态调整滤波系数的要求：当数据快速变化时，滤波结果能及时跟进（灵敏度优先）；当数据趋于稳定，在一个固定的点上下振荡时，滤波结果能趋于平稳（平稳度优先）

其实还可以用卡尔曼滤波，效果更好，估计是因为运算负荷问题而没有使用

AdvanceImuTracker

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
    //当里程计的数据小于2个时，选择从pose；否则选择里程计。因为里程计的精度更高
    imu_tracker->AddImuAngularVelocityObservation(
        odometry_data_.size() < 2 ? angular_velocity_from_poses_
                                  : angular_velocity_from_odometry_ );
    return;   // 没有IMU可以return了
  }
  //有imu时，此处调用了imu数据队列，ImuTracker维护的时间早于IMU数据队列最早的时间
  if (imu_tracker->time() < imu_data_.front().time)
  {
    // 先把ImuTracker更新到IMU数据来临的那一刻
    imu_tracker->Advance(imu_data_.front().time);
  }
  // 依次取出IMU数据队列中的数据，更新ImuTracker，直到IMU数据的时间比指定时间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;
      });
  // 用imu数据队列进行imu-tracker的数据更新.
  // imu_tracker主要负责对角速度进行插值, 并且估计重力向量
  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-位姿推测部分