智能车制作
标题:
求一份陀螺仪滤波算法
[打印本页]
作者:
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