phd-simscape-nass/matlab/nass_2_hac.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';

%% Colors for the figures
colors = colororder;

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

% HAC Plant
% <<ssec:nass_hac_plant>>

% The plant dynamics from force inputs $\bm{f}$ to the strut errors $\bm{\epsilon}_{\mathcal{L}}$ were first extracted from the multi-body model without implementation of the decentralized IFF.
% The influence of spindle rotation on plant dynamics was investigated, with results presented in Figure ref:fig:nass_undamped_plant_effect_Wz.
% While rotational motion introduces coupling effects at low frequencies, these remain minimal at operational velocities, owing to the high stiffness characteristics of the nano-hexapod assembly.

% Payload mass emerged as a significant parameter affecting system behavior, as illustrated in Figure ref:fig:nass_undamped_plant_effect_mass.
% As expected, increasing payload mass was found to decrease resonance frequencies while amplifying coupling at low frequency.
% These mass-dependent dynamic changes present considerable challenges for control system design, particularly for configurations with high payload masses.

% Additional operational parameters were systematically evaluated, including the $R_y$ tilt angle, $R_z$ spindle position, and micro-hexapod position.
% These factors were found to exert negligible influence on the plant dynamics, attributable to the effective mechanical decoupling achieved between the plant and micro-station dynamics.
% This decoupling characteristic ensures consistent performance across various operational configurations.
% This also validates the developed control kinematics.


%% Identify the IFF plant dynamics using the Simscape model

% Initialize each Simscape model elements
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeSimplifiedNanoHexapod();
initializeSample('type', 'cylindrical', 'm', 1);

% Initial Simscape Configuration
initializeSimscapeConfiguration('gravity', false);
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController('type', 'open-loop');
initializeReferences();

% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],     1, 'input');                  io_i = io_i + 1; % Actuator Inputs [N]
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL');  io_i = io_i + 1; % Strut errors [m]

% Identify HAC Plant without using IFF
initializeSample('type', 'cylindrical', 'm', 1);
G_m1 = linearize(mdl, io);
G_m1.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_m1.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

initializeSample('type', 'cylindrical', 'm', 25);
G_m25 = linearize(mdl, io);
G_m25.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_m25.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

initializeSample('type', 'cylindrical', 'm', 50);
G_m50 = linearize(mdl, io);
G_m50.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_m50.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

% Effect of Rotation
initializeSample('type', 'cylindrical', 'm', 1);
initializeReferences(...
    'Rz_type', 'rotating', ...
    'Rz_period', 1); % 360 deg/s

G_m1_Rz = linearize(mdl, io, 0.1);
G_m1_Rz.InputName  = {'f1',  'f2',  'f3',  'f4',  'f5',  'f6'};
G_m1_Rz.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

%% Effect of rotation on the HAC plant
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_m1(1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, $\Omega = 0$')
plot(freqs, abs(squeeze(freqresp(G_m1_Rz(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, $\Omega = 360$ deg/s')
plot(freqs, abs(squeeze(freqresp(G_m1(1,2), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_j$')
plot(freqs, abs(squeeze(freqresp(G_m1_Rz(1,2), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_j$')
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_m1(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_m1_Rz(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(G_m1(i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_m1_Rz(i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-11, 2e-5]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m1(i,i), freqs, 'Hz')))), 'color', colors(1,:));
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m1_Rz(i,i), freqs, 'Hz')))), 'color', colors(2,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-200, 20]);
yticks([-180:45:180]);

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

%% Effect of payload's mass on the HAC plant
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_m1( 1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, 1 kg')
plot(freqs, abs(squeeze(freqresp(G_m25(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, 25 kg')
plot(freqs, abs(squeeze(freqresp(G_m50(1,1), freqs, 'Hz'))), 'color', colors(3,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, 50 kg')
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_m1(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_m25(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_m50(i,j), freqs, 'Hz'))), 'color', [colors(3,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(G_m1( i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_m25(i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_m50(i,i), freqs, 'Hz'))), 'color', colors(3,:), ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-11, 2e-5]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m1(i,i), freqs, 'Hz')))), 'color', colors(1,:));
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25(i,i), freqs, 'Hz')))), 'color', colors(2,:));
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m50(i,i), freqs, 'Hz')))), 'color', colors(3,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-200, 20]);
yticks([-180:45:180]);

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



% #+name: fig:nass_undamped_plant_effect
% #+caption: Effect of the Spindle's rotational velocity on the positioning plant (\subref{fig:nass_undamped_plant_effect_Wz}) and effect of the payload's mass on the positioning plant (\subref{fig:nass_undamped_plant_effect_mass})
% #+attr_latex: :options [h!tbp]
% #+begin_figure
% #+attr_latex: :caption \subcaption{\label{fig:nass_undamped_plant_effect_Wz}Effect of rotational velocity $\Omega_z$}
% #+attr_latex: :options {0.48\textwidth}
% #+begin_subfigure
% #+attr_latex: :width 0.95\linewidth
% [[file:figs/nass_undamped_plant_effect_Wz.png]]
% #+end_subfigure
% #+attr_latex: :caption \subcaption{\label{fig:nass_undamped_plant_effect_mass}Effect of payload's mass}
% #+attr_latex: :options {0.48\textwidth}
% #+begin_subfigure
% #+attr_latex: :width 0.95\linewidth
% [[file:figs/nass_undamped_plant_effect_mass.png]]
% #+end_subfigure
% #+end_figure

% The Decentralized Integral Force Feedback was implemented in the multi-body model, and transfer functions from force inputs $\bm{f}^\prime$ of the damped plant to the strut errors $\bm{\epsilon}_{\mathcal{L}}$ were extracted from this model.

% The effectiveness of IFF implementation was first evaluated with a $1\,\text{kg}$ payload, as demonstrated in Figure ref:fig:nass_comp_undamped_damped_plant_m1.
% The results indicate successful damping of the nano-hexapod resonance modes, though a minor increase in low-frequency coupling was observed.
% This trade-off was considered acceptable given the overall improvement in system behavior.

% The benefits of IFF implementation were further assessed across the full range of payload configurations, with results presented in Figure ref:fig:nass_hac_plants.
% For all tested payloads ($1\,\text{kg}$, $25\,\text{kg}$ and $50\,\text{kg}$), decentralized IFF significantly damped the nano-hexapod modes and therefore simplified the system dynamics.
% More importantly, is the fact that in the vicinity of the wanted high authority control bandwidth (i.e. between $10\,\text{Hz}$ and $50\,\text{Hz}$), the damped dynamics (shown in red) exhibited minimal gain and phase variations with frequency.
% For the undamped system (shown in blue), achieving robust control with bandwidth above 10Hz while maintaining stability across different payload masses would be practically unfeasible.


%% Identify HAC Plant with IFF
initializeReferences(); % No Spindle Rotation
initializeController('type', 'iff'); % Implemented IFF controller
load('nass_K_iff.mat', 'Kiff'); % Load designed IFF controller

% 1kg payload
initializeSample('type', 'cylindrical', 'm', 1);
G_hac_m1 = linearize(mdl, io);
G_hac_m1.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_hac_m1.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

% 25kg payload
initializeSample('type', 'cylindrical', 'm', 25);
G_hac_m25 = linearize(mdl, io);
G_hac_m25.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_hac_m25.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

% 50kg payload
initializeSample('type', 'cylindrical', 'm', 50);
G_hac_m50 = linearize(mdl, io);
G_hac_m50.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_hac_m50.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

% Check stability
if not(isstable(G_hac_m1) && isstable(G_hac_m25) && isstable(G_hac_m50))
    warning('One of HAC plant is not stable')
end

%% Comparison of the OL plant and the plant with IFF - 1kg payload
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_m1( 1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$ - OL')
plot(freqs, abs(squeeze(freqresp(G_hac_m1(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$ - IFF')
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_m1(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_hac_m1(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(G_m1( i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_hac_m1(i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-10, 5e-5]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m1(i,i), freqs, 'Hz')))), 'color', colors(1,:));
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_hac_m1(i,i), freqs, 'Hz')))), 'color', colors(2,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-200, 20]);
yticks([-180:45:180]);

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

%% Comparison of all the undamped FRF and all the damped FRF
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_m1(    1,1), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], 'DisplayName', '$\epsilon\mathcal{L}_i/f_i$ - OL');
plot(freqs, abs(squeeze(freqresp(G_hac_m1(1,1), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], 'DisplayName', '$\epsilon\mathcal{L}_i/f_i^\prime$ - IFF');
for i = 1:6
    plot(freqs, abs(squeeze(freqresp(G_m1( i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_m25(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_m50(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
end
for i = 1:6
    plot(freqs, abs(squeeze(freqresp(G_hac_m1( i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_hac_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_hac_m50(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/N]'); set(gca, 'XTickLabel',[]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ylim([1e-10, 5e-5]);

ax2 = nexttile;
hold on;
for i =1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_m1( i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_m25(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_m50(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
end
for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_hac_m1( i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_hac_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_hac_m50(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
ylim([-200, 20]);
yticks([-180:45:180]);

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



% #+name: fig:nass_hac_plant
% #+caption: Effect of Decentralized Integral Force Feedback on the positioning plant for a $1\,\text{kg}$ sample mass (\subref{fig:nass_undamped_plant_effect_Wz}). Direct terms of the positioning plants for all considered payloads are shown in (\subref{fig:nass_undamped_plant_effect_mass}).
% #+attr_latex: :options [h!tbp]
% #+begin_figure
% #+attr_latex: :caption \subcaption{\label{fig:nass_comp_undamped_damped_plant_m1}Effect of IFF - $m = 1\,\text{kg}$}
% #+attr_latex: :options {0.48\textwidth}
% #+begin_subfigure
% #+attr_latex: :width 0.95\linewidth
% [[file:figs/nass_comp_undamped_damped_plant_m1.png]]
% #+end_subfigure
% #+attr_latex: :caption \subcaption{\label{fig:nass_hac_plants}Effect of IFF on the set of plants to control}
% #+attr_latex: :options {0.48\textwidth}
% #+begin_subfigure
% #+attr_latex: :width 0.95\linewidth
% [[file:figs/nass_hac_plants.png]]
% #+end_subfigure
% #+end_figure

% The coupling between the nano-hexapod and micro-station was evaluated through comparative analysis of plant dynamics under two mounting conditions.
% In the first configuration, the nano-hexapod was mounted on an ideally rigid support, while in the second configuration, it was installed on the micro-station with finite compliance.

% As illustrated in Figure ref:fig:nass_effect_ustation_compliance, the complex dynamics of the micro-station were found to have little impact on the plant dynamics.
% The only observable difference manifests as alternating poles and zeros above 100Hz, a frequency range sufficiently beyond the control bandwidth to avoid interference with system performance.
% This finding confirms effective dynamic decoupling between the nano-hexapod and the supporting micro-station structure.


%% Identify plant with "rigid" micro-station
initializeGround('type', 'rigid');
initializeGranite('type', 'rigid');
initializeTy('type', 'rigid');
initializeRy('type', 'rigid');
initializeRz('type', 'rigid');
initializeMicroHexapod('type', 'rigid');
initializeSimplifiedNanoHexapod();
initializeSample('type', 'cylindrical', 'm', 25);

initializeReferences();
initializeController('type', 'open-loop'); % Implemented IFF controller
load('nass_K_iff.mat', 'Kiff'); % Load designed IFF controller

% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],     1, 'input');                  io_i = io_i + 1; % Actuator Inputs [N]
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL');  io_i = io_i + 1; % Strut errors [m]

G_m25_rigid = linearize(mdl, io);
G_m25_rigid.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_m25_rigid.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

%% Effect of the micro-station limited compliance on the plant dynamics
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_m25_rigid( 1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$ - Rigid support')
plot(freqs, abs(squeeze(freqresp(G_m25(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$ - $\mu$-station support')
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_m25_rigid(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_m25(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(G_m25_rigid( i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_m25(i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-10, 5e-5]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25_rigid(i,i), freqs, 'Hz')))), 'color', colors(1,:));
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25(i,i), freqs, 'Hz')))), 'color', colors(2,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-200, 20]);
yticks([-180:45:180]);

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

% Effect of Nano-Hexapod Stiffness on System Dynamics
% <<ssec:nass_hac_stiffness>>

% The influence of nano-hexapod stiffness was investigated to validate earlier findings from simplified uniaxial and three-degree-of-freedom (3DoF) models.
% These models suggested that a moderate stiffness of approximately $1\,N/\mu m$ would provide better performance compared to either very stiff or very soft configurations.

% For the stiff nano-hexapod analysis, a system with actuator stiffness of $100\,N/\mu m$ was simulated with a $25\,\text{kg}$ payload.
% The transfer function from $\bm{f}$ to $\bm{\epsilon}_{\mathcal{L}}$ was evaluated under two conditions: mounting on an infinitely rigid base and mounting on the micro-station.
% As shown in Figure ref:fig:nass_stiff_nano_hexapod_coupling_ustation, significant coupling was observed between the nano-hexapod and micro-station dynamics.
% This coupling introduces complex behavior that proves difficult to model and predict accurately, corroborating the predictions of the simplified uniaxial model.

% The soft nano-hexapod configuration was evaluated using a stiffness of $0.01\,N/\mu m$ with a $25\,\text{kg}$ payload.
% Dynamic response was characterized at three rotational velocities: 0, 36, and 360 deg/s.
% Figure ref:fig:nass_soft_nano_hexapod_effect_Wz demonstrates that rotation substantially impacts system dynamics, manifesting as instability at high rotational velocities, increased coupling from gyroscopic effects, and rotation-dependent resonance frequencies.
% The current approach of controlling the motion in the strut frame proves inadequate for soft nano-hexapods; but even shifting control to the payload's center of mass frame would not overcome the substantial coupling and dynamic variations induced by gyroscopic effects.


%% Identify Dynamics with a Stiff nano-hexapod (100N/um)

% Initialize each Simscape model elements
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeSimplifiedNanoHexapod('actuator_k', 1e8, 'actuator_kp', 0, 'actuator_c', 1e3);
initializeSample('type', 'cylindrical', 'm', 25);

% Initial Simscape Configuration
initializeSimscapeConfiguration('gravity', false);
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController('type', 'open-loop');
initializeReferences();

% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],     1, 'input');                  io_i = io_i + 1; % Actuator Inputs [N]
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL');  io_i = io_i + 1; % Strut errors [m]

% Identify Plant
G_m25_pz = linearize(mdl, io);
G_m25_pz.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_m25_pz.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

% Compare with Nano-Hexapod alone (rigid micro-station)
initializeGround('type', 'rigid');
initializeGranite('type', 'rigid');
initializeTy('type', 'rigid');
initializeRy('type', 'rigid');
initializeRz('type', 'rigid');
initializeMicroHexapod('type', 'rigid');

% Identify Plant
G_m25_pz_rigid = linearize(mdl, io);
G_m25_pz_rigid.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_m25_pz_rigid.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

%% Stiff nano-hexapod - Coupling with the micro-station
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_m25_pz_rigid(1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$ - Rigid')
plot(freqs, abs(squeeze(freqresp(G_m25_pz(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$ - $\mu$-station')
plot(freqs, abs(squeeze(freqresp(G_m25_pz_rigid(1,2), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_j$ - Rigid')
plot(freqs, abs(squeeze(freqresp(G_m25_pz(1,2), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
     'DisplayName', '$\epsilon_{\mathcal{L}i}/f_j$ - $\mu$-station')
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_m25_pz_rigid(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(G_m25_pz(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
             'HandleVisibility', 'off');
    end
end
for i = 2:6
    plot(freqs, abs(squeeze(freqresp(G_m25_pz_rigid(i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G_m25_pz(i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-12, 3e-7]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25_pz_rigid(i,i), freqs, 'Hz')))), 'color', colors(1,:));
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25_pz(i,i), freqs, 'Hz')))), 'color', colors(2,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-200, 20]);
yticks([-180:45:180]);

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

%% Identify Dynamics with a Soft nano-hexapod (0.01N/um)
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeSimplifiedNanoHexapod('actuator_k', 1e4, 'actuator_kp', 0, 'actuator_c', 1);

% Initialize each Simscape model elements
initializeSample('type', 'cylindrical', 'm', 25); % 25kg payload
initializeController('type', 'open-loop');

% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],     1, 'input');                  io_i = io_i + 1; % Actuator Inputs [N]
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL');  io_i = io_i + 1; % Strut errors [m]

% Identify the dynamics without rotation
initializeReferences();
G_m1_vc = linearize(mdl, io);
G_m1_vc.InputName  = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
G_m1_vc.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

% Identify the dynamics with 36 deg/s rotation
initializeReferences(...
    'Rz_type', 'rotating', ...
    'Rz_period', 10); % 36 deg/s

G_m1_vc_Rz_slow = linearize(mdl, io, 0.1);
G_m1_vc_Rz_slow.InputName  = {'f1',  'f2',  'f3',  'f4',  'f5',  'f6'};
G_m1_vc_Rz_slow.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

% Identify the dynamics with 360 deg/s rotation
initializeReferences(...
    'Rz_type', 'rotating', ...
    'Rz_period', 1); % 360 deg/s

G_m1_vc_Rz_fast = linearize(mdl, io, 0.1);
G_m1_vc_Rz_fast.InputName  = {'f1',  'f2',  'f3',  'f4',  'f5',  'f6'};
G_m1_vc_Rz_fast.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};

%% Soft Nano-Hexapod - effect of rotational velocity on the dynamics
f = logspace(-1,2,200);
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(squeeze(freqresp(G_m1_vc(1,1), f, 'Hz'))), 'color', colors(1,:), ...
     'DisplayName', '$f_{ni}/f_i$ - $\Omega_z = 0$')
plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_slow(1,1), f, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', '$f_{ni}/f_i$ - $\Omega_z = 36$ deg/s')
plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_fast(1,1), f, 'Hz'))), 'color', colors(3,:), ...
     'DisplayName', '$f_{ni}/f_i$ - $\Omega_z = 360$ deg/s')
for i = 1:5
    for j = i+1:6
        plot(f, abs(squeeze(freqresp(G_m1_vc(i,j), f, 'Hz'))), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_slow(i,j), f, 'Hz'))), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_fast(i,j), f, 'Hz'))), 'color', [colors(3,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i = 2:6
    plot(f, abs(squeeze(freqresp(G_m1_vc(i,i), f, 'Hz'))), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
end
for i = 2:6
    plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_slow(i,i), f, 'Hz'))), 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
for i = 2:6
    plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_fast(i,i), f, 'Hz'))), 'color', colors(3,:), ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-9, 1e-2]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(f, 180/pi*angle(squeeze(freqresp(G_m1_vc(i,i), f, 'Hz'))), 'color', colors(1,:));
    plot(f, 180/pi*angle(squeeze(freqresp(G_m1_vc_Rz_slow(i,i), f, 'Hz'))), 'color', colors(2,:));
    plot(f, 180/pi*angle(squeeze(freqresp(G_m1_vc_Rz_fast(i,i), f, 'Hz'))), 'color', colors(3,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180:90:180]);

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

% Controller design
% <<ssec:nass_hac_controller>>

% A high authority controller was designed to meet two key requirements: stable for all payload masses (i.e. for all the damped plants of Figure ref:fig:nass_hac_plants), and achievement of sufficient bandwidth (targeted at 10Hz) for high performance operation.
% The controller structure is defined in Equation eqref:eq:nass_robust_hac, incorporating an integrator term for low frequency performance, a lead compensator for phase margin improvement, and a low-pass filter for robustness against high frequency modes.

% \begin{equation}\label{eq:nass_robust_hac}
%   K_{\text{HAC}}(s) = g_0 \cdot \underbrace{\frac{\omega_c}{s}}_{\text{int}} \cdot \underbrace{\frac{1}{\sqrt{\alpha}}\frac{1 + \frac{s}{\omega_c/\sqrt{\alpha}}}{1 + \frac{s}{\omega_c\sqrt{\alpha}}}}_{\text{lead}} \cdot \underbrace{\frac{1}{1 + \frac{s}{\omega_0}}}_{\text{LPF}}, \quad \left( \omega_c = 2\pi10\,\text{rad/s},\ \alpha = 2,\ \omega_0 = 2\pi80\,\text{rad/s} \right)
% \end{equation}


%% HAC Design
% Wanted crossover
wc = 2*pi*10; % [rad/s]

% Integrator
H_int = wc/s;

% Lead to increase phase margin
a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));

% Low Pass filter to increase robustness
H_lpf = 1/(1 + s/2/pi/80);

% Gain to have unitary crossover at wc
H_gain = 1./abs(evalfr(G_hac_m50(1,1), 1j*wc));

% Decentralized HAC
Khac = -H_gain  * ... % Gain
        H_int   * ... % Integrator
        H_lead   * ... % Low Pass filter
        H_lpf   * ... % Low Pass filter
        eye(6);       % 6x6 Diagonal

% The designed HAC controller is saved
save('./mat/nass_K_hac.mat', 'Khac');



% The controller's performance was evaluated through two complementary analyses.
% First, the decentralized loop gain, shown in Figure ref:fig:nass_hac_loop_gain, confirms the achievement of the desired 10Hz bandwidth.
% Second, the characteristic loci analysis presented in Figure ref:fig:nass_hac_loci demonstrates robustness for all payload masses, with adequate stability margins maintained throughout the operating envelope.


%% "Diagonal" loop gain for the High Authority Controller
f = logspace(-1, 2, 1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m1( i,i), f, 'Hz'))), ...
     'color', [colors(1,:), 0.5], 'DisplayName', '1kg');
plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m25(i,i), f, 'Hz'))), ...
     'color', [colors(2,:), 0.5], 'DisplayName', '25kg');
plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m50(i,i), f, 'Hz'))), ...
     'color', [colors(3,:), 0.5], 'DisplayName', '50kg');
for i = 2:6
    plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m1( i,i), f, 'Hz'))), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
    plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m25(i,i), f, 'Hz'))), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
    plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m50(i,i), f, 'Hz'))), 'color', [colors(3,:), 0.5], 'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
ylim([1e-2, 1e2]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(f, 180/pi*angle(squeeze(freqresp(-Khac(i,i)*G_hac_m1( i,i), f, 'Hz'))), 'color', [colors(1,:), 0.5]);
    plot(f, 180/pi*angle(squeeze(freqresp(-Khac(i,i)*G_hac_m25(i,i), f, 'Hz'))), 'color', [colors(2,:), 0.5]);
    plot(f, 180/pi*angle(squeeze(freqresp(-Khac(i,i)*G_hac_m50(i,i), f, 'Hz'))), 'color', [colors(3,:), 0.5]);
end
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([0.1, 100]);

%% Characteristic Loci for the High Authority Controller
Ldet_m1 = zeros(6, length(freqs));
Lmimo_m1 = squeeze(freqresp(-G_hac_m1*Khac, freqs, 'Hz'));
for i_f = 2:length(freqs)
    Ldet_m1(:, i_f) = eig(squeeze(Lmimo_m1(:,:,i_f)));
end
Ldet_m25 = zeros(6, length(freqs));
Lmimo_m25 = squeeze(freqresp(-G_hac_m25*Khac, freqs, 'Hz'));
for i_f = 2:length(freqs)
    Ldet_m25(:, i_f) = eig(squeeze(Lmimo_m25(:,:,i_f)));
end
Ldet_m50 = zeros(6, length(freqs));
Lmimo_m50 = squeeze(freqresp(-G_hac_m50*Khac, freqs, 'Hz'));
for i_f = 2:length(freqs)
    Ldet_m50(:, i_f) = eig(squeeze(Lmimo_m50(:,:,i_f)));
end

figure;
hold on;
plot(real(squeeze(Ldet_m1(1,:))), imag(squeeze(Ldet_m1(1,:))), ...
        '.', 'color', colors(1, :), ...
        'DisplayName', '1kg');
plot(real(squeeze(Ldet_m1(1,:))),-imag(squeeze(Ldet_m1(1,:))), ...
        '.', 'color', colors(1, :), ...
        'HandleVisibility', 'off');
plot(real(squeeze(Ldet_m25(1,:))), imag(squeeze(Ldet_m25(1,:))), ...
        '.', 'color', colors(2, :), ...
        'DisplayName', '25kg');
plot(real(squeeze(Ldet_m25(1,:))),-imag(squeeze(Ldet_m25(1,:))), ...
        '.', 'color', colors(2, :), ...
        'HandleVisibility', 'off');
plot(real(squeeze(Ldet_m50(1,:))), imag(squeeze(Ldet_m50(1,:))), ...
        '.', 'color', colors(3, :), ...
        'DisplayName', '50kg');
plot(real(squeeze(Ldet_m50(1,:))),-imag(squeeze(Ldet_m50(1,:))), ...
        '.', 'color', colors(3, :), ...
        'HandleVisibility', 'off');
for i = 2:6
    plot(real(squeeze(Ldet_m1(i,:))), imag(squeeze(Ldet_m1(i,:))), ...
            '.', 'color', colors(1, :), ...
            'HandleVisibility', 'off');
    plot(real(squeeze(Ldet_m1(i,:))), -imag(squeeze(Ldet_m1(i,:))), ...
            '.', 'color', colors(1, :), ...
            'HandleVisibility', 'off');

    plot(real(squeeze(Ldet_m25(i,:))), imag(squeeze(Ldet_m25(i,:))), ...
            '.', 'color', colors(2, :), ...
            'HandleVisibility', 'off');
    plot(real(squeeze(Ldet_m25(i,:))), -imag(squeeze(Ldet_m25(i,:))), ...
            '.', 'color', colors(2, :), ...
            'HandleVisibility', 'off');

    plot(real(squeeze(Ldet_m50(i,:))), imag(squeeze(Ldet_m50(i,:))), ...
            '.', 'color', colors(3, :), ...
            'HandleVisibility', 'off');
    plot(real(squeeze(Ldet_m50(i,:))), -imag(squeeze(Ldet_m50(i,:))), ...
            '.', 'color', colors(3, :), ...
            'HandleVisibility', 'off');
end
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
hold off;
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
xlabel('Real Part'); ylabel('Imaginary Part');
axis square
xlim([-1.8, 0.2]); ylim([-1, 1]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

% Tomography experiment
% <<ssec:nass_hac_tomography>>

% The Nano Active Stabilization System concept was validated through time-domain simulations of scientific experiments, with particular focus on tomography scanning due to its demanding performance requirements.
% Simulations were conducted at the maximum operational rotational velocity of $\Omega_z = 360\,\text{deg/s}$ to evaluate system performance under the most challenging conditions.

% Performance metrics were established based on anticipated future beamline specifications, which specify a beam size of 200nm (horizontal) × 100nm (vertical).
% The primary requirement stipulates that the point of interest must remain within these beam dimensions throughout operation.
% The simulation incorporated two principal disturbance sources: ground motion and spindle vibrations.
% Additional noise sources, including measurement noise and electrical noise from DAC and voltage amplifiers, were not included in this analysis as these parameters will be optimized during the detailed design phase.

% Figure ref:fig:nass_tomo_1kg_60rpm presents a comparative analysis of positioning errors under both open-loop and closed-loop conditions for a lightweight sample configuration (1kg).
% The results demonstrate the system's capability to maintain position within the specified beam dimensions, validating the fundamental concept of the stabilization system.


%% Simulation of tomography experiments
% Sample is not centered with the rotation axis
% This is done by offsetfing the micro-hexapod by 0.9um
P_micro_hexapod = [0.9e-6; 0; 0]; % [m]

open(mdl);
set_param(mdl, 'StopTime', '2');

initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod('AP', P_micro_hexapod);
initializeSample('type', 'cylindrical', 'm', 1);

initializeSimscapeConfiguration('gravity', false);
initializeLoggingConfiguration('log', 'all', 'Ts', 1e-3);

initializeDisturbances(...
    'Dw_x',  true, ... % Ground Motion - X direction
    'Dw_y',  true, ... % Ground Motion - Y direction
    'Dw_z',  true, ... % Ground Motion - Z direction
    'Fdy_x', false, ... % Translation Stage - X direction
    'Fdy_z', false, ... % Translation Stage - Z direction
    'Frz_x', true, ... % Spindle - X direction
    'Frz_y', true, ... % Spindle - Y direction
    'Frz_z', true);    % Spindle - Z direction

initializeReferences(...
    'Rz_type', 'rotating', ...
    'Rz_period', 1, ...
    'Dh_pos', [P_micro_hexapod; 0; 0; 0]);

% Open-Loop Simulation without Nano-Hexapod - 1kg payload
initializeSimplifiedNanoHexapod('type', 'none');
initializeController('type', 'open-loop');
sim(mdl);
exp_tomo_ol_m1 = simout;

% Closed-Loop Simulation with NASS
initializeSimplifiedNanoHexapod();
initializeController('type', 'hac-iff');
load('nass_K_iff.mat', 'Kiff');
load('nass_K_hac.mat', 'Khac');

% 1kg payload
initializeSample('type', 'cylindrical', 'm', 1);
sim(mdl);
exp_tomo_cl_m1 = simout;

% 25kg payload
initializeSample('type', 'cylindrical', 'm', 25);
sim(mdl);
exp_tomo_cl_m25 = simout;

% 50kg payload
initializeSample('type', 'cylindrical', 'm', 50);
sim(mdl);
exp_tomo_cl_m50 = simout;

%% Simulation of tomography experiment - 1kg payload - 360deg/s - XY errors
figure;
hold on;
plot(1e6*exp_tomo_ol_m1.y.x.Data, 1e6*exp_tomo_ol_m1.y.y.Data, 'DisplayName', 'OL')
plot(1e6*exp_tomo_cl_m1.y.x.Data(1e3:end), 1e6*exp_tomo_cl_m1.y.y.Data(1e3:end), 'color', colors(2,:), 'DisplayName', 'CL')
hold off;
xlabel('$D_x$ [$\mu$m]'); ylabel('$D_y$ [$\mu$m]');
axis equal
xlim([-2, 2]); ylim([-2, 2]);
xticks([-2:1:2]);
yticks([-2:1:2]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

%% Simulation of tomography experiment - 1kg payload - 360deg/s - YZ errors
figure;
tiledlayout(2, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(1e6*exp_tomo_ol_m1.y.y.Data,          1e6*exp_tomo_ol_m1.y.z.Data, 'DisplayName', 'OL')
plot(1e6*exp_tomo_cl_m1.y.y.Data(1e3:end), 1e6*exp_tomo_cl_m1.y.z.Data(1e3:end), 'color', colors(2,:), 'DisplayName', 'CL')
hold off;
xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]');
axis equal
xlim([-2, 2]); ylim([-0.4, 0.4]);
xticks([-2:1:2]);
yticks([-2:0.2:2]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile();
hold on;
plot(1e9*exp_tomo_cl_m1.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m1.y.z.Data(1e3:end), 'color', colors(2,:), 'DisplayName', 'CL')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam size')
hold off;
xlabel('$D_y$ [nm]'); ylabel('$D_z$ [nm]');
axis equal
xlim([-500, 500]); ylim([-100, 100]);
xticks([-500:100:500]);
yticks([-100:50:100]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;



% #+name: fig:nass_tomo_1kg_60rpm
% #+caption: Position error of the sample in the XY (\subref{fig:nass_tomo_1kg_60rpm_xy}) and YZ (\subref{fig:nass_tomo_1kg_60rpm_yz}) planes during a simulation of a tomography experiment at $360\,\text{deg/s}$. 1kg payload is placed on top of the nano-hexapod.
% #+attr_latex: :options [h!tbp]
% #+begin_figure
% #+attr_latex: :caption \subcaption{\label{fig:nass_tomo_1kg_60rpm_xy}XY plane}
% #+attr_latex: :options {0.48\textwidth}
% #+begin_subfigure
% #+attr_latex: :scale 0.9
% [[file:figs/nass_tomo_1kg_60rpm_xy.png]]
% #+end_subfigure
% #+attr_latex: :caption \subcaption{\label{fig:nass_tomo_1kg_60rpm_yz}YZ plane}
% #+attr_latex: :options {0.48\textwidth}
% #+begin_subfigure
% #+attr_latex: :scale 0.9
% [[file:figs/nass_tomo_1kg_60rpm_yz.png]]
% #+end_subfigure
% #+end_figure

% The robustness of the NASS to payload mass variation was evaluated through additional tomography scan simulations with 25kg and 50kg payloads, complementing the initial 1kg test case.
% As illustrated in Figure ref:fig:nass_tomography_hac_iff, system performance exhibits some degradation with increasing payload mass, aligning with predictions from the control analysis.
% While the positioning accuracy for heavier payloads is outside the specified limits, it remains within acceptable bounds for typical operating conditions.

% It should be noted that the maximum rotational velocity of 360deg/s is primarily intended for lightweight payload applications.
% For higher mass configurations, rotational velocities are foreseen to be below 36deg/s.


%% Simulation of tomography experiment - 1kg payload - 360deg/s - YZ errors
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(1e9*exp_tomo_cl_m1.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m1.y.z.Data(1e3:end), 'color', colors(1,:), 'DisplayName', '$m = 1$ kg')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam size')
hold off;
xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]');
axis equal
xlim([-200, 200]); ylim([-100, 100]);
xticks([-200:50:200]); yticks([-100:50:100]);

%% Simulation of tomography experiment - 25kg payload - 360deg/s - YZ errors
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(1e9*exp_tomo_cl_m25.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m25.y.z.Data(1e3:end), 'color', colors(2,:), 'DisplayName', '$m = 25$ kg')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam size')
hold off;
xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]');
axis equal
xlim([-200, 200]); ylim([-100, 100]);
xticks([-200:50:200]); yticks([-100:50:100]);

%% Simulation of tomography experiment - 50kg payload - 360deg/s - YZ errors
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(1e9*exp_tomo_cl_m50.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m50.y.z.Data(1e3:end), 'color', colors(3,:), 'DisplayName', '$m = 50$ kg')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam size')
hold off;
xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]');
axis equal
xlim([-200, 200]); ylim([-100, 100]);
xticks([-200:50:200]); yticks([-100:50:100]);