adapt_bandstop_with_bandpass.m

close all;
% Sample time
dt = 0.01;
% Simulation time
t_end = 40;
% Time vector
t = 0:dt:t_end;
% Actual time-varying frequency vector
w = 3 * 2* pi * ones(size(t));
w(t >=  5 & t < 10) = 5 * 2* pi;
w(t >= 10 & t < 15) = 1 * 2* pi;
w(t >= 15 & t < 20) = 7 * 2* pi;
w(t >= 20 & t < 25) = 6 * 2* pi;
w(t >= 25 & t < 35) = 2 * 2* pi;
w(t >= 35) = 4 * 2* pi;
% Construct the sinusoidal signal
noise_sigma = 2*0.15;
% Input signal amplitude
A = 2;
% Input signal
x = A .* sin(w .* t) + noise_sigma .* randn(size(t));
% Initialize the output signal
y_bp = zeros(size(t));
y = zeros(size(t));
% Initialize the estimated frequency
a_ini = 2* cos(1.5 * 2 * pi * dt);
a_max = 2* cos(0.2 * 2 * pi * dt);
a_est = zeros(size(t));
a_est(1:2) = a_ini;
% define the learning gain
lambda  = 0.8;
% define the stop-band filter's bandwidth
r       = 0.4;
r_bp    = 0.9;
for i = 3:length(t)
    % Band-pass filter to de-noise the raw input signal around the
    % best-guess for the estimated frequency. This filter is linked to the
    % estimated frequency by the Adaptive Stopband Filter.
    
    % This filter can have an adaptive depth parameter to accelerate the
    % convergence with large errors (try to play with different adaptive r
    % parameters)
    r_bp_adapt = r_bp; %max(0.1, r_bp - 0.5 * abs(y_bp(i - 1) - x(i-1)));
    y_bp(i) = (1-r_bp_adapt)*a_est(i - 1) * x(i - 1) + (r_bp_adapt^2 - 1)*x(i - 2) + r_bp_adapt * a_est(i - 1) * y_bp(i - 1) - r_bp_adapt^2 * y_bp(i - 2);
    % Adaptive Band-stop filter 
    y(i)        = y_bp(i) - a_est(i - 1) * y_bp(i - 1) + y_bp(i - 2) + r * a_est(i - 1) * y(i - 1) - r^2 * y(i - 2);
    % beta = diff(y)/diff(a)
    beta        = -y_bp(i - 1) + r * y(i - 1);
    % Update frequency
    a_est(i)    = min(a_max, a_est(i - 1) - lambda * dt * y(i) * beta);
end
% Estimated frequency in Hz
f_est = acos(a_est / 2) / (dt * 2 * pi);
% Plot the results
FontSize = 20;
h = figure;
hh(1) = subplot(2,1,1);
set(hh(1), 'FontSize', FontSize)
hold all;
plot(t, x, 'LineWidth', 2, 'DisplayName', '$$x_n$$')
plot(t, y_bp, 'LineWidth', 2, 'DisplayName', '$$y_{{band-pass}_n}$$')
plot(t, y, 'LineWidth', 2, 'DisplayName', '$$y_n$$')
grid on; box on;
xlabel('$$t [sec]$$', 'Interpreter', 'Latex')
ylabel('$$I/O$$', 'Interpreter', 'Latex')
ylim([-3, +5])
legend('show', 'Interpreter', 'Latex', 'FontSize', FontSize, 'Color', 'none', 'EdgeColor', 'none', 'Location', 'NorthWest');
hh(2) = subplot(2,1,2);
set(hh(2), 'FontSize', FontSize)
hold all;
plot(t, w / 2 / pi, '--', 'LineWidth', 2, 'DisplayName', '$$\omega(x_n)/2/\pi$$')
plot(t, f_est, 'LineWidth', 2, 'DisplayName', '$$\omega_n(stopband)/2/\pi$$')
grid on; box on;
xlabel('$$t [sec]$$', 'Interpreter', 'Latex')
ylabel('$$Frequency [hz]$$', 'Interpreter', 'Latex')
legend('show', 'Interpreter', 'Latex', 'FontSize', FontSize, 'Color', 'none', 'EdgeColor', 'none', 'Location', 'NorthWest');