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

学习MSCKF笔记——四元数基础
- 1. 四元数基本性质
- - 1.1 加法
  - 1.2 乘法
  - 1.3 共轭
  - 1.4 模
  - 1.5 逆
  - 1.6 单位四元数
  - 1.7 指数
  - 1.8 对数
- 2. 旋转的表示形式
- - 2.1 轴角与旋转的定义
  - 2.2 旋转矩阵与轴角的关系
  - 2.3 四元数与轴角的关系
  - 2.4 旋转矩阵和四元数的关系
- 3. 四元数的雅克比、微分和积分
- - 3.1 四元数的雅克比
  - 3.2 四元数的微分
  - 3.3 四元数的积分

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

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

1. 四元数基本性质

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

1.1 加法

加法就是实部虚部分别相加：
p±q=[pwpv]±[qwqv]=[pw±qwpv±qv]\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]p±q=[pwpv]±[qwqv]=[pw±qwpv±qv]满足交换律和结合律：p+q=q+pp+(q+r)=(p+q)+r\begin{aligned} \mathbf{p}+\mathbf{q} &=\mathbf{q}+\mathbf{p} \\ \mathbf{p}+(\mathbf{q}+\mathbf{r}) &=(\mathbf{p}+\mathbf{q})+\mathbf{r} \end{aligned}p+qp+(q+r)=q+p=(p+q)+r

1.2 乘法

乘法一共有三种计算法方式：
第一种：
p⊗q=[pwqw−pxqx−pyqy−pzqzpwqx+pxqw+pyqz−pzqypwqy−pxqz+pyqw+pzqxpwqz+pxqy−pyqx+pzqw]\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]p⊗q=⎣⎢⎢⎡pwqw−pxqx−pyqy−pzqzpwqx+pxqw+pyqz−pzqypwqy−pxqz+pyqw+pzqxpwqz+pxqy−pyqx+pzqw⎦⎥⎥⎤
第二种：p⊗q=[pwqw−pv⊤qvpwqv+qwpv+pv×qv]\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]p⊗q=[pwqw−pv⊤qvpwqv+qwpv+pv×qv]
第三种：q1⊗q2=[q1]Lq2and q1⊗q2=[q2]Rq1\mathbf{q}_{1} \otimes \mathbf{q}_{2}=\left[\mathbf{q}_{1}\right]_{L} \mathbf{q}_{2} \quad \text { and } \quad \mathbf{q}_{1} \otimes \mathbf{q}_{2}=\left[\mathbf{q}_{2}\right]_{R} \mathbf{q}_{1}q1⊗q2=[q1]Lq2 and q1⊗q2=[q2]Rq1其中：[q]L=[qw−qx−qy−qzqxqw−qzqyqyqzqw−qxqz−qyqxqw],[q]R=[qw−qx−qy−qzqxqwqz−qyqy−qzqwqxqzqy−qxqw][\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], \quad[\mathbf{q}]_{R}=\left[\begin{array}{ccccc} 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][q]L=⎣⎢⎢⎡qwqxqyqz−qxqwqz−qy−qy−qzqwqx−qzqy−qxqw⎦⎥⎥⎤,[q]R=⎣⎢⎢⎡qwqxqyqz−qxqw−qzqy−qyqzqw−qx−qz−qyqxqw⎦⎥⎥⎤或者表示为：[q]L=qwI+[0−qv⊤qv[qv]×],[q]R=qwI+[0−qv⊤qv−[qv]×][\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], \quad[\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][q]L=qwI+[0qv−qv⊤[qv]×],[q]R=qwI+[0qv−qv⊤−[qv]×]满足分配率和结合率(p⊗q)⊗r=p⊗(q⊗r)(\mathbf{p} \otimes \mathbf{q}) \otimes \mathbf{r}=\mathbf{p} \otimes(\mathbf{q} \otimes \mathbf{r})(p⊗q)⊗r=p⊗(q⊗r)p⊗(q+r)=p⊗q+p⊗r\mathbf{p} \otimes(\mathbf{q}+\mathbf{r})=\mathbf{p} \otimes \mathbf{q}+\mathbf{p} \otimes \mathbf{r} \quad p⊗(q+r)=p⊗q+p⊗r

1.3 共轭

四元数的共轭定义为q∗≜qw−qv=[qw−qv]\mathbf{q}^{*} \triangleq q_{w}-\mathbf{q}_{v}=\left[\begin{array}{c} q_{w} \\ -\mathbf{q}_{v} \end{array}\right]q∗≜qw−qv=[qw−qv]共轭的性质为q⊗q∗=q∗⊗q=qw2+qx2+qy2+qz2=[qw2+qx2+qy2+qz20v]\mathbf{q} \otimes \mathbf{q}^{*}=\mathbf{q}^{*} \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} \\ \mathbf{0}_{v} \end{array}\right]q⊗q∗=q∗⊗q=qw2+qx2+qy2+qz2=[qw2+qx2+qy2+qz20v](p⊗q)∗=q∗⊗p∗(\mathbf{p} \otimes \mathbf{q})^{*}=\mathbf{q}^{*} \otimes \mathbf{p}^{*}(p⊗q)∗=q∗⊗p∗

1.4 模

四元数的模的定义为：∥q∥≜q⊗q∗=q∗⊗q=qw2+qx2+qy2+qz2∈R\|\mathbf{q}\| \triangleq \sqrt{\mathbf{q} \otimes \mathbf{q}^{*}}=\sqrt{\mathbf{q}^{*} \otimes \mathbf{q}}=\sqrt{q_{w}^{2}+q_{x}^{2}+q_{y}^{2}+q_{z}^{2}} \in \mathbb{R}∥q∥≜q⊗q∗=q∗⊗q=qw2+qx2+qy2+qz2∈R模的性质为：∥p⊗q∥=∥q⊗p∥=∥p∥∥q∥\|\mathbf{p} \otimes \mathbf{q}\|=\|\mathbf{q} \otimes \mathbf{p}\|=\|\mathbf{p}\|\|\mathbf{q}\|∥p⊗q∥=∥q⊗p∥=∥p∥∥q∥

1.5 逆

四元数的逆的定义为：q⊗q−1=q−1⊗q=q1\mathbf{q} \otimes \mathbf{q}^{-1}=\mathbf{q}^{-1} \otimes \mathbf{q}=\mathbf{q}_{1}q⊗q−1=q−1⊗q=q1同样，可以被计算为q−1=q∗/∥q∥2\mathbf{q}^{-1}=\mathbf{q}^{*} /\|\mathbf{q}\|^{2}q−1=q∗/∥q∥2

1.6 单位四元数

单位四元数通常指∥q∥=1\|\mathbf{q}\|=1∥q∥=1，可以被写为：q=[cos⁡θusin⁡θ]\mathbf{q}=\left[\begin{array}{c} \cos \theta \\ \mathbf{u} \sin \theta \end{array}\right]q=[cosθusinθ]其中u=uxi+uyj+uzk\mathbf{u}=u_{x} i+u_{y} j+u_{z} ku=uxi+uyj+uzk是一个单位向量，其满足性质：q−1=q∗\mathbf{q}^{-1}=\mathbf{q}^{*}q−1=q∗

1.7 指数

这里的四元数的指数通常指纯四元数（实部为0的四元数），即q=[0,qv]\mathbf{q}=\left[0, \mathbf{q}_{v}\right]q=[0,qv]，根据乘法的定义可以有qv⊗qv=−qv⊤qv=−∥qv∥2\mathbf{q}_{v} \otimes \mathbf{q}_{v}=-\mathbf{q}_{v}^{\top} \mathbf{q}_{v}=-\left\|\mathbf{q}_{v}\right\|^{2}qv⊗qv=−qv⊤qv=−∥qv∥2那么对于纯单位四元数u\mathbf{u}u有∥u∥=1\|\mathbf{u}\|=1∥u∥=1u⊗u=−1\mathbf{u} \otimes \mathbf{u}=-1u⊗u=−1那么对于一个纯四元数我们可以定义为v=uθ\mathbf{v}=\mathbf{u} \thetav=uθ，那么我们可以得到纯四元数的幂运算为：v2=−θ2,v3=−uθ3,v4=θ4,v5=uθ5,v6=−θ6,⋯\mathbf{v}^{2}=-\theta^{2} \quad, \quad \mathbf{v}^{3}=-\mathbf{u} \theta^{3} \quad, \quad \mathbf{v}^{4}=\theta^{4} \quad, \quad \mathbf{v}^{5}=\mathbf{u} \theta^{5} \quad, \quad \mathbf{v}^{6}=-\theta^{6} \quad, \quad \cdotsv2=−θ2,v3=−uθ3,v4=θ4,v5=uθ5,v6=−θ6,⋯那么四元数的指数可以通过泰勒展开获得如下形式ev=∑k=0∞1k!vke^{\mathbf{v}}=\sum_{k=0}^{\infty} \frac{1}{k !} \mathbf{v}^{k}ev=k=0∑∞k!1vk将v=uθ\mathbf{v}=\mathbf{u} \thetav=uθ代入得euθ=(1−θ22!+θ44!+⋯)+(uθ−uθ33!+uθ55!+⋯)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)euθ=(1−2!θ2+4!θ4+⋯)+(uθ−3!uθ3+5!uθ5+⋯)进而我们可以获得经典的欧拉公式ev=euθ=cos⁡θ+usin⁡θ=[cos⁡θusin⁡θ]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]ev=euθ=cosθ+usinθ=[cosθusinθ]对于一般的四元数，我们可以进行指数形式的拓展有eq=eqw+qv=eqweqve^{\mathbf{q}}=e^{q_{w}+\mathbf{q}_{v}}=e^{q_{w}} e^{\mathbf{q}_ v}eq=eqw+qv=eqweqv令uθ=qv\mathbf{u} \theta=\mathbf{q}_{v}uθ=qv有eq=eqw[cos⁡∥qv∥qv∥qv∥sin⁡∥qv∥]e^{\mathbf{q}}=e^{q_{w}}\left[\begin{array}{c} \cos \left\|\mathbf{q}_{v}\right\| \\ \frac{\mathbf{q}_{v}}{\left\|\mathbf{q}_{v}\right\|} \sin \left\|\mathbf{q}_{v}\right\| \end{array}\right]eq=eqw[cos∥qv∥∥qv∥qvsin∥qv∥]

1.8 对数

有了前面的推到，这里可以给出单位四元数q=[cos⁡θ,usin⁡θ]\mathbf{q}=[\cos \theta, \mathbf{u} \sin \theta]q=[cosθ,usinθ]的对数形式log⁡q=log⁡(cos⁡θ+usin⁡θ)=log⁡(euθ)=uθ=[0uθ]\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]logq=log(cosθ+usinθ)=log(euθ)=uθ=[0uθ]这里可以看到一个单位四元数的对数是一个纯四元数，对于一般的四元数，我们可以拓展为log⁡q=log⁡(∥q∥q∥q∥)=log⁡∥q∥+log⁡q∥q∥=log⁡∥q∥+uθ=[log⁡∥q∥uθ]\log \mathbf{q}=\log \left(\|\mathbf{q}\| \frac{\mathbf{q}}{\|\mathbf{q}\|}\right)=\log \|\mathbf{q}\|+\log \frac{\mathbf{q}}{\|\mathbf{q}\|}=\log \|\mathbf{q}\|+\mathbf{u} \theta=\left[\begin{array}{c} \log \|\mathbf{q}\| \\ \mathbf{u} \theta \end{array}\right]logq=log(∥q∥∥q∥q)=log∥q∥+log∥q∥q=log∥q∥+uθ=[log∥q∥uθ]前面我们推到了纯四元数的幂运算，这里通过结合四元数的指数和对数的性质可以进一步推导单位四元数的幂运算，有qt=exp⁡(log⁡(qt))=exp⁡(tlog⁡(q))\mathbf{q}^{t}=\exp \left(\log \left(\mathbf{q}^{t}\right)\right)=\exp (t \log (\mathbf{q}))qt=exp(log(qt))=exp(tlog(q))因为是单位四元数，所以有q=[cos⁡θ,usin⁡θ]\mathbf{q}=[\cos \theta, \mathbf{u} \sin \theta]q=[cosθ,usinθ]，那么有qt=exp⁡(tuθ)=[cos⁡tθusin⁡tθ]\mathbf{q}^{t}=\exp (t \mathbf{u} \theta)=\left[\begin{array}{c} \cos t \theta \\ \mathbf{u} \sin t \theta \end{array}\right]qt=exp(tuθ)=[costθusintθ]

2. 旋转的表示形式

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

2.1 轴角与旋转的定义

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

对旋转操作表示最为直接的是轴角表示形式，如上图所示u\mathbf{u}u即为旋转轴，ϕ\phiϕ即为旋转角，对于向量x\mathbf{x}x可以分解为与旋转轴平行的部分x∥\mathbf{x}_{\|}x∥以及与旋转轴垂直的部分x⊥\mathbf{x}_{\perp}x⊥，如下：x∥=u(∥x∥cos⁡α)=uu⊤xx⊥=x−x∥=x−uu⊤x\begin{array}{l} \mathbf{x}_{\|}=\mathbf{u}(\|\mathbf{x}\| \cos \alpha)=\mathbf{u} \mathbf{u}^{\top} \mathbf{x} \\ \mathbf{x}_{\perp}=\mathbf{x}-\mathbf{x}_{\|}=\mathbf{x}-\mathbf{u} \mathbf{u}^{\top} \mathbf{x} \end{array}x∥=u(∥x∥cosα)=uu⊤xx⊥=x−x∥=x−uu⊤x向量平行部分不会受到旋转操作的影响，而垂直部分经过旋转操作变为x⊥′=x⊥cos⁡ϕ+(u×x)sin⁡ϕ\mathbf{x}_{\perp}^{\prime}=\mathbf{x}_{\perp} \cos \phi+(\mathbf{u} \times \mathbf{x}) \sin \phix⊥′=x⊥cosϕ+(u×x)sinϕ因此旋转操作可以表示为x′=x∥+x⊥cos⁡ϕ+(u×x)sin⁡ϕ\mathbf{x}^{\prime}=\mathbf{x}_{\|}+\mathbf{x}_{\perp} \cos \phi+(\mathbf{u} \times \mathbf{x}) \sin \phix′=x∥+x⊥cosϕ+(u×x)sinϕ旋转操作同时具备一下三条性质：
第一条：旋转保留向量的长度∥r(v)∥=⟨r(v),r(v)⟩=⟨v,v⟩≜∥v∥,∀v∈R3\|r(\mathbf{v})\|=\sqrt{\langle r(\mathbf{v}), r(\mathbf{v})\rangle}=\sqrt{\langle\mathbf{v}, \mathbf{v}\rangle} \triangleq\|\mathbf{v}\|, \quad \forall \mathbf{v} \in \mathbb{R}^{3}∥r(v)∥=⟨r(v),r(v)⟩=⟨v,v⟩≜∥v∥,∀v∈R3第二条：旋转保留向量间的角度：⟨r(v),r(w)⟩=⟨v,w⟩=∥v∥∥w∥cos⁡α,∀v,w∈R3\langle r(\mathbf{v}), r(\mathbf{w})\rangle=\langle\mathbf{v}, \mathbf{w}\rangle=\|\mathbf{v}\|\|\mathbf{w}\| \cos \alpha, \quad \forall \mathbf{v}, \mathbf{w} \in \mathbb{R}^{3}⟨r(v),r(w)⟩=⟨v,w⟩=∥v∥∥w∥cosα,∀v,w∈R3第三条：保留了向量间的相对方向u×v=w⟺r(u)×r(v)=r(w)\mathbf{u} \times \mathbf{v}=\mathbf{w} \Longleftrightarrow r(\mathbf{u}) \times r(\mathbf{v})=r(\mathbf{w})u×v=w⟺r(u)×r(v)=r(w)因此，旋转可以定义为SO(3)S O(3)SO(3)群，即：SO(3):{r:R3→R3/∀v,w∈R3,∥r(v)∥=∥v∥,r(v)×r(w)=r(v×w)}S O(3):\left\{r: \mathbb{R}^{3} \rightarrow \mathbb{R}^{3} / \forall \mathbf{v}, \mathbf{w} \in \mathbb{R}^{3},\|r(\mathbf{v})\|=\|\mathbf{v}\|, r(\mathbf{v}) \times r(\mathbf{w})=r(\mathbf{v} \times \mathbf{w})\right\}SO(3):{r:R3→R3/∀v,w∈R3,∥r(v)∥=∥v∥,r(v)×r(w)=r(v×w)}

2.2 旋转矩阵与轴角的关系

旋转矩阵和轴角之间的关系学习过《视觉SLAM十四讲》的同学应该都比较清楚了，这里先回顾一下，旋转矩阵对向量进行旋转操作的公式为：x′=Rx\mathbf{x}^{\prime}=\mathbf{R} \mathbf{x}x′=Rx旋转矩阵的性质有R⊤R=I=RR⊤\mathbf{R}^{\top} \mathbf{R}=\mathbf{I}=\mathbf{R} \mathbf{R}^{\top}R⊤R=I=RR⊤根据这个性质我们对时间进行微分有ddt(R⊤R)=R˙⊤R+R⊤R˙=0\frac{d}{d t}\left(\mathbf{R}^{\top} \mathbf{R}\right)=\dot{\mathbf{R}}^{\top} \mathbf{R}+\mathbf{R}^{\top} \dot{\mathbf{R}}=0dtd(R⊤R)=R˙⊤R+R⊤R˙=0然后可得R⊤R˙=−(R⊤R)⊤\mathbf{R}^{\top} \dot{\mathbf{R}}=-\left(\mathbf{R}^{\top} \mathbf{R}\right)^{\top}R⊤R˙=−(R⊤R)⊤这就意味着R⊤R˙\mathbf{R}^{\top} \dot{\mathbf{R}}R⊤R˙是一个反对称矩阵，那么我可以定义这样一个反对称矩阵为[ω]×≜[0−ωzωyωz0−ωx−ωyωx0][\boldsymbol{\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][ω]×≜⎣⎡0ωz−ωy−ωz0ωxωy−ωx0⎦⎤这样我们就可以通过ω=(ωx,ωy,ωz)∈R3\boldsymbol{\omega}=\left(\omega_{x}, \omega_{y}, \omega_{z}\right) \in \mathbf{R}^{3}ω=(ωx,ωy,ωz)∈R3来描述旋转矩阵及其微分间的关系，而由ω\boldsymbol{\omega}ω构成的反对称矩阵[ω]×[\boldsymbol{\omega}]_{\times}[ω]×即构成了我们熟悉的李代数空间so(3)\mathfrak{s o}(3)so(3)，因为我们的目的是用来描述相机的旋转变化，因此我们可以将旋转矩阵与时间ttt进行关联，然后就可以获得这样微分方程R˙(t)=R(t)[ω]×\dot{\mathbf{R}}(t)=\mathbf{R}(t)[\boldsymbol{\omega}]_{\times}R˙(t)=R(t)[ω]×我们可以发现，当R(0)=I\mathbf{R}(0)=\mathbf{I}R(0)=I时，R˙(0)=[ω]×\dot{\mathbf{R}}(0)=[\boldsymbol{\omega}]_{\times}R˙(0)=[ω]×，那么我们可以将so(3)\mathfrak{s o}(3)so(3)理解为SO(3)S O(3)SO(3)在原点处（t=0t=0t=0）的正切空间或者速度空间，当ω\boldsymbol{\omega}ω为常数时，微分方程对时间积分得R(t)=R(0)e[ω]×t=R(0)e[ωt]×\mathbf{R}(t)=\mathbf{R}(0) e^{[\boldsymbol{\omega}]_{\times} t}=\mathbf{R}(0) e^{[\boldsymbol{\omega} t]_{\times}}R(t)=R(0)e[ω]×t=R(0)e[ωt]×我们将ϕ≜ωΔt\boldsymbol{\phi} \triangleq \boldsymbol{\omega} \Delta tϕ≜ωΔt，那么有R=e[ϕ]×\mathbf{R}=e^{[\boldsymbol{\phi}]_{\times}}R=e[ϕ]×我们定义ϕ=v\boldsymbol{\phi}=\mathbf{v}ϕ=v那么ϕ\boldsymbol{\phi}ϕ、[ϕ]×[\boldsymbol{\phi}]_{\times}[ϕ]×和R\mathbf{R}R之间的关系可以通过下图描述：这里定义了旋转矩阵的指数映射Exp⁡:R3→SO(3);ϕ↦Exp⁡(ϕ)=e[ϕ]\operatorname{Exp}: \mathbb{R}^{3} \rightarrow S O(3) ; \boldsymbol{\phi} \mapsto \operatorname{Exp}(\boldsymbol{\phi})=e^{[\boldsymbol{\phi}]}Exp:R3→SO(3);ϕ↦Exp(ϕ)=e[ϕ]，我们进一步对指数映射进行展开，定义ϕ=ϕu\boldsymbol{\phi}=\phi \mathbf{u}ϕ=ϕu，u\mathbf{u}u是单位向量，那么[ϕ]×=ϕ[u]×[\phi]_{\times}=\phi[\mathbf{u}]_{\times}[ϕ]×=ϕ[u]×，指数映射就可以泰勒展开为R=eϕ[u]×=I+ϕ[u]×+12ϕ2[u]×2+13!ϕ3[u]×3+14!ϕ4[u]×4+…\mathbf{R}=e^{\phi[\mathbf{u}]_{\times}}=\mathbf{I}+\phi[\mathbf{u}]_{\times}+\frac{1}{2} \phi^{2}[\mathbf{u}]_{\times}^{2}+\frac{1}{3 !} \phi^{3}[\mathbf{u}]_{\times}^{3}+\frac{1}{4 !} \phi^{4}[\mathbf{u}]_{\times}^{4}+\ldotsR=eϕ[u]×=I+ϕ[u]×+21ϕ2[u]×2+3!1ϕ3[u]×3+4!1ϕ4[u]×4+…因为u\mathbf{u}u是单位向量，那么有[u]×2=uu⊤−I[u]×3=−[u]×\begin{array}{l} {[\mathbf{u}]_{\times}^{2}=\mathbf{u u}^{\top}-\mathbf{I}} \\ {[\mathbf{u}]_{\times}^{3}=-[\mathbf{u}]_{\times}} \end{array}[u]×2=uu⊤−I[u]×3=−[u]×以及[u]x4=−[u]x2[u]x5=[u]×[u]×6=[u]×2[u]×7=−[u]×⋯[\mathbf{u}]_{x}^{4}=-[\mathbf{u}]_{x}^{2} \quad[\mathbf{u}]_{x}^{5}=[\mathbf{u}]_{\times} \quad[\mathbf{u}]_{\times}^{6}=[\mathbf{u}]_{\times}^{2} \quad[\mathbf{u}]_{\times}^{7}=-[\mathbf{u}]_{\times} \cdots[u]x4=−[u]x2[u]x5=[u]×[u]×6=[u]×2[u]×7=−[u]×⋯因此可以将指数映射的泰勒展开式划分为以[u]×[\mathbf{u}]_{\times}[u]×和以[u]×2[\mathbf{u}]_{\times}^{2}[u]×2为基的两组，前面的系数分别是sin⁡ϕ\sin \phisinϕ和cos⁡ϕ\cos \phicosϕ的展开式，因此我们就获得了著名的Rodirigues公式：R=I+sin⁡ϕ[u]×+(1−cos⁡ϕ)[u]×2\mathbf{R}=\mathbf{I}+\sin \phi[\mathbf{u}]_{\times}+(1-\cos \phi)[\mathbf{u}]_{\times}^{2}R=I+sinϕ[u]×+(1−cosϕ)[u]×2有了指数映射我们逆推出对数映射Log⁡:SO(3)→R3;R↦Log⁡(R)=uϕ\operatorname{Log}: S O(3) \rightarrow \mathbb{R}^{3} ; \mathbf{R} \mapsto \operatorname{Log} (\mathbf{R})=\mathbf{u} \phiLog:SO(3)→R3;R↦Log(R)=uϕ，即ϕ=arccos⁡(trace⁡(R)−12)u=(R−R⊤)∨2sin⁡ϕ\begin{array}{l} \phi=\arccos \left(\frac{\operatorname{trace}(\mathbf{R})-1}{2}\right) \\ \mathbf{u}=\frac{\left(\mathbf{R}-\mathbf{R}^{\top}\right)^{\vee}}{2 \sin \phi} \end{array}ϕ=arccos(2trace(R)−1)u=2sinϕ(R−R⊤)∨这里我们再回过头来看下旋转矩阵对向量的操作x′=Rx\mathbf{x}^{\prime}=\mathbf{R} \mathbf{x}x′=Rx，我们用Rodirigues公式进行替换，有x′=Rx=(I+sin⁡ϕ[u]×+(1−cos⁡ϕ)[u]×2)x=x+sin⁡ϕ[u]×x+(1−cos⁡ϕ)[u]×2x=x+sin⁡ϕ(u×x)+(1−cos⁡ϕ)(uu⊤−I)x=x∥+x⊥+sin⁡ϕ(u×x)−(1−cos⁡ϕ)x⊥=x∥+(u×x)sin⁡ϕ+x⊥cos⁡ϕ\begin{aligned} \mathbf{x}^{\prime}=& \mathbf{R} \mathbf{x} \\ =&\left(\mathbf{I}+\sin \phi[\mathbf{u}]_{\times}+(1-\cos \phi)[\mathbf{u}]_{\times}^{2}\right) \mathbf{x} \\ =& \mathbf{x}+\sin \phi[\mathbf{u}]_{\times} \mathbf{x}+(1-\cos \phi)[\mathbf{u}]_{\times}^{2} \mathbf{x} \\ =& \mathbf{x}+\sin \phi(\mathbf{u} \times \mathbf{x})+(1-\cos \phi)\left(\mathbf{u} \mathbf{u}^{\top}-\mathbf{I}\right) \mathbf{x} \\ =& \mathbf{x}_{\|}+\mathbf{x}_{\perp}+\sin \phi(\mathbf{u} \times \mathbf{x})-(1-\cos \phi) \mathbf{x}_{\perp} \\ =& \mathbf{x}_{\|}+(\mathbf{u} \times \mathbf{x}) \sin \phi+\mathbf{x}_{\perp} \cos \phi \end{aligned}x′======Rx(I+sinϕ[u]×+(1−cosϕ)[u]×2)xx+sinϕ[u]×x+(1−cosϕ)[u]×2xx+sinϕ(u×x)+(1−cosϕ)(uu⊤−I)xx∥+x⊥+sinϕ(u×x)−(1−cosϕ)x⊥x∥+(u×x)sinϕ+x⊥cosϕ然后我们发现了什么！这前面定义的轴角对向量的旋转操作公式一模一样，那么说明ϕ≜ωΔt\boldsymbol{\phi} \triangleq \boldsymbol{\omega} \Delta tϕ≜ωΔt即旋转操作的轴角v\mathbf{v}v，ω\boldsymbol{\omega}ω即旋转角速度

2.3 四元数与轴角的关系

前面介绍旋转矩阵与轴角的关系，接下来按照同样的思路介绍四元数与轴角的关系，四元数对旋转的操作为：r(v)=q⊗v⊗q∗r(\mathbf{v})=\mathbf{q} \otimes \mathbf{v} \otimes \mathbf{q}^{*}r(v)=q⊗v⊗q∗根据旋转操作的性质，旋转操作不改变向量的长度∥q⊗v⊗q∗∥=∥q∥2∥v∥=∥v∥\left\|\mathbf{q} \otimes \mathbf{v} \otimes \mathbf{q}^{*}\right\|=\|\mathbf{q}\|^{2}\|\mathbf{v}\|=\|\mathbf{v}\|∥q⊗v⊗q∗∥=∥q∥2∥v∥=∥v∥，因此有q∗⊗q=1=q⊗q∗\mathbf{q}^{*} \otimes \mathbf{q}=1=\mathbf{q} \otimes \mathbf{q}^{*}q∗⊗q=1=q⊗q∗可以看出来，表示旋转的四元数都单位四元数，我们将这样的四元数构成空间定义为q∈S3q \in S^{3}q∈S3，同样我们进行微分操作d(q∗⊗q)dt=q˙∗⊗q+q∗⊗q˙=0\frac{d\left(\mathbf{q}^{*} \otimes \mathbf{q}\right)}{d t}=\dot{\mathbf{q}}^{*} \otimes \mathbf{q}+\mathbf{q}^{*} \otimes \dot{\mathbf{q}}=0dtd(q∗⊗q)=q˙∗⊗q+q∗⊗q˙=0然后可得q∗⊗q˙=−(q˙∗⊗q)=−(q∗⊗q˙)∗\mathbf{q}^{*} \otimes \dot{\mathbf{q}}=-\left(\dot{\mathbf{q}}^{*} \otimes \mathbf{q}\right)=-\left(\mathbf{q}^{*} \otimes \dot{\mathbf{q}}\right)^{*}q∗⊗q˙=−(q˙∗⊗q)=−(q∗⊗q˙)∗四元数与其负共轭相等，说明q∗⊗q˙\mathbf{q}^{*} \otimes \dot{\mathbf{q}}q∗⊗q˙是一个纯四元数，同理我们定义个纯四元数Ω∈Hp\Omega \in \mathbb{H}_{p}Ω∈Hp有q∗⊗q˙=Ω=[0Ω]∈Hp\mathbf{q}^{*} \otimes \dot{\mathbf{q}}=\Omega=\left[\begin{array}{l} 0 \\ \Omega \end{array}\right] \in \mathbb{H}_{p}q∗⊗q˙=Ω=[0Ω]∈Hp同样我们和时间ttt关联起来，然后就可以获得微分方程q(t)˙=q(t)⊗Ω\dot{\mathbf{q}(t)}=\mathbf{q}(t) \otimes \Omegaq(t)˙=q(t)⊗Ω，当q(0)=1\mathbf{q}(0)=1q(0)=1时，q˙(0)=Ω∈Hp\dot{\mathbf{q}}(0)=\Omega \in \mathbb{H}_{p}q˙(0)=Ω∈Hp，因此Hp\mathbb{H}_{p}Hp就是S3S^{3}S3的正切空间或者李代数，我们假设Ω\OmegaΩ是常数，我们就可以进一步积分有q(t)=q(0)⊗eΩΔt\mathbf{q}(t)=\mathbf{q}(0) \otimes e^{\Omega \Delta t}q(t)=q(0)⊗eΩΔt因为q(t)\mathbf{q}(t)q(t)和q(0)\mathbf{q}(0)q(0)都是单位四元数，因此eΩΔte^{\Omega \Delta t}eΩΔt也是单位四元数，我们定义V≜ΩΔt\mathbf{V} \triangleq \Omega \Delta tV≜ΩΔt，因此有q=eV\mathbf{q}=e^{\mathbf{V}}q=eV根据1.8节数四元数对数定义的介绍，V\mathbf{V}V是一个纯四元数，因此我们可以定义V=θu\mathbf{V}=\theta \mathbf{u}V=θu，然后我们定义v=ϕu\mathbf{v}=\phi \mathbf{u}v=ϕu,其中θ=ϕ/2\theta=\phi / 2θ=ϕ/2（和旋转矩阵一样，接下来会通过推到证明v\mathbf{v}v其实就是轴角），这里给出四元数指数映射关系这里定义的指数映射为Exp⁡(ϕ)≜exp⁡(ϕ/2)\operatorname{Exp}(\phi) \triangleq \exp (\phi / 2)Exp(ϕ)≜exp(ϕ/2)，我们对指数映射进行展开有q≜Exp⁡(ϕu)=eϕu/2=cos⁡ϕ2+usin⁡ϕ2=[cos⁡(ϕ/2)usin⁡(ϕ/2)]\mathbf{q} \triangleq \operatorname{Exp}(\phi \mathbf{u})=e^{\phi \mathbf{u} / 2}=\cos \frac{\phi}{2}+\mathbf{u} \sin \frac{\phi}{2}=\left[\begin{array}{c} \cos (\phi / 2) \\ \mathbf{u} \sin (\phi / 2) \end{array}\right]q≜Exp(ϕu)=eϕu/2=cos2ϕ+usin2ϕ=[cos(ϕ/2)usin(ϕ/2)]同理定义对数映射Log⁡:S3→R3;q↦Log⁡(q)=uϕ\operatorname{Log} : S^{3} \rightarrow \mathbb{R}^{3} ; \mathbf{q} \mapsto \operatorname{Log} (\mathbf{q})=\mathbf{u} \phiLog:S3→R3;q↦Log(q)=uϕ，其中ϕ=2arctan⁡(∥qv∥,qw)u=qv/∥qv∥\begin{array}{l} \phi=2 \arctan \left(\left\|\mathbf{q}_{v}\right\|, q_{w}\right) \\ \mathbf{u}=\mathbf{q}_{v} /\left\|\mathbf{q}_{v}\right\| \end{array}ϕ=2arctan(∥qv∥,qw)u=qv/∥qv∥最后我们回过头来看四元数对向量的旋转操作来证明四元数与轴角的关系x′=q⊗x⊗q∗=(cos⁡ϕ2+usin⁡ϕ2)⊗(0+x)⊗(cos⁡ϕ2−usin⁡ϕ2)=xcos⁡2ϕ2+(u⊗x−x⊗u)sin⁡ϕ2cos⁡ϕ2−u⊗x⊗usin⁡2ϕ2=xcos⁡2ϕ2+2(u×x)sin⁡ϕ2cos⁡ϕ2−(x(u⊤u)−2u(u⊤x))sin⁡2ϕ2=x(cos⁡2ϕ2−sin⁡2ϕ2)+(u×x)(2sin⁡ϕ2cos⁡ϕ2)+u(u⊤x)(2sin⁡2ϕ2)=xcos⁡ϕ+(u×x)sin⁡ϕ+u(u⊤x)(1−cos⁡ϕ)=(x−uu⊤x)cos⁡ϕ+(u×x)sin⁡ϕ+uu⊤x=x⊥cos⁡ϕ+(u×x)sin⁡ϕ+x∥\begin{aligned} \mathbf{x}^{\prime}=& \mathbf{q} \otimes \mathbf{x} \otimes \mathbf{q}^{*} \\ =&\left(\cos \frac{\phi}{2}+\mathbf{u} \sin \frac{\phi}{2}\right) \otimes(0+\mathbf{x}) \otimes\left(\cos \frac{\phi}{2}-\mathbf{u } \sin \frac{\phi}{2}\right) \\ =& \mathbf{x} \cos ^{2} \frac{\phi}{2}+(\mathbf{u} \otimes \mathbf{x}-\mathbf{x} \otimes \mathbf{u}) \sin \frac{\phi}{2} \cos \frac{\phi}{2}-\mathbf{u} \otimes \mathbf{x} \otimes \mathbf{u} \sin ^{2} \frac{\phi}{2} \\ =& \mathbf{x} \cos ^{2} \frac{\phi}{2}+2(\mathbf{u} \times \mathbf{x}) \sin \frac{\phi}{2} \cos \frac{\phi}{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{\phi}{2} \\ =& \mathbf{x}\left(\cos ^{2} \frac{\phi}{2}-\sin ^{2} \frac{\phi}{2}\right)+(\mathbf{u} \times \mathbf{x})\left(2 \sin \frac{\phi}{2} \cos \frac{\phi}{2}\right)+\mathbf{u}\left(\mathbf{u}^{\top} \mathbf{x}\right)\left(2 \sin ^{2} \frac{\phi}{2}\right) \\ =& \mathbf{x} \cos \phi+(\mathbf{u} \times \mathbf{x}) \sin \phi+\mathbf{u}\left(\mathbf{u}^{\top} \mathbf{x}\right)(1-\cos \phi) \\ =&\left(\mathbf{x}-\mathbf{u} \mathbf{u}^{\top} \mathbf{x}\right) \cos \phi+(\mathbf{u} \times \mathbf{x}) \sin \phi+\mathbf{u} \mathbf{u}^{\top} \mathbf{x} \\ =& \mathbf{x}_{\perp} \cos \phi+(\mathbf{u} \times \mathbf{x}) \sin \phi+\mathbf{x}_{\|} \end{aligned}x′========q⊗x⊗q∗(cos2ϕ+usin2ϕ)⊗(0+x)⊗(cos2ϕ−usin2ϕ)xcos22ϕ+(u⊗x−x⊗u)sin2ϕcos2ϕ−u⊗x⊗usin22ϕxcos22ϕ+2(u×x)sin2ϕcos2ϕ−(x(u⊤u)−2u(u⊤x))sin22ϕx(cos22ϕ−sin22ϕ)+(u×x)(2sin2ϕcos2ϕ)+u(u⊤x)(2sin22ϕ)xcosϕ+(u×x)sinϕ+u(u⊤x)(1−cosϕ)(x−uu⊤x)cosϕ+(u×x)sinϕ+uu⊤xx⊥cosϕ+(u×x)sinϕ+x∥可以看出来，前面我们定义的v=ϕu\mathbf{v}=\phi \mathbf{u}v=ϕu其实就是轴角，而四元数代表的角度θ\thetaθ其实为实际旋转角度的一半，这也就是论文中提到的四元素的double cover性质。

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

我们已知q⊗x⊗q∗=Rx\mathbf{q} \otimes \mathbf{x} \otimes \mathbf{q}^{*}=\mathbf{R} \mathbf{x}q⊗x⊗q∗=Rx令x\mathbf{x}x为单位向量，那么就可以获得R=[qw2+qx2−qy2−qz22(qxqy−qwqz)2(qxqz+qwqy)2(qxqy+qwqz)qw2−qx2+qy2−qz22(qyqz−qwqx)2(qxqz−qwqy)2(qyqz+qwqx)qw2−qx2−qy2+qz2]\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]R=⎣⎡qw2+qx2−qy2−qz22(qxqy+qwqz)2(qxqz−qwqy)2(qxqy−qwqz)qw2−qx2+qy2−qz22(qyqz+qwqx)2(qxqz+qwqy)2(qyqz−qwqx)qw2−qx2−qy2+qz2⎦⎤根据四元数的性质我们还可以获得另外一种表达形式q⊗x⊗q∗=[q∗]R[q]L[0x]=[0Rx]\mathbf{q} \otimes \mathbf{x} \otimes \mathbf{q}^{*}=\left[\begin{array}{l} \mathbf{q}^{*} \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]q⊗x⊗q∗=[q∗]R[q]L[0x]=[0Rx]那么我们可以获得R=(qw2−qv⊤qv)I+2qvqv⊤+2qw[qv]×\mathbf{R}=\left(q_{w}^{2}-\mathbf{q}_{v}^{\top} \mathbf{q}_{v}\right) \mathbf{I}+2 \mathbf{q}_{v} \mathbf{q}_{v}^{\top}+2 q_{w}\left[\mathbf{q}_{v}\right]_{\times}R=(qw2−qv⊤qv)I+2qvqv⊤+2qw[qv]×以上两种都是从四元数到旋转矩阵的关系，从旋转矩阵获得四元数可以根据上面第一种表达式直接逆推获得qw=(tr⁡(R)+12qx=R32−R234qwqy=R13−R314qwqz=R21−R124qw\begin{aligned} q_w &=\frac{\sqrt{(\operatorname{tr}(\mathbf{R})+1}}{2} \\ q_x &=\frac{\mathbf{R}_{32}-\mathbf{R}_{23}}{4 q_w} \\ q_y &=\frac{\mathbf{R}_{13}-\mathbf{R}_{31}}{4 q_w} \\ q_z &=\frac{\mathbf{R}_{21}-\mathbf{R}_{12}}{4 q_w} \end{aligned}qwqxqyqz=2(tr(R)+1=4qwR32−R23=4qwR13−R31=4qwR21−R12

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

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

3.1 四元数的雅克比

参考文献中给出了三种类型的雅克比
第一种是向量对向量的雅克比：∂(q⊗a⊗q∗)∂a=∂(Ra)∂a=R\frac{\partial(\mathbf{q} \otimes \mathbf{a} \otimes \mathbf{q} *)}{\partial \mathbf{a}}=\frac{\partial(\mathbf{R} \mathbf{a})}{\partial \mathbf{a}}=\mathbf{R}∂a∂(q⊗a⊗q∗)=∂a∂(Ra)=R第二种是向量对四元数的雅克比（推导过程参考文献）：∂(q⊗a⊗q∗)∂q=2[wa+v×a∣v⊤aI3+va⊤−av⊤−w[a]×]∈R3×4\frac{\partial\left(\mathbf{q} \otimes \mathbf{a} \otimes \mathbf{q}^{*}\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}∂q∂(q⊗a⊗q∗)=2[wa+v×a∣v⊤aI3+va⊤−av⊤−w[a]×]∈R3×4∂(q⊗a⊗q∗)∂q=2[wa+v×a∣v⊤aI3+va⊤−av⊤−w[a]×]∈R3×4\frac{\partial\left(\mathbf{q} \otimes \mathbf{a} \otimes \mathbf{q}^{*}\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}∂q∂(q⊗a⊗q∗)=2[wa+v×a∣v⊤aI3+va⊤−av⊤−w[a]×]∈R3×4第三种是向量对轴角的雅克比（推导过程参考文献）：∂(q⊗a⊗q∗)∂δθ=∂(Ra)∂δθ=−R{θ}[a]×Jr(θ)\frac{\partial\left(\mathbf{q} \otimes \mathbf{a} \otimes \mathbf{q}^{*}\right)}{\partial \delta \boldsymbol{\theta}}=\frac{\partial(\mathbf{R} \mathbf{a})}{\partial \delta \boldsymbol{\theta}}=-\mathbf{R}\{\boldsymbol{\theta}\}[\mathbf{a}]_{\times} \mathbf{J}_{r}(\boldsymbol{\theta})∂δθ∂(q⊗a⊗q∗)=∂δθ∂(Ra)=−R{θ}[a]×Jr(θ)第三种求导方式其实就是李代数求导方式，结合《视觉SLAM十四讲》中的知识，这种求导方式其实是不友好的，通常采用扰动模型进行，这篇文献中没有涉及，这里也不进一步进行展开了。

3.2 四元数的微分

四元数的微分在推导IMU的运动方程时将会用到，四元数的微分定义如下：q˙≜lim⁡Δt→0q(t+Δt)−q(t)Δt=lim⁡Δt→0q⊗ΔqL−qΔt=lim⁡Δt→0q⊗([1ΔϕL/2]−[10])Δt=lim⁡Δt→0q⊗[0ΔϕL/2]Δt=12q⊗[0ωL]\begin{aligned} \dot{\mathbf{q}} & \triangleq \lim _{\Delta t \rightarrow 0} \frac{\mathbf{q}(t+\Delta t)-\mathbf{q}(t)}{\Delta t} \\ &=\lim _{\Delta t \rightarrow 0} \frac{\mathbf{q} \otimes \Delta \mathbf{q}_{\mathcal{L}}-\mathbf{q}}{\Delta t} \\& =\lim _{\Delta t \rightarrow 0} \frac{\mathbf{q} \otimes\left(\left[\begin{array}{c} 1 \\ \Delta \boldsymbol{\phi}_{\mathcal{L}} / 2 \end{array}\right]-\left[\begin{array}{l} 1 \\ \mathbf{0} \end{array}\right]\right)}{\Delta t} \\& =\lim _{\Delta t \rightarrow 0} \frac{\mathbf{q} \otimes\left[\begin{array}{c} 0 \\ \Delta \boldsymbol{\phi}_{\mathcal{L}} / 2 \end{array}\right]}{\Delta t} \\& =\frac{1}{2} \mathbf{q} \otimes\left[\begin{array}{c} 0 \\ \boldsymbol{\omega}_{\mathcal{L}} \end{array}\right] \end{aligned} q˙≜Δt→0limΔtq(t+Δt)−q(t)=Δt→0limΔtq⊗ΔqL−q=Δt→0limΔtq⊗([1ΔϕL/2]−[10])=Δt→0limΔtq⊗[0ΔϕL/2]=21q⊗[0ωL]其中，ΔqL\Delta \mathbf{q}_\mathcal{L}ΔqL为一个局部扰动，根据四元数的指数映射有ΔqL≜Exp⁡(ΔϕLu)=eΔϕLu/2=cos⁡ΔϕL2+usin⁡ΔϕL2=[cos⁡(ΔϕL/2)usin⁡(ΔϕL/2)]\Delta \mathbf{q}_\mathcal{L} \triangleq \operatorname{Exp}(\Delta \phi_\mathcal{L} \mathbf{u})=e^{\Delta \phi_\mathcal{L} \mathbf{u} / 2}=\cos \frac{\Delta \phi_\mathcal{L}}{2}+\mathbf{u} \sin \frac{\Delta \phi_\mathcal{L}}{2}=\left[\begin{array}{c} \cos (\Delta \phi_\mathcal{L} / 2) \\ \mathbf{u} \sin (\Delta \phi_\mathcal{L}/ 2) \end{array}\right]ΔqL≜Exp(ΔϕLu)=eΔϕLu/2=cos2ΔϕL+usin2ΔϕL=[cos(ΔϕL/2)usin(ΔϕL/2)]当ΔϕL\Delta \phi_\mathcal{L}ΔϕL较小时，有如下近似：[cos⁡(ΔϕL/2)usin⁡(ΔϕL/2)]=[1ΔϕL/2]\left[\begin{array}{c} \cos \left(\Delta \phi_{\mathcal{L}} / 2\right) \\ \mathbf{u} \sin \left(\Delta \phi_{\mathcal{L}} / 2\right) \end{array}\right]=\left[\begin{array}{c} 1 \\ \Delta \phi_{\mathcal{L}} / 2 \end{array}\right][cos(ΔϕL/2)usin(ΔϕL/2)]=[1ΔϕL/2]根据四元数乘法规则，我们可以定义Ω(ω)≜[ω]R=[0−ω⊤ω−[ω]×]=[0−ωx−ωy−ωzωx0ωz−ωyωy−ωz0ωxωzωy−ωx0]\boldsymbol{\Omega}(\boldsymbol{\omega}) \triangleq[\boldsymbol{\omega}]_{R}=\left[\begin{array}{cc} 0 & -\boldsymbol{\omega}^{\top} \\ \boldsymbol{\omega} & -[\boldsymbol{\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]Ω(ω)≜[ω]R=[0ω−ω⊤−[ω]×]=⎣⎢⎢⎡0ωxωyωz−ωx0−ωzωy−ωyωz0−ωx−ωz−ωyωx0⎦⎥⎥⎤那么四元数的微分即q˙=12Ω(ωL)q=12q⊗ωL\dot{\mathbf{q}}=\frac{1}{2} \Omega\left(\boldsymbol{\omega}_{\mathcal{L}}\right) \mathbf{q}=\frac{1}{2} \mathbf{q} \otimes \boldsymbol{\omega}_{\mathcal{L}}q˙=21Ω(ωL)q=21q⊗ωL如果转化成旋转矩阵是R˙=R[ωL]\dot{\mathbf{R}}=\mathbf{R}\left[\boldsymbol{\omega}_{\mathcal{L}}\right]_{ }R˙=R[ωL]

3.3 四元数的积分

四元数的积分在运动状态的积分时会用到，这里只介绍下最为简单的零阶积分，在我们利用IMU进行测量是实际上获得是如下微分方程q˙(t)=12q(t)⊗ω(t)\dot{\mathbf{q}}(t)=\frac{1}{2} \mathbf{q}(t) \otimes \boldsymbol{\omega}(t)q˙(t)=21q(t)⊗ω(t)根据泰勒展开qn+1=qn+q˙nΔt+12!q¨nΔt2+13!q¨nΔt3+14!q¨nΔt4+⋯\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}+\cdotsqn+1=qn+q˙nΔt+2!1q¨nΔt2+3!1q¨nΔt3+4!1q¨nΔt4+⋯由于ω¨=0\ddot{\omega}=0ω¨=0，因此有q˙n=12qnωnq¨n=122qnωn2+12qnω˙q¨n=123qnωn3+14qnω˙ωn+12qωnω˙qn(i≥4)=12iqnωni+⋯\begin{aligned} \dot{\mathbf{q}}_{n} &=\frac{1}{2} \mathbf{q}_{n} \boldsymbol{\omega}_{n} \\ \ddot{\mathbf{q}}_{n} &=\frac{1}{2^{2}} \mathbf{q}_{n} \boldsymbol{\omega}_{n}^{2}+\frac{1}{2} \mathbf{q}_{n} \dot{\boldsymbol{\omega}} \\ \ddot{\mathbf{q}}_{n} &=\frac{1}{2^{3}} \mathbf{q}_{n} \boldsymbol{\omega}_{n}^{3}+\frac{1}{4} \mathbf{q}_{n} \dot{\boldsymbol{\omega}} \boldsymbol{\omega}_{n}+\frac{1}{2} \mathbf{q} \boldsymbol{\omega}_{n} \dot{\boldsymbol{\omega}} \\ \mathbf{q}_{n}^{(i \geq 4)} &=\frac{1}{2^{i}} \mathbf{q}_{n} \boldsymbol{\omega}_{n}^{i}+\cdots \end{aligned}q˙nq¨nq¨nqn(i≥4)=21qnωn=221qnωn2+21qnω˙=231qnωn3+41qnω˙ωn+21qωnω˙=2i1qnωni+⋯当角速度ω(t)\boldsymbol{\omega}(t)ω(t)为常数时，上面的泰勒展开式进一步退化为qn+1=qn⊗(1+12ωnΔt+12!(12ωnΔt)2+13!(12ωnΔt)3+⋯)\mathbf{q}_{n+1}=\mathbf{q}_{n} \otimes\left(1+\frac{1}{2} \boldsymbol{\omega}_{n} \Delta t+\frac{1}{2 !}\left(\frac{1}{2} \boldsymbol{\omega}_{n} \Delta t\right)^{2}+\frac{1}{3 !}\left(\frac{1}{2} \boldsymbol{\omega}_{n} \Delta t\right)^{3}+\cdots\right)qn+1=qn⊗(1+21ωnΔt+2!1(21ωnΔt)2+3!1(21ωnΔt)3+⋯)右测的泰勒展开式其实就是eωnΔt/2e^{\omega_{n} \Delta t / 2}eωnΔt/2，并且根据四元数的指数映射有eωΔt/2=Exp⁡(ωΔt)=q{ωΔt}=[cos⁡(∥ω∥Δt/2)ω∥ω∥sin⁡(∥ω∥Δt/2)]e^{\omega \Delta t / 2}=\operatorname{Exp}(\boldsymbol{\omega} \Delta t)=\mathbf{q}\{\boldsymbol{\omega} \Delta t\}=\left[\begin{array}{c} \cos (\|\boldsymbol{\omega}\| \Delta t / 2) \\ \frac{\omega}{\|\boldsymbol{\omega}\|} \sin (\|\boldsymbol{\omega}\| \Delta t / 2) \end{array}\right]eωΔt/2=Exp(ωΔt)=q{ωΔt}=[cos(∥ω∥Δt/2)∥ω∥ωsin(∥ω∥Δt/2)]对于前向积分有qn+1=qn⊗q{ωnΔt}\mathbf{q}_{n+1}=\mathbf{q}_{n} \otimes \mathbf{q}\left\{\boldsymbol{\omega}_{n} \Delta t\right\}qn+1=qn⊗q{ωnΔt}对于后向积分有qn=qn−1⊗q{ωnΔt}\mathbf{q}_{n}=\mathbf{q}_{n-1} \otimes \mathbf{q}\left\{\boldsymbol{\omega}_{n} \Delta t\right\}qn=qn−1⊗q{ωnΔt}对于中值积分有qn+1=qn⊗q{ω‾Δt}\mathbf{q}_{n+1}=\mathbf{q}_{n} \otimes \mathbf{q}\{\overline{\boldsymbol{\omega}} \Delta t\}qn+1=qn⊗q{ωΔt}

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

此外，对其他SLAM算法感兴趣的同学可以看考我的博客SLAM算法总结——经典SLAM算法框架总结