ICM20602互补滤波

实在滤不出数据，直接用原始数据即 fDeltaValue = 0;//不补偿

#define   CONSTANT      2     //补偿系数  减小要适当增大D  （取1到4）
#define   DT          0.044 //积分系数      决定了跟踪速度 0.044
#define   R_Gyro      0.0265  //角速度系数   0.0265
#define   R_Acc       0.046  //角度系数  0.046

float Angle;
float  AngleIntegral;

 void Complementary_Filter(void)
 {

     float  fDeltaValue = 0;                              //补偿量
     static uint8 first_angle=0;
     //角速度（消除零漂）
     ICM_Real.gyro[2] = (ICM_Offset.gyro[2] - icm_gyro_z)*R_Gyro; 
   	//加速度（消除零漂）
   	ICM_Real.acc[1] =  (icm_acc_y-  ICM_Offset.acc[1] )*R_Acc;

   if(!first_angle)//第一次进入初始化
   {
       first_angle=1;
       AngleIntegral=ICM_Real.acc[1];
   }

   //最终融合角度
   Angle = AngleIntegral;  
   
   //时间系数矫正、补偿量
   fDeltaValue = ( ICM_Real.acc[1] - Angle) / CONSTANT;    
   fDeltaValue = 0;//不补偿
   
   //角速度积分融合后的角度
   AngleIntegral += (ICM_Real.gyro[2] + fDeltaValue) * DT; 

 }

滤波代码2

float acc_ratio = 1.2;      //加速度计比例    
float gyro_ratio = 4.2;    //陀螺仪比例  
#define PI 3.141596
#define dt 0.01  //采样周期 陀螺仪角速度积分系数,增长缓慢就增加

float angle_calc(float angle_m, float gyro_m)    
{      
    float temp_angle;               
    float gyro_now;    
    float error_angle; 
	
    static float last_angle;    
	static uint8 first_angle;  
  
    if(!first_angle)//判断是否为第一次运行本函数    
    {    
        //如果是第一次运行，则将上次角度值设置为与加速度值一致    
        first_angle = 1;    
        last_angle = angle_m;    
    }    
    
    //根据测量到的加速度值转换为角度之后与上次的角度值求偏差    
    gyro_now = -gyro_m * gyro_ratio;  
    
    //根据偏差与陀螺仪测量得到的角度值计算当前角度值    
    error_angle = (angle_m - last_angle)*acc_ratio;  
    
    //保存当前角度值    
    temp_angle = last_angle + (error_angle + gyro_now)*dt;      
    last_angle = temp_angle;  
   
    return temp_angle;    
}