给 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 观测值，而将重力加速度对它们的影响去除（公式 ( ef{eq2:imu_pre_state_diff_delta_R_ij}) ( ef{eq2:imu_pre_state_diff_delta_v_ij}) ( ef{eq2:imu_pre_state_diff_delta_p_ij})），使得新定义的 IMU 观测值与起点的位置、姿态、速度无关，在优化过程中不会因为起点的这些状态量改变而重新积分。（去除重力加速度的影响，应是重力加速度与当前的姿态相关，只有将其去除才有可能使得 IMU 观测值与起点的姿态无关。）但是定义的 IMU 观测值依旧与起点的偏移值相关，使用 Taylor 展开拟合偏移值在优化过程中的变化，累积到足够大再进行积分。
我写这个东西，就是瞎bb，代码还没有验证。
看这个代码，知道 DSO 是左雅克比。
PRE_worldToCam = SE3::exp(w2c_leftEps()) * get_worldToCam_evalPT();
而 IMU 预积分论文是右雅克比，需要注意在对姿态、位置求导的过程中改变一下。
上面这个代码还有一点是需要注意的，DSO 是“worldToCam”表达位姿，而 IMU 预积分用“camToWorld”表达位置姿态。这个是真的蛋疼。下面对位姿的求导是有错误的，现在我没改过来。参考我的博客《Adjoint of SE(3)》能改过来。就是加一个负号，读者自己试一下。
注意 IMU 预积分论文的公式(6)。( ext{Exp}, ext{Log}) 的含义，这是复合映射。
Residuals
IMU 测量值可以看做真实值、漂移值（Bias）、高斯误差的和：
[egin{align} mathbin{_B ilde{pmb{omega}}_{WB}}(t) = mathbin{_Bpmb{omega}_{WB}}(t) + pmb{b}^g(t) + pmb{eta}^g(t) label{eq2:imu_pre_mes_w} \
mathbin{_B ilde{pmb{a}}}(t) = mathbf{R}_{WB}^T(t)(mathbin{_Wpmb{a}}(t) - mathbin{_Wpmb{g}}) + pmb{b}^a(t) + pmb{eta}^a(t) label{eq2:imu_pre_mes_a} end{align}]
其中下标的 (W, B) 分别表示世界（World）坐标系和 IMU （Body）坐标系，左下标 (mathbin{_Wcdot}, mathbin{_Bcdot}) 表示所在的坐标系，(mathbf{R}_{WB}) 的右下标表示从 IMU 坐标系旋转到世界坐标系，(mathbin{_Bpmb{omega}_{WB}}) 的右下标表示 IMU 坐标系相对于世界坐标系的瞬时角速度，右上标 (cdot^{g}, cdot^{a}) 分别表示陀螺仪（Gyroscope）角速度测量值相关、加速度计（Accelerometer）线加速度测量值相关。公式中的真实值是 (t) 时刻的角速度 (mathbin{_Bpmb{omega}_{WB}}(t))、线加速度 (mathbin{_Wpmb{a}}(t))，漂移值是 (t) 时刻的角速度漂移值 (pmb{b}^g(t))、线加速度漂移值 (pmb{b}^a(t))，高斯误差是 (t) 时刻的角速度测量值误差 (pmb{eta}^g(t))、线加速度测量误差 (pmb{eta}^a(t))。(mathbin{_Wpmb{g}}) 是世界坐标系下的重力加速度。
从 (t) 时刻开始，经过一小段时间 (Delta t) 到达 (t + Delta t) 时刻，使用欧拉方法（Euler Method），即假设在 $[t, t + Delta t] $ 内角速度与线加速度保持不变，得到以下结果：
[egin{align} mathbf{R}_{WB}(t+Delta t) &= mathbf{R}_{WB}(t) ext{Exp}(mathbin{_Bpmb{omega}_{WB}}(t)Delta t) \
mathbin{_Wmathbf{v}}(t+Delta t) &= mathbin{_Wmathbf{v}}(t) + mathbin{_Wmathbf{a}}(t)Delta t \
mathbin{_Wmathbf{p}}(t+Delta t) &= mathbin{_Wmathbf{p}}(t) + mathbin{_Wmathbf{v}}(t) Delta t + frac{1}{2}mathbin{_Wmathbf{a}}(t)Delta t^2 end{align}]
将公式 ( ef{eq2:imu_pre_mes_w}) ( ef{eq2:imu_pre_mes_a})，并忽略表示坐标系的下标，得到：
[egin{align} mathbf{R}(t+Delta t) &= mathbf{R}(t) ext{Exp}(( ilde{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)( ilde{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)( ilde{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) 时间段，按照公式 ( ef{eq2:imu_pre_delta_R}) ( ef{eq2:imu_pre_delta_v}) ( ef{eq2:imu_pre_delta_p})，可以从 (i) 时刻的状态值积分得到 (j) 时刻的状态值：
[egin{align}
mathbf{R}_j &= mathbf{R}_i prod_{k=i}^{j-1} ext{Exp}(( ilde{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( ilde{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}_kDelta 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( ilde{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^Tmathbf{R}_j, mathbf{v}_j - mathbf{v}_i, mathbf{p}_j - mathbf{p}_i)，然而这个太 NAIVE，这么写就不是预积分了。预积分定义的状态量如下：
[egin{align} Delta mathbf{R}_{ij} &doteq mathbf{R}_i^T mathbf{R}_j = prod_{k=i}^{j-1} ext{Exp}(( ilde{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}Deltamathbf{R}_{ik}( ilde{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}_iDelta t_{ij} - frac{1}{2}mathbf{g}Delta t_{ij}^2)
otag \
&= mathbf{R}_i^T(mathbf{p}_i + sum_{k=i}^{j-1}mathbf{v}_kDelta 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( ilde{mathbf{a}}_k - mathbf{b}^a_k - pmb{eta}^{ad}_k)Delta t^2
otag \
&phantom{=} - mathbf{p}_i - mathbf{v}_iDelta t_{ij} - frac{1}{2}mathbf{g}Delta t_{ij}^2)
otag \
&= mathbf{R}_i^T(sum_{k=i}^{j-1}mathbf{v}_kDelta t - mathbf{v}_iDelta t_{ij} + sum_{k=i}^{j-1}frac{1}{2}mathbf{g}Delta t^2 - frac{1}{2}mathbf{g}Delta t_{ij}^2 + frac{1}{2}sum_{k=i}^{j-1}mathbf{R}_k( ilde{mathbf{a}}_k - mathbf{b}^a_k - pmb{eta}^{ad}_k)Delta t^2)
otag \
&= mathbf{R}_i^T(sum_{k=i}^{j-1}(mathbf{v}_k - mathbf{v}_i)Delta t + (j-i)frac{1}{2}mathbf{g}Delta t^2 - (j-i)^2frac{1}{2}mathbf{g}Delta t^2 + frac{1}{2}sum_{k=i}^{j-1}mathbf{R}_k( ilde{mathbf{a}}_k - mathbf{b}^a_k - pmb{eta}^{ad}_k)Delta t^2)
otag \
&= mathbf{R}_i^T(sum_{k=i}^{j-1}(mathbf{v}_k - mathbf{v}_i)Delta t - {(j-i)(j-i+1) over 2}mathbf{g}Delta t^2 + frac{1}{2}sum_{k=i}^{j-1}mathbf{R}_k( ilde{mathbf{a}}_k - mathbf{b}^a_k - pmb{eta}^{ad}_k)Delta t^2)
otag \
&= mathbf{R}_i^T(sum_{k=i}^{j-1}(mathbf{v}_k - mathbf{v}_i)Delta t - (1+2+dots+j-i)mathbf{g}Delta t^2 + frac{1}{2}sum_{k=i}^{j-1}mathbf{R}_k( ilde{mathbf{a}}_k - mathbf{b}^a_k - pmb{eta}^{ad}_k)Delta t^2)
otag \
&= mathbf{R}_i^T(sum_{k=i}^{j-1}(mathbf{v}_k - mathbf{v}_i)Delta t - sum_{k=i}^{j-1}sum_{l=i}^{k}mathbf{g}Delta t^2 + frac{1}{2}sum_{k=i}^{j-1}mathbf{R}_k( ilde{mathbf{a}}_k - mathbf{b}^a_k - pmb{eta}^{ad}_k)Delta t^2)
otag \
&= mathbf{R}_i^T(sum_{k=i}^{j-1}(mathbf{v}_k - mathbf{v}_i - sum_{l=i}^{k}mathbf{g}Delta t)Delta t + frac{1}{2}sum_{k=i}^{j-1}mathbf{R}_k( ilde{mathbf{a}}_k - mathbf{b}^a_k - pmb{eta}^{ad}_k)Delta t^2)
otag \
&= mathbf{R}_i^T(sum_{k=i}^{j-1}(mathbf{v}_k - mathbf{v}_i - sum_{l=i}^{k-1}mathbf{g}Delta t - mathbf{g}Delta t)Delta t + frac{1}{2}sum_{k=i}^{j-1}mathbf{R}_k( ilde{mathbf{a}}_k - mathbf{b}^a_k - pmb{eta}^{ad}_k)Delta t^2)
otag \
&simeq (sum_{k=i}^{j-1}mathbf{R}_i^T(mathbf{v}_k - mathbf{v}_i - mathbf{g}Delta t_{ik})Delta t + frac{1}{2}sum_{k=i}^{j-1}mathbf{R}_i^Tmathbf{R}_k( ilde{mathbf{a}}_k - mathbf{b}^a_k - pmb{eta}^{ad}_k)Delta t^2)
otag \
&= sum_{k=i}^{j-1}(Delta mathbf{v}_{ik}Delta t + frac{1}{2}Deltamathbf{R}_{ik}( ilde{mathbf{a}}_k - mathbf{b}^a_k - pmb{eta}^{ad}_k)Delta t^2)
otag \
&= sum_{k=i}^{j-1}frac{3}{2}Deltamathbf{R}_{ik}( ilde{mathbf{a}}_k - mathbf{b}^a_k - pmb{eta}^{ad}_k)Delta t^2 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-1 $。因为 Bias 也是需要估计出来的，这种假设减少大量需要估计的状态量。此处假设形成的近似，限制了 IMU 预积分的时间间隔，在应用的过程中应该注意到这一点。
下一步是将高斯误差与测量值、漂移值分隔开，以方便估计高斯误差。由 BCH 公式可得（这个地方还是值得推导一下的）：
[egin{align} Deltamathbf{R}_{ij} &simeq prod_{k=i}^{j-1} left[ ext{Exp}(( ilde{pmb{omega}}_k - mathbf{b}^g_i)Delta t) ext{Exp}(-mathbf{J}^k_rpmb{eta}^{gd}_kDelta t)
ight]
otag \
&= ext{Exp}(( ilde{pmb{omega}}_i - mathbf{b}^g_i)Delta t) ext{Exp}(-mathbf{J}^i_rpmb{eta}^{gd}_iDelta t) ext{Exp}(( ilde{pmb{omega}}_{i+1} - mathbf{b}^g_i)Delta t) ext{Exp}(-mathbf{J}^{i+1}_rpmb{eta}^{gd}_{i+1}Delta t) dots
otag \
&phantom{=} ext{Exp}(( ilde{pmb{omega}}_{j-1} - mathbf{b}^g_i)Delta t) ext{Exp}(-mathbf{J}^{j-1}_rpmb{eta}^{gd}_{j-1}Delta t)
otag \
&= ext{Exp}(( ilde{pmb{omega}}_i - mathbf{b}^g_i)Delta t) ext{Exp}(( ilde{pmb{omega}}_{i+1} - mathbf{b}^g_i)Delta t) ext{Exp}(- ext{Exp}(( ilde{pmb{omega}}_{i+1} - mathbf{b}^g_i)Delta t)^Tmathbf{J}^i_rpmb{eta}^{gd}_iDelta t)
otag \
&phantom{=} ext{Exp}(-mathbf{J}^{i+1}_rpmb{eta}^{gd}_{i+1}Delta t) ext{Exp}(( ilde{pmb{omega}}_{i+2} - mathbf{b}^g_i)Delta t) ext{Exp}(-mathbf{J}^{i+2}_rpmb{eta}^{gd}_{i+2}Delta t) dots
otag \
&phantom{=} ext{Exp}(( ilde{pmb{omega}}_{j-1} - mathbf{b}^g_i)Delta t) ext{Exp}(-mathbf{J}^{j-1}_rpmb{eta}^{gd}_{j-1}Delta t)
otag \
&= ext{Exp}(( ilde{pmb{omega}}_i - mathbf{b}^g_i)Delta t) ext{Exp}(( ilde{pmb{omega}}_{i+1} - mathbf{b}^g_i)Delta t) ext{Exp}(- ext{Exp}(( ilde{pmb{omega}}_{i+1} - mathbf{b}^g_i)Delta t)^Tmathbf{J}^i_rpmb{eta}^{gd}_iDelta t)
otag \
&phantom{=} ext{Exp}(( ilde{pmb{omega}}_{i+2} - mathbf{b}^g_i)Delta t) ext{Exp}(- ext{Exp}(( ilde{pmb{omega}}_{i+2} - mathbf{b}^g_i)Delta t)^Tmathbf{J}^{i+1}_rpmb{eta}^{gd}_{i+1}Delta t) ext{Exp}(-mathbf{J}^{i+2}_rpmb{eta}^{gd}_{i+2}Delta t) dots
otag \
&phantom{=} ext{Exp}(( ilde{pmb{omega}}_{j-1} - mathbf{b}^g_i)Delta t) ext{Exp}(-mathbf{J}^{j-1}_rpmb{eta}^{gd}_{j-1}Delta t)
otag \
&= ext{Exp}(( ilde{pmb{omega}}_i - mathbf{b}^g_i)Delta t) ext{Exp}(( ilde{pmb{omega}}_{i+1} - mathbf{b}^g_i)Delta t) ext{Exp}(( ilde{pmb{omega}}_{i+2} - mathbf{b}^g_i)Delta t)
otag \
&phantom{=} ext{Exp}(- ext{Exp}(( ilde{pmb{omega}}_{i+2} - mathbf{b}^g_i)Delta t)^T ext{Exp}(( ilde{pmb{omega}}_{i+1} - mathbf{b}^g_i)Delta t)^Tmathbf{J}^i_rpmb{eta}^{gd}_iDelta t)
otag \
&phantom{=} ext{Exp}(- ext{Exp}(( ilde{pmb{omega}}_{i+2} - mathbf{b}^g_i)Delta t)^Tmathbf{J}^{i+1}_rpmb{eta}^{gd}_{i+1}Delta t) ext{Exp}(-mathbf{J}^{i+2}_rpmb{eta}^{gd}_{i+2}Delta t) dots
otag \
&phantom{=} ext{Exp}(( ilde{pmb{omega}}_{j-1} - mathbf{b}^g_i)Delta t) ext{Exp}(-mathbf{J}^{j-1}_rpmb{eta}^{gd}_{j-1}Delta t)
otag \
&= ext{Exp}(( ilde{pmb{omega}}_i - mathbf{b}^g_i)Delta t) ext{Exp}(( ilde{pmb{omega}}_{i+1} - mathbf{b}^g_i)Delta t) dots ext{Exp}(( ilde{pmb{omega}}_{j-1} - mathbf{b}^g_i)Delta t)
otag \
&phantom{=} ext{Exp}(- ext{Exp}(( ilde{pmb{omega}}_{j-1} - mathbf{b}^g_i)Delta t)^T dots ext{Exp}(( ilde{pmb{omega}}_{i+1} - mathbf{b}^g_i)Delta t)^Tmathbf{J}^i_rpmb{eta}^{gd}_iDelta t)
otag \
&phantom{=} ext{Exp}(- ext{Exp}(( ilde{pmb{omega}}_{j-1} - mathbf{b}^g_i)Delta t)^T dots ext{Exp}(( ilde{pmb{omega}}_{i+2} - mathbf{b}^g_i)Delta t)^Tmathbf{J}^{i+1}_rpmb{eta}^{gd}_{i+1}Delta t)
otag \
&phantom{=} ext{Exp}(- ext{Exp}(( ilde{pmb{omega}}_{j-1} - mathbf{b}^g_i)Delta t)^T dots ext{Exp}(( ilde{pmb{omega}}_{i+3} - mathbf{b}^g_i)Delta t)^Tmathbf{J}^{i+2}_rpmb{eta}^{gd}_{i+2}Delta t)
otag \
&phantom{=} dots
otag \
&phantom{=} ext{Exp}(- ext{Exp}(( ilde{pmb{omega}}_{j-1} - mathbf{b}^g_i)Delta t)^T ext{Exp}(( ilde{pmb{omega}}_{j-2} - mathbf{b}^g_i)Delta t)^Tmathbf{J}^{j-3}_rpmb{eta}^{gd}_{j-3}Delta t)
otag \
&phantom{=} ext{Exp}(- ext{Exp}(( ilde{pmb{omega}}_{j-1} - mathbf{b}^g_i)Delta t)^Tmathbf{J}^{j-2}_rpmb{eta}^{gd}_{j-2}Delta t)
otag \
&phantom{=} ext{Exp}(-mathbf{J}^{j-1}_rpmb{eta}^{gd}_{j-1}Delta t)
otag end{align}]
[egin{align}
&= ext{Exp}(( ilde{pmb{omega}}_i - mathbf{b}^g_i)Delta t) ext{Exp}(( ilde{pmb{omega}}_{i+1} - mathbf{b}^g_i)Delta t) dots ext{Exp}(( ilde{pmb{omega}}_{j-1} - mathbf{b}^g_i)Delta t)
otag \
&phantom{=} ext{Exp}(-( ext{Exp}(( ilde{pmb{omega}}_{i+1} - mathbf{b}^g_i)Delta t) dots ext{Exp}(( ilde{pmb{omega}}_{j-1} - mathbf{b}^g_i)Delta t))^Tmathbf{J}^i_rpmb{eta}^{gd}_iDelta t)
otag \
&phantom{=} ext{Exp}(-( ext{Exp}(( ilde{pmb{omega}}_{i+2} - mathbf{b}^g_i)Delta t) dots ext{Exp}(( ilde{pmb{omega}}_{j-1} - mathbf{b}^g_i)Delta t))^Tmathbf{J}^{i+1}_rpmb{eta}^{gd}_{i+1}Delta t)
otag \
&phantom{=} ext{Exp}(-( ext{Exp}(( ilde{pmb{omega}}_{i+3} - mathbf{b}^g_i)Delta t) dots ext{Exp}(( ilde{pmb{omega}}_{j-1} - mathbf{b}^g_i)Delta t))^Tmathbf{J}^{i+2}_rpmb{eta}^{gd}_{i+2}Delta t)
otag \
&phantom{=} dots
otag \
&phantom{=} ext{Exp}(-( ext{Exp}(( ilde{pmb{omega}}_{j-2} - mathbf{b}^g_i)Delta t) dots ext{Exp}(( ilde{pmb{omega}}_{j-1} - mathbf{b}^g_i)Delta t))^Tmathbf{J}^{j-3}_rpmb{eta}^{gd}_{j-3}Delta t)
otag \
&phantom{=} ext{Exp}(- ext{Exp}(( ilde{pmb{omega}}_{j-1} - mathbf{b}^g_i)Delta t)^Tmathbf{J}^{j-2}_rpmb{eta}^{gd}_{j-2}Delta t)
otag \
&phantom{=} ext{Exp}(-mathbf{J}^{j-1}_rpmb{eta}^{gd}_{j-1}Delta t)
otag \
&= Delta ilde{mathbf{R}}_{ij}
otag \
&phantom{=} ext{Exp}(-Delta ilde{mathbf{R}}_{i+1,j}^Tmathbf{J}^i_rpmb{eta}^{gd}_iDelta t)
otag \
&phantom{=} ext{Exp}(-Delta ilde{mathbf{R}}_{i+2,j}^Tmathbf{J}^{i+1}_rpmb{eta}^{gd}_{i+1}Delta t)
otag \
&phantom{=} ext{Exp}(-Delta ilde{mathbf{R}}_{i+3,j}^Tmathbf{J}^{i+2}_rpmb{eta}^{gd}_{i+2}Delta t)
otag \
&phantom{=} dots
otag \
&phantom{=} ext{Exp}(-Delta ilde{mathbf{R}}_{j-2,j}^Tmathbf{J}^{j-3}_rpmb{eta}^{gd}_{j-3}Delta t)
otag \
&phantom{=} ext{Exp}(-Delta ilde{mathbf{R}}_{j-1,j}^Tmathbf{J}^{j-2}_rpmb{eta}^{gd}_{j-2}Delta t)
otag \
&phantom{=} ext{Exp}(-Delta ilde{mathbf{R}}_{j,j}^Tmathbf{J}^{j-1}_rpmb{eta}^{gd}_{j-1}Delta t)
otag \
&= Delta ilde{mathbf{R}}_{ij} prod_{k=i}^{j-1} ext{Exp}(-Delta ilde{mathbf{R}}^T_{k+1, j}mathbf{J}^k_rpmb{eta}^{gd}_kDelta t)
otag \
&doteq Delta ilde{mathbf{R}}_{ij} ext{Exp}(-delta pmb{phi}_{ij}) label{eq2:imu_pre_mes_state_diff_delta_R_ij} end{align}]
其中 (mathbf{J}^k_rdoteqmathbf{J}^k_r( ilde{pmb{omega}}_k - mathbf{b}^g_i)) 是 (mathfrak{so}(3)) 的右雅可比。(Delta ilde{mathbf{R}}_{ij} doteq prod_{k=i}^{j-1} ext{Exp}(( ilde{pmb{omega}}_k - mathbf{b}^g_i)Delta t)) 是 IMU 预积分的虚拟旋转测量值，( ext{Exp}(-delta pmb{phi}_{ij})) 是对应的高斯误差。由此，可以进一步得到 IMU 预积分的虚拟速度和虚拟位移与它们的测量值、误差之间的关系：
[egin{align} Deltamathbf{v}_{ij} &simeq sum_{k=i}^{j-1} (Delta ilde{mathbf{R}}_{ik}(mathbf{I}-deltapmb{phi}^{wedge}_{ik})( ilde{mathbf{a}}_k-mathbf{b}^a_i)Delta t - Delta ilde{mathbf{R}}_{ik}pmb{eta}^{ad}_kDelta t)
otag \
&= Delta ilde{mathbf{v}}_{ij} + sum_{k=i}^{j-1} left[Delta ilde{mathbf{R}}_{ik}( ilde{mathbf{a}}_k-mathbf{b}^a_i)^{wedge}deltapmb{phi}_{ik}Delta t - Delta ilde{mathbf{R}}_{ik}pmb{eta}^{ad}_kDelta t
ight]
otag \
&doteq Delta ilde{mathbf{v}}_{ij} - deltamathbf{v}_{ij} label{eq2:imu_pre_mes_state_diff_delta_v_ij} \
Deltamathbf{p}_{ij} &simeq sum_{k=i}^{j-1}frac{3}{2}Delta ilde{mathbf{R}}_{ik}(mathbf{I}-deltapmb{phi}^{wedge}_{ik})( ilde{mathbf{a}}_k-mathbf{b}^a_i)Delta t^2 - sum_{k=i}^{j-1}frac{3}{2}Delta ilde{mathbf{R}}_{ik}pmb{eta}^{ad}_kDelta t^2
otag \
&= Delta ilde{mathbf{p}}_{ij} + sum_{k=i}^{j-1}left[frac{3}{2}Delta ilde{mathbf{R}}_{ik}( ilde{mathbf{a}}_k-mathbf{b}^a_i)^{wedge}deltapmb{phi}_{ik}Delta t^2 - frac{3}{2}Delta ilde{mathbf{R}}_{ik}pmb{eta}^{ad}_kDelta t^2
ight]
otag \
&doteq Delta ilde{mathbf{p}}_{ij} - deltamathbf{p}_{ij} label{eq2:imu_pre_mes_state_diff_delta_p_ij} end{align}]
整理一下虚拟测量值与其误差:
[egin{align} Delta ilde{mathbf{R}}_{ij} &doteq prod_{k=i}^{j-1} ext{Exp}(( ilde{pmb{omega}}_k - mathbf{b}^g_i)Delta t) \
ext{Exp}(-delta pmb{phi}_{ij}) &doteq prod_{k=i}^{j-1} ext{Exp}(-Delta ilde{mathbf{R}}^T_{k+1, j}mathbf{J}^k_rpmb{eta}^{gd}_kDelta t) label{eq2:imu_pre_mes_delta_phi_ij} \
Delta ilde{mathbf{v}}_{ij} &doteq sum_{k=i}^{j-1}Delta ilde{mathbf{R}}_{ik}( ilde{mathbf{a}}_k-mathbf{b}^a_i)Delta t \
-delta mathbf{v}_{ij} &doteq sum_{k=i}^{j-1} left[Delta ilde{mathbf{R}}_{ik}( ilde{mathbf{a}}_k-mathbf{b}^a_i)^{wedge}deltapmb{phi}_{ik}Delta t - Delta ilde{mathbf{R}}_{ik}pmb{eta}^{ad}_kDelta t
ight] label{eq2:imu_pre_mes_delta_v_ij} \
Delta ilde{mathbf{p}}_{ij} &doteq sum_{k=i}^{j-1}frac{3}{2}Delta ilde{mathbf{R}}_{ik}( ilde{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 ilde{mathbf{R}}_{ik}( ilde{mathbf{a}}_k-mathbf{b}^a_i)^{wedge}deltapmb{phi}_{ik}Delta t^2 - frac{3}{2}Delta ilde{mathbf{R}}_{ik}pmb{eta}^{ad}_kDelta t^2
ight] label{eq2:imu_pre_mes_delta_p_ij} \
end{align} ]
将公式 ( ef{eq2:imu_pre_mes_state_diff_delta_R_ij}) ( ef{eq2:imu_pre_mes_state_diff_delta_v_ij}) ( ef{eq2:imu_pre_mes_state_diff_delta_p_ij})分别代入公式 ( ef{eq2:imu_pre_state_diff_delta_R_ij}) ( ef{eq2:imu_pre_state_diff_delta_v_ij}) ( ef{eq2:imu_pre_state_diff_delta_p_ij})，得到虚拟状态量测量值与 (i, j) 时刻状态量的联系:
[egin{align} Delta ilde{mathbf{R}}_{ij} &= mathbf{R}^T_imathbf{R}_j ext{Exp}(deltapmb{phi}_{ij}) \
Delta ilde{mathbf{v}}_{ij} &= mathbf{R}^T_i(mathbf{v}_j-mathbf{v}_i-mathbf{g}Delta t_{ij}) + deltamathbf{v}_{ij} \
Delta ilde{mathbf{p}}_{ij} &= mathbf{R}^T_i(mathbf{p}_j-mathbf{p}_i-mathbf{v}_iDelta 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 ilde{mathbf{R}}_{ij}, Delta ilde{mathbf{v}}_{ij}, Delta ilde{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 预积分使用泰勒展开进行近似。如果漂移值的变化量过大，则需要重新积分。在漂移值处的泰勒展开形式如下：
[egin{align} Delta ilde{mathbf{R}}_{ij}(mathbf{b}^g_i) &simeq Delta ilde{mathbf{R}}_{ij}(ar{mathbf{b}}^g_i) ext{Exp}left(frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i
ight) label{eq2:imu_pre_mes_update_bias_R} \
Delta ilde{mathbf{v}}_{ij}(mathbf{b}^g_i, mathbf{b}^a_i) &simeq Delta ilde{mathbf{v}}_{ij}(ar{mathbf{b}}^g_i, ar{mathbf{b}}^a_i) + frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i + frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^a}deltamathbf{b}^a_i label{eq2:imu_pre_mes_update_bias_v} \
Delta ilde{mathbf{p}}_{ij}(mathbf{b}^g_i, mathbf{b}^a_i) &simeq Delta ilde{mathbf{p}}_{ij}(ar{mathbf{b}}^g_i, ar{mathbf{b}}^a_i) + frac{partialDeltaar{mathbf{p}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i + frac{partialDeltaar{mathbf{p}}_{ij}}{partialmathbf{b}^a}deltamathbf{b}^a_i label{eq2:imu_pre_mes_update_bias_p} end{align}]
得到虚拟测量值的误差计算方法：
[egin{align} mathbf{r}_{Deltamathbf{R}_{ij}} &doteq -delta pmb{phi}_{ij} = ext{Log}left( left( Delta ilde{mathbf{R}}_{ij}(ar{mathbf{b}}^g_i) ext{Exp}left( frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i
ight)
ight)^T mathbf{R}^T_imathbf{R}_j
ight) \
mathbf{r}_{Deltamathbf{v}_{ij}} &doteq -delta mathbf{v}_{ij} = mathbf{R}^T_i(mathbf{v}_j - mathbf{v}_i - mathbf{g}Delta t_{ij})
otag \
&- left( Delta ilde{mathbf{v}}_{ij}(ar{mathbf{b}}^g_i, ar{mathbf{b}}^a_i) + frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i + frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^a}deltamathbf{b}^a_i
ight) \
mathbf{r}_{Deltamathbf{p}_{ij}} &doteq -delta mathbf{p}_{ij} = mathbf{R}^T_i(mathbf{p}_j-mathbf{p}_i-mathbf{v}_iDelta t_{ij}-frac{1}{2}mathbf{g}Delta t^2_{ij})
otag \
&- left( Delta ilde{mathbf{p}}_{ij}(ar{mathbf{b}}^g_i, ar{mathbf{b}}^a_i) + frac{partialDeltaar{mathbf{p}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i + frac{partialDeltaar{mathbf{p}}_{ij}}{partialmathbf{b}^a}deltamathbf{b}^a_i
ight) end{align}]
Covariances
从 (i) 时刻到 (j) 时刻的积分使用了 (j - i) 组 IMU 测量值，这些测量值具有游走误差。这些误差的 covariance 需要累积，得到 IMU 预积分虚拟测量值的 covariance。
由公式 ( ef{eq2:imu_pre_mes_delta_phi_ij}) ( ef{eq2:imu_pre_mes_delta_v_ij}) ( ef{eq2:imu_pre_mes_delta_p_ij}) 可得：
[egin{align} delta pmb{phi}_{ij} &= sum_{k=i}^{j-1} Delta ilde{mathbf{R}}^T_{k+1, j}mathbf{J}^k_rpmb{eta}^{gd}_kDelta t \
delta mathbf{v}_{ij} &= sum_{k=i}^{j-1} left[-Delta ilde{mathbf{R}}_{ik}( ilde{mathbf{a}}_k-mathbf{b}^a_i)^{wedge}deltapmb{phi}_{ik}Delta t + Delta ilde{mathbf{R}}_{ik}pmb{eta}^{ad}_kDelta t
ight] \
delta mathbf{p}_{ij} &doteq sum_{k=i}^{j-1}left[-frac{3}{2}Delta ilde{mathbf{R}}_{ik}( ilde{mathbf{a}}_k-mathbf{b}^a_i)^{wedge}deltapmb{phi}_{ik}Delta t^2 + frac{3}{2}Delta ilde{mathbf{R}}_{ik}pmb{eta}^{ad}_kDelta t^2
ight] 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}) 的推导结果与文章不同，确实无法认同文章。）
[egin{align} delta pmb{phi}_{ij} &= sum_{k=i}^{j-1} Delta ilde{mathbf{R}}^T_{k+1, j}mathbf{J}^k_rpmb{eta}^{gd}_kDelta t
otag \ &= sum_{k=i}^{j-2} Delta ilde{mathbf{R}}^T_{k+1, j}mathbf{J}^k_rpmb{eta}^{gd}_kDelta t + Delta ilde{mathbf{R}}^T_{jj}mathbf{J}^{j-1}_rpmb{eta}^{gd}_{j-1}Delta t
otag \ &= sum_{k=i}^{j-2} (Delta ilde{mathbf{R}}_{k+1, j-1}Delta ilde{mathbf{R}}_{j-1, j})^Tmathbf{J}^k_rpmb{eta}^{gd}_kDelta t + mathbf{J}^{j-1}_rpmb{eta}^{gd}_{j-1}Delta t
otag \ &= Delta ilde{mathbf{R}}_{j-1, j}^Tsum_{k=i}^{j-2} Delta ilde{mathbf{R}}_{k+1, j-1}^Tmathbf{J}^k_rpmb{eta}^{gd}_kDelta t + mathbf{J}^{j-1}_rpmb{eta}^{gd}_{j-1}Delta t
otag \ &= Delta ilde{mathbf{R}}_{j-1, j}^T delta pmb{phi}_{i,j-1} + mathbf{J}^{j-1}_rpmb{eta}^{gd}_{j-1}Delta t \
delta mathbf{v}_{ij} &= sum_{k=i}^{j-1} left[-Delta ilde{mathbf{R}}_{ik}( ilde{mathbf{a}}_k-mathbf{b}^a_i)^{wedge}deltapmb{phi}_{ik}Delta t + Delta ilde{mathbf{R}}_{ik}pmb{eta}^{ad}_kDelta t
ight]
otag \ &= sum_{k=i}^{j-2} left[-Delta ilde{mathbf{R}}_{ik}( ilde{mathbf{a}}_k-mathbf{b}^a_i)^{wedge}deltapmb{phi}_{ik}Delta t + Delta ilde{mathbf{R}}_{ik}pmb{eta}^{ad}_kDelta t
ight]
otag \ &-Delta ilde{mathbf{R}}_{i,j-1}( ilde{mathbf{a}}_{j-1}-mathbf{b}^a_i)^{wedge}deltapmb{phi}_{i,j-1}Delta t + Delta ilde{mathbf{R}}_{i,j-1}pmb{eta}^{ad}_{j-1}Delta t
otag \ &= delta mathbf{v}_{i,j-1} -Delta ilde{mathbf{R}}_{i,j-1}( ilde{mathbf{a}}_{j-1}-mathbf{b}^a_i)^{wedge}deltapmb{phi}_{i,j-1}Delta t + Delta ilde{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 ilde{mathbf{R}}_{ik}( ilde{mathbf{a}}_k-mathbf{b}^a_i)^{wedge}deltapmb{phi}_{ik}Delta t^2 + frac{3}{2}Delta ilde{mathbf{R}}_{ik}pmb{eta}^{ad}_kDelta t^2
ight]
otag \ &= sum_{k=i}^{j-2}left[-frac{3}{2}Delta ilde{mathbf{R}}_{ik}( ilde{mathbf{a}}_k-mathbf{b}^a_i)^{wedge}deltapmb{phi}_{ik}Delta t^2 + frac{3}{2}Delta ilde{mathbf{R}}_{ik}pmb{eta}^{ad}_kDelta t^2
ight]
otag \ &-frac{3}{2}Delta ilde{mathbf{R}}_{i,j-1}( ilde{mathbf{a}}_{j-1}-mathbf{b}^a_i)^{wedge}deltapmb{phi}_{i,j-1}Delta t^2 + frac{3}{2}Delta ilde{mathbf{R}}_{i,j-1}pmb{eta}^{ad}_{j-1}Delta t^2
otag \ &= delta mathbf{p}_{i,j-1} -frac{3}{2}Delta ilde{mathbf{R}}_{i,j-1}( ilde{mathbf{a}}_{j-1}-mathbf{b}^a_i)^{wedge}deltapmb{phi}_{i,j-1}Delta t^2 + frac{3}{2}Delta ilde{mathbf{R}}_{i,j-1}pmb{eta}^{ad}_{j-1}Delta t^2end{align}]
于是有：
[egin{align} delta pmb{phi}_{i,k+1} &= Delta ilde{mathbf{R}}_{k,k+1}^T delta pmb{phi}_{ik} + mathbf{J}^{k}_rpmb{eta}^{gd}_{k}Delta t \
delta mathbf{v}_{i,k+1} &= delta mathbf{v}_{i,k} -Delta ilde{mathbf{R}}_{i,k}( ilde{mathbf{a}}_{k}-mathbf{b}^a_i)^{wedge}deltapmb{phi}_{i,k}Delta t + Delta ilde{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 ilde{mathbf{R}}_{i,k}( ilde{mathbf{a}}_{k}-mathbf{b}^a_i)^{wedge}deltapmb{phi}_{i,k}Delta t^2 + frac{3}{2}Delta ilde{mathbf{R}}_{i,k}pmb{eta}^{ad}_{k}Delta t^2 end{align}]
写成矩阵形式：
[egin{align} egin{bmatrix} delta pmb{phi}_{i,k+1} \ delta mathbf{v}_{i,k+1} \ delta mathbf{p}_{i,k+1} end{bmatrix} &= egin{bmatrix} Delta ilde{mathbf{R}}_{k,k+1}^T & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} \ -Delta ilde{mathbf{R}}_{i,k}( ilde{mathbf{a}}_{k}-mathbf{b}^a_i)^{wedge}Delta t & mathbf{I}_{3 imes3} & mathbf{0}_{3 imes3} \ -frac{3}{2}Delta ilde{mathbf{R}}_{i,k}( ilde{mathbf{a}}_{k}-mathbf{b}^a_i)^{wedge}Delta t^2 & mathbf{0}_{3 imes3} & mathbf{I}_{3 imes3} end{bmatrix} egin{bmatrix} delta pmb{phi}_{i,k} \ delta mathbf{v}_{i,k} \ delta mathbf{p}_{i,k} end{bmatrix}
otag \ &+ egin{bmatrix} mathbf{J}^{k}_r Delta t & mathbf{0}_{3 imes3} \ mathbf{0}_{3 imes3} & Delta ilde{mathbf{R}}_{i,k} Delta t \ mathbf{0}_{3 imes3} & frac{3}{2}Delta ilde{mathbf{R}}_{i,k} Delta t^2 end{bmatrix} egin{bmatrix} pmb{eta}^{gd}_{k} \ pmb{eta}^{ad}_{k} end{bmatrix} end{align}
]
令
[egin{align} mathbf{A} &= egin{bmatrix} Delta ilde{mathbf{R}}_{k,k+1}^T & mathbf{0}_{3 imes3} & mathbf{0}_{3 imes3} \ -Delta ilde{mathbf{R}}_{i,k}( ilde{mathbf{a}}_{k}-mathbf{b}^a_i)^{wedge}Delta t & mathbf{I}_{3 imes3} & mathbf{0}_{3 imes3} \ -frac{3}{2}Delta ilde{mathbf{R}}_{i,k}( ilde{mathbf{a}}_{k}-mathbf{b}^a_i)^{wedge}Delta t^2 & mathbf{0}_{3 imes3} & mathbf{I}_{3 imes3} end{bmatrix} \
mathbf{B} &= egin{bmatrix} mathbf{J}^{k}_r Delta t & mathbf{0}_{3 imes3} \ mathbf{0}_{3 imes3} & Delta ilde{mathbf{R}}_{i,k} Delta t \ mathbf{0}_{3 imes3} & frac{3}{2}Delta ilde{mathbf{R}}_{i,k} Delta t^2 end{bmatrix} end{align}]
covariance propagation 的一步：
[egin{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
计算公式 ( ef{eq2:imu_pre_mes_update_bias_R}) ( ef{eq2:imu_pre_mes_update_bias_v}) ( ef{eq2:imu_pre_mes_update_bias_p}) 中出现的 Jacobians，即 (frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g}, frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^g}, frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^a}, frac{partialDeltaar{mathbf{p}}_{ij}}{partialmathbf{b}^g}, frac{partialDeltaar{mathbf{p}}_{ij}}{partialmathbf{b}^a})。
更新 Bias ：(hat{mathbf{b}}_i leftarrow ar{mathbf{b}}_i + delta mathbf{b}_i)。
[egin{align} Delta ilde{mathbf{R}}_{ij}(hat{mathbf{b}}_i) &doteq prod_{k=i}^{j-1} ext{Exp}(( ilde{pmb{omega}}_k - ar{mathbf{b}}^g_i - delta mathbf{b}^g_i)Delta t)
otag \
&= prod_{k=i}^{j-1}left[ ext{Exp}(( ilde{pmb{omega}}_k - ar{mathbf{b}}^g_i)Delta t) ext{Exp}(- mathbf{J}^k_r delta mathbf{b}^g_i Delta t)
ight]
otag \
&= prod_{k=i}^{j-1}left[ ext{Exp}(- Delta ilde{mathbf{R}}_{k+1, j}(ar{mathbf{b}}_i^g)^T mathbf{J}^k_r delta mathbf{b}^g_i Delta t)
ight]
otag \
&= Delta ilde{mathbf{R}}_{ij}(ar{mathbf{b}}_i) ext{Exp}(sum_{k=i}^{j-1}left[- Delta ilde{mathbf{R}}_{k+1, j}(ar{mathbf{b}}_i^g)^T mathbf{J}^k_r Delta t
ight] delta mathbf{b}^g_i)
otag \
&= Delta ilde{mathbf{R}}_{ij}(ar{mathbf{b}}_i) ext{Exp}left(frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i
ight) end{align}]
上式中 (mathbf{J}^k_r = mathbf{J}_r(( ilde{pmb{omega}}_k - ar{mathbf{b}}^g_i)Delta t))。
[egin{align} Delta ilde{mathbf{v}}_{ij}(hat{mathbf{b}}_i) &doteq sum_{k=i}^{j-1}Delta ilde{mathbf{R}}_{ik}(hat{mathbf{b}}_i)( ilde{mathbf{a}}_k-ar{mathbf{b}}^a_i-delta mathbf{b}^a_i)Delta t
otag \
&= sum_{k=i}^{j-1}Delta ilde{mathbf{R}}_{ik}(ar{mathbf{b}}_i) ext{Exp}left(frac{partialDeltaar{mathbf{R}}_{ik}}{partialmathbf{b}^g}deltamathbf{b}^g_i
ight)( ilde{mathbf{a}}_k-ar{mathbf{b}}^a_i-delta mathbf{b}^a_i)Delta t
otag \
&= sum_{k=i}^{j-1}Delta ilde{mathbf{R}}_{ik}(ar{mathbf{b}}_i)left(mathbf{I} + left( frac{partialDeltaar{mathbf{R}}_{ik}}{partialmathbf{b}^g}deltamathbf{b}^g_i
ight)^{wedge}
ight)( ilde{mathbf{a}}_k-ar{mathbf{b}}^a_i-delta mathbf{b}^a_i)Delta t
otag \
&= sum_{k=i}^{j-1}Delta ilde{mathbf{R}}_{ik}(ar{mathbf{b}}_i)( ilde{mathbf{a}}_k-ar{mathbf{b}}^a_i)Delta t + sum_{k=i}^{j-1}Delta ilde{mathbf{R}}_{ik}(ar{mathbf{b}}_i)(-delta mathbf{b}^a_i)Delta t
otag \
&+ sum_{k=i}^{j-1}Delta ilde{mathbf{R}}_{ik}(ar{mathbf{b}}_i)left( frac{partialDeltaar{mathbf{R}}_{ik}}{partialmathbf{b}^g}deltamathbf{b}^g_i
ight)^{wedge}( ilde{mathbf{a}}_k-ar{mathbf{b}}^a_i)Delta t
otag \
&= sum_{k=i}^{j-1}Delta ilde{mathbf{R}}_{ik}(ar{mathbf{b}}_i)( ilde{mathbf{a}}_k-ar{mathbf{b}}^a_i)Delta t + sum_{k=i}^{j-1}-Delta ilde{mathbf{R}}_{ik}(ar{mathbf{b}}_i)Delta tdelta mathbf{b}^a_i
otag \
&+ sum_{k=i}^{j-1}-Delta ilde{mathbf{R}}_{ik}(ar{mathbf{b}}_i)( ilde{mathbf{a}}_k-ar{mathbf{b}}^a_i)^{wedge} frac{partialDeltaar{mathbf{R}}_{ik}}{partialmathbf{b}^g}Delta tdeltamathbf{b}^g_i
otag \
&= Delta ilde{mathbf{v}}_{ij}(ar{mathbf{b}}_i) + frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i + frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^a}deltamathbf{b}^a_i end{align}]
[ egin{align} Delta ilde{mathbf{p}}_{ij}(hat{mathbf{b}}_i) &doteq sum_{k=i}^{j-1}frac{3}{2}Delta ilde{mathbf{R}}_{ik}(hat{mathbf{b}}_i)( ilde{mathbf{a}}_k-ar{mathbf{b}}^a_i-delta mathbf{b}^a_i)Delta t^2
otag \
&= sum_{k=i}^{j-1}frac{3}{2}Delta ilde{mathbf{R}}_{ik}(ar{mathbf{b}}_i) ext{Exp}left(frac{partialDeltaar{mathbf{R}}_{ik}}{partialmathbf{b}^g}deltamathbf{b}^g_i
ight)( ilde{mathbf{a}}_k-ar{mathbf{b}}^a_i-delta mathbf{b}^a_i)Delta t^2
otag \
&= sum_{k=i}^{j-1}frac{3}{2}Delta ilde{mathbf{R}}_{ik}(ar{mathbf{b}}_i)left( mathbf{I} + left(frac{partialDeltaar{mathbf{R}}_{ik}}{partialmathbf{b}^g}deltamathbf{b}^g_i
ight)^{wedge}
ight)( ilde{mathbf{a}}_k-ar{mathbf{b}}^a_i-delta mathbf{b}^a_i)Delta t^2
otag \
&= sum_{k=i}^{j-1}frac{3}{2}Delta ilde{mathbf{R}}_{ik}(ar{mathbf{b}}_i)( ilde{mathbf{a}}_k-ar{mathbf{b}}^a_i)Delta t^2 + sum_{k=i}^{j-1}-frac{3}{2}Delta ilde{mathbf{R}}_{ik}(ar{mathbf{b}}_i)Delta t^2 delta mathbf{b}^a_i
otag \ &+ sum_{k=i}^{j-1}-frac{3}{2}Delta ilde{mathbf{R}}_{ik}(ar{mathbf{b}}_i)( ilde{mathbf{a}}_k-ar{mathbf{b}}^a_i)^{wedge}frac{partialDeltaar{mathbf{R}}_{ik}}{partialmathbf{b}^g}Delta t^2 deltamathbf{b}^g_i
otag \
&= Delta ilde{mathbf{p}}_{ij}(ar{mathbf{b}}^g_i, ar{mathbf{b}}^a_i) + frac{partialDeltaar{mathbf{p}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i + frac{partialDeltaar{mathbf{p}}_{ij}}{partialmathbf{b}^a}deltamathbf{b}^a_i end{align} ]
综合以上：
[egin{align} frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g} &= sum_{k=i}^{j-1}left[- Delta ilde{mathbf{R}}_{k+1, j}(ar{mathbf{b}}_i^g)^T mathbf{J}^k_r Delta t
ight]
otag \ frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^g} &= sum_{k=i}^{j-1}-Delta ilde{mathbf{R}}_{ik}(ar{mathbf{b}}_i)( ilde{mathbf{a}}_k-ar{mathbf{b}}^a_i)^{wedge} frac{partialDeltaar{mathbf{R}}_{ik}}{partialmathbf{b}^g}Delta t
otag \
frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^a} &= sum_{k=i}^{j-1}-Delta ilde{mathbf{R}}_{ik}(ar{mathbf{b}}_i)Delta t
otag \
frac{partialDeltaar{mathbf{p}}_{ij}}{partialmathbf{b}^g} &= sum_{k=i}^{j-1}-frac{3}{2}Delta ilde{mathbf{R}}_{ik}(ar{mathbf{b}}_i)( ilde{mathbf{a}}_k-ar{mathbf{b}}^a_i)^{wedge}frac{partialDeltaar{mathbf{R}}_{ik}}{partialmathbf{b}^g}Delta t^2
otag \
frac{partialDeltaar{mathbf{p}}_{ij}}{partialmathbf{b}^a} &= sum_{k=i}^{j-1}-frac{3}{2}Delta ilde{mathbf{R}}_{ik}(ar{mathbf{b}}_i)Delta t^2 end{align}]
Jacobians of (mathbf{r}_{Deltamathbf{R}_{ij}})
[egin{align} mathbf{r}_{Deltamathbf{R}_{ij}} &doteq -delta pmb{phi}_{ij} = ext{Log}left( left( Delta ilde{mathbf{R}}_{ij}(ar{mathbf{b}}^g_i) ext{Exp}left( frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i
ight)
ight)^T mathbf{R}^T_imathbf{R}_j
ight) end{align}
]
误差左乘或右乘，需要注意下面两个公式。这里按照 DSO 代码中误差左乘初始旋转矩阵，得到的结果如下。
[egin{align} &mathbf{r}_{Deltamathbf{R}_{ij}} ( ext{Exp}(delta pmb{phi}_{i})mathbf{R}_i)
otag \
&= ext{Log}left( left( Delta ilde{mathbf{R}}_{ij}(ar{mathbf{b}}^g_i) ext{Exp}left( frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i
ight)
ight)^T ( ext{Exp}(delta pmb{phi}_{i})mathbf{R}_i)^Tmathbf{R}_j
ight)
otag \
&= ext{Log}left( left( Delta ilde{mathbf{R}}_{ij}(ar{mathbf{b}}^g_i) ext{Exp}left( frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i
ight)
ight)^T mathbf{R}_i^T ext{Exp}(-delta pmb{phi}_{i}) mathbf{R}_j
ight)
otag \
&= ext{Log}left( left( Delta ilde{mathbf{R}}_{ij}(ar{mathbf{b}}^g_i) ext{Exp}left( frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i
ight)
ight)^T mathbf{R}_i^T mathbf{R}_j mathbf{R}_j^T ext{Exp}(-delta pmb{phi}_{i}) mathbf{R}_j
ight)
otag \
&= ext{Log}left( left( Delta ilde{mathbf{R}}_{ij}(ar{mathbf{b}}^g_i) ext{Exp}left( frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i
ight)
ight)^T mathbf{R}_i^T mathbf{R}_j ext{Exp}(-mathbf{R}_j^Tdelta pmb{phi}_{i})
ight)
otag \
&= ext{Log}left( left( Delta ilde{mathbf{R}}_{ij}(ar{mathbf{b}}^g_i) ext{Exp}left( frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i
ight)
ight)^T mathbf{R}_i^T mathbf{R}_j
ight)
otag \ &+ mathbf{J}^{-1}_rleft( ext{Log}left( left( Delta ilde{mathbf{R}}_{ij}(ar{mathbf{b}}^g_i) ext{Exp}left( frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i
ight)
ight)^T mathbf{R}_i^T mathbf{R}_j
ight)
ight)(-mathbf{R}_j^Tdelta pmb{phi}_{i})
otag \
&= mathbf{r}_{Deltamathbf{R}_{ij}} (mathbf{R}_i) - mathbf{J}^{-1}_r (mathbf{r}_{Deltamathbf{R}_{ij}} (mathbf{R}_i))mathbf{R}_j^Tdelta pmb{phi}_{i}end{align}]
[egin{align} &mathbf{r}_{Deltamathbf{R}_{ij}} ( ext{Exp}(delta pmb{phi}_{j})mathbf{R}_j)
otag \ &= ext{Log}left( left( Delta ilde{mathbf{R}}_{ij}(ar{mathbf{b}}^g_i) ext{Exp}left( frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i
ight)
ight)^T mathbf{R}_i^T ext{Exp}(delta pmb{phi}_{j}) mathbf{R}_j
ight)
otag \
&= ext{Log}left( left( Delta ilde{mathbf{R}}_{ij}(ar{mathbf{b}}^g_i) ext{Exp}left( frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i
ight)
ight)^T mathbf{R}_i^T mathbf{R}_j mathbf{R}_j^T ext{Exp}(delta pmb{phi}_{j}) mathbf{R}_j
ight)
otag \
&= ext{Log}left( left( Delta ilde{mathbf{R}}_{ij}(ar{mathbf{b}}^g_i) ext{Exp}left( frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i
ight)
ight)^T mathbf{R}_i^T mathbf{R}_j ext{Exp}(mathbf{R}_j^T delta pmb{phi}_{j})
ight)
otag \
&= mathbf{r}_{Deltamathbf{R}_{ij}} (mathbf{R}_j) + mathbf{J}^{-1}_r(mathbf{r}_{Deltamathbf{R}_{ij}} (mathbf{R}_j)) mathbf{R}_j^T delta pmb{phi}_{j} end{align}]
[egin{align} &mathbf{r}_{Deltamathbf{R}_{ij}} (deltamathbf{b}^g_i + ilde{deltamathbf{b}_{gi}})
otag \
&= ext{Log}left( left( Delta ilde{mathbf{R}}_{ij}(ar{mathbf{b}}^g_i) ext{Exp}left( frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g}(deltamathbf{b}^g_i + ilde{deltamathbf{b}_{gi}})
ight)
ight)^T mathbf{R}^T_imathbf{R}_j
ight)
otag \
&= ext{Log}left( left( Delta ilde{mathbf{R}}_{ij}(ar{mathbf{b}}^g_i) ext{Exp}left( frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i
ight) ext{Exp}left( mathbf{J}_rleft(frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i
ight)frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g} ilde{deltamathbf{b}_{gi}}
ight)
ight)^T mathbf{R}^T_imathbf{R}_j
ight)
otag \
&= ext{Log}left( ext{Exp}left( -mathbf{J}_rleft(frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i
ight)frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g} ilde{deltamathbf{b}_{gi}}
ight) left( Delta ilde{mathbf{R}}_{ij}(ar{mathbf{b}}^g_i) ext{Exp}left( frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i
ight)
ight)^T mathbf{R}^T_imathbf{R}_j
ight)
otag \
&= ext{Log}left( ext{Exp}left( -mathbf{J}_rleft(frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i
ight)frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g} ilde{deltamathbf{b}_{gi}}
ight) ext{Exp}(mathbf{r}_{Deltamathbf{R}_{ij}} (deltamathbf{b}^g_i))
ight)
otag \
&= mathbf{r}_{Deltamathbf{R}_{ij}} (deltamathbf{b}^g_i) -mathbf{J}_l^{-1}(mathbf{r}_{Deltamathbf{R}_{ij}} (deltamathbf{b}^g_i))mathbf{J}_rleft(frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i
ight)frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g} ilde{deltamathbf{b}_{gi}}end{align}]
综合以上：
[egin{align} frac{partial mathbf{r}_{Deltamathbf{R}_{ij}}}{partial delta pmb{phi}_i} &= - mathbf{J}^{-1}_r (mathbf{r}_{Deltamathbf{R}_{ij}} (mathbf{R}_i))mathbf{R}_j^T
otag \
frac{partial mathbf{r}_{Deltamathbf{R}_{ij}}}{partial delta pmb{p}_i} &= mathbf{0}
otag \
frac{partial mathbf{r}_{Deltamathbf{R}_{ij}}}{partial delta mathbf{v}_i} &= mathbf{0}
otag \
frac{partial mathbf{r}_{Deltamathbf{R}_{ij}}}{partial delta pmb{phi}_j} &= mathbf{J}^{-1}_r(mathbf{r}_{Deltamathbf{R}_{ij}} (mathbf{R}_j)) mathbf{R}_j^T
otag \
frac{partial mathbf{r}_{Deltamathbf{R}_{ij}}}{partial delta pmb{p}_j} &= mathbf{0}
otag \
frac{partial mathbf{r}_{Deltamathbf{R}_{ij}}}{partial delta mathbf{v}_j} &= mathbf{0}
otag \
frac{partial mathbf{r}_{Deltamathbf{R}_{ij}}}{partial ilde{deltamathbf{b}_{ai}}} &= mathbf{0}
otag \
frac{partial mathbf{r}_{Deltamathbf{R}_{ij}}}{partial ilde{deltamathbf{b}_{gi}}} &= -mathbf{J}_l^{-1}(mathbf{r}_{Deltamathbf{R}_{ij}} (deltamathbf{b}^g_i))mathbf{J}_rleft(frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i
ight)frac{partialDeltaar{mathbf{R}}_{ij}}{partialmathbf{b}^g}
otag end{align}]
Jacobians of (mathbf{r}_{Deltamathbf{v}_{ij}})
[egin{align} mathbf{r}_{Deltamathbf{v}_{ij}} &doteq -delta mathbf{v}_{ij} = mathbf{R}^T_i(mathbf{v}_j - mathbf{v}_i - mathbf{g}Delta t_{ij})
otag \
&- left( Delta ilde{mathbf{v}}_{ij}(ar{mathbf{b}}^g_i, ar{mathbf{b}}^a_i) + frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i + frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^a}deltamathbf{b}^a_i
ight) end{align}]
[egin{align} mathbf{r}_{Deltamathbf{v}_{ij}}( ext{Exp}(deltapmb{phi}_i)mathbf{R}_i) &= mathbf{R}^T_i ext{Exp}(-deltapmb{phi}_i)(mathbf{v}_j - mathbf{v}_i - mathbf{g}Delta t_{ij})
otag \
&- left( Delta ilde{mathbf{v}}_{ij}(ar{mathbf{b}}^g_i, ar{mathbf{b}}^a_i) + frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i + frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^a}deltamathbf{b}^a_i
ight)
otag \
&= mathbf{R}^T_i(mathbf{I} - deltapmb{phi}_i^{wedge})(mathbf{v}_j - mathbf{v}_i - mathbf{g}Delta t_{ij})
otag \
&- left( Delta ilde{mathbf{v}}_{ij}(ar{mathbf{b}}^g_i, ar{mathbf{b}}^a_i) + frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i + frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^a}deltamathbf{b}^a_i
ight)
otag \
&= mathbf{R}^T_i(mathbf{v}_j - mathbf{v}_i - mathbf{g}Delta t_{ij})^{wedge}deltapmb{phi}_i + mathbf{r}_{Deltamathbf{v}_{ij}}(mathbf{R}_i) end{align}]
[egin{align} mathbf{r}_{Deltamathbf{v}_{ij}}(mathbf{v}_i + delta mathbf{v}_i) &= mathbf{R}^T_i(mathbf{v}_j - mathbf{v}_i - delta mathbf{v}_i - mathbf{g}Delta t_{ij})
otag \
&- left( Delta ilde{mathbf{v}}_{ij}(ar{mathbf{b}}^g_i, ar{mathbf{b}}^a_i) + frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i + frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^a}deltamathbf{b}^a_i
ight)
otag \
&= -mathbf{R}_i^T delta mathbf{v}_i + mathbf{r}_{Deltamathbf{v}_{ij}}(mathbf{v}_i)end{align}]
[egin{align} mathbf{r}_{Deltamathbf{v}_{ij}}(mathbf{v}_j + delta mathbf{v}_j) &= mathbf{R}^T_i(mathbf{v}_j + delta mathbf{v}_j - mathbf{v}_i - mathbf{g}Delta t_{ij})
otag \
&- left( Delta ilde{mathbf{v}}_{ij}(ar{mathbf{b}}^g_i, ar{mathbf{b}}^a_i) + frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i + frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^a}deltamathbf{b}^a_i
ight)
otag \
&= mathbf{R}_i^T delta mathbf{v}_j + mathbf{r}_{Deltamathbf{v}_{ij}}(mathbf{v}_i)end{align}]
[egin{align} mathbf{r}_{Deltamathbf{v}_{ij}}(mathbf{v}_j + delta mathbf{v}_j) &= mathbf{R}^T_i(mathbf{v}_j + delta mathbf{v}_j - mathbf{v}_i - mathbf{g}Delta t_{ij})
otag \
&- left( Delta ilde{mathbf{v}}_{ij}(ar{mathbf{b}}^g_i, ar{mathbf{b}}^a_i) + frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i + frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^a}deltamathbf{b}^a_i
ight)
otag \
&= mathbf{R}_i^T delta mathbf{v}_j + mathbf{r}_{Deltamathbf{v}_{ij}}(mathbf{v}_i)end{align}]
[egin{align} mathbf{r}_{Deltamathbf{v}_{ij}}(deltamathbf{b}^g_i + ilde{deltamathbf{b}_{gi}}) &= mathbf{R}^T_i(mathbf{v}_j - mathbf{v}_i - mathbf{g}Delta t_{ij})
otag \
&- left( Delta ilde{mathbf{v}}_{ij}(ar{mathbf{b}}^g_i, ar{mathbf{b}}^a_i) + frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^g}(deltamathbf{b}^g_i + ilde{deltamathbf{b}_{gi}}) + frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^a}deltamathbf{b}^a_i
ight)
otag \
&= -frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^g} ilde{deltamathbf{b}_{gi}} + mathbf{r}_{Deltamathbf{v}_{ij}}(deltamathbf{b}^g_i) end{align}]
[egin{align} mathbf{r}_{Deltamathbf{v}_{ij}}(deltamathbf{b}^a_i + ilde{deltamathbf{b}_{ai}}) &= mathbf{R}^T_i(mathbf{v}_j - mathbf{v}_i - mathbf{g}Delta t_{ij})
otag \
&- left( Delta ilde{mathbf{v}}_{ij}(ar{mathbf{b}}^g_i, ar{mathbf{b}}^a_i) + frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i + frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^a}(deltamathbf{b}^a_i + ilde{deltamathbf{b}_{ai}})
ight)
otag \
&= -frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^a} ilde{deltamathbf{b}_{ai}} + mathbf{r}_{Deltamathbf{v}_{ij}}(deltamathbf{b}^a_i) end{align}]
综合以上：
[egin{align} frac{partial mathbf{r}_{Deltamathbf{v}_{ij}}}{partial delta pmb{phi}_i} &= mathbf{R}^T_i(mathbf{v}_j - mathbf{v}_i - mathbf{g}Delta t_{ij})^{wedge}
otag \
frac{partial mathbf{r}_{Deltamathbf{v}_{ij}}}{partial delta pmb{p}_i} &= mathbf{0}
otag \
frac{partial mathbf{r}_{Deltamathbf{v}_{ij}}}{partial delta mathbf{v}_i} &= -mathbf{R}_i^T
otag \
frac{partial mathbf{r}_{Deltamathbf{v}_{ij}}}{partial delta pmb{phi}_j} &= mathbf{0}
otag \
frac{partial mathbf{r}_{Deltamathbf{v}_{ij}}}{partial delta pmb{p}_j} &= mathbf{0}
otag \
frac{partial mathbf{r}_{Deltamathbf{v}_{ij}}}{partial delta mathbf{v}_j} &= mathbf{R}_i^T
otag \
frac{partial mathbf{r}_{Deltamathbf{v}_{ij}}}{partial ilde{deltamathbf{b}_{ai}}} &= -frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^a}
otag \
frac{partial mathbf{r}_{Deltamathbf{v}_{ij}}}{partial ilde{deltamathbf{b}_{gi}}} &= -frac{partialDeltaar{mathbf{v}}_{ij}}{partialmathbf{b}^g}
otag end{align}]
Jacobians of (mathbf{r}_{Deltamathbf{p}_{ij}})
[egin{align} mathbf{r}_{Deltamathbf{p}_{ij}} &doteq -delta mathbf{p}_{ij} = mathbf{R}^T_i(mathbf{p}_j-mathbf{p}_i-mathbf{v}_iDelta t_{ij}-frac{1}{2}mathbf{g}Delta t^2_{ij})
otag \
&- left( Delta ilde{mathbf{p}}_{ij}(ar{mathbf{b}}^g_i, ar{mathbf{b}}^a_i) + frac{partialDeltaar{mathbf{p}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i + frac{partialDeltaar{mathbf{p}}_{ij}}{partialmathbf{b}^a}deltamathbf{b}^a_i
ight) end{align}]
[egin{align} mathbf{r}_{Deltamathbf{p}_{ij}}( ext{Exp}(deltapmb{phi}_i)mathbf{R}_i) &= mathbf{R}^T_i ext{Exp}(-deltapmb{phi}_i)(mathbf{p}_j-mathbf{p}_i-mathbf{v}_iDelta t_{ij}-frac{1}{2}mathbf{g}Delta t^2_{ij})
otag \
&- left( Delta ilde{mathbf{p}}_{ij}(ar{mathbf{b}}^g_i, ar{mathbf{b}}^a_i) + frac{partialDeltaar{mathbf{p}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i + frac{partialDeltaar{mathbf{p}}_{ij}}{partialmathbf{b}^a}deltamathbf{b}^a_i
ight)
otag \
&= mathbf{R}^T_i(mathbf{I}-deltapmb{phi}_i^{wedge})(mathbf{p}_j-mathbf{p}_i-mathbf{v}_iDelta t_{ij}-frac{1}{2}mathbf{g}Delta t^2_{ij})
otag \
&- left( Delta ilde{mathbf{p}}_{ij}(ar{mathbf{b}}^g_i, ar{mathbf{b}}^a_i) + frac{partialDeltaar{mathbf{p}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i + frac{partialDeltaar{mathbf{p}}_{ij}}{partialmathbf{b}^a}deltamathbf{b}^a_i
ight)
otag \
&= mathbf{R}^T_i(mathbf{p}_j-mathbf{p}_i-mathbf{v}_iDelta t_{ij}-frac{1}{2}mathbf{g}Delta t^2_{ij})^{wedge}deltapmb{phi}_i + mathbf{r}_{Deltamathbf{p}_{ij}}(mathbf{R}_i) end{align}]
[egin{align} mathbf{r}_{Deltamathbf{p}_{ij}}(mathbf{p}_i + deltamathbf{p}_i) &= mathbf{R}^T_i(mathbf{p}_j-mathbf{p}_i-deltamathbf{p}_i-mathbf{v}_iDelta t_{ij}-frac{1}{2}mathbf{g}Delta t^2_{ij})
otag \
&- left( Delta ilde{mathbf{p}}_{ij}(ar{mathbf{b}}^g_i, ar{mathbf{b}}^a_i) + frac{partialDeltaar{mathbf{p}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i + frac{partialDeltaar{mathbf{p}}_{ij}}{partialmathbf{b}^a}deltamathbf{b}^a_i
ight)
otag \
&= -mathbf{R}^T_ideltamathbf{p}_i + mathbf{r}_{Deltamathbf{p}_{ij}}(mathbf{p}_i) end{align}]
[egin{align} mathbf{r}_{Deltamathbf{p}_{ij}}(mathbf{v}_i + deltamathbf{v}_i) &= mathbf{R}^T_i(mathbf{p}_j-mathbf{p}_i-mathbf{v}_iDelta t_{ij}-deltamathbf{v}_iDelta t_{ij}-frac{1}{2}mathbf{g}Delta t^2_{ij})
otag \
&- left( Delta ilde{mathbf{p}}_{ij}(ar{mathbf{b}}^g_i, ar{mathbf{b}}^a_i) + frac{partialDeltaar{mathbf{p}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i + frac{partialDeltaar{mathbf{p}}_{ij}}{partialmathbf{b}^a}deltamathbf{b}^a_i
ight)
otag \
&= -mathbf{R}^T_iDelta t_{ij}deltamathbf{v}_i + mathbf{r}_{Deltamathbf{p}_{ij}}(mathbf{v}_i) end{align}]
[egin{align} mathbf{r}_{Deltamathbf{p}_{ij}}(mathbf{p}_j + deltamathbf{p}_j) &= mathbf{R}^T_i(mathbf{p}_j+deltamathbf{p}_j-mathbf{p}_i-mathbf{v}_iDelta t_{ij}-frac{1}{2}mathbf{g}Delta t^2_{ij})
otag \
&- left( Delta ilde{mathbf{p}}_{ij}(ar{mathbf{b}}^g_i, ar{mathbf{b}}^a_i) + frac{partialDeltaar{mathbf{p}}_{ij}}{partialmathbf{b}^g}deltamathbf{b}^g_i + frac{partialDeltaar{mathbf{p}}_{ij}}{partialmathbf{b}^a}deltamathbf{b}^a_i
ight)
otag \
&= mathbf{R}^T_ideltamathbf{p}_j + mathbf{r}_{Deltamathbf{p}_{ij}}(mathbf{p}_i) end{align}]
综合以上：
[egin{align} frac{partial mathbf{r}_{Deltamathbf{p}_{ij}}}{partial delta pmb{phi}_i} &= mathbf{R}^T_i(mathbf{p}_j-mathbf{p}_i-mathbf{v}_iDelta t_{ij}-frac{1}{2}mathbf{g}Delta t^2_{ij})^{wedge}
otag \
frac{partial mathbf{r}_{Deltamathbf{p}_{ij}}}{partial delta pmb{p}_i} &= -mathbf{R}^T_i
otag \
frac{partial mathbf{r}_{Deltamathbf{p}_{ij}}}{partial delta mathbf{v}_i} &= -mathbf{R}^T_iDelta t_{ij}
otag \
frac{partial mathbf{r}_{Deltamathbf{p}_{ij}}}{partial delta pmb{phi}_j} &= mathbf{0}
otag \
frac{partial mathbf{r}_{Deltamathbf{p}_{ij}}}{partial delta pmb{p}_j} &= mathbf{R}^T_i
otag \
frac{partial mathbf{r}_{Deltamathbf{p}_{ij}}}{partial delta mathbf{v}_j} &= mathbf{0}
otag \
frac{partial mathbf{r}_{Deltamathbf{p}_{ij}}}{partial ilde{deltamathbf{b}_{ai}}} &= -frac{partialDeltaar{mathbf{p}}_{ij}}{partialmathbf{b}^a}
otag \
frac{partial mathbf{r}_{Deltamathbf{p}_{ij}}}{partial ilde{deltamathbf{b}_{gi}}} &= -frac{partialDeltaar{mathbf{p}}_{ij}}{partialmathbf{b}^g}
otag end{align}]