姿态解算
1.MPU6050 软件姿态解算。
首先呢，我们在做完智能车比赛之后就萌发了再做一个四旋翼的想法，当然也是想参加来年的全国电设。我们选用MPU6050作为传感器，MPU6050是一款三轴陀螺仪，三轴加速度计芯片，自动反馈三个轴上的角速度和加速度分量。拿这些数据怎么去变成我们所需要的对于四轴的控制量呢，我们需要通过这个6个数据分量融合成三个方向的欧拉角（YAW,ROLL,PITCH）。
这个过程，称之为姿态解算。也就是将MPU读取的三轴加速度和角速度分量融合成三个方向的欧拉角。
最基本的空间姿态解算方法，采用空间坐标系解算，我们知道，空间一个矢量可以正交分解到三个坐标轴上，于是 空间矢量的转动，可以分解到三个坐标轴的转动。（也就是坐标系的变换）。因为三个坐标系是正交的，我们将空间的矢量的转动，分解到三个面上的转动。每个面上都是空间矢量的投影。我们就可以得到一个面上的选择矩阵。
我们不难发现，当空间矢量投影到一个面上进行转动时，可以认为是垂直于这个面的坐标轴的转动（也就是YAW,ROLL,PIT中某一个角度的转动）
依次类推，得到其他两个面的旋转矩阵。
那么空间矢量的总的旋转矩阵就是三个分量矩阵的相乘。（b站视频那个分解图好像有问题）。
那么以上的过程又叫做空间坐标转换，原来的坐标系也就是天南地北坐标系，转换之后的坐标系是机体的坐标系（什么是机体坐标系，就是飞机水平放置的时候，和天地坐标系重合，飞机发生转动的时候偏离了天地坐标系，与机体地面垂直的是机体自身的竖直轴，其他类似）。也就是使用了yaw,roll,pit表示了飞机机体的姿态变化，而且更巧妙的是，这个空间旋转矩阵（以下称方向余弦矩阵）是正交矩阵，我们可以由矩阵推导出机体偏离天地坐标系的yaw,roll,pit。从而根据这个角度，控制四旋翼返回天地坐标系。可是这种 计算方法，特别慢，实际对于STM32来说，不可行。
#通过四元数的姿态结算
这里引入罗德里格旋转，使用原向量（也就是矢量本身），他所绕的旋转轴，和他们俩的叉乘所组成的坐标系来描述矢量的选择。那么旋转之后的矢量和原来矢量之间有一个关系，通过刚体选择矩阵表示，而这个刚体旋转矩阵的元素正好非常巧妙的符合四元数的形式。于是罗德里格选择矩阵可以用四元数表示。那么我们如何求解四元数呢。四元数表示罗德里格选择矩阵。
#求解四元数，
四元数中包含了飞行器姿态信息，姿态信息是随时间变化的，那么四元数肯定也是随时间变化的，我们队四元数求微分方程得到一个四元数与三轴角速度的形式，再使用一阶龙格库塔法，迭代求解这个微分方程。为了计算出正确的角速度（MPU有误差）。我们需要进行传感器数据融合。
#传感器数据融合
得到角度的方式有两种，一种是对角速度积分（比如0.1s采集一次角速度w，就认为这个角度是0.1w°）。或者是对加速度正交得到角度。误差：积分会导致误差累积，是一种低频误差，而加速度计很敏感，会有高频误差。于是对于加速度计数据和角速度数据进行互补滤波（向量外积补偿法）。来得到矫正的角度。
#理论重力加速度分量和实际重力加速度分量
理论重力加速度，使用四元数表示的旋转矩阵将【0,0，1】（理论重力加速度矩阵）进行分解，得到理论重力加速度矢量矩阵（坐标）。而实际的重力加速度我们可以直接从加速度计读取。根据向量叉乘的规则，以及sinx~x我们可以计算出角度误差。
#罗德里格旋转
因为空间矢量的旋转可以分解到三个轴的旋转，那么我们就分别对每个轴进行罗德里格旋转，就可以将空间旋转分解为三个轴的三次罗德里格旋转表示空间旋转矩阵。
罗德里格选择使用矢量和一个旋转轴和他们叉乘的矢量作为坐标系，
#一阶龙格库塔就是把很短时间间隔的数据变化看做是线性的。