% Matlab Init                                              :noexport:ignore:

%% test_nhexa_3_model.m
% Compare the measured dynamics from u to de and to Vs with the Simscape model

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

%% Initialize Parameters for Simscape model
table_type = 'Rigid'; % On top of vibration table
device_type = 'None'; % On top of vibration table
payload_num = 0; % No Payload

% Simulink Model name
mdl = 'test_nhexa_simscape';

%% Colors for the figures
colors = colororder;

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

% Extract transfer function matrices from the Simscape Model      :noexport:

%% Set to true only if all the dynamics should again computed
% from the simscape model
extract_simscape_dynamics = false
if extract_simscape_dynamics

    %% Extract the transfer function matrix from the Simscape model
    % Initialization of the Simscape model
    table_type = 'Suspended'; % On top of vibration table
    device_type = 'Hexapod'; % Nano-Hexapod
    payload_num = 0; % No Payload

    n_hexapod = initializeNanoHexapod('flex_bot_type', '4dof', ...
                                      'flex_top_type', '4dof', ...
                                      'motion_sensor_type', 'plates', ...
                                      'actuator_type', '2dof');

    % Identify the FRF matrix from u to [de,Vs]
    clear io; io_i = 1;
    io(io_i) = linio([mdl, '/u'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
    io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoders
    io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Encoders

    G_de = {};
    G_Vs = {};

    for i = [0:3]
        payload_num = i; % Change the payload on the nano-hexapod
        G = exp(-s*1e-4)*linearize(mdl, io, 0.0);
        G.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
        G.OutputName = {'de1', 'de2', 'de3', 'de4', 'de5', 'de6', ...
                        'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'};
        G_de(i+1) = {G({'de1', 'de2', 'de3', 'de4', 'de5', 'de6'},:)};
        G_Vs(i+1) = {G({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'},:)};
    end

    % Save the identified plants
    save('./mat/test_nhexa_simscape_masses.mat', 'G_Vs', 'G_de')

    %% The same identification is performed, but this time with
    % "flexible" model of the APA
    table_type = 'Suspended'; % On top of vibration table
    device_type = 'Hexapod'; % Nano-Hexapod
    payload_num = 0; % No Payload

    n_hexapod = initializeNanoHexapod('flex_bot_type', '4dof', ...
                                      'flex_top_type', '4dof', ...
                                      'motion_sensor_type', 'plates', ...
                                      'actuator_type', 'flexible');

    G_de = {};
    G_Vs = {};

    for i = [0:3]
        payload_num = i; % Change the payload on the nano-hexapod
        G = exp(-s*1e-4)*linearize(mdl, io, 0.0);
        G.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
        G.OutputName = {'de1', 'de2', 'de3', 'de4', 'de5', 'de6', ...
                        'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'};
        G_de(i+1) = {G({'de1', 'de2', 'de3', 'de4', 'de5', 'de6'},:)};
        G_Vs(i+1) = {G({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'},:)};
    end

    % Save the identified plants
    save('./mat/test_nhexa_simscape_flexible_masses.mat', 'G_Vs', 'G_de')

end

% Nano-Hexapod model dynamics
% <<ssec:test_nhexa_comp_model>>


%% Load Simscape Model and measured FRF
sim_ol  = load('test_nhexa_simscape_masses.mat', 'G_Vs', 'G_de');
frf_ol  = load('test_nhexa_identified_frf_masses.mat', 'f', 'G_Vs', 'G_de');



% The Simscape model of the nano-hexapod is first configured with 4-DoF flexible joints, 2-DoF APA and rigid top and bottom platforms.
% The stiffness of the flexible joints are chosen based on the values estimated using the test bench and based on FEM.
% The parameters of the APA model are the ones determined from the test bench of the APA.
% The $6 \times 6$ transfer function matrices from $\mathbf{u}$ to $\mathbf{d}_e$ and from $\mathbf{u}$ to $\mathbf{V}_s$ are extracted then from the Simscape model.

% A first feature that should be checked is that the model well represents the "direct" terms of the measured FRF matrix.
% To do so, the diagonal terms of the extracted transfer function matrices are compared with the measured FRF in Figure ref:fig:test_nhexa_comp_simscape_diag.
% It can be seen that the 4 suspension modes of the nano-hexapod (at 122Hz, 143Hz, 165Hz and 191Hz) are well modelled.
% The three resonances that were attributed to "internal" flexible modes of the struts (at 237Hz, 349Hz and 395Hz) cannot be seen in the model, which is reasonable as the APA are here modelled as a simple uniaxial 2-DoF system.
% At higher frequencies, no resonances can be seen in the model, as the as the top plate and the encoder supports are modelled as rigid bodies.


%% Diagonal elements of the FRF matrix from u to de
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:,1, 1)), 'color', [colors(1,:),0.5], ...
     'DisplayName', '$d_{ei}/u_i$ - FRF')
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{1}(1,1), freqs, 'Hz'))), 'color', [colors(2,:),0.5], ...
     'DisplayName', '$d_{ei}/u_i$ - Model')
for i = 2:6
    plot(frf_ol.f, abs(frf_ol.G_de{1}(:,i, i)), 'color', [colors(1,:),0.5], ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{1}(i,i), freqs, 'Hz'))), 'color', [colors(2,:),0.5], ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-8, 5e-4]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(frf_ol.f, 180/pi*angle(frf_ol.G_de{1}(:,i, i)), 'color', [colors(1,:),0.5]);
    plot(freqs, 180/pi*angle(squeeze(freqresp(sim_ol.G_de{1}(i,i), freqs, 'Hz'))), 'color', [colors(2,:),0.5]);
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);

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

%% Diagonal elements of the FRF matrix from u to Vs
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(frf_ol.f, abs(frf_ol.G_Vs{1}(:,1, 1)), 'color', [colors(1,:),0.5], ...
     'DisplayName', '$V_{si}/u_i$ - FRF')
plot(freqs, abs(squeeze(freqresp(sim_ol.G_Vs{1}(1,1), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
     'DisplayName', '$V_{si}/u_i$ - Model')
for i = 2:6
    plot(frf_ol.f, abs(frf_ol.G_Vs{1}(:,i, i)), 'color', [colors(1,:),0.5], ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(sim_ol.G_Vs{1}(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
legend('location', 'southeast');

ax2 = nexttile;
hold on;
for i = 1:6
    plot(frf_ol.f, 180/pi*angle(frf_ol.G_Vs{1}(:,i, i)), 'color', [colors(1,:),0.5]);
    plot(freqs, 180/pi*angle(squeeze(freqresp(sim_ol.G_Vs{1}(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);

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

% Modelling dynamical coupling
% <<ssec:test_nhexa_comp_model_coupling>>

% Another wanted feature of the model is that it well represents the coupling in the system as this is often the limiting factor for the control of MIMO systems.
% Instead of comparing the full 36 elements of the $6 \times 6$ FFR matrix from $\mathbf{u}$ to $\mathbf{d}_e$, only the first "column" is compared (Figure ref:fig:test_nhexa_comp_simscape_de_all), which corresponds to the transfer function from the command $u_1$ to the six measured encoder displacements $d_{e1}$ to $d_{e6}$.
% It can be seen that the coupling in the model is well matching the measurements up to the first un-modelled flexible mode at 237Hz.
% Similar results are observed for all the other coupling terms, as well as for the transfer function from $\mathbf{u}$ to $\mathbf{V}_s$.


%% Comparison of the plants (encoder output) when tuning the misalignment
i_input = 1;

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

ax1 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 1, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{1}(1, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e1}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]');

ax2 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 2, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{1}(2, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e2}/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(frf_ol.f, abs(frf_ol.G_de{1}(:, 3, i_input)), ...
     'DisplayName', 'Measurements');
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{1}(3, i_input), freqs, 'Hz'))), ...
     'DisplayName', 'Model (2-DoF APA)');
text(54, 4e-4, '$d_{e3}/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(frf_ol.f, abs(frf_ol.G_de{1}(:, 4, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{1}(4, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e4}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
xticks([50, 100, 200, 400])

ax5 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 5, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{1}(5, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e5}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xticks([50, 100, 200, 400])

ax6 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 6, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{1}(6, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e6}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
xticks([50, 100, 200, 400])

linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'xy');
xlim([50, 5e2]); ylim([1e-8, 5e-4]);



% #+name: fig:test_nhexa_comp_simscape_de_all
% #+caption: Comparison of the measured (in blue) and modelled (in red) frequency transfer functions from the first control signal $u_1$ to the six encoders $d_{e1}$ to $d_{e6}$
% #+RESULTS:
% [[file:figs/test_nhexa_comp_simscape_de_all.png]]

% The APA300ML are then modelled with a /super-element/ extracted from a FE-software.
% The obtained transfer functions from $u_1$ to the six measured encoder displacements $d_{e1}$ to $d_{e6}$ are compared with the measured FRF in Figure ref:fig:test_nhexa_comp_simscape_de_all_flex.
% While the damping of the suspension modes for the /super-element/ is underestimated (which could be solved by properly tuning the proportional damping coefficients), the flexible modes of the struts at 237Hz and 349Hz are well modelled.
% Even the mode 395Hz can be observed in the model.
% Therefore, if the modes of the struts are to be modelled, the /super-element/ of the APA300ML may be used, at the cost of obtaining a much higher order model.


%% Load the plant model with Flexible APA
flex_ol = load('test_nhexa_simscape_flexible_masses.mat', 'G_Vs', 'G_de');

%% Comparison of the plants (encoder output) when tuning the misalignment
i_input = 1;

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

ax1 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 1, i_input)));
plot(freqs, abs(squeeze(freqresp(flex_ol.G_de{1}(1, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e1}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]');

ax2 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 2, i_input)));
plot(freqs, abs(squeeze(freqresp(flex_ol.G_de{1}(2, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e2}/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(frf_ol.f, abs(frf_ol.G_de{1}(:, 3, i_input)), ...
     'DisplayName', 'Measurements');
plot(freqs, abs(squeeze(freqresp(flex_ol.G_de{1}(3, i_input), freqs, 'Hz'))), ...
     'DisplayName', 'Model (Flexible APA)');
text(54, 4e-4, '$d_{e3}/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(frf_ol.f, abs(frf_ol.G_de{1}(:, 4, i_input)));
plot(freqs, abs(squeeze(freqresp(flex_ol.G_de{1}(4, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e4}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
xticks([50, 100, 200, 400])

ax5 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 5, i_input)));
plot(freqs, abs(squeeze(freqresp(flex_ol.G_de{1}(5, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e5}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xticks([50, 100, 200, 400])

ax6 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{1}(:, 6, i_input)));
plot(freqs, abs(squeeze(freqresp(flex_ol.G_de{1}(6, i_input), freqs, 'Hz'))));
text(54, 4e-4, '$d_{e6}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
xticks([50, 100, 200, 400])

linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'xy');
xlim([50, 5e2]); ylim([1e-8, 5e-4]);

% Modelling the effect of payload mass
% <<ssec:test_nhexa_comp_model_masses>>

% Another important characteristics of the model is that it should well represents the dynamics of the system for all considered payloads.
% The model dynamics is therefore compared with the measured dynamics for 4 payloads (no payload, 13kg, 26kg and 39kg) in Figure ref:fig:test_nhexa_comp_simscape_diag_masses.
% The observed shift to lower frequency of the suspension modes with an increased payload mass is well represented by the Simscape model.
% The complex conjugate zeros are also well matching with the experiments both for the encoder outputs (Figure ref:fig:test_nhexa_comp_simscape_de_diag_masses) and the force sensor outputs (Figure ref:fig:test_nhexa_comp_simscape_Vs_diag_masses).

% Note that the model displays smaller damping that what is observed experimentally for high values of the payload mass.
% One option could be to tune the damping as a function of the mass (similar to what is done with the Rayleigh damping).
% However, as decentralized IFF will be applied, the damping will be brought actively, and the open-loop damping value should have very little impact on the obtained plant.


%% Bode plot for the transfer function from u to de
masses = [0, 13, 26, 39];
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i_mass = [0:3]
    plot(frf_ol.f, abs(frf_ol.G_de{i_mass+1}(:,1, 1)), 'color', [colors(i_mass+1,:), 0.2], ...
         'DisplayName', sprintf('Meas (%i kg)', masses(i_mass+1)));
    for i = 2:6
        plot(frf_ol.f, abs(frf_ol.G_de{i_mass+1}(:,i, i)), 'color', [colors(i_mass+1,:), 0.2], ...
             'HandleVisibility', 'off');
    end
    set(gca, 'ColorOrderIndex', i_mass+1)
    plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{i_mass+1}(1,1), freqs, 'Hz'))), '--', ...
         'DisplayName', 'Simscape');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/u$ [m/V]'); set(gca, 'XTickLabel',[]);
ylim([5e-8, 1e-3]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i_mass = [0:3]
    for i =1:6
        plot(frf_ol.f, 180/pi*angle(frf_ol.G_de{i_mass+1}(:,i, i)), 'color', [colors(i_mass+1,:), 0.2]);
    end
    set(gca, 'ColorOrderIndex', i_mass+1)
    plot(freqs, 180/pi*angle(squeeze(freqresp(sim_ol.G_de{i_mass+1}(1,1), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360);
ylim([-45, 180]);

linkaxes([ax1,ax2],'x');
xlim([20, 2e2]);
xticks([20, 50, 100, 200])

%% Bode plot for the transfer function from u to Vs
masses = [0, 13, 26, 39];
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i_mass = 0:3
    plot(frf_ol.f, abs(frf_ol.G_Vs{i_mass+1}(:,1, 1)), 'color', [colors(i_mass+1,:), 0.2], ...
         'DisplayName', sprintf('Meas (%i kg)', masses(i_mass+1)));
    for i = 2:6
        plot(frf_ol.f, abs(frf_ol.G_Vs{i_mass+1}(:,i, i)), 'color', [colors(i_mass+1,:), 0.2], ...
             'HandleVisibility', 'off');
    end
    plot(freqs, abs(squeeze(freqresp(sim_ol.G_Vs{i_mass+1}(1,1), freqs, 'Hz'))), '--', 'color', colors(i_mass+1,:), ...
         'DisplayName', 'Simscape');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/u$ [V/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-3, 1e2]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i_mass = 0:3
    for i =1:6
        plot(frf_ol.f, 180/pi*angle(frf_ol.G_Vs{i_mass+1}(:,i, i)), 'color', [colors(i_mass+1,:), 0.2]);
    end
    plot(freqs, 180/pi*angle(squeeze(freqresp(sim_ol.G_Vs{i_mass+1}(i,i), freqs, 'Hz'))), '--', 'color', colors(i_mass+1,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);

linkaxes([ax1,ax2],'x');
xlim([20, 2e2]);
xticks([20, 50, 100, 200])



% #+name: fig:test_nhexa_comp_simscape_diag_masses
% #+caption: Comparison of the diagonal elements (i.e. "direct" terms) of the measured FRF matrix and the identified dynamics from the Simscape model. Both for the dynamics from $u$ to $d_e$ (\subref{fig:test_nhexa_comp_simscape_de_diag}) and from $u$ to $V_s$ (\subref{fig:test_nhexa_comp_simscape_Vs_diag})
% #+attr_latex: :options [htbp]
% #+begin_figure
% #+attr_latex: :caption \subcaption{\label{fig:test_nhexa_comp_simscape_de_diag_masses}from $u$ to $d_e$}
% #+attr_latex: :options {0.49\textwidth}
% #+begin_subfigure
% #+attr_latex: :width 0.95\linewidth
% [[file:figs/test_nhexa_comp_simscape_de_diag_masses.png]]
% #+end_subfigure
% #+attr_latex: :caption \subcaption{\label{fig:test_nhexa_comp_simscape_Vs_diag_masses}from $u$ to $V_s$}
% #+attr_latex: :options {0.49\textwidth}
% #+begin_subfigure
% #+attr_latex: :width 0.95\linewidth
% [[file:figs/test_nhexa_comp_simscape_Vs_diag_masses.png]]
% #+end_subfigure
% #+end_figure

% In order to also check if the model well represents the coupling when high payload masses are used, the transfer functions from $u_1$ to $d_{e1}$ to $d_{e6}$ are compared in the case of the 39kg payload in Figure ref:fig:test_nhexa_comp_simscape_de_all_high_mass.
% Excellent match between the experimental coupling and the model coupling is observed.
% The model therefore well represents the system dynamical coupling for different considered payloads.


%% Comparison of the plants (encoder output) when tuning the misalignment
i_input = 1;

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

ax1 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{4}(:, 1, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{4}(1, i_input), freqs, 'Hz'))));
text(12, 4e-4, '$d_{e1}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]');

ax2 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{4}(:, 2, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{4}(2, i_input), freqs, 'Hz'))));
text(12, 4e-4, '$d_{e2}/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(frf_ol.f, abs(frf_ol.G_de{4}(:, 3, i_input)), ...
     'DisplayName', 'Measurements');
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{4}(3, i_input), freqs, 'Hz'))), ...
     'DisplayName', 'Model (2-DoF APA)');
text(12, 4e-4, '$d_{e3}/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(frf_ol.f, abs(frf_ol.G_de{4}(:, 4, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{4}(4, i_input), freqs, 'Hz'))));
text(12, 4e-4, '$d_{e4}/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, 50, 100, 200, 400])

ax5 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{4}(:, 5, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{4}(5, i_input), freqs, 'Hz'))));
text(12, 4e-4, '$d_{e5}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xticks([10, 50, 100, 200, 400])

ax6 = nexttile();
hold on;
plot(frf_ol.f, abs(frf_ol.G_de{4}(:, 6, i_input)));
plot(freqs, abs(squeeze(freqresp(sim_ol.G_de{4}(6, i_input), freqs, 'Hz'))));
text(12, 4e-4, '$d_{e6}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
xticks([10, 50, 100, 200, 400])

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