简介
在 VIO 残差函数的构建 这篇博客中,我们总结了在 VIO 中 视觉残差和 IMU 残差时怎么构建的,而在 BA 中,我们还需要进行相对应雅可比的计算来进行优化,这篇博客主要就是总结怎么计算的视觉残差和 IMU 残差的雅可比。
视觉残差
根据上一篇博客我们知道,对于每一个特征点, 在第 $j$
帧中被观测到并构建的视觉残差的形式为:
$$
\boldsymbol{r}_c = \begin{bmatrix}
\frac{x_{c_j}}{z_{c_j}} - u_{c_j}\\
\frac{y_{c_j}}{z_{c_j}} - v_{c_j}
\end{bmatrix}
$$
注意,由于我们在优化过程中不估计相机内参,所以这里(以及之后的)推导直接用该特征点在相机坐标系下归一化平面($z=1$
的平面)下的坐标来表示,从特征点的像素坐标到归一化坐标需要借助相机内参进行反投影,这里不再赘述,可以参考网上的资料。
每一个特征点都有可能在不同帧被观测到,以第 $i$
帧作为观测到该特征点的第一帧(参考帧),这里我们用 $\boldsymbol{f}_{c}$
, $\boldsymbol{f}_{b}$
以及 $\boldsymbol{f}_{w}$
来分别作为该特征点在相机坐标系、机体(IMU)坐标系以及世界(全局)坐标系的坐标表示,其中 $b, c$
的下表表示对应在第几帧的机体或者相机坐标系。那么对于该特征点,其在参考帧的相机坐标为:
$$
\boldsymbol{f}_{c_i} = \frac{1}{\lambda}\begin{bmatrix}
u_{c_i}\\
v_{c_i}\\
1
\end{bmatrix}
$$
其在第 $j$
帧下相机坐标系的坐标为:
$$
\begin{aligned}
\begin{bmatrix}
\boldsymbol{f}_{c_j}\\
1
\end{bmatrix} &= \boldsymbol{T}_{bc}^{-1}\boldsymbol{T}_{wb_j}^{-1}\boldsymbol{T}_{wb_i}\boldsymbol{T}_{bc}
\begin{bmatrix}
\boldsymbol{f}_{c_i}\\
1\\
\end{bmatrix}
\end{aligned}
$$
其中,$(x_f, y_f, z_f)$
为特征点在某一时刻相机坐标系下的坐标值,是要估计的量;$(u_f, v_f)$
是该特征值在那一时刻相机归一化平面下的坐标。我们以 IMU 的位姿作为机体的参考位姿,因此跟视觉残差相关的参数有:第 i 帧下的位姿,包括位置 $\boldsymbol{p}_{wb_i}$
和姿态 $\boldsymbol{R}_{wb_i}$
;第 j 帧下的位姿 $\boldsymbol{p}_{wb_j}, \boldsymbol{q}_{wb_j} (\boldsymbol{R}_{wb_j})$
,以及相机和 IMU 之间的外参 $\boldsymbol{p}_{bc}, \boldsymbol{R}_{bc}$
,还有参考帧下的逆深度 $\lambda$
,下面依次对其进行求导得到雅可比。
首先根据链式法则,可以先将残差对 $f_{c_j}$
求导,然后将 $f_{c_j}$
对各待估计量求导即可:
$$
J_c=\frac{\partial\boldsymbol{r}_c}{\partial\boldsymbol{x}}= \frac{\partial\boldsymbol{r}_c}{\partial\boldsymbol{f}_{c_j}}\frac{\partial\boldsymbol{f}_{c_j}}{\partial\boldsymbol{x}}
$$
首先求第一部分,过程比较简单:
$$
\begin{aligned}
\frac{\partial\boldsymbol{r}_c}{\partial\boldsymbol{f}_{c_j}} &= \begin{bmatrix}
\frac{1}{z_{c_j}} & 0 & -\frac{x_{c_j}}{z_{c_j}^2}\\
0 & \frac{1}{z_{c_j}} & -\frac{y_{c_j}}{z_{c_j}^2}
\end{bmatrix}
\end{aligned}
$$
然后将第二部分对各个估计量求导,这里为了方便运算,需要将上式的旋转矩阵转换为旋转+平移的转换方式,如下所示:
$$
\begin{aligned}
\boldsymbol{f}_{b_i} =& \boldsymbol{R}_{bc}\boldsymbol{f}_{c_i} + \boldsymbol{p}_{bc}\\
\boldsymbol{f}_{w} =& \boldsymbol{R}_{wb_i}\boldsymbol{f}_{b_i} + \boldsymbol{p}_{wb_i}\\
\boldsymbol{f}_{b_j} =& \boldsymbol{R}_{b_jw}\boldsymbol{f}_{w} + \boldsymbol{p}_{b_jw}\\
=& \boldsymbol{R}_{wb_j}^T\boldsymbol{f}_{w}-\boldsymbol{R}^T_{wb_j}\boldsymbol{p}_{wb_j}\\
\boldsymbol{f}_{c_j} =& \boldsymbol{R}_{bc}^T\boldsymbol{f}_{b_j} -\boldsymbol{R}^T_{bc}\boldsymbol{p}_{bc}\\
\Rightarrow\boldsymbol{f}_{c_j} =&\boldsymbol{R}_{bc}^T(\boldsymbol{R}_{wb_j}^T(\boldsymbol{R}_{wb_i}(\boldsymbol{R}_{bc}\boldsymbol{f}_{c_i} + \boldsymbol{p}_{bc}) + \boldsymbol{p}_{wb_i})-\boldsymbol{R}^T_{wb_j}\boldsymbol{p}_{wb_j})-\boldsymbol{R}^T_{bc}\boldsymbol{p}_{bc}\\
=& \boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{R}_{bc}\boldsymbol{f}_{c_i} \\
&+\boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{p}_{bc}\\
&+ \boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{p}_{wb_i}\\
&- \boldsymbol{R}_{bc}^T\boldsymbol{R}^T_{wb_j}\boldsymbol{p}_{wb_j}\\
&- \boldsymbol{R}^T_{bc}\boldsymbol{p}_{bc}
\end{aligned}
$$
通过上述转换,不同参数之间的关系比较清晰了(至少对于位置而言如此),下面分别对各参数进行求导:
- 逆深度
$\lambda$
:
对逆深度同样可以借助链式法则:
$$
\begin{aligned}
\frac{\partial\boldsymbol{f}_{c_j}}{\partial\lambda} &= \frac{\partial\boldsymbol{f}_{c_j}}{\partial\boldsymbol{f}_{c_i}}\frac{\partial\boldsymbol{f}_{c_i}}{\partial\lambda}\\
&= \boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{R}_{bc}(-\frac{1}{\lambda^2}
\begin{bmatrix}
u_{c_i}\\
v_{c_i}\\
1
\end{bmatrix})\\
&= -\frac{1}{\lambda^2}\boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{R}_{bc}\begin{bmatrix}
u_{c_i}\\
v_{c_i}\\
1
\end{bmatrix}\\
&= -\frac{1}{\lambda}\boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{R}_{bc}\boldsymbol{f}_{c_i}
\end{aligned}
$$
- i 时刻机体位置
$\boldsymbol{p}_{wb_i}$
:
如下所示:
$$
\begin{aligned}
\frac{\partial\boldsymbol{f}_{c_j}}{\partial\boldsymbol{p}_{wb_i}} = \boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T
\end{aligned}
$$
- i 时刻机体姿态
$\boldsymbol{q}_{wb_i}(\boldsymbol{R}_{wb_i})$
:
只关注 $\boldsymbol{f}_{c_j}$
中与 i 时刻姿态有关的部分并进行整理:
$$
\begin{aligned}
&\boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{R}_{bc}\boldsymbol{f}_{c_i}
+\boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{p}_{bc}\\
=& \boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}(\boldsymbol{R}_{bc}\boldsymbol{f}_{c_i} +\boldsymbol{p}_{bc})\\
=& \boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{f}_{b_i}
\end{aligned}
$$
利用右扰动模型求导,思路可以参考 一些常见的关于旋转的函数对旋转求导过程的推导:
$$
\begin{aligned}
\frac{\partial\boldsymbol{f}_{c_j}}{\partial\boldsymbol{q}_{wb_i}} &= \lim_{\boldsymbol{\psi}\rightarrow 0}\frac{\boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\exp{(\boldsymbol{\psi}^\wedge)}\boldsymbol{f}_{b_i} - \boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{f}_{b_i}}{\boldsymbol{\psi}}\\
&\approx\lim_{\boldsymbol{\psi}\rightarrow 0}\frac{\boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}(\boldsymbol{I} + \boldsymbol{\psi}^\wedge)\boldsymbol{f}_{b_i} - \boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{f}_{b_i}}{\boldsymbol{\psi}}\\
&=\lim_{\boldsymbol{\psi}\rightarrow 0}\frac{\boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}(\boldsymbol{\psi}^\wedge\boldsymbol{f}_{b_i})}{\boldsymbol{\psi}}\\
&= \lim_{\boldsymbol{\psi}\rightarrow 0}\frac{\boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}(-\boldsymbol{f}_{b_i}^\wedge\boldsymbol{\psi})}{\boldsymbol{\psi}}\\
&= -\boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{f}_{b_i}^\wedge
\end{aligned}
$$
- j 时刻机体位置
$\boldsymbol{p}_{wb_j}$
:
思路同 $\boldsymbol{p}_{wb_i}$
$$
\begin{aligned}
\frac{\partial\boldsymbol{f}_{c_j}}{\partial\boldsymbol{p}_{wb_i}} = -\boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T
\end{aligned}
$$
- j 时刻机体姿态
$\boldsymbol{q}_{wb_j}(\boldsymbol{R}_{wb_j})$
思路同 $\boldsymbol{q}_{wb_i}$
,同样去除 $\boldsymbol{f}_{c_j}$
中无关的部分,并进行整理:
$$
\begin{aligned}
&\boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{R}_{bc}\boldsymbol{f}_{c_i} \\
&+\boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{p}_{bc}\\
&+ \boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{p}_{wb_i}\\
&-\boldsymbol{R}_{bc}^T\boldsymbol{R}^T_{wb_j}\boldsymbol{p}_{wb_j}\\
=& \boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{f}_{b_i} \\
&+ \boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{p}_{wb_i}\\
&-\boldsymbol{R}_{bc}^T\boldsymbol{R}^T_{wb_j}\boldsymbol{p}_{wb_j}\\
=& \boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T(\boldsymbol{f}_{w}-\boldsymbol{p}_{wb_j})
\end{aligned}
$$
根据 $\boldsymbol{q}_{wb_i}$
的思路,求导结果为:
$$
\begin{aligned}
\frac{\partial\boldsymbol{f}_{c_j}}{\partial\boldsymbol{q}_{wb_i}} &= \lim_{\boldsymbol{\psi}\rightarrow 0}\frac{\boldsymbol{R}_{bc}^T(\boldsymbol{R}_{wb_j}\exp{(\boldsymbol{\psi}^\wedge)})^T(\boldsymbol{f}_{w}-\boldsymbol{p}_{wb_j})- \boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T(\boldsymbol{f}_{w}-\boldsymbol{p}_{wb_j})}{\boldsymbol{\psi}}\\
&= \lim_{\boldsymbol{\psi}\rightarrow 0}\frac{\boldsymbol{R}_{bc}^T\exp{(-\boldsymbol{\psi}^\wedge)}\boldsymbol{R}_{wb_j}^T(\boldsymbol{f}_{w}-\boldsymbol{p}_{wb_j})- \boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T(\boldsymbol{f}_{w}-\boldsymbol{p}_{wb_j})}{\boldsymbol{\psi}}\\
&\approx \lim_{\boldsymbol{\psi}\rightarrow 0}\frac{\boldsymbol{R}_{bc}^T(\boldsymbol{I}-\boldsymbol{\psi}^\wedge)\boldsymbol{R}_{wb_j}^T(\boldsymbol{f}_{w}-\boldsymbol{p}_{wb_j})- \boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T(\boldsymbol{f}_{w}-\boldsymbol{p}_{wb_j})}{\boldsymbol{\psi}}\\
&= \lim_{\boldsymbol{\psi}\rightarrow 0}\frac{\boldsymbol{R}_{bc}^T(-\boldsymbol{\psi}^\wedge)\boldsymbol{R}_{wb_j}^T(\boldsymbol{f}_{w}-\boldsymbol{p}_{wb_j})}{\boldsymbol{\psi}}\\
&= \boldsymbol{R}_{bc}^T(\boldsymbol{R}_{wb_j}^T(\boldsymbol{f}_{w}-\boldsymbol{p}_{wb_j}))^\wedge\\
&= \boldsymbol{R}_{bc}^T(\boldsymbol{f}_{b_j})^\wedge
\end{aligned}
$$
- 相机外参——相机在 Body 坐标系中的位置
$\boldsymbol{p}_{bc}$
:
思路同 $\boldsymbol{p}_{wb_i}$
$$
\begin{aligned}
\frac{\partial\boldsymbol{f}_{c_j}}{\partial\boldsymbol{p}_{bc}} &= \boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i} - \boldsymbol{R}^T_{bc}\\
&= \boldsymbol{R}_{bc}^T(\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i} - \boldsymbol{I})
\end{aligned}
$$
- 相机外参——相机在 Body 坐标系中的姿态
$\boldsymbol{q}_{bc}$
:
思路同 $\boldsymbol{q}_{wb_i}$
,同样去除 $\boldsymbol{f}_{c_j}$
中无关的部分,并进行整理,这里有两部分:
$$
\begin{aligned}
\boldsymbol{f}_{b_i} =& \boldsymbol{R}_{bc}\boldsymbol{f}_{c_i} + \boldsymbol{p}_{bc}\\
\boldsymbol{f}_{w} =& \boldsymbol{R}_{wb_i}\boldsymbol{f}_{b_i} + \boldsymbol{p}_{wb_i}\\
\boldsymbol{f}_{b_j} =& \boldsymbol{R}_{b_jw}\boldsymbol{f}_{w} + \boldsymbol{p}_{b_jw}\\
=& \boldsymbol{R}_{wb_j}^T\boldsymbol{f}_{w}-\boldsymbol{R}^T_{wb_j}\boldsymbol{p}_{wb_j}\\
\boldsymbol{f}_{c_j} =& \boldsymbol{R}_{bc}^T\boldsymbol{f}_{b_j} -\boldsymbol{R}^T_{bc}\boldsymbol{p}_{bc}\\
\Rightarrow\boldsymbol{f}_{c_j} =&\boldsymbol{R}_{bc}^T(\boldsymbol{R}_{wb_j}^T(\boldsymbol{R}_{wb_i}(\boldsymbol{R}_{bc}\boldsymbol{f}_{c_i} + \boldsymbol{p}_{bc}) + \boldsymbol{p}_{wb_i})-\boldsymbol{R}^T_{wb_j}\boldsymbol{p}_{wb_j})-\boldsymbol{R}^T_{bc}\boldsymbol{p}_{bc}\\
=& \boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{R}_{bc}\boldsymbol{f}_{c_i} \\
&+\boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{p}_{bc}\\
&+ \boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{p}_{wb_i}\\
&- \boldsymbol{R}_{bc}^T\boldsymbol{R}^T_{wb_j}\boldsymbol{p}_{wb_j}\\
&- \boldsymbol{R}^T_{bc}\boldsymbol{p}_{bc}
\end{aligned}
$$
第一部分:
$$
\boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{R}_{bc}\boldsymbol{f}_{c_i}
$$
求导过程为(注意有两个地方需要加扰动),这里思路和上面类似,为了节省空间省略掉加扰动和近似以及和原变量相减的过程,中间省去二阶小量:
$$
\begin{aligned}
&\frac{\partial(\boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{R}_{bc}\boldsymbol{f}_{c_i})}{\partial\boldsymbol{q}_{bc}}\\
=& \lim_{\boldsymbol{\psi}\rightarrow 0}\frac{-\boldsymbol{\psi}^\wedge\boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{R}_{bc}\boldsymbol{f}_{c_i} + \boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{R}_{bc}\boldsymbol{\psi}^\wedge\boldsymbol{f}_{c_i} - \boldsymbol{\psi}^\wedge\boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{R}_{bc}\boldsymbol{\psi}^\wedge\boldsymbol{f}_{c_i}}{\boldsymbol{\psi}}\\
\approx&\lim_{\boldsymbol{\psi}\rightarrow 0}\frac{-\boldsymbol{\psi}^\wedge\boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{R}_{bc}\boldsymbol{f}_{c_i} + \boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{R}_{bc}\boldsymbol{\psi}^\wedge\boldsymbol{f}_{c_i}}{\boldsymbol{\psi}}\\
=&\lim_{\boldsymbol{\psi}\rightarrow 0}\frac{(\boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{R}_{bc}\boldsymbol{f}_{c_i})^\wedge\boldsymbol{\psi} + \boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{R}_{bc}\boldsymbol{f}_{c_i}^\wedge\boldsymbol{\psi}}{\boldsymbol{\psi}}\\
=&(\boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{R}_{bc}\boldsymbol{f}_{c_i})^\wedge - \boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{R}_{bc}\boldsymbol{f}_{c_i}^\wedge\\
\end{aligned}
$$
第二部分:
$$
\begin{aligned}
&\boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{R}_{wb_i}\boldsymbol{p}_{bc}
+ \boldsymbol{R}_{bc}^T\boldsymbol{R}_{wb_j}^T\boldsymbol{p}_{wb_i}
- \boldsymbol{R}_{bc}^T\boldsymbol{R}^T_{wb_j}\boldsymbol{p}_{wb_j}
- \boldsymbol{R}^T_{bc}\boldsymbol{p}_{bc}\\
=&\boldsymbol{R}_{bc}^T(\boldsymbol{R}_{wb_j}^T(\boldsymbol{R}_{wb_i}\boldsymbol{p}_{bc} + \boldsymbol{p}_{wb_i} - \boldsymbol{p}_{wb_j}) - \boldsymbol{p}_{bc})
\end{aligned}
$$
将 $\boldsymbol{R}_{bc}^T$
后面的部分视为一个整体,根据上面的推导过程很简单可以得到结果为:
$$
\begin{aligned}
&\frac{\partial(\boldsymbol{R}_{bc}^T(\boldsymbol{R}_{wb_j}^T(\boldsymbol{R}_{wb_i}\boldsymbol{p}_{bc} + \boldsymbol{p}_{wb_i} - \boldsymbol{p}_{wb_j}) - \boldsymbol{p}_{bc}))}{\partial\boldsymbol{q}_{bc}}\\
=&(\boldsymbol{R}_{bc}^T(\boldsymbol{R}_{wb_j}^T(\boldsymbol{R}_{wb_i}\boldsymbol{p}_{bc} + \boldsymbol{p}_{wb_i} - \boldsymbol{p}_{wb_j}) - \boldsymbol{p}_{bc}))^\wedge
\end{aligned}
$$
至此,视觉误差的雅可比求导推导完成。
IMU 残差
IMU 的残差为:
$$
\begin{aligned}
\begin{bmatrix}
\boldsymbol{r}_p\\
\boldsymbol{r}_v\\
\boldsymbol{r}_q\\
\boldsymbol{r}_{ba}\\
\boldsymbol{r}_{bg}\\
\end{bmatrix}_{15\times 1} &=
\begin{bmatrix}
\delta\boldsymbol{p}_{b_ib_j}\\
\delta\boldsymbol{v}_{b_ib_j}\\
\delta\boldsymbol{\theta}_{b_ib_j}\\
\delta\boldsymbol{b}^a_{b_ib_j}\\
\delta\boldsymbol{b}^g_{b_ib_j}\\
\end{bmatrix}\\
&=
\begin{bmatrix}
\boldsymbol{R}_{b_iw}(\boldsymbol{p}_{wb_j} - \boldsymbol{p}_{wb_i} - \boldsymbol{v}^w_i\Delta t + \frac{1}{2}\boldsymbol{g}^w\Delta t^2) - \boldsymbol{\alpha}_{b_ib_j}\\
\boldsymbol{R}_{b_iw}(\boldsymbol{v}_j^w - \boldsymbol{v}^w_i + \boldsymbol{g}^w\Delta t) - \boldsymbol{\beta}_{b_ib_j}\\
2\left[\boldsymbol{\gamma}_{b_ib_j}^{-1}\otimes(\boldsymbol{q}_{b_iw}\otimes\boldsymbol{q}_{wb_j})\right]_{xyz}\\
\boldsymbol{b}_j^a - \boldsymbol{b}_i^a\\
\boldsymbol{b}_j^g - \boldsymbol{b}_i^g
\end{bmatrix}
\end{aligned}
$$
其中,预积分量的计算方式为:
$$
\begin{aligned}
\boldsymbol{\omega} &= \frac{1}{2}((\boldsymbol{\omega}^{b_k}-\boldsymbol{b}^g_k)+(\boldsymbol{\omega}^{b_{k+1}}-\boldsymbol{b}^g_{k+1}))\\
\boldsymbol{a} &= \frac{1}{2}(\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_k} -\boldsymbol{b}^g_k) + \boldsymbol{R}_{b_ib_{k+1}}(\boldsymbol{a}^{b_{k+1}} -\boldsymbol{b}^g_{k+1}))\\
{\boldsymbol{\alpha}}_{b_ib_{k+1}} &= {\boldsymbol{\alpha}}_{b_ib_k} + {\boldsymbol{\beta}}_{b_ib_k}\delta t + \frac{1}{2}\boldsymbol{a}\delta t^2\\
{\boldsymbol{\beta}}_{b_ib_{k+1}} &= {\boldsymbol{\beta}}_{b_ib_k} + \boldsymbol{a}\delta t\\
\boldsymbol{{\boldsymbol{\gamma}}}_{b_ib_{k+1}}&= \boldsymbol{{\boldsymbol{\gamma}}}_{b_ib_k} \otimes\begin{bmatrix}
1\\
\frac{1}{2}\boldsymbol{\omega}\delta t
\end{bmatrix}
\end{aligned}
$$
另外,预积分涉及到零偏的部分近似更新方式为:
$$
\begin{aligned}
{\boldsymbol{\alpha}}_{b_ib_{k}} &= \bar{{\boldsymbol{\alpha}}}_{b_ib_k} + J^{\boldsymbol{\alpha}}_{b^a_{i}}\delta\boldsymbol{b}_{b_i}^a + J^{\boldsymbol{\alpha}}_{b^g_{i}}\delta\boldsymbol{b}_{b_i}^g\\
{\boldsymbol{\beta}}_{b_ib_{k}} &= \bar{{\boldsymbol{\beta}}}_{b_ib_k} + + J^{\boldsymbol{\beta}}_{b^a_{i}}\delta\boldsymbol{b}_{b_i}^a + J^{\boldsymbol{\beta}}_{b^g_{i}}\delta\boldsymbol{b}_{b_i}^g\\
\boldsymbol{{\boldsymbol{\gamma}}}_{b_ib_{k}}&= \boldsymbol{{\boldsymbol{\gamma}}}_{b_ib_k} \otimes\begin{bmatrix}
1\\
\frac{1}{2}J^{\boldsymbol{\gamma}}_{b^g_{i}}\delta\boldsymbol{b}_{b_i}^g
\end{bmatrix}
\end{aligned}
$$
要估计的量为 i,j 时刻的状态量以及加速度计和陀螺仪的零偏共十个变量,$\boldsymbol{p}_{wb_i}, \boldsymbol{v}^{w}_{i}, \boldsymbol{q}_{wb_i}, \boldsymbol{b}^a_{b_i}, \boldsymbol{b}^g_{b_i}, \boldsymbol{p}_{wb_j}, \boldsymbol{v}^{w}_{j}, \boldsymbol{q}_{wb_j}, \boldsymbol{b}^a_{b_j}, \boldsymbol{b}^g_{b_j}$
下面分部分进行推导:
$\boldsymbol{r}_{bg}$
:
一共十个变量:
$$
\begin{aligned}
\frac{\partial\boldsymbol{r}_{bg}}{\partial\boldsymbol{x}} &=
\begin{bmatrix}
\frac{\partial\boldsymbol{r}_{bg}}{\partial\boldsymbol{p}_{wb_i}} &
\frac{\partial\boldsymbol{r}_{bg}}{\partial\boldsymbol{v}^{w}_{i}} &
\frac{\partial\boldsymbol{r}_{bg}}{\partial\boldsymbol{q}_{wb_i}} &
\frac{\partial\boldsymbol{r}_{bg}}{\partial\boldsymbol{b}^a_{b_i}} &
\frac{\partial\boldsymbol{r}_{bg}}{\partial\boldsymbol{b}^g_{b_i}} &
\frac{\partial\boldsymbol{r}_{bg}}{\partial\boldsymbol{p}_{wb_j}} &
\frac{\partial\boldsymbol{r}_{bg}}{\partial\boldsymbol{v}^{w}_{j}} &
\frac{\partial\boldsymbol{r}_{bg}}{\partial\boldsymbol{q}_{wb_j}} &
\frac{\partial\boldsymbol{r}_{bg}}{\partial\boldsymbol{b}^a_{b_j}} &
\frac{\partial\boldsymbol{r}_{bg}}{\partial\boldsymbol{b}^g_{b_j}}
\end{bmatrix}\\
&=
\begin{bmatrix}
\boldsymbol{0} &
\boldsymbol{0} &
\boldsymbol{0} &
\boldsymbol{0} &
-\boldsymbol{I} &
\boldsymbol{0} &
\boldsymbol{0} &
\boldsymbol{0} &
\boldsymbol{0} &
\boldsymbol{I}
\end{bmatrix}
\end{aligned}
$$
$\boldsymbol{r}_{ba}$
:
同上:
$$
\begin{aligned}
\frac{\partial\boldsymbol{r}_{bg}}{\partial\boldsymbol{x}} &=
\begin{bmatrix}
\frac{\partial\boldsymbol{r}_{ba}}{\partial\boldsymbol{p}_{wb_i}} &
\frac{\partial\boldsymbol{r}_{ba}}{\partial\boldsymbol{v}^{w}_{i}} &
\frac{\partial\boldsymbol{r}_{ba}}{\partial\boldsymbol{q}_{wb_i}} &
\frac{\partial\boldsymbol{r}_{ba}}{\partial\boldsymbol{b}^a_{b_i}} &
\frac{\partial\boldsymbol{r}_{ba}}{\partial\boldsymbol{b}^g_{b_i}} &
\frac{\partial\boldsymbol{r}_{ba}}{\partial\boldsymbol{p}_{wb_j}} &
\frac{\partial\boldsymbol{r}_{ba}}{\partial\boldsymbol{v}^{w}_{j}} &
\frac{\partial\boldsymbol{r}_{ba}}{\partial\boldsymbol{q}_{wb_j}} &
\frac{\partial\boldsymbol{r}_{ba}}{\partial\boldsymbol{b}^a_{b_j}} &
\frac{\partial\boldsymbol{r}_{ba}}{\partial\boldsymbol{b}^g_{b_j}}
\end{bmatrix}\\
&=
\begin{bmatrix}
\boldsymbol{0} &
\boldsymbol{0} &
\boldsymbol{0} &
-\boldsymbol{I} &
\boldsymbol{0} &
\boldsymbol{0} &
\boldsymbol{0} &
\boldsymbol{0} &
\boldsymbol{I} &
\boldsymbol{0}
\end{bmatrix}
\end{aligned}
$$
$\boldsymbol{r}_q$
:
同样需要对十个状态量进行求导:
$$
\begin{aligned}
\frac{\partial\boldsymbol{r}_{bg}}{\partial\boldsymbol{x}} &=
\begin{bmatrix}
\frac{\partial\boldsymbol{r}_{q}}{\partial\boldsymbol{p}_{wb_i}} &
\frac{\partial\boldsymbol{r}_{q}}{\partial\boldsymbol{v}^{w}_{i}} &
\frac{\partial\boldsymbol{r}_{q}}{\partial\boldsymbol{q}_{wb_i}} &
\frac{\partial\boldsymbol{r}_{q}}{\partial\boldsymbol{b}^a_{b_i}} &
\frac{\partial\boldsymbol{r}_{q}}{\partial\boldsymbol{b}^g_{b_i}} &
\frac{\partial\boldsymbol{r}_{q}}{\partial\boldsymbol{p}_{wb_j}} &
\frac{\partial\boldsymbol{r}_{q}}{\partial\boldsymbol{v}^{w}_{j}} &
\frac{\partial\boldsymbol{r}_{q}}{\partial\boldsymbol{q}_{wb_j}} &
\frac{\partial\boldsymbol{r}_{q}}{\partial\boldsymbol{b}^a_{b_j}} &
\frac{\partial\boldsymbol{r}_{q}}{\partial\boldsymbol{b}^g_{b_j}}
\end{bmatrix}\\
&=
\begin{bmatrix}
\boldsymbol{0} &
\boldsymbol{0} &
\frac{\partial\boldsymbol{r}_{q}}{\partial\boldsymbol{q}_{wb_i}} &
\boldsymbol{0} &
\frac{\partial\boldsymbol{r}_{q}}{\partial\boldsymbol{b}^g_{b_i}} &
\boldsymbol{0} &
\boldsymbol{0} &
\frac{\partial\boldsymbol{r}_{q}}{\partial\boldsymbol{q}_{wb_j}} &
\boldsymbol{0} &
\boldsymbol{0}
\end{bmatrix}
\end{aligned}
$$
总共有3项不为零,需要单独求导计算。$\boldsymbol{r}_q$
由于旋转矩阵不服从正常的加减法,所以不能用李群加右扰动然后相减的方法进行求导。这里我们直接在四元数上加扰动,四元数是遵守通常的加法的,所以我们可以使用减法来进行求导,以对 $\boldsymbol{q}_{wb_i}$
求导为例,如下所示:
对 $\boldsymbol{q}_{wb_i}$
求导,需要在残差中对 $\boldsymbol{q}_{wb_i}$
加扰动:
$$
\begin{aligned}
\delta\boldsymbol{r}_{q}&=\left[\boldsymbol{\gamma}_{b_ib_j}^{-1}\otimes((\boldsymbol{q}_{wb_i}\otimes\begin{bmatrix}
1\\
\frac{1}{2}\delta\boldsymbol{\theta}_{wb_i}
\end{bmatrix})^*\otimes\boldsymbol{q}_{wb_j})\right]_{xyz} - \left[\boldsymbol{\gamma}_{b_ib_j}^{-1}\otimes(\boldsymbol{q}_{b_iw}^*\otimes\boldsymbol{q}_{wb_j})\right]_{xyz}\\
&= \left[\boldsymbol{\gamma}_{b_ib_j}^{-1}\otimes\begin{bmatrix}
1\\
-\frac{1}{2}\delta\boldsymbol{\theta}_{wb_i}
\end{bmatrix}\otimes\boldsymbol{q}_{wb_i}^*\otimes\boldsymbol{q}_{wb_j}\right]_{xyz} - \left[\boldsymbol{\gamma}_{b_ib_j}^{-1}\otimes\boldsymbol{q}_{wb_i}^*\otimes\boldsymbol{q}_{wb_j}\right]_{xyz}\\
&= \left[\boldsymbol{\gamma}_{b_ib_j}^{-1}\otimes\begin{bmatrix}
0\\
-\frac{1}{2}\delta\boldsymbol{\theta}_{wb_i}
\end{bmatrix}\otimes\boldsymbol{q}_{wb_i}^*\otimes\boldsymbol{q}_{wb_j}\right]_{xyz}\\
&= ([\boldsymbol{\gamma}_{b_ib_j}^{-1}]_L[\boldsymbol{q}_{wb_i}^*\otimes\boldsymbol{q}_{wb_j}]_R\begin{bmatrix}
0\\
-\frac{1}{2}\delta\boldsymbol{\theta}_{wb_i}
\end{bmatrix})_{xyz}\\
&= -([\boldsymbol{\gamma}_{b_ib_j}]_R)_{3\times3}([\boldsymbol{q}_{wb_j}^*\otimes\boldsymbol{q}_{wb_i}]_L)_{3\times3}\frac{1}{2}\delta\boldsymbol{\theta}_{wb_i}\\
&= -([\boldsymbol{q}_{wb_j}^*\otimes\boldsymbol{q}_{wb_i}]_L)_{3\times3}([\boldsymbol{\gamma}_{b_ib_j}]_R)_{3\times3}
\frac{1}{2}\delta\boldsymbol{\theta}_{wb_i}
\end{aligned}
$$
上述利用了四元数运算中的若干性质,可以参考:移动机器人位姿不同的表示方法
化简之后,因此求导就比较简单了:
$$
\frac{\partial\boldsymbol{r}_q}{\partial\boldsymbol{q}_{wb_i}} = \lim_{\delta\boldsymbol{\theta}_{wb_i}\rightarrow 0}\frac{\delta\boldsymbol{r}_{q}}{\frac{1}{2}\delta\boldsymbol{\theta}_{wb_i}}=-([\boldsymbol{q}_{wb_j}^*\otimes\boldsymbol{q}_{wb_i}]_L)_{3\times3}([\boldsymbol{\gamma}_{b_ib_j}]_R)_{3\times3}
$$
在代码运算中,为了避免对矩阵取块这类操作,我们可以采用以下方式获得结果:
$$
\frac{\partial\boldsymbol{r}_q}{\partial\boldsymbol{q}_{wb_i}} = -\begin{bmatrix}
\boldsymbol{0}_{3\times1} & \boldsymbol{I}_{3\times3}
\end{bmatrix}[\boldsymbol{q}_{wb_j}^*\otimes\boldsymbol{q}_{wb_i}]_L[\boldsymbol{\gamma}_{b_ib_j}]_R\begin{bmatrix}
1\\
\boldsymbol{I}
\end{bmatrix}
$$
对 $\boldsymbol{q}_{wb_j}$
求导,思路是类似的:
$$
\begin{aligned}
\delta\boldsymbol{r}_{q}&=\left[\boldsymbol{\gamma}_{b_ib_j}^{-1}\otimes\boldsymbol{q}_{wb_i}^*\otimes(\boldsymbol{q}_{wb_j}\otimes\begin{bmatrix}
1\\
\frac{1}{2}\delta\boldsymbol{\theta}_{wb_j}
\end{bmatrix})\right]_{xyz} - \left[\boldsymbol{\gamma}_{b_ib_j}^{-1}\otimes\boldsymbol{q}_{b_iw}^*\otimes\boldsymbol{q}_{wb_j}\right]_{xyz}\\
&=\left[\boldsymbol{\gamma}_{b_ib_j}^{-1}\otimes\boldsymbol{q}_{wb_i}^*\otimes(\boldsymbol{q}_{wb_j}\otimes\begin{bmatrix}
0\\
\frac{1}{2}\delta\boldsymbol{\theta}_{wb_j}
\end{bmatrix})\right]_{xyz}\\
&= ([\boldsymbol{\gamma}_{b_ib_j}^{-1}\otimes\boldsymbol{q}_{wb_i}^*\otimes\boldsymbol{q}_{wb_j}]_L\begin{bmatrix}
0\\
\frac{1}{2}\delta\boldsymbol{\theta}_{wb_j}
\end{bmatrix})_{xyz}
\end{aligned}
$$
因此,求导结果为:
$$
\frac{\partial\boldsymbol{r}_q}{\partial\boldsymbol{q}_{wb_j}} = \begin{bmatrix}
\boldsymbol{0}_{3\times1} & \boldsymbol{I}_{3\times3}
\end{bmatrix}[\boldsymbol{\gamma}_{b_ib_j}^{-1}\otimes\boldsymbol{q}_{wb_i}^*\otimes\boldsymbol{q}_{wb_j}]_L\begin{bmatrix}
0\\
\boldsymbol{I}
\end{bmatrix}
$$
对 i 时刻陀螺仪 bias $\boldsymbol{b}^g_{b_i}$
求导,将 $\gamma_{b_ib_j}$
对 bias 的更新模型代入,然后思路和之前基本一样:
$$
\begin{aligned}
\delta\boldsymbol{r}_{q}&=\left[(\boldsymbol{\gamma}_{b_ib_j}\otimes\begin{bmatrix}
1\\
\frac{1}{2}J^\gamma_{b_i^g}\delta\boldsymbol{\gamma}_{b_ib_j}
\end{bmatrix})^{-1}\otimes\boldsymbol{q}_{wb_i}^*\otimes\boldsymbol{q}_{wb_j}\right]_{xyz} - \left[\boldsymbol{\gamma}_{b_ib_j}^{-1}\otimes\boldsymbol{q}_{b_iw}^*\otimes\boldsymbol{q}_{wb_j}\right]_{xyz}\\
&=\left[\begin{bmatrix}
0\\
-\frac{1}{2}J^\gamma_{b_i^g}\delta\boldsymbol{\gamma}_{b_ib_j}
\end{bmatrix}\otimes\boldsymbol{\gamma}_{b_ib_j}^*\otimes\boldsymbol{q}_{wb_i}^*\otimes\boldsymbol{q}_{wb_j}\right]_{xyz}\\
&= \left[[\boldsymbol{\gamma}_{b_ib_j}^*\otimes\boldsymbol{q}_{wb_i}^*\otimes\boldsymbol{q}_{wb_j}]_R\begin{bmatrix}
0\\
-\frac{1}{2}J^\gamma_{b_i^g}\delta\boldsymbol{\gamma}_{b_ib_j}
\end{bmatrix}\right]_{xyz}\\
&= \left[[\boldsymbol{\gamma}_{b_ib_j}^*\otimes\boldsymbol{q}_{wb_i}^*\otimes\boldsymbol{q}_{wb_j}]_R\begin{bmatrix}
0\\
-\frac{1}{2}J^\gamma_{b_i^g}\delta\boldsymbol{\gamma}_{b_ib_j}
\end{bmatrix}\right]_{xyz}
\end{aligned}
$$
求导结果为:
$$
\begin{aligned}
\frac{\partial\boldsymbol{r}_q}{\partial\boldsymbol{b}^g_{b_i}} &= -\begin{bmatrix}
\boldsymbol{0}_{3\times1} & \boldsymbol{I}_{3\times3}
\end{bmatrix}[\boldsymbol{\gamma}_{b_ib_j}^*\otimes\boldsymbol{q}_{wb_i}^*\otimes\boldsymbol{q}_{wb_j}]_R\begin{bmatrix}
0\\
J^\gamma_{b_i^g}
\end{bmatrix}\\
&=-\begin{bmatrix}
\boldsymbol{0}_{3\times1} & \boldsymbol{I}_{3\times3}
\end{bmatrix}[\boldsymbol{q}_{wb_j}^*\otimes\boldsymbol{q}_{wb_i}\otimes\boldsymbol{\gamma}_{b_ib_j}]_L\begin{bmatrix}
0\\
J^\gamma_{b_i^g}
\end{bmatrix}
\end{aligned}
$$
接下来是 $\boldsymbol{r}_v$
:
$$
\begin{aligned}
\frac{\partial\boldsymbol{r}_{bg}}{\partial\boldsymbol{x}} &=
\begin{bmatrix}
\frac{\partial\boldsymbol{r}_{v}}{\partial\boldsymbol{p}_{wb_i}} &
\frac{\partial\boldsymbol{r}_{v}}{\partial\boldsymbol{v}^{w}_{i}} &
\frac{\partial\boldsymbol{r}_{v}}{\partial\boldsymbol{q}_{wb_i}} &
\frac{\partial\boldsymbol{r}_{v}}{\partial\boldsymbol{b}^a_{b_i}} &
\frac{\partial\boldsymbol{r}_{v}}{\partial\boldsymbol{b}^g_{b_i}} &
\frac{\partial\boldsymbol{r}_{v}}{\partial\boldsymbol{p}_{wb_j}} &
\frac{\partial\boldsymbol{r}_{v}}{\partial\boldsymbol{v}^{w}_{j}} &
\frac{\partial\boldsymbol{r}_{v}}{\partial\boldsymbol{q}_{wb_j}} &
\frac{\partial\boldsymbol{r}_{v}}{\partial\boldsymbol{b}^a_{b_j}} &
\frac{\partial\boldsymbol{r}_{v}}{\partial\boldsymbol{b}^g_{b_j}}
\end{bmatrix}\\
&=
\begin{bmatrix}
\boldsymbol{0} &
\frac{\partial\boldsymbol{r}_{v}}{\partial\boldsymbol{v}^{w}_{i}} &
\frac{\partial\boldsymbol{r}_{v}}{\partial\boldsymbol{q}_{wb_i}} &
\frac{\partial\boldsymbol{r}_{v}}{\partial\boldsymbol{b}^a_{b_i}} &
\frac{\partial\boldsymbol{r}_{v}}{\partial\boldsymbol{b}^g_{b_i}} &
\boldsymbol{0} &
\frac{\partial\boldsymbol{r}_{v}}{\partial\boldsymbol{v}^{w}_{j}} &
\boldsymbol{0} &
\boldsymbol{0} &
\boldsymbol{0}
\end{bmatrix}
\end{aligned}
$$
总共有5项不为零,需要单独求导计算,由于误差形式比较简单,大部份比较简单的推导就直接给出结果了:
$$
\begin{aligned}
\frac{\partial\boldsymbol{r}_{v}}{\partial\boldsymbol{v}^{w}_{i}} &= -\boldsymbol{R}_{b_iw}\\
\frac{\partial\boldsymbol{r}_{v}}{\partial\boldsymbol{v}^{w}_{j}} &= \boldsymbol{R}_{b_iw}\\
\frac{\partial\boldsymbol{r}_{v}}{\partial\boldsymbol{b}^a_{b_i}} &= -J^\beta_{b_i^a}\\
\frac{\partial\boldsymbol{r}_{v}}{\partial\boldsymbol{b}^g_{b_i}} &= -J^\beta_{b_i^g}
\end{aligned}
$$
对 i 时刻的求导使用的技巧在之前的推导过程中也用过很多次了,这里简单带过:
$$
\begin{aligned}
\frac{\partial\boldsymbol{r}_{v}}{\partial\boldsymbol{q}_{wb_i}} &= \lim_{\delta\boldsymbol{\theta}\rightarrow 0}\frac{(\boldsymbol{R}_{wb_i}\exp{(\delta\boldsymbol{\theta}^\wedge)})^{-1}(\boldsymbol{v}_j^w - \boldsymbol{v}^w_i + \boldsymbol{g}^w\Delta t)-\boldsymbol{R}_{wb_i}^{-1}(\boldsymbol{v}_j^w - \boldsymbol{v}^w_i + \boldsymbol{g}^w\Delta t)}{\delta\boldsymbol{\theta}}\\
&= (\boldsymbol{R}_{wb_i}^{-1}(\boldsymbol{v}_j^w - \boldsymbol{v}^w_i + \boldsymbol{g}^w\Delta t))^\wedge
\end{aligned}
$$
最后是 $\boldsymbol{r}_p$
,思路和上述过程基本一样,这里直接给出结果:
$$
\begin{aligned}
\frac{\partial\boldsymbol{r}_{bg}}{\partial\boldsymbol{x}} &=
\begin{bmatrix}
\frac{\partial\boldsymbol{r}_{p}}{\partial\boldsymbol{p}_{wb_i}} &
\frac{\partial\boldsymbol{r}_{p}}{\partial\boldsymbol{v}^{w}_{i}} &
\frac{\partial\boldsymbol{r}_{p}}{\partial\boldsymbol{q}_{wb_i}} &
\frac{\partial\boldsymbol{r}_{p}}{\partial\boldsymbol{b}^a_{b_i}} &
\frac{\partial\boldsymbol{r}_{p}}{\partial\boldsymbol{b}^g_{b_i}} &
\frac{\partial\boldsymbol{r}_{p}}{\partial\boldsymbol{p}_{wb_j}} &
\frac{\partial\boldsymbol{r}_{p}}{\partial\boldsymbol{v}^{w}_{j}} &
\frac{\partial\boldsymbol{r}_{p}}{\partial\boldsymbol{q}_{wb_j}} &
\frac{\partial\boldsymbol{r}_{p}}{\partial\boldsymbol{b}^a_{b_j}} &
\frac{\partial\boldsymbol{r}_{p}}{\partial\boldsymbol{b}^g_{b_j}}
\end{bmatrix}\\
&=
\begin{bmatrix}
-\boldsymbol{R}_{b_iw} &
-\boldsymbol{R}_{b_iw}\Delta t &
\frac{\partial\boldsymbol{r}_{p}}{\partial\boldsymbol{q}_{wb_i}} &
-J^\alpha_{b_i^a}\
-J^\alpha_{b_i^g}&
\boldsymbol{R}_{b_iw}&
\boldsymbol{0}&
\boldsymbol{0}&
\boldsymbol{0}&
\boldsymbol{0}&
\end{bmatrix}
\end{aligned}
$$
其中,第三项为:
$$
\frac{\partial\boldsymbol{r}_{p}}{\partial\boldsymbol{q}_{wb_i}} = (\boldsymbol{R}_{b_iw}(\boldsymbol{p}_{wb_j} - \boldsymbol{p}_{wb_i} - \boldsymbol{v}^w_i\Delta t + \frac{1}{2}\boldsymbol{g}^w\Delta t^2))^\wedge
$$
至此,IMU 误差的雅可比也全部推导完成。