IMU预积分（VINS）

1.为什么需要预积分

2.预积分理论

[2.1 预积分模型](#2.1 预积分模型)

[2.2 预积分在优化中的使用](#2.2 预积分在优化中的使用)

[2.2.1 残差设计](#2.2.1 残差设计)

[2.2.2 推导残差的雅可比](#2.2.2 推导残差的雅可比)

[2.2.2.1 位置残差的雅可比](#2.2.2.1 位置残差的雅可比)

[2.2.2.2 姿态残差的雅可比](#2.2.2.2 姿态残差的雅可比)

[2.2.2.3 速度残差的雅可比](#2.2.2.3 速度残差的雅可比)

[2.2.2.4 加速度计bias残差的雅可比](#2.2.2.4 加速度计bias残差的雅可比)

[2.2.2.5 陀螺仪bias残差的雅可比](#2.2.2.5 陀螺仪bias残差的雅可比)

[2.2.3 推导残差的方差](#2.2.3 推导残差的方差)

[2.2.3.1 姿态误差递推方程](#2.2.3.1 姿态误差递推方程)

[2.2.3.2 速度误差递推方程](#2.2.3.2 速度误差递推方程)

[2.2.3.3 位置误差递推方程](#2.2.3.3 位置误差递推方程)

[2.2.3.4 加速度计bias误差递推方程](#2.2.3.4 加速度计bias误差递推方程)

[2.2.3.5 陀螺仪bias误差递推方程](#2.2.3.5 陀螺仪bias误差递推方程)

[2.3 预积分项更新](#2.3 预积分项更新)

[2.3.1 预积分项更新](#2.3.1 预积分项更新)

[2.3.2 预积分对Bias的雅可比是如何计算的？](#2.3.2 预积分对Bias的雅可比是如何计算的？)

[3. IMU帧间积分约束的代码实现](#3. IMU帧间积分约束的代码实现)

[3.1 帧间惯性解算以及方差和预积分项雅可比递推](#3.1 帧间惯性解算以及方差和预积分项雅可比递推)

[3.2 二元约束边定义](#3.2 二元约束边定义)

[3.3 计算残差](#3.3 计算残差)

[3.4 计算雅可比](#3.4 计算雅可比)

注：本篇笔记的主体内容来源于对深蓝学院《多传感器融合定位》课程的学习，推荐有一定基础的 SLAM 初学者学习该课程。课程中提供了一个完整的带有imu预积分的后端优化框架，可供体会从理论到代码实现的过程。

1.为什么需要预积分

（1）imu的频率远高于Lidar和相机，在Lidar关键帧i和j之间会产生大量数据，并且惯性解算位姿递推时依赖于上一时刻的状态（、、、）：

但i 时刻的状态（位置、姿态、速度、加速度计/陀螺仪bias ）在每次优化迭代后可能发生改变。为了避免每次优化迭代后重新从 i 时刻到 j 时刻进行大量积分，那么就在进行 i 时刻到 j 的位姿递推时，只计算[i , j ]之间的相对状态，作为预积分的结果，只需要在优化完成后，将其叠加到 i 时刻，就得到 j 时刻的状态；

（2）位姿递推过程中包含了bias（图片中的四元数q可以看作是旋转矩阵R。笔记中使用四元数乘法符号进行计算的q是四元数，其他的q都代表R）：

而bias作为状态量在优化过程也会变化，进而导致预积分结果发生变化（需要重新积分）。为了避免重新积分，将预积分结果对bias变化量求偏导（利用预积分量在先验bias处的一阶泰勒展开），计算雅可比，最终可以根据bias的变化量直接计算出新的积分结果。

图中的三个预积分量分别代表位置、速度、姿态。

帧间相对积分项是作为一个二元约束边，去约束图优化中相邻两个节点的状态的。但由于imu测量白噪音和bias随机游走的存在，这个约束是不太准的，需要随着bias的修正而修正。

2.预积分理论

2.1 预积分模型

已知惯性导航在连续时间下的微分方程为：

根据该微分方程，可知从 𝑖 时刻到 𝑗 时刻 IMU的积分结果为：

其中，由于加速度计测量的结果是在载体b系，所以需要先将其根据当前位姿转换到世界w系。并且加速度计测量的是"比力"，所以需要减去重力加速度。

根据预积分的要求，需要求相对结果，而且不依赖于上一时刻位姿，因此需要对上式做转换。由于，把它带入(4)-(6)式可得：

可见，此时积分项就转换成[i , j ]之间的相对状态了，则位姿的起点为。为了整理公式，把积分相关的项用下面的式子代替：

此时，使用代表预积分项，其积分过程为连续时间下，而实际数据是离散的，所以预积分项也可以表示为使用中值积分的离散形式：

经过以上的推导，此时状态更新的公式可以整理为：

需要注意的是，实际情况下陀螺仪和加速度计的bias模型为：

即认为bias是在变化的，这样便于估计不同时刻的bias值，而不是整个系统运行时间内都当做常值对待。这更符合低精度mems的实际情况。但在预积分时，由于两个关键帧之间的时间较短，因此认为 𝑖 和 𝑗 时刻的bias相等。

ai解读：

这里做了一个工程上的合理假设：假设在较短时刻[i , j ]内，IMU的零偏是一个常数。预积分的核心目的是把连续的 IMU 测量值积分为一个相对位姿的确定性数值 （即名义值、、）。

真实世界中，MEMS IMU 的零偏确实是随时间缓慢漂移的（随机游走模型：）。但是，在连续两个关键帧（i 到 j ）之间，时间间隔通常极短（例如 0.1 秒）。在这个极短的窗口期内，物理零偏的变化量微乎其微。

如果不假设恒定： 预积分项的积分号内部的就是一个关于时间的未知函数。这就变成了一个死局------我们连到底怎么变的都不知道，根本无法求出积分的解析解或离散数值解。

假设恒定 （）：这是为了让积分"算得下去"。我们暂且认为这 0.1 秒内零偏"冻结"在 i 时刻的值，这样就可以把含有零偏的项当做常数提出来，顺利算出预积分量。

bias的更新发生在后端优化阶段 。跳出单次积分，来到整个后端的图优化视角，在几秒、几分钟的运行过程中，bias的变化就非常可观了。在因子图中，和是两个完全独立的、待优化的状态量节点。为了让整体残差最小，优化器在求雅可比矩阵并解法向方程（）时，就会倒逼着去调整和的值。

惯性解算的过程中包含了一步，即加速度和角速度减去对应的bias。。在优化过程中，bias作为状态量也会发生变化，从而引起预积分结果变化。为了避免bias变化后，重新做预积分，可以把预积分结果在先验bias处泰勒展开，表达成下面的形式，这样就可以根据bias的变化量直接算出新的预积分结果。

上图中将预积分的结果在bias处进行了泰勒展开，但其雅可比计算的具体怎样实现的看后面。

2.2 预积分在优化中的使用

优化问题中损失函数由残差函数组成：

当考虑方差（权重）时，可以写为：

利用高斯牛顿方法，求解该优化问题，需要解下面的方程：

即，在优化中使用一项信息，需要设计残差，并推导它的雅可比和方差。

损失函数要考虑方差的原因是为了给不同来源的信息设置合理的权重。

在多传感器融合中，不同传感器的测量精度、噪声水平是完全不同的。如果不考虑方差，损失函数退化为普通的欧氏距离求和。这会导致优化器"一视同仁"，可能会让一个误差极大的劣质传感器把整体轨迹拉偏。引入方差后，损失函数变为了基于马氏距离的加权最小二乘形式。

这里的是信息矩阵，它在数学上等于协方差矩阵的逆。

方差越小（极其精准）：说明该测量非常可靠，其对应的信息矩阵权重极大，优化器会拼命调整状态变量去满足这个约束。

方差越大（噪声很大）：说明该测量不确定性高，权重极小，即使残差很大，优化器也会认为"情有可原"，从而不会被带偏。

2.2.1 残差设计

此时状态更新的公式为：

根据"残差=状态量 - 测量值"（状态量可以看作是图优化节点中保存的待优化状态，测量值来自于imu积分结果），可以很容易写出一种残差形式如下：

其中，认为姿态的残差/误差是一个小量，则：

为了不受单位四元数模长为1的约束，并且这个误差的虚部刚好等于旋转误差向量的1/2，所以简化这个残差模型，使用真实的物理旋转误差向量作为残差传入求解器。则：

为了进一步简化过程，对p和v的残差左乘：

ai解读：这么做有三个决定性的好处。

物理等价性：不改变最优解 。从纯数学角度来看，左乘一个正交的旋转矩阵，并不会改变误差向量的模长（即）（在p和v的残差中q 代表旋转矩阵R）。如果优化器能够把世界坐标系下的误差压到0，那么它在载体坐标系下的投影自然也是0。因此，它完全不会改变图优化最终收敛的轨迹。

协方差矩阵的"原生态"对齐 。

解耦状态量与测量量，极大简化雅可比推导。

2.2.2 推导残差的雅可比

此时残差为：

待优化变量为：

但在实际使用中，往往都是使用扰动量，因此实际是对以下扰动量求雅可比：

2.2.2.1 位置残差的雅可比

位置残差中的q代表旋转矩阵R，下面在求导过程中替换为R 。且因为扰动定义在载体b系，所以求导过程采用右乘扰动模型。对上面十个扰动量分别计算雅可比。位置残差与i时刻的位置、姿态、速度、加速度计零偏、陀螺仪零偏、j时刻位置有关。

（1）i时刻的扰动量

（2）j时刻扰动量

2.2.2.2 姿态残差的雅可比

姿态残差中的q代表四元数 。且因为扰动定义在载体b系，所以求导过程采用右乘扰动模型。姿态残差与i时刻姿态、i时刻陀螺仪的零偏、j时刻姿态有关。

（1）i时刻的扰动量

（2）j时刻的扰动量

因为假设在[i, j]的短时间内零偏恒定，所以预积分项只受的影响。

2.2.2.3 速度残差的雅可比

速度残差中的q代表旋转矩阵R，下面在求导过程中替换为R 。且因为扰动定义在载体b系，所以求导过程采用右乘扰动模型。速度残差与i时刻姿态、速度、加速度计零偏、陀螺仪零偏、j时刻速度有关。

（1）i时刻扰动量

（2）j时刻扰动量

2.2.2.4 加速度计bias残差的雅可比

bias残差只与i、j两个时刻的bias有关：

2.2.2.5 陀螺仪bias残差的雅可比

bias残差只与i、j两个时刻的bias有关：

2.2.3 推导残差的方差

在融合时，需要给不同信息设置权重，而权重由方差的逆得来。离散时间下的协方差传递方程为：

是从i时刻累积到当前k时刻的协方差（不确定性），方差初值为0：

是已有误差的自然传播，过去积攒的误差随着系统的运动学演化，矩阵描述了状态的转移规律；

是在k时刻新注入的传感器噪声，为k时刻的噪声系数矩阵，为IMU测量白噪音和零偏随机游走的协方差矩阵。

IMU的测量是高频的，在两个关键帧i和j之间可能包含了几十甚至上百个IMU数据点。每一个单独的数据点都带有微小的白噪声。随着时间的推移，这些微小的噪声会像滚雪球一样不断累积、放大。

和的推导如下：

连续时间下的误差微分方程一般写为如下形式：

其中，分别代表位置、姿态、速度、加速度计bias、陀螺仪bias，分别代表测量白噪音和bias随机游走。

根据惯性导航解算及误差分析的推导，连续时间下每个误差状态的微分方程为：

虽然物理世界是连续的，在真实世界中，机器人的运动、姿态的变化、误差的累积，都是随着时间t 连续发生的平滑曲线，但由于数据采集是离散的，所以需要对微分方程进行离散化，得到离散时间下的状态递推方程：

根据导数的定义，当时间间隔非常短时，状态x的导数可以近似表示为：

则：

接下来，对每个状态分量显式地推导其递推方程的形式，运用中值积分进行离散化近似。由于惯性解算有顺序，所以先推姿态，再速度、位置、bias。

2.2.3.1 姿态误差递推方程

其中，为陀螺仪的测量白噪音，为陀螺仪的零偏。没有对陀螺仪的零偏使用中值积分。

2.2.3.2 速度误差递推方程

则离散时间下应该有：

把前面求得的的表达式代入上式，可得：

经过一系列合并同类项以后，最终的合并结果为：

由导数形式可以得到递推形式如下：

其中，为加速度计的测量白噪音，为加速度计的零偏。

需要注意的是，在使用中值积分进行离散化的过程中，对旋转、加速度、测量白噪音使用了中值积分，没有对加速度计的零偏使用中值积分 。并且在对R进行中值积分时直接对旋转矩阵R进行相加并除以2。

ai解读：

2.2.3.3 位置误差递推方程

则离散时间下应该有：

由导数形式可以写出递推形式：

2.2.3.4 加速度计bias误差递推方程

2.2.3.5 陀螺仪bias误差递推方程

根据上述推导，完整的状态递推方程为：

上面的矩阵中，有：

这里就相较于误差状态卡尔曼滤波（ESKF）推导中使用的一阶泰勒展开，得到了一个更麻烦但准确的离散状态下误差状态的递推公式。

2.3 预积分项更新

2.3.1 预积分项更新

在优化后，imu零偏发生改变，但之前计算的预积分量是基于旧的bias积分得到的。为了避免重新积分，预积分理论对预积分值在先验bias处进行了一阶泰勒展开，得到预积分值对bias的雅可比。当优化器给出一个微小的bias变化量时，直接利用雅可比矩阵计算出一个线性补偿量，叠加到原预积分结果上：

完成预积分结果相对bias变化的快速更新。

2.3.2 预积分对Bias的雅可比是如何计算的？

ai解读：

1.无法直接求导的困境

雅可比没有明确的闭式解：因为预积分是一个跨越很长时间、包含成百上千次 IMU 采样的累加过程。比如，陀螺仪 Bias 的一点点变化，会影响第一步的姿态；第一步的姿态变了，会影响第二步重力在载体系下的投影；这又会影响第二步的加速度，进而影响第三步的速度和位移......这种复杂的"蝴蝶效应"导致我们写不出一个简单的解析代数式。

2.巧妙借用误差状态传递矩阵

既然一步求不出来，我们就跟着IMU积分一步一步地递推出来！

在前面推导预积分方差更新时，我们得到了一个包含系统所有误差演化规律的离散传递方程：

如果我们忽略噪声，两边对初始状态求导，根据微积分的链式法则，雅可比矩阵的递推公式呼之欲出：

（1）具体步骤：

1）起点（ k = i 时刻）： 刚开始预积分时，系统没有任何累计误差，此时雅可比矩阵初始化为一个的单位矩阵（因为状态量的总维度是15）：

2）循环递推 ：之后每收到一帧IMU数据，我们就根据当前的测量值算出对应的15x15的矩阵，然后执行一次矩阵乘法：

3）终点（k = j时刻） ：当到达下一帧雷达/视觉关键帧时，我们手里就得到了一个最终的巨大雅可比矩阵。这个矩阵物理意义就是：最终的15维误差状态，对初始15维误差状态的偏导数大全。

（2）提取我们需要的Bias雅可比块：

根据状态向量的排列顺序，这个巨大的矩阵可以划分为5x5的子块（每个子块是3x3）。我们要找的雅可比，就是去这个大矩阵里"按图索骥"抠出来的特定子块：

3. IMU帧间积分约束的代码实现

3.1 帧间惯性解算以及方差和预积分项雅可比递推

cpp 复制代码

void IMUPreIntegrator::UpdateState(void) {
    static double T = 0.0;

    //
    // parse measurements:
    //
    // get measurement handlers:
    const IMUData &prev_imu_data = imu_data_buff_.at(0);
    const IMUData &curr_imu_data = imu_data_buff_.at(1);

    // get time delta:
    T = curr_imu_data.time - prev_imu_data.time;

    // get measurements:
    const Eigen::Vector3d prev_w(
        prev_imu_data.angular_velocity.x - state.b_g_i_.x(),
        prev_imu_data.angular_velocity.y - state.b_g_i_.y(),
        prev_imu_data.angular_velocity.z - state.b_g_i_.z()
    );
    const Eigen::Vector3d curr_w(
        curr_imu_data.angular_velocity.x - state.b_g_i_.x(),
        curr_imu_data.angular_velocity.y - state.b_g_i_.y(),
        curr_imu_data.angular_velocity.z - state.b_g_i_.z()
    );

    const Eigen::Vector3d prev_a(
        prev_imu_data.linear_acceleration.x - state.b_a_i_.x(),
        prev_imu_data.linear_acceleration.y - state.b_a_i_.y(),
        prev_imu_data.linear_acceleration.z - state.b_a_i_.z()
    );
    const Eigen::Vector3d curr_a(
        curr_imu_data.linear_acceleration.x - state.b_a_i_.x(),
        curr_imu_data.linear_acceleration.y - state.b_a_i_.y(),
        curr_imu_data.linear_acceleration.z - state.b_a_i_.z()
    );

    //
    // TODO: a. update mean:
    //
    // 1. get w_mid:
    Eigen::Vector3d w_mid = (curr_w + prev_w) * 0.50;

    Eigen::Matrix3d prev_R = state.theta_ij_.matrix();

    // 2. update relative orientation, so3:
    Eigen::Vector3d rotation_vector = w_mid * T;
    Sophus::SO3d d_theta_ij = Sophus::SO3d::exp(rotation_vector);
    state.theta_ij_ *= d_theta_ij;

    // 3. get a_mid:
    Eigen::Matrix3d curr_R = state.theta_ij_.matrix();
    Eigen::Vector3d a_mid = (curr_R * curr_a + prev_R * prev_a) * 0.50;

    // 4. update relative translation:
    state.alpha_ij_ += state.beta_ij_ * T + 0.50 * a_mid * T * T;

    // 5. update relative velocity:
    state.beta_ij_ += a_mid * T;

    //
    // TODO: b. update covariance:
    //
    // 1. intermediate results:
    //

    // TODO: 2. set up F:
    // F12,F13,F14,F15
    Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
    F_.block<3,3>(INDEX_ALPHA, INDEX_THETA) = -0.25 * T * T * (prev_R * skew(prev_a) + 
                                            curr_R * skew(curr_a) * (I - skew(w_mid) * T));
    F_.block<3,3>(INDEX_ALPHA, INDEX_BETA) = Eigen::Matrix3d::Identity() * T;
    F_.block<3,3>(INDEX_ALPHA, INDEX_B_A) = -0.25 * (prev_R + curr_R) * T * T;
    F_.block<3,3>(INDEX_ALPHA, INDEX_B_G) = 0.25 * T * T* T * curr_R * skew(curr_a);

    // F22,F25
    F_.block<3,3>(INDEX_THETA, INDEX_THETA) = I - skew(w_mid) * T;
    F_.block<3,3>(INDEX_THETA, INDEX_B_G) = -I * T;

    // F32,F34,F35
    F_.block<3,3>(INDEX_BETA, INDEX_THETA) = -0.5 * T * (prev_R * skew(prev_a) + 
                                            curr_R * skew(curr_a) * (I - skew(w_mid) * T));
    F_.block<3,3>(INDEX_BETA, INDEX_B_A) = -0.5 * (prev_R + curr_R) * T;
    F_.block<3,3>(INDEX_BETA, INDEX_B_G) = 0.5 * T * T * curr_R * skew(curr_a);


    //
    // TODO: 3. set up G:
    // B11,B12,B13,B14
    Eigen::Matrix3d B12(-0.125 * T * T * T * curr_R * skew(curr_a));
    B_.block<3,3>(INDEX_ALPHA, INDEX_M_ACC_PREV) = 0.25 * prev_R * T * T;
    B_.block<3,3>(INDEX_ALPHA, INDEX_M_GYR_PREV) = B12;
    B_.block<3,3>(INDEX_ALPHA, INDEX_M_ACC_CURR) = 0.25 * curr_R * T * T;
    B_.block<3,3>(INDEX_ALPHA, INDEX_M_GYR_CURR) = B12;

    // B22,B24
    B_.block<3,3>(INDEX_THETA, INDEX_M_GYR_PREV) = 0.5 * I * T;
    B_.block<3,3>(INDEX_THETA, INDEX_M_GYR_CURR) = 0.5 * I * T;

    // B31,B32,B33,B34
    Eigen::Matrix3d B32(-0.25 * T * T * curr_R * skew(curr_a));
    B_.block<3,3>(INDEX_BETA, INDEX_M_ACC_PREV) = 0.5 * prev_R * T;
    B_.block<3,3>(INDEX_BETA, INDEX_M_GYR_PREV) = B32;
    B_.block<3,3>(INDEX_BETA, INDEX_M_ACC_CURR) = 0.5 * curr_R * T;
    B_.block<3,3>(INDEX_BETA, INDEX_M_GYR_CURR) = B32;

    // B45
    B_.block<3,3>(INDEX_B_A, INDEX_R_ACC_PREV) = I * T;

    // B56
    B_.block<3,3>(INDEX_B_G, INDEX_R_GYR_PREV) = I * T;


    // TODO: 4. update P_:
    P_ = F_ * P_ * F_.transpose() + B_ * Q_ * B_.transpose();

    // 
    // TODO: 5. update Jacobian:
    //
    J_ = F_ * J_;
}

3.2 二元约束边定义

cpp 复制代码

class EdgePRVAGIMUPreIntegration : public g2o::BaseBinaryEdge<15, Vector15d, g2o::VertexPRVAG, g2o::VertexPRVAG> {
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    // 维度定义
    static const int INDEX_P = 0;
    static const int INDEX_R = 3;
    static const int INDEX_V = 6;
    static const int INDEX_A = 9;
    static const int INDEX_G = 12;

	EdgePRVAGIMUPreIntegration()
	 : g2o::BaseBinaryEdge<15, Vector15d, g2o::VertexPRVAG, g2o::VertexPRVAG>() {
	 }

    // 计算残差
    virtual void computeError() override;

    // 计算雅可比
    virtual void linearizeOplus() override;
    
	void setT(const double &T) {
		T_ = T;
	}

	void setGravitiy(const Eigen::Vector3d &g) {
		g_ = g;
	}

	void setJacobian(const Eigen::MatrixXd &J) {
		J_ = J;
	}

    virtual void setMeasurement(const Vector15d& m) override {
		_measurement = m;
	}


private:
	double T_ = 0.0;

	Eigen::Vector3d g_ = Eigen::Vector3d::Zero();

	Eigen::MatrixXd J_;
};

3.3 计算残差

cpp 复制代码

virtual void computeError() override {
	g2o::VertexPRVAG* v0 = dynamic_cast<g2o::VertexPRVAG*>(_vertices[0]);
	g2o::VertexPRVAG* v1 = dynamic_cast<g2o::VertexPRVAG*>(_vertices[1]);

    // 获取相邻节点i和j的状态
	const Eigen::Vector3d &pos_i = v0->estimate().pos;
	const Sophus::SO3d    &ori_i = v0->estimate().ori;
	const Eigen::Vector3d &vel_i = v0->estimate().vel;
	const Eigen::Vector3d &b_a_i = v0->estimate().b_a;
	const Eigen::Vector3d &b_g_i = v0->estimate().b_g;

	const Eigen::Vector3d &pos_j = v1->estimate().pos;
	const Sophus::SO3d    &ori_j = v1->estimate().ori;
	const Eigen::Vector3d &vel_j = v1->estimate().vel;
	const Eigen::Vector3d &b_a_j = v1->estimate().b_a;
	const Eigen::Vector3d &b_g_j = v1->estimate().b_g;

	// 取出imu积分项的bias
	Eigen::Vector3d b_a_ii = _measurement.block<3, 1>(INDEX_A, 0);
	Eigen::Vector3d b_g_ii = _measurement.block<3, 1>(INDEX_G, 0);
    // 计算与i节点bias的变化量
	Eigen::Vector3d d_b_a_i = b_a_i - b_a_ii;
	Eigen::Vector3d d_b_g_i = b_g_i - b_g_ii;

	// 构造局部的修正观测量
	Eigen::Vector3d corrected_P = _measurement.block<3, 1>(INDEX_P, 0) + 
									J_.block<3, 3>(INDEX_P, INDEX_A) * d_b_a_i + 
									J_.block<3, 3>(INDEX_P, INDEX_G) * d_b_g_i;
	
	Sophus::SO3d corrected_R = Sophus::SO3d::exp(_measurement.block<3, 1>(INDEX_R, 0)) * 
								Sophus::SO3d::exp(J_.block<3, 3>(INDEX_R, INDEX_G) * d_b_g_i);
	
	Eigen::Vector3d corrected_V = _measurement.block<3, 1>(INDEX_V, 0) + 
									J_.block<3, 3>(INDEX_V, INDEX_A) * d_b_a_i + 
									J_.block<3, 3>(INDEX_V, INDEX_G) * d_b_g_i;

	// 计算残差
	_error.block<3, 1>(INDEX_P, 0) = Eigen::Vector3d::Zero();
	_error.block<3, 1>(INDEX_R, 0) = Eigen::Vector3d::Zero();
	_error.block<3, 1>(INDEX_V, 0) = Eigen::Vector3d::Zero();
	_error.block<3, 1>(INDEX_A, 0) = Eigen::Vector3d::Zero();
	_error.block<3, 1>(INDEX_G, 0) = Eigen::Vector3d::Zero();

	_error.block<3, 1>(INDEX_P, 0) = ori_i.inverse() * (pos_j - pos_i - vel_i * T_ + 
										0.5 * g_ * T_ * T_) - corrected_P;
	Eigen::Quaterniond q = (corrected_R.inverse() * ori_i.inverse() * ori_j).unit_quaternion();
	_error.block<3, 1>(INDEX_R, 0) = 2.0 * q.vec();
	// _error.block<3, 1>(INDEX_R, 0) = (corrected_R.inverse() * ori_i.inverse() * ori_j).log();
	_error.block<3, 1>(INDEX_V, 0) = ori_i.inverse() * (vel_j - vel_i + g_ * T_) - 
										corrected_V;
	_error.block<3, 1>(INDEX_A, 0) = b_a_j - b_a_i;
	_error.block<3, 1>(INDEX_G, 0) = b_g_j - b_g_i;
}

计算残差时，需要使用根据bias变化量更新后的imu积分项。

3.4 计算雅可比

cpp 复制代码

// 计算反对称矩阵
Eigen::Matrix3d skew(const Eigen::Vector3d &v)
{
	Eigen::Matrix3d m;
	m << 0, -v(2), v(1),
		v(2), 0, -v(0),
		-v(1), v(0), 0;
	return m;
}

// 雅可比解析求导
virtual void linearizeOplus() override
{
	g2o::VertexPRVAG* v0 = dynamic_cast<g2o::VertexPRVAG*>(_vertices[0]);
	g2o::VertexPRVAG* v1 = dynamic_cast<g2o::VertexPRVAG*>(_vertices[1]);
	
    // 获取相邻节点i和j的状态
	const Eigen::Vector3d &pos_i = v0->estimate().pos;
	const Sophus::SO3d &ori_i = v0->estimate().ori;
	const Eigen::Vector3d &vel_i = v0->estimate().vel;
	const Eigen::Vector3d &b_a_i = v0->estimate().b_a;
	const Eigen::Vector3d &b_g_i = v0->estimate().b_g;
	
	const Eigen::Vector3d &pos_j = v1->estimate().pos;
	const Sophus::SO3d &ori_j = v1->estimate().ori;
	const Eigen::Vector3d &vel_j = v1->estimate().vel;
	const Eigen::Vector3d &b_a_j = v1->estimate().b_a;
	const Eigen::Vector3d &b_g_j = v1->estimate().b_g;
	
    // 将两个节点的雅可比矩阵先全部置为0
	_jacobianOplusXi.setZero();
	_jacobianOplusXj.setZero();

	const Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
	const Eigen::Matrix3d R_wbi_inv = ori_i.inverse().matrix();
	// 1.位置残差雅可比
	// 1.1 对i时刻 位置、姿态、速度、加速度计/陀螺仪bias
	_jacobianOplusXi.block<3, 3>(INDEX_P, INDEX_P) = - R_wbi_inv;
	_jacobianOplusXi.block<3, 3>(INDEX_P, INDEX_R) = skew(R_wbi_inv * (pos_j - pos_i - vel_i * T_ + 
														0.5 * g_ * T_ * T_));
	_jacobianOplusXi.block<3, 3>(INDEX_P, INDEX_V) = - R_wbi_inv * T_;
	_jacobianOplusXi.block<3, 3>(INDEX_P, INDEX_A) = - J_.block<3, 3>(INDEX_P, INDEX_A);
	_jacobianOplusXi.block<3, 3>(INDEX_P, INDEX_G) = - J_.block<3, 3>(INDEX_P, INDEX_G);
	// 1.2 对j时刻 位置
	_jacobianOplusXj.block<3, 3>(INDEX_P, INDEX_P) = R_wbi_inv;

	Eigen::Vector3d d_b_g_i = b_g_i - _measurement.block<3, 1>(INDEX_G, 0);
	Sophus::SO3d corrected_ORI = Sophus::SO3d::exp(_measurement.block<3, 1>(INDEX_R, 0)) * 
								Sophus::SO3d::exp(J_.block<3, 3>(INDEX_R, INDEX_G) * d_b_g_i);
	// 2.姿态残差雅可比
	// 2.1 对i时刻 姿态、陀螺仪bias h对应公式推导时的q1，v对应q2
	Eigen::Quaterniond h = (ori_j.inverse() * ori_i).unit_quaternion(), k = corrected_ORI.unit_quaternion();
	Eigen::Vector3d hv = h.vec(), kv = k.vec();
	_jacobianOplusXi.block<3, 3>(INDEX_R, INDEX_R) = hv * kv.transpose() - (h.w() * I + skew(hv)) * 
													(k.w() * I - skew(kv));
	
	h = (ori_j.inverse() * ori_i * corrected_ORI).unit_quaternion();
	hv = h.vec();
	_jacobianOplusXi.block<3, 3>(INDEX_R, INDEX_G) = - (h.w() * I + skew(hv)) * J_.block<3, 3>(INDEX_R, INDEX_G);

	// 2.2 对j时刻 姿态
	h = (corrected_ORI.inverse() * ori_i.inverse() * ori_j).unit_quaternion();
	hv = h.vec();
	_jacobianOplusXj.block<3, 3>(INDEX_R, INDEX_R) = h.w() * I + skew(hv);

	// 3.速度残差雅可比
	// 3.1 对i时刻 姿态、速度、加速度计/陀螺仪bias
	_jacobianOplusXi.block<3, 3>(INDEX_V, INDEX_R) = skew(R_wbi_inv * (vel_j - vel_i + g_ * T_));
	_jacobianOplusXi.block<3, 3>(INDEX_V, INDEX_V) = - R_wbi_inv;
	_jacobianOplusXi.block<3, 3>(INDEX_V, INDEX_A) = - J_.block<3, 3>(INDEX_V, INDEX_A);
	_jacobianOplusXi.block<3, 3>(INDEX_V, INDEX_G) = - J_.block<3, 3>(INDEX_V, INDEX_G);
	// 3.2 对j时刻 速度
	_jacobianOplusXj.block<3, 3>(INDEX_V, INDEX_V) = R_wbi_inv;

	// 4.加速度计bias残差雅可比
	// 4.1 对i时刻
	_jacobianOplusXi.block<3, 3>(INDEX_A, INDEX_A) = - I;
	// 4.2 对j时刻
	_jacobianOplusXj.block<3, 3>(INDEX_A, INDEX_A) = I;
	
	// 5.陀螺仪bias残差雅可比
	// 5.1 对i时刻
	_jacobianOplusXi.block<3, 3>(INDEX_G, INDEX_G) = - I;
	// 5.2 对j时刻
	_jacobianOplusXj.block<3, 3>(INDEX_G, INDEX_G) = I;
}

IMU预积分（VINS）

1.为什么需要预积分

2.预积分理论

2.1 预积分模型

2.2 预积分在优化中的使用

2.2.1 残差设计

2.2.2 推导残差的雅可比

2.2.2.1 位置残差的雅可比

2.2.2.2 姿态残差的雅可比

2.2.2.3 速度残差的雅可比

2.2.2.4 加速度计bias残差的雅可比

2.2.2.5 陀螺仪bias残差的雅可比

2.2.3 推导残差的方差

2.2.3.1 姿态误差递推方程

2.2.3.2 速度误差递推方程

2.2.3.3 位置误差递推方程

2.2.3.4 加速度计bias误差递推方程

2.2.3.5 陀螺仪bias误差递推方程

2.3 预积分项更新

2.3.1 预积分项更新

2.3.2 预积分对Bias的雅可比是如何计算的 ？

3. IMU帧间积分约束的代码实现

3.1 帧间惯性解算以及方差和预积分项雅可比递推

3.2 二元约束边定义

3.3 计算残差

3.4 计算雅可比

2.3.2 预积分对Bias的雅可比是如何计算的？