一阶RC低通滤波算法原理与实现（带系数修改的一阶滤波）

阿让啊

于 2024-05-06 10:07:19 发布

阅读量1k

点赞数 38

分类专栏： IMU 文章标签： c语言 vscode

本文链接：https://blog.csdn.net/kivenx/article/details/138484277

版权

IMU 专栏收录该内容

2 篇文章 0 订阅

订阅专栏

一阶滤波，又叫一阶惯性滤波，或一阶低通滤波，软件实现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。