姿态解算
转载自：http://www.crazepony.com/wiki/attitude-algorithm.html
姿态解算简介
作者：nieyong
本文需要讲清楚在无人机飞行器算法中，什么是姿态，怎么表示姿态，如何得到姿态。
什么是姿态
姿态就是指飞行器的俯仰/横滚/航向情况。在咱们地球上，就是指飞行器在地球坐标系中的俯仰/横滚/航向情况。飞行器需要实时知道当前自己的姿态，才能够根据需要操控其接下来的动作，例如保持平稳，例如实现翻滚。
下面是学术型的严密论述。
数学模型
姿态是用来描述一个刚体的固连坐标系和参考坐标系之间的角位置关系，有一些数学表示方法。很常见的就是欧拉角，四元数，矩阵，轴角。
地球坐标系又叫做地理坐标系，是固定不变的。正北，正东，正向上构成了这个坐标系的X，Y，Z轴，我们用坐标系R表示。四轴飞行器上固定着一个坐标系，我们一般称之为机体坐标系，用坐标系r表示。那么我们就可以用欧拉角，四元数等来描述r和R的角位置关系。这就是四轴飞行器姿态解算的数学模型和基础。
姿态表示方式
姿态有多种数学表示方式，常见的是四元数，欧拉角，矩阵和轴角。他们各自有其自身的优点，在不同的领域使用不同的表示方式。在四轴飞行器中使用到了四元数和欧拉角。Crazepony开源四轴飞行器也是一样的。
四元数
四元数是由爱尔兰数学家威廉·卢云·哈密顿在1843年发现的数学概念。从明确地角度而言，四元数是复数的不可交换延伸。如把四元数的集合考虑成多维实数空间的话，四元数就代表着一个四维空间，相对于复数为二维空间。
四元数大量用于电脑绘图（及相关的图像分析）上表示三维物件的旋转及方位。四元数亦见于控制论、信号处理、姿态控制、物理和轨道力学，都是用来表示旋转和方位。
相对于另几种旋转表示法（矩阵，欧拉角，轴角），四元数具有某些方面的优势，如速度更快、提供平滑插值、有效避免万向锁问题、存储空间较小等等。
以上部分摘自维基百科-四元数。
欧拉角
莱昂哈德·欧拉用欧拉角来描述刚体在三维欧几里得空间的取向。对于在三维空间里的一个参考系，任何坐标系的取向，都可以用三个欧拉角来表现。参考系又称为实验室参考系，是静止不动的。而坐标系则固定于刚体，随着刚体的旋转而旋转。
以上部分摘自维基百科-欧拉角。下面我们通过图例来看看欧拉角是如何产生的，并且分别对应哪个角度。
姿态解算
姿态解算需要解决的是无人机飞行器在地球坐标系中姿态。
姿态解算的英文是attitude algorithm，也叫做姿态分析，姿态估计，姿态融合。姿态解算是指根据IMU数据（陀螺仪、加速度计、罗盘等）求解出飞行器的空中姿态，所以也叫做IMU数据融合（IMU Data Fusing）。
角位置关系测量
如上所说，地球坐标系R是固定的。四轴飞行器上固定一个坐标系r，这个坐标系r在坐标系R中运动。那么如何知道坐标系r和坐标系R的角位置关系呢，也就是怎么知道飞行器相对于地球这个固定坐标系R转动了一下航向，或者侧翻了一下机身，或者掉头下栽。这就是传感器需要测量的数据，传感器包括陀螺仪，加速度计，磁力计。通过获得这些测量数据，得到坐标系r和坐标系R的角位置关系。
惯性测量模块：IMU（Inertial Measurement Unit），提供飞行器在空间姿态的传感器原始数据，一般由陀螺仪传感器/加速度传感器/电子罗盘提供飞行器9DOF数据。
飞行器根据陀螺仪的三轴角速度对时间积分得到的俯仰/横滚/航向角，这是快速解算。快速解算得到的姿态是存在误差的，而且误差会累加，如果再结合三轴地磁和三轴加速度数据进行校正，得到准确的姿态，这就是深度解算。
当然，快速解算的姿态一般是不能够用于控制飞行器的，因为误差太大。我们一般说的姿态解算就是深度解算。
四元数和欧拉角在姿态解算中如何使用
姿态解算的核心在于旋转，一般旋转有4种表示方式：矩阵表示、欧拉角表示、轴角表示和四元数表示。矩阵表示适合变换向量，欧拉角最直观，轴角表示则适合几何推导，而在组合旋转方面，四元数表示最佳。因为姿态解算需要频繁组合旋转和用旋转变换向量，所以采用四元数保存飞行器的姿态。
在Crazepony中，使用四元数来保存飞行器的姿态（也就是在地球坐标系中的俯仰/横滚/航向情况）。在需要控制的时候，会将四元数转化为欧拉角，然后输入到姿态控制算法中。
姿态控制算法的输入参数必须要是欧拉角。
下面就是Crazepony开源四轴飞行器上，姿态解算到姿态控制的整个流程。AD值是指MPU6050的陀螺仪和加速度值，3个维度的陀螺仪值和3个维度的加速度值，每个值为16位精度。AD值通过姿态解算算法得到飞行器当前的姿态（姿态使用四元数表示），然后将四元数转化为欧拉角，用于姿态控制算法（PID控制）中。
姿态解算算法
如何根据IMU提供的数据（三轴陀螺仪，三轴加速度计，电子罗盘数据），得到准确的姿态呢？
飞行器中的姿态一般使用四元数表示。只有当需要输入到控制算法的时候，才会将四元数转化为欧拉角。
常见的算法有下面几种：
- 非线性互补滤波算法
- 卡尔曼滤波算法
- Mahony互补滤波算法（Crazepony开源四轴飞行器使用这种）