智能车制作

标题: 直立车用到的三种融合，在这里都有 [打印本页]

作者: wustyuyi 时间: 2016-1-17 20:13
标题: 直立车用到的三种融合，在这里都有
/******************************三种滤波函数，每种都调试下，比较动态和静态过程****************************************************/
/*******************下面滤波过程很重要，很重要，很重要，重要的事情说三遍*******************************************************/
/*******************下面滤波过程很重要，很重要，很重要，重要的事情说三遍*******************************************************/
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;    //*************
}    //*************
   //*************
/************************上面滤波过程很重要，很重要，很重要，重要的事情说三遍**************************************************************/
/************************上面滤波过程很重要，很重要，很重要，重要的事情说三遍**************************************************************/

作者: 执子守候 时间: 2016-1-18 08:53