>
返回
VIO

VIO 残差函数的构建

简介

在视觉 SLAM 中，我们通过最小化特征点在归一化平面上的位置（估计值）和图像中匹配到的该图像的位置（观测值）的差即重投影误差来对特征点和位姿进行优化。这里我们只有一个观测作为信息来源即相机获取的图像，而在 VIO 中，除了相机以外我们还有 IMU 的测量数据作为另一信息来源，这种情况下我们怎么对这个观测进行误差构建呢？并且对于 IMU 的误差和视觉的误差怎么分配权重并且求其最小值呢，这是这篇博客想要解决的问题。

VIO 残差函数的构建

VIO 中残差总共由三部分组成：先验误差、视觉误差和 IMU 误差，这里不同的方差之间用各自的协方差矩阵来统一单位和权重，即各自的残差为：

$$
\begin{aligned}
    r &= ||\boldsymbol{f}(x)||^2_\Sigma\\
    &= \boldsymbol{f}^T(x)\Sigma^{-1}\boldsymbol{f}(x)
\end{aligned}
$$

上面提到的三部分方差分别为：

先验残差，prior

$$
||\boldsymbol{r}_p + \boldsymbol{J}_p\chi||_{\Sigma_p}^2
$$

每个时间下的IMU 残差：

$$
\begin{aligned}
    ||\boldsymbol{r}_b(\boldsymbol{z}_{b_ib_{i+1}}, \chi)||_{\Sigma_{b_ib_{i+1}}}^2
\end{aligned}
$$

对每个点的视觉残差：

$$
\begin{aligned}
    r_{f_j}^{c_i}= ||\boldsymbol{r}_f(\boldsymbol{z}_{\boldsymbol{f}_j}^{c_i}, \chi)||^2_{\Sigma_{\boldsymbol{f}_j^{c_i}}}
\end{aligned}
$$

对每个误差项用鲁棒核函数 $\rho()$ 来降低 outliers 的影响，这样最终的要优化的问题为：

$$
\begin{aligned}
    \min_{\chi}\boldsymbol{F}(x) =& \min_{\chi}(\rho(||\boldsymbol{r}_p - \boldsymbol{J}_p\chi||_{\Sigma_p}^2)\\
    &+ \sum_{i\in B}\rho(||\boldsymbol{r}_b(\boldsymbol{z}_{b_ib_{i+1}}, \chi)||_{\Sigma_{b_ib_{i+1}}}^2) \\
    &+ \sum_{(i,j)\in F} \rho(||\boldsymbol{r}_f(\boldsymbol{z}_{\boldsymbol{f}_j}^{c_i}, \chi)||^2_{\Sigma_{\boldsymbol{f}_j^{c_i}}})) 
\end{aligned}
$$

这里每一项误差都和我们要优化的状态量建立起关系，为了节约计算量，这里我们采用滑动窗口的方式来进行 Bundle Adjustment，在 i 时刻，要优化的状态量为：

$$
\begin{aligned}
\chi &= \left[\boldsymbol{x}_n, \boldsymbol{x}_{n+1}, ... , \boldsymbol{x}_{n+N}, \lambda_m, \lambda_{m+1}, ... , \lambda_{m+M}\right] \\
\boldsymbol{x}_i &= \left[\boldsymbol{p}_{wb_i}, \boldsymbol{q}_{wb_i}, \boldsymbol{v}_i^w, \boldsymbol{b}_a^{b_i}, \boldsymbol{b}_g^{b_i} \right], \qquad i \in [n, n + N]
\end{aligned}
$$

上式中，要优化的状态量为两部分，一部分是 IMU 载体本身的状态量；另一部分为观测到的路标（特征点）的状态量。假设在一个滑动窗口中载体状态量（关键帧的数量）有 N 个，在所有关键帧中观测到的路标有 M 个。则：

  • 载体的状态量包括其在惯性坐标系中的位置、速度、姿态以及 IMU 中加速度计和陀螺仪在载体坐标系下的零偏
  • 路标的状态量则用逆深度来表示，关于逆深度会在后面进行分析

先验误差在最后介绍滑动窗口的处理方法时在进行分析，先对视觉重投影误差以及 IMU 误差进行分析。

视觉重投影误差

对一个特征点，视觉重投影误差定义为一个特征点在归一化相机坐标系下估计值和观测值的差值，归一化坐标系是某一时刻相机坐标系下 $z = 1$ 的二维平面坐标系，重投影误差如下所示：

$$
\boldsymbol{r}_c = \begin{bmatrix}
    \frac{x}{z} - u\\
    \frac{y}{z} - v
\end{bmatrix}
$$

其中，$(x, y, z)$ 为特征点在某一时刻相机坐标系下的坐标值，是要估计的量；$(u, v)$ 是该特征值在那一时刻相机归一化平面下的坐标，注意相机的像素坐标也经常用 $(u, v)$ 来表示，这里要注意区分。

理想情况下，特征点在相机坐标系下和归一化平面下坐标的对应关系可以用一个逆深度参数 $\lambda = \frac{1}{z}$ 来表示：

$$
\begin{bmatrix}
    x\\y\\z
\end{bmatrix} =
\frac{1}{\lambda}
\begin{bmatrix}
    u\\v\\1
\end{bmatrix}
$$

这里我们使用 $\lambda$ 而不是 $z$ 的原因为，对一个特征点它距归一化平面的距离有可能很远以至于 $z = \infty$，这样用 $z$ 来建立关系的话方程没办法求解，而 $z$ 不可能为 0，因为能观测到的特征点不可能在归一化平面后面，因此不存在这个问题。

对于一个特征点而言，我们使用它被首次观测到的那一帧作为参考值，使用逆深度 $\lambda$ 和它在那一帧的观测值即可求出其在相机坐标系下的三维坐标，所以我们优化的时候只需要估计逆深度即可。而该特征值在其他帧被观测到时候，我们以它在初始化帧的三维坐标值作为基准构建残差。过程为：

假设该特征值在 i 帧中被首次观测并初始化，然后在第 j 帧又被观测到，首先通过相机之间的位姿关系计算其在第 j 帧的相机坐标系下的三维坐标：

$$
\begin{aligned}
\boldsymbol{p}_{c_j} &= \boldsymbol{T}_{c_jc_i}\boldsymbol{p}_{c_i}\\
    \begin{bmatrix}
    x_{c_j}\\
    y_{c_j}\\
    z_{c_j}\\
    1
    \end{bmatrix} &= \boldsymbol{T}_{bc}^{-1}\boldsymbol{T}_{wb_j}^{-1}\boldsymbol{T}_{wb_i}\boldsymbol{T}_{bc}
    \begin{bmatrix}
        \frac{1}{\lambda} u_{c_i}\\
        \frac{1}{\lambda} v_{c_i}\\
        \frac{1}{\lambda}\\
        1\\
    \end{bmatrix}
    \end{aligned}
$$

然后再用同样的方法计算其和第 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}
$$

IMU 误差

计算方法

要计算 IMU 的误差，需要对 IMU 进行积分得到相应的状态量和估计的状态量作差。通过 IMU 积分得到状态量的过程推导参考我之前写的博客：基于 IMU 的惯性导航解算及误差分析，这里给出结论：

$$
\begin{aligned}
    \boldsymbol{q}_{wb_j} &=\int_{t\in[i,j]}{(\boldsymbol{q}_{wb_t}\otimes\begin{bmatrix}
        1 \\
        \frac{1}{2}\boldsymbol{\omega}^{b_t}
    \end{bmatrix})}\delta t\\
    \boldsymbol{v}^w_j &= \boldsymbol{v}^w_i + \int_{t\in[i,j]}{(\boldsymbol{R}_{wb_t}\boldsymbol{a} - \boldsymbol{g}^w)}\delta t\\
    \boldsymbol{p}_{wb_j} &= \boldsymbol{p}_{wb_i} + \boldsymbol{v}^w_i\Delta t + \int\int_{t\in[i,j]}{(\boldsymbol{R}_{wb_i}\boldsymbol{a} - \boldsymbol{g}^w)}\delta t^2
\end{aligned}
$$

注：在这里及后续的推导中，在不同情况下会同时使用四元数和旋转矩阵来表示姿态，大致为：

  • 在描述一个待优化的状态量时，会使用四元数，这样做的好处是我们只需要更新三个参数（四元数的独立参数为 3 个，而旋转矩阵 为 8 个）
  • 在使用姿态来对点或者向量进行旋转时，以及部分求导场景时，会使用旋转矩阵来描述姿态，这样做的好处是旋转矩阵的旋转操作遵循通常的矩阵运算，在计算或者求导时相对方便
  • 在需要旋转向量时将作为状态量的四元数进行转换即可。

通过这个计算公式，我们可以计算 i, j 时刻之间的姿态变化量以及第 j 时刻在世界坐标系下的姿态，速度和位置。

由于 IMU 通常测量频率比较高，假设 IMU 频率为 100 Hz，意味着一秒可以产生 100 个测量值，如果这其中 100 个测量值都要估计的话要估计变量为 1500 个，这样计算量太大了，因此通常的做法是一定间隔内取一个（例如 1 秒钟取一个或者和相机的频率一致）。但可以注意到即便是这样，如果在优化过程中任意一个 $\boldsymbol{q}_{wb_i}$ 改变了我们必须重新计算整个积分过程，计算量比较大。因此有必要使用一些方法来降低这个计算，这就是预积分的动机。

IMU 预积分

常规的计算方法之所以需要重新计算的原因是积分中的所有项都是在世界坐标系下的，而其中很多项是没有必要的重新计算，以姿态更新为例：假设 0 ~ 100 个数据的更新为：

$$
\boldsymbol{q}_{wb_{100}} =\int_{t\in[0,100]}{(\boldsymbol{q}_{wb_t}\otimes\begin{bmatrix}
        1 \\
        \frac{1}{2}\boldsymbol{\omega}^{b_t}
    \end{bmatrix})}\delta t
$$

如果我们改变了 $\boldsymbol{q}_{wb_0}$($\boldsymbol{R}_{wb_0}$)，如果我们按照上面的更新公式来看的话必须重新进行一次积分（进行 ~100 旋转），但实际上我们知道 1 ~ 99 之间的相对旋转结果之前已经算过了，而且经过 $\boldsymbol{q}_{wb_0}$ 更新也没有改变，如果将其存储起来重新利用的话实际上只需要进行一次旋转即可：

$$
\begin{aligned}
    \boldsymbol{q}_{wb_{100}} &= \boldsymbol{q}_{wb_{0}}\otimes\boldsymbol{q}_{b_0b_{100}}\\
    &= \boldsymbol{q}_{wb_{0}}\otimes\int_{t\in[0,100]}{(\boldsymbol{q}_{b_0b_t}\otimes\begin{bmatrix}
        1 \\
        \frac{1}{2}\boldsymbol{\omega}^{b_t}
    \end{bmatrix})}\delta t
\end{aligned}
$$

上式中积分项也有一项四元数，表示在该时刻下 IMU 相对于第 0 时刻 IMU 的旋转，这个是不会随着 $\boldsymbol{q}_{wb_0}$ ($\boldsymbol{R}_{wb_i}$)的改变而改变的，所以可以复用。

根据这个思路，我们可以写出预积分模型下的 IMU 解算方法：

姿态：

$$
\begin{aligned}
    \boldsymbol{q}_{wb_j} &=\boldsymbol{q}_{wb_i}\otimes\int_{t\in[i,j]}{(\boldsymbol{q}_{b_ib_t}\otimes\begin{bmatrix}
        1 \\
        \frac{1}{2}\boldsymbol{\omega}^{b_t}
    \end{bmatrix})}\delta t
\end{aligned}
$$

速度：

$$
\begin{aligned}
    \boldsymbol{v}^w_j &= \boldsymbol{v}^w_i + \int_{t\in[i,j]}{(\boldsymbol{R}_{wb_t}\boldsymbol{a}^w - \boldsymbol{g}^w)}\delta t\\
    &= \boldsymbol{v}^w_i - \boldsymbol{g}^w\Delta t + \int_{t\in[i,j]}{(\boldsymbol{R}_{wb_t}\boldsymbol{a}^w)}\delta t \\
    &= \boldsymbol{v}^w_i - \boldsymbol{g}^w\Delta t + \boldsymbol{R}_{wb_i}\int_{t\in[i,j]}{(\boldsymbol{R}_{b_ib_t}\boldsymbol{a}^{b_i})}\delta t
\end{aligned}
$$

位置：

$$
\begin{aligned}
    \boldsymbol{p}_{wb_j} &= \boldsymbol{p}_{wb_i} + \boldsymbol{v}^w_i\Delta t + \int\int_{t\in[i,j]}{(\boldsymbol{R}_{wb_i}\boldsymbol{a}^w - \boldsymbol{g}^w)}\delta t^2\\
    &= \boldsymbol{p}_{wb_i} + \boldsymbol{v}^w_i\Delta t - \frac{1}{2}\boldsymbol{g}^w\Delta t^2 + \int\int_{t\in[i,j]}{(\boldsymbol{R}_{wb_t}\boldsymbol{a}^w)}\delta t^2 \\
    &= \boldsymbol{p}_{wb_i} + \boldsymbol{v}^w_i\Delta t - \frac{1}{2}\boldsymbol{g}^w\Delta t^2 + \boldsymbol{R}_{wb_i}\int\int_{t\in[i,j]}{(\boldsymbol{R}_{b_ib_t}\boldsymbol{a}^{b_i})}\delta t^2
\end{aligned}
$$

整理一下有：

$$
\begin{aligned}
    \boldsymbol{q}_{wb_j} &=\boldsymbol{q}_{wb_i}\otimes\int_{t\in[i,j]}{(\boldsymbol{q}_{b_ib_t}\otimes\begin{bmatrix}
        1 \\
        \frac{1}{2}\boldsymbol{\omega}^{b_t}
    \end{bmatrix})}\delta t\\
    \boldsymbol{v}^w_j &= \boldsymbol{v}^w_i - \boldsymbol{g}^w\Delta t + \boldsymbol{R}_{wb_i}\int_{t\in[i,j]}{(\boldsymbol{R}_{b_ib_t}\boldsymbol{a}^{b_i})}\delta t\\
    \boldsymbol{p}_{wb_j} &= \boldsymbol{p}_{wb_i} + \boldsymbol{v}^w_i\Delta t - \frac{1}{2}\boldsymbol{g}^w\Delta t^2 + \boldsymbol{R}_{wb_i}\int\int_{t\in[i,j]}{(\boldsymbol{R}_{b_ib_t}\boldsymbol{a}^{b_i})}\delta t^2
\end{aligned}
$$

这样做的好处是，在进行了一次上式中的积分计算之后，任意一个 $\boldsymbol{q}_{wb_i}$ ($\boldsymbol{R}_{wb_i}$)更新后，在更新与其有关系的状态量时，我们不需要重新计算上式中的积分部分，只需要进行一次计算即可，因此这种方法就叫预积分。

为了简洁，下面用三个变量分别表示上式中的三个预积分量：

$$
\begin{aligned}
    \boldsymbol{{\boldsymbol{\gamma}}}_{b_ib_j} &= \int_{t\in[i,j]}{(\boldsymbol{q}_{b_ib_t}\otimes\begin{bmatrix}
        1 \\
        \frac{1}{2}\boldsymbol{\omega}^{b_t}
    \end{bmatrix})}\delta t\\
    \boldsymbol{{\boldsymbol{\beta}}}_{b_ib_j} &= \int_{t\in[i,j]}{(\boldsymbol{R}_{b_ib_t}\boldsymbol{a}^{b_i})}\delta t\\
    \boldsymbol{{\boldsymbol{\alpha}}}_{b_ib_j} &= \int\int_{t\in[i,j]}{(\boldsymbol{R}_{b_ib_t}\boldsymbol{a}^{b_i})}\delta t^2
\end{aligned}
$$

则 IMU 从时刻 i 到时刻 j 的状态更新方程为：

$$
\begin{bmatrix}
    \boldsymbol{p}_{wb_j}\\
    \boldsymbol{v}^w_j\\
    \boldsymbol{q}_{wb_j}\\
    \boldsymbol{b}_j^a\\
    \boldsymbol{b}_j^g
\end{bmatrix} = 
\begin{bmatrix}
    \boldsymbol{p}_{wb_i} + \boldsymbol{v}^w_i\Delta t - \frac{1}{2}\boldsymbol{g}^w\Delta t^2 + \boldsymbol{R}_{wb_i}\boldsymbol{{\boldsymbol{\alpha}}}_{b_ib_j}\\
    \boldsymbol{v}^w_i - \boldsymbol{g}^w\Delta t + \boldsymbol{R}_{wb_i}\boldsymbol{{\boldsymbol{\beta}}}_{b_ib_j}\\
    \boldsymbol{q}_{wb_i}\otimes\boldsymbol{{\boldsymbol{\gamma}}}_{b_ib_j}\\
    \boldsymbol{b}_i^a\\
    \boldsymbol{b}_i^g
\end{bmatrix}
$$

计算 IMU 预积分量

在上一部分中，我们实际上使用一个定义好的 IMU 预积分量来代替 IMU 的原始观测作为状态估计问题中的观测量。因此，我们需要推导以下几个部分：

  • 预积分的计算方法：如何对一个时间段内的原始测量值进行累积（积分）得到预积分量
  • 预积分的观测模型：我们使用预积分作为 IMU 的观测量，因此需要像别的观测一样构建观测模型（方程），构建形如 $z = h(x, \omega)$ 的方程
  • 使用预积分构建约束：利用观测模型和实际观测的预积分构建残差方程（约束）

预积分计算方法

在离散时间下，由于 IMU 的测量值是离散的，因此上述的连续积分没办法计算，离散情况下我们一般用中值法来计算预积分量，即两个时刻之间的测量值用这两个时刻的测量值的平均值来代替，如下所示：

$$
\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}))
\end{aligned}
$$

则预积分量的计算方法为：

$$
\begin{aligned}
{\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}
$$

注意，这里预积分量中引入了陀螺仪和加速度计的零偏。而这两个零偏也是估计量，因此，如果我们不做处理的话，每次零偏更新时我们不得不重新进行预积分的计算，这违背了我们引入预积分的初衷。因此，我们希望能够得到预积分量的计算和零偏变化值的关系，这样每当零偏估计更新时，我们可以用更新量与更新预积分而不是重新计算。我们对三个预积分量在 i 时刻的零偏处，进行一阶泰勒展开如下所示：

$$
\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}
$$

其中，

$$
\begin{aligned}
    J^{\boldsymbol{\alpha}}_{b^a_{i}} &= \frac{\partial{\boldsymbol{\alpha}}_{b_ib_j}}{\partial\boldsymbol{b}_i^a}\\
    J^{\boldsymbol{\alpha}}_{b^g_{i}} &= \frac{\partial{\boldsymbol{\alpha}}_{b_ib_j}}{\partial\boldsymbol{b}_i^g}\\
    J^{\boldsymbol{\beta}}_{b^a_{i}} &= \frac{\partial{\boldsymbol{\beta}}_{b_ib_j}}{\partial\boldsymbol{b}_i^a}\\
    J^{\boldsymbol{\beta}}_{b^g_{i}} &= \frac{\partial{\boldsymbol{\beta}}_{b_ib_j}}{\partial\boldsymbol{b}_i^g}\\
    J^{\boldsymbol{\gamma}}_{b^g_{i}} &= \frac{\partial{\boldsymbol{\gamma}}_{b_ib_j}}{\partial\boldsymbol{b}_i^g}\\
\end{aligned}
$$

相关的雅可比矩阵计算方式会在最后进行推导。

预积分观测模型

在这一部分中，我们想要得到的观测值（这里为预积分量）和状态值（这里为载体的位姿和速度等）之间的关系，以及推导这个模型中的噪声怎么对观测方程的均值和方差造成影响。

首先来看噪声，这里我们假设 IMU 测量的噪声为高斯白噪声，因此不会对预积分量的均值有影响；主要推导 IMU 噪声的方差和预积分方差之间的传递关系。

和视觉等其他传感器不同，视觉中的对测量的方差我们可以通过标定或者假定来确定；对于 IMU 的协方差，我们对于一个数据的方差（不确定度）可以通过标定来确定，但是对于一段时间内的的测量值计算而来的预计分量怎么计算其方差呢，下面来看怎么通过单个测量值的方差来求其组合的方差。

由数理统计的知识得，对于一个变量 $x \in \mathcal{N}(0, \Sigma_x)$，其线性变换后的变量有：$y = Ax, y\in\mathcal{N}(0, \Sigma_y), \Sigma_y=A\Sigma_xA^T$

因此，只需要知道 imu 噪声和预积分之间的线性递推关系，即可通过上式来求解预积分的协方差了，为了篇幅的整洁，将这部分放在后面。

推导这种线性递推关系有两种方法：

  • 我们已经建立了离散时间下的状态方程，借鉴扩展卡尔曼滤波（EKF）的思路，直接通过一阶泰勒展开离散时间下的状态方程，得到离散时间下的误差递推关系
  • 在连续时间下建立微分方程，推导误差的递推关系，在对其进行离散化

为了篇幅的整洁，将这部分放到后面整理。接下来先进行预积分的观测模型推导，已知载体状态量，从预积分的原始定义式中，我们不难得到其观测方程，如下所示：

$$
\begin{bmatrix}
    \tilde{\boldsymbol{\alpha}}_{{b_k}b_{k+1}}\\
    \tilde{\boldsymbol{\beta}}_{{b_k}b_{k+1}}\\
    \tilde{\boldsymbol{\gamma}}_{{b_k}b_{k+1}}\\
    \boldsymbol{0}\\
    \boldsymbol{0}\\
\end{bmatrix} = 
\begin{bmatrix}
    \boldsymbol{R}_{b_kw}(\boldsymbol{p}_{wb_{k+1}} - \boldsymbol{p}_{wb_{k}} + \frac{1}{2}\boldsymbol{g}^w\Delta t^2 - \boldsymbol{v}^w_{b_k}\Delta t)\\
    \boldsymbol{R}_{b_kw}(\boldsymbol{v}^w_{b_{k+1}} + \boldsymbol{g}^w\Delta t - \boldsymbol{v}^w_{b_k})\\
    \boldsymbol{q}_{wb_{k}}^{-1}\otimes\boldsymbol{q}_{wb_{k+1}}\\
    \boldsymbol{b}^a_{b_{k+1}} - \boldsymbol{b}^a_{b_{k}}\\
    \boldsymbol{b}^g_{b_{k+1}} - \boldsymbol{b}^g_{b_{k}}\\
\end{bmatrix}
$$

上式中，左侧为预积分量的观测值，右侧为根据状态量得到的预积分预测值，通过比较这两部分可以构建关于预积分量和状态量之间的约束关系。

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}
$$

上式中，零偏的误差即两时刻的值相减即可，实际观测值为 0；姿态的误差则是用两个状态值求出相对一个相对旋转和我们预先求的相对旋转的逆相乘即可，由于四元数本身时过参数化的（四个变量中只有三个是独立的），因此我们用一个三维向量 $\boldsymbol{\theta}$ 来表示其残差（取其虚部）；速度和位置的误差也是先通过简单相减，然后再减去常数项和预积分量构建误差。

注：实际上 IMU 加速度计和陀螺仪的零偏是随着时间变化的，因此求误差的时候理论上需要在相减的基础上再减去一个噪声项 $n\Delta t$，但由于预积分中两个时刻一般相隔不会太大，因此假设 IMU 在这两个时刻的零偏相等。

IMU 预积分误差的方差的两种推导方法

基于离散系统推导线性递推关系

对于每一个时刻的状态量 $\boldsymbol{x}_k$ 来说，其值与两部分有关系：上一时刻状态量 $\boldsymbol{x}_{k-1}$ ，以及输入控制值 $\boldsymbol{u}_{k-1}$，这里对应的是 IMU 测量值，假设相邻时刻的状态的传递方程为：

$$
\boldsymbol{x}_{k+1} = f(\boldsymbol{x}_{k}, \boldsymbol{u}_{k})
$$

误差的传递同样有两部分组成，一部分是上一时刻状态量得误差值，还有一部分是观测的测量误差，这里假设测量误差噪声为 $\boldsymbol{n}_{k-1}$。这个传递函数通常是一个非线性函数，因此对其进行一阶泰勒函数展开有：

$$
\begin{aligned}
    \boldsymbol{x}_{k+1} &= f(\boldsymbol{x}_{k}, \boldsymbol{u}_{k})\\
    \tilde{\boldsymbol{x}}_{k+1} + \delta \boldsymbol{x}_{k+1} &= f(\tilde{\boldsymbol{x}}_{k} + \delta\boldsymbol{x}_{k}, \tilde{\boldsymbol{u}}_{k} + \boldsymbol{n}_{k})\\
    &= f(\tilde{\boldsymbol{x}}_{k}, \tilde{\boldsymbol{u}}_{k}) + \boldsymbol{F}\delta\boldsymbol{x}_{k} + \boldsymbol{G}\boldsymbol{n}_{k}\\
    &= \tilde{\boldsymbol{x}}_{k+1} + \boldsymbol{F}\delta\boldsymbol{x}_{k} + \boldsymbol{G}\boldsymbol{n}_{k}\\
    \Rightarrow \delta \boldsymbol{x}_{k+1} &= \boldsymbol{F}\delta\boldsymbol{x}_{k} + \boldsymbol{G}\boldsymbol{n}_{k}
\end{aligned}
$$

上式中，$\boldsymbol{F}$ 为 $\boldsymbol{x}_k$ 对 $\boldsymbol{x}_{k-1}$ 的雅克比矩阵，$\boldsymbol{G}$ 为 $\boldsymbol{x}_k$ 对输入量 $\boldsymbol{u}_{k-1}$ 的雅克比矩阵。这样我们就知道了相邻时刻之间误差和测量噪声的线性传递关系。

这里，我们关注的状态量显然为 $\boldsymbol{x}_{b_k}=(\boldsymbol{{\boldsymbol{\alpha}}}_{b_k}, \boldsymbol{{\boldsymbol{\gamma}}}_{b_k}, \boldsymbol{{\boldsymbol{\beta}}}_{b_k}, \boldsymbol{b}^a_{b_k}, \boldsymbol{b}^g_{b_k})$（为了表示简洁，下标省略了 $b_i$，记住预积分量都是表示 $b_i$ 到 $b_k$ 的积分量即可），输入量（控制量）为加速度计和陀螺仪的测量数据 $\boldsymbol{u}_{b_k} = (\boldsymbol{\omega}^{b_k}, \boldsymbol{a}^{b_k}, \boldsymbol{\omega}^{b_{k+1}}, \boldsymbol{a}^{b_{k+1}}, \boldsymbol{n}_{b^a_k}, \boldsymbol{n}_{b^g_k})$(由于使用中值积分，因此加速度计和陀螺仪的测量值有两个)，考虑噪声的非线性状态转移方程为：

$$
\begin{aligned}
    \boldsymbol{\omega} &= \frac{1}{2}((\bar{\boldsymbol{\omega}}^{b_k} + \boldsymbol{n}_k^g -\boldsymbol{b}^g_k)+(\bar{\boldsymbol{\omega}}^{b_{k+1}} + \boldsymbol{n}_{k+1}^g -\boldsymbol{b}^g_{k+1}))\\
    \boldsymbol{a} &= \frac{1}{2}(\boldsymbol{R}_{b_ib_k}(\bar{\boldsymbol{a}}^{b_k} + \boldsymbol{n}_k^a -\boldsymbol{b}^a_k) + \boldsymbol{R}_{b_ib_{k+1}}(\bar{\boldsymbol{a}}^{b_{k+1}} + \boldsymbol{n}_{k+1}^a -\boldsymbol{b}^a_{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}\\
    \boldsymbol{b}_{k+1}^a &= \boldsymbol{b}_{k}^a + \boldsymbol{n}_{b_k^a}\Delta t\\
    \boldsymbol{b}_{k+1}^g &= \boldsymbol{b}_{k}^g + \boldsymbol{n}_{b_k^g}\Delta t
\end{aligned}
$$

因此这里的误差为 $\boldsymbol{\eta}_{ik} = [\delta\boldsymbol{{\boldsymbol{\alpha}}}_{k+1}, \delta\boldsymbol{\theta}_{k+1}, \delta\boldsymbol{{\boldsymbol{\beta}}}_{k+1}, \delta\boldsymbol{b}_{k+1}^a, \delta\boldsymbol{b}_{k+1}^g]^T$。

接下来直接求出雅克比矩阵 $\boldsymbol{F}$ 和 $\boldsymbol{G}$ 即可通过线性变换计算出预积分误差的协方差矩阵。这两个矩阵求起来比较繁琐，所以放到最后进行。

基于连续系统推导线性递推关系

除了上述方程描述一个系统以外，还可以用通过微分方程构建线性递推关系，我们知道：

$$
\dot{\boldsymbol{v}} = \boldsymbol{Ra}^b + \boldsymbol{g}
$$

考虑误差项，有：

$$
\begin{aligned}
\dot{\boldsymbol{v}} + \dot{\delta \boldsymbol{v}} &= \boldsymbol{R}(\boldsymbol{I} + (\delta\boldsymbol{\theta})^\wedge)(\boldsymbol{a}^b+\delta\boldsymbol{a}^b) + \boldsymbol{g}+ \delta\boldsymbol{g}\\
\dot{\boldsymbol{v}} + \dot{\delta \boldsymbol{v}} &= \boldsymbol{Ra}^b + \boldsymbol{g} + \boldsymbol{R}\delta\boldsymbol{a}^b + \boldsymbol{R}(\delta\boldsymbol{\theta})^\wedge(\boldsymbol{a}^b+\delta\boldsymbol{a}^b)+ \delta\boldsymbol{g}\\
\Rightarrow \dot{\delta \boldsymbol{v}} &=  \boldsymbol{R}\delta\boldsymbol{a}^b +\boldsymbol{R}(\delta\boldsymbol{\theta})^\wedge(\boldsymbol{a}^b+\delta\boldsymbol{a}^b)+ \delta\boldsymbol{g}\\
&\approx\boldsymbol{R}\delta\boldsymbol{a}^b +\boldsymbol{R}(\delta\boldsymbol{\theta})^\wedge\boldsymbol{a}^b+ \delta\boldsymbol{g}\\
&= \boldsymbol{R}\delta\boldsymbol{a}^b - \boldsymbol{R}(\boldsymbol{a}^b)^\wedge\delta\boldsymbol{\theta}+ \delta\boldsymbol{g}
\end{aligned}
$$

这样我们相当于构建了这么一个关于误差的线性系统：

$$
\dot{\delta\boldsymbol{x}} = \boldsymbol{A}\delta\boldsymbol{\boldsymbol{x}} + \boldsymbol{B}\delta\boldsymbol{n}
$$

因此在某一时刻，有：

$$
\begin{aligned}
    \dot{\delta\boldsymbol{x}}_k &= \boldsymbol{A}\delta\boldsymbol{x}_k + \boldsymbol{B}\delta\boldsymbol{n}_k\\
    \Rightarrow \delta\boldsymbol{x}_{k+1} &= \delta\boldsymbol{x}_k + \dot{\delta\boldsymbol{x}}_k\Delta t\\
    &= \delta\boldsymbol{x}_k + \boldsymbol{A}\Delta t\delta\boldsymbol{x}_k + \boldsymbol{B}\Delta t\delta\boldsymbol{n}_k\\
    &= (\boldsymbol{I} + \boldsymbol{A}\Delta t)\delta\boldsymbol{x}_k + \boldsymbol{B}\Delta t\delta\boldsymbol{n}_k
\end{aligned}
$$

和上一种方法比较，不难看出：

$$
\begin{aligned}
    \boldsymbol{F} &= \boldsymbol{I} + \boldsymbol{A}\Delta t\\
    \boldsymbol{G} &= \boldsymbol{B}\Delta t
\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}^a_k) + \boldsymbol{R}_{b_ib_{k+1}}(\boldsymbol{a}^{b_{k+1}} -\boldsymbol{b}^a_{k+1}))\\
    {\boldsymbol{\alpha}}_{b_ib_{k+1}} &= f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k) = {\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}} &= f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k) ={\boldsymbol{\beta}}_{b_ib_k} + \boldsymbol{a}\delta t\\
\boldsymbol{{\boldsymbol{\gamma}}}_{b_ib_{k+1}}&= f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k) =\boldsymbol{{\boldsymbol{\gamma}}}_{b_ib_k} \otimes\begin{bmatrix}
    1\\
    \frac{1}{2}\boldsymbol{\omega}\delta t
\end{bmatrix}\\
    \boldsymbol{b}_{k+1}^a &= f_4(\boldsymbol{x}_{k}, \boldsymbol{u}_k) =\boldsymbol{b}_{k}^a + \boldsymbol{n}_{b_k^a}\Delta t\\
    \boldsymbol{b}_{k+1}^g &= f_5(\boldsymbol{x}_{k}, \boldsymbol{u}_k) =\boldsymbol{b}_{k}^g + \boldsymbol{n}_{b_k^g}\Delta t
\end{aligned}
$$
  • $\boldsymbol{F}$ 推导

$\boldsymbol{F}$ 为状态方程 $\boldsymbol{x}_{k+1} = f(\boldsymbol{x}_k, \boldsymbol{u}_k)$ 对上一时刻状态量 $\boldsymbol{x}_k$ 的 雅可比，下面先将其形式以及比较简单的部分列出来，对不太直观的部分单独进行推导：

$$
\begin{aligned}
    \boldsymbol{F} &= \frac{\partial f(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{x}_k}\\
    &= \begin{bmatrix}
        \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\alpha}}}_{b_k}} &
        \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\beta}}}_{b_k}} &
        \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\gamma}}}_{b_k}} &
        \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^a_{b_k}} &
        \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^g_{b_k}} \\
        \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\alpha}}}_{b_k}} &
        \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\beta}}}_{b_k}} &
        \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\gamma}}}_{b_k}} &
        \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^a_{b_k}} &
        \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^g_{b_k}} \\
        \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\alpha}}}_{b_k}} &
        \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\beta}}}_{b_k}} &
        \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\gamma}}}_{b_k}} &
        \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^a_{b_k}} &
        \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^g_{b_k}} \\
        \frac{\partial f_4(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\alpha}}}_{b_k}} &
        \frac{\partial f_4(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\beta}}}_{b_k}} &
        \frac{\partial f_4(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\gamma}}}_{b_k}} &
        \frac{\partial f_4(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^a_{b_k}} &
        \frac{\partial f_4(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^g_{b_k}} \\
        \frac{\partial f_5(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\alpha}}}_{b_k}} &
        \frac{\partial f_5(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\beta}}}_{b_k}} &
        \frac{\partial f_5(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\gamma}}}_{b_k}} &
        \frac{\partial f_5(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^a_{b_k}} &
        \frac{\partial f_5(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^g_{b_k}}
    \end{bmatrix}\\
    &= \begin{bmatrix}
        \boldsymbol{I} & \boldsymbol{I}\delta t & f_{13} & f_{14} & f_{15} \\
        \boldsymbol{0} & \boldsymbol{I} & f_{23} & f_{24} & f_{25} \\
        \boldsymbol{0} & \boldsymbol{0} & f_{33} & \boldsymbol{0} & f_{35} \\
        \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{I} & \boldsymbol{0} \\
        \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{I} 
    \end{bmatrix}
\end{aligned}
$$

接下来对上面不太直观的部分进行推导：

第一行：

$f_{13}$：

$$
\begin{aligned}
    f_{13} &= \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\gamma}}}_{b_k}}\\
    &= \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{R}_{b_ib_k}}\\
    &= \frac{\partial (\frac{1}{2}\boldsymbol{a}\delta t^2)}{\partial\boldsymbol{R}_{b_ib_k}}\\
    &= (\frac{1}{2}\delta t^2) \frac{\partial \boldsymbol{a}}{\partial\boldsymbol{R}_{b_ib_k}}\\
    \frac{\partial \boldsymbol{a}}{\partial\boldsymbol{R}_{b_ib_k}} &= \frac{1}{2}(\frac{\partial\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_k}-\boldsymbol{b}^a_{b_k})}{\partial\boldsymbol{R}_{b_ib_k}} + \frac{\partial\boldsymbol{R}_{b_ib_{k+1}}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})}{\partial\boldsymbol{R}_{b_ib_{k}}})
\end{aligned}
$$

下面对两部分分别来看，第一部分的推导在这篇博客 一些常见的关于旋转的函数对旋转求导过程的推导 里面提及到了，这里直接给出结果：

$$
\frac{\partial\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_k}-\boldsymbol{b}^a_{b_k})}{\partial\boldsymbol{R}_{b_ib_k}} = -\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_k}-\boldsymbol{b}^a_{b_k})^\wedge
$$

第二部分：

$$
\begin{aligned}
\frac{\partial\boldsymbol{R}_{b_ib_{k+1}}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})}{\partial\boldsymbol{R}_{b_ib_{k}}}) &= \frac{\partial\boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega}\delta t)^\wedge})(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})}{\partial\boldsymbol{R}_{b_ib_{k}}}\\
&= \lim_{\boldsymbol{\psi} \rightarrow 0}\frac{\boldsymbol{R}_{b_ib_{k}}\exp{(\boldsymbol{\psi}^\wedge})\exp{((\boldsymbol{\omega}\delta t)^\wedge})(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})-\boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega}\delta t)^\wedge})(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})}{\boldsymbol{\psi}}\\
&\approx \lim_{\boldsymbol{\psi} \rightarrow 0}\frac{\boldsymbol{R}_{b_ib_{k}}(\boldsymbol{I} + \boldsymbol{\psi}^\wedge)\exp{((\boldsymbol{\omega}\delta t)^\wedge})(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})-\boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega}\delta t)^\wedge})(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})}{\boldsymbol{\psi}}\\
&= \lim_{\boldsymbol{\psi} \rightarrow 0}\frac{\boldsymbol{R}_{b_ib_{k}}\boldsymbol{\psi}^\wedge\exp{((\boldsymbol{\omega}\delta t)^\wedge})(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})}{\boldsymbol{\psi}}\\
&= -\lim_{\boldsymbol{\psi} \rightarrow 0}\frac{\boldsymbol{R}_{b_ib_{k}}(\exp{((\boldsymbol{\omega}\delta t)^\wedge})(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}}))^\wedge\boldsymbol{\psi}}{\boldsymbol{\psi}}\\
&= -\boldsymbol{R}_{b_ib_{k}}(\exp{((\boldsymbol{\omega}\delta t)^\wedge})(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}}))^\wedge\\
&= -\boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega}\delta t)^\wedge})(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})^\wedge\exp{((\boldsymbol{\omega}\delta t)^\wedge})^T\\
&= -\boldsymbol{R}_{b_ib_{k+1}}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})^\wedge\exp{((-\boldsymbol{\omega}\delta t)^\wedge})\\
&\approx -\boldsymbol{R}_{b_ib_{k+1}}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})^\wedge(\boldsymbol{I} -\boldsymbol{\omega}^\wedge\delta t)
\end{aligned}
$$

上式中利用了李群和反对称算子的这个性质，证明过程TODO，可以参考 Properties of skew symmetric matrices

$$
(\boldsymbol{Ra})^\wedge = \boldsymbol{Ra}^\wedge\boldsymbol{R}^T
$$

将两部分汇总一下，有：

$$
\begin{aligned}
    f_{13} &= (\frac{1}{2}\delta t^2) \frac{\partial \boldsymbol{a}}{\partial\boldsymbol{R}_{b_ib_k}}\\
    &= \frac{1}{4}(\frac{\partial\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_k}-\boldsymbol{b}^a_{b_k})}{\partial\boldsymbol{R}_{b_ib_k}} + \frac{\partial\boldsymbol{R}_{b_ib_{k+1}}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})}{\partial\boldsymbol{R}_{b_ib_{k}}})\delta t^2\\
    &= -\frac{1}{4}(\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_k}-\boldsymbol{b}^a_{b_k})^\wedge +\boldsymbol{R}_{b_ib_{k+1}}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k}})^\wedge(\boldsymbol{I} -\boldsymbol{\omega}^\wedge\delta t))\delta t^2
\end{aligned}
$$

接下来很多部分的计算涉及到相同的化简技巧时会直接给出结果，不进行重复推导。

$f_{14}$：

$$  
\begin{aligned}
    f_{14} &= \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^a_{b_k}}\\
    &= \frac{1}{4}\delta t^2 \frac{\partial (-\boldsymbol{R}_{b_ib_k}\boldsymbol{b}_k^a)}{\partial\boldsymbol{b}^a_{b_k}} + \frac{\partial (-\boldsymbol{R}_{b_ib_{k+1}}\boldsymbol{b}_{k+1}^a)}{\partial\boldsymbol{b}^a_{b_k}}\\
    &= -\frac{1}{4}\delta t^2 (\frac{\partial (\boldsymbol{R}_{b_ib_k}\boldsymbol{b}_k^a)}{\partial\boldsymbol{b}^a_{b_k}} + \frac{\partial (\boldsymbol{R}_{b_ib_{k+1}}\boldsymbol{b}_{k+1}^a)}{\partial\boldsymbol{b}^a_{b_k}})\\
    &= -\frac{1}{4} (\boldsymbol{R}_{b_ib_k}+ \boldsymbol{R}_{b_ib_{k+1}})\delta t^2\\
\end{aligned}
$$

$f_{15}$:

$$
\begin{aligned}
    f_{15} &= \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^g_{b_k}}\\
    &= \frac{1}{2}\delta t^2\frac{\partial\boldsymbol{a}}{\partial\boldsymbol{b}^g_{b_k}}\\
    &= \frac{1}{4}\delta t^2
    \frac{\partial(\boldsymbol{R}_{b_ib_{k+1}}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}}))}{\partial\boldsymbol{b}^g_{b_k}}\\
    &= \frac{1}{4}\delta t^2\frac{\partial(\boldsymbol{R}_{b_ib_{k}}\exp{(\boldsymbol{\omega}^\wedge\delta t)}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}}))}{\partial\boldsymbol{b}^g_{b_k}}\\
    \frac{\partial(\boldsymbol{R}_{b_ib_{k}}\exp{(\boldsymbol{\omega}^\wedge\delta t)}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}}))}{\partial\boldsymbol{b}^g_{b_k}} &= \lim_{\delta\boldsymbol{b}^g_{b_k}\rightarrow 0}\frac{
        (\boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega}-\delta\boldsymbol{b}^g_{b_k})^\wedge\delta t)}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})) - 
        (\boldsymbol{R}_{b_ib_{k}}\exp{(\boldsymbol{\omega}^\wedge\delta t)}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}}))
    }{\delta\boldsymbol{b}^g_{b_k}}\\
\end{aligned}
$$

公式比较长，先单独上式分子中的第一部分进行化简：

$$
\begin{aligned}
    & \boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega}-\delta\boldsymbol{b}^g_{b_k})^\wedge\delta t)}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}}) \\
    =&   \boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega}\delta t)^\wedge)\exp{(-(\boldsymbol{J}_r(\boldsymbol{\omega}\delta t)\delta\boldsymbol{b}^g_{b_k}\delta t)^\wedge)}}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})\\
    =&   \boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega}\delta t)^\wedge)(\boldsymbol{I} -(\boldsymbol{J}_r(\boldsymbol{\omega}\delta t)\delta\boldsymbol{b}^g_{b_k}\delta t)^\wedge))}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})\\
    \approx& \boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega}\delta t)^\wedge)(\boldsymbol{I} -(\boldsymbol{I}\delta\boldsymbol{b}^g_{b_k}\delta t)^\wedge))}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})
\end{aligned}
$$

这里利用了李代数扰动的性质：$\exp{(\boldsymbol{\phi} + \delta\boldsymbol{\psi})} = \exp{\boldsymbol{\phi}}\exp{(\boldsymbol{J}_r(\phi)\delta\boldsymbol{\psi})^\wedge}$ ,并且当 $\boldsymbol{\phi}$ 很小时 $J_r(\boldsymbol{\phi})\approx\boldsymbol{I}$， 可以参考我的这篇博客 移动机器人位姿不同的表示方法。将结果代入上式化简后有：

$$
\begin{aligned}
    f_{15} &= \frac{1}{4}\delta t^2\lim_{\delta\boldsymbol{b}^g_{b_k}\rightarrow 0}\frac{
            \boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega}\delta t)^\wedge)(-(\delta\boldsymbol{b}^g_{b_k}\delta t)^\wedge))}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})
        }{\delta\boldsymbol{b}^g_{b_k}}\\
        &= \frac{1}{4}\delta t^2\lim_{\delta\boldsymbol{b}^g_{b_k}\rightarrow 0}\frac{
            \boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega}\delta t)^\wedge)(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})^\wedge(\delta\boldsymbol{b}^g_{b_k}\delta t)}
        }{\delta\boldsymbol{b}^g_{b_k}}\\
        &= \frac{1}{4}\delta t^2 \boldsymbol{R}_{b_ib_{k}} \exp{((\boldsymbol{\omega}\delta t)^\wedge)}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})^\wedge\delta t\\
        &= \frac{1}{4}\boldsymbol{R}_{b_ib_{k+1}}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})^\wedge\delta t^3
\end{aligned}
$$

第二行：

  • $f_{23}$:

基本和 $f_{13}$ 推导过程一致：

$$
\begin{aligned}
    f_{23} &= \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{{\boldsymbol{\gamma}}}_{b_k}}\\
    &= \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{R}_{b_k}}\\
    &= -\frac{1}{2}(\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_k}-\boldsymbol{b}^a_{b_k})^\wedge +\boldsymbol{R}_{b_ib_{k+1}}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k}})^\wedge(\boldsymbol{I} -\boldsymbol{\omega}^\wedge\delta t))\delta t
\end{aligned}
$$
  • $f_{24}$:

基本和 $f_{14}$ 推导过程一致：

$$
\begin{aligned}
    f_{24} &= \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^a_{b_k}}\\
    &= -\frac{1}{2} (\boldsymbol{R}_{b_ib_k}+ \boldsymbol{R}_{b_ib_{k+1}})\delta t
\end{aligned}
$$
  • $f_{25}$:

基本和 $f_{15}$ 推导过程一致：

$$
\begin{aligned}
    f_{25} &= \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{b}^g_{b_k}}\\
    &= \frac{1}{2}\boldsymbol{R}_{b_ib_{k+1}}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})^\wedge\delta t^2
\end{aligned}
$$

第三行：

  • $f_{33}$:

这里注意，由于求导的分子项是一个旋转矩阵（或者四元数），不支持通常的加法，所以这里的推导不能像上面一样加扰动然后直接进行减法（对 $f_{13}$ 由于分子是一个向量所以可以直接相减），这里我们要对分子和分母分别加扰动（$\delta\theta_{b_{k+1}}, \delta\theta_{b_{k}}$），通过求出这两个变量的比值来求导，推导过程如下：

$$
\begin{aligned}
    \boldsymbol{R}_{b_ib_{k+1}}\exp{(\delta\boldsymbol{\theta_{b_{k+1}}})^\wedge} &= \boldsymbol{R}_{b_ib_{k}}\exp{(\delta\boldsymbol{\theta_{b_{k}}})^\wedge}\exp{(\boldsymbol{\omega}\delta t)^\wedge}\\
    \exp{(\delta\boldsymbol{\theta_{b_{k+1}}})^\wedge}&= \boldsymbol{R}_{b_ib_{k+1}}^{-1}\boldsymbol{R}_{b_ib_{k}}\exp{(\delta\boldsymbol{\theta_{b_{k}}})^\wedge}\exp{(\boldsymbol{\omega}\delta t)^\wedge}\\
    \exp{(\delta\boldsymbol{\theta_{b_{k+1}}})^\wedge} &= \boldsymbol{R}_{b_{k+1}b_{k}}\exp{(\delta\boldsymbol{\theta_{b_{k}}})^\wedge}\exp{(\boldsymbol{\omega}\delta t)^\wedge}\\
    \exp{(\delta\boldsymbol{\theta_{b_{k+1}}})^\wedge} &=\boldsymbol{R}_{b_{k}b_{k+1}}^T\exp{(\delta\boldsymbol{\theta_{b_{k}}})^\wedge}\boldsymbol{R}_{b_{k}b_{k+1}}\\
    \exp{(\delta\boldsymbol{\theta_{b_{k+1}}})^\wedge} &=\exp{(\boldsymbol{R}_{b_{k}b_{k+1}}^T\delta\boldsymbol{\theta_{b_{k}}})^\wedge}\\
    \Rightarrow \delta\boldsymbol{\theta_{b_{k+1}}}^\wedge &= \boldsymbol{R}_{b_{k}b_{k+1}}^T\delta\boldsymbol{\theta_{b_{k}}}\\
    \delta\boldsymbol{\theta_{b_{k+1}}} &= \exp{(-\boldsymbol{\omega}\delta t)^\wedge}\delta\boldsymbol{\theta_{b_{k}}}\\
    \Rightarrow f_{33} &= \frac{\delta\boldsymbol{\theta_{b_{k+1}}}}{\delta\boldsymbol{\theta_{b_{k}}}} = \exp{(-\boldsymbol{\omega}\delta t)^\wedge}\\
    &\approx \boldsymbol{I} - \boldsymbol{\omega}^\wedge\delta t
\end{aligned}
$$
  • $f_{35}$:

推导思路同上，这次是将扰动加在 $\boldsymbol{b}^g_{b_k}$ 上，同理在计算过程中 $\boldsymbol{\omega}$ 要减去零偏，所以体现在求导上也是相减，利用李代数扰动性质的化简参考 $f_{15}$ 的推导过程，如下所示：

$$
\begin{aligned}
    \boldsymbol{R}_{b_ib_{k+1}}\exp{(\boldsymbol{\delta\theta_{b_{k+1}}})^\wedge} &= \boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega} - \delta\boldsymbol{b}_{b_k})\delta t)^\wedge}\\
    \exp{(\delta\boldsymbol{\theta_{b_{k+1}}})^\wedge}&=\boldsymbol{R}_{b_ib_{k+1}}^T\boldsymbol{R}_{b_ib_{k}}\exp{((\boldsymbol{\omega} - \delta\boldsymbol{b}_{b_k})\delta t)^\wedge}\\
    \exp{(\delta\boldsymbol{\theta_{b_{k+1}}})^\wedge}&= \exp{(-\boldsymbol{\omega}\delta t)}\exp{((\boldsymbol{\omega} - \delta\boldsymbol{b}_{b_k})\delta t)^\wedge}\\
    \exp{(\delta\boldsymbol{\theta_{b_{k+1}}})^\wedge}&\approx \exp{(-\boldsymbol{\omega}\delta t)}\exp{(\boldsymbol{\omega}\delta t)}\exp{(J_r(\boldsymbol{\omega}\delta t)( - \delta\boldsymbol{b}_{b_k})\delta t)^\wedge}\\
    \exp{(\boldsymbol{\delta\theta_{b_{k+1}}})^\wedge}&\approx \exp{(( - \delta\boldsymbol{b}_{b_k})\delta t)^\wedge}\\
    \Rightarrow \delta\boldsymbol{\theta_{b_{k+1}}} &= - \delta\boldsymbol{b}_{b_k}\delta t\\
    f_{35} = \frac{\delta\boldsymbol{\theta_{b_{k+1}}}}{\delta\boldsymbol{b}^g_{b_k}} &= \boldsymbol{I}\delta t
\end{aligned}
$$
  • $\boldsymbol{G}$ 推导

$\boldsymbol{G}$ 为状态方程 $\boldsymbol{x}_{k+1} = f(\boldsymbol{x}_k, \boldsymbol{u}_k)$ 对控制量 $\boldsymbol{u}_k$ 的 雅可比，同样下面先将其形式以及比较简单的部分列出来，对不太直观的部分单独进行推导：

$$
\begin{aligned}
    \boldsymbol{G} &=
    \frac{\partial f(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{u}_k}\\
    &= \begin{bmatrix}
        \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_k}} &
        \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_k}} &
        \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_{k+1}}} &
        \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_{k+1}}} &
        \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{n}^a_{b_k}} &
        \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{n}^g_{b_k}}\\
        \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_k}} &
        \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_k}} &
        \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_{k+1}}} &
        \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_{k+1}}} &
        \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{n}^a_{b_k}} &
        \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{n}^g_{b_k}}\\
        \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_k}} &
        \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_k}} &
        \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_{k+1}}} &
        \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_{k+1}}} &
        \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{n}^a_{b_k}} &
        \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{n}^g_{b_k}}\\
        \frac{\partial f_4(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_k}} &
        \frac{\partial f_4(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_k}} &
        \frac{\partial f_4(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_{k+1}}} &
        \frac{\partial f_4(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_{k+1}}} &
        \frac{\partial f_4(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{n}^a_{b_k}} &
        \frac{\partial f_4(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{n}^g_{b_k}}\\
        \frac{\partial f_5(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_k}} &
        \frac{\partial f_5(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_k}} &
        \frac{\partial f_5(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_{k+1}}} &
        \frac{\partial f_5(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_{k+1}}} &
        \frac{\partial f_5(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{n}^a_{b_k}} &
        \frac{\partial f_5(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{n}^g_{b_k}}
    \end{bmatrix}\\
    &= \begin{bmatrix}
        g_{11} & g_{12} & g_{13} & g_{14} & \boldsymbol{0} & \boldsymbol{0}\\
        g_{21} & g_{22} & g_{23} & g_{24} & \boldsymbol{0} & \boldsymbol{0}\\
        g_{31} & \boldsymbol{0} & g_{33} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0}\\
        \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{I}\delta t & \boldsymbol{0}\\
        \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{0} & \boldsymbol{I}\delta t
    \end{bmatrix}
\end{aligned}
$$

上式中，在进行 $\boldsymbol{\omega}$ 和 $\boldsymbol{a}$ 的计算时，为了简化计算，我们通常认为这个时刻的零偏保持不变。只在进行下一时刻零偏的估计时考虑噪声作为输入，前面三项对加速度计和陀螺仪的输入（噪声）求导为 0。

下面对剩余的 10 项进行推导：

第一行：

  • $g_{11}$:

推导过程如下：

$$
\begin{aligned}
    g_{11} &= \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_k}}\\
    &= \frac{\partial{\frac{1}{2}\boldsymbol{a}\delta t^2}}{\partial\boldsymbol{\omega}^{b_k}}\\
    &= \frac{1}{4}\delta t^2\frac{\partial{\boldsymbol{R}_{b_ib_k}\exp{(\boldsymbol{\omega}\delta t)^\wedge}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}}})}{\partial\boldsymbol{\omega}^{b_k}}\\
    &= -\frac{1}{4}\delta t^2\frac{\partial{\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}}})^\wedge(\boldsymbol{\omega}\delta t)}{\partial\boldsymbol{\omega}^{b_k}}\\
    &= -\frac{1}{4}\delta t^2\frac{\partial{\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}}})^\wedge(\boldsymbol{\omega}\delta t)}{\partial\boldsymbol{\omega}^{b_k}}\\
    &= -\frac{1}{8}\delta t^2\frac{\partial{\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}}})^\wedge(\boldsymbol{\omega}^{b_k}\delta t)}{\partial\boldsymbol{\omega}^{b_k}}\\
    &= -\frac{1}{8}\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})^\wedge\delta t^3
\end{aligned}
$$
  • $g_{12}$:

推导过程如下，跟上面过程类似：

$$
\begin{aligned}
    g_{12} &= \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_k}}\\
    &= \frac{\partial{\frac{1}{2}\boldsymbol{a}\delta t^2}}{\partial\boldsymbol{a}^{b_k}}\\
    &=  \frac{1}{4}\delta t^2\frac{\partial{\boldsymbol{R}_{b_ib_k}\boldsymbol{a}^{b_k}}}{\partial\boldsymbol{a}^{b_k}}\\
    &= \frac{1}{4}\boldsymbol{R}_{b_ib_k}\delta t^2
\end{aligned}
$$
  • $g_{13}$:

推导过程如下，跟 $g_{11}$ 基本一样：

$$
\begin{aligned}
    g_{13} &= \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_{k+1}}}\\
    &= -\frac{1}{8}\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})^\wedge\delta t^3\\
    &= g_{11}
\end{aligned}
$$
  • $g_{14}$:

推导过程如下，跟 $g_{12}$ 基本一样：

$$
\begin{aligned}
    g_{14} &= \frac{\partial f_1(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_{k+1}}}\\
    &= \frac{1}{4}\boldsymbol{R}_{b_ib_{k+1}}\delta t^2
\end{aligned}
$$

第二行：

  • $g_{21}$:

推导过程如下，跟 $g_{11}$ 基本一样：

$$
\begin{aligned}
    g_{21} &= \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_k}}\\
    &= -\frac{1}{4}\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})^\wedge\delta t^2
\end{aligned}
$$
  • $g_{22}$:

推导过程如下，跟 $g_{11}$ 基本一样：

$$
\begin{aligned}
    g_{22} &= \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_k}} \\
    &= \frac{1}{2}\boldsymbol{R}_{b_ib_k}\delta t
\end{aligned}
$$
  • $g_{23}$:

推导过程如下，结果跟 $g_{21}$ 一样：

$$
\begin{aligned}
    g_{23} &= \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_{k+1}}}\\
    &= -\frac{1}{4}\boldsymbol{R}_{b_ib_k}(\boldsymbol{a}^{b_{k+1}}-\boldsymbol{b}^a_{b_{k+1}})^\wedge\delta t^2\\
    &= g_{21}
\end{aligned}
$$
  • $g_{24}$:

推导过程如下，跟 $g_{22}$ 基本一样：

$$
\begin{aligned}
    g_{24} &= \frac{\partial f_2(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{a}^{b_{k+1}}} \\
    &= \frac{1}{2}\boldsymbol{R}_{b_ib_{k+1}}\delta t
\end{aligned}
$$

第三行：

  • $g_{31}$:

推导过程如下，跟 $f_{35}$ 基本一样：

$$
\begin{aligned}
    g_{31} &= \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_k}}\\
    &= \frac{1}{2}\boldsymbol{I}\delta t
\end{aligned}
$$
  • $g_{33}$:

推导过程如下，跟 $f_{35}$ 基本一样：

$$
\begin{aligned}
    g_{33} &= \frac{\partial f_3(\boldsymbol{x}_{k}, \boldsymbol{u}_k)}{\partial\boldsymbol{\omega}^{b_{k+1}}}\\
    &= \frac{1}{2}\boldsymbol{I}\delta t\\
    &= g_{33}
\end{aligned}
$$

参考资料

© 2020 - 2022 XT 的技术博客
Built with Hugo
Theme Stack designed by Jimmy