基于mpu6050的数字滤波算法(修改)

基于mpu6050的数字滤波算法向莹光2015-10-7一阶低通滤波...................................................................................................................................1二阶低通滤波...................................................................................................................................2互补滤波...........................................................................................................................................4卡尔曼滤波.......................................................................................................................................5IIR数字滤波....................................................................................................................................8简述三轴加速度计，三轴陀螺仪mpu6050用了很久，接触过很多应用在6050数据处理上的算法，这里将它们整理起来。造福于大家，同时抛砖引玉，希望大家都能将自己的好东西分享出来。当然只是知识与经验，模块啥的我是不会分享的，你有钱的话，我们可以商量下。这些滤波算法是各有所长，有其特色，现整理将其展现给大家，并加上个人看法与见解，不足之处，希望大家指出，修正使之更完善。同时期盼实验室后浪推前浪，顶起一片天。在介绍滤波器之前，讲明一些东西。Mpu6050的加速度计，具有长期可靠，短期噪声大，加速度计多采用低通滤波。陀螺仪，短时间可靠长期不稳定，故常用积分求角度，对陀螺仪高通滤波。了解这些特性就可以有选择性的去设计滤波器了----------一阶低通滤波先给大家介绍下一阶低通滤波器数学模型的建立吧！（其实是为了装逼嘿嘿！大神请忽视）右图是一阶RC电路，也是硬件一阶低通滤波器微分方程：()()()dytRCytxtdt差分方程：()[(1)]()()ynTynTRCynTxnTT整理得：()[(1)]()RCTynTynTxnTTRCRCT令TaRCT可得：()(1)[(1)]()ynTaynTaxnT系数1/2cTafT截止频率12cfRCC程序源码：/*====================================================================**函数:LPF_1st**功能:一阶低通滤波**备注:deltaT采样周期M_PI=3.1415926Fcut截止频率**函数原型：y(n)=（1-a）y(n-1)+a*x(n)**==================================================================*//*----------------------一阶低通滤波器系数计算-------------------------*/floatLPF_1st_Factor_Cal(floatdeltaT,floatFcut){returndeltaT/(deltaT+1/(2*M_PI*Fcut));}/*----------------------一阶低通滤波器------------------------*/floatLPF_1st(floatoldData,floatnewData,floatlpf_factor){returnoldData*(1-lpf_factor)+newData*lpf_factor;}说明：低通滤波器。Mpu6050内部滤波器的频率典型值为5Hz，我们一般设置42Hz，这样硬件初步滤波，然后在软件在滤波一次，截止频率我设置10Hz，你也可以尝试下5Hz，20Hz，高的就不用设置了，因为硬件已经滤过一次，软件再滤没什么效果了。二阶低通滤波二阶滤波器其实有低通、高通、带通、带阻几种。这里因为时间有限，只整理低通滤波，其他的，大家有兴趣的话，可以帮助完善下。这里就不再详细了，直接上推导公式，当然你兴趣的话，可以帮我完善O(∩_∩)O哈哈~我也可以偷下懒……。先建立二阶RC电路数学模型：222()()()2()()dytdytRCRCytxtdtdt令RC，同时求差分方程，有：22()2[(1)][(2)]()[(1)]+2()()ynTynTynTynTynTynTxnTTT整理得：22222222222()()+[(1)][(2)]222TTynTxnTynTynTTTTTTT令12cafT可得：22222122()()+[(1)][(2)]212121aaaynTxnTynTynTaaaaaaC程序源码：/*==================================================================***函数:LPF_1st**功能:二阶低通滤波**备注:deltaT采样周期M_PI=3.1415926Fcut截止频率LPF2ndData_t二阶低通滤波器系数结构体**==================================================================*//*----------------------二阶低通滤波器系数计算-------------------------*/voidLPF_2nd_Factor_Cal(floatdeltaT,floatFcut,LPF2ndData_t*lpf_data){floata=1/(2*M_PI*Fcut*deltaT);lpf_data-b0=1/(a*a+2*a+1);lpf_data-a1=(2*a*a+2*a)/(a*a+2*a+1);lpf_data-a2=(a*a)/(a*a+2*a+1);}/*----------------------二阶低通滤波器------------------------*/floatLPF_2nd(LPF2ndData_t*lpf_2nd,floatnewData){floatlpf_2nd_data;lpf_2nd_data=newData*lpf_2nd-b0+lpf_2nd-lastout*lpf_2nd-a1-lpf_2nd-preout*lpf_2nd-a2;lpf_2nd-preout=lpf_2nd-lastout;lpf_2nd-lastout=lpf_2nd_data;returnlpf_2nd_data;}说明：二阶低通滤波，相比于一阶低通滤波，对通频带以外的信号抑制能力更强，效果更好。一阶，二阶你都可以试试，那个效果更好用哪个。互补滤波滤波器的只是把两部分数据按权重加起来，使其能输出一个有意义的、准确的线性估计。一般互补滤波用到加速度计与陀螺仪的数据融合，因为其各有优点，可有很好的综合利用。互补滤波知识不难，我就不整理了。有人想帮忙的话，感激不尽！C程序源码：/*==================================================================***函数:CF_1st**功能:一阶互补滤波**备注:deltaT采样周期，tau时间常数，建议值1.2f**==================================================================*//*----------------------互补滤波器系数计算-------------------------*/floatCF_Factor_Cal(floatdeltaT,floattau){returntau/(deltaT+tau);}/*----------------------一阶互补滤波器-----------------------------*/floatCF_1st(floatgyroData,floataccData,floatcf_factor){return(gyroData*cf_factor+accData*(1-cf_factor));}改进型互补滤波：数学模型：*(*)(1)*angleaoldanglegryodtaaccel/*----------------------一阶互补滤波器-----------------------------*/floatCF2_1st(floatoldData,floatgyroData,floataccData,floatcf_factor,floatdeltaT){return((gyroData*deltaT+oldData)*cf_factor+accData*(1-cf_factor));}说明：互补滤波，谁的权重比越大，就是越相信谁。至于上面哪种效果好点，自己用下就知道了，第一种我没用过，改进型互补滤波加入了上一次数据，即前面的结果对现在的结果有影响。在这里搬砖一下比人对该进型滤波的研究。当0.98a时,如果滤波器在每秒执行100次的循环里运行，滤波器的时间常数将会是：0.98*0.01sec0.49sec10.02adta时间常数定义了是该相信陀螺仪还是加速度传感器的界限，当时间周期小于半秒的时候，陀螺仪的积分起主要的作用加速度传感器的噪声将会被滤除，当时间周期大于半秒的时候，加速度传感器要的比重要比陀螺仪大，这时候可能会有漂移。首先，你会定一个时间常数然后用它去计算滤波器系数。根据时间常数可以调整响应的快慢。假如你的陀螺仪每秒漂移2度（当然，可能是最坏的估计），这时为了保证在每个方向的漂移不会超过几度，你可能需要一个小于1秒的时间常数。但是随着时间常数的减小，加速度传感器的噪声就会被更多的引入到系统之中。请记住，要想得到合适的滤波器系数，采样率是很重要的。如果你改变了你的程序，增加了浮点运算，这两个因素的会使采样率就会下降，除非你重新计算你的滤波器条件，否则你的时间常数是不会减小的。卡尔曼滤波整理了，一上午，腰酸背痛的，哎！没假期，还没国庆，以后打死不做程序猿、代码君。当然啦，这只是吐槽！搞科技，高收入，媳妇房子以后不用愁，为了将来孩子的奶粉钱，当爹的受点苦算什么，孩子也肯定是亲生的，你们这群坏银不要乱想…….那么什么是卡尔曼，个人总结一句话，卡尔曼滤波算法就是用上一时刻最优结果估算当前时刻值，再拿这一次的测量值和估算值计算当前最优值（大神勿喷）。卡尔曼算法原理有点高深，我是没懂其精髓。就将别人写的给整理下吧！卡尔曼就几个基本公式：X(k|k-1)=AX(k-1|k-1)+BU(k)………..(1)P(k|k-1)=AP(k-1|k-1)A’+Q………(2)X(k|k)=X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))………(3)Kg(k)=P(k|k-1)H’/(HP(k|k-1)H’+R)………(4)P(k|k)=（I-Kg(k)H）P(k|k-1)………(5)C程序源码：/*===================================