求一份陀螺仪滤波算法

智心科技

//================================================================================================
//  实现功能：清华滤波方案
//  输入参数：无
//  输出参数：无
//  函数说明：待考察
//================================================================================================
#define GRAVITY_ADJUST_TIME_CONSTANT 4 //重力补偿系数

float GRAVITY_OFFSET  =2453;// 2195    2435   2561             //加速度  z
float GYROSCOPE_OFFSET=1545;// 1574     1568   1997         //陀螺仪  AR  2033

float GRAVITY_ANGLE_RATIO=0.087;        //实际计算值
float DT=0.004;                        //表示为4ms，定时器4MS
float GYROSCOPE_ANGLE_RATIO=0.26;        //预设计算值（待测量）0.45     0.24完美（0.23至0.26）温飘


// max  2890
// min   825
// sub  2065

float g_fCarAngle;
float g_fGravityAngle;
float g_fGyroscopeAngleSpeed;
double g_fGyroscopeAngleIntegral;

void AngleCalculate(float VOLTAGE_GRAVITY,float VOLTAGE_GYRO)
{
        float fDeltaValue;
       
        // 归一处理
        g_fGravityAngle = (GRAVITY_OFFSET - VOLTAGE_GRAVITY) * GRAVITY_ANGLE_RATIO;
        g_fGyroscopeAngleSpeed = (GYROSCOPE_OFFSET -VOLTAGE_GYRO) * GYROSCOPE_ANGLE_RATIO;
       
        // 计算融合角度
        g_fCarAngle = g_fGyroscopeAngleIntegral;
        fDeltaValue = (g_fGravityAngle - g_fCarAngle) / GRAVITY_ADJUST_TIME_CONSTANT;
        //g_fGyroscopeAngleIntegral += (g_fGyroscopeAngleSpeed + fDeltaValue) / GYROSCOPE_ANGLE_SIGMA_FREQUENCY;
        g_fGyroscopeAngleIntegral += (g_fGyroscopeAngleSpeed + fDeltaValue)*DT;
}