Demo entry 6742694

h

   

Submitted by d on May 18, 2018 at 12:04
Language: C++. Code size: 1.0 kB.

volatile float Zangle = 0;  //最终准确角度输出变量定义
volatile float Gyro_Data = 0; //陀螺仪
float Q =1, R =3900;    //调整卡尔曼的滞后 3900 
static float RealData = 0,RealData_P =10000;
float NowData = 0,NowData_P =0 ;
float Kg = 0,gyroscope_rate = 0,gyroscope_rat = 0,accelerometer_angle;
int  Gyro1_zero=0;
void kalman_update(void)
{
   if(zeroflag>1000)   //与开机自检有关,没用到的可以删去     
  {zeroflag=1001;  //确保zeroflag不会溢出
  
   Gyro1_zero=zerosub/1000;   //陀螺仪开机自检累加1000次后取均值 得到陀螺仪零偏值
   Gyro1  = Gyro1 - Gyro1_zero;  //陀螺仪AD采集值减去陀螺仪零偏值        
   Gyro_Data = Gyro1; 
   gyroscope_rate = Gyro1*0.0235*0.005;  //0.0235 是转换角度的比例值 0.005是控制周期
   gyroscope_rat =gyroscope_rat-Gyro1*0.0235*0.005;     //积分角速度得到角度//卡尔曼五个公式的算法实现                                                                             
    NowData = RealData-gyroscope_rate;
    NowData_P = Q+RealData_P;
    Kg = NowData_P/(NowData_P+R);
    RealData = NowData +Kg*(accelerometer_angle - NowData);
    RealData_P =(1-Kg)*NowData_P;
      Zangle =  RealData;   //将准确角度结果给Zangle
      }
}

This snippet took 0.00 seconds to highlight.

Back to the Entry List or Home.

Delete this entry (admin only).