坐标系

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

坐标系变换

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

基于欧拉角的坐标变换

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

图片引自《惯性导航》，作者秦永元，侵删
根据几何关系，可以得到
rx2=OA+AB+BCrx2=ODcosα+BDsinα+BFsinαrx2=ODcosα+DFsinαrx2=rx1cosα+ry1sinα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 \\ rx2=OA+AB+BCrx2=ODcosα+BDsinα+BFsinαrx2=ODcosα+DFsinαrx2=rx1cosα+ry1sinα
ry2=DE−DAry2=DFcosα−ODsinαry2=ry1cosα−rx1sinαr_{y2}=DE-DA \\ r_{y2}=DFcos\alpha-ODsin\alpha \\ r_{y2}=r_{y1}cos\alpha-r_{x1}sin\alpha ry2=DE−DAry2=DFcosα−ODsinαry2=ry1cosα−rx1sinα
以矩阵形式表达
[rx2ry2]=[cosαsinα−sinαcosα][rx1ry1]\begin{bmatrix} r_{x2} \\ r_{y2}\end{bmatrix}= \begin{bmatrix} cos\alpha & sin\alpha \\ -sin\alpha &cos\alpha\end{bmatrix}\ \begin{bmatrix} r_{x1} \\ r_{y1}\end{bmatrix} [rx2ry2]=[cosα−sinαsinαcosα] [rx1ry1]
但是对于一个三维向量来说，绕ZZZ轴旋转并不改变他的zzz坐标，因此完整的变换过程应该为
[rx2ry2rz2]=[cosαsinα0−sinαcosα0001][rx1ry1rz1]\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} ⎣⎡rx2ry2rz2⎦⎤=⎣⎡cosα−sinα0sinαcosα0001⎦⎤ ⎣⎡rx1ry1rz1⎦⎤
因此，可以得到旋转ZZZ轴时的旋转矩阵为
rot(Z,α)=[cosαsinα0−sinαcosα0001]rot(Z,\alpha)= \begin{bmatrix} cos\alpha & sin\alpha &0\\ -sin\alpha &cos\alpha &0 \\ 0 &0 &1\end{bmatrix} rot(Z,α)=⎣⎡cosα−sinα0sinαcosα0001⎦⎤
同理，可以推导得到绕YYY轴和XXX轴的旋转矩阵
rot(Y,β)=[cosβ0−sinβ010sinβ0cosβ]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β⎦⎤
rot(X,γ)=[1000cosγsinγ0−sinγcosγ]rot(X,\gamma)= \begin{bmatrix} 1 &0 &0\\ 0&cos\gamma& sin\gamma\\ 0 &-sin\gamma &cos\gamma\end{bmatrix} rot(X,γ)=⎣⎡1000cosγ−sinγ0sinγcosγ⎦⎤
以Z−Y−XZ-Y-XZ−Y−X的顺序合成复合旋转，可以得到：
CEB=rot(X,γ)rot(Y,β)rot(Z,α)=[cosβcosαcosβsinα−sinβsinγsinβcosα−sinαcosγsinαsinβsinγ+cosγcosαsinγcosβcosαcosγsinβ+sinγsinαsinαcosγsinβ−sinγcosαcosγcosβ]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β⎦⎤
这里要稍微提一下，从单轴的旋转矩阵可以看出，每个矩阵都是正交矩阵，因此变换矩阵CEBC_{E}^{B}CEB也是正交矩阵，即满足关系
(CEB)T=(CEB)−=CBE(C_{E}^{B})^{T}=(C_{E}^{B})^{-}=C_{B}^{E} (CEB)T=(CEB)−=CBE
即只要知道其中一个变换矩阵就可以实现双向变换过程。
但是由于欧拉法的变换是通过合成单独旋转得到的，就存在一个万向锁的问题(Gambal lock)，有兴趣的可以自行查找资料，此处不作赘述。

基于四元数的坐标变换

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

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

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

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

则令c=r⃗c=\vec{r}c=r，a=b=u⃗a=b=\vec{u}a=b=u，可以得到
u⃗×(u⃗×r⃗)=(u⃗⋅r⃗)u⃗−(u⃗⋅u⃗)r⃗=(u⃗⋅r⃗)u⃗−r⃗\vec{u}\times(\vec{u}\times\vec{r})=(\vec{u}\cdot \vec{r})\vec{u}-(\vec{u}\cdot\vec{u})\vec{r}=(\vec{u}\cdot \vec{r})\vec{u}-\vec{r} u×(u×r)=(u⋅r)u−(u⋅u)r=(u⋅r)u−r
则可以得到
(u⃗⋅r⃗)u⃗=u⃗×(u⃗×r⃗)+r⃗(\vec{u}\cdot \vec{r})\vec{u}=\vec{u}\times(\vec{u}\times\vec{r})+\vec{r} (u⋅r)u=u×(u×r)+r
则进一步推导得到
OA′=(1−cosθ)(r⃗⋅u⃗)u⃗+r⃗cosθ+(u⃗×r⃗)sinθ=(1−cosθ)(u⃗×(u⃗×r⃗)+r⃗)+r⃗cosθ+(u⃗×r⃗)sinθ=(1−cosθ)(u⃗×(u⃗×r⃗))+r⃗+(u⃗×r⃗)sinθOA^{'}=(1-cos\theta)(\vec{r}\cdot \vec{u})\vec{u}+\vec{r}cos\theta+(\vec{u} \times \vec{r} )sin\theta \\ =(1-cos\theta)(\vec{u}\times(\vec{u}\times\vec{r})+\vec{r})+\vec{r}cos\theta+(\vec{u} \times \vec{r} )sin\theta \\ = (1-cos\theta)(\vec{u}\times(\vec{u}\times\vec{r}))+\vec{r}+(\vec{u} \times \vec{r} )sin\theta OA′=(1−cosθ)(r⋅u)u+rcosθ+(u×r)sinθ=(1−cosθ)(u×(u×r)+r)+rcosθ+(u×r)sinθ=(1−cosθ)(u×(u×r))+r+(u×r)sinθ

这里要插一个补充的内容
对于叉乘来说，假设有两个任意向量A⃗=[a1a2a3]\vec{A}=\begin{bmatrix} a1 \\ a2 \\ a3 \end{bmatrix}A=⎣⎡a1a2a3⎦⎤和B⃗=[b1b2b3]\vec{B}=\begin{bmatrix} b1 \\ b2 \\ b3 \end{bmatrix}B=⎣⎡b1b2b3⎦⎤。那么两个向量的叉乘可以写成
A⃗×B⃗=∣i⃗a1b1j⃗a2b2k⃗a3b3∣=[a2b3−b2a3a3b1−a1b3a1b2−a2b1]=[0−a3a2a30−a1−a2a10][b1b2b3]=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=∣∣ijka1a2a3b1b2b3∣∣=⎣⎡a2b3−b2a3a3b1−a1b3a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤⎣⎡b1b2b3⎦⎤=C⋅B
所以可以把叉乘理解为对叉乘右侧的一次坐标变换。

令u⃗×X⃗=C⋅X⃗\vec{u}\times\vec{X}=C\cdot\vec{X}u×X=C⋅X，其中u⃗\vec{u}u和X⃗=[x1x2x3]\vec{X}=\begin{bmatrix}x1\\x2\\x3\end{bmatrix}X=⎣⎡x1x2x3⎦⎤为任意向量，CCC为等效矩阵。
[lmn]×[x1x2x3]=[−nx2+mx3nx1−lx3−mx1+lx2]=[0−nmn0−l−ml0][x1x2x3]\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−n0lm−l0⎦⎤⎣⎡x1x2x3⎦⎤
故C=[0−nmn0−l−ml0]C=\begin{bmatrix}0&-n&m\\n&0&-l\\-m&l&0\end{bmatrix}C=⎣⎡0n−m−n0lm−l0⎦⎤
那么可以继续推导得到
OA′=r⃗′=(1−cosθ)C⋅(C⋅r⃗)+r⃗+C⋅r⃗sinθ=(1−cosθ)C2⋅r⃗+r⃗+Csinθ⋅r⃗OA^{'}=\vec{r}^{'}=(1-cos\theta)C\cdot (C\cdot\vec{r})+\vec{r}+C\cdot\vec{r}sin\theta=(1-cos\theta) C^{2}\cdot\vec{r}+\vec{r}+Csin\theta\cdot\vec{r} OA′=r′=(1−cosθ)C⋅(C⋅r)+r+C⋅rsinθ=(1−cosθ)C2⋅r+r+Csinθ⋅r
整理得到
r⃗′=(I+(1−cosθ)C2+Csinθ)r⃗\vec{r}^{'}=(I+(1-cos\theta)C^{2}+Csin\theta)\vec{r} r′=(I+(1−cosθ)C2+Csinθ)r
利用三角函数的半角公式和倍角公式可以得到
1−cosθ=2sin2θ2sinθ=2sinθ2cosθ21-cos\theta =2sin^{2}\frac{\theta}{2} \\ sin\theta =2sin\frac{\theta}{2}cos\frac{\theta}{2} 1−cosθ=2sin22θsinθ=2sin2θcos2θ
代入后可以得到
r⃗′=(I+2sin2θ2C2+2sinθ2cosθ2C)r⃗\vec{r}^{'}=(I+2sin^{2}\frac{\theta}{2}C^{2}+ 2sin\frac{\theta}{2}cos\frac{\theta}{2}C)\vec{r} r′=(I+2sin22θC2+2sin2θcos2θC)r

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

那么从机体坐标系到地球坐标系的变换矩阵DBED_{B}^{E}DBE表达式为
DBE=[100010001]+2sin2θ2[0−nmn0−l−ml0][0−nmn0−l−ml0]+2sinθ2cosθ2[0−nmn0−l−ml0]=[100010001]+2sin2θ2[−n2−m2lmlnlm−n2−l2mnlnmn−m2−l2]+2sinθ2cosθ2[0−nmn0−l−ml0]=[1−2sin2θ2n2−2sin2θ2m22sin2θ2lm−2sinθ2cosθ2n2sin2θ2ln+2sinθ2cosθ2m2sin2θ2lm+2sinθ2cosθ2n1−2sin2θ2n2−2sin2θ2l22sin2θ2mn−2sinθ2cosθ2l2sin2θ2ln−2sinθ2cosθ2m2sin2θ2mn+2sinθ2cosθ2l1−2sin2θ2m2−2sin2θ2l2]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=⎣⎡100010001⎦⎤+2sin22θ⎣⎡0n−m−n0lm−l0⎦⎤⎣⎡0n−m−n0lm−l0⎦⎤+2sin2θcos2θ⎣⎡0n−m−n0lm−l0⎦⎤=⎣⎡100010001⎦⎤+2sin22θ⎣⎡−n2−m2lmlnlm−n2−l2mnlnmn−m2−l2⎦⎤+2sin2θcos2θ⎣⎡0n−m−n0lm−l0⎦⎤=⎣⎡1−2sin22θn2−2sin22θm22sin22θlm+2sin2θcos2θn2sin22θln−2sin2θcos2θm2sin22θlm−2sin2θcos2θn1−2sin22θn2−2sin22θl22sin22θmn+2sin2θcos2θl2sin22θln+2sin2θcos2θm2sin22θmn−2sin2θcos2θl1−2sin22θm2−2sin22θl2⎦⎤

可把我累成狗了

接下来这一步就要回到我们一开始提到的单位四元数的表达形式了
Q=cosθ2+(li⃗+mj⃗+nk⃗)sinθ2Q=cos\frac{\theta}{2}+(l\vec{i}+m\vec{j }+n\vec{k})sin\frac{\theta}{2} Q=cos2θ+(li+mj+nk)sin2θ
令q0=cosθ2q_{0}=cos\frac{\theta}{2}q0=cos2θ，q1=lsinθ2q_{1}=lsin\frac{\theta}{2}q1=lsin2θ，q2=msinθ2q_{2}=msin\frac{\theta}{2}q2=msin2θ，q3=nsinθ2q_{3}=nsin\frac{\theta}{2}q3=nsin2θ，则四元数可以写成
Q=q0+q1i⃗+q2j⃗+q3k⃗Q=q_{0}+q_{1}\vec{i}+q_{2}\vec{j}+q_{3}\vec{k} Q=q0+q1i+q2j+q3k
并且坐标转换矩阵可以写成
DBE=[1−2sin2θ2n2−2sin2θ2m22sin2θ2lm−2sinθ2cosθ2n2sin2θ2ln+2sinθ2cosθ2m2sin2θ2lm+2sinθ2cosθ2n1−2sin2θ2n2−2sin2θ2l22sin2θ2mn−2sinθ2cosθ2l2sin2θ2ln−2sinθ2cosθ2m2sin2θ2mn+2sinθ2cosθ2l1−2sin2θ2m2−2sin2θ2l2]=[1−2q32−2q222q1q2−2q0q32q1q3+2q0q22q1q2+2q0q31−2q32−2q122q2q3−2q0q12q1q3−2q0q22q2q3+2q0q11−2q22−2q12]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θm2sin22θlm−2sin2θcos2θn1−2sin22θn2−2sin22θl22sin22θmn+2sin2θcos2θl2sin22θln+2sin2θcos2θm2sin22θmn−2sin2θcos2θl1−2sin22θm2−2sin22θl2⎦⎤=⎣⎡1−2q32−2q222q1q2+2q0q32q1q3−2q0q22q1q2−2q0q31−2q32−2q122q2q3+2q0q12q1q3+2q0q22q2q3−2q0q11−2q22−2q12⎦⎤
又该四元数是单位四元数，即满足q02+q12+q22+q32=1q_{0}^{2}+q_{1}^{2}+q_{2}^{2}+q_{3}^{2}=1q02+q12+q22+q32=1
可以简化得到
DBE=[q02+q12−q32−q222q1q2−2q0q32q1q3+2q0q22q1q2+2q0q3q02+q22−q12−q322q2q3−2q0q12q1q3−2q0q22q2q3+2q0q1q02+q32−q12−q22]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−q222q1q2+2q0q32q1q3−2q0q22q1q2−2q0q3q02+q22−q12−q322q2q3+2q0q12q1q3+2q0q22q2q3−2q0q1q02+q32−q12−q22⎦⎤
同样，这个变换矩阵是正交矩阵，性质与欧拉角得到的变换矩阵的完全一致。
那么就可以得到欧拉角和四元数的关系。
{2q1q3−2q0q2=−sin⁡β2q0q1+2q2q3=sin⁡γcos⁡βq02−q12−q22+q32=cos⁡γcos⁡βq02+q12−q22−q32=cos⁡αcos⁡β2q0q3+2q1q2=cos⁡βsin⁡α\left\{\begin{array}{l} 2 q_{1} q_{3}-2 q_{0} q_{2}=-\sin \beta \\ 2 q_{0} q_{1}+2 q_{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 \\ 2 q_{0} q_{3}+2 q_{1} q_{2}=\cos \beta \sin \alpha \end{array}\right. ⎩⎨⎧2q1q3−2q0q2=−sinβ2q0q1+2q2q3=sinγcosβq02−q12−q22+q32=cosγcosβq02+q12−q22−q32=cosαcosβ2q0q3+2q1q2=cosβsinα
{β=arcsin⁡(2q1q3−2q0q2)γ=arctan⁡(2q0q1+2q2q3q02−q12−q22+q32)α=arctan⁡(2q0q3+2q1q2q02+q12−q22−q32)\left\{\begin{array}{l} \beta=\arcsin \left(2 q_{1} q_{3}-2 q_{0} q_{2}\right) \\ \gamma=\arctan \left(\frac{2 q_{0} q_{1}+2 q_{2} q_{3}}{q_{0}^{2}-q_{1}^{2}-q_{2}^{2}+q_{3}^{2}}\right) \\ \alpha=\arctan \left(\frac{2 q_{0} q_{3}+2 q_{1} q_{2}}{q_{0}^{2}+q_{1}^{2}-q_{2}^{2}-q_{3}^{2}}\right) \end{array}\right. ⎩⎨⎧β=arcsin(2q1q3−2q0q2)γ=arctan(q02−q12−q22+q322q0q1+2q2q3)α=arctan(q02+q12−q22−q322q0q3+2q1q2)
太累了不把过程详细列出来了，自行请便吧。

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

一、Mahony姿态解算——坐标系变换相关推荐

Pixhawk代码分析-姿态解算篇D
姿态解算篇D 基础知识研究多旋翼无人机前期主要需要了解其气动布局和复杂的动力学模型,然后就是姿态解算和控制器的设计.为了实现精确四旋翼飞行器的姿态估计,首先就是需要了解各传感器采集的数据和误差存在的 ...
Pixhawk代码分析-姿态解算篇C
姿态解算篇C 前言终于到ardupilot源代码的姿态解算了,有了前期关于mahony姿态解算算法的基础以后,理解源代码的姿态解算算法就快多了,所有的东西都在脑海中初步有了一个框架:首先要做什么,然 ...
Pixhawk之姿态解算篇（4）_补充篇
一.开篇大家期待已久的第四篇来了,但是本篇可能比较水啊~~~见谅~~~ 首先,上一周没有什么收获,虽然看了不少的论文,但是却没有什么质的飞越~~~~ 看的论文都是关于姿态解算的,用的算法大部分也都是 ...
Pixhawk之姿态解算篇（3）_源码姿态解算算法分析
一.开篇终于到ardupilot源代码的姿态解算了,有了前期关于mahony姿态解算算法的基础以后,理解源代码的姿态解算算法就快多了,所有的东西都在脑海中初步有了一个框架:首先要做什么,然后再做什么 ...
一阶高通滤波+二阶Mahony滤波的四元数姿态解算
此次实验我使用icm20602进行 icm20602输出有以下特点: 3轴陀螺仪可选量程有±250dps,±500dps,±1000dps,±2000dps.(dps:degrees per seco ...
stm32 MPU6050 姿态解算 Mahony互补滤波算法
文章目录 0.介绍 1,理论分析 1.1 MPU6050 1.2 Mahony算法原理 2,代码实现 1.1 MPU6050初始化及数据读取 1.2 Mahony算法c语言实现 1.3 将代码移植到你 ...
姿态解算四元数、方向余弦、欧拉角、Mahony滤波、四轴
姿态解算四元数.方向余弦.欧拉角.Mahony滤波说明:本文只是做了一些总结,需要一些对这方面的基础概念的了解. 一般人千万不要试图去深入探讨四元数 1. 方向余弦矩阵方向余弦矩阵是使用欧拉角( ...
姿态解算与左右手坐标系
姿态解算与左右手坐标系一般姿态解算都是使用右手坐标系,但是unity中是左手坐标系,这样的话,硬件产生的四元数直接输入到unity中,姿态是无法对应的,一种方法是调整四元数的顺序和正负号来达到姿 ...
十二.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法＜上＞
这是"四轮车驱动控制"系列,分多个小节来介绍: 1. 八.四轮车驱动开发之一:正/逆向运动学分析 2. 九.四轮车驱动开发之二: 配置PWM驱动直流电机 3. 十.四轮车驱动开发之 ...

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

坐标系

坐标系变换

基于欧拉角的坐标变换

基于四元数的坐标变换

一、Mahony姿态解算——坐标系变换相关推荐

最新文章

热门文章