MPU6050姿态解算

代码挺简单，性能和准确度都不错。
Q_FILTER是低通滤波我取0.01，dT是代码执行频率的单位时间

基本原理

可以想象一下，这个芯片里面有六个侧力的秤，组成一个正方体。然后正方体内部放一个大小正好的单位质量小球，这个小球压在哪个侧面上，就会产生对应一个轴的数据输出。比如小球压在下面，这个面就是z轴的正值，小球压上面，这个面就是z轴的负值。对向两个面一组，一面为正值，一面为负值。正方体六个面所测的力就是三个轴的加速度值，这就是加速度计的一个直观理解。

在传感器静止状态下：通过三轴加速度计的数值可以得到一个以mpu6050为参考系的重力方向的向量，通过该向量即可算出，Pitch和Roll。

在传感器非静止状态下：通过角速度计对三轴加速度计静止状态的数据进行补偿，本文使用滤波器的方式。

通过以上方法可以得到Pitch和Roll，Yaw可以结合积分器或者使用其他传感器实现，本文没有提供Yaw的计算方法。