%% Clear Workspace and Close figures
clear; close all; clc;

%% Intialize Laplace variable
s = zpk('s');

freqs = logspace(-1, 3, 1000);

% Generate Complementary filters with different order               :noexport:
% The weights are generated automatically base on the wanted blending frequency and the order.
% For each weight, the synthesis of the filter is made and the result are displayed figure [[fig:order_filter]].


n_array = 1:3;
Hhs = {zeros(1, length(n_array))};
gammas = zeros(1, length(n_array));

for i = 1:length(n_array)
    f0 = 10; n = n_array(i); alpha = 1;

    G0 = 1e7;
    b = 2*pi*f0/(sqrt((G0/sqrt(2))^(2/n)-1));
    Wh = alpha*G0/(1+s/b)^n;
    b = 2*pi*f0/2^(1/2/n)*sqrt(G0^(2/n)-2^(1/n));
    Wl = alpha*G0*((s/b)/(s/b + 1))^n;
    P = [0   Wh;
         Wl -Wl;
         1   0];
    [Hh, ~, gamma, ~] = hinfsyn(minreal(P), 1, 1,'TOLGAM', 0.001, 'GMAX', 10, 'GMIN', 0.01, 'METHOD', 'ric', 'DISPLAY', 'on');
    Hhs(i) = {Hh};
    gammas(i) = gamma;
end

figure;
% Magnitude
ax1 = subaxis(2,1,1);
hold on;
for i = 1:length(n_array)
    set(gca,'ColorOrderIndex',i);
    plot(freqs, abs(squeeze(freqresp(Hhs{i}, freqs, 'Hz'))));
    set(gca,'ColorOrderIndex',i);
    plot(freqs, abs(squeeze(freqresp(1-Hhs{i}, freqs, 'Hz'))), '--');
end
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude [m/N]');
hold off;
% Phase
ax2 = subaxis(2,1,2);
hold on;
for i = 1:length(n_array)
    set(gca,'ColorOrderIndex',i);
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Hhs{i}, freqs, 'Hz')))), 'DisplayName', sprintf('n = %i', n_array(i)));
    set(gca,'ColorOrderIndex',i);
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(1-Hhs{i}, freqs, 'Hz')))), '--', 'HandleVisibility', 'off');
end
set(gca,'xscale','log');
yticks(-270:90:270);
ylim([-270 270]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
legend('Location', 'northeast');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);

% Analytical formula for complementary filters of 1st, 2nd and 3rd order :noexport:
% The approximate analytical formula for complementary filters of 1st, 2nd and 3rd orders are defined below.
% Their bode plot are shown on figure ref:fig:comp_filters_order.


f0 = 1; % [Hz]

Hh1 = (s/2/pi/f0)/((s/2/pi/f0)+1);
Hl1 = 1/((s/2/pi/f0)+1);

Hh2 = (s/f0)^2*((s/f0)+14.3)/(((s/f0)+2*pi)*((s/f0)^2 + 8*(s/f0) + 40));
Hl2 = 90.4*((s/f0)+2.78)/(((s/f0)+2*pi)*((s/f0)^2 + 8*(s/f0) + 40));

Hh3 = (s/f0)^3*((s/f0)^2 + 20*(s/f0) + 200)/(((s/f0)+2*pi)*((s/f0)^2 + 10*(s/f0) + 40)*((s/f0)^2 + 4*(s/f0) + 40));
Hl3 = 2*pi*200*((s/f0)^2 + 4*(s/f0) + 8)/(((s/f0)+2*pi)*((s/f0)^2 + 10*(s/f0) + 40)*((s/f0)^2 + 4*(s/f0) + 40));

w0 = 2*pi; % [Hz]

Hh1 = (s/w0)/((s/w0)+1);
Hl1 = 1/((s/w0)+1);

Hh2 = (s/w0)^2*((s/w0)+3)/(((s/w0)+1)*((s/w0)^2 + 2*(s/w0) + 1));
Hl2 = (3*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + 2*(s/w0) + 1));

Hh3 = (s/f0)^3*((s/f0)^2 + 20*(s/f0) + 200)/(((s/f0)+2*pi)*((s/f0)^2 + 10*(s/f0) + 40)*((s/f0)^2 + 4*(s/f0) + 40));
Hl3 = 2*pi*200*((s/f0)^2 + 4*(s/f0) + 8)/(((s/f0)+2*pi)*((s/f0)^2 + 10*(s/f0) + 40)*((s/f0)^2 + 4*(s/f0) + 40));

figure;
% Magnitude
ax1 = subaxis(2,1,1);
hold on;
set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(Hh1, freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(Hl1, freqs, 'Hz'))));
set(gca,'ColorOrderIndex',2); plot(freqs, abs(squeeze(freqresp(Hh2, freqs, 'Hz'))));
set(gca,'ColorOrderIndex',2); plot(freqs, abs(squeeze(freqresp(Hl2, freqs, 'Hz'))));
set(gca,'ColorOrderIndex',3); plot(freqs, abs(squeeze(freqresp(Hh3, freqs, 'Hz'))));
set(gca,'ColorOrderIndex',3); plot(freqs, abs(squeeze(freqresp(Hl3, freqs, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude [m/N]');
hold off;
% Phase
ax2 = subaxis(2,1,2);
hold on;
set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(Hh1, freqs, 'Hz'))), 'DisplayName', '1st order');
set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(Hl1, freqs, 'Hz'))), 'HandleVisibility', 'off');
set(gca,'ColorOrderIndex',2); plot(freqs, 180/pi*angle(squeeze(freqresp(Hh2, freqs, 'Hz'))), 'DisplayName', '2nd order');
set(gca,'ColorOrderIndex',2); plot(freqs, 180/pi*angle(squeeze(freqresp(Hl2, freqs, 'Hz'))), 'HandleVisibility', 'off');
set(gca,'ColorOrderIndex',3); plot(freqs, 180/pi*angle(squeeze(freqresp(Hh3, freqs, 'Hz'))), 'DisplayName', '3rd order');
set(gca,'ColorOrderIndex',3); plot(freqs, 180/pi*angle(squeeze(freqresp(Hl3, freqs, 'Hz'))), 'HandleVisibility', 'off');
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
legend('Location', 'northeast');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);



% #+LABEL: fig:comp_filters_order
% #+CAPTION: Bode plot of complementary filters of order 1, 2 and 3
% #+RESULTS: fig:comp_filters_order
% [[file:figs/comp_filters_order.png]]

% For each order, we plot the low pass filter, the high pass filter and the sum of the two to check their complementary properties.

freqs = logspace(-2, 2, 1000);

figure;
ax1=subaxis(1, 3, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(Hl1, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Hh1, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Hl1+Hh1, freqs, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude');
title('1st Order');

ax2=subaxis(1, 3, 2);
hold on;
plot(freqs, abs(squeeze(freqresp(Hl2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Hh2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Hl2+Hh2, freqs, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'YTickLabel',[]);
xlabel('Frequency [Hz]');
title('2nd Order');

ax3=subaxis(1, 3, 3);
hold on;
plot(freqs, abs(squeeze(freqresp(Hl3, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Hh3, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Hl3+Hh3, freqs, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
title('3rd Order');

linkaxes([ax1,ax2,ax3],'x');
linkaxes([ax1,ax2,ax3],'y');
set(gca, 'YTickLabel',[]);



% #+LABEL: fig:comp_filters_magnitude
% #+CAPTION: Magnitude of complementary filters with order 1, 2 and 3
% #+RESULTS: fig:comp_filters_magnitude
% [[file:figs/comp_filters_magnitude.png]]



freqs = logspace(-2, 2, 1000);

figure;
ax1=subaxis(1, 3, 1);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Hl1, freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(Hh1, freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(Hl1+Hh1, freqs, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log');
yticks(-180:90:180);
ylim([-180 180]);
ylabel('Phase [deg]');
title('1st Order');

ax2=subaxis(1, 3, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Hl2, freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(Hh2, freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(Hl2+Hh2, freqs, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log');
set(gca, 'YTickLabel',[]);
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]');
title('2nd Order');

ax3=subaxis(1, 3, 3);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Hl3, freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(Hh3, freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(Hl3+Hh3, freqs, 'Hz'))), 'k--');
hold off;
set(gca, 'XScale', 'log');
set(gca, 'YTickLabel',[]);
title('3rd Order');

yticks(-180:90:180);
ylim([-180 180]);



% #+LABEL: fig:comp_filters_phase
% #+CAPTION: Phase of complementary filters with order 1, 2 and 3
% #+RESULTS: fig:comp_filters_phase
% [[file:figs/comp_filters_phase.png]]

% We then plot the loop gain obtained for each filter in the nominal case $K G^\prime H_L = H_H^{-1} H_L$ (figure ref:fig:comp_filters_loop_gain).


freqs = logspace(-2, 2, 1000);

figure;
% Magnitude
ax1 = subaxis(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(Hl1/Hh1, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Hl2/Hh2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Hl3/Hh3, freqs, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude [m/N]');
ylim([1e-5 1e5]);
hold off;
% Phase
ax2 = subaxis(2,1,2);
hold on;
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Hl1/Hh1, freqs, 'Hz')))), 'DisplayName', '1st order');
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Hl2/Hh2, freqs, 'Hz')))), 'DisplayName', '2nd order');
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Hl3/Hh3, freqs, 'Hz'))))-360, 'DisplayName', '3rd order');
set(gca,'xscale','log');
yticks(-270:90:90);
ylim([-270 90]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
legend('Location', 'northeast');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);



% #+LABEL: fig:comp_filters_loop_gain
% #+CAPTION: Loop gain ${H_H}^{-1}H_L$ for complementary filters with order 1, 2 and 3
% #+RESULTS: fig:comp_filters_loop_gain
% [[file:figs/comp_filters_loop_gain.png]]

% Obtained stability margins are display on table ref:tab:GM_PM_comp_filters.


[gm1, pm1] = margin(Hl1/Hh1);
[gm2, pm2] = margin(Hl2/Hh2);
[gm3, pm3] = margin(Hl3/Hh3);
data = [abs(20*log10(gm1)), pm1;
        abs(20*log10(gm2)), pm2;
        abs(20*log10(gm3)), pm3];

data2orgtable(data, {'1', '2', '3'}, {'Order', 'GM [db]', 'PM [deg]'}, ' %.1f ');

% Result
% Their bode plot is shown Fig. ref:fig:comp_filter_1st_order.


w0 = 2*pi; % [rad/s]

Hh1 = (s/w0)/((s/w0)+1);
Hl1 = 1/((s/w0)+1);

freqs = logspace(-2, 2, 1000);

figure;
% Magnitude
ax1 = subaxis(2,1,1);
hold on;
set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(Hh1, freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(Hl1, freqs, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');
hold off;
% Phase
ax2 = subaxis(2,1,2);
hold on;
set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(Hh1, freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(Hl1, freqs, 'Hz'))));
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Relative Frequency $\frac{\omega}{\omega_0}$'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);



% #+LABEL: fig:comp_filter_1st_order
% #+CAPTION: Bode plot of first order complementary filter
% #+RESULTS: fig:comp_filter_1st_order
% [[file:figs/comp_filter_1st_order.png]]

% The obtain loop gain $L = H_L{H_H}^{-1}$ is shown Fig. ref:fig:comp_filter_1st_order_loop_gain.


figure;
ax1 = subaxis(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(Hl1/Hh1, freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');
% Phase
ax2 = subaxis(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Hl1/Hh1, freqs, 'Hz'))));
hold off;
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Relative Frequency $\frac{\omega}{\omega_0}$'); ylabel('Phase [deg]');
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);

% Result
% Bode plots of those filters for multiple values of $\alpha$ are displayed on figure ref:fig:comp_filter_2nd_order_alphas.

% We also plot the loop gain obtained for different values of $\alpha$: $L = H_L{H_H}^{-1}$ (figure ref:fig:comp_filter_2nd_order_loop_gain)


alphas = [0.1, 1, 10, 100];
w0 = 2*pi*1;

figure;
ax1 = subaxis(2,1,1);
hold on;
for i = 1:length(alphas)
  alpha = alphas(i);
  Hh2 = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
  Hl2 = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Hh2, freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Hl2, freqs, 'Hz'))));
end
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');
hold off;
ylim([1e-4, 20]);
% Phase
ax2 = subaxis(2,1,2);
hold on;
for i = 1:length(alphas)
  alpha = alphas(i);
  Hh2 = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
  Hl2 = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Hh2, freqs, 'Hz'))), 'DisplayName', sprintf('$\\alpha = %g$', alpha));
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Hl2, freqs, 'Hz'))), 'HandleVisibility', 'off');
end
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Relative Frequency $\frac{\omega}{\omega_0}$'); ylabel('Phase [deg]');
legend('Location', 'northeast');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);



% #+LABEL: fig:comp_filter_2nd_order_alphas
% #+CAPTION: Second order complementary filters
% #+RESULTS: fig:comp_filter_2nd_order_alphas
% [[file:figs/comp_filter_2nd_order_alphas.png]]


figure;
ax1 = subaxis(2,1,1);
hold on;
for i = 1:length(alphas)
  alpha = alphas(i);
  Hh2 = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
  Hl2 = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Hl2/Hh2, freqs, 'Hz'))));
end
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
hold off;
% Phase
ax2 = subaxis(2,1,2);
hold on;
for i = 1:length(alphas)
  alpha = alphas(i);
  Hh2 = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
  Hl2 = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Hl2/Hh2, freqs, 'Hz'))), 'DisplayName', sprintf('$\\alpha = %g$', alpha));
end
hold off;
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Relative Frequency $\frac{\omega}{\omega_0}$'); ylabel('Phase [deg]');
legend('Location', 'northeast');
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);

% Parameter Study
% We then study the effect of $\alpha$ on the obtained performance and stability margins (figure ref:fig:comp_filter_2nd_order_study_alphas).


alphas = logspace(-1, 1, 10);

Ms = zeros(1, length(alphas));
dist_reject_w_10 = zeros(1, length(alphas));

for i=1:length(alphas)
  alpha = alphas(i);
  Hh2 = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
  Hl2 = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
  Ms(i) = hinfnorm(Hh2);
  dist_reject_w_10(i) = abs(freqresp(Hh2, w0/10));
end

figure;
ax1 = subplot(1, 2, 1);
plot(alphas, 20*log10(Ms./(Ms-1)));
xlabel('$\alpha$'); ylabel('Guaranted GM $\frac{M_S}{M_S-1}$ [dB]');
set(gca, 'XScale', 'log');
ax2 = subplot(1, 2, 2);
plot(alphas, (360/2/pi)./Ms);
xlabel('$\alpha$'); ylabel('Guaranted PM $\frac{1}{M_S}$ [deg]');
set(gca, 'XScale', 'log');



% #+LABEL: fig:comp_filter_2nd_order_study_alphas
% #+CAPTION: Guaranted GM and PM as a function of $\alpha$
% #+RESULTS: fig:comp_filter_2nd_order_study_alphas
% [[file:figs/comp_filter_2nd_order_study_alphas.png]]



figure;
plot(alphas, 20*log10(1./dist_reject_w_10));
xlabel('$\alpha$'); ylabel('Disturbance Rejection at $\frac{\omega_0}{10} [dB]$');
set(gca, 'XScale', 'log');

% Results

alpha = 1;
beta = 10;
w0 = 2*pi*1;

Hh3 = (s/w0)^3 * ((s/w0)^2 + (1+alpha+beta)*(s/w0) + (1+(alpha+1)*(beta+1)))/((s/w0 + 1)*((s/w0)^2+alpha*(s/w0)+1)*((s/w0)^2+beta*(s/w0)+1));
Hl3 = ((1+(alpha+1)*(beta+1))*(s/w0)^2 + (1+alpha+beta)*(s/w0) + 1)/((s/w0 + 1)*((s/w0)^2+alpha*(s/w0)+1)*((s/w0)^2+beta*(s/w0)+1));

alphas = [0.1, 1, 10, 100];
beta = 10;
w0 = 2*pi*1;

figure;
ax1 = subaxis(2,1,1);
hold on;
for i = 1:length(alphas)
  alpha = alphas(i);
  Hh3 = (s/w0)^3 * ((s/w0)^2 + (1+alpha+beta)*(s/w0) + (1+(alpha+1)*(beta+1)))/((s/w0 + 1)*((s/w0)^2+alpha*(s/w0)+1)*((s/w0)^2+beta*(s/w0)+1));
  Hl3 = ((1+(alpha+1)*(beta+1))*(s/w0)^2 + (1+alpha+beta)*(s/w0) + 1)/((s/w0 + 1)*((s/w0)^2+alpha*(s/w0)+1)*((s/w0)^2+beta*(s/w0)+1));
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Hh3, freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',i);
  plot(freqs, abs(squeeze(freqresp(Hl3, freqs, 'Hz'))));
end
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');
ylim([1e-5, 20]);
hold off;
% Phase
ax2 = subaxis(2,1,2);
hold on;
for i = 1:length(alphas)
  alpha = alphas(i);
  Hh3 = (s/w0)^3 * ((s/w0)^2 + (1+alpha+beta)*(s/w0) + (1+(alpha+1)*(beta+1)))/((s/w0 + 1)*((s/w0)^2+alpha*(s/w0)+1)*((s/w0)^2+beta*(s/w0)+1));
  Hl3 = ((1+(alpha+1)*(beta+1))*(s/w0)^2 + (1+alpha+beta)*(s/w0) + 1)/((s/w0 + 1)*((s/w0)^2+alpha*(s/w0)+1)*((s/w0)^2+beta*(s/w0)+1));
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Hh3, freqs, 'Hz'))), 'DisplayName', sprintf('$\\alpha = %g$', alpha));
  set(gca,'ColorOrderIndex',i);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Hl3, freqs, 'Hz'))), 'HandleVisibility', 'off');
end
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Relative Frequency $\frac{\omega}{\omega_0}$'); ylabel('Phase [deg]');
legend('Location', 'southeast');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);



% #+LABEL: fig:comp_filter_3rd_order
% #+CAPTION: Bode plot of 3rd order complementary filters, $\beta = 10$
% #+RESULTS: fig:comp_filter_3rd_order
% [[file:figs/comp_filter_3rd_order.png]]



figure;
ax1 = subaxis(2,1,1);
hold on;
for i = 1:length(alphas)
  alpha = alphas(i);
  Hh3 = (s/w0)^3 * ((s/w0)^2 + (1+alpha+beta)*(s/w0) + (1+(alpha+1)*(beta+1)))/((s/w0 + 1)*((s/w0)^2+alpha*(s/w0)+1)*((s/w0)^2+beta*(s/w0)+1));
  Hl3 = ((1+(alpha+1)*(beta+1))*(s/w0)^2 + (1+alpha+beta)*(s/w0) + 1)/((s/w0 + 1)*((s/w0)^2+alpha*(s/w0)+1)*((s/w0)^2+beta*(s/w0)+1));
  plot(freqs, abs(squeeze(freqresp(Hl3/Hh3, freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
% Phase
ax2 = subaxis(2,1,2);
hold on;
for i = 1:length(alphas)
  alpha = alphas(i);
  Hh3 = (s/w0)^3 * ((s/w0)^2 + (1+alpha+beta)*(s/w0) + (1+(alpha+1)*(beta+1)))/((s/w0 + 1)*((s/w0)^2+alpha*(s/w0)+1)*((s/w0)^2+beta*(s/w0)+1));
  Hl3 = ((1+(alpha+1)*(beta+1))*(s/w0)^2 + (1+alpha+beta)*(s/w0) + 1)/((s/w0 + 1)*((s/w0)^2+alpha*(s/w0)+1)*((s/w0)^2+beta*(s/w0)+1));
  plot(freqs, 180/pi*angle(squeeze(freqresp(Hl3/Hh3, freqs, 'Hz'))), 'DisplayName', sprintf('$\\alpha = %g$', alpha));
end
hold off;
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
legend('Location', 'northeast');
xlabel('Relative Frequency $\frac{\omega}{\omega_0}$'); ylabel('Phase [deg]');
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);

% Parametric Study

alphas = logspace(-1, 1, 10);
Ms = zeros(1, length(alphas));
noise_reject_w_10 = zeros(1, length(alphas));

for i=1:length(alphas)
  alpha = alphas(i);
  beta = 5*alphas(i);
  Hh3 = (s/w0)^3 * ((s/w0)^2 + (1+alpha+beta)*(s/w0) + (1+(alpha+1)*(beta+1)))/((s/w0 + 1)*((s/w0)^2+alpha*(s/w0)+1)*((s/w0)^2+beta*(s/w0)+1));
  Ms(i) = hinfnorm(Hh3);
  noise_reject_w_10(i) = abs(freqresp(Hh3, w0/10));
end

figure;
ax1 = subplot(1, 3, 1);
plot(alphas, 20*log10(Ms./(Ms-1)));
xlabel('$\alpha$'); ylabel('Guaranted Gain Margin $\frac{M_S}{M_S-1}$ [dB]');
set(gca, 'XScale', 'log');
ax2 = subplot(1, 3, 2);
plot(alphas, (360/2/pi)./Ms);
xlabel('$\alpha$'); ylabel('Guaranted Phase Margin $\frac{1}{M_S}$ [deg]');
set(gca, 'XScale', 'log');
ax3 = subplot(1, 3, 3);
plot(alphas, 20*log10(1./noise_reject_w_10));
xlabel('$\alpha$'); ylabel('Disturbance Rejection at $\frac{\omega_0}{10}$ [dB]');
set(gca, 'XScale', 'log');

% Compare 2nd and 3rd order filters
% Compare performance when having similar stability margins.


alpha = 1.7;
beta = 5*1.7;
Hh3 = (s/w0)^3 * ((s/w0)^2 + (1+alpha+beta)*(s/w0) + (1+(alpha+1)*(beta+1)))/((s/w0 + 1)*((s/w0)^2+alpha*(s/w0)+1)*((s/w0)^2+beta*(s/w0)+1));
Hl3 = ((1+(alpha+1)*(beta+1))*(s/w0)^2 + (1+alpha+beta)*(s/w0) + 1)/((s/w0 + 1)*((s/w0)^2+alpha*(s/w0)+1)*((s/w0)^2+beta*(s/w0)+1));

alpha = 1.4;
Hh2 = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
Hl2 = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));

figure;
ax1 = subaxis(2,1,1);
hold on;
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Hh2, freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Hl2, freqs, 'Hz'))));
set(gca,'ColorOrderIndex',2);
plot(freqs, abs(squeeze(freqresp(Hh3, freqs, 'Hz'))));
set(gca,'ColorOrderIndex',2);
plot(freqs, abs(squeeze(freqresp(Hl3, freqs, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');
ylim([1e-5, 20]);
hold off;
% Phase
ax2 = subaxis(2,1,2);
hold on;
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Hh2, freqs, 'Hz'))), 'DisplayName', '2nd order');
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Hl2, freqs, 'Hz'))), 'HandleVisibility', 'off');
set(gca,'ColorOrderIndex',2);
plot(freqs, 180/pi*angle(squeeze(freqresp(Hh3, freqs, 'Hz'))), 'DisplayName', '3rd order');
set(gca,'ColorOrderIndex',2);
plot(freqs, 180/pi*angle(squeeze(freqresp(Hl3, freqs, 'Hz'))), 'HandleVisibility', 'off');
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Relative Frequency $\frac{\omega}{\omega_0}$'); ylabel('Phase [deg]');
legend('Location', 'southeast');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);



% #+LABEL: fig:filter_order_bode_plot
% #+CAPTION: Bode Plot
% #+RESULTS: fig:filter_order_bode_plot
% [[file:figs/filter_order_bode_plot.png]]



figure;
ax1 = subaxis(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(Hl2/Hh2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Hl3/Hh3, freqs, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');
hold off;
% Phase
ax2 = subaxis(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Hl2/Hh2, freqs, 'Hz'))), 'DisplayName', '2nd order');
plot(freqs, 180/pi*angle(squeeze(freqresp(Hl3/Hh3, freqs, 'Hz'))), 'DisplayName', '3rd order');
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Relative Frequency $\frac{\omega}{\omega_0}$'); ylabel('Phase [deg]');
legend('Location', 'southeast');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
