平衡滤波是这样,怎么调陀螺仪的比例系数都不能很好的跟踪加速度计的值

qq904305159

程序:
float get_UpstandAngle()
{

    float g_fCarAngle=0.0;
        float fDeltaValue;

    g_acc=get_AccAngle();                      //加速度计值                                       
    anglespeed=get_AngleSpeed(Gyrox);          //陀螺仪值                               

    g_fCarAngle=g_fGyroscopeAngleIntegral;

    fDeltaValue=(g_acc-g_fCarAngle)/GRAVITY_ADJUST_TIME_CONSTANT;//补偿系数为0.2
    g_fGyroscopeAngleIntegral+=(anglespeed+fDeltaValue)/GYROSOCOPE_ANGLE_SIGME_FREQUENCY;//采样平率为200
                                                                                                               
    return (float)g_fCarAngle;
}


中断定时是5ms,是哪里出了问题,该怎么调啊?

qq904305159

红色是加速度计,黄色是陀螺仪,粉红色是融合后的角度

浅笑无痕

角速度为正反馈  试着把角速度反号  然后调整陀螺仪比例因子和积分时间

qq904305159

浅笑无痕 发表于 2016-2-27 13:34
角速度为正反馈  试着把角速度反号  然后调整陀螺仪比例因子和积分时间

积分时间是不是"GYROSOCOPE_ANGLE_SIGME_FREQUENCY",调整了它,定时的5ms要改变吗?

感觉现在变好了点呢!

浅笑无痕

qq904305159 发表于 2016-2-27 13:40
积分时间是不是"GYROSOCOPE_ANGLE_SIGME_FREQUENCY",调整了它,定时的5ms要改变吗?

感觉现在变好了点呢 ...

还是有问题的 你的角速度还是没反号   角速度 和你融合的角度变化方向是一致的  懂？？？

浅笑无痕

qq904305159 发表于 2016-2-27 13:40
积分时间是不是"GYROSOCOPE_ANGLE_SIGME_FREQUENCY",调整了它,定时的5ms要改变吗?

感觉现在变好了点呢 ...

已经改了哦，那你只要稍微调整陀螺仪比例因子了，做到跟随加速度计 不过冲 跟随快就行

qq904305159

浅笑无痕 发表于 2016-2-27 14:00
已经改了哦，那你只要稍微调整陀螺仪比例因子了，做到跟随加速度计 不过冲 跟随快就行

有两个地方不管是增大比例因子还是减小比例因子感觉总是调不好,我在图上标出来了,这里该怎么调呢?

浅笑无痕

时间常数可以1-4之间改变  也就是0.25-1   积分频率看你采样时间 5ms的话就是0.005附近改变   再就是稍微大点改变陀螺仪的比例因子