50Hz陷波器C程序源码
几种常用的50Hz陷波器实现，包括IIR和FIR两种类型，适用于嵌入式系统和实时信号处理。
1. 二阶IIR陷波器（最常用）
/**
* @file notch_filter.c
* @brief 50Hz陷波器 - 二阶IIR实现
* @date 2024-01-15
*/
#include <stdio.h>
#include <math.h>
// 陷波器结构体
typedef struct {
float b0, b1, b2; // 分子系数
float a1, a2; // 分母系数（a0=1）
float w1, w2; // 状态变量
float fs; // 采样频率
float f0; // 陷波频率
float r; // 极半径 (0.9-0.99)
} NotchFilter;
/**
* @brief 初始化陷波器
* @param nf 陷波器结构体指针
* @param fs 采样频率(Hz)
* @param f0 陷波频率(Hz)
* @param r 极半径(0.9-0.99，越大陷波越窄)
*/
void notch_init(NotchFilter *nf, float fs, float f0, float r) {
float omega = 2.0f * M_PI * f0 / fs;
nf->fs = fs;
nf->f0 = f0;
nf->r = r;
// 计算系数
nf->b0 = 1.0f;
nf->b1 = -2.0f * cosf(omega);
nf->b2 = 1.0f;
nf->a1 = -2.0f * r * cosf(omega);
nf->a2 = r * r;
// 初始化状态
nf->w1 = 0.0f;
nf->w2 = 0.0f;
}
/**
* @brief 处理单个采样点
* @param nf 陷波器结构体指针
* @param x 输入采样值
* @return 滤波后的输出值
*/
float notch_process(NotchFilter *nf, float x) {
float w0 = x - nf->a1 * nf->w1 - nf->a2 * nf->w2;
float y = nf->b0 * w0 + nf->b1 * nf->w1 + nf->b2 * nf->w2;
// 更新状态
nf->w2 = nf->w1;
nf->w1 = w0;
return y;
}
/**
* @brief 重置陷波器状态
* @param nf 陷波器结构体指针
*/
void notch_reset(NotchFilter *nf) {
nf->w1 = 0.0f;
nf->w2 = 0.0f;
}
/* 使用示例
int main() {
NotchFilter filter;
float fs = 1000.0f; // 1kHz采样率
float f0 = 50.0f; // 50Hz陷波
float r = 0.95f; // 极半径
// 初始化陷波器
notch_init(&filter, fs, f0, r);
// 处理信号
float input_signal[1000];
float output_signal[1000];
for(int i = 0; i < 1000; i++) {
// 假设输入信号包含50Hz干扰
input_signal[i] = sinf(2 * M_PI * 10 * i / fs) + 0.5f * sinf(2 * M_PI * 50 * i / fs);
output_signal[i] = notch_process(&filter, input_signal[i]);
}
return 0;
}
*/
2. 定点版本（适用于无FPU的MCU）
/**
* @file notch_filter_fixed.c
* @brief 定点50Hz陷波器（适用于STM32、51单片机等）
*/
#include <stdint.h>
// 定点格式：Q15格式（16位有符号，小数部分15位）
typedef int16_t q15_t;
typedef int32_t q31_t;
// 定点陷波器结构体
typedef struct {
q15_t b0, b1, b2; // Q15格式系数
q15_t a1, a2; // Q15格式系数
q31_t w1, w2; // Q31格式状态变量
uint8_t shift; // 缩放移位
} NotchFilterFixed;
/**
* @brief 初始化定点陷波器
* @param nf 陷波器结构体指针
* @param fs 采样频率(Hz)
* @param f0 陷波频率(Hz)
* @param r 极半径(0.9-0.99)
*/
void notch_fixed_init(NotchFilterFixed *nf, float fs, float f0, float r) {
float omega = 2.0f * M_PI * f0 / fs;
float b0 = 1.0f;
float b1 = -2.0f * cosf(omega);
float b2 = 1.0f;
float a1 = -2.0f * r * cosf(omega);
float a2 = r * r;
// 转换为Q15格式（系数范围通常在[-2, 2]）
nf->b0 = (q15_t)(b0 * 32767.0f);
nf->b1 = (q15_t)(b1 * 32767.0f);
nf->b2 = (q15_t)(b2 * 32767.0f);
nf->a1 = (q15_t)(a1 * 32767.0f);
nf->a2 = (q15_t)(a2 * 32767.0f);
nf->w1 = 0;
nf->w2 = 0;
nf->shift = 0; // 可根据需要调整
}
/**
* @brief 定点陷波器处理
* @param nf 陷波器结构体指针
* @param x Q15格式输入
* @return Q15格式输出
*/
q15_t notch_fixed_process(NotchFilterFixed *nf, q15_t x) {
q31_t w0, y;
// w0 = x - a1*w1 - a2*w2
w0 = ((q31_t)x << 15) - ((q31_t)nf->a1 * nf->w1 >> 15) - ((q31_t)nf->a2 * nf->w2 >> 15);
// y = b0*w0 + b1*w1 + b2*w2
y = ((q31_t)nf->b0 * w0 >> 15) + ((q31_t)nf->b1 * nf->w1 >> 15) + ((q31_t)nf->b2 * nf->w2 >> 15);
// 更新状态
nf->w2 = nf->w1;
nf->w1 = w0 >> nf->shift;
// 饱和处理
if (y > 32767) y = 32767;
if (y < -32768) y = -32768;
return (q15_t)y;
}
3. FIR陷波器（线性相位）
/**
* @file fir_notch.c
* @brief FIR型50Hz陷波器（线性相位）
*/
#include <math.h>
#define FIR_ORDER 101 // FIR阶数（必须是奇数以保证线性相位）
// FIR陷波器结构体
typedef struct {
float coeffs[FIR_ORDER + 1]; // 滤波器系数
float buffer[FIR_ORDER + 1]; // 环形缓冲区
int index; // 当前缓冲区索引
} FIRNotchFilter;
/**
* @brief 设计FIR陷波器系数（使用窗函数法）
* @param nf 陷波器结构体指针
* @param fs 采样频率(Hz)
* @param f0 陷波频率(Hz)
* @param width 陷波宽度(Hz)
*/
void fir_notch_design(FIRNotchFilter *nf, float fs, float f0, float width) {
int N = FIR_ORDER;
float fc1 = (f0 - width/2) / fs; // 下截止频率
float fc2 = (f0 + width/2) / fs; // 上截止频率
int i;
// 设计带阻滤波器
for (i = 0; i <= N; i++) {
if (i == N/2) {
nf->coeffs[i] = 1.0f - 2.0f * (fc2 - fc1);
} else {
float omega1 = 2.0f * M_PI * fc1 * (i - N/2);
float omega2 = 2.0f * M_PI * fc2 * (i - N/2);
nf->coeffs[i] = (sinf(omega2) - sinf(omega1)) / (M_PI * (i - N/2));
}
// 应用汉明窗
nf->coeffs[i] *= (0.54f - 0.46f * cosf(2.0f * M_PI * i / N));
}
// 归一化
float sum = 0;
for (i = 0; i <= N; i++) {
sum += nf->coeffs[i];
}
for (i = 0; i <= N; i++) {
nf->coeffs[i] /= sum;
}
// 初始化缓冲区
for (i = 0; i <= N; i++) {
nf->buffer[i] = 0.0f;
}
nf->index = 0;
}
/**
* @brief FIR陷波器处理
* @param nf 陷波器结构体指针
* @param x 输入采样值
* @return 滤波后的输出值
*/
float fir_notch_process(FIRNotchFilter *nf, float x) {
int i, j;
float y = 0.0f;
// 存入新样本
nf->buffer[nf->index] = x;
// 计算输出
j = nf->index;
for (i = 0; i <= FIR_ORDER; i++) {
y += nf->coeffs[i] * nf->buffer[j];
j = (j == 0) ? FIR_ORDER : j - 1; // 环形缓冲区索引递减
}
// 更新索引
nf->index = (nf->index == FIR_ORDER) ? 0 : nf->index + 1;
return y;
}
4. 自适应陷波器（LMS算法）
/**
* @file adaptive_notch.c
* @brief 自适应50Hz陷波器（可跟踪频率变化）
*/
#include <math.h>
// 自适应陷波器结构体
typedef struct {
float w1, w2; // 权系数
float mu; // 步长因子
float omega; // 角频率
float x_delay; // 输入延迟
float y_delay; // 输出延迟
} AdaptiveNotch;
/**
* @brief 初始化自适应陷波器
* @param an 自适应陷波器结构体指针
* @param fs 采样频率(Hz)
* @param f0 初始陷波频率(Hz)
* @param mu 步长因子（0.01-0.1）
*/
void adaptive_notch_init(AdaptiveNotch *an, float fs, float f0, float mu) {
an->w1 = 0.0f;
an->w2 = 0.0f;
an->mu = mu;
an->omega = 2.0f * M_PI * f0 / fs;
an->x_delay = 0.0f;
an->y_delay = 0.0f;
}
/**
* @brief 自适应陷波器处理
* @param an 自适应陷波器结构体指针
* @param x 输入采样值
* @return 滤波后的输出值
*/
float adaptive_notch_process(AdaptiveNotch *an, float x) {
float y, e;
// 陷波器输出: y[n] = x[n] - w1*x[n-1] - w2*x[n-2]
y = x - an->w1 * an->x_delay - an->w2 * an->y_delay;
// 误差信号（参考信号）
e = x - y;
// 更新权系数（LMS算法）
an->w1 += an->mu * e * an->x_delay;
an->w2 += an->mu * e * an->y_delay;
// 更新延迟
an->y_delay = y;
an->x_delay = x;
return y;
}
5. 使用示例与测试
/**
* @file main.c
* @brief 50Hz陷波器使用示例
*/
#include <stdio.h>
#include <math.h>
#include <time.h>
// 包含陷波器头文件
#include "notch_filter.c"
// 生成测试信号（包含50Hz干扰）
float generate_test_signal(float t, float fs) {
// 10Hz有用信号 + 50Hz干扰 + 噪声
float signal = sinf(2 * M_PI * 10 * t);
float interference = 0.5f * sinf(2 * M_PI * 50 * t);
float noise = 0.1f * ((float)rand() / RAND_MAX - 0.5f);
return signal + interference + noise;
}
int main() {
// 初始化随机数种子
srand(time(NULL));
// 配置参数
float fs = 1000.0f; // 采样频率1kHz
float f0 = 50.0f; // 50Hz陷波
float r = 0.95f; // 极半径
int num_samples = 1000; // 采样点数
// 创建陷波器
NotchFilter filter;
notch_init(&filter, fs, f0, r);
printf("50Hz陷波器测试开始...\n");
printf("采样频率: %.1f Hz\n", fs);
printf("陷波频率: %.1f Hz\n", f0);
printf("极半径: %.2f\n", r);
printf("\n时间(ms)\t输入\t\t输出\t\t衰减(dB)\n");
// 处理信号
float t = 0.0f;
float dt = 1.0f / fs;
for (int i = 0; i < num_samples; i++) {
// 生成测试信号
float input = generate_test_signal(t, fs);
// 陷波滤波
float output = notch_process(&filter, input);
// 计算衰减（dB）
float attenuation = 20.0f * log10f(fabsf(output) / (fabsf(input) + 1e-6f));
// 打印前20个点
if (i < 20) {
printf("%.1f\t\t%.4f\t\t%.4f\t\t%.1f\n",
t * 1000, input, output, attenuation);
}
t += dt;
}
printf("\n测试完成！\n");
return 0;
}
参考代码 50HZ陷波器的C程序源码 www.youwenfan.com/contentcnu/60216.html
6. 优化
/**
* @file optimization_tips.c
* @brief 陷波器性能优化建议
*/
// 1. 查表法替代实时计算三角函数
static const float cos_table[256] = {
// 预计算的余弦值表
};
// 2. 使用SIMD指令（如果支持）
#ifdef __ARM_NEON
#include <arm_neon.h>
void neon_notch_process(float *input, float *output, int len) {
// NEON优化实现
}
#endif
// 3. 循环展开
void unrolled_notch_process(NotchFilter *nf, float *input, float *output, int len) {
int i;
for (i = 0; i < len - 3; i += 4) {
output[i] = notch_process(nf, input[i]);
output[i+1] = notch_process(nf, input[i+1]);
output[i+2] = notch_process(nf, input[i+2]);
output[i+3] = notch_process(nf, input[i+3]);
}
// 处理剩余样本
for (; i < len; i++) {
output[i] = notch_process(nf, input[i]);
}
}
// 4. 内存对齐
__attribute__((aligned(16))) float aligned_buffer[1024];
选择：
| 应用场景 | 推荐方案 | 优点 | 缺点 |
|---|---|---|---|
| 通用嵌入式 | 二阶IIR | 计算量小，效果好 | 相位非线性 |
| 音频处理 | FIR陷波器 | 线性相位，无失真 | 延迟大，计算量大 |
| 电网监测 | 自适应陷波器 | 跟踪频率漂移 | 实现复杂 |
| 无FPU的MCU | 定点IIR | 速度快，资源占用少 | 精度有限 |
注意：应用中，50Hz工频干扰通常还伴随高次谐波（100Hz、150Hz等），可能需要多个陷波器级联使用。
浙公网安备 33010602011771号