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

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

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

% Definition of the plant
%   <<sec:plant>>

% The studied system consists of a solid positioned on top of a motorized uni-axial soft suspension.

% The absolute position $x$ of the solid is measured using an inertial sensor and a force $F$ can be applied to the mass using a voice coil actuator.

% The model of the system is represented on figure [[fig:mech_sys_alone]] where the mass of the solid is $m = 20\ [kg]$, the stiffness of the suspension is $k = 10^4\ [N/m]$ and the damping of the system is $c = 10^2\ [N/(m/s)]$.

% #+name: fig:mech_sys_alone
% #+caption: One degree of freedom system
% [[file:figs/mech_sys_alone.png]]

% The plant $G$ is defined on matlab and its bode plot is shown on figure [[fig:bode_plot_mech_sys]].


m = 20;  % [kg]
k = 1e4; % [N/m]
c = 1e2; % [N/(m/s)]

G = 1/(m*s^2 + c*s + k);

figure;
ax1 = subaxis(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(G, freqs, 'Hz'))), 'k-');
hold off;
xlim([0.1, 100]);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude [m/N]');

ax2 = subaxis(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G, freqs, 'Hz'))), 'k-');
hold off;
yticks(-180:90:180);
ylim([-180 180]);
xlim([0.1, 1000]);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
linkaxes([ax1,ax2],'x');

% Multiplicative input uncertainty
%   <<sec:uncertainty>>
% We choose to use the multiplicative input uncertainty to model the plant uncertainty:
% \[ \Pi_I: \ G^\prime(s) = G(s) (1 + w_I(s) \Delta(s)),\text{ with } |\Delta(j\omega)| < 1 \ \forall \omega \]


% The uncertainty weight $w_I$ has the following form:
% \[ w_I(s) = \frac{\tau s + r_0}{(\tau/r_\infty) s + 1} \]
% where $r_0=0.1$ is the relative uncertainty at steady-state, $1/\tau=80\text{Hz}$ is the frequency at which the relative uncertainty reaches 100%, and $r_\infty=10$ is the magnitude of the weight at high frequency.

% We defined the uncertainty weight on matlab. Its bode plot is shown on figure [[fig:bode_wi]].


r0 = 0.1;
rinf = 10;
tau = 1/2/pi/80;

wI = (tau*s + r0)/((tau/rinf)*s+1);

figure;
hold on;
plot(freqs, abs(squeeze(freqresp(wI, freqs, 'Hz'))), 'k-');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
xlim([freqs(1), freqs(end)]);
ylim([1e-1, 10]);
xticks([0.1, 1, 10, 100, 1000])



% #+NAME: fig:bode_wi
% #+CAPTION: Bode plot of $w_I$ ([[./figs/bode_wi.png][png]], [[./figs/bode_wi.pdf][pdf]])
% [[file:figs/bode_wi.png]]

% The uncertain model is created with the =ultidyn= function. Elements in the uncertainty set $\Pi_I$ are computed and their bode plot is shown on figure [[fig:plant_uncertainty_bode_plot]].


Delta = ultidyn('Delta', [1 1]);

Gd = G*(1+wI*Delta);
Gds = usample(Gd, 20);

figure;
ax1 = subplot(2,1,1);
hold on;
for i=1:length(Gds)
    plot(freqs, abs(squeeze(freqresp(Gds(:, :, i), freqs, 'Hz'))), '-', 'color', [0, 0, 0, 0.1]);
end
plot(freqs, abs(squeeze(freqresp(Gd, freqs, 'Hz'))), 'k-');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude [m/N]');
hold off;
% Phase
ax2 = subplot(2,1,2);
hold on;
for i=1:length(Gds)
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gds(:, :, i), freqs, 'Hz')))), '-', 'color', [0, 0, 0, 0.1]);
end
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G, freqs, 'Hz')))), 'k-');
set(gca,'xscale','log');
yticks(-360:90:180);
ylim([-360 0]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
xticks([0.1, 1, 10, 100, 1000])

% Specifications and performance weights
%   <<sec:specifications>>

% The control objective is to isolate the displacement $x$ of the mass from the ground motion $w$.

% The specifications are described below:
% - at least a factor $10$ of disturbance rejection at $2\ \text{Hz}$ and with a slope of $2$ below $2\ \text{Hz}$ until a rejection of $10^3$
% - the noise attenuation should be at least $10$ above $100\ \text{Hz}$ and with a slope of $-2$ above

% These specifications can be represented as upper bounds on the closed loop transfer functions $S$ and $T$ (see figure [[fig:bode_requirements]]).


figure;
hold on;
set(gca,'ColorOrderIndex',1)
plot([100, 1000], [0.1, 0.001], ':', 'DisplayName', '$|T|$ - Upper bound');
set(gca,'ColorOrderIndex',2)
plot([0.1, 0.2, 2], [0.001, 0.001, 0.1], ':', 'DisplayName', '$|S|$ - Upper bound');

set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
xlim([freqs(1), freqs(end)]);
ylim([1e-3, 10]);
xticks([0.1, 1, 10, 100, 1000]);
legend('location', 'northeast');



% #+NAME: fig:bode_requirements
% #+CAPTION: Upper bounds on $S$ and $T$ ([[./figs/bode_requirements.png][png]], [[./figs/bode_requirements.pdf][pdf]])
% [[file:figs/bode_requirements.png]]

% We now define two weights, $w_S(s)$ and $w_T(s)$ such that $1/|w_S|$ and $1/|w_T|$ are lower than the previously defined upper bounds.
% Then, the performance specifications are satisfied if the following condition is valid:
% \[ \big|S(j\omega)\big| < \frac{1}{|w_S(j\omega)|} ; \quad \big|T(j\omega)\big| < \frac{1}{|w_T(j\omega)|}, \quad \forall \omega \]

% The weights are defined as follow. They magnitude is compared with the upper bounds on $S$ and $T$ on figure [[fig:compare_weights_upper_bounds_S_T]].

wS = 1600/(s+0.13)^2;
wT = 1000*((s/(2*pi*1000)))^2;

figure;
hold on;
set(gca,'ColorOrderIndex',1)
plot(freqs, 1./abs(squeeze(freqresp(wT, freqs, 'Hz'))), '--', 'DisplayName', '$1/|w_T|$');
set(gca,'ColorOrderIndex',1)
plot([100, 1000], [0.1, 0.001], ':', 'DisplayName', '$|T|$ - Upper bound');
set(gca,'ColorOrderIndex',2)
plot(freqs, 1./abs(squeeze(freqresp(wS, freqs, 'Hz'))), '--', 'DisplayName', '$1/|w_S|$');
set(gca,'ColorOrderIndex',2)
plot([0.1, 0.2, 2], [0.001, 0.001, 0.1], ':', 'DisplayName', '$|S|$ - Upper bound');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
xlim([freqs(1), freqs(end)]);
ylim([1e-4, 10]);
xticks([0.1, 1, 10, 100, 1000]);
legend('location', 'northeast');

% Upper bounds on the norm of the complementary filters for NP, RS and RP
%   <<sec:upper_bounds_filters>>

% Now that we have defined $w_I$, $w_S$ and $w_T$, we can derive conditions for Nominal Performance, Robust Stability and Robust Performance ($j\omega$ is omitted here for readability):
% \begin{align*}
%   \text{NP} &\Leftrightarrow |H_H| < \frac{1}{|w_S|} \text{ and } |H_L| < \frac{1}{|w_T|} \quad \forall \omega \\
%   \text{RS} &\Leftrightarrow |H_L| < \frac{1}{|w_I| (2 + |w_I|)} \quad \forall \omega \\
%   \text{RP for } S &\Leftarrow |H_H| < \frac{1 + |w_I|}{|w_S| (2 + |w_I|)} \quad \forall \omega \\
%   \text{RP for } T &\Leftrightarrow |H_L| < \frac{1}{|w_T| (1 + |w_I|) + |w_I|} \quad \forall \omega
% \end{align*}

% These conditions are upper bounds on the complementary filters used for control.

% We plot these conditions on figure [[fig:weights_NP_RS_RP]].


figure;
hold on;
set(gca,'ColorOrderIndex',1)
plot(freqs, 1./abs(squeeze(freqresp(wT, freqs, 'Hz'))), '--', 'DisplayName', 'NP - $H_L$');
set(gca,'ColorOrderIndex',1)
plot(freqs, 1./(abs(squeeze(freqresp(wT, freqs, 'Hz'))) .* (1 + abs(squeeze(freqresp(wI, freqs, 'Hz')))) + abs(squeeze(freqresp(wI, freqs, 'Hz')))), ':', 'DisplayName', 'RP for T - $H_L$');
set(gca,'ColorOrderIndex',1)
plot(freqs, 1./(abs(squeeze(freqresp(wI, freqs, 'Hz'))) .* (2 + abs(squeeze(freqresp(wI, freqs, 'Hz'))))), '-.', 'DisplayName', 'RS - $H_L$');
set(gca,'ColorOrderIndex',2)
plot(freqs, 1./abs(squeeze(freqresp(wS, freqs, 'Hz'))), '--', 'DisplayName', 'NP - $H_H$');
set(gca,'ColorOrderIndex',2)
plot(freqs, (1 + abs(squeeze(freqresp(wI, freqs, 'Hz'))))./(abs(squeeze(freqresp(wS, freqs, 'Hz'))) .* (2 + abs(squeeze(freqresp(wI, freqs, 'Hz'))))), ':', 'DisplayName', 'RP for S - $H_H$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
xlim([freqs(1), freqs(end)]);
ylim([1e-3, 10]);
xticks([0.1, 1, 10, 100, 1000]);
legend('location', 'northeast');

% H-Infinity synthesis of complementary filters
%   <<sec:h_infinity>>

% We here synthesize the complementary filters using the $\mathcal{H}_\infty$ synthesis.
% The goal is to specify upper bounds on the norms of $H_L$ and $H_H$ while ensuring their complementary property ($H_L + H_H = 1$).

% In order to do so, we use the generalized plant shown on figure [[fig:sf_hinf_filters_plant_b]] where $w_L$ and $w_H$ weighting transfer functions that will be used to shape $H_L$ and $H_H$ respectively.

% #+name: fig:sf_hinf_filters_plant_b
% #+caption: Generalized plant used for the $\mathcal{H}_\infty$ synthesis of the complementary filters
% [[file:figs/sf_hinf_filters_plant_b.png]]

% The $\mathcal{H}_\infty$ synthesis applied on this generalized plant will give a transfer function $H_L$ (figure [[fig:sf_hinf_filters_b]]) such that the $\mathcal{H}_\infty$ norm of the transfer function from $w$ to $[z_H,\ z_L]$ is less than one:
% \[ \left\| \begin{array}{c} H_L w_L \\ (1 - H_L) w_H \end{array} \right\|_\infty < 1 \]

% Thus, if the above condition is verified, we can define $H_H = 1 - H_L$ and we have that:
% \[ \left\| \begin{array}{c} H_L w_L \\ H_H w_H \end{array} \right\|_\infty < 1 \]
% Which is almost (with an maximum error of $\sqrt{2}$) equivalent to:
% \begin{align*}
%   |H_L| &< \frac{1}{|w_L|}, \quad \forall \omega \\
%   |H_H| &< \frac{1}{|w_H|}, \quad \forall \omega
% \end{align*}

% We then see that $w_L$ and $w_H$ can be used to shape both $H_L$ and $H_H$ while ensuring (by definition of $H_H = 1 - H_L$) their complementary property.

% #+name: fig:sf_hinf_filters_b
% #+caption: $\mathcal{H}_\infty$ synthesis of the complementary filters
% [[file:figs/sf_hinf_filters_b.png]]


% Thus, if we choose $w_L$ and $w_H$ such that $1/|w_L|$ and $1/|w_H|$ lie below the upper bounds of figure [[fig:weights_NP_RS_RP]], we will ensure the NP, RS and RP of the controlled system.

% Depending if we are interested only in NP, RS or RP, we can adjust the weights $w_L$ and $w_H$.


omegab = 2*pi*9;
wH = (omegab)^2/(s + omegab*sqrt(1e-5))^2;
omegab = 2*pi*28;
wL = (s + omegab/(4.5)^(1/3))^3/(s*(1e-4)^(1/3) + omegab)^3;

figure;
hold on;
set(gca,'ColorOrderIndex',1)
plot(freqs, 1./abs(squeeze(freqresp(wT, freqs, 'Hz'))), '--', 'DisplayName', 'NP - $H_L$');
set(gca,'ColorOrderIndex',1)
plot(freqs, 1./(abs(squeeze(freqresp(wT, freqs, 'Hz'))) .* (1 + abs(squeeze(freqresp(wI, freqs, 'Hz')))) + abs(squeeze(freqresp(wI, freqs, 'Hz')))), ':', 'DisplayName', 'RP for T - $H_L$');
set(gca,'ColorOrderIndex',1)
plot(freqs, 1./(abs(squeeze(freqresp(wI, freqs, 'Hz'))) .* (2 + abs(squeeze(freqresp(wI, freqs, 'Hz'))))), '-.', 'DisplayName', 'RS - $H_L$');
set(gca,'ColorOrderIndex',2)
plot(freqs, 1./abs(squeeze(freqresp(wS, freqs, 'Hz'))), '--', 'DisplayName', 'NP - $H_H$');
set(gca,'ColorOrderIndex',2)
plot(freqs, (1 + abs(squeeze(freqresp(wI, freqs, 'Hz'))))./(abs(squeeze(freqresp(wS, freqs, 'Hz'))) .* (2 + abs(squeeze(freqresp(wI, freqs, 'Hz'))))), ':', 'DisplayName', 'RP for S - $H_H$');

set(gca,'ColorOrderIndex',1)
plot(freqs, 1./abs(squeeze(freqresp(wL, freqs, 'Hz'))), '-', 'DisplayName', '$w_L$');
set(gca,'ColorOrderIndex',2)
plot(freqs, 1./abs(squeeze(freqresp(wH, freqs, 'Hz'))), '-', 'DisplayName', '$w_H$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
xlim([freqs(1), freqs(end)]);
ylim([1e-3, 10]);
xticks([0.1, 1, 10, 100, 1000]);
legend('location', 'northeast');



% #+NAME: fig:weights_wl_wh
% #+CAPTION: Weights on the complementary filters $w_L$ and $w_H$ and the associated performance weights ([[./figs/weights_wl_wh.png][png]], [[./figs/weights_wl_wh.pdf][pdf]])
% [[file:figs/weights_wl_wh.png]]

% We define the generalized plant $P$ on matlab.

P = [0   wL;
     wH -wH;
     1   0];



% And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command.

[Hl_hinf, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');



% #+RESULTS:
% #+begin_example
% [Hl_hinf, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
% Test bounds:      0.0000 <  gamma  <=      1.7285

%   gamma    hamx_eig  xinf_eig  hamy_eig   yinf_eig   nrho_xy   p/f
%     1.729   4.1e+01   8.4e-12   1.8e-01    0.0e+00    0.0000    p
%     0.864   3.9e+01 -5.8e-02#  1.8e-01    0.0e+00    0.0000    f
%     1.296   4.0e+01   8.4e-12   1.8e-01    0.0e+00    0.0000    p
%     1.080   4.0e+01   8.5e-12   1.8e-01    0.0e+00    0.0000    p
%     0.972   3.9e+01 -4.2e-01#  1.8e-01    0.0e+00    0.0000    f
%     1.026   4.0e+01   8.5e-12   1.8e-01    0.0e+00    0.0000    p
%     0.999   3.9e+01   8.5e-12   1.8e-01    0.0e+00    0.0000    p
%     0.986   3.9e+01 -1.2e+00#  1.8e-01    0.0e+00    0.0000    f
%     0.993   3.9e+01 -8.2e+00#  1.8e-01    0.0e+00    0.0000    f
%     0.996   3.9e+01   8.5e-12   1.8e-01    0.0e+00    0.0000    p
%     0.994   3.9e+01   8.5e-12   1.8e-01    0.0e+00    0.0000    p
%     0.993   3.9e+01 -3.2e+01#  1.8e-01    0.0e+00    0.0000    f

%  Gamma value achieved:     0.9942
% #+end_example

% We then define the high pass filter $H_H = 1 - H_L$. The bode plot of both $H_L$ and $H_H$ is shown on figure [[fig:hinf_filters_results]].

Hh_hinf = 1 - Hl_hinf;

figure;
hold on;
set(gca,'ColorOrderIndex',1)
plot(freqs, 1./abs(squeeze(freqresp(wL, freqs, 'Hz'))), '--', 'DisplayName', '$w_L$');
set(gca,'ColorOrderIndex',2)
plot(freqs, 1./abs(squeeze(freqresp(wH, freqs, 'Hz'))), '--', 'DisplayName', '$w_H$');

set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(Hl_hinf, freqs, 'Hz'))), '-', 'DisplayName', '$H_L$ - $\mathcal{H}_\infty$');
set(gca,'ColorOrderIndex',2)
plot(freqs, abs(squeeze(freqresp(Hh_hinf, freqs, 'Hz'))), '-', 'DisplayName', '$H_H$ - $\mathcal{H}_\infty$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
xlim([freqs(1), freqs(end)]);
ylim([1e-3, 10]);
xticks([0.1, 1, 10, 100, 1000]);
legend('location', 'northeast');

% Complementary filters using analytical formula
%   <<sec:analytical_formula>>

% We here use analytical formula for the complementary filters $H_L$ and $H_H$.

% The first two formulas that are used to generate complementary filters are:
% \begin{align*}
%   H_L(s) &= \frac{(1+\alpha) (\frac{s}{\omega_0})+1}{\left((\frac{s}{\omega_0})+1\right) \left((\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1\right)}\\
%   H_H(s) &= \frac{(\frac{s}{\omega_0})^2 \left((\frac{s}{\omega_0})+1+\alpha\right)}{\left((\frac{s}{\omega_0})+1\right) \left((\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1\right)}
% \end{align*}
% where:
% - $\omega_0$ is the blending frequency in rad/s.
% - $\alpha$ is used to change the shape of the filters:
%   - Small values for $\alpha$ will produce high magnitude of the filters $|H_L(j\omega)|$ and $|H_H(j\omega)|$ near $\omega_0$ but smaller value for $|H_L(j\omega)|$ above $\approx 1.5 \omega_0$ and for $|H_H(j\omega)|$ below $\approx 0.7 \omega_0$
%   - A large $\alpha$ will do the opposite

% This is illustrated on figure [[fig:comp_filters_param_alpha]].
% As it is usually wanted to have the $\| S \|_\infty < 2$, $\alpha$ between $0.5$ and $1$ gives a good trade-off between the performance and the robustness.
% The slope of those filters at high and low frequencies is $-2$ and $2$ respectively for $H_L$ and $H_H$.


freqs_study = logspace(-2, 2, 10000);
alphas = [0.1, 1, 10];
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_study, abs(squeeze(freqresp(Hh2, freqs_study, 'Hz'))));
  set(gca,'ColorOrderIndex',i);
  plot(freqs_study, abs(squeeze(freqresp(Hl2, freqs_study, 'Hz'))));
end
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');
hold off;
ylim([1e-3, 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_study, 180/pi*angle(squeeze(freqresp(Hh2, freqs_study, 'Hz'))), 'DisplayName', sprintf('$\\alpha = %g$', alpha));
  set(gca,'ColorOrderIndex',i);
  plot(freqs_study, 180/pi*angle(squeeze(freqresp(Hl2, freqs_study, '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_study(1), freqs_study(end)]);



% #+NAME: fig:comp_filters_param_alpha
% #+CAPTION: Effect of the parameter $\alpha$ on the shape of the generated second order complementary filters ([[./figs/comp_filters_param_alpha.png][png]], [[./figs/comp_filters_param_alpha.pdf][pdf]])
% [[file:figs/comp_filters_param_alpha.png]]


% The parameters $\alpha$ and $\omega_0$ are chosen in order to have that the complementary filters stay below the defined upper bounds.

% The obtained complementary filters are shown on figure [[fig:complementary_filters_second_order]].
% The Robust Performance is not fulfilled for $T$, and we see that the RP condition as a slop of $-3$. We thus have to use different formula for the complementary filters here.


w0 = 2*pi*13;
alpha = 0.8;

Hh2_ana = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
Hl2_ana = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));

figure;
hold on;
set(gca,'ColorOrderIndex',1)
plot(freqs, 1./abs(squeeze(freqresp(wT, freqs, 'Hz'))), '--', 'DisplayName', 'NP - $H_L$');
set(gca,'ColorOrderIndex',1)
plot(freqs, 1./(abs(squeeze(freqresp(wT, freqs, 'Hz'))) .* (1 + abs(squeeze(freqresp(wI, freqs, 'Hz')))) + abs(squeeze(freqresp(wI, freqs, 'Hz')))), ':', 'DisplayName', 'RP for T - $H_L$');
set(gca,'ColorOrderIndex',1)
plot(freqs, 1./(abs(squeeze(freqresp(wI, freqs, 'Hz'))) .* (2 + abs(squeeze(freqresp(wI, freqs, 'Hz'))))), '-.', 'DisplayName', 'RS - $H_L$');
set(gca,'ColorOrderIndex',2)
plot(freqs, 1./abs(squeeze(freqresp(wS, freqs, 'Hz'))), '--', 'DisplayName', 'NP - $H_H$');
set(gca,'ColorOrderIndex',2)
plot(freqs, (1 + abs(squeeze(freqresp(wI, freqs, 'Hz'))))./(abs(squeeze(freqresp(wS, freqs, 'Hz'))) .* (2 + abs(squeeze(freqresp(wI, freqs, 'Hz'))))), ':', 'DisplayName', 'RP for S - $H_H$');

set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(Hl2_ana, freqs, 'Hz'))), '-', 'DisplayName', '$H_L$ - Analytical');
set(gca,'ColorOrderIndex',2)
plot(freqs, abs(squeeze(freqresp(Hh2_ana, freqs, 'Hz'))), '-', 'DisplayName', '$H_H$ - Analytical');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
xlim([freqs(1), freqs(end)]);
ylim([1e-3, 10]);
xticks([0.1, 1, 10, 100, 1000]);
legend('location', 'northeast');



% #+NAME: fig:complementary_filters_second_order
% #+CAPTION: Second order complementary filters using the analytical formula ([[./figs/complementary_filters_second_order.png][png]], [[./figs/complementary_filters_second_order.pdf][pdf]])
% [[file:figs/complementary_filters_second_order.png]]


% The following formula gives complementary filters with slopes of $-3$ and $3$:
% \begin{align*}
%   H_L(s) &= \frac{\left(1+(\alpha+1)(\beta+1)\right) (\frac{s}{\omega_0})^2 + (1+\alpha+\beta)(\frac{s}{\omega_0}) + 1}{\left(\frac{s}{\omega_0} + 1\right) \left( (\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1 \right) \left( (\frac{s}{\omega_0})^2 + \beta (\frac{s}{\omega_0}) + 1 \right)}\\
%   H_H(s) &= \frac{(\frac{s}{\omega_0})^3 \left( (\frac{s}{\omega_0})^2 + (1+\alpha+\beta) (\frac{s}{\omega_0}) + (1+(\alpha+1)(\beta+1)) \right)}{\left(\frac{s}{\omega_0} + 1\right) \left( (\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1 \right) \left( (\frac{s}{\omega_0})^2 + \beta (\frac{s}{\omega_0}) + 1 \right)}
% \end{align*}

% The parameters are:
% - $\omega_0$ is the blending frequency in rad/s
% - $\alpha$ and $\beta$ that are used to change the shape of the filters similarly to the parameter $\alpha$ for the second order complementary filters

% The filters are defined below and the result is shown on figure [[fig:complementary_filters_third_order]] where we can see that the complementary filters are below the defined upper bounds.


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

Hh3_ana = (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_ana = ((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));

figure;
hold on;
set(gca,'ColorOrderIndex',1)
plot(freqs, 1./abs(squeeze(freqresp(wT, freqs, 'Hz'))), '--', 'DisplayName', 'NP - $H_L$');
set(gca,'ColorOrderIndex',1)
plot(freqs, 1./(abs(squeeze(freqresp(wT, freqs, 'Hz'))) .* (1 + abs(squeeze(freqresp(wI, freqs, 'Hz')))) + abs(squeeze(freqresp(wI, freqs, 'Hz')))), ':', 'DisplayName', 'RP for T - $H_L$');
set(gca,'ColorOrderIndex',1)
plot(freqs, 1./(abs(squeeze(freqresp(wI, freqs, 'Hz'))) .* (2 + abs(squeeze(freqresp(wI, freqs, 'Hz'))))), '-.', 'DisplayName', 'RS - $H_L$');
set(gca,'ColorOrderIndex',2)
plot(freqs, 1./abs(squeeze(freqresp(wS, freqs, 'Hz'))), '--', 'DisplayName', 'NP - $H_H$');
set(gca,'ColorOrderIndex',2)
plot(freqs, (1 + abs(squeeze(freqresp(wI, freqs, 'Hz'))))./(abs(squeeze(freqresp(wS, freqs, 'Hz'))) .* (2 + abs(squeeze(freqresp(wI, freqs, 'Hz'))))), ':', 'DisplayName', 'RP for S - $H_H$');

set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(Hl3_ana, freqs, 'Hz'))), '-', 'DisplayName', '$H_L$ - Analytical');
set(gca,'ColorOrderIndex',2)
plot(freqs, abs(squeeze(freqresp(Hh3_ana, freqs, 'Hz'))), '-', 'DisplayName', '$H_H$ - Analytical');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
xlim([freqs(1), freqs(end)]);
ylim([1e-3, 10]);
xticks([0.1, 1, 10, 100, 1000]);
legend('location', 'northeast');

% Comparison of complementary filters
%   <<sec:comp_filters>>
% The generated complementary filters using $\mathcal{H}_\infty$ and the analytical formulas are compared on figure [[fig:comp_hinf_analytical]].

% Although they are very close to each other, there is some difference to note here:
% - the analytical formula provides a very simple way to generate the complementary filters (and thus the controller), they could even be used to tune the controller online using the parameters $\alpha$ and $\omega_0$. However, these formula have the property that $|H_H|$ and $|H_L|$ are symmetrical with the frequency $\omega_0$ which may not be desirable.
% - while the $\mathcal{H}_\infty$ synthesis of the complementary filters is not as straightforward as using the analytical formula, it provides a more optimized procedure to obtain the complementary filters

% The complementary filters obtained with the $\mathcal{H}_\infty$ will be used for further analysis.


figure;

ax1 = subplot(2,1,1);
hold on;
set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(Hl_hinf, freqs, 'Hz'))), '--');
set(gca,'ColorOrderIndex',2)
plot(freqs, abs(squeeze(freqresp(Hh_hinf, freqs, 'Hz'))), '--');

set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(Hl2_ana, freqs, 'Hz'))), '-');
set(gca,'ColorOrderIndex',2)
plot(freqs, abs(squeeze(freqresp(Hh2_ana, freqs, 'Hz'))), '-');

set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(Hl3_ana, freqs, 'Hz'))), ':');
set(gca,'ColorOrderIndex',2)
plot(freqs, abs(squeeze(freqresp(Hh3_ana, freqs, 'Hz'))), ':');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude');
hold off;
ylim([1e-4, 10]);

ax2 = subplot(2,1,2);
hold on;
set(gca,'ColorOrderIndex',1)
plot(freqs, 180/pi*phase(squeeze(freqresp(Hl_hinf, freqs, 'Hz'))), '--', 'DisplayName', '$H_L$ - $\mathcal{H}_\infty$');
set(gca,'ColorOrderIndex',2)
plot(freqs, 180/pi*phase(squeeze(freqresp(Hh_hinf, freqs, 'Hz'))), '--', 'DisplayName', '$H_H$ - $\mathcal{H}_\infty$');

set(gca,'ColorOrderIndex',1)
plot(freqs, 180/pi*phase(squeeze(freqresp(Hl2_ana, freqs, 'Hz'))), '-', 'DisplayName', '$H_L$ - $2$');
set(gca,'ColorOrderIndex',2)
plot(freqs, 180/pi*phase(squeeze(freqresp(Hh2_ana, freqs, 'Hz'))), '-', 'DisplayName', '$H_H$ - $2$');

set(gca,'ColorOrderIndex',1)
plot(freqs, 180/pi*phase(squeeze(freqresp(Hl3_ana, freqs, 'Hz'))), ':', 'DisplayName', '$H_L$ - $3$');
set(gca,'ColorOrderIndex',2)
plot(freqs, 180/pi*phase(squeeze(freqresp(Hh3_ana, freqs, 'Hz')))+360, ':', 'DisplayName', '$H_H$ - $3$');
set(gca, 'XScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks([-360:90:360]);
legend('location', 'northeast');

linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
xticks([0.1, 1, 10, 100, 1000]);

% Controller Analysis
%   <<sec:controller_analysis>>

% The controller $K$ is computed from the plant model $G$ and the low pass filter $H_H$:
% \[ K = G^{-1} H_H^{-1} \]

% As this is not proper and thus realizable, a second order low pass filter is added with a crossover frequency much larger than the control bandwidth.


omega = 2*pi*1000;
K = 1/(Hh_hinf*G) * 1/((1+s/omega)*(1+s/omega+(s/omega)^2));

K = zpk(minreal(K));

zpk(K)



% #+RESULTS:
% #+begin_example
% zpk(K)

% ans =

%         4.961e12 (s+9.915e04) (s^2 + 5s + 500) (s^2 + 284.6s + 2.135e04) (s^2 + 130.5s + 9887)
%   --------------------------------------------------------------------------------------------------
%   (s+9.914e04) (s+6283) (s^2 + 0.3576s + 0.03198) (s^2 + 413.8s + 6.398e04) (s^2 + 6283s + 3.948e07)

% Continuous-time zero/pole/gain model.
% #+end_example

% The bode plot of the controller is shown on figure [[fig:bode_plot_controller]]:
% - two integrator are present at low frequency
% - the resonance of the plant at $3.5\ \text{Hz}$ is inverted (notched)
% - a lead is added at $10\ \text{Hz}$


figure;
% Magnitude
ax1 = subplot(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(K, freqs, 'Hz'))), 'k-');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude [N/m]');
% ylim([1e3, 1e8])
hold off;

% Phase
ax2 = subplot(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(K, freqs, 'Hz'))), 'k-');
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
xticks([0.1, 1, 10, 100, 1000])

% Nominal Stability and Nominal Performance
%   <<sec:nominal_stability_performance>>

% The nominal stability of the system is first checked with the =allmargin= matlab command.


allmargin(K*G*Hl_hinf)



% #+RESULTS:
% #+begin_example
% allmargin(K*G*Hl_hinf)
% ans =
%   struct with fields:

%      GainMargin: 4.46426896164391
%     GMFrequency: 243.854595348016
%     PhaseMargin: 35.7045152899792
%     PMFrequency: 88.3664383511655
%     DelayMargin: 0.00705201387841809
%     DMFrequency: 88.3664383511655
%          Stable: 1
% #+end_example

% The system is stable and the stability margins are good.

% The bode plot of the loop gain $L = K*G*H_L$ is shown on figure [[fig:bode_plot_loop_gain]].


figure;
% Magnitude
ax1 = subplot(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(K*G*Hl_hinf, freqs, 'Hz'))), 'k-');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude [m/N]');
hold off;

% Phase
ax2 = subplot(2,1,2);
hold on;
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(K*G*Hl_hinf, freqs, 'Hz')))), 'k-');
set(gca,'xscale','log');
yticks(-270:90:0);
ylim([-270 0]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
xticks([0.1, 1, 10, 100, 1000])



% #+NAME: fig:bode_plot_loop_gain
% #+CAPTION: Bode Plot of the Loop Gain $L = K G H_L$ ([[./figs/bode_plot_loop_gain.png][png]], [[./figs/bode_plot_loop_gain.pdf][pdf]])
% [[file:figs/bode_plot_loop_gain.png]]

% In order to check the Nominal Performance of the system, we compute the sensibility and the complementary sensibility transfer functions.


S = 1/(K*G*Hl_hinf + 1);
T = K*G*Hl_hinf/(K*G*Hl_hinf + 1);



% We then compare their norms with the upper bounds on the performance of the system (figure [[fig:verification_NP]]).
% As expected, we guarantee the Nominal Performance of the system.


figure;
hold on;
set(gca,'ColorOrderIndex',1)
plot([100, 1000], [0.1, 0.001], ':', 'DisplayName', 'Upper bound on $|T|$');
set(gca,'ColorOrderIndex',2)
plot([0.1, 0.2, 2], [0.001, 0.001, 0.1], ':', 'DisplayName', 'Upper bound on $|S|$');
set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(T, freqs, 'Hz'))), '-', 'DisplayName', '$T$');
set(gca,'ColorOrderIndex',2)
plot(freqs, abs(squeeze(freqresp(S, freqs, 'Hz'))), '-', 'DisplayName', '$S$');

set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
xlim([freqs(1), freqs(end)]);
ylim([1e-4, 10]);
xticks([0.1, 1, 10, 100, 1000]);
legend('location', 'northeast');

% Robust Stability and Robust Performance
%   <<sec:robustness_analysis>>
% In order to verify the Robust stability of the system, we can use the following equivalence:
% \[ \text{RS} \Leftrightarrow \left| w_I T \right| < 1 \quad \forall \omega  \]

% This is shown on figure [[fig:robust_stability]].


Ts = Gds*K*Hl_hinf/(Gds*K*Hl_hinf + 1);
Ss = 1/(Gds*K*Hl_hinf + 1);

figure;
hold on;
plot(freqs, abs(squeeze(freqresp(wI*T, freqs, 'Hz'))), 'k-');
plot([freqs(1) freqs(end)], [1 1], 'k--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylim([0.02, 2])



% #+NAME: fig:robust_stability
% #+CAPTION: Robust Stability Check: $|w_I T| < 1, \quad \forall \omega$ ([[./figs/robust_stability.png][png]], [[./figs/robust_stability.pdf][pdf]])
% [[file:figs/robust_stability.png]]

% To check Robust Stability, we can also look at the loop gain of the uncertain system (figure [[fig:loop_gain_robustness]]) or the Nyquist plot (figure [[fig:nyquist_robustness]]).


figure;
ax2 = subplot(2,1,1);
hold on;
for i=1:length(Gds)
    plot(freqs, abs(squeeze(freqresp(Gds(:, :, i)*K*Hl_hinf, freqs, 'Hz'))), '-', 'color', [0, 0, 0, 0.1]);
end
plot(freqs, abs(squeeze(freqresp(G*K*Hl_hinf, freqs, 'Hz'))), 'k-');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude [m/N]');
ylim([1e-4 1e4]);
hold off;
% Phase
ax2 = subplot(2,1,2);
hold on;
for i=1:length(Gds)
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gds(:, :, i)*K*Hl_hinf, freqs, 'Hz')))), '-', 'color', [0, 0, 0, 0.1]);
end
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G*K*Hl_hinf, freqs, 'Hz')))), 'k-');
set(gca,'xscale','log');
yticks(-360:90:180);
ylim([-270 0]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
xticks([0.1, 1, 10, 100, 1000])



% #+NAME: fig:loop_gain_robustness
% #+CAPTION: Loop Gain of the uncertain system ([[./figs/loop_gain_robustness.png][png]], [[./figs/loop_gain_robustness.pdf][pdf]])
% [[file:figs/loop_gain_robustness.png]]



freqs_nyquist = logspace(0, 4, 100);

figure;
hold on;
for i=1:length(Gds)
    plot(real(squeeze(freqresp(Gds(:, :, i)*K*Hl_hinf, freqs_nyquist, 'Hz'))), imag(squeeze(freqresp(Gds(:, :, i)*K*Hl_hinf, freqs_nyquist, 'Hz'))), 'color', [0, 0, 0, 0.1]);
end
plot(real(squeeze(freqresp(G*K*Hl_hinf, freqs_nyquist, 'Hz'))), imag(squeeze(freqresp(G*K*Hl_hinf, freqs_nyquist, 'Hz'))), 'k');
hold off;
xlim([-1.4, 0.2]); ylim([-1.4, 0.2]);
xticks(-1.4:0.2:0.2); yticks(-1.4:0.2:0.2);
xlabel('Real Part'); ylabel('Imaginary Part');



% #+NAME: fig:nyquist_robustness
% #+CAPTION: Nyquist plot of the uncertain system ([[./figs/nyquist_robustness.png][png]], [[./figs/nyquist_robustness.pdf][pdf]])
% [[file:figs/nyquist_robustness.png]]

% The Robust Performance is verified by plotting $|S|$ and $|T|$ for the uncertain system along side the upper bounds defined for performance.
% This is shown on figure [[fig:robust_performance_result]] and we can indeed confirmed that the robust performance property of the system is valid.


figure;
hold on;
plot(freqs, abs(squeeze(freqresp(Ts(:, :, i), freqs, 'Hz'))), 'color', [0, 0.4470, 0.7410, 0.1]     , 'DisplayName', '$|T|$ - Uncertain');
plot(freqs, abs(squeeze(freqresp(Ss(:, :, i), freqs, 'Hz'))), 'color', [0.8500, 0.3250, 0.0980, 0.1], 'DisplayName', '$|S|$ - Uncertain');

for i=2:length(Gds)
    plot(freqs, abs(squeeze(freqresp(Ts(:, :, i), freqs, 'Hz'))), 'color', [0, 0.4470, 0.7410, 0.1]     , 'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(Ss(:, :, i), freqs, 'Hz'))), 'color', [0.8500, 0.3250, 0.0980, 0.1], 'HandleVisibility', 'off');
end

set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(G*K*Hl_hinf/(1+G*K*Hl_hinf), freqs, 'Hz'))), 'DisplayName', '$|T|$ - Nominal');
set(gca,'ColorOrderIndex',2)
plot(freqs, abs(squeeze(freqresp(1/(1+G*K*Hl_hinf), freqs, 'Hz'))), 'DisplayName', '$|S|$ - Nominal');

set(gca,'ColorOrderIndex',1)
plot([100, 1000], [0.1, 0.001], ':', 'DisplayName', '$|T|$ - Upper bound');
set(gca,'ColorOrderIndex',2)
plot([0.1, 0.2, 2], [0.001, 0.001, 0.1], ':', 'DisplayName', '$|S|$ - Upper bound');

set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
hold off;
xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
xlim([freqs(1), freqs(end)]);
ylim([1e-4, 5]);
xticks([0.1, 1, 10, 100, 1000]);
legend('location', 'northeastoutside');

% Pre-filter
%   <<sec:pre_filter>>

% For now, we have not specified any performance requirements on the input usage due to change of reference.
% Do limit the input usage due to change of reference, we can use a pre-filter $K_r$ as shown on figure [[fig:sf_arch_class_prefilter]].

% If we want a response time of 0.01 second, we can choose a first order low pass filter with a crossover frequency at $1/0.01 = 100\ \text{Hz}$ as defined below.


Kr = 1/(1+s/2/pi/100);



% We then run a simulation for a step of $1\mu m$ for the system without and with the pre-filter $K_r$ (figure [[fig:u_and_y_with_Kr]]).
% This confirms that a pre-filter can be used to limit the input usage due to change of reference using this architecture.


t  = linspace(0, 0.02, 1000);

opts = stepDataOptions;
opts.StepAmplitude = 1e-6;

u  = step((K)/(1+G*K*Hl_hinf),      t, opts);
uf = step((Kr*K)/(1+G*K*Hl_hinf),   t, opts);
y  = step((K*G)/(1+G*K*Hl_hinf),    t, opts);
yf = step((Kr*G*K)/(1+G*K*Hl_hinf), t, opts);

figure;
ax1 = subplot(2,1,1);
hold on;
plot(t, u, 'k--', 'DisplayName', 'Without Pre-filter');
plot(t, uf, 'k-', 'DisplayName', 'With Pre-Filter');
hold off;
ylabel('Command Input [N]');
set(gca, 'XTickLabel',[]);
legend('location', 'northeast');

ax2 = subplot(2,1,2);
hold on;
plot(t, y,  'k--');
plot(t, yf, 'k-' );
hold off;
xlabel('Time [s]');
ylabel('Output [m]');

% Controller using classical techniques
%   <<sec:sisotool_controller>>
% A controller is designed using =SISOTOOL= with a bandwidth of approximately $20\ \text{Hz}$ and with two integrator.

% The obtained controller is shown below.

Kf = 1.1814e12*(s+10.15)*(s+9.036)*(s+53.8)/(s^2*(s+216.1)*(s+1200)*(s+1864));

zpk(Kf)



% #+RESULTS:
% #+begin_example
% zpk(Kf)

% ans =

%   1.1814e12 (s+10.15) (s+9.036) (s+53.8)
%   --------------------------------------
%      s^2 (s+216.1) (s+1200) (s+1864)

% Continuous-time zero/pole/gain model.
% #+end_example

% The loop gain for both cases are compared on figure [[fig:loop_gain_compare]].


figure;
% Magnitude
ax1 = subplot(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(K*G*Hl_hinf, freqs, 'Hz'))),  'k--');
plot(freqs, abs(squeeze(freqresp(Kf*G, freqs, 'Hz'))), 'k-');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude [N/m]');
% ylim([1e3, 1e8])
hold off;

% Phase
ax2 = subplot(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(K*G*Hl_hinf, freqs, 'Hz'))), 'k--', 'DisplayName', '$K G H_L$ - $\mathcal{H}_\infty$');
plot(freqs, 180/pi*angle(squeeze(freqresp(Kf*G, freqs, 'Hz'))), 'k-', 'DisplayName', '$K G$ - SISOTOOL');
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
legend('location', 'northwest');

linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
xticks([0.1, 1, 10, 100, 1000])



% #+NAME: fig:loop_gain_compare
% #+CAPTION: Comparison of the Loop Gains ([[./figs/loop_gain_compare.png][png]], [[./figs/loop_gain_compare.pdf][pdf]])
% [[file:figs/loop_gain_compare.png]]

% The Robust Stability of the system is verified using the Nyquist plot on figure [[fig:nyquist_plot_sisotool_controller]].


freqs_nyquist = logspace(0, 4, 100);

figure;
hold on;
for i=1:length(Gds)
    plot(real(squeeze(freqresp(Gds(:, :, i)*Kf, freqs_nyquist, 'Hz'))), imag(squeeze(freqresp(Gds(:, :, i)*Kf, freqs_nyquist, 'Hz'))), 'color', [0, 0, 0, 0.1]);
end
plot(real(squeeze(freqresp(G*Kf, freqs_nyquist, 'Hz'))), imag(squeeze(freqresp(G*Kf, freqs_nyquist, 'Hz'))), 'k');
hold off;
xlim([-1.4, 0.2]); ylim([-1.4, 0.2]);
xticks(-1.4:0.2:0.2); yticks(-1.4:0.2:0.2);
xlabel('Real Part'); ylabel('Imaginary Part');



% #+NAME: fig:nyquist_plot_sisotool_controller
% #+CAPTION: Nyquist Plot of the uncertain system ([[./figs/nyquist_plot_sisotool_controller.png][png]], [[./figs/nyquist_plot_sisotool_controller.pdf][pdf]])
% [[file:figs/nyquist_plot_sisotool_controller.png]]

% The closed loop sensitivity and complementary sensitivity transfer functions are computed.
% And finally, the Robust Performance of both systems are compared on figure [[fig:robust_performance_compare]].


Sf = 1/(Kf*G + 1);
Tf = Kf*G/(Kf*G + 1);

Tfs = Gds*Kf/(Gds*Kf + 1);
Sfs = 1/(Gds*Kf + 1);

figure;
ax1 = subplot(1, 2, 1);
title('$K$ - SISOTOOL');
hold on;

set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(Tf, freqs, 'Hz'))), 'DisplayName', '$|T|$ - Nominal');
set(gca,'ColorOrderIndex',2)
plot(freqs, abs(squeeze(freqresp(Sf, freqs, 'Hz'))), 'DisplayName', '$|S|$ - Nominal');

plot(freqs, abs(squeeze(freqresp(Tfs(:, :, i), freqs, 'Hz'))), 'color', [0, 0.4470, 0.7410, 0.1]     , 'DisplayName', '$|T|$ - Uncertain');
plot(freqs, abs(squeeze(freqresp(Sfs(:, :, i), freqs, 'Hz'))), 'color', [0.8500, 0.3250, 0.0980, 0.1], 'DisplayName', '$|S|$ - Uncertain');

for i=2:length(Gds)
    plot(freqs, abs(squeeze(freqresp(Tfs(:, :, i), freqs, 'Hz'))), 'color', [0, 0.4470, 0.7410, 0.1]     , 'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(Sfs(:, :, i), freqs, 'Hz'))), 'color', [0.8500, 0.3250, 0.0980, 0.1], 'HandleVisibility', 'off');
end

set(gca,'ColorOrderIndex',1)
plot([100, 1000], [0.1, 0.001], ':', 'DisplayName', '$|T|$ - Upper bound');
set(gca,'ColorOrderIndex',2)
plot([0.1, 0.2, 2], [0.001, 0.001, 0.1], ':', 'DisplayName', '$|S|$ - Upper bound');

set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
hold off;
xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
xlim([freqs(1), freqs(end)]);
xticks([0.1, 1, 10, 100, 1000]);

ax2 = subplot(1, 2, 2);
title('$K$ - complementary filters');
hold on;
set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(T, freqs, 'Hz'))), 'DisplayName', '$|T|$ - Nominal');
set(gca,'ColorOrderIndex',2)
plot(freqs, abs(squeeze(freqresp(S, freqs, 'Hz'))), 'DisplayName', '$|S|$ - Nominal');

plot(freqs, abs(squeeze(freqresp(Ts(:, :, i), freqs, 'Hz'))), 'color', [0, 0.4470, 0.7410, 0.1]     , 'DisplayName', '$|T|$ - Uncertain');
plot(freqs, abs(squeeze(freqresp(Ss(:, :, i), freqs, 'Hz'))), 'color', [0.8500, 0.3250, 0.0980, 0.1], 'DisplayName', '$|S|$ - Uncertain');

for i=2:length(Gds)
    plot(freqs, abs(squeeze(freqresp(Ts(:, :, i), freqs, 'Hz'))), 'color', [0, 0.4470, 0.7410, 0.1]     , 'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(Ss(:, :, i), freqs, 'Hz'))), 'color', [0.8500, 0.3250, 0.0980, 0.1], 'HandleVisibility', 'off');
end

set(gca,'ColorOrderIndex',1)
plot([100, 1000], [0.1, 0.001], ':', 'DisplayName', '$|T|$ - Upper bound');
set(gca,'ColorOrderIndex',2)
plot([0.1, 0.2, 2], [0.001, 0.001, 0.1], ':', 'DisplayName', '$|S|$ - Upper bound');

set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
hold off;
xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
xlim([freqs(1), freqs(end)]);
xticks([0.1, 1, 10, 100, 1000]);

linkaxes([ax1 ax2], 'y')
ylim([1e-4, 10]);
