[资料]

直立车用到的三种融合，在这里都有

2016-1-26 02:02:06 3873 Angle

0 /****************************三种滤波函数，每种都调试下，比较动态和静态过程************************************************/ /***************下面滤波过程很重要，很重要，很重要，重要的事情说三遍***************************************************/ /***************下面滤波过程很重要，很重要，很重要，重要的事情说三遍***************************************************/ 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_Kgyro_m;//角速度修正 //************ x=x+gyro_mdt; //********** 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_Kgyro_m;//角速度修正 //************ Angle_Difference_Value=(angle_m-Angle_Filter_Temp)1.0;//融合后的角度与加速度静态角度差值 //************ GYRO_Integration=GYRO_Integration+(gyro_m+Angle_Difference_Value )dt; //********** return GYRO_Integration; //********* } //********* //********* /********************上面滤波过程很重要，很重要，很重要，重要的事情说三遍**********************************************************/ /********************上面滤波过程很重要，很重要，很重要，重要的事情说三遍************************************************************/ 3 三种融合.zip 1.31 KB , 下载次数: 9
举报淘帖0 只看该作者最近下载过的用户(1) 相关推荐 • 刷机时用到的三种工具分别是什么 1194 • 永磁同步电机三种无速度的方法 1331 • STM32的三种启动模式 1057 • 关于高速ADC的选择与应用你想要的都在这里 2156 • 常见三种接口详解 2999 • 请问这个电感在这里是滤波吗？ 2379 • 你要的自供电都在这里 3408 • 学习中遇到问题可以在这里发帖请教吗 2365 • 为什么在这里下的文件都有损坏 2806 • 为什么在这里下的文件都有损坏 4097 3条评论发表评论只看该作者