Reference:

1. 深蓝学院-多传感器融合
2. 多传感器融合定位理论基础

文章跳转：

1. 多传感器融合定位一-3D激光里程计其一：ICP
2. 多传感器融合定位二-3D激光里程计其二：NDT
3. 多传感器融合定位三-3D激光里程计其三：点云畸变补偿
4. 多传感器融合定位四-3D激光里程计其四：点云线面特征提取
5. 多传感器融合定位五-点云地图构建及定位
6. 多传感器融合定位六-惯性导航原理及误差分析
7. 多传感器融合定位七-惯性导航解算及误差分析其一
8. 多传感器融合定位八-惯性导航解算及误差分析其二
9. 多传感器融合定位九-基于滤波的融合方法Ⅰ其一
10. 多传感器融合定位十-基于滤波的融合方法Ⅰ其二
11. 多传感器融合定位十一-基于滤波的融合方法Ⅱ
12. 多传感器融合定位十二-基于图优化的建图方法其一
13. 多传感器融合定位十三-基于图优化的建图方法其二
14. 多传感器融合定位十四-基于图优化的定位方法
15. 多传感器融合定位十五-多传感器时空标定(综述)

1. 基于预积分的融合方案流程

1.1 优化问题分析

优化问题可以等效为如下形式：

三种约束分别通过以下方式获得：

1. 激光里程计约束：使用激光里程计，计算每个关键帧位姿，进而得到相对位姿；
2. IMU约束：在上一个关键帧位姿基础上，进行惯性积分，从而得到两关键帧相对位姿；
3. RTK约束：直接测量得到。

1.2 预积分的作用

问题：位姿每次优化后会发生变化，其后的IMU惯性积分就要重新进行，运算量过大。
解决思路：直接计算两帧之间的相对位姿，而不依赖初始值影响，即所谓的预积分。

预积分不是为了提高精度(与粗暴方法比，而非不融合方法)，而是为了提高效率。关于预积分的文章，这一篇从理解上讲的非常清晰明了：imu预积分原理的个人理解

1.3 基于预积分的建图方案流程

由于此处讨论的优化方案包含组合导航系统，且认为外参已标定，因此会和常见的lio/vio中的方案有所不同，它不包含以下内容：

1. 初始化lidar和IMU之间的外参(有外参的配置文件，已经标定好了，有了这个配置文件，外参的估计就没有意义了)；
2. 初始化速度、陀螺仪bias等(因为这里有组合导航，组合导航解决了大部分的问题，组合导航完成后有初始速度、初始速度和初始姿态，同时一般的组合导航会告诉初始的bias是多少，所以在这种情况下这两项也变成了已知的问题了，不需要做初始化来估计了)；
3. 初始化重力(如果知道经纬度就知道了精确的重力，只有在不知道经纬度的情况下才需要初始化去优化重力，因为组合导航经纬度都有了，经纬度是可以通过直接计算得到的而不需要通过优化得到)；
4. 世界坐标系对齐(组合导航已经对齐)(组合导航有了姿态，这个姿态就是在世界坐标系下的一个姿态)。

2. 预积分模型设计

在第8讲中，已知导航的微分方程如下：
$$\begin{array}{rl}& {\dot{\mathbf{p}}}_{w{b}_t}={\mathbf{v}}_t^w\\ & {\dot{\mathbf{v}}}_t^w={\mathbf{a}}_t^w\\ & {\dot{\mathbf{q}}}_{w{b}_t}={\mathbf{q}}_{w{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\mathbf{\omega }}^{b_t}\end{array}\right]\end{array}$$根据该微分方程，可知从 $i$ 时刻到 $j$ 时刻 IMU的积分结果为
$$\begin{array}{c}{\mathbf{p}}_{w{b}_j}={\mathbf{p}}_{w{b}_i}+{\mathbf{v}}_i^w\Delta t+{\iint }_{t\in [i,j]}\left({\mathbf{q}}_{w{b}_t}{\mathbf{a}}^{b_t}-{\mathbf{g}}^w\right)\delta t^2\\ {\mathbf{v}}_j^w={\mathbf{v}}_i^w+{\int }_{t\in [i,j]}\left({\mathbf{q}}_{w{b}_t}{\mathbf{a}}^{b_t}-{\mathbf{g}}^w\right)\delta t\\ {\mathbf{q}}_{w{b}_j}={\int }_{t\in [i,j]}{\mathbf{q}}_{w{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\mathbf{\omega }}^{b_t}\end{array}\right]\delta t\end{array}$$根据预积分的要求，需要求相对结果，而且不依赖于上一时刻位姿，因此需要对上式做转换。
由于 ${\mathbf{q}}_{w{b}_t}={\mathbf{q}}_{w{b}_i}\otimes {\mathbf{q}}_{b_i{b}_t}$，把它带入(4)-(6)式可得
$$\begin{array}{c}{\mathbf{p}}_{w{b}_j}={\mathbf{p}}_{w{b}_i}+{\mathbf{v}}_i^w\Delta t-\frac{1}{2}{\mathbf{g}}^w\Delta t^2+{\mathbf{q}}_{w{b}_i}[{\iint }_{t\in [i,j]}\left({\mathbf{q}}_{b_i{b}_t}{\mathbf{a}}^{b_t}\right)\delta t^2\\ {\mathbf{v}}_j^w={\mathbf{v}}_i^w-{\mathbf{g}}^w\Delta t+{\mathbf{q}}_{w{b}_i}{\int }_{t\in [i,j]}\left({\mathbf{q}}_{b_i{b}_t}{\mathbf{a}}^{b_t}\right)\delta t]\\ {\mathbf{q}}_{w{b}_j}={\mathbf{q}}_{w{b}_i}{\int }_{t\in [i,j]}{\mathbf{q}}_{b_i{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\mathbf{\omega }}^{b_t}\end{array}\right]\delta t\end{array}$$可见，此时需要积分的项，就完全和 $i$ 时刻的状态无关了。

为了整理公式，把积分相关的项用下面的式子代替：
$$\begin{array}{rl}& {\mathbf{\alpha }}_{b_i{b}_j}={\iint }_{t\in [i,j]}\left({\mathbf{q}}_{b_i{b}_t}{\mathbf{a}}^{b_t}\right)\delta t^2\\ & {\mathbf{\beta }}_{b_i{b}_j}={\int }_{t\in [i,j]}\left({\mathbf{q}}_{b_i{b}_t}{\mathbf{a}}^{b_t}\right)\delta t\\ & {\mathbf{q}}_{b_i{b}_j}={\int }_{t\in [i,j]}{\mathbf{q}}_{b_i{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\mathbf{\omega }}^{b_t}\end{array}\right]\delta t\end{array}$$实际使用中使用离散形式，而非连续形式，由于在解算中，一般采用中值积分方法，即：
$$\begin{array}{rl}& \mathbf{\omega }=\frac{1}{2}\left[\left({\mathbf{\omega }}^{b_k}-{\mathbf{b}}_k^g\right)+\left({\mathbf{\omega }}^{b_{k+1}}-{\mathbf{b}}_k^g\right)\right]\\ & \mathbf{a}=\frac{1}{2}\left[{\mathbf{q}}_{b_i{b}_k}\left({\mathbf{a}}^{b_k}-{\mathbf{b}}_k^a\right)+{\mathbf{q}}_{b_i{b}_{k+1}}\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a\right)\right]\end{array}$$那么预积分的离散形式可以表示为：
$$\begin{array}{rl}& {\mathbf{\alpha }}_{b_i{b}_{k+1}}={\mathbf{\alpha }}_{b_i{b}_k}+{\mathbf{\beta }}_{b_i{b}_k}\delta t+\frac{1}{2}\mathbf{a}\delta t^2\\ & {\mathbf{\beta }}_{b_i{b}_{k+1}}={\mathbf{\beta }}_{b_i{b}_k}+\mathbf{a}\delta t\\ & {\mathbf{q}}_{b_i{b}_{k+1}}={\mathbf{q}}_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\mathbf{\omega }\delta t\end{array}\right]\end{array}$$经过以上的推导，此时状态更新的公式可以整理为：
$$\left[\begin{array}{c}{\mathbf{p}}_{w{b}_j}\\ {\mathbf{v}}_j^w\\ {\mathbf{q}}_{w{b}_j}\\ {\mathbf{b}}_j^a\\ {\mathbf{b}}_j^g\end{array}\right]=\left[\begin{array}{c}{\mathbf{p}}_{w{b}_i}+{\mathbf{v}}_i^w\Delta t-\frac{1}{2}{\mathbf{g}}^w\Delta t^2+{\mathbf{q}}_{w{b}_i}{\mathbf{\alpha }}_{b_i{b}_j}\\ {\mathbf{v}}_i^w-{\mathbf{g}}^w\Delta t+{\mathbf{q}}_{w{b}_i}{\mathbf{\beta }}_{b_i{b}_j}\\ {\mathbf{q}}_{w{b}_i}{\mathbf{q}}_{b_i{b}_j}\\ {\mathbf{b}}_i^a\\ {\mathbf{b}}_i^g\end{array}\right]$$需要注意的是，陀螺仪和加速度计的模型为：
$$\begin{array}{r}{\mathbf{b}}_{k+1}^a={\mathbf{b}}_k^a+{\mathbf{n}}_{{\mathbf{b}}_k^a}\delta t\\ {\mathbf{b}}_{k+1}^g={\mathbf{b}}_k^g+{\mathbf{n}}_{{\mathbf{b}}_k^g}\delta t\end{array}$$即认为bias是在变化的，这样便于估计不同时刻的bias值，而不是整个系统运行时间内都当做常值对待。这更符合低精度mems的实际情况。但在预积分时，由于两个关键帧之间的时间较短，因此认为 $i$ 和 $j$ 时刻的bias相等。

需要注意的一点是，预积分的结果中包含了bias，在优化过程中，bias作为状态量也会发生变化，从而引起预积分结果变化。
为了避免bias变化后，重新做预积分，可以把预积分结果在bias处泰勒展开，表达成下面的形式，这样就可以根据bias的变化量直接算出新的预积分结果。
$$\begin{array}{rl}& {\mathbf{\alpha }}_{b_i{b}_j}={\overline{\mathbf{\alpha }}}_{b_i{b}_j}+{\mathbf{J}}_{b_i^a}^{\alpha }\delta {\mathbf{b}}_i^a+{\mathbf{J}}_{b_i^g}^{\alpha }\delta {\mathbf{b}}_i^g\\ & {\mathbf{\beta }}_{b_i{b}_j}={\overline{\mathbf{\beta }}}_{b_i{b}_j}+{\mathbf{J}}_{b_i^a}^{\beta }\delta {\mathbf{b}}_i^a+{\mathbf{J}}_{b_i^g}^{\beta }\delta {\mathbf{b}}_i^g\\ & {\mathbf{q}}_{b_i{b}_j}={\overline{\mathbf{q}}}_{b_i{b}_j}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\mathbf{J}}_{b_i^q}^q\delta {\mathbf{b}}_i^g\end{array}\right]\end{array}$$其中：
$$\begin{array}{rl}{\mathbf{J}}_{b_i^a}^{\alpha }& =\frac{\partial {\alpha }_{b_i{b}_j}}{\partial \delta {\mathbf{b}}_i^a}\\ {\mathbf{J}}_{b_i^g}^{\alpha }& =\frac{\partial {\mathbf{\alpha }}_{b_i{b}_j}}{\partial \delta {\mathbf{b}}_i^j}\\ {\mathbf{J}}_{b_i^a}^{\beta }& =\frac{\partial {\beta }_{b_i{b}_j}}{\partial \delta {\mathbf{b}}_i^a}\\ {\mathbf{J}}_{b_i^g}^{\beta }& =\frac{\partial {\beta }_{b_i{b}_j}}{\partial \delta {\mathbf{b}}_i^a}\\ {\mathbf{J}}_{b_i^g}^q& =\frac{{\mathbf{q}}_{b_i{b}_j}}{\partial {\mathbf{b}}_i^q}\end{array}$$注: 此处暂时不直接给出以上各雅可比的结果，它的推导放在后面进行。

3. 预积分在优化中的使用

3.1 使用方法

1. 凸优化回顾
   损失函数由残差函数组成：
   $$\underset{x}{\min }F(x)=\frac{1}{2}\Vert f(x){\Vert }_2^2$$当考虑方差时，可以写为：
   $$\underset{x}{\min }F(x)=f(x{)}^T\Omega f(x)$$利用高斯牛顿方法，求解该优化问题，需要解下面的方程：
   $$\underset{H}{\underbrace{J^T\Omega J}}\Delta x=\underset{g}{\underbrace{-J^T\Omega f(x)}}$$即，在优化中使用一项信息，需要设计残差， 并推导它的雅可比和方差。
2. 预积分的使用
   按照优化的套路，在优化中使用预积分，需要：
   a. 设计残差
   b. 推导残差关于待优化变量的雅可比
   c. 计算残差的方差
   除此以外，预积分中还多一项计算，即推导bias变化时，预积分量重新计算的方法。

3.2 残差设计

在优化时，需要知道残差关于状态量的雅可比。由于已知姿态位姿更新的方法如下：
$$\left[\begin{array}{c}{\mathbf{p}}_{w{b}_j}\\ {\mathbf{q}}_{w{b}_j}\\ {\mathbf{v}}_j^w\\ {\mathbf{b}}_j^a\\ {\mathbf{b}}_j^g\end{array}\right]=\left[\begin{array}{c}{\mathbf{p}}_{w{b}_i}+{\mathbf{v}}_i^w\Delta t-\frac{1}{2}{\mathbf{g}}^w\Delta t^2+{\mathbf{q}}_{w{b}_i}{\mathbf{\alpha }}_{b_i{b}_j}\\ {\mathbf{q}}_{w{b}_i}{\mathbf{q}}_{b_i{b}_j}\\ {\mathbf{v}}_i^w-{\mathbf{g}}^w\Delta t+{\mathbf{q}}_{w{b}_i}{\mathbf{\beta }}_{b_i{b}_j}\\ {\mathbf{b}}_i^a\\ {\mathbf{b}}_i^g\end{array}\right]$$因此，可以很容易写出一种残差形式如下：
$$\left[\begin{array}{c}{\mathbf{r}}_p\\ {\mathbf{r}}_q\\ {\mathbf{r}}_v\\ {\mathbf{r}}_{ba}\\ {\mathbf{r}}_{bg}\end{array}\right]=\left[\begin{array}{c}{\mathbf{p}}_{w{b}_j}-{\mathbf{p}}_{w{b}_i}-{\mathbf{v}}_i^w\Delta t+\frac{1}{2}{\mathbf{g}}^w\Delta t^2-{\mathbf{q}}_{w{b}_i}{\mathbf{\alpha }}_{b_i{b}_j}\\ 2{\left[{\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes \left({\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right)\right]}_{xyz}\\ {\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t-{\mathbf{q}}_{w{b}_i}{\mathbf{\beta }}_{b_i{b}_j}\\ {\mathbf{b}}_j^a-{\mathbf{b}}_i^a\\ {\mathbf{b}}_j^g-{\mathbf{b}}_i^g\end{array}\right]$$但是和预积分相关的量，仍然与上一时刻的姿态有关，无法直接加减，因此，把残差修正为以下形式：
$$\left[\begin{array}{c}{\mathbf{r}}_p\\ {\mathbf{r}}_q\\ {\mathbf{r}}_v\\ {\mathbf{r}}_{ba}\\ {\mathbf{r}}_{bg}\end{array}\right]=\left[\begin{array}{c}{\mathbf{q}}_{w{b}_i}^{\ast }\left({\mathbf{p}}_{w{b}_j}-{\mathbf{p}}_{w{b}_i}-{\mathbf{v}}_i^w\Delta t+\frac{1}{2}{\mathbf{g}}^w\Delta t^2\right)-{\mathbf{\alpha }}_{b_i{b}_j}\\ 2{\left[{\mathbf{q}}_{{\mathbf{q}}_{b_j}^{\ast }}\otimes \left({\mathbf{q}}_{w_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right)\right]}_{xyz}\\ {\mathbf{q}}_{w{b}_i}^{\ast }\left({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t\right)-{\mathbf{\beta }}_{b_i{b}_j}\\ {\mathbf{b}}_j^a-{\mathbf{b}}_i^a\\ {\mathbf{b}}_j^g-{\mathbf{b}}_i^g\end{array}\right]$$待优化的变量是 $\left[\begin{array}{lllll}{\mathbf{p}}_{w{b}_i}& {\mathbf{q}}_{w{b}_i}& {\mathbf{v}}_i^w& {\mathbf{b}}_i^a& {\mathbf{b}}_i^g\end{array}\right]\left[\begin{array}{lllll}{\mathbf{p}}_{w{b}_j}& {\mathbf{q}}_{w{b}_j}& {\mathbf{v}}_j^w& {\mathbf{b}}_j^a& {\mathbf{b}}_j^g\end{array}\right]$
但在实际使用中，往往都是使用扰动量，因此实际是对以下变量求雅可比
$$\begin{array}{rl}& \left[\begin{array}{lllll}\delta {\mathbf{p}}_{w{b}_i}& \delta {\theta }_{w{b}_i}& \delta {\mathbf{v}}_i^w& \delta {\mathbf{b}}_i^a& \delta {\mathbf{b}}_i^g\end{array}\right]\\ & \left[\begin{array}{lllll}\delta {\mathbf{p}}_{w{b}_j}& \delta {\theta }_{w{b}_j}& \delta {\mathbf{v}}_j^w& \delta {\mathbf{b}}_j^a& \delta {\mathbf{b}}_j^g\end{array}\right]\end{array}$$此处只对几个比较复杂的雅可比进行推导，其余比较简单，感兴趣的可自行完成。

3.3 残差雅可比的推导

3.3.1 姿态残差的雅可比

1. 对 $i$ 时刻姿态误差的雅可比：
   $$\begin{array}{rl}\frac{\partial {\mathbf{r}}_q}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}& =\frac{\partial 2{\left[{\mathbf{q}}_{b_j{b}_i}\otimes \left({\mathbf{q}}_{b_{i}w}\otimes {\mathbf{q}}_{w{b}_j}\right)\right]}_{xyz}}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}\\ & =\frac{\partial 2{\left[{\mathbf{q}}_{b_i,b_j}^{\ast }\otimes {\left({\mathbf{q}}_{w{b}_i}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}\end{array}\right]\right)}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right]}_{xyz}}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}\\ & =\frac{\partial -2{\left[{\left({\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes {\left({\mathbf{q}}_{w{b}_i}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\mathbf{\theta }}_{b_i}{b}_i^{\prime }\end{array}\right]\right)}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right)}^{\ast }\right]}_{xyz}}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i}}\\ & =\frac{\partial -2{\left[{\mathbf{q}}_{w{b}_j}^{\ast }\otimes \left({\mathbf{q}}_{w{b}_i}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}\end{array}\right]\right)\otimes {\mathbf{q}}_{b_i{b}_j}\right]}_{xyz}}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}\end{array}$$上式可以化简为：
   $$\begin{array}{rl}\frac{\partial {\mathbf{r}}_q}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}& =-2\left[\begin{array}{ll}0& \mathbf{I}\end{array}\right]\frac{\partial {\mathbf{q}}_{w{b}_j}^{\ast }\otimes \left({\mathbf{q}}_{w{b}_i}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}\end{array}\right]\right)\otimes {\mathbf{q}}_{b_i{b}_j}}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}\\ & =-2\left[\begin{array}{ll}0& \mathbf{I}\end{array}\right]\frac{\partial {\left[{\mathbf{q}}_{w{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}\right]}_L{\left[{\mathbf{q}}_{b_i{b}_j}\right]}_R\left[\begin{array}{c}1\\ \frac{1}{2}\delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}\end{array}\right]}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}\\ & =-2\left[\begin{array}{ll}0& \mathbf{I}\end{array}\right]{\left[{\mathbf{q}}_{w{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}\right]}_L{\left[{\mathbf{q}}_{b_i{b}_j}\right]}_R\left[\begin{array}{c}0\\ \frac{1}{2}\mathbf{I}\end{array}\right]\end{array}$$

2. 对 $j$ 时刻姿态误差的雅可比：
   $$\begin{array}{rl}\frac{\partial {\mathbf{r}}_q}{\partial \delta {\mathbf{\theta }}_{b_j{b}_j^{\prime }}}& =\frac{\partial 2{\left[{\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\mathbf{\theta }}_{b_j{b}_j^{\prime }}\end{array}\right]\right]}_{xyz}}{\partial \delta {\mathbf{\theta }}_{b_j{b}_j^{\prime }}}\\ & =\frac{\partial 2{\left[{\left[{\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right]}_L\left[\begin{array}{c}1\\ \frac{1}{2}\delta {\mathbf{\theta }}_{b_j{b}_j^{\prime }}\end{array}\right]\right]}_{xyz}}{\partial \delta {\mathbf{\theta }}_{b_j{b}_j^{\prime }}}\\ & =2\left[\begin{array}{ll}0& \mathbf{I}\end{array}\right]{\left[{\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right]}_L\left[\begin{array}{c}0\\ \frac{1}{2}\mathbf{I}\end{array}\right]\end{array}$$

3. 对 $i$ 时刻陀螺仪bias误差的雅可比：
   $$\begin{array}{rl}& \frac{\partial {\mathbf{r}}_q}{\partial \delta {\mathbf{b}}_i^g}=\frac{\partial 2{\left[{\left({\mathbf{q}}_{b_i{b}_j}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\mathbf{J}}_{b_i^q\delta {\mathbf{b}}_i^g}^q\end{array}\right]\right)}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right]}_{xyz}}{\partial \delta {\mathbf{b}}_i^g}\\ & =\frac{\partial -2{\left[{\left({\left({\mathbf{q}}_{b_i{b}_j}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\mathbf{J}}_{b_i^q}^q\delta {\mathbf{b}}_i^g\end{array}\right]\right)}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right)}^{\ast }\right]}_{xyz}}{\partial \delta {\mathbf{b}}_i^g}\\ & =\frac{\partial -2{\left[{\mathbf{q}}_{w{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}\otimes \left({\mathbf{q}}_{b_i{b}_j}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\mathbf{J}}_{b_i^o}\delta {\mathbf{b}}_i^g\end{array}\right]\right)\right]}_{xyz}}{\partial \delta {\mathbf{b}}_i^g}\\ & =-2\left[\begin{array}{ll}0& \mathbf{I}\end{array}\right]{\left[{\mathbf{q}}_{w{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}\otimes {\mathbf{q}}_{b_i{b}_j}\right]}_L\left[\begin{array}{c}0\\ \frac{1}{2}{\mathbf{J}}_{b_i^q}^q\end{array}\right]\\ & \end{array}$$

3.3.2 速度残差的雅可比

1. 对 $i$ 时刻姿态误差的雅可比：
   $$\begin{array}{rl}\frac{\partial {\mathbf{r}}_v}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i}}& =\frac{\partial {\left({\mathbf{q}}_{w{b}_i}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\mathbf{\theta }}_{b_i{b}_i}\end{array}\right]\right)}^{-1}\left({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t\right)}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}\\ & =\frac{\partial {\left({\mathbf{R}}_{w{b}_i}\exp \left({\left[\delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}\right]}_{\times }\right)\right)}^{-1}\left({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t\right)}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}\\ & =\frac{\partial \exp \left({\left[-\delta {\mathbf{\theta }}_{b_i{b}_i}\right]}_{\times }\right){\mathbf{R}}_{b_{i}w}\left({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t\right)}{\partial \delta {\mathbf{\theta }}_{b_i}{b}_i^{\prime }}\\ & =\frac{\partial \left(\mathbf{I}-{\left[\delta {\mathbf{\theta }}_{b_i{b}_i}\right]}_{\times }\right){\mathbf{R}}_{b_{i}w}\left({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t\right)}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}\\ & =\frac{\partial -{\left[\delta {\mathbf{\theta }}_{b_i{b}_i}\right]}_{\times }{\mathbf{R}}_{b_{i}w}\left({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t\right)}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}\\ & ={\left[{\mathbf{R}}_{b_{i}w}\left({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t\right)\right]}_{\times }\end{array}$$
2. 对 $i$ 时刻速度误差的雅可比：
   $$\frac{\partial {\mathbf{r}}_v}{\partial \delta {\mathbf{v}}_i^w}=-{\mathbf{R}}_{b_{i}w}$$
3. 对 $i$ 时刻加速度biasi吴差的雅可比：
   $$\frac{\partial {\mathbf{r}}_v}{\partial \delta {\mathbf{b}}_i^a}=-\frac{\partial {\mathbf{\beta }}_{b_i{b}_j}}{\partial \delta {\mathbf{b}}_i^a}=-{\mathbf{J}}_{b_i^a}^{\beta }$$

3.3.3 位置残差的雅可比

由于位置残差的形式与速度残差极其相似，因此不再重复推导。