IMU预积分--详细推导过程 _imu

一、提前了解
二、预积分的目的
1.IMU通过加速度计和陀螺仪测出的是加速度和角速度，通过积分获得两帧之间的旋转和位移的变换；
2.在后端非线性优化的时候，需要优化位姿，每次调整位姿都需要在它们之间重新传递IMU测量值，需要重新积分，这将非常耗时，为了避免重新传递测量值，所以采取预积分策略。
三、进入预积分主题 1.IMU模型
其中：
2.当前时刻的PVQ的连续表达形式
对图像第k k k 帧和第k + 1 k+1 k+1 帧之间的所有IMU进行积分，对应的IMU坐标系为 b k b_k bk? 和b k + 1 b_{k+1} bk+1? ，根据k时刻的数据，积分求得k + 1 k+1 k+1 时刻的数据，求出的是在世界坐标系下的值：
其中：
Δ t k Δt_k Δtk? ：表示[k,k+1]之间的时间间隔
q t b k q^{bk}_t qtbk?：表示在本体（IMU）坐标系下，t 时刻到 b k b_k bk?时刻位姿的变换矩阵。
推导过程中旋转采用四元数表示。
对于上式中，第三个公式的推导过程如下：
首先需要先了解四元数的基本知识：
（1）四元数由实部和虚部构成，可以将实部写在前面，也可以将虚部写在前面，本文将实部写在前面：
q = [ q 0 , q 1 , q 2 , q 3 ] = [ s , v ? ] q=[q_0,q_1,q_2,q_3]=[s,\vec{v}] q=[q0?,q1?,q2?,q3?]=[s,v]
即： q = q 0 + q 1 i + q 2 j + q 3 k q=q_0+q_1i+q_2j+q_3k q=q0?+q1?i+q2?j+q3?k （虚部写在前）
（2）四元数与旋转向量 ( n ? , θ ) (\vec{n},\theta) (n,θ)的转换：
q = [ c o s θ 2 n ? s i n θ 2 ] q=\begin{} cos\frac{\theta}{2}\\ \vec{n}sin\frac{\theta}{2} \\ \end{} q=[cos2θ?nsin2θ??]
当发生一个微小的旋转，即旋转角度趋近于0，则有：
（3）四元数求导：
【IMU预积分--详细推导过程】w w w：表示的是角速度，上式中对角度进行求导，得到角速度
由以上四元数的基本知识可以得到：第三个公式的推导过程如下：：
针对上述2.当前时刻的PVQ连续表达形式可知，若要求得当前时刻的旋转、位置、速度，需要前一时刻的状态量，但是当这些状态改变时，我们就需要重新传递IMU的测量值，特别是在基于优化的算法中，每次调整位姿都需要在它们之间重新传递IMU测量值，这种测量策略非常耗时，为了避免重新传递测量值，采用预积分算法。
通俗的解释：根据上一时刻计算出当前时刻的PVQ，当后端对上一时刻的值进行优化时，则需要根据优化好的值，重新计算当前帧的PVQ 。
3.当前时刻的PVQ的中值法离散表达形式
对第 i i i个IMU时刻到 i + 1 i+1 i+1时刻的IMU进行积分
其中：
i i i 表示的是在 [ t k , t k + 1 ] [t_k,t_{k+1}] [tk?,tk+1?] 中IMU测量值对应的离散时刻；
δ t \delta{t} δt 是IMU测量值i i i 和i + 1 i+1 i+1 之间的时间间隔。
由以上四元数的基本知识可以得到：第三个公式的推导过程如下：：
其中
即取中值作为i i i 时刻的预测值
q i q_i qi?：用四元数表示，上式中表示imu坐标系到世界坐标系的变化。
4.两帧之间PVQ增量的连续形式
文章开始讲述，预积分的目的，在后端进行非线性优化的时候，需要迭代更新 k k k帧的 v v v和 R R R，这将导致我们需要根据每次迭代后的新值进行积分，这非常耗时，此时就需要将优化变量从第 k k k帧到第 k + 1 k+1 k+1帧的IMU预积分中分离出来，将2中各式左右两侧各乘 R w b k R^{b_k}_w Rwbk?? 得：
由以上三幅图可知：
积分结果
α b k + 1 b k \alpha^{b_k}_{b_{k+1}} αbk+1?bk??、 β b k + 1 b k \beta^{b_k}_{b_{k+1}} βbk+1?bk??、 γ b k + 1 b k \gamma^{b_k}_{b_{k+1}} γbk+1?bk??，可以理解为 b k + 1 b_{k+1} bk+1?相对于 b k b_k bk?的相对运动量；
其中：积分中与IMU的测量值以及偏置有关；