学习MSCKF笔记——四元数基础

学习MSCKF笔记——四元数基础

其实老早就想对四元数相关知识进行一个总结了，这次学习MSCKF时，阅读了参考文献《Quaternion Kinematics for the Error-state Kalman Filter》,这篇文献对四元数相关知识进行了一个非常详细的说明，这篇博客是对这篇参考文献中四元数相关知识的笔记，如果想了解得比较深入，建议还是直接看文献。

1. 四元数基本性质

这一部分是描述了四元数的一些基本性质，是后面推到旋转微分积分的基础。

1.1 加法

加法就是实部虚部分别相加：
$$\mathbf{p}\pm \mathbf{q}=\left[\begin{array}{c}{p}_w\\ {\mathbf{p}}_v\end{array}\right]\pm \left[\begin{array}{c}{q}_w\\ {\mathbf{q}}_v\end{array}\right]=\left[\begin{array}{c}{p}_w\pm q_w\\ {\mathbf{p}}_v\pm {\mathbf{q}}_v\end{array}\right]$$满足交换律和结合律：$$\begin{array}{rl}\mathbf{p}+\mathbf{q}& =\mathbf{q}+\mathbf{p}\\ \mathbf{p}+(\mathbf{q}+\mathbf{r})& =(\mathbf{p}+\mathbf{q})+\mathbf{r}\end{array}$$

1.2 乘法

乘法一共有三种计算法方式：
第一种：
$$\mathbf{p}\otimes \mathbf{q}=\left[\begin{array}{c}{p}_w{q}_w-p_x{q}_x-p_y{q}_y-p_z{q}_z\\ p_w{q}_x+p_x{q}_w+p_y{q}_z-p_z{q}_y\\ p_w{q}_y-p_x{q}_z+p_y{q}_w+p_z{q}_x\\ p_w{q}_z+p_x{q}_y-p_y{q}_x+p_z{q}_w\end{array}\right]$$
第二种：$$\mathbf{p}\otimes \mathbf{q}=\left[\begin{array}{c}{p}_w{q}_w-{\mathbf{p}}_v^{\top }{\mathbf{q}}_v\\ p_w{\mathbf{q}}_v+q_w{\mathbf{p}}_v+{\mathbf{p}}_v\times {\mathbf{q}}_v\end{array}\right]$$
第三种：$${\mathbf{q}}_1\otimes {\mathbf{q}}_2={\left[{\mathbf{q}}_1\right]}_L{\mathbf{q}}_2\text{ and }{\mathbf{q}}_1\otimes {\mathbf{q}}_2={\left[{\mathbf{q}}_2\right]}_R{\mathbf{q}}_1$$其中：$$[\mathbf{q}{]}_L=\left[\begin{array}{cccc}{q}_w& -q_x& -q_y& -q_z\\ q_x& q_w& -q_z& q_y\\ q_y& q_z& q_w& -q_x\\ q_z& -q_y& q_x& q_w\end{array}\right],[\mathbf{q}{]}_R=\left[\begin{array}{cccc}{q}_w& -q_x& -q_y& -q_z\\ q_x& q_w& q_z& -q_y\\ q_y& -q_z& q_w& q_x\\ q_z& q_y& -q_x& q_w\end{array}\right]$$或者表示为：$$[\mathbf{q}{]}_L=q_w\mathbf{I}+\left[\begin{array}{cc}0& -{\mathbf{q}}_v^{\top }\\ {\mathbf{q}}_v& {\left[{\mathbf{q}}_v\right]}_{\times }\end{array}\right],[\mathbf{q}{]}_R=q_w\mathbf{I}+\left[\begin{array}{cc}0& -{\mathbf{q}}_v^{\top }\\ {\mathbf{q}}_v& -{\left[{\mathbf{q}}_v\right]}_{\times }\end{array}\right]$$满足分配率和结合率$$(\mathbf{p}\otimes \mathbf{q})\otimes \mathbf{r}=\mathbf{p}\otimes (\mathbf{q}\otimes \mathbf{r})$$$$\mathbf{p}\otimes (\mathbf{q}+\mathbf{r})=\mathbf{p}\otimes \mathbf{q}+\mathbf{p}\otimes \mathbf{r}$$

1.3 共轭

四元数的共轭定义为$${\mathbf{q}}^{\ast }\triangleq q_w-{\mathbf{q}}_v=\left[\begin{array}{c}{q}_w\\ -{\mathbf{q}}_v\end{array}\right]$$共轭的性质为$$\mathbf{q}\otimes {\mathbf{q}}^{\ast }={\mathbf{q}}^{\ast }\otimes \mathbf{q}=q_w^2+q_x^2+q_y^2+q_z^2=\left[\begin{array}{c}{q}_w^2+q_x^2+q_y^2+q_z^2\\ 0_v\end{array}\right]$$$$(\mathbf{p}\otimes \mathbf{q}{)}^{\ast }={\mathbf{q}}^{\ast }\otimes {\mathbf{p}}^{\ast }$$

1.4 模

四元数的模的定义为：$$\Vert \mathbf{q}\Vert \triangleq \sqrt{\mathbf{q}\otimes {\mathbf{q}}^{\ast }}=\sqrt{{\mathbf{q}}^{\ast }\otimes \mathbf{q}}=\sqrt{q_w^2+q_x^2+q_y^2+q_z^2}\in \mathbb{R}$$模的性质为：$$\Vert \mathbf{p}\otimes \mathbf{q}\Vert =\Vert \mathbf{q}\otimes \mathbf{p}\Vert =\Vert \mathbf{p}\Vert \Vert \mathbf{q}\Vert$$

1.5 逆

四元数的逆的定义为：$$\mathbf{q}\otimes {\mathbf{q}}^{-1}={\mathbf{q}}^{-1}\otimes \mathbf{q}={\mathbf{q}}_1$$同样，可以被计算为$${\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }/\Vert \mathbf{q}{\Vert }^2$$

1.6 单位四元数

单位四元数通常指$\Vert \mathbf{q}\Vert =1$，可以被写为：$$\mathbf{q}=\left[\begin{array}{c}\cos \theta \\ \mathbf{u}\sin \theta \end{array}\right]$$其中$\mathbf{u}=u_{x}i+u_{y}j+u_{z}k$是一个单位向量，其满足性质：$${\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }$$

1.7 指数

这里的四元数的指数通常指纯四元数（实部为0的四元数），即$\mathbf{q}=\left[0,{\mathbf{q}}_v\right]$，根据乘法的定义可以有$${\mathbf{q}}_v\otimes {\mathbf{q}}_v=-{\mathbf{q}}_v^{\top }{\mathbf{q}}_v=-{\Vert {\mathbf{q}}_v\Vert }^2$$那么对于纯单位四元数$\mathbf{u}$有$$\Vert \mathbf{u}\Vert =1$$$$\mathbf{u}\otimes \mathbf{u}=-1$$那么对于一个纯四元数我们可以定义为$\mathbf{v}=\mathbf{u}\theta$，那么我们可以得到纯四元数的幂运算为：$${\mathbf{v}}^2=-{\theta }^2,{\mathbf{v}}^3=-\mathbf{u}{\theta }^3,{\mathbf{v}}^4={\theta }^4,{\mathbf{v}}^5=\mathbf{u}{\theta }^5,{\mathbf{v}}^6=-{\theta }^6,\cdots$$那么四元数的指数可以通过泰勒展开获得如下形式$$e^{\mathbf{v}}=\sum _{k=0}^{\infty }\frac{1}{k!}{\mathbf{v}}^k$$将$\mathbf{v}=\mathbf{u}\theta$代入得$$e^{\mathbf{u}\theta }=\left(1-\frac{{\theta }^2}{2!}+\frac{{\theta }^4}{4!}+\cdots \right)+\left(\mathbf{u}\theta -\frac{\mathbf{u}{\theta }^3}{3!}+\frac{\mathbf{u}{\theta }^5}{5!}+\cdots \right)$$进而我们可以获得经典的欧拉公式$$e^{\mathbf{v}}=e^{\mathbf{u}\theta }=\cos \theta +\mathbf{u}\sin \theta =\left[\begin{array}{c}\cos \theta \\ \mathbf{u}\sin \theta \end{array}\right]$$对于一般的四元数，我们可以进行指数形式的拓展有$$e^{\mathbf{q}}=e^{q_w+{\mathbf{q}}_v}=e^{q_w}{e}^{{\mathbf{q}}_v}$$令$\mathbf{u}\theta ={\mathbf{q}}_v$有$$e^{\mathbf{q}}=e^{q_w}\left[\begin{array}{c}\cos \Vert {\mathbf{q}}_v\Vert \\ \frac{{\mathbf{q}}_v}{\Vert {\mathbf{q}}_v\Vert }\sin \Vert {\mathbf{q}}_v\Vert \end{array}\right]$$

1.8 对数

有了前面的推到，这里可以给出单位四元数$\mathbf{q}=[\cos \theta ,\mathbf{u}\sin \theta ]$的对数形式$$\log \mathbf{q}=\log (\cos \theta +\mathbf{u}\sin \theta )=\log \left(e^{\mathbf{u}\theta }\right)=\mathbf{u}\theta =\left[\begin{array}{c}0\\ \mathbf{u}\theta \end{array}\right]$$这里可以看到一个单位四元数的对数是一个纯四元数，对于一般的四元数，我们可以拓展为$$\log \mathbf{q}=\log \left(\Vert \mathbf{q}\Vert \frac{\mathbf{q}}{\Vert \mathbf{q}\Vert }\right)=\log \Vert \mathbf{q}\Vert +\log \frac{\mathbf{q}}{\Vert \mathbf{q}\Vert }=\log \Vert \mathbf{q}\Vert +\mathbf{u}\theta =\left[\begin{array}{c}\log \Vert \mathbf{q}\Vert \\ \mathbf{u}\theta \end{array}\right]$$前面我们推到了纯四元数的幂运算，这里通过结合四元数的指数和对数的性质可以进一步推导单位四元数的幂运算，有$${\mathbf{q}}^t=\exp \left(\log \left({\mathbf{q}}^t\right)\right)=\exp (t\log (\mathbf{q}))$$因为是单位四元数，所以有$\mathbf{q}=[\cos \theta ,\mathbf{u}\sin \theta ]$，那么有$${\mathbf{q}}^t=\exp (t\mathbf{u}\theta )=\left[\begin{array}{c}\cos t\theta \\ \mathbf{u}\sin t\theta \end{array}\right]$$

2. 旋转的表示形式

研究四元数的目的就是用来表示旋转，接下来就是总结下四元数如何表示旋转以及四元数与其他旋转表示形式之间的关系，包括轴角和旋转矩阵。

2.1 轴角与旋转的定义

对一个向量进行旋转的操作如下图所示：

对旋转操作表示最为直接的是轴角表示形式，如上图所示$\mathbf{u}$即为旋转轴，$\varphi$即为旋转角，对于向量$\mathbf{x}$可以分解为与旋转轴平行的部分${\mathbf{x}}_{\Vert }$以及与旋转轴垂直的部分${\mathbf{x}}_{\perp }$，如下：$$\begin{array}{l}{\mathbf{x}}_{\Vert }=\mathbf{u}(\Vert \mathbf{x}\Vert \cos \alpha )=\mathbf{u}{\mathbf{u}}^{\top }\mathbf{x}\\ {\mathbf{x}}_{\perp }=\mathbf{x}-{\mathbf{x}}_{\Vert }=\mathbf{x}-\mathbf{u}{\mathbf{u}}^{\top }\mathbf{x}\end{array}$$向量平行部分不会受到旋转操作的影响，而垂直部分经过旋转操作变为$${\mathbf{x}}_{\perp }^{\prime }={\mathbf{x}}_{\perp }\cos \varphi +(\mathbf{u}\times \mathbf{x})\sin \varphi$$因此旋转操作可以表示为$${\mathbf{x}}^{\prime }={\mathbf{x}}_{\Vert }+{\mathbf{x}}_{\perp }\cos \varphi +(\mathbf{u}\times \mathbf{x})\sin \varphi$$旋转操作同时具备一下三条性质：
第一条：旋转保留向量的长度$$\Vert r(\mathbf{v})\Vert =\sqrt{⟨r(\mathbf{v}),r(\mathbf{v})⟩}=\sqrt{⟨\mathbf{v},\mathbf{v}⟩}\triangleq \Vert \mathbf{v}\Vert ,\forall \mathbf{v}\in {\mathbb{R}}^3$$第二条：旋转保留向量间的角度：$$⟨r(\mathbf{v}),r(\mathbf{w})⟩=⟨\mathbf{v},\mathbf{w}⟩=\Vert \mathbf{v}\Vert \Vert \mathbf{w}\Vert \cos \alpha ,\forall \mathbf{v},\mathbf{w}\in {\mathbb{R}}^3$$第三条：保留了向量间的相对方向$$\mathbf{u}\times \mathbf{v}=\mathbf{w}⟺r(\mathbf{u})\times r(\mathbf{v})=r(\mathbf{w})$$因此，旋转可以定义为$SO(3)$群，即：$$SO(3):\left\{r:{\mathbb{R}}^3\to {\mathbb{R}}^3/\forall \mathbf{v},\mathbf{w}\in {\mathbb{R}}^3,\Vert r(\mathbf{v})\Vert =\Vert \mathbf{v}\Vert ,r(\mathbf{v})\times r(\mathbf{w})=r(\mathbf{v}\times \mathbf{w})\right\}$$

2.2 旋转矩阵与轴角的关系

旋转矩阵和轴角之间的关系学习过《视觉SLAM十四讲》的同学应该都比较清楚了，这里先回顾一下，旋转矩阵对向量进行旋转操作的公式为：$${\mathbf{x}}^{\prime }=\mathbf{R}\mathbf{x}$$旋转矩阵的性质有$${\mathbf{R}}^{\top }\mathbf{R}=\mathbf{I}=\mathbf{R}{\mathbf{R}}^{\top }$$根据这个性质我们对时间进行微分有$$\frac{d}{dt}\left({\mathbf{R}}^{\top }\mathbf{R}\right)={\dot{\mathbf{R}}}^{\top }\mathbf{R}+{\mathbf{R}}^{\top }\dot{\mathbf{R}}=0$$然后可得$${\mathbf{R}}^{\top }\dot{\mathbf{R}}=-{\left({\mathbf{R}}^{\top }\mathbf{R}\right)}^{\top }$$这就意味着${\mathbf{R}}^{\top }\dot{\mathbf{R}}$是一个反对称矩阵，那么我可以定义这样一个反对称矩阵为$$[\mathbf{\omega }{]}_{\times }\triangleq \left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right]$$这样我们就可以通过$\mathbf{\omega }=\left({\omega }_x,{\omega }_y,{\omega }_z\right)\in {\mathbf{R}}^3$来描述旋转矩阵及其微分间的关系，而由$\mathbf{\omega }$构成的反对称矩阵$[\mathbf{\omega }{]}_{\times }$即构成了我们熟悉的李代数空间$\mathfrak{s}\mathfrak{o}(3)$，因为我们的目的是用来描述相机的旋转变化，因此我们可以将旋转矩阵与时间$t$进行关联，然后就可以获得这样微分方程$$\dot{\mathbf{R}}(t)=\mathbf{R}(t)[\mathbf{\omega }{]}_{\times }$$我们可以发现，当$\mathbf{R}(0)=\mathbf{I}$时，$\dot{\mathbf{R}}(0)=[\mathbf{\omega }{]}_{\times }$，那么我们可以将$\mathfrak{s}\mathfrak{o}(3)$理解为$SO(3)$在原点处（$t=0$）的正切空间或者速度空间，当$\mathbf{\omega }$为常数时，微分方程对时间积分得$$\mathbf{R}(t)=\mathbf{R}(0)e^{[\mathbf{\omega }{]}_{\times }t}=\mathbf{R}(0)e^{[\mathbf{\omega }t{]}_{\times }}$$我们将$\mathbf{\varphi }\triangleq \mathbf{\omega }\Delta t$，那么有$$\mathbf{R}=e^{[\mathbf{\varphi }{]}_{\times }}$$我们定义$\mathbf{\varphi }=\mathbf{v}$那么$\mathbf{\varphi }$、$[\mathbf{\varphi }{]}_{\times }$和$\mathbf{R}$之间的关系可以通过下图描述：这里定义了旋转矩阵的指数映射$\mathrm{Exp}:{\mathbb{R}}^3\to SO(3);\mathbf{\varphi }\mapsto \mathrm{Exp}(\mathbf{\varphi })=e^{[\mathbf{\varphi }]}$，我们进一步对指数映射进行展开，定义$\mathbf{\varphi }=\varphi \mathbf{u}$，$\mathbf{u}$是单位向量，那么$[\varphi {]}_{\times }=\varphi [\mathbf{u}{]}_{\times }$，指数映射就可以泰勒展开为$$\mathbf{R}=e^{\varphi [\mathbf{u}{]}_{\times }}=\mathbf{I}+\varphi [\mathbf{u}{]}_{\times }+\frac{1}{2}{\varphi }^2[\mathbf{u}{]}_{\times }^2+\frac{1}{3!}{\varphi }^3[\mathbf{u}{]}_{\times }^3+\frac{1}{4!}{\varphi }^4[\mathbf{u}{]}_{\times }^4+\dots$$因为$\mathbf{u}$是单位向量，那么有$$\begin{array}{l}[\mathbf{u}{]}_{\times }^2={\mathbf{u}\mathbf{u}}^{\top }-\mathbf{I}\\ [\mathbf{u}{]}_{\times }^3=-[\mathbf{u}{]}_{\times }\end{array}$$以及$$[\mathbf{u}{]}_x^4=-[\mathbf{u}{]}_x^2[\mathbf{u}{]}_x^5=[\mathbf{u}{]}_{\times }[\mathbf{u}{]}_{\times }^6=[\mathbf{u}{]}_{\times }^2[\mathbf{u}{]}_{\times }^7=-[\mathbf{u}{]}_{\times }\cdots$$因此可以将指数映射的泰勒展开式划分为以$[\mathbf{u}{]}_{\times }$和以$[\mathbf{u}{]}_{\times }^2$为基的两组，前面的系数分别是$\sin \varphi$和$\cos \varphi$的展开式，因此我们就获得了著名的Rodirigues公式：$$\mathbf{R}=\mathbf{I}+\sin \varphi [\mathbf{u}{]}_{\times }+(1-\cos \varphi )[\mathbf{u}{]}_{\times }^2$$有了指数映射我们逆推出对数映射$\mathrm{Log}:SO(3)\to {\mathbb{R}}^3;\mathbf{R}\mapsto \mathrm{Log}(\mathbf{R})=\mathbf{u}\varphi$，即$$\begin{array}{l}\varphi =\arccos \left(\frac{\mathrm{trace}(\mathbf{R})-1}{2}\right)\\ \mathbf{u}=\frac{{\left(\mathbf{R}-{\mathbf{R}}^{\top }\right)}^{\vee }}{2\sin \varphi }\end{array}$$这里我们再回过头来看下旋转矩阵对向量的操作${\mathbf{x}}^{\prime }=\mathbf{R}\mathbf{x}$，我们用Rodirigues公式进行替换，有$$\begin{array}{rl}{\mathbf{x}}^{\prime }=& \mathbf{R}\mathbf{x}\\ =& \left(\mathbf{I}+\sin \varphi [\mathbf{u}{]}_{\times }+(1-\cos \varphi )[\mathbf{u}{]}_{\times }^2\right)\mathbf{x}\\ =& \mathbf{x}+\sin \varphi [\mathbf{u}{]}_{\times }\mathbf{x}+(1-\cos \varphi )[\mathbf{u}{]}_{\times }^2\mathbf{x}\\ =& \mathbf{x}+\sin \varphi (\mathbf{u}\times \mathbf{x})+(1-\cos \varphi )\left(\mathbf{u}{\mathbf{u}}^{\top }-\mathbf{I}\right)\mathbf{x}\\ =& {\mathbf{x}}_{\Vert }+{\mathbf{x}}_{\perp }+\sin \varphi (\mathbf{u}\times \mathbf{x})-(1-\cos \varphi ){\mathbf{x}}_{\perp }\\ =& {\mathbf{x}}_{\Vert }+(\mathbf{u}\times \mathbf{x})\sin \varphi +{\mathbf{x}}_{\perp }\cos \varphi \end{array}$$然后我们发现了什么！这前面定义的轴角对向量的旋转操作公式一模一样，那么说明$\mathbf{\varphi }\triangleq \mathbf{\omega }\Delta t$即旋转操作的轴角$\mathbf{v}$，$\mathbf{\omega }$即旋转角速度

2.3 四元数与轴角的关系

前面介绍旋转矩阵与轴角的关系，接下来按照同样的思路介绍四元数与轴角的关系，四元数对旋转的操作为：$$r(\mathbf{v})=\mathbf{q}\otimes \mathbf{v}\otimes {\mathbf{q}}^{\ast }$$根据旋转操作的性质，旋转操作不改变向量的长度$\Vert \mathbf{q}\otimes \mathbf{v}\otimes {\mathbf{q}}^{\ast }\Vert =\Vert \mathbf{q}{\Vert }^2\Vert \mathbf{v}\Vert =\Vert \mathbf{v}\Vert$，因此有$${\mathbf{q}}^{\ast }\otimes \mathbf{q}=1=\mathbf{q}\otimes {\mathbf{q}}^{\ast }$$可以看出来，表示旋转的四元数都单位四元数，我们将这样的四元数构成空间定义为$q\in S^3$，同样我们进行微分操作$$\frac{d\left({\mathbf{q}}^{\ast }\otimes \mathbf{q}\right)}{dt}={\dot{\mathbf{q}}}^{\ast }\otimes \mathbf{q}+{\mathbf{q}}^{\ast }\otimes \dot{\mathbf{q}}=0$$然后可得$${\mathbf{q}}^{\ast }\otimes \dot{\mathbf{q}}=-\left({\dot{\mathbf{q}}}^{\ast }\otimes \mathbf{q}\right)=-{\left({\mathbf{q}}^{\ast }\otimes \dot{\mathbf{q}}\right)}^{\ast }$$四元数与其负共轭相等，说明${\mathbf{q}}^{\ast }\otimes \dot{\mathbf{q}}$是一个纯四元数，同理我们定义个纯四元数$\Omega \in {\mathbb{H}}_p$有$${\mathbf{q}}^{\ast }\otimes \dot{\mathbf{q}}=\Omega =\left[\begin{array}{l}0\\ \Omega \end{array}\right]\in {\mathbb{H}}_p$$同样我们和时间$t$关联起来，然后就可以获得微分方程$$\dot{\mathbf{q}(t)}=\mathbf{q}(t)\otimes \Omega$$，当$\mathbf{q}(0)=1$时，$\dot{\mathbf{q}}(0)=\Omega \in {\mathbb{H}}_p$，因此${\mathbb{H}}_p$就是$S^3$的正切空间或者李代数，我们假设$\Omega$是常数，我们就可以进一步积分有$$\mathbf{q}(t)=\mathbf{q}(0)\otimes e^{\Omega \Delta t}$$因为$\mathbf{q}(t)$和$\mathbf{q}(0)$都是单位四元数，因此$e^{\Omega \Delta t}$也是单位四元数，我们定义$\mathbf{V}\triangleq \Omega \Delta t$，因此有$$\mathbf{q}=e^{\mathbf{V}}$$根据1.8节数四元数对数定义的介绍，$\mathbf{V}$是一个纯四元数，因此我们可以定义$\mathbf{V}=\theta \mathbf{u}$，然后我们定义$\mathbf{v}=\varphi \mathbf{u}$,其中$\theta =\varphi /2$（和旋转矩阵一样，接下来会通过推到证明$\mathbf{v}$其实就是轴角），这里给出四元数指数映射关系这里定义的指数映射为$\mathrm{Exp}(\varphi )\triangleq \exp (\varphi /2)$，我们对指数映射进行展开有$$\mathbf{q}\triangleq \mathrm{Exp}(\varphi \mathbf{u})=e^{\varphi \mathbf{u}/2}=\cos \frac{\varphi }{2}+\mathbf{u}\sin \frac{\varphi }{2}=\left[\begin{array}{c}\cos (\varphi /2)\\ \mathbf{u}\sin (\varphi /2)\end{array}\right]$$同理定义对数映射$\mathrm{Log}:S^3\to {\mathbb{R}}^3;\mathbf{q}\mapsto \mathrm{Log}(\mathbf{q})=\mathbf{u}\varphi$，其中$$\begin{array}{l}\varphi =2\arctan \left(\Vert {\mathbf{q}}_v\Vert ,q_w\right)\\ \mathbf{u}={\mathbf{q}}_v/\Vert {\mathbf{q}}_v\Vert \end{array}$$最后我们回过头来看四元数对向量的旋转操作来证明四元数与轴角的关系$$\begin{array}{rl}{\mathbf{x}}^{\prime }=& \mathbf{q}\otimes \mathbf{x}\otimes {\mathbf{q}}^{\ast }\\ =& \left(\cos \frac{\varphi }{2}+\mathbf{u}\sin \frac{\varphi }{2}\right)\otimes (0+\mathbf{x})\otimes \left(\cos \frac{\varphi }{2}-\mathbf{u}\sin \frac{\varphi }{2}\right)\\ =& \mathbf{x}{\cos }^2\frac{\varphi }{2}+(\mathbf{u}\otimes \mathbf{x}-\mathbf{x}\otimes \mathbf{u})\sin \frac{\varphi }{2}\cos \frac{\varphi }{2}-\mathbf{u}\otimes \mathbf{x}\otimes \mathbf{u}{\sin }^2\frac{\varphi }{2}\\ =& \mathbf{x}{\cos }^2\frac{\varphi }{2}+2(\mathbf{u}\times \mathbf{x})\sin \frac{\varphi }{2}\cos \frac{\varphi }{2}-\left(\mathbf{x}\left({\mathbf{u}}^{\top }\mathbf{u}\right)-2\mathbf{u}\left({\mathbf{u}}^{\top }\mathbf{x}\right)\right){\sin }^2\frac{\varphi }{2}\\ =& \mathbf{x}\left({\cos }^2\frac{\varphi }{2}-{\sin }^2\frac{\varphi }{2}\right)+(\mathbf{u}\times \mathbf{x})\left(2\sin \frac{\varphi }{2}\cos \frac{\varphi }{2}\right)+\mathbf{u}\left({\mathbf{u}}^{\top }\mathbf{x}\right)\left(2{\sin }^2\frac{\varphi }{2}\right)\\ =& \mathbf{x}\cos \varphi +(\mathbf{u}\times \mathbf{x})\sin \varphi +\mathbf{u}\left({\mathbf{u}}^{\top }\mathbf{x}\right)(1-\cos \varphi )\\ =& \left(\mathbf{x}-\mathbf{u}{\mathbf{u}}^{\top }\mathbf{x}\right)\cos \varphi +(\mathbf{u}\times \mathbf{x})\sin \varphi +\mathbf{u}{\mathbf{u}}^{\top }\mathbf{x}\\ =& {\mathbf{x}}_{\perp }\cos \varphi +(\mathbf{u}\times \mathbf{x})\sin \varphi +{\mathbf{x}}_{\Vert }\end{array}$$可以看出来，前面我们定义的$\mathbf{v}=\varphi \mathbf{u}$其实就是轴角，而四元数代表的角度$\theta$其实为实际旋转角度的一半，这也就是论文中提到的四元素的double cover性质。

2.4 旋转矩阵和四元数的关系

我们已知$$\mathbf{q}\otimes \mathbf{x}\otimes {\mathbf{q}}^{\ast }=\mathbf{R}\mathbf{x}$$令$\mathbf{x}$为单位向量，那么就可以获得$$\mathbf{R}=\left[\begin{array}{ccc}{q}_w^2+q_x^2-q_y^2-q_z^2& 2\left(q_x{q}_y-q_w{q}_z\right)& 2\left(q_x{q}_z+q_w{q}_y\right)\\ 2\left(q_x{q}_y+q_w{q}_z\right)& q_w^2-q_x^2+q_y^2-q_z^2& 2\left(q_y{q}_z-q_w{q}_x\right)\\ 2\left(q_x{q}_z-q_w{q}_y\right)& 2\left(q_y{q}_z+q_w{q}_x\right)& q_w^2-q_x^2-q_y^2+q_z^2\end{array}\right]$$根据四元数的性质我们还可以获得另外一种表达形式$$\mathbf{q}\otimes \mathbf{x}\otimes {\mathbf{q}}^{\ast }={\left[\begin{array}{l}{\mathbf{q}}^{\ast }\end{array}\right]}_R[\mathbf{q}{]}_L\left[\begin{array}{l}0\\ \mathbf{x}\end{array}\right]=\left[\begin{array}{c}0\\ \mathbf{R}\mathbf{x}\end{array}\right]$$那么我们可以获得$$\mathbf{R}=\left(q_w^2-{\mathbf{q}}_v^{\top }{\mathbf{q}}_v\right)\mathbf{I}+2{\mathbf{q}}_v{\mathbf{q}}_v^{\top }+2q_w{\left[{\mathbf{q}}_v\right]}_{\times }$$以上两种都是从四元数到旋转矩阵的关系，从旋转矩阵获得四元数可以根据上面第一种表达式直接逆推获得$$\begin{array}{rl}{q}_w& =\frac{\sqrt{(\mathrm{tr}(\mathbf{R})+1}}{2}\\ q_x& =\frac{{\mathbf{R}}_{32}-{\mathbf{R}}_{23}}{4q_w}\\ q_y& =\frac{{\mathbf{R}}_{13}-{\mathbf{R}}_{31}}{4q_w}\\ q_z& =\frac{{\mathbf{R}}_{21}-{\mathbf{R}}_{12}}{4q_w}\end{array}$$

3. 四元数的雅克比、微分和积分

参考文献中这一部分还介绍了很多内容，包括旋转的雅克比等，由于篇幅原因，这里只总结其中关于四元数积分和微分的一部分

3.1 四元数的雅克比

参考文献中给出了三种类型的雅克比
第一种是向量对向量的雅克比：$$\frac{\partial (\mathbf{q}\otimes \mathbf{a}\otimes \mathbf{q}\ast )}{\partial \mathbf{a}}=\frac{\partial (\mathbf{R}\mathbf{a})}{\partial \mathbf{a}}=\mathbf{R}$$第二种是向量对四元数的雅克比（推导过程参考文献）：$$\frac{\partial \left(\mathbf{q}\otimes \mathbf{a}\otimes {\mathbf{q}}^{\ast }\right)}{\partial \mathbf{q}}=2\left[w\mathbf{a}+\mathbf{v}\times \mathbf{a}\mid {\mathbf{v}}^{\top }\mathbf{a}{\mathbf{I}}_3+\mathbf{v}{\mathbf{a}}^{\top }-\mathbf{a}{\mathbf{v}}^{\top }-w[\mathbf{a}{]}_{\times }\right]\in {\mathbb{R}}^{3\times 4}$$$$\frac{\partial \left(\mathbf{q}\otimes \mathbf{a}\otimes {\mathbf{q}}^{\ast }\right)}{\partial \mathbf{q}}=2\left[w\mathbf{a}+\mathbf{v}\times \mathbf{a}\mid {\mathbf{v}}^{\top }\mathbf{a}{\mathbf{I}}_3+\mathbf{v}{\mathbf{a}}^{\top }-\mathbf{a}{\mathbf{v}}^{\top }-w[\mathbf{a}{]}_{\times }\right]\in {\mathbb{R}}^{3\times 4}$$第三种是向量对轴角的雅克比（推导过程参考文献）：$$\frac{\partial \left(\mathbf{q}\otimes \mathbf{a}\otimes {\mathbf{q}}^{\ast }\right)}{\partial \delta \mathbf{\theta }}=\frac{\partial (\mathbf{R}\mathbf{a})}{\partial \delta \mathbf{\theta }}=-\mathbf{R}\{\mathbf{\theta }\}[\mathbf{a}{]}_{\times }{\mathbf{J}}_r(\mathbf{\theta })$$第三种求导方式其实就是李代数求导方式，结合《视觉SLAM十四讲》中的知识，这种求导方式其实是不友好的，通常采用扰动模型进行，这篇文献中没有涉及，这里也不进一步进行展开了。

3.2 四元数的微分

四元数的微分在推导IMU的运动方程时将会用到，四元数的微分定义如下：$$\begin{array}{rl}\dot{\mathbf{q}}& \triangleq \underset{\Delta t\to 0}{\lim }\frac{\mathbf{q}(t+\Delta t)-\mathbf{q}(t)}{\Delta t}\\ & =\underset{\Delta t\to 0}{\lim }\frac{\mathbf{q}\otimes \Delta {\mathbf{q}}_{\mathcal{L}}-\mathbf{q}}{\Delta t}\\ & =\underset{\Delta t\to 0}{\lim }\frac{\mathbf{q}\otimes \left(\left[\begin{array}{c}1\\ \Delta {\mathbf{\varphi }}_{\mathcal{L}}/2\end{array}\right]-\left[\begin{array}{l}1\\ 0\end{array}\right]\right)}{\Delta t}\\ & =\underset{\Delta t\to 0}{\lim }\frac{\mathbf{q}\otimes \left[\begin{array}{c}0\\ \Delta {\mathbf{\varphi }}_{\mathcal{L}}/2\end{array}\right]}{\Delta t}\\ & =\frac{1}{2}\mathbf{q}\otimes \left[\begin{array}{c}0\\ {\mathbf{\omega }}_{\mathcal{L}}\end{array}\right]\end{array}$$其中，$\Delta {\mathbf{q}}_{\mathcal{L}}$为一个局部扰动，根据四元数的指数映射有$$\Delta {\mathbf{q}}_{\mathcal{L}}\triangleq \mathrm{Exp}(\Delta {\varphi }_{\mathcal{L}}\mathbf{u})=e^{\Delta {\varphi }_{\mathcal{L}}\mathbf{u}/2}=\cos \frac{\Delta {\varphi }_{\mathcal{L}}}{2}+\mathbf{u}\sin \frac{\Delta {\varphi }_{\mathcal{L}}}{2}=\left[\begin{array}{c}\cos (\Delta {\varphi }_{\mathcal{L}}/2)\\ \mathbf{u}\sin (\Delta {\varphi }_{\mathcal{L}}/2)\end{array}\right]$$当$\Delta {\varphi }_{\mathcal{L}}$较小时，有如下近似：$$\left[\begin{array}{c}\cos \left(\Delta {\varphi }_{\mathcal{L}}/2\right)\\ \mathbf{u}\sin \left(\Delta {\varphi }_{\mathcal{L}}/2\right)\end{array}\right]=\left[\begin{array}{c}1\\ \Delta {\varphi }_{\mathcal{L}}/2\end{array}\right]$$根据四元数乘法规则，我们可以定义$$\mathbf{\Omega }(\mathbf{\omega })\triangleq [\mathbf{\omega }{]}_R=\left[\begin{array}{cc}0& -{\mathbf{\omega }}^{\top }\\ \mathbf{\omega }& -[\mathbf{\omega }{]}_{\times }\end{array}\right]=\left[\begin{array}{cccc}0& -{\omega }_x& -{\omega }_y& -{\omega }_z\\ {\omega }_x& 0& {\omega }_z& -{\omega }_y\\ {\omega }_y& -{\omega }_z& 0& {\omega }_x\\ {\omega }_z& {\omega }_y& -{\omega }_x& 0\end{array}\right]$$那么四元数的微分即$$\dot{\mathbf{q}}=\frac{1}{2}\Omega \left({\mathbf{\omega }}_{\mathcal{L}}\right)\mathbf{q}=\frac{1}{2}\mathbf{q}\otimes {\mathbf{\omega }}_{\mathcal{L}}$$如果转化成旋转矩阵是$$\dot{\mathbf{R}}=\mathbf{R}{\left[{\mathbf{\omega }}_{\mathcal{L}}\right]}_{}$$

3.3 四元数的积分

四元数的积分在运动状态的积分时会用到，这里只介绍下最为简单的零阶积分，在我们利用IMU进行测量是实际上获得是如下微分方程$$\dot{\mathbf{q}}(t)=\frac{1}{2}\mathbf{q}(t)\otimes \mathbf{\omega }(t)$$根据泰勒展开$${\mathbf{q}}_{n+1}={\mathbf{q}}_n+{\dot{\mathbf{q}}}_n\Delta t+\frac{1}{2!}{\ddot{\mathbf{q}}}_n\Delta t^2+\frac{1}{3!}{\ddot{\mathbf{q}}}_n\Delta t^3+\frac{1}{4!}{\ddot{\mathbf{q}}}_n\Delta t^4+\cdots$$由于$\ddot{\omega }=0$，因此有$$\begin{array}{rl}{\dot{\mathbf{q}}}_n& =\frac{1}{2}{\mathbf{q}}_n{\mathbf{\omega }}_n\\ {\ddot{\mathbf{q}}}_n& =\frac{1}{2^2}{\mathbf{q}}_n{\mathbf{\omega }}_n^2+\frac{1}{2}{\mathbf{q}}_n\dot{\mathbf{\omega }}\\ {\ddot{\mathbf{q}}}_n& =\frac{1}{2^3}{\mathbf{q}}_n{\mathbf{\omega }}_n^3+\frac{1}{4}{\mathbf{q}}_n\dot{\mathbf{\omega }}{\mathbf{\omega }}_n+\frac{1}{2}\mathbf{q}{\mathbf{\omega }}_n\dot{\mathbf{\omega }}\\ {\mathbf{q}}_n^{(i\ge 4)}& =\frac{1}{2^i}{\mathbf{q}}_n{\mathbf{\omega }}_n^i+\cdots \end{array}$$当角速度$\mathbf{\omega }(t)$为常数时，上面的泰勒展开式进一步退化为$${\mathbf{q}}_{n+1}={\mathbf{q}}_n\otimes \left(1+\frac{1}{2}{\mathbf{\omega }}_n\Delta t+\frac{1}{2!}{\left(\frac{1}{2}{\mathbf{\omega }}_n\Delta t\right)}^2+\frac{1}{3!}{\left(\frac{1}{2}{\mathbf{\omega }}_n\Delta t\right)}^3+\cdots \right)$$右测的泰勒展开式其实就是$e^{{\omega }_n\Delta t/2}$，并且根据四元数的指数映射有$$e^{\omega \Delta t/2}=\mathrm{Exp}(\mathbf{\omega }\Delta t)=\mathbf{q}\{\mathbf{\omega }\Delta t\}=\left[\begin{array}{c}\cos (\Vert \mathbf{\omega }\Vert \Delta t/2)\\ \frac{\omega }{\Vert \mathbf{\omega }\Vert }\sin (\Vert \mathbf{\omega }\Vert \Delta t/2)\end{array}\right]$$对于前向积分有$${\mathbf{q}}_{n+1}={\mathbf{q}}_n\otimes \mathbf{q}\left\{{\mathbf{\omega }}_n\Delta t\right\}$$对于后向积分有$${\mathbf{q}}_n={\mathbf{q}}_{n-1}\otimes \mathbf{q}\left\{{\mathbf{\omega }}_n\Delta t\right\}$$对于中值积分有$${\mathbf{q}}_{n+1}={\mathbf{q}}_n\otimes \mathbf{q}\{\overline{\mathbf{\omega }}\Delta t\}$$

到这里就按照文献的思路总结了部分四元数的基础知识，中间有一部分知识还需要再研究研究，比如四元数的雅克比，这一部分等研究MSCKF的后端时再仔细推导推导，有问题欢迎指出交流～