智能车制作

标题: 求一份陀螺仪滤波算法 [打印本页]

作者: one_SH55Y    时间: 2017-3-6 16:55
标题: 求一份陀螺仪滤波算法
大家谁有清华的滤波算法,给一份呗!648671505@qq.com

作者: 失心恋2015    时间: 2017-3-6 21:42
同求  1510347480@qq.com
作者: 智心科技    时间: 2017-3-9 01:08
//================================================================================================
//  实现功能:清华滤波方案
//  输入参数:无
//  输出参数:无
//  函数说明:待考察
//================================================================================================
#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;
}




欢迎光临 智能车制作 (http://111.231.132.190/) Powered by Discuz! X3.2