IMU预积分

IMU预积分

0 预备知识

• SO(3)的指数映射：
  $$\exp ({\varphi }^{\wedge })=\mathbf{I}+\frac{\sin (\Vert \varphi \Vert )}{\Vert \varphi \Vert }{\varphi }^{\wedge }+\frac{1-\cos (\Vert \varphi \Vert )}{\Vert \varphi {\Vert }^2}({\varphi }^{\wedge }{)}^2$$

  $$\exp ({\varphi }^{\wedge })\approx \mathbf{I}+{\varphi }^{\wedge }$$

• SO(3)的对数映射：
  $$\begin{gathered} \log (\mathbf{R}{)}^{\vee }=\mathbf{u}\theta \\ \log (\mathbf{R})=\frac{\theta \cdot (\mathbf{R}-{\mathbf{R}}^T)}{2\sin (\theta )},\theta =\arccos (\frac{tr(\mathbf{R}-1)}{2}) \end{gathered}$$

• 李代数的一阶近似：
  $$\begin{gathered} \text{Exp}(\varphi +\delta \varphi )\approx \text{Exp}(\varphi )\text{Exp}({\mathbf{J}}_r(\varphi )\delta \varphi ) \\ \text{Log}(\text{Exp}(\varphi )\text{Exp}(\delta \varphi ))\approx \varphi +{\mathbf{J}}_r^{-1}(\varphi )\delta \varphi \end{gathered}$$
  其中，
  $$\begin{gathered} {\mathbf{J}}_r(\varphi )=\mathbf{I}-\frac{1-\cos (\Vert \varphi \Vert )}{\Vert \varphi {\Vert }^2}{\varphi }^{\wedge }+\frac{\Vert \varphi \Vert -\sin (\Vert \varphi \Vert )}{\Vert \varphi {\Vert }^3}({\varphi }^{\wedge }{)}^2 \\ {\mathbf{J}}_r^{-1}(\varphi )=\mathbf{I}+\frac{1}{2}{\varphi }^{\wedge }+(\frac{1}{\Vert \varphi {\Vert }^2}+\frac{1+\cos (\Vert \varphi \Vert )}{2\Vert \varphi \Vert \sin (\Vert \varphi \Vert )})({\varphi }^{\wedge }{)}^2 \end{gathered}$$

• 伴随性质：
  $$\begin{gathered} \mathbf{R}\text{Exp}(\varphi ){\mathbf{R}}^T=\exp (\mathbf{R}{\varphi }^{\wedge }{\mathbf{R}}^T)=\text{Exp}(\mathbf{R}\varphi ) \\ \text{Exp}(\varphi )\mathbf{R}=\mathbf{R}\text{Exp}({\mathbf{R}}^T\varphi ) \end{gathered}$$

1 Visual-Inertial State Estimation

1. 系统状态

   系统在时间$i$的状态为${\mathbf{x}}_i=[{\mathbf{R}}_i,{\mathbf{p}}_i,{\mathbf{v}}_i,{\mathbf{b}}_i]$，其中$({\mathbf{R}}_i,{\mathbf{p}}_i)\in SE(3)$，${\mathbf{b}}_i=[{\mathbf{b}}_i^g，{\mathbf{b}}_i^a]\in {\mathbb{R}}^6$。

   令${\mathcal{K}}_k$为到k时刻位置的关键帧集合，则我们估计所有关键帧的状态：
   X k = { x i } i ∈ K k \mathcal X_k=\{\mathbf x_i\}_{i\in \mathcal K_k} Xk​={xi​}i∈Kk​​

2. 观测量
   观测量包括视觉观测和IMU观测：
   Z k = { C i , I i j } ( i , j ) ∈ K k \mathcal Z_k=\{C_i,I_{ij} \}_{(i,j)\in\mathcal K_k} Zk​={Ci​,Iij​}(i,j)∈Kk​​

3. 因子图和MAP估计
   

   $$\begin{gathered} p({\mathcal{X}}_k|{\mathcal{Z}}_k)\propto p({\mathcal{X}}_0)p({\mathcal{Z}}_k|{\mathcal{X}}_k)=p({\mathcal{X}}_0)\prod _{(i,j)\in {\mathcal{K}}_k}p({\mathcal{C}}_i,{\mathcal{I}}_{ij}|{\mathcal{X}}_k) \\ =p({\mathcal{X}}_0)\prod _{(i,j)\in {\mathcal{K}}_k}p({\mathcal{I}}_{ij}|{\mathbf{x}}_i,{\mathbf{x}}_j)\prod _{i\in {\mathcal{K}}_k}\prod _{l\in {\mathcal{C}}_i}p({\mathbf{z}}_{il}|{\mathbf{x}}_i) \end{gathered}$$

2 IMU测量模型

1. 测量模型

其中，${}_B{\tilde{\omega }}_{WB}$为测量值，${}_B{\omega }_{WB}$为真实值。

2. 动力学
   $${\dot{\mathbf{R}}}_{WB}={\mathbf{R}}_{WB}{}_B{\omega }_{WB}^{\wedge },{}_W\dot{\mathbf{v}}={}_W\mathbf{a},{}_W\dot{\mathbf{p}}={}_W\mathbf{v}$$
   假设在$[t,t+\Delta t]$时间内，${}_W\mathbf{a}$和${}_B{\omega }_{WB}$保持不变，则有：

将测量值代入，可以得到：

其中，$\text{Cov}({\eta }^{gd}(t))=\frac{1}{\Delta t}\text{Cov}({\eta }^g(t))$。

3 IMU预积分

根据动力学模型，我们可以得到两个连续关键帧的IMU积分：

为避免在时间$t_i$线性化时需要重新积分，构造相对增量：

其中，$\Delta {\mathbf{R}}_{ik}={\mathbf{R}}_i^T{\mathbf{R}}_k,\Delta {\mathbf{v}}_{ik}={\mathbf{R}}_i^T({\mathbf{v}}_k-{\mathbf{v}}_i-\mathbf{g}\Delta t_{ik})$。然而，上述这种形式仍然是关于bias的函数，为此，提出了IMU预积分。

3.1 预积分测量模型

首先，假设两帧之间的bias不变：
$${\mathbf{b}}_i^g={\mathbf{b}}_{i+1}^g=\cdots ={\mathbf{b}}_{j-1}^g,{\mathbf{b}}_i^a={\mathbf{b}}_{i+1}^a=\cdots ={\mathbf{b}}_{j-1}^a$$
对$\Delta {\mathbf{R}}_{ij}$做一阶近似，利用开始提到的BCH近似和伴随性质，可得：

其中，${\mathbf{J}}_r^k={\mathbf{J}}_r(({\tilde{w}}_k-{\mathbf{b}}_i^g)\Delta t)$。

对$\Delta {\mathbf{v}}_{ij}$做一阶近似，可得：

对$\Delta {\mathbf{p}}_{ij}$做一阶近似，可得：

由此，将相对增量写成了“真实值+噪声”的测量模型：

$$\left[\begin{array}{c}\Delta {\tilde{\mathbf{R}}}_{ij}\\ \Delta {\tilde{\mathbf{v}}}_{ij}\\ \Delta {\tilde{\mathbf{p}}}_{ij}\\ 0\\ 0\end{array}\right]=\left[\begin{array}{c}{\mathbf{R}}_i^T{\mathbf{R}}_j\text{Exp}(\delta {\varphi }_{ij})\\ {\mathbf{R}}_i^T({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\Delta t_{ij})+\delta {\mathbf{v}}_{ij}\\ {\mathbf{R}}_i^T({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t_{ij}^2)+\delta {\mathbf{p}}_{ij}\\ {\mathbf{b}}_j^a-{\mathbf{b}}_i^a+\delta {\mathbf{b}}_{ij}^a\\ {\mathbf{b}}_j^g-{\mathbf{b}}_i^g+\delta {\mathbf{b}}_{ij}^g\end{array}\right]$$
其中，预积分测量值为：
$$\begin{array}{rl}\Delta {\tilde{\mathbf{R}}}_{ij}& =\prod _{k=i}^{j-1}\text{Exp}(({\tilde{\omega }}_k-{\mathbf{b}}_i^g)\Delta t)\\ \Delta {\tilde{\mathbf{v}}}_{ij}& =\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a)\Delta t\\ \Delta {\tilde{\mathbf{p}}}_{ij}& =\sum _{k=i}^{j-1}(\Delta {\tilde{\mathbf{v}}}_{ik}\Delta t+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a)\Delta t^2)\end{array}$$
其前向传播的形式为：
$$\begin{array}{rl}\Delta {\tilde{\mathbf{R}}}_{ik+1}& =\Delta {\tilde{\mathbf{R}}}_{ik}\text{Exp}(({\tilde{\omega }}_k-{\mathbf{b}}_i^g)\Delta t)\\ \Delta {\tilde{\mathbf{v}}}_{ik+1}& =\Delta {\tilde{\mathbf{v}}}_{ik}+\Delta {\tilde{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a)\Delta t\\ \Delta {\tilde{\mathbf{p}}}_{ik+1}& =\Delta {\tilde{\mathbf{p}}}_{ik}+\Delta {\tilde{\mathbf{v}}}_{ik}\Delta t+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a)\Delta t^2\end{array}$$

3.2 噪声传递

预积分噪声设定为零均值高斯分布：
$${\eta }_{ij}^{\Delta }=[\delta {\varphi }_{ij},\delta {\mathbf{v}}_{ij},\delta {\mathbf{p}}_{ij}]\sim \mathcal{N}(0_{9\times 1},{\Sigma }_{ij})$$
误差传递为：
$$\begin{array}{rl}{\eta }_{ik+1}^{\Delta }& =\mathbf{A}{\eta }_{ik}^{\Delta }+\mathbf{B}{\eta }_k^d\\ \left[\begin{array}{c}\delta {\varphi }_{ik+1}\\ \delta {\mathbf{v}}_{ik+1}\\ \delta {\mathbf{p}}_{ik+1}\end{array}\right]& =\left[\begin{array}{ccc}\Delta {\tilde{\mathbf{R}}}_{kk+1}^T& 0_{3\times 3}& 0_{3\times 3}\\ -\Delta {\tilde{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a{)}^{\wedge }\Delta t& {\mathbf{I}}_{3\times 3}& 0_{3\times 3}\\ -\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a{)}^{\wedge }\Delta t^2& {\mathbf{I}}_{3\times 3}\Delta t& {\mathbf{I}}_{3\times 3}\end{array}\right]\left[\begin{array}{c}\delta {\varphi }_{ik}\\ \delta {\mathbf{v}}_{ik}\\ \delta {\mathbf{p}}_{ik}\end{array}\right]+\left[\begin{array}{cc}{\mathbf{J}}_r^k\Delta t& 0_{3\times 3}\\ 0_{3\times 3}& \Delta {\tilde{\mathbf{R}}}_{ik}\Delta t\\ 0_{3\times 3}& \frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}\Delta t^2\end{array}\right]\left[\begin{array}{c}{\eta }_k^{gd}\\ {\eta }_k^{ad}\end{array}\right]\end{array}$$
其中，${\eta }_k^d=[{\eta }_k^{gd},{\eta }_k^{ad}]$。

协方差传递为：
$${\Sigma }_{ik+1}=\mathbf{A}{\Sigma }_{ik}{\mathbf{A}}^T+\mathbf{B}{\Sigma }_{\eta }{\mathbf{B}}^T$$
其中，${\Sigma }_{ii}=0_{9\times 9}$，${\Sigma }_{\eta }\in {\mathbb{R}}^{6\times 6}$为IMU噪声协方差。

3.3 bias更新

之前的推导假设了bias不变，然而实际情况是bias会随时间发生微小变化，因此可以定义bias更新$\mathbf{b}\leftarrow \bar{\mathbf{b}}+\delta \mathbf{b}$，并对预积分测量值进行更新：

其中，雅可比项的递推形式为：

$$\begin{array}{rl}\frac{\partial \Delta {\bar{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}^g}& =-\sum _{k=i}^{j-1}[\Delta {\tilde{\mathbf{R}}}_{k+1j}(\overline{{\mathbf{b}}_i}{)}^T{\mathbf{J}}_r^k\Delta t]\\ \frac{\partial \Delta {\bar{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}^a}& =-\sum _{k=i}^{j-1}\Delta {\bar{\mathbf{R}}}_{ik}\Delta t\\ \frac{\partial \Delta {\bar{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}^g}& =-\sum _{k=i}^{j-1}\Delta {\bar{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-{\bar{\mathbf{b}}}_i^a{)}_{\times }\frac{\partial \Delta {\bar{\mathbf{R}}}_{ik}}{\partial {\mathbf{b}}^g}\Delta t\\ \frac{\partial \Delta {\bar{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}^a}& =\sum _{k=i}^{j-1}(\frac{\partial \Delta {\bar{\mathbf{v}}}_{ik}}{\partial {\mathbf{b}}^a}\Delta t-\frac{1}{2}\Delta {\bar{\mathbf{R}}}_{ik}\Delta t^2)\\ \frac{\partial \Delta {\bar{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}^g}& =\sum _{k=i}^{j-1}(\frac{\partial \Delta {\bar{\mathbf{v}}}_{ik}}{\partial {\mathbf{b}}^g}\Delta t-\frac{1}{2}\Delta {\bar{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-{\bar{\mathbf{b}}}_i^a{)}_{\times }\frac{\partial \Delta {\bar{\mathbf{R}}}_{ik}}{\partial {\mathbf{b}}^g}\Delta t^2)\end{array}$$

3.4 IMU因子

预积分误差${\mathbf{r}}_{{\mathcal{I}}_{ij}}=[{\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}},{\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}},{\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}]\in {\mathbb{R}}^9$，定义如下：

通常还包括bias项：

优化变量：

雅可比项：

1）${\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}$的雅可比：

2）${\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}$的雅可比：

3）${\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}$的雅可比：

4 代码实现（VINS-Mono）

IntegrationBase类

IntegrationBase类是VINS-Mono中用于处理IMU预积分的类，包含的成员变量如下：

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

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

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

double sum_dt; //IMU预积分区间[i,j]的总时长
Eigen::Vector3d delta_p; // 位置预积分P_ij
Eigen::Quaterniond delta_q; // 旋转四元数预积分R_ij
Eigen::Vector3d delta_v; // 速度预积分V_ij

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

预积分测量模型

预积分测量值通过中值积分计算，其前向传播函数为propagate，该函数实现了$ij$的预积分递推。函数以当前IMU测量值和时间间隔为输入，首先调用midPointIntegration计算预积分的一步递推，然后对预积分初值进行重置，用于下一步递推。
在VINS-Mono中，预积分测量值可以用中值积分进行前向传播计算（如VINS-Mono中），预积分测量值的前向传播可以写为：
$$\begin{array}{rl}\Delta {\tilde{\mathbf{R}}}_{ij}& =\prod _{k=i}^{j-1}\text{Exp}({\hat{\omega }}_k\Delta t)\\ \Delta {\tilde{\mathbf{v}}}_{ij}& =\sum _{k=i}^{j-1}{\hat{\mathbf{a}}}_k\Delta t\\ \Delta {\tilde{\mathbf{p}}}_{ij}& =\sum _{k=i}^{j-1}(\Delta {\tilde{\mathbf{v}}}_{ik}\Delta t+\frac{1}{2}{\hat{\mathbf{a}}}_k\Delta t^2)\end{array}$$
其中，加速度和角速度用中值积分表示：
$$\begin{gathered} {\hat{\omega }}_k=\frac{1}{2}({\tilde{\omega }}_k+{\tilde{\omega }}_{k+1})-{\mathbf{b}}_i^g \\ {\hat{\mathbf{a}}}_k=\frac{1}{2}[\Delta {\tilde{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a)+\Delta {\tilde{\mathbf{R}}}_{ik+1}({\tilde{\mathbf{a}}}_{k+1}-{\mathbf{b}}_i^a)] \end{gathered}$$

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;  

}

中值积分

midPointIntegration函数用于计算预积分的一次递推，即$ik\to ik+1$，并更新雅可比和协方差。
在VINS-Mono中，预积分噪声设定为零均值高斯分布：
$${\eta }_{ij}^{\Delta }=[\delta {\varphi }_{ij},\delta {\mathbf{v}}_{ij},\delta {\mathbf{p}}_{ij},\delta {\mathbf{b}}_{ij}^a,\delta {\mathbf{b}}_{ij}^g]\sim \mathcal{N}(0_{15\times 1},{\Sigma }_{ij})$$
误差传递为：
$${\eta }_{ik+1}^{\Delta }={\mathbf{F}}_k{\eta }_{ik}^{\Delta }+{\mathbf{V}}_k{\eta }_k^d$$
使用中值积分得到离散化形式如下：
$$\begin{array}{rl}\left[\begin{array}{c}\delta {\mathbf{p}}_{ik+1}\\ \delta {\varphi }_{ik+1}\\ \delta {\mathbf{v}}_{ik+1}\\ \delta {\mathbf{b}}_{ik+1}^a\\ \delta {\mathbf{b}}_{ik+1}^g\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{p}}_{ik}\\ \delta {\varphi }_{ik}\\ \delta {\mathbf{v}}_{ik}\\ \delta {\mathbf{b}}_{ik}^a\\ \delta {\mathbf{b}}_{ik}^g\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{\Delta {\tilde{\mathbf{R}}}_{ik}\Delta t}{2}& v_{21}& \frac{\Delta {\tilde{\mathbf{R}}}_{ik+1}\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}}_k^a\\ {\mathbf{n}}_k^g\\ {\mathbf{n}}_{k+1}^a\\ {\mathbf{n}}_{k+1}^g\\ {\mathbf{n}}_k^{ad}\\ {\mathbf{n}}_k^{gd}\end{array}\right]\end{array}$$
其中，矩阵${\mathbf{F}}_k$中的元素如下：
$$\begin{array}{rl}{f}_{01}& =-\frac{1}{4}\Delta {\tilde{\mathbf{R}}}_{ik}\lfloor {\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a{\rfloor }_{\times }\Delta t^2-\frac{1}{4}\Delta {\tilde{\mathbf{R}}}_{ik+1}\lfloor {\tilde{\mathbf{a}}}_{k+1}-{\mathbf{b}}_i^a{\rfloor }_{\times }(\mathbf{I}-\lfloor {\hat{\omega }}_k{\rfloor }_{\times }\Delta t)\Delta t^2\\ f_{03}& =-\frac{1}{4}(\Delta {\tilde{\mathbf{R}}}_{ik}+\Delta {\tilde{\mathbf{R}}}_{ik+1})\Delta t^2\\ f_{04}& =\frac{1}{4}\Delta {\tilde{\mathbf{R}}}_{ik+1}\lfloor {\tilde{\mathbf{a}}}_{k+1}-{\mathbf{b}}_i^a{\rfloor }_{\times }\Delta t^3\\ f_{11}& =\mathbf{I}-\lfloor {\hat{\omega }}_k{\rfloor }_{\times }\Delta t\\ f_{21}& =\left[\begin{array}{c}-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}\lfloor {\tilde{\mathbf{a}}}_k-{\mathbf{b}}_i^a{\rfloor }_{\times }-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik+1}\lfloor {\tilde{\mathbf{a}}}_{k+1}-{\mathbf{b}}_i^a{\rfloor }_{\times }(\mathbf{I}-\lfloor {\hat{\omega }}_k{\rfloor }_{\times }\Delta t)\end{array}\right]\Delta t\\ f_{23}& =-\frac{1}{2}(\Delta {\tilde{\mathbf{R}}}_{ik}+\Delta {\tilde{\mathbf{R}}}_{ik+1})\Delta t\\ f_{24}& =\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik+1}\lfloor {\tilde{\mathbf{a}}}_{k+1}-{\mathbf{b}}_i^a{\rfloor }_{\times }\Delta t^2\end{array}$$
矩阵${\mathbf{V}}_{j-1}$中的元素如下：
$$\begin{array}{rl}{v}_{00}& =\frac{1}{4}\Delta {\tilde{\mathbf{R}}}_{ik}\Delta t^2\\ v_{01}& =-\frac{1}{8}\Delta {\tilde{\mathbf{R}}}_{ik+1}\lfloor {\tilde{\mathbf{a}}}_{k+1}-{\mathbf{b}}_i^a{\rfloor }_{\times }\Delta t^3\\ v_{02}& =\frac{1}{4}\Delta {\tilde{\mathbf{R}}}_{ik+1}\Delta t^2\\ v_{03}& =v_{01}\\ v_{21}& =-\frac{1}{4}\Delta {\tilde{\mathbf{R}}}_{ik+1}\lfloor {\tilde{\mathbf{a}}}_{k+1}-{\mathbf{b}}_i^a{\rfloor }_{\times }\Delta t^2\\ v_{23}& =v_{21}\end{array}$$
根据误差传递，可以进一步得到雅可比传递为：
$${\mathbf{J}}_{ij}={\mathbf{F}}_{j-1}{\mathbf{J}}_{ij-1}$$
其中，${\mathbf{J}}_{ii}={\mathbf{I}}_{15\times 15}$。协方差传递为：
$${\Sigma }_{ij}={\mathbf{F}}_{j-1}{\Sigma }_{ij-1}{\mathbf{F}}_{j-1}^T+{\mathbf{V}}_{j-1}{\Sigma }_n{\mathbf{V}}_{j-1}^T$$
其中，${\Sigma }_{ii}=0_{15\times 15},{\Sigma }_n=diag({\eta }_a^2,{\eta }_g^2,{\eta }_a^2,{\eta }_g^2,{\eta }_{b_a}^2,{\eta }_{b_g}^2{)}_{18\times 18}$为IMU测量噪声的协方差。

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+1时刻的PVQ预积分项，并认为bias不变
    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;
    result_linearized_bg = linearized_bg;         
	
    // 更新雅可比和协方差
    if(update_jacobian)
    {
        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;
		
        // 计算F矩阵
        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_01
        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_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.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.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.block<3, 3>(12, 12) = Matrix3d::Identity(); 
		
        // 计算V矩阵
        MatrixXd V = MatrixXd::Zero(15,18);
        V.block<3, 3>(0, 0) =  0.25 * delta_q.toRotationMatrix() * _dt * _dt; // v_00
        V.block<3, 3>(0, 3) =  0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * _dt * 0.5 * _dt; // v_01
        V.block<3, 3>(0, 6) =  0.25 * result_delta_q.toRotationMatrix() * _dt * _dt; // v_02
        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.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_21
        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_23
        V.block<3, 3>(9, 12) = MatrixXd::Identity(3,3) * _dt;
        V.block<3, 3>(12, 15) = MatrixXd::Identity(3,3) * _dt;
		
        // 更新
        jacobian = F * jacobian;
        covariance = F * covariance * F.transpose() + V * noise * V.transpose();
    }
}

bias更新

在优化过程中bias会更新，repropagate函数实现了基于新的bias对已有的IMU预积分进行重新递推，函数的输入参数为优化后的bias值。

$$\begin{array}{rl}\frac{\partial \Delta {\bar{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}^g}& ={\mathbf{J}}_{ij}^{(1,4)}=-\sum _{k=i}^{j-1}[\Delta {\tilde{\mathbf{R}}}_{k+1j}(\overline{{\mathbf{b}}_i}{)}^T{\mathbf{J}}_r^k\Delta t]\\ \frac{\partial \Delta {\bar{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}^a}& ={\mathbf{J}}_{ij}^{(2,3)}=-\sum _{k=i}^{j-1}\Delta {\bar{\mathbf{R}}}_{ik}\Delta t\\ \frac{\partial \Delta {\bar{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}^g}& ={\mathbf{J}}_{ij}^{(2,4)}=-\sum _{k=i}^{j-1}\Delta {\bar{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-{\bar{\mathbf{b}}}_i^a{)}_{\times }\frac{\partial \Delta {\bar{\mathbf{R}}}_{ik}}{\partial {\mathbf{b}}^g}\Delta t\\ \frac{\partial \Delta {\bar{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}^a}& ={\mathbf{J}}_{ij}^{(0,3)}=\sum _{k=i}^{j-1}(\frac{\partial \Delta {\bar{\mathbf{v}}}_{ik}}{\partial {\mathbf{b}}^a}\Delta t-\frac{1}{2}\Delta {\bar{\mathbf{R}}}_{ik}\Delta t^2)\\ \frac{\partial \Delta {\bar{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}^g}& ={\mathbf{J}}_{ij}^{(0,4)}=\sum _{k=i}^{j-1}(\frac{\partial \Delta {\bar{\mathbf{v}}}_{ik}}{\partial {\mathbf{b}}^g}\Delta t-\frac{1}{2}\Delta {\bar{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-{\bar{\mathbf{b}}}_i^a{)}_{\times }\frac{\partial \Delta {\bar{\mathbf{R}}}_{ik}}{\partial {\mathbf{b}}^g}\Delta t^2)\end{array}$$

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();
    // 对[i,j]两帧间的IMU预积分进行重新计算
    for (int i = 0; i < static_cast<int>(dt_buf.size()); i++)
        propagate(dt_buf[i], acc_buf[i], gyr_buf[i]);
}

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();
    // 对[i,j]两帧间的IMU预积分进行重新计算
    for (int i = 0; i < static_cast<int>(dt_buf.size()); i++)
        propagate(dt_buf[i], acc_buf[i], gyr_buf[i]);
}

IMU因子

evaluate函数计算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;
	// 预积分项关于bias的雅可比
    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);
	// bias更新量
    Eigen::Vector3d dba = Bai - linearized_ba;
    Eigen::Vector3d dbg = Bgi - linearized_bg;
	
    // 预积分关于bias的线性化修正（对应3.3节）
    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;
	
    // 计算优化残差（对应3.4节，注意使用修正后的预积分项）
    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;
}

IMUFactor::Evaluate函数计算优化变量的雅可比。

virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
    // 优化变量
    Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
    Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);

    Eigen::Vector3d Vi(parameters[1][0], parameters[1][1], parameters[1][2]);
    Eigen::Vector3d Bai(parameters[1][3], parameters[1][4], parameters[1][5]);
    Eigen::Vector3d Bgi(parameters[1][6], parameters[1][7], parameters[1][8]);

    Eigen::Vector3d Pj(parameters[2][0], parameters[2][1], parameters[2][2]);
    Eigen::Quaterniond Qj(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);

    Eigen::Vector3d Vj(parameters[3][0], parameters[3][1], parameters[3][2]);
    Eigen::Vector3d Baj(parameters[3][3], parameters[3][4], parameters[3][5]);
    Eigen::Vector3d Bgj(parameters[3][6], parameters[3][7], parameters[3][8]);

#if 0
    if ((Bai - pre_integration->linearized_ba).norm() > 0.10 ||
        (Bgi - pre_integration->linearized_bg).norm() > 0.01)
    {
        pre_integration->repropagate(Bai, Bgi);
    }
#endif
	// 计算预积分残差
    Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);
    residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,
                                        Pj, Qj, Vj, Baj, Bgj);

    Eigen::Matrix<double, 15, 15> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose();
    residual = sqrt_info * residual;

    // 计算雅可比
    if (jacobians)
    {
        double sum_dt = pre_integration->sum_dt;
        Eigen::Matrix3d dp_dba = pre_integration->jacobian.template block<3, 3>(O_P, O_BA);
        Eigen::Matrix3d dp_dbg = pre_integration->jacobian.template block<3, 3>(O_P, O_BG);

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

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

        if (pre_integration->jacobian.maxCoeff() > 1e8 || pre_integration->jacobian.minCoeff() < -1e8)
        {
            ROS_WARN("numerical unstable in preintegration");
        }
        if (jacobians[0])
        {
            Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);
            jacobian_pose_i.setZero();

            jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix();
            jacobian_pose_i.block<3, 3>(O_P, O_R) = Utility::skewSymmetric(Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt));

#if 0
        jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Qj.inverse() * Qi).toRotationMatrix();
#else
            Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
            jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)).bottomRightCorner<3, 3>();
#endif
            jacobian_pose_i.block<3, 3>(O_V, O_R) = Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi));

            jacobian_pose_i = sqrt_info * jacobian_pose_i;

            if (jacobian_pose_i.maxCoeff() > 1e8 || jacobian_pose_i.minCoeff() < -1e8)
            {
                ROS_WARN("numerical unstable in preintegration");
            }
        }
        if (jacobians[1])
        {
            Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_i(jacobians[1]);
            jacobian_speedbias_i.setZero();
            jacobian_speedbias_i.block<3, 3>(O_P, O_V - O_V) = -Qi.inverse().toRotationMatrix() * sum_dt;
            jacobian_speedbias_i.block<3, 3>(O_P, O_BA - O_V) = -dp_dba;
            jacobian_speedbias_i.block<3, 3>(O_P, O_BG - O_V) = -dp_dbg;
#if 0
        jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -dq_dbg;
#else
            jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() * Qi * pre_integration->delta_q).bottomRightCorner<3, 3>() * dq_dbg;
#endif
            jacobian_speedbias_i.block<3, 3>(O_V, O_V - O_V) = -Qi.inverse().toRotationMatrix();
            jacobian_speedbias_i.block<3, 3>(O_V, O_BA - O_V) = -dv_dba;
            jacobian_speedbias_i.block<3, 3>(O_V, O_BG - O_V) = -dv_dbg;

            jacobian_speedbias_i.block<3, 3>(O_BA, O_BA - O_V) = -Eigen::Matrix3d::Identity();

            jacobian_speedbias_i.block<3, 3>(O_BG, O_BG - O_V) = -Eigen::Matrix3d::Identity();

            jacobian_speedbias_i = sqrt_info * jacobian_speedbias_i;
        }
        if (jacobians[2])
        {
            Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[2]);
            jacobian_pose_j.setZero();

            jacobian_pose_j.block<3, 3>(O_P, O_P) = Qi.inverse().toRotationMatrix();

#if 0
        	jacobian_pose_j.block<3, 3>(O_R, O_R) = Eigen::Matrix3d::Identity();
#else
            Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
            jacobian_pose_j.block<3, 3>(O_R, O_R) = Utility::Qleft(corrected_delta_q.inverse() * Qi.inverse() * Qj).bottomRightCorner<3, 3>();
#endif
            jacobian_pose_j = sqrt_info * jacobian_pose_j;
        }
        if (jacobians[3])
        {
            Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_j(jacobians[3]);
            jacobian_speedbias_j.setZero();

            jacobian_speedbias_j.block<3, 3>(O_V, O_V - O_V) = Qi.inverse().toRotationMatrix();

            jacobian_speedbias_j.block<3, 3>(O_BA, O_BA - O_V) = Eigen::Matrix3d::Identity();

            jacobian_speedbias_j.block<3, 3>(O_BG, O_BG - O_V) = Eigen::Matrix3d::Identity();

            jacobian_speedbias_j = sqrt_info * jacobian_speedbias_j;
        }
    }

    return true;
}

5 代码实现（ORB_SLAM3）

Preintegrated类

Preintegrated类是ORB_SLAM3中用于处理IMU预积分的类，包含的成员变量如下：

float dT; //预积分总时长\Delta t_{ij}
Eigen::Matrix<float,15,15> C; // 噪声协方差 \Sigma_{ij}, \Sigma_{\eta}
Eigen::Matrix<float,15,15> Info;
Eigen::DiagonalMatrix<float,6> Nga, NgaWalk;

// Values for the original bias (when integration was computed)
Bias b;
Eigen::Matrix3f dR;
Eigen::Vector3f dV, dP;
Eigen::Matrix3f JRg, JVg, JVa, JPg, JPa;
Eigen::Vector3f avgA, avgW;

预积分传递

在ORB_SLAM3中，预积分传递使用欧拉积分计算，由IntegrateNewMeasurement函数实现。

void Preintegrated::IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt)
{
    mvMeasurements.push_back(integrable(acceleration,angVel,dt));

    // Position is updated firstly, as it depends on previously computed velocity and rotation.
    // Velocity is updated secondly, as it depends on previously computed rotation.
    // Rotation is the last to be updated.

    //Matrices to compute covariance
    Eigen::Matrix<float,9,9> A;
    A.setIdentity();
    Eigen::Matrix<float,9,6> B;
    B.setZero();

    Eigen::Vector3f acc, accW;
    acc << acceleration(0)-b.bax, acceleration(1)-b.bay, acceleration(2)-b.baz;
    accW << angVel(0)-b.bwx, angVel(1)-b.bwy, angVel(2)-b.bwz;

    avgA = (dT*avgA + dR*acc*dt)/(dT+dt);
    avgW = (dT*avgW + accW*dt)/(dT+dt);

    // Update delta position dP and velocity dV (rely on no-updated delta rotation)
    dP = dP + dV*dt + 0.5f*dR*acc*dt*dt;
    dV = dV + dR*acc*dt;

    // Compute velocity and position parts of matrices A and B (rely on non-updated delta rotation)
    Eigen::Matrix<float,3,3> Wacc = Sophus::SO3f::hat(acc);

    A.block<3,3>(3,0) = -dR*dt*Wacc;
    A.block<3,3>(6,0) = -0.5f*dR*dt*dt*Wacc;
    A.block<3,3>(6,3) = Eigen::DiagonalMatrix<float,3>(dt, dt, dt);
    B.block<3,3>(3,3) = dR*dt;
    B.block<3,3>(6,3) = 0.5f*dR*dt*dt;


    // Update position and velocity jacobians wrt bias correction
    JPa = JPa + JVa*dt -0.5f*dR*dt*dt;
    JPg = JPg + JVg*dt -0.5f*dR*dt*dt*Wacc*JRg;
    JVa = JVa - dR*dt;
    JVg = JVg - dR*dt*Wacc*JRg;

    // Update delta rotation
    IntegratedRotation dRi(angVel,b,dt);
    dR = NormalizeRotation(dR*dRi.deltaR);

    // Compute rotation parts of matrices A and B
    A.block<3,3>(0,0) = dRi.deltaR.transpose();
    B.block<3,3>(0,0) = dRi.rightJ*dt;

    // Update covariance
    C.block<9,9>(0,0) = A * C.block<9,9>(0,0) * A.transpose() + B*Nga*B.transpose();
    C.block<6,6>(9,9) += NgaWalk;

    // Update rotation jacobian wrt bias correction
    JRg = dRi.deltaR.transpose()*JRg - dRi.rightJ*dt;

    // Total integrated time
    dT += dt;
}

bias更新

GetDelta***和 GetUpdatedDelta***函数实现了bias更新。

Eigen::Vector3f Preintegrated::GetDeltaVelocity(const Bias &b_)
{
    std::unique_lock<std::mutex> lock(mMutex);
    Eigen::Vector3f dbg, dba;
    dbg << b_.bwx-b.bwx,b_.bwy-b.bwy,b_.bwz-b.bwz;
    dba << b_.bax-b.bax,b_.bay-b.bay,b_.baz-b.baz;
    return dV + JVg * dbg + JVa * dba;
}

Eigen::Vector3f Preintegrated::GetUpdatedDeltaVelocity()
{
    std::unique_lock<std::mutex> lock(mMutex);
    return dV + JVg * db.head(3) + JVa * db.tail(3);
}

IMU因子

ORB_SLAM3中的IMU因子及雅可比计算放在G2oType.cc中。其中，EdgeInertial::computeError函数实现了IMU残差的计算。
$${\mathbf{r}}_{{\mathcal{I}}_{ij}}=[{\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}},{\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}},{\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}]\in {\mathbb{R}}^9$$

void EdgeInertial::computeError()
{
    // TODO Maybe Reintegrate inertial measurments when difference between linearization point and current estimate is too big
    const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]);
    const VertexVelocity* VV1= static_cast<const VertexVelocity*>(_vertices[1]);
    const VertexGyroBias* VG1= static_cast<const VertexGyroBias*>(_vertices[2]);
    const VertexAccBias* VA1= static_cast<const VertexAccBias*>(_vertices[3]);
    const VertexPose* VP2 = static_cast<const VertexPose*>(_vertices[4]);
    const VertexVelocity* VV2 = static_cast<const VertexVelocity*>(_vertices[5]);
    const IMU::Bias b1(VA1->estimate()[0],VA1->estimate()[1],VA1->estimate()[2],VG1->estimate()[0],VG1->estimate()[1],VG1->estimate()[2]);
    const Eigen::Matrix3d dR = mpInt->GetDeltaRotation(b1).cast<double>();
    const Eigen::Vector3d dV = mpInt->GetDeltaVelocity(b1).cast<double>();
    const Eigen::Vector3d dP = mpInt->GetDeltaPosition(b1).cast<double>();

    const Eigen::Vector3d er = LogSO3(dR.transpose()*VP1->estimate().Rwb.transpose()*VP2->estimate().Rwb);
    const Eigen::Vector3d ev = VP1->estimate().Rwb.transpose()*(VV2->estimate() - VV1->estimate() - g*dt) - dV;
    const Eigen::Vector3d ep = VP1->estimate().Rwb.transpose()*(VP2->estimate().twb - VP1->estimate().twb
                                                               - VV1->estimate()*dt - g*dt*dt/2) - dP;

    _error << er, ev, ep;
}

EdgeInertial::linearizeOplus函数计算了残差关于优化变量的雅可比。
$$\begin{array}{rl}\mathbf{J}[0{]}_{9\times 6}& =[\frac{\partial {\mathbf{r}}_{{\mathcal{I}}_{ij}}}{\partial {\mathbf{R}}_i},\frac{\partial {\mathbf{r}}_{{\mathcal{I}}_{ij}}}{\partial {\mathbf{p}}_i}]=\left[\begin{array}{cc}\frac{\partial {\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}}{\partial {\mathbf{R}}_i}& \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}}{\partial {\mathbf{p}}_i}\\ \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}}{\partial {\mathbf{R}}_i}& \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}}{\partial {\mathbf{p}}_i}\\ \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}}{\partial {\mathbf{R}}_i}& \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}}{\partial {\mathbf{p}}_i}\end{array}\right]\\ & =\left[\begin{array}{cc}-{\mathbf{J}}_r^{-1}({\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}){\mathbf{R}}_j^T{\mathbf{R}}_i& 0\\ [{\mathbf{R}}_i^T({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\Delta t_{ij}){]}^{\wedge }& 0\\ [{\mathbf{R}}_i^T({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t_{ij}^2){]}^{\wedge }& -\mathbf{I}\end{array}\right]\end{array}$$

$$\begin{array}{rl}\mathbf{J}[1{]}_{9\times 3}& =\frac{\partial {\mathbf{r}}_{{\mathcal{I}}_{ij}}}{\partial {\mathbf{v}}_i}=\left[\begin{array}{c}\frac{\partial {\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}}{\partial {\mathbf{v}}_i}\\ \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}}{\partial {\mathbf{v}}_i}\\ \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}}{\partial {\mathbf{v}}_i}\end{array}\right]=\left[\begin{array}{c}0\\ -{\mathbf{R}}_i^T\\ -{\mathbf{R}}_i^T\Delta t_{ij}\end{array}\right]\\ \mathbf{J}[2{]}_{9\times 3}& =\frac{\partial {\mathbf{r}}_{{\mathcal{I}}_{ij}}}{\partial {\mathbf{b}}_i^g}=\left[\begin{array}{c}\frac{\partial {\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}}{\partial {\mathbf{b}}_i^g}\\ \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}}{\partial {\mathbf{b}}_i^g}\\ \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}}{\partial {\mathbf{b}}_i^g}\end{array}\right]=\left[\begin{array}{c}-{\mathbf{J}}_r^{-1}({\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}})\text{Exp}({\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}{)}^T{\mathbf{J}}_r(\frac{\partial \Delta {\bar{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}^g}\delta {\mathbf{b}}^g)\frac{\partial \Delta {\bar{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}^g}\\ -\frac{\partial \Delta {\bar{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}^g}\\ -\frac{\partial \Delta {\bar{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}^g}\end{array}\right]\\ \mathbf{J}[3{]}_{9\times 3}& =\frac{\partial {\mathbf{r}}_{{\mathcal{I}}_{ij}}}{\partial {\mathbf{b}}_i^a}=\left[\begin{array}{c}\frac{\partial {\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}}{\partial {\mathbf{b}}_i^a}\\ \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}}{\partial {\mathbf{b}}_i^a}\\ \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}}{\partial {\mathbf{b}}_i^a}\end{array}\right]=\left[\begin{array}{c}0\\ -\frac{\partial \Delta {\bar{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}^a}\\ -\frac{\partial \Delta {\bar{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}^a}\end{array}\right]\end{array}$$

$$\begin{array}{rl}\mathbf{J}[4{]}_{9\times 6}& =[\frac{\partial {\mathbf{r}}_{{\mathcal{I}}_{ij}}}{\partial {\mathbf{R}}_j},\frac{\partial {\mathbf{r}}_{{\mathcal{I}}_{ij}}}{\partial {\mathbf{p}}_j}]=\left[\begin{array}{cc}\frac{\partial {\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}}{\partial {\mathbf{R}}_j}& \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}}{\partial {\mathbf{p}}_j}\\ \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}}{\partial {\mathbf{R}}_j}& \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}}{\partial {\mathbf{p}}_j}\\ \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}}{\partial {\mathbf{R}}_j}& \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}}{\partial {\mathbf{p}}_j}\end{array}\right]\\ & =\left[\begin{array}{cc}{\mathbf{J}}_r^{-1}({\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}})& 0\\ 0& 0\\ 0& {\mathbf{R}}_i^T{\mathbf{R}}_j\end{array}\right]\\ \mathbf{J}[5{]}_{9\times 3}& =\frac{\partial {\mathbf{r}}_{{\mathcal{I}}_{ij}}}{\partial {\mathbf{v}}_j}=\left[\begin{array}{c}\frac{\partial {\mathbf{r}}_{\Delta {\mathbf{R}}_{ij}}}{\partial {\mathbf{v}}_j}\\ \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{v}}_{ij}}}{\partial {\mathbf{v}}_j}\\ \frac{\partial {\mathbf{r}}_{\Delta {\mathbf{p}}_{ij}}}{\partial {\mathbf{v}}_j}\end{array}\right]=\left[\begin{array}{c}0\\ {\mathbf{R}}_i^T\\ 0\end{array}\right]\end{array}$$

void EdgeInertial::linearizeOplus()
{
    const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]);
    const VertexVelocity* VV1= static_cast<const VertexVelocity*>(_vertices[1]);
    const VertexGyroBias* VG1= static_cast<const VertexGyroBias*>(_vertices[2]);
    const VertexAccBias* VA1= static_cast<const VertexAccBias*>(_vertices[3]);
    const VertexPose* VP2 = static_cast<const VertexPose*>(_vertices[4]);
    const VertexVelocity* VV2= static_cast<const VertexVelocity*>(_vertices[5]);
    const IMU::Bias b1(VA1->estimate()[0],VA1->estimate()[1],VA1->estimate()[2],VG1->estimate()[0],VG1->estimate()[1],VG1->estimate()[2]);
    const IMU::Bias db = mpInt->GetDeltaBias(b1);
    Eigen::Vector3d dbg;
    dbg << db.bwx, db.bwy, db.bwz;

    const Eigen::Matrix3d Rwb1 = VP1->estimate().Rwb;
    const Eigen::Matrix3d Rbw1 = Rwb1.transpose();
    const Eigen::Matrix3d Rwb2 = VP2->estimate().Rwb;

    const Eigen::Matrix3d dR = mpInt->GetDeltaRotation(b1).cast<double>();
    const Eigen::Matrix3d eR = dR.transpose()*Rbw1*Rwb2;
    const Eigen::Vector3d er = LogSO3(eR);
    const Eigen::Matrix3d invJr = InverseRightJacobianSO3(er);

    // Jacobians wrt Pose 1
    _jacobianOplus[0].setZero();
     // rotation
    _jacobianOplus[0].block<3,3>(0,0) = -invJr*Rwb2.transpose()*Rwb1; // OK
    _jacobianOplus[0].block<3,3>(3,0) = Sophus::SO3d::hat(Rbw1*(VV2->estimate() - VV1->estimate() - g*dt)); // OK
    _jacobianOplus[0].block<3,3>(6,0) = Sophus::SO3d::hat(Rbw1*(VP2->estimate().twb - VP1->estimate().twb
                                                   - VV1->estimate()*dt - 0.5*g*dt*dt)); // OK
    // translation
    _jacobianOplus[0].block<3,3>(6,3) = -Eigen::Matrix3d::Identity(); // OK

    // Jacobians wrt Velocity 1
    _jacobianOplus[1].setZero();
    _jacobianOplus[1].block<3,3>(3,0) = -Rbw1; // OK
    _jacobianOplus[1].block<3,3>(6,0) = -Rbw1*dt; // OK

    // Jacobians wrt Gyro 1
    _jacobianOplus[2].setZero();
    _jacobianOplus[2].block<3,3>(0,0) = -invJr*eR.transpose()*RightJacobianSO3(JRg*dbg)*JRg; // OK
    _jacobianOplus[2].block<3,3>(3,0) = -JVg; // OK
    _jacobianOplus[2].block<3,3>(6,0) = -JPg; // OK

    // Jacobians wrt Accelerometer 1
    _jacobianOplus[3].setZero();
    _jacobianOplus[3].block<3,3>(3,0) = -JVa; // OK
    _jacobianOplus[3].block<3,3>(6,0) = -JPa; // OK

    // Jacobians wrt Pose 2
    _jacobianOplus[4].setZero();
    // rotation
    _jacobianOplus[4].block<3,3>(0,0) = invJr; // OK
    // translation
    _jacobianOplus[4].block<3,3>(6,3) = Rbw1*Rwb2; // OK

    // Jacobians wrt Velocity 2
    _jacobianOplus[5].setZero();
    _jacobianOplus[5].block<3,3>(3,0) = Rbw1; // OK
}

[参考文献]
[1] IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation
[2] VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator
[3] ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM