本帖最后由 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;
}
大神们帮我看一眼波形指点一下 不胜感激 有兴趣咱们也可以一起交流交流 程序其实也没什么,想要的我就分享啦 取之正点原子,还之正点原子 一起进步
一周热门 更多>