四轴思路（持续更新）

遥控器：发送数据
MPU6050：加速度做一维卡尔曼滤波，角速度做一介低通滤波。
然后四元数姿态解算。
接着串级PID控制平衡，外环角度环，内环角速度环。
高度控制：高度和速度进行互补滤波（气压计得到的是高度，速度通过Z轴加速度积分获得，而加速度通过陀螺仪的Z轴得到。）
最后电机输出。

四元数姿态解算公式：

第一行为载体坐标下X方向单位化向量，第一列为x向量，第二列为y向量，第三列为z向量。
第一行为载体坐标下Y方向单位化向量，第一列为x向量，第二列为y向量，第三列为z向量。
第一行为载体坐标下Z方向单位化向量，第一列为x向量，第二列为y向量，第三列为z向量。

龙格库塔求解四元数。。未完待续。。