一、Mahony姿态解算——坐标系变换

坐标系

在飞行器姿态解算中，通常以地球坐标系（E系）为绝对坐标系，以机体坐标系（B系）为相对坐标系，并通常设初始状态时E系和B系重合。

坐标系变换

我们将空间中某一向量从一个坐标系，通过坐标系变换，映射到另一个坐标系的过程，称做坐标系变换。假设有两个同原坐标系$A$和$B$，其中坐标系$B$是$A$绕$Z$轴旋转$90°$得到。此时有一个位于坐标系A中的向量 a A = [ 1 0 0 ] a_{A}=\begin{bmatrix}1 \\ 0 \\ 0 \end{bmatrix} aA​= ​100​ ​，若我们要得到该向量在坐标系$B$中的坐标值。由于两个坐标系的关系简单，因此可以很快得到 a B = [ 0 − 1 0 ] a_{B}=\begin{bmatrix}0 \\ -1 \\ 0 \end{bmatrix} aB​= ​0−10​ ​。那么，就存在一个变换矩阵$C_A^B$满足关系：
$$a_B=C_A^B{a}_A$$
这个矩阵$C_A^B$就称作从$A$系到$B$的变换矩阵。

基于欧拉角的坐标变换

欧拉角的坐标变换本质是，以绕地球坐标系三个坐标轴的单独旋转，合成机体坐标系的复合旋转。
设绕$Z$轴旋转$\alpha °$,设绕$Y$轴旋转$\beta °$，设绕$X$轴旋转$\gamma °$。
以$Z$轴旋转为例子。

图片引自《惯性导航》，作者秦永元，侵删
根据几何关系，可以得到
$$\begin{gathered} r_{x2}=OA+AB+BC \\ {r}_{x2}=ODcos\alpha +BDsin\alpha +BFsin\alpha \\ {r}_{x2}=ODcos\alpha +DFsin\alpha \\ {r}_{x2}=r_{x1}cos\alpha +r_{y1}sin\alpha \end{gathered}$$
$$\begin{gathered} r_{y2}=DE-DA \\ {r}_{y2}=DFcos\alpha -ODsin\alpha \\ {r}_{y2}=r_{y1}cos\alpha -r_{x1}sin\alpha \end{gathered}$$
以矩阵形式表达
$$\left[\begin{array}{c}{r}_{x2}\\ r_{y2}\end{array}\right]=\left[\begin{array}{cc}cos\alpha & sin\alpha \\ -sin\alpha & cos\alpha \end{array}\right]\left[\begin{array}{c}{r}_{x1}\\ r_{y1}\end{array}\right]$$
但是对于一个三维向量来说，绕$Z$轴旋转并不改变他的$z$坐标，因此完整的变换过程应该为
[ r x 2 r y 2 r z 2 ] = [ c o s α s i n α 0 − s i n α c o s α 0 0 0 1 ]   [ r x 1 r y 1 r z 1 ] \begin{bmatrix} r_{x2} \\ r_{y2}\\ r_{z2}\end{bmatrix}= \begin{bmatrix} cos\alpha & sin\alpha &0\\ -sin\alpha &cos\alpha &0 \\ 0 &0 &1\end{bmatrix}\ \begin{bmatrix} r_{x1} \\ r_{y1}\\r_{z1}\end{bmatrix} ​rx2​ry2​rz2​​ ​= ​cosα−sinα0​sinαcosα0​001​ ​  ​rx1​ry1​rz1​​ ​
因此，可以得到旋转$Z$轴时的旋转矩阵为
r o t ( Z , α ) = [ c o s α s i n α 0 − s i n α c o s α 0 0 0 1 ] rot(Z,\alpha)= \begin{bmatrix} cos\alpha & sin\alpha &0\\ -sin\alpha &cos\alpha &0 \\ 0 &0 &1\end{bmatrix} rot(Z,α)= ​cosα−sinα0​sinαcosα0​001​ ​
同理，可以推导得到绕$Y$轴和$X$轴的旋转矩阵
r o t ( Y , β ) = [ c o s β 0 − s i n β 0 1 0 s i n β 0 c o s β ] rot(Y,\beta)= \begin{bmatrix} cos\beta& 0&-sin\beta\\ 0 &1 &0 \\ sin\beta &0 &cos\beta\end{bmatrix} rot(Y,β)= ​cosβ0sinβ​010​−sinβ0cosβ​ ​
r o t ( X , γ ) = [ 1 0 0 0 c o s γ s i n γ 0 − s i n γ c o s γ ] rot(X,\gamma)= \begin{bmatrix} 1 &0 &0\\ 0&cos\gamma& sin\gamma\\ 0 &-sin\gamma &cos\gamma\end{bmatrix} rot(X,γ)= ​100​0cosγ−sinγ​0sinγcosγ​ ​
以$Z-Y-X$的顺序合成复合旋转，可以得到：
C E B = r o t ( X , γ ) r o t ( Y , β ) r o t ( Z , α ) = [ c o s β c o s α c o s β s i n α − s i n β s i n γ s i n β c o s α − s i n α c o s γ s i n α s i n β s i n γ + c o s γ c o s α s i n γ c o s β c o s α c o s γ s i n β + s i n γ s i n α s i n α c o s γ s i n β − s i n γ c o s α c o s γ c o s β ] C_{E}^{B}=rot(X,\gamma)rot(Y,\beta)rot(Z,\alpha)= \begin{bmatrix} cos\beta cos\alpha &cos\beta sin\alpha &-sin\beta\\ sin\gamma sin\beta cos\alpha -sin\alpha cos\gamma&sin\alpha sin\beta sin\gamma+cos\gamma cos\alpha & sin\gamma cos\beta\\ cos\alpha cos\gamma sin\beta+sin\gamma sin\alpha &sin\alpha cos\gamma sin\beta-sin\gamma cos\alpha &cos\gamma cos\beta \end{bmatrix} CEB​=rot(X,γ)rot(Y,β)rot(Z,α)= ​cosβcosαsinγsinβcosα−sinαcosγcosαcosγsinβ+sinγsinα​cosβsinαsinαsinβsinγ+cosγcosαsinαcosγsinβ−sinγcosα​−sinβsinγcosβcosγcosβ​ ​
这里要稍微提一下，从单轴的旋转矩阵可以看出，每个矩阵都是正交矩阵，因此变换矩阵$C_E^B$也是正交矩阵，即满足关系
$$(C_E^B{)}^T=(C_E^B{)}^{-}=C_B^E$$
即只要知道其中一个变换矩阵就可以实现双向变换过程。
但是由于欧拉法的变换是通过合成单独旋转得到的，就存在一个万向锁的问题(Gambal lock)，有兴趣的可以自行查找资料，此处不作赘述。

基于四元数的坐标变换

关于四元数想更直观系统了解的可以参考这个视频

对比欧拉角的坐标变换，四元数的坐标变换将整个坐标系的复合旋转，直接看作是坐标系绕某一过原点的轴旋转得到的结果。而这种旋转可以用一个单位四元数来描述，即
$$Q=cos\frac{\theta }{2}+(li+mj+nk)sin\frac{\theta }{2}$$
其中$i$、$j$、$k$ 是地球坐标系的基向量，并且满足关系$l^2+m^2+n^2=1$，$\theta$是坐标系绕该轴所旋转的角度。
令 u ⃗ = [ l m n ] \vec{u}=\begin{bmatrix} l\\m\\n\end{bmatrix} u = ​lmn​ ​

图片引自《惯性导航》，作者秦永元，侵删
向量$r$在机体坐标系中相对静止，但随着机体坐标系的旋转，其旋转后的向量会在地球坐标系中发生变化。我们把变化后的向量定义为$r^{{}^{\prime }}$。通过图中的几何关系可以得到。
$$\begin{gathered} O{O}^{{}^{\prime }}=(r\cdot u)u \\ {O}^{{}^{\prime }}A=r-(r\cdot u)u \\ {O}^{{}^{\prime }}B=u\times O^{{}^{\prime }}A=u\times (r-(r\cdot u)u)=u\times r-u\times (r\cdot u)u=u\times r \\ {O}^{{}^{\prime }}{A}^{{}^{\prime }}=O^{{}^{\prime }}Acos\theta +O^{{}^{\prime }}Bsin\theta =(r-(r\cdot u)u)cos\theta +(u\times r)sin\theta \\ =rcos\theta -(r\cdot u)ucos\theta +(u\times r)sin\theta \\ O{A}^{{}^{\prime }}=O{O}^{{}^{\prime }}+O^{{}^{\prime }}{A}^{{}^{\prime }}=(r\cdot u)u+rcos\theta -(r\cdot u)ucos\theta +(u\times r)sin\theta \\ =(1-cos\theta )(r\cdot u)u+rcos\theta +(u\times r)sin\theta \end{gathered}$$
又对于向量的叉乘公式，有矢量三重积
$$a\times (b\times c)=(a\cdot c)b-(a\cdot b)c$$

简单证明一下
$b$和$c$叉乘得到垂直于$bc$平面的向量，因此当$a$和该向量进行叉乘时，得到的向量肯定位于$bc$平面，就满足关系
$$a\times (b\times c)=mb+nc$$
其中$m$和$n$为常数。又向量$a$和向量$mb+nc$垂直，则满足关系
$$a\cdot (mb+cn)=mab+nac=0$$
为了使等式成立，可以得到
$$m=ac,n=-ab$$
得证，此证明方法也称为流氓证法\doge

则令$c=r$，$a=b=u$，可以得到
$$u\times (u\times r)=(u\cdot r)u-(u\cdot u)r=(u\cdot r)u-r$$
则可以得到
$$(u\cdot r)u=u\times (u\times r)+r$$
则进一步推导得到
$$\begin{gathered} O{A}^{{}^{\prime }}=(1-cos\theta )(r\cdot u)u+rcos\theta +(u\times r)sin\theta \\ =(1-cos\theta )(u\times (u\times r)+r)+rcos\theta +(u\times r)sin\theta \\ =(1-cos\theta )(u\times (u\times r))+r+(u\times r)sin\theta \end{gathered}$$

这里要插一个补充的内容
对于叉乘来说，假设有两个任意向量 A ⃗ = [ a 1 a 2 a 3 ] \vec{A}=\begin{bmatrix} a1 \\ a2 \\ a3 \end{bmatrix} A = ​a1a2a3​ ​和 B ⃗ = [ b 1 b 2 b 3 ] \vec{B}=\begin{bmatrix} b1 \\ b2 \\ b3 \end{bmatrix} B = ​b1b2b3​ ​。 那么两个向量的叉乘可以写成
A ⃗ × B ⃗ = ∣ i ⃗ a 1 b 1 j ⃗ a 2 b 2 k ⃗ a 3 b 3 ∣ = [ a 2 b 3 − b 2 a 3 a 3 b 1 − a 1 b 3 a 1 b 2 − a 2 b 1 ] = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] [ b 1 b 2 b 3 ] = C ⋅ B ⃗ \vec{A}\times\vec{B}=\begin{vmatrix} \vec{i}&a1 & b1\\ \vec{j}& a2 & b2\\ \vec{k}& a3 &b3 \end{vmatrix}=\begin{bmatrix} a2b3-b2a3\\a3b1-a1b3\\a1b2-a2b 1\end{bmatrix} =\begin{bmatrix} 0&-a3&a2\\a3&0&-a1\\-a2&a1&0\end{bmatrix}\begin{bmatrix} b1 \\ b2 \\ b3 \end{bmatrix}=C\cdot\vec{B} A ×B = ​i j ​k ​a1a2a3​b1b2b3​ ​= ​a2b3−b2a3a3b1−a1b3a1b2−a2b1​ ​= ​0a3−a2​−a30a1​a2−a10​ ​ ​b1b2b3​ ​=C⋅B
所以可以把叉乘理解为对叉乘右侧的一次坐标变换。

令$u\times X=C\cdot X$，其中$u$和 X ⃗ = [ x 1 x 2 x 3 ] \vec{X}=\begin{bmatrix}x1\\x2\\x3\end{bmatrix} X = ​x1x2x3​ ​为任意向量，$C$为等效矩阵。
[ l m n ] × [ x 1 x 2 x 3 ] = [ − n x 2 + m x 3 n x 1 − l x 3 − m x 1 + l x 2 ] = [ 0 − n m n 0 − l − m l 0 ] [ x 1 x 2 x 3 ] \begin{bmatrix}l\\m\\n\end{bmatrix}\times\begin{bmatrix}x1\\x2\\x3\end{bmatrix}=\begin{bmatrix}-nx2+mx3\\nx1-lx3\\-mx1+lx2\end{bmatrix}=\begin{bmatrix}0&-n&m\\n&0&-l\\-m&l&0\end{bmatrix}\begin{bmatrix}x1\\x2\\x3\end{bmatrix} ​lmn​ ​× ​x1x2x3​ ​= ​−nx2+mx3nx1−lx3−mx1+lx2​ ​= ​0n−m​−n0l​m−l0​ ​ ​x1x2x3​ ​
故 C = [ 0 − n m n 0 − l − m l 0 ] C=\begin{bmatrix}0&-n&m\\n&0&-l\\-m&l&0\end{bmatrix} C= ​0n−m​−n0l​m−l0​ ​
那么可以继续推导得到
$$O{A}^{{}^{\prime }}=r^{{}^{\prime }}=(1-cos\theta )C\cdot (C\cdot r)+r+C\cdot rsin\theta =(1-cos\theta )C^2\cdot r+r+Csin\theta \cdot r$$
整理得到
$$r^{{}^{\prime }}=(I+(1-cos\theta )C^2+Csin\theta )r$$
利用三角函数的半角公式和倍角公式可以得到
$$\begin{gathered} 1-cos\theta =2si{n}^2\frac{\theta }{2} \\ sin\theta =2sin\frac{\theta }{2}cos\frac{\theta }{2} \end{gathered}$$
代入后可以得到
$$r^{{}^{\prime }}=(I+2si{n}^2\frac{\theta }{2}{C}^2+2sin\frac{\theta }{2}cos\frac{\theta }{2}C)r$$

写到这里发现忽略了一个点的描述。
在一开始的时候我们提到，初始时刻未发生旋转时，我们假设机体坐标系和地球坐标系重合，因此空间中的任意向量无论在机体坐标系还是地球坐标系都是相同的坐标。因此，可以把向量$r$看作是初始时刻的向量，$r^{{}^{\prime }}$是旋转后映射在地球坐标系的新坐标值（因为向量是在机体坐标系中固定静止的）。也就是说，$r$既是初始时刻向量在地球坐标系的坐标，也是向量在机体坐标系从始至终的坐标。那么可以上面的变换可以理解为
$$\begin{gathered} r^{{}^{\prime }}=Dr=D{r}^B \\ {r}^E=D_B^E\cdot r^B \end{gathered}$$
包括上面提到的欧拉角的坐标变换也是基于这个前提进行的。可以回头看思考以下。

那么从机体坐标系到地球坐标系的变换矩阵$D_B^E$表达式为
D B E = [ 1 0 0 0 1 0 0 0 1 ] + 2 s i n 2 θ 2 [ 0 − n m n 0 − l − m l 0 ] [ 0 − n m n 0 − l − m l 0 ] + 2 s i n θ 2 c o s θ 2 [ 0 − n m n 0 − l − m l 0 ] = [ 1 0 0 0 1 0 0 0 1 ] + 2 s i n 2 θ 2 [ − n 2 − m 2 l m l n l m − n 2 − l 2 m n l n m n − m 2 − l 2 ] + 2 s i n θ 2 c o s θ 2 [ 0 − n m n 0 − l − m l 0 ] = [ 1 − 2 s i n 2 θ 2 n 2 − 2 s i n 2 θ 2 m 2 2 s i n 2 θ 2 l m − 2 s i n θ 2 c o s θ 2 n 2 s i n 2 θ 2 l n + 2 s i n θ 2 c o s θ 2 m 2 s i n 2 θ 2 l m + 2 s i n θ 2 c o s θ 2 n 1 − 2 s i n 2 θ 2 n 2 − 2 s i n 2 θ 2 l 2 2 s i n 2 θ 2 m n − 2 s i n θ 2 c o s θ 2 l 2 s i n 2 θ 2 l n − 2 s i n θ 2 c o s θ 2 m 2 s i n 2 θ 2 m n + 2 s i n θ 2 c o s θ 2 l 1 − 2 s i n 2 θ 2 m 2 − 2 s i n 2 θ 2 l 2 ] D_{B}^{E}=\begin{bmatrix}1&0&0\\0&1&0\\0&0&1\end{bmatrix}+2sin^{2}\frac{\theta}{2}\begin{bmatrix}0&-n&m\\n&0&-l\\-m&l&0\end{bmatrix}\begin{bmatrix}0&-n&m\\n&0&-l\\-m&l&0\end{bmatrix}+2sin\frac{\theta}{2}cos\frac{\theta}{2}\begin{bmatrix}0&-n&m\\n&0&-l\\-m&l&0\end{bmatrix} \\ =\begin{bmatrix}1&0&0\\0&1&0\\0&0&1\end{bmatrix}+2sin^{2}\frac{\theta}{2}\begin{bmatrix}-n^{2}-m^{2}&lm&ln\\lm&-n^{2}-l^{2}&mn\\ln&mn&-m^{2}-l^{2}\end{bmatrix}+2sin\frac{\theta}{2}cos\frac{\theta}{2}\begin{bmatrix}0&-n&m\\n&0&-l\\-m&l&0\end{bmatrix}\\ =\begin{bmatrix}1-2sin^{2}\frac{\theta}{2}n^{2}-2sin^{2}\frac{\theta}{2}m^{2} &2sin^{2}\frac{\theta}{2}lm-2sin\frac{\theta}{2}cos\frac{\theta}{2}n &2sin^{2}\frac{\theta}{2}ln+2sin\frac{\theta}{2}cos\frac{\theta}{2}m \\2sin^{2}\frac{\theta}{2}lm+2sin\frac{\theta}{2}cos\frac{\theta}{2}n &1-2sin^{2}\frac{\theta}{2}n^{2}-2sin^{2}\frac{\theta}{2}l^{2} &2sin^{2}\frac{\theta}{2}mn-2sin\frac{\theta}{2}cos\frac{\theta}{2}l \\2sin^{2}\frac{\theta}{2}ln-2sin\frac{\theta}{2}cos\frac{\theta}{2}m &2sin^{2}\frac{\theta}{2}mn+2sin\frac{\theta}{2}cos\frac{\theta}{2}l &1-2sin^{2}\frac{\theta}{2}m^{2}-2sin^{2}\frac{\theta}{2}l^{2} \end{bmatrix} DBE​= ​100​010​001​ ​+2sin22θ​ ​0n−m​−n0l​m−l0​ ​ ​0n−m​−n0l​m−l0​ ​+2sin2θ​cos2θ​ ​0n−m​−n0l​m−l0​ ​= ​100​010​001​ ​+2sin22θ​ ​−n2−m2lmln​lm−n2−l2mn​lnmn−m2−l2​ ​+2sin2θ​cos2θ​ ​0n−m​−n0l​m−l0​ ​= ​1−2sin22θ​n2−2sin22θ​m22sin22θ​lm+2sin2θ​cos2θ​n2sin22θ​ln−2sin2θ​cos2θ​m​2sin22θ​lm−2sin2θ​cos2θ​n1−2sin22θ​n2−2sin22θ​l22sin22θ​mn+2sin2θ​cos2θ​l​2sin22θ​ln+2sin2θ​cos2θ​m2sin22θ​mn−2sin2θ​cos2θ​l1−2sin22θ​m2−2sin22θ​l2​ ​

可把我累成狗了

接下来这一步就要回到我们一开始提到的单位四元数的表达形式了
$$Q=cos\frac{\theta }{2}+(li+mj+nk)sin\frac{\theta }{2}$$
令$q_0=cos\frac{\theta }{2}$，$q_1=lsin\frac{\theta }{2}$，$q_2=msin\frac{\theta }{2}$，$q_3=nsin\frac{\theta }{2}$，则四元数可以写成
$$Q=q_0+q_{1}i+q_{2}j+q_{3}k$$
并且坐标转换矩阵可以写成
D B E = [ 1 − 2 s i n 2 θ 2 n 2 − 2 s i n 2 θ 2 m 2 2 s i n 2 θ 2 l m − 2 s i n θ 2 c o s θ 2 n 2 s i n 2 θ 2 l n + 2 s i n θ 2 c o s θ 2 m 2 s i n 2 θ 2 l m + 2 s i n θ 2 c o s θ 2 n 1 − 2 s i n 2 θ 2 n 2 − 2 s i n 2 θ 2 l 2 2 s i n 2 θ 2 m n − 2 s i n θ 2 c o s θ 2 l 2 s i n 2 θ 2 l n − 2 s i n θ 2 c o s θ 2 m 2 s i n 2 θ 2 m n + 2 s i n θ 2 c o s θ 2 l 1 − 2 s i n 2 θ 2 m 2 − 2 s i n 2 θ 2 l 2 ] = [ 1 − 2 q 3 2 − 2 q 2 2 2 q 1 q 2 − 2 q 0 q 3 2 q 1 q 3 + 2 q 0 q 2 2 q 1 q 2 + 2 q 0 q 3 1 − 2 q 3 2 − 2 q 1 2 2 q 2 q 3 − 2 q 0 q 1 2 q 1 q 3 − 2 q 0 q 2 2 q 2 q 3 + 2 q 0 q 1 1 − 2 q 2 2 − 2 q 1 2 ] D_{B}^{E} =\begin{bmatrix}1-2sin^{2}\frac{\theta}{2}n^{2}-2sin^{2}\frac{\theta}{2}m^{2} &2sin^{2}\frac{\theta}{2}lm-2sin\frac{\theta}{2}cos\frac{\theta}{2}n &2sin^{2}\frac{\theta}{2}ln+2sin\frac{\theta}{2}cos\frac{\theta}{2}m \\2sin^{2}\frac{\theta}{2}lm+2sin\frac{\theta}{2}cos\frac{\theta}{2}n &1-2sin^{2}\frac{\theta}{2}n^{2}-2sin^{2}\frac{\theta}{2}l^{2} &2sin^{2}\frac{\theta}{2}mn-2sin\frac{\theta}{2}cos\frac{\theta}{2}l \\2sin^{2}\frac{\theta}{2}ln-2sin\frac{\theta}{2}cos\frac{\theta}{2}m &2sin^{2}\frac{\theta}{2}mn+2sin\frac{\theta}{2}cos\frac{\theta}{2}l &1-2sin^{2}\frac{\theta}{2}m^{2}-2sin^{2}\frac{\theta}{2}l^{2} \end{bmatrix} \\ =\begin{bmatrix}1-2q_{3}^2-2q_{2}^2 &2q_{1}q_{2}-2q_{0}q_{3} &2q_{1}q_{3}+2q_{0}q_{2} \\2q_{1}q_{2}+2q_{0}q_{3} &1-2q_{3}^2-2q_{1}^2 &2q_{2}q_{3}-2q_{0}q_{1} \\2q_{1}q_{3}-2q_{0}q_{2} &2q_{2}q_{3}+2q_{0}q_{1} &1-2q_{2}^2-2q_{1}^2 \end{bmatrix} DBE​= ​1−2sin22θ​n2−2sin22θ​m22sin22θ​lm+2sin2θ​cos2θ​n2sin22θ​ln−2sin2θ​cos2θ​m​2sin22θ​lm−2sin2θ​cos2θ​n1−2sin22θ​n2−2sin22θ​l22sin22θ​mn+2sin2θ​cos2θ​l​2sin22θ​ln+2sin2θ​cos2θ​m2sin22θ​mn−2sin2θ​cos2θ​l1−2sin22θ​m2−2sin22θ​l2​ ​= ​1−2q32​−2q22​2q1​q2​+2q0​q3​2q1​q3​−2q0​q2​​2q1​q2​−2q0​q3​1−2q32​−2q12​2q2​q3​+2q0​q1​​2q1​q3​+2q0​q2​2q2​q3​−2q0​q1​1−2q22​−2q12​​ ​
又该四元数是单位四元数，即满足$q_0^2+q_1^2+q_2^2+q_3^2=1$
可以简化得到
D B E = [ q 0 2 + q 1 2 − q 3 2 − q 2 2 2 q 1 q 2 − 2 q 0 q 3 2 q 1 q 3 + 2 q 0 q 2 2 q 1 q 2 + 2 q 0 q 3 q 0 2 + q 2 2 − q 1 2 − q 3 2 2 q 2 q 3 − 2 q 0 q 1 2 q 1 q 3 − 2 q 0 q 2 2 q 2 q 3 + 2 q 0 q 1 q 0 2 + q 3 2 − q 1 2 − q 2 2 ] D_{B}^{E} =\begin{bmatrix}q_{0}^2+q_{1}^2-q_{3}^2-q_{2}^2 &2q_{1}q_{2}-2q_{0}q_{3} &2q_{1}q_{3}+2q_{0}q_{2} \\2q_{1}q_{2}+2q_{0}q_{3} &q_{0}^2+q_{2}^2-q_{1}^2-q_{3}^2 &2q_{2}q_{3}-2q_{0}q_{1} \\2q_{1}q_{3}-2q_{0}q_{2} &2q_{2}q_{3}+2q_{0}q_{1} &q_{0}^2+q_{3}^2-q_{1}^2-q_{2}^2 \end{bmatrix} DBE​= ​q02​+q12​−q32​−q22​2q1​q2​+2q0​q3​2q1​q3​−2q0​q2​​2q1​q2​−2q0​q3​q02​+q22​−q12​−q32​2q2​q3​+2q0​q1​​2q1​q3​+2q0​q2​2q2​q3​−2q0​q1​q02​+q32​−q12​−q22​​ ​
同样，这个变换矩阵是正交矩阵，性质与欧拉角得到的变换矩阵的完全一致。
那么就可以得到欧拉角和四元数的关系。
$$\{\begin{array}{l}2q_1{q}_3-2q_0{q}_2=-\sin \beta \\ 2q_0{q}_1+2q_2{q}_3=\sin \gamma \cos \beta \\ q_0^2-q_1^2-q_2^2+q_3^2=\cos \gamma \cos \beta \\ q_0^2+q_1^2-q_2^2-q_3^2=\cos \alpha \cos \beta \\ 2q_0{q}_3+2q_1{q}_2=\cos \beta \sin \alpha \end{array}$$
$$\{\begin{array}{l}\beta =\arcsin \left(2q_1{q}_3-2q_0{q}_2\right)\\ \gamma =\arctan \left(\frac{2q_0{q}_1+2q_2{q}_3}{q_0^2-q_1^2-q_2^2+q_3^2}\right)\\ \alpha =\arctan \left(\frac{2q_0{q}_3+2q_1{q}_2}{q_0^2+q_1^2-q_2^2-q_3^2}\right)\end{array}$$
太累了不把过程详细列出来了，自行请便吧。

到这里，不知道在看的过程中有没有思考过这样一个问题，为什么我旋转角度是$\theta$，但是四元数里的角度却是$\frac{\theta }{2}$，这个问题我在新接触的时候也疑惑过。现在从结论上来看，半角是为了方便计算后面的变换矩阵进行妥协的结果吧（纯粹个人马后炮结论）。那如果四元数变成半角是否会影响到结果以及如何实现的问题，事实上从推导过程可以看出，变换矩阵是基于旋转$\theta$得到的几何关系推导得到的，因此不管四元数中是否为半角，都将不影响其结论的正确性，影响的仅仅是在实际开发过程中计算四元数四个参数的过程。这个作为后续的内容继续补充。