>
返回

VIO 中残差雅可比的推导

简介

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 误差的雅可比也全部推导完成。

参考资料

Built with Hugo
Theme Stack designed by Jimmy