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

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

%% Path for functions, data and scripts
addpath('./mat/'); % Path for data
addpath('./src/'); % Path for Functions

%% Colors for the figures
colors = colororder;

%% Simscape model name
mdl = 'rotating_model';

%% Load "Generic" system dynamics
load('rotating_generic_plants.mat', 'Gs', 'Wzs');

% Identify plant with parallel stiffnesses                        :noexport:

%% Tuv Stage
mn = 0.5; % Tuv mass [kg]

%% Sample
ms = 0.5; % Sample mass [kg]

%% General Configuration
model_config = struct();
model_config.controller = "open_loop"; % Default: Open-Loop
model_config.Tuv_type   = "parallel_k";    % Default: 2DoF stage

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/controller'],        1, 'openinput');  io_i = io_i + 1; % [Fu, Fv]
io(io_i) = linio([mdl, '/fd'],                1, 'openinput');  io_i = io_i + 1; % [Fdu, Fdv]
io(io_i) = linio([mdl, '/translation_stage'], 1, 'openoutput'); io_i = io_i + 1; % [Fmu, Fmv]
io(io_i) = linio([mdl, '/translation_stage'], 2, 'openoutput'); io_i = io_i + 1; % [Du, Dv]
io(io_i) = linio([mdl, '/ext_metrology'],     1, 'openoutput'); io_i = io_i + 1; % [Dx, Dy]

% Effect of the parallel stiffness on the IFF plant
% The IFF plant (transfer function from $[F_u, F_v]$ to $[f_u, f_v]$) is identified in three different cases:
% - without parallel stiffness $k_p = 0$
% - with a small parallel stiffness $k_p < m \Omega^2$
% - with a large parallel stiffness $k_p > m \Omega^2$


Wz = 0.1; % The rotation speed [rad/s]

%% No parallel Stiffness
kp = 0; % Parallel Stiffness [N/m]
cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
kn = 1 - kp; % Stiffness [N/m]
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]

G_no_kp = linearize(mdl, io, 0);
G_no_kp.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
G_no_kp.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

%% Small parallel Stiffness
kp = 0.5*(mn+ms)*Wz^2; % Parallel Stiffness [N/m]
cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
kn = 1 - kp; % Stiffness [N/m]
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]

G_low_kp = linearize(mdl, io, 0);
G_low_kp.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
G_low_kp.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

%% Large parallel Stiffness
kp = 1.5*(mn+ms)*Wz^2; % Parallel Stiffness [N/m]
cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
kn = 1 - kp; % Stiffness [N/m]
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]

G_high_kp = linearize(mdl, io, 0);
G_high_kp.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
G_high_kp.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};



% The Bode plots of the obtained dynamics are shown in Figure ref:fig:rotating_iff_effect_kp.
% One can see that for $k_p > m \Omega^2$, the two real zeros with $k_p < m \Omega^2$ are transformed into two complex conjugate zeros and the systems shows alternating complex conjugate poles and zeros.


%% Effect of the parallel stiffness on the IFF plant
freqs = logspace(-2, 1, 1000);

figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

% Magnitude
ax1 = nexttile([2, 1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_no_kp(  'fu', 'Fu'), freqs, 'rad/s'))), '-', ...
     'DisplayName', '$k_p = 0$')
plot(freqs, abs(squeeze(freqresp(G_low_kp( 'fu', 'Fu'), freqs, 'rad/s'))), '-', ...
     'DisplayName', '$k_p < m\Omega^2$')
plot(freqs, abs(squeeze(freqresp(G_high_kp('fu', 'Fu'), freqs, 'rad/s'))), '-', ...
     'DisplayName', '$k_p > m\Omega^2$')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
ylim([1e-5, 5e1]);
legend('location', 'southeast', 'FontSize', 8);

% Phase
ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_no_kp(  'fu', 'Fu'), freqs, 'rad/s'))), '-')
plot(freqs, 180/pi*angle(squeeze(freqresp(G_low_kp( 'fu', 'Fu'), freqs, 'rad/s'))), '-')
plot(freqs, 180/pi*angle(squeeze(freqresp(G_high_kp('fu', 'Fu'), freqs, 'rad/s'))), '-')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([-180 180]);
hold off;
xticks([1e-2,1e-1,1,1e1])
xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})

linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);



% #+name: fig:rotating_iff_effect_kp
% #+caption: Effect of the parallel stiffness on the IFF plant: Bode plot of $G_{k}(1,1) = f_u/F_u$ without parallel spring, with parallel spring stiffness $k_p < m \Omega^2$ and $k_p > m \Omega^2$, $\Omega = 0.1 \omega_0$
% #+RESULTS:
% [[file:figs/rotating_iff_effect_kp.png]]

% Figure ref:fig:rotating_iff_kp_root_locus shows the Root Locus plots for $k_p = 0$, $k_p < m \Omega^2$ and $k_p > m \Omega^2$ when $K_F$ is a pure integrator as in Eq. eqref:eq:Kf_pure_int.
% It is shown that if the added stiffness is higher than the maximum negative stiffness, the poles of the closed-loop system are bounded on the (stable) left half-plane, and hence the *unconditional stability of IFF is recovered*.


%% Root Locus for IFF without parallel spring, with small parallel spring and with large parallel spring
gains = logspace(-2, 2, 200);

figure;
tiledlayout(1, 3, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([1,2]);
hold on;
plot(real(pole(G_no_kp({'fu','fv'},{'Fu','Fv'})*(1/s))),  imag(pole(G_no_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'x', 'color', colors(1,:), ...
     'DisplayName', '$k_p = 0$','MarkerSize',8);
plot(real(tzero(G_no_kp({'fu','fv'},{'Fu','Fv'})*(1/s))),  imag(tzero(G_no_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'o', 'color', colors(1,:), ...
     'HandleVisibility', 'off','MarkerSize',8);
for g = gains
    cl_poles = pole(feedback(G_no_kp({'fu','fv'},{'Fu','Fv'}), (g/s)*eye(2)));
    plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(1,:), ...
         'HandleVisibility', 'off','MarkerSize',4);
end

plot(real(pole(G_low_kp({'fu','fv'},{'Fu','Fv'})*(1/s))),  imag(pole(G_low_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'x', 'color', colors(2,:), ...
     'DisplayName', '$k_p < m\Omega^2$','MarkerSize',8);
plot(real(tzero(G_low_kp({'fu','fv'},{'Fu','Fv'})*(1/s))),  imag(tzero(G_low_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'o', 'color', colors(2,:), ...
     'HandleVisibility', 'off','MarkerSize',8);
for g = gains
    cl_poles = pole(feedback(G_low_kp({'fu','fv'},{'Fu','Fv'}), (g/s)*eye(2)));
    plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(2,:), ...
         'HandleVisibility', 'off','MarkerSize',4);
end

plot(real(pole(G_high_kp({'fu','fv'},{'Fu','Fv'})*(1/s))),  imag(pole(G_high_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'x', 'color', colors(3,:), ...
     'DisplayName', '$k_p > m\Omega^2$','MarkerSize',8);
plot(real(tzero(G_high_kp({'fu','fv'},{'Fu','Fv'})*(1/s))),  imag(tzero(G_high_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'o', 'color', colors(3,:), ...
     'HandleVisibility', 'off','MarkerSize',8);
for g = gains
    cl_poles = pole(feedback(G_high_kp({'fu','fv'},{'Fu','Fv'}), (g/s)*eye(2)));
    plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(3,:), ...
         'HandleVisibility', 'off','MarkerSize',4);
end
hold off;
axis square;
xlim([-2.25, 0.25]); ylim([-1.25, 1.25]);
xticks([-2, -1, 0])
xticklabels({'$-2\omega_0$', '$-\omega_0$', '$0$'})
yticks([-1, 0, 1])
yticklabels({'$-\omega_0$', '$0$', '$\omega_0$'})

xlabel('Real Part'); ylabel('Imaginary Part');
leg = legend('location', 'northwest', 'FontSize', 8);
leg.ItemTokenSize(1) = 8;

ax2 = nexttile();
hold on;
plot(real(pole(G_no_kp( {'fu','fv'},{'Fu','Fv'})*(1/s))),  imag(pole(G_no_kp( {'fu','fv'},{'Fu','Fv'})*(1/s))), 'x', 'color', colors(1,:), 'MarkerSize',8);
plot(real(tzero(G_no_kp({'fu','fv'},{'Fu','Fv'})*(1/s))),  imag(tzero(G_no_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'o', 'color', colors(1,:), 'MarkerSize',8);
for g = gains
    cl_poles = pole(feedback(G_no_kp({'fu','fv'},{'Fu','Fv'}), (g/s)*eye(2)));
    plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(1,:), 'MarkerSize', 4);
end

plot(real(pole(G_low_kp( {'fu','fv'},{'Fu','Fv'})*(1/s))),  imag(pole(G_low_kp( {'fu','fv'},{'Fu','Fv'})*(1/s))), 'x', 'color', colors(2,:), 'MarkerSize',8);
plot(real(tzero(G_low_kp({'fu','fv'},{'Fu','Fv'})*(1/s))),  imag(tzero(G_low_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'o', 'color', colors(2,:), 'MarkerSize',8);
for g = gains
    cl_poles = pole(feedback(G_low_kp({'fu','fv'},{'Fu','Fv'}), (g/s)*eye(2)));
    plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(2,:), 'MarkerSize', 4);
end

plot(real(pole(G_high_kp( {'fu','fv'},{'Fu','Fv'})*(1/s))),  imag(pole(G_high_kp( {'fu','fv'},{'Fu','Fv'})*(1/s))), 'x', 'color', colors(3,:), 'MarkerSize',8);
plot(real(tzero(G_high_kp({'fu','fv'},{'Fu','Fv'})*(1/s))),  imag(tzero(G_high_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'o', 'color', colors(3,:), 'MarkerSize',8);
for g = gains
    cl_poles = pole(feedback(G_high_kp({'fu','fv'},{'Fu','Fv'}), (g/s)*eye(2)));
    plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(3,:), 'MarkerSize',4);
end
hold off;
axis equal;
xlim([-0.04, 0.04]); ylim([-0.08, 0.08]);
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
xlabel('Real Part'); ylabel('Imaginary Part');
title('Zoom on controller pole')

% Effect of $k_p$ on the attainable damping

% Even though the parallel stiffness $k_p$ has no impact on the open-loop poles (as the overall stiffness $k$ is kept constant), it has a large impact on the transmission zeros.
% Moreover, as the attainable damping is generally proportional to the distance between poles and zeros cite:preumont18_vibrat_contr_activ_struc_fourt_edition, the parallel stiffness $k_p$ is foreseen to have some impact on the attainable damping.

% To study this effect, Root Locus plots for several parallel stiffnesses $k_p > m \Omega^2$ are shown in Figure ref:fig:rotating_iff_kp_root_locus_effect_kp.
% The frequencies of the transmission zeros of the system are increasing with an increase of the parallel stiffness $k_p$ (thus getting closer to the poles) and the associated attainable damping is reduced.

% #+begin_important
% Therefore, even though the parallel stiffness $k_p$ should be larger than $m \Omega^2$ for stability reasons, it should not be taken too large as this would limit the attainable damping.
% #+end_important


%% Tested parallel stiffnesses
kps = [2, 20, 40]*(mn + ms)*Wz^2;

%% Root Locus: Effect of the parallel stiffness on the attainable damping
gains = logspace(-2, 4, 500);

figure;
hold on;
for kp_i = 1:length(kps)
    kp = kps(kp_i); % Parallel Stiffness [N/m]
    cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
    kn = 1 - kp; % Stiffness [N/m]
    cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]

    % Identify dynamics
    G = linearize(mdl, io, 0);
    G.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
    G.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

    plot(real(pole(G({'fu', 'fv'}, {'Fu', 'Fv'})*(1/s*eye(2)))),  imag(pole(G({'fu', 'fv'}, {'Fu', 'Fv'})*(1/s*eye(2)))), 'x', 'color', colors(kp_i,:), ...
         'DisplayName', sprintf('$k_p = %.1f m \\Omega^2$', kp/((mn+ms)*Wz^2)),'MarkerSize',8);
    plot(real(tzero(G({'fu', 'fv'}, {'Fu', 'Fv'})*(1/s*eye(2)))),  imag(tzero(G({'fu', 'fv'}, {'Fu', 'Fv'})*(1/s*eye(2)))), 'o', 'color', colors(kp_i,:), ...
         'HandleVisibility', 'off','MarkerSize',8);
    for g = gains
        cl_poles = pole(feedback(G({'fu', 'fv'}, {'Fu', 'Fv'}), (g/s)*eye(2)));
        plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(kp_i,:),'MarkerSize',4, ...
             'HandleVisibility', 'off');
    end
end
hold off;
axis square;
% xlim([-1.15, 0.05]); ylim([0, 1.2]);
xlim([-2.25, 0.25]); ylim([-1.25, 1.25]);
xticks([-2, -1, 0])
xticklabels({'$-2\omega_0$', '$-\omega_0$', '$0$'})
yticks([-1, 0, 1])
yticklabels({'$-\omega_0$', '$0$', '$\omega_0$'})

xlabel('Real Part'); ylabel('Imaginary Part');
leg = legend('location', 'northwest', 'FontSize', 8);
leg.ItemTokenSize(1) = 12;



% #+name: fig:rotating_iff_kp_root_locus_effect_kp
% #+caption: Root Locus: Effect of the parallel stiffness on the attainable damping, $\Omega = 0.1 \omega_0$
% #+RESULTS:
% [[file:figs/rotating_iff_kp_root_locus_effect_kp.png]]

% This is confirmed by the Figure ref:fig:rotating_iff_kp_optimal_gain where the attainable closed-loop damping ratio $\xi_{\text{cl}}$ and the associated optimal control gain $g_\text{opt}$ are computed as a function of the parallel stiffness.


%% Computes the optimal parameters and attainable simultaneous damping
alphas = logspace(-2, 0, 100);
alphas(end) = []; % Remove last point

opt_xi = zeros(1, length(alphas)); % Optimal simultaneous damping
opt_gain = zeros(1, length(alphas)); % Corresponding optimal gain

Kiff = 1/s*eye(2);

for alpha_i = 1:length(alphas)
    kp = alphas(alpha_i);
    cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
    kn = 1 - kp; % Stiffness [N/m]
    cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]

    % Identify dynamics
    G = linearize(mdl, io, 0);
    G.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
    G.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

    fun = @(g)computeSimultaneousDamping(g, G({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff);

    [g_opt, xi_opt] = fminsearch(fun, 2);
    opt_xi(alpha_i) = 1/xi_opt;
    opt_gain(alpha_i) = g_opt;
end

%% Attainable damping as a function of the stiffness ratio
figure;
yyaxis left
plot(alphas, opt_xi, '-', 'DisplayName', '$\xi_{cl}$');
set(gca, 'YScale', 'lin');
ylim([0,1]);
ylabel('Damping Ratio $\xi$');

yyaxis right
hold on;
plot(alphas, opt_gain, '-', 'DisplayName', '$g_{opt}$');
set(gca, 'YScale', 'lin');
ylim([0,2.5]);
ylabel('Controller gain $g$');

set(gca, 'XScale', 'log');
legend('location', 'northeast', 'FontSize', 8);

xlabel('$k_p$');
xlim([0.01, 1]);
xticks([0.01, 0.1, 1])
xticklabels({'$m\Omega^2$', '$10m\Omega^2$', '$100m\Omega^2$'})

% Damped plant
% Let's choose a parallel stiffness equal to $k_p = 2 m \Omega^2$ and compute the damped plant.
% The damped and undamped transfer functions from $F_u$ to $d_u$ are compared in Figure ref:fig:rotating_iff_kp_damped_plant.

% Even though the two resonances are well damped, the IFF changes the low frequency behavior of the plant which is usually not wanted.
% This is due to the fact that "pure" integrators are used, and that the low frequency loop gains becomes large below some frequency.


%% Identify dynamics with parallel stiffness = 2mW^2
Wz = 0.1; % [rad/s]
kp = 2*(mn + ms)*Wz^2; % Parallel Stiffness [N/m]
cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
kn = 1 - kp; % Stiffness [N/m]
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]

% Identify dynamics
G = linearize(mdl, io, 0);
G.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
G.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

%% IFF controller with pure integrator
Kiff_kp = (2.2/s)*eye(2);
Kiff_kp.InputName = {'fu', 'fv'};
Kiff_kp.OutputName = {'Fu', 'Fv'};

%% Compute the damped plant
G_cl_iff_kp = feedback(G, Kiff_kp, 'name');

%% Damped plant with IFF - Transfer function from $F_u$ to $d_u$
freqs = logspace(-3, 1, 1000);

figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

% Magnitude
ax1 = nexttile([2, 1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros(1,3), ...
    'DisplayName', '$d_u/F_u$ - OL')
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:), ...
    'DisplayName', '$d_u/F_u$ - IFF + $k_p$')
plot(freqs, abs(squeeze(freqresp(G('Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [zeros(1,3), 0.5], ...
    'DisplayName', '$d_v/F_u$ - OL')
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp('Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(1,:), 0.5], ...
    'DisplayName', '$d_v/F_u$ - IFF + $k_p$')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
ldg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
ldg.ItemTokenSize(1) = 20;
ylim([1e-6, 1e2]);

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros(1,3))
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_iff_kp('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([-180 180]);
xticks([1e-3,1e-2,1e-1,1,1e1])
xticklabels({'$0.001 \omega_0$', '$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})

linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);



% #+name: fig:rotating_iff_kp_damped_plant
% #+caption: Damped plant with IFF - Transfer function from $F_u$ to $d_u$
% #+RESULTS:
% [[file:figs/rotating_iff_kp_damped_plant.png]]

% In order to lower the low frequency gain, an high pass filter is added to the IFF controller (which is equivalent as shifting the controller pole to the left in the complex plane):
% \begin{equation}
%   K_{\text{IFF}}(s) = g\frac{1}{\omega_i + s} \begin{bmatrix}
%   1 & 0 \\
%   0 & 1
% \end{bmatrix}
% \end{equation}

% Let's see how the high pass filter impacts the attainable damping.
% The controller gain $g$ is kept constant while $\omega_i$ is changed, and the minimum damping ratio of the damped plant is computed.
% The obtained damping ratio as a function of $\omega_i/\omega_0$ (where $\omega_0$ is the resonance of the system without rotation) is shown in Figure ref:fig:rotating_iff_kp_added_hpf_effect_damping.


w0 = sqrt((kn+kp)/(mn+ms)); % Resonance frequency [rad/s]
wis = w0*logspace(-2, 0, 100); % LPF cut-off [rad/s]

%% Computes the obtained damping as a function of the HPF cut-off frequency
opt_xi = zeros(1, length(wis)); % Optimal simultaneous damping

for wi_i = 1:length(wis)
    Kiff_kp_hpf = (2.2/(s + wis(wi_i)))*eye(2);
    Kiff_kp_hpf.InputName = {'fu', 'fv'};
    Kiff_kp_hpf.OutputName = {'Fu', 'Fv'};

    [~, xi] = damp(feedback(G, Kiff_kp_hpf, 'name'));
    opt_xi(wi_i) = min(xi);
end

%% Effect of the high pass filter cut-off frequency on the obtained damping
figure;
plot(wis, opt_xi, '-');
set(gca, 'XScale', 'log');
set(gca, 'YScale', 'lin');
ylim([0,1]);
ylabel('Damping Ratio $\xi$');
xlabel('$\omega_i/\omega_0$');



% #+name: fig:rotating_iff_kp_added_hpf_effect_damping
% #+caption: Effect of the high pass filter cut-off frequency on the obtained damping
% #+RESULTS:
% [[file:figs/rotating_iff_kp_added_hpf_effect_damping.png]]

% Let's choose $\omega_i = 0.1 \cdot \omega_0$ and compute the damped plant again.
% The Bode plots of the undamped, damped with "pure" IFF, and with added high pass filters are shown in Figure ref:fig:rotating_iff_kp_added_hpf_damped_plant.
% The added high pass filter gives almost the same damping properties while giving acceptable low frequency behavior.


%% Compute the damped plant with added High Pass Filter
Kiff_kp_hpf = (2.2/(s + 0.1*w0))*eye(2);
Kiff_kp_hpf.InputName = {'fu', 'fv'};
Kiff_kp_hpf.OutputName = {'Fu', 'Fv'};

G_cl_iff_hpf_kp = feedback(G, Kiff_kp_hpf, 'name');

%% Bode plot of the direct and coupling terms for several rotating velocities
freqs = logspace(-3, 1, 1000);

figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

% Magnitude
ax1 = nexttile([2, 1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G(              'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros( 1,3), ...
    'DisplayName', '$d_u/F_u$ - OL')
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp(    'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:), ...
    'DisplayName', '$d_u/F_u$ - IFF + $k_p$')
plot(freqs, abs(squeeze(freqresp(G_cl_iff_hpf_kp('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(2,:), ...
    'DisplayName', '$d_u/F_u$ - IFF + $k_p$ + HPF')
plot(freqs, abs(squeeze(freqresp(G(              'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [zeros( 1,3), 0.5], ...
    'DisplayName', '$d_v/F_u$ - OL')
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp(    'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(1,:), 0.5], ...
    'DisplayName', '$d_v/F_u$ - IFF + $k_p$')
plot(freqs, abs(squeeze(freqresp(G_cl_iff_hpf_kp('Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(2,:), 0.5], ...
    'DisplayName', '$d_v/F_u$ - IFF + $k_p$ + HPF')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
ldg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
ldg.ItemTokenSize(1) = 20;

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G(              'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros( 1,3))
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_iff_kp(    'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:))
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_iff_hpf_kp('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(2,:))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([-180 180]);
xticks([1e-3,1e-2,1e-1,1,1e1])
xticklabels({'$0.001 \omega_0$', '$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})

linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
