一阶滤波,又叫一阶惯性滤波,或一阶低通滤波,软件实现RC低通滤波器的功能。
式中:α为滤波系数,X ( n ) 为本次采样值,Y ( n − 1 ) 为上次滤波输出值,Y ( n ) 为本次滤波输出值。
一阶滤波算法的特点
-
对于周期干扰有良好的抑制作用(优)
-
带来了相位滞后,导致灵敏度低(缺)
-
不能滤除频率高于采样频率的二分之一(称为奈奎斯特频率)的干扰(例如采样频率为100Hz,则它不能滤除50Hz以上的干扰信号)(缺)
-
滤波系数越小,滤波结果越平稳,灵敏度越低
-
滤波系数越大,灵敏度越高,但滤波结果越不稳定
一阶滤波无法完美地兼顾灵敏度和平稳度。有时,我们只能寻找一个平衡,在可接受的灵敏度范围内取得尽可能好的平稳度。
#define a 0.01 // 滤波系数a(0-1)
static float oldOutData = 0.0f;
float filter(void)
{
float nowdata = 0.0f;
nowData = get_Data(); //读取到的最新数据
nowOutData = a * nowData + (1.0f - a) * oldOutData;
oldOutData = nowOutData;
return nowOutData;
}
基于一阶滤波无法完美地兼顾灵敏度和平稳度,故此需要能够动态调整滤波系数,实现带系数修改的一阶滤波函数(一阶RC低通滤波)。
以下为实现原则:
实现功能:
-
当数据快速变化时,滤波结果能及时跟进,并且数据的变化越快,灵敏度应该越高(灵敏度优先原则)
-
当数据趋于稳定,并在一个范围内振荡时,滤波结果能趋于平稳(平稳度优先原则)
-
当数据稳定后,滤波结果能逼近并最终等于采样数据(消除因计算中小数带来的误差)
调整前判断:
-
数据变化方向是否为同一个方向(如当连续两次的采样值都比其上次滤波结果大时,视为变化方向一致,否则视为不一致)
-
数据变化是否较快(主要是判断采样值和上一次滤波结果之间的差值)
调整原则:
-
当两次数据变化不一致时,说明有抖动,将滤波系数清零,忽略本次新采样值
-
当数据持续向一个方向变化时,逐渐提高滤波系数,提供本次采样值得权;
-
当数据变化较快(差值>消抖计数加速反应阈值)时,要加速提高滤波系数
-
/一阶低通滤波参数 #define Threshold_1 8 //阈值1用于一阶带参滤波器,变化角度大于此值时,计数增加 #define Threshold_2 30 //阈值2用于一阶带参滤波器,计数值大于此值时,增大参数,增强滤波跟随 static char new_flag = 0;//本次数据变化方向 static float K_x=0; //滤波系数 static char num_x=0;//滤波计数器 static float k_k = 0.15; //滤波系数(代表在滤波结果中的权重) static char filter_flag_X = 0; //上次数据变化方向 static char filter_flag_Y = 0; static char filter_flag_Z = 0; static float OLD_DATA_X = 0; //存储上次滤波角度结果 static float OLD_DATA_Y = 0; static float OLD_DATA_Z = 0; /*****带系数修改的一阶滤波函数(一阶RC低通滤波) 输入:NEW_DATA 新采样的角度值 OLD_DATA 上次滤波获得的角度结果 filter_flag 上次数据变化方向 输出:result 本次滤波角度结果 */ float filter_RC(float NEW_DATA,float* OLD_DATA,char* filter_flag) { //角度变化方向,new_flag=1表示角度增加,=0表示角度正在减小 if((NEW_DATA-(*OLD_DATA))>0) new_flag=1; else if((NEW_DATA-(*OLD_DATA))<0) new_flag=0; if(new_flag==(*filter_flag)) //此次变化与前一次变化方向是否一致,相等表示角度变化方向一致 { num_x++; if(fabsf((NEW_DATA-(*OLD_DATA)))>Threshold_1) //当变化角度大于Threshold_1度的时候,进行计数器num快速增加,以达到快速增大K值,提高跟随性 num_x+=5; if(num_x>Threshold_2) //计数阈值设置,当角度递增或递减速度达到一定速率时,增大K值 { K_x=k_k+0.2; //0.2为K_x的增长值,看实际需要修改 num_x=0; } } else { num_x=0; K_x=0.01; //角度变化稳定时K_x值,看实际修改 } (*OLD_DATA) =(1-K_x)*(*OLD_DATA)+K_x*NEW_DATA; (*filter_flag) = new_flag; return (*OLD_DATA); }
最终改进算法,兼顾了灵敏度和平稳度的要求;同时又不太消耗系统的RAM。
-
设计过程和基础代码参考了古月居大佬,特此致谢!