IMU状态预积分代码实现 —— IMU状态预积分类

实现IMU状态预积分类

实现IMU状态预积分类

首先，实现预积分自身的结构。一个预积分类应该存储一下数据：

预积分的观测量 $\bigtriangleup \tilde{R} _{ij},\bigtriangleup \tilde{v} _{ij},\bigtriangleup \tilde{p} _{ij}$
预积分开始时的IMU零偏 $b_{g},b_{a}$
在积分时期内的测量噪声 $\Sigma _{i,k+1}$
各积分量对IMU零偏的雅克比矩阵
整个积分时间 $\bigtriangleup t_{ij}$

以上都是必要的信息。除此之外，也可以将IMU的读数记录在预积分类中（当然，也可以不记录，因为都已经积分过了）。同时，IMU的测量噪声和零偏随机游走噪声也可以作为配置参数，写在预积分类中。

声明这个类

class IMUPreintegration {

参数配置项
其中包括：

陀螺仪初始零偏
加速度计初始零偏
陀螺噪声
加计噪声

    /// 参数配置项
    /// 初始的零偏需要设置，其他可以不改
    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());

中间省略函数的声明，之后再写。

下面完成类的成员变量定义
整体预积分时间

    double dt_ = 0;                          // 整体预积分时间

噪声矩阵，累积噪声矩阵 $\Sigma _{i,k+1}$ ，测量噪声矩阵 $Cov(\eta_{d,k} )$

    Mat9d cov_ = Mat9d::Zero();              // 累计噪声矩阵
    Mat6d noise_gyro_acce_ = Mat6d::Zero();  // 测量噪声矩阵

预积分开始时的IMU零偏 $b_{g},b_{a}$

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

预积分的观测量 $\bigtriangleup \tilde{R} _{ij},\bigtriangleup \tilde{v} _{ij},\bigtriangleup \tilde{p} _{ij}$

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

各积分量对IMU零偏的雅克比矩阵

    // 雅可比矩阵
    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零偏相关的噪声项并不直接和预积分类有关，所以将它们挪到优化类当中。这个类主要完成对IMU数据进行预积分操作，然后提供积分之后的观测量与噪声值。

下面来看单个IMU的积分函数，首先在类中进行声明。

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

来看函数具体实现

整体而言，它按照以下顺序更新内部的成员变量：

更新位置和速度的测量值
更新运动模型的噪声矩阵
更新观测量对零偏的各雅克比矩阵
更新旋转部分的测量值
更新积分时间在这里插入代码片

void IMUPreintegration::Integrate(const IMU &imu, double dt) {

去掉零偏的测量

    Vec3d gyr = imu.gyro_ - bg_;  // 陀螺
    Vec3d acc = imu.acce_ - ba_;  // 加计

更新预积分速度观测量和位置观测量

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

对应公式为
在这里插入图片描述

预积分旋转观测 dR先不更新，因为A, B阵还需要现在的dR

下面计算运动方程雅克比矩阵系数A、B阵，用于更新噪声项
在这里插入图片描述

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

加速度计的伴随矩阵和t的平方

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

公式中的这个地方有用到，避免重复计算
在这里插入图片描述

    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();

计算A矩阵中对应的各个块，分别对应公式如下，A矩阵中的A.block<3, 3>(0, 0)块，之后用更新完的dR 更新
在这里插入图片描述

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

更新B矩阵的各块，分别对应公式如下
在这里插入图片描述

    // 更新各雅可比
    dP_dba_ = dP_dba_ + dV_dba_ * dt - 0.5f * dR_.matrix() * dt2;                     
    dP_dbg_ = dP_dbg_ + dV_dbg_ * dt - 0.5f * dR_.matrix() * dt2 * acc_hat * dR_dbg_; 
    dV_dba_ = dV_dba_ - dR_.matrix() * dt;                                             
    dV_dbg_ = dV_dbg_ - dR_.matrix() * dt * acc_hat * dR_dbg_;

更新各雅克比矩阵对应公式依次为：
在这里插入图片描述

下面更新预积分旋转部分观测量

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

对应公式：
在这里插入图片描述
其中右雅克比矩阵的计算是为了更新上面的B矩阵

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

利用更新完的dR和右雅克比矩阵更新A、B阵中对应的块
对应公式：
在这里插入图片描述

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

利用填充好的A阵和B阵，来更新噪声项
对应公式如下：
在这里插入图片描述
其中 $Cov(\eta_{d,k} )$ 即代码中的noise_gyro_acce_的构成就是陀螺仪和加计的噪声构成的对角矩阵，在构造函数中构成的

    const float ng2 = options.noise_gyro_ * options.noise_gyro_;
    const float na2 = options.noise_acce_ * options.noise_acce_;
    noise_gyro_acce_.diagonal() << ng2, ng2, ng2, na2, na2, na2;

下则继续更新预积分旋转观测量对陀螺仪零偏的雅克比矩阵

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

对应公式如下：
在这里插入图片描述

最后增加积分时间：

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

这样就完成了一次对IMU数据的操作。需要注意的是，如果不进行优化，则预积分和直接积分的效果是完全一致的，都是将IMU数据一次性地积分。在预积分之后，也可以向ESKF一样，从起始状态向最终状态进行预测。

预测函数实现如下：

    /**
     * 从某个起始点开始预测积分之后的状态
     * @param start 起始时时刻状态
     * @return  预测的状态
    */
    NavStated IMUPreintegration::Predict(const sad::NavStated &start, const Vec3d &grav) const {
        SO3 Rj = start.R_ * dR_;
        Vec3d vj = start.R_ * dv_ + start.v_ + grav * dt_;
        Vec3d pj = start.R_ * dp_ + start.p_ + start.v_ * dt_ + 0.5f * grav * dt_ * dt_;

        auto state = NavStated(start.timestamp_ + dt_, Rj, pj, vj);
        state.bg_ = bg_;
        state.ba_ = ba_;
        return state;
    }

与ESKF不同的是，预积分可以对多个IMU数据进行预测，可以从任意起始时刻向后预测，而ESKF通常只在当前状态下，针对单个IMU数据，向下一时刻预测。

获取修正之后的观测量，bias可以与预积分时期的不同，会有一阶修正

// 预积分旋转零偏更新修正后测量值
SO3 IMUPreintegration::GetDeltaRotation(const Vec3d &bg) { return dR_ * SO3::exp(dR_dbg_ * (bg - bg_)); }

对应公式为：
在这里插入图片描述
预积分速度零偏更新修正后测量值

    // 预积分速度零偏更新修正后测量值
    Vec3d IMUPreintegration::GetDeltaVelocity(const Vec3d &bg, const Vec3d &ba) {
        return dv_ + dV_dbg_ * (bg - bg_) + dV_dba_ * (ba - ba_);
    }

对应公式为：
在这里插入图片描述
预积分位置零偏更新修正后测量值

    // 预积分位置零偏更新修正后测量值
    Vec3d IMUPreintegration::GetDeltaPosition(const Vec3d &bg, const Vec3d &ba) {
        return dp_ + dP_dbg_ * (bg - bg_) + dP_dba_ * (ba - ba_);
    }