多传感器融合定位十二-基于图优化的建图方法其一

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}{rlr}& {\dot{\mathbf{p}}}_{w{b}_t}={\mathbf{v}}_t^w& (1)\\ & {\dot{\mathbf{v}}}_t^w={\mathbf{a}}_t^w& (2)\\ & {\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]& (3)\end{array}$$根据该微分方程，可知从 $i$ 时刻到 $j$ 时刻 IMU的积分结果为(这里可以发现，每个积分都依赖了上一时刻的结果 ${\mathbf{q}}_{w{b}_t}$。当上一时刻的结果发生变化的时候，需要重新积分)：
$$\begin{array}{cc}{\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& (4)\\ {\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& (5)\\ {\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& (6)\end{array}$$根据预积分的要求，需要求相对结果，而且不依赖于上一时刻位姿，因此需要对上式做转换。
由于 ${\mathbf{q}}_{w{b}_t}={\mathbf{q}}_{w{b}_i}\otimes {\mathbf{q}}_{b_i{b}_t}$，把它带入(4)-(6)式可得(把 ${\mathbf{q}}_{w{b}_i}$ 提出来就完事了，积分里面的 ${\mathbf{q}}_{b_i{b}_t}$ 只与 $i$ 时刻后面时刻相关，与本身的姿态已经无关了)：
$$\begin{array}{cc}{\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]}({\mathbf{q}}_{b_i{b}_t}{\mathbf{a}}^{b_t})\delta t^2& (7)\\ {\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& (8)\\ {\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& (9)\end{array}$$可见，此时需要积分的项，就完全和 $i$ 时刻的状态无关了。

为了整理公式，把积分相关的项用下面的式子代替：
$$\begin{array}{rlr}& {\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& (10)\\ & {\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& (11)\\ & {\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& (12)\end{array}$$实际使用中使用离散形式，而非连续形式，由于在解算中，一般采用中值积分方法，即：
$$\begin{array}{rlr}& \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]& (13)\\ & \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]& (14)\end{array}$$那么预积分的离散形式可以表示为，其中 $\mathbf{a}$ 和 $\mathbf{\omega }$ 分别对应上面的两个公式：
$$\begin{array}{rlr}& {\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& (15)\\ & {\mathbf{\beta }}_{b_i{b}_{k+1}}={\mathbf{\beta }}_{b_i{b}_k}+\mathbf{a}\delta t& (16)\\ & {\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]& (17)\end{array}$$经过以上的推导，此时状态更新的公式可以整理为(前面的和前几章的推导几乎一样，就是多了 ${\mathbf{q}}_{w{b}_i}{\mathbf{\alpha }}_{b_i{b}_j}$，${\mathbf{q}}_{w{b}_i}{\mathbf{\beta }}_{b_i{b}_j}$，${\mathbf{q}}_{w{b}_i}{\mathbf{q}}_{b_i{b}_j}$，这三个量也就是预积分要积分的量，而且这几个量和上一时刻的位姿无关)：
$$\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 相等。(也就是上面公式中写的 $b_j^a=b_i^a$, $b_j^g=b_i^g$。这肯定不是严格相等的，滤波时也讲了它有一个随机游走，建立模型的时候要将随机游走，也就是噪声与时间相乘的这一项 ${\mathbf{n}}_{{\mathbf{b}}_k^a}$ 考虑进来。注意，这里相邻的两时刻指的是关键帧时刻。)

还有一点需要特别说明的是，我们认为我们预积分起到一个，无论优化状态关键帧的位姿怎样变化，预积分都不跟着变化的目的，这也是预积分的本来目的。但是现在的预积分并没有实现这个目的，主要原因在于，预积分在积分的时候，陀螺仪是有 bias 的，虽然可以将 bias 减去，但是在积分的时候还没有做优化，这时减去的 bias 是一个不那么精确的 bias。优化了以后 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]$$但是和预积分相关的量，仍然与上一时刻的姿态有关，无法直接加减，因此，把残差修正为以下形式(为了不受前一时刻的影响，对残差部分项左乘了一个 ${\mathbf{q}}_{w{b}_i}^{\ast }$。至于为什么只在这两项上面乘残差，因为只有这两项上面有信息素。残差只需要有差异就可以，这个差异可以表示成任何形式，只要没有改变这个差异原始的性质就可以。比如说下面的残差 ${\mathbf{b}}_j^a-{\mathbf{b}}_i^a$，让其变成 $5\ast ({\mathbf{b}}_j^a-{\mathbf{b}}_i^a)$ 也是可以的。既然可以随意更改残差，那肯定怎样简单就怎样来了)：
$$\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}}_{b_i{b}_j}^{\ast }\otimes \left({\mathbf{q}}_{w{b}_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 姿态残差的雅可比

比如说下面是对 $i$ 时刻求雅可比，残差是 ${\mathbf{q}}_{b_j{b}_i}\otimes \left({\mathbf{q}}_{b_{i}w}\otimes {\mathbf{q}}_{w{b}_j}\right)$，既然是求对 ${\mathbf{q}}_{b_{i}w}$ 的雅可比，那需要在它的上面加一个扰动 $\left[\begin{array}{c}1\\ \frac{1}{2}\delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}\end{array}\right]$。这是固定套路里面的第一步，即对要求雅可比的状态量加上一个扰动。第二步是，加的扰动这个量需要将它移动到最右边去，不移动到最右边去，这个量左边有值右边也有值就求不出雅可比来。所有的推导过程都是围绕这两个过程来做的。看似很复杂，但规律都是相同的。

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$ 时刻加速度 bias 误差的雅可比：
   $$\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 位置残差的雅可比

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