三种陷波滤波代码

 原理贴：常见三种陷波滤波器（Notch Filter）的离散化设计_开飞机的小道士的博客-CSDN博客

clear all;
close all;
clc;

fc = 100;
fbw = 40;

depth = 0.001;%陷波深度衰减100倍，depth范围[-0.707,0.707]
wc = 2 * pi * fc;    % 陷波中央频率，单位：rad/s 
wbw = 2 * pi * fbw;  % 陷波宽度，单位：rad/s 
Ts = 0.001;          % 离散化采样时间，单位：s
a = [1 0 wc^2];
b = [1 wbw wc^2];
sys = tf(a, b);
sysd_tustin = c2d(sys, Ts, 'tustin');
sysd_matched = c2d(sys, Ts, 'matched');

%% tustin test
% a0 = 4 + 4 * pi^2 * fc^2 * Ts^2;
a0 = 4 + wc^2 * Ts^2;
% a1 = 8 * pi^2 * fc^2 * Ts^2 - 8;
a1 = 2 * wc^2 * Ts^2 - 8;
a2 = a0;
b0 = a0 + 4 * pi * fbw * Ts;
b1 = a1;
b2 = a0 - 4 * pi * fbw * Ts;
ad = [a0 a1 a2]./b0;
bd = [b0 b1 b2]./b0;
ab_1 = ad;
bb_1 = bd;
sysd_tustin_test = tf(ad, bd, Ts);

%% zpm test
alpha = -wbw / 2;
beta = sqrt(4*wc^2-wbw^2) / 2;
r = exp(alpha * Ts);
Kz = (1 - 2 * r * cos(beta * Ts) + r^2) / (2 - 2 * cos(wc * Ts));
a0 = Kz;
a1 = -2 * Kz * cos(wc * Ts);
a2 = Kz;
b0 = 1;
b1 = 2 * r * cos(beta * Ts);
b2 = -r^2;
ad = [a0 a1 a2];
bd = [b0 -b1 -b2];
ab_2 = ad;
bb_2 = bd;
sysd_matched_test = tf(ad, bd, Ts);

%% 三参数

B = 2 * pi * wbw;%带宽角速度
k1=sqrt((-sqrt(B*B/(wc*wc)+1)+1)/(4*depth*depth-2));
k1=sqrt(t2/t3);
k2=k1*depth;

a0 = 4+wc^2*Ts^2+4*k2*wc*Ts;
a1 = 2*wc^2*Ts^2-8;
a2 = 4-4*wc*k2*Ts+wc^2*Ts^2;
b0 = 4+wc^2*Ts^2+4*k1*wc*Ts;
b1 = 2*wc^2*Ts^2-8;
b2 = 4-4*wc*k1*Ts+wc^2*Ts^2;

ad = [a0 a1 a2]./b0;
bd = [b0 b1 b2]./b0;
ab_3 = ad;
bb_3 = bd;

% H=tf(ab_3,bb_3,Ts);
% Hc=d2c(H,'tustin');%将离散系统转换为连续系统,
% P=bodeoptions;
% P.Grid='on';
% P.FreqUnits='Hz';
% bode(Hc,P)
% aa = 0;

%% figure
% figure(1);
% P=bodeoptions;
% P.FreqUnits = 'Hz';
% bode(sys, P);
% grid on;
% title('sys');
% 
% figure(2);
% bode(sysd_tustin_test, P);
% grid on;
% title('sysd\_tustin\_test');
% 
% figure(3);
% bode(sysd_matched_test, P);
% grid on;
% title('sysd\_matched\_test');

%% signal test
t = 0:Ts:1;
f0 = 10;
f1 = 5;
f2 = fc;
f3 = 20;

r = sin(2*pi*f0*t) + sin(2*pi*f1*t) + sin(2*pi*f2*t) + sin(2*pi*f3*t);
rig = sin(2*pi*f0*t) + sin(2*pi*f1*t) + sin(2*pi*f3*t);
y_sys_filtered = lsim(sys, r, t);
y_sysd_tustin_filtered = dlsim(sysd_tustin_test.num, sysd_tustin_test.den, r);
y_sysd_matched_filtered = dlsim(sysd_matched_test.num, sysd_matched_test.den, r);



y1 = r;
y2 = r;
y3 = r;
for i = 4:length(r)
    y1(i) = ab_1(1)*r(i)+ab_1(2)*r(i-1)+ab_1(3)*r(i-2) - bb_1(2)*y1(i-1)-bb_1(3)*y1(i-2);   
    y2(i) = ab_2(1)*r(i)+ab_2(2)*r(i-1)+ab_2(3)*r(i-2) - bb_2(2)*y2(i-1)-bb_2(3)*y2(i-2);    
    y3(i) = ab_3(1)*r(i)+ab_3(2)*r(i-1)+ab_3(3)*r(i-2) - bb_3(2)*y3(i-1)-bb_3(3)*y3(i-2);    
end
% figure(4)
% hold on
% plot(r, 'linewidth',3)
% plot(y1, 'linewidth',2)
% 
% figure(5)
% hold on
% plot(r, 'linewidth',3)
% plot(y2, 'linewidth',2)
% 
% 
% figure(6)
% hold on
% plot(r, 'linewidth',3)
% plot(y3, 'linewidth',2)

figure
hold on
grid on
plot(r, 'linewidth',3)
plot(y1, 'linewidth',2)
plot(y2, 'linewidth',2)
plot(y3, 'linewidth',2)
plot(rig,'k--')
legend('r','y1', 'y2', 'y3','rig');

%% display
figure
lw = 2;
plot(t, r);
hold on;
grid on;
plot(t, y_sys_filtered, 'LineWidth',lw);
hold on;
plot(t, y_sysd_tustin_filtered, 'LineWidth',lw);
hold on;
plot(t, y_sysd_matched_filtered, 'LineWidth',lw);
hold on;
legend('Input', 'y\_sys\_filtered', 'y\_sysd\_tustin\_filtered', 'y\_sysd\_matched\_filtered');
title('filtered signal');

figure
hold on
grid on
plot(t,y1)
plot(t, y_sysd_tustin_filtered);
title('DuiBi\_y1\_y\_sysd\_tustin\_filtered')

figure
hold on
grid on
plot(t,y2)
plot(t, y_sysd_matched_filtered);
title('DuiBi\_y2\_y\_sysd\_matched\_filtered')