本帖最后由 zhaoyuaiweide 于 2016-11-12 22:00 编辑

最近学习32做了个小作品，其中用到了6050，但是y轴和z轴的波形不太好感觉，滤的波形，请大神们看看我的波形问题在哪里？  基于stm32的摄像头和显示出来的视频中间用一个东西传输（类似于图传的东西吧），大神们@正点原子 有什么建议吗？  多谢了，废话不多说，上波形，上头追视频  ，上滤波代码




// 一阶互补算法
// 经验 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;
}
// 二阶互补算法
float Second_order_filter(float Com2_angle,float angle_pt,float angle,float dt)
{   
    float x1,x2,K = 0.5;      //0.5
        static float y1;
    x1 = (angle - Com2_angle) * K * K;
    y1 = y1 + x1 * dt;
    x2 = y1 + 2 * K *(angle - Com2_angle) + angle_pt;
    Com2_angle = Com2_angle + x2 * dt;
        return Com2_angle;
}
// 卡尔曼滤波
float Kalman_filter(float Klm_angle,float angle_pt,float angle,float dt)
{
        static float bias;
        static float P_00,P_01,P_10,P_11;
        float K_0,K_1;
    Klm_angle += (angle_pt - bias) * dt;       // 先验估计
    P_00 += -(P_10 + P_01) * dt + Q_angle *dt;
    P_01 += -P_11 * dt;
    P_10 += -P_11 * dt;
    P_11 += +Q_omega * dt;                     // 先验估计误差协方差

    K_0 = P_00 / (P_00 + R_angle);
    K_1 = P_10 / (P_00 + R_angle);

    bias += K_1 * (angle - Klm_angle);
    Klm_angle += K_0 * (angle - Klm_angle);    // 后验估计
    P_00 -= K_0 * P_00;
    P_01 -= K_0 * P_01;
    P_10 -= K_1 * P_00;
    P_11 -= K_1 * P_01;                        // 后验估计误差协方差
        return Klm_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;
}