本文链接： https://blog.csdn.net/xiaoma_bk/article/details/89492816

1.kalman_filter定义

1.1.高斯分布类

1.1.1定义高斯分布类

定义高斯分布类，T表示数据类型 ，N表示状态变量的维数
这里的数据类型一般都是int float double之类的．

template <typename T, int N>
class GaussianDistribution
{
public:

 //构造函数　需要指定高斯分布的均值和协方差
 GaussianDistribution(const Eigen::Matrix<T, N, 1>& mean,
                      const Eigen::Matrix<T, N, N>& covariance)
     : mean_(mean), covariance_(covariance) {}

 //返回均值
 const Eigen::Matrix<T, N, 1>& GetMean() const { return mean_; }

 //放回方差
 const Eigen::Matrix<T, N, N>& GetCovariance() const { return covariance_; }

private:
 Eigen::Matrix<T, N, 1> mean_;
 Eigen::Matrix<T, N, N> covariance_;
};

1.1.2两个高斯分布相加

这里的加号定义为：对应的均值相加　对应的方差相加

template <typename T, int N>
GaussianDistribution<T, N> operator+(const GaussianDistribution<T, N>& lhs,
                                     const GaussianDistribution<T, N>& rhs) {
  return GaussianDistribution<T, N>(lhs.GetMean() + rhs.GetMean(),
                                    lhs.GetCovariance() + rhs.GetCovariance());
}

1.1.3高斯分布乘以一个数／数组

或者说高斯分布通过一个线性变换之后生成的新的高斯分布

template <typename T, int N, int M>
GaussianDistribution<T, N> operator*(const Eigen::Matrix<T, N, M>& lhs,
                                     const GaussianDistribution<T, M>& rhs) {
  return GaussianDistribution<T, N>(
      lhs * rhs.GetMean(), lhs * rhs.GetCovariance() * lhs.transpose());
}

2.里程计

2.1里程计状态

A.时间time，B.里程计位姿odometer_pose，C.状态位姿odometer_pose（滤波器time估计的真实位姿）

struct OdometryState
{
  OdometryState(common::Time time, const transform::Rigid3d& odometer_pose,
                const transform::Rigid3d& state_pose);
  OdometryState() {}

  //时间
  common::Time time = common::Time::min();

  //里程计位姿
  transform::Rigid3d odometer_pose = transform::Rigid3d::Identity();

  //状态位姿　即滤波器估计的时刻time的　机器人的真实位姿
  transform::Rigid3d state_pose = transform::Rigid3d::Identity();
};

2.2里程计状态跟踪器

所谓的OdometryStateTracker相当于OdometryState的缓冲器
保存最近的winodw_size个里程计数据
A.双端队列odometry_states_ ，B.窗口大小window_size_

2.2.1类申明

class OdometryStateTracker
{
 public:
  //用来存储OdometryState的双端队列
  using OdometryStates = std::deque<OdometryState>;

  explicit OdometryStateTracker(int window_size);

  //得到一整个缓冲区  （返回整个里程计队列）
  const OdometryStates& odometry_states() const;

  // Adds a new 'odometry_state' and makes sure the maximum number of previous
  // odometry states is not exceeded.
  // 增加一个OdometryState到队列中
  void AddOdometryState(const OdometryState& odometry_state);

  // Returns true if no elements are present in the odometry queue.
  // 判断是否为空
  bool empty() const;

  // Retrieves the most recent OdometryState or an empty one if non yet present.
  //　返回最近的OdometryState（odometry_states_.back();）
  const OdometryState& newest() const;

 private:
  OdometryStates odometry_states_;  //双端队列
  size_t window_size_;              //窗口大小
};

2.2.2类定义

增加里程计数量到队列

如果超过了window_size_，则会把过时的里程计状态去除掉

void OdometryStateTracker::AddOdometryState(
    const OdometryState& odometry_state)
{
  odometry_states_.push_back(odometry_state);
  while (odometry_states_.size() > window_size_)
  {
    odometry_states_.pop_front();
  }
}

3.位姿推算

3.1 基本类型

1.协方差，带协方差的位姿

typedef Eigen::Matrix3d Pose2DCovariance;
typedef Eigen::Matrix<double, 6, 6> PoseCovariance;

//带协方差的位姿
struct PoseAndCovariance
{
  transform::Rigid3d pose;
  PoseCovariance covariance;
};

2.带协方差的位姿 * 号重载

对这个位姿做转换
功能：求得一个带方差的位姿，经过一个线性变化之后，得到的新的位姿和方差

PoseAndCovariance operator*(const transform::Rigid3d& transform,
                            const PoseAndCovariance& pose_and_covariance)
{
  /*用方差生成一个0均值的高斯分布*/
  GaussianDistribution<double, 6> distribution(
      Eigen::Matrix<double, 6, 1>::Zero(), pose_and_covariance.covariance);

  Eigen::Matrix<double, 6, 6> linear_transform;

  linear_transform << transform.rotation().matrix(), Eigen::Matrix3d::Zero(),
      Eigen::Matrix3d::Zero(), transform.rotation().matrix();

  return {transform * pose_and_covariance.pose,
          (linear_transform * distribution).GetCovariance()};
}

3.把机器人3d位姿的方差转换成机器人2d位姿的方差
(x,y,z,alpha,beta,theta)->(x,y,theta)

/**
 * @brief Project2D
 * 把机器人3d位姿的方差转换成机器人2d位姿的方差
 * 即提取(x,y,yaw)的方差
 * @param covariance    机器人3d位姿的方差
 * @return
 */
Pose2DCovariance Project2D(const PoseCovariance& covariance)
{
  Pose2DCovariance projected_covariance;
  projected_covariance.block<2, 2>(0, 0) = covariance.block<2, 2>(0, 0);
  projected_covariance.block<2, 1>(0, 2) = covariance.block<2, 1>(0, 5);
  projected_covariance.block<1, 2>(2, 0) = covariance.block<1, 2>(5, 0);
  projected_covariance(2, 2) = covariance(5, 5);
  return projected_covariance;
}

4.用机器人的2d位姿方差　和　位置方差　＆　朝向方差　生成一个3d位姿方差
转换时，z,alpha,beta,协方差还没有

/**
 * @brief Embed3D
 * 用机器人的2d位姿方差　和　位置方差　＆　朝向方差　生成一个3d位姿方差
 * @param embedded_covariance       2d位姿方差
 * @param position_variance         位姿方差
 * @param orientation_variance      朝向方差
 * @return
 */
PoseCovariance Embed3D(const Pose2DCovariance& embedded_covariance,
                       const double position_variance,
                       const double orientation_variance)
{
  PoseCovariance covariance;
  covariance.setZero();
  covariance.block<2, 2>(0, 0) = embedded_covariance.block<2, 2>(0, 0);
  covariance.block<2, 1>(0, 5) = embedded_covariance.block<2, 1>(0, 2);
  covariance.block<1, 2>(5, 0) = embedded_covariance.block<1, 2>(2, 0);
  covariance(5, 5) = embedded_covariance(2, 2);

  covariance(2, 2) = position_variance;
  covariance(3, 3) = orientation_variance;
  covariance(4, 4) = orientation_variance;
  return covariance;
}

5.从proto中加载以及保存到proto

PoseCovariance PoseCovarianceFromProtoMatrix(
    const sensor::proto::Matrix& proto_matrix);

proto::PoseTrackerOptions CreatePoseTrackerOptions(
    common::LuaParameterDictionary* parameter_dictionary);

3.2 ImuTracker

imutracker这个imu_tracker是PoseTracker的一部分．主要用来计算机器人的姿态

注意：这里的计算机器人的姿态　在整个PoseTracker里面　相当于对机器人位姿的预测.
imu_tracker的主要功能是：对imu进行姿态结算，同时还能对姿态进行一些预测(匀角速度预测)，也就是说如果我们有一个直接能输出角度和角速度的imu的话，那么这里的imu_tracker其实作用是不大的．因此这里的imu的预测和观测其实是相对于姿态结算来说的，并不是说相对于整个pose_tracker来说的．

3.2.1成员变量

const proto::PoseTrackerOptions options_; (imu重力时间常数)
common::Time time_; //当期时间
common::Time last_linear_acceleration_time_; //上一次更新线性加速度计的时间
-Eigen::Quaterniond orientation_; //四元数，当前角度
Eigen::Vector3d gravity_direction_; //重力方向　－－imu加速度
Eigen::Vector3d imu_angular_velocity_; //imu角速度

3.2.2核心函数

1.根据时间预测角度，重力方向

跟新当前的角度，重力方向，时间

预测时刻time的机器人的位姿　这里预测是假设机器人是以imu_angular_velocity_匀角速度运动
计算增量，得到匀速角增量转化四元素，跟新数据

/*
 * IMUTracker预测时间time时候的姿态
 * 假设在时间time_ 到 time　这段时间机器人以imu_angular_velocity_的角速度匀速运动
 * 计算出来在time时刻机器人的角度应该运动到什么角度
 */
void ImuTracker::Predict(const common::Time time)
{
  CHECK_LE(time_, time);

  //时间增量
  const double delta_t = common::ToSeconds(time - time_);

  //按照当前的角速度imu_angular_velocity_运行时间delta_t　得到delta_t时间内的角度的增量　并转换为四元数
  const Eigen::Quaterniond rotation =
      transform::AngleAxisVectorToRotationQuaternion(
          Eigen::Vector3d(imu_angular_velocity_ * delta_t));

  //更新机器人的当前角度 \ 重力矢量
  //角度更新了　那么对应的重力矢量应该也更新
  orientation_ = (orientation_ * rotation).normalized();
  gravity_direction_ = rotation.inverse() * gravity_direction_;
  time_ = time;
}

2. 增加线性加速度观测到IMU跟踪器中

该线加速度不影响角速度，但影响角度和重力方向

增加一个加速度计读数的观测，通过预测的重力矢量　和　观测的　重力矢量　来对重力矢量进行更新，融合方式为互补滤波，预测的重力矢量为假设机器人是以imu_angular_velocity_匀角速度运行的来进行积分的，观测的重力矢量即为imu的读数，在重力矢量更新完成之后，重新调整机器人的角度使得角度和重力矢量匹配

/*
 * 增加一个加速度计读数的观测
 * 通过预测的重力矢量　和　观测的　重力矢量　来对重力矢量进行更新．
 * 融合方式为互补滤波
 *
 * 在重力矢量更新完成之后，重新调整机器人的角度使得角度和重力矢量匹配
*/
void ImuTracker::AddImuLinearAccelerationObservation(
    const common::Time time, const Eigen::Vector3d& imu_linear_acceleration)
{
  //预测time时刻 机器人的角度　因此也会更新预测的加速度矢量
  Predict(time);

  // Update the 'gravity_direction_' with an exponential moving average using
  // the 'imu_gravity_time_constant'.
  // 用重力时间常数来进行指数移动平均
  // 计算时间增量
  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;

  const double alpha =
      1. - std::exp(-delta_t / options_.imu_gravity_time_constant());

  //根据预测和观测来更新重力矢量
  gravity_direction_ =
      (1. - alpha) * gravity_direction_ + alpha * imu_linear_acceleration;

  // Change the 'orientation_' so that it agrees with the current
  // 'gravity_direction_'.
  // 修改机器人的角度　使得由它的角度计算出来的重力矢量　和　更新后的重力矢量重合
  const Eigen::Quaterniond rotation = Eigen::Quaterniond::FromTwoVectors(
      gravity_direction_, orientation_.inverse() * Eigen::Vector3d::UnitZ());

  orientation_ = (orientation_ * rotation).normalized();

  CHECK_GT((orientation_.inverse() * Eigen::Vector3d::UnitZ())
               .dot(gravity_direction_),
           0.);
}

3.增加角速度观测到跟踪器中

根据时间跟新角速度，重力方向，然后跟新角加速度

/*
 * 增加一个角速度观测
*/
void ImuTracker::AddImuAngularVelocityObservation(
    const common::Time time, const Eigen::Vector3d& imu_angular_velocity)
{
  //预测t时刻机器人的姿态
  Predict(time);

  //更新机器人的角速度
  imu_angular_velocity_ = imu_angular_velocity;
}

4.无迹卡尔曼 UKF实现过程

4.1 普通函数定义

4.1.1.返回一个数的平方

template <typename FloatType>
constexpr FloatType sqr(FloatType a)
{
  return a * a;
}

4.1.2.返回一个向量的外积.

外积的结果是一个N*N的矩阵.
向量为一个列向量．列向量＊行向量 = N*N矩阵

template <typename FloatType, int N>
Eigen::Matrix<FloatType, N, N> OuterProduct(
    const Eigen::Matrix<FloatType, N, 1>& v)
{
  return v * v.transpose();
}

4.1.3.判断矩阵Ａ是否为一个对称矩阵

template <typename FloatType, int N>
void CheckSymmetric(const Eigen::Matrix<FloatType, N, N>& A)
{
  // This should be pretty much Eigen::Matrix<>::Zero() if the matrix is
  // symmetric.
  const FloatType norm = (A - A.transpose()).norm();
  CHECK(!std::isnan(norm) && std::abs(norm) < 1e-5)
      << "Symmetry check failed with norm: '" << norm << "' from matrix:\n"
      << A;
}

4.1.4.返回正定对称矩阵的开方

template <typename FloatType, int N>
Eigen::Matrix<FloatType, N, N> MatrixSqrt(
    const Eigen::Matrix<FloatType, N, N>& A)
{
  CheckSymmetric(A);

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<FloatType, N, N>>
      adjoint_eigen_solver((A + A.transpose()) / 2.);
  const auto& eigenvalues = adjoint_eigen_solver.eigenvalues();
  CHECK_GT(eigenvalues.minCoeff(), -1e-5)
      << "MatrixSqrt failed with negative eigenvalues: "
      << eigenvalues.transpose();

  return adjoint_eigen_solver.eigenvectors() *
         adjoint_eigen_solver.eigenvalues()
             .cwiseMax(Eigen::Matrix<FloatType, N, 1>::Zero())
             .cwiseSqrt()
             .asDiagonal() *
         adjoint_eigen_solver.eigenvectors().transpose();
}

4.2无迹卡尔曼类

ukf类主要实现了probabilistic robotics上面介绍的ukf滤波器
同时针对四元数追踪做了一些修改

状态类型，状态协方差类型

  using StateType = Eigen::Matrix<FloatType, N, 1>;
  using StateCovarianceType = Eigen::Matrix<FloatType, N, N>;

4.2.1构造函数

显性构造，定义了一个UKF滤波器

参数：

initial_belief N维的高斯分布（均值N1，方差NN）
add_delta {state+delta} （state，delta，N*1）
compute_delta {target-origin} （target，origin，N*1）

  explicit UnscentedKalmanFilter(
      const GaussianDistribution<FloatType, N>& initial_belief,
      std::function<StateType(const StateType& state, const StateType& delta)>
          add_delta = [](const StateType& state,
                         const StateType& delta) { return state + delta; },
      std::function<StateType(const StateType& origin, const StateType& target)>
          compute_delta =
              [](const StateType& origin, const StateType& target) {
                return target - origin;
              })
      : belief_(initial_belief),
        add_delta_(add_delta),
        compute_delta_(compute_delta) {}

4.2.2成员变量

一个高斯的假设
GaussianDistribution<FloatType, N> belief_;
const std::function<StateType(const StateType& state, const StateType& delta)> add_delta_;
const std::function<StateType(const StateType& origin, const StateType& target)>

4.3 该类中的核心函数

4.3.1 ukf的预测过程

控制/预测对滤波器的影响。控制必须是由函数g隐式添加，也执行状态转换。'epsilon’是控制和模型噪声的附加组合。

  void Predict(std::function<StateType(const StateType&)> g,
               const GaussianDistribution<FloatType, N>& epsilon)

参数 g 状态（N1矩阵） epsilon 高斯分布（NN矩阵）

步骤
1.检查epsilon中的方差是否为对称矩阵

   CheckSymmetric(epsilon.GetCovariance());

2.得到t-1时刻的置信度的均值和标准差

    const StateType& mu = belief_.GetMean();
    const StateCovarianceType sqrt_sigma = MatrixSqrt(belief_.GetCovariance());

3.计算新的均值与方差，采用几个sigma叠加

    std::vector<StateType> Y;
    Y.reserve(2 * N + 1);
    Y.emplace_back(g(mu));

    const FloatType kSqrtNPlusLambda = std::sqrt(N + kLambda);
    for (int i = 0; i < N; ++i)
    {
      // Order does not matter here as all have the same weights in the
      // summation later on anyways.
      Y.emplace_back(g(add_delta_(mu, kSqrtNPlusLambda * sqrt_sigma.col(i))));
      Y.emplace_back(g(add_delta_(mu, -kSqrtNPlusLambda * sqrt_sigma.col(i))));
    }
    const StateType new_mu = ComputeMean(Y);
        StateCovarianceType new_sigma =
        kCovWeight0 * OuterProduct<FloatType, N>(compute_delta_(new_mu, Y[0]));
    for (int i = 0; i < N; ++i)
    {
      new_sigma += kCovWeightI * OuterProduct<FloatType, N>(
                                     compute_delta_(new_mu, Y[2 * i + 1]));
      new_sigma += kCovWeightI * OuterProduct<FloatType, N>(
                                     compute_delta_(new_mu, Y[2 * i + 2]));
    }

4.检查新的方差是否为对称矩阵，并跟新均值与方差

    CheckSymmetric(new_sigma);

    belief_ = GaussianDistribution<FloatType, N>(new_mu, new_sigma) + epsilon;

4.3.2 ukf的观测过程

卡尔曼滤波器的观察步骤。 'h’观察的转移状态均值为0，即传感器读数应该为零已包含在此功能中。 'delta’是测量噪声和必须有零均值。

  void Observe(
      std::function<Eigen::Matrix<FloatType, K, 1>(const StateType&)> h,
      const GaussianDistribution<FloatType, K>& delta)

参数 h 状态（N1矩阵） delta 高斯分布（NN矩阵）
步骤
1.检查delta的方差是否为对称矩阵，且其均值为0
2.得到t-1时刻的置信度的均值和标准差

    CheckSymmetric(delta.GetCovariance());
    // We expect zero mean delta.
    CHECK_NEAR(delta.GetMean().norm(), 0., 1e-9);

    // Get the state mean and matrix root of its covariance.
    const StateType& mu = belief_.GetMean();
    const StateCovarianceType sqrt_sigma = MatrixSqrt(belief_.GetCovariance());

3.正如在卡夫的论文中，我们计算包含零均值西格玛点的W，因为这就是我们所需要的。

    std::vector<StateType> W;
    W.reserve(2 * N + 1);
    W.emplace_back(StateType::Zero());

    std::vector<Eigen::Matrix<FloatType, K, 1>> Z;
    Z.reserve(2 * N + 1);
    Z.emplace_back(h(mu));

    Eigen::Matrix<FloatType, K, 1> z_hat = kMeanWeight0 * Z[0];
    const FloatType kSqrtNPlusLambda = std::sqrt(N + kLambda);
    for (int i = 0; i < N; ++i)
    {
      // Order does not matter here as all have the same weights in the
      // summation later on anyways.
      W.emplace_back(kSqrtNPlusLambda * sqrt_sigma.col(i));
      Z.emplace_back(h(add_delta_(mu, W.back())));

      W.emplace_back(-kSqrtNPlusLambda * sqrt_sigma.col(i));
      Z.emplace_back(h(add_delta_(mu, W.back())));

      z_hat += kMeanWeightI * Z[2 * i + 1];
      z_hat += kMeanWeightI * Z[2 * i + 2];
    }

5.更新状态计算新的置信度

    Eigen::Matrix<FloatType, K, K> S =
        kCovWeight0 * OuterProduct<FloatType, K>(Z[0] - z_hat);
    for (int i = 0; i < N; ++i)
    {
      S += kCovWeightI * OuterProduct<FloatType, K>(Z[2 * i + 1] - z_hat);
      S += kCovWeightI * OuterProduct<FloatType, K>(Z[2 * i + 2] - z_hat);
    }
    CheckSymmetric(S);
    S += delta.GetCovariance();

    Eigen::Matrix<FloatType, N, K> sigma_bar_xz =
        kCovWeight0 * W[0] * (Z[0] - z_hat).transpose();
    for (int i = 0; i < N; ++i)
    {
      sigma_bar_xz +=
          kCovWeightI * W[2 * i + 1] * (Z[2 * i + 1] - z_hat).transpose();
      sigma_bar_xz +=
          kCovWeightI * W[2 * i + 2] * (Z[2 * i + 2] - z_hat).transpose();
    }

    const Eigen::Matrix<FloatType, N, K> kalman_gain =
        sigma_bar_xz * S.inverse();
    const StateCovarianceType new_sigma =
        belief_.GetCovariance() - kalman_gain * S * kalman_gain.transpose();
    CheckSymmetric(new_sigma);

    belief_ = GaussianDistribution<FloatType, N>(
        add_delta_(mu, kalman_gain * -z_hat), new_sigma);

5.PoseTracker

位姿跟踪器　该跟踪器利用ukf来对位姿进行跟踪
通过融合imu和激光雷达scan-match的过程来不断的迭代ukf

5.1成员变量

5.1.1枚举

1.系统的状态

这个枚举类型表示系统的　状态变量的下标　和　对应的状态变量的位数

从这个枚举变量我们可以看到，整个系统变量的维数为９维．
分别为：xyz的位置　roll\pitch\yaw三个欧拉角　xyz三轴的速度

  enum {
    kMapPositionX = 0,
    kMapPositionY,
    kMapPositionZ,
    kMapOrientationX,
    kMapOrientationY,
    kMapOrientationZ,
    kMapVelocityX,
    kMapVelocityY,
    kMapVelocityZ,
    kDimension  // We terminate loops with this.
  };

2.环境模型

kalman滤波器的预测方程的选择　2d预测方程　还是 3d预测方程

如果车子在平面运动的话，那么只需要２d的预测方程就可以了，但是如果要在整个空间中运动，就需要3d的预测方程

  enum class ModelFunction { k2D, k3D };

3.别名

  //定义一个kalman滤波器
  using KalmanFilter = UnscentedKalmanFilter<double, kDimension>;
  //定义状态变量
  using State = KalmanFilter::StateType;
  //定义协方差矩阵
  using StateCovariance = Eigen::Matrix<double, kDimension, kDimension>;
  //定义高斯分布
  using Distribution = GaussianDistribution<double, kDimension>;

5.1.2 构造

模型（2d），卡尔曼滤波器，IMU跟踪器，Odom跟踪器。的确定

PoseTracker::PoseTracker(const proto::PoseTrackerOptions& options,
                         const ModelFunction& model_function,
                         const common::Time time)
    : options_(options),
      model_function_(model_function),
      time_(time),
      kalman_filter_(KalmanFilterInit(), AddDelta, ComputeDelta),
      imu_tracker_(options, time),
      odometry_state_tracker_(options.num_odometry_states()) {}

1.卡尔曼初始化

返回一个均值为0方差为无穷小的高斯分布

/**
 * @brief PoseTracker::KalmanFilterInit
 * kalman滤波器的初始化　返回一个均值为０方差为无穷小高斯分布
 * @return
 */
PoseTracker::Distribution PoseTracker::KalmanFilterInit()
{
  //状态变量初始为0
  State initial_state = State::Zero();
  // We are certain about the complete state at the beginning. We define the
  // initial pose to be at the origin and axis aligned. Additionally, we claim
  // that we are not moving.
  StateCovariance initial_covariance = 1e-9 * StateCovariance::Identity();
  return Distribution(initial_state, initial_covariance);
}

2.AddDelta

PoseTracker::State AddDelta(const PoseTracker::State& state,
                            const PoseTracker::State& delta)
{
  PoseTracker::State new_state = state + delta;

  const Eigen::Quaterniond orientation =
      transform::AngleAxisVectorToRotationQuaternion(
          Eigen::Vector3d(state[PoseTracker::kMapOrientationX],
                          state[PoseTracker::kMapOrientationY],
                          state[PoseTracker::kMapOrientationZ]));

  const Eigen::Vector3d rotation_vector(delta[PoseTracker::kMapOrientationX],
                                        delta[PoseTracker::kMapOrientationY],
                                        delta[PoseTracker::kMapOrientationZ]);

  CHECK_LT(rotation_vector.norm(), M_PI / 2.)
      << "Sigma point is far from the mean, recovered delta may be incorrect.";
  const Eigen::Quaterniond rotation =
      transform::AngleAxisVectorToRotationQuaternion(rotation_vector);

  const Eigen::Vector3d new_orientation =
      transform::RotationQuaternionToAngleAxisVector(orientation * rotation);

  new_state[PoseTracker::kMapOrientationX] = new_orientation.x();
  new_state[PoseTracker::kMapOrientationY] = new_orientation.y();
  new_state[PoseTracker::kMapOrientationZ] = new_orientation.z();
  return new_state;
}

3.ComputeDelta

PoseTracker::State ComputeDelta(const PoseTracker::State& origin,
                                const PoseTracker::State& target)
{
  PoseTracker::State delta = target - origin;
  const Eigen::Quaterniond origin_orientation =
      transform::AngleAxisVectorToRotationQuaternion(
          Eigen::Vector3d(origin[PoseTracker::kMapOrientationX],
                          origin[PoseTracker::kMapOrientationY],
                          origin[PoseTracker::kMapOrientationZ]));

  const Eigen::Quaterniond target_orientation =
      transform::AngleAxisVectorToRotationQuaternion(
          Eigen::Vector3d(target[PoseTracker::kMapOrientationX],
                          target[PoseTracker::kMapOrientationY],
                          target[PoseTracker::kMapOrientationZ]));

  const Eigen::Vector3d rotation =
      transform::RotationQuaternionToAngleAxisVector(
          origin_orientation.inverse() * target_orientation);

  delta[PoseTracker::kMapOrientationX] = rotation.x();
  delta[PoseTracker::kMapOrientationY] = rotation.y();
  delta[PoseTracker::kMapOrientationZ] = rotation.z();
  return delta;
}

4.卡尔曼滤波器初始化

根据输入的参数初始化，默认均值方差都为0

5.1.3 成员变量

  const proto::PoseTrackerOptions options_;     //一些配置参数　各个方差之类的
  const ModelFunction model_function_;          //滤波器预测函数 2D or 3D
  common::Time time_;                           //上一次更新的时刻
  KalmanFilter kalman_filter_;                  //kalman滤波器
  ImuTracker imu_tracker_;                      //imu_tracker
  OdometryStateTracker odometry_state_tracker_; //odometrystate跟踪器(缓冲区)

5.2 核心函数

5.2.1 得到t时刻估计的机器人位姿和方差

时间 t 不能超前当前时间
调用得到 t 时刻的高斯分布，然后取均值与方差

/**
 * @brief PoseTracker::GetPoseEstimateMeanAndCovariance
 * 得到t时刻，ukf滤波器估计的机器人的位姿和方差
 * @param time          对应的时间
 * @param pose          返回的机器人位姿
 * @param covariance    返回的机器人方差
 */
void PoseTracker::GetPoseEstimateMeanAndCovariance(const common::Time time,
                                                   transform::Rigid3d* pose,
                                                   PoseCovariance* covariance)
{
  const Distribution belief = GetBelief(time);
  *pose = RigidFromState(belief.GetMean());
  static_assert(kMapPositionX == 0, "Cannot extract PoseCovariance.");
  static_assert(kMapPositionY == 1, "Cannot extract PoseCovariance.");
  static_assert(kMapPositionZ == 2, "Cannot extract PoseCovariance.");
  static_assert(kMapOrientationX == 3, "Cannot extract PoseCovariance.");
  static_assert(kMapOrientationY == 4, "Cannot extract PoseCovariance.");
  static_assert(kMapOrientationZ == 5, "Cannot extract PoseCovariance.");
  *covariance = belief.GetCovariance().block<6, 6>(0, 0);
  covariance->block<2, 2>(3, 3) +=
      options_.imu_gravity_variance() * Eigen::Matrix2d::Identity();
}

1.得到时刻t的位姿的分布

得到时刻t的位姿的分布　得到的位姿分布，也就得到了位姿

首先进行了时间的预测，然后从UKF中得到置信度

PoseTracker::Distribution PoseTracker::GetBelief(const common::Time time)
{
  Predict(time);
  return kalman_filter_.GetBelief();
}

2.根据时间 time 的预测

/**
 * @brief PoseTracker::Predict
 * 预测time时刻的系统的状态
 * 滤波器预测函数　因此这这个系统中　imu中的数据都是用来预测系统位置的．
 * 因此每次imu数据的更新都需要调用这个函数来对系统状态进行新的预测
 * 这个预测函数会根据系统指定的model_function来自动选择系统的预测方程
 * @param time  对应的时刻
 */
void PoseTracker::Predict(const common::Time time)
{
  imu_tracker_.Predict(time);
  CHECK_LE(time_, time);
  const double delta_t = common::ToSeconds(time - time_);
  if (delta_t == 0.)
  {
    return;
  }
  kalman_filter_.Predict(
      [this, delta_t](const State& state) -> State {
        switch (model_function_)
        {
          case ModelFunction::k2D:
            return ModelFunction2D(state, delta_t);
          case ModelFunction::k3D:
            return ModelFunction3D(state, delta_t);
          default:
            LOG(FATAL);
        }
      },
      BuildModelNoise(delta_t));
  time_ = time;
}

构建间隔时间为t的噪声模型

/**
 * @brief PoseTracker::BuildModelNoise
 * 构建间隔时间为t的噪声模型
 * 该噪声模型的噪声随着时间间隔的增大而增大
 * @param delta_t   对应的时间间隔
 * @return
 */
const PoseTracker::Distribution PoseTracker::BuildModelNoise(
    const double delta_t) const
{
  // Position is constant, but orientation changes.
  StateCovariance model_noise = StateCovariance::Zero();

  model_noise.diagonal() <<
      // Position in map.
      options_.position_model_variance() * delta_t,
      options_.position_model_variance() * delta_t,
      options_.position_model_variance() * delta_t,

      // Orientation in map.
      options_.orientation_model_variance() * delta_t,
      options_.orientation_model_variance() * delta_t,
      options_.orientation_model_variance() * delta_t,

      // Linear velocities in map.
      options_.velocity_model_variance() * delta_t,
      options_.velocity_model_variance() * delta_t,
      options_.velocity_model_variance() * delta_t;

  return Distribution(State::Zero(), model_noise);
}

二维平面系统的预测函数

// A specialization of ModelFunction3D that limits the z-component of position
// and velocity to 0.
/**
 * @brief ModelFunction2D
 * 二维平面系统的预测函数　该函数被PoseTracker的Predict()函数调用
 * 主要功能为根据系统当前时刻的状态state　预测delta_t时刻后系统的状态
 * 与上面不同的是，这是二维平面的．
 * @param state     系统当前时刻的状态
 * @param delta_t   对应的delta_t
 * @return
 */
PoseTracker::State ModelFunction2D(const PoseTracker::State& state,
                                   const double delta_t)
{
  auto new_state = ModelFunction3D(state, delta_t);
  new_state[PoseTracker::kMapPositionZ] = 0.;
  new_state[PoseTracker::kMapVelocityZ] = 0.;
  new_state[PoseTracker::kMapOrientationX] = 0.;
  new_state[PoseTracker::kMapOrientationY] = 0.;
  return new_state;
}

2.根据时间 time 的预测

5.2.2 从state中计算出来一个刚体变换

从机器人的位姿生成一个转换矩阵

/**
 * @brief PoseTracker::RigidFromState
 * 从机器人的位姿生成一个转换矩阵
 * @param state 机器人的位姿
 * @return
 */
transform::Rigid3d PoseTracker::RigidFromState(
    const PoseTracker::State& state)
{
  return transform::Rigid3d(
      Eigen::Vector3d(state[PoseTracker::kMapPositionX],
                      state[PoseTracker::kMapPositionY],
                      state[PoseTracker::kMapPositionZ]),
      transform::AngleAxisVectorToRotationQuaternion(
          Eigen::Vector3d(state[PoseTracker::kMapOrientationX],
                          state[PoseTracker::kMapOrientationY],
                          state[PoseTracker::kMapOrientationZ])) *
          imu_tracker_.orientation());
}

5.2.3 添加一个IMU观测

加速度观测

/**
 * @brief PoseTracker::AddImuLinearAccelerationObservation
 * 增加一个imu的加速度读数
 * @param time                      加速度计读数对应的时间
 * @param imu_linear_acceleration　　加速度计读数
 */
void PoseTracker::AddImuLinearAccelerationObservation(
    const common::Time time, const Eigen::Vector3d& imu_linear_acceleration)
{
  //更新imu＿tracker对应的linearAcc
  imu_tracker_.AddImuLinearAccelerationObservation(time,
                                                   imu_linear_acceleration);
  //更新系统状态(机器人位姿变量)
  Predict(time);
}

角度观测

/**
 * @brief PoseTracker::AddImuAngularVelocityObservation
 * 增加一个imu的角速度观测
 * @param time                  角速度观测对应的时间
 * @param imu_angular_velocity  角速度观测
 */
void PoseTracker::AddImuAngularVelocityObservation(
    const common::Time time, const Eigen::Vector3d& imu_angular_velocity)
{
  //更新imu_tracker对应的angularVel
  imu_tracker_.AddImuAngularVelocityObservation(time, imu_angular_velocity);
  //预测时刻time的系统状态
  Predict(time);
}

5.2.4 添加一Pose观测

这个真正的ukf　pose_tracker的观测方程．前面的imu的观测对于整个滤波器来说，都只是预测

/**
 * @brief PoseTracker::AddPoseObservation
 * 增加一个地图坐标系的位姿观测　一般这个观测来自scan-match　或者　里程计读数
 * 这个才真正称得上ukf滤波器的观测值．前面的imu读数是用来更新ukf滤波器的预测值的
 * @param time          对应的时刻t
 * @param pose          对应的位姿
 * @param covariance    对应的协方差矩阵
 */
void PoseTracker::AddPoseObservation(const common::Time time,
                                     const transform::Rigid3d& pose,
                                     const PoseCovariance& covariance)
{
  //ukf滤波器进行预测
  Predict(time);

  // Noise covariance is taken directly from the input values.
  // 构建噪声分布
  const GaussianDistribution<double, 6> delta(
      Eigen::Matrix<double, 6, 1>::Zero(), covariance);

  //ukf融合观测进行滤波器的更新
  kalman_filter_.Observe<6>(
      [this, &pose](const State& state) -> Eigen::Matrix<double, 6, 1> {
        const transform::Rigid3d state_pose = RigidFromState(state);
        const Eigen::Vector3d delta_orientation =
            transform::RotationQuaternionToAngleAxisVector(
                pose.rotation().inverse() * state_pose.rotation());
        const Eigen::Vector3d delta_translation =
            state_pose.translation() - pose.translation();
        Eigen::Matrix<double, 6, 1> return_value;
        return_value << delta_translation, delta_orientation;
        return return_value;
      },
      delta);
}/**
 * @brief PoseTracker::AddPoseObservation
 * 增加一个地图坐标系的位姿观测　一般这个观测来自scan-match　或者　里程计读数
 * 这个才真正称得上ukf滤波器的观测值．前面的imu读数是用来更新ukf滤波器的预测值的
 * @param time          对应的时刻t
 * @param pose          对应的位姿
 * @param covariance    对应的协方差矩阵
 */
void PoseTracker::AddPoseObservation(const common::Time time,
                                     const transform::Rigid3d& pose,
                                     const PoseCovariance& covariance)
{
  //ukf滤波器进行预测
  Predict(time);

  // Noise covariance is taken directly from the input values.
  // 构建噪声分布
  const GaussianDistribution<double, 6> delta(
      Eigen::Matrix<double, 6, 1>::Zero(), covariance);

  //ukf融合观测进行滤波器的更新
  kalman_filter_.Observe<6>(
      [this, &pose](const State& state) -> Eigen::Matrix<double, 6, 1> {
        const transform::Rigid3d state_pose = RigidFromState(state);
        const Eigen::Vector3d delta_orientation =
            transform::RotationQuaternionToAngleAxisVector(
                pose.rotation().inverse() * state_pose.rotation());
        const Eigen::Vector3d delta_translation =
            state_pose.translation() - pose.translation();
        Eigen::Matrix<double, 6, 1> return_value;
        return_value << delta_translation, delta_orientation;
        return return_value;
      },
      delta);
}
### 5.2.5 添加一里程计观测
这里面会调用上面的函数来对滤波器进行更新，说明把里程计的数据也当做观测数据． 这样一般来说要里程计数据的精度比较高才行．
```c
// Updates from the odometer are in the odometer's map-like frame, called the
// 'odometry' frame. The odometer_pose converts data from the map frame
// into the odometry frame.
/**
 * @brief PoseTracker::AddOdometerPoseObservation
 * 增加一个里程计坐标系的位姿观测
 * 这里面会调用上面的滤波器更新函数
 * @param time              对应的时刻
 * @param odometer_pose     里程计位姿
 * @param covariance        里程计位姿对应的协方差
 */
void PoseTracker::AddOdometerPoseObservation(
    const common::Time time, const transform::Rigid3d& odometer_pose,
    const PoseCovariance& covariance)
{
  /*如果里程计缓冲区里面有数据*/
  if (!odometry_state_tracker_.empty())
  {
    /*计算里程计的增量*/
    const auto& previous_odometry_state = odometry_state_tracker_.newest();
    const transform::Rigid3d delta =
        previous_odometry_state.odometer_pose.inverse() * odometer_pose;

    //根据里程计得到机器人的位姿 PS:下面是state_pose　不是odometer_pose
    //state_pose表示机器人的状态
    //odometer_pose表示机器人的里程计状态
    const transform::Rigid3d new_pose =
        previous_odometry_state.state_pose * delta;
    AddPoseObservation(time, new_pose, covariance);
  }

  //计算t时刻的分布　相当于计算ukf滤波器time时刻估计的位姿
  const Distribution belief = GetBelief(time);

  /*往里程计缓冲区中加入一个里程计观测　加入了里程计观测　和　ukf滤波器预测*/
  odometry_state_tracker_.AddOdometryState(
      {time, odometer_pose, RigidFromState(belief.GetMean())});
}

cartographer 老版本 UKF

1.kalman_filter定义

1.1.高斯分布类

1.1.1定义高斯分布类

1.1.2两个高斯分布相加

1.1.3高斯分布乘以一个数／数组

2.里程计

2.1里程计状态

2.2里程计状态跟踪器

2.2.1类申明

2.2.2类定义

增加里程计数量到队列

3.位姿推算

3.1 基本类型

3.2 ImuTracker

3.2.1成员变量

3.2.2核心函数

1.根据时间预测角度，重力方向

2. 增加线性加速度观测到IMU跟踪器中

3.增加角速度观测到跟踪器中

4.无迹卡尔曼 UKF实现过程

4.1 普通函数定义

4.1.1.返回一个数的平方

4.1.2.返回一个向量的外积.

4.1.3.判断矩阵Ａ是否为一个对称矩阵

4.1.4.返回正定对称矩阵的开方

4.2无迹卡尔曼类

4.2.1构造函数

4.2.2成员变量

4.3 该类中的核心函数

4.3.1 ukf的预测过程

4.3.2 ukf的观测过程

5.PoseTracker

5.1成员变量

5.1.1枚举

1.系统的状态

2.环境模型

3.别名

5.1.2 构造

1.卡尔曼初始化

2.AddDelta

3.ComputeDelta

4.卡尔曼滤波器初始化

5.1.3 成员变量

5.2 核心函数

5.2.1 得到t时刻估计的机器人位姿和方差

1.得到时刻t的位姿的分布

2.根据时间 time 的预测

2.根据时间 time 的预测

5.2.2 从state中计算出来一个刚体变换

5.2.3 添加一个IMU观测

5.2.4 添加一Pose观测

猜你喜欢