IMU预积分类——IntegrationBase

IMU预积分的理论推导

为了便于代码分析，这里先给出VINS-Mono中IMU预积分的理论推导。

IMU噪声与零偏模型

IMU的量测模型可表示为：
$$\{\begin{array}{rl}& {\hat{\mathbf{a}}}^b={\mathbf{a}}^b+{\mathbf{b}}_a+{\mathbf{R}}_w^b{\mathbf{g}}^w+{\mathbf{n}}_a\\ & {\hat{\mathbf{\omega }}}^b={\mathbf{\omega }}^b+{\mathbf{b}}_{\omega }+{\mathbf{n}}_{\omega }\end{array}\text{\hspace{1em}(1)}$$
式中，${\hat{\mathbf{a}}}^b$和${\hat{\mathbf{\omega }}}^b$分别表示IMU的加速度和角速度测量信息；${\mathbf{a}}^b$和${\mathbf{\omega }}^b$分别为载体真实的加速度和角速度；${\mathbf{b}}_a$和${\mathbf{b}}_{\omega }$分别为加速度计和陀螺仪零偏，建模为随机游走过程:
$$\{\begin{array}{rl}& {\mathbf{n}}_{{\mathbf{b}}_a}\sim \mathcal{N}\left(0,{\mathbf{\sigma }}_{b_a}^2\right),{\mathbf{n}}_{{\mathbf{b}}_{\omega }}\sim \mathcal{N}\left(0,{\mathbf{\sigma }}_{b_{\omega }}^2\right)\\ & \dot{{\mathbf{b}}_a}={\mathbf{n}}_{{\mathbf{b}}_a},\dot{{\mathbf{b}}_{\omega }}={\mathbf{n}}_{{\mathbf{b}}_{\omega }}\end{array}\text{\hspace{1em}(2)}$$
${\mathbf{R}}_w^b$为世界坐标系到本体坐标系的坐标旋转矩阵；${\mathbf{g}}^w$为世界坐标系下的重力加速度矢量；${\mathbf{n}}_a\sim \mathcal{N}\left(0,{\mathbf{\sigma }}_a^2\right)$和${\mathbf{n}}_{\omega }\sim \left(0,{\mathbf{\sigma }}_{\omega }^2\right)$为器件的高斯白噪声。

IMU预积分状态传播方程

连续时间形式

载体的位置、速度和姿态四元数在时间区间$\left[t_k,t_{k+1}\right]$内的传播方程为：
$$\{\begin{array}{rl}& {\mathbf{p}}_{k+1}^w={\mathbf{p}}_k^w+{\mathbf{v}}_k^w\Delta t_k+{\iint }_{t_i\in \left[t_k,t_{k+1}\right]}\left({\mathbf{R}}_{b_{t_i}}^w\left({\hat{\mathbf{a}}}_{t_i}-{\mathbf{b}}_a-{\mathbf{n}}_a\right)-{\mathbf{g}}^w\right)d{t}^2\\ & {\mathbf{v}}_{k+1}^w={\mathbf{v}}_k^w+{\int }_{t_i\in \left[t_k,t_{k+1}\right]}\left({\mathbf{R}}_{b_{t_i}}^w\left({\hat{\mathbf{a}}}_{t_i}-{\mathbf{b}}_a-{\mathbf{n}}_a\right)-{\mathbf{g}}^w\right)dt\\ & {\mathbf{q}}_w^{b_{k+1}}={\mathbf{q}}_w^{b_k}\otimes {\int }_{t_i\in \left[t_k,t_{k+1}\right]}\frac{1}{2}\mathbf{\Omega }\left({\hat{\mathbf{\omega }}}_{wb}^w-{\mathbf{b}}_{\omega }-{\mathbf{n}}_{\omega }\right){\mathbf{q}}_k^{w}dt\end{array}\text{\hspace{1em}(3)}$$
式中${\mathbf{q}}_w^{b_{k+1}}$为世界坐标系到本体坐标系的旋转四元数，$\otimes$表示四元数乘法，$\mathbf{\Omega }\left(\mathbf{\omega }\right)$为上一小节中由$\mathbf{\omega }$组成的齐次矩阵，${\mathbf{b}}_k$和${\mathbf{b}}_{k+1}$分别为第$k$帧和第$k+1$帧对应的体坐标系，${\mathbf{b}}_{t_i}$为$t_i$时刻的体坐标系。

可以看出，IMU的由第$k$帧向第$k+1$帧的状态传播需要第$k$帧的旋转、位置和速度信息，这使得后端优化时，每当$k$帧位姿优化完后均需要重新计算$k+1$的相关变量；而若将式(3)中右侧的积分项表示在$k$帧对应的本体系下，则该项不包含任何需要后端优化的位姿变量而只依赖于IMU的量测信息，更加便于后端优化处理。因此，增量形式的IMU状态传播方程为：
$$\{\begin{array}{rl}& {\mathbf{R}}_w^{b_k}{\mathbf{p}}_{k+1}^w={\mathbf{R}}_w^{b_k}\left({\mathbf{p}}_k^w+{\mathbf{v}}_k^w\Delta t_k-\frac{1}{2}{\mathbf{g}}^w\Delta t_k^2\right)+{\hat{\mathbf{\alpha }}}_{k+1}^{b_k}\\ & {\mathbf{R}}_w^{b_k}{\mathbf{v}}_{k+1}^w={\mathbf{R}}_w^{b_k}\left({\mathbf{v}}_k^w-{\mathbf{g}}^w\Delta t_k\right)+{\hat{\mathbf{\beta }}}_{k+1}^{b_k}\\ & {\mathbf{q}}_{b_k}^w\otimes {\mathbf{q}}_w^{b_{k+1}}={\hat{\mathbf{\gamma }}}_{k+1}^{b_k}\end{array}\text{\hspace{1em}(4)}$$
式中，${\mathbf{\alpha }}_{k+1}^{b_k}$、${\mathbf{\beta }}_{k+1}^{b_k}$和${\mathbf{\gamma }}_{\mathbf{k}+1}^{{\mathbf{b}}_{\mathbf{k}}}$分别为$b_k$坐标系下第$k$和$k+1$帧之间IMU的预积分项：
$$\{\begin{array}{rl}& {\hat{\mathbf{\alpha }}}_{k+1}^{b_k}={\iint }_{t_i\in \left[t_k,t_{k+1}\right]}\left({\mathbf{R}}_{b_{t_i}}^{b_k}\left({\hat{\mathbf{a}}}_{t_i}-{\mathbf{b}}_a-{\mathbf{n}}_a\right)\right)d{t}^2\\ & {\hat{\mathbf{\beta }}}_{k+1}^{b_k}={\int }_{t_i\in \left[t_k,t_{k+1}\right]}\left({\mathbf{R}}_{b_{t_i}}^{b_k}\left({\hat{\mathbf{a}}}_{t_i}-{\mathbf{b}}_a-{\mathbf{n}}_a\right)\right)dt\\ & {\hat{\mathbf{\gamma }}}_{k+1}^{b_k}={\int }_{t_i\in \left[t_k,t_{k+1}\right]}\frac{1}{2}\mathbf{\Omega }\left({\hat{\mathbf{\omega }}}_{wb}^w-{\mathbf{b}}_{\omega }-{\mathbf{n}}_{\omega }\right){\mathbf{\gamma }}_{b_k}^{t_i}dt\end{array}\text{\hspace{1em}(5)}$$
可以看出，(5)中的IMU预积分项只和实际IMU期间量测以及IMU零偏有关。当后端优化的IMU零偏变化时，若变化较小，则使用IMU预积分项关于零偏的一阶近似更新预积分项；否则，则按照式(5)重新积分IMU得到预积分项。IMU预积分关于零偏的线性化更新表达式可以表示为：
$$\{\begin{array}{rl}& {\hat{\mathbf{\alpha }}}_k^{k+1}\approx {\hat{\mathbf{\alpha }}}_k^{k+1}+{\mathbf{J}}_{{\mathbf{b}}_a}^{\mathbf{\alpha }}\delta {\mathbf{b}}_a+{\mathbf{J}}_{{\mathbf{b}}_{\omega }}^{\mathbf{\alpha }}\delta {\mathbf{b}}_{\omega }\\ & {\hat{\mathbf{\beta }}}_k^{k+1}\approx {\hat{\mathbf{\beta }}}_k^{k+1}+{\mathbf{J}}_{{\mathbf{b}}_a}^{\mathbf{\beta }}\delta {\mathbf{b}}_a+{\mathbf{J}}_{{\mathbf{b}}_{\omega }}^{\mathbf{\beta }}\delta {\mathbf{b}}_{\omega }\\ & {\hat{\mathbf{\gamma }}}_k^{k+1}={\hat{\mathbf{\gamma }}}_k^{k+1}\otimes {\mathbf{J}}_{{\mathbf{b}}_{\omega }}^{\mathbf{\gamma }}\delta {\mathbf{b}}_{\omega }\end{array}\text{\hspace{1em}(6)}$$
这里公式的第三行与原文不同的原因是，原文中虽然偏导数写的是${\mathbf{J}}_{{\mathbf{b}}_{\omega }}^{\mathbf{\gamma }}$，但为了与后续误差动力学方程对应，实际采用的偏导数是${\mathbf{J}}_{{\mathbf{b}}_{\omega }}^{\mathbf{\theta }}$，即：
$${\mathbf{J}}_{{\mathbf{b}}_{\omega }}^{\mathbf{\gamma }}\delta {\mathbf{b}}_{\omega }=\left[\begin{array}{c}1\\ \frac{1}{2}{\mathbf{J}}_{{\mathbf{b}}_{\omega }}^{\mathbf{\theta }}\delta {\mathbf{b}}_{\omega }\end{array}\right]\text{\hspace{1em}(7)}$$

离散时间形式

为了与程序中对应，需要对连续形式的IMU传播方程进行离散化，由于程序中使用的是中值积分，这里也仅给出中值积分的IMU预积分离散传播方程为：
$$\{\begin{array}{rl}& {\mathbf{p}}_{i+1}^w={\mathbf{p}}_i^w+{\mathbf{v}}_i^w\Delta t_i+\frac{1}{2}{\overline{\mathbf{a}}}_{t_i}\delta t^2\\ & {\mathbf{v}}_{i+1}^w={\mathbf{v}}_i^w+{\overline{\mathbf{a}}}_{t_i}\delta t\\ & {\mathbf{q}}_w^{b_{i+1}}={\mathbf{q}}_w^{b_i}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\overline{\mathbf{\omega }}}_{t_i}\delta t\end{array}\right]\end{array}\text{\hspace{1em}(8)}$$
其中：
$$\{\begin{array}{rl}& {\overline{\mathbf{a}}}_{t_i}=\frac{1}{2}\left[{\mathbf{R}}_{b_{t_i}}^w\left({\hat{\mathbf{a}}}_{t_i}-{\mathbf{b}}_a\right)+{\mathbf{R}}_{b_{t_{i+1}}}^w\left({\hat{\mathbf{a}}}_{t_{i+1}}-{\mathbf{b}}_a\right)\right]-{\mathbf{g}}^w\\ & {\overline{\mathbf{\omega }}}_{t_i}=\frac{1}{2}\left({\hat{\mathbf{\omega }}}_{t_i}+{\hat{\mathbf{\omega }}}_{t_{i+1}}\right)-{\mathbf{b}}_{\omega }\end{array}\text{\hspace{1em}(9)}$$

IMU预积分误差动力学方程

连续时间形式

在VINS的优化中，由于采用马氏距离构建二乘函数，需要对状态的协方差矩阵进行更新，而协方差矩阵的一步预测依赖于状态的误差动力学方程，因此这里我们首先就IMU预积分项的误差动力学方程进行推导。涉及的状态量包括位置增量${\mathbf{\alpha }}_{k+1}^{b_k}$、速度增量${\mathbf{\beta }}_{k+1}^{b_k}$、姿态增量${\mathbf{\theta }}_{k+1}^{b_k}$、加速度计零偏${\mathbf{b}}_a$和陀螺仪零偏${\mathbf{b}}_{\mathbf{\omega }}$，其中由于陀螺器件测量为角增量，这里使用角增量${\mathbf{\theta }}_{k+1}^{b_k}$代替四元数增量${\mathbf{\gamma }}_{k+1}^{b_k}$。

由于我们需要求解的是上述各增量的误差动力学方程，因此系统状态空间矢量可记为：
$$\delta \mathbf{x}=\left[\begin{array}{ccccccccc}\delta {\mathbf{\alpha }}_t^{b_k}& \delta {\mathbf{\beta }}_t^{b_k}& \delta {\mathbf{\theta }}_t^{b_k}& \delta {\mathbf{b}}_a& \delta {\mathbf{b}}_{\omega }& {\mathbf{n}}_a& {\mathbf{n}}_{\omega }& {\mathbf{n}}_{{\mathbf{b}}_a}& {\mathbf{n}}_{{\mathbf{b}}_{\omega }}\end{array}\right]\text{\hspace{1em}(10)}$$
由于增量表示的是$k+1$和$k$之间的相对值，这里为了书写方便，下标使用$k$而非前面$k+1$，同时改用下标$t$表示IMU测量时刻，以区别图像帧时刻$k$，但两者本质上是同一个量。同时，由于导数运算的顺序可以交换，即$\delta \dot{\mathbf{a}}=\dot{\delta \mathbf{a}}$，我们可以先求当前变量的时间导数，再求其对各状态量的偏导数。

• 位置增量误差$\delta {\mathbf{\alpha }}_t^{b_k}$
  位置增量的时间导数为${\dot{\mathbf{\alpha }}}_t^{b_k}={\mathbf{\beta }}_t^{b_k}$，显然，除了偏导数$\delta {\dot{\mathbf{\alpha }}}_t^{b_k}/\delta {\mathbf{\beta }}_t^{b_k}=\mathbf{I}$外，位置增量的导数对其余状态增量的导数为零，即其余状态增量的变化对位置增量误差的导数没有贡献.
• 速度增量误差$\delta {\mathbf{\beta }}_t^{b_k}$
  速度增量的时间导数为${\dot{\mathbf{\beta }}}_t^{b_k}={\mathbf{R}}_{b_t}^{b_k}\left({\hat{\mathbf{a}}}_t-{\mathbf{b}}_a-{\mathbf{n}}_a\right)$，可以看出非零偏导数共有三项，分别为：
  $$\begin{array}{rl}\frac{\delta {\mathbf{\beta }}_t^{b_k}}{\delta {\mathbf{\theta }}_k^{b_k}}& =-{\mathbf{R}}_{b_t}^{b_k}\left({\hat{\mathbf{a}}}_t-{\mathbf{b}}_a-{\mathbf{n}}_a\right)\\ & \approx -{\mathbf{R}}_{b_t}^{b_k}\left({\hat{\mathbf{a}}}_t-{\mathbf{b}}_a\right)\\ \frac{\delta {\mathbf{\beta }}_t^{b_k}}{{\mathbf{b}}_a}& =-{\mathbf{R}}_{b_t}^{b_k}\\ \frac{\delta {\mathbf{\beta }}_t^{b_k}}{{\mathbf{n}}_a}& =-{\mathbf{R}}_{b_t}^{b_k}\end{array}\text{\hspace{1em}(11)}$$
  其中，由于状态递推的本质是随机变量均值的传播，而${\mathbf{n}}_a$是零均值的高斯白噪声，可作为零值处理。
• 角增量误差$\delta {\mathbf{\theta }}_t^{b_k}$
  由于角增量的运算中涉及四元数的乘法，使用导数微分定义求解会比较复杂，这里我们换一种思路，假设无任何偏差的四元数时间导数和带各项扰动的四元数时间导数分别为${\dot{\mathbf{q}}}_n$和${\dot{\mathbf{q}}}_t$，则：
  $$\begin{array}{rl}{\dot{\mathbf{q}}}_t& =\frac{1}{2}\underset{\delta \mathbf{\theta }}{\mathbf{q}\otimes \delta \mathbf{q}}\otimes \left[\begin{array}{c}0\\ \hat{\mathbf{\omega }}-{\mathbf{b}}_{\omega }-{\mathbf{n}}_{\omega }-\delta {\mathbf{b}}_{\omega }\end{array}\right]\\ {\dot{\mathbf{q}}}_n& =\frac{1}{2}\mathbf{q}\otimes \left[\begin{array}{c}0\\ \hat{\mathbf{\omega }}-{\mathbf{b}}_{\omega }\end{array}\right]\end{array}\text{\hspace{1em}(12)}$$
  同时，${\dot{\mathbf{q}}}_t$又可表示为：
  $$\begin{array}{rl}{\dot{\mathbf{q}}}_t& =\frac{\partial \left(\mathbf{q}\otimes \delta \mathbf{q}\right)}{\partial t}=\dot{\mathbf{q}}\otimes \delta \mathbf{q}+\mathbf{q}\otimes \dot{\delta \mathbf{q}}\\ & =\frac{1}{2}\mathbf{q}\otimes \left[\begin{array}{c}0\\ \mathbf{\omega }-{\mathbf{b}}_{\omega }\end{array}\right]\otimes \delta \mathbf{q}+\mathbf{q}\otimes \dot{\delta \mathbf{q}}\end{array}\text{\hspace{1em}(13)}$$
  令上述两式相等可得到：
  $$\begin{array}{rl}& \frac{1}{2}\mathbf{q}\otimes \delta \mathbf{q}\otimes \left[\begin{array}{c}0\\ \hat{\mathbf{\omega }}-{\mathbf{b}}_{\omega }-{\mathbf{n}}_{\omega }-\delta {\mathbf{b}}_{\omega }\end{array}\right]=\frac{1}{2}\mathbf{q}\otimes \left[\begin{array}{c}0\\ \mathbf{\omega }-{\mathbf{b}}_{\omega }\end{array}\right]\otimes \delta \mathbf{q}+\mathbf{q}\otimes \dot{\delta \mathbf{q}}\\ \Rightarrow & \frac{1}{2}\delta \mathbf{q}\otimes \left[\begin{array}{c}0\\ \hat{\mathbf{\omega }}-{\mathbf{b}}_{\omega }-{\mathbf{n}}_{\omega }-\delta {\mathbf{b}}_{\omega }\end{array}\right]=\frac{1}{2}\left[\begin{array}{c}0\\ \mathbf{\omega }-{\mathbf{b}}_{\omega }\end{array}\right]\otimes \delta \mathbf{q}+\dot{\delta \mathbf{q}}\\ \Rightarrow & 2\dot{\delta }\mathbf{q}=\delta \mathbf{q}\otimes \left[\begin{array}{c}0\\ \hat{\mathbf{\omega }}-{\mathbf{b}}_{\omega }-{\mathbf{n}}_{\omega }-\delta {\mathbf{b}}_{\omega }\end{array}\right]-\left[\begin{array}{c}0\\ \mathbf{\omega }-{\mathbf{b}}_{\omega }\end{array}\right]\otimes \delta \mathbf{q}\\ \Rightarrow & 2\dot{\delta \mathbf{q}}=\mathcal{R}\left(\left[\begin{array}{c}0\\ \hat{\mathbf{\omega }}-{\mathbf{b}}_{\omega }-{\mathbf{n}}_{\omega }-\delta {\mathbf{b}}_{\omega }\end{array}\right]\right)\delta \mathbf{q}-\mathcal{L}\left(\left[\begin{array}{c}0\\ {\mathbf{n}}_{\omega }+\delta {\mathbf{b}}_{\omega }\end{array}\right]\right)\delta \mathbf{q}\\ \Rightarrow & 2\dot{\delta \mathbf{q}}=\left[\begin{array}{cc}0& {\left({\mathbf{n}}_{\omega }+\delta {\mathbf{b}}_{\omega }\right)}^T\\ -\left({\mathbf{n}}_{\omega }-\delta {\mathbf{b}}_{\omega }\right)& -{\left(2\hat{\mathbf{\omega }}-2{\mathbf{b}}_{\omega }-{\mathbf{n}}_{\omega }-\delta {\mathbf{b}}_{\omega }\right)}^{\wedge }\end{array}\right]\left[\begin{array}{c}1\\ \frac{\delta \mathbf{\theta }}{2}\end{array}\right]\end{array}\text{\hspace{1em}(14)}$$
  将$\dot{\delta \mathbf{q}}=\left[\begin{array}{c}0\\ \frac{1}{2}\dot{\delta \mathbf{\theta }}\end{array}\right]$带入上式可以并忽略零均值项${\mathbf{n}}_{\omega }$以及二阶小量$\delta {\mathbf{b}}_{\omega }\delta \mathbf{\theta }$可以得到：
  $$\begin{array}{rl}\left[\begin{array}{c}0\\ \dot{\delta \mathbf{\theta }}\end{array}\right]& =\left[\begin{array}{cc}0& {\left({\mathbf{n}}_{\omega }+\delta {\mathbf{b}}_{\omega }\right)}^T\\ -\left({\mathbf{n}}_{\omega }-\delta {\mathbf{b}}_{\omega }\right)& -{\left(2\hat{\mathbf{\omega }}-2{\mathbf{b}}_{\omega }-{\mathbf{n}}_{\omega }-\delta {\mathbf{b}}_{\omega }\right)}^{\wedge }\end{array}\right]\left[\begin{array}{c}1\\ \frac{\delta \mathbf{\theta }}{2}\end{array}\right]\\ \Rightarrow \dot{\delta \mathbf{\theta }}& \approx -{\left(\hat{\mathbf{\omega }}-{\mathbf{b}}_{\omega }\right)}^{\wedge }\delta \mathbf{\theta }-{\mathbf{n}}_{\omega }-\delta {\mathbf{b}}_{\omega }\end{array}\text{\hspace{1em}(15)}$$
• 加速度计零偏误差$\delta {\mathbf{b}}_a$：根据IMU的器件模型有$\delta {\dot{\mathbf{b}}}_a={\mathbf{n}}_{{\mathbf{b}}_a}$。
• 陀螺零偏误差$\delta {\mathbf{b}}_{\omega }$：同样根据IMU的器件模型有$\delta {\dot{\mathbf{b}}}_{\omega }={\mathbf{n}}_{{\mathbf{b}}_{\omega }}$。

至此，我们已经获得了所有非零均值状态量动力学方程，写成矩阵形式为：
$$\begin{array}{rl}\left[\begin{array}{c}\delta {\dot{\mathbf{\alpha }}}_t^{b_k}\\ \delta {\dot{\mathbf{\beta }}}_t^{b_k}\\ \delta {\dot{\mathbf{\theta }}}_t^{b_k}\\ \delta \dot{{\mathbf{b}}_a}\\ \delta \dot{{\mathbf{b}}_{\omega }}\end{array}\right]& =\left[\begin{array}{ccccc}0& \mathbf{I}& 0& 0& 0\\ 0& 0& -{\mathbf{R}}_{b_t}^{b_k}{\left({\hat{\mathbf{a}}}_t-{\mathbf{b}}_a\right)}^{\wedge }& -{\mathbf{R}}_{b_t}^{b_k}& 0\\ 0& 0& -{\left({\hat{\mathbf{\omega }}}_t-{\mathbf{b}}_{\omega }\right)}^{\wedge }& 0& -\mathbf{I}\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}\delta {\mathbf{\alpha }}_t^{b_k}\\ \delta {\mathbf{\beta }}_t^{b_k}\\ \delta {\mathbf{\theta }}_t^{b_k}\\ \delta {\mathbf{b}}_a\\ \delta {\mathbf{b}}_{\omega }\end{array}\right]\\ & +\left[\begin{array}{cccc}0& 0& 0& 0\\ -{\mathbf{R}}_{b_t}^{b_k}& 0& 0& 0\\ 0& -\mathbf{I}& 0& 0\\ 0& 0& \mathbf{I}& 0\\ 0& 0& 0& \mathbf{I}\end{array}\right]\left[\begin{array}{c}{\mathbf{n}}_a\\ {\mathbf{n}}_{\omega }\\ {\mathbf{n}}_{{\mathbf{b}}_a}\\ {\mathbf{n}}_{{\mathbf{b}}_{\omega }}\end{array}\right]\\ \Rightarrow {\dot{\delta \mathbf{x}}}_t& ={\mathbf{f}}_t\delta {\mathbf{x}}_t+{\mathbf{v}}_t{\mathbf{n}}_t\end{array}\text{\hspace{1em}(16)}$$
根据导数的定义，可以将上式改写为递推形式：
$$\begin{array}{rl}\frac{\delta {\mathbf{x}}_{t+1}-\delta {\mathbf{x}}_t}{\delta t}& ={\mathbf{f}}_t\delta {\mathbf{x}}_t+{\mathbf{v}}_t{\mathbf{n}}_t\\ \delta {\mathbf{x}}_{t+1}& =\delta {\mathbf{x}}_t+{\mathbf{f}}_t\delta {\mathbf{x}}_t\delta t+{\mathbf{v}}_t{\mathbf{n}}_t\delta t\\ & =\underset{{\mathbf{F}}_t}{\left(\mathbf{I}+{\mathbf{f}}_t\delta t\right)}\delta {\mathbf{x}}_t+\underset{{\mathbf{V}}_t}{{\mathbf{v}}_t\delta t}{\mathbf{n}}_t\end{array}\text{\hspace{1em}(17)}$$
可以看出，上式是一个标准的状态空间模型，给出了随机变量均值的传播方程；同样，我们可以直接写出随机变量方差的传播方程：
$$\begin{array}{rl}{\mathbf{P}}_{t+1}& ={\mathbf{F}}_t{\mathbf{P}}_t{\mathbf{F}}_t^T+{\mathbf{V}}_t{\mathbf{Q}}_t{\mathbf{V}}_t^T\\ {\mathbf{P}}_0& =0\\ {\mathbf{Q}}_0& =\left[\begin{array}{cccc}{\sigma }_a^2& 0& 0& 0\\ 0& {\sigma }_{\omega }^2& 0& 0\\ 0& 0& {\sigma }_{{\mathbf{b}}_a}^2& 0\\ 0& 0& 0& {\sigma }_{{\mathbf{b}}_{\omega }}^2\end{array}\right]\end{array}\text{\hspace{1em}(18)}$$
误差雅克比矩阵的更新为：
$$\begin{array}{rl}{\mathbf{J}}_{k+1}& =\prod _{t\in \left[k,k+1\right]}{\mathbf{F}}_t{\mathbf{J}}_k\\ {\mathbf{J}}_k& =\mathbf{I}\end{array}\text{\hspace{1em}(19)}$$
注意这里的下标为图像帧$k$而非IMU量测$t$。

离散时间形式

同样，这里采用中值积分对式（16）进行离散化，由于离散化推导过程较为繁杂，这里直接给出结论，详细过程参见github链接。
$$\begin{array}{rl}\left[\begin{array}{c}\delta {\mathbf{\alpha }}_{t+1}\\ \delta {\mathbf{\theta }}_{t+1}\\ \delta {\mathbf{\beta }}_{t+1}\\ \delta {{\mathbf{b}}_a}_{t+1}\\ \delta {{\mathbf{b}}_{\omega }}_{t+1}\end{array}\right]& =\left[\begin{array}{ccccc}\mathbf{I}& f_{01}& \delta t\mathbf{I}& f_{03}& f_{04}\\ 0& f_{11}& 0& 0& -\delta t\mathbf{I}\\ 0& f_{21}& \mathbf{I}& f_{23}& f_{24}\\ 0& 0& 0& \mathbf{I}& 0\\ 0& 0& 0& 0& \mathbf{I}\end{array}\right]\left[\begin{array}{c}\delta {\mathbf{\alpha }}_t\\ \delta {\mathbf{\theta }}_t\\ \delta {\mathbf{\beta }}_t\\ \delta {{\mathbf{b}}_a}_t\\ \delta {{\mathbf{b}}_{\omega }}_t\end{array}\right]\\ & +\left[\begin{array}{cccccc}{v}_{00}& v_{01}& v_{02}& v_{03}& 0& 0\\ 0& -\frac{\delta t}{2}\mathbf{I}& 0& -\frac{\delta t}{2}\mathbf{I}& 0& 0\\ -\frac{{\mathbf{R}}_{b_t}^{b_k}\delta t}{2}& v_{21}& -\frac{{\mathbf{R}}_{b_{t+1}}^{b_k}\delta t}{2}& v_{23}& 0& 0\\ 0& 0& 0& 0& \delta t\mathbf{I}& 0\\ 0& 0& 0& 0& 0& \delta t\mathbf{I}\end{array}\right]\left[\begin{array}{c}{{\mathbf{n}}_a}_t\\ {{\mathbf{n}}_{\omega }}_t\\ {{\mathbf{n}}_a}_{t+1}\\ {{\mathbf{n}}_{\omega }}_{t+1}\\ {\mathbf{n}}_{{\mathbf{b}}_a}\\ {\mathbf{n}}_{{\mathbf{b}}_{\omega }}\end{array}\right]\end{array}\text{\hspace{1em}(20)}$$
$\mathbf{F}$矩阵中对应元素具体表达式如下：
$$\begin{array}{rl}{f}_{01}& =\left\{-\frac{1}{4}{\mathbf{R}}_{b_t}^{b_k}{\left({\hat{\mathbf{a}}}_t-{\mathbf{b}}_a\right)}^{\wedge }\delta t^2-\frac{1}{4}{\mathbf{R}}_{b_{t+1}}^{b_k}{\left({\hat{\mathbf{a}}}_{t+1}-{\mathbf{b}}_a\right)}^{\wedge }\left[\mathbf{I}-{\left(\frac{{\hat{\mathbf{\omega }}}_t+{\hat{\mathbf{\omega }}}_{t+1}}{2}-{\mathbf{b}}_{\omega }\right)}^{\wedge }\delta t\right]\delta t^2\right\}\\ f_{03}& =-\frac{1}{4}\left({\mathbf{R}}_{b_t}^{b_k}+{\mathbf{R}}_{b_{t+1}}^{b_k}\right)\delta t^2\\ f_{04}& =\frac{1}{4}{\mathbf{R}}_{b_{t+1}}^{b_k}{\left({\hat{\mathbf{a}}}_{t+1}-{\mathbf{b}}_a\right)}^{\wedge }\delta t^3\\ f_{11}& =\left[\mathbf{I}-{\left(\frac{{\hat{\mathbf{\omega }}}_t+{\hat{\mathbf{\omega }}}_{t+1}}{2}-{\mathbf{b}}_{\omega }\right)}^{\wedge }\delta t\right]\\ f_{21}& =\left\{-\frac{1}{2}{\mathbf{R}}_{b_t}^{b_k}{\left({\hat{\mathbf{a}}}_t-{\mathbf{b}}_a\right)}^{\wedge }-\frac{1}{2}{\mathbf{R}}_{b_{t+1}}^{b_k}{\left({\hat{\mathbf{a}}}_{t+1}-{\mathbf{b}}_a\right)}^{\wedge }\left[\mathbf{I}-{\left(\frac{{\hat{\mathbf{\omega }}}_t+{\hat{\mathbf{\omega }}}_{t+1}}{2}-{\mathbf{b}}_{\omega }\right)}^{\wedge }\delta t\right]\right\}\delta t\\ f_{23}& =-\frac{1}{2}\left({\mathbf{R}}_{b_t}^{b_k}+{\mathbf{R}}_{b_{t+1}}^{b_k}\right)\delta t\\ f_{24}& =\frac{1}{2}{\mathbf{R}}_{b_{t+1}}^{b_k}{\left({\hat{\mathbf{a}}}_{t+1}-{\mathbf{b}}_a\right)}^{\wedge }\delta t^2\end{array}\text{\hspace{1em}(21)}$$
$\mathbf{V}$矩阵中对应元素具体表达式如下：
$$\begin{array}{rl}{v}_{00}& =-\frac{1}{4}{\mathbf{R}}_{b_t}^{b_k}\delta t^2\\ v_{01}& =\frac{1}{8}{\mathbf{R}}_{b_{t+1}}^{b_k}{\left({\hat{\mathbf{a}}}_{t+1}-{\mathbf{b}}_a\right)}^{\wedge }\delta t^3\\ v_{02}& =-\frac{1}{4}{\mathbf{R}}_{b_{t+1}}^{b_k}\delta t^2\\ v_{03}& =\frac{1}{8}{\mathbf{R}}_{b_{t+1}}^{b_k}{\left({\hat{\mathbf{a}}}_{t+1}-{\mathbf{b}}_a\right)}^{\wedge }\delta t^3\\ v_{21}& =\frac{1}{4}{\mathbf{R}}_{b_{t+1}}^{b_k}{\left({\hat{\mathbf{a}}}_{t+1}-{\mathbf{b}}_a\right)}^{\wedge }\delta t^2\\ v_{23}& =\frac{1}{4}{\mathbf{R}}_{b_{t+1}}^{b_k}{\left({\hat{\mathbf{a}}}_{t+1}-{\mathbf{b}}_a\right)}^{\wedge }\delta t^2\end{array}\text{\hspace{1em}(22)}$$

IntegrationBase类详解

成员变量

IntegrationBase类是VINS-Mono中用于处理IMU预积分相关功能的类，位于vins_estimator/src/factor/integration_base.h头文件中，包含的成员变量如下：

    double dt; // 每次预积分的时间周期长度
    Eigen::Vector3d acc_0, gyr_0; // t时刻对应的IMU测量值
    Eigen::Vector3d acc_1, gyr_1; // t+1时刻对应的IMU测量值

    const Eigen::Vector3d linearized_acc, linearized_gyr; // k帧图像时刻对应的IMU测量值
    Eigen::Vector3d linearized_ba, linearized_bg; // 加速度计和陀螺仪零偏，在[k,k+1]区间上视为不变

    Eigen::Matrix<double, 15, 15> jacobian, covariance; // 预积分误差的雅克比矩阵
    Eigen::Matrix<double, 18, 18> noise; //系统噪声矩阵

    double sum_dt; //所有IMU预积分区间的总时长，由于量测的不同步性，不一定有sum_dt = (k+1)-k
    Eigen::Vector3d delta_p; // 位置预积分
    Eigen::Quaterniond delta_q; // 旋转四元数预积分
    Eigen::Vector3d delta_v; // 速度预积分

    std::vector<double> dt_buf; // 用于存储每次预积分时间dt的寄存器
    std::vector<Eigen::Vector3d> acc_buf; // 用于存储每次预积分加速度量测的寄存器
    std::vector<Eigen::Vector3d> gyr_buf; // 用于存储每次预积分角速度量测的寄存器

成员函数

构造函数（IntegrationBase）

VINS-Mono要求预积分中的参数必须进行初始化，因此移除了默认构造函数，仅允许使用如下所示的唯一构造函数：

 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()}

    {
        // 噪声矩阵
        // ACC_N, GYR_N: 加速度计和陀螺白噪声均值
        // ACC_W, GYR_W: 加速度计和陀螺随机游走
        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();
    }

该构造函数接收第k帧时刻对应的加速度、角速度、加速度计零偏和陀螺仪零偏，并完成各成员变量的初始化，初始化中的各成员变量含义上一节已说明，此处不再赘述。

中值积分函数（midPointIntegration）

该函数用于求解IMU的预积分状态递推和更新对应的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)

函数接口参数分别为：

• t时刻的IMU量测：_acc_0, _gyr_0
• t时刻的位置、姿态、速度预积分估计值：delta_p, delta_q, delta_v
• t+1时刻的IMU量测：_acc_1, _gyr_1
• t+1时刻的位置、姿态、速度预积分估计值：result_delta_p, result_delta_q, result_delta_v
• t和t+1时刻加速度计和陀螺仪零偏估计值：result_linearized_ba, result_linearized_ba, result_linearized_ba, result_linearized_bg
• Jacobian矩阵更新选项：update_jacobian

和论文中推导采用欧拉积分法不同，代码中实际采用的是中值积分法，且认为在整个$[k,k+1]$过程中，加速度计和陀螺仪零偏保持不变。函数主要分为两部分：

• 预积分估计值的一步递推
  由于代码内容和上述理论推倒完全一致，这里不做过多说明。

   Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba); 
   Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg; /// average angular rate
   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); /// average acceleration
   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;
   result_linearized_bg = linearized_bg;      

• Jacobian矩阵更新
  该部分代码内容与前文的理论推倒基本一致，但噪声传播矩阵V的前四行与理论推导正负相反，具体原因目前尚不明确，待分析完后续代码补充。

   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;
   // symmetric matrix
   /// angular velocity symmetric
   R_w_x<<0, -w_x(2), w_x(1),
          w_x(2), 0, -w_x(0),
          -w_x(1), w_x(0), 0;
   /// a_0_x symmetric
   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;
    /// a_1_x symmetric
   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_00
   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_01
   F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt; /// f_02
   F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt; /// f_03
   F.block<3, 3>(0, 12) = 0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * _dt; /// f_04 

   F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt; /// f_11
   F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt; /// f_14
            
   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_21
   F.block<3, 3>(6, 6) = Matrix3d::Identity(); /// f_22
   F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt; /// f_23
   F.block<3, 3>(6, 12) = 0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt; /// f_24
            
   F.block<3, 3>(9, 9) = Matrix3d::Identity(); /// f_33
            
   F.block<3, 3>(12, 12) = Matrix3d::Identity(); /// f_44
   
   MatrixXd V = MatrixXd::Zero(15,18);
   V.block<3, 3>(0, 0) =  0.25 * delta_q.toRotationMatrix() * _dt * _dt; /// v_00 (opposite to equation)
   V.block<3, 3>(0, 3) =  0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * _dt * 0.5 * _dt; /// v_01 (opposite to equation)
   V.block<3, 3>(0, 6) =  0.25 * result_delta_q.toRotationMatrix() * _dt * _dt; /// v_02 (opposite to equation)
   V.block<3, 3>(0, 9) =  V.block<3, 3>(0, 3); /// v_03 

   V.block<3, 3>(3, 3) =  0.5 * MatrixXd::Identity(3,3) * _dt; /// v_11 (opposite to equation)
   V.block<3, 3>(3, 9) =  0.5 * MatrixXd::Identity(3,3) * _dt; /// v_13 (opposite to equation)

   V.block<3, 3>(6, 0) =  0.5 * delta_q.toRotationMatrix() * _dt; /// v_20 (opposite to equation)
   V.block<3, 3>(6, 3) =  0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * 0.5 * _dt; /// v_21 (opposite to equation)
   V.block<3, 3>(6, 6) =  0.5 * result_delta_q.toRotationMatrix() * _dt; /// v_22 (opposite to equation)
   V.block<3, 3>(6, 9) =  V.block<3, 3>(6, 3); /// v_23 

   V.block<3, 3>(9, 12) = MatrixXd::Identity(3,3) * _dt; /// v_34

   V.block<3, 3>(12, 15) = MatrixXd::Identity(3,3) * _dt; /// v_45

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

状态传播函数（propagate）

该函数的内核为上述中值积分函数，主要用于实现IMU预积分的递推，函数原型如下：

void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)

函数的入口参数分别为IMU预积分的时间间隔_dt和当前IMU量测_acc_1、_gyr_1。propagate函数首先调用midPointIntegration计算IMU预积分的一步递推，随后对IMU预积分初值进行重置（注意每次递推需要对四元数进行归一化），用于下一步递推，详细代码如下：

 // 获取输入参数
 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;
// 调用中值积分进行IMU预积分一步递推
 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;  

状态重传播函数（reprogate）

该函数在后端完成优化，给出加速度计和陀螺仪零偏新的最优估计值后，基于新的估计值对已有的IMU预积分进行重递推，函数入口参数为优化后的零偏值_linearized_ba和_linearized_bg。

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();
    // 调用propagate函数进行重递推
    for (int i = 0; i < static_cast<int>(dt_buf.size()); i++)
        propagate(dt_buf[i], acc_buf[i], gyr_buf[i]);
 }

变量存储函数（push_back）

该函数向IntegrationBase中的数据寄存器存入每一个IMU量测的时间间隔dt、加速度计量测值acc和陀螺仪量测值gyr，并基于量测值对IMU预积分项进行一步递推，函数原型如下：

   void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)
    {
        dt_buf.push_back(dt);
        acc_buf.push_back(acc);
        gyr_buf.push_back(gyr);
        propagate(dt, acc, gyr);
    }

残差评估函数（evaluate）

该函数用于在后端非线性优化中每一次优化后评估IMU预积分的残差，函数入口参数分别为：

• 优化后的第k帧预积分状态变Pi、Qi、Vi、Bai、Bgi
• 优化后的第k+1帧预积分状态量Pj、Qj、Vj、Baj、Bgj

函数返回值为k和k+1之间IMU预积分状态与非线性优化状态之间的残差，用于构建后端的非线性优化误差函数（理想状态下残差应该为零，实际情况下残差应该逐渐收敛于一个小值附近）。

Eigen::Matrix<double, 15, 1> evaluate(const Eigen::Vector3d &Pi, const Eigen::Quaterniond &Qi, const Eigen::Vector3d &Vi, const Eigen::Vector3d &Bai, const Eigen::Vector3d &Bgi,
                                      const Eigen::Vector3d &Pj, const Eigen::Quaterniond &Qj, const Eigen::Vector3d &Vj, const Eigen::Vector3d &Baj, const Eigen::Vector3d &Bgj)
{
    Eigen::Matrix<double, 15, 1> residuals;

    Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);
    Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);

    Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG);

    Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA);
    Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG);

    Eigen::Vector3d dba = Bai - linearized_ba;
    Eigen::Vector3d dbg = Bgi - linearized_bg;

    Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
    Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
    Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;
	// 计算优化残差
    residuals.block<3, 1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p;
    residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();
    residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;
    residuals.block<3, 1>(O_BA, 0) = Baj - Bai;
    residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi;
    return residuals;
}