陷波器及其算法（基于C语言）

首先，陷波器的传递函数是：

利用matlab对其进行离散化，tustin变换。

syms w0  s Ts z xi   % 定义符号变量
G1 =(s^2+w0^2)/(s^2+2*w0*xi*s+w0^2)         %传递函数
sys_s2c = 2*(z-1)/Ts/(z+1)
G2 = subs(G1,s,sys_s2c)    %离散化  tustin变换
G3 = collect(G2,z)  % 将表达式G2中的以z为变量合并相同次幂；

得传递函数为：

其中：



令：

则有算法：

y(out) = b0*x(k) + b1*(k-1) +b2*(k-2) - a1*y(k-1) - a2*y(k-2);

y(k-2) = y(k-1);
y(k-1) = y(out);

x(k-2) = x(k-1);
x(k-1) = x(k);

记录一下工程项目里的关于陷波器的算法部分。

struct {
	float32 cn1;
	float32 cn2;
	float32 rn1;
	float32 rn2;
//	float32 a0;
	float32 a1;
	float32 a2;
	float32 b0;
	float32 b1;
	float32 b2;
} VboostFilter;
float32 VboostNotchOut;

VboostFilter.a1 = -1.98035;
VboostFilter.a2 = 0.981331;
VboostFilter.b0 = 0.990666;
VboostFilter.b1 = -1.98035;
VboostFilter.b2 = 0.990666;

VboostNotchOut = VboostFilter.b0*ADValue.f32VBoost + VboostFilter.b1*VboostFilter.rn1 + 	    VboostFilter.b2*VboostFilter.rn2- (VboostFilter.a1*VboostFilter.cn1 + VboostFilter.a2*VboostFilter.cn2);

VboostFilter.cn2 = VboostFilter.cn1;
VboostFilter.cn1 = VboostNotchOut;

VboostFilter.rn2 = VboostFilter.rn1;
VboostFilter.rn1 = ADValue.f32VBoost;

还有一篇写的挺好的，函数化；
https://blog.csdn.net/arris1992/article/details/81013519