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

• 1. 基于预积分的融合方案流程
• • 1.1 优化问题分析
  • 1.2 预积分的作用
  • 1.3 基于预积分的建图方案流程
• 2. 预积分模型设计
• 3. 预积分在优化中的使用
• • 3.1 使用方法
  • 3.2 残差设计
  • 3.3 残差雅可比的推导
  • • 3.3.1 姿态残差的雅可比
    • 3.3.2 速度残差的雅可比
    • 3.3.3 位置残差的雅可比

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惯性积分就要重新进行，运算量过大。
解决思路：直接计算两帧之间的相对位姿，而不依赖初始值影响，即所谓的预积分。

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

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

1. 初始化lidar和IMU之间的外参；
2. 初始化速度、陀螺仪bias等；
3. 初始化重力；
4. 世界坐标系对齐(组合导航已经对齐)。

2. 预积分模型设计

在第6讲中，已知导航的微分方程如下
p˙wbt=vtwv˙tw=atwq˙wbt=qwbt⊗[012ωbt]\begin{aligned} & \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} \boldsymbol{\omega}^{b_t} \end{array}\right] \end{aligned} ​p˙​wbt​​=vtw​v˙tw​=atw​q˙​wbt​​=qwbt​​⊗[021​ωbt​​]​根据该微分方程，可知从 iii 时刻到 jjj 时刻 IMU的积分结果为
pwbj=pwbi+viwΔt+∬t∈[i,j](qwbtabt−gw)δt2vjw=viw+∫t∈[i,j](qwbtabt−gw)δtqwbj=∫t∈[i,j]qwbt⊗[012ωbt]δt\begin{gathered} \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} \boldsymbol{\omega}^{b_t} \end{array}\right] \delta t \end{gathered} pwbj​​=pwbi​​+viw​Δt+∬t∈[i,j]​(qwbt​​abt​−gw)δt2vjw​=viw​+∫t∈[i,j]​(qwbt​​abt​−gw)δtqwbj​​=∫t∈[i,j]​qwbt​​⊗[021​ωbt​​]δt​根据预积分的要求，需要求相对结果，而且不依赖于上一时刻位姿，因此需要对上式做转换。
由于 qwbt=qwbi⊗qbibt\mathbf{q}_{w b_t}=\mathbf{q}_{w b_i} \otimes \mathbf{q}_{b_i b_t}qwbt​​=qwbi​​⊗qbi​bt​​ ，把它带入(4)-(6)式可得
pwbj=pwbi+viwΔt−12gwΔt2+qwbi[∬t∈[i,j](qbibtabt)δt2vjw=viw−gwΔt+qwbi∫t∈[i,j](qbibtabt)δt]qwbj=qwbi∫t∈[i,j]qbibt⊗[012ωbt]δt\begin{gathered} \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}\left[\iint_{t \in[i, j]}\left(\mathbf{q}_{b_i b_t} \mathbf{a}^{b_t}\right) \delta t^2\right. \\ \left.\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\right] \\ \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} \boldsymbol{\omega}^{b_t} \end{array}\right] \delta t \end{gathered} pwbj​​=pwbi​​+viw​Δt−21​gwΔt2+qwbi​​[∬t∈[i,j]​(qbi​bt​​abt​)δt2vjw​=viw​−gwΔt+qwbi​​∫t∈[i,j]​(qbi​bt​​abt​)δt]qwbj​​=qwbi​​∫t∈[i,j]​qbi​bt​​⊗[021​ωbt​​]δt​可见，此时需要积分的项，就完全和 iii 时刻的状态无关了。

为了整理公式，把积分相关的项用下面的式子 代替
αbibj=∬t∈[i,j](qbibtabt)δt2βbibj=∫t∈[i,j](qbibtabt)δtqbibj=∫t∈[i,j]qbibt⊗[012ωbt]δt\begin{aligned} & \boldsymbol{\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 \\ & \boldsymbol{\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} \boldsymbol{\omega}^{b_t} \end{array}\right] \delta t \end{aligned} ​αbi​bj​​=∬t∈[i,j]​(qbi​bt​​abt​)δt2βbi​bj​​=∫t∈[i,j]​(qbi​bt​​abt​)δtqbi​bj​​=∫t∈[i,j]​qbi​bt​​⊗[021​ωbt​​]δt​实际使用中使用离散形式，而非连续形式，由于 在解算中，一般采用中值积分方法，即
ω=12[(ωbk−bkg)+(ωbk+1−bkg)]a=12[qbibk(abk−bka)+qbibk+1(abk+1−bka)]\begin{aligned} & \boldsymbol{\omega}=\frac{1}{2}\left[\left(\boldsymbol{\omega}^{b_k}-\mathbf{b}_k^g\right)+\left(\boldsymbol{\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{aligned} ​ω=21​[(ωbk​−bkg​)+(ωbk+1​−bkg​)]a=21​[qbi​bk​​(abk​−bka​)+qbi​bk+1​​(abk+1​−bka​)]​那么预积分的离散形式可以表示为
αbibk+1=αbibk+βbibkδt+12aδt2βbibk+1=βbibk+aδtqbibk+1=qbibk⊗[112ωδt]\begin{aligned} & \boldsymbol{\alpha}_{b_i b_{k+1}}=\boldsymbol{\alpha}_{b_i b_k}+\boldsymbol{\beta}_{b_i b_k} \delta t+\frac{1}{2} \mathbf{a} \delta t^2 \\ & \boldsymbol{\beta}_{b_i b_{k+1}}=\boldsymbol{\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} \boldsymbol{\omega} \delta t \end{array}\right] \end{aligned} ​αbi​bk+1​​=αbi​bk​​+βbi​bk​​δt+21​aδt2βbi​bk+1​​=βbi​bk​​+aδtqbi​bk+1​​=qbi​bk​​⊗[121​ωδt​]​经过以上的推导，此时状态更新的公式可以整理为
[pwbjvjwqwbjbjabjg]=[pwbi+viwΔt−12gwΔt2+qwbiαbibjviw−gwΔt+qwbiβbibjqwbiqbibjbiabig]\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} \boldsymbol{\alpha}_{b_i b_j} \\ \mathbf{v}_i^w-\mathbf{g}^w \Delta t+\mathbf{q}_{w b_i} \boldsymbol{\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] ​pwbj​​vjw​qwbj​​bja​bjg​​​=​pwbi​​+viw​Δt−21​gwΔt2+qwbi​​αbi​bj​​viw​−gwΔt+qwbi​​βbi​bj​​qwbi​​qbi​bj​​bia​big​​​需要注意的是，陀螺仪和加速度计的模型为
bk+1a=bka+nbkaδtbk+1g=bkg+nbkgδt\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} bk+1a​=bka​+nbka​​δtbk+1g​=bkg​+nbkg​​δt​即认为bias是在变化的，这样便于估计不同时刻的bias值，而不是整个系统运行时间内都当做常值对待。这更符合低精度mems的实际情况。但在预积分时，由于两个关键帧之间的时间较短，因此认为 iii 和 jjj 时刻的bias相等。

需要注意的一点是，预积分的结果中包含了bias，在优化过程中，bias作为状态量也会发生变化，从而引起预积分结果变化。
为了避免bias变化后，重新做预积分，可以把预积分结果在bias处泰勒展开，表达成下面的形式， 这样就可以根据bias的变化量直接算出新的预积 分结果。
αbibj=α‾bibj+Jbiaαδbia+Jbigαδbigβbibj=β‾bibj+Jbiaβδbia+Jbigβδbigqbibj=q‾bibj⊗[112Jbiqqδbig]\begin{aligned} & \boldsymbol{\alpha}_{b_i b_j}=\overline{\boldsymbol{\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 \\ & \boldsymbol{\beta}_{b_i b_j}=\overline{\boldsymbol{\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{aligned} ​αbi​bj​​=αbi​bj​​+Jbia​α​δbia​+Jbig​α​δbig​βbi​bj​​=β​bi​bj​​+Jbia​β​δbia​+Jbig​β​δbig​qbi​bj​​=q​bi​bj​​⊗[121​Jbiq​q​δbig​​]​其中
Jbiaα=∂αbibj∂δbiaJbigα=∂αbibj∂δbijJbiaβ=∂βbibj∂δbiaJbigβ=∂βbibj∂δbiaJbigq=qbibj∂biq\begin{aligned} \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 \boldsymbol{\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{aligned} Jbia​α​Jbig​α​Jbia​β​Jbig​β​Jbig​q​​=∂δbia​∂αbi​bj​​​=∂δbij​∂αbi​bj​​​=∂δbia​∂βbi​bj​​​=∂δbia​∂βbi​bj​​​=∂biq​qbi​bj​​​​注: 此处暂时不直接给出以上各雅可比的结果，它的推导放在后面进行。

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

3.1 使用方法

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

3.2 残差设计

在优化时，需要知道残差关于状态量的雅可比。由于已知姿态位姿更新的方法如下：
[pwbjqwbjvjwbjabjg]=[pwbi+viwΔt−12gwΔt2+qwbiαbibjqwbiqbibjviw−gwΔt+qwbiβbibjbiabig]\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} \boldsymbol{\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} \boldsymbol{\beta}_{b_i b_j} \\ \mathbf{b}_i^a \\ \mathbf{b}_i^g \end{array}\right] ​pwbj​​qwbj​​vjw​bja​bjg​​​=​pwbi​​+viw​Δt−21​gwΔt2+qwbi​​αbi​bj​​qwbi​​qbi​bj​​viw​−gwΔt+qwbi​​βbi​bj​​bia​big​​​因此，可以很容易写出一种残差形式如下:
[rprqrvrbarbg]=[pwbj−pwbi−viwΔt+12gwΔt2−qwbiαbibj2[qbibj∗⊗(qwbi∗⊗qwbj)]xyzvjw−viw+gwΔt−qwbiβbibjbja−biabjg−big]\left[\begin{array}{c} \mathbf{r}_p \\ \mathbf{r}_q \\ \mathbf{r}_v \\ \mathbf{r}_{b a} \\ \mathbf{r}_{b g} \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} \boldsymbol{\alpha}_{b_i b_j} \\ 2\left[\mathbf{q}_{b_i b_j}^* \otimes\left(\mathbf{q}_{w b_i}^* \otimes \mathbf{q}_{w b_j}\right)\right]_{x y z} \\ \mathbf{v}_j^w-\mathbf{v}_i^w+\mathbf{g}^w \Delta t-\mathbf{q}_{w b_i} \boldsymbol{\beta}_{b_i b_j} \\ \mathbf{b}_j^a-\mathbf{b}_i^a \\ \mathbf{b}_j^g-\mathbf{b}_i^g \end{array}\right] ​rp​rq​rv​rba​rbg​​​=​pwbj​​−pwbi​​−viw​Δt+21​gwΔt2−qwbi​​αbi​bj​​2[qbi​bj​∗​⊗(qwbi​∗​⊗qwbj​​)]xyz​vjw​−viw​+gwΔt−qwbi​​βbi​bj​​bja​−bia​bjg​−big​​​但是和预积分相关的量，仍然与上一时刻的姿态有关，无法直接加减，因此，把残差修正为以下形式
[rprqrvrbarbg]=[qwbi∗(pwbj−pwbi−viwΔt+12gwΔt2)−αbibj2[qqbj∗⊗(qwi∗⊗qwbj)]xyzqwbi∗(vjw−viw+gwΔt)−βbibjbja−biabjg−big]\left[\begin{array}{c} \mathbf{r}_p \\ \mathbf{r}_q \\ \mathbf{r}_v \\ \mathbf{r}_{b a} \\ \mathbf{r}_{b g} \end{array}\right]=\left[\begin{array}{c} \mathbf{q}_{w b_i}^*\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)-\boldsymbol{\alpha}_{b_i b_j} \\ 2\left[\mathbf{q}_{\mathbf{q}_{b_j}^*} \otimes\left(\mathbf{q}_{w_i}^* \otimes \mathbf{q}_{w b_j}\right)\right]_{x y z} \\ \mathbf{q}_{w b_i}^*\left(\mathbf{v}_j^w-\mathbf{v}_i^w+\mathbf{g}^w \Delta t\right)-\boldsymbol{\beta}_{b_i b_j} \\ \mathbf{b}_j^a-\mathbf{b}_i^a \\ \mathbf{b}_j^g-\mathbf{b}_i^g \end{array}\right] ​rp​rq​rv​rba​rbg​​​=​qwbi​∗​(pwbj​​−pwbi​​−viw​Δt+21​gwΔt2)−αbi​bj​​2[qqbj​∗​​⊗(qwi​∗​⊗qwbj​​)]xyz​qwbi​∗​(vjw​−viw​+gwΔt)−βbi​bj​​bja​−bia​bjg​−big​​​待优化的变量是 [pwbiqwbiviwbiabig][pwbjqwbjvjwbjabjg]\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][pwbi​​​qwbi​​​viw​​bia​​big​​][pwbj​​​qwbj​​​vjw​​bja​​bjg​​]
但在实际使用中，往往都是使用扰动量，因此实际是对以下变量求雅可比
[δpwbiδθwbiδviwδbiaδbig][δpwbjδθwbjδvjwδbjaδbjg]\begin{aligned} & {\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{aligned} ​[δpwbi​​​δθwbi​​​δviw​​δbia​​δbig​​][δpwbj​​​δθwbj​​​δvjw​​δbja​​δbjg​​]​此处只对几个比较复杂的雅可比进行推导，其余比较简单，感兴趣的可自行完成。

3.3 残差雅可比的推导

3.3.1 姿态残差的雅可比

1. 对 iii 时刻姿态误差的雅可比
   ∂rq∂δθbibi′=∂2[qbjbi⊗(qbiw⊗qwbj)]xyz∂δθbibi′=∂2[qbi,bj∗⊗(qwbi⊗[112δθbibi′])∗⊗qwbj]xyz∂δθbibi′=∂−2[(qbibj∗⊗(qwbi⊗[112δθbibi′])∗⊗qwbj)∗]xyz∂δθbibi=∂−2[qwbj∗⊗(qwbi⊗[112δθbibi′])⊗qbibj]xyz∂δθbibi′\begin{aligned} \frac{\partial \mathbf{r}_q}{\partial \delta \boldsymbol{\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]_{x y z}}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} \\ & =\frac{\partial 2\left[\mathbf{q}_{b_i, b_j}^* \otimes\left(\mathbf{q}_{w b_i} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \boldsymbol{\theta}_{b_i b_i^{\prime}} \end{array}\right]\right)^* \otimes \mathbf{q}_{w b_j}\right]_{x y z}}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} \\ & =\frac{\partial-2\left[\left(\mathbf{q}_{b_i b_j}^* \otimes\left(\mathbf{q}_{w b_i} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \boldsymbol{\theta}_{b_i} b_i^{\prime} \end{array}\right]\right)^* \otimes \mathbf{q}_{w b_j}\right)^*\right]_{x y z}}{\partial \delta \boldsymbol{\theta}_{b_i b_i}} \\ & =\frac{\partial-2\left[\mathbf{q}_{w b_j}^* \otimes\left(\mathbf{q}_{w b_i} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \boldsymbol{\theta}_{b_i b_i^{\prime}} \end{array}\right]\right) \otimes \mathbf{q}_{b_i b_j}\right]_{x y z}}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} \end{aligned} ∂δθbi​bi′​​∂rq​​​=∂δθbi​bi′​​∂2[qbj​bi​​⊗(qbi​w​⊗qwbj​​)]xyz​​=∂δθbi​bi′​​∂2[qbi​,bj​∗​⊗(qwbi​​⊗[121​δθbi​bi′​​​])∗⊗qwbj​​]xyz​​=∂δθbi​bi​​∂−2[(qbi​bj​∗​⊗(qwbi​​⊗[121​δθbi​​bi′​​])∗⊗qwbj​​)∗]xyz​​=∂δθbi​bi′​​∂−2[qwbj​∗​⊗(qwbi​​⊗[121​δθbi​bi′​​​])⊗qbi​bj​​]xyz​​​上式可以化简为
   ∂rq∂δθbibi′=−2[0I]∂qwbj∗⊗(qwbi⊗[112δθbibi′])⊗qbibj∂δθbibi′=−2[0I]∂[qwbj∗⊗qwbi]L[qbibj]R[112δθbibi′]∂δθbibi′=−2[0I][qwbj∗⊗qwbi]L[qbibj]R[012I]\begin{aligned} \frac{\partial \mathbf{r}_q}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} & =-2\left[\begin{array}{ll} \mathbf{0} & \mathbf{I} \end{array}\right] \frac{\partial \mathbf{q}_{w b_j}^* \otimes\left(\mathbf{q}_{w b_i} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \boldsymbol{\theta}_{b_i b_i^{\prime}} \end{array}\right]\right) \otimes \mathbf{q}_{b_i b_j}}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} \\ & =-2\left[\begin{array}{ll} \mathbf{0} & \mathbf{I} \end{array}\right] \frac{\partial\left[\mathbf{q}_{w b_j}^* \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 \boldsymbol{\theta}_{b_i b_i^{\prime}} \end{array}\right]}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} \\ & =-2\left[\begin{array}{ll} \mathbf{0} & \mathbf{I} \end{array}\right]\left[\mathbf{q}_{w b_j}^* \otimes \mathbf{q}_{w b_i}\right]_L\left[\mathbf{q}_{b_i b_j}\right]_R\left[\begin{array}{c} \mathbf{0} \\ \frac{1}{2} \mathbf{I} \end{array}\right] \end{aligned} ∂δθbi​bi′​​∂rq​​​=−2[0​I​]∂δθbi​bi′​​∂qwbj​∗​⊗(qwbi​​⊗[121​δθbi​bi′​​​])⊗qbi​bj​​​=−2[0​I​]∂δθbi​bi′​​∂[qwbj​∗​⊗qwbi​​]L​[qbi​bj​​]R​[121​δθbi​bi′​​​]​=−2[0​I​][qwbj​∗​⊗qwbi​​]L​[qbi​bj​​]R​[021​I​]​

2. 对 jjj 时刻姿态误差的雅可比
   ∂rq∂δθbjbj′=∂2[qbibj∗⊗qwbi∗⊗qwbj⊗[112δθbjbj′]]xyz∂δθbjbj′=∂2[[qbibj∗⊗qwbi∗⊗qwbj]L[112δθbjbj′]]xyz∂δθbjbj′=2[0I][qbibj∗⊗qwbi∗⊗qwbj]L[012I]\begin{aligned} \frac{\partial \mathbf{r}_q}{\partial \delta \boldsymbol{\theta}_{b_j b_j^{\prime}}} & =\frac{\partial 2\left[\mathbf{q}_{b_i b_j}^* \otimes \mathbf{q}_{w b_i}^* \otimes \mathbf{q}_{w b_j} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \boldsymbol{\theta}_{b_j b_j^{\prime}} \end{array}\right]\right]_{x y z}}{\partial \delta \boldsymbol{\theta}_{b_j b_j^{\prime}}} \\ & =\frac{\partial 2\left[\left[\mathbf{q}_{b_i b_j}^* \otimes \mathbf{q}_{w b_i}^* \otimes \mathbf{q}_{w b_j}\right]_L\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \boldsymbol{\theta}_{b_j b_j^{\prime}} \end{array}\right]\right]_{x y z}}{\partial \delta \boldsymbol{\theta}_{b_j b_j^{\prime}}} \\ & =2\left[\begin{array}{ll} \mathbf{0} & \mathbf{I} \end{array}\right]\left[\mathbf{q}_{b_i b_j}^* \otimes \mathbf{q}_{w b_i}^* \otimes \mathbf{q}_{w b_j}\right]_L\left[\begin{array}{c} \mathbf{0} \\ \frac{1}{2} \mathbf{I} \end{array}\right] \end{aligned} ∂δθbj​bj′​​∂rq​​​=∂δθbj​bj′​​∂2[qbi​bj​∗​⊗qwbi​∗​⊗qwbj​​⊗[121​δθbj​bj′​​​]]xyz​​=∂δθbj​bj′​​∂2[[qbi​bj​∗​⊗qwbi​∗​⊗qwbj​​]L​[121​δθbj​bj′​​​]]xyz​​=2[0​I​][qbi​bj​∗​⊗qwbi​∗​⊗qwbj​​]L​[021​I​]​

3. 对 iii 时刻陀螺仪bias误差的雅可比
   ∂rq∂δbig=∂2[(qbibj⊗[112Jbiqδbigq])∗⊗qwbi∗⊗qwbj]xyz∂δbig=∂−2[((qbibj⊗[112Jbiqqδbig])∗⊗qwbi∗⊗qwbj)∗]xyz∂δbig=∂−2[qwbj∗⊗qwbi⊗(qbibj⊗[112Jbioδbig])]xyz∂δbig=−2[0I][qwbj∗⊗qwbi⊗qbibj]L[012Jbiqq]\begin{aligned} & \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)^* \otimes \mathbf{q}_{w b_i}^* \otimes \mathbf{q}_{w b_j}\right]_{x y z}}{\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)^* \otimes \mathbf{q}_{w b_i}^* \otimes \mathbf{q}_{w b_j}\right)^*\right]_{x y z}}{\partial \delta \mathbf{b}_i^g} \\ & =\frac{\partial-2\left[\mathbf{q}_{w b_j}^* \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]_{x y z}}{\partial \delta \mathbf{b}_i^g} \\ & =-2\left[\begin{array}{ll} \mathbf{0} & \mathbf{I} \end{array}\right]\left[\mathbf{q}_{w b_j}^* \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{aligned} ​∂δbig​∂rq​​=∂δbig​∂2[(qbi​bj​​⊗[121​Jbiq​δbig​q​​])∗⊗qwbi​∗​⊗qwbj​​]xyz​​=∂δbig​∂−2[((qbi​bj​​⊗[121​Jbiq​q​δbig​​])∗⊗qwbi​∗​⊗qwbj​​)∗]xyz​​=∂δbig​∂−2[qwbj​∗​⊗qwbi​​⊗(qbi​bj​​⊗[121​Jbio​​δbig​​])]xyz​​=−2[0​I​][qwbj​∗​⊗qwbi​​⊗qbi​bj​​]L​[021​Jbiq​q​​]​

3.3.2 速度残差的雅可比

1. 对 iii 时刻姿态误差的雅可比
   ∂rv∂δθbibi=∂(qwbi⊗[112δθbibi])−1(vjw−viw+gwΔt)∂δθbibi′=∂(Rwbiexp⁡([δθbibi′]×))−1(vjw−viw+gwΔt)∂δθbibi′=∂exp⁡([−δθbibi]×)Rbiw(vjw−viw+gwΔt)∂δθbibi′=∂(I−[δθbibi]×)Rbiw(vjw−viw+gwΔt)∂δθbibi′=∂−[δθbibi]×Rbiw(vjw−viw+gwΔt)∂δθbibi′=[Rbiw(vjw−viw+gwΔt)]×\begin{aligned} \frac{\partial \mathbf{r}_v}{\partial \delta \boldsymbol{\theta}_{b_i b_i}} & =\frac{\partial\left(\mathbf{q}_{w b_i} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \boldsymbol{\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 \boldsymbol{\theta}_{b_i b_i^{\prime}}} \\ & =\frac{\partial\left(\mathbf{R}_{w b_i} \exp \left(\left[\delta \boldsymbol{\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 \boldsymbol{\theta}_{b_i b_i^{\prime}}} \\ & =\frac{\partial \exp \left(\left[-\delta \boldsymbol{\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 \boldsymbol{\theta}_{b_i} b_i^{\prime}} \\ & =\frac{\partial\left(\mathbf{I}-\left[\delta \boldsymbol{\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 \boldsymbol{\theta}_{b_i b_i^{\prime}}} \\ & =\frac{\partial-\left[\delta \boldsymbol{\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 \boldsymbol{\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{aligned} ∂δθbi​bi​​∂rv​​​=∂δθbi​bi′​​∂(qwbi​​⊗[121​δθbi​bi​​​])−1(vjw​−viw​+gwΔt)​=∂δθbi​bi′​​∂(Rwbi​​exp([δθbi​bi′​​]×​))−1(vjw​−viw​+gwΔt)​=∂δθbi​​bi′​∂exp([−δθbi​bi​​]×​)Rbi​w​(vjw​−viw​+gwΔt)​=∂δθbi​bi′​​∂(I−[δθbi​bi​​]×​)Rbi​w​(vjw​−viw​+gwΔt)​=∂δθbi​bi′​​∂−[δθbi​bi​​]×​Rbi​w​(vjw​−viw​+gwΔt)​=[Rbi​w​(vjw​−viw​+gwΔt)]×​​
2. 对 iii 时刻速度误差的雅可比
   ∂rv∂δviw=−Rbiw\frac{\partial \mathbf{r}_v}{\partial \delta \mathbf{v}_i^w}=-\mathbf{R}_{b_i w} ∂δviw​∂rv​​=−Rbi​w​
3. 对 iii 时刻加速度biasi吴差的雅可比
   ∂rv∂δbia=−∂βbibj∂δbia=−Jbiaβ\frac{\partial \mathbf{r}_v}{\partial \delta \mathbf{b}_i^a}=-\frac{\partial \boldsymbol{\beta}_{b_i b_j}}{\partial \delta \mathbf{b}_i^a}=-\mathbf{J}_{b_i^a}^\beta ∂δbia​∂rv​​=−∂δbia​∂βbi​bj​​​=−Jbia​β​

3.3.3 位置残差的雅可比

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