ORB_SLAM3_IMU预积分理论推导(残差)

火柴的初心

已于 2023-06-11 22:53:51 修改

阅读量873

点赞数 1

CC 4.0 BY-SA版权

分类专栏： ORB_SLAM3_源码解析文章标签：算法人工智能 c++ SLAM 自动驾驶

于 2023-04-10 10:26:55 首次发布

本文链接：https://blog.csdn.net/He3he3he/article/details/130054238

ORB_SLAM3_源码解析专栏收录该内容

20 篇文章

订阅专栏

ORB_SLAM3是一个视觉惯性里程计系统，其关键部分包括初始化、IMU预积分和残差计算。在初始化阶段，系统构建帧并进行匹配。IMU预积分考虑了姿态和速度的累积误差，而残差用于优化过程，通过一阶近似修正避免了积分的重新计算。这些理论对于理解和实现高精度的定位至关重要。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

在这里插入图片描述

残差

预积分的测量值采用一阶近似修正，免去了积分重新运算，降低计算量
状态包含： $\mathbf{R}_{i}, \mathbf{p}_{i}, \mathbf{v}_{i}, \mathbf{R}_{j}, \mathbf{p}_{j}, \mathbf{v}_{j}, \delta \mathbf{b}_{i}^{g}, \delta \mathbf{b}_{i}^{a}$ ，其中关于bias的是bias的偏差
$\begin{aligned} \mathbf{r}_{\Delta \mathbf{R}_{i j}} & \triangleq \log \left\{\left[\Delta \tilde{\mathbf{R}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}\right) \cdot \operatorname{Exp}\left(\frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right)\right]^{T} \cdot \mathbf{R}_{i}^{T} \mathbf{R}_{j}\right\} \\ & \triangleq \log \left[\left(\Delta \hat{\mathbf{R}}_{i j}\right)^{T} \Delta \mathbf{R}_{i j}\right] \\ \mathbf{r}_{\Delta \mathbf{v}_{i j}} & \triangleq \mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\left[\Delta \tilde{\mathbf{v}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}, \overline{\mathbf{b}}_{i}^{a}\right)+\frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \overline{\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a}\right] \\ & \triangleq \Delta \mathbf{v}_{i j}-\Delta \hat{\mathbf{v}}_{i j} \\ \mathbf{r}_{\Delta \mathbf{p}_{i j}} & \triangleq \mathbf{R}_{i}^{T}\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \cdot \Delta t_{i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2}\right)-\left[\Delta \tilde{\mathbf{p}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}, \overline{\mathbf{b}}_{i}^{a}\right)+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a}\right] \\ & \triangleq \Delta \mathbf{p}_{i j}-\Delta \hat{\mathbf{p}}_{i j} \end{aligned}$

$\mathbf{r}_{\Delta \mathbf{R}_{i j}}$

$\mathbf{r}_{\Delta \mathbf{R}_{i j}}$ 关于 $\delta \vec{\phi}_{i}$ 的jacobian
$\begin{aligned} \mathbf{r}_{\Delta \mathbf{R}_{i j}}\left(\mathbf{R}_{i} \operatorname{Exp}\left(\delta \vec{\phi}_{i}\right)\right) & =\log \left[\left(\Delta \hat{\mathbf{R}}_{i j}\right)^{T}\left(\mathbf{R}_{i} \operatorname{Exp}\left(\delta \vec{\phi}_{i}\right)\right)^{T} \mathbf{R}_{j}\right] \\ & \stackrel{(1)}{=} \log \left[\left(\Delta \hat{\mathbf{R}}_{i j}\right)^{T} \operatorname{Exp}\left(-\delta \vec{\phi}_{i}\right) \mathbf{R}_{i}^{T} \mathbf{R}_{j}\right] \\ & \stackrel{2}{=} \log \left[\left(\Delta \hat{\mathbf{R}}_{i j}\right)^{T} \mathbf{R}_{i}^{T} \mathbf{R}_{j} \operatorname{Exp}\left(-\mathbf{R}_{j}^{T} \mathbf{R}_{i} \delta \vec{\phi}_{i}\right)\right] \\ & =\log \left\{\operatorname{Exp}\left[\log \left(\left(\Delta \hat{\mathbf{R}}_{i j}\right)^{T} \mathbf{R}_{i}^{T} \mathbf{R}_{j}\right)\right] \cdot \operatorname{Exp}\left(-\mathbf{R}_{j}^{T} \mathbf{R}_{i} \delta \vec{\phi}\right)\right\} \\ & =\log \left[\operatorname{Exp}\left(\mathbf{r}_{\Delta \mathbf{R}_{i j}}\left(\mathbf{R}_{i}\right)\right) \cdot \operatorname{Exp}\left(-\mathbf{R}_{j}^{T} \mathbf{R}_{i} \delta \vec{\phi}_{i}\right)\right] \\ & \stackrel{3}{\approx} \mathbf{r}_{\Delta \mathbf{R}_{i j}}\left(\mathbf{R}_{i}\right)-\mathbf{J}_{r}^{-1}\left(\mathbf{r}_{\Delta \mathbf{R}_{i j}}\left(\mathbf{R}_{i}\right)\right) \mathbf{R}_{j}^{T} \mathbf{R}_{i} \delta \vec{\phi}_{i} \\ & \stackrel{4}{=} \mathbf{r}_{\Delta \mathbf{R}_{i j}}-\mathbf{J}_{r}^{-1}\left(\mathbf{r}_{\Delta \mathbf{R}_{i j}}\right) \mathbf{R}_{j}^{T} \mathbf{R}_{i} \delta \vec{\phi}_{i} \end{aligned}$
可以得到：
$\frac{\partial \mathbf{r}_{\Delta \mathbf{R}_{i j}}}{\partial \delta \vec{\phi}_{i}}=-\mathbf{J}_{r}^{-1}\left(\mathbf{r}_{\Delta \mathbf{R}_{i j}}\right) \mathbf{R}_{j}^{T} \mathbf{R}_{i}$
$\mathbf{r}_{\Delta \mathbf{R}_{i j}}$ 关于 $\delta \vec{\phi}_{j}$ 的jacobian
$\begin{aligned} \mathbf{r}_{\Delta \mathbf{R}_{i j}}\left(\mathbf{R}_{j} \operatorname{Exp}\left(\delta \vec{\phi}_{j}\right)\right) & =\log \left[\left(\Delta \hat{\mathbf{R}}_{i j}\right)^{T} \mathbf{R}_{i}^{T} \mathbf{R}_{j} \operatorname{Exp}\left(\delta \vec{\phi}_{j}\right)\right] \\ & =\log \left\{\operatorname{Exp}\left[\log \left(\left(\Delta \hat{\mathbf{R}}_{i j}\right)^{T} \mathbf{R}_{i}^{T} \mathbf{R}_{j}\right)\right] \cdot \operatorname{Exp}\left(\delta \vec{\phi}_{j}\right)\right\} \\ & =\log \left\{\operatorname{Exp}\left(\mathbf{r}_{\Delta \mathbf{R}_{i j}}\left(\mathbf{R}_{j}\right)\right) \cdot \operatorname{Exp}\left(\delta \vec{\phi}_{j}\right)\right\} \\ & \stackrel{(1)}{\approx} \mathbf{r}_{\Delta \mathbf{R}_{i j}}\left(\mathbf{R}_{j}\right)+\mathbf{J}_{r}^{-1}\left(\mathbf{r}_{\Delta \mathbf{R}_{i j}}\left(\mathbf{R}_{j}\right)\right) \delta \vec{\phi}_{j} \\ & \stackrel{2}{=} \mathbf{r}_{\Delta \mathbf{R}_{i j}}+\mathbf{J}_{r}^{-1}\left(\mathbf{r}_{\Delta \mathbf{R}_{i j}}\right) \delta \vec{\phi}_{j} \end{aligned}$
可以得到：
$\frac{\partial \mathbf{r}_{\Delta \mathbf{R}_{i j}}}{\partial \delta \vec{\phi}_{j}}=\mathbf{J}_{r}^{-1}\left(\mathbf{r}_{\Delta \mathbf{R}_{i j}}\right)$
$\mathbf{r}_{\Delta \mathbf{R}_{i j}}$ 关于 $\delta p_{i}$ 的jacobian
$\frac{\partial \mathbf{r}_{\Delta \mathbf{R}_{i j}}}{\partial \delta \mathbf{p}_{i}}=\frac{\partial \mathbf{r}_{\Delta \mathbf{R}_{i j}}}{\partial \mathbf{p}_{i}}=\mathbf{0}$
$\mathbf{r}_{\Delta \mathbf{R}_{i j}}$ 关于 $\delta p_{j}$ 的jacobian
$\frac{\partial \mathbf{r}_{\Delta \mathbf{R}_{i j}}}{\partial \delta \mathbf{p}_{j}}=\frac{\partial \mathbf{r}_{\Delta \mathbf{R}_{i j}}}{\partial \mathbf{p}_{j}}=\mathbf{0}$
$\mathbf{r}_{\Delta \mathbf{R}_{i j}}$ 关于 $\delta v_{i}$ 的jacobian
$\frac{\partial \mathbf{r}_{\Delta \mathbf{R}_{i j}}}{\partial \delta \mathbf{v}_{i}}=\frac{\partial \mathbf{r}_{\Delta \mathbf{R}_{i j}}}{\partial \mathbf{v}_{i}}=\mathbf{0}$
$\mathbf{r}_{\Delta \mathbf{R}_{i j}}$ 关于 $\delta v_{j}$ 的jacobian
$\frac{\partial \mathbf{r}_{\Delta \mathbf{R}_{i j}}}{\partial \delta \mathbf{v}_{j}}=\frac{\partial \mathbf{r}_{\Delta \mathbf{R}_{i j}}}{\partial \mathbf{v}_{j}}=\mathbf{0}$
$\mathbf{r}_{\Delta \mathbf{R}_{i j}}$ 关于 $\delta \mathbf{b}_{i}^{a}$ 的jacobian
$\frac{\partial \mathbf{r}_{\Delta \mathbf{R}_{i j}}}{\partial \delta \mathbf{b}_{i}^{a}}=\frac{\partial \mathbf{r}_{\Delta \mathbf{R}_{i j}}}{\partial \delta \mathbf{b}_{i}^{a}}=\mathbf{0}$
$\mathbf{r}_{\Delta \mathbf{R}_{i j}}$ 关于 $\delta \mathbf{b}_{i}^{g}$ 的jacobian
$\begin{array}{l} \mathbf{r}_{\Delta \mathbf{R}_{i j}}\left(\delta \mathbf{b}_{i}^{g}+\widetilde{\delta \mathbf{b}_{i}^{g}}\right)=\log \left\{\left[\Delta \tilde{\mathbf{R}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}\right) \operatorname{Exp}\left(\frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \overline{\mathbf{b}}^{g}}\left(\delta \mathbf{b}_{i}^{g}+\widetilde{\delta \mathbf{b}_{i}^{g}}\right)\right)\right]^{T} \mathbf{R}_{i}^{T} \mathbf{R}_{j}\right\} \\ \stackrel{(1)}{\approx} \log \left\{\left[\Delta \tilde{\mathbf{R}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}\right) \operatorname{Exp}\left(\frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right) \operatorname{Exp}\left(\mathbf{J}_{r}\left(\frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right) \frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \widetilde{\delta \mathbf{b}_{i}^{g}}\right)\right]^{T} \Delta \mathbf{R}_{i j}\right\} \\ \stackrel{(2)}{=} \log \left\{\left[\Delta \hat{\mathbf{R}}_{i j} \cdot \operatorname{Exp}\left(\boldsymbol{\varepsilon} \cdot \frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \widehat{\delta \mathbf{b}_{i}^{g}}\right)\right]^{T} \Delta \mathbf{R}_{i j}\right\} \\ \stackrel{(3)}{=} \log \left[\operatorname{Exp}\left(-\boldsymbol{\varepsilon} \cdot \frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \widetilde{\delta \mathbf{b}_{i}^{g}}\right) \Delta \hat{\mathbf{R}}_{i j}^{T} \Delta \mathbf{R}_{i j}\right] \\ =\log \left[\operatorname{Exp}\left(-\boldsymbol{\varepsilon} \cdot \frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \widetilde{\delta \mathbf{b}_{i}^{g}}\right) \operatorname{Exp}\left(\log \left(\Delta \hat{\mathbf{R}}_{i j}^{T} \Delta \mathbf{R}_{i j}\right)\right)\right] \\ =\log \left[\operatorname{Exp}\left(-\boldsymbol{\varepsilon} \cdot \frac{\partial \Delta \overline{\mathbf{R}}_{i j} \widetilde{\delta \overline{\mathbf{b}}_{i}^{g}}}{\partial \overline{\mathbf{b}}^{g}}\right) \operatorname{Exp}\left(\mathbf{r}_{\Delta \mathbb{R}_{i j}}\left(\delta \mathbf{b}_{i}^{g}\right)\right)\right] \\ \stackrel{(4)}{=} \log \left\{\operatorname{Exp}\left(\mathbf{r}_{\Delta \mathbf{R}_{i j}}\left(\delta \mathbf{b}_{i}^{g}\right)\right) \operatorname{Exp}\left[-\operatorname{Exp}\left(-\mathbf{r}_{\Delta \mathbf{R}_{i j}}\left(\delta \mathbf{b}_{i}^{g}\right)\right) \cdot \varepsilon \cdot \frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \widetilde{\delta \mathbf{b}_{i}^{g}}\right]\right\} \\ \stackrel{(5)}{\approx} \mathbf{r}_{\Delta \mathbf{R}_{i j}}\left(\delta \mathbf{b}_{i}^{g}\right)-\mathbf{J}_{r}^{-1}\left(\mathbf{r}_{\Delta \mathbf{R}_{i j}}\left(\delta \mathbf{b}_{i}^{g}\right)\right) \cdot \operatorname{Exp}\left(-\mathbf{r}_{\Delta \mathbf{R}_{i j}}\left(\delta \mathbf{b}_{i}^{g}\right)\right) \cdot \boldsymbol{\varepsilon} \cdot \frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \widetilde{\delta \mathbf{b}_{i}^{g}} \\ \stackrel{(6)}{=} \mathbf{r}_{\Delta \mathbf{R}_{i j}}-\mathbf{J}_{r}^{-1}\left(\mathbf{r}_{\Delta \mathbb{R}_{i j}}\right) \cdot \operatorname{Exp}\left(-\mathbf{r}_{\Delta \mathbf{R}_{i j}}\right) \cdot \mathbf{J}_{r}\left(\frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right) \cdot \frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \cdot \widetilde{\delta \mathbf{b}_{i}^{g}} \\ \end{array}$
可以得到：
$\frac{\partial \mathbf{r}_{\Delta \mathbf{R}_{i j}}}{\partial \widetilde{\delta \mathbf{b}_{i}^{g}}}=\frac{\partial \mathbf{r}_{\Delta \mathbf{R}_{i j}}}{\partial \delta \mathbf{b}_{i}^{g}}=-\mathbf{J}_{r}^{-1}\left(\mathbf{r}_{\Delta \mathbf{R}_{i j}}\right) \cdot \operatorname{Exp}\left(-\mathbf{r}_{\Delta \mathbf{R}_{i j}}\right) \cdot \mathbf{J}_{r}\left(\frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right) \cdot \frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \overline{\mathbf{b}}^{g}}$

$\mathbf{r}_{\Delta \mathbf{v}_{i j}}$

$\mathbf{r}_{\Delta \mathbf{v}_{i j}}$ 关于 $\delta \vec{\phi}_{i}$ 的jacobian
$\begin{aligned} \mathbf{r}_{\Delta \mathbf{v}_{i j}}\left(\mathbf{R}_{i} \operatorname{Exp}\left(\delta \vec{\phi}_{i}\right)\right) & =\left(\mathbf{R}_{i} \operatorname{Exp}\left(\delta \vec{\phi}_{i}\right)\right)^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\Delta \hat{\mathbf{v}}_{i j} \\ & \stackrel{(1)}{=} \operatorname{Exp}\left(-\delta \overrightarrow{\phi_{i}}\right) \cdot \mathbf{R}_{i}^{T} \cdot\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\Delta \hat{\mathbf{v}}_{i j} \\ & \stackrel{(2)}{\approx}\left(\mathbf{I}-\left(\delta \vec{\phi}_{i}\right)^{\wedge}\right) \cdot \mathbf{R}_{i}^{T} \cdot\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\Delta \hat{\mathbf{v}}_{i j} \\ & =\mathbf{R}_{i}^{T} \cdot\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\Delta \hat{\mathbf{v}}_{i j}-\left(\delta \vec{\phi}_{i}\right)^{\wedge} \cdot \mathbf{R}_{i}^{T} \cdot\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right) \\ & \stackrel{(3)}{=} \mathbf{r}_{\Delta \mathbf{v}_{i j}}\left(\mathbf{R}_{i}\right)+\left[\mathbf{R}_{i}^{T} \cdot\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)\right]^{\wedge} \cdot \delta \vec{\phi}_{i} \\ & \stackrel{(4)}{=} \mathbf{r}_{\Delta \mathbf{v}_{i j}}+\left[\mathbf{R}_{i}^{T} \cdot\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)\right]^{\wedge} \cdot \delta \vec{\phi}_{i} \end{aligned}$
可以得到：
$\frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \delta \vec{\phi}_{i}}=\left[\mathbf{R}_{i}^{T} \cdot\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)\right]^{\wedge}$
$\mathbf{r}_{\Delta \mathbf{v}_{i j}}$ 关于 $\delta \vec{\phi}_{j}$ 的jacobian
$\frac{\partial \mathbf{r}_{\Delta v_{ij}}}{\partial \delta \vec{\phi}_{j}}=\mathbf{0}$
$\mathbf{r}_{\Delta \mathbf{v}_{i j}}$ 关于 $\delta p_{i}$ 的jacobian
$\frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \delta \mathbf{p}_{i}}=\frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \mathbf{p}_{i}}=\mathbf{0}$
$\mathbf{r}_{\Delta \mathbf{v}_{i j}}$ 关于 $\delta p_{j}$ 的jacobian
$\frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \delta \mathbf{p}_{j}}=\frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \mathbf{p}_{j}}=\mathbf{0}$
$\mathbf{r}_{\Delta \mathbf{v}_{i j}}$ 关于 $\delta v_{i}$ 的jacobian
$\begin{aligned} \mathbf{r}_{\Delta \mathbf{v}_{i j}}\left(\mathbf{v}_{i}+\delta \mathbf{v}_{i}\right) & =\mathbf{R}_{i}^{T} \cdot\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\delta \mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\Delta \hat{\mathbf{v}}_{i j} \\ & =\mathbf{R}_{i}^{T} \cdot\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\Delta \hat{\mathbf{v}}_{i j}-\mathbf{R}_{i}^{T} \delta \mathbf{v}_{i} \\ & =\mathbf{r}_{\Delta \mathbf{v}_{i j}}\left(\mathbf{v}_{i}\right)-\mathbf{R}_{i}^{T} \delta \mathbf{v}_{i} \\ & \stackrel{11}{ }=\mathbf{r}_{\Delta \mathbf{v}_{i j}}-\mathbf{R}_{i}^{T} \delta \mathbf{v}_{i} \end{aligned}$
可以得到：
$\frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \delta \mathbf{v}_{i}}=\frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \mathbf{v}_{i}}=-\mathbf{R}_{i}^{T}$
$\mathbf{r}_{\Delta \mathbf{v}_{i j}}$ 关于 $\delta v_{j}$ 的jacobian
$\begin{aligned} \mathbf{r}_{\Delta \mathbf{v}_{i j}}\left(\mathbf{v}_{j}+\delta \mathbf{v}_{j}\right) & =\mathbf{R}_{i}^{T} \cdot\left(\mathbf{v}_{j}+\delta \mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\Delta \hat{\mathbf{v}}_{i j} \\ & =\mathbf{R}_{i}^{T} \cdot\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)-\Delta \hat{\mathbf{v}}_{i j}+\mathbf{R}_{i}^{T} \delta \mathbf{v}_{j} \\ & =\mathbf{r}_{\Delta \mathbf{v}_{i j}}\left(\mathbf{v}_{j}\right)+\mathbf{R}_{i}^{T} \delta \mathbf{v}_{j} \\ & =\mathbf{r}_{\Delta \mathbf{v}_{i j}}+\mathbf{R}_{i}^{T} \delta \mathbf{v}_{j} \end{aligned}$
可以得到：
$\frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \delta \mathbf{v}_{j}}=\frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \mathbf{v}_{j}}=\mathbf{R}_{i}^{T}$
$\mathbf{r}_{\Delta \mathbf{v}_{i j}}$ 关于 $\delta \mathbf{b}_{i}^{a}$ 的jacobian
$\frac{\partial \mathbf{r}_{\Delta v_{i j}}}{\partial \widetilde{\delta \mathbf{b}_{i}^{\mathrm{g}}}}=\frac{\partial \mathbf{r}_{\Delta v_{i j}}}{\partial \delta \mathbf{b}_{i}^{\mathrm{g}}}=-\frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \mathbf{b}^{g}}$
$\mathbf{r}_{\Delta \mathbf{v}_{i j}}$ 关于 $\delta \mathbf{b}_{i}^{g}$ 的jacobian
$\frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \widetilde{\delta \mathbf{b}_{i}^{\mathrm{a}}}}=\frac{\partial \mathbf{r}_{\Delta \mathbf{v}_{i j}}}{\partial \delta \mathbf{b}_{i}^{\mathrm{a}}}=-\frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \mathbf{b}^{a}}$

$\mathbf{r}_{\Delta \mathbf{p}_{i j}}$

$\mathbf{r}_{\Delta \mathbf{p}_{i j}}$ 关于 $\delta \vec{\phi}_{i}$ 的jacobian
$\begin{aligned} \mathbf{r}_{\Delta \mathbf{p}_{\mathrm{p}}}\left(\mathbf{R}_{i} \operatorname{Exp}\left(\delta \overrightarrow{\phi_{i}}\right)\right) & =\left(\mathbf{R}_{i} \operatorname{Exp}\left(\delta \vec{\phi}_{i}\right)\right)^{T}\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \cdot \Delta t_{i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2}\right)-\Delta \hat{\mathbf{p}}_{i j} \\ & \stackrel{(1)}{=} \operatorname{Exp}\left(-\delta \vec{\phi}_{i}\right) \cdot \mathbf{R}_{i}^{T} \cdot\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \cdot \Delta t_{i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2}\right)-\Delta \hat{\mathbf{p}}_{i j} \\ & \stackrel{(2)}{ }\left(\mathbf{I}-\left(\delta \vec{\phi}_{i}\right)^{\wedge}\right) \cdot \mathbf{R}_{i}^{T} \cdot\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \cdot \Delta t_{i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2}\right)-\Delta \hat{\mathbf{p}}_{i j} \\ & =\mathbf{R}_{i}^{T} \cdot\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \cdot \Delta t_{i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2}\right)-\Delta \hat{\mathbf{p}}_{i j}-\left(\delta \vec{\phi}_{i}\right)^{\wedge} \mathbf{R}_{i}^{T} \cdot\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \cdot \Delta t_{i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2}\right) \\ & \stackrel{(3)}{=} \mathbf{r}_{\Delta \mathbf{p}_{y j}}\left(\mathbf{R}_{i}\right)+\left[\mathbf{R}_{i}^{T} \cdot\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \cdot \Delta t_{i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2}\right)\right]^{\wedge} \cdot \delta \vec{\phi}_{i} \\ & \stackrel{(4)}{=} \mathbf{r}_{\Delta \mathbf{p}_{y}}+\left[\mathbf{R}_{i}^{T} \cdot\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \cdot \Delta t_{i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2}\right)\right] \cdot \delta \vec{\phi}_{i} \end{aligned}$
可以得到：
$\frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \delta \overrightarrow{\phi_{i}}}=\left[\mathbf{R}_{i}^{T} \cdot\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \cdot \Delta t_{i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2}\right)\right]^{\wedge}$
$\mathbf{r}_{\Delta \mathbf{p}_{i j}}$ 关于 $\delta \vec{\phi}_{j}$ 的jacobian
$\frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \delta \vec{\phi}_{j}}=\mathbf{0}$
$\mathbf{r}_{\Delta \mathbf{p}_{i j}}$ 关于 $\delta p_{i}$ 的jacobian
$\begin{aligned} \mathbf{r}_{\Delta \mathbf{p}_{i j}}\left(\mathbf{p}_{i}+\mathbf{R}_{i} \cdot \delta \mathbf{p}_{i}\right) & =\mathbf{R}_{i}^{T}\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{R}_{i} \cdot \delta \mathbf{p}_{i}-\mathbf{v}_{i} \cdot \Delta t_{i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2}\right)-\Delta \hat{\mathbf{p}}_{i j} \\ & =\mathbf{R}_{i}^{T}\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \cdot \Delta t_{i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2}\right)-\Delta \hat{\mathbf{p}}_{i j}-\mathbf{I} \cdot \delta \mathbf{p}_{i} \\ & =\mathbf{r}_{\Delta \mathbf{p}_{i j}}\left(\mathbf{p}_{i}\right)-\mathbf{I} \cdot \delta \mathbf{p}_{i} \\ & \stackrel{1}{ } \\ & =\mathbf{r}_{\Delta \mathbf{p}_{i j}}-\mathbf{I} \cdot \delta \mathbf{p}_{i} \end{aligned}$
$\mathbf{r}_{\Delta \mathbf{p}_{i j}}$ 关于 $\delta p_{j}$ 的jacobian
$\begin{aligned} \mathbf{r}_{\Delta \mathbf{p}_{i j}}\left(\mathbf{p}_{j}+\mathbf{R}_{j} \cdot \delta \mathbf{p}_{j}\right) & =\mathbf{R}_{i}^{T}\left(\mathbf{p}_{j}+\mathbf{R}_{j} \cdot \delta \mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \cdot \Delta t_{i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2}\right)-\Delta \hat{\mathbf{p}}_{i j} \\ & =\mathbf{R}_{i}^{T}\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \cdot \Delta t_{i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2}\right)-\Delta \hat{\mathbf{p}}_{i j}+\mathbf{R}_{i}^{T} \mathbf{R}_{j} \cdot \delta \mathbf{p}_{j} \\ & =\mathbf{r}_{\Delta \mathbf{p}_{i j}}\left(\mathbf{p}_{j}\right)+\mathbf{R}_{i}^{T} \mathbf{R}_{j} \cdot \delta \mathbf{p}_{j} \\ & \stackrel{(1)}{=} \mathbf{r}_{\Delta \mathbf{p}_{i j}}+\mathbf{R}_{i}^{T} \mathbf{R}_{j} \cdot \delta \mathbf{p}_{j} \end{aligned}$
可以得到：
$\frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \delta \mathbf{p}_{j}}=\frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \mathbf{p}_{j}}=\mathbf{R}_{i}^{T} \mathbf{R}_{j}$
$\mathbf{r}_{\Delta \mathbf{p}_{i j}}$ 关于 $\delta v_{i}$ 的jacobian
$\begin{aligned} \mathbf{r}_{\Delta \mathbf{p}_{i j}}\left(\mathbf{v}_{i}+\delta \mathbf{v}_{i}\right) & =\mathbf{R}_{i}^{T}\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \cdot \Delta t_{i j}-\delta \mathbf{v}_{i} \cdot \Delta t_{i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2}\right)-\Delta \hat{\mathbf{p}}_{i j} \\ & =\mathbf{R}_{i}^{T}\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \cdot \Delta t_{i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2}\right)-\Delta \hat{\mathbf{p}}_{i j}-\mathbf{R}_{i}^{T} \Delta t_{i j} \cdot \delta \mathbf{v}_{i} \\ & =\mathbf{r}_{\Delta \mathbf{p}_{i j}}\left(\mathbf{v}_{i}\right)-\mathbf{R}_{i}^{T} \Delta t_{i j} \cdot \delta \mathbf{v}_{i} \\ & \stackrel{(1)}{ } \\ & =\mathbf{r}_{\Delta \mathbf{p}_{i j}}-\mathbf{R}_{i}^{T} \Delta t_{i j} \cdot \delta \mathbf{v}_{i} \end{aligned}$
可以得到：
$\frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \delta \mathbf{v}_{i}}=\frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \mathbf{v}_{i}}=-\mathbf{R}_{i}^{T} \Delta t_{i j}$
$\mathbf{r}_{\Delta \mathbf{p}_{i j}}$ 关于 $\delta v_{j}$ 的jacobian

$\frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \delta \mathbf{v}_{j}}=\frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \mathbf{v}_{j}}=\mathbf{0}$

$\mathbf{r}_{\Delta \mathbf{p}_{i j}}$ 关于 $\delta \mathbf{b}_{i}^{a}$ 的jacobian

$\frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \widehat{\delta \mathbf{b}_{i}^{g}}}=\frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \delta \mathbf{b}_{i}^{g}}=-\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \mathbf{b}^{g}}$

$\mathbf{r}_{\Delta \mathbf{p}_{i j}}$ 关于 $\delta \mathbf{b}_{i}^{g}$ 的jacobian

$\frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{i j}}}{\partial \delta \mathbf{b}_{i}^{a}}=-\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \mathbf{b}^{a}}$