问题来源是 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}_{21}\)，在经过时间 \(\Delta t\) 之后变化成 \(\mathbf{C}_{21}^{\prime}\)。

则有

\[\begin{align} \mathbf{C}_{21}^T &= \underrightarrow{\mathcal{F}_1}\underrightarrow{\mathcal{F}^T_2} \\ {\mathbf{C}_{21}^{\prime}}^T &= \underrightarrow{\mathcal{F}_1}\underrightarrow{\mathcal{F}^T_2}^{\prime} \end{align}\]

对于 \({\mathbf{C}_{21}^{\prime}}^T\) 的计算比较困难。假设在这个空间中有一组单位正交基，为了简单起见就将这组基设定在 \(\underrightarrow{\mathcal{F}_1}\) 的各条轴上，长度都为 1 。将以上这些向量都用它们在这组单位正交基下的坐标表示，考虑各个向量的坐标维度：

1. \(\underrightarrow{\mathcal{F}_1}, \underrightarrow{\mathcal{F}_2}\)，\(9 \times 1\)；
2. \(\underrightarrow{\mathcal{F}^T_1}, \underrightarrow{\mathcal{F}^T_2}\)，\(3 \times 3\)，注意此处是与转置符号系统的习惯冲突的；
3. \(\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}\) 。

使用性质 \((\mathbf{C}\mathbf{v})^{\wedge} = \mathbf{C} \mathbf{v}^{\wedge} \mathbf{C}^T\)，可得进行以下推导：

\[\begin{align} {\mathbf{C}_{21}^{\prime}}^T &= \underrightarrow{\mathcal{F}_1}((\underrightarrow{\omega}_{21} \Delta t) \times \underrightarrow{\mathcal{F}^T_2} + \underrightarrow{\mathcal{F}^T_2}) \notag \\ &= \underrightarrow{\mathcal{F}_1}(\underrightarrow{\omega}_{21} \Delta t) \times \underrightarrow{\mathcal{F}^T_2} + \underrightarrow{\mathcal{F}_1}\underrightarrow{\mathcal{F}^T_2} \notag \\ &= \underrightarrow{\mathcal{F}_1}(\underrightarrow{\mathcal{F}^T_2} \mathbf{\omega}^{21}_2 \Delta t)^{\wedge} \underrightarrow{\mathcal{F}^T_2} + \mathbf{C}_{21}^T \notag \\ &= \underrightarrow{\mathcal{F}_1} \underrightarrow{\mathcal{F}^T_2} ( \mathbf{\omega}^{21}_2 \Delta t)^{\wedge} \underrightarrow{\mathcal{F}^T_2}^T \underrightarrow{\mathcal{F}^T_2} + \mathbf{C}_{21}^T \notag \\ &= \mathbf{C}_{21}^T (\mathbf{\omega}^{21}_2 \Delta t)^{\wedge} \mathbf{I} + \mathbf{C}_{21}^T \notag \\ &= \mathbf{C}_{21}^T (\mathbf{I} + (\mathbf{\omega}^{21}_2 \Delta t)^{\wedge}) \notag \\ &= \mathbf{C}_{21}^T \text{Exp}(\mathbf{\omega}^{21}_2 \Delta t) \end{align}\]

变换一下坐标系的表示方式，并且将 \({\mathbf{C}_{21}^{\prime}}\) 写作 \(\mathbf{C}_{21}(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} \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}\]

参考文献

[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).