%% 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

%% Colors for the figures
colors = colororder;

%% Frequency Vector [Hz]
freqs = logspace(0, 3, 1000);

%% Load the PSD of disturbances
load('uniaxial_disturbance_psd.mat', 'f', 'psd_ft', 'psd_xf');

%% Load Plants Dynamics
load('uniaxial_plants.mat', 'G_vc_light', 'G_md_light', 'G_pz_light', ...
                            'G_vc_mid',   'G_md_mid',   'G_pz_mid', ...
                            'G_vc_heavy', 'G_md_heavy', 'G_pz_heavy');

% Plant Dynamics for Active Damping
% The plant dynamics for all three active damping techniques are shown in Figure ref:fig:uniaxial_plant_active_damping_techniques.
% All have *alternating poles and zeros* meaning that the phase do not vary by more than $\pm 90\,\text{deg}$ which makes the design of a robust controller very easy.
% The reason all three plants in Figure ref:fig:uniaxial_plant_active_damping_techniques have alternating poles and zeros is because the three sensors are all collocated with the actuator [[cite:&preumont18_vibrat_contr_activ_struc_fourt_edition Chapter 7]].

% When the nano-hexapod's suspension modes are at lower frequencies than the resonances of the micro-station (blue and red curves in Figure ref:fig:uniaxial_plant_active_damping_techniques), the resonances of the micro-stations have little impact on the IFF and DVF transfer functions.

% For the stiff nano-hexapod (yellow curves), the micro-station dynamics can be seen on the transfer functions in Figure ref:fig:uniaxial_plant_active_damping_techniques.
% Therefore, it is expected that the micro-station dynamics might impact the achievable damping if a stiff nano-hexapod is used.


%% Damped plants for three considered payload masses - Comparison of active damping techniques
figure;
tiledlayout(3, 3, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_vc_light('fm', 'f'), freqs, 'Hz'))), '-',  'color', colors(1,:), 'DisplayName', '$m_s = 1\,kg$');
plot(freqs, abs(squeeze(freqresp(G_vc_mid(  'fm', 'f'), freqs, 'Hz'))), '-.', 'color', colors(1,:), 'DisplayName', '$m_s = 25\,kg$');
plot(freqs, abs(squeeze(freqresp(G_vc_heavy('fm', 'f'), freqs, 'Hz'))), '--', 'color', colors(1,:), 'DisplayName', '$m_s = 50\,kg$');
plot(freqs, abs(squeeze(freqresp(G_md_light('fm', 'f'), freqs, 'Hz'))), '-',  'color', colors(2,:), 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_md_mid(  'fm', 'f'), freqs, 'Hz'))), '-.', 'color', colors(2,:), 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_md_heavy('fm', 'f'), freqs, 'Hz'))), '--', 'color', colors(2,:), 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_pz_light('fm', 'f'), freqs, 'Hz'))), '-',  'color', colors(3,:), 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_pz_mid(  'fm', 'f'), freqs, 'Hz'))), '-.', 'color', colors(3,:), 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_pz_heavy('fm', 'f'), freqs, 'Hz'))), '--', 'color', colors(3,:), 'HandleVisibility', 'off');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
title('IFF: $f_m/f$');
ldg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
ldg.ItemTokenSize = [20, 1];

ax2 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_vc_light('dL', 'f'), freqs, 'Hz'))), '-',  'color', colors(1,:));
plot(freqs, abs(squeeze(freqresp(G_vc_mid(  'dL', 'f'), freqs, 'Hz'))), '-.', 'color', colors(1,:));
plot(freqs, abs(squeeze(freqresp(G_vc_heavy('dL', 'f'), freqs, 'Hz'))), '--', 'color', colors(1,:));
plot(freqs, abs(squeeze(freqresp(G_md_light('dL', 'f'), freqs, 'Hz'))), '-',  'color', colors(2,:));
plot(freqs, abs(squeeze(freqresp(G_md_mid(  'dL', 'f'), freqs, 'Hz'))), '-.', 'color', colors(2,:));
plot(freqs, abs(squeeze(freqresp(G_md_heavy('dL', 'f'), freqs, 'Hz'))), '--', 'color', colors(2,:));
plot(freqs, abs(squeeze(freqresp(G_pz_light('dL', 'f'), freqs, 'Hz'))), '-',  'color', colors(3,:));
plot(freqs, abs(squeeze(freqresp(G_pz_mid(  'dL', 'f'), freqs, 'Hz'))), '-.', 'color', colors(3,:));
plot(freqs, abs(squeeze(freqresp(G_pz_heavy('dL', 'f'), freqs, 'Hz'))), '--', 'color', colors(3,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('RDC: $d\mathcal{L}/f$');

ax3 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_vc_light('vn', 'f'), freqs, 'Hz'))), '-',  'color', colors(1,:), 'DisplayName', '$k_n = 0.01\,N/\mu m$');
plot(freqs, abs(squeeze(freqresp(G_vc_mid(  'vn', 'f'), freqs, 'Hz'))), '-.', 'color', colors(1,:), 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_vc_heavy('vn', 'f'), freqs, 'Hz'))), '--', 'color', colors(1,:), 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_md_light('vn', 'f'), freqs, 'Hz'))), '-',  'color', colors(2,:), 'DisplayName', '$k_n = 1\,N/\mu m$');
plot(freqs, abs(squeeze(freqresp(G_md_mid(  'vn', 'f'), freqs, 'Hz'))), '-.', 'color', colors(2,:), 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_md_heavy('vn', 'f'), freqs, 'Hz'))), '--', 'color', colors(2,:), 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_pz_light('vn', 'f'), freqs, 'Hz'))), '-',  'color', colors(3,:), 'DisplayName', '$k_n = 100\,N/\mu m$');
plot(freqs, abs(squeeze(freqresp(G_pz_mid(  'vn', 'f'), freqs, 'Hz'))), '-.', 'color', colors(3,:), 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_pz_heavy('vn', 'f'), freqs, 'Hz'))), '--', 'color', colors(3,:), 'HandleVisibility', 'off');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/s/N]'); set(gca, 'XTickLabel',[]);
title('DVF: $v_n/f$');
ldg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
ldg.ItemTokenSize = [20, 1];

ax1b = nexttile();
hold on;
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_light('fm', 'f'), freqs, 'Hz')))), '-',  'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_mid(  'fm', 'f'), freqs, 'Hz')))), '-.', 'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_heavy('fm', 'f'), freqs, 'Hz')))), '--', 'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_light('fm', 'f'), freqs, 'Hz')))), '-',  'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_mid(  'fm', 'f'), freqs, 'Hz')))), '-.', 'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_heavy('fm', 'f'), freqs, 'Hz')))), '--', 'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_light('fm', 'f'), freqs, 'Hz')))), '-',  'color', colors(3,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_mid(  'fm', 'f'), freqs, 'Hz')))), '-.', 'color', colors(3,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_heavy('fm', 'f'), freqs, 'Hz')))), '--', 'color', colors(3,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
xticks([1e0, 1e1, 1e2]);
yticks(-360:90:360);
ylim([-200, 20]);

ax2b = nexttile();
hold on;
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_light('dL', 'f'), freqs, 'Hz')))), '-',  'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_mid(  'dL', 'f'), freqs, 'Hz')))), '-.', 'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_heavy('dL', 'f'), freqs, 'Hz')))), '--', 'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_light('dL', 'f'), freqs, 'Hz')))), '-',  'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_mid(  'dL', 'f'), freqs, 'Hz')))), '-.', 'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_heavy('dL', 'f'), freqs, 'Hz')))), '--', 'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_light('dL', 'f'), freqs, 'Hz')))), '-',  'color', colors(3,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_mid(  'dL', 'f'), freqs, 'Hz')))), '-.', 'color', colors(3,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_heavy('dL', 'f'), freqs, 'Hz')))), '--', 'color', colors(3,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
xticks([1e0, 1e1, 1e2]);
yticks(-360:90:360);
ylim([-200, 20]);

ax3b = nexttile();
hold on;
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_light('vn', 'f'), freqs, 'Hz')))), '-',  'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_mid(  'vn', 'f'), freqs, 'Hz')))), '-.', 'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_heavy('vn', 'f'), freqs, 'Hz')))), '--', 'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_light('vn', 'f'), freqs, 'Hz')))), '-',  'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_mid(  'vn', 'f'), freqs, 'Hz')))), '-.', 'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_heavy('vn', 'f'), freqs, 'Hz')))), '--', 'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_light('vn', 'f'), freqs, 'Hz')))), '-',  'color', colors(3,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_mid(  'vn', 'f'), freqs, 'Hz')))), '-.', 'color', colors(3,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_heavy('vn', 'f'), freqs, 'Hz')))), '--', 'color', colors(3,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
xticks([1e0, 1e1, 1e2]);
yticks(-360:90:360);
ylim([-110, 110]);

linkaxes([ax1,ax2,ax3,ax1b,ax2b,ax3b],'x');
xlim([1, 1000]);

% Achievable Damping - Root Locus
% <<ssec:uniaxial_active_damping_achievable_damping>>
% The Root Locus are computed for the three nano-hexapod stiffnesses and for the three active damping techniques.
% They are shown in Figure ref:fig:uniaxial_root_locus_damping_techniques.

% All three active damping approach can lead to *critical damping* of the nano-hexapod suspension mode.

% There is even some damping authority on micro-station modes in the following cases:
% - IFF with a stiff nano-hexapod (Figure ref:fig:uniaxial_root_locus_damping_techniques, right) ::
%   This can be understood from the mechanical equivalent of IFF shown in Figure ref:fig:uniaxial_active_damping_iff_equiv (right) considering an high stiffness $k$.
%   The micro-station top platform is connected to an inertial mass (the nano-hexapod) through a damper, which damps the micro-station suspension suspension mode.
% - DVF with a stiff nano-hexapod (Figure ref:fig:uniaxial_root_locus_damping_techniques, right) ::
%   In that case, the "sky hook damper" (see mechanical equivalent of IFF in Figure ref:fig:uniaxial_active_damping_dvf_equiv, right) is connected to the micro-station top platform through the stiff nano-hexapod.
% - RDC with a soft nano-hexapod (Figure ref:fig:uniaxial_root_locus_damping_techniques_micro_station_mode) ::
%   At the frequency of the micro-station mode, the nano-hexapod top mass is behaving as an inertial reference as the suspension mode of the soft nano-hexapod is at much lower frequency.
%   The micro-station and the nano-hexapod masses are connected through a large damper induced by RDC (see mechanical equivalent in Figure ref:fig:uniaxial_active_damping_rdc_equiv, right) which allows some damping of the micro-station.


%% Active Damping Robustness to change of sample's mass - Root Locus for all three damping techniques with 3 different sample's masses
figure;
tiledlayout(1, 3, 'TileSpacing', 'Compact', 'Padding', 'None');

%% Soft Nano-Hexapod
ax1 = nexttile();
hold on;
% IFF
plot(real(pole(G_vc_light('fm', 'f'))),  imag(pole(G_vc_light('fm', 'f'))), 'x', 'color', colors(1,:), ...
     'HandleVisibility', 'off');
plot(real(zero(G_vc_light('fm', 'f'))),  imag(zero(G_vc_light('fm', 'f'))), 'o', 'color', colors(1,:), ...
     'DisplayName', 'IFF');
for g = logspace(0, 2, 400)
    clpoles = pole(feedback(G_vc_light('fm', 'f'), g/s, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
         'HandleVisibility', 'off');
end

% RDC
plot(real(pole(G_vc_light('dL', 'f'))),  imag(pole(G_vc_light('dL', 'f'))), 'x', 'color', colors(2,:), ...
     'HandleVisibility', 'off');
plot(real(zero(G_vc_light('dL', 'f'))),  imag(zero(G_vc_light('dL', 'f'))), 'o', 'color', colors(2,:), ...
     'DisplayName', 'RDC');
for g = logspace(1,3,400)
    clpoles = pole(feedback(G_vc_light('dL', 'f'), -g*s, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end

% DVF
plot(real(pole(G_vc_light('vn', 'f'))),  imag(pole(G_vc_light('vn', 'f'))), 'x', 'color', colors(3,:), ...
     'HandleVisibility', 'off');
plot(real(zero(G_vc_light('vn', 'f'))),  imag(zero(G_vc_light('vn', 'f'))), 'o', 'color', colors(3,:), ...
     'DisplayName', 'DVF');
for g = logspace(1,3,400)
    clpoles = pole(feedback(G_vc_light('vn', 'f'), -g, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ...
         'HandleVisibility', 'off');
end
hold off;
axis square;
xlabel('Real Part'); ylabel('Imaginary Part');
title('$k_n = 0.01\,N/\mu m$')
ldg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
ldg.ItemTokenSize = [10, 1];
xlim([-30, 0]); ylim([0, 30]);

%% Medium-Stiff Nano-Hexapod
ax2 = nexttile();
hold on;
% IFF
plot(real(pole(G_md_light('fm', 'f'))),  imag(pole(G_md_light('fm', 'f'))), 'x', 'color', colors(1,:), ...
     'HandleVisibility', 'off');
plot(real(zero(G_md_light('fm', 'f'))),  imag(zero(G_md_light('fm', 'f'))), 'o', 'color', colors(1,:), ...
     'HandleVisibility', 'off');
for g = logspace(0,3,400)
    clpoles = pole(feedback(G_md_light('fm', 'f'), g/s, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
         'HandleVisibility', 'off');
end

% RDC
plot(real(pole(G_md_light('dL', 'f'))),  imag(pole(G_md_light('dL', 'f'))), 'x', 'color', colors(2,:), ...
     'HandleVisibility', 'off');
plot(real(zero(G_md_light('dL', 'f'))),  imag(zero(G_md_light('dL', 'f'))), 'o', 'color', colors(2,:), ...
     'HandleVisibility', 'off');
for g = logspace(2,4,400)
    clpoles = pole(feedback(G_md_light('dL', 'f'), -g*s, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end

% DVF
plot(real(pole(G_md_light('vn', 'f'))),  imag(pole(G_md_light('vn', 'f'))), 'x', 'color', colors(3,:), ...
     'HandleVisibility', 'off');
plot(real(zero(G_md_light('vn', 'f'))),  imag(zero(G_md_light('vn', 'f'))), 'o', 'color', colors(3,:), ...
     'HandleVisibility', 'off');
for g = logspace(2,4,400)
    clpoles = pole(feedback(G_md_light('vn', 'f'), -g, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ...
         'HandleVisibility', 'off');
end
hold off;
axis square;
xlabel('Real Part'); ylabel('Imaginary Part');
title('$k_n = 1\,N/\mu m$')
xlim([-300, 0]); ylim([0, 300]);

%% Stiff Nano-Hexapod
ax3 = nexttile();
hold on;
% IFF
plot(real(pole(G_pz_light('fm', 'f'))),  imag(pole(G_pz_light('fm', 'f'))), 'x', 'color', colors(1,:), ...
     'HandleVisibility', 'off');
plot(real(zero(G_pz_light('fm', 'f'))),  imag(zero(G_pz_light('fm', 'f'))), 'o', 'color', colors(1,:), ...
     'HandleVisibility', 'off');
for g = logspace(2,5,400)
    clpoles = pole(feedback(G_pz_light('fm', 'f'), g/s, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
         'HandleVisibility', 'off');
end

% RDC
plot(real(pole(G_pz_light('dL', 'f'))),  imag(pole(G_pz_light('dL', 'f'))), 'x', 'color', colors(2,:), ...
     'HandleVisibility', 'off');
plot(real(zero(G_pz_light('dL', 'f'))),  imag(zero(G_pz_light('dL', 'f'))), 'o', 'color', colors(2,:), ...
     'HandleVisibility', 'off');
for g = logspace(3,6,400)
    clpoles = pole(feedback(G_pz_light('dL', 'f'), -g*s, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end

% DVF
plot(real(pole(G_pz_light('vn', 'f'))),  imag(pole(G_pz_light('vn', 'f'))), 'x', 'color', colors(3,:), ...
     'HandleVisibility', 'off');
plot(real(zero(G_pz_light('vn', 'f'))),  imag(zero(G_pz_light('vn', 'f'))), 'o', 'color', colors(3,:), ...
     'HandleVisibility', 'off');
for g = logspace(3,6,400)
    clpoles = pole(feedback(G_pz_light('vn', 'f'), -g, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ...
         'HandleVisibility', 'off');
end
hold off;
axis square;
xlabel('Real Part'); ylabel('Imaginary Part');
title('$k_n = 100\,N/\mu m$')
xlim([-4000, 0]); ylim([0, 4000]);



% #+name: fig:uniaxial_root_locus_damping_techniques
% #+caption: Root Loci for the three active damping techniques (IFF in blue, RDC in red and DVF in yellow). This is shown for three nano-hexapod stiffnesses. The Root Loci are zoomed on the suspension mode of the nano-hexapod.
% #+RESULTS:
% [[file:figs/uniaxial_root_locus_damping_techniques.png]]


%% Root Locus for the three damping techniques
figure;

hold on;
% IFF
plot(real(pole(G_md_mid('fm', 'f'))),  imag(pole(G_md_mid('fm', 'f'))), 'x', 'color', colors(1,:), ...
     'DisplayName', 'IFF');
plot(real(zero(G_md_mid('fm', 'f'))),  imag(zero(G_md_mid('fm', 'f'))), 'o', 'color', colors(1,:), ...
     'HandleVisibility', 'off');
for g = logspace(1,4,500)
    clpoles = pole(feedback(G_md_mid('fm', 'f'), g/s, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
         'HandleVisibility', 'off');
end

% RDC
plot(real(pole(G_md_mid('dL', 'f'))),  imag(pole(G_md_mid('dL', 'f'))), 'x', 'color', colors(2,:), ...
     'DisplayName', 'RDC');
plot(real(zero(G_md_mid('dL', 'f'))),  imag(zero(G_md_mid('dL', 'f'))), 'o', 'color', colors(2,:), ...
     'HandleVisibility', 'off');
for g = logspace(2,5,500)
    clpoles = pole(feedback(G_md_mid('dL', 'f'), -g*s, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end

% DVF
plot(real(pole(G_md_mid('vn', 'f'))),  imag(pole(G_md_mid('vn', 'f'))), 'x', 'color', colors(3,:), ...
     'DisplayName', 'DVF');
plot(real(zero(G_md_mid('vn', 'f'))),  imag(zero(G_md_mid('vn', 'f'))), 'o', 'color', colors(3,:), ...
     'HandleVisibility', 'off');
for g = logspace(2,5,500)
    clpoles = pole(feedback(G_md_mid('vn', 'f'), -tf(g), +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ...
         'HandleVisibility', 'off');
end
hold off;
xlim([-2100, 0]); ylim([0, 2100]);
axis square;
xlabel('Real Part'); ylabel('Imaginary Part');
ldg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
ldg.ItemTokenSize = [10, 1];

% Active Damping Controller Optimization and Damped plants        :noexport:

%% Design of Active Damping controllers to have reasonable damping
% IFF
K_iff_vc = 20/(s + 2*pi*0.01);
K_iff_vc.InputName = {'fm'};
K_iff_vc.OutputName = {'f'};
K_iff_md = 200/(s + 2*pi*0.01);
K_iff_md.InputName = {'fm'};
K_iff_md.OutputName = {'f'};
K_iff_pz = 4000/(s + 2*pi*0.01);
K_iff_pz.InputName = {'fm'};
K_iff_pz.OutputName = {'f'};

% RDC
K_rdc_vc = -1e3*s;
K_rdc_vc.InputName = {'dL'};
K_rdc_vc.OutputName = {'f'};
K_rdc_md = -1e4*s;
K_rdc_md.InputName = {'dL'};
K_rdc_md.OutputName = {'f'};
K_rdc_pz = -1e5*s;
K_rdc_pz.InputName = {'dL'};
K_rdc_pz.OutputName = {'f'};

% DVF
K_dvf_vc = -tf(1e3);
K_dvf_vc.InputName = {'vn'};
K_dvf_vc.OutputName = {'f'};
K_dvf_md = -tf(8e3);
K_dvf_md.InputName = {'vn'};
K_dvf_md.OutputName = {'f'};
K_dvf_pz = -tf(2e5);
K_dvf_pz.InputName = {'vn'};
K_dvf_pz.OutputName = {'f'};

%% Save Active Damping Controller
save('./mat/uniaxial_active_damping_controllers.mat', 'K_iff_vc', 'K_iff_md', 'K_iff_pz', ...
                                                      'K_rdc_vc', 'K_rdc_md', 'K_rdc_pz', ...
                                                      'K_dvf_vc', 'K_dvf_md', 'K_dvf_pz');

%% Compute Damped Plants
% IFF
G_iff_vc_light = feedback(G_vc_light, K_iff_vc, 'name', +1);
G_iff_vc_mid   = feedback(G_vc_mid  , K_iff_vc, 'name', +1);
G_iff_vc_heavy = feedback(G_vc_heavy, K_iff_vc, 'name', +1);
G_iff_md_light = feedback(G_md_light, K_iff_md, 'name', +1);
G_iff_md_mid   = feedback(G_md_mid  , K_iff_md, 'name', +1);
G_iff_md_heavy = feedback(G_md_heavy, K_iff_md, 'name', +1);
G_iff_pz_light = feedback(G_pz_light, K_iff_pz, 'name', +1);
G_iff_pz_mid   = feedback(G_pz_mid  , K_iff_pz, 'name', +1);
G_iff_pz_heavy = feedback(G_pz_heavy, K_iff_pz, 'name', +1);

% RDC
G_rdc_vc_light = feedback(G_vc_light, K_rdc_vc, 'name', +1);
G_rdc_vc_mid   = feedback(G_vc_mid  , K_rdc_vc, 'name', +1);
G_rdc_vc_heavy = feedback(G_vc_heavy, K_rdc_vc, 'name', +1);
G_rdc_md_light = feedback(G_md_light, K_rdc_md, 'name', +1);
G_rdc_md_mid   = feedback(G_md_mid  , K_rdc_md, 'name', +1);
G_rdc_md_heavy = feedback(G_md_heavy, K_rdc_md, 'name', +1);
G_rdc_pz_light = feedback(G_pz_light, K_rdc_pz, 'name', +1);
G_rdc_pz_mid   = feedback(G_pz_mid  , K_rdc_pz, 'name', +1);
G_rdc_pz_heavy = feedback(G_pz_heavy, K_rdc_pz, 'name', +1);

% DVF
G_dvf_vc_light = feedback(G_vc_light, K_dvf_vc, 'name', +1);
G_dvf_vc_mid   = feedback(G_vc_mid  , K_dvf_vc, 'name', +1);
G_dvf_vc_heavy = feedback(G_vc_heavy, K_dvf_vc, 'name', +1);
G_dvf_md_light = feedback(G_md_light, K_dvf_md, 'name', +1);
G_dvf_md_mid   = feedback(G_md_mid  , K_dvf_md, 'name', +1);
G_dvf_md_heavy = feedback(G_md_heavy, K_dvf_md, 'name', +1);
G_dvf_pz_light = feedback(G_pz_light, K_dvf_pz, 'name', +1);
G_dvf_pz_mid   = feedback(G_pz_mid  , K_dvf_pz, 'name', +1);
G_dvf_pz_heavy = feedback(G_pz_heavy, K_dvf_pz, 'name', +1);

%% Verify Stability
% IFF
isstable(G_iff_vc_light) && isstable(G_iff_vc_mid) && isstable(G_iff_vc_heavy) && ...
isstable(G_iff_md_light) && isstable(G_iff_md_mid) && isstable(G_iff_md_heavy) && ...
isstable(G_iff_pz_light) && isstable(G_iff_pz_mid) && isstable(G_iff_pz_heavy)

% RDC
isstable(G_rdc_vc_light) && isstable(G_rdc_vc_mid) && isstable(G_rdc_vc_heavy) && ...
isstable(G_rdc_md_light) && isstable(G_rdc_md_mid) && isstable(G_rdc_md_heavy) && ...
isstable(G_rdc_pz_light) && isstable(G_rdc_pz_mid) && isstable(G_rdc_pz_heavy)

% DVF
isstable(G_dvf_vc_light) && isstable(G_dvf_vc_mid) && isstable(G_dvf_vc_heavy) && ...
isstable(G_dvf_md_light) && isstable(G_dvf_md_mid) && isstable(G_dvf_md_heavy) && ...
isstable(G_dvf_pz_light) && isstable(G_dvf_pz_mid) && isstable(G_dvf_pz_heavy)

%% Save Damped Plants
save('./mat/uniaxial_damped_plants.mat', 'G_iff_vc_light', 'G_iff_md_light', 'G_iff_pz_light', ...
                                         'G_rdc_vc_light', 'G_rdc_md_light', 'G_rdc_pz_light', ...
                                         'G_dvf_vc_light', 'G_dvf_md_light', 'G_dvf_pz_light', ...
                                         'G_iff_vc_mid',   'G_iff_md_mid',   'G_iff_pz_mid', ...
                                         'G_rdc_vc_mid',   'G_rdc_md_mid',   'G_rdc_pz_mid', ...
                                         'G_dvf_vc_mid',   'G_dvf_md_mid',   'G_dvf_pz_mid', ...
                                         'G_iff_vc_heavy', 'G_iff_md_heavy', 'G_iff_pz_heavy', ...
                                         'G_rdc_vc_heavy', 'G_rdc_md_heavy', 'G_rdc_pz_heavy', ...
                                         'G_dvf_vc_heavy', 'G_dvf_md_heavy', 'G_dvf_pz_heavy');

% Change of sensitivity to disturbances
% <<ssec:uniaxial_active_damping_sensitivity_disturbances>>

% The sensitivity to disturbances (direct forces $f_s$, stage vibrations $f_t$ and floor motion $x_f$) for all three active damping techniques are compared in Figure ref:fig:uniaxial_sensitivity_dist_active_damping.
% The comparison is done with the nano-hexapod having a stiffness $k_n = 1\,N/\mu m$.

% #+begin_important
% Conclusions from Figure ref:fig:uniaxial_sensitivity_dist_active_damping are:
% - IFF degrades the sensitivity to direct forces on the sample (i.e. the compliance) below the resonance of the nano-hexapod
% - RDC degrades the sensitivity to stage vibrations around the nano-hexapod's resonance as compared to the other two methods
% - both IFF and DVF degrades the sensitivity to floor motion below the resonance of the nano-hexapod
% #+end_important


%% Change of sensitivity to disturbance with all three active damping strategies
figure;
tiledlayout(1, 3, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(freqs, abs(squeeze(freqresp(G_md_mid('d', 'fs'), freqs, 'Hz'))), 'k-');
plot(freqs, abs(squeeze(freqresp(G_iff_md_mid('d', 'fs'), freqs, 'Hz'))), 'color', colors(1,:));
plot(freqs, abs(squeeze(freqresp(G_rdc_md_mid('d', 'fs'), freqs, 'Hz'))), 'color', colors(2,:));
plot(freqs, abs(squeeze(freqresp(G_dvf_md_mid('d', 'fs'), freqs, 'Hz'))), 'color', colors(3,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d/f_{s}$ [m/N]'); xlabel('Frequency [Hz]');
xticks([1e0, 1e1, 1e2]);

ax2 = nexttile();
hold on;
plot(freqs, abs(squeeze(freqresp(G_md_mid('d', 'ft'), freqs, 'Hz'))), 'k-');
plot(freqs, abs(squeeze(freqresp(G_iff_md_mid('d', 'ft'), freqs, 'Hz'))), 'color', colors(1,:));
plot(freqs, abs(squeeze(freqresp(G_rdc_md_mid('d', 'ft'), freqs, 'Hz'))), 'color', colors(2,:));
plot(freqs, abs(squeeze(freqresp(G_dvf_md_mid('d', 'ft'), freqs, 'Hz'))), 'color', colors(3,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d/f_{t}$ [m/N]'); xlabel('Frequency [Hz]');
xticks([1e0, 1e1, 1e2]);

ax3 = nexttile();
hold on;
plot(freqs, abs(squeeze(freqresp(G_md_mid('d', 'xf'), freqs, 'Hz'))), 'k-', 'DisplayName', 'OL');
plot(freqs, abs(squeeze(freqresp(G_iff_md_mid('d', 'xf'), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', 'IFF');
plot(freqs, abs(squeeze(freqresp(G_rdc_md_mid('d', 'xf'), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', 'RDC');
plot(freqs, abs(squeeze(freqresp(G_dvf_md_mid('d', 'xf'), freqs, 'Hz'))), 'color', colors(3,:), 'DisplayName', 'DVF');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d/x_{f}$ [m/m]'); xlabel('Frequency [Hz]');
xticks([1e0, 1e1, 1e2]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);

linkaxes([ax1,ax2,ax3],'x');
xlim([1, 500]);

% Noise Budgeting after Active Damping
% <<ssec:uniaxial_active_damping_noise_budget>>
% Cumulative Amplitude Spectrum of the distance $d$ with all three active damping techniques are compared in Figure ref:fig:uniaxial_cas_active_damping.
% All three active damping methods are giving similar results (except the RDC which is a little bit worse for the stiff nano-hexapod).

% Compared to the open-loop case, the active damping helps to lower the vibrations induced by the nano-hexapod resonance.


%% Cumulative Amplitude Spectrum of the distance d with all three active damping techniques
figure;
tiledlayout(1, 3, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_vc_mid('d', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_vc_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
     'color', 'black', 'DisplayName', 'OL');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_vc_mid('d', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_iff_vc_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
     'color', colors(1,:), 'DisplayName', 'IFF');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_rdc_vc_mid('d', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_rdc_vc_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
     'color', colors(2,:), 'DisplayName', 'RDC');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_dvf_vc_mid('d', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_dvf_vc_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
     'color', colors(3,:), 'DisplayName', 'DVF');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('CAS of $d$ [m]'); xlabel('Frequency [Hz]');
xticks([1e0, 1e1, 1e2]);
title('$k_n = 0.01\,N/\mu m$')

ax2 = nexttile();
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_md_mid('d', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_md_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
     'color', 'black', 'DisplayName', 'OL');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_md_mid('d', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_iff_md_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
     'color', colors(1,:), 'DisplayName', 'IFF');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_rdc_md_mid('d', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_rdc_md_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
     'color', colors(2,:), 'DisplayName', 'RDC');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_dvf_md_mid('d', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_dvf_md_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
     'color', colors(3,:), 'DisplayName', 'DVF');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
xticks([1e0, 1e1, 1e2]);
title('$k_n = 1\,N/\mu m$')

ax3 = nexttile();
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_pz_mid('d', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_pz_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
     'color', 'black', 'DisplayName', 'OL');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_pz_mid('d', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_iff_pz_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
     'color', colors(1,:), 'DisplayName', 'IFF');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_rdc_pz_mid('d', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_rdc_pz_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
     'color', colors(2,:), 'DisplayName', 'RDC');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_dvf_pz_mid('d', 'ft'), f, 'Hz'))).^2 + ...
                                     psd_xf.*abs(squeeze(freqresp(G_dvf_pz_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
     'color', colors(3,:), 'DisplayName', 'DVF');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
xticks([1e0, 1e1, 1e2]);
title('$k_n = 100\,N/\mu m$')
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);

linkaxes([ax1,ax2,ax3], 'xy')
xlim([1, 500]);
ylim([2e-10, 3e-6])

% Obtained Closed Loop Response
% The transfer functions from the plant input $f$ to the relative displacement $d$ while the active damping is implemented are shown in Figure ref:fig:uniaxial_damped_plant_three_active_damping_techniques.
% All three active damping techniques yield similar damped plants.


%% Obtained damped transfer function from f to d for the three damping techniques
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_md_mid('d', 'f'), freqs, 'Hz'))), 'k-', 'DisplayName', 'OL');
plot(freqs, abs(squeeze(freqresp(G_iff_md_mid('d', 'f'), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', 'IFF');
plot(freqs, abs(squeeze(freqresp(G_rdc_md_mid('d', 'f'), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', 'RDC');
plot(freqs, abs(squeeze(freqresp(G_dvf_md_mid('d', 'f'), freqs, 'Hz'))), 'color', colors(3,:), 'DisplayName', 'DVF');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d/f$ [m/N]'); set(gca, 'XTickLabel',[]);
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile();
hold on;
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_mid('d', 'f'), freqs, 'Hz')))), 'k-');
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_md_mid('d', 'f'), freqs, 'Hz')))), 'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_rdc_md_mid('d', 'f'), freqs, 'Hz')))), 'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_dvf_md_mid('d', 'f'), freqs, 'Hz')))), 'color', colors(3,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
yticks(-360:90:360);
ylim([-270, 90]);

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



% #+name: fig:uniaxial_damped_plant_three_active_damping_techniques
% #+caption: Obtained damped transfer function from f to d for the three damping techniques
% #+RESULTS:
% [[file:figs/uniaxial_damped_plant_three_active_damping_techniques.png]]

% The damped plants are shown in Figure ref:fig:uniaxial_damped_plant_change_sample_mass for all three techniques, with the three considered nano-hexapod stiffnesses and sample's masses.

%% Damped plant - Robustness to change of sample's mass
figure;
tiledlayout(3, 3, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_vc_mid('d', 'f'), freqs, 'Hz'))), '-', 'color', [0, 0, 0, 0.5], 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_md_mid('d', 'f'), freqs, 'Hz'))), '-', 'color', [0, 0, 0, 0.5], 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_pz_mid('d', 'f'), freqs, 'Hz'))), '-', 'color', [0, 0, 0, 0.5], 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_iff_vc_light('d', 'f'), freqs, 'Hz'))), '-',  'color', colors(1,:), 'DisplayName', '$k_n = 0.01\,N/\mu m$');
plot(freqs, abs(squeeze(freqresp(G_iff_vc_mid(  'd', 'f'), freqs, 'Hz'))), '-.', 'color', colors(1,:), 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_iff_vc_heavy('d', 'f'), freqs, 'Hz'))), '--', 'color', colors(1,:), 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_iff_md_light('d', 'f'), freqs, 'Hz'))), '-',  'color', colors(2,:), 'DisplayName', '$k_n = 1\,N/\mu m$');
plot(freqs, abs(squeeze(freqresp(G_iff_md_mid(  'd', 'f'), freqs, 'Hz'))), '-.', 'color', colors(2,:), 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_iff_md_heavy('d', 'f'), freqs, 'Hz'))), '--', 'color', colors(2,:), 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_iff_pz_light('d', 'f'), freqs, 'Hz'))), '-',  'color', colors(3,:), 'DisplayName', '$k_n = 100\,N/\mu m$');
plot(freqs, abs(squeeze(freqresp(G_iff_pz_mid(  'd', 'f'), freqs, 'Hz'))), '-.', 'color', colors(3,:), 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_iff_pz_heavy('d', 'f'), freqs, 'Hz'))), '--', 'color', colors(3,:), 'HandleVisibility', 'off');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('IFF');
ylim([5e-10, 1e-3]);
ldg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
ldg.ItemTokenSize = [20, 1];

ax2 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_vc_mid('d', 'f'), freqs, 'Hz'))), '-', 'color', [0, 0, 0, 0.5]);
plot(freqs, abs(squeeze(freqresp(G_md_mid('d', 'f'), freqs, 'Hz'))), '-', 'color', [0, 0, 0, 0.5]);
plot(freqs, abs(squeeze(freqresp(G_pz_mid('d', 'f'), freqs, 'Hz'))), '-', 'color', [0, 0, 0, 0.5]);
plot(freqs, abs(squeeze(freqresp(G_rdc_vc_light('d', 'f'), freqs, 'Hz'))), '-',  'color', colors(1,:));
plot(freqs, abs(squeeze(freqresp(G_rdc_vc_mid(  'd', 'f'), freqs, 'Hz'))), '-.', 'color', colors(1,:));
plot(freqs, abs(squeeze(freqresp(G_rdc_vc_heavy('d', 'f'), freqs, 'Hz'))), '--', 'color', colors(1,:));
plot(freqs, abs(squeeze(freqresp(G_rdc_md_light('d', 'f'), freqs, 'Hz'))), '-',  'color', colors(2,:));
plot(freqs, abs(squeeze(freqresp(G_rdc_md_mid(  'd', 'f'), freqs, 'Hz'))), '-.', 'color', colors(2,:));
plot(freqs, abs(squeeze(freqresp(G_rdc_md_heavy('d', 'f'), freqs, 'Hz'))), '--', 'color', colors(2,:));
plot(freqs, abs(squeeze(freqresp(G_rdc_pz_light('d', 'f'), freqs, 'Hz'))), '-',  'color', colors(3,:));
plot(freqs, abs(squeeze(freqresp(G_rdc_pz_mid(  'd', 'f'), freqs, 'Hz'))), '-.', 'color', colors(3,:));
plot(freqs, abs(squeeze(freqresp(G_rdc_pz_heavy('d', 'f'), freqs, 'Hz'))), '--', 'color', colors(3,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
title('RDC');
ylim([5e-10, 1e-3]);

ax3 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_vc_mid('d', 'f'), freqs, 'Hz'))), '-', 'color', [0, 0, 0, 0.5], 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_md_mid('d', 'f'), freqs, 'Hz'))), '-', 'color', [0, 0, 0, 0.5], 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_pz_mid('d', 'f'), freqs, 'Hz'))), '-', 'color', [0, 0, 0, 0.5], 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_dvf_vc_light('d', 'f'), freqs, 'Hz'))), '-',  'color', colors(1,:), 'DisplayName', '$m_s = 1\,kg$');
plot(freqs, abs(squeeze(freqresp(G_dvf_vc_mid(  'd', 'f'), freqs, 'Hz'))), '-.', 'color', colors(1,:), 'DisplayName', '$m_s = 25\,kg$');
plot(freqs, abs(squeeze(freqresp(G_dvf_vc_heavy('d', 'f'), freqs, 'Hz'))), '--', 'color', colors(1,:), 'DisplayName', '$m_s = 50\,kg$');
plot(freqs, abs(squeeze(freqresp(G_dvf_md_light('d', 'f'), freqs, 'Hz'))), '-',  'color', colors(2,:), 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_dvf_md_mid(  'd', 'f'), freqs, 'Hz'))), '-.', 'color', colors(2,:), 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_dvf_md_heavy('d', 'f'), freqs, 'Hz'))), '--', 'color', colors(2,:), 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_dvf_pz_light('d', 'f'), freqs, 'Hz'))), '-',  'color', colors(3,:), 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_dvf_pz_mid(  'd', 'f'), freqs, 'Hz'))), '-.', 'color', colors(3,:), 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_dvf_pz_heavy('d', 'f'), freqs, 'Hz'))), '--', 'color', colors(3,:), 'HandleVisibility', 'off');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
title('DVF');
ylim([5e-10, 1e-3]);
ldg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
ldg.ItemTokenSize = [20, 1];

ax1b = nexttile();
hold on;
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_mid('d', 'f'), freqs, 'Hz')))), '-', 'color', [0, 0, 0, 0.5]);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_mid('d', 'f'), freqs, 'Hz')))), '-', 'color', [0, 0, 0, 0.5]);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_mid('d', 'f'), freqs, 'Hz')))), '-', 'color', [0, 0, 0, 0.5]);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_vc_light('d', 'f'), freqs, 'Hz')))), '-',  'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_vc_mid(  'd', 'f'), freqs, 'Hz')))), '-.', 'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_vc_heavy('d', 'f'), freqs, 'Hz')))), '--', 'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_md_light('d', 'f'), freqs, 'Hz')))), '-',  'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_md_mid(  'd', 'f'), freqs, 'Hz')))), '-.', 'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_md_heavy('d', 'f'), freqs, 'Hz')))), '--', 'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_pz_light('d', 'f'), freqs, 'Hz')))), '-',  'color', colors(3,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_pz_mid(  'd', 'f'), freqs, 'Hz')))), '-.', 'color', colors(3,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_pz_heavy('d', 'f'), freqs, 'Hz')))), '--', 'color', colors(3,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
xticks([1e0, 1e1, 1e2]);
yticks(-360:90:360);
ylim([-200, 20]);

ax2b = nexttile();
hold on;
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_mid('d', 'f'), freqs, 'Hz')))), '-', 'color', [0, 0, 0, 0.5]);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_mid('d', 'f'), freqs, 'Hz')))), '-', 'color', [0, 0, 0, 0.5]);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_mid('d', 'f'), freqs, 'Hz')))), '-', 'color', [0, 0, 0, 0.5]);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_rdc_vc_light('d', 'f'), freqs, 'Hz')))), '-',  'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_rdc_vc_mid(  'd', 'f'), freqs, 'Hz')))), '-.', 'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_rdc_vc_heavy('d', 'f'), freqs, 'Hz')))), '--', 'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_rdc_md_light('d', 'f'), freqs, 'Hz')))), '-',  'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_rdc_md_mid(  'd', 'f'), freqs, 'Hz')))), '-.', 'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_rdc_md_heavy('d', 'f'), freqs, 'Hz')))), '--', 'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_rdc_pz_light('d', 'f'), freqs, 'Hz')))), '-',  'color', colors(3,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_rdc_pz_mid(  'd', 'f'), freqs, 'Hz')))), '-.', 'color', colors(3,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_rdc_pz_heavy('d', 'f'), freqs, 'Hz')))), '--', 'color', colors(3,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
xticks([1e0, 1e1, 1e2]);
yticks(-360:90:360);
ylim([-200, 20]);

ax3b = nexttile();
hold on;
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_mid('d', 'f'), freqs, 'Hz')))), '-', 'color', [0, 0, 0, 0.5]);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_mid('d', 'f'), freqs, 'Hz')))), '-', 'color', [0, 0, 0, 0.5]);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_mid('d', 'f'), freqs, 'Hz')))), '-', 'color', [0, 0, 0, 0.5]);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_dvf_vc_light('d', 'f'), freqs, 'Hz')))), '-',  'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_dvf_vc_mid(  'd', 'f'), freqs, 'Hz')))), '-.', 'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_dvf_vc_heavy('d', 'f'), freqs, 'Hz')))), '--', 'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_dvf_md_light('d', 'f'), freqs, 'Hz')))), '-',  'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_dvf_md_mid(  'd', 'f'), freqs, 'Hz')))), '-.', 'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_dvf_md_heavy('d', 'f'), freqs, 'Hz')))), '--', 'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_dvf_pz_light('d', 'f'), freqs, 'Hz')))), '-',  'color', colors(3,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_dvf_pz_mid(  'd', 'f'), freqs, 'Hz')))), '-.', 'color', colors(3,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_dvf_pz_heavy('d', 'f'), freqs, 'Hz')))), '--', 'color', colors(3,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
xticks([1e0, 1e1, 1e2]);
yticks(-360:90:360);
ylim([-200, 20]);

linkaxes([ax1,ax2,ax3,ax1b,ax2b,ax3b],'x');
xlim([1, 1e3]);

% Robustness to change of payload's mass
% <<ssec:uniaxial_active_damping_robustness>>

% The Root Locus for the three damping techniques are shown in Figure ref:fig:uniaxial_active_damping_robustness_mass_root_locus for three sample's mass (1kg, 25kg and 50kg).
% The closed-loop poles are shown by the squares for a specific gain.

% We can see that having heavier samples yields larger damping for IFF and smaller damping for RDC and DVF.


%% Active Damping Robustness to change of sample's mass - Root Locus for all three damping techniques with 3 different sample's masses
figure;
tiledlayout(1, 3, 'TileSpacing', 'Compact', 'Padding', 'None');

%% Integral Force Feedback
ax1 = nexttile();
hold on;
% Light Sample
plot(real(pole(G_md_light('fm', 'f'))),  imag(pole(G_md_light('fm', 'f'))), 'x', 'color', colors(1,:), ...
     'HandleVisibility', 'off');
plot(real(zero(G_md_light('fm', 'f'))),  imag(zero(G_md_light('fm', 'f'))), 'o', 'color', colors(1,:), ...
     'HandleVisibility', 'off');
for g = logspace(1,4,100)
    clpoles = pole(feedback(G_md_light('fm', 'f'), g/s, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
         'HandleVisibility', 'off');
end
clpoles = pole(feedback(G_md_light('fm', 'f'), K_iff_md, +1));
plot(real(clpoles), imag(clpoles), 'square', 'color', colors(1,:), ...
     'DisplayName', '$m_s = 1\,kg$');

% Mid Sample
plot(real(pole(G_md_mid('fm', 'f'))),  imag(pole(G_md_mid('fm', 'f'))), 'x', 'color', colors(2,:), ...
     'HandleVisibility', 'off');
plot(real(zero(G_md_mid('fm', 'f'))),  imag(zero(G_md_mid('fm', 'f'))), 'o', 'color', colors(2,:), ...
     'HandleVisibility', 'off');
for g = logspace(1,4,100)
    clpoles = pole(feedback(G_md_mid('fm', 'f'), g/s, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
clpoles = pole(feedback(G_md_mid('fm', 'f'), K_iff_md, +1));
plot(real(clpoles), imag(clpoles), 'square', 'color', colors(2,:), ...
     'DisplayName', '$m_s = 25\,kg$');

% Heavy Sample
plot(real(pole(G_md_heavy('fm', 'f'))),  imag(pole(G_md_heavy('fm', 'f'))), 'x', 'color', colors(3,:), ...
     'HandleVisibility', 'off');
plot(real(zero(G_md_heavy('fm', 'f'))),  imag(zero(G_md_heavy('fm', 'f'))), 'o', 'color', colors(3,:), ...
     'HandleVisibility', 'off');
for g = logspace(1,4,100)
    clpoles = pole(feedback(G_md_heavy('fm', 'f'), g/s, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ...
         'HandleVisibility', 'off');
end
clpoles = pole(feedback(G_md_heavy('fm', 'f'), K_iff_md, +1));
plot(real(clpoles), imag(clpoles), 'square', 'color', colors(3,:), ...
     'DisplayName', '$m_s = 50\,kg$');
hold off;
axis square;
xlabel('Real Part'); ylabel('Imaginary Part');
title('IFF')
ldg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
ldg.ItemTokenSize = [10, 1];

%% Relative Damping Control
ax2 = nexttile();
hold on;
% Light Sample
plot(real(pole(G_md_light('dL', 'f'))),  imag(pole(G_md_light('dL', 'f'))), 'x', 'color', colors(1,:), ...
     'DisplayName', '$m_s = 1\,kg$');
plot(real(zero(G_md_light('dL', 'f'))),  imag(zero(G_md_light('dL', 'f'))), 'o', 'color', colors(1,:), ...
     'HandleVisibility', 'off');
for g = logspace(2,5,100)
    clpoles = pole(feedback(G_md_light('dL', 'f'), -g*s, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
         'HandleVisibility', 'off');
end
clpoles = pole(feedback(G_md_light('dL', 'f'), K_rdc_md, +1));
plot(real(clpoles), imag(clpoles), 'square', 'color', colors(1,:), ...
     'HandleVisibility', 'off');

% Mid Sample
plot(real(pole(G_md_mid('dL', 'f'))),  imag(pole(G_md_mid('dL', 'f'))), 'x', 'color', colors(2,:), ...
     'DisplayName', '$m_s = 25\,kg$');
plot(real(zero(G_md_mid('dL', 'f'))),  imag(zero(G_md_mid('dL', 'f'))), 'o', 'color', colors(2,:), ...
     'HandleVisibility', 'off');
for g = logspace(2,5,100)
    clpoles = pole(feedback(G_md_mid('dL', 'f'), -g*s, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
clpoles = pole(feedback(G_md_mid('dL', 'f'), K_rdc_md, +1));
plot(real(clpoles), imag(clpoles), 'square', 'color', colors(2,:), ...
     'HandleVisibility', 'off');

% Heavy Sample
plot(real(pole(G_md_heavy('dL', 'f'))),  imag(pole(G_md_heavy('dL', 'f'))), 'x', 'color', colors(3,:), ...
     'DisplayName', '$m_s = 50\,kg$');
plot(real(zero(G_md_heavy('dL', 'f'))),  imag(zero(G_md_heavy('dL', 'f'))), 'o', 'color', colors(3,:), ...
     'HandleVisibility', 'off');
for g = logspace(2,5,100)
    clpoles = pole(feedback(G_md_heavy('dL', 'f'), -g*s, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ...
         'HandleVisibility', 'off');
end
clpoles = pole(feedback(G_md_heavy('dL', 'f'), K_rdc_md, +1));
plot(real(clpoles), imag(clpoles), 'square', 'color', colors(3,:), ...
     'HandleVisibility', 'off');
hold off;
axis square;
xlabel('Real Part'); ylabel('Imaginary Part');
title('RDC');

%% Direct Velocity Feedback
ax3 = nexttile();
hold on;
% Light Sample
plot(real(pole(G_md_light('vn', 'f'))),  imag(pole(G_md_light('vn', 'f'))), 'x', 'color', colors(1,:), ...
     'DisplayName', '$m_s = 1\,kg$');
plot(real(zero(G_md_light('vn', 'f'))),  imag(zero(G_md_light('vn', 'f'))), 'o', 'color', colors(1,:), ...
     'HandleVisibility', 'off');
for g = logspace(2,5,100)
    clpoles = pole(feedback(G_md_light('vn', 'f'), -g, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
         'HandleVisibility', 'off');
end
clpoles = pole(feedback(G_md_light('vn', 'f'), K_dvf_md, +1));
plot(real(clpoles), imag(clpoles), 'square', 'color', colors(1,:), ...
     'HandleVisibility', 'off');

% Mid Sample
plot(real(pole(G_md_mid('vn', 'f'))),  imag(pole(G_md_mid('vn', 'f'))), 'x', 'color', colors(2,:), ...
     'DisplayName', '$m_s = 25\,kg$');
plot(real(zero(G_md_mid('vn', 'f'))),  imag(zero(G_md_mid('vn', 'f'))), 'o', 'color', colors(2,:), ...
     'HandleVisibility', 'off');
for g = logspace(2,5,100)
    clpoles = pole(feedback(G_md_mid('vn', 'f'), -g, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
clpoles = pole(feedback(G_md_mid('vn', 'f'), K_dvf_md, +1));
plot(real(clpoles), imag(clpoles), 'square', 'color', colors(2,:), ...
     'HandleVisibility', 'off');

% Heavy Sample
plot(real(pole(G_md_heavy('vn', 'f'))),  imag(pole(G_md_heavy('vn', 'f'))), 'x', 'color', colors(3,:), ...
     'DisplayName', '$m_s = 50\,kg$');
plot(real(zero(G_md_heavy('vn', 'f'))),  imag(zero(G_md_heavy('vn', 'f'))), 'o', 'color', colors(3,:), ...
     'HandleVisibility', 'off');
for g = logspace(2,5,100)
    clpoles = pole(feedback(G_md_heavy('vn', 'f'), -g, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ...
         'HandleVisibility', 'off');
end
clpoles = pole(feedback(G_md_heavy('vn', 'f'), K_dvf_md, +1));
plot(real(clpoles), imag(clpoles), 'square', 'color', colors(3,:), ...
     'HandleVisibility', 'off');
hold off;
axis square;
xlabel('Real Part'); ylabel('Imaginary Part');
title('DVF');

linkaxes([ax1,ax2,ax3],'xy');
xlim([-300, 0]); ylim([0, 300]);