高翔【自动驾驶与机器人中的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 =

原文地址：https://blog.csdn.net/xiaoyaolangwj/article/details/142619050

免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！