谢谢楼主啊，这些资料真棒

谢了

使用四元数算法进行计算，下面有代码看一下

使用mpu6050读取的参数，代入到下面的函数，就可以了。

void mix_gyrAcc_crossMethod(quaternion * attitude,const float gyr[3],const float acc[3],float interval)
{
    const static float FACTOR = 0.001;//取接近0的数
    //
    float w_q = attitude->w;
    float x_q = attitude->x;
    float y_q = attitude->y;
    float z_q = attitude->z;
    float x_q_2 = x_q * 2;
    float y_q_2 = y_q * 2;
    float z_q_2 = z_q * 2;
    //
    // 加速度计的读数，单位化。
    float a_rsqrt = math_rsqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
    float x_aa = acc[0] * a_rsqrt;
    float y_aa = acc[1] * a_rsqrt;
    float z_aa = acc[2] * a_rsqrt;   //加速度计测量出的加速度向量(载体坐标系下)
    //
    // 载体坐标下的重力加速度向量，单位化。
    float x_ac = x_q*z_q_2 - w_q*y_q_2;
    float y_ac = y_q*z_q_2 + w_q*x_q_2; //通过四元数旋转矩阵与地理坐标系下的重力加速度向量[0 0 0 1]叉乘得到载体坐标系下的重力加速度向量
    float z_ac = 1 - x_q*x_q_2 - y_q*y_q_2;//（主要）角速度计测出的四元数表示的载体坐标系下的重力加速度向量（这里已转换成载体坐标系下）
    //
    // 测量值与常量的叉积。
    float x_ca = y_aa * z_ac - z_aa * y_ac;
    float y_ca = z_aa * x_ac - x_aa * z_ac;
    float z_ca = x_aa * y_ac - y_aa * x_ac;//角速度计测出的角度误差，叠加的FACTOR大小可以实验试凑
    //
    // 构造增量旋转。
    float delta_x = gyr[0] * interval / 2 + x_ca * FACTOR;
    float delta_y = gyr[1] * interval / 2 + y_ca * FACTOR;
    float delta_z = gyr[2] * interval / 2 + z_ca * FACTOR;
    //
    // 融合，四元数乘法。
    attitude->w = w_q         - x_q*delta_x - y_q*delta_y - z_q*delta_z;
    attitude->x = w_q*delta_x + x_q         + y_q*delta_z - z_q*delta_y;
    attitude->y = w_q*delta_y - x_q*delta_z + y_q         + z_q*delta_x;
    attitude->z = w_q*delta_z + x_q*delta_y - y_q*delta_x + z_q;
    quaternion_normalize(attitude);//归一化
}

参考

mpu6050相关总结 - Powered by Discuz! http://geek-workshop.com/forum.php?mod=viewthread&tid=9146&highlight=MPU6050

这是我之前做过的两轮自平衡车最终版控制程序，你参考一下吧~

这个我写过一篇论文，也刊登了

2.1 角度算法处理的思想

对MPU-6050模块的角度数据处理，算法很多，比如最常见的积分处理，卡尔曼滤波处理等等。本设计充分利用STM32的滴答定时器，采用积分处理。我们知道在误差要求允许范围内对不连续的数据进行求和有时候可以替代积分，本设计就是依据这个思想来实现的。通过STM32的滴答定时器来记录系统运行的时间，然后通过两次运行的时间差，作为一个叠加也就是所谓的积分来处理。通过滴答定时器产生1ms的定时，然后让运行时间变量(RT)1ms加1，从而获得当前程序运行的时间(NT),再通过与上次运行的时间(LT)差值来作为积分数值(TS)，从而可以得出如下两个式子：(a)NT = RT;(b)TS = NT - LT。下面利用这个思想和两个基本式子来进行角度的实际应用。

2.2 积分算法在MPU-6050模块中的运用

通过积分算法和上面的两个式子可以得出相对校准的角度，下面以X轴的角度为例，算法实现的主要步骤如下[1]：

(1)先通过IIC获得角度初始值，ax = GetData(ACCEL_XOUT_H)；

(2)对加速度进行量化，得出单位为g的加速度值-2g量程，Ax=ax/16384.00；

(3)用加速到计算X轴和水平坐标系之间的夹角， Angle_accX= atan(Ax / Az)*180/pi;通过反正切获得弧度值，乘以180度再除以π；

(4)对角速度做量化-2000°量程，ggx=gx/16.38;

(5)对角度进行积分处理，NowTime=RunningTime;获得当前程序运行的时间；TimeSpan=NowTime-LastTime;近似积分；

(6)通过对角度积分实现X轴的角度测量，这里假设X轴的起始角度为0度，则Gx=Gx+(ggx-Gx_offset)*TimeSpan/1000;LastTime=NowTime;(记录当前运行的时间作为下次的减数)。

通过上面6个步骤基本可以获取最终的角度，通过大量的实验和实践证明，该方法可行，而且获得角度也相对准确。下面用一个函数实现上面的思想，程序所涉及的变量定义可以作为全局变量来处理。