IMU 预积分推导

标签：进一步   上进   时间   rom   需要   翻译   uri   i+1   方法   

给 StereoDSO 加 IMU，想直接用 OKVIS 的代码，但是有点看不懂。知乎上郑帆写的文章《四元数矩阵与 so(3) 左右雅可比》提到 OKVIS 的预积分是使用四元数，而预积分论文中使用 so(3) 的右雅克比。才疏学浅，先整理好 so(3) 的预积分，写好 StereoDSO 加上 IMU，再考虑其他的东西。
以下的内容参考预积分的的论文，还有它的 Supplementary Material。预积分的论文中有一些 typo 所以看上去还是比较迷的，参考网络上多份预积分论文的 pdf，我整理一下整个预积分的推导过程。所以以下内容也可以看做是预积分内容的翻译。

预积分的意义。IMU 测量设备的角速度、线加速度，通过积分可以获得设备在一段时间内的位置、姿态、速度改变量，积分是在 Manifold 上进行，所以不同积分起点，位置、姿态、速度改变量也不同。在 VIO 的应用过程中，需要将 IMU 误差进入到优化过程，优化迭代计算。由于以上 Manifold 的特性，使得每次优化起点发生变化，IMU 测量得到的相对位置、姿态、速度发生变化，所以每次优化迭代需要对 IMU 测量值重新积分，过程耗时。为了解决这个不断积分的问题，IMU 预积分不将位置、姿态、速度改变量作为 IMU 观测值，而将重力加速度对它们的影响去除（公式 (\ref{eq2:imu_pre_state_diff_delta_R_ij}) (\ref{eq2:imu_pre_state_diff_delta_v_ij}) (\ref{eq2:imu_pre_state_diff_delta_p_ij})），使得新定义的 IMU 观测值与起点的位置、姿态、速度无关，在优化过程中不会因为起点的这些状态量改变而重新积分。（去除重力加速度的影响，应是重力加速度与当前的姿态相关，只有将其去除才有可能使得 IMU 观测值与起点的姿态无关。）但是定义的 IMU 观测值依旧与起点的偏移值相关，使用 Taylor 展开拟合偏移值在优化过程中的变化，累积到足够大再进行积分。

我写这个东西，就是瞎bb，代码还没有验证。

DSO 是左雅克比（看这个代码就知道了，https://github.com/JakobEngel/dso/blob/master/src/FullSystem/HessianBlocks.h#L190 ），而 IMU 预积分论文是右雅克比。需要注意在特定的地方改变一下。

Residuals

IMU 测量值可以看做真实值、漂移值（Bias）、高斯误差的和：
\[\begin{align} \mathbin{_B\tilde{\pmb{\omega}}_{WB}}(t) = \mathbin{_B\pmb{\omega}_{WB}}(t) + \pmb{b}^g(t) + \pmb{\eta}^g(t) \label{eq2:imu_pre_mes_w} \\mathbin{_B\tilde{\pmb{a}}}(t) = \mathbf{R}_{WB}^T(t)(\mathbin{_W\pmb{a}}(t) - \mathbin{_W\pmb{g}}) + \pmb{b}^a(t) + \pmb{\eta}^a(t) \label{eq2:imu_pre_mes_a} \end{align}\]
其中下标的 \(W, B\) 分别表示世界（World）坐标系和 IMU （Body）坐标系，左下标 \(\mathbin{_W\cdot}, \mathbin{_B\cdot}\) 表示所在的坐标系，\(\mathbf{R}_{WB}\) 的右下标表示从 IMU 坐标系旋转到世界坐标系，\(\mathbin{_B\pmb{\omega}_{WB}}\) 的右下标表示 IMU 坐标系相对于世界坐标系的瞬时角速度，右上标 \(\cdot^{g}, \cdot^{a}\) 分别表示陀螺仪（Gyroscope）角速度测量值相关、加速度计（Accelerometer）线加速度测量值相关。公式中的真实值是 \(t\) 时刻的角速度 \(\mathbin{_B\pmb{\omega}_{WB}}(t)\)、线加速度 \(\mathbin{_W\pmb{a}}(t)\)，漂移值是 \(t\) 时刻的角速度漂移值 \(\pmb{b}^g(t)\)、线加速度漂移值 \(\pmb{b}^a(t)\)，高斯误差是 \(t\) 时刻的角速度测量值误差 \(\pmb{\eta}^g(t)\)、线加速度测量误差 \(\pmb{\eta}^a(t)\)。\(\mathbin{_W\pmb{g}}\) 是世界坐标系下的重力加速度。

从 \(t\) 时刻开始，经过一小段时间 \(\Delta t\) 到达 \(t + \Delta t\) 时刻，使用欧拉方法（Euler Method），即假设在 $[t, t + \Delta t] $ 内角速度与线加速度保持不变，得到以下结果：
\[\begin{align} \mathbf{R}_{WB}(t+\Delta t) &= \mathbf{R}_{WB}(t) \text{Exp}(\mathbin{_B\pmb{\omega}_{WB}}(t)\Delta t) \ \mathbin{_W\mathbf{v}}(t+\Delta t) &= \mathbin{_W\mathbf{v}}(t) + \mathbin{_W\mathbf{a}}(t)\Delta t \ \mathbin{_W\mathbf{p}}(t+\Delta t) &= \mathbin{_W\mathbf{p}}(t) + \mathbin{_W\mathbf{v}}(t) \Delta t + \frac{1}{2}\mathbin{_W\mathbf{a}}(t)\Delta t^2 \end{align}\]
将公式 (\ref{eq2:imu_pre_mes_w}) (\ref{eq2:imu_pre_mes_a})，并忽略表示坐标系的下标，得到：
\[\begin{align} \mathbf{R}(t+\Delta t) &= \mathbf{R}(t) \text{Exp}((\tilde{\pmb{\omega}}(t) - \mathbf{b}^g(t) - \pmb{\eta}^{gd}(t))\Delta t) \label{eq2:imu_pre_delta_R} \ \mathbf{v}(t+\Delta t) &= \mathbf{v}(t) + \mathbf{g}\Delta t + \mathbf{R}(t)(\tilde{\mathbf{a}}(t) - \mathbf{b}^a(t) - \pmb{\eta}^{ad}(t))\Delta t \label{eq2:imu_pre_delta_v} \ \mathbf{p}(t+\Delta t) &= \mathbf{p}(t) + \mathbf{v}(t)\Delta t + \frac{1}{2}\mathbf{g}\Delta t^2 + \frac{1}{2}\mathbf{R}(t)(\tilde{\mathbf{a}}(t) - \mathbf{b}^a(t) - \pmb{\eta}^{ad}(t))\Delta t^2 \label{eq2:imu_pre_delta_p} \end{align}\]
其中上标 \(\cdot^{d}\) 表示离散（discrete），是在 \(\Delta t\) 时间段内的测量值的高斯误差。

从现在开始，考虑离散的情况。假设 \(\Delta t\) 是 IMU 测量值的时间间隔，从第 \(i\) 个 IMU 测量值积分到第 \(j\) 个 IMU 测量值，中间一共经历 \(j - i\) 个 \(\Delta t\) 时间段，按照公式 (\ref{eq2:imu_pre_delta_R}) (\ref{eq2:imu_pre_delta_v}) (\ref{eq2:imu_pre_delta_p})，可以从 \(i\) 时刻的状态值积分得到 \(j\) 时刻的状态值：
\[\begin{align} \mathbf{R}_j &= \mathbf{R}_i \prod_{k=i}^{j-1}\text{Exp}((\tilde{\pmb{\omega}}_k - \mathbf{b}^g_k - \pmb{\eta}^{gd}_k)\Delta t) \ \mathbf{v}_j &= \mathbf{v}_i + \mathbf{g}\Delta t_{ij} + \sum_{k=i}^{j-1}\mathbf{R}_k(\tilde{\mathbf{a}}_k - \mathbf{b}^a_k - \pmb{\eta}^{ad}_k)\Delta t\ \mathbf{p}_j &= \mathbf{p}_i + \sum_{k=i}^{j-1}\mathbf{v}_k\Delta t + \sum_{k=i}^{j-1}\frac{1}{2}\mathbf{g}\Delta t^2 + \frac{1}{2}\sum_{k=i}^{j-1}\mathbf{R}_k(\tilde{\mathbf{a}}_k - \mathbf{b}^a_k - \pmb{\eta}^{ad}_k)\Delta t^2 \end{align}\]
其中引入了新的变量 \(\Delta t_{ij} \doteq \sum_{k = i}^{j-1}\Delta t\)。接下来定义 \(i, j\) 时刻状态量之间的“差值”。按照“差值”的字面意思，可以直接写出 \(\mathbf{R}_i^T\mathbf{R}_j, \mathbf{v}_j - \mathbf{v}_i, \mathbf{p}_j - \mathbf{p}_i\)，然而这个太 NAIVE，这么写就不是预积分了。预积分定义的状态量如下：
\[\begin{align} \Delta \mathbf{R}_{ij} &\doteq \mathbf{R}_i^T \mathbf{R}_j = \prod_{k=i}^{j-1}\text{Exp}((\tilde{\pmb{\omega}}_k - \mathbf{b}^g_k - \pmb{\eta}^{gd}_k)\Delta t) \label{eq2:imu_pre_state_diff_delta_R_ij} \\Delta \mathbf{v}_{ij} &\doteq \mathbf{R}_i^T(\mathbf{v}_j - \mathbf{v}_i - \mathbf{g}\Delta t_{ij}) = \sum_{k=i}^{j-1}\Delta\mathbf{R}_{ik}(\tilde{\mathbf{a}}_k - \mathbf{b}^a_k - \pmb{\eta}^{ad}_k)\Delta t \label{eq2:imu_pre_state_diff_delta_v_ij} \\Delta \mathbf{p}_{ij} &\doteq \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) \notag \&= \sum_{k=i}^{j-1}(\Delta\mathbf{v}_{ik}\Delta t + \frac{1}{2}\Delta\mathbf{R}_{ik}(\tilde{\mathbf{a}}_k - \mathbf{b}^a_k - \pmb{\eta}^{ad}_k)\Delta t) \notag \&= \sum_{k=i}^{j-1}\frac{3}{2}\Delta\mathbf{R}_{ik}(\tilde{\mathbf{a}}_k - \mathbf{b}^a_k - \pmb{\eta}^{ad}_k)\Delta t \label{eq2:imu_pre_state_diff_delta_p_ij} \end{align}\]
上面定义的状态量都与 \(i, j\) 时刻的状态量 \(\mathbf{R}_i, \mathbf{v}_i, \mathbf{p}_i, \mathbf{R}_j, \mathbf{v}_j, \mathbf{p}_j\) 无关。考虑到上面出现了中间时刻的角速度测量漂移值 \(\mathbf{b}^g_k\) 和线加速度测量漂移值 \(\mathbf{b}^a_k\)，为方便起见，假设 $\mathbf{b}^g_k = \mathbf{b}^g_i, \mathbf{b}^a_k = \mathbf{b}^a_i, k=i, i+1, \dots, j $。因为 Bias 也是需要估计出来的，这种假设减少大量需要估计的状态量。此处假设形成的近似，限制了 IMU 预积分的时间间隔，在应用的过程中应该注意到这一点。

下一步是将高斯误差与测量值、漂移值分隔开，以方便估计高斯误差。由 BCH 公式可得（这个地方还是值得推导一下的，第二个等号神复杂）：
\[\begin{align} \Delta\mathbf{R}_{ij} &\simeq \prod_{k=i}^{j-1} \left[ \text{Exp}((\tilde{\pmb{\omega}}_k - \mathbf{b}^g_i)\Delta t) \text{Exp}(-\mathbf{J}^k_r\pmb{\eta}^{gd}_k\Delta t) \right] \notag \ &= \Delta\tilde{\mathbf{R}}_{ij} \prod_{k=i}^{j-1} \text{Exp}(-\Delta\tilde{\mathbf{R}}^T_{k+1, j}\mathbf{J}^k_r\pmb{\eta}^{gd}_k\Delta t) \notag \ &\doteq \Delta\tilde{\mathbf{R}}_{ij} \text{Exp}(-\delta \pmb{\phi}_{ij}) \label{eq2:imu_pre_mes_state_diff_delta_R_ij} \end{align}\]
其中 \(\mathbf{J}^k_r\doteq\mathbf{J}^k_r(\tilde{\pmb{\omega}}_k - \mathbf{b}^g_i)\) 是 \(\mathfrak{so}(3)\) 的右雅可比。\(\Delta\tilde{\mathbf{R}}_{ij} \doteq \prod_{k=i}^{j-1}\exp((\tilde{\pmb{\omega}}_k - \mathbf{b}^g_i)\Delta t)\) 是 IMU 预积分的虚拟旋转测量值，\(\exp(-\delta \pmb{\phi}_{ij})\) 是对应的高斯误差。由此，可以进一步得到 IMU 预积分的虚拟速度和虚拟位移与它们的测量值、误差之间的关系：
\[\begin{align} \Delta\mathbf{v}_{ij} &\simeq \sum_{k=i}^{j-1} (\Delta\tilde{\mathbf{R}}_{ik}(\mathbf{I}-\delta\pmb{\phi}^{\wedge}_{ik})(\tilde{\mathbf{a}}_k-\mathbf{b}^a_i)\Delta t - \Delta\tilde{\mathbf{R}}_{ik}\pmb{\eta}^{ad}_k\Delta t) \notag \ &= \Delta\tilde{\mathbf{v}}_{ij} + \sum_{k=i}^{j-1} \left[\Delta\tilde{\mathbf{R}}_{ik}(\tilde{\mathbf{a}}_k-\mathbf{b}^a_i)^{\wedge}\delta\pmb{\phi}_{ik}\Delta t - \Delta\tilde{\mathbf{R}}_{ik}\pmb{\eta}^{ad}_k\Delta t \right] \notag \ &\doteq \Delta\tilde{\mathbf{v}}_{ij} - \delta\mathbf{v}_{ij} \label{eq2:imu_pre_mes_state_diff_delta_v_ij} \\Delta\mathbf{p}_{ij} &\simeq \sum_{k=i}^{j-1}\frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}(\mathbf{I}-\delta\pmb{\phi}^{\wedge}_{ik})(\tilde{\mathbf{a}}_k-\mathbf{b}^a_i)\Delta t^2 - \sum_{k=i}^{j-1}\frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}\pmb{\eta}^{ad}_k\Delta t^2 \notag \ &= \Delta\tilde{\mathbf{p}}_{ij} + \sum_{k=i}^{j-1}\left[\frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}(\tilde{\mathbf{a}}_k-\mathbf{b}^a_i)^{\wedge}\delta\pmb{\phi}_{ik}\Delta t^2 - \frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}\pmb{\eta}^{ad}_k\Delta t^2\right] \notag \ &\doteq \Delta\tilde{\mathbf{p}}_{ij} - \delta\mathbf{p}_{ij} \label{eq2:imu_pre_mes_state_diff_delta_p_ij} \end{align}\]

整理一下虚拟测量值与其误差:
\[\begin{align} \Delta\tilde{\mathbf{R}}_{ij} &\doteq \prod_{k=i}^{j-1}\text{Exp}((\tilde{\pmb{\omega}}_k - \mathbf{b}^g_i)\Delta t) \\text{Exp}(-\delta \pmb{\phi}_{ij}) &\doteq \prod_{k=i}^{j-1} \text{Exp}(-\Delta\tilde{\mathbf{R}}^T_{k+1, j}\mathbf{J}^k_r\pmb{\eta}^{gd}_k\Delta t) \label{eq2:imu_pre_mes_delta_phi_ij} \\Delta\tilde{\mathbf{v}}_{ij} &\doteq \sum_{k=i}^{j-1}\Delta\tilde{\mathbf{R}}_{ik}(\tilde{\mathbf{a}}_k-\mathbf{b}^a_i)\Delta t \-\delta \mathbf{v}_{ij} &\doteq \sum_{k=i}^{j-1} \left[\Delta\tilde{\mathbf{R}}_{ik}(\tilde{\mathbf{a}}_k-\mathbf{b}^a_i)^{\wedge}\delta\pmb{\phi}_{ik}\Delta t - \Delta\tilde{\mathbf{R}}_{ik}\pmb{\eta}^{ad}_k\Delta t \right] \label{eq2:imu_pre_mes_delta_v_ij} \\Delta\tilde{\mathbf{p}}_{ij} &\doteq \sum_{k=i}^{j-1}\frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}(\tilde{\mathbf{a}}_k-\mathbf{b}^a_i)\Delta t^2 \-\delta \mathbf{p}_{ij} &\doteq \sum_{k=i}^{j-1}\left[\frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}(\tilde{\mathbf{a}}_k-\mathbf{b}^a_i)^{\wedge}\delta\pmb{\phi}_{ik}\Delta t^2 - \frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}\pmb{\eta}^{ad}_k\Delta t^2\right] \label{eq2:imu_pre_mes_delta_p_ij} \\end{align} \]

将公式 (\ref{eq2:imu_pre_mes_state_diff_delta_R_ij}) (\ref{eq2:imu_pre_mes_state_diff_delta_v_ij}) (\ref{eq2:imu_pre_mes_state_diff_delta_p_ij})分别代入公式 (\ref{eq2:imu_pre_state_diff_delta_R_ij}) (\ref{eq2:imu_pre_state_diff_delta_v_ij}) (\ref{eq2:imu_pre_state_diff_delta_p_ij})，得到虚拟状态量测量值与 \(i, j\) 时刻状态量的联系:
\[\begin{align} \Delta\tilde{\mathbf{R}}_{ij} &= \mathbf{R}^T_i\mathbf{R}_j\text{Exp}(\delta\pmb{\phi}_{ij}) \ \Delta\tilde{\mathbf{v}}_{ij} &= \mathbf{R}^T_i(\mathbf{v}_j-\mathbf{v}_i-\mathbf{g}\Delta t_{ij}) + \delta\mathbf{v}_{ij} \ \Delta\tilde{\mathbf{p}}_{ij} &= \mathbf{R}^T_i(\mathbf{p}_j-\mathbf{p}_i-\mathbf{v}\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t^2_{ij}) + \delta \mathbf{p}_{ij} \end{align}\]
优化通过调整 \(i, j\) 时刻的状态量 \(\mathbf{R}_i, \mathbf{v}_i, \mathbf{p}_i, \mathbf{R}_j, \mathbf{v}_j, \mathbf{p}_j\)，最小化 \(\delta \pmb{\phi}_{ij}, \delta \mathbf{v}_{ij}, \delta \mathbf{p}_{ij}\)。

以上 \(\Delta\tilde{\mathbf{R}}_{ij}, \Delta\tilde{\mathbf{v}}_{ij}, \Delta\tilde{\mathbf{p}}_{ij}\) 虽然与 \(\mathbf{R}_i, \mathbf{v}_i, \mathbf{p}_i, \mathbf{R}_j, \mathbf{v}_j, \mathbf{p}_j\) 无关，但与 \(i\) 时刻的漂移值 \(\mathbf{b}^g_i, \mathbf{b}^a_i\) 相关，这两个漂移值也是需要估计的状态量，在优化过程中会发生改变，使得需要重新积分。为了解决这个问题，IMU 预积分使用泰勒展开进行近似。如果漂移值的变化量过大，则需要重新积分。在漂移值处的泰勒展开形式如下：

\[\begin{align} \Delta\tilde{\mathbf{R}}_{ij}(\mathbf{b}^g_i) &\simeq \Delta\tilde{\mathbf{R}}_{ij}(\bar{\mathbf{b}}^g_i)\text{Exp}\left(\frac{\partial\Delta\bar{\mathbf{R}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i\right) \label{eq2:imu_pre_mes_update_bias_R} \ \Delta\tilde{\mathbf{v}}_{ij}(\mathbf{b}^g_i, \mathbf{b}^a_i) &\simeq \Delta\tilde{\mathbf{v}}_{ij}(\bar{\mathbf{b}}^g_i, \bar{\mathbf{b}}^a_i) + \frac{\partial\Delta\bar{\mathbf{v}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i + \frac{\partial\Delta\bar{\mathbf{v}}_{ij}}{\partial\mathbf{b}^a}\delta\mathbf{b}^a_i \label{eq2:imu_pre_mes_update_bias_v} \ \Delta\tilde{\mathbf{p}}_{ij}(\mathbf{b}^g_i, \mathbf{b}^a_i) &\simeq \Delta\tilde{\mathbf{p}}_{ij}(\bar{\mathbf{b}}^g_i, \bar{\mathbf{b}}^a_i) + \frac{\partial\Delta\bar{\mathbf{p}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i + \frac{\partial\Delta\bar{\mathbf{p}}_{ij}}{\partial\mathbf{b}^a}\delta\mathbf{b}^a_i \label{eq2:imu_pre_mes_update_bias_p} \end{align}\]

得到虚拟测量值的误差计算方法：

\[\begin{align} \mathbf{r}_{\Delta\mathbf{R}_{ij}} &\doteq -\delta \pmb{\phi}_{ij} = \text{Log}\left( \left( \Delta\tilde{\mathbf{R}}_{ij}(\bar{\mathbf{b}}^g_i) \text{Exp}\left( \frac{\partial\Delta\bar{\mathbf{R}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i \right) \right)^T \mathbf{R}^T_i\mathbf{R}_j \right) \ \mathbf{r}_{\Delta\mathbf{v}_{ij}} &\doteq -\delta \mathbf{v}_{ij} = \mathbf{R}^T_i(\mathbf{v}_j - \mathbf{v}_i - \mathbf{g}\Delta t_{ij}) \notag \ &- \left( \Delta\tilde{\mathbf{v}}_{ij}(\bar{\mathbf{b}}^g_i, \bar{\mathbf{b}}^a_i) + \frac{\partial\Delta\bar{\mathbf{v}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i + \frac{\partial\Delta\bar{\mathbf{v}}_{ij}}{\partial\mathbf{b}^a}\delta\mathbf{b}^a_i \right) \ \mathbf{r}_{\Delta\mathbf{p}_{ij}} &\doteq -\delta \mathbf{p}_{ij} = \mathbf{R}^T_i(\mathbf{p}_j-\mathbf{p}_i-\mathbf{v}\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t^2_{ij}) \notag \ &- \left( \Delta\tilde{\mathbf{p}}_{ij}(\bar{\mathbf{b}}^g_i, \bar{\mathbf{b}}^a_i) + \frac{\partial\Delta\bar{\mathbf{p}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i + \frac{\partial\Delta\bar{\mathbf{p}}_{ij}}{\partial\mathbf{b}^a}\delta\mathbf{b}^a_i \right) \end{align}\]

Covariances

从 \(i\) 时刻到 \(j\) 时刻的积分使用了 \(j - i\) 组 IMU 测量值，这些测量值具有游走误差。这些误差的 covariance 需要累积，得到 IMU 预积分虚拟测量值的 covariance。

由公式 (\ref{eq2:imu_pre_mes_delta_phi_ij}) (\ref{eq2:imu_pre_mes_delta_v_ij}) (\ref{eq2:imu_pre_mes_delta_p_ij}) 可得：
\[\begin{align} \delta \pmb{\phi}_{ij} &= \sum_{k=i}^{j-1} \Delta\tilde{\mathbf{R}}^T_{k+1, j}\mathbf{J}^k_r\pmb{\eta}^{gd}_k\Delta t \\delta \mathbf{v}_{ij} &= \sum_{k=i}^{j-1} \left[-\Delta\tilde{\mathbf{R}}_{ik}(\tilde{\mathbf{a}}_k-\mathbf{b}^a_i)^{\wedge}\delta\pmb{\phi}_{ik}\Delta t + \Delta\tilde{\mathbf{R}}_{ik}\pmb{\eta}^{ad}_k\Delta t \right] \\delta \mathbf{p}_{ij} &\doteq \sum_{k=i}^{j-1}\left[-\frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}(\tilde{\mathbf{a}}_k-\mathbf{b}^a_i)^{\wedge}\delta\pmb{\phi}_{ik}\Delta t^2 + \frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}\pmb{\eta}^{ad}_k\Delta t^2\right] \end{align}\]

考虑 \(\delta \pmb{\phi}_{ij}, \delta \mathbf{v}_{ij}, \delta \mathbf{p}_{ij}\) 的迭代计算过程，即从 \(\delta \pmb{\phi}_{ik}, \delta \mathbf{v}_{ik}, \delta \mathbf{p}_{ik}\) 计算 \(\delta \pmb{\phi}_{i,k+1}, \delta \mathbf{v}_{i,k+1}, \delta \mathbf{p}_{i,k+1}\) 的过程。（注意，这里 \(\delta \mathbf{p}_{ij}\) 的推导结果与文章不同，确实无法认同文章。）

\[\begin{align} \delta \pmb{\phi}_{ij} &= \sum_{k=i}^{j-1} \Delta\tilde{\mathbf{R}}^T_{k+1, j}\mathbf{J}^k_r\pmb{\eta}^{gd}_k\Delta t \notag \\ &= \sum_{k=i}^{j-2} \Delta\tilde{\mathbf{R}}^T_{k+1, j}\mathbf{J}^k_r\pmb{\eta}^{gd}_k\Delta t + \Delta\tilde{\mathbf{R}}^T_{jj}\mathbf{J}^{j-1}_r\pmb{\eta}^{gd}_{j-1}\Delta t \notag \\ &= \sum_{k=i}^{j-2} (\Delta\tilde{\mathbf{R}}_{k+1, j-1}\Delta\tilde{\mathbf{R}}_{j-1, j})^T\mathbf{J}^k_r\pmb{\eta}^{gd}_k\Delta t + \mathbf{J}^{j-1}_r\pmb{\eta}^{gd}_{j-1}\Delta t \notag \\ &= \Delta\tilde{\mathbf{R}}_{j-1, j}^T\sum_{k=i}^{j-2} \Delta\tilde{\mathbf{R}}_{k+1, j-1}^T\mathbf{J}^k_r\pmb{\eta}^{gd}_k\Delta t + \mathbf{J}^{j-1}_r\pmb{\eta}^{gd}_{j-1}\Delta t \notag \\ &= \Delta\tilde{\mathbf{R}}_{j-1, j}^T \delta \pmb{\phi}_{i,j-1} + \mathbf{J}^{j-1}_r\pmb{\eta}^{gd}_{j-1}\Delta t \\ \delta \mathbf{v}_{ij} &= \sum_{k=i}^{j-1} \left[-\Delta\tilde{\mathbf{R}}_{ik}(\tilde{\mathbf{a}}_k-\mathbf{b}^a_i)^{\wedge}\delta\pmb{\phi}_{ik}\Delta t + \Delta\tilde{\mathbf{R}}_{ik}\pmb{\eta}^{ad}_k\Delta t \right] \notag \\ &= \sum_{k=i}^{j-2} \left[-\Delta\tilde{\mathbf{R}}_{ik}(\tilde{\mathbf{a}}_k-\mathbf{b}^a_i)^{\wedge}\delta\pmb{\phi}_{ik}\Delta t + \Delta\tilde{\mathbf{R}}_{ik}\pmb{\eta}^{ad}_k\Delta t \right] \notag \\ &-\Delta\tilde{\mathbf{R}}_{i,j-1}(\tilde{\mathbf{a}}_{j-1}-\mathbf{b}^a_i)^{\wedge}\delta\pmb{\phi}_{i,j-1}\Delta t + \Delta\tilde{\mathbf{R}}_{i,j-1}\pmb{\eta}^{ad}_{j-1}\Delta t \notag \\ &= \delta \mathbf{v}_{i,j-1} -\Delta\tilde{\mathbf{R}}_{i,j-1}(\tilde{\mathbf{a}}_{j-1}-\mathbf{b}^a_i)^{\wedge}\delta\pmb{\phi}_{i,j-1}\Delta t + \Delta\tilde{\mathbf{R}}_{i,j-1}\pmb{\eta}^{ad}_{j-1}\Delta t \\delta \mathbf{p}_{ij} &= \sum_{k=i}^{j-1}\left[-\frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}(\tilde{\mathbf{a}}_k-\mathbf{b}^a_i)^{\wedge}\delta\pmb{\phi}_{ik}\Delta t^2 + \frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}\pmb{\eta}^{ad}_k\Delta t^2\right] \notag \\ &= \sum_{k=i}^{j-2}\left[-\frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}(\tilde{\mathbf{a}}_k-\mathbf{b}^a_i)^{\wedge}\delta\pmb{\phi}_{ik}\Delta t^2 + \frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}\pmb{\eta}^{ad}_k\Delta t^2\right] \notag \\ &-\frac{3}{2}\Delta\tilde{\mathbf{R}}_{i,j-1}(\tilde{\mathbf{a}}_{j-1}-\mathbf{b}^a_i)^{\wedge}\delta\pmb{\phi}_{i,j-1}\Delta t^2 + \frac{3}{2}\Delta\tilde{\mathbf{R}}_{i,j-1}\pmb{\eta}^{ad}_{j-1}\Delta t^2 \notag \\ &= \delta \mathbf{p}_{i,j-1} -\frac{3}{2}\Delta\tilde{\mathbf{R}}_{i,j-1}(\tilde{\mathbf{a}}_{j-1}-\mathbf{b}^a_i)^{\wedge}\delta\pmb{\phi}_{i,j-1}\Delta t^2 + \frac{3}{2}\Delta\tilde{\mathbf{R}}_{i,j-1}\pmb{\eta}^{ad}_{j-1}\Delta t^2\end{align}\]

于是有：

\[\begin{align} \delta \pmb{\phi}_{i,k+1} &= \Delta\tilde{\mathbf{R}}_{k,k+1}^T \delta \pmb{\phi}_{ik} + \mathbf{J}^{k}_r\pmb{\eta}^{gd}_{k}\Delta t \\delta \mathbf{v}_{i,k+1} &= \delta \mathbf{v}_{i,k} -\Delta\tilde{\mathbf{R}}_{i,k}(\tilde{\mathbf{a}}_{k}-\mathbf{b}^a_i)^{\wedge}\delta\pmb{\phi}_{i,k}\Delta t + \Delta\tilde{\mathbf{R}}_{i,k}\pmb{\eta}^{ad}_{k}\Delta t \\delta \mathbf{p}_{i,k+1} &= \delta \mathbf{p}_{i,k} -\frac{3}{2}\Delta\tilde{\mathbf{R}}_{i,k}(\tilde{\mathbf{a}}_{k}-\mathbf{b}^a_i)^{\wedge}\delta\pmb{\phi}_{i,k}\Delta t^2 + \frac{3}{2}\Delta\tilde{\mathbf{R}}_{i,k}\pmb{\eta}^{ad}_{k}\Delta t^2 \end{align}\]

写成矩阵形式：

\[\begin{align} \begin{bmatrix} \delta \pmb{\phi}_{i,k+1} \\ \delta \mathbf{v}_{i,k+1} \\ \delta \mathbf{p}_{i,k+1} \end{bmatrix} &= \begin{bmatrix} \Delta\tilde{\mathbf{R}}_{k,k+1}^T & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} \\ -\Delta\tilde{\mathbf{R}}_{i,k}(\tilde{\mathbf{a}}_{k}-\mathbf{b}^a_i)^{\wedge}\Delta t & \mathbf{I}_{3\times3} & \mathbf{0}_{3\times3} \\ -\frac{3}{2}\Delta\tilde{\mathbf{R}}_{i,k}(\tilde{\mathbf{a}}_{k}-\mathbf{b}^a_i)^{\wedge}\Delta t^2 & \mathbf{0}_{3\times3} & \mathbf{I}_{3\times3} \end{bmatrix} \begin{bmatrix} \delta \pmb{\phi}_{i,k} \\ \delta \mathbf{v}_{i,k} \\ \delta \mathbf{p}_{i,k} \end{bmatrix} \notag \\ &+ \begin{bmatrix} \mathbf{J}^{k}_r \Delta t & \mathbf{0}_{3\times3} \\ \mathbf{0}_{3\times3} & \Delta\tilde{\mathbf{R}}_{i,k} \Delta t \\ \mathbf{0}_{3\times3} & \frac{3}{2}\Delta\tilde{\mathbf{R}}_{i,k} \Delta t^2 \end{bmatrix} \begin{bmatrix} \pmb{\eta}^{gd}_{k} \\ \pmb{\eta}^{ad}_{k} \end{bmatrix} \end{align}\]

令

\[\begin{align} \mathbf{A} &= \begin{bmatrix} \Delta\tilde{\mathbf{R}}_{k,k+1}^T & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} \\ -\Delta\tilde{\mathbf{R}}_{i,k}(\tilde{\mathbf{a}}_{k}-\mathbf{b}^a_i)^{\wedge}\Delta t & \mathbf{I}_{3\times3} & \mathbf{0}_{3\times3} \\ -\frac{3}{2}\Delta\tilde{\mathbf{R}}_{i,k}(\tilde{\mathbf{a}}_{k}-\mathbf{b}^a_i)^{\wedge}\Delta t^2 & \mathbf{0}_{3\times3} & \mathbf{I}_{3\times3} \end{bmatrix} \\mathbf{B} &= \begin{bmatrix} \mathbf{J}^{k}_r \Delta t & \mathbf{0}_{3\times3} \\ \mathbf{0}_{3\times3} & \Delta\tilde{\mathbf{R}}_{i,k} \Delta t \\ \mathbf{0}_{3\times3} & \frac{3}{2}\Delta\tilde{\mathbf{R}}_{i,k} \Delta t^2 \end{bmatrix} \end{align}\]

covariance propagation 的一步：
\[\begin{align} \pmb{\Sigma}_{i, k+1} = \mathbf{A} \pmb{\Sigma}_{i, k} \mathbf{A}^T + \mathbf{B} \pmb{\Sigma}_{\eta} \mathbf{B}^T \end{align}\]

Jacobians

Jacobians of Biases

计算公式 (\ref{eq2:imu_pre_mes_update_bias_R}) (\ref{eq2:imu_pre_mes_update_bias_v}) (\ref{eq2:imu_pre_mes_update_bias_p}) 中出现的 Jacobians，即 \(\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{v}}_{ij}}{\partial\mathbf{b}^a}, \frac{\partial\Delta\bar{\mathbf{p}}_{ij}}{\partial\mathbf{b}^g}, \frac{\partial\Delta\bar{\mathbf{p}}_{ij}}{\partial\mathbf{b}^a}\)。

更新 Bias ：\(\hat{\mathbf{b}}_i \leftarrow \bar{\mathbf{b}}_i + \delta \mathbf{b}_i\)。

\[\begin{align} \Delta\tilde{\mathbf{R}}_{ij}(\hat{\mathbf{b}}_i) &\doteq \prod_{k=i}^{j-1}\text{Exp}((\tilde{\pmb{\omega}}_k - \bar{\mathbf{b}}^g_i - \delta \mathbf{b}^g_i)\Delta t) \notag \\ &= \prod_{k=i}^{j-1}\left[\text{Exp}((\tilde{\pmb{\omega}}_k - \bar{\mathbf{b}}^g_i)\Delta t) \text{Exp}(- \mathbf{J}^k_r \delta \mathbf{b}^g_i \Delta t)\right] \notag \&= \prod_{k=i}^{j-1}\left[\text{Exp}(- \Delta\tilde{\mathbf{R}}_{k+1, j}(\bar{\mathbf{b}}_i^g)^T \mathbf{J}^k_r \delta \mathbf{b}^g_i \Delta t)\right] \notag \&= \Delta\tilde{\mathbf{R}}_{ij}(\bar{\mathbf{b}}_i) \text{Exp}(\sum_{k=i}^{j-1}\left[- \Delta\tilde{\mathbf{R}}_{k+1, j}(\bar{\mathbf{b}}_i^g)^T \mathbf{J}^k_r \Delta t\right] \delta \mathbf{b}^g_i) \notag \\ &= \Delta\tilde{\mathbf{R}}_{ij}(\bar{\mathbf{b}}_i)\text{Exp}\left(\frac{\partial\Delta\bar{\mathbf{R}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i\right) \end{align}\]

上式中 \(\mathbf{J}^k_r = \mathbf{J}_r((\tilde{\pmb{\omega}}_k - \bar{\mathbf{b}}^g_i)\Delta t)\)。

\[\begin{align} \Delta\tilde{\mathbf{v}}_{ij}(\hat{\mathbf{b}}_i) &\doteq \sum_{k=i}^{j-1}\Delta\tilde{\mathbf{R}}_{ik}(\hat{\mathbf{b}}_i)(\tilde{\mathbf{a}}_k-\bar{\mathbf{b}}^a_i-\delta \mathbf{b}^a_i)\Delta t \notag \&= \sum_{k=i}^{j-1}\Delta\tilde{\mathbf{R}}_{ik}(\bar{\mathbf{b}}_i)\text{Exp}\left(\frac{\partial\Delta\bar{\mathbf{R}}_{ik}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i\right)(\tilde{\mathbf{a}}_k-\bar{\mathbf{b}}^a_i-\delta \mathbf{b}^a_i)\Delta t \notag \&= \sum_{k=i}^{j-1}\Delta\tilde{\mathbf{R}}_{ik}(\bar{\mathbf{b}}_i)\left(\mathbf{I} + \left( \frac{\partial\Delta\bar{\mathbf{R}}_{ik}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i \right)^{\wedge}\right)(\tilde{\mathbf{a}}_k-\bar{\mathbf{b}}^a_i-\delta \mathbf{b}^a_i)\Delta t \notag \\ &= \sum_{k=i}^{j-1}\Delta\tilde{\mathbf{R}}_{ik}(\bar{\mathbf{b}}_i)(\tilde{\mathbf{a}}_k-\bar{\mathbf{b}}^a_i)\Delta t + \sum_{k=i}^{j-1}\Delta\tilde{\mathbf{R}}_{ik}(\bar{\mathbf{b}}_i)(-\delta \mathbf{b}^a_i)\Delta t \notag \&+ \sum_{k=i}^{j-1}\Delta\tilde{\mathbf{R}}_{ik}(\bar{\mathbf{b}}_i)\left( \frac{\partial\Delta\bar{\mathbf{R}}_{ik}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i \right)^{\wedge}(\tilde{\mathbf{a}}_k-\bar{\mathbf{b}}^a_i)\Delta t \notag \&= \sum_{k=i}^{j-1}\Delta\tilde{\mathbf{R}}_{ik}(\bar{\mathbf{b}}_i)(\tilde{\mathbf{a}}_k-\bar{\mathbf{b}}^a_i)\Delta t + \sum_{k=i}^{j-1}-\Delta\tilde{\mathbf{R}}_{ik}(\bar{\mathbf{b}}_i)\Delta t\delta \mathbf{b}^a_i \notag \&+ \sum_{k=i}^{j-1}-\Delta\tilde{\mathbf{R}}_{ik}(\bar{\mathbf{b}}_i)(\tilde{\mathbf{a}}_k-\bar{\mathbf{b}}^a_i)^{\wedge} \frac{\partial\Delta\bar{\mathbf{R}}_{ik}}{\partial\mathbf{b}^g}\Delta t\delta\mathbf{b}^g_i \notag \&= \Delta\tilde{\mathbf{v}}_{ij}(\bar{\mathbf{b}}_i) + \frac{\partial\Delta\bar{\mathbf{v}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i + \frac{\partial\Delta\bar{\mathbf{v}}_{ij}}{\partial\mathbf{b}^a}\delta\mathbf{b}^a_i \end{align}\]

\[ \begin{align} \Delta\tilde{\mathbf{p}}_{ij}(\hat{\mathbf{b}}_i) &\doteq \sum_{k=i}^{j-1}\frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}(\hat{\mathbf{b}}_i)(\tilde{\mathbf{a}}_k-\bar{\mathbf{b}}^a_i-\delta \mathbf{b}^a_i)\Delta t^2 \notag \&= \sum_{k=i}^{j-1}\frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}(\bar{\mathbf{b}}_i)\text{Exp}\left(\frac{\partial\Delta\bar{\mathbf{R}}_{ik}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i\right)(\tilde{\mathbf{a}}_k-\bar{\mathbf{b}}^a_i-\delta \mathbf{b}^a_i)\Delta t^2 \notag \&= \sum_{k=i}^{j-1}\frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}(\bar{\mathbf{b}}_i)\left( \mathbf{I} + \left(\frac{\partial\Delta\bar{\mathbf{R}}_{ik}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i\right)^{\wedge} \right)(\tilde{\mathbf{a}}_k-\bar{\mathbf{b}}^a_i-\delta \mathbf{b}^a_i)\Delta t^2 \notag \&= \sum_{k=i}^{j-1}\frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}(\bar{\mathbf{b}}_i)(\tilde{\mathbf{a}}_k-\bar{\mathbf{b}}^a_i)\Delta t^2 + \sum_{k=i}^{j-1}-\frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}(\bar{\mathbf{b}}_i)\Delta t^2 \delta \mathbf{b}^a_i \notag \\ &+ \sum_{k=i}^{j-1}-\frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}(\bar{\mathbf{b}}_i)(\tilde{\mathbf{a}}_k-\bar{\mathbf{b}}^a_i)^{\wedge}\frac{\partial\Delta\bar{\mathbf{R}}_{ik}}{\partial\mathbf{b}^g}\Delta t^2 \delta\mathbf{b}^g_i \notag \&= \Delta\tilde{\mathbf{p}}_{ij}(\bar{\mathbf{b}}^g_i, \bar{\mathbf{b}}^a_i) + \frac{\partial\Delta\bar{\mathbf{p}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i + \frac{\partial\Delta\bar{\mathbf{p}}_{ij}}{\partial\mathbf{b}^a}\delta\mathbf{b}^a_i \end{align} \]

综合以上：

\[\begin{align} \frac{\partial\Delta\bar{\mathbf{R}}_{ij}}{\partial\mathbf{b}^g} &= \sum_{k=i}^{j-1}\left[- \Delta\tilde{\mathbf{R}}_{k+1, j}(\bar{\mathbf{b}}_i^g)^T \mathbf{J}^k_r \Delta t\right] \notag \\ \frac{\partial\Delta\bar{\mathbf{v}}_{ij}}{\partial\mathbf{b}^g} &= \sum_{k=i}^{j-1}-\Delta\tilde{\mathbf{R}}_{ik}(\bar{\mathbf{b}}_i)(\tilde{\mathbf{a}}_k-\bar{\mathbf{b}}^a_i)^{\wedge} \frac{\partial\Delta\bar{\mathbf{R}}_{ik}}{\partial\mathbf{b}^g}\Delta t \notag \\ \frac{\partial\Delta\bar{\mathbf{v}}_{ij}}{\partial\mathbf{b}^a} &= \sum_{k=i}^{j-1}-\Delta\tilde{\mathbf{R}}_{ik}(\bar{\mathbf{b}}_i)\Delta t \notag \\ \frac{\partial\Delta\bar{\mathbf{p}}_{ij}}{\partial\mathbf{b}^g} &= \sum_{k=i}^{j-1}-\frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}(\bar{\mathbf{b}}_i)(\tilde{\mathbf{a}}_k-\bar{\mathbf{b}}^a_i)^{\wedge}\frac{\partial\Delta\bar{\mathbf{R}}_{ik}}{\partial\mathbf{b}^g}\Delta t^2 \notag \\frac{\partial\Delta\bar{\mathbf{p}}_{ij}}{\partial\mathbf{b}^a} &= \sum_{k=i}^{j-1}-\frac{3}{2}\Delta\tilde{\mathbf{R}}_{ik}(\bar{\mathbf{b}}_i)\Delta t^2 \end{align}\]

Jacobians of \(\mathbf{r}_{\Delta\mathbf{R}_{ij}}\)

\[\begin{align} \mathbf{r}_{\Delta\mathbf{R}_{ij}} &\doteq -\delta \pmb{\phi}_{ij} = \text{Log}\left( \left( \Delta\tilde{\mathbf{R}}_{ij}(\bar{\mathbf{b}}^g_i) \text{Exp}\left( \frac{\partial\Delta\bar{\mathbf{R}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i \right) \right)^T \mathbf{R}^T_i\mathbf{R}_j \right) \end{align}\]

误差左乘或右乘，需要注意下面两个公式。这里按照 DSO 代码中误差左乘初始旋转矩阵，得到的结果如下。

\[\begin{align} &\mathbf{r}_{\Delta\mathbf{R}_{ij}} (\text{Exp}(\delta \pmb{\phi}_{i})\mathbf{R}_i) \notag \&= \text{Log}\left( \left( \Delta\tilde{\mathbf{R}}_{ij}(\bar{\mathbf{b}}^g_i) \text{Exp}\left( \frac{\partial\Delta\bar{\mathbf{R}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i \right) \right)^T (\text{Exp}(\delta \pmb{\phi}_{i})\mathbf{R}_i)^T\mathbf{R}_j \right) \notag \&= \text{Log}\left( \left( \Delta\tilde{\mathbf{R}}_{ij}(\bar{\mathbf{b}}^g_i) \text{Exp}\left( \frac{\partial\Delta\bar{\mathbf{R}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i \right) \right)^T \mathbf{R}_i^T \text{Exp}(-\delta \pmb{\phi}_{i}) \mathbf{R}_j \right) \notag \&= \text{Log}\left( \left( \Delta\tilde{\mathbf{R}}_{ij}(\bar{\mathbf{b}}^g_i) \text{Exp}\left( \frac{\partial\Delta\bar{\mathbf{R}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i \right) \right)^T \mathbf{R}_i^T \mathbf{R}_j \mathbf{R}_j^T \text{Exp}(-\delta \pmb{\phi}_{i}) \mathbf{R}_j \right) \notag \\ &= \text{Log}\left( \left( \Delta\tilde{\mathbf{R}}_{ij}(\bar{\mathbf{b}}^g_i) \text{Exp}\left( \frac{\partial\Delta\bar{\mathbf{R}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i \right) \right)^T \mathbf{R}_i^T \mathbf{R}_j \text{Exp}(-\mathbf{R}_j^T\delta \pmb{\phi}_{i}) \right) \notag \&= \text{Log}\left( \left( \Delta\tilde{\mathbf{R}}_{ij}(\bar{\mathbf{b}}^g_i) \text{Exp}\left( \frac{\partial\Delta\bar{\mathbf{R}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i \right) \right)^T \mathbf{R}_i^T \mathbf{R}_j \right) \notag \\ &+ \mathbf{J}^{-1}_r\left(\text{Log}\left( \left( \Delta\tilde{\mathbf{R}}_{ij}(\bar{\mathbf{b}}^g_i) \text{Exp}\left( \frac{\partial\Delta\bar{\mathbf{R}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i \right) \right)^T \mathbf{R}_i^T \mathbf{R}_j \right)\right)(-\mathbf{R}_j^T\delta \pmb{\phi}_{i}) \notag \&= \mathbf{r}_{\Delta\mathbf{R}_{ij}} (\mathbf{R}_i) - \mathbf{J}^{-1}_r (\mathbf{r}_{\Delta\mathbf{R}_{ij}} (\mathbf{R}_i))\mathbf{R}_j^T\delta \pmb{\phi}_{i}\end{align}\]

\[\begin{align} &\mathbf{r}_{\Delta\mathbf{R}_{ij}} (\text{Exp}(\delta \pmb{\phi}_{j})\mathbf{R}_j) \notag \\ &= \text{Log}\left( \left( \Delta\tilde{\mathbf{R}}_{ij}(\bar{\mathbf{b}}^g_i) \text{Exp}\left( \frac{\partial\Delta\bar{\mathbf{R}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i \right) \right)^T \mathbf{R}_i^T \text{Exp}(\delta \pmb{\phi}_{j}) \mathbf{R}_j \right) \notag \\ &= \text{Log}\left( \left( \Delta\tilde{\mathbf{R}}_{ij}(\bar{\mathbf{b}}^g_i) \text{Exp}\left( \frac{\partial\Delta\bar{\mathbf{R}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i \right) \right)^T \mathbf{R}_i^T \mathbf{R}_j \mathbf{R}_j^T \text{Exp}(\delta \pmb{\phi}_{j}) \mathbf{R}_j \right) \notag \\ &= \text{Log}\left( \left( \Delta\tilde{\mathbf{R}}_{ij}(\bar{\mathbf{b}}^g_i) \text{Exp}\left( \frac{\partial\Delta\bar{\mathbf{R}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i \right) \right)^T \mathbf{R}_i^T \mathbf{R}_j \text{Exp}(\mathbf{R}_j^T \delta \pmb{\phi}_{j}) \right) \notag \&= \mathbf{r}_{\Delta\mathbf{R}_{ij}} (\mathbf{R}_j) + \mathbf{J}^{-1}_r(\mathbf{r}_{\Delta\mathbf{R}_{ij}} (\mathbf{R}_j)) \mathbf{R}_j^T \delta \pmb{\phi}_{j} \end{align}\]

Jacobians of \(\mathbf{r}_{\Delta\mathbf{v}_{ij}}\)

\[\begin{align} \mathbf{r}_{\Delta\mathbf{v}_{ij}} &\doteq -\delta \mathbf{v}_{ij} = \mathbf{R}^T_i(\mathbf{v}_j - \mathbf{v}_i - \mathbf{g}\Delta t_{ij}) \notag \ &- \left( \Delta\tilde{\mathbf{v}}_{ij}(\bar{\mathbf{b}}^g_i, \bar{\mathbf{b}}^a_i) + \frac{\partial\Delta\bar{\mathbf{v}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i + \frac{\partial\Delta\bar{\mathbf{v}}_{ij}}{\partial\mathbf{b}^a}\delta\mathbf{b}^a_i \right) \end{align}\]

应当与论文中一致，待填。

Jacobians of \(\mathbf{r}_{\Delta\mathbf{p}_{ij}}\)

\[\begin{align} \mathbf{r}_{\Delta\mathbf{p}_{ij}} &\doteq -\delta \mathbf{p}_{ij} = \mathbf{R}^T_i(\mathbf{p}_j-\mathbf{p}_i-\mathbf{v}\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t^2_{ij}) \notag \ &- \left( \Delta\tilde{\mathbf{p}}_{ij}(\bar{\mathbf{b}}^g_i, \bar{\mathbf{b}}^a_i) + \frac{\partial\Delta\bar{\mathbf{p}}_{ij}}{\partial\mathbf{b}^g}\delta\mathbf{b}^g_i + \frac{\partial\Delta\bar{\mathbf{p}}_{ij}}{\partial\mathbf{b}^a}\delta\mathbf{b}^a_i \right) \end{align}\]

应当与论文中一致，待填。

IMU 预积分推导

标签：进一步   上进   时间   rom   需要   翻译   uri   i+1   方法   

原文地址：https://www.cnblogs.com/JingeTU/p/9048986.html