现在通用的姿态传感器是MPU6050，这种传感器能输出三个加速度acc和三个转动gyro。
有了这几个数据就可以通过数学解算得到飞行器的姿态。
欧拉角
对于任何参考系，一个刚体的取向，是依照顺序，从这参考系，做三个欧拉角的旋转而设定的。所以，刚体的取向可以用三个基本旋转矩阵来决定。换句话说，任何关于刚体旋转的旋转矩阵是由三个基本旋转矩阵复合而成的。
对于在三维空间里的一个参考系，任何坐标系的取向，都可以用三个欧拉角来表现。参考系又称为实验室参考系，是静止不动的。而坐标系则固定于刚体，随着刚体的旋转而旋转。
四元数
参考：http://www.crazepony.com/book/wiki/quaternions.html
- 四元数介绍
我们知道在平面(x,y)中的旋转可以用复数来表示，同样的三维中的旋转可以用单位四元数来描述。我们来定义一个四元数：
我们可以把它写成，其中，。那么V是矢量，表示三维空间中的旋转轴。w是标量，表示旋转角度。那么就是绕轴V旋转w度，所以一个四元数可以表示一个完整的旋转。只有单位四元数才可以表示旋转，至于为什么，因为这就是四元数表示旋转的约束条件。
相对于另几种旋转表示法（矩阵，欧拉角，轴角），四元数具有某些方面的优势，如速度更快、提供平滑插值、有效避免万向锁问题、存储空间较小等等。
姿态解算的核心在于旋转，一般旋转有4种表示方式：矩阵表示、欧拉角表示、轴角表示和四元数表示。矩阵表示适合变换向量，欧拉角最直观，轴角表示则适合几何推导，而在组合旋转方面，四元数表示最佳。因为姿态解算需要频繁组合旋转和用旋转变换向量，所以采用四元数保存组合姿态、辅以矩阵来变换向量的方案。
总结来说，姿态解算中使用四元数来保存飞行器的姿态，包括旋转和方位。在获得四元数之后，会将其转化为欧拉角，然后输入到姿态控制算法中。
姿态控制算法的输入参数必须要是欧拉角。AD值是指MPU6050的陀螺仪和加速度值，3个维度的陀螺仪值和3个维度的加速度值，每个值为16位精度。AD值必须先转化为四元数，然后通过四元数转化为欧拉角。这个四元数可能是软解，主控芯片（STM32）读取到AD值，用软件从AD值算得，也可能是通过MPU6050中的DMP硬解，主控芯片（STM32）直接读取到四元数。
- MPU6050中四元数与欧拉角的关系
数学推导其关系
-
旋转一个角度的坐标变换：
坐标变换 -
欧拉角旋转一个角度
-
引入四元数后，刚才用欧拉角描述的方向余弦矩阵用四元数描述则为：
所以在软件解算中，我们要首先把加速度计采集到的值(三维向量)转化为单位向量,即向量除以模，传入参数是陀螺仪x,y,z值和加速度计x,y,z值：
首先由三个加计的值，转化为单位向量（值除以模）：
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) {
float norm;
float vx, vy, vz;
float ex, ey, ez;
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
由角速度得到四元数，其中qn的初始值是0
// integrate quaternion rate and normalise
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
下面把四元数换算成方向余弦中的第三行的三个元素。刚好vx,vy,vz 其实就是上一次的欧拉角（四元数）的机体坐标参考系换算出来的重力的单位向量。
// estimated direction of gravity
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
- 向量间的误差，可以用向量叉积（也叫向量外积、叉乘）来表示，exyz就是两个重力向量的叉积。
这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。（你可以自己拿东西想象一下）由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。
用叉积误差来做PI修正陀螺零偏
// integral error scaled integral gain
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
- 欧拉角：
Q_ANGLE.Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw
Q_ANGLE.Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Q_ANGLE.Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
整理思路：
- 对加速度进行单位化处理：
//加速度单位化处理
norm = sqrt(ax*ax + ay*ay + az*az);
if(norm==0) return 0;
ax = ax / norm;
ay = ay / norm;
az = az / norm;
把飞行器上次计算得到的姿态（四元数）换算成“方向余弦矩阵”中的第三列的三个元素。根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
- 在机体坐标系上，加速度计测出来的重力向量是ax、ay、az。
由上次姿态解算的姿态（可以简单认为是陀螺积分）来推算出的重力向量是vx、vy、vz。
它们之间的误差向量，就是上次姿态解算（可以认为是陀螺仪积分）后的姿态和加速度计测出来的姿态之间的误差。
//坐标系和重力叉积运算
ex = (ay*vz - az*vy);
ey = (az*vx - ax*vz);
ez = (ax*vy - ay*vx);
//
//误差
exInt = exInt + ex*bs004_quad_Ki;
eyInt = eyInt + ey*bs004_quad_Ki;
ezInt = ezInt + ez*bs004_quad_Ki;
- 用叉乘误差来做PI修正陀螺零偏，通过调节Kp，Ki两个参数，可以控制加速度计修正陀螺仪积分姿态的速度。
//消除误差
gx = gx + bs004_quad_Kp*ex + exInt;
gy = gy + bs004_quad_Kp*ey + eyInt;
gz = gz + bs004_quad_Kp*ez + ezInt;
- 最后得到四元数&欧拉角：
//最终四元数的值
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*bs004_quad_halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*bs004_quad_halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*bs004_quad_halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*bs004_quad_halfT;
//
//四元数单位化
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
if(norm==0) return 0;
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
//
//欧拉角转换
bs004_imu_roll=asin(-2*q1q3 + 2*q0q2)*57.30f;
bs004_imu_pitch=atan2(2*q2q3 + 2*q0q1, -2*q1q1-2*q2q2 + 1)*57.30f;
bs004_imu_yaw=bs004_imu_yaw+2*gz_input/bs004_mpu6050_gyro_scale;
