大神帮忙看下波形为什么不圆润？

zhaoyuaiweide

我dt给的0.02


// 一阶互补算法
// 经验 angle站的比重大一点  数据不跳动更正常
//优点：有效去除加速度计数据的抖动
//缺点：当惯性滤波对于过去的值有较大的权重时，即上式中K值很小时，系统会出现较大的之后效应，当K=0时，完全采用新的加速度计的值，得到的数据抖动较大。
float First_order_filter(float Com_angle,float angle_pt,float angle,float dt)
{
    float A,K = 0.06; //K反应惯性滤波的系数,这个值越小就越和原始波形重合 原始历程数字0.075

    A = K / (K + dt);       //dt不变的情况下，k增加，A增加
    Com_angle = A * (Com_angle + angle_pt * dt) + (1-A) * angle;    //dt越大  加速度转换的角度站的比重越大
        return Com_angle;
}




//加速度计得到角度
//x,y,z:x,y,z方向的重力加速度分量(不需要单位,直接数值即可)
//dir:要获得的角度.0,与Z轴的角度;1,与X轴的角度;2,与Y轴的角度.
//返回值:角度值.单位0.1°.
float get_angle(float x,float y,float z,u8 dir)
{
        float temp;
         float res=0;
        switch(dir)
        {
                case 0://与自然Z轴的角度
                         temp=sqrt((x*x+y*y))/z;
                         res=atan(temp);
                         break;
                case 1://与自然X轴的角度
                         temp=x/sqrt((y*y+z*z));
                         res=atan(temp);
                         break;
                 case 2://与自然Y轴的角度
                         temp=y/sqrt((x*x+z*z));
                         res=atan(temp);
                         break;
         }
        return res*180/3.14159265;
}      


y轴使用加速度计融合的角度算的   z轴因为没有罗盘校准所以就直接用了dmp算出来的yaw；




我的问题是1.一阶滤波是应该让加速度计计算出来的站的比重大点还是陀螺仪部分大点 ？
                 2.dt是不是应该小点？
                 3.我调整了dt和k两个参数，一个不变一个放大很多倍或者给变小很多倍，我的波形仍然有方形，不圆润，请问大神们为什么？？






菜鸟不胜感激大神们！！！

正点原子

帮顶