前几天研究学习了下MPU6050的姿态计算。Roll和Pitch角很容易得出，但是Yaw角就莫法直接得出。
因为重力加速度的Z轴，在相对地平面东西南北旋转时并无变化，因此只能靠Z轴的角速度来惯性累计估算旋转角度。
MPU6050仅靠Gryo角加速度来计算水平旋转时的Yaw偏航角度，关键是首先要去除零偏，然后是使用一阶互补积分滤波和卡尔曼滤波平滑数据跳动。

重点是要去除零偏，MPU6050的芯片即使静止放置不动，Gryo角加速度的数据也会不停偏移，必须要除掉这个偏移值。
而滤波算法的重点就是采样率，MPU6050初始化是什么采样率，取数据就得保证这个采样率，最好接上中断脚，用中断读取。
如果采样数据丢多了，角加速度根据算法累加出来的角度就会差很多。

要注意单片机读取问题，很多亲拿出来的数据漂的厉害，要么零偏没有去除，要么就是滤波算法采样有问题，读取数据的采样率不够，跟滤波算法带入的dt积分时间参数对不上。
我STC单片机跑得慢，主频只有30mHz，测试取一次数据要3-4ms左右，设置的分频是0x7，采样速率=1k/(1+7)=125Hz，间隔8ms读一次采样，加上发送的延迟，勉强够。

下面演示实际代码
首先拿到IMU的数据

/* IMU Data */
float gyroX, gyroY, gyroZ;
gyroX = (i2cData[8] << 8) | i2cData[9];
gyroY = (i2cData[10] << 8) | i2cData[11];
gyroZ = (i2cData[12] << 8) | i2cData[13];

gyroX /= 16.384f; // 我的MPU6050初始化的±2000LSB量程 (32768 / 2000) = 16.384f
gyroY /= 16.384f;
gyroZ /= 16.384f;

这里一开始让IMU静止不动，用1秒钟时间统计零偏，
我的采样率是125Hz，dt间隔时间是8ms，也就是统计125次采样做积分来确定零偏值：

btw:
上一篇写的卡尔曼滤波代码里面dt取值实际有问题
按10ms取一次数据，其实角速度数据已经有丢样，另外角速度没有去除零偏值，
所以积分出来的角度漂的很快，因重力加速度计算出来的角度权重比较大，所以最终结果只有卡尔曼滤波出来的角度比较准。
因上篇主要目的是演示卡尔曼滤波，就不回去改了。

static const float dt = 8.0 / 1000;  // 间隔8ms
static const ZERO_OFFSET_COUN (1 / dt) // 1/8=125次每秒

static int g_GetZeroOffset = 0;
static float gyroX_offset = 0.0f, gyroY_offset = 0.0f, gyroZ_offset = 0.0f;

// 间隔8ms一次采样，统计125次，共1秒时间
if (g_GetZeroOffset++ < ZERO_OFFSET_COUN)
{
	gyroX_offset += gyroX * dt;  // 每次8%积分，累计加权125次
	gyroY_offset += gyroY * dt;
	gyroZ_offset += gyroZ * dt;
}

// 除去零偏
gyroX -= gyroX_offset;
gyroY -= gyroY_offset;
gyroZ -= gyroZ_offset;

然后就是靠惯性乘积去拿到XYZ三轴的旋转偏角

static float integralX = 0.0f, integralY = 0.0f, integralZ = 0.0f;
if (g_GetZeroOffset > ZERO_OFFSET_COUN) // 统计完零偏后开始累计偏角
{
	integralX += gyroX * dt;  // 每次8%权重累计偏转角度
	integralY += gyroY * dt;
	integralZ += gyroZ * dt;
	// 360°一个循环
	if (integralX > 360)
		integralX -= 360;
	if (integralX < -360)
		integralX += 360;
	if (integralY > 360)
		integralY -= 360;
	if (integralY < -360)
		integralY += 360;

	if (integralZ > 360)
		integralZ -= 360;
	if (integralZ < -360)
		integralZ += 360;
}

这里integralZ就是Yaw角，物体相对地平面东南西北旋转的角度啦。
板子芯片正面朝天空摆放，逆时针转是正，顺时针转是负。
integralX和integralY分别对应Roll和Pitch角。

去除零偏后，测试板子静止摆放不动几分钟，积分的角度在零点几°范围来回摆。
静止摆10分钟的漂移也只有约1°内。几分钟的使用，按1°取值的精度足够用。
如果需要Yaw角长时间保持不飘，还是需要加磁力计。
只要Roll和Pitch角的数据，跟重力加速度计算出来的姿态角做个互补滤波或者卡尔曼滤波即可。

我看网上很多同志说漂移严重，应该很多都是没去除零偏，或者采样取值有问题，一般都是采样丢数据了。

来回旋转的精度没法精确测试，手拿板子旋转90°，再摆回去，来回转四五次，误差目测在1°内。