%% test_id31_3_iff.m

%% 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
addpath('./STEPS/'); % Path for STEPS
addpath('./subsystems/'); % Path for Subsystems Simulink files

%% Data directory
data_dir = './mat/';

% Simulink Model name
mdl = 'nass_model_id31';

%% Colors for the figures
colors = colororder;

%% Frequency Vector
freqs = logspace(log10(1), log10(2e3), 1000);

%% Sampling Time
Ts = 1e-4;

%% Specifications for Experiments
specs_dz_peak = 50; % [nm]
specs_dy_peak = 100; % [nm]
specs_ry_peak = 0.85; % [urad]
specs_dz_rms = 15; % [nm RMS]
specs_dy_rms = 30; % [nm RMS]
specs_ry_rms = 0.25; % [urad RMS]

% Load identified FRF for IFF Plant and Multi-Body Model
load('test_id31_identified_open_loop_plants.mat', 'G_iff_m0_Wz0', 'G_iff_m1_Wz0', 'G_iff_m2_Wz0', 'G_iff_m3_Wz0', 'f');
load('test_id31_simscape_open_loop_plants.mat', 'Gm_m0_Wz0', 'Gm_m1_Wz0', 'Gm_m2_Wz0', 'Gm_m3_Wz0');

figure;
tiledlayout(2, 3, 'TileSpacing', 'tight', 'Padding', 'tight');

ax1 = nexttile();
hold on;
plot(f, abs(G_iff_m0_Wz0(:, 1, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('Vs1', 'u1'), freqs, 'Hz'))));
text(12, 4e1, '$V_{s1}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]');
yticks([1e-2, 1e-1, 1e0, 1e1]);

ax2 = nexttile();
hold on;
plot(f, abs(G_iff_m0_Wz0(:, 2, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('Vs2', 'u1'), freqs, 'Hz'))));
text(12, 4e1, '$V_{s2}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);

ax3 = nexttile();
hold on;
plot(f, abs(G_iff_m0_Wz0(:, 3, 1)), ...
     'DisplayName', 'Measurements');
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('Vs3', 'u1'), freqs, 'Hz'))), ...
     'DisplayName', 'Model (2-DoF APA)');
text(12, 4e1, '$V_{s3}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax4 = nexttile();
hold on;
plot(f, abs(G_iff_m0_Wz0(:, 4, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('Vs4', 'u1'), freqs, 'Hz'))));
text(12, 4e1, '$V_{s4}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
xticks([10, 20, 50, 100, 200])
yticks([1e-2, 1e-1, 1e0, 1e1]);

ax5 = nexttile();
hold on;
plot(f, abs(G_iff_m0_Wz0(:, 5, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('Vs5', 'u1'), freqs, 'Hz'))));
text(12, 4e1, '$V_{s5}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xticks([10, 20, 50, 100, 200])

ax6 = nexttile();
hold on;
plot(f, abs(G_iff_m0_Wz0(:, 6, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('Vs6', 'u1'), freqs, 'Hz'))));
text(12, 4e1, '$V_{s6}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
xticks([10, 20, 50, 100, 200])

linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'xy');
xlim([10, 5e2]); ylim([1e-2, 5e1]);

%% IFF Controller Design
% Second order high pass filter
wz = 2*pi*10;
xiz = 0.7;
Ghpf = (s^2/wz^2)/(s^2/wz^2 + 2*xiz*s/wz + 1);

% IFF Controller
Kiff = -1e2 * ...              % Gain
       1/(0.01*2*pi + s) * ... % LPF: provides integral action
       Ghpf * ...              % 2nd order HPF (limit low frequency gain)
       eye(6);                 % Diagonal 6x6 controller (i.e. decentralized)

Kiff.InputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'};
Kiff.OutputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};

% The designed IFF controller is saved
save('./mat/test_id31_K_iff.mat', 'Kiff');

%% Bode plot of the designed decentralized IFF controller
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(squeeze(freqresp(Kiff(1,1), f, 'Hz'))), 'color', colors(1,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
ylim([1e-2, 1e1]);

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(squeeze(freqresp(Kiff(1,1), f, 'Hz'))), 'color', colors(1,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-180, 180])

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

%% Loop gain for the decentralized IFF controller
Kiff_frf = squeeze(freqresp(Kiff(1,1), f, 'Hz'));

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

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_iff_m0_Wz0(:, 1, 1).*Kiff_frf), 'color', colors(1,:), ...
     'DisplayName', '$m = 0$ kg');
plot(f, abs(G_iff_m1_Wz0(:, 1, 1).*Kiff_frf), 'color', colors(2,:), ...
     'DisplayName', '$m = 13$ kg');
plot(f, abs(G_iff_m2_Wz0(:, 1, 1).*Kiff_frf), 'color', colors(3,:), ...
     'DisplayName', '$m = 26$ kg');
plot(f, abs(G_iff_m3_Wz0(:, 1, 1).*Kiff_frf), 'color', colors(4,:), ...
     'DisplayName', '$m = 39$ kg');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
ylim([1e-2, 1e1]);
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(-G_iff_m0_Wz0(:,1,1).*Kiff_frf), 'color', colors(1,:));
plot(f, 180/pi*angle(-G_iff_m1_Wz0(:,1,1).*Kiff_frf), 'color', colors(2,:));
plot(f, 180/pi*angle(-G_iff_m2_Wz0(:,1,1).*Kiff_frf), 'color', colors(3,:));
plot(f, 180/pi*angle(-G_iff_m3_Wz0(:,1,1).*Kiff_frf), 'color', colors(4,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-180, 180])

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

%% Root Locus for IFF
gains = logspace(-2, 2, 100);
Gm_iff_m0 = Gm_m0_Wz0({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'});
Gm_iff_m1 = Gm_m1_Wz0({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'});
Gm_iff_m2 = Gm_m2_Wz0({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'});
Gm_iff_m3 = Gm_m3_Wz0({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'});

figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;
plot(real(pole(Gm_iff_m0)),  imag(pole(Gm_iff_m0)),  'x', 'color', colors(1,:), ...
    'DisplayName', '$g = 0$');
plot(real(tzero(Gm_iff_m0)), imag(tzero(Gm_iff_m0)), 'o', 'color', colors(1,:), ...
    'HandleVisibility', 'off');

for g = gains
    clpoles = pole(feedback(Gm_iff_m0, g*Kiff, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
        'HandleVisibility', 'off');
end

% Optimal gain
clpoles = pole(feedback(Gm_iff_m0, Kiff, +1));
plot(real(clpoles), imag(clpoles), 'kx', ...
    'DisplayName', '$g_{opt}$');
hold off;
axis equal;
xlim([-600, 0]); ylim([0, 1500]);
xticks([-600:300:0]);
yticks([0:300:1500]);
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
xlabel('Real part'); ylabel('Imaginary part');

%% description
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;
plot(real(pole(Gm_iff_m1)),  imag(pole(Gm_iff_m1)),  'x', 'color', colors(2,:), ...
    'DisplayName', '$g = 0$');
plot(real(tzero(Gm_iff_m1)), imag(tzero(Gm_iff_m1)), 'o', 'color', colors(2,:), ...
    'HandleVisibility', 'off');

for g = gains
    clpoles = pole(feedback(Gm_iff_m1, g*Kiff, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ...
        'HandleVisibility', 'off');
end

% Optimal gain
clpoles = pole(feedback(Gm_iff_m1, Kiff, +1));
plot(real(clpoles), imag(clpoles), 'kx', ...
    'DisplayName', '$g_{opt}$');
hold off;
axis equal;
xlim([-200, 0]); ylim([0, 500]);
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
xlabel('Real part'); ylabel('Imaginary part');

figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;
plot(real(pole(Gm_iff_m2)),  imag(pole(Gm_iff_m2)),  'x', 'color', colors(3,:), ...
    'DisplayName', '$g = 0$');
plot(real(tzero(Gm_iff_m2)), imag(tzero(Gm_iff_m2)), 'o', 'color', colors(3,:), ...
    'HandleVisibility', 'off');

for g = gains
    clpoles = pole(feedback(Gm_iff_m2, g*Kiff, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ...
        'HandleVisibility', 'off');
end

% Optimal gain
clpoles = pole(feedback(Gm_iff_m2, Kiff, +1));
plot(real(clpoles), imag(clpoles), 'kx', ...
    'DisplayName', '$g_{opt}$');
hold off;
axis equal;
xlim([-200, 0]); ylim([0, 500]);
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
xlabel('Real part'); ylabel('Imaginary part');

figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;
plot(real(pole(Gm_iff_m3)),  imag(pole(Gm_iff_m3)),  'x', 'color', colors(4,:), ...
    'DisplayName', '$g = 0$');
plot(real(tzero(Gm_iff_m3)), imag(tzero(Gm_iff_m3)), 'o', 'color', colors(4,:), ...
    'HandleVisibility', 'off');

for g = gains
    clpoles = pole(feedback(Gm_iff_m3, g*Kiff, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(4,:), ...
        'HandleVisibility', 'off');
end

% Optimal gain
clpoles = pole(feedback(Gm_iff_m3, Kiff, +1));
plot(real(clpoles), imag(clpoles), 'kx', ...
    'DisplayName', '$g_{opt}$');
hold off;
axis equal;
xlim([-200, 0]); ylim([0, 500]);
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
xlabel('Real part'); ylabel('Imaginary part');

%% Estimate damped plant from Multi-Body model
Gm_hac_m0_Wz0 = feedback(Gm_m0_Wz0, Kiff, 'name', +1);
Gm_hac_m1_Wz0 = feedback(Gm_m1_Wz0, Kiff, 'name', +1);
Gm_hac_m2_Wz0 = feedback(Gm_m2_Wz0, Kiff, 'name', +1);
Gm_hac_m3_Wz0 = feedback(Gm_m3_Wz0, Kiff, 'name', +1);

% Check Stability
if not(isstable(Gm_hac_m0_Wz0) && isstable(Gm_hac_m1_Wz0) && isstable(Gm_hac_m2_Wz0) && isstable(Gm_hac_m3_Wz0))
    warning("One of the damped system with decentralized IFF is not stable");
end

% The estimated damped plants from the multi-body model are saved
save('./mat/test_id31_simscape_damped_plants.mat', 'Gm_hac_m0_Wz0', 'Gm_hac_m1_Wz0', 'Gm_hac_m2_Wz0', 'Gm_hac_m3_Wz0');

%% Comparison of the open-loop plants and the estimated damped plant with IFF
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.3], ...
    'DisplayName', '$-\epsilon\mathcal{L}_{1}/u_1$ - 0 kg');
plot(freqs, abs(squeeze(freqresp(Gm_m1_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', [colors(2,:), 0.3], ...
    'DisplayName', '$-\epsilon\mathcal{L}_{1}/u_1$ - 13 kg');
plot(freqs, abs(squeeze(freqresp(Gm_m2_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', [colors(3,:), 0.3], ...
    'DisplayName', '$-\epsilon\mathcal{L}_{1}/u_1$ - 26 kg');
plot(freqs, abs(squeeze(freqresp(Gm_m3_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', [colors(4,:), 0.3], ...
    'DisplayName', '$-\epsilon\mathcal{L}_{1}/u_1$ - 39 kg');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', '$-\epsilon\mathcal{L}_{1}/u_1^\prime$ - 0 kg');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m1_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', '$-\epsilon\mathcal{L}_{1}/u_1^\prime$ - 13 kg');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m2_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', '$-\epsilon\mathcal{L}_{1}/u_1^\prime$ - 26 kg');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m3_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', colors(4,:), ...
    'DisplayName', '$-\epsilon\mathcal{L}_{1}/u_1^\prime$ - 39 kg');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-7, 4e-4]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0_Wz0('eL1','u1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.3]);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m1_Wz0('eL1','u1'), freqs, 'Hz'))), 'color', [colors(2,:), 0.3]);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m2_Wz0('eL1','u1'), freqs, 'Hz'))), 'color', [colors(3,:), 0.3]);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m3_Wz0('eL1','u1'), freqs, 'Hz'))), 'color', [colors(4,:), 0.3]);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_hac_m0_Wz0('eL1','u1'), freqs, 'Hz')))), 'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_hac_m1_Wz0('eL1','u1'), freqs, 'Hz')))), 'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_hac_m2_Wz0('eL1','u1'), freqs, 'Hz')))), 'color', colors(3,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_hac_m3_Wz0('eL1','u1'), freqs, 'Hz')))), 'color', colors(4,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-20, 200])

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

%% Identification of the damped Plant (transfer function from u' to dL)

% Load identification data
data_m0 = load('2023-08-17_17-53_damp_plant_m0_Wz0.mat');
data_m1 = load('2023-08-10_16-01_damp_plant_m1_Wz0.mat');
data_m2 = load('2023-08-10_17-28_damp_plant_m2_Wz0.mat');
data_m3 = load('2023-08-10_18-16_damp_plant_m3_Wz0.mat');

% Hannning Windows
Ts = 1e-4; % Sampling Time [s]
Nfft = floor(2.0/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

% And we get the frequency vector
[~, f] = tfestimate(data_m0.uL1.id_plant, data_m0.uL1.e_L1, win, Noverlap, Nfft, 1/Ts);

% Identification without any payload
G_hac_m0_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
    eL = [data_m0.(sprintf("uL%i", i_strut)).e_L1 ; data_m0.(sprintf("uL%i", i_strut)).e_L2 ; data_m0.(sprintf("uL%i", i_strut)).e_L3 ; data_m0.(sprintf("uL%i", i_strut)).e_L4 ; data_m0.(sprintf("uL%i", i_strut)).e_L5 ; data_m0.(sprintf("uL%i", i_strut)).e_L6]';

    G_hac_m0_Wz0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts);
end

% Identification with 1 "payload layer"
G_hac_m1_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
    eL = [data_m1.(sprintf("uL%i", i_strut)).e_L1 ; data_m1.(sprintf("uL%i", i_strut)).e_L2 ; data_m1.(sprintf("uL%i", i_strut)).e_L3 ; data_m1.(sprintf("uL%i", i_strut)).e_L4 ; data_m1.(sprintf("uL%i", i_strut)).e_L5 ; data_m1.(sprintf("uL%i", i_strut)).e_L6]';

    G_hac_m1_Wz0(:,:,i_strut) = tfestimate(data_m1.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts);
end

% Identification with 2 "payload layers"
G_hac_m2_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
    eL = [data_m2.(sprintf("uL%i", i_strut)).e_L1 ; data_m2.(sprintf("uL%i", i_strut)).e_L2 ; data_m2.(sprintf("uL%i", i_strut)).e_L3 ; data_m2.(sprintf("uL%i", i_strut)).e_L4 ; data_m2.(sprintf("uL%i", i_strut)).e_L5 ; data_m2.(sprintf("uL%i", i_strut)).e_L6]';

    G_hac_m2_Wz0(:,:,i_strut) = tfestimate(data_m2.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts);
end

% Identification with 3 "payload layers"
G_hac_m3_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
    eL = [data_m3.(sprintf("uL%i", i_strut)).e_L1 ; data_m3.(sprintf("uL%i", i_strut)).e_L2 ; data_m3.(sprintf("uL%i", i_strut)).e_L3 ; data_m3.(sprintf("uL%i", i_strut)).e_L4 ; data_m3.(sprintf("uL%i", i_strut)).e_L5 ; data_m3.(sprintf("uL%i", i_strut)).e_L6]';

    G_hac_m3_Wz0(:,:,i_strut) = tfestimate(data_m3.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts);
end

% The identified dynamics are then saved for further use.
save('./mat/test_id31_identified_damped_plants.mat', 'G_hac_m0_Wz0', 'G_hac_m1_Wz0', 'G_hac_m2_Wz0', 'G_hac_m3_Wz0', 'f');

%% Comparison of the identified HAC plant and the HAC plant extracted from the simscape model
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_hac_m0_Wz0(:, 1, 1)), 'color', [colors(1,:), 0.2], ...
     'DisplayName', '$m = 0$ kg');
for i = 2:6
    plot(f, abs(G_hac_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.2], ...
         'HandleVisibility', 'off')
end
plot(f, abs(G_hac_m1_Wz0(:, 1, 1)), 'color', [colors(2,:), 0.2], ...
     'DisplayName', '$m = 13$ kg');
for i = 2:6
    plot(f, abs(G_hac_m1_Wz0(:,i, i)), 'color', [colors(2,:), 0.2], ...
         'HandleVisibility', 'off')
end
plot(f, abs(G_hac_m2_Wz0(:, 1, 1)), 'color', [colors(3,:), 0.2], ...
     'DisplayName', '$m = 26$ kg');
for i = 2:6
    plot(f, abs(G_hac_m2_Wz0(:,i, i)), 'color', [colors(3,:), 0.2], ...
         'HandleVisibility', 'off')
end
plot(f, abs(G_hac_m3_Wz0(:, 1, 1)), 'color', [colors(4,:), 0.2], ...
     'DisplayName', '$m = 39$ kg');
for i = 2:6
    plot(f, abs(G_hac_m3_Wz0(:,i, i)), 'color', [colors(4,:), 0.2], ...
         'HandleVisibility', 'off')
end
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:), ...
     'DisplayName', '$m = 0$ kg (model)');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m1_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:), ...
     'DisplayName', '$m = 13$ kg (model)');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m2_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:), ...
     'DisplayName', '$m = 26$ kg (model)');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m3_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:), ...
     'DisplayName', '$m = 39$ kg (model)');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([2e-7, 3e-5]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
plot(f, 180/pi*unwrapphase(angle(G_hac_m0_Wz0(:,1,1)), f), 'color', [colors(1,:), 0.2]);
for i = 2:6
    plot(f, 180/pi*unwrapphase(angle(G_hac_m0_Wz0(:,i, i)), f), 'color', [colors(1,:), 0.2]);
end
plot(f, 180/pi*unwrapphase(angle(G_hac_m1_Wz0(:,1,1)), f), 'color', [colors(2,:), 0.2]);
for i = 2:6
    plot(f, 180/pi*unwrapphase(angle(G_hac_m1_Wz0(:,i, i)), f), 'color', [colors(2,:), 0.2]);
end
plot(f, 180/pi*unwrapphase(angle(G_hac_m2_Wz0(:,1,1)), f), 'color', [colors(3,:), 0.2]);
for i = 2:6
    plot(f, 180/pi*unwrapphase(angle(G_hac_m2_Wz0(:,i, i)), f), 'color', [colors(3,:), 0.2]);
end
plot(f, 180/pi*unwrapphase(angle(G_hac_m3_Wz0(:,1,1)), f), 'color', [colors(4,:), 0.2]);
for i = 2:6
    plot(f, 180/pi*unwrapphase(angle(G_hac_m3_Wz0(:,i, i)), f), 'color', [colors(4,:), 0.2]);
end
plot(freqs, 180/pi*unwrapphase(angle(squeeze(freqresp(-exp(-3e-4*s)*Gm_hac_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))), f), '--', 'color', colors(1,:));
plot(freqs, 180/pi*unwrapphase(angle(squeeze(freqresp(-exp(-3e-4*s)*Gm_hac_m1_Wz0('eL1', 'u1'), freqs, 'Hz'))), f), '--', 'color', colors(2,:));
plot(freqs, 180/pi*unwrapphase(angle(squeeze(freqresp(-exp(-3e-4*s)*Gm_hac_m2_Wz0('eL1', 'u1'), freqs, 'Hz'))), f), '--', 'color', colors(3,:));
plot(freqs, 180/pi*unwrapphase(angle(squeeze(freqresp(-exp(-3e-4*s)*Gm_hac_m3_Wz0('eL1', 'u1'), freqs, 'Hz'))), f), '--', 'color', colors(4,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-270, 20])

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