1. 概述
- 本小节内容为以下文献的阅读记录
- Sola J. Quaternion kinematics for the error-state Kalman filter[J]. arXiv preprint arXiv:1711.02508, 2017.
2. 四元数基础
2.1 定义
- 四元数的一般形式：$Q = a + bi + cj + dk \in H$, $i^2 = j^2 = l^2 = ijk = -1$, $ij = -ji = k$, $jk = -kj = i$, $ki = -ik = j$
- 其他表达方式：$Q = q_w + \bf{q}_v$, $\bf{q}_v = q_xi + q_yj + q_zk = (q_x, q_y, q_z)$，或$Q =
2.2 性质
- 和
- 乘积
- 单位四元数
- 共轭四元数
- 范数
- 逆
- Quaternion commutator
3. 旋转
3.1 三维旋转方程
三维旋转方程的推导可结合如下图例：
我们很容易得到如下等式：$\bf{x} = \bf{x_{||}} + \bf{x}_{\perp}$，$\bf{x}_{\parallel} = \bf{u}(||x||\cos\alpha) = \bf{uu^Tx}$, $\bf{x}_{\perp} = \bf{x - x}_{\parallel} = \bf{x - uu^Tx}$。
因此，$\bf{x}’_{\parallel} = \bf{x}_{\parallel}$.
另$\bf{e}_1 = \bf{x}_{\perp}$, $\bf{e}_2 = \bf{u \times x}_{\perp} = \bf{u \times x}$，
我们得到：$\bf{x}’_{\perp} = \bf{e}_1\cos\phi + \bf{e}_2\sin\phi = \bf{x}_{\perp}\cos\phi + (\bf{u \times x})\sin\phi$，
因此，我们得到如下旋转公式：
$\bf{x}’ = \bf{x}_{\parallel} + \bf{x}_{\perp}\cos\phi + (\bf{u \times x})\sin\phi$。
3.2 旋转群
$SO(3)$定义了旋转矩阵的集合，具体定义如下：
$SO(3): \{e: R^3\to R^3 /\forall \bf{v, w}\in R^3, ||r(\bf{v})|| = ||\bf{v}||, r(\bf{v}) \times r(\bf{w}) = r(\bf{v \times w})\}$。
一个很好地表达是使用四元数来表示，唯一的区别是四元数双覆盖旋转矩阵。
旋转矩阵和四元数的$SO(3)$如下表格所示：
3.3 旋转矩阵的指数映射
指数映射和对数映射是分析三维空间旋转的强有力的工具。指数映射方便我们定义旋转的导数、扰动和速度。
由$R^TR = I$，可得，
此处，可以看出$R^T\dot{R}$为斜对称矩阵。斜对称矩阵与向量之间的关系为：
因此，我们得到：$R^T\dot{R} = [w]_{\times}$，进一步地得到$\dot{R} = R[w]_{\times}$，即旋转矩阵的微分方程。
如果假设w是常向量，因此我们利用积分得到：
另$\bf{v} \triangleq \bf{w}\Delta t$，得到
$R = e^{[\bf{v}]_{\times}}$。
上述公式即为指数映射，即：
3.4 旋转矩阵和旋转向量
旋转矩阵和旋转向量之间的关系可通过罗德里格斯公式进行转换。罗德里格斯公式本文将不再推导，感兴趣的同学可以参考诸多文献。
本文给出转换公式如下：
其中，$R$为旋转矩阵，$\bf{v}$为旋转向量。
3.5 旋转矩阵的对数映射
对数映射是指数映射的逆，即：
其中，
3.6 四元数的指数映射
类似于旋转矩阵的求微分过程，我们对四元数进行同样的动作，得到：
我们得到$q^*\otimes\dot{q}$是纯四元数。另$\Omega$为纯四元数，我们得到：
假设$\Omega$为常向量，我们积分得到，
另$\bf{V} = \Omega\Delta t$，我们得到四元数的指数映射为：
当我们引入角速度$w$时，由$w = 2\Omega$，可得：
3.7 四元数的对数映射
对数映射是指数映射的逆，即：
4. 扰动、微分和积分
4.1 加和减操作
向量的加和减操作比较直观，但是作用在$SO(3)$上则不同。分别介绍如下。
- 加
- 产出$SO(3)$
- 减
- 产出向量
4.2 微分定义
- 函数微分定义
欧拉积分得到：
- $SO(3)$到$SO(3)$的微分定义
欧拉积分得到：
- 向量到$SO(3)$
欧拉积分：
- $SO(3)$到向量空间
欧拉积分：
4.3 旋转的雅克比
- 向量的雅克比
- 向量到四元数的雅克比
因此，我们得到$\bf{a}’$对w和$\bf{v}$的偏导为：
因此，我们得到：
4.3.1 $SO(3)$的右雅克比
属性：
4.3.2 向量的雅克比
