视觉惯性SLAM

IMU基本模型

IMU信号本身带有误差，为了更好的在优化问题中使用IMU信号，一般需要建立IMU误差模型(IMU对实际运动的观测和实际的运动的值之间的误差)。其中，最常用的是将其误差模型简化为偏移和测量噪声两个部分。
则，角速度和加速度的观测值一般被表示为：

\[\tilde{\boldsymbol{\omega}}(t)=\boldsymbol{\omega}(t)+\mathbf{b}_g(t)+\eta_g(t) \]

\[\tilde{a}(t)=a(t)+\mathbf{b}_a(t)+\eta_a(t)=R^{wT}(a^w-g^w)+\mathbf{b}_a(t)+\eta_a(t) \]

其中，带~的表示观测值，不带~的表示真实值。w表示world坐标系，不带w表示body坐标系。\(b(t)\)表示偏移，\(\eta(t)\)表示高斯白噪声。\(R^w\)表示world转到body坐标系下。因此\(a(t)=R^{wT}(a^w-g^w)\)表示不包含重力加速度的三轴加速度，\(a(t)\)整体表示的才是纯纯的去除重力的IMU的真值,\(a^w\)代表整体的真值，是包含了重力的真值。

• 偏移\(b(t)\)是随时间缓慢变化的陀螺仪和加速度计的bias，这个噪声从IMU开机以来就一直存在并且不断积累，和是否从IMU读取数据无关。在VINS-Mono中，被建模成随机游走，也就是\(b(t)\)的导数服从高斯分布。
• 白噪声\(\eta(t)\)也被建模为高斯分布。

这里有一个关于重力加速度的问题，为什么要减去重力加速度？不解是因为，比如IMU放在桌子上静止，加速度应该是0，不仅受重力，也受支持力啊可是。个人认为解释大概是这样的：加速度计内部是由弹簧实现的，根据弹簧形变计算IMU的加速度。弹簧形变产生的力已经包含了这部分重力加速度。因此需要减去重力加速度(这样弹簧就变为原来的形变了)计算不受重力加速度影响的三轴加速度。而这里也是加速度计很少用来计算速度和位置的原因，因为一旦姿态不准确的话，重力带来的误差对速度和位置的积分会带来严重的影响。但是在同个增量中，可以通过将增加重力的IMU测量值和估计值进行判断，获得重力的真实方向(在那个方向上变化最大)

优化

基于MSCKF的VISLAM

MSCKF是多状态约束卡尔曼滤波(mutil-state coinstraint Kalman filter)。MSCKF使用迭代式的IMU积分技术，通过对IMU读书的积分，得到作为状态的先验分布，再将图像特征信息作为观测，通过扩展卡尔曼滤波的方法求解状态的后验分布。

基于IMU预积分技术的VISLAM

图优化的核心是构造误差函数，通过最小化观测值和估计值之间的误差来建立优化。加入IMU之后，这一问题就变为图优化中的一条边，可以认为IMU的积分得到了新的观测值。而估计值可以是通过特征匹配等方式得到的。同时，在以BA优化的算法中一般是建立在关键帧上的约束，也就是PVQ相对于上一关键帧的增量是比较重要的。

\[error_{ij}=PVQ增量估计值_{ij}-PVQ增量观测值_{ij} \]

PVQ指位置、速度和姿态

但是上文说道，测量偏移\(b(t)\)是随着时间变化的，在图优化的过程中，要进行局部甚至全局的反复优化，随着优化的推进，IMU的测量偏差(bias)也会变化，残差中的PVQ增量观测值就需要重新计算。IMU预积分就提供了一个近似的测量值修正方法，免去了积分的重新计算，是预积分降低计算量的关键。

总的来说，就是求解相邻两帧之间PVQ增量的测量值，试图通过在既有IMU预积分测量值上添加一个近似修正量的方式来避免重新积分。相当于对增量(关键帧间的增量)的增量(预积分bias的建模)进行积分避免了重新积分。

预积分bias的建模

为了避免重新积分，IMU预积分的解决方案是：
设定 “新偏差=旧偏差+更新量”，获得bias的更新量，然后把PVQ增量测量值当做bias的函数，求该函数对bias的导数，则bias更新量与函数导数(斜率)的乘积就是bias发生变化后PVQ测量值的修正量(近似值)。
这样一来每当偏差发生变化就能够通过线性运算直接获得新的测量值，而不需要重新积分。

\[PVQ增量的测量值(新)=PVQ增量的测量值(旧)+\frac{\delta PVQ增量测量值}{\delta 测量偏差bias}\times偏差更新量 \]

这就是IMU预积分理论避免重新积分，降低运算量的关键。

推导

设k=i时，积分得到的PVQ为\(p_i\),\(v_i\),\(R_i\)，则直接用i到j之间的IMU真值更新的公式应该为：

\[\mathbf{R}_j=\mathbf{R}_i\cdot\prod\limits_{k=i}^{j-1}\mathrm{Exp}\Big(\Big(\tilde{\boldsymbol{\omega}}_k-\mathbf{b}_k^g-\boldsymbol{\eta}_k^{gd}\Big)\cdot\Delta t\Big) \]

\[\mathbf{v}_j=\mathbf{v}_i+\sum_{k=i}^ja^w\Delta t=\mathbf{v}_i+\mathbf{g}\cdot\Delta t_{ij}+\sum_{k=i}^{j-1}\mathbf{R}_k\cdot\left(\mathbf{\tilde{a}}_k-\mathbf{b}_k^a-\eta_k^{ad}\right)\cdot\Delta t \]

\[\mathbf{p}_j=\mathbf{p}_i+\sum_{k=i}^j\mathbf{v}_k\cdot\Delta t+\frac{1}{2}a^w\Delta t^2=\mathbf{p}_i+\sum_{k=i}^{j-1}\left[\mathbf{v}_k\cdot\Delta t+\frac{1}{2}\mathbf{g}\cdot\Delta t^2+\frac{1}{2}\mathbf{R}_k\cdot\left(\mathbf{\tilde{a}}_k-\mathbf{b}_k^a-\eta_k^{ad}\right)\cdot\Delta t^2\right] \]

因为目标是找到bias之间的关系，使用:

\[\begin{aligned} \Delta\mathbf{R}_{ij}& \triangleq\mathbf{R}_i^T\mathbf{R}_j=\prod_{k=i}^{j-1}\mathrm{Exp}\Big(\Big(\tilde{\boldsymbol{\omega}}_k-\mathbf{b}_k^g-\boldsymbol{\eta}_k^{gd}\Big)\cdot\Delta t\Big) \end{aligned}\tag{4} \]

\[\begin{aligned} \Delta\mathbf{v}_{ij}& \triangleq\mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g}\cdot\Delta t_{ij}\right)=\sum_{k=i}^{j-1}\Delta\mathbf{R}_{ik}\cdot\left(\tilde{\mathbf{a}}_k-\mathbf{b}_k^a-\eta_k^{ad}\right)\cdot\Delta t \end{aligned}\tag{5} \]

\[\begin{aligned} \Delta\mathbf{p}_{ij}& \triangleq\mathbf{R}_{i}^{T}\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i}\cdot\Delta t_{ij}-\frac{1}{2}\mathbf{g}\cdot\Delta t_{ij}^{2}\right) =\sum_{k=i}^{j-1}\left[\Delta\mathbf{v}_{ik}\cdot\Delta t+\frac{1}{2}\Delta\mathbf{R}_{ik}\cdot\left(\mathbf{\tilde{a}}_k-\mathbf{b}_k^a-\boldsymbol{\eta}_k^{ad}\right)\cdot\Delta t^2\right] \end{aligned}\tag{6} \]

上面是普通的IMU积分过程，上一节讲到我们的核心其实是找到增量和bias之间的关系，可以视为要找到:

\[\Delta R_{ij}\triangleq\Delta\tilde{\mathbf{R}}_{ij}(b^g_i)exp(-\delta \phi_{ij}) \]

\[\Delta v_{ij}\triangleq\Delta\tilde{\mathbf{v}}_{ij}(b^g_i,b^a_i)-\delta v_{ij} \]

\[\Delta p_{ij}\triangleq\Delta\tilde{\mathbf{p}}_{ij}(b^g_i,b^a_i)-\delta p_{ij} \]

因此，假设在预积分的区间内，两帧的偏差是相等的，即\(b_i^g\)=\(b_{i+1}^g\)=...=\(b_j^g\)以及\(b_i^a\)=\(b_{i+1}^a\)=...=\(b_j^a\):

• \(\Delta R_{ij}\):

  \[\begin{aligned} \Delta\mathbf{R}_{ij}& =\prod_{k=i}^{j-1}\mathrm{Exp}\Big(\left(\tilde{\boldsymbol{\omega}}_k-\mathbf{b}_i^g\right)\Delta t-\eta_k^{gd}\Delta t\Big) \\ &\overset{1}{\approx}\prod_{k=i}^{j-1}\left\{\mathrm{Exp}\big(\left(\tilde{\omega}_k-\mathbf{b}_i^g\right)\Delta t\big)\cdot\mathrm{Exp}\Big(-\mathbf{J}_r\left(\left(\tilde{\omega}_k-\mathbf{b}_i^g\right)\Delta t\right)\cdot\eta_k^{gd}\Delta t\Big)\right\}\\ &\overset{2}{=}\operatorname{Exp}\big(\left(\tilde{\boldsymbol{\omega}}_i-\mathbf{b}_i^g\right)\Delta t\big)\cdot\operatorname{Exp}\bigg(-\mathbf{J}_r^i\cdot\eta_i^{gd}\Delta t\bigg)\cdot\operatorname{Exp}\big(\left(\tilde{\boldsymbol{\omega}}_{i+1}-\mathbf{b}_i^g\right)\Delta t\bigg)\cdot\operatorname{Exp}\bigg(-\mathbf{J}_r^{i+1}\cdot \eta_{i+1}^{gd}\Delta t\bigg)...\\ & =\Delta\tilde{\mathbf{R}}_{i,i+1}\cdot\mathrm{Exp}\left(-\mathbf{J}_{r}^{i}\cdot\eta_{i}^{gd}\Delta t\right)\cdot\Delta\tilde{\mathbf{R}}_{i+1,i+2}\cdot\mathrm{Exp}\left(-\mathbf{J}_{r}^{i+1}\cdot\eta_{i+1}^{gd}\Delta t\right)\cdot\Delta\tilde{\mathbf{R}}_{i+2,i+3}\ldots \\ & \overset{3}{\operatorname*{=}}\Delta\tilde{\mathbf{R}}_{i,i+1}\cdot\Delta\tilde{\mathbf{R}}_{i+1,i+2}\cdot\mathrm{Exp}\left(-\Delta\tilde{\mathbf{R}}_{i+1,i+2}^T\cdot\mathbf{J}_r^i\cdot\eta_i^{gd}\Delta t\right)\cdot\mathrm{Exp}\left(-\mathbf{J}_r^{i+1}\cdot\eta_{i+1}^{gd}\Delta t\right)\cdot\Delta\tilde{\mathbf{R}}_{i+2,i+3}\ldots \\ & \overset{4}{=}\Delta\tilde{\mathbf{R}}_{i,j}\cdot\mathrm{Exp}\Big(-\Delta\tilde{\mathbf{R}}_{i+1,j}^T\cdot\mathbf{J}_r^i\cdot\eta_i^{gd}\Delta t\Big)\cdot\mathrm{Exp}\Big(-\Delta\tilde{\mathbf{R}}_{i+2,j}^T\cdot\mathbf{J}_r^{i+1}\cdot\eta_{i+1}^{gd}\Delta t\Big)\ldots \\ & =\Delta\tilde{\mathbf{R}}_{ij}\cdot\prod_{k=i}^{j-1}\operatorname{Exp}\Big(-\Delta\tilde{\mathbf{R}}_{k+1j}^T\cdot\mathbf{J}_r^k\cdot\boldsymbol{\eta}_k^{gd}\Delta t\Big) \end{aligned} \]

  其中，1处使用了性质，当\(\delta \vec{\phi}\)是小量时:

  \[\mathrm{Exp}(\vec{\phi}+\delta\vec{\phi})\approx\mathrm{Exp}(\vec{\phi})\cdot\mathrm{Exp}\Big(\mathbf{J}_r(\vec{\phi})\cdot\delta\vec{\phi}\Big) \]

  2处将累乘展开，3和4处利用Adjoint性质，将\(\Delta\tilde{\mathbf{R}}\)换到最左侧：

  \[\mathrm{Exp}(\vec{\phi})\cdot\mathbf{R}=\mathbf{R}\cdot\mathrm{Exp}\Big(\mathbf{R}^T\vec{\phi}\Big) \]

  其中，令：

  \[\mathbf{J}_r^k=\mathbf{J}_r\left(\left(\tilde{\boldsymbol{\omega}}_k-\mathbf{b}_i^g\right)\Delta t\right) \]
  \[\Delta\tilde{\mathbf{R}}_{ij}=\prod_{k=i}^{j-1}\operatorname{Exp}(\left(\tilde{\boldsymbol{\omega}}_k-\mathbf{b}_i^g\right)\Delta t) \]
  \[\mathrm{Exp}\Big(-\delta\vec{\phi}_{ij}\Big)=\prod_{k=i}^{j-1}\mathrm{Exp}\Big(-\Delta\tilde{\mathbf{R}}_{k+1j}^T\cdot\mathbf{J}_r^k\cdot\eta_k^{gd}\Delta t\Big) \]

  则有：

  \[\Delta\mathbf{R}_{ij}\triangleq\Delta\mathbf{\tilde{\mathbf{R}}}_{ij}\cdot\mathrm{Exp}\Big(-\delta\vec{\phi}_{ij}\Big)\tag{13} \]

  这样的话，\(\Delta\tilde{\mathbf{R}}_{ij}\)即P增量测量值，它由陀螺仪测量值和对陀螺仪偏差的估计得到，而\(\delta\vec{\phi}_{ij}\)或\(Exp(\vec{\phi}_{ij})\)即为测量噪声。

• \(\Delta \mathbf{v}_{ij}\):
  将(13)代入式(5)，得到：

  \[\begin{aligned} \Delta\mathbf{v}_{ij}& =\sum_{k=i}^{j-1}\Delta\mathbf{R}_{ik}\cdot\left(\mathbf{\tilde{f}}_k-\mathbf{b}_i^a-\eta_k^{ad}\right)\cdot\Delta t \\ &\approx\sum_{k=i}^{j-1}\Delta\tilde{\mathbf{R}}_{ik}\cdot\mathrm{Exp}\Big(-\delta\vec{\phi}_{ik}\Big)\cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}-\eta_{k}^{ad}\right)\cdot\Delta t \\ &\overset{1}{\approx}\sum_{k=i}^{j-1}\Delta\tilde{\mathbf{R}}_{ik}\cdot\left(\mathbf{I}-\delta\vec{\phi}_{ik}^{\wedge}\right)\cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}-\boldsymbol{\eta}_{k}^{ad}\right)\cdot\Delta t \\ &\overset{2}{\approx}\sum_{k=i}^{j-1}\left[\Delta\tilde{\mathbf{R}}_{ik}\cdot\left(\mathbf{I}-\delta\vec{\phi}_{ik}^{\wedge}\right)\cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right)\cdot\Delta t-\Delta\tilde{\mathbf{R}}_{ik}\eta_{k}^{ad}\Delta t\right]\\ & \stackrel{3}{=}\sum_{k=i}^{j-1}\left[\Delta\tilde{\mathbf{R}}_{ik}\cdot\left(\tilde{\mathbf{f}}_k-\mathbf{b}_i^a\right)\cdot\Delta t+\Delta\tilde{\mathbf{R}}_{ik}\cdot\left(\tilde{\mathbf{f}}_k-\mathbf{b}_i^a\right)^\wedge\cdot\delta\vec{\phi}_{ik}\cdot\Delta t-\right. \Delta\tilde{\mathbf{R}}_{ik}\eta_{k}^{ad}\Delta t\Big]\\ & =\sum_{k=i}^{j-1}\left[\Delta\tilde{\mathbf{R}}_{ik}\cdot\left(\tilde{\mathbf{f}}_k-\mathbf{b}_i^a\right)\cdot\Delta t\right]+\sum_{k=i}^{j-1}\left[\Delta\tilde{\mathbf{R}}_{ik}\cdot\left(\tilde{\mathbf{f}}_k-\mathbf{b}_i^a\right)^{\wedge}\cdot\delta\vec{\phi}_{ik}\cdot\Delta t-\Delta\tilde{\mathbf{R}}_{ik}\eta_k^{ad}\Delta t\right] \end{aligned} \]

  其中1处使用了，当\(\vec\phi\)是小量时，有一阶近似： \(\exp\left(\vec{\phi}^{\wedge}\right)\approx\mathbf{I}+\vec{\phi}^{\wedge}\)，或\(\operatorname{Exp}(\vec{\phi})\approx\mathbf{I}+\vec{\phi}^{\wedge}\)的性质。
  2处忽略高阶小项，\(\delta\vec{\phi}_{ik}^{\wedge}\eta_{k}^{ad}\)。
  3处使用\(\mathbf{a}^{\wedge}\mathbf{b}=-\mathbf{b}^{\wedge}\mathbf{a}\)
  再令：

  \[\begin{aligned} &\Delta\tilde{\mathbf{v}}_{ij} \triangleq\sum_{k=i}^{j-1}\left[\Delta\tilde{\mathbf{R}}_{ik}\cdot\left(\tilde{\mathbf{f}}_k-\mathbf{b}_i^a\right)\cdot\Delta t\right] \\ &\delta\mathbf{v}_{ij} \triangleq\sum_{k=i}^{j-1}\left[\Delta\tilde{\mathbf{R}}_{ik}\eta_{k}^{ad}\Delta t-\Delta\tilde{\mathbf{R}}_{ik}\cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right)^{\wedge}\cdot\delta\vec{\phi}_{ik}\cdot\Delta t\right] \end{aligned}\]

  得到：

  \[\Delta\mathbf{v}_{ij}\triangleq\Delta\tilde{\mathbf{v}}_{ij}-\delta\mathbf{v}_{ij}\tag{16} \]

  \(\tilde{\mathbf{v}}_{ij}\)即速度增量测量值，它由IMU测量值和对偏差的估计或猜测计算得到。\(\delta\mathbf{v}_{ij}\)是其测量噪声。

• \(\Delta\mathbf{p}_{ij}\)
  将(13)和(16)代入(6)可得：

  \[\begin{aligned} \Delta\mathbf{p}_{ij}& =\sum_{k=i}^{j-1}\left[\Delta\mathbf{v}_{ik}\cdot\Delta t+\frac{1}{2}\Delta\mathbf{R}_{ik}\cdot\left(\tilde{\mathbf{f}}_k-\mathbf{b}_i^a-\boldsymbol{\eta}_k^{ad}\right)\cdot\Delta t^2\right] \\ &\approx\sum_{k=i}^{j-1}\left[(\Delta\tilde{\mathbf{v}}_{ik}-\delta\mathbf{v}_{ik})\cdot\Delta t+\frac{1}{2}\Delta\tilde{\mathbf{R}}_{ik}\cdot\mathrm{Exp}\Big(-\delta\vec{\phi}_{ik}\Big)\cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}-\boldsymbol{\eta}_{k}^{ad}\right)\cdot\Delta t^{2}\right. \\ &\stackrel{(1)}{\approx}\sum_{k=i}^{j-1}\left[(\Delta\tilde{\mathbf{v}}_{ik}-\delta\mathbf{v}_{ik})\cdot\Delta t+\frac{1}{2}\Delta\tilde{\mathbf{R}}_{ik}\cdot\left(\mathbf{I}-\delta\vec{\phi}_{ik}^{\wedge}\right)\cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}-\boldsymbol{\eta}_{k}^{ad}\right)\cdot\Delta t^{2}\right] \\ &\stackrel{(2)}{\approx}\sum_{k=i}^{j-1}\left[(\Delta\tilde{\mathbf{v}}_{ik}-\delta\mathbf{v}_{ik})\cdot\Delta t\right. \\ &\left.+\frac12\Delta\tilde{\mathbf{R}}_{ik}\cdot\left(\mathbf{I}-\delta\vec{\phi}_{ik}^{\wedge}\right)\cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right)\cdot\Delta t^{2}-\frac12\Delta\tilde{\mathbf{R}}_{ik}\boldsymbol{\eta}_{k}^{ad}\Delta t^{2}\right]\\ &\overset{(3)}{=}\sum_{k=i}^{j-1}\left[\Delta\tilde{\mathbf{v}}_{ik}\Delta t+\frac12\Delta\tilde{\mathbf{R}}_{ik}\cdot\left(\tilde{\mathbf{f}}_k-\mathbf{b}_i^a\right)\Delta t^2\right. \\ &+\frac{1}{2}\Delta\tilde{\mathbf{R}}_{ik}\cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right)^{\wedge}\delta\vec{\phi}_{ik}\Delta t^{2}-\frac{1}{2}\Delta\tilde{\mathbf{R}}_{ik}\eta_{k}^{ad}\Delta t^{2}-\delta\mathbf{v}_{ik}\Delta t \end{aligned}\]

  其中1处使用了，当\(\vec\phi\)是小量时，有一阶近似： \(\exp\left(\vec{\phi}^{\wedge}\right)\approx\mathbf{I}+\vec{\phi}^{\wedge}\)，或\(\operatorname{Exp}(\vec{\phi})\approx\mathbf{I}+\vec{\phi}^{\wedge}\)的性质。
  2处忽略高阶小项，\(\delta\vec{\phi}_{ik}^{\wedge}\eta_{k}^{ad}\)。
  3处使用\(\mathbf{a}^{\wedge}\mathbf{b}=-\mathbf{b}^{\wedge}\mathbf{a}\)
  再令：

  \[\Delta\tilde{\mathbf{p}}_{ij}\triangleq\sum_{k=i}^{j-1}\left[\Delta\tilde{\mathbf{v}}_{ik}\Delta t+\frac{1}{2}\Delta\tilde{\mathbf{R}}_{ik}\cdot\left(\tilde{\mathbf{f}}_k-\mathbf{b}_i^a\right)\Delta t^2\right] \]
  \[\delta\mathbf{p}_{ij}\triangleq\sum_{k=i}^{j-1}\left[\delta\mathbf{v}_{ik}\Delta t-\frac{1}{2}\Delta\mathbf{\tilde{R}}_{ik}\cdot\left(\mathbf{\tilde{f}}_k-\mathbf{b}_i^a\right)^{\wedge}\delta\vec{\phi}_{ik}\Delta t^2+\frac{1}{2}\Delta\mathbf{\tilde{R}}_{ik}\eta_k^{ad}\Delta t^2\right] \]

  得到：

  \[\Delta\mathbf{p}_{ij}\triangleq\Delta\mathbf{\tilde{p}}_{ij}-\delta\mathbf{p}_{ij} \]

  \(\Delta\mathbf{p}_{ij}\)即位置增量测量值，它由IMU测量值和对偏差的估计得到。\(\delta\mathbf{p}_{ij}\)即其测量噪声。
  因此，得到了PVQ增量真值和测量值的关系：

\[\Delta R_{ij}\triangleq\Delta\tilde{\mathbf{R}}_{ij}(b^g_i)exp(-\delta \phi_{ij}) \]

\[\Delta v_{ij}\triangleq\Delta\tilde{\mathbf{v}}_{ij}(b^g_i,b^a_i)-\delta v_{ij} \]

\[\Delta p_{ij}\triangleq\Delta\tilde{\mathbf{p}}_{ij}(b^g_i,b^a_i)-\delta p_{ij} \]

代入PVQ增量真值表达式(4)(5)(6)得到：

\[\begin{aligned}&\Delta\tilde{\mathbf{R}}_{ij}\approx\Delta\mathbf{R}_{ij}\operatorname{Exp}\Big(\delta\vec{\phi}_{ij}\Big)=\mathbf{R}_i^T\mathbf{R}_j\operatorname{Exp}\Big(\delta\vec{\phi}_{ij}\Big)\\&\Delta\tilde{\mathbf{v}}_{ij}\approx\Delta\mathbf{v}_{ij}+\delta\mathbf{v}_{ij}=\mathbf{R}_i^T\left(\mathbf{v}_j-\mathbf{v}_i-\mathbf{g}\cdot\Delta t_{ij}\right)+\delta\mathbf{v}_{ij}\\&\Delta\tilde{\mathbf{p}}_{ij}\approx\Delta\mathbf{p}_{ij}+\delta\mathbf{p}_{ij}=\mathbf{R}_i^T\left(\mathbf{p}_j-\mathbf{p}_i-\mathbf{v}_i\cdot\Delta t_{ij}-\frac12\mathbf{g}\cdot\Delta t_{ij}^2\right)+\delta\mathbf{p}_{ij}\end{aligned} \]

上述表达式即为PVQ增量测量值(含IMU测量值及偏差估计值)与真值之间的关系，即形如“测量值=真值+噪声”的形式。

偏差更新时的预积分测量值更新

前面的预积分计算，都是在假设积分区间内陀螺和加计的偏差恒定的基础上推导的。当 bias 发生变化时，若仍按照前述公式，预积分测量值需要整个重新计算一遍，这将非常的耗费算力。为了解决这个问题，提出了利用线性化来进行偏差变化时预积分项的一阶近似更新方法。

使用\(_{\mathbf{b}_i}^{-g}\)和\(_{\mathbf{b}_i}^{-a}\)表示旧的偏差，新的偏差\(\hat{\mathbf{b}}_i^g\)和\(\hat{\mathbf{b}}_i^a\)由旧偏差和更新量\(\delta\mathbf{b}_i^g\)和\(\delta\mathbf{b}_i^a\)相加\(\hat{\mathbf{b}}_{i}^{g}\leftarrow\overline{\mathbf{b}}_{i}^{g}+\delta\mathbf{b}_{i}^{g}\)，\(\hat{\mathbf{b}}_{i}^{a}\leftarrow\overline{\mathbf{b}}_{i}^{a}+\delta\mathbf{b}_{i}^{a}\)得到。于是有预积分关于偏差估计值变化的一阶近似更新公式如下：

\[\begin{aligned} &\Delta\tilde{\mathbf{R}}_{ij}\left(\hat{\mathbf{b}}_{i}^{g}\right)\approx\Delta\tilde{\mathbf{R}}_{ij}\left(\overline{\mathbf{b}}_{i}^{g}\right)\cdot\mathrm{Exp}\left(\frac{\partial\Delta\overline{\mathbf{R}}_{ij}}{\partial\overline{\mathbf{b}}^{g}}\delta\mathbf{b}_{i}^{g}\right) \\ &\Delta\tilde{\mathbf{v}}_{ij}\left(\hat{\mathbf{b}}_{i}^{g},\hat{\mathbf{b}}_{i}^{a}\right)\approx\Delta\tilde{\mathbf{v}}_{ij}\left(\overline{\mathbf{b}}_{i}^{g},\overline{\mathbf{b}}_{i}^{a}\right)+\frac{\partial\Delta\overline{\mathbf{v}}_{ij}}{\partial\overline{\mathbf{b}}^{g}}\delta\mathbf{b}_{i}^{g}+\frac{\partial\Delta\overline{\mathbf{v}}_{ij}}{\partial\overline{\mathbf{b}}^{a}}\delta\mathbf{b}_{i}^{a} \\ &\Delta\tilde{\mathbf{p}}_{ij}\left(\hat{\mathbf{b}}_{i}^{g},\hat{\mathbf{b}}_{i}^{a}\right)\approx\Delta\tilde{\mathbf{p}}_{ij}\left(\overline{\mathbf{b}}_{i}^{g},\overline{\mathbf{b}}_{i}^{a}\right)+\frac{\partial\Delta\overline{\mathbf{p}}_{ij}}{\partial\overline{\mathbf{b}}^{-g}}\delta\mathbf{b}_{i}^{g}+\frac{\partial\Delta\overline{\mathbf{p}}_{ij}}{\partial\overline{\mathbf{b}}^{-a}}\delta\mathbf{b}_{i}^{a} \end{aligned}\]

为了便于理解，做符号简化如下：

\[\begin{aligned}&\Delta\hat{\mathbf{R}}_{ij}\doteq\Delta\tilde{\mathbf{R}}_{ij}\left(\hat{\mathbf{b}}_i^g\right),\quad\Delta\overline{\mathbf{R}}_{ij}\doteq\Delta\tilde{\mathbf{R}}_{ij}\left(\overline{\mathbf{b}}_i^g\right)\\&\Delta\hat{\mathbf{v}}_{ij}\doteq\Delta\tilde{\mathbf{v}}_{ij}\left(\hat{\mathbf{b}}_i^g,\hat{\mathbf{b}}_i^a\right),\quad\Delta\overline{\mathbf{v}}_{ij}\doteq\Delta\tilde{\mathbf{v}}_{ij}\left(\overline{\mathbf{b}}_i^g,\overline{\mathbf{b}}_i^a\right)\\&\Delta\hat{\mathbf{p}}_{ij}\doteq\Delta\tilde{\mathbf{p}}_{ij}\left(\hat{\mathbf{b}}_i^g,\hat{\mathbf{b}}_i^a\right),\quad\Delta\overline{\mathbf{p}}_{ij}\doteq\Delta\tilde{\mathbf{p}}_{ij}\left(\overline{\mathbf{b}}_i^g,\overline{\mathbf{b}}_i^a\right)\end{aligned} \]

得到简化后的公式如下：

\[\begin{aligned} &\Delta\hat{\mathbf{R}}_{ij}\approx\Delta\overline{{\mathbf{R}}}_{ij}\cdot\mathrm{Exp}\left(\frac{\partial\Delta\overline{{\mathbf{R}}}_{ij}}{\partial\overline{{\mathbf{b}}}^{g}}\delta\mathbf{b}_{i}^{g}\right) \\ &\Delta\hat{\mathbf{v}}_{ij}\approx\Delta\overline{\mathbf{v}}_{ij}+\frac{\partial\Delta\bar{\mathbf{v}}_{ij}}{\partial\overline{\mathbf{b}}^{g}}\delta\mathbf{b}_{i}^{g}+\frac{\partial\Delta\overline{\mathbf{v}}_{ij}}{\partial\overline{\mathbf{b}}^{a}}\delta\mathbf{b}_{i}^{a} \\ &\Delta\hat{\mathbf{p}}_{ij}\approx\Delta\overline{\mathbf{p}}_{ij}+\frac{\partial\Delta\overline{\mathbf{p}}_{ij}}{\partial\overline{\mathbf{b}}^{-g}}\delta\mathbf{b}_{i}^{g}+\frac{\partial\Delta\overline{\mathbf{p}}_{ij}}{\partial\overline{\mathbf{b}}^{-a}}\delta\mathbf{b}_{i}^{a} \end{aligned}\tag{36}\]

该式说明了IMU预积分是如何修正测量值的，为什么雅可比能够起到修正值的作用？

• 新偏差=旧偏差+更新量，如果把测量值当做偏差的函数，只需要在旧的测量值上添加一个近似的修正量就可以获得近似的新测量值，而不需要重新积分。而这个修正量(增量)就是用偏差的更新量乘以函数的导数(即斜率，雅可比)获得。

这样一来，对于i、j两帧之间的IMU积分我们只需要做一次就可以了(即式36中的 \(\Delta\overline{\mathbf{R}}_{ij}\), \(\Delta\overline{\mathbf{v}}_{ij}\), \(\Delta\overline{\mathbf{p}}_{ij}\))，通过测量值函数对偏差的偏导数(即雅可比)和偏差更新量\(\delta\mathbf{b}_i^g\)，\(\delta\mathbf{b}_i^a\)就可以近似的计算出修正量，获得新测量值的近似值，而不需要重新积分。

如果优化过程中起始位姿发生了变化，则雅可比也相应更新。而偏差更新量\(\delta\mathbf{b}_i^g\)，\(\delta\mathbf{b}_i^a\)。本身就是待优化的变量之一，自然也是相应更新。从而测量值的修正量实现了自动更新。

以上就是IMU预积分避免重新积分，降低运算量的关键。

其中的偏导项定义如下：

\[\begin{aligned} &\frac{\partial\Delta\overline{\mathbf{R}}_{ij}}{\partial\vec{\mathbf{b}}^g} =\sum_{k=i}^{j-1}\left(-\Delta\overline{\mathbf{R}}_{k+1j}^T\mathbf{J}_r^k\Delta t\right) \\ &\frac{\partial\Delta\overline{\mathbf{v}}_{ij}}{\partial\vec{\mathbf{b}}^{g}} =-\sum_{k=i}^{j-1}\left(\Delta\overline{\mathbf{R}}_{ik}\cdot\left(\tilde{\mathbf{f}}_k-\overline{\mathbf{b}}_i^a\right)^\wedge\frac{\partial\Delta\overline{\mathbf{R}}_{ik}}{\partial\overline{\mathbf{b}}^g}\Delta t\right) \\ &\frac{\partial\Delta\overline{\mathbf{v}}_{ij}}{\partial\overline{\mathbf{b}}^a} =-\sum_{k=i}^{j-1}\left(\Delta\overline{\mathbf{R}}_{ik}\Delta t\right) \\ &\frac{\partial\Delta\overline{\mathbf{p}}_{ij}}{\partial\overline{\mathbf{b}}^g} =\sum_{k=i}^{j-1}\left[\frac{\partial\Delta\overline{\mathbf{v}}_{ik}}{\partial\overline{\mathbf{b}}^g}\Delta t-\frac12\Delta\overline{\mathbf{R}}_{ik}\cdot\left(\tilde{\mathbf{f}}_k-\overline{\mathbf{b}}_i^a\right)^\wedge\frac{\partial\Delta\overline{\mathbf{R}}_{ik}}{\partial\overline{\mathbf{b}}^g}\Delta t^2\right] \\ &\frac{\partial\Delta\overline{\mathbf{p}}_{ij}}{\partial\vec{\mathbf{b}}^a} =\sum_{k=i}^{j-1}\left[\frac{\partial\Delta\overline{\mathbf{v}}_{ik}}{\partial\overline{\mathbf{b}}^{-a}}\Delta t-\frac12\Delta\overline{\mathbf{R}}_{ik}\Delta t^2\right] \end{aligned}\]

其中\(\mathbf{J}_r^k=\mathbf{J}_r\left(\left(\tilde{\boldsymbol{\omega}}_k-\mathbf{b}_i^g\right)\Delta t\right)\)

非线性优化

IMU的测量值和通过非IMU方式获得的预测值之间建立优化问题，完成非线性优化。
而在这中间通过近似修正的方式避免了重新积分，这是预积分降低计算量的关键。(优化更新偏差，偏差修正之前的积分，积分再反馈到新的优化中)

参考

https://zhuanlan.zhihu.com/p/473227932
《增强现实：原理、算法与应用》