MPU6050 + 一阶互补滤波+二阶互补滤波+卡尔曼滤波 +波形比较

1、卡尔曼滤波函数

void Kalman_Filter_X(float Accel, float Gyro)
{
	Angle_X_Final += (Gyro - Q_bias_x) * dt;       //先验估计

	Pdot[0] = Q_angle - PP[0][1] - PP[1][0];     //Pk-先验估计误差协方差的微分
	Pdot[1] = -PP[1][1];
	Pdot[2] = -PP[1][1];
	Pdot[3] = Q_gyro;

	PP[0][0] += Pdot[0] * dt;   //Pk-先验估计误差协方差的微分的积分=先验估计误差协方差
	PP[0][1] += Pdot[1] * dt;   
	PP[1][0] += Pdot[2] * dt;
	PP[1][1] += Pdot[3] * dt;

	Angle_err_x = Accel - Angle_X_Final;	//zk-先验估计

	PCt_0 = C_0 * PP[0][0];
	PCt_1 = C_0 * PP[1][0];

	E = R_angle + C_0 * PCt_0;

	K_0 = PCt_0 / E;
	K_1 = PCt_1 / E;

	t_0 = PCt_0;
	t_1 = C_0 * PP[0][1];

	PP[0][0] -= K_0 * t_0;	   //后验估计误差协方差
	PP[0][1] -= K_0 * t_1;
	PP[1][0] -= K_1 * t_0;
	PP[1][1] -= K_1 * t_1;

	Angle_X_Final += K_0 * Angle_err_x;	//后验估计
	Q_bias_x += K_1 * Angle_err_x;	  //后验估计
	Gyro_x = Gyro - Q_bias_x;	 //输出值（后验估计）的微分=角速度
}

2、一、二阶互补滤波函数

void yijiehubu_R(float angle_m, float gyro_m)
{
	float K1 =0.1;
	angle_R = K1 * angle_m + (1-K1) * (angle_R + gyro_m * dt);
	A_R = angle_R;
}

void erjiehubu_P(float angle_m, float gyro_m)
{
	float K = 0.05;
	float y1;
	float x1 = (angle_m - angle_P) * K * K;
	y1 = y1 + x1 * dt;
	float x2 = y1 + 2 * K *(angle_m - angle_P) + gyro_m;
	angle_P = angle_P + x2 * dt;
	A_P = angle_P;
}


float K2 =0.2;
float x1,x2,y1;
float angle2;

void Erjielvbo(float angle_m,float gyro_m)
{
	x1=(angle_m-angle2)*(1-K2)*(1-K2);
	y1=y1+x1*dt;
	x2=y1+2*(1-K2)*(angle_m-angle2)+gyro_m;
	angle2=angle2+ x2*dt;
	A2_P = angle2;
}

3、一阶互补滤波与二阶互补滤波比较

4、原始波形与卡尔曼滤波比较

5、卡尔曼滤波与一阶互补滤波比较

6、二阶互补滤波与卡尔曼滤波比较

7、简单总结

• • 卡尔曼滤波与二阶互补滤波波形基本一致
  • 一阶互补滤波较二阶互补滤波与卡尔曼滤波效果较差
  • 三种波形均无滞后现象