高翔【自动驾驶与机器人中的SLAM技术】学习笔记（十一）ESKF中融合速度观测量；发散的原因；如何解决发散；以及对slam的理解

带着问题去学习：

1、slam发散的原因？

2、如何解决/限制发散？

3、如何在已经有观察值和预测值的ESKF中，再引入一个其他其他观察量？

一、多传感器融合的思考——轮速计

反思为何需要融合多个传感器？

我认为根本上的原因，是因为有些传感器在某些场景下会失灵、效果不佳。

比如RTK在过隧道时，无法使用，单IMU工作下，里程计很多就发散了。

发散的原因是缺少速度的观测。所以引入了轮速计。

1. 位移单位时间采样即为速度。
2. 轮速观测是在前进方向的上的观测；两个轮子的位移差，结合车身尺寸计算车身的旋转。
3. 颠簸路面，轮式编码器不太适用。平路slam效果更佳。
4. 编码器也有效果不佳时——打滑时。融合可以纠正。

轮速观测作为一个速度观测，并非XYZ三个方向上的速度观测，只观测了前进方向上的速度。对IMU有一个单一约束，就可以使得整体位姿没有那么容易发散。

template <typename S>
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 obs
    double velo_l =