IMU 预积分（仅供参考）
IMU Pre-Integration
本文章记录在学习 VIO 的过程中对 IMU 预积分的一些理解和公式的证明，也看了很多的文章和论文，知识点和证明过于复杂，特此记录，方便复习回顾，参考文献和网址也已经在文章最后给出，大家也可以自行查阅。
目录
0. 部分先验知识
\[\boldsymbol{\omega}^{\wedge}=\left[\begin{array}{c}
\omega_{1} \\
\omega_{2} \\
\omega_{3}
\end{array}\right]^{\wedge}=\left[\begin{array}{ccc}
0 & -\omega_{3} & \omega_{2} \\
\omega_{3} & 0 & -\omega_{1} \\
-\omega_{2} & \omega_{1} & 0
\end{array}\right] \in \mathfrak{s o}(3) .
\]
\[\mathbf{a}^{\wedge} \mathbf{b}=-\mathbf{b}^{\wedge} \mathbf{a}, \quad \forall \mathbf{a}, \mathbf{b} \in \mathbb{R}^{3}
\]
\[\exp \left(\phi^{\wedge}\right)=\mathbf{I}+\frac{\sin (\|\phi\|)}{\|\phi\|} \phi^{\wedge}+\frac{1-\cos (\|\phi\|)}{\|\phi\|^{2}}\left(\phi^{\wedge}\right)^{2} .
\]
\[\exp \left(\phi^{\wedge}\right) \approx \mathbf{I}+\phi^{\wedge} \\
Taylor \ Expansion \ approx
\]
\[\begin{aligned}
\frac{\partial\left(\exp \left(\phi^{\wedge}\right) \mathrm{p}\right)}{\partial \phi} &=\lim _{\delta \phi \rightarrow 0} \frac{\exp \left((\phi+\delta \phi)^{\wedge}\right) \mathrm{p}-\exp \left(\phi^{\wedge}\right) \mathrm{p}}{\delta \phi} \\
1: &=\lim _{\delta \phi \rightarrow 0} \frac{\exp \left(\left(\mathrm{J}_{1} \delta \phi\right)^{\wedge}\right) \exp \left(\phi^{\wedge}\right) \mathrm{p}-\exp \left(\phi^{\wedge}\right) \mathrm{p}}{\delta \phi} \\
2: & \approx \lim _{\delta \phi \rightarrow 0} \frac{\left(\mathrm{I}+\left(\mathrm{J}_{1} \delta \phi\right)^{\wedge}\right) \exp \left(\phi^{\wedge}\right) \mathrm{p}-\exp \left(\phi^{\wedge}\right) \mathrm{p}}{\delta \phi} \\
3: &=\lim _{\delta \phi \rightarrow 0} \frac{\left(\mathrm{J}_{1} \delta \phi\right)^{\wedge} \exp \left(\phi^{\wedge}\right) \mathrm{p}}{\delta \phi} \\
4: &=\lim _{\delta \phi \rightarrow 0} \frac{-\left(\exp \left(\phi^{\wedge}\right) \mathrm{p}\right)^{\wedge} \mathrm{J}_{1} \delta \phi}{\delta \phi} \\
5: &=-(\mathrm{Rp})^{\wedge} \mathrm{J}_{1}
\end{aligned}
\]
1.0 预积分定义
1.1 引出预积分
与传统IMU的运动学积分不同，预积分可以将一段时间内的IMU测量数据累计起来，建立预积分测量，因而十分适合以关键帧为基础单元的激光或视觉SLAM系统。
IMU 的真实值为 \(\boldsymbol{\omega}, \mathbf{a}\) , 测量值为 \(\tilde{\boldsymbol{\omega}}\), $\tilde{\mathbf{a}} $, 则有：
\[\begin{aligned}
\tilde{\boldsymbol{\omega}}^{b} &=\boldsymbol{\omega}^{b}+\mathbf{b}^{g}+\mathbf{n}^{g} \\
\tilde{\mathbf{a}}^{b} &=\mathbf{q}_{b w}\left(\mathbf{a}^{w}+\mathbf{g}^{w}\right)+\mathbf{b}^{a}+\mathbf{n}^{a}
\end{aligned}
\]
上标 \(g\) 表示 \(gyro\), \(a\) 表示 $ acc$， \(w\) 表示在世界坐标系 \(world\), \(b\) 表示 $ imu$ 机体坐标系 \(body\)。$ \mathrm{PVQ}$ 对时间的导数可写成:
\[\begin{array}{l}
\dot{\mathbf{p}}_{w b_{t}}=\mathbf{v}_{t}^{w} \\
\dot{\mathbf{v}}_{t}^{w}=\mathbf{a}_{t}^{w} \\
\dot{\mathbf{q}}_{w b_{t}}=\mathbf{q}_{w b_{t}} \otimes\left[\begin{array}{c}
0 \\
\frac{1}{2} \boldsymbol{\omega}^{b_{t}}
\end{array}\right]
\end{array}
\]
预积分的过程可以用如下图来示意：
从第 $ i $ 时刻的 $ \mathrm{PVQ}$ 对 IMU 的测量值进行积分得到第 $j $ 时刻的 $ \mathrm{PVQ}$:
\[\begin{array}{l}
\mathbf{p}_{w b_{j}}=\mathbf{p}_{w b_{i}}+\mathbf{v}_{i}^{w} \Delta t+\iint_{t \in[i, j]}\left(\mathbf{q}_{w b_{t}} \mathbf{a}^{b_{t}}-\mathbf{g}^{w}\right) \delta t^{2} \\
\mathbf{v}_{j}^{w}=\mathbf{v}_{i}^{w}+\int_{t \in[i, j]}\left(\mathbf{q}_{w b_{t}} \mathbf{a}^{b_{t}}-\mathbf{g}^{w}\right) \delta t \\
\mathbf{q}_{w b_{j}}=\int_{t \in[i, j]} \mathbf{q}_{w b_{t}} \otimes\left[\begin{array}{c}
0 \\
\frac{1}{2} \boldsymbol{\omega}^{b_{t}}
\end{array}\right] \delta t
\end{array}
\]
问题:每次 $q_{wb_t} $ 优化更新后,都需要重新进行积分,运算量较大。
一个很简单的公式转换,就可以将积分模型转为预积分模型:
\[\mathbf{q}_{w b_{t}}=\mathbf{q}_{w b_{i}} \otimes \mathbf{q}_{b_{i} b_{t}}
\]
那么,$ \mathrm{PVQ}$ 积分公式中的积分项则变成相对于第 \(i\) 时刻的姿态,而不是相对于世界坐标系的姿态:
\[\begin{array}{l}
\mathbf{p}_{w b_{j}}=\mathbf{p}_{w b_{i}}+\mathbf{v}_{i}^{w} \Delta t-\frac{1}{2} \mathbf{g}^{w} \Delta t^{2}+\mathbf{q}_{w b_{i}} \iint_{t \in[i, j]}\left(\mathbf{q}_{b_{i} b_{t}} \mathbf{a}^{b_{t}}\right) \delta t^{2} \\
\mathbf{v}_{j}^{w}=\mathbf{v}_{i}^{w}-\mathbf{g}^{w} \Delta t+\mathbf{q}_{w b_{i}} \int_{t \in[i, j]}\left(\mathbf{q}_{b_{i} b_{t}} \mathbf{a}^{b_{t}}\right) \delta t \\
\mathbf{q}_{w b_{j}}=\mathbf{q}_{w b_{i}} \int_{t \in[i, j]} \mathbf{q}_{b_{i} b_{t}} \otimes\left[\begin{array}{c}
0 \\
\frac{1}{2} \boldsymbol{\omega}^{b_{t}}
\end{array}\right] \delta t
\end{array}
\]
1.2 预积分量
预积分量仅仅跟 IMU 测量值有关,它将一段时间内的 IMU 数据直接积分起来就得到了预积分量:
\[\begin{array}{l}
\boldsymbol{\alpha}_{b_{i} b_{j}}=\iint_{t \in[i, j]}\left(\mathbf{q}_{b_{i} b_{t}} \mathbf{a}^{b_{t}}\right) \delta t^{2}\\
\boldsymbol{\beta}_{b_{i} b_{j}}=\int_{t \in[i, j]}\left(\mathbf{q}_{b_{i} b_{t}} \mathbf{a}^{b_{t}}\right) \delta t\\
\mathbf{q}_{b_{i} b_{j}}=\int_{t \in[i, j]} \mathbf{q}_{b_{i} b_{t}} \otimes\left[\begin{array}{c}
0 \\
\frac{1}{2} \omega^{b_{t}}
\end{array}\right] \delta t
\end{array}
\]
其中 \(\mathbf{a}\) 表示的是位移的增量，\(\mathbf{b}\) 表 示的是速度的增量，\(\mathbf{q}\) 表示的是旋转的增量。很显然，对加速度进行一次积分即得到了速度增量，对加速度进行两次积分即得到了位移增量。
将上面三个量分别带入公式\((5)\)重新整理下 $ \mathrm{PVQ}$ 的积分公式,有:
\[\left[\begin{array}{c}
\mathbf{p}_{w b_{j}} \\
\mathbf{v}_{j}^{w} \\
\mathbf{q}_{w b_{j}} \\
\mathbf{b}_{j}^{a} \\
\mathbf{b}_{j}^{g}
\end{array}\right]=\left[\begin{array}{c}
\mathbf{p}_{w b_{i}}+\mathbf{v}_{i}^{w} \Delta t-\frac{1}{2} \mathbf{g}^{w} \Delta t^{2}+\mathbf{q}_{w b_{i}} \boldsymbol{\alpha}_{b_{i} b_{j}} \\
\mathbf{v}_{i}^{w}-\mathbf{g}^{w} \Delta t+\mathbf{q}_{w b_{i}} \boldsymbol{\beta}_{b_{i} b_{j}} \\
\mathbf{q}_{w b_{i}} \mathbf{q}_{b_{i} b_{j}} \\
\mathbf{b}_{i}^{a} \\
\mathbf{b}_{i}^{g}
\end{array}\right]
\]
2.0 IMU 的预积分误差
2.1 定义
定义:一段时间内 IMU 构建的预积分量作为测量值,对两时刻之间的状态量进行约束
\[\left[\begin{array}{c}
\mathbf{r}_{p} \\
\mathbf{r}_{q} \\
\mathbf{r}_{v} \\
\mathbf{r}_{b a} \\
\mathbf{r}_{b g}
\end{array}\right]_{15 \times 1}=\left[\begin{array}{c}
\mathbf{q}_{b_{i} w}\left(\mathbf{p}_{w b_{j}}-\mathbf{p}_{w b_{i}}-\mathbf{v}_{i}^{w} \Delta t+\frac{1}{2} \mathbf{g}^{w} \Delta t^{2}\right)-\boldsymbol{\alpha}_{b_{i} b_{j}} \\
2\left[\mathbf{q}_{b_{j} b_{i}} \otimes\left(\mathbf{q}_{b_{i} w} \otimes \mathbf{q}_{w b_{j}}\right)\right]_{x y z} \\
\mathbf{q}_{b_{i} w}\left(\mathbf{v}_{j}^{w}-\mathbf{v}_{i}^{w}+\mathbf{g}^{w} \Delta t\right)-\boldsymbol{\beta}_{b_{i} b_{j}} \\
\mathbf{b}_{j}^{a}-\mathbf{b}_{i}^{a} \\
\mathbf{b}_{j}^{g}-\mathbf{b}_{i}^{g}
\end{array}\right]
\]
其实就是将公式\((7)\) 中的等式单独拿出来，然后做了一个差的运算，作为误差项。例如公式 \((7)\) 中的第一行：
\[\begin{aligned}
\mathbf{p}_{w b_{j}} = \mathbf{p}_{w b_{i}}+\mathbf{v}_{i}^{w} \Delta t-\frac{1}{2} \mathbf{g}^{w} \Delta t^{2}+\mathbf{q}_{w b_{i}} \boldsymbol{\alpha}_{b_{i} b_{j}} \\
\mathbf{q}_{w b_{i}} \boldsymbol{\alpha}_{b_{i} b_{j}} =\mathbf{p}_{w b_{j}} -\mathbf{p}_{w b_{i}}+\mathbf{v}_{i}^{w} \Delta t-\frac{1}{2} \mathbf{g}^{w} \Delta t^{2}\\
\boldsymbol{\alpha}_{b_{i} b_{j}} =\mathbf{q}_{ b_{i}w}(\mathbf{p}_{w b_{j}} -\mathbf{p}_{w b_{i}}+\mathbf{v}_{i}^{w} \Delta t-\frac{1}{2} \mathbf{g}^{w} \Delta t^{2})\\
\mathbf{r}_{p} = \mathbf{q}_{ b_{i}w}(\mathbf{p}_{w b_{j}} -\mathbf{p}_{w b_{i}}+\mathbf{v}_{i}^{w} \Delta t-\frac{1}{2} \mathbf{g}^{w} \Delta t^{2}) - \boldsymbol{\alpha}_{b_{i} b_{j}} \\
\end{aligned}
\]
公式 \((10)\) 中的其他项也是如此
上面误差中位移, 速度, 偏置都是直接相减得到。第二项是关于四元 数的旋转误差, 其中 \([\cdot]_{x y z}\) 表示只取四元数的虚部 $(x, y, z) $ 组成的三维向量。
2.2 预积分的离散形式
这里使用 mid-point 方法,即两个相邻时刻 \(k\) 到 \(k+1\) 的位姿是用两个时刻的测量值 \(\mathbf{a}\), \(\boldsymbol{\omega}\) 的平 均值来计算:
\[\begin{aligned}
\boldsymbol{\omega} &=\frac{1}{2}\left(\left(\boldsymbol{\omega}^{b_{k}}-\mathbf{b}_{k}^{g}\right)+\left(\boldsymbol{\omega}^{b_{k+1}}-\mathbf{b}_{k}^{g}\right)\right) \\
\mathbf{q}_{b_{i} b_{k+1}} &=\mathbf{q}_{b_{i} b_{k}} \otimes\left[\begin{array}{c}
1 \\
\frac{1}{2} \boldsymbol{\omega} \delta t
\end{array}\right] \\
\mathbf{a} &=\frac{1}{2}\left(\mathbf{q}_{b_{i} b_{k}}\left(\mathbf{a}^{b_{k}}-\mathbf{b}_{k}^{a}\right)+\mathbf{q}_{b_{i} b_{k+1}}\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right) \\
\boldsymbol{\alpha}_{b_{i} b_{k+1}} &=\boldsymbol{\alpha}_{b_{i} b_{k}}+\boldsymbol{\beta}_{b_{i} b_{k}} \delta t+\frac{1}{2} \mathbf{a} \delta t^{2} \\
\boldsymbol{\beta}_{b_{i} b_{k+1}} &=\boldsymbol{\beta}_{b_{i} b_{k}}+\mathbf{a} \delta t \\
\mathbf{b}_{k+1}^{a} &=\mathbf{b}_{k}^{a}+\mathbf{n}_{\mathbf{b}_{k}^{a}} \delta t \\
\mathbf{b}_{k+1}^{g} &=\mathbf{b}_{k}^{g}+\mathbf{n}_{\mathbf{b}_{k}^{g}} \delta t
\end{aligned}
\]
2.3 预积分量的方差
question: 一个 IMU 数据作为测量值的噪声方差我们能够标定。现在,一段时间内多个 IMU 数据积分形成的预积分量的方差呢?
Covariance Propagation
已知一个变量 $\mathbf{y}=\mathbf{A x}, \mathbf{x} \in \mathcal{N}\left(0, \boldsymbol{\Sigma}_{x}\right) $, 则有 \(\boldsymbol{\Sigma}_{y}=A \boldsymbol{\Sigma}_{x} A^{\top}\)
\[\begin{aligned}
\boldsymbol{\Sigma}_{y} &=E\left((\mathbf{A x})(\mathbf{A} \mathbf{x})^{\top}\right) \\
&=E\left(\mathbf{A} \mathbf{x} \mathbf{x}^{\top} \mathbf{A}^{\top}\right) \\
&=A \boldsymbol{\Sigma}_{x} A^{\top}
\end{aligned}
\]
所以,要推导预积分量的协方差,我们需要知道 imu 噪声和预积分量之间的线性递推关系。
假设已知了相邻时刻误差的线性传递方程:
\[\boldsymbol{\eta}_{i k}=\mathbf{F}_{k-1} \boldsymbol{\eta}_{i k-1}+\mathbf{G}_{k-1} \mathbf{n}_{k-1}
\]
比如: 状态量误差为 \(\boldsymbol{\eta}_{i k}=\left[\delta \boldsymbol{\theta}_{i k}, \delta \mathbf{v}_{i k}, \delta \mathbf{p}_{i k}\right]\) , 测量噪声为 \(\mathbf{n}_{k}=\left[\mathbf{n}_{k}^{g}, \mathbf{n}_{k}^{a}\right]_{\text {。 }}\)
误差的传递由两部分组成:当前时刻的误差传递给下一时刻，当前时刻测量噪声传递给下一时刻。
协方差矩阵可以通过递推计算得到:
\[\boldsymbol{\Sigma}_{i k}=\mathbf{F}_{k-1} \boldsymbol{\Sigma}_{i k-1} \mathbf{F}_{k-1}^{\top}+\mathbf{G}_{k-1} \boldsymbol{\Sigma}_{\mathbf{n}} \mathbf{G}_{k-1}^{\top}
\]
其中, \(\Sigma_{\mathbf{n}}\) 是测量噪声的协方差矩阵, 方差从 \(i\) 时刻开始进行递推, \(\boldsymbol{\Sigma}_{i i}=\mathbf{0}\) 。
2.4 状态误差线性递推公式的推导
2.4.1 简介
通常对于状态量之间的递推关系是非线性的方程如 \(\mathbf{x}_{k}=f\left(\mathbf{x}_{k-1}, \mathbf{u}_{k-1}\right)\), 其中状态量为 \(\mathbf{x}, \mathbf{u}\) 为系统的输入量。
我们可以用两种方法来推导状态误差传递的线性递推关系:
- 一种是基于一阶泰勒展开的误差递推方程。
- 一种是基于误差随时间变化的递推方程。
2.4.2 基于一阶泰勒展开的误差递推方程
令状态量为 $\mathbf{x}=\hat{\mathbf{x}}+\delta \mathbf{x} $, 其中, 真值为 \(\hat{\mathbf{x}} , 误差为 \delta \mathbf{x}\) 。另外, 输入量 $ u $ 的噪声为 $ n$ 。
基于泰勒展开的误差传递（应用于 EKF 的协方差预测）
非线性系统 \(\mathbf{x}_{k}=f\left(\mathbf{x}_{k-1}, \mathbf{u}_{k-1}\right)\) 的状态误差的线性递推关系如下:
\[\delta \mathbf{x}_{k}=\mathbf{F} \delta \mathbf{x}_{k-1}+\mathbf{G} \mathbf{n}_{k-1}
\]
其中, \(\mathbf{F}\) 是状态量 $\mathrm{x}{k} $ 对状态量 \(\mathrm{x}_{k-1}\) 的雅克比矩阵, \(\mathbf{G}\) 是状态量 $ \mathrm{x}$ 对输入量 $\mathbf{u}_{k-1} $ 的雅克比矩阵。
证明：对非线性状态方程进行一阶泰勒展开有:
\[\begin{aligned}
\mathbf{x}_{k} &=f\left(\mathbf{x}_{k-1}, \mathbf{u}_{k-1}\right) \\
\hat{\mathbf{x}}_{k}+\delta \mathbf{x}_{k} &=f\left(\hat{\mathbf{x}}_{k-1}+\delta \mathbf{x}_{k-1}, \hat{\mathbf{u}}_{k-1}+\mathbf{n}_{k-1}\right) \\
\underline{\hat{\mathbf{x}}_{k}}+\delta \mathbf{x}_{k} &=\underline{f\left(\hat{\mathbf{x}}_{k-1}, \hat{\mathbf{u}}_{k-1}\right)}+\mathbf{F} \delta \mathbf{x}_{k-1}+\mathbf{G} \mathbf{n}_{k-1}
\end{aligned}
\]
2.4.3 基于误差随时间变化的递推方程
如果我们能够推导状态误差随时间变化的导数关系,比如:
\[\dot{\delta} \mathbf{x}=\mathbf{A} \delta \mathbf{x}+\mathbf{B} \mathbf{n}
\]
则误差状态的传递方程为:
\[\begin{aligned}
\delta \mathbf{x}_{k} &=\delta \mathbf{x}_{k-1}+ \dot\delta \mathbf{x}_{k-1} \Delta t \\
\rightarrow \delta \mathbf{x}_{k} &=(\mathbf{I}+\mathbf{A} \Delta t) \delta \mathbf{x}_{k-1}+\mathbf{B} \Delta t \mathbf{n}_{k-1}
\end{aligned}
\]
对上面式子作个简单展开即可得到：
\[\begin{aligned}
\delta \mathbf{x}_{k} &=\delta \mathbf{x}_{k-1}+\dot \delta \mathbf{x}_{k-1} \Delta t \\
\delta \mathbf{x}_{k} &=\delta \mathbf{x}_{k-1}+ (\mathbf{A} \delta \mathbf{x}_{k-1} + \mathbf{Bn_{k-1}} )\Delta t \\
\delta \mathbf{x}_{k} &=\delta \mathbf{x}_{k-1}+ \mathbf{A} \delta \mathbf{x}_{k-1}\Delta t + \mathbf{Bn_{k-1}} \Delta t \\
\\ \rightarrow \delta \mathbf{x}_{k} &=(\mathbf{I}+\mathbf{A} \Delta t) \delta \mathbf{x}_{k-1}+\mathbf{B} \Delta t \mathbf{n}_{k-1}
\end{aligned}
\]
这两种推导方式的可以看出有:
\[\mathbf{F}=\mathbf{I}+\mathbf{A} \Delta t, \mathbf{G}=\mathbf{B} \Delta t
\]
2.4.4 对比分析
第一种方法不是很好么,为什么会想着去弄误差随时间的变化呢 ? 这是因为 VIO 系统中已经知道了状态的导数和状态之间的转移矩阵。
如: 我们已经知道速度和状态量之间的关系:
\[\dot{\mathbf{v}}=\mathbf{R}\mathbf{a}^b+\mathbf{g}
\]
那我们就可以推导速度的误差和状态误差之间的关系,再每一项上都加上各自的误差就有:
\[\begin{aligned}
\dot{\mathbf{v}}+\dot{\delta} \mathbf{v} &=\mathbf{R}\left(\mathbf{I}+[\delta \boldsymbol{\theta}]_{\times}\right)\left(\mathbf{a}^{b}+\delta \mathbf{a}^{b}\right)+\mathbf{g}+\delta \mathbf{g} \\
\dot{\delta \mathbf{v}} &=\mathbf{R} \delta \mathbf{a}^{b}+\mathbf{R}[\delta \boldsymbol{\theta}]_{\times}\left(\mathbf{a}^{b}+\delta \mathbf{a}^{b}\right)+\delta \mathbf{g} \\
\dot{\delta \mathbf{v}} &=\mathbf{R} \delta \mathbf{a}^{\mathbf{b}}-\mathbf{R}\left[\mathbf{a}^{b}\right]_{\times} \delta \boldsymbol{\theta}+\delta \mathbf{g}
\end{aligned}
\]
由此就能以此类推，写出整个 \(\mathbf{A}\) 和 \(\mathbf{B}\) 其他方程了。
2.5 预积分的误差递推公式推导
首先回顾预积分的误差递推公式,将测量噪声也考虑进模型:
\[\begin{aligned}
\boldsymbol{\omega} &=\frac{1}{2}\left(\left(\boldsymbol{\omega}^{b_{k}}+\mathbf{n}_{k}^{g}-\mathbf{b}_{k}^{g}\right)+\left(\boldsymbol{\omega}^{b_{k+1}}+\mathbf{n}_{k+1}^{g}-\mathbf{b}_{k}^{g}\right)\right) \\
\mathbf{q}_{b_{i} b_{k+1}} &=\mathbf{q}_{b_{i} b_{k}} \otimes\left[\begin{array}{c}
1 \\
\frac{1}{2} \boldsymbol{\omega} \delta t
\end{array}\right] \\
\mathbf{a} &=\frac{1}{2}\left(\mathbf{q}_{b_{i} b_{k}}\left(\mathbf{a}^{b_{k}}+\mathbf{n}_{k}^{a}-\mathbf{b}_{k}^{a}\right) +\mathbf{q}_{b_{i} b_{k+1}}\left(\mathbf{a}^{\left.\left.b_{k+1}+\mathbf{n}_{k+1}^{a}-\mathbf{b}_{k}^{a}\right)\right)}\right.\right.\\
\boldsymbol{\alpha}_{b_{i} b_{k+1}} &=\boldsymbol{\alpha}_{b_{i} b_{k}}+\boldsymbol{\beta}_{b_{i} b_{k}} \delta t+\frac{1}{2} \mathbf{a} \delta t^{2} \\
\boldsymbol{\beta}_{b_{i} b_{k+1}} &=\boldsymbol{\beta}_{b_{i} b_{k}}+\mathbf{a} \delta t \\
\mathbf{b}_{k+1}^{a} &=\mathbf{b}_{k}^{a}+\mathbf{n}_{\mathbf{b}_{k}^{a}} \delta t \\
\mathbf{b}_{k+1}^{g} &=\mathbf{b}_{k}^{g}+\mathbf{n}_{\mathbf{b}_{k}^{g}} \delta t
\end{aligned}
\]
确定误差传递的状态量,噪声量,然后开始构建传递方程。
-
预积分误差传递的形式:
用前面一阶泰勒展开的推导方式,我们希望能推导出如下的形式:
\[\left[\begin{array}{c} \delta \boldsymbol{\alpha}_{b_{k+1}} b_{k+1}^{\prime} \\ \delta \boldsymbol{\theta}_{b_{k+1} b_{k+1}^{\prime}} \\ \delta \boldsymbol{\beta}_{b_{k+1} b_{k+1}^{\prime}} \\ \delta \mathbf{b}_{k+1}^{a} \\ \delta \mathbf{b}_{k+1}^{g} \end{array}\right] =\mathbf{F} \left[\begin{array}{c} \delta \boldsymbol{\alpha}_{b_{k+1}} b_{k}^{\prime} \\ \delta \boldsymbol{\theta}_{b_{k+1} b_{k}^{\prime}} \\ \delta \boldsymbol{\beta}_{b_{k+1} b_{k}^{\prime}} \\ \delta \mathbf{b}_{k}^{a} \\ \delta \mathbf{b}_{k}^{g} \end{array}\right] + \mathbf{G}\left[\begin{array}{c} \mathbf{n}_{k}^{a} \\ \mathbf{n}_{k}^{g} \\ \mathbf{n}_{k+1}^{a} \\ \mathbf{n}_{k+1}^{g} \\ \mathbf{n}_{\mathbf{b}_{k}^{a}} \\ \mathbf{n}_{\mathbf{b}_{k}^{g}} \end{array}\right] \]\(\mathbf{F}, \mathbf{G}\) 为两个时刻间的协方差传递矩阵
\[\begin{aligned}
\mathbf{F} =\left[\begin{array}{cccccc}
\mathbf{I} & \mathbf{f}_{12} & \mathbf{I} \delta t & -\frac{1}{4}\left(\mathbf{q}_{b_{i} b_{k}}+\mathbf{q}_{b_{i} b_{k+1}}\right) \delta t^{2} & \mathbf{f}_{15} \\
\mathbf{0} & \mathbf{I}-[\boldsymbol{\omega}]_{\times} & \mathbf{0} & \mathbf{0} & -\mathbf{I} \delta t \\
\mathbf{0} & \mathbf{f}_{32} & \mathbf{I} & -\frac{1}{2}\left(\mathbf{q}_{b_{i} b_{k}}+\mathbf{q}_{b_{i} b_{k+1}}\right) \delta t & \mathbf{f}_{35} \\
\mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} & \mathbf{0} \\
\mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & & \mathbf{I}
\end{array}\right] \\
\mathbf{G}=\left[\begin{array}{cccccc}
\frac{1}{4} \mathbf{q}_{b_{i} b_{k}} \delta t^{2} & \mathbf{g}_{12} & \frac{1}{4} \mathbf{q}_{b_{i} b_{k+1}} \delta t^{2} & \mathbf{g}_{14} & \mathbf{0} & \mathbf{0} \\
\mathbf{0} & \frac{1}{2} \mathbf{I} \delta t & \mathbf{0} & \frac{1}{2} \mathbf{I} \delta t & \mathbf{0} & \mathbf{0} \\
\frac{1}{2} \mathbf{q}_{b_{i} b_{k}} \delta t & \mathbf{g}_{32} & \frac{1}{2} \mathbf{q}_{b_{i} b_{k+1}} \delta t & \mathbf{g}_{34} & \mathbf{0} & \mathbf{0} \\
\mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} \delta t & \mathbf{0} \\
\mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} \delta t
\end{array}\right]
\end{aligned}
\]
其中的系数为：
\[\begin{aligned}
&\mathbf{f}_{12}=\frac{\partial \boldsymbol{\alpha}_{b_{i} b_{k+1}}}{\partial \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}}=-\frac{1}{4}\left(\mathbf{R}_{b_{i} b_{k}}\left[\mathbf{a}^{b_{k}}-\mathbf{b}_{k}^{a}\right]_{\times} \delta t^{2}+\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right]_{\times}\left(\mathbf{I}-[\boldsymbol{\omega}]_{\times} \delta t\right) \delta t^{2}\right)\\
&\mathbf{f}_{32}=\frac{\partial \boldsymbol{\beta}_{b_{i} b_{k+1}}}{\partial \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}}=-\frac{1}{2}\left(\mathbf{R}_{b_{i} b_{k}}\left[\mathbf{a}^{b_{k}}-\mathbf{b}_{k}^{a}\right]_{\times} \delta t+\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right]_{\times}\left(\mathbf{I}-[\boldsymbol{\omega}]_{\times} \delta t\right) \delta t\right)\\
&\mathbf{f}_{15}=\frac{\partial \boldsymbol{\alpha}_{b_{i} b_{k+1}}}{\partial \delta \mathbf{b}_{k}^{g}}=-\frac{1}{4}\left(\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right]_{\times} \delta t^{2}\right)(-\delta t)\\
&\mathbf{f}_{35}=\frac{\partial \boldsymbol{\beta}_{b_{i} b_{k+1}}}{\partial \delta \mathbf{b}_{k}^{g}}=-\frac{1}{2}\left(\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right]_{\times} \delta t\right)(-\delta t)\\
&\mathbf{g}_{12}=\frac{\partial \boldsymbol{\alpha}_{b_{i} b_{k+1}}}{\partial \mathbf{n}_{k}^{g}}=\mathbf{g}_{14}=\frac{\partial \boldsymbol{\alpha}_{b_{i} b_{k+1}}}{\partial \mathbf{n}_{k+1}^{g}}=-\frac{1}{4}\left(\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right]_{\times} \delta t^{2}\right)\left(\frac{1}{2} \delta t\right)\\
&\mathbf{g}_{32}=\frac{\partial \boldsymbol{\beta}_{b_{i} b_{k+1}}}{\partial \mathbf{n}_{k}^{g}}=\mathbf{g}_{34}=\frac{\partial \boldsymbol{\beta}_{b_{i} b_{k+1}}}{\partial \mathbf{n}_{k+1}^{g}}=-\frac{1}{2}\left(\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right]_{\times} \delta t^{2}\right)\left(\frac{1}{2} \delta t\right)
\end{aligned}
\]
- 证明：
公式简化约定，考虑到公式的编辑篇幅,为了对一些求导公式进行简化,这里做一些简单的约定, 比如求导公式:
\[\frac{\partial \mathbf{x}_{a}}{\partial \delta \boldsymbol{\theta}}=\lim _{\delta \theta \rightarrow 0} \frac{\mathbf{R}_{a b} \exp \left([\delta \boldsymbol{\theta}]_{\times}\right) \mathbf{x}_{b}-\mathbf{R}_{a b} \mathbf{x}_{b}}{\delta \boldsymbol{\theta}} \\
\]
后续直接简写为 :
\[\frac{\partial \mathbf{x}_{a}}{\partial \delta \boldsymbol{\theta}}=\frac{\partial \mathbf{R}_{a b} \exp \left([\delta \boldsymbol{\theta}]_{\times}\right) \mathbf{x}_{b}}{\partial \delta \boldsymbol{\theta}}
\]
雅克比矩阵 F 的推导:
\(\mathbf{β}\) 对各状态量的雅克比推导,即 \(\mathbf{F}\) 第三行，速度预积分量 \(\mathbf{β}\) 的递推计算形式:
\[\begin{aligned}
\boldsymbol{\beta}_{b_{i} b_{k+1}} &=\boldsymbol{\beta}_{b_{i} b_{k}}+\mathbf{a} \delta t \\
&=\boldsymbol{\beta}_{b_{i} b_{k}}+\frac{1}{2}\left(\mathbf{q}_{b_{i} b_{k}}\left(\mathbf{a}^{b_{k}}-\mathbf{b}_{k}^{a}\right)+\mathbf{q}_{b_{i} b_{k+1}}\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right) \delta t
\end{aligned}
\]
$ \mathrm{f}_{33} $ : 对上一时刻速度预积分量的 \(Jacobian\)
\[\mathbf{f}_{33}=\frac{\partial \boldsymbol{\beta}_{b_{i} b_{k+1}}}{\partial \delta \boldsymbol{\beta}_{b_{k} b_{k}^{\prime}}}=\mathbf{I}_{3 \times 3}
\]
\(\mathrm{f}_{32}\) : 对角度预积分量的 \(Jacobian\)
首先将速度预积分量 \(\mathbf{β}\) 的递推计算形式写成如下形式:
\[\boldsymbol{\beta}_{b_{i} b_{k+1}}=\boldsymbol{\beta}_{b_{i} b_{k}}+\frac{1}{2}\left(\mathbf{q}_{b_{i} b_{k}}\left(\mathbf{a}^{b_{k}}-\mathbf{b}_{k}^{a}\right)+\mathbf{q}_{b_{i} b_{k}} \otimes\left[\begin{array}{c}
1 \\
\frac{1}{2} \boldsymbol{\omega} \delta t
\end{array}\right]\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right) \delta t
\]
\(\mathrm{f}_{32}\) : 对角度预积分量的\(Jacobian\)
那么,速度的预积分量对角度预积分量的 \(Jacobian\):
\[\frac{\partial \boldsymbol{\beta}_{b_{i} b_{k+1}}}{\partial \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}}=
\mathbf{ \frac{\partial a\delta t}{\partial \delta \theta_{b_{k} b_{k}^{\prime}}} }
\]
其中分子和一写成：
\[\begin{aligned}
\mathbf{a} \delta t=& \frac{1}{2} \mathbf{q}_{b_{i} b_{k}} \otimes\left[\begin{array}{c}
1 \\
\frac{1}{2} \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}
\end{array}\right]\left(\mathbf{a}^{b_{k}}-\mathbf{b}_{k}^{a}\right) \delta t \\
&+\frac{1}{2} \mathbf{q}_{b_{i} b_{k}} \otimes\left[\begin{array}{c}
1 \\
\frac{1}{2} \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}
\end{array}\right] \otimes\left[\begin{array}{c}
1 \\
\frac{1}{2} \boldsymbol{\omega} \delta t
\end{array}\right]\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right) \delta t \\
=& \underbrace{\frac{1}{2} \mathbf{R}_{b_{i} b_{k}} \exp \left(\left[\delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}\right]_{\times}\right)\left(\mathbf{a}^{b_{k}}-\mathbf{b}_{k}^{a}\right) \delta t}_{\text {Part } 1} \\
&+\underbrace{\frac{1}{2} \mathbf{R}_{b_{i} b_{k}} \exp \left(\left[\delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}\right]_{\times}\right) \exp \left([\boldsymbol{\omega} \delta t]_{\times}\right)\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right) \delta t}_{\text {Part } 2}
\end{aligned}
\]
\(P art 1\) 对应的的雅克比为:
\[\begin{aligned}
\frac{\partial \mathbf{R}_{b_{i} b_{k}} \exp \left(\left[\delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}\right]_{\times}\right)\left(\mathbf{a}^{b_{k}}-\mathbf{b}_{k}^{a}\right) \delta t}{\partial \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}} &=\frac{\partial \mathbf{R}_{b_{i} b_{k}}\left(\mathbf{I}+\left[\delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}\right]_{\times}\right)\left(\mathbf{a}^{b_{k}}-\mathbf{b}_{k}^{a}\right) \delta t}{\partial \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}} \\
&=\frac{\partial-\mathbf{R}_{b_{i} b_{k}}\left[\left(\mathbf{a}^{b_{k}}-\mathbf{b}_{k}^{a}\right) \delta t\right]_{\times} \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}}{\partial \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}} \\
&=-\mathbf{R}_{b_{i} b_{k}}\left[\left(\mathbf{a}^{b_{k}}-\mathbf{b}_{k}^{a}\right) \delta t\right]_{\times}
\end{aligned}
\]
\(Part2\) 对应的雅可比为：
\[\begin{aligned}
&\frac{\partial \mathbf{R}_{b_{i} b_{k}} \exp \left(\left[\delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}\right]_{\times}\right) \exp \left([\boldsymbol{\omega} \delta t]_{\times}\right)\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right) \delta t}{\partial \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}} \\
&=\frac{\left.\partial \mathbf{R}_{b_{i} b_{k}}\left(\mathbf{I}+\left[\delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}\right]_{\times}\right) \exp \left([\boldsymbol{\omega} \delta t]_{\times}\right)\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right) \delta t}{\partial \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}} \\
&=-\frac{\partial \mathbf{R}_{b_{i} b_{k}}\left[\exp \left([\boldsymbol{\omega} \delta t]_{\times}\right)\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right) \delta t\right]_{\times} \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}}{\partial \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}} \\
&=-\frac{\partial \mathbf{R}_{b_{i} b_{k}} \exp \left([\boldsymbol{\omega} \delta t]_{\times}\right)\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right) \delta t\right]_{\times} \exp \left([-\boldsymbol{\omega} \delta t]_{\times}\right) \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}}{\partial \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}} \\
&=-\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right) \delta t\right]_{\times} \exp \left([-\boldsymbol{\omega} \delta t]_{\times}\right) \\
&\approx-\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right) \delta t\right]_{\times}\left(\mathbf{I}-[\boldsymbol{\omega} \delta t]_{\times}\right)
\end{aligned}
\]
将上面两部分综合起来就能得到
\[\mathbf{f}_{32}=\frac{\partial \boldsymbol{\beta}_{b_{i} b_{k+1}}}{\partial \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}}=-\frac{1}{2}\left(\mathbf{R}_{b_{i} b_{k}}\left[\mathbf{a}^{b_{k}}-\mathbf{b}_{k}^{a}\right]_{\times} \delta t+\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right]_{\times}\left(\mathbf{I}-[\boldsymbol{\omega}]_{\times} \delta t\right) \delta t\right)\\
\]
$ \mathrm{f}_{35} $ : 速度预积分量对 \(k\) 时刻角速度 \(bias\) 的 \(Jacobian\)
递推公式如下：
\[\begin{aligned}
\boldsymbol{\omega} &=\frac{1}{2}\left(\left(\boldsymbol{\omega}^{b_{k}}-\mathbf{b}_{k}^{g}\right)+\left(\boldsymbol{\omega}^{b_{k+1}}-\mathbf{b}_{k}^{g}\right)\right)=\frac{1}{2}\left(\boldsymbol{\omega}^{b_{k}}+\boldsymbol{\omega}^{b_{k+1}}\right)-\mathbf{b}_{k}^{g} \\
\boldsymbol{\beta}_{b_{i} b_{k+1}} &=\boldsymbol{\beta}_{b_{i} b_{k}}+\frac{1}{2}(\mathbf{q}_{b_{i} b_{k}}\left(\mathbf{a}^{b_{k}}-\mathbf{b}_{k}^{a}\right)+\underbrace{\left.\mathbf{q}_{b_{i} b_{k}} \otimes\left[\begin{array}{c}
1 \\
\frac{1}{2} \boldsymbol{\omega} \delta t
\end{array}\right]\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right) \delta t}
\end{aligned}
\]
只有括号中的公式部分和角速度 bias 有关系, 因此雅克比的推导只考括号中的公式部分。
\[\begin{aligned}
\mathbf{f}_{35}=\frac{\partial \boldsymbol{\beta}_{b_{i} b_{k+1}}}{\partial \delta \mathbf{b}_{k}^{g}} &=\frac{\partial \frac{1}{2} \mathbf{q}_{b_{i} b_{k}} \otimes\left[\begin{array}{c}
1 \\
\frac{1}{2} \boldsymbol{\omega} \delta t
\end{array}\right] \otimes\left[\begin{array}{c}
1 \\
-\frac{1}{2} \delta \mathbf{b}_{k}^{g} \delta t
\end{array}\right]\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right) \delta t}{\partial \delta \mathbf{b}_{k}^{g}} \\
&=\frac{1}{2} \frac{\partial \mathbf{R}_{b_{i} b_{k+1}} \exp \left(\left[-\delta \mathbf{b}_{k}^{g} \delta t\right]_{\times}\right)\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right) \delta t}{\partial \delta \mathbf{b}_{k}^{g}} \\
&=\frac{1}{2} \frac{\partial \mathbf{R}_{b_{i} b_{k+1}}\left(\mathbf{I}+\left[-\delta \mathbf{b}_{k}^{g} \delta t\right]_{\times}\right)\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right) \delta t}{\partial \delta \mathbf{b}_{k}^{g}} \\
&=\frac{1}{2} \frac{\partial-\mathbf{R}_{b_{i} b_{k+1}}\left(\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right) \delta t\right]_{\times}\right)\left(-\delta \mathbf{b}_{k}^{g} \delta t\right)}{\partial \delta \mathbf{b}_{k}^{g}} \\
&=-\frac{1}{2}\left(\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right]_{\times} \delta t\right)(-\delta t)
\end{aligned}
\]
旋转预积分量的 \(Jacobian\),即 \(F\) 第二行，旋转预积分的递推公式为：
\[\begin{array}{l}
\boldsymbol{\omega}=\frac{1}{2}\left(\left(\boldsymbol{\omega}^{b_{k}}-\mathbf{b}_{k}^{g}\right)+\left(\boldsymbol{\omega}^{b_{k+1}}-\mathbf{b}_{k}^{g}\right)\right)\\
\mathbf{q}_{b_{i} b_{k+1}}=\mathbf{q}_{b_{i} b_{k}} \otimes\left[\begin{array}{c}
1 \\
\frac{1}{2} \boldsymbol{\omega} \delta t
\end{array}\right]\\
=\mathbf{q}_{b_{i} b_{k}} \otimes\left[\begin{array}{c}
1 \\
\frac{1}{2}\left(\frac{1}{2}\left(\boldsymbol{\omega}^{b_{k}}+\boldsymbol{\omega}^{b_{k+1}}\right)-\mathbf{b}_{k}^{g}\right) \delta t
\end{array}\right]
\end{array}
\]
\(\mathrm{f}_{22}\) : 前一时刻的旋转误差 \(\delta \theta_{b_{k} b_{k}^{\prime}}\) 如何影响当前旋转误差 \(\delta \theta_{b_{k+1} b_{k+1}^{\prime}}\)。假设两个时刻的真值为 \(\mathbf{q}_{b_{i} b_{k+1}} \mathbf{q}_{b_{i} b_{k}}\) , 两个时刻间的增量真值为 \(\mathbf{q}_{b_{k} b_{k+1}}\) 。推导过程只考虑一个变量, 即旋转误差\(\delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}\)的影响, 而不 考虑测量值角速度 \(bias\) 误差影响。可以假设
\[\mathbf{q}_{b_{k} b_{k+1}} \approx\left[\begin{array}{c}1 \\ \frac{1}{2} \boldsymbol{\omega} \delta t\end{array}\right]
\]
另外, 三元组四元数相乘有如下性质：
\[\mathbf{q} \otimes \mathbf{p} \otimes \mathbf{q}^{*}=[\mathbf{q}]_{L}[\mathbf{q}]_{R}^{\top} \mathbf{p}=\left[\begin{array}{c}
p_{w} \\
\mathbf{R} \mathbf{p}_{v}
\end{array}\right]
\]
其中 \(\mathbf{R}\) 是和 \(\mathbf{q}\) 对应的旋转矩阵, $p_{w} $ 为 \(\mathbf{p}\) 的实部， \(\mathbf{p}_{v}\) 为 \(\mathbf{p}\) 的虚部。
下面开始详细推导:
\[\begin{aligned}
\mathbf{q}_{b_{i} b_{k+1}} \otimes\left[\begin{array}{c}
1 \\
\frac{1}{2} \delta \boldsymbol{\theta}_{b_{k+1} b_{k+1}^{\prime}}
\end{array}\right] &=\mathbf{q}_{b_{i} b_{k}} \otimes\left[\begin{array}{c}
1 \\
\frac{1}{2} \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}
\end{array}\right] \otimes\left[\begin{array}{c}
1 \\
\frac{1}{2} \boldsymbol{\omega} \delta t
\end{array}\right] \\
{\left[\begin{array}{c}
1 \\
2
\end{array}\right] } &\left.=\mathbf{q}_{b_{k+1} b_{k+1}^{\prime}}^{*}\right] \otimes \mathbf{q}_{b_{i} b_{k}} \otimes\left[\begin{array}{c}
1 \\
\frac{1}{2} \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}
\end{array}\right] \otimes\left[\begin{array}{c}
1 \\
\frac{1}{2} \boldsymbol{\omega} \delta t
\end{array}\right] \\
&=\mathbf{q}_{b_{k+1} b_{k}} \otimes\left[\begin{array}{c}
1 \\
\frac{1}{2} \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}
\end{array}\right] \otimes\left[\begin{array}{c}
1 \\
\frac{1}{2} \boldsymbol{\omega} \delta t
\end{array}\right] \\
& \approx\left[\begin{array}{c}
1 \\
-\frac{1}{2} \boldsymbol{\omega} \delta t
\end{array}\right] \otimes\left[\begin{array}{c}
1 \\
\frac{1}{2} \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}
\end{array}\right] \otimes\left[\begin{array}{c}
1 \\
\frac{1}{2} \boldsymbol{\omega} \delta t
\end{array}\right] \\
&=\left[\begin{array}{c}
1 \\
\frac{1}{2} \mathbf{R} \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}
\end{array}\right]
\end{aligned}
\]
只考虑公式中的虚部:
\[\begin{aligned}
\delta \boldsymbol{\theta}_{b_{k+1} b_{k+1}^{\prime}} &=\boldsymbol{R} \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}} \\
&=\exp \left([-\boldsymbol{\omega} \delta t]_{\times}\right) \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}} \\
& \approx\left(\mathbf{I}-[\boldsymbol{\omega} \delta t]_{\times}\right) \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}
\end{aligned}
\]
那么, 第 \(k+1\) 时刻的旋转预积分的误差相对于第 \(k\) 时刻的 $ Jacobian$ 为:
\[\mathbf{f}_{22}=\frac{\partial \delta \boldsymbol{\theta}_{b_{k+1} b_{k+1}^{\prime}}}{\partial \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}}=\mathbf{I}-[\boldsymbol{\omega} \delta t]_{\times}
\]
渔已授完,\(F\), \(G\) 中的其他鱼靠大家去捞了...
- 四元数求导的直观理解（不够严谨） https://zhuanlan.zhihu.com/p/395511536
