STM32的MPU6050卡尔曼滤波融合数据控制平衡车

最近学习卡尔曼滤波的方法来融合MPU6050的加速度计所得的角度以及陀螺仪的角速度数据。一开始去B站搜视频看原理，然后找到优快云上的一篇博客，参考了初始值的设定后写了下面的滤波算法。 该up主视频讲的很好，视频链接：精通（教你从理论到实践）_哔哩哔哩_bilibili

算法的伪代码如下：

完全按照着伪代码编写的代码：

float dt=0.005;		  //每5ms进行一次滤波                 
float Kalman_Filter_x(float Accel,float Gyro)		
{
    //Accel：通过加速度计计算的角度，Gyro：陀螺仪输出的角速度，这两个值的获取查一查都有

	static float angle;
	float Q_angle=0.001; // 参考别人的的值
	float Q_gyro=0.003;	//参考别人的的值
	float R_angle=0.5;		// 参考别人的的值

	static float Q_bias, Angle_err;
	static float E;
	static float K_0, K_1;
	static float P[2][2] = { { 1, 0 },{ 0, 1 } };//参考别人的初始值

	angle+=(Gyro - Q_bias) * dt; //先验估计

	P[0][0] += Q_angle -( P[0][1] + P[1][0]) * dt;   // Pk-先验估计误差协方差微分的积分
	P[0][1] += -P[1][1] * dt;   // =先验估计误差协方差
	P[1][0] +=-P[1][1] * dt;
	P[1][1] += Q_gyro;
		
	Angle_err = Accel - angle;	//zk-先验估计
	
	
	E = R_angle + P[0][0];
	
	K_0 = P[0][0] / E;
	K_1 = P[1][0] / E;
	

	P[0][0] -= K_0 * P[0][0];		 //后验估计误差协方差
	P[0][1] -= K_0 * P[0][1];
	P[1][0] -= K_1 * P[0][0];
	P[1][1] -= K_1 * P[0][1];
		
	angle	+= K_0 * Angle_err;	 //后验估计
	Q_bias	+= K_1 * Angle_err;	 //后验估计
	
	return angle;
}

写完后用PID来控制小车，发现小车刚开始可以稳定，但是小车后面幅度越晃越大，最后直接失去平衡。

之后参考了另一篇优快云博客的代码，将P[0][0]和P[1][1]的先验估计做了修改，令Q_angle和Q_gyro都乘上dt，相当于对协方差矩阵Q乘上dt。这两个值分别代表了加速度算出来的角度方差以及陀螺仪角速度的方差，两个值都与时间相关，当距离上次更新时间越长，那么这两个方差也会越大，就比如陀螺仪积分出来的角度，时间越长，那么偏差累积也会越大。

其实这个操作也就相当于去对Q_angle和Q_gyro进行调节，取到一个更加适合的值，如果dt=0.05，最终的值其实就是Q_angle = 0.00005, Q_gyro = 0.00015。

修改后代码如下：

float Kalman_Filter_y(float Accel,float Gyro)		
{
    //Accel：通过加速度计计算的角度，Gyro：陀螺仪输出的角速度，这两个值的获取查一查都有

	static float angle;
	float Q_angle=0.001; // 参考别人的的值
	float Q_gyro=0.003;	//参考别人的的值
	float R_angle=0.5;		// 参考别人的的值

	static float Q_bias, Angle_err;
	static float E;
	static float K_0, K_1;
	static float P[2][2] = { { 1, 0 },{ 0, 1 } };//参考别人的初始值

	angle+=(Gyro - Q_bias) * dt; //先验估计

	P[0][0] += ( Q_angle -P[0][1] - P[1][0]) * dt;   // 修改了这里
	P[0][1] += -P[1][1] * dt;   // =先验估计误差协方差
	P[1][0] +=-P[1][1] * dt;
	P[1][1] += Q_gyro* dt;  // 修改了这里
		
	Angle_err = Accel - angle;	//zk-先验估计
	
	
	E = R_angle + P[0][0];
	
	K_0 = P[0][0] / E;
	K_1 = P[1][0] / E;
	

	P[0][0] -= K_0 * P[0][0];		 //后验估计误差协方差
	P[0][1] -= K_0 * P[0][1];
	P[1][0] -= K_1 * P[0][0];
	P[1][1] -= K_1 * P[0][1];
		
	angle	+= K_0 * Angle_err;	 //后验估计
	Q_bias	+= K_1 * Angle_err;	 //后验估计
	
	return angle;
}

上电后小车可以平稳运行了。