% Matlab Init                                              :noexport:ignore:

%% test_nhexa_dynamics.m
% Identification of the nano-hexapod dynamics from u to dL and to Vs
% Encoders are fixed to the plates

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

%% Colors for the figures
colors = colororder;

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

% Identification of the dynamics
% <<ssec:test_nhexa_identification>>

% The dynamics of the nano-hexapod from the six command signals ($u_1$ to $u_6$) the six measured displacement by the encoders ($d_{e1}$ to $d_{e6}$) and to the six force sensors ($V_{s1}$ to $V_{s6}$) are identified by generating a low pass filtered white noise for each of the command signals, one by one.

% The $6 \times 6$ FRF matrix from $\mathbf{u}$ ot $\mathbf{d}_e$ is shown in Figure ref:fig:test_nhexa_identified_frf_de.
% The diagonal terms are displayed using colorful lines, and all the 30 off-diagonal terms are displayed by grey lines.

% All the six diagonal terms are well superimposed up to at least $1\,kHz$, indicating good manufacturing and mounting uniformity.
% Below the first suspension mode, good decoupling can be observed (the amplitude of the all of off-diagonal terms are $\approx 20$ times smaller than the diagonal terms).

% From 10Hz up to 1kHz, around 10 resonance frequencies can be observed.
% The first 4 are suspension modes (at 122Hz, 143Hz, 165Hz and 191Hz) which correlate the modes measured during the modal analysis in Section ref:ssec:test_nhexa_enc_struts_modal_analysis.
% Then, three modes at 237Hz, 349Hz and 395Hz are attributed to the internal strut resonances (this will be checked in Section ref:ssec:test_nhexa_comp_model_coupling).
% Except the mode at 237Hz, their amplitude is rather low.
% Two modes at 665Hz and 695Hz are attributed to the flexible modes of the top platform.
% Other modes can be observed above 1kHz, which can be attributed to flexible modes of the encoder supports or to flexible modes of the top platform.

% Up to at least 1kHz, an alternating pole/zero pattern is observed, which renders the control easier to tune.
% This would not have been the case if the encoders were fixed to the struts.


%% Load identification data
load('test_nhexa_identification_data_mass_0.mat', 'data');

%% Setup useful variables
Ts = 1e-4; % Sampling Time [s]
Nfft = floor(1/Ts); % Number of points for the FFT computation
win = hanning(Nfft); % Hanning window
Noverlap = floor(Nfft/2); % Overlap between frequency analysis

% And we get the frequency vector
[~, f] = tfestimate(data{1}.u, data{1}.de, win, Noverlap, Nfft, 1/Ts);

%% Transfer function from u to dLm
G_de = zeros(length(f), 6, 6);

for i = 1:6
    G_de(:,:,i) = tfestimate(data{i}.u, data{i}.de, win, Noverlap, Nfft, 1/Ts);
end

%% Transfer function from u to Vs
G_Vs = zeros(length(f), 6, 6);

for i = 1:6
    G_Vs(:,:,i) = tfestimate(data{i}.u, data{i}.Vs, win, Noverlap, Nfft, 1/Ts);
end

%% Bode plot for the transfer function from u to dLm
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:5
    for j = i+1:6
        plot(f, abs(G_de(:, i, j)), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i =1:6
    set(gca,'ColorOrderIndex',i)
    plot(f, abs(G_de(:,i, i)), ...
         'DisplayName', sprintf('$d_{e,%i}/u_%i$', i, i));
end
plot(f, abs(G_de(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$d_{e,i}/u_j$');
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', 4);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i =1:6
    set(gca,'ColorOrderIndex',i)
    plot(f, 180/pi*angle(G_de(:,i, i)));
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([10, 2e3]);



% #+name: fig:test_nhexa_identified_frf_de
% #+caption: Measured FRF for the transfer function from $\mathbf{u}$ to $\mathbf{d}_e$. The 6 diagonal terms are the colorfull lines (all superimposed), and the 30 off-diagonal terms are the shaded black lines.
% #+RESULTS:
% [[file:figs/test_nhexa_identified_frf_de.png]]


% Similarly, the $6 \times 6$ FRF matrix from $\mathbf{u}$ to $\mathbf{V}_s$ is shown in Figure ref:fig:test_nhexa_identified_frf_Vs.
% Alternating poles and zeros is observed up to at least 2kHz, which is a necessary characteristics in order to apply decentralized IFF.
% Similar to what was observed for the encoder outputs, all the "diagonal" terms are well superimposed, indicating that the same controller can be applied for all the struts.
% The first flexible mode of the struts as 235Hz is appearing, and therefore is should be possible to add some damping to this mode using IFF.


%% Bode plot of the IFF Plant (transfer function from u to Vs)
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:5
    for j = i+1:6
        plot(f, abs(G_Vs(:, i, j)), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i =1:6
    set(gca,'ColorOrderIndex',i)
    plot(f, abs(G_Vs(:,i , i)), ...
         'DisplayName', sprintf('$V_{s%i}/u_%i$', i, i));
end
plot(f, abs(G_Vs(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$V_{si}/u_j$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 4);
leg.ItemTokenSize(1) = 15;
ylim([1e-3, 6e1]);

ax2 = nexttile;
hold on;
for i =1:6
    set(gca,'ColorOrderIndex',i)
    plot(f, 180/pi*angle(G_Vs(:,i, i)));
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([10, 2e3]);

% Effect of payload mass on the dynamics
% <<ssec:test_nhexa_added_mass>>

% As one major challenge in the control of the NASS is the wanted robustness to change of payload mass, it is necessary to understand how the dynamics of the nano-hexapod changes with a change of payload mass.

% In order to study this change of dynamics with the payload mass, up to three "cylindrical masses" of $13\,kg$ each can be added for a total of $\approx 40\,kg$.
% These three cylindrical masses on top of the nano-hexapod are shown in Figure ref:fig:test_nhexa_table_mass_3.

% #+name: fig:test_nhexa_table_mass_3
% #+caption: Picture of the nano-hexapod with the added three cylindrical masses for a total of $\approx 40\,kg$
% #+attr_org: :width 800px
% #+attr_latex: :width 0.8\linewidth
% [[file:figs/test_nhexa_table_mass_3.jpg]]


%% Load identification Data
meas_added_mass = {...
    load('test_nhexa_identification_data_mass_0.mat', 'data'), ....
    load('test_nhexa_identification_data_mass_1.mat', 'data'), ....
    load('test_nhexa_identification_data_mass_2.mat', 'data'), ....
    load('test_nhexa_identification_data_mass_3.mat', 'data')};

%% Setup useful variables
Ts = 1e-4; % Sampling Time [s]
Nfft = floor(1/Ts); % Number of points for the FFT computation
win = hanning(Nfft); % Hanning window
Noverlap = floor(Nfft/2); % Overlap between frequency analysis

% And we get the frequency vector
[~, f] = tfestimate(meas_added_mass{1}.data{1}.u, meas_added_mass{1}.data{1}.de, win, Noverlap, Nfft, 1/Ts);

G_de = {};

for i_mass = [0:3]
    G_de(i_mass+1) = {zeros(length(f), 6, 6)};
    for i_strut = 1:6
        G_de{i_mass+1}(:,:,i_strut) = tfestimate(meas_added_mass{i_mass+1}.data{i_strut}.u, meas_added_mass{i_mass+1}.data{i_strut}.de, win, Noverlap, Nfft, 1/Ts);
    end
end

%% IFF Plant (transfer function from u to Vs)
G_Vs = {};

for i_mass = [0:3]
    G_Vs(i_mass+1) = {zeros(length(f), 6, 6)};
    for i_strut = 1:6
        G_Vs{i_mass+1}(:,:,i_strut) = tfestimate(meas_added_mass{i_mass+1}.data{i_strut}.u, meas_added_mass{i_mass+1}.data{i_strut}.Vs, win, Noverlap, Nfft, 1/Ts);
    end
end

save('./mat/test_nhexa_identified_frf_masses.mat', 'f', 'G_Vs', 'G_de')

%% Load the identified transfer functions
frf_ol = load('test_nhexa_identified_frf_masses.mat', 'f', 'G_Vs', 'G_de');



% The obtained frequency response functions from actuator signal $u_i$ to the associated encoder $d_{ei}$ for the four payload conditions (no mass, 13kg, 26kg and 39kg) are shown in Figure ref:fig:test_nhexa_identified_frf_de_masses.
% As expected, the frequency of the suspension modes are decreasing with an increase of the payload mass.
% The low frequency gain does not change as it is linked to the stiffness property of the nano-hexapod, and not to its mass property.

% The frequencies of the two flexible modes of the top plate are first decreased a lot when the first mass is added (from $\approx 700\,Hz$ to $\approx 400\,Hz$).
% This is due to the fact that the added mass is composed of two half cylinders which are not fixed together.
% It therefore adds a lot of mass to the top plate without adding stiffness in one direction.
% When more than one "mass layer" is added, the half cylinders are added with some angles such that rigidity are added in all directions (see how the three mass "layers" are positioned in Figure ref:fig:test_nhexa_table_mass_3).
% In that case, the frequency of these flexible modes are increased.
% In practice, the payload should be one solid body, and no decrease of the frequency of this flexible mode should be observed.
% The apparent amplitude of the flexible mode of the strut at 237Hz becomes smaller as the payload mass is increased.

% The measured FRF from $u_i$ to $V_{si}$ are shown in Figure ref:fig:test_nhexa_identified_frf_Vs_masses.
% For all the tested payloads, the measured FRF always have alternating poles and zeros, indicating that IFF can be applied in a robust way.


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

ax1 = nexttile([2,1]);
hold on;
for i_mass = [0:3]
    % Diagonal terms
    plot(frf_ol.f, abs(frf_ol.G_de{i_mass+1}(:,1, 1)), 'color', [colors(i_mass+1,:), 0.5], ...
         'DisplayName', sprintf('$d_{ei}/u_i$ - %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.5], ...
             'HandleVisibility', 'off');
    end
    % % Off-Diagonal terms
    % for i = 1:5
    %     for j = i+1:6
    %         plot(frf_ol.f, abs(frf_ol.G_de{i_mass+1}(:,i,j)), 'color', [colors(i_mass+1,:), 0.2], ...
    %              'HandleVisibility', 'off');
    %     end
    % end
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', 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.5]);
    end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-90, 180])

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

%% Bode plot for the transfer function from u to dLm
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i_mass = [0:3]
    % Diagonal terms
    plot(frf_ol.f, abs(frf_ol.G_Vs{i_mass+1}(:,1, 1)), 'color', [colors(i_mass+1,:), 0.5], ...
         'DisplayName', sprintf('$V_{si}/u_i$ - %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.5], ...
             'HandleVisibility', 'off');
    end
    % % Off-Diagonal terms
    % for i = 1:5
    %     for j = i+1:6
    %         plot(frf_ol.f, abs(frf_ol.G_Vs{i_mass+1}(:,i,j)), 'color', [colors(i_mass+1,:), 0.2], ...
    %              'HandleVisibility', 'off');
    %     end
    % end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-2, 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.5]);
    end
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([10, 2e3]);