Vins-Mono 论文 && Coding 一 4(1). imu 预积分

本文链接：https://blog.csdn.net/weixin_40072161/article/details/109552941

一、IMU 传感器模型

1. 坐标系

世界坐标系 $\overrightarrow{F^{w}}$ , z 轴和重力方向对齐 (The direction of the gravity is aligned with the z-axis of the world frame)

机体坐标系 $\overrightarrow{F^{b}}$ ，在 vins 中，指的是 imu 坐标系

2. 传感器测量值

1. linear acceleration with noise: $\large \hat{a}_{t}$

2. angular velocity with noise: $\large \hat{\omega}_{t}$

3. 传感器噪声建模

考虑 bias, 高斯白噪声，重力加速度叠加影响

$\large \hat{a}_{t}=a_t + b_{a_t} + n_a + R^w_tg^w$

$\large \hat{\omega}_{t}=\omega_t + b_{\omega_t} + n_\omega$ (1)

其中 $\large \hat{a}_{t}$ ， $\large \hat{\omega}_{t}$ 是 imu 测量值

$\large a_{t}$ ， $\large \omega_{t}$ 是 noise-free 线加速度和角速度(imu 坐标系下)

$\large b_{a_{t}}$ , $\large b_{\omega_{t}}$ 对应 bias

$\large n_{a}$ , $\large n_{\omega}$ 对应高斯白噪声, $\large n_{a}\sim N(0,\sigma _{a}^{2})$ , $\large n_{\omega }\sim N(0,\sigma _{\omega }^{2})$

$\large R_{w}^{t}$ 是机体坐标系的旋转矩阵

$\large g^w=\begin{pmatrix} 0 & 0 & g \end{pmatrix}^T$ ，代表重力加速度

二、运动模型

1. 微分方程

PVQ 在世界坐标系下表示

(1). 对位置求导

$\frac{dp^w_t}{dt}=v^w_t$ (2)

(2). 对速度求导

$\frac{dv^w_t}{dt}=a^w_t$ (3)

(3). 对四元数求导

$\dot{q}=\lim_{\Delta t\rightarrow 0} \frac{q(t+\Delta t)-q(t)}{\Delta t}$

$=\lim_{\Delta t\rightarrow 0} \frac{q\otimes \Delta q_L - q}{\Delta t}$

$=\lim_{\Delta t\rightarrow 0} \frac{q\otimes \left (\begin{pmatrix} \Delta\phi _L/2 \\ 1 \end{pmatrix} - \begin{pmatrix} 0 \\ 1 \end{pmatrix} \right ) }{\Delta t}$

$=\lim_{\Delta t\rightarrow 0} \frac{q\otimes \begin{pmatrix} \Delta\phi _L/2 \\ 0 \end{pmatrix} }{\Delta t}$

$= \frac{1}{2}q\otimes \begin{pmatrix} \omega_t \\ 0 \end{pmatrix}$

$= \frac{1}{2}\Omega (\omega_t)\cdot q$ (4)

其中， $\Omega (\omega_t)=[\omega_t]_R = \begin{pmatrix} -[\omega_t]_\times & \omega_t \\ -\omega_t^T & 0 \end{pmatrix}$ ， $[\omega_t]_\times$ 为 $\omega_t$ 的叉乘

注意: 在有的参考文献中，四元数的实部在前面， $\Omega (\omega_t)$ 的表达式会有差异，这里虚部在前是为了和 vins 中的写法保持一致

(4). bais 随机游走

$\large b_{a_{t}}$ , $\large b_{\omega_{t}}$ 被建模为随机游走, 其导数是高斯白噪声

$\dot{b}_{a_t}=n_{b_a}$ , $\dot{b}_{\omega_t}=n_{b_\omega}$

$\large n_{b_a}\sim N(0,\sigma _{b_a}^{2})$ , $\large n_{b_\omega }\sim N(0,\sigma _{b_\omega }^{2})$ (5)

2. 连续形式积分

(1). 速度积分

更新 V

$v_{b_{k+1}}^{w}=v_{b_{k}}^{w}+\int_{t\in [t_{k},t_{k+1}]} (R_t^w\cdot a_t)dt$

$=v_{b_{k}}^{w}+\int_{t\in [t_{k},t_{k+1}]} (R_t^w\cdot (\hat{a}_t - b_{a_t} - n_{a} - R^t_w\cdot g^w))dt$

$=v_{b_{k}}^{w}+\int_{t\in [t_{k},t_{k+1}]} (R_t^w\cdot (\hat{a}_t - b_{a_t}-n_{a}))-g^w)dt$ (6)

(2).位置积分

更新 P

$\large p_{b_{k+1}}^{w}=p_{b_{k}}^{w}+ \int_{t\in [t_{k},t_{k+1}]} v^w_tdt$

$\large =p_{b_{k}}^{w}+ \int_{t\in [t_{k},t_{k+1}]} (v_{b_{k}}^{w}+\int_{t\in [t_{k},t_{k+1}]} (R_t^w\cdot (\hat{a}_t - b_{a_t}-n_{a}))-g^w)dt)dt$

$\large =p_{b_{k}}^{w}+v_{b_{k}}^{w}\cdot \Delta t+ \iint_{t\in [t_{k},t_{k+1}]} ( (R_t^w\cdot (\hat{a}_t - b_{a_t}-n_{a})-g^w))dt^2$ (7)

(3). 四元数积分

更新 Q

$q_{b_{k+1}}^{w}= q_{b_{k}}^{w}\otimes\int_{t\in [t_{k},t_{k+1}]} \tfrac{1}{2} \cdot \Omega (\omega_t)q_{t}^{b_{k}} dt$

$= q_{b_{k}}^{w}\otimes\int_{t\in [t_{k},t_{k+1}]} \tfrac{1}{2} \cdot \Omega (\hat{\omega_t} - b_{\omega _t}-n_{\omega })q_{t}^{b_{k}} dt$ (8)

3. 结论

From (6).(7).(8) It can be seen that the IMU state propagation requires rotation, position, and velocity of the frame $\large b_k$ . When these starting states change, we need to repropagate IMU measurements. Especially, in the optimization-based algorithm, every time we adjust poses, we will need to repropagate IMU measurements between them.

三、预积分

1. 基本概念

After changing the reference frame from the world frame to the local frame $\large b_k$ , we can only preintegrate the parts that are related to linear acceleration $\large \hat{a}$ and angular velocity $\large \hat{\omega }$

即更新 PVQ 增量

2. 连续形式

将式 (6). (7). (8) 左乘 $R^{b_{k}}_{w}$ ，把积分部分单独拎出来

$R^{b_k}_wp^w_{b_{k+1}}=R^{b_k}_w(p^w_{b_k}+v^w_{b_k} \Delta t_k-\frac{1}{2}g^w\Delta t_k^2)+\alpha ^{b_k}_{b_{k+1}}$

$R^{b_k}_wv^w_{b_{k+1}}=R^{b_k}_w(v^w_{b_k} - g^w\Delta t_k)+\beta^{b_k}_{b_{k+1}}$

$q^{b_k}_w\otimes q^w_{b_{k+1}}=\gamma ^{b_k}_b_{k+1}$ (9)

其中

$\alpha ^{b_k}_{b_{k+1}}= \iint_{t\in [t_{k},t_{k+1}]} R_t^{b_k}\cdot (\hat{a}_t - b_{a_t}-n_{a})dt^2$

$\beta ^{b_k}_{b_{k+1}}= \int_{t\in [t_{k},t_{k+1}]} R_t^{b_k}\cdot (\hat{a}_t - b_{a_t}-n_{a})dt$

$\gamma ^{b_k}_{b_{k+1}}= \int_{t\in [t_{k},t_{k+1}]} \tfrac{1}{2} \cdot \Omega (\hat{\omega_t} - b_{\omega _t}-n_{\omega })\gamma_{t}^{b_{k}} dt$ (10)

It can be seen that the preintegration terms can be obtained solely with IMU measurements by taking $\large b_k$ as the reference frame given bias. $\large \alpha ^{b_k}_{b_{k+1}}$ , $\large \beta ^{b_k}_{b_{k+1}}$ , and $\large \gamma ^{b_k}_{b_{k+1}}$ are only related to IMU biases instead of other states in $\large b_k$ and $\large b_{k+1}$ .

3. 离散形式

At the beginning, $\large \alpha ^{b_k}_{b_{k+1}}$ , $\large \beta ^{b_k}_{b_{k+1}}$ are 0, and $\large \gamma ^{b_k}_{b_{k+1}}$ is identity quaternion

(1). 积分方式

对于非线性微分方程:

$\dot{x}=f(t,x)$ (11)

通过 龙格-库塔(Runge-Kutta)公式可以得到该微分方程的数值解

<1>. zero-order hold

Euler method: assumes that the derivative $f(\cdot )$ is constant over the interval

$x_{n+1}=x_n+\Delta t \cdot f(t_n, x_n)$ (12)

Midpoint method: assumes that the derivative is the one at the midpoint of the interval

$x_{n+1}=x_n+\Delta t \cdot f\left ( t_n+\frac{1}{2}\Delta t, x_n+\frac{1}{2}\Delta t \cdot f(t_n, x_n) \right )$ (13)

<2>. higher-order hold

RK4 integration

$x_{n+1}=x_n+\frac{\Delta t}{6} \cdot (k_1 + 2k_2 + 2k_3 + k_4)$

k_1=f(t_n, x_n)

$k_2=f(t_n+\frac{1}{2}\Delta t, x_n + \frac{\Delta t}{2}k_1)$

$k_3=f(t_n+\frac{1}{2}\Delta t, x_n + \frac{\Delta t}{2}k_2)$

$k_4=f(t_n+\Delta t, x_n + \Delta t\cdot k_3)$ (14)

(2). Euler Integration

$\large \hat{\alpha}^{b_k}_{i+1}=\hat{\alpha}^{b_k}_{i}+\hat{\beta}^{b_k}_{i}\delta t + \frac{1}{2}R(\hat{\gamma}^{b_k}_i)(\hat{a}_i-b_{a_i}) {\delta{t}}^2$

$\large \hat{\beta}^{b_k}_{i+1}=\hat{\beta}^{b_k}_{i}+R(\hat{\gamma}^{b_k}_i)(\hat{a}_i-b_{a_i}) \delta{t}$

$\large \hat{\gamma}^{b_k}_{i+1}=\hat{\gamma}^{b_k}_{i}\otimes \bigl(\begin{matrix} 1\\ \frac{1}{2} (\hat{\omega}_i-b_{\omega _i}) \delta t\end{matrix}\bigr)$ (15)

where $\large i$ is discrete moment corresponding to a IMU measurement within $\large [t_k, t_{k+1}]$ . $\large \delta _t$ is the time interval between two IMU
measurements $\large i$ and $\large i+1$ .

(3). Midpoint Integration

$\large \hat{\alpha}^{b_k}_{i+1}=\hat{\alpha}^{b_k}_{i}+\hat{\beta}^{b_k}_{i}\delta t + \frac{1}{2} \large \bar{\hat{a}}_i{\delta{t}}^2$

$\large \hat{\beta}^{b_k}_{i+1}=\hat{\beta}^{b_k}_{i}+\large \bar{\hat{a}}_i{\delta {t}}$

$\large \hat{\gamma}^{b_k}_{i+1}=\hat{\gamma}^{b_k}_{i}\otimes \bigl(\begin{matrix} 1\\ \frac{1}{2} (\bar{\hat{\omega }}_i-b_{\omega _i}) \delta t\end{matrix}\bigr)$

$\large \bar{\hat{a}}_i=\frac{1}{2}[R(\hat{\gamma }^{b_k}_{i})(\hat{a}_{i}-b_{a_i})+R(\hat{\gamma }^{b_k}_{i+1})(\hat{a}_{i+1}-b_{a_i})]$

$\large \bar{\hat{\omega }}_i=\frac{1}{2}(\hat{\omega }_{i}+\hat{\omega }_{i+1} )$ (16)

4. error state dynamic

(1). 预积分 error state 定义

true state: 考虑噪声、bias 的随机游走，表示为 x_t

nominal state: 不考虑噪声、bias 的随机游走，表示为 $\hat{x}_t$

error state: true state 和 nominal state 的差值，表示为 $\delta x = x_t - \hat{x}_t$

$\large \delta {\alpha}^{b_k}_t = \alpha^{b_k}_t - \hat{\alpha}^{b_k}_t$ (17)

$\large \delta {\beta}^{b_k}_t = {\beta}^{b_k}_t - \hat{\beta}^{b_k}_t$ (18)

$\large \gamma ^{b_k}_t = \hat{\gamma ^{b_k}_t}\otimes \bigl(\begin{smallmatrix} 1\\ \frac{1}{2}\delta\theta ^{b_k}_t \end{smallmatrix}\bigr)$ (19)

$\large \delta b_{a_t} = b_{a_t}-\hat{b_{a_t}}$ (20)

$\large \delta b_{\omega_t} = b_{\omega_t} - \hat{b_{\omega_t}}$ (21)

其中 $\large x$ 是 true state， $\large \hat{x}$ 是 nominal state

由于四元数是过参数化，所以用旋转矢量的扰动 $\large \delta\theta ^{b_k}_t$ 来表示 error state

(2). true-state kinematics

$\dot{\alpha} ^{b_k}_{t}= \beta^{b_k}_t$ (22)

$\dot{\beta} ^{b_k}_{t}= R^{b_k}_t \cdot (\hat{a}_t - b_{a_t} - n_a)$ (23)

$\dot \gamma ^{b_k}_t= \frac{1}{2} \cdot \Omega (\hat{\omega_t} - b_{\omega _t}-n_{\omega })\gamma_{t}^{b_{k}}$ (24)

$\dot{b}_{a_t}=n_{b_a}$ (25)

$\dot{b}_{\omega_t}=n_{b_\omega}$ (26)

(3). nominal-state kinematics

$\dot{\hat{\alpha}} ^{b_k}_{t}= \hat{\beta}^{b_k}_t$ (27)

$\dot{\hat{\beta}} ^{b_k}_{t}= \hat{R}^{b_k}_t\cdot (\hat{a}_t - \hat{b}_{a_t})$ (28)

$\dot{\hat{\gamma}} ^{b_k}_t= \frac{1}{2} \cdot \Omega (\hat{\omega_t} - \hat{b}_{\omega _t}) \hat{\gamma}_{t}^{b_{k}}$ (29)

$\dot{\hat{b}}_{a_t}=0$ (30)

$\dot{\hat{b}}_{\omega_t}=0$ (31)

(4). error-state kinematics

<1>. position error

由式(22)(27)，可得出

$\large \dot{\delta \alpha}^{b_k}_t = \dot{\alpha}^{b_k}_t- \dot{\hat{\alpha}}^{b_k}_t= {\beta}^{b_k}_t - \hat{\beta}^{b_k}_t=\delta {\beta}^{b_k}_t$ (32)

<2>. bias error

由式(25)(30), (26)(31)，可得出

$\large \dot{\delta b}_{a_t} = \dot{b}_{a_t}-\dot{\hat b}_{a_t}=n_{b_a}$ (33)

$\large \dot{\delta b}_{\omega_t} = \dot{b}_{\omega_t}-\dot{\hat b}_{\omega_t}=n_{b_\omega}$ (34)

<3>. velocity error

$R^{b_k}_t = \hat{R}^{b_k}_t[I+[\delta \theta]_\times]+O(||\delta \theta||^2)$

由式(23)(28)，以及 $[a]_\times b=-[b]_\times a$ 可得出

$\large \dot{\delta \beta}^{b_k}_t = -R^{b_k}_t [\hat{a}_t-b_{a_t}]_\times \delta \theta^{b_k}_t -R^{b_k}_t \delta b_{a_t} -R^{b_k}_t n_a$ (35)

<4>. orientation error

对式(19) 求导，其中 $\large \delta q = \bigl(\begin{smallmatrix} 1\\ \frac{1}{2}\delta\theta ^{b_k}_t \end{smallmatrix}\bigr)$

左边： $\dot{\gamma} ^{b_k}_t = \frac{1}{2} \gamma ^{b_k}_t \otimes \omega _t= \frac{1}{2} \hat{\gamma}^{b_k}_t \otimes \delta q \otimes \omega _t$ (36)

右边： $\dot{\gamma} ^{b_k}_t = \dot{\hat{\gamma}}^{b_k}_t \otimes \delta q + \hat{\gamma}^{b_k}_t \otimes \dot{\delta q}=\frac{1}{2} \hat{\gamma}^{b_k}_t \otimes \hat{\omega}_t \otimes \delta q + \hat{\gamma}^{b_k}_t \otimes \dot{\delta q}$ (37)

(36),(37) 同时消去 $\hat{\gamma}^{b_k}_t$ ，得到

$\delta q \otimes \omega _t = \hat{\omega}_t \otimes \delta q + 2\dot{\delta q}$ ，那么可以得到 $\delta \theta^{b_k}_t$ 的导数

$\bigl(\begin{matrix} 0 \\ \dot{\delta \theta ^{b_k}_t} \end{matrix}\bigr)=\dot{2\delta q} = \delta q \otimes \omega_t - \hat{\omega_t} \otimes \delta q$

$= [q]_R(\omega_t) \cdot \delta q + [q]_L(\hat{\omega}_t) \cdot \delta q$

$=\bigl(\begin{matrix} 0 & -(\omega_t - \hat{\omega _t})^T \\ (\omega_t - \hat{\omega _t}) & -(\omega_t + \hat{\omega _t})_\times \end{matrix}\bigr) \cdot \bigl(\begin{smallmatrix} 1\\ \frac{1}{2}\delta\theta ^{b_k}_t \end{smallmatrix}\bigr)$ (38)

令 $\delta \omega = \omega_t - \hat{\omega}_t$ 提取第二行，得到

$\dot{\delta \theta ^{b_k}_t} = \delta \omega - (2\hat{\omega}_t + \delta \omega)_\times \cdot \frac{1}{2}\delta\theta ^{b_k}_t$

$= \delta \omega - (\hat{\omega}_t)_\times \delta \theta^{b_k}_t - \frac{1}{2} (\delta \omega)_\times \delta \theta^{b_k}_t$

$\approx -(\hat{\omega}_t)\delta \theta^{b_k}_t + \delta \omega$

$= -(\hat{\omega}_m - b_{\omega_t})_\times \delta \theta ^{b_k}_t - \delta b_{\omega_t}-n_\omega$ (39)

<5>. 矩阵形式

$\begin{pmatrix} \delta \dot{\alpha}^{b_k}_t \\ \delta \dot{\beta}^{b_k}_t \\ \delta \dot{\theta}^{b_k}_t \\ \delta \dot{b}_{a_t} \\ \delta \dot{b}_{\omega_t} \\ \end{pmatrix} = \begin{pmatrix} 0 & I & 0 & 0 & 0 \\ 0 & 0 & -R^{b_k}_t[\hat{a}_t-b_{a_t}]_\times & -R^{b_k}_t & 0 \\ 0 & 0 & -[\hat{\omega}_t - b_{\omega_t}]_\times & 0 & -I \\ 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 \\ \end{pmatrix} \begin{pmatrix} \delta \alpha^{b_k}_t \\ \delta \beta^{b_k}_t \\ \delta \theta^{b_k}_t \\ \delta b_{a_t} \\ \delta b_{\omega_t} \\ \end{pmatrix}$

$=\begin{pmatrix} 0 & 0 & 0 & 0 \\ -R^{b_k}_t & 0 & 0 & 0 \\ 0 & -I & 0 & 0 \\ 0 & 0 & I & 0 \\ 0 & 0 & 0 & I \end{pmatrix}\begin{pmatrix} n_a \\ n_\omega \\ n_{b_a} \\ n_{b_\omega} \\ \end{pmatrix} = F_t \delta z^{b_k}_t + G_t n_t$ (40)

其中， $\large F_t\in R^{15\times 15}$ ， $\large G_t\in R^{15\times 12}$ ， $\large \delta z_t^{b_k}\in R^{15\times 1}$ ， $\large n_t\in R^{12\times 1}$ ，

(5). error dynamic 离散形式(欧拉积分)

<1>. 在 $\large \delta t$ 时间间隔内，认为一阶导数 $\large F_t$ 恒定，那么离散化后得到的状态转移矩阵为：

$\large F_d=exp(F_t\cdot \delta t)\approx I+F_t\cdot \delta t$ (41)

<2>. 高斯噪声积分

连续形式高斯噪声：

$Q_t = diag(\sigma ^2_a, \sigma ^2_\omega, \sigma ^2_{b_a}, \sigma ^2_{b_\omega})$ (42)

积分得到

$Q_d= \int_0^{\delta t} F_d(\tau )G_tQ_tG^T_tF_d(\tau)^T$

$=\delta t F_d G_t Q_t G_t^T F_d^T$

$\approx \delta t G_tQ_tG^T_t$ (43)

<3>. covariance

初始时刻, 协方差为0, 即 $\large P^{b_k}_{b_k}=0$

$\large P^{b_k}_{t+\delta t}=F_dP^{b_k}_{t}F_d^T +Q_d$

$\large =(1+F_t\delta t)P^{b_k}_{t}(1+F_t\delta t)^T + \delta_tG_tQ_tG_t^T$ (44)

<4>. 雅可比迭代

初始时刻 $\large J_{b_k} = I$

$\large J_{t+\delta t} = (1+F_t\delta t)J_t$ , $\large t\in[k, k+1]$ (45)

(6). error dynamic 离散形式(中值积分)

<1>. 误差传播公式

(46)

简写为 $\large \delta z_{k+1}=F\delta z_{k}+ Vn$

其中， $\large F_t\in R^{15\times 15}$ ， $\large G_t\in R^{15\times 18}$ ， $\large \delta z_t^{b_k}\in R^{15\times 1}$ ， $\large n_t\in R^{18\times 1}$

<2>. 协方差预测

对于协方差:

$\large P_{k+1}=FP_{k}F^T + VQV^T$ (47)

初始时刻, 协方差为0, 即 $\large P^{b_k}_{b_k}=0$ ,

且 $\large n \sim N(0, Q), Q = diag (\sigma_a^2, \sigma_\omega^2 , \sigma_a^2, \sigma_\omega^2 , \sigma_{b_a}^2 , \sigma_{b_\omega}^2 )$ (48)

<3>. 雅可比迭代

$\large J_{k+1} = FJ_k$ , 初始时刻 $\large J_{b_k} = I$

5. correct preintegration results

When the estimation of bias changes, if the change is small,we adjust $\large \alpha ^{b_k}_{b_{k+1}}$ , $\large \beta ^{b_k}_{b_{k+1}}$ , and $\large \gamma ^{b_k}_{b_{k+1}}$ by their first-order approximations with respect to the bias(参考式(49)), otherwise we do repropagation. This strategy saves a significant amount of computational resources for optimization-based algorithms, since we do not need to propagate IMU measurements repeatedly.

$\alpha ^{b_k}_{b_{k+1}} \approx \hat{\alpha} ^{b_k}_{b_{k+1}} + J^\alpha_{b_a}\delta b_{a_k}+J^\alpha_{b_\omega}\delta b_{\omega_k}$

$\beta ^{b_k}_{b_{k+1}} \approx \hat{\beta} ^{b_k}_{b_{k+1}} + J^\beta_{b_a}\delta b_{a_k}+J^\beta_{b_\omega}\delta b_{\omega_k}$

$\gamma ^{b_k}_{b_{k+1}} \approx \hat{\gamma} ^{b_k}_{b_{k+1}} \otimes \begin{pmatrix} 1 \\ \frac{1}{2}J^\gamma_{b_\omega}\delta b_{w_k} \end{pmatrix}$ (49)

关于雅可比， $\large J^\alpha_{b_a}=\frac{\delta\alpha ^{b_k}_{b_{k+1}}}{\delta b_{a_k}}$ ， $\large J^\alpha_{b_\omega}=\frac{\delta\alpha ^{b_k}_{b_{k+1}}}{\delta b_{\omega_k}}$ ， $\large J^\beta_{b_a}=\frac{\delta\beta ^{b_k}_{b_{k+1}}}{\delta b_{a_k}}$ ， $\large J^\beta_{b_\omega}=\frac{\delta\beta ^{b_k}_{b_{k+1}}}{\delta b_{\omega_k}}$ ， $\large J^\gamma _{b_\omega}=\frac{\delta\theta ^{b_k}_{b_{k+1}}}{\delta b_{\omega_k}}$ ， 可以从上述离散化求得的雅可比 $\large J_{b_{k+1}}$ 中提取

四、Vins 源码解析—— class Estimator

1. API

// t: delta_time, 距离上一次 imu data 的时间差
// linear_acceleration: 线加速度
// angular_velocity: 角速度
void Estimator::processIMU(double t, const Vector3d &linear_acceleration, const Vector3d &angular_velocity);

2. processIMU 流程

(1). 创建 imu 预积分对象

// 对于视觉帧对应的第一个 imu data, 需要创建 imu 预积分对象
if (!pre_integrations[frame_count]) {
    pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
}

(2). imu 预积分

pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);

(3). 缓存原始数据

dt_buf[frame_count].push_back(dt);
linear_acceleration_buf[frame_count].push_back(linear_acceleration);
angular_velocity_buf[frame_count].push_back(angular_velocity);

(4). 依据 imu 观测更新 PVQ

中值积分

int j = frame_count;         
Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
Vs[j] += dt * un_acc;

(5). 更新 acc_0, gyr_0

acc_0 = linear_acceleration;
gyr_0 = angular_velocity;

五、Vins 源码解析—— class IntegrationBase

1. 成员变量

const Eigen::Vector3d linearized_acc, linearized_gyr;	初始时刻的 acc, gyr，用于 repropagate()
std::vector<double> dt_buf; std::vector<Eigen::Vector3d> acc_buf; std::vector<Eigen::Vector3d> gyr_buf;	原始 imu 观测，用于 repropagate()
double dt; Eigen::Vector3d acc_0, gyr_0; Eigen::Vector3d acc_1, gyr_1;	最新的两帧 acc, gyr 观测
double sum_dt; Eigen::Vector3d delta_p; Eigen::Quaterniond delta_q; Eigen::Vector3d delta_v; Eigen::Vector3d linearized_ba, linearized_bg;	imu 预积分结果
Eigen::Matrix<double, 18, 18> noise	imu 噪声
Eigen::Matrix<double, 15, 15> jacobian	jacobian
Eigen::Matrix<double, 15, 15> covariance	预积分约束 covariance

2. 构造函数

// _acc_0, _gyr_0: the first acc, gyr
// _linearized_ba, _linearized_bg: the acc, gyr bias
// 初始化: delta_p, delta_q, delta_v: 设置为 identity/zero
IntegrationBase(const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg)
        : acc_0{_acc_0}, gyr_0{_gyr_0}, linearized_acc{_acc_0}, linearized_gyr{_gyr_0},
          linearized_ba{_linearized_ba}, linearized_bg{_linearized_bg},
            jacobian{Eigen::Matrix<double, 15, 15>::Identity()}, covariance{Eigen::Matrix<double, 15, 15>::Zero()},
          sum_dt{0.0}, delta_p{Eigen::Vector3d::Zero()}, delta_q{Eigen::Quaterniond::Identity()}, delta_v{Eigen::Vector3d::Zero()}

    {   
        // set noise matrix, 更新 Q
        noise = Eigen::Matrix<double, 18, 18>::Zero();
        noise.block<3, 3>(0, 0) =  (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
        noise.block<3, 3>(3, 3) =  (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
        noise.block<3, 3>(6, 6) =  (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
        noise.block<3, 3>(9, 9) =  (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
        noise.block<3, 3>(12, 12) =  (ACC_W * ACC_W) * Eigen::Matrix3d::Identity();
        noise.block<3, 3>(15, 15) =  (GYR_W * GYR_W) * Eigen::Matrix3d::Identity();
    }

3. 处理新的数据

void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)
{
    // 压入 buffer
    dt_buf.push_back(dt);
    acc_buf.push_back(acc);
    gyr_buf.push_back(gyr);
    // 用原始的 bias 预积分
    propagate(dt, acc, gyr);
}

4. propagate

imu 预积分

void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)
{
    dt = _dt;
    acc_1 = _acc_1;
    gyr_1 = _gyr_1;
    Vector3d result_delta_p;
    Quaterniond result_delta_q;
    Vector3d result_delta_v;
    Vector3d result_linearized_ba;
    Vector3d result_linearized_bg;

    // 中值预积分 
    midPointIntegration(_dt, acc_0, gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v, linearized_ba, linearized_bg, result_delta_p, result_delta_q, result_delta_v, result_linearized_ba, result_linearized_bg, 1);

   // 更新中值预积分结果
   delta_p = result_delta_p;
   delta_q = result_delta_q;
   delta_v = result_delta_v;
   linearized_ba = result_linearized_ba;
   linearized_bg = result_linearized_bg;
   delta_q.normalize();
   sum_dt += dt;
   acc_0 = acc_1;
   gyr_0 = gyr_1;    
}

5. repropagate（循环调用 propagate）

用新的 bias，从初始时刻至最新时刻重新走一遍预积分

void repropagate(const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg) {
        sum_dt = 0.0;
        acc_0 = linearized_acc;
        gyr_0 = linearized_gyr;
        delta_p.setZero();
        delta_q.setIdentity();
        delta_v.setZero();
        linearized_ba = _linearized_ba;
        linearized_bg = _linearized_bg;
        jacobian.setIdentity();
        covariance.setZero();
        for (int i = 0; i < static_cast<int>(dt_buf.size()); i++)
            propagate(dt_buf[i], acc_buf[i], gyr_buf[i]);
}

6. 预积分（中值积分）

(1). 更新 pvq 增量

// 从 t0 时刻的状态推导 t1 时刻状态
// _dt:  t0->t1时间间隔
// _acc0: t0时加速度; _acc1: t1时加速度
// _gyr_0: t0时角速度; _gyr1: t1时角速度
// delta_p: t0时 \alpaha
// delta_q: t0时 \gamma
// delta_v: t0时 \beta
// linearized_ba, linearized_bg: bias
// result_delta_p: t1时 \alpaha
// reulst_delta_q: t1时 \gamma
// reulst_delta_v: t1时 \beta
// update_jacobian: 是否更新雅可比
void midPointIntegration(double _dt, const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
                            const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,
                            const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,
                            const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,
                            Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,
                            Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)
    {
        // 中值预积分
        Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
        Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
        result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
        Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
        result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
        result_delta_v = delta_v + un_acc * _dt;
        result_linearized_ba = linearized_ba;  // bias 没有改变
        result_linearized_bg = linearized_bg;         

        if(update_jacobian)  {
           // 更新 jacobian 和 covariance
           ......
        }

    }

(2). 更新 jacobian 和 Covariance

参考 error state dynamic（covariance）

    Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
    Vector3d a_0_x = _acc_0 - linearized_ba;
    Vector3d a_1_x = _acc_1 - linearized_ba;
    Matrix3d R_w_x, R_a_0_x, R_a_1_x;

    R_w_x<<0, -w_x(2), w_x(1),
                w_x(2), 0, -w_x(0),
                -w_x(1), w_x(0), 0;
    R_a_0_x<<0, -a_0_x(2), a_0_x(1),
                a_0_x(2), 0, -a_0_x(0),
                -a_0_x(1), a_0_x(0), 0;
    R_a_1_x<<0, -a_1_x(2), a_1_x(1),
                a_1_x(2), 0, -a_1_x(0),
                -a_1_x(1), a_1_x(0), 0;

    MatrixXd F = MatrixXd::Zero(15, 15);
    F.block<3, 3>(0, 0) = Matrix3d::Identity();
    F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt + 
                                  -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
    F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
    F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
    F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
    F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
    F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
    F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt + 
                                  -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;
    F.block<3, 3>(6, 6) = Matrix3d::Identity();
    F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;
    F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;
    F.block<3, 3>(9, 9) = Matrix3d::Identity();
    F.block<3, 3>(12, 12) = Matrix3d::Identity();
    //cout<<"A"<<endl<<A<<endl;

    MatrixXd V = MatrixXd::Zero(15,18);
    V.block<3, 3>(0, 0) =  0.25 * delta_q.toRotationMatrix() * _dt * _dt;
    V.block<3, 3>(0, 3) =  0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * _dt * 0.5 * _dt;
    V.block<3, 3>(0, 6) =  0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;
    V.block<3, 3>(0, 9) =  V.block<3, 3>(0, 3);
    V.block<3, 3>(3, 3) =  0.5 * MatrixXd::Identity(3,3) * _dt;
    V.block<3, 3>(3, 9) =  0.5 * MatrixXd::Identity(3,3) * _dt;
    V.block<3, 3>(6, 0) =  0.5 * delta_q.toRotationMatrix() * _dt;
    V.block<3, 3>(6, 3) =  0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * 0.5 * _dt;
    V.block<3, 3>(6, 6) =  0.5 * result_delta_q.toRotationMatrix() * _dt;
    V.block<3, 3>(6, 9) =  V.block<3, 3>(6, 3);
    V.block<3, 3>(9, 12) = MatrixXd::Identity(3,3) * _dt;
    V.block<3, 3>(12, 15) = MatrixXd::Identity(3,3) * _dt;

    //step_jacobian = F;
    //step_V = V;
    jacobian = F * jacobian;
    covariance = F * covariance * F.transpose() + V * noise * V.transpose();

7. evaluate()

参考 imu factor