预积分原理与应用

在视觉惯性系统中，预积分目前是处理惯性传感器的最常用的方法，效果也非常不错。预积分的理论推导部分比较复杂，这次学习我也只是了解了大概，还没有手推一遍，这次主要为了会用，等之后实践经验足后一定要手推一边公式加强理解。
参考资料主要是
1、高翔：自动驾驶与机器人中的SLAM技术：第四章预积分学
2、邱笑晨：预积分总结与公式推导：https://github.com/PetWorm/IMU-Preintegration-Propogation-Doc.git
3、CSDN中的相关博客

1、预积分的定义：

我们不妨假设从关键帧i 和j之间的IMU数据被累计起来（i j之间采用普通积分，不做优化），这个过程通常可以持续若干秒钟。这种被累计起来的观测被称为预积分。
SLAM系统中为了减小优化求解器的负担，采用了关键帧策略，IMU的速率显然要快于关键帧的插入，紧耦合的方式要求共同使用IMU和图像的信息来估计状态量，预积分来协调两者关系：通过重新参数化，把关键帧之间的IMU测量值积分成相对运动的约束，避免了因为初始条件变化造成的重复积分，在下一个关键帧到来之前先对IMU数据积分，谓之预积分。

2、为什么要预积分：

在后端非线性优化的时候，需要优化位姿，每次调整位姿都需要在它们之间重新传递IMU测量值，需要重新积分，这将非常耗时，为了避免重新传递测量值，所以采取预积分策略。
在优化求解的时候，有i、j相邻两关键帧，关键帧i的位姿是相对于世界系而言的，如果直接利用IMU、加速度计示数在关键帧i的位姿上继续进行积分得到j相对于世界坐标系的位姿变换，那么迭代优化时，i的位姿变化了以后，就需要重新进行积分了。而预积分解算的是两帧间的相对位姿，就不用全部重新进行积分，减轻了计算压力。

3、公式推导思路概述

（详细推导可以去参考资料看看，尤其是邱笑晨：预积分总结与公式推导非常详细）

（1）推导测量模型：

在一个IMU系统里，我们考虑它的五个变量：旋转、平移、角速度、线速度与加速度。最终推导得到理想的预积分值：

（2）噪声分离：

将噪声项从预积分理想值中分离出来，使得预积分测量值（由IMU测量数据计算得到）具有理想值“加”白噪声的形式。

（3）噪声项递推关系：

由于噪声项的定义比较复杂，会使用同样的思路来处理各种噪声项，我们会将复杂的噪声项线性化，保留一阶项系数，然后推导线性模型下的协方差矩阵变化。

（4）零偏更新时的预积分测量值更新：

假定预积分观测是随零偏线性变化的。
更新公式：

偏导项计算：

（5）预积分的雅可比矩阵：

残差指的是预积分计算值（由非 IMU 的其他方式猜测/估计的预积分值）与测量值的差别。在进行 optimization 时，将对所有待优化的导航状态进行 lifting（可以理解为“更新”），从而影响预积分计算值和预积分测量值，进而改变残差值，optimization 的最终目的是要使残差（的加权范数）最小。



然后可以参考邱笑晨的文档，剩下的部分就是推导R,V,P的雅可比矩阵，比较多这里就不粘贴了。

4、代码实践

（这部分参考自动驾驶与机器人中的SLAM技术）
基本预积分类实现：

class IMUPreintegration {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    /// 参数配置项
    /// 初始的零偏需要设置，其他可以不改
    struct Options {
        Options() {}
        Vec3d init_bg_ = Vec3d::Zero();  // 初始零偏
        Vec3d init_ba_ = Vec3d::Zero();  // 初始零偏
        double noise_gyro_ = 1e-2;       // 陀螺噪声，标准差
        double noise_acce_ = 1e-1;       // 加计噪声，标准差
    };

    IMUPreintegration(Options options = Options());

    /**
     * 插入新的IMU数据
     * @param imu   imu 读数
     * @param dt    时间差
     */
    void Integrate(const IMU &imu, double dt);

    /**
     * 从某个起始点开始预测积分之后的状态
     * @param start 起始时时刻状态
     * @return  预测的状态
     */
    NavStated Predict(const NavStated &start, const Vec3d &grav = Vec3d(0, 0, -9.81)) const;

    /// 获取修正之后的观测量，bias可以与预积分时期的不同，会有一阶修正
    SO3 GetDeltaRotation(const Vec3d &bg);
    Vec3d GetDeltaVelocity(const Vec3d &bg, const Vec3d &ba);
    Vec3d GetDeltaPosition(const Vec3d &bg, const Vec3d &ba);

   public:
    double dt_ = 0;                          // 整体预积分时间
    Mat9d cov_ = Mat9d::Zero();              // 累计噪声矩阵
    Mat6d noise_gyro_acce_ = Mat6d::Zero();  // 测量噪声矩阵

    // 零偏
    Vec3d bg_ = Vec3d::Zero();
    Vec3d ba_ = Vec3d::Zero();

    // 预积分观测量
    SO3 dR_;
    Vec3d dv_ = Vec3d::Zero();
    Vec3d dp_ = Vec3d::Zero();

    // 雅可比矩阵
    Mat3d dR_dbg_ = Mat3d::Zero();
    Mat3d dV_dbg_ = Mat3d::Zero();
    Mat3d dV_dba_ = Mat3d::Zero();
    Mat3d dP_dbg_ = Mat3d::Zero();
    Mat3d dP_dba_ = Mat3d::Zero();
};

单个IMU积分函数：

void IMUPreintegration::Integrate(const IMU &imu, double dt) {
    // 去掉零偏的测量
    Vec3d gyr = imu.gyro_ - bg_;  // 陀螺
    Vec3d acc = imu.acce_ - ba_;  // 加计

    // 更新dv, dp, 见(4.13), (4.16)
    dp_ = dp_ + dv_ * dt + 0.5f * dR_.matrix() * acc * dt * dt;
    dv_ = dv_ + dR_ * acc * dt;

    // dR先不更新，因为A, B阵还需要现在的dR

    // 运动方程雅可比矩阵系数，A,B阵，见(4.29)
    // 另外两项在后面
    Eigen::Matrix<double, 9, 9> A;
    A.setIdentity();
    Eigen::Matrix<double, 9, 6> B;
    B.setZero();

    Mat3d acc_hat = SO3::hat(acc);
    double dt2 = dt * dt;

    // NOTE A, B左上角块与公式稍有不同
    A.block<3, 3>(3, 0) = -dR_.matrix() * dt * acc_hat;
    A.block<3, 3>(6, 0) = -0.5f * dR_.matrix() * acc_hat * dt2;
    A.block<3, 3>(6, 3) = dt * Mat3d::Identity();

    B.block<3, 3>(3, 3) = dR_.matrix() * dt;
    B.block<3, 3>(6, 3) = 0.5f * dR_.matrix() * dt2;

    // 更新各雅可比，见式(4.39)
    dP_dba_ = dP_dba_ + dV_dba_ * dt - 0.5f * dR_.matrix() * dt2;                      // (4.39d)
    dP_dbg_ = dP_dbg_ + dV_dbg_ * dt - 0.5f * dR_.matrix() * dt2 * acc_hat * dR_dbg_;  // (4.39e)
    dV_dba_ = dV_dba_ - dR_.matrix() * dt;                                             // (4.39b)
    dV_dbg_ = dV_dbg_ - dR_.matrix() * dt * acc_hat * dR_dbg_;                         // (4.39c)

    // 旋转部分
    Vec3d omega = gyr * dt;         // 转动量
    Mat3d rightJ = SO3::jr(omega);  // 右雅可比
    SO3 deltaR = SO3::exp(omega);   // exp后
    dR_ = dR_ * deltaR;             // (4.9)

    A.block<3, 3>(0, 0) = deltaR.matrix().transpose();
    B.block<3, 3>(0, 0) = rightJ * dt;

    // 更新噪声项
    cov_ = A * cov_ * A.transpose() + B * noise_gyro_acce_ * B.transpose();

    // 更新dR_dbg
    dR_dbg_ = deltaR.matrix().transpose() * dR_dbg_ - rightJ * dt;  // (4.39a)

    // 增量积分时间
    dt_ += dt;
}