二阶滤波算法 C语言实现
时间: 2023-08-20 08:19:30 浏览: 57
以下是一个基于差分方程实现的二阶Butterworth滤波器的C语言代码示例:
```c
typedef struct {
float x_k_2; // x[k-2]
float x_k_1; // x[k-1]
float y_k_2; // y[k-2]
float y_k_1; // y[k-1]
float a1; // 系数a1
float a2; // 系数a2
float b0; // 系数b0
float b1; // 系数b1
float b2; // 系数b2
} SecondOrderFilter;
void initSecondOrderFilter(SecondOrderFilter* filter, float sampleFreq, float cutoffFreq)
{
float wc = 2.0f * PI * cutoffFreq;
float T = 1.0f / sampleFreq;
float k = tan(wc * T / 2.0f);
float k2 = k * k;
float sqrt2 = sqrt(2.0f);
filter->b0 = 1.0f / (1.0f + sqrt2 * k + k2);
filter->b1 = 2.0f * filter->b0;
filter->b2 = filter->b0;
filter->a1 = 2.0f * filter->b0 * (k2 - 1.0f);
filter->a2 = filter->b0 * (1.0f - sqrt2 * k + k2);
filter->x_k_2 = 0.0f;
filter->x_k_1 = 0.0f;
filter->y_k_2 = 0.0f;
filter->y_k_1 = 0.0f;
}
float applySecondOrderFilter(SecondOrderFilter* filter, float x)
{
float y = filter->b0 * x + filter->b1 * filter->x_k_1 + filter->b2 * filter->x_k_2
- filter->a1 * filter->y_k_1 - filter->a2 * filter->y_k_2;
filter->x_k_2 = filter->x_k_1;
filter->x_k_1 = x;
filter->y_k_2 = filter->y_k_1;
filter->y_k_1 = y;
return y;
}
```
代码中,我们定义了一个 `SecondOrderFilter` 结构体,包含了滤波器的状态和系数。在初始化函数 `initSecondOrderFilter` 中,我们根据采样频率 `sampleFreq` 和截止频率 `cutoffFreq` 计算滤波器的系数,并初始化滤波器的状态。在滤波函数 `applySecondOrderFilter` 中,我们根据差分方程计算滤波器的输出,并更新滤波器的状态。
使用时,我们可以先调用 `initSecondOrderFilter` 函数初始化滤波器,然后在采集到的数据中使用 `applySecondOrderFilter` 函数进行滤波处理。