零.先修知识补充
0.1机体坐标系
机体坐标系，有名体轴系，是指固定在飞行器或者飞机上的遵循右手法则的三维正交直角坐标系。
在机体坐标系下，飞机本身是静止的。然而，在飞行中，飞机的机体坐标系与大气流坐标系之间存在相对运动。因此需要引入一个相对于机体坐标系固定的参考点，来描述飞机在飞行过程中相对于大气流的运动状态。在飞机的机体坐标系下，可以定义三个坐标轴：X轴指向飞机的前进方向，Y轴指向飞机的右侧方向，Z轴指向飞机的下方方向
因此，在机体坐标系下，可以测量出相对于飞机前进方向的速度和相对于飞机旋转轴的旋转角速度。机体坐标系下的速度被称为机体速度，它描述了飞机相对于自身的运动状态，包括飞机相对于空气的速度以及空气对飞机的作用力。机体坐标系下的机身旋转角速度被称为机体角速度，它描述了飞机相对于自身旋转的速度，包括绕X、Y、Z三个轴的旋转速度。机体角速度是描述飞机姿态和稳定性的重要参数，能够帮助飞行员控制飞机的飞行姿态
0.2惯性坐标系
坐标系的指向在惯性空间中保持不变的坐标系称为惯性坐标系。惯性系与惯性坐标系是有区别的。惯性系是惯性参考系的简称。满足牛顿第一定律的参考系称为惯性参考系。且相对惯性系作匀速直线运动的物体也是惯性参考系
惯性坐标系是为了简化世界坐标系到物体坐标系的转化而产生的。惯性坐标系的原点与物体坐标系的原点重合，惯性坐标系的轴平行于世界坐标系的轴。引入了惯性坐标系之后，物体坐标系转换到惯性坐标系只需旋转，从惯性坐标系转换到世界坐标系只需平移。例如，现在有一个骰子，原地旋转之后，希望可以描述它的旋转，向左转多少或者向右转多少。这样就要给定一个原始状态，在这个状态给定下，我们就可以轻松的描述它是如何旋转的，那么这个原始状态本质上就是惯性坐标系，只不过这个惯性坐标系原点与骰子质心一致，两个坐标轴分别平行于标准坐标系
0.3大地坐标系
大地坐标系是大地测量中以参考椭球面为基准面建立起来的坐标系。地面点的位置用大地经度L、大地纬度B和大地高度H表示。大地坐标系的确立包括选择一个椭球、对椭球进行定位和确定大地起算数据。 一个形状、大小和定位、定向都已确定的地球椭球叫参考椭球。参考椭球一旦确定，则标志着大地坐标系已经建立
一.Mahony算法是什么？
Mahony算法是常见的姿态融合算法，将加速度计、磁力计、陀螺仪共九轴数据，融合解算出机体四元数。本篇文章只介绍融合加速度计和陀螺仪的六轴数据算法
二.空间姿态的常规描述
姿态解算中的姿态实际上指的是机体坐标轴与地理坐标系的旋转关系。其常用描述形式有三种：欧拉角、方向余弦矩阵，四元数
2.1四元数
四元数与定义与复数类似，但复数只有一个虚部，而四元数一共有三个。所有四元数都可以写成下面形式：
q=a+bi+cj+dk （ a,b,c,d\in R ）或 q=q_0+q_1i+q_2j+q_3k ( q_0,q_1,q_2,q_3\in R )
/q表示四元数/
/ i^2=j^2=k^2=ijk=-1 /
与复数类似，四元数就是基{1,i,j,k}的线性组合。同样的，四元数也可以写成向量形式：
q=\left[\begin{array}{l}{q_{0}^{}} \\{q_{1}^{}} \\{q_{2}^{}} \\{q_{3}^{}}\end{array}\right]\\
另外，我们也可以分离实部与虚部，即通过一个三维向量表示虚部，从而将四元数表示为标量与向量的有序对形式
q=[s, \mathbf{v}] \quad\left(\mathbf{v}=\left[\begin{array}{l}{x} \\{y} \\{z}\end{array}\right], s, x, y, z \in \mathbb{R}\right)\\
2.1.1 模长
仿照复数的定义，我们可以将四元数 q=a+bi+cj+dk 的模长定义为
||q||=\sqrt{a^2+b^2+c^2+d^2}
用有序形式表示为
\begin{aligned}\|q\| &=\sqrt{s^{2}+\|\mathbf{v}\|^{2}} \\&=\sqrt{s^{2}+\mathbf{v} \cdot \mathbf{v}}\end{aligned}\\
2.1.2加减运算与标量乘法
四元数的加减运算与标量乘法同复数类似，将分量各自运算即可
与复数相同，四元数的标量乘法同样遵循交换律，即sq = qs ，其中s为标量
2.1.3四元数乘法
不遵循交换律，即一般情况下 q_1q_2\ne q_2q_1
证明：
2.1.4纯四元数
与复数类似，实部为0的四元数被称为纯四元数
2.1.5单位四元数
定义模长为1的四元数为单位四元数
/用于表示旋转的四元数一定是单位四元数！/
三.利用四元数表示姿态
3.1利用四元数表示旋转
3.1.1罗德里格旋转公式
3.1.2四元数旋转公式
通过罗德里格旋转公式，我们可以推导出四元数表示旋转的定理：
3.2利用四元数表示姿态矩阵
将四元数旋转公式表示为矩阵形式：
q v q^{*}=\left[\begin{array}{cccc} {1} & {0} & {0} & {0} \\ {0} & {1-2 q_{2}^{2}-2 q_{3}^{2}} & {2 q_{1} q_{2}-2 q_{0} q_{3}} & {2 q_{0} q_{2}+2 q_{1} q_{3}} \\ {0} & {2 q_{1} q_{2}+2 q_{0} q_{3}} & {1-2 q_{1}^{2}-2 q_{3}^{2}} & {2 q_{2} q_{3}-2 q_{0} q_{1}} \\ {0} & {2 q_{1} q_{3}-2 q_{0} q_{2}} & {2 q_{0} q_{1}+2 q_{2} q_{3}} & {1-2 q_{1}^{2}-2 q_{2}^{2}} \end{array}\right] v\\
压缩为三维方阵，得到矩阵旋转公式：
3.3欧拉角
3.3.1欧拉角算法
为了使四元数更具有操作性，更好理解，我们引入欧拉角
实现旋转方式：
3.3.2通过四元数反解欧拉角
四.基于四元数的姿态解算
4.1四元数求解
四元数描述机体坐标系与大地坐标系的旋转关系，因此对于机体的姿态解算需要实时更新四元数。通过构建四元数关于时间的微分方程来研究此问题
4.2传感器数据融合
4.2.1基本原理
对于六轴数据，计算角速度的方法有两种，一是通过对角速度积分得到角度，另一种则是通过对加速度进行正交分解得到角度
但是这两种方法各有不足：
因此，可以通过融合两种数据以获得准确姿态，即通过加速度计补偿角速度
4.2.2误差补偿
