問題來源是 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十四講.
