## 转载请注明出处，欢迎转载 ##

目录

1、算法推导

2、反思与探讨

3、参考文献

---

1、算法推导

我相信大家在很多不同的地方都听说过IMU预积分这个名词，尤其是基于图优化的框架下，几乎都会用到IMU预积分，那为什需要IMU预积分呢？一方面是因为IMU数据频率往往高于图像的频率，一般都能达到100~500Hz，而图像往往只有30~60Hz，所以为了获得每个图像帧对应的IMU数据，就需要对两个图像帧之间的IMU数据进行积分，才能实现图像帧和IMU数据的意义配对；另一方面，在图优的框架下，经常需要对历史状态进行更新，如果不使用预积分的话，每当一个状态发生变化时，就需要从头往后，运用每一帧IMU数据进行计算，直至更新完所有的状态量为止，这样显然就过于费时费力。但当我们使用IMU预积分时，当图中某个状态量发生了变化，可以直接通过预积分的值直接更新之后的每个关键帧的状态量，这就高效很多。因此可以形象地把IMU预积分理解为，将连续不断的IMU数据根据关键帧的时间戳进行打断，然后每次只记录前后帧的相对位姿关系。

这里对VINS中的IMU预积分进行推导：

对于连续时间的IMU预积分，已知k时刻的状态量，k+1时刻可以表示为

                                                  (1)

                                                                       (2)

                                                          (3)

其中代表k+1时刻world坐标系下的位置，和为世界坐标系下的线加速度和角速度，根据IMU模型，有

                                                         (4)

                                                                  (5)

其中和分别代表IMU坐标系下的线加速度和角速度，分别为零均值随机噪声，分别为加速度及角速度的bias，为重力加速度。将上式代入式(1)-(3)，则有

                                  (6)

                                              (7)

                                                 (8)

再对式(6)-(8)左边同时乘以后移项，则有

                              (9)

                                  (10)

                                           (11)

等式(9)-(11)的右侧则为IMU的预积分值，可以看到，IMU预积分值与IMU的位姿无关，仅仅代表第k帧至第k+1帧的PVQ变化值。也可以从另一个方面进行理解，等式左侧均为系统的优化变量，而等式的右侧则为两帧间的观测值，用，和表示PVQ的预积分值（与论文保持一致），则图优化中的IMU残差可以表示为

                               (12)

                                                (13)

                                               (14)

                                                                   (15)

                                                                   (16)

推导完残差以后，很自然的就需要推导残差项关于各状态量的雅克比矩阵，其中状态量为

雅克比的推导主要用到摄动法，即对状态量增加一个小的摄动量，观察结果对摄动量的变化情况，为了书写方便，以下以分别代表：

• 项雅克比

                                                                             (17)

                                                                              (18)

                                                                      (19)

                                                                 (20)

                                                                                    

                                                                                           (21)

                                                        (22)

                                                                        (23)

---

• 项雅克比

                                                                   (24)

                                                                       (25)

                                                                         (26)

                

         (27)

                                                                               (28)

                                                                           (29)

                                                                          (30)

---

• 项雅克比 

                                                                (31)

                                                                            (32)

                                                                                           (33)

                                                                  (34)

                                               

                                          (35)

                                                                             (36)

至此，IMU预积分残差项已经基本推导完成了。但是在优化问题中，还有一项非常重要的参数，就是IMU残差对应的信息矩阵没有给出。由于VINS中IMU协方差的更新采用ESKF的方式进行递推，这里不加证明的直接给出其协方差矩阵的递推方式，具体的证明可以查看[2]和[3]：

                                                              (37)

其中

 至此，VINS中整个关于IMU预积分的部分已经推导完毕，最后再整体的梳理一下，预积分部分主要要把握三个部分，分别是状态值的更新，协方差的更新以及IMU残差的构建。

2、反思与探讨

整体推到完VINS的预积分过程之后，我发现它和经典的IMU预积分[3]，还有ORBSLAM3[5]里的预积分都有些许的差别，总结起来主要有以下几点：

• 计算角度残差的不同

VINS中的方向残差是公式(14)，而[3]和[5]中的方向残差为

差别主要是多了一个LOG函数，将李群映射到李代数上，而VINS则是只取四元数的虚部进行比较，精度上可能有一定差距。

• 协方差更新的方式不同

VINS中使用的是ESKF里协方差的更新方式，而[3][5]则是通过具体的推导给出的协方差更新方式，具体请查看文献[3][5]的内容

• 角度的表示不同（VINS用的是四元数，而[3][5]用的是SO3）

3、参考文献

[1] Tong Q , Li P , Shen S . VINS-Mono[J]. IEEE Transactions on Robotics, 2018.

[2] VINS 论文推导及代码解析. 崔华坤

[3] J Solà. Quaternion kinematics for the error-state Kalman filter[J].  2017.

[4] Forster C ,  Carlone L ,  Dellaert F , et al. IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation (supplementary material)[J]. Georgia Institute of Technology, 2015.

[5] Campos C ,  Elvira R , JJG Rodríguez, et al. ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM[J].  2020.