PoseExtrapolator & ImuTracker

1. ImuTracker

• 使用IMU的角速度和線性加速度跟蹤方向。 因爲平均線性加速度（假設慢速運動）是重力的直接測量，所以滾動/俯仰(roll/pitch)不會漂移，儘管偏航(yaw)會漂移。
• 輸入：
  • 時間 (common::Time time)
  • 線加速度 (Eigen::Vector3d linear_acceleration)
  • 角速度 (Eigen::Vector3d angular_velocity)
  • 輸入實例：

header: 
  seq: 34496
  stamp: 
    secs: 1474964355
    nsecs: 312731655
  frame_id: "gyro_link"
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: -0.0015271630955
  y: 0.024740042147
  z: -0.0226020138133
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: 0.0
  y: 0.0
  z: 9.8
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

• 輸出：當前的方向估計（Eigen::Quaterniond orientation_）

2. PoseExtrapolator

• 解決IMU、Odometry(里程計)和位姿信息進行融合的問題
• 輸入
  • IMU
  • 里程計
  • 位姿信息(Pose)
• 輸出
  • 根據位姿計算的線速度和角速度
  • 根據里程計計算的線速度和角速度
  • 推測某一時刻的位姿(ExtrapolatePose(const common::Time time))
    • 旋轉：ImuTracker的方向角的變化量
    • 平移：里程計或者位姿線速度計算的移動量