IMU状态预积分功能实现与测试

前言
实现IMU状态预积分类
测试程序验证预积分与直接积分的效果
结果

前言

在这里插入图片描述

预积分：是一种十分常见的IMU数据处理方法。

与传统的IMU运动学积分不同，预积分可以将一段时间内的IMU测量数据累积，建立预积分测量，同时还能保证测量值与状态变量无关。

如果以吃饭来比喻的话，ESKF像是一口口地吃菜，而预积分则是从锅里先把菜一块块夹到碗里，再把碗里的菜一口气吃掉。

无论是LIO系统还是VIO系统，预积分已经称为诸多紧耦合IMU系统的标准方法。

在ESKF中，将两个GNSS观测之间的IMU数据进行积分，作为ESKF的预测过程。
这种做法把IMU数据看成某种一次性的使用方式：将它们积分到当前估计值上，然后用观测数据更新当时的估计值。
这种做法和此时的状态估计值有关。如果状态量发送了改变，能否重复利用这些IMU数据呢？
从物理意义上看，IMU反映的是两个时刻间车辆的角度变化量和速度变化量。如果希望IMU的计算与当时的状态估计无关，那么应该用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, dpdp_ = 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_dbgdR_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数据，向下一时刻预测。

测试程序验证预积分与直接积分的效果

下面写一个测试程序，验证在单个方向上存在固定角速度与加速度时，预积分与直接积分的效果是否有明显差异。

如果起始状态相同，则它们得到的结果也完全一致。

类内部定义一个测试功能函数

    void IMUPreintegration::Test_Fuction(void){

设定imu的数据频率为100hz，所以时间间隔为0.01s。设置一个饶z轴的旋转角速度，与沿x轴的加速度，还有就是重力大小

        // 测试在恒定角速度与加速度 运转下的预积分情况double imu_time_span = 0.01;       // IMU测量间隔Vec3d constant_omega(0, 0, M_PI);  // 角速度为180度/s，转1秒应该等于转180度Vec3d constant_acce(0.1, 0, 0);  // x 方向上的恒定加速度Vec3d gravity(0, 0, -9.81);         // Z 向上，重力方向为负

声明一个起始状态和一次结束状态

        // 声明一个起始状态和一次结束状态 sad::NavStated start_status(0), end_status(1.0);

声明直接积分状态

        // 声明 直接积分 状态Sophus::SO3d R;Vec3d p = Vec3d::Zero();Vec3d v = Vec3d::Zero();

通过循环，预积分不断的更新，直接积分也不断的更新

        for (int i = 1; i <= 100; ++i) {double time = imu_time_span * i;Vec3d acce = constant_acce - gravity;  // 加速度计应该测量到一个向上的力Integrate(sad::IMU(time, constant_omega, acce), imu_time_span);sad::NavStated this_status = Predict(start_status, gravity);p = p + v * imu_time_span + 0.5 * gravity * imu_time_span * imu_time_span +0.5 * (R * acce) * imu_time_span * imu_time_span;v = v + gravity * imu_time_span + (R * acce) * imu_time_span;R = R * Sophus::SO3d::exp(constant_omega * imu_time_span);}

获得预积分最终状态

        // 获得预积分最终状态end_status = Predict(start_status,gravity);

打印各方法的数据，以进行比较

        std::cout << "preinteg result: "<<std::endl;std::cout << "end rotation: \n" << end_status.R_.matrix()<<std::endl;std::cout << "end trans: \n" << end_status.p_.transpose()<<std::endl;std::cout << "end v: \n" << end_status.v_.transpose()<<std::endl;std::cout << "direct integ result: "<<std::endl;std::cout << "end rotation: \n" << R.matrix()<<std::endl;std::cout << "end trans: \n" << p.transpose()<<std::endl;std::cout << "end v: \n" << v.transpose()<<std::endl;

结果

preinteg result:
end rotation:
-1 3.1225e-16 0
-3.1225e-16 -1 0
0 0 1
end trans:
0.0207609 0.0315101 -4.44089e-15
end v:
0.001 0.0636567 -1.77636e-15
direct integ result:
end rotation:
-1 3.1225e-16 0
-3.1225e-16 -1 0
0 0 1
end trans:
0.0207609 0.0315101 0
end v:
0.001 0.0636567 0