问题来源是 IMU 中 Gyroscope 测量的角速度实际含义，从而帮助理解 IMU 预积分过程中 Rotation Matrix 的积分过程（即文献 [1] 中公式 (30) 的第一个等式）。
解决这个问题，参考文献 [2] State Estimation for Robotics 的 6.2.4 Rotational Kinematics 与 6.4.4 Inertial Measurement Unit 。
1. 角速度测量值含义
在对文献 [2] 6.2.4 有基本印象之后，阅读文献[2] 6.4.4。
在文献 [2] 6.4.4 中，图 Figure 6.14 有三个坐标系——惯性（世界）坐标系 \(\underrightarrow{\mathcal{F}_i}\)、载具坐标系 \(\underrightarrow{\mathcal{F}_v}\)、IMU 坐标系 \(\underrightarrow{\mathcal{F}_s}\)。
IMU 坐标系与载具坐标系不重合，所以 IMU 的测量值不能直接在载具坐标系在使用，需要进行转换。
文献[2]公式 (6.149) 给出了 Gyroscope 测量值 \(\mathbf{\omega}\) 与所需要的载具角速度 \(\mathbf{\omega}^{vi}_v\) 的关系：
\[\mathbf{\omega} = \mathbf{C}_{sv}\mathbf{\omega}^{vi}_v，
\]
由文献 [2] 公式 (6.3)
\[\underrightarrow{\mathcal{F}_1}^T = \underrightarrow{\mathcal{F}_2}^T \mathbf{C}_{21}
\]
得
\[\mathbf{\omega} = \underrightarrow{\mathcal{F}_s}\underrightarrow{\mathcal{F}_v}^T\mathbf{\omega}^{vi}_v，
\]
由文献 [2] 公式 (6.40)
\[\underrightarrow{\omega}_{21} = \underrightarrow{\mathcal{F}_2}^T \mathbf{\omega}^{21}_2
\]
得
\[\begin{align}
\mathbf{\omega} &= \underrightarrow{\mathcal{F}_s}\underrightarrow{\mathbf{\omega}}_{vi} \notag \\
\underrightarrow{\mathcal{F}_s}^T \mathbf{\omega} &= \underrightarrow{\mathbf{\omega}}_{vi}
\end{align}，\]
因为 IMU 与载具之间形成刚体，不存在相对运动，所以从角速度的空间向量（注意，并不是向量在某基底下的向量坐标）角度看，IMU 与载具的角速度相同。即
\[\underrightarrow{\mathbf{\omega}}_{si} = \underrightarrow{\mathbf{\omega}}_{vi}。
\]
所以，认为角速度测量值
\[\mathbf{\omega} = \mathbf{\omega}^{si}_s。
\]
注意 \(\underrightarrow{\omega}_{21}\) 的含义，在 6.2.4 有一句话引用如下
The angular velocity of frame 2 with respect to frame 1 is denoted by \(\underrightarrow{\omega}_{21}\).
所以 \(\underrightarrow{\mathbf{\omega}}_{si}\) 理解为 IMU 坐标系 \(\underrightarrow{\mathcal{F}_s}\) 相对惯性坐标系 \(\underrightarrow{\mathcal{F}_i}\) 的角速度向量，\(\mathbf{\omega}^{si}_s\) 符号理解为IMU 坐标系 \(\underrightarrow{\mathcal{F}_s}\) 相对惯性坐标系 \(\underrightarrow{\mathcal{F}_i}\) 的角速度向量在 IMU 坐标系 \(\underrightarrow{\mathcal{F}_s}\) 下的坐标。
2. 从角速度积分
参考 6.2.4 仔细理解公式 (6.36)。这是从向量的角度理解角速度，而不是从向量坐标角度理解。
将此处的 \(\underrightarrow{\mathcal{F}_1}\) 对应惯性坐标系 \(\underrightarrow{\mathcal{F}_i}\)，\(\underrightarrow{\mathcal{F}_2}\) 对应 IMU 坐标系 \(\underrightarrow{\mathcal{F}_s}\)。
角速度在空间中可以表示为一个向量，按照右手螺旋的习惯，此向量方向标明了旋转方向，此向量的长度标明了角速度标量。
在某一时刻 \(\underrightarrow{\mathcal{F}_1}, \underrightarrow{\mathcal{F}_2}\) 之间存在角速度 \(\underrightarrow{\omega}_{21}\)，考虑 \(\underrightarrow{\mathcal{F}_2}\) 各轴（也就是该坐标系基底所在的向量） \(\underrightarrow{2_1}, \underrightarrow{2_2}, \underrightarrow{2_3}\) 对时间的变化率（当然是在 \(\underrightarrow{\mathcal{F}_1}\) 下观察的结果，在 \(\underrightarrow{\mathcal{F}_2}\) 下结果为 0，所谓相对运动）。得到以下结果（参考图理解）：
\[\begin{align}
\underrightarrow{2^{\bullet}_1} = \underrightarrow{\omega}_{21} \times \underrightarrow{2_1} \\
\underrightarrow{2^{\bullet}_2} = \underrightarrow{\omega}_{21} \times \underrightarrow{2_2} \\
\underrightarrow{2^{\bullet}_3} = \underrightarrow{\omega}_{21} \times \underrightarrow{2_3}
\end{align}\]
现在考虑经过足够短的时间 \(\Delta t\)，坐标系 \(\underrightarrow{\mathcal{F}_2}\) 经过 \(\Delta t\) 时间移动到 \(\underrightarrow{\mathcal{F}_2}^{\prime}\) 。
\[\begin{align}
\underrightarrow{2^{\prime}_1} &= \underrightarrow{2^{\bullet}_1} \Delta t + \underrightarrow{2_1} \notag \\
&= (\underrightarrow{\omega}_{21} \Delta t) \times \underrightarrow{2_1} + \underrightarrow{2_1} \\
\underrightarrow{2^{\prime}_2} &= \underrightarrow{2^{\bullet}_2} \Delta t + \underrightarrow{2_2} \notag \\
&= (\underrightarrow{\omega}_{21} \Delta t) \times \underrightarrow{2_2} + \underrightarrow{2_2} \\
\underrightarrow{2^{\prime}_3} &= \underrightarrow{2^{\bullet}_3} \Delta t + \underrightarrow{2_3} \notag \\
&= (\underrightarrow{\omega}_{21} \Delta t) \times \underrightarrow{2_3} + \underrightarrow{2_3}
\end{align}\]
合并，写作
\[\underrightarrow{\mathcal{F}^T_2}^{\prime} = (\underrightarrow{\omega}_{21} \Delta t) \times \underrightarrow{\mathcal{F}^T_2} + \underrightarrow{\mathcal{F}^T_2}
\]
其中
\[\begin{align}
\underrightarrow{\omega}_{21} &= \underrightarrow{\mathcal{F}^T_2} \mathbf{\omega}^{21}_2 \\
\underrightarrow{\omega}_{21} &= \underrightarrow{\mathcal{F}^T_1} \mathbf{\omega}^{21}_1
\end{align}\]
考虑 \(\underrightarrow{\mathcal{F}_1}, \underrightarrow{\mathcal{F}_2}\) 之间的旋转矩阵 \(\mathbf{C}_{12}\)，在经过时间 \(\Delta t\) 之后变化成 \(\mathbf{C}_{12}^{\prime}\)。
则有
\[\begin{align}
\mathbf{C}_{12} &= \underrightarrow{\mathcal{F}_1}\underrightarrow{\mathcal{F}^T_2} \\
{\mathbf{C}_{12}^{\prime}} &= \underrightarrow{\mathcal{F}_1}\underrightarrow{\mathcal{F}^T_2}^{\prime}
\end{align}\]
对于 \({\mathbf{C}_{21}^{\prime}}^T\) 的计算比较困难。假设在这个空间中有一组单位正交基，为了简单起见将这组基设定在 \(\underrightarrow{\mathcal{F}_1}\) 的各条轴上（同时也是在 \(\underrightarrow{\mathcal{F}_1}\) 上求对时间的变化率），长度都为 1 。将以上这些向量都用它们在这组单位正交基下的坐标表示，考虑各个向量的坐标维度：
- \(\underrightarrow{\mathcal{F}_1}, \underrightarrow{\mathcal{F}_2}\)，\(3 \times 3\)，\(\underrightarrow{\mathcal{F}_1}=\begin{bmatrix} \underrightarrow{1_1} \\ \underrightarrow{1_2} \\ \underrightarrow{1_3} \end{bmatrix}, \underrightarrow{\mathcal{F}_2}=\begin{bmatrix} \underrightarrow{2_1} \\ \underrightarrow{2_2} \\ \underrightarrow{2_3} \end{bmatrix}\)，矩阵的每一行都是对应坐标系基底在该坐标系下的坐标；
- \(\underrightarrow{\mathcal{F}^T_1}, \underrightarrow{\mathcal{F}^T_2}\)，\(3 \times 3\)，\(\underrightarrow{\mathcal{F}^T_1}=\begin{bmatrix} \underrightarrow{1_1} & \underrightarrow{1_2} & \underrightarrow{1_3} \end{bmatrix}, \underrightarrow{\mathcal{F}^T_2}=\begin{bmatrix} \underrightarrow{2_1} & \underrightarrow{2_2} & \underrightarrow{2_3} \end{bmatrix}\)，矩阵的每一列都是对应坐标系基底在该坐标系下的坐标；
- \(\underrightarrow{\omega}_{21}\)，\(3 \times 1\)，并且 \({\underrightarrow{\omega}_{21}}_{3 \times 1} = \underrightarrow{\mathcal{F}^T_2}_{3 \times 3} {\mathbf{\omega}^{21}_2}_{3 \times 1}\) 。
\[\begin{align}
\mathbf{C}_{12} &= \underrightarrow{\mathcal{F}_1}\underrightarrow{\mathcal{F}^T_2} \notag \\
&= \mathbf{I} \underrightarrow{\mathcal{F}^T_2} \notag \\
&= \underrightarrow{\mathcal{F}^T_2}
\end{align}\]
于是 \(\mathbf{C}_{12}\) 的每一列都是 \(\underrightarrow{\mathcal{F}_2}\) 对应坐标系基底在 \(\underrightarrow{\mathcal{F}_1}\) 坐标系下的坐标。
\[\begin{align}
\mathbf{C}_{12}^{\prime} &= \underrightarrow{\mathcal{F}_1}\underrightarrow{\mathcal{F}^T_2}^{\prime} \notag \\
&= \underrightarrow{\mathcal{F}_1}((\underrightarrow{\omega}_{21} \Delta t) \times \underrightarrow{\mathcal{F}^T_2} + \underrightarrow{\mathcal{F}^T_2}) \notag \\
&= \mathbf{I}((\mathbf{\omega}^{21}_1\Delta t)^{\wedge} \mathbf{C}_{12} + \mathbf{C}_{12}) \notag \\
&= \mathbf{C}_{12} + (\mathbf{\omega}^{21}_1 \Delta t)^{\wedge} \mathbf{C}_{12} \notag \\
&= \mathbf{C}_{12} + \mathbf{C}_{12} \mathbf{C}_{12}^T (\mathbf{\omega}^{21}_1\Delta t)^{\wedge} \mathbf{C}_{12} \notag \\
&= \mathbf{C}_{12} + \mathbf{C}_{12} (\mathbf{C}_{12}^T \mathbf{\omega}^{21}_1 \Delta t)^{\wedge} \notag \\
&= \mathbf{C}_{12} + \mathbf{C}_{12} (\mathbf{\omega}^{21}_2 \Delta t)^{\wedge} \notag \\
&= \mathbf{C}_{12} (\mathbf{I} + (\mathbf{\omega}^{21}_2 \Delta t)^{\wedge}) \notag \\
&= \mathbf{C}_{12} \text{Exp}(\mathbf{\omega}^{21}_2 \Delta t)
\end{align}\]
以上第三个等号，可以看做是将所有向量都投影到 \(\underrightarrow{\mathcal{F}_1}\) 坐标系下进行后续的运算。因为旋转矩阵不是坐标系中的坐标，所以上式的第一个等号没有错。
变换一下坐标系的表示方式，并且将 \({\mathbf{C}_{12}^{\prime}}\) 写作 \(\mathbf{C}_{12}(t+\Delta t)\)。\(\mathbf{\omega}^{si}_s\) 对应 \(\mathbf{\omega}^{21}_2\)，所以上式可以写作：
\[\begin{align}
{\mathbf{C}_{is}(t+\Delta t)} &= \mathbf{C}_{is}(t) \text{Exp}(\mathbf{\omega}^{si}_s \Delta t)\label{eq1:R}
\end{align}\]
至此，得到 IMU 姿态积分公式。
3. 微分方程
3.1. 旋转矩阵的微分
旋转矩阵微分 \(\dot{\mathbf{C}_{is}}(t)\) 的定义是：
\[\begin{align}
{\mathbf{C}_{is}(t+\Delta t)} &= \mathbf{C}_{is}(t) + \dot{\mathbf{C}_{is}}(t) \Delta t
\end{align}\]
所以现在从公式 (\(\ref{eq1:R}\)) 出发，凑出以上公式，所以需要分解 \(\text{Exp}(\mathbf{\omega}^{si}_s \Delta t)\)，要求 \(\mathbf{\omega}^{si}_s \Delta t\) 很小，可以做以下分解：
\[\begin{align}
{\mathbf{C}_{is}(t+\Delta t)} &= \mathbf{C}_{is}(t) \text{Exp}(\mathbf{\omega}^{si}_s \Delta t) \notag \\
&= \mathbf{C}_{is}(t) (\mathbf{I} + {\mathbf{\omega}^{si}_s}^{\wedge} \Delta t) \notag \\
&= \mathbf{C}_{is}(t) + \mathbf{C}_{is}(t) {\mathbf{\omega}^{si}_s}^{\wedge} \Delta t
\end{align}\]
得到微分 \(\dot{\mathbf{C}_{is}}(t)\)：
\[\begin{align}
\dot{\mathbf{C}_{is}}(t) = \mathbf{C}_{is}(t) {\mathbf{\omega}^{si}_s}^{\wedge} \label{eq3.1.3}
\end{align}\]
3.2. 四元数的微分
以下使用 Hamilton 形式四元数。
从语义上理解，公式 (\(\ref{eq1:R}\)) 表示 Rotation Matrix \(\mathbf{C}_{is}\) 在时刻 \(t\) 的值为 \(\mathbf{C}_{is}(t)\)，在此基础上经过 \(\Delta t\) 的时间，在时刻 \(t + \Delta t\) 的值为 \(\mathbf{C}_{is}(t) \text{Exp}(\mathbf{\omega}^{si}_s(t) \Delta t)\)，即在 \(\mathbf{C}_{is}(t)\) 的基础上被右乘了一个矩阵 \(\text{Exp}(\mathbf{\omega}^{si}_s \Delta t)\)，这个矩阵表示的旋转轴角为 \(\mathbf{\omega}^{si}_s(t) \Delta t\)。（注意，不是在 \(\mathbf{C}_{is}(t)\) 的基础上继续旋转 \(\mathbf{\omega}^{si}_s(t) \Delta t\)，因为此处 \(\mathbf{C}_{is}(t)\) 是被右乘。）按照这个旋转的含义，将公式中的 Rotation Matrix 换成 quaternion 。
\[\begin{align}
\mathbf{q}_{is}(t + \Delta t) &= \mathbf{q}_{is}(t) \otimes q\{ \mathbf{\omega}^{si}_s(t) \Delta t \} \notag \\
&= \mathbf{q}_{is}(t) \otimes \begin{bmatrix}
1 \\ {1 \over 2}\mathbf{\omega}^{si}_s(t) \Delta t
\end{bmatrix} \notag \\
&= \left(\mathbf{I} + \begin{bmatrix}
0 & - {1 \over 2}{\mathbf{\omega}^{si}_s(t)}^T \Delta t \\
{1 \over 2}\mathbf{\omega}^{si}_s(t) \Delta t & -{1 \over 2}{\mathbf{\omega}^{si}_s(t)}^{\wedge} \Delta t
\end{bmatrix}\right) \mathbf{q}_{is}(t) \phantom{\ } \phantom{\ } \phantom{\ } (论文 [3] 公式 (19)) \notag \\
&= \mathbf{q}_{is}(t) + {1 \over 2}\begin{bmatrix}
0 & - {\mathbf{\omega}^{si}_s(t)}^T \\
\mathbf{\omega}^{si}_s(t) & -{\mathbf{\omega}^{si}_s(t)}^{\wedge}
\end{bmatrix} \mathbf{q}_{is}(t) \Delta t
\end{align}\]
由定义可以得到 quaternion 的微分方程。
\[\begin{align}
\dot{\mathbf{q}_{is}}(t) &= {1 \over 2}\begin{bmatrix}
0 & - {\mathbf{\omega}^{si}_s(t)}^T \\
\mathbf{\omega}^{si}_s(t) & -{\mathbf{\omega}^{si}_s(t)}^{\wedge}
\end{bmatrix} \mathbf{q}_{is}(t) \notag \\
&= {1 \over 2} \mathbf{q}_{is}(t) \otimes \begin{bmatrix}
0 \\ \mathbf{\omega}^{si}_s(t)
\end{bmatrix} \notag \\
&= {1 \over 2} \mathbf{q}_{is}(t) \otimes \mathbf{\omega}^{si}_s(t)
\end{align}\]
4. 附录：角速度的方向
上文中使用到了角速度 \(\mathbf{\omega}^{si}_s\)，理解为IMU 坐标系 \(\underrightarrow{\mathcal{F}_s}\) 相对惯性坐标系 \(\underrightarrow{\mathcal{F}_i}\) 的角速度向量在 IMU 坐标系 \(\underrightarrow{\mathcal{F}_s}\) 下的坐标。
\(\mathbf{\omega}^{is}_s\) 与 \(\mathbf{\omega}^{si}_s\) 都是在 \(\underrightarrow{\mathcal{F}_s}\) 下的坐标，只不过相对关系相反。相对关系相反，表示它们所表示的这个向量在空间中反向，而在量值上他们相等，所以这两个向量是反向量关系。反向量在同一个坐标系下的坐标是相反数，\(\mathbf{\omega}^{is}_s = -\mathbf{\omega}^{si}_s\) 。
角速度的推导参考 [4] 4.1.2 李代数的引出。\(\ref{eq3.1.3}\) 省略时间 \(t\)，变形得到以下。
\[\begin{align}
\mathbf{C}_{is}^T\dot{\mathbf{C}_{is}} = {\mathbf{\omega}^{si}_s}^{\wedge}
\end{align}\]
然而上式并不能与 [4] 的公式对应。但是它能与 [3] 的公式 (67) 对应，所以通过上式我们能够得到 [3] 中旋转向量的方向。
正常情况下一般如同 [4] 一样，从 \(\mathbf{C}\mathbf{C}^T=\mathbf{I}\) 两边对时间取微分做；而不是同 [3] 一样，从 \(\mathbf{C}^T\mathbf{C}=\mathbf{I}\) 做。
参考文献
[1] Forster, Christian, Luca Carlone, Frank Dellaert, and Davide Scaramuzza. "On-Manifold Preintegration for Real-Time Visual--Inertial Odometry." IEEE Transactions on Robotics 33, no. 1 (2016): 1-21.
[2] Barfoot, Timothy D. State Estimation for Robotics. Cambridge University Press, 2017.
[3] Sola, Joan. "Quaternion kinematics for the error-state KF." Laboratoire dAnalyse et dArchitecture des Systemes-Centre national de la recherche scientifique (LAAS-CNRS), Toulouse, France, Tech. Rep (2012).
[4] 视觉SLAM十四讲.
