版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/u011992534/article/details/53557984
/******************************三种滤波函数，每种都调试下，比较动态和静态过程****************************************************/
/*******************下面滤波过程很重要，很重要，很重要，重要的事情说三遍*******************************************************/
/*******************下面滤波过程很重要，很重要，很重要，重要的事情说三遍*******************************************************/
#define dt 0.002
float Angle,Gyro_y; //*************
float Q_angle=0.001;//0.001 //*************
float Q_gyro=0.003;//0.03 //*************
float R_angle=0.5;//0.5 //*************
//float dt=0.01;//0.1 //dt为kalman滤波器采样时间; //*************
char C_0 = 1; //*************
float Q_bias, Angle_err; //*************
float PCt_0, PCt_1, E; //*************
float K_0, K_1, t_0, t_1; //*************
float Pdot[4] ={0,0,0,0}; //*************
float PP[2][2] = { { 1, 0 },{ 0, 1 } }; //*************
float w_kp=1;//0.945 //*************
/*矩阵卡尔曼函数*/ //*************
float Kalman_Filter(float Accel,float Gyro) //*************
{ //*************
Angle+=(Gyro - Q_bias) * 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 = Accel - Angle; //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 += K_0 * Angle_err; //后验估计 //*************
Q_bias += K_1 * Angle_err; //后验估计 //*************
Gyro_y = Gyro - Q_bias; //输出值(后验估计)的微分=角速度 //*************
return Angle; //*************
} //*************
/**************************************************************************************
角度融合
**************************************************************************************/ //*************
float kalman_filter(float angle_m,float gyro_m)//非矩阵卡尔曼滤波 //*************
{ //*************
static float x=0; //*************
static float P=0.000001; //*************
static float Q=0.000001; //*************
static float R=0.35;//0.35 //*************
static float k=0; //*************
gyro_m=W_K*gyro_m;//角速度修正 //*************
x=x+gyro_m*dt; //*************
P=P+Q; //*************
k=P/(P+R); //*************
x=x+k*(angle_m-x); //*************
P=(1-k)*P; //*************
return x; //*************
} //*************
//*************
float GYRO_Integration; //*************
float Complement_Filter(float angle_m,float gyro_m)//互补融合滤波//************* //*************
{ //*************
float Angle_Filter_Temp;//互补融合滤波中间量 //*************
float Angle_Difference_Value;//融合后的角度与加速度静态角度差值，作为反馈量加入积分回路，让静差为0 //*************
//*************
Angle_Filter_Temp=GYRO_Integration; //*************
gyro_m=W_K*gyro_m;//角速度修正 //*************
Angle_Difference_Value=(angle_m-Angle_Filter_Temp)*1.0;//融合后的角度与加速度静态角度差值 //*************
GYRO_Integration=GYRO_Integration+(gyro_m+Angle_Difference_Value )*dt; //*************
return GYRO_Integration; //*************
} //*************
//*************
/************************上面滤波过程很重要，很重要，很重要，重要的事情说三遍**************************************************************/
/************************上面滤波过程很重要，很重要，很重要，重要的事情说三遍**************************************************************/