高翔【自动驾驶与机器人中的SLAM技术】学习笔记（九）imu运动学；lambda表达式；bind；function；std::move()

一、IMU运动学

1、测量值：

常用六轴IMU是由陀螺仪（Gyroscope）和加速度计（Acclerator）两部分组成。

• 陀螺仪测量：角速度。
• 加速度计：加速度。

安装要尽量保证IMU的安装位置在车辆中心。避免由IMU与载体系不重合引来的问题。

2、噪声模型

IMU噪声由两部分组成：

• 测量噪声（Measurement Noise）。
• 零偏(Bias)。
  • 哪怕车辆静止：加速度和角速度测量值均值也不为零，而是带有一定的偏移。
  • 偏移量是由IMU内部的机电测量装置导致的。
  • 建立数学模型，认为零偏也是系统的状态量。

 

测量值：数学模型为：理论值 + 零偏 + 噪声。 

• 噪声：是一个高斯过程，协方差随着时间变得越来越大，IMU本身的测量值也随着采样时间变长而变得更加不准确。因此采样频率越高的IMU，其精度也会相对较高。
  • 角速度就是角度的导数，加速度又是速度的导数。所以 IMU 的测量噪声，也可以解释为角度的随机游走和速度的随机游走。因此请不要看到随机游走这四个字，只想到零偏部分，而应该从整体层面看待问题。
• 零偏：随机游走/布朗运动/维纳过程。而零偏部分由布朗运动描述，呈现随机游走状态。表现在实际当中，则可以认为一个 IMU 的零偏会从某个初始值开始，随机地向附近做不规律的运动。运动的幅度越大，就称它的零偏越不稳定。所以质量好的IMU，零偏应该保持在初始值附近不动。
  • 随机游走实际上就是导数为高斯过程的随机过程，

3、思维教育

请读者注意，这里的高斯过程和布朗运动过程，都是IMU 测量数据的数学模型。数学模型并不一定是和真实世界完全对应的。有时，数学模型是对真实世界的一种简化，便于后续的算法计算。读者应当理解这种思想。后面我们对许多系统进行线性近似，保留各种一阶项，也是基于这种简化的思想。真实 IMU 的测量噪声和零偏受到非常多因素的影响，例如载体的震动、温度，IMU自身的受力、标定与安装误差，等等。把它们建模为两个随机过程，更多是为了方便状态估计算法的计算，不是完美、精确的建模方式。这种先简化、再补偿的思想，在现实中十分常见。

我想起一句名言：为啥研究各种线性假设：因为非线性的大家数学水平都达不到。所以都会把问题转为线性近似。直白点说：一阶泰勒公式展开。局部有效。不断逼近。

总结一句：差不多得了。

4、离散时间噪声模型：

这个离散的是咱们代码最感兴趣的。其中时间间隔对应代码中dt。 

5、查看IMU手册： 

噪声和零偏参考值。

---

---

二、IMU航迹推算

通过IMU的测量值，估算推断系统的运动。

只有IMU 时如何推断系统的运动状态。我们发现这样做是可行的，但只有IMU 的系统需要对 IMU 读数进行二次积分，其测量误差与零偏的存在会导致状态变量很快地漂移。

1、PVQ状态方程

增量形式：

这个公式，特别是公式3.15，编程要用的。

2、累计运动

三、IMU递推代码实验

1、IMU代码

首先看IMU这个类/结构体：在common/imu.h中。在common中，说明后面的IMU都是这个结构。

//
// Created by xiang on 2021/7/19.
//

#ifndef MAPPING_IMU_H
#define MAPPING_IMU_H

#include <memory>
#include "common/eigen_types.h"

namespace sad {

/// IMU 读数
struct IMU {
    IMU() = default;
    IMU(double t, const Vec3d& gyro, const Vec3d& acce) : timestamp_(t), gyro_(gyro), acce_(acce) {}

    double timestamp_ = 0.0;
    Vec3d gyro_ = Vec3d::Zero();   // gyroscope 陀螺仪
    Vec3d acce_ = Vec3d::Zero();   // accelerometer 加速度计
};
}  // namespace sad

using IMUPtr = std::shared_ptr<sad::IMU>;

#endif  // MAPPING_IMU_H

属性：分别是：

• 时间戳timestamp_
• 陀螺仪数值gyro_
• 加速度计数值acce_

然后这三个属性的构造函数。以及基于这个类别的智能指针。

---

2、导航状态NavState

在看书中代码之前，还需要看一个类：NavStated状态变量这个泛型类。也在common中，nav_state.h

//
// Created by xiang on 2021/7/19.
//

#ifndef SAD_NAV_STATE_H
#define SAD_NAV_STATE_H

#include <sophus/so3.hpp>
#include "common/eigen_types.h"

namespace sad {

/**
 * 导航用的状态变量
 * @tparam T    标量类型
 *
 * 这是个封装好的类，部分程序使用本结构体，也有一部分程序使用散装的pvq，都是可以的
 */
template <typename T>
struct NavState {
    using Vec3 = Eigen::Matrix<T, 3, 1>;
    using SO3 = Sophus::SO3<T>;

    NavState() = default;

    // from time, R, p, v, bg, ba
    explicit NavState(double time, const SO3& R = SO3(), const Vec3& t = Vec3::Zero(), const Vec3& v = Vec3::Zero(),
                      const Vec3& bg = Vec3::Zero(), const Vec3& ba = Vec3::Zero())
        : timestamp_(time), R_(R), p_(t), v_(v), bg_(bg), ba_(ba) {}

    // from pose and vel
    NavState(double time, const SE3& pose, const Vec3& vel = Vec3::Zero())
        : timestamp_(time), R_(pose.so3()), p_(pose.translation()), v_(vel) {}

    /// 转换到Sophus
    Sophus::SE3<T> GetSE3() const { return SE3(R_, p_); }

    friend std::ostream& operator<<(std::ostream& os, const NavState<T>& s) {
        os << "p: " << s.p_.transpose() << ", v: " << s.v_.transpose()
           << ", q: " << s.R_.unit_quaternion().coeffs().transpose() << ", bg: " << s.bg_.transpose()
           << ", ba: " << s.ba_.transpose();
        return os;
    }

    double timestamp_ = 0;    // 时间
    SO3 R_;                   // 旋转