add all files

.gitattributes

@@ -0,0 +1 @@
+*.mat filter=lfs diff=lfs merge=lfs -text

A1-nass-uniaxial-model/uniaxial_1_micro_station_model.m

@@ -0,0 +1,91 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+%% Path for functions, data and scripts
+addpath('./mat/'); % Path for data
+
+%% Colors for the figures
+colors = colororder;
+
+%% Uniaxial Simscape model name
+mdl = 'nass_uniaxial_model';
+
+%% Frequency Vector [Hz]
+freqs = logspace(0, 3, 1000);
+
+%% Load measured FRF
+load('meas_microstation_frf.mat');
+
+%% Parameters - Mass
+mh = 15;   % Micro Hexapod [kg]
+mt = 1200; % Ty + Ry + Rz [kg]
+mg = 2500; % Granite [kg]
+
+%% Parameters - Stiffnesses
+kh = 6.11e+07; % [N/m]
+kt = 5.19e+08; % [N/m]
+kg = 9.50e+08; % [N/m]
+
+%% Parameters - damping
+ch = 2*0.05*sqrt(kh*mh); % [N/(m/s)]
+ct = 2*0.05*sqrt(kt*mt); % [N/(m/s)]
+cg = 2*0.08*sqrt(kg*mg); % [N/(m/s)]
+
+%% Save model parameters
+save('./mat/uniaxial_micro_station_parameters.mat', 'mh', 'mt', 'mg', 'ch', 'ct', 'cg', 'kh', 'kt', 'kg')
+
+%% Disable the Nano-Hexpod for now
+model_config = struct();
+model_config.nhexa = "none";
+model_config.controller = "open_loop";
+
+%% Identify the transfer function from u to taum
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/micro_station/Fg'], 1, 'openinput');  io_i = io_i + 1; % Hammer on Granite
+io(io_i) = linio([mdl, '/micro_station/Fh'], 1, 'openinput');  io_i = io_i + 1; % Hammer on Hexapod
+io(io_i) = linio([mdl, '/micro_station/xg'], 1, 'openoutput'); io_i = io_i + 1; % Absolute motion of Granite
+io(io_i) = linio([mdl, '/micro_station/xh'], 1, 'openoutput'); io_i = io_i + 1; % Absolute motion of Hexapod
+
+%% Perform the model extraction
+G_id = linearize(mdl, io, 0.0);
+G_id.InputName = {'Fg', 'Fh'};
+G_id.OutputName = {'Dg', 'Dh'};
+
+%% Comparison of the measured FRF and identified ones from the uniaxial model
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f(f>20), abs(frf_Fhz_to_Dhz(f>20)), '-', 'color', colors(1,:), 'DisplayName', '$x_{h,z}/F_{h,z}$');
+plot(f(f>20), abs(frf_Fgz_to_Dhz(f>20)), '-', 'color', colors(2,:), 'DisplayName', '$x_{h,z}/F_{g,z}$');
+plot(f(f>20), abs(frf_Fgz_to_Dgz(f>20)), '-', 'color', colors(3,:), 'DisplayName', '$x_{g,z}/F_{g,z}$');
+plot(freqs, abs(squeeze(freqresp(G_id('Dh', 'Fh'), freqs, 'Hz'))), '--', 'color', colors(1,:), 'DisplayName', '$x_{h,z}/F_{h,z}$ (model)');
+plot(freqs, abs(squeeze(freqresp(G_id('Dh', 'Fg'), freqs, 'Hz'))), '--', 'color', colors(2,:), 'DisplayName', '$x_{h,z}/F_{g,z}$ (model)');
+plot(freqs, abs(squeeze(freqresp(G_id('Dg', 'Fg'), freqs, 'Hz'))), '--', 'color', colors(3,:), 'DisplayName', '$x_{g,z}/F_{g,z}$ (model)');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+ylim([1e-10, 2e-7]);
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 2);
+
+ax2 = nexttile;
+hold on;
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_id('Dh', 'Fh'), freqs, 'Hz')))), '--', 'color', colors(1,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_id('Dh', 'Fg'), freqs, 'Hz')))), '--', 'color', colors(2,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_id('Dg', 'Fg'), freqs, 'Hz')))), '--', 'color', colors(3,:));
+plot(f(f>20), 180/pi*unwrap(angle(frf_Fhx_to_Dhx(f>20))), '-', 'color', colors(1,:));
+plot(f(f>30), 180/pi*unwrap(angle(frf_Fgx_to_Dhx(f>30))), '-', 'color', colors(2,:));
+plot(f(f>20), 180/pi*unwrap(angle(frf_Fgx_to_Dgx(f>20))), '-', 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-360, 90]);
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 500]);

A1-nass-uniaxial-model/uniaxial_2_nano_hexapod_model.m

@@ -0,0 +1,191 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+%% Path for functions, data and scripts
+addpath('./mat/'); % Path for data
+
+%% Colors for the figures
+colors = colororder;
+
+%% Uniaxial Simscape model name
+mdl = 'nass_uniaxial_model';
+
+%% Frequency Vector [Hz]
+freqs = logspace(0, 3, 1000);
+
+%% Load the micro-station parameters
+load('uniaxial_micro_station_parameters.mat')
+
+%% Nano-Hexapod Parameters
+mn = 15; % [kg]
+kn = 1e7; % [N/m]
+cn = 2*0.01*sqrt(mn*kn); % [N/(m/s)]
+
+%% Sample Mass
+ms = 10; % [kg]
+
+%% Use 1DoF Nano-Hexpod model
+model_config = struct();
+model_config.nhexa = "1dof";
+model_config.controller = "open_loop";
+
+%% Identify the transfer function from disturbances and force actuator to d
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/controller'],       1, 'openinput');  io_i = io_i + 1; % Force Actuator
+io(io_i) = linio([mdl, '/fs'],               1, 'openinput');  io_i = io_i + 1; % Force applied on the sample
+io(io_i) = linio([mdl, '/micro_station/xf'], 1, 'openinput');  io_i = io_i + 1; % Floor Motion
+io(io_i) = linio([mdl, '/micro_station/ft'], 1, 'openinput');  io_i = io_i + 1; % Stage disturbances
+io(io_i) = linio([mdl, '/d'] ,               1, 'openoutput'); io_i = io_i + 1; % Metrology
+
+%% Perform the model extraction
+G_ol = linearize(mdl, io, 0.0);
+G_ol.InputName = {'f', 'fs', 'xf', 'ft'};
+G_ol.OutputName = {'d'};
+
+%% Sensitivity to disturbances - Fs
+figure;
+plot(freqs, abs(squeeze(freqresp(G_ol('d', 'fs'), freqs, 'Hz'))));
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude $d/f_{s}$ [m/N]'); xlabel('Frequency [Hz]');
+xticks([1e0, 1e1, 1e2]);
+xlim([1, 500]);
+
+%% Sensitivity to disturbances - Ft
+figure;
+plot(freqs, abs(squeeze(freqresp(G_ol('d', 'ft'), freqs, 'Hz'))));
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude $d/f_{t}$ [m/N]'); xlabel('Frequency [Hz]');
+xticks([1e0, 1e1, 1e2]);
+xlim([1, 500]);
+
+%% Sensitivity to disturbances - xf
+figure;
+plot(freqs, abs(squeeze(freqresp(G_ol('d', 'xf'), freqs, 'Hz'))));
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude $d/x_{f}$ [m/m]'); xlabel('Frequency [Hz]');
+xticks([1e0, 1e1, 1e2]);
+xlim([1, 500]);
+ylim([1e-2, 1e2]);
+
+%% Bode Plot of the transfer function from actuator forces to measured displacement by the metrology
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_ol('d', 'f'), freqs, 'Hz'))));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude $d/f$ [m/N]'); set(gca, 'XTickLabel',[]);
+
+ax2 = nexttile;
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_ol('d', 'f'), freqs, 'Hz'))));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 0]);
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 500]);
+
+%% Use 1DoF Nano-Hexpod model
+model_config = struct();
+model_config.nhexa = "1dof";
+model_config.controller = "open_loop";
+
+%% Nano-Hexapod Mass
+mn = 15; % Nano-Hexapod mass [kg]
+
+%% Identification of all combination of stiffnesses / masses
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/controller'],       1, 'openinput');  io_i = io_i + 1; % Actuator Force
+io(io_i) = linio([mdl, '/micro_station/xf'], 1, 'openinput');  io_i = io_i + 1; % Floor Motion
+io(io_i) = linio([mdl, '/micro_station/ft'], 1, 'openinput');  io_i = io_i + 1; % Stage vibrations
+io(io_i) = linio([mdl, '/fs'],               1, 'openinput');  io_i = io_i + 1; % Direct sample forces
+io(io_i) = linio([mdl, '/dL'],               1, 'openoutput'); io_i = io_i + 1; % Relative Motion Sensor
+io(io_i) = linio([mdl, '/fm'],               1, 'openoutput'); io_i = io_i + 1; % Force Sensor
+io(io_i) = linio([mdl, '/vn'] ,              1, 'openoutput'); io_i = io_i + 1; % Geophone
+io(io_i) = linio([mdl, '/d'] ,               1, 'openoutput'); io_i = io_i + 1; % Metrology Output
+
+%% Light Sample
+ms = 1; % Sample Mass [kg]
+
+% Voice Coil (i.e. soft) Nano-Hexapod
+kn = 1e4; % Nano-Hexapod Stiffness [N/m]
+cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+G_vc_light = linearize(mdl, io, 0.0);
+G_vc_light.InputName = {'f', 'xf', 'ft', 'fs'};
+G_vc_light.OutputName = {'dL', 'fm', 'vn', 'd'};
+
+% APA (i.e. relatively stiff) Nano-Hexapod
+kn = 1e6; % Nano-Hexapod Stiffness [N/m]
+cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+G_md_light = linearize(mdl, io, 0.0);
+G_md_light.InputName = {'f', 'xf', 'ft', 'fs'};
+G_md_light.OutputName = {'dL', 'fm', 'vn', 'd'};
+
+% Piezoelectric (i.e. stiff) Nano-Hexapod
+kn = 1e8; % Nano-Hexapod Stiffness [N/m]
+cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+G_pz_light = linearize(mdl, io, 0.0);
+G_pz_light.InputName = {'f', 'xf', 'ft', 'fs'};
+G_pz_light.OutputName = {'dL', 'fm', 'vn', 'd'};
+
+%% Mid Sample
+ms = 25; % Sample Mass [kg]
+
+% Voice Coil (i.e. soft) Nano-Hexapod
+kn = 1e4; % Nano-Hexapod Stiffness [N/m]
+cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+G_vc_mid = linearize(mdl, io, 0.0);
+G_vc_mid.InputName = {'f', 'xf', 'ft', 'fs'};
+G_vc_mid.OutputName = {'dL', 'fm', 'vn', 'd'};
+
+% APA (i.e. relatively stiff) Nano-Hexapod
+kn = 1e6; % Nano-Hexapod Stiffness [N/m]
+cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+G_md_mid = linearize(mdl, io, 0.0);
+G_md_mid.InputName = {'f', 'xf', 'ft', 'fs'};
+G_md_mid.OutputName = {'dL', 'fm', 'vn', 'd'};
+
+% Piezoelectric (i.e. stiff) Nano-Hexapod
+kn = 1e8; % Nano-Hexapod Stiffness [N/m]
+cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+G_pz_mid = linearize(mdl, io, 0.0);
+G_pz_mid.InputName = {'f', 'xf', 'ft', 'fs'};
+G_pz_mid.OutputName = {'dL', 'fm', 'vn', 'd'};
+
+%% Heavy Sample
+ms = 50; % Sample Mass [kg]
+
+% Voice Coil (i.e. soft) Nano-Hexapod
+kn = 1e4; % Nano-Hexapod Stiffness [N/m]
+cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+G_vc_heavy = linearize(mdl, io, 0.0);
+G_vc_heavy.InputName = {'f', 'xf', 'ft', 'fs'};
+G_vc_heavy.OutputName = {'dL', 'fm', 'vn', 'd'};
+
+% APA (i.e. relatively stiff) Nano-Hexapod
+kn = 1e6; % Nano-Hexapod Stiffness [N/m]
+cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+G_md_heavy = linearize(mdl, io, 0.0);
+G_md_heavy.InputName = {'f', 'xf', 'ft', 'fs'};
+G_md_heavy.OutputName = {'dL', 'fm', 'vn', 'd'};
+
+% Piezoelectric (i.e. stiff) Nano-Hexapod
+kn = 1e8; % Nano-Hexapod Stiffness [N/m]
+cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+G_pz_heavy = linearize(mdl, io, 0.0);
+G_pz_heavy.InputName = {'f', 'xf', 'ft', 'fs'};
+G_pz_heavy.OutputName = {'dL', 'fm', 'vn', 'd'};
+
+%% Save All Identified Plants
+save('./mat/uniaxial_plants.mat', 'G_vc_light', 'G_md_light', 'G_pz_light', ...
+                                  'G_vc_mid',   'G_md_mid',   'G_pz_mid', ...
+                                  'G_vc_heavy', 'G_md_heavy', 'G_pz_heavy');

A1-nass-uniaxial-model/uniaxial_3_disturbances.m

@@ -0,0 +1,115 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+%% Path for functions, data and scripts
+addpath('./mat/'); % Path for data
+
+%% Colors for the figures
+colors = colororder;
+
+%% Uniaxial Simscape model name
+mdl = 'nass_uniaxial_model';
+
+%% Frequency Vector [Hz]
+freqs = logspace(0, 3, 1000);
+
+%% Load the micro-station parameters
+load('uniaxial_micro_station_parameters.mat');
+
+%% Compute Floor Motion Spectral Density
+% Load floor motion data
+% t: time in [s]
+% V: measured voltage genrated by the geophone and amplified by a 60dB gain voltage amplifier [V]
+load('ground_motion_measurement.mat', 't', 'V');
+
+% Geophone Transfer Function
+Tg = 88; % Sensitivity [V/(m/s)]
+w0 = 2*2*pi; % Cut-off frequency [rad/s]
+xi = 0.7; % Damping ratio
+
+G_geo = Tg*s*s^2/(s^2 + 2*xi*w0*s + w0^2); % Geophone's transfer function [V/m]
+
+% Voltage amplifier transfer function
+g0 = 10^(60/20); % [abs]
+
+% Compute measured voltage PSD
+Ts = (t(2)-t(1)); % Sampling Time [s]
+Nfft = floor(2/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+
+[psd_V, f] = pwelch(V, win, Noverlap, Nfft, 1/Ts); % [V^2/Hz]
+
+% Ground Motion ASD
+psd_xf = psd_V./abs(squeeze(freqresp(G_geo*g0, f, 'Hz'))).^2; % [m^2/Hz]
+
+%% Amplitude Spectral Density of the measured Floor motion on ID31
+figure;
+plot(f, sqrt(psd_xf), 'DisplayName', '$\Gamma_{x_{f}}$');
+set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Ampl. Spectral Density $\left[\frac{m}{\sqrt{Hz}}\right]$')
+legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+xlim([1, 500]);
+xticks([1e0, 1e1, 1e2]);
+
+%% Estimation of the Spectral density of the stage vibrations
+
+% Measured velocity of granite and hexapod during spindle rotation
+% t: time in [s]
+% vg: measured granite velocity [m/s]
+% vg: measured micro-hexapod's top platform velocity [m/s]
+load('meas_spindle_on.mat', 't', 'vg', 'vh');
+spindle_off = load('meas_spindle_off.mat', 't', 'vg', 'vh'); % No Rotation
+
+% Compute Power Spectral Density of the relative velocity between granite and hexapod during spindle rotation
+Fs = 1/(t(2)-t(1)); % Sampling Frequency [Hz]
+win = hanning(ceil(2*Fs)); % Hanning window
+
+[psd_vft, f] = pwelch(vh-vg, win, [], [], Fs); % [(m/s)^2/Hz]
+[psd_off, ~] = pwelch(spindle_off.vh-spindle_off.vg, win, [], [], Fs); % [(m/s)^2/Hz]
+
+% Disable the Nano-Hexpod for now
+model_config = struct();
+model_config.nhexa = "none";
+model_config.controller = "open_loop";
+
+% Identify the transfer function from u to taum
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/micro_station/ft'], 1, 'openinput');  io_i = io_i + 1; % Stage Disturbance Force
+io(io_i) = linio([mdl, '/micro_station/xg'], 1, 'openoutput'); io_i = io_i + 1; % Absolute motion of Granite
+io(io_i) = linio([mdl, '/micro_station/xh'], 1, 'openoutput'); io_i = io_i + 1; % Absolute motion of Hexapod
+
+% Perform the model extraction
+G = linearize(mdl, io, 0.0);
+G.InputName = {'ft'};
+G.OutputName = {'Dg', 'Dh'};
+
+% Power Spectral Density of the equivalent force ft [N/Hz^2]
+psd_ft = (psd_vft./(2*pi*f).^2)./abs(squeeze(freqresp(G('Dh', 'ft') - G('Dg', 'ft'), f, 'Hz'))).^2;
+
+%% Amplitude Spectral Density of the relative motion measured between the granite and the micro-hexapod's top platform during Spindle rotating
+figure;
+hold on;
+plot(f, sqrt(psd_vft)./(2*pi*f), 'DisplayName', '6rpm');
+plot(f, sqrt(psd_off)./(2*pi*f), 'DisplayName', '0rpm');
+hold off;
+set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Ampl. Spectral Density $\left[\frac{m}{\sqrt{Hz}}\right]$')
+legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+xlim([1, 500]); ylim([1e-12, 1e-7])
+
+%% Estimated disturbance force ft from measurement and uniaxial model
+figure;
+hold on;
+plot(f, sqrt(psd_ft), 'DisplayName', '$\Gamma_{f_{t}}$');
+hold off;
+set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Ampl. Spectral Density $\left[\frac{N}{\sqrt{Hz}}\right]$')
+xlim([1, 500]);
+legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+
+%% Save PSD of disturbances
+save('./mat/uniaxial_disturbance_psd.mat', 'f', 'psd_ft', 'psd_xf');

A1-nass-uniaxial-model/uniaxial_4_dynamic_noise_budget.m

@@ -0,0 +1,110 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+%% Path for functions, data and scripts
+addpath('./mat/'); % Path for data
+
+%% Colors for the figures
+colors = colororder;
+
+%% Frequency Vector [Hz]
+freqs = logspace(0, 3, 1000);
+
+%% Load the PSD of disturbances
+load('uniaxial_disturbance_psd.mat', 'f', 'psd_ft', 'psd_xf');
+
+%% Load Plants Dynamics
+load('uniaxial_plants.mat', 'G_vc_light', 'G_md_light', 'G_pz_light', ...
+                            'G_vc_mid',   'G_md_mid',   'G_pz_mid', ...
+                            'G_vc_heavy', 'G_md_heavy', 'G_pz_heavy');
+
+%% Sensitivity to disturbances for three different nano-hexpod stiffnesses
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_vc_light('d', 'fs'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G_md_light('d', 'fs'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G_pz_light('d', 'fs'), freqs, 'Hz'))));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude $d/f_{s}$ [m/N]'); xlabel('Frequency [Hz]');
+xticks([1e0, 1e1, 1e2]);
+xlim([1, 500]);
+
+%% Sensitivity to disturbances for three different nano-hexpod stiffnesses
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_vc_light('d', 'ft'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G_md_light('d', 'ft'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G_pz_light('d', 'ft'), freqs, 'Hz'))));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude $d/f_{t}$ [m/N]'); xlabel('Frequency [Hz]');
+xticks([1e0, 1e1, 1e2]);
+xlim([1, 500]);
+
+%% Sensitivity to disturbances for three different nano-hexpod stiffnesses
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_vc_light('d', 'xf'), freqs, 'Hz'))), 'DisplayName', '$k_n = 0.01\,N/\mu m$');
+plot(freqs, abs(squeeze(freqresp(G_md_light('d', 'xf'), freqs, 'Hz'))), 'DisplayName', '$k_n = 1   \,N/\mu m$');
+plot(freqs, abs(squeeze(freqresp(G_pz_light('d', 'xf'), freqs, 'Hz'))), 'DisplayName', '$k_n = 100 \,N/\mu m$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude $d/x_{f}$ [m/m]'); xlabel('Frequency [Hz]');
+xticks([1e0, 1e1, 1e2]);
+leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15
+xlim([1, 500]);
+
+%% Cumulative Amplitude Spectrum of the relative motion d, due to both the floor motion and the stage vibrations
+figure;
+hold on;
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_vc_light('d', 'ft'), f, 'Hz'))).^2)))), '-', 'color', colors(1,:), 'DisplayName', '$f_t$');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_md_light('d', 'ft'), f, 'Hz'))).^2)))), '-', 'color', colors(2,:), 'DisplayName', '$f_t$');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_pz_light('d', 'ft'), f, 'Hz'))).^2)))), '-', 'color', colors(3,:), 'DisplayName', '$f_t$');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_xf.*abs(squeeze(freqresp(G_vc_light('d', 'xf'), f, 'Hz'))).^2)))), '--', 'color', colors(1,:), 'DisplayName', '$x_f$ ($k_n = 0.01\,N/\mu m$)');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_xf.*abs(squeeze(freqresp(G_md_light('d', 'xf'), f, 'Hz'))).^2)))), '--', 'color', colors(2,:), 'DisplayName', '$x_f$ ($k_n = 1   \,N/\mu m$)');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_xf.*abs(squeeze(freqresp(G_pz_light('d', 'xf'), f, 'Hz'))).^2)))), '--', 'color', colors(3,:), 'DisplayName', '$x_f$ ($k_n = 100 \,N/\mu m$)');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('CAS [m]'); xlabel('Frequency [Hz]');
+leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
+leg.ItemTokenSize(1) = 15
+
+xlim([1, 500]);
+ylim([1e-12, 3e-6])
+
+%% Cumulative Amplitude Spectrum of the relative motion d due to all disturbances, for two sample masses
+figure;
+hold on;
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_vc_light('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_vc_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', colors(1,:), 'DisplayName', '$m_s = 1\,kg$, $k_n = 0.01\,N/\mu m$');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_md_light('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_md_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', colors(2,:), 'DisplayName', '$m_s = 1\,kg$, $k_n = 1\,N/\mu m$');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_pz_light('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_pz_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', colors(3,:), 'DisplayName', '$m_s = 1\,kg$, $k_n = 100\,N/\mu m$');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_vc_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_vc_heavy('d', 'xf'), f, 'Hz'))).^2)))), '--', ...
+     'color', colors(1,:), 'DisplayName', '$m_s = 50\,kg$, $k_n = 0.01\,N/\mu m$');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_md_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_md_heavy('d', 'xf'), f, 'Hz'))).^2)))), '--', ...
+     'color', colors(2,:), 'DisplayName', '$m_s = 50\,kg$, $k_n = 1\,N/\mu m$');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_pz_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_pz_heavy('d', 'xf'), f, 'Hz'))).^2)))), '--', ...
+     'color', colors(3,:), 'DisplayName', '$m_s = 50\,kg$, $k_n = 100\,N/\mu m$');
+plot([1, 1e3], [20e-9, 20e-9], 'k--', 'HandleVisibility', 'off');
+text(4, 1e-8, '20 nm RMS', 'horizontalalignment', 'center');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('CAS [m]'); xlabel('Frequency [Hz]');
+leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15
+
+xlim([1, 500]);
+ylim([1e-12, 3e-6])

A1-nass-uniaxial-model/uniaxial_5_active_damping.m

@@ -0,0 +1,630 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+%% Path for functions, data and scripts
+addpath('./mat/'); % Path for data
+
+%% Colors for the figures
+colors = colororder;
+
+%% Frequency Vector [Hz]
+freqs = logspace(0, 3, 1000);
+
+%% Load the PSD of disturbances
+load('uniaxial_disturbance_psd.mat', 'f', 'psd_ft', 'psd_xf');
+
+%% Load Plants Dynamics
+load('uniaxial_plants.mat', 'G_vc_light', 'G_md_light', 'G_pz_light', ...
+                            'G_vc_mid',   'G_md_mid',   'G_pz_mid', ...
+                            'G_vc_heavy', 'G_md_heavy', 'G_pz_heavy');
+
+%% Damped plants for three considered payload masses - Comparison of active damping techniques
+% Integral Force Feedback
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_vc_light('fm', 'f'), freqs, 'Hz'))), '-',  'color', colors(1,:), 'DisplayName', '$m_s = 1\,kg$');
+plot(freqs, abs(squeeze(freqresp(G_vc_mid(  'fm', 'f'), freqs, 'Hz'))), '-.', 'color', colors(1,:), 'DisplayName', '$m_s = 25\,kg$');
+plot(freqs, abs(squeeze(freqresp(G_vc_heavy('fm', 'f'), freqs, 'Hz'))), '--', 'color', colors(1,:), 'DisplayName', '$m_s = 50\,kg$');
+plot(freqs, abs(squeeze(freqresp(G_md_light('fm', 'f'), freqs, 'Hz'))), '-',  'color', colors(2,:), 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_md_mid(  'fm', 'f'), freqs, 'Hz'))), '-.', 'color', colors(2,:), 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_md_heavy('fm', 'f'), freqs, 'Hz'))), '--', 'color', colors(2,:), 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_pz_light('fm', 'f'), freqs, 'Hz'))), '-',  'color', colors(3,:), 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_pz_mid(  'fm', 'f'), freqs, 'Hz'))), '-.', 'color', colors(3,:), 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_pz_heavy('fm', 'f'), freqs, 'Hz'))), '--', 'color', colors(3,:), 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
+ldg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+ldg.ItemTokenSize = [20, 1];
+
+ax1b = nexttile();
+hold on;
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_light('fm', 'f'), freqs, 'Hz')))), '-',  'color', colors(1,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_mid(  'fm', 'f'), freqs, 'Hz')))), '-.', 'color', colors(1,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_heavy('fm', 'f'), freqs, 'Hz')))), '--', 'color', colors(1,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_light('fm', 'f'), freqs, 'Hz')))), '-',  'color', colors(2,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_mid(  'fm', 'f'), freqs, 'Hz')))), '-.', 'color', colors(2,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_heavy('fm', 'f'), freqs, 'Hz')))), '--', 'color', colors(2,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_light('fm', 'f'), freqs, 'Hz')))), '-',  'color', colors(3,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_mid(  'fm', 'f'), freqs, 'Hz')))), '-.', 'color', colors(3,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_heavy('fm', 'f'), freqs, 'Hz')))), '--', 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+xticks([1e0, 1e1, 1e2]);
+yticks(-360:90:360);
+ylim([-200, 20]);
+
+linkaxes([ax1,ax1b],'x');
+xlim([1, 1000]);
+
+% Relative Motion Control
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax2 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_vc_light('dL', 'f'), freqs, 'Hz'))), '-',  'color', colors(1,:));
+plot(freqs, abs(squeeze(freqresp(G_vc_mid(  'dL', 'f'), freqs, 'Hz'))), '-.', 'color', colors(1,:));
+plot(freqs, abs(squeeze(freqresp(G_vc_heavy('dL', 'f'), freqs, 'Hz'))), '--', 'color', colors(1,:));
+plot(freqs, abs(squeeze(freqresp(G_md_light('dL', 'f'), freqs, 'Hz'))), '-',  'color', colors(2,:));
+plot(freqs, abs(squeeze(freqresp(G_md_mid(  'dL', 'f'), freqs, 'Hz'))), '-.', 'color', colors(2,:));
+plot(freqs, abs(squeeze(freqresp(G_md_heavy('dL', 'f'), freqs, 'Hz'))), '--', 'color', colors(2,:));
+plot(freqs, abs(squeeze(freqresp(G_pz_light('dL', 'f'), freqs, 'Hz'))), '-',  'color', colors(3,:));
+plot(freqs, abs(squeeze(freqresp(G_pz_mid(  'dL', 'f'), freqs, 'Hz'))), '-.', 'color', colors(3,:));
+plot(freqs, abs(squeeze(freqresp(G_pz_heavy('dL', 'f'), freqs, 'Hz'))), '--', 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+ax2b = nexttile();
+hold on;
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_light('dL', 'f'), freqs, 'Hz')))), '-',  'color', colors(1,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_mid(  'dL', 'f'), freqs, 'Hz')))), '-.', 'color', colors(1,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_heavy('dL', 'f'), freqs, 'Hz')))), '--', 'color', colors(1,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_light('dL', 'f'), freqs, 'Hz')))), '-',  'color', colors(2,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_mid(  'dL', 'f'), freqs, 'Hz')))), '-.', 'color', colors(2,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_heavy('dL', 'f'), freqs, 'Hz')))), '--', 'color', colors(2,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_light('dL', 'f'), freqs, 'Hz')))), '-',  'color', colors(3,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_mid(  'dL', 'f'), freqs, 'Hz')))), '-.', 'color', colors(3,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_heavy('dL', 'f'), freqs, 'Hz')))), '--', 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+xticks([1e0, 1e1, 1e2]);
+yticks(-360:90:360);
+ylim([-200, 20]);
+
+linkaxes([ax2,ax2b],'x');
+xlim([1, 1000]);
+
+% Direct Velocity Feedback
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax3 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_vc_light('vn', 'f'), freqs, 'Hz'))), '-',  'color', colors(1,:), 'DisplayName', '$k_n = 0.01\,N/\mu m$');
+plot(freqs, abs(squeeze(freqresp(G_vc_mid(  'vn', 'f'), freqs, 'Hz'))), '-.', 'color', colors(1,:), 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_vc_heavy('vn', 'f'), freqs, 'Hz'))), '--', 'color', colors(1,:), 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_md_light('vn', 'f'), freqs, 'Hz'))), '-',  'color', colors(2,:), 'DisplayName', '$k_n = 1\,N/\mu m$');
+plot(freqs, abs(squeeze(freqresp(G_md_mid(  'vn', 'f'), freqs, 'Hz'))), '-.', 'color', colors(2,:), 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_md_heavy('vn', 'f'), freqs, 'Hz'))), '--', 'color', colors(2,:), 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_pz_light('vn', 'f'), freqs, 'Hz'))), '-',  'color', colors(3,:), 'DisplayName', '$k_n = 100\,N/\mu m$');
+plot(freqs, abs(squeeze(freqresp(G_pz_mid(  'vn', 'f'), freqs, 'Hz'))), '-.', 'color', colors(3,:), 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_pz_heavy('vn', 'f'), freqs, 'Hz'))), '--', 'color', colors(3,:), 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/s/N]'); set(gca, 'XTickLabel',[]);
+ldg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+ldg.ItemTokenSize = [20, 1];
+
+ax3b = nexttile();
+hold on;
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_light('vn', 'f'), freqs, 'Hz')))), '-',  'color', colors(1,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_mid(  'vn', 'f'), freqs, 'Hz')))), '-.', 'color', colors(1,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_heavy('vn', 'f'), freqs, 'Hz')))), '--', 'color', colors(1,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_light('vn', 'f'), freqs, 'Hz')))), '-',  'color', colors(2,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_mid(  'vn', 'f'), freqs, 'Hz')))), '-.', 'color', colors(2,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_heavy('vn', 'f'), freqs, 'Hz')))), '--', 'color', colors(2,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_light('vn', 'f'), freqs, 'Hz')))), '-',  'color', colors(3,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_mid(  'vn', 'f'), freqs, 'Hz')))), '-.', 'color', colors(3,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_heavy('vn', 'f'), freqs, 'Hz')))), '--', 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+xticks([1e0, 1e1, 1e2]);
+yticks(-360:90:360);
+ylim([-110, 110]);
+
+linkaxes([ax3,ax3b],'x');
+xlim([1, 1000]);
+
+%% Design of Active Damping controllers to have reasonable damping
+% IFF
+K_iff_vc = 20/(s + 2*pi*0.01);
+K_iff_vc.InputName = {'fm'};
+K_iff_vc.OutputName = {'f'};
+K_iff_md = 200/(s + 2*pi*0.01);
+K_iff_md.InputName = {'fm'};
+K_iff_md.OutputName = {'f'};
+K_iff_pz = 4000/(s + 2*pi*0.01);
+K_iff_pz.InputName = {'fm'};
+K_iff_pz.OutputName = {'f'};
+
+% RDC
+K_rdc_vc = -1e3*s;
+K_rdc_vc.InputName = {'dL'};
+K_rdc_vc.OutputName = {'f'};
+K_rdc_md = -1e4*s;
+K_rdc_md.InputName = {'dL'};
+K_rdc_md.OutputName = {'f'};
+K_rdc_pz = -1e5*s;
+K_rdc_pz.InputName = {'dL'};
+K_rdc_pz.OutputName = {'f'};
+
+% DVF
+K_dvf_vc = -tf(1e3);
+K_dvf_vc.InputName = {'vn'};
+K_dvf_vc.OutputName = {'f'};
+K_dvf_md = -tf(8e3);
+K_dvf_md.InputName = {'vn'};
+K_dvf_md.OutputName = {'f'};
+K_dvf_pz = -tf(2e5);
+K_dvf_pz.InputName = {'vn'};
+K_dvf_pz.OutputName = {'f'};
+
+%% Save Active Damping Controller
+save('./mat/uniaxial_active_damping_controllers.mat', 'K_iff_vc', 'K_iff_md', 'K_iff_pz', ...
+                                                      'K_rdc_vc', 'K_rdc_md', 'K_rdc_pz', ...
+                                                      'K_dvf_vc', 'K_dvf_md', 'K_dvf_pz');
+
+%% Compute Damped Plants
+% IFF
+G_iff_vc_light = feedback(G_vc_light, K_iff_vc, 'name', +1);
+G_iff_vc_mid   = feedback(G_vc_mid  , K_iff_vc, 'name', +1);
+G_iff_vc_heavy = feedback(G_vc_heavy, K_iff_vc, 'name', +1);
+G_iff_md_light = feedback(G_md_light, K_iff_md, 'name', +1);
+G_iff_md_mid   = feedback(G_md_mid  , K_iff_md, 'name', +1);
+G_iff_md_heavy = feedback(G_md_heavy, K_iff_md, 'name', +1);
+G_iff_pz_light = feedback(G_pz_light, K_iff_pz, 'name', +1);
+G_iff_pz_mid   = feedback(G_pz_mid  , K_iff_pz, 'name', +1);
+G_iff_pz_heavy = feedback(G_pz_heavy, K_iff_pz, 'name', +1);
+
+% RDC
+G_rdc_vc_light = feedback(G_vc_light, K_rdc_vc, 'name', +1);
+G_rdc_vc_mid   = feedback(G_vc_mid  , K_rdc_vc, 'name', +1);
+G_rdc_vc_heavy = feedback(G_vc_heavy, K_rdc_vc, 'name', +1);
+G_rdc_md_light = feedback(G_md_light, K_rdc_md, 'name', +1);
+G_rdc_md_mid   = feedback(G_md_mid  , K_rdc_md, 'name', +1);
+G_rdc_md_heavy = feedback(G_md_heavy, K_rdc_md, 'name', +1);
+G_rdc_pz_light = feedback(G_pz_light, K_rdc_pz, 'name', +1);
+G_rdc_pz_mid   = feedback(G_pz_mid  , K_rdc_pz, 'name', +1);
+G_rdc_pz_heavy = feedback(G_pz_heavy, K_rdc_pz, 'name', +1);
+
+% DVF
+G_dvf_vc_light = feedback(G_vc_light, K_dvf_vc, 'name', +1);
+G_dvf_vc_mid   = feedback(G_vc_mid  , K_dvf_vc, 'name', +1);
+G_dvf_vc_heavy = feedback(G_vc_heavy, K_dvf_vc, 'name', +1);
+G_dvf_md_light = feedback(G_md_light, K_dvf_md, 'name', +1);
+G_dvf_md_mid   = feedback(G_md_mid  , K_dvf_md, 'name', +1);
+G_dvf_md_heavy = feedback(G_md_heavy, K_dvf_md, 'name', +1);
+G_dvf_pz_light = feedback(G_pz_light, K_dvf_pz, 'name', +1);
+G_dvf_pz_mid   = feedback(G_pz_mid  , K_dvf_pz, 'name', +1);
+G_dvf_pz_heavy = feedback(G_pz_heavy, K_dvf_pz, 'name', +1);
+
+%% Verify Stability
+% IFF
+isstable(G_iff_vc_light) && isstable(G_iff_vc_mid) && isstable(G_iff_vc_heavy) && ...
+isstable(G_iff_md_light) && isstable(G_iff_md_mid) && isstable(G_iff_md_heavy) && ...
+isstable(G_iff_pz_light) && isstable(G_iff_pz_mid) && isstable(G_iff_pz_heavy)
+
+% RDC
+isstable(G_rdc_vc_light) && isstable(G_rdc_vc_mid) && isstable(G_rdc_vc_heavy) && ...
+isstable(G_rdc_md_light) && isstable(G_rdc_md_mid) && isstable(G_rdc_md_heavy) && ...
+isstable(G_rdc_pz_light) && isstable(G_rdc_pz_mid) && isstable(G_rdc_pz_heavy)
+
+% DVF
+isstable(G_dvf_vc_light) && isstable(G_dvf_vc_mid) && isstable(G_dvf_vc_heavy) && ...
+isstable(G_dvf_md_light) && isstable(G_dvf_md_mid) && isstable(G_dvf_md_heavy) && ...
+isstable(G_dvf_pz_light) && isstable(G_dvf_pz_mid) && isstable(G_dvf_pz_heavy)
+
+%% Save Damped Plants
+save('./mat/uniaxial_damped_plants.mat', 'G_iff_vc_light', 'G_iff_md_light', 'G_iff_pz_light', ...
+                                         'G_rdc_vc_light', 'G_rdc_md_light', 'G_rdc_pz_light', ...
+                                         'G_dvf_vc_light', 'G_dvf_md_light', 'G_dvf_pz_light', ...
+                                         'G_iff_vc_mid',   'G_iff_md_mid',   'G_iff_pz_mid', ...
+                                         'G_rdc_vc_mid',   'G_rdc_md_mid',   'G_rdc_pz_mid', ...
+                                         'G_dvf_vc_mid',   'G_dvf_md_mid',   'G_dvf_pz_mid', ...
+                                         'G_iff_vc_heavy', 'G_iff_md_heavy', 'G_iff_pz_heavy', ...
+                                         'G_rdc_vc_heavy', 'G_rdc_md_heavy', 'G_rdc_pz_heavy', ...
+                                         'G_dvf_vc_heavy', 'G_dvf_md_heavy', 'G_dvf_pz_heavy');
+
+%% Active Damping Robustness to change of sample's mass - Root Locus for all three damping techniques with 3 different sample's masses
+% Soft Nano-Hexapod
+figure;
+hold on;
+% IFF
+plot(real(pole(G_vc_light('fm', 'f'))),  imag(pole(G_vc_light('fm', 'f'))), 'x', 'color', colors(1,:), ...
+     'HandleVisibility', 'off');
+plot(real(zero(G_vc_light('fm', 'f'))),  imag(zero(G_vc_light('fm', 'f'))), 'o', 'color', colors(1,:), ...
+     'DisplayName', 'IFF');
+for g = logspace(0,2,400)
+    clpoles = pole(feedback(G_vc_light('fm', 'f'), g/s, +1));
+    plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
+         'HandleVisibility', 'off');
+end
+
+% RDC
+plot(real(pole(G_vc_light('dL', 'f'))),  imag(pole(G_vc_light('dL', 'f'))), 'x', 'color', colors(2,:), ...
+     'HandleVisibility', 'off');
+plot(real(zero(G_vc_light('dL', 'f'))),  imag(zero(G_vc_light('dL', 'f'))), 'o', 'color', colors(2,:), ...
+     'DisplayName', 'RDC');
+for g = logspace(1,3,400)
+    clpoles = pole(feedback(G_vc_light('dL', 'f'), -g*s, +1));
+    plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ...
+         'HandleVisibility', 'off');
+end
+
+% DVF
+plot(real(pole(G_vc_light('vn', 'f'))),  imag(pole(G_vc_light('vn', 'f'))), 'x', 'color', colors(3,:), ...
+     'HandleVisibility', 'off');
+plot(real(zero(G_vc_light('vn', 'f'))),  imag(zero(G_vc_light('vn', 'f'))), 'o', 'color', colors(3,:), ...
+     'DisplayName', 'DVF');
+for g = logspace(1,3,400)
+    clpoles = pole(feedback(G_vc_light('vn', 'f'), -g, +1));
+    plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ...
+         'HandleVisibility', 'off');
+end
+hold off;
+axis square;
+xlabel('Real Part'); ylabel('Imaginary Part');
+ldg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+ldg.ItemTokenSize = [10, 1];
+xlim([-30, 0]); ylim([0, 30]);
+ytickangle(90)
+
+% Medium-Stiff Nano-Hexapod
+figure;
+hold on;
+% IFF
+plot(real(pole(G_md_light('fm', 'f'))),  imag(pole(G_md_light('fm', 'f'))), 'x', 'color', colors(1,:), ...
+     'HandleVisibility', 'off');
+plot(real(zero(G_md_light('fm', 'f'))),  imag(zero(G_md_light('fm', 'f'))), 'o', 'color', colors(1,:), ...
+     'HandleVisibility', 'off');
+for g = logspace(0,3,400)
+    clpoles = pole(feedback(G_md_light('fm', 'f'), g/s, +1));
+    plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
+         'HandleVisibility', 'off');
+end
+
+% RDC
+plot(real(pole(G_md_light('dL', 'f'))),  imag(pole(G_md_light('dL', 'f'))), 'x', 'color', colors(2,:), ...
+     'HandleVisibility', 'off');
+plot(real(zero(G_md_light('dL', 'f'))),  imag(zero(G_md_light('dL', 'f'))), 'o', 'color', colors(2,:), ...
+     'HandleVisibility', 'off');
+for g = logspace(2,4,400)
+    clpoles = pole(feedback(G_md_light('dL', 'f'), -g*s, +1));
+    plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ...
+         'HandleVisibility', 'off');
+end
+
+% DVF
+plot(real(pole(G_md_light('vn', 'f'))),  imag(pole(G_md_light('vn', 'f'))), 'x', 'color', colors(3,:), ...
+     'HandleVisibility', 'off');
+plot(real(zero(G_md_light('vn', 'f'))),  imag(zero(G_md_light('vn', 'f'))), 'o', 'color', colors(3,:), ...
+     'HandleVisibility', 'off');
+for g = logspace(2,4,400)
+    clpoles = pole(feedback(G_md_light('vn', 'f'), -g, +1));
+    plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ...
+         'HandleVisibility', 'off');
+end
+hold off;
+axis square;
+xlabel('Real Part'); ylabel('Imaginary Part');
+xlim([-300, 0]); ylim([0, 300]);
+ytickangle(90)
+
+% Stiff Nano Hexapod
+figure;
+hold on;
+% IFF
+plot(real(pole(G_pz_light('fm', 'f'))),  imag(pole(G_pz_light('fm', 'f'))), 'x', 'color', colors(1,:), ...
+     'HandleVisibility', 'off');
+plot(real(zero(G_pz_light('fm', 'f'))),  imag(zero(G_pz_light('fm', 'f'))), 'o', 'color', colors(1,:), ...
+     'HandleVisibility', 'off');
+for g = logspace(2,5,400)
+    clpoles = pole(feedback(G_pz_light('fm', 'f'), g/s, +1));
+    plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
+         'HandleVisibility', 'off');
+end
+
+% RDC
+plot(real(pole(G_pz_light('dL', 'f'))),  imag(pole(G_pz_light('dL', 'f'))), 'x', 'color', colors(2,:), ...
+     'HandleVisibility', 'off');
+plot(real(zero(G_pz_light('dL', 'f'))),  imag(zero(G_pz_light('dL', 'f'))), 'o', 'color', colors(2,:), ...
+     'HandleVisibility', 'off');
+for g = logspace(3,6,400)
+    clpoles = pole(feedback(G_pz_light('dL', 'f'), -g*s, +1));
+    plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ...
+         'HandleVisibility', 'off');
+end
+
+% DVF
+plot(real(pole(G_pz_light('vn', 'f'))),  imag(pole(G_pz_light('vn', 'f'))), 'x', 'color', colors(3,:), ...
+     'HandleVisibility', 'off');
+plot(real(zero(G_pz_light('vn', 'f'))),  imag(zero(G_pz_light('vn', 'f'))), 'o', 'color', colors(3,:), ...
+     'HandleVisibility', 'off');
+for g = logspace(3,6,400)
+    clpoles = pole(feedback(G_pz_light('vn', 'f'), -g, +1));
+    plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ...
+         'HandleVisibility', 'off');
+end
+hold off;
+axis square;
+xlabel('Real Part'); ylabel('Imaginary Part');
+xlim([-4000, 0]); ylim([0, 4000]);
+ytickangle(90)
+
+%% Root Locus for the three damping techniques
+
+figure;
+
+hold on;
+% IFF
+plot(real(pole(G_md_mid('fm', 'f'))),  imag(pole(G_md_mid('fm', 'f'))), 'x', 'color', colors(1,:), ...
+     'DisplayName', 'IFF');
+plot(real(zero(G_md_mid('fm', 'f'))),  imag(zero(G_md_mid('fm', 'f'))), 'o', 'color', colors(1,:), ...
+     'HandleVisibility', 'off');
+for g = logspace(1,4,500)
+    clpoles = pole(feedback(G_md_mid('fm', 'f'), g/s, +1));
+    plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
+         'HandleVisibility', 'off');
+end
+
+% RDC
+plot(real(pole(G_md_mid('dL', 'f'))),  imag(pole(G_md_mid('dL', 'f'))), 'x', 'color', colors(2,:), ...
+     'DisplayName', 'RDC');
+plot(real(zero(G_md_mid('dL', 'f'))),  imag(zero(G_md_mid('dL', 'f'))), 'o', 'color', colors(2,:), ...
+     'HandleVisibility', 'off');
+% Estimate the maximum damping added by RDC
+gs = logspace(2,5,500);
+phis = zeros(size(gs));
+for i = 1:length(gs)
+    g = gs(i);
+    clpoles = pole(feedback(G_md_mid('dL', 'f'), -g*s, +1));
+    plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ...
+         'HandleVisibility', 'off');
+    % Estimate damping of u-station mode
+    ustation_pole = clpoles(imag(clpoles)>1000);
+    phis(i) = atan2(abs(real(ustation_pole)), abs(imag(ustation_pole)));
+end
+[~, i_max] = max(phis);
+plot([0, -5e3*sin(phis(i_max))], [0, 5e3*cos(phis(i_max))], 'k--', 'HandleVisibility', 'off');
+clpoles_max = pole(feedback(G_md_mid('dL', 'f'), -gs(i_max)*s, +1));
+ustation_pole = clpoles_max(imag(clpoles_max)>1000);
+plot(real(ustation_pole), imag(ustation_pole), 'kx', ...
+        'HandleVisibility', 'off');
+% Plot angle
+plot(-8e2*sin(0:0.01:max(phis)), 8e2*cos(sin(0:0.01:max(phis))), 'k-', 'HandleVisibility', 'off')
+text(-200, 850, '$\phi$', 'horizontalalignment', 'center');
+text(real(ustation_pole)-100, imag(ustation_pole), '$\xi = \sin(\phi)$', 'horizontalalignment', 'right');
+
+% DVF
+plot(real(pole(G_md_mid('vn', 'f'))),  imag(pole(G_md_mid('vn', 'f'))), 'x', 'color', colors(3,:), ...
+     'DisplayName', 'DVF');
+plot(real(zero(G_md_mid('vn', 'f'))),  imag(zero(G_md_mid('vn', 'f'))), 'o', 'color', colors(3,:), ...
+     'HandleVisibility', 'off');
+for g = logspace(2,5,500)
+    clpoles = pole(feedback(G_md_mid('vn', 'f'), -tf(g), +1));
+    plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ...
+         'HandleVisibility', 'off');
+end
+hold off;
+xlim([-2100, 0]); ylim([0, 2100]);
+axis square;
+xlabel('Real Part'); ylabel('Imaginary Part');
+ldg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+ldg.ItemTokenSize = [10, 1];
+
+%% Obtained damped transfer function from f to d for the three damping techniques
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_vc_mid('d', 'f'), freqs, 'Hz'))), 'k-', 'DisplayName', 'OL');
+plot(freqs, abs(squeeze(freqresp(G_iff_vc_mid('d', 'f'), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', 'IFF');
+plot(freqs, abs(squeeze(freqresp(G_rdc_vc_mid('d', 'f'), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', 'RDC');
+plot(freqs, abs(squeeze(freqresp(G_dvf_vc_mid('d', 'f'), freqs, 'Hz'))), 'color', colors(3,:), 'DisplayName', 'DVF');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude $d/f$ [m/N]'); set(gca, 'XTickLabel',[]);
+
+ax2 = nexttile();
+hold on;
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_mid('d', 'f'), freqs, 'Hz')))), 'k-');
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_vc_mid('d', 'f'), freqs, 'Hz')))), 'color', colors(1,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_rdc_vc_mid('d', 'f'), freqs, 'Hz')))), 'color', colors(2,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_dvf_vc_mid('d', 'f'), freqs, 'Hz')))), 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+yticks(-360:90:360);
+ylim([-270, 90]);
+xticks([1e0, 1e1, 1e2]);
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 500]);
+
+%% Obtained damped transfer function from f to d for the three damping techniques
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_md_mid('d', 'f'), freqs, 'Hz'))), 'k-', 'DisplayName', 'OL');
+plot(freqs, abs(squeeze(freqresp(G_iff_md_mid('d', 'f'), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', 'IFF');
+plot(freqs, abs(squeeze(freqresp(G_rdc_md_mid('d', 'f'), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', 'RDC');
+plot(freqs, abs(squeeze(freqresp(G_dvf_md_mid('d', 'f'), freqs, 'Hz'))), 'color', colors(3,:), 'DisplayName', 'DVF');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude $d/f$ [m/N]'); set(gca, 'XTickLabel',[]);
+
+ax2 = nexttile();
+hold on;
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_mid('d', 'f'), freqs, 'Hz')))), 'k-');
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_md_mid('d', 'f'), freqs, 'Hz')))), 'color', colors(1,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_rdc_md_mid('d', 'f'), freqs, 'Hz')))), 'color', colors(2,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_dvf_md_mid('d', 'f'), freqs, 'Hz')))), 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+yticks(-360:90:360);
+ylim([-270, 90]);
+xticks([1e0, 1e1, 1e2]);
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 500]);
+
+%% Obtained damped transfer function from f to d for the three damping techniques
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_pz_mid('d', 'f'), freqs, 'Hz'))), 'k-', 'DisplayName', 'OL');
+plot(freqs, abs(squeeze(freqresp(G_iff_pz_mid('d', 'f'), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', 'IFF');
+plot(freqs, abs(squeeze(freqresp(G_rdc_pz_mid('d', 'f'), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', 'RDC');
+plot(freqs, abs(squeeze(freqresp(G_dvf_pz_mid('d', 'f'), freqs, 'Hz'))), 'color', colors(3,:), 'DisplayName', 'DVF');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude $d/f$ [m/N]'); set(gca, 'XTickLabel',[]);
+leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15;
+
+ax2 = nexttile();
+hold on;
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_mid('d', 'f'), freqs, 'Hz')))), 'k-');
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_pz_mid('d', 'f'), freqs, 'Hz')))), 'color', colors(1,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_rdc_pz_mid('d', 'f'), freqs, 'Hz')))), 'color', colors(2,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_dvf_pz_mid('d', 'f'), freqs, 'Hz')))), 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+yticks(-360:90:360);
+ylim([-270, 90]);
+xticks([1e0, 1e1, 1e2]);
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 500]);
+
+%% Change of sensitivity to disturbance with all three active damping strategies
+% FS
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_md_mid('d', 'fs'), freqs, 'Hz'))), 'k-');
+plot(freqs, abs(squeeze(freqresp(G_iff_md_mid('d', 'fs'), freqs, 'Hz'))), 'color', colors(1,:));
+plot(freqs, abs(squeeze(freqresp(G_rdc_md_mid('d', 'fs'), freqs, 'Hz'))), 'color', colors(2,:));
+plot(freqs, abs(squeeze(freqresp(G_dvf_md_mid('d', 'fs'), freqs, 'Hz'))), 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude $d/f_{s}$ [m/N]'); xlabel('Frequency [Hz]');
+xticks([1e0, 1e1, 1e2]);
+xlim([1, 500]);
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_md_mid('d', 'ft'), freqs, 'Hz'))), 'k-');
+plot(freqs, abs(squeeze(freqresp(G_iff_md_mid('d', 'ft'), freqs, 'Hz'))), 'color', colors(1,:));
+plot(freqs, abs(squeeze(freqresp(G_rdc_md_mid('d', 'ft'), freqs, 'Hz'))), 'color', colors(2,:));
+plot(freqs, abs(squeeze(freqresp(G_dvf_md_mid('d', 'ft'), freqs, 'Hz'))), 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude $d/f_{t}$ [m/N]'); xlabel('Frequency [Hz]');
+xticks([1e0, 1e1, 1e2]);
+xlim([1, 500]);
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_md_mid('d', 'xf'), freqs, 'Hz'))), 'k-', 'DisplayName', 'OL');
+plot(freqs, abs(squeeze(freqresp(G_iff_md_mid('d', 'xf'), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', 'IFF');
+plot(freqs, abs(squeeze(freqresp(G_rdc_md_mid('d', 'xf'), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', 'RDC');
+plot(freqs, abs(squeeze(freqresp(G_dvf_md_mid('d', 'xf'), freqs, 'Hz'))), 'color', colors(3,:), 'DisplayName', 'DVF');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude $d/x_{f}$ [m/m]'); xlabel('Frequency [Hz]');
+xticks([1e0, 1e1, 1e2]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+xlim([1, 500]);
+
+%% Cumulative Amplitude Spectrum of the distance d with all three active damping techniques
+figure;
+hold on;
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_vc_mid('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_vc_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', 'black', 'DisplayName', 'OL');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_vc_mid('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_iff_vc_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', colors(1,:), 'DisplayName', 'IFF');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_rdc_vc_mid('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_rdc_vc_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', colors(2,:), 'DisplayName', 'RDC');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_dvf_vc_mid('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_dvf_vc_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', colors(3,:), 'DisplayName', 'DVF');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('CAS of $d$ [m]'); xlabel('Frequency [Hz]');
+xticks([1e0, 1e1, 1e2]);
+xlim([1, 500]);
+ylim([2e-10, 3e-6])
+
+figure;
+hold on;
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_md_mid('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_md_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', 'black', 'DisplayName', 'OL');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_md_mid('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_iff_md_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', colors(1,:), 'DisplayName', 'IFF');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_rdc_md_mid('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_rdc_md_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', colors(2,:), 'DisplayName', 'RDC');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_dvf_md_mid('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_dvf_md_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', colors(3,:), 'DisplayName', 'DVF');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+xticks([1e0, 1e1, 1e2]);
+xlim([1, 500]);
+ylim([2e-10, 3e-6])
+
+figure;
+hold on;
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_pz_mid('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_pz_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', 'black', 'DisplayName', 'OL');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_pz_mid('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_iff_pz_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', colors(1,:), 'DisplayName', 'IFF');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_rdc_pz_mid('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_rdc_pz_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', colors(2,:), 'DisplayName', 'RDC');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_dvf_pz_mid('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_dvf_pz_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', colors(3,:), 'DisplayName', 'DVF');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+xticks([1e0, 1e1, 1e2]);
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+xlim([1, 500]);
+ylim([2e-10, 3e-6])

A1-nass-uniaxial-model/uniaxial_6_hac_lac.m

@@ -0,0 +1,565 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+%% Path for functions, data and scripts
+addpath('./mat/'); % Path for data
+
+%% Colors for the figures
+colors = colororder;
+
+%% Frequency Vector [Hz]
+freqs = logspace(0, 3, 1000);
+
+%% Load the PSD of disturbances
+load('uniaxial_disturbance_psd.mat', 'f', 'psd_ft', 'psd_xf');
+
+%% Load Plants Dynamics
+load('uniaxial_plants.mat', 'G_vc_light', 'G_md_light', 'G_pz_light', ...
+                            'G_vc_mid',   'G_md_mid',   'G_pz_mid', ...
+                            'G_vc_heavy', 'G_md_heavy', 'G_pz_heavy');
+
+%% Load Damped Plants
+load('uniaxial_damped_plants.mat', 'G_iff_vc_light', 'G_iff_md_light', 'G_iff_pz_light', ...
+                                   'G_rdc_vc_light', 'G_rdc_md_light', 'G_rdc_pz_light', ...
+                                   'G_dvf_vc_light', 'G_dvf_md_light', 'G_dvf_pz_light', ...
+                                   'G_iff_vc_mid',   'G_iff_md_mid',   'G_iff_pz_mid', ...
+                                   'G_rdc_vc_mid',   'G_rdc_md_mid',   'G_rdc_pz_mid', ...
+                                   'G_dvf_vc_mid',   'G_dvf_md_mid',   'G_dvf_pz_mid', ...
+                                   'G_iff_vc_heavy', 'G_iff_md_heavy', 'G_iff_pz_heavy', ...
+                                   'G_rdc_vc_heavy', 'G_rdc_md_heavy', 'G_rdc_pz_heavy', ...
+                                   'G_dvf_vc_heavy', 'G_dvf_md_heavy', 'G_dvf_pz_heavy');
+
+%% Damped plant - Robustness to change of sample's mass
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_vc_light('d', 'f'), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
+plot(freqs, abs(squeeze(freqresp(G_iff_vc_light('d', 'f'), freqs, 'Hz'))), 'color', colors(1,:));
+plot(freqs, abs(squeeze(freqresp(G_iff_vc_mid(  'd', 'f'), freqs, 'Hz'))), 'color', colors(2,:));
+plot(freqs, abs(squeeze(freqresp(G_iff_vc_heavy('d', 'f'), freqs, 'Hz'))), 'color', colors(3,:));
+loglog(10.^(0.4*cos([0:0.01:2*pi])+log10(100)), ...
+       10.^(0.8*sin([0:0.01:2*pi]-pi/4)+log10(8e-8)), 'k--');
+text(20, 4e-8, sprintf('Small\nInteraction'), 'horizontalalignment', 'center');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+ylim([5e-10, 1e-3]);
+
+ax1b = nexttile();
+hold on;
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_light('d', 'f'), freqs, 'Hz')))), 'color', [colors(1,:), 0.5]);
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_vc_light('d', 'f'), freqs, 'Hz')))), 'color', colors(1,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_vc_mid(  'd', 'f'), freqs, 'Hz')))), 'color', colors(2,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_vc_heavy('d', 'f'), freqs, 'Hz')))), 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+xticks([1e0, 1e1, 1e2]);
+yticks(-360:90:360);
+ylim([-200, 20]);
+
+linkaxes([ax1,ax1b],'x');
+xlim([1, 1e3]);
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+ax2 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_md_light('d', 'f'), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
+plot(freqs, abs(squeeze(freqresp(G_iff_md_light('d', 'f'), freqs, 'Hz'))), 'color', colors(1,:));
+plot(freqs, abs(squeeze(freqresp(G_iff_md_mid(  'd', 'f'), freqs, 'Hz'))), 'color', colors(2,:));
+plot(freqs, abs(squeeze(freqresp(G_iff_md_heavy('d', 'f'), freqs, 'Hz'))), 'color', colors(3,:));
+loglog(10.^(0.4*cos([0:0.01:2*pi])+log10(200)), ...
+       10.^(0.8*sin([0:0.01:2*pi]-pi/4)+log10(2e-8)), 'k--');
+text(40, 1e-8, sprintf('Small\nInteraction'), 'horizontalalignment', 'center');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+ylim([5e-10, 1e-3]);
+
+ax2b = nexttile();
+hold on;
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_md_light('d', 'f'), freqs, 'Hz')))), 'color', [colors(1,:), 0.5]);
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_md_light('d', 'f'), freqs, 'Hz')))), 'color', colors(1,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_md_mid(  'd', 'f'), freqs, 'Hz')))), 'color', colors(2,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_md_heavy('d', 'f'), freqs, 'Hz')))), 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+xticks([1e0, 1e1, 1e2]);
+yticks(-360:90:360);
+ylim([-200, 20]);
+
+linkaxes([ax2,ax2b],'x');
+xlim([1, 1e3]);
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+ax3 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_pz_light('d', 'f'), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], 'DisplayName', '$m_s = 1\,kg$, OL');
+plot(freqs, abs(squeeze(freqresp(G_iff_pz_light('d', 'f'), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', '$m_s = 1\,kg$, IFF');
+plot(freqs, abs(squeeze(freqresp(G_iff_pz_mid(  'd', 'f'), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', '$m_s = 25\,kg$, IFF');
+plot(freqs, abs(squeeze(freqresp(G_iff_pz_heavy('d', 'f'), freqs, 'Hz'))), 'color', colors(3,:), 'DisplayName', '$m_s = 50\,kg$, IFF');
+loglog(10.^(0.8*cos([0:0.01:2*pi])+log10(350)), ...
+       10.^(1.2*sin([0:0.01:2*pi])+log10(8e-9)), 'k--', 'HandleVisibility', 'off');
+text(200, 5e-7, sprintf('$\\mu$ Station\nCoupling'), 'horizontalalignment', 'center');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+ylim([5e-10, 1e-3]);
+ldg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+ldg.ItemTokenSize = [20, 1];
+
+ax3b = nexttile();
+hold on;
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_light('d', 'f'), freqs, 'Hz')))), 'color', [colors(1,:), 0.5]);
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_pz_light('d', 'f'), freqs, 'Hz')))), 'color', colors(1,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_pz_mid(  'd', 'f'), freqs, 'Hz')))), 'color', colors(2,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_pz_heavy('d', 'f'), freqs, 'Hz')))), 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+xticks([1e0, 1e1, 1e2]);
+yticks(-360:90:360);
+ylim([-200, 20]);
+
+linkaxes([ax3,ax3b],'x');
+xlim([1, 1e3]);
+
+%% High Authority Controller - Soft Nano-Hexapod
+% Lead to increase phase margin
+a  = 5;  % Amount of phase lead / width of the phase lead / high frequency gain
+wc = 2*pi*20; % Frequency with the maximum phase lead [rad/s]
+H_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
+
+% Lag at low frequency
+H_lag = (s + 2*pi*5)/(s + 2*pi*0.01);
+
+% Low Pass filter to increase robustness
+H_lpf = 1/(1 + s/2/pi/200);
+
+% High Authority Controller
+K_hac_vc = 4e5    * ... % Gain
+           H_lead * ... % Lead
+           H_lag  * ... % Lag
+           H_lpf;       % LPF
+K_hac_vc.InputName = {'d'};
+K_hac_vc.OutputName = {'f'};
+
+%% High Authority Controller - Mid Stiffness Nano-Hexapod
+% Lead to increase phase margin
+a  = 4;  % Amount of phase lead / width of the phase lead / high frequency gain
+wc = 2*pi*70; % Frequency with the maximum phase lead [rad/s]
+
+H_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
+
+% Lag at low frequency
+H_lag = ((s + 2*pi*15)/(s + 2*pi*0.01))^2;
+
+% Low Pass filter to increase robustness
+H_lpf = 1/(1 + s/2/pi/300);
+
+% High Authority Controller
+K_hac_md = 3e6    * ... % Gain
+           H_lead * ... % Lead
+           H_lag  * ... % Lag
+           H_lpf;       % LPF
+K_hac_md.InputName = {'d'};
+K_hac_md.OutputName = {'f'};
+
+%% High Authority Controller - Stiff Nano-Hexapod
+% Lead to increase phase margin
+a  = 5;  % Amount of phase lead / width of the phase lead / high frequency gain
+wc = 2*pi*100; % Frequency with the maximum phase lead [rad/s]
+H_lead = ((1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))))^2;
+
+% Integrator
+H_int = 1/(s + 2*pi*0.01)^2;
+
+% Low Pass filter to increase robustness
+H_lpf = 1/(1 + s/2/pi/500);
+
+% High Authority Controller
+K_hac_pz = 6e12   * ... % Gain
+           H_lead * ... % Lead
+           H_int  * ... % Lag
+           H_lpf;       % LPF
+K_hac_pz.InputName = {'d'};
+K_hac_pz.OutputName = {'f'};
+
+%% Save High Authority Controllers
+save('./mat/uniaxial_high_authority_controllers.mat', ...
+        'K_hac_vc', 'K_hac_md', 'K_hac_pz');
+
+%% Compute Loop gain for Nyquist Plot
+L_vc_light = squeeze(freqresp(K_hac_vc*G_iff_vc_light('d', 'f'), freqs, 'Hz'));
+L_vc_mid   = squeeze(freqresp(K_hac_vc*G_iff_vc_mid(  'd', 'f'), freqs, 'Hz'));
+L_vc_heavy = squeeze(freqresp(K_hac_vc*G_iff_vc_heavy('d', 'f'), freqs, 'Hz'));
+
+L_md_light = squeeze(freqresp(K_hac_md*G_iff_md_light('d', 'f'), freqs, 'Hz'));
+L_md_mid   = squeeze(freqresp(K_hac_md*G_iff_md_mid(  'd', 'f'), freqs, 'Hz'));
+L_md_heavy = squeeze(freqresp(K_hac_md*G_iff_md_heavy('d', 'f'), freqs, 'Hz'));
+
+L_pz_light = squeeze(freqresp(K_hac_pz*G_iff_pz_light('d', 'f'), freqs, 'Hz'));
+L_pz_mid   = squeeze(freqresp(K_hac_pz*G_iff_pz_mid(  'd', 'f'), freqs, 'Hz'));
+L_pz_heavy = squeeze(freqresp(K_hac_pz*G_iff_pz_heavy('d', 'f'), freqs, 'Hz'));
+
+%% Nyquist Plot - Hight Authority Controller for all three nano-hexapod stiffnesses and all sample masses
+figure;
+hold on;
+plot(real(L_vc_light), +imag(L_vc_light), '-', 'color', [colors(1,:), 0.5], 'DisplayName', '$k_n = 0.01\,N/\mu m$')
+plot(real(L_vc_light), -imag(L_vc_light), '-', 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off')
+plot(real(L_vc_mid  ), +imag(L_vc_mid  ), '-', 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off')
+plot(real(L_vc_mid  ), -imag(L_vc_mid  ), '-', 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off')
+plot(real(L_vc_heavy), +imag(L_vc_heavy), '-', 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off')
+plot(real(L_vc_heavy), -imag(L_vc_heavy), '-', 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off')
+
+plot(real(L_md_light), +imag(L_md_light), '-', 'color', [colors(2,:), 0.5], 'DisplayName', '$k_n = 1\,N/\mu m$')
+plot(real(L_md_light), -imag(L_md_light), '-', 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off')
+plot(real(L_md_mid  ), +imag(L_md_mid  ), '-', 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off')
+plot(real(L_md_mid  ), -imag(L_md_mid  ), '-', 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off')
+plot(real(L_md_heavy), +imag(L_md_heavy), '-', 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off')
+plot(real(L_md_heavy), -imag(L_md_heavy), '-', 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off')
+
+plot(real(L_pz_light), +imag(L_pz_light), '-', 'color', [colors(3,:), 0.5], 'DisplayName', '$k_n = 100\,N/\mu m$')
+plot(real(L_pz_light), -imag(L_pz_light), '-', 'color', [colors(3,:), 0.5], 'HandleVisibility', 'off')
+plot(real(L_pz_mid  ), +imag(L_pz_mid  ), '-', 'color', [colors(3,:), 0.5], 'HandleVisibility', 'off')
+plot(real(L_pz_mid  ), -imag(L_pz_mid  ), '-', 'color', [colors(3,:), 0.5], 'HandleVisibility', 'off')
+plot(real(L_pz_heavy), +imag(L_pz_heavy), '-', 'color', [colors(3,:), 0.5], 'HandleVisibility', 'off')
+plot(real(L_pz_heavy), -imag(L_pz_heavy), '-', 'color', [colors(3,:), 0.5], 'HandleVisibility', 'off')
+plot(-1, 0, 'kx', 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
+xlabel('Real'); ylabel('Imag');
+legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+xlim([-3.8, 0.2]); ylim([-2, 2]);
+axis square;
+
+%% Nyquist Plot - Hight Authority Controller - Soft Nano-Hexapod
+figure;
+hold on;
+plot(real(L_vc_light), +imag(L_vc_light), '-', 'color', colors(1,:), 'DisplayName', '$m_s = 1\,$kg')
+plot(real(L_vc_light), -imag(L_vc_light), '-', 'color', colors(1,:), 'HandleVisibility', 'off')
+plot(real(L_vc_mid  ), +imag(L_vc_mid  ), '-', 'color', colors(2,:), 'DisplayName', '$m_s = 25\,$kg')
+plot(real(L_vc_mid  ), -imag(L_vc_mid  ), '-', 'color', colors(2,:), 'HandleVisibility', 'off')
+plot(real(L_vc_heavy), +imag(L_vc_heavy), '-', 'color', colors(3,:), 'DisplayName', '$m_s = 50\,$kg')
+plot(real(L_vc_heavy), -imag(L_vc_heavy), '-', 'color', colors(3,:), 'HandleVisibility', 'off')
+% Minimum modul margin
+vc_mod_margin = min([min(abs(L_vc_light + 1)), min(abs(L_vc_mid + 1)), min(abs(L_vc_heavy + 1))]);
+plot(-1 + vc_mod_margin*cos(linspace(0,2*pi,100)), vc_mod_margin*sin(linspace(0,2*pi,100)), 'k-', 'DisplayName', sprintf('$r = %.2f$', vc_mod_margin))
+plot(-1, 0, 'kx', 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
+xlabel('Real'); ylabel('Imag');
+leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15
+xlim([-3.8, 0.2]); ylim([-2, 2]);
+axis square;
+
+%% Nyquist Plot - Hight Authority Controller - Soft Nano-Hexapod
+figure;
+hold on;
+plot(real(L_md_light), +imag(L_md_light), '-', 'color', colors(1,:), 'DisplayName', '$m_s = 1\,$kg')
+plot(real(L_md_light), -imag(L_md_light), '-', 'color', colors(1,:), 'HandleVisibility', 'off')
+plot(real(L_md_mid  ), +imag(L_md_mid  ), '-', 'color', colors(2,:), 'DisplayName', '$m_s = 25\,$kg')
+plot(real(L_md_mid  ), -imag(L_md_mid  ), '-', 'color', colors(2,:), 'HandleVisibility', 'off')
+plot(real(L_md_heavy), +imag(L_md_heavy), '-', 'color', colors(3,:), 'DisplayName', '$m_s = 50\,$kg')
+plot(real(L_md_heavy), -imag(L_md_heavy), '-', 'color', colors(3,:), 'HandleVisibility', 'off')
+% Minimum modul margin
+md_mod_margin = min([min(abs(L_md_light + 1)), min(abs(L_md_mid + 1)), min(abs(L_md_heavy + 1))]);
+plot(-1 + md_mod_margin*cos(linspace(0,2*pi,100)), md_mod_margin*sin(linspace(0,2*pi,100)), 'k-', 'DisplayName', sprintf('$r = %.2f$', md_mod_margin))
+plot(-1, 0, 'kx', 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
+xlabel('Real'); ylabel('Imag');
+leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15
+xlim([-3.8, 0.2]); ylim([-2, 2]);
+axis square;
+
+%% Nyquist Plot - Hight Authority Controller - Soft Nano-Hexapod
+figure;
+hold on;
+plot(real(L_pz_light), +imag(L_pz_light), '-', 'color', colors(1,:), 'DisplayName', '$m_s = 1\,$kg')
+plot(real(L_pz_light), -imag(L_pz_light), '-', 'color', colors(1,:), 'HandleVisibility', 'off')
+plot(real(L_pz_mid  ), +imag(L_pz_mid  ), '-', 'color', colors(2,:), 'DisplayName', '$m_s = 25\,$kg')
+plot(real(L_pz_mid  ), -imag(L_pz_mid  ), '-', 'color', colors(2,:), 'HandleVisibility', 'off')
+plot(real(L_pz_heavy), +imag(L_pz_heavy), '-', 'color', colors(3,:), 'DisplayName', '$m_s = 50\,$kg')
+plot(real(L_pz_heavy), -imag(L_pz_heavy), '-', 'color', colors(3,:), 'HandleVisibility', 'off')
+% Minimum modul margin
+pz_mod_margin = min([min(abs(L_pz_light + 1)), min(abs(L_pz_mid + 1)), min(abs(L_pz_heavy + 1))]);
+plot(-1 + pz_mod_margin*cos(linspace(0,2*pi,100)), pz_mod_margin*sin(linspace(0,2*pi,100)), 'k-', 'DisplayName', sprintf('$r = %.2f$', pz_mod_margin))
+plot(-1, 0, 'kx', 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
+xlabel('Real'); ylabel('Imag');
+leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15
+xlim([-3.8, 0.2]); ylim([-2, 2]);
+axis square;
+
+%% Loop Gain - High Authority Controller - Relatively soft nano-hexapod
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(L_vc_light), 'color', colors(1,:), 'DisplayName', '$m_s = 1\,$kg');
+plot(freqs, abs(L_vc_mid),   'color', colors(2,:), 'DisplayName', '$m_s = 25\,$kg');
+plot(freqs, abs(L_vc_heavy), 'color', colors(3,:), 'DisplayName', '$m_s = 50\,$kg');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
+ylim([1e-3, 1e3]);
+yticks([1e-2, 1, 1e2])
+leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15
+
+ax2 = nexttile;
+hold on;
+plot(freqs, 180/pi*unwrap(angle(L_vc_light)), 'color', colors(1,:));
+plot(freqs, 180/pi*unwrap(angle(L_vc_mid  )), 'color', colors(2,:));
+plot(freqs, 180/pi*unwrap(angle(L_vc_heavy)), 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:45:360);
+ylim([-225, -90]);
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 500]);
+xticks([1, 10, 100]);
+
+%% Loop Gain - High Authority Controller - Relatively stiff nano-hexapod
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(L_md_light), 'color', colors(1,:), 'DisplayName', '$m_s = 1\,$kg');
+plot(freqs, abs(L_md_mid),   'color', colors(2,:), 'DisplayName', '$m_s = 25\,$kg');
+plot(freqs, abs(L_md_heavy), 'color', colors(3,:), 'DisplayName', '$m_s = 50\,$kg');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
+ylim([1e-3, 1e3]);
+yticks([1e-2, 1, 1e2])
+
+ax2 = nexttile;
+hold on;
+plot(freqs, 180/pi*unwrap(angle(L_md_light)), 'color', colors(1,:));
+plot(freqs, 180/pi*unwrap(angle(L_md_mid  )), 'color', colors(2,:));
+plot(freqs, 180/pi*unwrap(angle(L_md_heavy)), 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:45:360);
+ylim([-225, -90]);
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 500]);
+xticks([1, 10, 100]);
+
+%% Loop Gain - High Authority Controller - Stiff nano-hexapod
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(L_pz_light), 'color', colors(1,:), 'DisplayName', '$m_s = 1\,$kg');
+plot(freqs, abs(L_pz_mid),   'color', colors(2,:), 'DisplayName', '$m_s = 25\,$kg');
+plot(freqs, abs(L_pz_heavy), 'color', colors(3,:), 'DisplayName', '$m_s = 50\,$kg');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
+ylim([1e-3, 1e3]);
+yticks([1e-2, 1, 1e2])
+
+ax2 = nexttile;
+hold on;
+plot(freqs, 180/pi*unwrap(angle(L_pz_light)), 'color', colors(1,:));
+plot(freqs, 180/pi*unwrap(angle(L_pz_mid  )), 'color', colors(2,:));
+plot(freqs, 180/pi*unwrap(angle(L_pz_heavy)), 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:45:360);
+ylim([-225, -90]);
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 500]);
+xticks([1, 10, 100]);
+
+%% Compute Closed Loop Systems
+G_hac_iff_vc_light = feedback(G_iff_vc_light, K_hac_vc, 'name', -1);
+G_hac_iff_vc_mid   = feedback(G_iff_vc_mid  , K_hac_vc, 'name', -1);
+G_hac_iff_vc_heavy = feedback(G_iff_vc_heavy, K_hac_vc, 'name', -1);
+
+G_hac_iff_md_light = feedback(G_iff_md_light, K_hac_md, 'name', -1);
+G_hac_iff_md_mid   = feedback(G_iff_md_mid  , K_hac_md, 'name', -1);
+G_hac_iff_md_heavy = feedback(G_iff_md_heavy, K_hac_md, 'name', -1);
+
+G_hac_iff_pz_light = feedback(G_iff_pz_light, K_hac_pz, 'name', -1);
+G_hac_iff_pz_mid   = feedback(G_iff_pz_mid  , K_hac_pz, 'name', -1);
+G_hac_iff_pz_heavy = feedback(G_iff_pz_heavy, K_hac_pz, 'name', -1);
+
+%% Verify Stability
+isstable(G_hac_iff_vc_light) && isstable(G_hac_iff_vc_mid) && isstable(G_hac_iff_vc_heavy)
+
+isstable(G_hac_iff_md_light) && isstable(G_hac_iff_md_mid) && isstable(G_hac_iff_md_heavy)
+
+isstable(G_hac_iff_pz_light) && isstable(G_hac_iff_pz_mid) && isstable(G_hac_iff_pz_heavy)
+
+%% Change of sensitivity to disturbances with LAC and with HAC-LAC
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_md_mid(    'd', 'fs'), freqs, 'Hz'))), 'k-');
+plot(freqs, abs(squeeze(freqresp(G_iff_md_mid('d', 'fs'), freqs, 'Hz'))), 'color', colors(1,:));
+plot(freqs, abs(squeeze(freqresp(G_hac_iff_md_mid('d', 'fs'), freqs, 'Hz'))), 'color', colors(2,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xticks([1e0, 1e1, 1e2]);
+ylabel('Amplitude $d/f_{s}$ [m/N]'); xlabel('Frequency [Hz]');
+xlim([1, 500]);
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_md_mid(    'd', 'ft'), freqs, 'Hz'))), 'k-');
+plot(freqs, abs(squeeze(freqresp(G_iff_md_mid('d', 'ft'), freqs, 'Hz'))), 'color', colors(1,:));
+plot(freqs, abs(squeeze(freqresp(G_hac_iff_md_mid('d', 'ft'), freqs, 'Hz'))), 'color', colors(2,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xticks([1e0, 1e1, 1e2]);
+ylabel('Amplitude $d/f_{t}$ [m/N]'); xlabel('Frequency [Hz]');
+xlim([1, 500]);
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_md_mid(    'd', 'xf'), freqs, 'Hz'))), 'k-', 'DisplayName', 'OL');
+plot(freqs, abs(squeeze(freqresp(G_iff_md_mid('d', 'xf'), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', 'IFF');
+plot(freqs, abs(squeeze(freqresp(G_hac_iff_md_mid('d', 'xf'), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', 'HAC-IFF');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xticks([1e0, 1e1, 1e2]);
+ylabel('Amplitude $d/x_{f}$ [m/m]'); xlabel('Frequency [Hz]');
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+xlim([1, 500]);
+
+%% Cumulative Amplitude Spectrum for all three nano-hexapod stiffnesses - Comparison of OL, IFF and HAC-LAC cases
+figure;
+hold on;
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_vc_mid('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_vc_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [0,0,0,0.5], 'DisplayName', 'OL');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_vc_light('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_vc_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [0,0,0,0.5], 'HandleVisibility', 'off');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_vc_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_vc_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [0,0,0,0.5], 'HandleVisibility', 'off');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_vc_mid('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_iff_vc_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [colors(1,:), 0.5], 'DisplayName', 'IFF');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_vc_light('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_iff_vc_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_vc_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_iff_vc_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_vc_mid('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_vc_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [colors(2,:), 0.5], 'DisplayName', 'HAC-IFF');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_vc_light('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_vc_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_vc_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_vc_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xticks([1e0, 1e1, 1e2]);
+ylabel('CAS of $d$ [m]'); xlabel('Frequency [Hz]');
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+xlim([1, 500]);
+ylim([2e-10, 3e-6])
+
+figure;
+hold on;
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_md_mid('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_md_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [0,0,0,0.5]);
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_md_light('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_md_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [0,0,0,0.5]);
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_md_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_md_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [0,0,0,0.5]);
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_md_mid('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_iff_md_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [colors(1,:), 0.5]);
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_md_light('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_iff_md_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [colors(1,:), 0.5]);
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_md_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_iff_md_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [colors(1,:), 0.5]);
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_md_mid('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_md_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [colors(2,:), 0.5]);
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_md_light('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_md_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [colors(2,:), 0.5]);
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_md_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_md_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [colors(2,:), 0.5]);
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xticks([1e0, 1e1, 1e2]);
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+xlim([1, 500]);
+ylim([2e-10, 3e-6])
+
+figure;
+hold on;
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_pz_mid('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_pz_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [0,0,0,0.5]);
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_pz_light('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_pz_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [0,0,0,0.5]);
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_pz_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_pz_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [0,0,0,0.5]);
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_pz_mid('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_iff_pz_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [colors(1,:), 0.5]);
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_pz_light('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_iff_pz_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [colors(1,:), 0.5]);
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_iff_pz_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_iff_pz_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [colors(1,:), 0.5]);
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_mid('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_mid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [colors(2,:), 0.5]);
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_light('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_light('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [colors(2,:), 0.5]);
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_heavy('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_heavy('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+     'color', [colors(2,:), 0.5]);
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xticks([1e0, 1e1, 1e2]);
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+xlim([1, 500]);
+ylim([2e-10, 3e-6])

A1-nass-uniaxial-model/uniaxial_7_support_compliance.m

@@ -0,0 +1,202 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+%% Path for functions, data and scripts
+addpath('./mat/'); % Path for data
+
+%% Colors for the figures
+colors = colororder;
+
+%% Load the micro-station parameters
+load('uniaxial_micro_station_parameters.mat')
+
+%% Frequency Vector [Hz]
+freqs = logspace(0, 3, 1000);
+
+%% Nano-Hexapod Parameters
+m = 20; % Mass [kg]
+
+% "Soft" Nano-Hexapod
+k_soft = m*(2*pi*10)^2; % Stiffness [N/m]
+c_soft = 0.1*2*sqrt(m*k_soft); % Damping [N/(m/s)]
+
+% "Mid" Nano-Hexapod
+k_mid = m*(2*pi*70)^2; % Stiffness [N/m]
+c_mid = 0.1*2*sqrt(m*k_mid); % Damping [N/(m/s)]
+
+% "Stiff" Nano-Hexapod
+k_stiff = m*(2*pi*350)^2; % Stiffness [N/m]
+c_stiff = 0.1*2*sqrt(m*k_stiff); % Damping [N/(m/s)]
+
+%% Compute the transfer functions for considered nano-hexapods - From F to L'
+% "Soft" Nano-Hexapod
+G_soft_a = 1/(m*s^2 + c_soft*s + k_soft); % Transfer function from F to L'
+
+% "Mid" Nano-Hexapod
+G_mid_a = 1/(m*s^2 + c_mid*s + k_mid); % Transfer function from F to L'
+
+% "Stiff" Nano-Hexapod
+G_stiff_a = 1/(m*s^2 + c_stiff*s + k_stiff); % Transfer function from F to L'
+
+%% Obtained transfer functions from F to L when neglecting support compliance
+freqs = logspace(0, 3, 1000);
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_soft_a, freqs, 'Hz'))), '-', 'color', colors(1,:));
+text(50, 5e-5, '$\omega_n =$ 10Hz', 'horizontalalignment', 'center');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
+xlim([freqs(1), freqs(end)]);
+xticks([1e0, 1e1, 1e2]);
+ylim([1e-9, 1e-4]);
+yticks([1e-9, 1e-7, 1e-5]);
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_mid_a, freqs, 'Hz'))), '-', 'color', colors(1,:));
+text(70, 3e-6, '$\omega_n =$ 70Hz', 'horizontalalignment', 'center');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+xlim([freqs(1), freqs(end)]);
+xticks([1e0, 1e1, 1e2]);
+ylim([1e-9, 1e-4]);
+yticks([1e-9, 1e-7, 1e-5]);
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_stiff_a, freqs, 'Hz'))), '-', 'color', colors(1,:), ...
+     'DisplayName', '$L^\prime/F$');
+text(200, 8e-8, '$\omega_n =$ 400Hz', 'horizontalalignment', 'center');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+legend('location', 'northeast');
+xlim([freqs(1), freqs(end)]);
+xticks([1e0, 1e1, 1e2]);
+ylim([1e-9, 1e-4]);
+yticks([1e-9, 1e-7, 1e-5]);
+
+%% Parameters of the support compliance
+w0h = 2*pi*70; % [rad/s]
+xih = 0.1; % [-]
+
+mh = 20; % [kg]
+kh = mh*w0h^2;
+ch = xih*2*sqrt(kh*mh);
+
+%% Compute the transfer functions from F to L and from F to d for considered Nano-Hexapods
+% "Soft" Nano-Hexapod
+G_soft = (mh*s^2 + ch*s + kh)/(m*s^2*(c_soft*s + k_soft) + (m*s^2 + c_soft*s + k_soft)*(mh*s^2 + ch*s + kh)); % d/F
+G_soft_r = (1 - m*s^2*G_soft)/(c_soft*s + k_soft); % L/F
+
+% "Mid" Nano-Hexapod
+G_mid = (mh*s^2 + ch*s + kh)/(m*s^2*(c_mid*s + k_mid) + (m*s^2 + c_mid*s + k_mid)*(mh*s^2 + ch*s + kh)); % d/F
+G_mid_r = (1 - m*s^2*G_mid)/(c_mid*s + k_mid); % L/F
+
+% "Stiff" Nano-Hexapod
+G_stiff = (mh*s^2 + ch*s + kh)/(m*s^2*(c_stiff*s + k_stiff) + (m*s^2 + c_stiff*s + k_stiff)*(mh*s^2 + ch*s + kh)); % d/F
+G_stiff_r = (1 - m*s^2*G_stiff)/(c_stiff*s + k_stiff); % L/F
+
+%% Effect of the support compliance on the transfer functions from F to L and from F to d
+freqs = logspace(0, 3, 1000);
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_soft_a, freqs, 'Hz'))), '-', 'color', colors(1,:));
+plot(freqs, abs(squeeze(freqresp(G_soft_r, freqs, 'Hz'))), '-', 'color', colors(2,:));
+loglog(10.^(0.3*cos(0:0.01:2*pi)+log10(60)), ...
+       10.^(0.6*sin(0:0.01:2*pi)+log10(4e-7)), 'k--');
+text(8, 3e-7, sprintf('Support\nDynamics'), 'horizontalalignment', 'center');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
+xlim([freqs(1), freqs(end)]);
+xticks([1e0, 1e1, 1e2]);
+ylim([1e-9, 1e-4]);
+yticks([1e-9, 1e-7, 1e-5]);
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_mid_a, freqs, 'Hz'))), '-', 'color', colors(1,:));
+plot(freqs, abs(squeeze(freqresp(G_mid_r, freqs, 'Hz'))), '-', 'color', colors(2,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]');
+set(gca, 'YTickLabel',[]);
+xlim([freqs(1), freqs(end)]);
+xticks([1e0, 1e1, 1e2]);
+ylim([1e-9, 1e-4]);
+yticks([1e-9, 1e-7, 1e-5]);
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_stiff_a, freqs, 'Hz'))), '-', 'color', colors(1,:), ...
+     'DisplayName', '$L^\prime/F$');
+plot(freqs, abs(squeeze(freqresp(G_stiff_r, freqs, 'Hz'))), '-', 'color', colors(2,:), ...
+     'DisplayName', '$L/F$');
+loglog(10.^(0.3*cos(0:0.01:2*pi)+log10(50)), ...
+       10.^(0.3*sin(0:0.01:2*pi)+log10(8e-9)), 'k--', 'HandleVisibility', 'off');
+text(50, 3e-8, 'No effect', 'horizontalalignment', 'center');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+xlim([freqs(1), freqs(end)]);
+xticks([1e0, 1e1, 1e2]);
+ylim([1e-9, 1e-4]);
+yticks([1e-9, 1e-7, 1e-5]);
+
+%% Effect of the support compliance on the transfer functions from F to L and from F to d
+freqs = logspace(0, 3, 1000);
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_soft_a, freqs, 'Hz'))), '-', 'color', colors(1,:));
+plot(freqs, abs(squeeze(freqresp(G_soft,   freqs, 'Hz'))), '-', 'color', colors(3,:));
+loglog(10.^(0.3*cos(0:0.01:2*pi)+log10(60)), ...
+       10.^(0.6*sin(0:0.01:2*pi)+log10(4e-7)), 'k--');
+text(8, 3e-7, 'No effect', 'horizontalalignment', 'center');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
+xlim([freqs(1), freqs(end)]);
+xticks([1e0, 1e1, 1e2]);
+ylim([1e-9, 1e-4]);
+yticks([1e-9, 1e-7, 1e-5]);
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_mid_a, freqs, 'Hz'))), '-', 'color', colors(1,:));
+plot(freqs, abs(squeeze(freqresp(G_mid,   freqs, 'Hz'))), '-', 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]');
+set(gca, 'YTickLabel',[]);
+xlim([freqs(1), freqs(end)]);
+xticks([1e0, 1e1, 1e2]);
+ylim([1e-9, 1e-4]);
+yticks([1e-9, 1e-7, 1e-5]);
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_stiff_a, freqs, 'Hz'))), '-', 'color', colors(1,:), ...
+     'DisplayName', '$L^\prime/F$');
+plot(freqs, abs(squeeze(freqresp(G_stiff,   freqs, 'Hz'))), '-', 'color', colors(3,:), ...
+     'DisplayName', '$d/F$');
+loglog(10.^(0.4*cos(0:0.01:2*pi)+log10(50)), ...
+       10.^(0.8*sin(0:0.01:2*pi)+log10(8e-9)), 'k--', 'HandleVisibility', 'off');
+text(50, 2e-7, sprintf('Support\nDynamics'), 'horizontalalignment', 'center');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+xlim([freqs(1), freqs(end)]);
+xticks([1e0, 1e1, 1e2]);
+ylim([1e-9, 1e-4]);
+yticks([1e-9, 1e-7, 1e-5]);

A1-nass-uniaxial-model/uniaxial_8_payload_dynamics.m

@@ -0,0 +1,397 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+%% Path for functions, data and scripts
+addpath('./mat/'); % Path for data
+
+%% Colors for the figures
+colors = colororder;
+
+%% Uniaxial Simscape model name
+mdl = 'nass_uniaxial_model';
+
+%% Load the micro-station parameters
+load('uniaxial_micro_station_parameters.mat')
+
+%% Load the PSD of disturbances
+load('uniaxial_disturbance_psd.mat', 'f', 'psd_ft', 'psd_xf');
+
+%% Load Active Damping Controller
+load('uniaxial_active_damping_controllers.mat', 'K_iff_vc', 'K_iff_md', 'K_iff_pz', ...
+                                                'K_rdc_vc', 'K_rdc_md', 'K_rdc_pz', ...
+                                                'K_dvf_vc', 'K_dvf_md', 'K_dvf_pz');
+
+%% Load High Authority Controllers
+load('uniaxial_high_authority_controllers.mat', 'K_hac_vc', 'K_hac_md', 'K_hac_pz');
+
+%% Frequency Vector [Hz]
+freqs = logspace(0, 3, 1000);
+
+%% Soft Nano-Hexapod
+% Light payload mass
+mn = 15; % Nano-Hexapod mass [kg]
+ms = 1; % Sample Mass [kg]
+kn = 1e4; % Nano-Hexapod (soft) Stiffness [N/m]
+cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+
+% Rigid sample
+G_vc_rigid_light = 1/((mn + ms)*s^2 + cn*s + kn);
+
+% Soft Sample
+ws = 2*pi*20;
+ks = ms * ws^2;
+cs = 2*0.01*sqrt(ms*ks);
+G_vc_soft_light = (ms*s^2 + cs*s + ks)/((mn*s^2 + cn*s + kn)*(ms*s^2 + cs*s + ks) + ms*s^2*(cs*s + ks));
+
+% Stiff Sample
+ws = 2*pi*200;
+ks = ms * ws^2;
+cs = 2*0.01*sqrt(ms*ks);
+G_vc_stiff_light = (ms*s^2 + cs*s + ks)/((mn*s^2 + cn*s + kn)*(ms*s^2 + cs*s + ks) + ms*s^2*(cs*s + ks));
+
+% Heavy payload mass
+mn = 15; % Nano-Hexapod mass [kg]
+ms = 50; % Sample Mass [kg]
+kn = 1e4; % Nano-Hexapod (soft) Stiffness [N/m]
+cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+
+% Rigid sample
+G_vc_rigid_heavy = 1/((mn + ms)*s^2 + cn*s + kn);
+
+% Soft Sample
+ws = 2*pi*20;
+ks = ms * ws^2;
+cs = 2*0.01*sqrt(ms*ks);
+G_vc_soft_heavy = (ms*s^2 + cs*s + ks)/((mn*s^2 + cn*s + kn)*(ms*s^2 + cs*s + ks) + ms*s^2*(cs*s + ks));
+
+% Stiff Sample
+ws = 2*pi*200;
+ks = ms * ws^2;
+cs = 2*0.01*sqrt(ms*ks);
+G_vc_stiff_heavy = (ms*s^2 + cs*s + ks)/((mn*s^2 + cn*s + kn)*(ms*s^2 + cs*s + ks) + ms*s^2*(cs*s + ks));
+
+%% Effect of the payload dynamics on the soft Nano-Hexapod. Light sample on the right, and heavy sample on the left
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_vc_rigid_light, freqs, 'Hz'))), '-',  'color', colors(1,:), 'DisplayName', 'Rigid sample');
+plot(freqs, abs(squeeze(freqresp(G_vc_stiff_light, freqs, 'Hz'))), '-', 'color', colors(2,:), 'DisplayName', '$\omega_s = 200\,Hz$');
+plot(freqs, abs(squeeze(freqresp(G_vc_soft_light, freqs, 'Hz'))), '-', 'color', colors(3,:), 'DisplayName', '$\omega_s = 20\,Hz$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+ylim([1e-10, 1e-2])
+
+ax1b = nexttile();
+hold on;
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_rigid_light, freqs, 'Hz')))), '-',  'color', colors(1,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_stiff_light, freqs, 'Hz')))), '-', 'color', colors(2,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_soft_light, freqs, 'Hz')))), '-', 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+xticks([1e0, 1e1, 1e2]);
+yticks(-360:90:360);
+ylim([-200, 20]);
+
+linkaxes([ax1,ax1b],'x');
+xlim([1, 1000]);
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax2 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_vc_rigid_heavy, freqs, 'Hz'))), '-',  'color', colors(1,:), 'DisplayName', 'Rigid sample');
+plot(freqs, abs(squeeze(freqresp(G_vc_stiff_heavy, freqs, 'Hz'))), '-', 'color', colors(2,:), 'DisplayName', '$\omega_s = 200\,Hz$');
+plot(freqs, abs(squeeze(freqresp(G_vc_soft_heavy, freqs, 'Hz'))), '-', 'color', colors(3,:), 'DisplayName', '$\omega_s = 20\,Hz$');
+plot(freqs, abs(squeeze(freqresp(1/(mn*s^2), freqs, 'Hz'))), '-', 'color', [0,0,0,0.5], 'DisplayName', '$\frac{1}{m_n s^2}$');
+plot(freqs, abs(squeeze(freqresp(1/((mn + ms)*s^2), freqs, 'Hz'))), '--', 'color', [0,0,0,0.5], 'DisplayName', '$\frac{1}{(m_n + m_s) s^2}$');
+text(2.2, 2e-3, '$\omega_n = \sqrt{\frac{k_n}{m_n + m_s}}$', 'horizontalalignment', 'left');
+text(20, 1e-8, '$\omega_s = \sqrt{\frac{k_s}{m_s}}$', 'horizontalalignment', 'center');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+ldg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+ldg.ItemTokenSize = [20, 1];
+ylim([1e-10, 1e-2])
+
+ax2b = nexttile();
+hold on;
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_rigid_heavy, freqs, 'Hz')))), '-',  'color', colors(1,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_stiff_heavy, freqs, 'Hz')))), '-', 'color', colors(2,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_soft_heavy, freqs, 'Hz')))), '-', 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+xticks([1e0, 1e1, 1e2]);
+yticks(-360:90:360);
+ylim([-200, 20]);
+
+linkaxes([ax2,ax2b],'x');
+xlim([1, 1000]);
+
+%% Stiff Nano-Hexapod
+% Light payload mass
+mn = 15; % Nano-Hexapod mass [kg]
+ms = 1; % Sample Mass [kg]
+kn = 1e8; % Nano-Hexapod (soft) Stiffness [N/m]
+cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+
+% Rigid sample
+G_pz_rigid_light = 1/((mn + ms)*s^2 + cn*s + kn);
+
+% Soft Sample
+ws = 2*pi*20;
+ks = ms * ws^2;
+cs = 2*0.01*sqrt(ms*ks);
+G_pz_soft_light = (ms*s^2 + cs*s + ks)/((mn*s^2 + cn*s + kn)*(ms*s^2 + cs*s + ks) + ms*s^2*(cs*s + ks));
+
+% Stiff Sample
+ws = 2*pi*200;
+ks = ms * ws^2;
+cs = 2*0.01*sqrt(ms*ks);
+G_pz_stiff_light = (ms*s^2 + cs*s + ks)/((mn*s^2 + cn*s + kn)*(ms*s^2 + cs*s + ks) + ms*s^2*(cs*s + ks));
+
+% Heavy payload mass
+mn = 15; % Nano-Hexapod mass [kg]
+ms = 50; % Sample Mass [kg]
+kn = 1e8; % Nano-Hexapod (soft) Stiffness [N/m]
+cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+
+% Rigid sample
+G_pz_rigid_heavy = 1/((mn + ms)*s^2 + cn*s + kn);
+
+% Soft Sample
+ws = 2*pi*20;
+ks = ms * ws^2;
+cs = 2*0.01*sqrt(ms*ks);
+G_pz_soft_heavy = (ms*s^2 + cs*s + ks)/((mn*s^2 + cn*s + kn)*(ms*s^2 + cs*s + ks) + ms*s^2*(cs*s + ks));
+
+% Stiff Sample
+ws = 2*pi*200;
+ks = ms * ws^2;
+cs = 2*0.01*sqrt(ms*ks);
+G_pz_stiff_heavy = (ms*s^2 + cs*s + ks)/((mn*s^2 + cn*s + kn)*(ms*s^2 + cs*s + ks) + ms*s^2*(cs*s + ks));
+
+%% Effect of the payload dynamics on the stiff Nano-Hexapod. Light sample on the right, and heavy sample on the left
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_pz_rigid_light, freqs, 'Hz'))), '-',  'color', colors(1,:));
+plot(freqs, abs(squeeze(freqresp(G_pz_stiff_light, freqs, 'Hz'))), '-', 'color', colors(2,:));
+plot(freqs, abs(squeeze(freqresp(G_pz_soft_light, freqs, 'Hz'))), '-', 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+ylim([1e-10, 1e-6])
+
+ax1b = nexttile();
+hold on;
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_rigid_light, freqs, 'Hz')))), '-',  'color', colors(1,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_stiff_light, freqs, 'Hz')))), '-', 'color', colors(2,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_soft_light, freqs, 'Hz')))), '-', 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+xticks([1e0, 1e1, 1e2]);
+yticks(-360:90:360);
+ylim([-200, 20]);
+
+linkaxes([ax1,ax1b],'x');
+xlim([1, 1000]);
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax2 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_pz_rigid_heavy, freqs, 'Hz'))), '-',  'color', colors(1,:), 'DisplayName', 'Rigid sample');
+plot(freqs, abs(squeeze(freqresp(G_pz_stiff_heavy, freqs, 'Hz'))), '-', 'color', colors(2,:), 'DisplayName', 'Stiff sample: $\omega_s = 200\,Hz$');
+plot(freqs, abs(squeeze(freqresp(G_pz_soft_heavy, freqs, 'Hz'))), '-', 'color', colors(3,:), 'DisplayName', 'Soft sample: $\omega_s = 20\,Hz$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+ylim([1e-10, 1e-6])
+ldg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+ldg.ItemTokenSize = [20, 1];
+
+ax2b = nexttile();
+hold on;
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_rigid_heavy, freqs, 'Hz')))), '-',  'color', colors(1,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_stiff_heavy, freqs, 'Hz')))), '-', 'color', colors(2,:));
+plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_soft_heavy, freqs, 'Hz')))), '-', 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+xticks([1e0, 1e1, 1e2]);
+yticks(-360:90:360);
+ylim([-200, 20]);
+
+linkaxes([ax2,ax2b],'x');
+xlim([1, 1000]);
+
+%% Nano-Hexpod model
+model_config = struct();
+model_config.controller = "open_loop";
+mn = 15; % Nano-Hexapod mass [kg]
+ms = 1; % Sample Mass [kg]
+
+%% Identification
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/controller'],       1, 'openinput');  io_i = io_i + 1; % Actuator Force
+io(io_i) = linio([mdl, '/micro_station/xf'], 1, 'openinput');  io_i = io_i + 1; % Floor Motion
+io(io_i) = linio([mdl, '/micro_station/ft'], 1, 'openinput');  io_i = io_i + 1; % Stage vibrations
+io(io_i) = linio([mdl, '/fs'],               1, 'openinput');  io_i = io_i + 1; % Direct sample forces
+io(io_i) = linio([mdl, '/dL'],               1, 'openoutput'); io_i = io_i + 1; % Relative Motion Sensor
+io(io_i) = linio([mdl, '/fm'],               1, 'openoutput'); io_i = io_i + 1; % Force Sensor
+io(io_i) = linio([mdl, '/vn'] ,              1, 'openoutput'); io_i = io_i + 1; % Geophone
+io(io_i) = linio([mdl, '/d'] ,               1, 'openoutput'); io_i = io_i + 1; % Metrology Output
+io(io_i) = linio([mdl, '/y'] ,               1, 'openoutput'); io_i = io_i + 1; % Sample's position
+
+%% Soft Nano-Hexapod
+% Light payload mass
+kn = 1e4; % Nano-Hexapod (soft) Stiffness [N/m]
+cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+
+% Rigid Sample
+model_config.nhexa = "1dof";
+G_vc_light_rigid = linearize(mdl, io, 0.0);
+G_vc_light_rigid.InputName = {'f', 'xf', 'ft', 'fs'};
+G_vc_light_rigid.OutputName = {'dL', 'fm', 'vn', 'd', 'y'};
+
+% Soft Sample
+model_config.nhexa = "2dof";
+ws = 2*pi*20;
+ks = ms * ws^2;
+cs = 2*0.01*sqrt(ms*ks);
+
+G_vc_light_soft = linearize(mdl, io, 0.0);
+G_vc_light_soft.InputName = {'f', 'xf', 'ft', 'fs'};
+G_vc_light_soft.OutputName = {'dL', 'fm', 'vn', 'd', 'y'};
+
+% Rigid Sample
+model_config.nhexa = "2dof";
+ws = 2*pi*200;
+ks = ms * ws^2;
+cs = 2*0.01*sqrt(ms*ks);
+
+G_vc_light_stiff = linearize(mdl, io, 0.0);
+G_vc_light_stiff.InputName = {'f', 'xf', 'ft', 'fs'};
+G_vc_light_stiff.OutputName = {'dL', 'fm', 'vn', 'd', 'y'};
+
+%% Stiff Nano-Hexapod
+% Light payload mass
+kn = 1e8; % Nano-Hexapod (soft) Stiffness [N/m]
+cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+
+% Rigid Sample
+model_config.nhexa = "1dof";
+G_pz_light_rigid = linearize(mdl, io, 0.0);
+G_pz_light_rigid.InputName = {'f', 'xf', 'ft', 'fs'};
+G_pz_light_rigid.OutputName = {'dL', 'fm', 'vn', 'd', 'y'};
+
+% Soft Sample
+model_config.nhexa = "2dof";
+ws = 2*pi*20;
+ks = ms * ws^2;
+cs = 2*0.01*sqrt(ms*ks);
+
+G_pz_light_soft = linearize(mdl, io, 0.0);
+G_pz_light_soft.InputName = {'f', 'xf', 'ft', 'fs'};
+G_pz_light_soft.OutputName = {'dL', 'fm', 'vn', 'd', 'y'};
+
+% Rigid Sample
+model_config.nhexa = "2dof";
+ws = 2*pi*200;
+ks = ms * ws^2;
+cs = 2*0.01*sqrt(ms*ks);
+
+G_pz_light_stiff = linearize(mdl, io, 0.0);
+G_pz_light_stiff.InputName = {'f', 'xf', 'ft', 'fs'};
+G_pz_light_stiff.OutputName = {'dL', 'fm', 'vn', 'd', 'y'};
+
+%% Apply IFF and verify stability
+% Soft Nano-Hexapod
+G_iff_vc_light_rigid = feedback(G_vc_light_rigid, K_iff_vc, 'name', +1);
+G_iff_vc_light_soft  = feedback(G_vc_light_soft , K_iff_vc, 'name', +1);
+G_iff_vc_light_stiff = feedback(G_vc_light_stiff, K_iff_vc, 'name', +1);
+
+isstable(G_iff_vc_light_rigid)
+isstable(G_iff_vc_light_soft)
+isstable(G_iff_vc_light_stiff)
+
+% Stiff Nano-Hexapod
+G_iff_pz_light_rigid = feedback(G_pz_light_rigid, K_iff_pz, 'name', +1);
+G_iff_pz_light_soft  = feedback(G_pz_light_soft , K_iff_pz, 'name', +1);
+G_iff_pz_light_stiff = feedback(G_pz_light_stiff, K_iff_pz, 'name', +1);
+
+isstable(G_iff_pz_light_rigid)
+isstable(G_iff_pz_light_soft)
+isstable(G_iff_pz_light_stiff)
+
+%% Compute closed-loop plants and verify stability
+% Soft Nano-Hexapod
+G_hac_iff_vc_light_rigid = feedback(G_iff_vc_light_rigid, K_hac_vc, 'name', -1);
+G_hac_iff_vc_light_soft  = feedback(G_iff_vc_light_soft , K_hac_vc, 'name', -1);
+G_hac_iff_vc_light_stiff = feedback(G_iff_vc_light_stiff, K_hac_vc, 'name', -1);
+
+isstable(G_hac_iff_vc_light_rigid)
+isstable(G_hac_iff_vc_light_soft)
+isstable(G_hac_iff_vc_light_stiff)
+
+% Stiff Nano-Hexapod
+G_hac_iff_pz_light_rigid = feedback(G_iff_pz_light_rigid, K_hac_pz, 'name', -1);
+G_hac_iff_pz_light_soft  = feedback(G_iff_pz_light_soft , K_hac_pz, 'name', -1);
+G_hac_iff_pz_light_stiff = feedback(G_iff_pz_light_stiff, K_hac_pz, 'name', -1);
+
+isstable(G_hac_iff_pz_light_rigid)
+isstable(G_hac_iff_pz_light_soft)
+isstable(G_hac_iff_pz_light_stiff)
+
+%% Cumulative Amplitude Spectrum of d - Effect of Sample's flexibility
+figure;
+hold on;
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_light_rigid('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_light_rigid('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+    'DisplayName', 'Rigid sample');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_light_stiff('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_light_stiff('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+    'DisplayName', '$\omega_s = 200\,$Hz');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_light_soft('d', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_light_soft('d', 'xf'), f, 'Hz'))).^2)))), '-', ...
+    'DisplayName', '$\omega_s = 20\,$Hz');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('CAS of $d$ [m]'); xlabel('Frequency [Hz]');
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+xlim([1, 500]);
+xticks([1e0, 1e1, 1e2]);
+ylim([2e-10, 2e-7])
+
+%% Cumulative Amplitude Spectrum - Effect of Sample's flexibility
+figure;
+hold on;
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_light_rigid('y', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_light_rigid('y', 'xf'), f, 'Hz'))).^2)))), '-', ...
+    'DisplayName', 'Rigid sample');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_light_stiff('y', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_light_stiff('y', 'xf'), f, 'Hz'))).^2)))), '-', ...
+    'DisplayName', '$\omega_s = 200\,$Hz');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(psd_ft.*abs(squeeze(freqresp(G_hac_iff_pz_light_soft('y', 'ft'), f, 'Hz'))).^2 + ...
+                                     psd_xf.*abs(squeeze(freqresp(G_hac_iff_pz_light_soft('y', 'xf'), f, 'Hz'))).^2)))), '-', ...
+    'DisplayName', '$\omega_s = 20\,$Hz');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('CAS of $y$ [m]'); xlabel('Frequency [Hz]');
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+xlim([1, 500]);
+xticks([1e0, 1e1, 1e2]);
+ylim([2e-10, 2e-7])

A2-nass-rotating-3dof-model/rotating_1_system_description.m

@@ -0,0 +1,187 @@
+%% 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
+
+%% Colors for the figures
+colors = colororder;
+
+%% Simscape model name
+mdl = 'rotating_model';
+
+%% Model parameters for first analysis
+kn = 1;    % Actuator Stiffness [N/m]
+mn = 1;    % Payload Mass [kg]
+cn = 0.05; % Actuator Damping [N/(m/s)]
+
+xin = cn/(2*sqrt(kn*mn)); % Modal Damping [-]
+w0n = sqrt(kn/mn);       % Natural Frequency [rad/s]
+
+%% Computation of the poles as a function of the rotating velocity
+Wzs = linspace(0, 2, 51); % Vector of rotation speeds [rad/s]
+
+p_ws = zeros(4, length(Wzs));
+
+for i = 1:length(Wzs)
+    Wz = Wzs(i);
+
+    pole_G = pole(1/(((s^2)/(w0n^2) + 2*xin*s/w0n + 1 - (Wz^2)/(w0n^2))^2 + (2*Wz*s/(w0n^2))^2));
+    [~, i_sort] = sort(imag(pole_G));
+    p_ws(:, i) = pole_G(i_sort);
+end
+
+clear pole_G;
+
+%% Campbell diagram - Real and Imaginary parts of the poles as a function of the rotating velocity
+figure;
+hold on;
+plot(Wzs, real(p_ws(1, :)), '-', 'color', colors(1,:), 'DisplayName', '$p_{+}$')
+plot(Wzs, real(p_ws(4, :)), '-', 'color', colors(1,:), 'HandleVisibility', 'off')
+plot(Wzs, real(p_ws(2, :)), '-', 'color', colors(2,:), 'DisplayName', '$p_{-}$')
+plot(Wzs, real(p_ws(3, :)), '-', 'color', colors(2,:), 'HandleVisibility', 'off')
+plot(Wzs, zeros(size(Wzs)), 'k--', 'HandleVisibility', 'off')
+hold off;
+xlabel('Rotational Speed $\Omega$'); ylabel('Real Part');
+leg = legend('location', 'northwest', 'FontSize', 8);
+leg.ItemTokenSize(1) = 8;
+xlim([0, 2*w0n]);
+xticks([0,w0n/2,w0n,3/2*w0n,2*w0n])
+xticklabels({'$0$', '', '$\omega_0$', '', '$2 \omega_0$'})
+ylim([-3*xin, 3*xin]);
+yticks([-3*xin, -2*xin, -xin, 0, xin, 2*xin, 3*xin])
+yticklabels({'', '', '$-\xi\omega_0$', '$0$', ''})
+
+figure
+hold on;
+plot(Wzs, imag(p_ws(1, :)), '-', 'color', colors(1,:))
+plot(Wzs, imag(p_ws(4, :)), '-', 'color', colors(1,:))
+plot(Wzs, imag(p_ws(2, :)), '-', 'color', colors(2,:))
+plot(Wzs, imag(p_ws(3, :)), '-', 'color', colors(2,:))
+plot(Wzs, zeros(size(Wzs)), 'k--')
+hold off;
+xlabel('Rotational Speed $\Omega$'); ylabel('Imaginary Part');
+xlim([0, 2*w0n]);
+xticks([0,w0n/2,w0n,3/2*w0n,2*w0n])
+xticklabels({'$0$', '', '$\omega_0$', '', '$2 \omega_0$'})
+ylim([-3*w0n, 3*w0n]);
+yticks([-3*w0n, -2*w0n, -w0n, 0, w0n, 2*w0n, 3*w0n])
+yticklabels({'', '', '$-\omega_0$', '$0$', '$\omega_0$', '', ''})
+
+%% Identify the dynamics for several rotating velocities
+% Sample
+ms = 0.5; % Sample mass [kg]
+
+% Tuv Stage
+kn = 1; % Stiffness [N/m]
+mn = 0.5; % Tuv mass [kg]
+cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
+
+% General Configuration
+model_config = struct();
+model_config.controller = "open_loop"; % Default: Open-Loop
+model_config.Tuv_type   = "normal";    % Default: 2DoF stage
+
+% Input/Output definition
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/controller'],        1, 'openinput');  io_i = io_i + 1; % [Fu, Fv]
+io(io_i) = linio([mdl, '/fd'],                1, 'openinput');  io_i = io_i + 1; % [Fdu, Fdv]
+io(io_i) = linio([mdl, '/xf'],                1, 'openinput');  io_i = io_i + 1; % [Dfx, Dfy]
+io(io_i) = linio([mdl, '/translation_stage'], 1, 'openoutput'); io_i = io_i + 1; % [Fmu, Fmv]
+io(io_i) = linio([mdl, '/translation_stage'], 2, 'openoutput'); io_i = io_i + 1; % [Du, Dv]
+io(io_i) = linio([mdl, '/ext_metrology'],     1, 'openoutput'); io_i = io_i + 1; % [Dx, Dy]
+
+% Tested rotating velocities [rad/s]
+Wzs = [0, 0.1, 0.2, 0.7, 1.2]; % Vector of rotation speeds [rad/s]
+
+Gs  = {zeros(2, 2, length(Wzs))}; % Direct terms
+
+for i = 1:length(Wzs)
+    Wz = Wzs(i);
+
+    %% Linearize the model
+    G = linearize(mdl, io, 0);
+
+    %% Input/Output definition
+    G.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
+    G.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+    Gs{:,:,i} = G;
+end
+
+% Save All Identified Plants
+save('./mat/rotating_generic_plants.mat', 'Gs', 'Wzs');
+
+%% Bode plot of the direct and coupling terms for several rotating velocities
+freqs = logspace(-1, 1, 1000);
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+% Magnitude
+ax1 = nexttile([2, 1]);
+hold on;
+for i = 1:length(Wzs)
+    plot(freqs, abs(squeeze(freqresp(Gs{i}('du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:))
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
+ylim([1e-2, 1e2]);
+yticks([1e-2,1e-1,1,1e1,1e2])
+yticklabels({'$0.01/k$', '$0.1/k$', '$1/k$', '$10/k$', '$100/k$'})
+
+ax2 = nexttile;
+hold on;
+for i = 1:length(Wzs)
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:))
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
+yticks(-180:90:180);
+ylim([-180 180]);
+xticks([1e-1,1,1e1])
+xticklabels({'$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2, 1]);
+hold on;
+for i = 1:length(Wzs)
+    plot(freqs, abs(squeeze(freqresp(Gs{i}('dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:), ...
+         'DisplayName', sprintf('$\\Omega = %.1f \\omega_0$', Wzs(i)))
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
+ldg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+ldg.ItemTokenSize = [10, 1];
+ylim([1e-2, 1e2]);
+yticks([1e-2,1e-1,1,1e1,1e2])
+yticklabels({'$0.01/k$', '$0.1/k$', '$1/k$', '$10/k$', '$100/k$'})
+
+
+ax2 = nexttile;
+hold on;
+for i = 1:length(Wzs)
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:));
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
+yticks(-180:90:180);
+ylim([-180 180]);
+xticks([1e-1,1,1e1])
+xticklabels({'$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);

A2-nass-rotating-3dof-model/rotating_2_iff_pure_int.m

@@ -0,0 +1,86 @@
+%% 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
+
+%% Colors for the figures
+colors = colororder;
+
+%% Simscape model name
+mdl = 'rotating_model';
+
+%% Load "Generic" system dynamics
+load('rotating_generic_plants.mat', 'Gs', 'Wzs');
+
+%% Bode plot of the direct and coupling term for Integral Force Feedback - Effect of rotation
+freqs = logspace(-2, 1, 1000);
+
+Wz_i = [1,3,4];
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+% Magnitude
+ax1 = nexttile([2, 1]);
+hold on;
+for i = 1:length(Wz_i)
+    plot(freqs, abs(squeeze(freqresp(Gs{Wz_i(i)}('fu', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:), ...
+         'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Wzs(Wz_i(i))),'MarkerSize',8);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
+ylim([1e-3, 1e2]);
+leg = legend('location', 'northwest', 'FontSize', 8);
+
+ax2 = nexttile;
+hold on;
+for i = 1:length(Wz_i)
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{Wz_i(i)}('fu', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:))
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
+yticks(-180:90:180);
+ylim([0 180]);
+xticks([1e-2,1e-1,1,1e1])
+xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);
+
+%% Root Locus for the Decentralized Integral Force Feedback controller
+Kiff = 1/s*eye(2);
+
+gains = logspace(-2, 4, 300);
+Wz_i = [1,3,4];
+
+figure;
+hold on;
+for i = 1:length(Wz_i)
+    plot(real(pole(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)),  imag(pole(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), 'x', 'color', colors(i,:), ...
+         'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Wzs(Wz_i(i))),'MarkerSize',8);
+    plot(real(tzero(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)),  imag(tzero(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), 'o', 'color', colors(i,:), ...
+         'HandleVisibility', 'off','MarkerSize',8);
+    for g = gains
+        cl_poles = pole(feedback(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'}), g*Kiff, -1));
+        plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(i,:), ...
+             'HandleVisibility', 'off','MarkerSize',4);
+    end
+end
+hold off;
+axis square;
+xlim([-1.8, 0.2]); ylim([0, 2]);
+xticks([-1, 0])
+xticklabels({'-$\omega_0$', '$0$'})
+yticks([0, 1, 2])
+yticklabels({'$0$', '$\omega_0$', '$2 \omega_0$'})
+
+xlabel('Real Part'); ylabel('Imaginary Part');
+leg = legend('location', 'northwest', 'FontSize', 8);
+leg.ItemTokenSize(1) = 8;

A2-nass-rotating-3dof-model/rotating_3_iff_hpf.m

@@ -0,0 +1,210 @@
+%% 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
+
+%% Colors for the figures
+colors = colororder;
+
+%% Simscape model name
+mdl = 'rotating_model';
+
+%% Load "Generic" system dynamics
+load('rotating_generic_plants.mat', 'Gs', 'Wzs');
+
+%% Modified IFF - parameters
+g = 2; % Controller gain
+wi = 0.1; % HPF Cut-Off frequency [rad/s]
+
+Kiff     = (g/s)*eye(2); % Pure IFF
+Kiff_hpf = (g/(wi+s))*eye(2); % IFF with added HPF
+
+%% Loop gain for the IFF with pure integrator and modified IFF with added  high-pass filter
+freqs = logspace(-2, 1, 1000);
+Wz_i = 2;
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+% Magnitude
+ax1 = nexttile([2, 1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(Gs{Wz_i}('fu', 'Fu')*Kiff(1,1), freqs, 'rad/s'))))
+plot(freqs, abs(squeeze(freqresp(Gs{Wz_i}('fu', 'Fu')*Kiff_hpf(1,1), freqs, 'rad/s'))))
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Loop Gain');
+
+% Phase
+ax2 = nexttile;
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{Wz_i}('fu', 'Fu')*Kiff(1,1), freqs, 'rad/s'))), 'DisplayName', 'IFF')
+plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{Wz_i}('fu', 'Fu')*Kiff_hpf(1,1), freqs, 'rad/s'))), 'DisplayName', 'IFF,HPF')
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
+yticks(-180:90:180);
+ylim([-90 180]);
+xticks([1e-2,1e-1,1,1e1])
+xticklabels({'', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
+leg = legend('location', 'southwest', 'FontSize', 8);
+leg.ItemTokenSize(1) = 15;
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);
+
+%%  High-Pass Filter Cut-Off Frequency
+wis = [0.01, 0.05, 0.1]; % [rad/s]
+
+%% Root Locus for the initial IFF and the modified IFF
+gains = logspace(-2, 4, 200);
+
+figure;
+hold on;
+for wi_i = 1:length(wis)
+    wi = wis(wi_i);
+    Kiff = (1/(wi+s))*eye(2);
+    L(wi_i) = plot(nan, nan, '.', 'color', colors(wi_i,:), 'DisplayName', sprintf('$\\omega_i = %.2f \\omega_0$', wi));
+    for g = gains
+        clpoles = pole(feedback(Gs{2}({'fu', 'fv'}, {'Fu', 'Fv'}), g*Kiff));
+        plot(real(clpoles), imag(clpoles), '.', 'color', colors(wi_i,:),'MarkerSize',4);
+    end
+    plot(real(pole(Gs{2}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), ...
+         imag(pole(Gs{2}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), ...
+         'x', 'color', colors(wi_i,:),'MarkerSize',8);
+    plot(real(tzero(Gs{2}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), ...
+         imag(tzero(Gs{2}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), ...
+         'o', 'color', colors(wi_i,:),'MarkerSize',8);
+end
+hold off;
+axis equal;
+xlim([-2.3, 0.1]); ylim([-1.2, 1.2]);
+xticks([-2:1:2]); yticks([-2:1:2]);
+leg = legend(L, 'location', 'southwest', 'FontSize', 8);
+leg.ItemTokenSize(1) = 8;
+xlabel('Real Part'); ylabel('Imaginary Part');
+clear L
+
+xlim([-1.25, 0.25]); ylim([-1.25, 1.25]);
+xticks([-1, 0])
+xticklabels({'$-\omega_0$', '$0$'})
+yticks([-1, 0, 1])
+yticklabels({'$-\omega_0$', '$0$', '$\omega_0$'})
+ytickangle(90)
+
+%% Compute the optimal control gain
+wis = logspace(-2, 1, 100); % [rad/s]
+
+opt_xi = zeros(1, length(wis)); % Optimal simultaneous damping
+opt_gain = zeros(1, length(wis)); % Corresponding optimal gain
+
+for wi_i = 1:length(wis)
+    wi = wis(wi_i);
+    Kiff = 1/(s + wi)*eye(2);
+
+    fun = @(g)computeSimultaneousDamping(g, Gs{2}({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff);
+
+    [g_opt, xi_opt] = fminsearch(fun, 0.5*wi*((1/Wzs(2))^2 - 1));
+    opt_xi(wi_i) = 1/xi_opt;
+    opt_gain(wi_i) = g_opt;
+end
+
+%% Attainable damping ratio as a function of wi/w0. Corresponding control gain g_opt and g_max are also shown
+figure;
+yyaxis left
+plot(wis, opt_xi, '-', 'DisplayName', '$\xi_{cl}$');
+set(gca, 'YScale', 'lin');
+ylim([0,1]);
+ylabel('Damping Ratio $\xi$');
+
+yyaxis right
+hold on;
+plot(wis, opt_gain, '-', 'DisplayName', '$g_{opt}$');
+plot(wis, wis*((1/Wzs(2))^2 - 1), '--', 'DisplayName', '$g_{max}$');
+hold off;
+set(gca, 'YScale', 'lin');
+ylim([0,10]);
+ylabel('Controller gain $g$');
+
+xlabel('$\omega_i/\omega_0$');
+set(gca, 'XScale', 'log');
+legend('location', 'northeast', 'FontSize', 8);
+
+%% Compute damped plant
+wis = [0.03, 0.1, 0.5]; % [rad/s]
+g = 2; % Gain
+
+Gs_iff_hpf  = {};
+
+for i = 1:length(wis)
+    Kiff_hpf = (g/(wis(i)+s))*eye(2); % IFF with added HPF
+    Kiff_hpf.InputName = {'fu', 'fv'};
+    Kiff_hpf.OutputName = {'Fu', 'Fv'};
+
+    Gs_iff_hpf(i) = {feedback(Gs{2}, Kiff_hpf, 'name')};
+end
+
+%% Effect of $\omega_i$ on the damped plant coupling
+freqs = logspace(-2, 1, 1000);
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+% Magnitude
+ax1 = nexttile([2, 1]);
+hold on;
+for i = 1:length(wis)
+    plot(freqs, abs(squeeze(freqresp(Gs_iff_hpf{i}('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(i,:)], ...
+         'DisplayName', sprintf('$d_u/F_u$, $\\omega_i = %.2f \\omega_0$', wis(i)))
+    plot(freqs, abs(squeeze(freqresp(Gs_iff_hpf{i}('Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(i,:), 0.5], ...
+         'DisplayName', sprintf('$d_v/F_u$, $\\omega_i = %.2f \\omega_0$', wis(i)))
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
+leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 20;
+
+ax2 = nexttile;
+hold on;
+for i = 1:length(wis)
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_iff_hpf{i}('Du', 'Fu'), freqs, 'rad/s'))), '-')
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
+yticks(-180:90:180);
+ylim([-180 0]);
+xticks([1e-2,1e-1,1,1e1])
+xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);
+
+%% Effect of $\omega_i$ on the obtained compliance
+freqs = logspace(-2, 1, 1000);
+
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+% Magnitude
+ax1 = nexttile();
+hold on;
+for i = 1:length(wis)
+    plot(freqs, abs(squeeze(freqresp(Gs_iff_hpf{i}('Du', 'Fdx'), freqs, 'rad/s'))), '-', 'color', [colors(i,:)], ...
+         'DisplayName', sprintf('$d_{x}/F_{dx}$, $\\omega_i = %.2f \\omega_0$', wis(i)))
+end
+plot(freqs, abs(squeeze(freqresp(Gs{2}('Du', 'Fdx'), freqs, 'rad/s'))), 'k--', ...
+     'DisplayName', '$d_{x}/F_{dx}$, OL')
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [rad/s]'); ylabel('Compliance [m/N]');
+leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 20;
+xticks([1e-2,1e-1,1,1e1])
+xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})

A2-nass-rotating-3dof-model/rotating_4_iff_kp.m

@@ -0,0 +1,339 @@
+%% 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
+
+%% Colors for the figures
+colors = colororder;
+
+%% Simscape model name
+mdl = 'rotating_model';
+
+%% Load "Generic" system dynamics
+load('rotating_generic_plants.mat', 'Gs', 'Wzs');
+
+%% Tuv Stage
+mn = 0.5; % Tuv mass [kg]
+
+%% Sample
+ms = 0.5; % Sample mass [kg]
+
+%% General Configuration
+model_config = struct();
+model_config.controller = "open_loop"; % Default: Open-Loop
+model_config.Tuv_type   = "parallel_k";    % Default: 2DoF stage
+
+%% Input/Output definition
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/controller'],        1, 'openinput');  io_i = io_i + 1; % [Fu, Fv]
+io(io_i) = linio([mdl, '/fd'],                1, 'openinput');  io_i = io_i + 1; % [Fdu, Fdv]
+io(io_i) = linio([mdl, '/translation_stage'], 1, 'openoutput'); io_i = io_i + 1; % [Fmu, Fmv]
+io(io_i) = linio([mdl, '/translation_stage'], 2, 'openoutput'); io_i = io_i + 1; % [Du, Dv]
+io(io_i) = linio([mdl, '/ext_metrology'],     1, 'openoutput'); io_i = io_i + 1; % [Dx, Dy]
+
+Wz = 0.1; % The rotation speed [rad/s]
+
+%% No parallel Stiffness
+kp = 0; % Parallel Stiffness [N/m]
+cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
+kn = 1 - kp; % Stiffness [N/m]
+cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
+
+G_no_kp = linearize(mdl, io, 0);
+G_no_kp.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
+G_no_kp.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+%% Small parallel Stiffness
+kp = 0.5*(mn+ms)*Wz^2; % Parallel Stiffness [N/m]
+cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
+kn = 1 - kp; % Stiffness [N/m]
+cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
+
+G_low_kp = linearize(mdl, io, 0);
+G_low_kp.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
+G_low_kp.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+%% Large parallel Stiffness
+kp = 1.5*(mn+ms)*Wz^2; % Parallel Stiffness [N/m]
+cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
+kn = 1 - kp; % Stiffness [N/m]
+cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
+
+G_high_kp = linearize(mdl, io, 0);
+G_high_kp.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
+G_high_kp.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+%% Effect of the parallel stiffness on the IFF plant
+freqs = logspace(-2, 1, 1000);
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+% Magnitude
+ax1 = nexttile([2, 1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_no_kp(  'fu', 'Fu'), freqs, 'rad/s'))), '-', ...
+     'DisplayName', '$k_p = 0$')
+plot(freqs, abs(squeeze(freqresp(G_low_kp( 'fu', 'Fu'), freqs, 'rad/s'))), '-', ...
+     'DisplayName', '$k_p < m\Omega^2$')
+plot(freqs, abs(squeeze(freqresp(G_high_kp('fu', 'Fu'), freqs, 'rad/s'))), '-', ...
+     'DisplayName', '$k_p > m\Omega^2$')
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
+ylim([1e-4, 5e1]);
+legend('location', 'southeast', 'FontSize', 8);
+
+% Phase
+ax2 = nexttile;
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_no_kp(  'fu', 'Fu'), freqs, 'rad/s'))), '-')
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_low_kp( 'fu', 'Fu'), freqs, 'rad/s'))), '-')
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_high_kp('fu', 'Fu'), freqs, 'rad/s'))), '-')
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
+yticks(-180:90:180);
+ylim([0 180]);
+hold off;
+xticks([1e-2,1e-1,1,1e1])
+xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);
+
+%% Root Locus for IFF without parallel spring, with small parallel spring and with large parallel spring
+gains = logspace(-2, 2, 200);
+
+figure;
+hold on;
+plot(real(pole(G_no_kp({'fu','fv'},{'Fu','Fv'})*(1/s))),  imag(pole(G_no_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'x', 'color', colors(1,:), ...
+     'DisplayName', '$k_p = 0$','MarkerSize',8);
+plot(real(tzero(G_no_kp({'fu','fv'},{'Fu','Fv'})*(1/s))),  imag(tzero(G_no_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'o', 'color', colors(1,:), ...
+     'HandleVisibility', 'off','MarkerSize',8);
+for g = gains
+    cl_poles = pole(feedback(G_no_kp({'fu','fv'},{'Fu','Fv'}), (g/s)*eye(2)));
+    plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(1,:), ...
+         'HandleVisibility', 'off','MarkerSize',4);
+end
+
+plot(real(pole(G_low_kp({'fu','fv'},{'Fu','Fv'})*(1/s))),  imag(pole(G_low_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'x', 'color', colors(2,:), ...
+     'DisplayName', '$k_p < m\Omega^2$','MarkerSize',8);
+plot(real(tzero(G_low_kp({'fu','fv'},{'Fu','Fv'})*(1/s))),  imag(tzero(G_low_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'o', 'color', colors(2,:), ...
+     'HandleVisibility', 'off','MarkerSize',8);
+for g = gains
+    cl_poles = pole(feedback(G_low_kp({'fu','fv'},{'Fu','Fv'}), (g/s)*eye(2)));
+    plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(2,:), ...
+         'HandleVisibility', 'off','MarkerSize',4);
+end
+
+plot(real(pole(G_high_kp({'fu','fv'},{'Fu','Fv'})*(1/s))),  imag(pole(G_high_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'x', 'color', colors(3,:), ...
+     'DisplayName', '$k_p > m\Omega^2$','MarkerSize',8);
+plot(real(tzero(G_high_kp({'fu','fv'},{'Fu','Fv'})*(1/s))),  imag(tzero(G_high_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'o', 'color', colors(3,:), ...
+     'HandleVisibility', 'off','MarkerSize',8);
+for g = gains
+    cl_poles = pole(feedback(G_high_kp({'fu','fv'},{'Fu','Fv'}), (g/s)*eye(2)));
+    plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(3,:), ...
+         'HandleVisibility', 'off','MarkerSize',4);
+end
+hold off;
+axis square;
+xlim([-2.25, 0.25]); ylim([-1.25, 1.25]);
+xticks([-2, -1, 0])
+xticklabels({'$-2\omega_0$', '$-\omega_0$', '$0$'})
+yticks([-1, 0, 1])
+yticklabels({'$-\omega_0$', '$0$', '$\omega_0$'})
+
+xlabel('Real Part'); ylabel('Imaginary Part');
+leg = legend('location', 'northwest', 'FontSize', 8);
+leg.ItemTokenSize(1) = 8;
+
+%% Tested parallel stiffnesses
+kps = [2, 20, 40]*(mn + ms)*Wz^2;
+
+%% Root Locus: Effect of the parallel stiffness on the attainable damping
+gains = logspace(-2, 4, 500);
+
+figure;
+hold on;
+for kp_i = 1:length(kps)
+    kp = kps(kp_i); % Parallel Stiffness [N/m]
+    cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
+    kn = 1 - kp; % Stiffness [N/m]
+    cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
+
+    % Identify dynamics
+    G = linearize(mdl, io, 0);
+    G.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
+    G.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+    plot(real(pole(G({'fu', 'fv'}, {'Fu', 'Fv'})*(1/s*eye(2)))),  imag(pole(G({'fu', 'fv'}, {'Fu', 'Fv'})*(1/s*eye(2)))), 'x', 'color', colors(kp_i,:), ...
+         'DisplayName', sprintf('$k_p = %.1f m \\Omega^2$', kp/((mn+ms)*Wz^2)),'MarkerSize',8);
+    plot(real(tzero(G({'fu', 'fv'}, {'Fu', 'Fv'})*(1/s*eye(2)))),  imag(tzero(G({'fu', 'fv'}, {'Fu', 'Fv'})*(1/s*eye(2)))), 'o', 'color', colors(kp_i,:), ...
+         'HandleVisibility', 'off','MarkerSize',8);
+    for g = gains
+        cl_poles = pole(feedback(G({'fu', 'fv'}, {'Fu', 'Fv'}), (g/s)*eye(2)));
+        plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(kp_i,:),'MarkerSize',4, ...
+             'HandleVisibility', 'off');
+    end
+end
+hold off;
+axis square;
+% xlim([-1.15, 0.05]); ylim([0, 1.2]);
+xlim([-2.25, 0.25]); ylim([-1.25, 1.25]);
+xticks([-2, -1, 0])
+xticklabels({'$-2\omega_0$', '$-\omega_0$', '$0$'})
+yticks([-1, 0, 1])
+yticklabels({'$-\omega_0$', '$0$', '$\omega_0$'})
+
+xlabel('Real Part'); ylabel('Imaginary Part');
+leg = legend('location', 'northwest', 'FontSize', 8);
+leg.ItemTokenSize(1) = 12;
+
+%% Computes the optimal parameters and attainable simultaneous damping
+alphas = logspace(-2, 0, 100);
+alphas(end) = []; % Remove last point
+
+opt_xi = zeros(1, length(alphas)); % Optimal simultaneous damping
+opt_gain = zeros(1, length(alphas)); % Corresponding optimal gain
+
+Kiff = 1/s*eye(2);
+
+for alpha_i = 1:length(alphas)
+    kp = alphas(alpha_i);
+    cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
+    kn = 1 - kp; % Stiffness [N/m]
+    cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
+
+    % Identify dynamics
+    G = linearize(mdl, io, 0);
+    G.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
+    G.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+    fun = @(g)computeSimultaneousDamping(g, G({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff);
+
+    [g_opt, xi_opt] = fminsearch(fun, 2);
+    opt_xi(alpha_i) = 1/xi_opt;
+    opt_gain(alpha_i) = g_opt;
+end
+
+%% Attainable damping as a function of the stiffness ratio
+figure;
+yyaxis left
+plot(alphas, opt_xi, '-', 'DisplayName', '$\xi_{cl}$');
+set(gca, 'YScale', 'lin');
+ylim([0,1]);
+ylabel('Damping Ratio $\xi$');
+
+yyaxis right
+hold on;
+plot(alphas, opt_gain, '-', 'DisplayName', '$g_{opt}$');
+set(gca, 'YScale', 'lin');
+ylim([0,2.5]);
+ylabel('Controller gain $g$');
+
+set(gca, 'XScale', 'log');
+legend('location', 'northeast', 'FontSize', 8);
+
+xlabel('$k_p$');
+xlim([0.01, 1]);
+xticks([0.01, 0.1, 1])
+xticklabels({'$m\Omega^2$', '$10m\Omega^2$', '$100m\Omega^2$'})
+
+%% Identify dynamics with parallel stiffness = 2mW^2
+Wz = 0.1; % [rad/s]
+kp = 2*(mn + ms)*Wz^2; % Parallel Stiffness [N/m]
+cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
+kn = 1 - kp; % Stiffness [N/m]
+cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
+
+% Identify dynamics
+G = linearize(mdl, io, 0);
+G.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
+G.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+%% IFF controller with pure integrator
+Kiff_kp = (2.2/s)*eye(2);
+Kiff_kp.InputName = {'fu', 'fv'};
+Kiff_kp.OutputName = {'Fu', 'Fv'};
+
+%% Compute the damped plant
+G_cl_iff_kp = feedback(G, Kiff_kp, 'name');
+
+w0 = sqrt((kn+kp)/(mn+ms)); % Resonance frequency [rad/s]
+wis = w0*logspace(-2, 0, 100); % LPF cut-off [rad/s]
+
+%% Computes the obtained damping as a function of the HPF cut-off frequency
+opt_xi = zeros(1, length(wis)); % Optimal simultaneous damping
+
+for wi_i = 1:length(wis)
+    Kiff_kp_hpf = (2.2/(s + wis(wi_i)))*eye(2);
+    Kiff_kp_hpf.InputName = {'fu', 'fv'};
+    Kiff_kp_hpf.OutputName = {'Fu', 'Fv'};
+
+    [~, xi] = damp(feedback(G, Kiff_kp_hpf, 'name'));
+    opt_xi(wi_i) = min(xi);
+end
+
+%% Effect of the  high-pass filter cut-off frequency on the obtained damping
+figure;
+plot(wis, opt_xi, '-');
+set(gca, 'XScale', 'log');
+set(gca, 'YScale', 'lin');
+ylim([0,1]);
+ylabel('Damping Ratio $\xi$');
+xlabel('$\omega_i/\omega_0$');
+
+%% Compute the damped plant with added  High-Pass Filter
+Kiff_kp_hpf = (2.2/(s + 0.1*w0))*eye(2);
+Kiff_kp_hpf.InputName = {'fu', 'fv'};
+Kiff_kp_hpf.OutputName = {'Fu', 'Fv'};
+
+G_cl_iff_hpf_kp = feedback(G, Kiff_kp_hpf, 'name');
+
+%% Bode plot of the direct and coupling terms for several rotating velocities
+freqs = logspace(-3, 1, 1000);
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+% Magnitude
+ax1 = nexttile([2, 1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G(              'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros( 1,3), ...
+    'DisplayName', '$d_u/F_u$ - OL')
+plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp(    'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:), ...
+    'DisplayName', '$d_u/F_u$ - IFF + $k_p$')
+plot(freqs, abs(squeeze(freqresp(G_cl_iff_hpf_kp('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(2,:), ...
+    'DisplayName', '$d_u/F_u$ - IFF + $k_p$ + HPF')
+plot(freqs, abs(squeeze(freqresp(G(              'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [zeros( 1,3), 0.5], ...
+    'HandleVisibility', 'off')
+plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp(    'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(1,:), 0.5], ...
+    'HandleVisibility', 'off')
+plot(freqs, abs(squeeze(freqresp(G_cl_iff_hpf_kp('Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(2,:), 0.5], ...
+    'HandleVisibility', 'off')
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
+ldg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+ldg.ItemTokenSize(1) = 10;
+
+ax2 = nexttile;
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G(              'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros( 1,3))
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_iff_kp(    'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:))
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_iff_hpf_kp('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(2,:))
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
+yticks(-180:90:180);
+ylim([-180 90]);
+xticks([1e-3,1e-2,1e-1,1,1e1])
+xticklabels({'$0.001 \omega_0$', '$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);

A2-nass-rotating-3dof-model/rotating_5_rdc.m

@@ -0,0 +1,96 @@
+%% 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
+
+%% Colors for the figures
+colors = colororder;
+
+%% Simscape model name
+mdl = 'rotating_model';
+
+%% Load "Generic" system dynamics
+load('rotating_generic_plants.mat', 'Gs', 'Wzs');
+
+%% Root Locus for Relative Damping Control
+Krdc = s*eye(2); % Relative damping controller
+
+gains = logspace(-2, 2, 300); % Tested gains
+Wz_i = [1,3,4];
+
+figure;
+hold on;
+for i = 1:length(Wz_i)
+    plot(real(pole(Gs{Wz_i(i)}({'du', 'dv'}, {'Fu', 'Fv'})*Krdc)),  imag(pole(Gs{Wz_i(i)}({'du', 'dv'}, {'Fu', 'Fv'})*Krdc)), 'x', 'color', colors(i,:), ...
+         'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Wzs(Wz_i(i))),'MarkerSize',8);
+    plot(real(tzero(Gs{Wz_i(i)}({'du', 'dv'}, {'Fu', 'Fv'})*Krdc)),  imag(tzero(Gs{Wz_i(i)}({'du', 'dv'}, {'Fu', 'Fv'})*Krdc)), 'o', 'color', colors(i,:), ...
+         'HandleVisibility', 'off','MarkerSize',8);
+    for g = gains
+        cl_poles = pole(feedback(Gs{Wz_i(i)}({'du', 'dv'}, {'Fu', 'Fv'}), g*Krdc, -1));
+        plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(i,:), ...
+             'HandleVisibility', 'off','MarkerSize',4);
+    end
+end
+hold off;
+axis square;
+xlim([-1.8, 0.2]); ylim([0, 2]);
+xticks([-1, 0])
+xticklabels({'-$\omega_0$', '$0$'})
+yticks([0, 1, 2])
+yticklabels({'$0$', '$\omega_0$', '$2 \omega_0$'})
+
+xlabel('Real Part'); ylabel('Imaginary Part');
+leg = legend('location', 'northwest', 'FontSize', 8);
+leg.ItemTokenSize(1) = 8;
+
+i = 2;
+
+%% Relative Damping Controller
+Krdc = 2*s*eye(2);
+Krdc.InputName = {'Du', 'Dv'};
+Krdc.OutputName = {'Fu', 'Fv'};
+
+%% Compute the damped plant
+G_cl_rdc = feedback(Gs{i}, Krdc, 'name');
+
+%% Damped plant using Relative Damping Control
+freqs = logspace(-3, 2, 1000);
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+% Magnitude
+ax1 = nexttile([2, 1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(Gs{i}(   'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros(1,3), ...
+    'DisplayName', 'OL - $G_d(1,1)$')
+plot(freqs, abs(squeeze(freqresp(G_cl_rdc('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:), ...
+    'DisplayName', 'RDC - $G_d(1,1)$')
+plot(freqs, abs(squeeze(freqresp(Gs{i}(   'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [zeros(1,3), 0.5], ...
+    'DisplayName', 'OL - $G_d(2,1)$')
+plot(freqs, abs(squeeze(freqresp(G_cl_rdc('Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(1,:), 0.5], ...
+    'DisplayName', 'RDC - $G_d(2,1)$')
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
+ldg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
+ldg.ItemTokenSize(1) = 20;
+ylim([1e-4, 1e2]);
+
+ax2 = nexttile;
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros(1,3))
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_rdc('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:))
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
+yticks(-180:90:180);
+ylim([-180 0]);
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);

A2-nass-rotating-3dof-model/rotating_6_act_damp_comparison.m

@@ -0,0 +1,213 @@
+%% 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
+
+%% Colors for the figures
+colors = colororder;
+
+%% Simscape model name
+mdl = 'rotating_model';
+
+%% The rotating speed is set to $\Omega = 0.1 \omega_0$.
+Wz = 0.1; % [rad/s]
+
+%% Masses
+ms = 0.5; % Sample mass [kg]
+mn = 0.5; % Tuv mass [kg]
+
+%% General Configuration
+model_config = struct();
+model_config.controller = "open_loop"; % Default: Open-Loop
+
+%% Input/Output definition
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/controller'],        1, 'openinput');  io_i = io_i + 1; % [Fu, Fv]
+io(io_i) = linio([mdl, '/fd'],                1, 'openinput');  io_i = io_i + 1; % [Fdu, Fdv]
+io(io_i) = linio([mdl, '/xf'],                1, 'openinput');  io_i = io_i + 1; % [Dfx, Dfy]
+io(io_i) = linio([mdl, '/translation_stage'], 1, 'openoutput'); io_i = io_i + 1; % [Fmu, Fmv]
+io(io_i) = linio([mdl, '/translation_stage'], 2, 'openoutput'); io_i = io_i + 1; % [Du, Dv]
+io(io_i) = linio([mdl, '/ext_metrology'],     1, 'openoutput'); io_i = io_i + 1; % [Dx, Dy]
+
+%% Identifying plant with parallel stiffness
+model_config.Tuv_type   = "parallel_k";
+
+% Parallel stiffness
+kp = 2*(mn+ms)*Wz^2; % Parallel Stiffness [N/m]
+cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
+
+% Tuv Stage
+kn = 1 - kp; % Stiffness [N/m]
+cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
+
+% Linearize
+G_kp = linearize(mdl, io, 0);
+G_kp.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
+G_kp.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+%% Identifying plant with no parallel stiffness
+model_config.Tuv_type   = "normal";
+
+% Tuv Stage
+kn = 1; % Stiffness [N/m]
+cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]
+
+% Linearize
+G = linearize(mdl, io, 0);
+G.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
+G.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+%% IFF Controller
+Kiff = (2.2/(s + 0.1))*eye(2);
+Kiff.InputName = {'fu', 'fv'};
+Kiff.OutputName = {'Fu', 'Fv'};
+
+%% IFF Controller with added stiffness
+Kiff_kp = (2.2/(s + 0.1))*eye(2);
+Kiff_kp.InputName = {'fu', 'fv'};
+Kiff_kp.OutputName = {'Fu', 'Fv'};
+
+%% Relative Damping Controller
+Krdc = 2*s*eye(2);
+Krdc.InputName = {'Du', 'Dv'};
+Krdc.OutputName = {'Fu', 'Fv'};
+
+%% Comparison of active damping techniques for rotating platform - Root Locus
+gains = logspace(-2, 2, 500);
+
+figure;
+hold on;
+% IFF
+plot(real(pole(G({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)),  imag(pole(G({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), 'x', 'color', colors(1,:), ...
+     'DisplayName', 'IFF with HPF', 'MarkerSize', 8);
+plot(real(tzero(G({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)),  imag(tzero(G({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), 'o', 'color', colors(1,:), ...
+     'HandleVisibility', 'off', 'MarkerSize', 8);
+for g = gains
+    cl_poles = pole(feedback(G({'fu', 'fv'}, {'Fu', 'Fv'}), g*Kiff));
+    plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(1,:),'MarkerSize',4, ...
+         'HandleVisibility', 'off');
+end
+% IFF with parallel stiffness
+plot(real(pole(G_kp({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff_kp)),  imag(pole(G_kp({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff_kp)), 'x', 'color', colors(2,:), ...
+     'DisplayName', 'IFF with $k_p$', 'MarkerSize', 8);
+plot(real(tzero(G_kp({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff_kp)),  imag(tzero(G_kp({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff_kp)), 'o', 'color', colors(2,:), ...
+     'HandleVisibility', 'off', 'MarkerSize', 8);
+for g = gains
+    cl_poles = pole(feedback(G_kp({'fu', 'fv'}, {'Fu', 'Fv'}), g*Kiff_kp));
+    plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(2,:),'MarkerSize',4, ...
+         'HandleVisibility', 'off');
+end
+% RDC
+plot(real(pole(G({'Du', 'Dv'}, {'Fu', 'Fv'})*Krdc)),  imag(pole(G({'Du', 'Dv'}, {'Fu', 'Fv'})*Krdc)), 'x', 'color', colors(3,:), ...
+     'DisplayName', 'RDC', 'MarkerSize', 8);
+plot(real(tzero(G({'Du', 'Dv'}, {'Fu', 'Fv'})*Krdc)),  imag(tzero(G({'Du', 'Dv'}, {'Fu', 'Fv'})*Krdc)), 'o', 'color', colors(3,:), ...
+     'HandleVisibility', 'off', 'MarkerSize', 8);
+for g = gains
+    cl_poles = pole(feedback(G({'Du', 'Dv'}, {'Fu', 'Fv'}), g*Krdc));
+    plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(3,:),'MarkerSize',4, ...
+         'HandleVisibility', 'off');
+end
+hold off;
+axis square;
+xlim([-1.15, 0.05]); ylim([0, 1.2]);
+
+xlabel('Real Part'); ylabel('Imaginary Part');
+leg = legend('location', 'northwest', 'FontSize', 8);
+leg.ItemTokenSize(1) = 12;
+
+%% Compute Damped plants
+G_cl_iff    = feedback(G,    Kiff,    'name');
+G_cl_iff_kp = feedback(G_kp, Kiff_kp, 'name');
+G_cl_rdc    = feedback(G,    Krdc,    'name');
+
+%% Comparison of the damped plants obtained with the three active damping techniques
+freqs = logspace(-2, 2, 1000);
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+% Magnitude
+ax1 = nexttile([2, 1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G(          'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros(1,3), ...
+    'DisplayName', 'OL')
+plot(freqs, abs(squeeze(freqresp(G_cl_iff(   'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:), ...
+    'DisplayName', 'IFF + HPF')
+plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(2,:), ...
+    'DisplayName', 'IFF + $k_p$')
+plot(freqs, abs(squeeze(freqresp(G_cl_rdc(   'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(3,:), ...
+    'DisplayName', 'RDC')
+plot(freqs, abs(squeeze(freqresp(G(          'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [zeros(1,3), 0.5], ...
+    'DisplayName', 'Coupling')
+plot(freqs, abs(squeeze(freqresp(G_cl_iff(   'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(1,:), 0.5], ...
+    'DisplayName', 'Coupling')
+plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp('Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(2,:), 0.5], ...
+    'DisplayName', 'Coupling')
+plot(freqs, abs(squeeze(freqresp(G_cl_rdc(   'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(3,:), 0.5], ...
+    'DisplayName', 'Coupling')
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
+ldg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
+ldg.ItemTokenSize = [10, 1];
+ylim([1e-6, 1e2])
+
+ax2 = nexttile;
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G(          'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros(1,3))
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_iff(   'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:))
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_iff_kp('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(2,:))
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_rdc(   'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(3,:))
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
+yticks(-180:90:180);
+ylim([-180 15]);
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);
+xticks([1e-2,1e-1,1,1e1,1e2])
+xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$', '$100 \omega_0$'})
+
+%% Comparison of the obtained transmissibility and compliance for the three tested active damping techniques
+freqs = logspace(-2, 2, 1000);
+
+% transmissibility
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G(          'Dx', 'Dfx'), freqs, 'rad/s'))), '-', 'color', zeros(1,3), ...
+    'DisplayName', 'OL')
+plot(freqs, abs(squeeze(freqresp(G_cl_iff(   'Dx', 'Dfx'), freqs, 'rad/s'))), '-', 'color', colors(1,:), ...
+    'DisplayName', 'IFF + HPF')
+plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp('Dx', 'Dfx'), freqs, 'rad/s'))), '-', 'color', colors(2,:), ...
+    'DisplayName', 'IFF + $k_p$')
+plot(freqs, abs(squeeze(freqresp(G_cl_rdc(   'Dx', 'Dfx'), freqs, 'rad/s'))), '-', 'color', colors(3,:), ...
+    'DisplayName', 'RDC')
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [rad/s]'); ylabel('Transmissibility [m/m]');
+xlim([freqs(1), freqs(end)]);
+
+% Compliance
+figure;
+ax1 = nexttile();
+hold on;
+plot(freqs, abs(squeeze(freqresp(G(          'Dx', 'Fdx'), freqs, 'rad/s'))), '-', 'color', zeros(1,3), ...
+    'DisplayName', 'OL')
+plot(freqs, abs(squeeze(freqresp(G_cl_iff(   'Dx', 'Fdx'), freqs, 'rad/s'))), '-', 'color', colors(1,:), ...
+    'DisplayName', 'IFF + HPF')
+plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp('Dx', 'Fdx'), freqs, 'rad/s'))), '-', 'color', colors(2,:), ...
+    'DisplayName', 'IFF + $k_p$')
+plot(freqs, abs(squeeze(freqresp(G_cl_rdc(   'Dx', 'Fdx'), freqs, 'rad/s'))), '-', 'color', colors(3,:), ...
+    'DisplayName', 'RDC')
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [rad/s]'); ylabel('Compliance [m/N]');
+ldg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
+ldg.ItemTokenSize = [10, 1];
+xlim([freqs(1), freqs(end)]);

A2-nass-rotating-3dof-model/rotating_7_nano_hexapod.m

@@ -0,0 +1,682 @@
+%% 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
+
+%% Colors for the figures
+colors = colororder;
+
+%% Simscape model name
+mdl = 'rotating_model';
+
+%% Nano-Hexapod
+mn = 15; % Nano-Hexapod mass [kg]
+
+%% Light Sample
+ms = 1; % Sample Mass [kg]
+
+%% General Configuration
+model_config = struct();
+model_config.controller = "open_loop"; % Default: Open-Loop
+model_config.Tuv_type   = "normal";    % Default: 2DoF stage
+
+%% Input/Output definition
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/controller'],        1, 'openinput');  io_i = io_i + 1; % [Fu, Fv]
+io(io_i) = linio([mdl, '/fd'],                1, 'openinput');  io_i = io_i + 1; % [Fdx, Fdy]
+io(io_i) = linio([mdl, '/xf'],                1, 'openinput');  io_i = io_i + 1; % [Dfx, Dfy]
+io(io_i) = linio([mdl, '/translation_stage'], 1, 'openoutput'); io_i = io_i + 1; % [Fmu, Fmv]
+io(io_i) = linio([mdl, '/translation_stage'], 2, 'openoutput'); io_i = io_i + 1; % [Du, Dv]
+io(io_i) = linio([mdl, '/ext_metrology'],     1, 'openoutput'); io_i = io_i + 1; % [Dx, Dy]
+
+%% Voice Coil (i.e. soft) Nano-Hexapod
+kn = 1e4; % Nano-Hexapod Stiffness [N/m]
+cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+
+Wz = 0; % Rotating Velocity [rad/s]
+G_vc_norot = linearize(mdl, io, 0.0);
+G_vc_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
+G_vc_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+Wz = 2*pi; % Rotating Velocity [rad/s]
+G_vc_fast = linearize(mdl, io, 0.0);
+G_vc_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
+G_vc_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+%% APA (i.e. relatively stiff) Nano-Hexapod
+kn = 1e6; % Nano-Hexapod Stiffness [N/m]
+cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+
+Wz = 0; % Rotating Velocity [rad/s]
+G_md_norot = linearize(mdl, io, 0.0);
+G_md_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
+G_md_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+Wz = 2*pi; % Rotating Velocity [rad/s]
+G_md_fast = linearize(mdl, io, 0.0);
+G_md_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
+G_md_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+%% Piezoelectric (i.e. stiff) Nano-Hexapod
+kn = 1e8; % Nano-Hexapod Stiffness [N/m]
+cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+
+Wz = 0; % Rotating Velocity [rad/s]
+G_pz_norot = linearize(mdl, io, 0.0);
+G_pz_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
+G_pz_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+Wz = 2*pi; % Rotating Velocity [rad/s]
+G_pz_fast = linearize(mdl, io, 0.0);
+G_pz_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
+G_pz_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+%% Compute negative spring in [N/m]
+Kneg_light = (15+1)*(2*pi)^2;
+
+%% Effect of rotation on the nano-hexapod dynamics
+freqs = logspace(0, 1, 1000);
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_vc_norot('Du', 'Fu'), freqs, 'Hz'))), '--',  'color', colors(1,:), ...
+    'DisplayName', '$\Omega = 0\,$rpm, $D_u/F_u$');
+plot(freqs, abs(squeeze(freqresp(G_vc_fast( 'Du', 'Fu'), freqs, 'Hz'))), '-' ,  'color', colors(1,:), ...
+    'DisplayName', '$\Omega = 60\,$rpm, $D_u/F_u$');
+plot(freqs, abs(squeeze(freqresp(G_vc_fast( 'Dv', 'Fu'), freqs, 'Hz'))), '-' , 'color', [colors(1,:), 0.5], ...
+    'DisplayName', '$\Omega = 60\,$rpm, $D_v/F_u$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
+ylim([1e-6, 1e-2])
+
+ax2 = nexttile;
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_vc_norot('Du', 'Fu'), freqs, 'Hz'))), '--', 'color', colors(1,:));
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_vc_fast( 'Du', 'Fu'), freqs, '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, 0]);
+
+linkaxes([ax1,ax2],'x');
+% xlim([1, 1e3]);
+
+%% Effect of rotation on the nano-hexapod dynamics
+freqs = logspace(1, 2, 1000);
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_md_norot('Du', 'Fu'), freqs, 'Hz'))), '--',  'color', colors(2,:), ...
+    'DisplayName', '$\Omega = 0\,$rpm, $D_u/F_u$');
+plot(freqs, abs(squeeze(freqresp(G_md_fast( 'Du', 'Fu'), freqs, 'Hz'))), '-' ,  'color', colors(2,:), ...
+    'DisplayName', '$\Omega = 60\,$rpm, $D_u/F_u$');
+plot(freqs, abs(squeeze(freqresp(G_md_fast( 'Dv', 'Fu'), freqs, 'Hz'))), '-' , 'color', [colors(2,:), 0.5], ...
+    'DisplayName', '$\Omega = 60\,$rpm, $D_v/F_u$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 1e-4])
+
+ax2 = nexttile;
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_md_norot('Du', 'Fu'), freqs, 'Hz'))), '--', 'color', colors(2,:));
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_md_fast( 'Du', 'Fu'), freqs, 'Hz'))), '-' , 'color', colors(2,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 0]);
+
+linkaxes([ax1,ax2],'x');
+
+%% Effect of rotation on the nano-hexapod dynamics
+freqs = logspace(2, 3, 1000);
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_pz_norot('Du', 'Fu'), freqs, 'Hz'))), '--',  'color', colors(3,:), ...
+    'DisplayName', '$\Omega = 0\,$rpm, $D_u/F_u$');
+plot(freqs, abs(squeeze(freqresp(G_pz_fast( 'Du', 'Fu'), freqs, 'Hz'))), '-' ,  'color', colors(3,:), ...
+    'DisplayName', '$\Omega = 60\,$rpm, $D_u/F_u$');
+plot(freqs, abs(squeeze(freqresp(G_pz_fast( 'Dv', 'Fu'), freqs, 'Hz'))), '-' , 'color', [colors(3,:), 0.5], ...
+    'DisplayName', '$\Omega = 60\,$rpm, $D_v/F_u$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
+ylim([1e-10, 1e-6])
+
+ax2 = nexttile;
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz_norot('Du', 'Fu'), freqs, 'Hz'))), '--', 'color', colors(3,:));
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz_fast( 'Du', 'Fu'), freqs, 'Hz'))), '-' , 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 0]);
+
+linkaxes([ax1,ax2],'x');
+
+%% Compute the optimal control gain
+wis = logspace(-2, 3, 200); % [rad/s]
+
+opt_iff_hpf_xi_vc = zeros(1, length(wis)); % Optimal simultaneous damping
+opt_iff_hpf_gain_vc = zeros(1, length(wis)); % Corresponding optimal gain
+
+opt_iff_hpf_xi_md = zeros(1, length(wis)); % Optimal simultaneous damping
+opt_iff_hpf_gain_md = zeros(1, length(wis)); % Corresponding optimal gain
+
+opt_iff_hpf_xi_pz = zeros(1, length(wis)); % Optimal simultaneous damping
+opt_iff_hpf_gain_pz = zeros(1, length(wis)); % Corresponding optimal gain
+
+for wi_i = 1:length(wis)
+    wi = wis(wi_i);
+    Kiff = 1/(s + wi)*eye(2);
+
+    fun = @(g)computeSimultaneousDamping(g, G_vc_fast({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff);
+
+    [g_opt, xi_opt] = fminsearch(fun, 0.1);
+    opt_iff_hpf_xi_vc(wi_i) = 1/xi_opt;
+    opt_iff_hpf_gain_vc(wi_i) = g_opt;
+
+    fun = @(g)computeSimultaneousDamping(g, G_md_fast({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff);
+
+    [g_opt, xi_opt] = fminsearch(fun, 0.1);
+    opt_iff_hpf_xi_md(wi_i) = 1/xi_opt;
+    opt_iff_hpf_gain_md(wi_i) = g_opt;
+
+    fun = @(g)computeSimultaneousDamping(g, G_pz_fast({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff);
+
+    [g_opt, xi_opt] = fminsearch(fun, 0.1);
+    opt_iff_hpf_xi_pz(wi_i) = 1/xi_opt;
+    opt_iff_hpf_gain_pz(wi_i) = g_opt;
+end
+
+%% Find optimal parameters with at least a gain margin of 2
+i_iff_hpf_vc = find(opt_iff_hpf_gain_vc < 0.5*(wis*((sqrt(1e4/16)/(2*pi))^2 - 1)));
+i_iff_hpf_vc = i_iff_hpf_vc(1);
+
+i_iff_hpf_md = find(opt_iff_hpf_xi_md > 0.95*max(opt_iff_hpf_xi_md));
+i_iff_hpf_md = i_iff_hpf_md(end)+1;
+
+i_iff_hpf_pz = find(opt_iff_hpf_xi_pz > 0.95*max(opt_iff_hpf_xi_pz));
+i_iff_hpf_pz = i_iff_hpf_pz(end)+1;
+
+%% Optimal modified IFF parameters that yields maximum simultaneous damping
+figure;
+yyaxis left
+hold on;
+plot(wis, opt_iff_hpf_xi_vc, '-', 'DisplayName', '$\xi_{cl}$');
+plot(wis(i_iff_hpf_vc), opt_iff_hpf_xi_vc(i_iff_hpf_vc), '.', 'MarkerSize', 15, 'HandleVisibility', 'off');
+hold off;
+set(gca, 'YScale', 'lin');
+ylim([0,1]);
+ylabel('Damping Ratio $\xi$');
+
+yyaxis right
+hold on;
+plot(wis, opt_iff_hpf_gain_vc, '-', 'DisplayName', '$g_{opt}$');
+plot(wis, wis*((sqrt(1e4/16)/(2*pi))^2 - 1), '--', 'DisplayName', '$g_{max}$');
+plot(wis(i_iff_hpf_vc), opt_iff_hpf_gain_vc(i_iff_hpf_vc), '.', 'MarkerSize', 15, 'HandleVisibility', 'off');
+set(gca, 'YScale', 'lin');
+ylim([0,200]);
+xlabel('$\omega_i$ [rad/s]');
+set(gca, 'YTickLabel',[]);
+ylabel('Controller gain $g$');
+set(gca, 'XScale', 'log');
+xticks([1e-2,1,1e2])
+legend('location', 'northwest', 'FontSize', 8);
+
+figure;
+yyaxis left
+hold on;
+plot(wis, opt_iff_hpf_xi_md, '-');
+plot(wis(i_iff_hpf_md), opt_iff_hpf_xi_md(i_iff_hpf_md), '.', 'MarkerSize', 15, 'HandleVisibility', 'off');
+hold off;
+set(gca, 'YScale', 'lin');
+ylim([0,1]);
+ylabel('Damping Ratio $\xi$');
+
+yyaxis right
+hold on;
+plot(wis, opt_iff_hpf_gain_md, '-');
+plot(wis(i_iff_hpf_md), opt_iff_hpf_gain_md(i_iff_hpf_md), '.', 'MarkerSize', 15, 'HandleVisibility', 'off');
+plot(wis, wis*((sqrt(1e6/16)/(2*pi))^2 - 1), '--');
+set(gca, 'YScale', 'lin');
+ylim([0,1000]);
+xlabel('$\omega_i$ [rad/s]');
+ylabel('Controller gain $g$');
+set(gca, 'YTickLabel',[]);
+set(gca, 'XScale', 'log');
+xticks([1e-2,1,1e2])
+
+figure;
+yyaxis left
+hold on;
+plot(wis, opt_iff_hpf_xi_pz, '-');
+plot(wis(i_iff_hpf_pz), opt_iff_hpf_xi_pz(i_iff_hpf_pz), '.', 'MarkerSize', 15, 'HandleVisibility', 'off');
+hold off;
+set(gca, 'YScale', 'lin');
+ylim([0,1]);
+ylabel('Damping Ratio $\xi$');
+
+yyaxis right
+hold on;
+plot(wis, opt_iff_hpf_gain_pz, '-');
+plot(wis(i_iff_hpf_pz), opt_iff_hpf_gain_pz(i_iff_hpf_pz), '.', 'MarkerSize', 15, 'HandleVisibility', 'off');
+plot(wis, wis*((sqrt(1e8/16)/(2*pi))^2 - 1), '--');
+set(gca, 'YScale', 'lin');
+ylim([0,10000]);
+xlabel('$\omega_i$ [rad/s]');
+set(gca, 'YTickLabel',[]);
+ylabel('Controller gain $g$');
+set(gca, 'XScale', 'log');
+xticks([1e-2,1,1e2])
+
+%% Maximum rotating velocity
+Wz = 2*pi; % [rad/s]
+
+%% Minimum parallel stiffness
+kp_min = (mn + ms) * Wz^2; % [N/m]
+
+%% Parameters for simulation
+mn = 15; % Nano-Hexapod mass [kg]
+ms = 1; % Sample Mass [kg]
+
+%% IFF Controller
+Kiff_vc = 1/(s + 0.1*sqrt(1e4/(mn+ms)))*eye(2); % IFF
+Kiff_md = 1/(s + 0.1*sqrt(1e6/(mn+ms)))*eye(2); % IFF
+Kiff_pz = 1/(s + 0.1*sqrt(1e8/(mn+ms)))*eye(2); % IFF
+
+%% General Configuration
+model_config = struct();
+model_config.controller = "open_loop"; % Default: Open-Loop
+model_config.Tuv_type   = "parallel_k";    % Default: 2DoF stage
+
+%% Computes the optimal parameters and attainable simultaneous damping - Voice Coil nano-hexapod
+kps_vc = logspace(log10(kp_min), log10(1e4), 100); % Tested parallel stiffnesses [N/m]
+kps_vc(end) = [];
+
+opt_iff_kp_xi_vc   = zeros(1, length(kps_vc)); % Optimal simultaneous damping
+opt_iff_kp_gain_vc = zeros(1, length(kps_vc)); % Corresponding optimal gain
+
+for kp_i = 1:length(kps_vc)
+    % Voice Coil Nano-Hexapod
+    kp = kps_vc(kp_i);
+    cp = 2*0.001*sqrt((ms + mn)*kp);
+    kn = 1e4 - kp; % Nano-Hexapod Stiffness [N/m]
+    cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+
+    % Identify dynamics
+    Giff_vc = linearize(mdl, io, 0);
+    Giff_vc.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
+    Giff_vc.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+    fun = @(g)computeSimultaneousDamping(g, Giff_vc({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff_vc);
+
+    [g_opt, xi_opt] = fminsearch(fun, 0.1);
+    opt_iff_kp_xi_vc(kp_i) = 1/xi_opt;
+    opt_iff_kp_gain_vc(kp_i) = g_opt;
+end
+
+%% Computes the optimal parameters and attainable simultaneous damping - APA nano-hexapod
+kps_md = logspace(log10(kp_min), log10(1e6), 100); % Tested parallel stiffnesses [N/m]
+kps_md(end) = [];
+
+opt_iff_kp_xi_md   = zeros(1, length(kps_md)); % Optimal simultaneous damping
+opt_iff_kp_gain_md = zeros(1, length(kps_md)); % Corresponding optimal gain
+
+
+for kp_i = 1:length(kps_md)
+    % Voice Coil Nano-Hexapod
+    kp = kps_md(kp_i);
+    cp = 2*0.001*sqrt((ms + mn)*kp);
+    kn = 1e6 - kp; % Nano-Hexapod Stiffness [N/m]
+    cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+
+    % Identify dynamics
+    Giff_md = linearize(mdl, io, 0);
+    Giff_md.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
+    Giff_md.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+    fun = @(g)computeSimultaneousDamping(g, Giff_md({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff_md);
+
+    [g_opt, xi_opt] = fminsearch(fun, 0.1);
+    opt_iff_kp_xi_md(kp_i) = 1/xi_opt;
+    opt_iff_kp_gain_md(kp_i) = g_opt;
+end
+
+%% Computes the optimal parameters and attainable simultaneous damping - Piezo nano-hexapod
+kps_pz = logspace(log10(kp_min), log10(1e8), 100); % Tested parallel stiffnesses [N/m]
+kps_pz(end) = [];
+
+opt_iff_kp_xi_pz   = zeros(1, length(kps_pz)); % Optimal simultaneous damping
+opt_iff_kp_gain_pz = zeros(1, length(kps_pz)); % Corresponding optimal gain
+
+
+for kp_i = 1:length(kps_pz)
+    % Voice Coil Nano-Hexapod
+    kp = kps_pz(kp_i);
+    cp = 2*0.001*sqrt((ms + mn)*kp);
+    kn = 1e8 - kp; % Nano-Hexapod Stiffness [N/m]
+    cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+
+    % Identify dynamics
+    Giff_pz = linearize(mdl, io, 0);
+    Giff_pz.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
+    Giff_pz.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+    fun = @(g)computeSimultaneousDamping(g, Giff_pz({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff_pz);
+
+    [g_opt, xi_opt] = fminsearch(fun, 0.1);
+    opt_iff_kp_xi_pz(kp_i) = 1/xi_opt;
+    opt_iff_kp_gain_pz(kp_i) = g_opt;
+end
+
+%% Find result with wanted parallel stiffness
+[~, i_kp_vc] = min(abs(kps_vc - 1e3));
+[~, i_kp_md] = min(abs(kps_md - 1e4));
+[~, i_kp_pz] = min(abs(kps_pz - 1e6));
+
+%% Identify plants with choosen Parallel stiffnesses
+model_config.Tuv_type   = "parallel_k";    % Default: 2DoF stage
+
+% Voice Coil
+kp = 1e3;
+cp = 2*0.001*sqrt((ms + mn)*kp);
+kn = 1e4-kp; % Nano-Hexapod Stiffness [N/m]
+cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+
+% Identify dynamics
+Wz = 2*pi; % [rad/s]
+G_vc_kp_fast = linearize(mdl, io, 0);
+G_vc_kp_fast.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
+G_vc_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+Wz = 0; % [rad/s]
+G_vc_kp_norot = linearize(mdl, io, 0);
+G_vc_kp_norot.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
+G_vc_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+% APA
+kp = 1e4;
+cp = 2*0.001*sqrt((ms + mn)*kp);
+kn = 1e6 - kp; % Nano-Hexapod Stiffness [N/m]
+cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+
+% Identify dynamics
+Wz = 2*pi; % [rad/s]
+G_md_kp_fast = linearize(mdl, io, 0);
+G_md_kp_fast.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
+G_md_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+Wz = 0; % [rad/s]
+G_md_kp_norot = linearize(mdl, io, 0);
+G_md_kp_norot.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
+G_md_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+% Piezo
+kp = 1e6;
+cp = 2*0.001*sqrt((ms + mn)*kp);
+kn = 1e8 - kp; % Nano-Hexapod Stiffness [N/m]
+cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+
+% Identify dynamics
+Wz = 2*pi; % [rad/s]
+G_pz_kp_fast = linearize(mdl, io, 0);
+G_pz_kp_fast.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
+G_pz_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+Wz = 0; % [rad/s]
+G_pz_kp_norot = linearize(mdl, io, 0);
+G_pz_kp_norot.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
+G_pz_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+%% Optimal IFF gain and associated simultaneous damping as a function of the parallel stiffness
+figure;
+hold on;
+plot(kps_vc, opt_iff_kp_xi_vc, '-', ...
+     'color', colors(1,:), 'DisplayName', '$k_n = 0.01\,N/\mu m$');
+plot(kps_vc(i_kp_vc), opt_iff_kp_xi_vc(i_kp_vc), '.', ...
+     'color', colors(1,:), 'MarkerSize', 15, 'HandleVisibility', 'off');
+plot(kps_md, opt_iff_kp_xi_md, '-', ...
+     'color', colors(2,:), 'DisplayName', '$k_n = 1\,N/\mu m$');
+plot(kps_md(i_kp_md), opt_iff_kp_xi_md(i_kp_md), '.', ...
+     'color', colors(2,:), 'MarkerSize', 15, 'HandleVisibility', 'off');
+plot(kps_pz, opt_iff_kp_xi_pz, '-', ...
+     'color', colors(3,:), 'DisplayName', '$k_n = 100\,N/\mu m$');
+plot(kps_pz(i_kp_pz), opt_iff_kp_xi_pz(i_kp_pz), '.', ...
+     'color', colors(3,:), 'MarkerSize', 15, 'HandleVisibility', 'off');
+hold off;
+xlabel('$k_p [N/m]$');
+ylabel('Damping Ratio $\xi$');
+set(gca, 'XScale', 'log');
+set(gca, 'YScale', 'lin');
+ylim([0,1]);
+yticks([0:0.2:1])
+legend('location', 'southeast', 'FontSize', 8);
+xlim([kps_pz(1), kps_pz(end)])
+
+%% Computes the optimal parameters and attainable simultaneous damping - Piezo nano-hexapod
+rdc_gains = 2*logspace(1, 5, 200);
+% Obtained simultaneous damping
+rdc_xi_vc = zeros(1, length(rdc_gains));
+rdc_xi_md = zeros(1, length(rdc_gains));
+rdc_xi_pz = zeros(1, length(rdc_gains));
+
+Krdc = s*eye(2);
+Krdc.InputName  = {'Du', 'Dv'};
+Krdc.OutputName = {'Fu', 'Fv'};
+
+for g_i = 1:length(rdc_gains)
+    [~, xi] = damp(feedback(G_vc_fast({'Du', 'Dv'}, {'Fu', 'Fv'}), rdc_gains(g_i)*Krdc));
+    rdc_xi_vc(g_i) = min(xi);
+
+    [~, xi] = damp(feedback(G_md_fast({'Du', 'Dv'}, {'Fu', 'Fv'}), rdc_gains(g_i)*Krdc));
+    rdc_xi_md(g_i) = min(xi);
+
+    [~, xi] = damp(feedback(G_pz_fast({'Du', 'Dv'}, {'Fu', 'Fv'}), rdc_gains(g_i)*Krdc));
+    rdc_xi_pz(g_i) = min(xi);
+end
+
+%% Optimal RDC
+[~, i_rdc_vc] = min(abs(rdc_xi_vc - 0.99));
+[~, i_rdc_md] = min(abs(rdc_xi_md - 0.99));
+[~, i_rdc_pz] = min(abs(rdc_xi_pz - 0.99));
+
+Krdc_vc = rdc_gains(i_rdc_vc)*Krdc;
+Krdc_md = rdc_gains(i_rdc_md)*Krdc;
+Krdc_pz = rdc_gains(i_rdc_pz)*Krdc;
+
+%% Optimal IFF gain and associated simultaneous damping as a function of the parallel stiffness
+figure;
+hold on;
+plot(rdc_gains, rdc_xi_vc, '-', ...
+     'color', colors(1,:), 'DisplayName', '$k_n = 0.01\,N/\mu m$');
+plot(rdc_gains(i_rdc_vc), rdc_xi_vc(i_rdc_vc), '.', ...
+     'color', colors(1,:), 'MarkerSize', 15, 'HandleVisibility', 'off');
+plot(rdc_gains, rdc_xi_md, '-', ...
+     'color', colors(2,:), 'DisplayName', '$k_n = 1\,N/\mu m$');
+plot(rdc_gains(i_rdc_md), rdc_xi_md(i_rdc_md), '.', ...
+     'color', colors(2,:), 'MarkerSize', 15, 'HandleVisibility', 'off');
+plot(rdc_gains, rdc_xi_pz, '-', ...
+     'color', colors(3,:), 'DisplayName', '$k_n = 100\,N/\mu m$');
+plot(rdc_gains(i_rdc_pz), rdc_xi_pz(i_rdc_pz), '.', ...
+     'color', colors(3,:), 'MarkerSize', 15, 'HandleVisibility', 'off');
+hold off;
+xlabel('Relative Damping Controller gain $g$');
+ylabel('Damping Ratio $\xi$');
+set(gca, 'XScale', 'log');
+set(gca, 'YScale', 'lin');
+ylim([0,1]);
+yticks([0:0.2:1])
+xlim([rdc_gains(1), rdc_gains(end)])
+legend('location', 'southeast', 'FontSize', 8);
+
+%% Closed-Loop Plants - IFF with HPF
+G_vc_norot_iff_hpf = feedback(G_vc_norot, Kiff_hpf_vc, 'name');
+G_vc_fast_iff_hpf  = feedback(G_vc_fast,  Kiff_hpf_vc, 'name');
+
+G_md_norot_iff_hpf = feedback(G_md_norot, Kiff_hpf_md, 'name');
+G_md_fast_iff_hpf  = feedback(G_md_fast,  Kiff_hpf_md, 'name');
+
+G_pz_norot_iff_hpf = feedback(G_pz_norot, Kiff_hpf_pz, 'name');
+G_pz_fast_iff_hpf  = feedback(G_pz_fast,  Kiff_hpf_pz, 'name');
+
+%% Closed-Loop Plants - IFF with Parallel Stiffness
+G_vc_norot_iff_kp = feedback(G_vc_kp_norot, Kiff_kp_vc, 'name');
+G_vc_fast_iff_kp  = feedback(G_vc_kp_fast,  Kiff_kp_vc, 'name');
+
+G_md_norot_iff_kp = feedback(G_md_kp_norot, Kiff_kp_md, 'name');
+G_md_fast_iff_kp  = feedback(G_md_kp_fast,  Kiff_kp_md, 'name');
+
+G_pz_norot_iff_kp = feedback(G_pz_kp_norot, Kiff_kp_pz, 'name');
+G_pz_fast_iff_kp  = feedback(G_pz_kp_fast,  Kiff_kp_pz, 'name');
+
+%% Closed-Loop Plants - RDC
+G_vc_norot_rdc = feedback(G_vc_norot, Krdc_vc, 'name');
+G_vc_fast_rdc  = feedback(G_vc_fast,  Krdc_vc, 'name');
+
+G_md_norot_rdc = feedback(G_md_norot, Krdc_md, 'name');
+G_md_fast_rdc  = feedback(G_md_fast,  Krdc_md, 'name');
+
+G_pz_norot_rdc = feedback(G_pz_norot, Krdc_pz, 'name');
+G_pz_fast_rdc  = feedback(G_pz_fast,  Krdc_pz, 'name');
+
+%% Comparison of the damped plants (direct and coupling terms) for the three proposed active damping techniques (IFF with HPF, IFF with $k_p$ and RDC) applied on the three nano-hexapod stiffnesses
+freqs_vc = logspace(-1, 2, 1000);
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [zeros(1,3)]);
+plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast( 'Dv', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [zeros(1,3), 0.5]);
+plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_hpf( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(1,:)]);
+plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_hpf( 'Dv', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(1,:), 0.5]);
+plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_kp( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(2,:)]);
+plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_kp( 'Dv', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(2,:), 0.5]);
+plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_rdc( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(3,:)]);
+plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_rdc( 'Dv', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(3,:), 0.5]);
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 1e-2])
+
+ax2 = nexttile;
+hold on;
+plot(freqs_vc, 180/pi*angle(squeeze(freqresp(G_vc_fast( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [zeros(1,3)]);
+plot(freqs_vc, 180/pi*angle(squeeze(freqresp(G_vc_fast_iff_hpf( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(1,:)]);
+plot(freqs_vc, 180/pi*angle(squeeze(freqresp(G_vc_fast_iff_kp( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(2,:)]);
+plot(freqs_vc, 180/pi*angle(squeeze(freqresp(G_vc_fast_rdc( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(3,:)]);
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 0]);
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs_vc(1), freqs_vc(end)]);
+
+freqs_md = logspace(0, 3, 1000);
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs_md, abs(squeeze(freqresp(G_md_fast( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [zeros(1,3)]);
+plot(freqs_md, abs(squeeze(freqresp(G_md_fast( 'Dv', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [zeros(1,3), 0.5]);
+plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_hpf( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(1,:)]);
+plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_hpf( 'Dv', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(1,:), 0.5]);
+plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_kp( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(2,:)]);
+plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_kp( 'Dv', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(2,:), 0.5]);
+plot(freqs_md, abs(squeeze(freqresp(G_md_fast_rdc( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(3,:)]);
+plot(freqs_md, abs(squeeze(freqresp(G_md_fast_rdc( 'Dv', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(3,:), 0.5]);
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
+ylim([1e-10, 1e-4])
+
+ax2 = nexttile;
+hold on;
+plot(freqs_md, 180/pi*angle(squeeze(freqresp(G_md_fast( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [zeros(1,3)]);
+plot(freqs_md, 180/pi*angle(squeeze(freqresp(G_md_fast_iff_hpf( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(1,:)]);
+plot(freqs_md, 180/pi*angle(squeeze(freqresp(G_md_fast_iff_kp( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(2,:)]);
+plot(freqs_md, 180/pi*angle(squeeze(freqresp(G_md_fast_rdc( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(3,:)]);
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 0]);
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs_md(1), freqs_md(end)]);
+
+freqs_pz = logspace(0, 3, 1000);
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [zeros(1,3)], ...
+    'DisplayName', 'OL');
+plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast( 'Dv', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [zeros(1,3), 0.5], ...
+    'DisplayName', 'Coupling');
+plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_hpf( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(1,:)], ...
+    'DisplayName', 'IFF + HPF');
+plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_hpf( 'Dv', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(1,:), 0.5], ...
+    'HandleVisibility', 'off');
+plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_kp( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(2,:)], ...
+    'DisplayName', 'IFF + $k_p$');
+plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_kp( 'Dv', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(2,:), 0.5], ...
+    'HandleVisibility', 'off');
+plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_rdc( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(3,:)], ...
+    'DisplayName', 'RDC');
+plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_rdc( 'Dv', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(3,:), 0.5], ...
+    'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
+ylim([1e-12, 1e-6])
+ldg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+ldg.ItemTokenSize = [20, 1];
+
+ax2 = nexttile;
+hold on;
+plot(freqs_pz, 180/pi*angle(squeeze(freqresp(G_pz_fast( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [zeros(1,3)]);
+plot(freqs_pz, 180/pi*angle(squeeze(freqresp(G_pz_fast_iff_hpf( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(1,:)]);
+plot(freqs_pz, 180/pi*angle(squeeze(freqresp(G_pz_fast_iff_kp( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(2,:)]);
+plot(freqs_pz, 180/pi*angle(squeeze(freqresp(G_pz_fast_rdc( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(3,:)]);
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 0]);
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs_pz(1), freqs_pz(end)]);

A2-nass-rotating-3dof-model/rotating_8_nass.m

@@ -0,0 +1,470 @@
+%% 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
+
+%% Colors for the figures
+colors = colororder;
+
+%% Nano-Hexapod on top of Micro-Station model
+mdl = 'nass_rotating_model';
+
+%% Load micro-station parameters
+load('uniaxial_micro_station_parameters.mat')
+
+%% Load controllers
+load('nass_controllers.mat');
+
+%% System parameters
+mn = 15; % Nano-Hexapod mass [kg]
+ms = 1; % Sample Mass [kg]
+
+% General Configuration
+model_config = struct();
+model_config.controller = "open_loop"; % Default: Open-Loop
+model_config.Tuv_type   = "normal";    % Default: 2DoF stage
+
+% Input/Output definition
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/controller'],   1, 'openinput');  io_i = io_i + 1; % Actuator Forces [Fu, Fv]
+io(io_i) = linio([mdl, '/fd'],           1, 'openinput');  io_i = io_i + 1; % Direct Forces on Sample [Fdx, Fdy]
+io(io_i) = linio([mdl, '/xf'],           1, 'openinput');  io_i = io_i + 1; % Floor Motion [Dfx, Dfy]
+io(io_i) = linio([mdl, '/ft'],           1, 'openinput');  io_i = io_i + 1; % Micro-Station Disturbances [Ftx, Fty]
+io(io_i) = linio([mdl, '/nano_hexapod'], 1, 'openoutput'); io_i = io_i + 1; % [Fmu, Fmv]
+io(io_i) = linio([mdl, '/nano_hexapod'], 2, 'openoutput'); io_i = io_i + 1; % [Du, Dv]
+io(io_i) = linio([mdl, '/ext_metrology'],1, 'openoutput'); io_i = io_i + 1; % [Dx, Dy]
+
+%% Identify plant without parallel stiffness
+% Voice Coil (i.e. soft) Nano-Hexapod
+kn = 1e4; % Nano-Hexapod Stiffness [N/m]
+cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+
+Wz = 0; % Rotating Velocity [rad/s]
+G_vc_norot = linearize(mdl, io, 0.0);
+G_vc_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
+G_vc_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+Wz = 2*pi; % Rotating Velocity [rad/s]
+G_vc_fast = linearize(mdl, io, 0.0);
+G_vc_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
+G_vc_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+% APA (i.e. relatively stiff) Nano-Hexapod
+kn = 1e6; % Nano-Hexapod Stiffness [N/m]
+cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+
+Wz = 0; % Rotating Velocity [rad/s]
+G_md_norot = linearize(mdl, io, 0.0);
+G_md_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
+G_md_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+Wz = 2*pi; % Rotating Velocity [rad/s]
+G_md_fast = linearize(mdl, io, 0.0);
+G_md_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
+G_md_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+% Piezoelectric (i.e. stiff) Nano-Hexapod
+kn = 1e8; % Nano-Hexapod Stiffness [N/m]
+cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+
+Wz = 0; % Rotating Velocity [rad/s]
+G_pz_norot = linearize(mdl, io, 0.0);
+G_pz_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
+G_pz_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+Wz = 2*pi; % Rotating Velocity [rad/s]
+G_pz_fast = linearize(mdl, io, 0.0);
+G_pz_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
+G_pz_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+%% Identify plants with Parallel stiffnesses
+model_config.Tuv_type   = "parallel_k";    % Default: 2DoF stage
+
+% Voice Coil
+kp = 1e3;
+cp = 2*0.001*sqrt((ms + mn)*kp);
+kn = 1e4-kp; % Nano-Hexapod Stiffness [N/m]
+cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+
+% Identify dynamics
+Wz = 2*pi; % [rad/s]
+G_vc_kp_fast = linearize(mdl, io, 0);
+G_vc_kp_fast.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
+G_vc_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+Wz = 0; % [rad/s]
+G_vc_kp_norot = linearize(mdl, io, 0);
+G_vc_kp_norot.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
+G_vc_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+% APA
+kp = 1e4;
+cp = 2*0.001*sqrt((ms + mn)*kp);
+kn = 1e6 - kp; % Nano-Hexapod Stiffness [N/m]
+cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+
+% Identify dynamics
+Wz = 2*pi; % [rad/s]
+G_md_kp_fast = linearize(mdl, io, 0);
+G_md_kp_fast.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
+G_md_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+Wz = 0; % [rad/s]
+G_md_kp_norot = linearize(mdl, io, 0);
+G_md_kp_norot.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
+G_md_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+% Piezo
+kp = 1e5;
+cp = 2*0.001*sqrt((ms + mn)*kp);
+kn = 1e8 - kp; % Nano-Hexapod Stiffness [N/m]
+cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]
+
+% Identify dynamics
+Wz = 2*pi; % [rad/s]
+G_pz_kp_fast = linearize(mdl, io, 0);
+G_pz_kp_fast.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
+G_pz_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+Wz = 0; % [rad/s]
+G_pz_kp_norot = linearize(mdl, io, 0);
+G_pz_kp_norot.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
+G_pz_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
+
+%% Compute dampepd plants
+% Closed-Loop Plants - IFF with HPF
+G_vc_norot_iff_hpf = feedback(G_vc_norot, Kiff_hpf_vc, 'name');
+G_vc_fast_iff_hpf  = feedback(G_vc_fast,  Kiff_hpf_vc, 'name');
+
+G_md_norot_iff_hpf = feedback(G_md_norot, Kiff_hpf_md, 'name');
+G_md_fast_iff_hpf  = feedback(G_md_fast,  Kiff_hpf_md, 'name');
+
+G_pz_norot_iff_hpf = feedback(G_pz_norot, Kiff_hpf_pz, 'name');
+G_pz_fast_iff_hpf  = feedback(G_pz_fast,  Kiff_hpf_pz, 'name');
+
+% Closed-Loop Plants - IFF with Parallel Stiffness
+G_vc_norot_iff_kp = feedback(G_vc_kp_norot, Kiff_kp_vc, 'name');
+G_vc_fast_iff_kp  = feedback(G_vc_kp_fast,  Kiff_kp_vc, 'name');
+
+G_md_norot_iff_kp = feedback(G_md_kp_norot, Kiff_kp_md, 'name');
+G_md_fast_iff_kp  = feedback(G_md_kp_fast,  Kiff_kp_md, 'name');
+
+G_pz_norot_iff_kp = feedback(G_pz_kp_norot, Kiff_kp_pz, 'name');
+G_pz_fast_iff_kp  = feedback(G_pz_kp_fast,  Kiff_kp_pz, 'name');
+
+% Closed-Loop Plants - RDC
+G_vc_norot_rdc = feedback(G_vc_norot, Krdc_vc, 'name');
+G_vc_fast_rdc  = feedback(G_vc_fast,  Krdc_vc, 'name');
+
+G_md_norot_rdc = feedback(G_md_norot, Krdc_md, 'name');
+G_md_fast_rdc  = feedback(G_md_fast,  Krdc_md, 'name');
+
+G_pz_norot_rdc = feedback(G_pz_norot, Krdc_pz, 'name');
+G_pz_fast_rdc  = feedback(G_pz_fast,  Krdc_pz, 'name');
+
+%% Bode plot of the transfer function from nano-hexapod actuator to measured motion by the external metrology
+freqs_vc = logspace(-1, 2, 1000);
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast('Dx', 'Fu'), freqs_vc, 'Hz'))), 'color', zeros(1,3), ...
+    'DisplayName', 'OL');
+plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast('Dy', 'Fu'), freqs_vc, 'Hz'))), 'color', [zeros(1,3), 0.5], ...
+    'DisplayName', 'Coupling');
+plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dx', 'Fu'), freqs_vc, 'Hz'))), 'color', colors(1,:), ...
+    'DisplayName', 'IFF + $k_p$');
+plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dy', 'Fu'), freqs_vc, 'Hz'))), 'color', [colors(1,:), 0.5], ...
+    'HandleVisibility', 'off');
+plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dx', 'Fu'), freqs_vc, 'Hz'))), 'color', colors(2,:), ...
+    'DisplayName', 'IFF + HPF');
+plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dy', 'Fu'), freqs_vc, 'Hz'))), 'color', [colors(2,:), 0.5], ...
+    'HandleVisibility', 'off');
+plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_rdc('Dx', 'Fu'), freqs_vc, 'Hz'))), 'color', colors(3,:), ...
+    'DisplayName', 'RDC');
+plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_rdc('Dy', 'Fu'), freqs_vc, 'Hz'))), 'color', [colors(3,:), 0.5], ...
+    'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 1e-2])
+
+ax2 = nexttile;
+hold on;
+plot(freqs_vc, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_fast('Dx', 'Fu'), freqs_vc, 'Hz')))), 'color', zeros(1,3));
+plot(freqs_vc, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_fast_iff_hpf('Dx', 'Fu'), freqs_vc, 'Hz')))), 'color', colors(1,:));
+plot(freqs_vc, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_fast_iff_kp('Dx', 'Fu'), freqs_vc, 'Hz')))), 'color', colors(2,:));
+plot(freqs_vc, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_fast_rdc('Dx', 'Fu'), freqs_vc, 'Hz')))), 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([ -200, 20]);
+
+linkaxes([ax,ax2],'x');
+xlim([freqs_vc(1), freqs_vc(end)]);
+xticks([1e-1, 1e0, 1e1]);
+
+freqs_md = logspace(0, 3, 1000);
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs_md, abs(squeeze(freqresp(G_md_fast('Dx', 'Fu'), freqs_md, 'Hz'))), 'color', zeros(1,3), ...
+    'DisplayName', 'OL');
+plot(freqs_md, abs(squeeze(freqresp(G_md_fast('Dy', 'Fu'), freqs_md, 'Hz'))), 'color', [zeros(1,3), 0.5], ...
+    'DisplayName', 'Coupling');
+plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dx', 'Fu'), freqs_md, 'Hz'))), 'color', colors(1,:), ...
+    'DisplayName', 'IFF + $k_p$');
+plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dy', 'Fu'), freqs_md, 'Hz'))), 'color', [colors(1,:), 0.5], ...
+    'HandleVisibility', 'off');
+plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_kp('Dx', 'Fu'), freqs_md, 'Hz'))), 'color', colors(2,:), ...
+    'DisplayName', 'IFF + HPF');
+plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_kp('Dy', 'Fu'), freqs_md, 'Hz'))), 'color', [colors(2,:), 0.5], ...
+    'HandleVisibility', 'off');
+plot(freqs_md, abs(squeeze(freqresp(G_md_fast_rdc('Dx', 'Fu'), freqs_md, 'Hz'))), 'color', colors(3,:), ...
+    'DisplayName', 'RDC');
+plot(freqs_md, abs(squeeze(freqresp(G_md_fast_rdc('Dy', 'Fu'), freqs_md, 'Hz'))), 'color', [colors(3,:), 0.5], ...
+    'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
+ylim([1e-10, 1e-4])
+
+ax2 = nexttile;
+hold on;
+plot(freqs_md, 180/pi*unwrap(angle(squeeze(freqresp(G_md_fast('Dx', 'Fu'), freqs_md, 'Hz')))), 'color', zeros(1,3));
+plot(freqs_md, 180/pi*unwrap(angle(squeeze(freqresp(G_md_fast_iff_hpf('Dx', 'Fu'), freqs_md, 'Hz')))), 'color', colors(1,:));
+plot(freqs_md, 180/pi*unwrap(angle(squeeze(freqresp(G_md_fast_iff_kp('Dx', 'Fu'), freqs_md, 'Hz')))), 'color', colors(2,:));
+plot(freqs_md, 180/pi*unwrap(angle(squeeze(freqresp(G_md_fast_rdc('Dx', 'Fu'), freqs_md, 'Hz')))), 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([ -200, 20]);
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs_md(1), freqs_md(end)]);
+xticks([1e0, 1e1, 1e2]);
+
+freqs_pz = logspace(0, 3, 1000);
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast('Dx', 'Fu'), freqs_pz, 'Hz'))), 'color', zeros(1,3), ...
+    'DisplayName', 'OL');
+plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast('Dy', 'Fu'), freqs_pz, 'Hz'))), 'color', [zeros(1,3), 0.5], ...
+    'DisplayName', 'Coupling');
+plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dx', 'Fu'), freqs_pz, 'Hz'))), 'color', colors(1,:), ...
+    'DisplayName', 'IFF + $k_p$');
+plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dy', 'Fu'), freqs_pz, 'Hz'))), 'color', [colors(1,:), 0.5], ...
+    'HandleVisibility', 'off');
+plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dx', 'Fu'), freqs_pz, 'Hz'))), 'color', colors(2,:), ...
+    'DisplayName', 'IFF + HPF');
+plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dy', 'Fu'), freqs_pz, 'Hz'))), 'color', [colors(2,:), 0.5], ...
+    'HandleVisibility', 'off');
+plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_rdc('Dx', 'Fu'), freqs_pz, 'Hz'))), 'color', colors(3,:), ...
+    'DisplayName', 'RDC');
+plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_rdc('Dy', 'Fu'), freqs_pz, 'Hz'))), 'color', [colors(3,:), 0.5], ...
+    'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
+ylim([1e-12, 1e-6])
+ldg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+ldg.ItemTokenSize = [20, 1];
+
+ax2 = nexttile;
+hold on;
+plot(freqs_pz, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_fast('Dx', 'Fu'), freqs_pz, 'Hz')))), 'color', zeros(1,3));
+plot(freqs_pz, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_fast_iff_hpf('Dx', 'Fu'), freqs_pz, 'Hz')))), 'color', colors(1,:));
+plot(freqs_pz, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_fast_iff_kp('Dx', 'Fu'), freqs_pz, 'Hz')))), 'color', colors(2,:));
+plot(freqs_pz, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_fast_rdc('Dx', 'Fu'), freqs_pz, 'Hz')))), 'color', colors(3,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([ -200, 20]);
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs_pz(1), freqs_pz(end)]);
+xticks([1e0, 1e1, 1e2]);
+
+%% Effect of Floor motion on the position error - Comparison of active damping techniques for the three nano-hexapod stiffnesses
+freqs = logspace(-1, 3, 1000);
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_vc_fast('Dx', 'Dfx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
+    'DisplayName', 'OL');
+plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(1,:), ...
+    'DisplayName', 'IFF + $k_p$');
+plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(2,:), ...
+    'DisplayName', 'IFF + HPF');
+plot(freqs, abs(squeeze(freqresp(G_vc_fast_rdc('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(3,:), ...
+    'DisplayName', 'RDC');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/x_{f,x}$ [m/N]');
+xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
+xtickangle(0)
+ylim([1e-4, 1e2]);
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_md_fast('Dx', 'Dfx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
+    'DisplayName', 'OL');
+plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(1,:), ...
+    'DisplayName', 'IFF + $k_p$');
+plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_kp('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(2,:), ...
+    'DisplayName', 'IFF + HPF');
+plot(freqs, abs(squeeze(freqresp(G_md_fast_rdc('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(3,:), ...
+    'DisplayName', 'RDC');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/x_{f,x}$ [m/N]');
+xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
+xtickangle(0)
+ylim([1e-4, 1e2]);
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_pz_fast('Dx', 'Dfx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
+    'DisplayName', 'OL');
+plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(1,:), ...
+    'DisplayName', 'IFF + $k_p$');
+plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(2,:), ...
+    'DisplayName', 'IFF + HPF');
+plot(freqs, abs(squeeze(freqresp(G_pz_fast_rdc('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(3,:), ...
+    'DisplayName', 'RDC');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/x_{f,x}$ [m/N]');
+xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
+xtickangle(0)
+ldg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+ldg.ItemTokenSize = [15, 1];
+ylim([1e-4, 1e2]);
+
+%% Effect of micro-station vibrations on the position error - Comparison of active damping techniques for the three nano-hexapod stiffnesses
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_vc_fast('Dx', 'Ftx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
+    'DisplayName', 'OL');
+plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(1,:), ...
+    'DisplayName', 'IFF + $k_p$');
+plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(2,:), ...
+    'DisplayName', 'IFF + HPF');
+plot(freqs, abs(squeeze(freqresp(G_vc_fast_rdc('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(3,:), ...
+    'DisplayName', 'RDC');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{t,x}$ [m/N]');
+xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
+xtickangle(0)
+ylim([1e-12, 2e-7]);
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_md_fast('Dx', 'Ftx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
+    'DisplayName', 'OL');
+plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(1,:), ...
+    'DisplayName', 'IFF + $k_p$');
+plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_kp('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(2,:), ...
+    'DisplayName', 'IFF + HPF');
+plot(freqs, abs(squeeze(freqresp(G_md_fast_rdc('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(3,:), ...
+    'DisplayName', 'RDC');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{t,x}$ [m/N]');
+xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
+xtickangle(0)
+ylim([1e-12, 2e-7]);
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_pz_fast('Dx', 'Ftx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
+    'DisplayName', 'OL');
+plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(1,:), ...
+    'DisplayName', 'IFF + $k_p$');
+plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(2,:), ...
+    'DisplayName', 'IFF + HPF');
+plot(freqs, abs(squeeze(freqresp(G_pz_fast_rdc('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(3,:), ...
+    'DisplayName', 'RDC');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{t,x}$ [m/N]');
+xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
+xtickangle(0)
+ldg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+ldg.ItemTokenSize = [20, 1];
+ylim([1e-12, 2e-7]);
+
+%% Effect of sample forces on the position error - Comparison of active damping techniques for the three nano-hexapod stiffnesses
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_vc_fast('Dx', 'Fdx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
+    'DisplayName', 'OL');
+plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(1,:), ...
+    'DisplayName', 'IFF + $k_p$');
+plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(2,:), ...
+    'DisplayName', 'IFF + HPF');
+plot(freqs, abs(squeeze(freqresp(G_vc_fast_rdc('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(3,:), ...
+    'DisplayName', 'RDC');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{s,x}$ [m/N]');
+xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
+xtickangle(0)
+ylim([1e-8, 1e-2])
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_md_fast('Dx', 'Fdx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
+    'DisplayName', 'OL');
+plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(1,:), ...
+    'DisplayName', 'IFF + $k_p$');
+plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_kp('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(2,:), ...
+    'DisplayName', 'IFF + HPF');
+plot(freqs, abs(squeeze(freqresp(G_md_fast_rdc('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(3,:), ...
+    'DisplayName', 'RDC');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{s,x}$ [m/N]');
+xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
+xtickangle(0)
+ylim([1e-8, 1e-2])
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_pz_fast('Dx', 'Fdx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
+    'DisplayName', 'OL');
+plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(1,:), ...
+    'DisplayName', 'IFF + $k_p$');
+plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(2,:), ...
+    'DisplayName', 'IFF + HPF');
+plot(freqs, abs(squeeze(freqresp(G_pz_fast_rdc('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(3,:), ...
+    'DisplayName', 'RDC');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{s,x}$ [m/N]');
+xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
+xtickangle(0)
+ldg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+ldg.ItemTokenSize = [20, 1];
+
+linkaxes([ax1,ax2,ax3], 'y')
+ylim([1e-8, 1e-2])

A2-nass-rotating-3dof-model/src/computeSimultaneousDamping.m

@@ -0,0 +1,8 @@
+function [xi_min] = computeSimultaneousDamping(g, G, K)
+    [~, xi] = damp(minreal(feedback(G, g*K), [], false));
+    xi_min = 1/min(xi);
+
+    if xi_min < 0
+        xi_min = 1e8;
+    end
+end

A2-nass-rotating-3dof-model/src/rootLocusPolesSorted.m

@@ -0,0 +1,94 @@
+  function [poles] = rootLocusPolesSorted(G, K, gains, args)
+  % rootLocusPolesSorted -
+  %
+  % Syntax: [poles] = rootLocusPolesSorted(G, K, gains, args)
+  %
+  % Inputs:
+  %    - G, K, gains, args -
+  %
+  % Outputs:
+  %    - poles -
+
+  arguments
+      G
+      K
+      gains
+      args.minreal double {mustBeNumericOrLogical} = false
+      args.p_half  double {mustBeNumericOrLogical} = false
+      args.d_max   double {mustBeNumeric} = -1
+  end
+
+  if args.minreal
+      p1 = pole(minreal(feedback(G, gains(1)*K)));
+      [~, i_uniq] = uniquetol([real(p1), imag(p1)], 1e-10, 'ByRows', true);
+      p1 = p1(i_uniq);
+
+      poles = zeros(length(p1), length(gains));
+      poles(:, 1) = p1;
+  else
+      p1 = pole(feedback(G, gains(1)*K));
+      [~, i_uniq] = uniquetol([real(p1), imag(p1)], 1e-10, 'ByRows', true);
+      p1 = p1(i_uniq);
+
+      poles = zeros(length(p1), length(gains));
+      poles(:, 1) = p1;
+  end
+
+  if args.minreal
+      p2 = pole(minreal(feedback(G, gains(2)*K)));
+      [~, i_uniq] = uniquetol([real(p2), imag(p2)], 1e-10, 'ByRows', true);
+      p2 = p2(i_uniq);
+      poles(:, 2) = p2;
+  else
+      p2 = pole(feedback(G, gains(2)*K));
+      [~, i_uniq] = uniquetol([real(p2), imag(p2)], 1e-10, 'ByRows', true);
+      p2 = p2(i_uniq);
+      poles(:, 2) = p2;
+  end
+
+  for g_i = 3:length(gains)
+      % Estimated value of the poles
+      poles_est = poles(:, g_i-1) + (poles(:, g_i-1) - poles(:, g_i-2))*(gains(g_i) - gains(g_i-1))/(gains(g_i-1) - gains(g_i - 2));
+
+      % New values for the poles
+      poles_gi = pole(feedback(G, gains(g_i)*K));
+      [~, i_uniq] = uniquetol([real(poles_gi), imag(poles_gi)], 1e-10, 'ByRows', true);
+      poles_gi = poles_gi(i_uniq);
+
+      % Array of distances between all the poles
+      poles_dist = sqrt((poles_est-poles_gi.').*conj(poles_est-poles_gi.'));
+
+      % Get indices corresponding to distances from lowest to highest
+      [~, c] = sort(min(poles_dist));
+
+      as = 1:length(poles_gi);
+
+      % for each column of poles_dist corresponding to the i'th pole
+      % with closest previous poles
+      for p_i = c
+          % Get the indice a_i of the previous pole that is the closest
+          % to pole c(p_i)
+          [~, a_i] = min(poles_dist(:, p_i));
+
+          poles(as(a_i), g_i) = poles_gi(p_i);
+
+          % Remove old poles that are already matched
+          % poles_gi(as(a_i), :) = [];
+          poles_dist(a_i, :) = [];
+          as(a_i) = [];
+      end
+  end
+
+
+  if args.d_max > 0
+      poles = poles(max(abs(poles(:, 2:end) - poles(:, 1:end-1))') > args.d_max, :);
+  end
+
+  if args.p_half
+      poles = poles(1:round(end/2), :);
+  end
+
+  [~, s_p] = sort(imag(poles(:,1)), 'descend');
+  poles = poles(s_p, :);
+
+  poles = poles.';

A3-micro-station-modal-analysis/mat/acc_pos.txt

@@ -0,0 +1,23 @@
+    23  1.5500e-001 -9.0000e-002 -5.9400e-001     
+    22  0.0000e+000  1.8000e-001 -5.9400e-001     
+    21 -1.5500e-001 -9.0000e-002 -5.9400e-001     
+    20  8.6490e-001 -5.0600e-001 -9.5060e-001     
+    19  8.7500e-001  7.9900e-001 -9.5060e-001     
+    18 -7.3500e-001  8.1400e-001 -9.5060e-001     
+    17 -7.3000e-001 -5.2600e-001 -9.5060e-001     
+    16  2.9500e-001 -4.8100e-001 -7.8560e-001     
+    15  4.5000e-001  5.3400e-001 -7.8560e-001     
+    14 -4.8000e-001  5.3400e-001 -7.8560e-001     
+    13 -3.2000e-001 -4.4600e-001 -7.8560e-001     
+    12  4.7500e-001 -4.1900e-001 -4.2730e-001     
+    11  4.7500e-001  4.2400e-001 -4.2730e-001     
+    10 -4.6500e-001  4.0700e-001 -4.2730e-001     
+     9 -4.7500e-001 -4.1400e-001 -4.2730e-001     
+     8  3.8000e-001 -3.0000e-001 -4.1680e-001     
+     7  4.2000e-001  2.8000e-001 -4.1680e-001     
+     6 -4.2000e-001  2.8000e-001 -4.1680e-001     
+     5 -3.8500e-001 -3.0000e-001 -4.1680e-001     
+     4  6.4000e-002 -6.4000e-002 -2.7000e-001     
+     3  6.4000e-002  6.4000e-002 -2.7000e-001     
+     2 -6.4000e-002  6.4000e-002 -2.7000e-001     
+     1 -6.4000e-002 -6.4000e-002 -2.7000e-001     

A3-micro-station-modal-analysis/mat/mode_damps.txt

@@ -0,0 +1,16 @@
+12.20318
+11.66888
+6.19561
+2.79104
+2.76253
+4.34928
+1.25546
+3.65470
+2.94088
+3.19084
+1.55526
+3.13166
+2.76141
+1.34304
+2.43201
+1.38400

A3-micro-station-modal-analysis/mat/mode_freqs.txt

@@ -0,0 +1,16 @@
+11.86509
+18.55747
+37.82163
+39.07850
+56.31944
+69.78452
+72.49325
+84.83446
+91.26350
+105.47266
+106.57165
+112.67669
+124.20538
+145.30034
+150.52113
+165.42632

A3-micro-station-modal-analysis/mat/mode_modal_a.txt

@@ -0,0 +1,16 @@
+4.13559e+003 +6.22828e+003
+2.76278e+002 +1.74197e+004
+-1.32270e+004 +2.17346e+004
+-2.48397e+005 -1.60998e+005
+-4.23967e+004 +7.06852e+004
+-7.36964e+003 +4.57024e+004
+1.37806e+005 +3.00336e+005
+-1.31109e+004 +2.81759e+004
+5.59259e+003 -4.27543e+004
+-5.28869e+004 +6.38436e+003
+3.71578e+004 +1.57745e+004
+-4.24659e+004 +7.90956e+003
+-3.57355e+004 +1.13161e+005
+5.24764e+004 -1.45211e+005
+1.97228e+005 +2.51758e+005
+-3.00273e+005 +3.27201e+005

A3-micro-station-modal-analysis/mat/mode_modal_b.txt

@@ -0,0 +1,16 @@
+4.98475e+005 -2.49344e+005
+2.02102e+006 +2.05017e+005
+4.96035e+006 +3.45724e+006
+-4.12180e+007 +5.98638e+007
+2.45891e+007 +1.56880e+007
+1.98796e+007 +4.09986e+006
+1.37577e+008 -6.10466e+007
+1.47532e+007 +7.53272e+006
+-2.44115e+007 -3.92655e+006
+3.11045e+006 +3.51656e+007
+1.09485e+007 -2.47140e+007
+4.65546e+006 +3.02251e+007
+8.75076e+007 +3.03162e+007
+-1.31915e+008 -4.96844e+007
+2.42567e+008 -1.80683e+008
+3.35742e+008 +3.16782e+008

A3-micro-station-modal-analysis/mat/model_solidworks_com.txt

@@ -0,0 +1,18 @@
+0.045
+0.144
+-1.251
+0.052
+0.258
+-0.778
+0.000
+0.014
+-0.600
+0.000
+-0.005
+-0.628
+0.000
+0.000
+-0.580
+-0.004
+0.006
+-0.319

A3-micro-station-modal-analysis/modal_1_meas_setup.m

@@ -0,0 +1,83 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+%% Path for functions, data and scripts
+addpath('./mat/'); % Path for data
+
+%% Colors for the figures
+colors = colororder;
+
+%% Load Accelerometer positions
+acc_pos = readtable('mat/acc_pos.txt', 'ReadVariableNames', false);
+acc_pos = table2array(acc_pos(:, 1:4));
+[~, i] = sort(acc_pos(:, 1));
+acc_pos = acc_pos(i, 2:4);
+
+%% Load raw data
+meas1_raw = load('mat/meas_raw_1.mat');
+
+% Sampling Frequency [Hz]
+Fs = 1/meas1_raw.Track1_X_Resolution;
+
+% Time just before the impact occurs [s]
+impacts = [5.937, 11.228, 16.681, 22.205, 27.350, 32.714, 38.115, 43.888, 50.407]-0.01;
+
+% Time vector [s]
+time = linspace(0, meas1_raw.Track1_X_Resolution*length(meas1_raw.Track1), length(meas1_raw.Track1));
+
+%% Raw measurement of the Accelerometer
+figure;
+hold on;
+plot(time-22.2, meas1_raw.Track2, 'DisplayName', '$X_{1,x}$ [$m/s^2$]');
+plot(time-22.2, 1e-3*meas1_raw.Track1, 'DisplayName', '$F_{z}$ [kN]');
+hold off;
+xlabel('Time [s]');
+ylabel('Amplitude');
+xlim([0, 0.2])
+ylim([-2, 2]);
+legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+
+%% Frequency Analysis
+Nfft = floor(5.0*Fs); % Number of frequency points
+win = hanning(Nfft); % Windowing
+Noverlap = floor(Nfft/2); % Overlap for frequency analysis
+
+%% Comnpute the power spectral density of the force and acceleration
+[pxx_force, f] = pwelch(meas1_raw.Track1, win, Noverlap, Nfft, Fs);
+[pxx_acc,   ~] = pwelch(meas1_raw.Track2, win, Noverlap, Nfft, Fs);
+
+%% Normalized Amplitude Spectral Density of the measured force and acceleration
+figure;
+hold on;
+plot(f, sqrt(pxx_acc./max(pxx_acc(f<200))), 'DisplayName', '$\Gamma_{X_{1,x}}$');
+plot(f, sqrt(pxx_force./max(pxx_force(f<200))), 'DisplayName', '$\Gamma_{F_{z}}$');
+hold off;
+set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Normalized Spectral Density');
+xlim([0, 200]);
+xticks([0:20:200]);
+ylim([0, 1])
+legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+
+%% Compute the transfer function and Coherence
+[G1,   f] = tfestimate(meas1_raw.Track1, meas1_raw.Track2, win, Noverlap, Nfft, Fs);
+[coh1, ~] = mscohere(  meas1_raw.Track1, meas1_raw.Track2, win, Noverlap, Nfft, Fs);
+
+%% Frequency Response Function between the force and the acceleration
+figure;
+plot(f, abs(G1));
+xlabel('Frequency [Hz]'); ylabel('FRF [$m/s^2/N$]')
+set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'log');
+xlim([0, 200]);
+xticks([0:20:200]);
+
+%% Frequency Response Function between the force and the acceleration
+figure;
+plot(f, coh1);
+xlabel('Frequency [Hz]'); ylabel('Coherence [-]')
+set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
+xlim([0, 200]); ylim([0,1]);
+xticks([0:20:200]);

A3-micro-station-modal-analysis/modal_2_frf_processing.m

@@ -0,0 +1,132 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+%% Path for functions, data and scripts
+addpath('./mat/'); % Path for data
+
+%% Colors for the figures
+colors = colororder;
+
+%% Load frequency response matrix
+load('frf_matrix.mat', 'freqs', 'frf');
+
+%% Load Accelerometer positions
+acc_pos = readtable('mat/acc_pos.txt', 'ReadVariableNames', false);
+acc_pos = table2array(acc_pos(:, 1:4));
+[~, i] = sort(acc_pos(:, 1));
+acc_pos = acc_pos(i, 2:4);
+
+%% Accelerometers ID connected to each solid body
+solids = {};
+solids.gbot = [17, 18, 19, 20]; % bottom granite
+solids.gtop = [13, 14, 15, 16]; % top granite
+solids.ty   = [9, 10, 11, 12]; % Ty stage
+solids.ry   = [5, 6, 7, 8]; % Ry stage
+solids.rz   = [21, 22, 23]; % Rz stage
+solids.hexa = [1, 2, 3, 4]; % Hexapod
+
+% Names of the solid bodies
+solid_names = fields(solids);
+
+%% Save the accelerometer positions are well as the solid bodies
+save('mat/geometry.mat', 'solids', 'solid_names', 'acc_pos');
+
+%% Extract the CoM of considered solid bodies
+model_com = reshape(table2array(readtable('mat/model_solidworks_com.txt', 'ReadVariableNames', false)), [3, 6]);
+
+%% Frequency Response Matrix - Response expressed at the CoM of the solid bodies
+frfs_CoM = zeros(length(solid_names)*6, 3, 801);
+
+for solid_i = 1:length(solid_names)
+  % Number of accelerometers fixed to this solid body
+  solids_i = solids.(solid_names{solid_i});
+
+  % "Jacobian" matrix to go from accelerometer frame to CoM frame
+  A = zeros(3*length(solids_i), 6);
+  for i = 1:length(solids_i)
+    acc_i = solids_i(i);
+
+    acc_pos_com = acc_pos(acc_i, :).' - model_com(:, solid_i);
+
+    A(3*(i-1)+1:3*i, 1:3) = eye(3);
+    A(3*(i-1)+1:3*i, 4:6) = [ 0               acc_pos_com(3) -acc_pos_com(2) ;
+                             -acc_pos_com(3)  0               acc_pos_com(1) ;
+                              acc_pos_com(2) -acc_pos_com(1)  0];
+  end
+
+  for exc_dir = 1:3
+    frfs_CoM((solid_i-1)*6+1:solid_i*6, exc_dir, :) = A\squeeze(frf((solids_i(1)-1)*3+1:solids_i(end)*3, exc_dir, :));
+  end
+end
+
+%% Save the computed FRF at the CoM
+save('mat/frf_com.mat', 'frfs_CoM');
+
+%% Compute the FRF at the accelerometer location from the CoM reponses
+frfs_A = zeros(size(frf));
+
+% For each excitation direction
+for exc_dir = 1:3
+  % For each solid
+  for solid_i = 1:length(solid_names)
+    v0 = squeeze(frfs_CoM((solid_i-1)*6+1:(solid_i-1)*6+3, exc_dir, :));
+    W0 = squeeze(frfs_CoM((solid_i-1)*6+4:(solid_i-1)*6+6, exc_dir, :));
+
+    % For each accelerometer attached to the current solid
+    for acc_i = solids.(solid_names{solid_i})
+      % We get the position of the accelerometer expressed in frame O
+      pos = acc_pos(acc_i, :).' - model_com(:, solid_i);
+      % pos = acc_pos(acc_i, :).';
+      posX = [0 pos(3) -pos(2); -pos(3) 0 pos(1) ; pos(2) -pos(1) 0];
+
+      frfs_A(3*(acc_i-1)+1:3*(acc_i-1)+3, exc_dir, :) = v0 + posX*W0;
+    end
+  end
+end
+
+%% Comparison of the original accelerometer response and reconstructed response from the solid body response
+exc_names = {'$F_x$', '$F_y$', '$F_z$'};
+DOFs = {'x', 'y', 'z', '\theta_x', '\theta_y', '\theta_z'};
+
+solid_i = 6; % Considered solid body
+exc_dir = 1; % Excited direction
+
+accs_i = solids.(solid_names{solid_i}); % Accelerometers fixed to this solid body
+
+figure;
+tiledlayout(2, 2, 'TileSpacing', 'Tight', 'Padding', 'None');
+
+for i = 1:length(accs_i)
+  acc_i = accs_i(i);
+  nexttile();
+
+  hold on;
+  for dir_i = 1:3
+    plot(freqs, abs(squeeze(frf(3*(acc_i-1)+dir_i, exc_dir, :))), '-', 'color', [colors(dir_i,:), 0.5], 'linewidth', 2.5, 'DisplayName', sprintf('$a_{%i,%s}$ - meas', acc_i, DOFs{dir_i}));
+  end
+  for dir_i = 1:3
+    plot(freqs, abs(squeeze(frfs_A(3*(acc_i-1)+dir_i, exc_dir, :))), '-', 'color', colors(dir_i, :), 'DisplayName', sprintf('$a_{%i,%s}$ - solid body', acc_i, DOFs{dir_i}));
+  end
+  hold off;
+
+  if i > 2
+    xlabel('Frequency [Hz]');
+  else
+    set(gca, 'XTickLabel',[]);
+  end
+
+  if rem(i, 2) == 1
+    ylabel('Amplitude [$\frac{m/s^2}{N}$]');
+  else
+    set(gca, 'YTickLabel',[]);
+  end
+
+  set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'log');
+  xlim([0, 200]); ylim([1e-6, 3e-2]);
+  xticks([0:20:200]);
+  leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
+  leg.ItemTokenSize(1) = 15;
+end

A3-micro-station-modal-analysis/modal_3_analysis.m

@@ -0,0 +1,151 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+%% Path for functions, data and scripts
+addpath('./mat/'); % Path for data
+
+%% Colors for the figures
+colors = colororder;
+
+%% Load frequency response matrix
+load('frf_matrix.mat', 'freqs', 'frf');
+
+%% Computation of the modal indication function
+MIF = zeros(size(frf, 2), size(frf, 2), size(frf, 3));
+
+for i = 1:length(freqs)
+  [~,S,~] = svd(frf(:, :, i));
+  MIF(:, :, i) = S'*S;
+end
+
+%% Modal Indication Function
+figure;
+hold on;
+for i = 1:size(MIF, 1)
+  plot(freqs, squeeze(MIF(i, i, :)), 'DisplayName', sprintf('MIF${}_%i$', i));
+end
+hold off;
+set(gca, 'Xscale', 'lin'); set(gca, 'Yscale', 'log');
+xlabel('Frequency [Hz]'); ylabel('CMIF Amplitude');
+xticks([0:20:200]);
+xlim([0, 200]);
+ylim([1e-6, 2e-2]);
+ldg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+
+%% Load modal parameters
+shapes_m = readtable('mat/mode_shapes.txt',              'ReadVariableNames', false);  % [Sign / Real / Imag]
+freqs_m  = table2array(readtable('mat/mode_freqs.txt',   'ReadVariableNames', false)); % in [Hz]
+damps_m  = table2array(readtable('mat/mode_damps.txt',   'ReadVariableNames', false)); % in [%]
+modal_a  = table2array(readtable('mat/mode_modal_a.txt', 'ReadVariableNames', false)); % [Real / Imag]
+modal_b  = table2array(readtable('mat/mode_modal_b.txt', 'ReadVariableNames', false)); % [Real / Imag]
+
+%% Guess the number of modes identified from the length of the imported data.
+acc_n = 23; % Number of accelerometers
+dir_n = 3; % Number of directions
+dirs = 'XYZ';
+
+mod_n = size(shapes_m,1)/acc_n/dir_n; % Number of modes
+
+%% Mode shapes are split into 3 parts (direction plus sign, real part and imaginary part)
+% we aggregate them into one array of complex numbers
+T_sign = table2array(shapes_m(:, 1));
+T_real = table2array(shapes_m(:, 2));
+T_imag = table2array(shapes_m(:, 3));
+
+mode_shapes = zeros(mod_n, dir_n, acc_n);
+
+for mod_i = 1:mod_n
+  for acc_i = 1:acc_n
+    % Get the correct section of the signs
+    T = T_sign(acc_n*dir_n*(mod_i-1)+1:acc_n*dir_n*mod_i);
+    for dir_i = 1:dir_n
+      % Get the line corresponding to the sensor
+      i = find(contains(T, sprintf('%i%s',acc_i, dirs(dir_i))), 1, 'first')+acc_n*dir_n*(mod_i-1);
+      mode_shapes(mod_i, dir_i, acc_i) = str2num([T_sign{i}(end-1), '1'])*complex(T_real(i),T_imag(i));
+    end
+  end
+end
+
+%% Create the eigenvalue and eigenvector matrices
+eigen_val_M = diag(2*pi*freqs_m.*(-damps_m/100 + j*sqrt(1 - (damps_m/100).^2))); % Lambda = diagonal matrix
+eigen_vec_M = reshape(mode_shapes, [mod_n, acc_n*dir_n]).'; % Phi, vecnorm(eigen_vec_M) = 1
+
+% Add complex conjugate eigenvalues and eigenvectors
+eigen_val_ext_M = blkdiag(eigen_val_M, conj(eigen_val_M));
+eigen_vec_ext_M = [eigen_vec_M, conj(eigen_vec_M)];
+
+%% "Modal A" and "Modal B" matrices
+modal_a_M = diag(complex(modal_a(:, 1), modal_a(:, 2)));
+modal_b_M = diag(complex(modal_b(:, 1), modal_b(:, 2)));
+
+modal_a_ext_M = blkdiag(modal_a_M, conj(modal_a_M));
+modal_b_ext_M = blkdiag(modal_b_M, conj(modal_b_M));
+
+%% Synthesize the full FRF matrix from the modal model
+Hsyn = zeros(acc_n*dir_n, acc_n*dir_n, length(freqs));
+
+for i = 1:length(freqs)
+  Hsyn(:, :, i) = eigen_vec_ext_M*diag(1./(diag(modal_a_ext_M).*(j*2*pi*freqs(i) - diag(eigen_val_ext_M))))*eigen_vec_ext_M.';
+end
+
+%% Derivate two times to have the acceleration response
+for i = 1:size(Hsyn, 1)
+  Hsyn(i, :, :) = squeeze(Hsyn(i, :, :)).*(j*2*pi*freqs).^2;
+end
+
+acc_o = 11; dir_o = 3;
+acc_i = 11; dir_i = 3;
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(frf( 3*(acc_o-1)+dir_o,             dir_i, :))), 'DisplayName', 'Measured');
+plot(freqs, abs(squeeze(Hsyn(3*(acc_o-1)+dir_o, 3*(acc_i-1)+dir_i, :))), 'DisplayName', 'Synthesized');
+hold off;
+set(gca, 'xscale', 'lin');
+set(gca, 'yscale', 'log');
+xlabel('Frequency [Hz]');
+ylabel('Magnitude [$\frac{m/s^2}{N}$]');
+ldg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+ldg.ItemTokenSize = [10, 1];
+xticks([0:40:200]);
+xlim([1, 200]);
+ylim([1e-6, 1e-1]);
+
+acc_o = 15; dir_o = 3;
+acc_i = 11; dir_i = 3;
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(frf( 3*(acc_o-1)+dir_o,             dir_i, :))), 'DisplayName', 'Measured');
+plot(freqs, abs(squeeze(Hsyn(3*(acc_o-1)+dir_o, 3*(acc_i-1)+dir_i, :))), 'DisplayName', 'Synthesized');
+hold off;
+set(gca, 'xscale', 'lin');
+set(gca, 'yscale', 'log');
+xlabel('Frequency [Hz]');
+ylabel('Magnitude [$\frac{m/s^2}{N}$]');
+ldg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+ldg.ItemTokenSize = [10, 1];
+xticks([0:40:200]);
+xlim([1, 200]);
+ylim([1e-6, 1e-1]);
+
+acc_o = 2; dir_o = 1;
+acc_i = 11; dir_i = 2;
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(frf( 3*(acc_o-1)+dir_o,             dir_i, :))), 'DisplayName', 'Measured');
+plot(freqs, abs(squeeze(Hsyn(3*(acc_o-1)+dir_o, 3*(acc_i-1)+dir_i, :))), 'DisplayName', 'Synthesized');
+hold off;
+set(gca, 'xscale', 'lin');
+set(gca, 'yscale', 'log');
+xlabel('Frequency [Hz]');
+ylabel('Magnitude [$\frac{m/s^2}{N}$]');
+ldg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+ldg.ItemTokenSize = [10, 1];
+xticks([0:40:200]);
+xlim([1, 200]);
+ylim([1e-6, 1e-1]);

A4-simscape-micro-station/src/circlefit.m

@@ -0,0 +1,21 @@
+function   [xc,yc,R,a] = circlefit(x,y)
+%
+%   [xc yx R] = circfit(x,y)
+%
+%   fits a circle  in x,y plane in a more accurate
+%   (less prone to ill condition )
+%  procedure than circfit2 but using more memory
+%  x,y are column vector where (x(i),y(i)) is a measured point
+%
+%  result is center point (yc,xc) and radius R
+%  an optional output is the vector of coeficient a
+% describing the circle's equation
+%
+%   x^2+y^2+a(1)*x+a(2)*y+a(3)=0
+%
+%  By:  Izhak bucher 25/oct /1991,
+    x=x(:); y=y(:);
+    a=[x y ones(size(x))]\[-(x.^2+y.^2)];
+    xc = -.5*a(1);
+    yc = -.5*a(2);
+    R  =  sqrt((a(1)^2+a(2)^2)/4-a(3));

A4-simscape-micro-station/src/computeJacobian.m

@@ -0,0 +1,35 @@
+  function [stewart] = computeJacobian(stewart)
+  % computeJacobian -
+  %
+  % Syntax: [stewart] = computeJacobian(stewart)
+  %
+  % Inputs:
+  %    - stewart - With at least the following fields:
+  %      - geometry.As [3x6] - The 6 unit vectors for each strut expressed in {A}
+  %      - geometry.Ab [3x6] - The 6 position of the joints bi expressed in {A}
+  %      - actuators.K [6x1] - Total stiffness of the actuators
+  %
+  % Outputs:
+  %    - stewart - With the 3 added field:
+  %        - kinematics.J [6x6] - The Jacobian Matrix
+  %        - kinematics.K [6x6] - The Stiffness Matrix
+  %        - kinematics.C [6x6] - The Compliance Matrix
+
+  assert(isfield(stewart.geometry, 'As'),   'stewart.geometry should have attribute As')
+  As = stewart.geometry.As;
+
+  assert(isfield(stewart.geometry, 'Ab'),   'stewart.geometry should have attribute Ab')
+  Ab = stewart.geometry.Ab;
+
+  assert(isfield(stewart.actuators, 'K'),   'stewart.actuators should have attribute K')
+  Ki = stewart.actuators.K;
+
+  J = [As' , cross(Ab, As)'];
+
+  K = J'*diag(Ki)*J;
+
+  C = inv(K);
+
+  stewart.kinematics.J = J;
+  stewart.kinematics.K = K;
+  stewart.kinematics.C = C;

A4-simscape-micro-station/src/computeJointsPose.m

@@ -0,0 +1,78 @@
+  function [stewart] = computeJointsPose(stewart)
+  % computeJointsPose -
+  %
+  % Syntax: [stewart] = computeJointsPose(stewart)
+  %
+  % Inputs:
+  %    - stewart - A structure with the following fields
+  %        - platform_F.Fa   [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
+  %        - platform_M.Mb   [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
+  %        - platform_F.FO_A [3x1] - Position of {A} with respect to {F}
+  %        - platform_M.MO_B [3x1] - Position of {B} with respect to {M}
+  %        - geometry.FO_M   [3x1] - Position of {M} with respect to {F}
+  %
+  % Outputs:
+  %    - stewart - A structure with the following added fields
+  %        - geometry.Aa    [3x6]   - The i'th column is the position of ai with respect to {A}
+  %        - geometry.Ab    [3x6]   - The i'th column is the position of bi with respect to {A}
+  %        - geometry.Ba    [3x6]   - The i'th column is the position of ai with respect to {B}
+  %        - geometry.Bb    [3x6]   - The i'th column is the position of bi with respect to {B}
+  %        - geometry.l     [6x1]   - The i'th element is the initial length of strut i
+  %        - geometry.As    [3x6]   - The i'th column is the unit vector of strut i expressed in {A}
+  %        - geometry.Bs    [3x6]   - The i'th column is the unit vector of strut i expressed in {B}
+  %        - struts_F.l     [6x1]   - Length of the Fixed part of the i'th strut
+  %        - struts_M.l     [6x1]   - Length of the Mobile part of the i'th strut
+  %        - platform_F.FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}
+  %        - platform_M.MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
+
+  assert(isfield(stewart.platform_F, 'Fa'),   'stewart.platform_F should have attribute Fa')
+  Fa = stewart.platform_F.Fa;
+
+  assert(isfield(stewart.platform_M, 'Mb'),   'stewart.platform_M should have attribute Mb')
+  Mb = stewart.platform_M.Mb;
+
+  assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A')
+  FO_A = stewart.platform_F.FO_A;
+
+  assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B')
+  MO_B = stewart.platform_M.MO_B;
+
+  assert(isfield(stewart.geometry,   'FO_M'), 'stewart.geometry should have attribute FO_M')
+  FO_M = stewart.geometry.FO_M;
+
+  Aa = Fa - repmat(FO_A, [1, 6]);
+  Bb = Mb - repmat(MO_B, [1, 6]);
+
+  Ab = Bb - repmat(-MO_B-FO_M+FO_A, [1, 6]);
+  Ba = Aa - repmat( MO_B+FO_M-FO_A, [1, 6]);
+
+  As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As
+
+  l = vecnorm(Ab - Aa)';
+
+  Bs = (Bb - Ba)./vecnorm(Bb - Ba);
+
+  FRa = zeros(3,3,6);
+  MRb = zeros(3,3,6);
+
+  for i = 1:6
+    FRa(:,:,i) = [cross([0;1;0], As(:,i)) , cross(As(:,i), cross([0;1;0], As(:,i))) , As(:,i)];
+    FRa(:,:,i) = FRa(:,:,i)./vecnorm(FRa(:,:,i));
+
+    MRb(:,:,i) = [cross([0;1;0], Bs(:,i)) , cross(Bs(:,i), cross([0;1;0], Bs(:,i))) , Bs(:,i)];
+    MRb(:,:,i) = MRb(:,:,i)./vecnorm(MRb(:,:,i));
+  end
+
+  stewart.geometry.Aa = Aa;
+  stewart.geometry.Ab = Ab;
+  stewart.geometry.Ba = Ba;
+  stewart.geometry.Bb = Bb;
+  stewart.geometry.As = As;
+  stewart.geometry.Bs = Bs;
+  stewart.geometry.l  = l;
+
+  stewart.struts_F.l  = l/2;
+  stewart.struts_M.l  = l/2;
+
+  stewart.platform_F.FRa = FRa;
+  stewart.platform_M.MRb = MRb;

A4-simscape-micro-station/src/computeReferencePose.m

@@ -0,0 +1,77 @@
+  function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
+  % computeReferencePose - Compute the homogeneous transformation matrix corresponding to the wanted pose of the sample
+  %
+  % Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
+  %
+  % Inputs:
+  %    - Dy - Reference of the Translation Stage [m]
+  %    - Ry - Reference of the Tilt Stage [rad]
+  %    - Rz - Reference of the Spindle [rad]
+  %    - Dh - Reference of the Micro Hexapod (Pitch, Roll, Yaw angles) [m, m, m, rad, rad, rad]
+  %    - Dn - Reference of the Nano Hexapod [m, m, m, rad, rad, rad]
+  %
+  % Outputs:
+  %    - WTr -
+
+    %% Translation Stage
+    Rty = [1 0 0 0;
+           0 1 0 Dy;
+           0 0 1 0;
+           0 0 0 1];
+
+    %% Tilt Stage - Pure rotating aligned with Ob
+    Rry = [ cos(Ry) 0 sin(Ry) 0;
+            0       1 0       0;
+           -sin(Ry) 0 cos(Ry) 0;
+            0       0 0       1];
+
+    %% Spindle - Rotation along the Z axis
+    Rrz = [cos(Rz) -sin(Rz) 0 0 ;
+           sin(Rz)  cos(Rz) 0 0 ;
+           0        0       1 0 ;
+           0        0       0 1 ];
+
+
+    %% Micro-Hexapod
+    Rhx = [1 0           0;
+           0 cos(Dh(4)) -sin(Dh(4));
+           0 sin(Dh(4))  cos(Dh(4))];
+
+    Rhy = [ cos(Dh(5)) 0 sin(Dh(5));
+           0           1 0;
+           -sin(Dh(5)) 0 cos(Dh(5))];
+
+    Rhz = [cos(Dh(6)) -sin(Dh(6)) 0;
+           sin(Dh(6))  cos(Dh(6)) 0;
+           0           0          1];
+
+    Rh = [1 0 0 Dh(1) ;
+          0 1 0 Dh(2) ;
+          0 0 1 Dh(3) ;
+          0 0 0 1 ];
+
+    Rh(1:3, 1:3) = Rhz*Rhy*Rhx;
+
+    %% Nano-Hexapod
+    Rnx = [1 0           0;
+           0 cos(Dn(4)) -sin(Dn(4));
+           0 sin(Dn(4))  cos(Dn(4))];
+
+    Rny = [ cos(Dn(5)) 0 sin(Dn(5));
+           0           1 0;
+           -sin(Dn(5)) 0 cos(Dn(5))];
+
+    Rnz = [cos(Dn(6)) -sin(Dn(6)) 0;
+           sin(Dn(6))  cos(Dn(6)) 0;
+           0           0          1];
+
+    Rn = [1 0 0 Dn(1) ;
+          0 1 0 Dn(2) ;
+          0 0 1 Dn(3) ;
+          0 0 0 1 ];
+
+    Rn(1:3, 1:3) = Rnz*Rny*Rnx;
+
+    %% Total Homogeneous transformation
+    WTr = Rty*Rry*Rrz*Rh*Rn;
+  end

A4-simscape-micro-station/src/describeMicroStationSetup.m

@@ -0,0 +1,141 @@
+  function [] = describeMicroStationSetup()
+  % describeMicroStationSetup -
+  %
+  % Syntax: [] = describeMicroStationSetup()
+  %
+  % Inputs:
+  %    -  -
+  %
+  % Outputs:
+  %    -  -
+
+  load('./mat/nass_model_conf_simscape.mat', 'conf_simscape');
+
+  fprintf('Simscape Configuration:\n');
+
+  if conf_simscape.type == 1
+      fprintf('- Gravity is included\n');
+  else
+      fprintf('- Gravity is not included\n');
+  end
+
+  fprintf('\n');
+
+  load('./mat/nass_model_disturbances.mat', 'args');
+
+  fprintf('Disturbances:\n');
+  if ~args.enable
+      fprintf('- No disturbance is included\n');
+  else
+      if args.Dwx && args.Dwy && args.Dwz
+          fprintf('- Ground motion\n');
+      end
+      if args.Fdy_x && args.Fdy_z
+          fprintf('- Vibrations of the Translation Stage\n');
+      end
+      if args.Frz_z
+          fprintf('- Vibrations of the Spindle\n');
+      end
+  end
+  fprintf('\n');
+
+  load('./mat/nass_model_references.mat', 'args');
+
+  fprintf('Reference Tracking:\n');
+  fprintf('- Translation Stage:\n');
+  switch args.Dy_type
+    case 'constant'
+      fprintf('  - Constant Position\n');
+      fprintf('  - Dy = %.0f [mm]\n', args.Dy_amplitude*1e3);
+    case 'triangular'
+      fprintf('  - Triangular Path\n');
+      fprintf('  - Amplitude = %.0f [mm]\n', args.Dy_amplitude*1e3);
+      fprintf('  - Period = %.0f [s]\n', args.Dy_period);
+    case 'sinusoidal'
+      fprintf('  - Sinusoidal Path\n');
+      fprintf('  - Amplitude = %.0f [mm]\n', args.Dy_amplitude*1e3);
+      fprintf('  - Period = %.0f [s]\n', args.Dy_period);
+  end
+
+  fprintf('- Tilt Stage:\n');
+  switch args.Ry_type
+    case 'constant'
+      fprintf('  - Constant Position\n');
+      fprintf('  - Ry = %.0f [mm]\n', args.Ry_amplitude*1e3);
+    case 'triangular'
+      fprintf('  - Triangular Path\n');
+      fprintf('  - Amplitude = %.0f [mm]\n', args.Ry_amplitude*1e3);
+      fprintf('  - Period = %.0f [s]\n', args.Ry_period);
+    case 'sinusoidal'
+      fprintf('  - Sinusoidal Path\n');
+      fprintf('  - Amplitude = %.0f [mm]\n', args.Ry_amplitude*1e3);
+      fprintf('  - Period = %.0f [s]\n', args.Ry_period);
+  end
+
+  fprintf('- Spindle:\n');
+  switch args.Rz_type
+    case 'constant'
+      fprintf('  - Constant Position\n');
+      fprintf('  - Rz = %.0f [deg]\n', 180/pi*args.Rz_amplitude);
+    case { 'rotating', 'rotating-not-filtered' }
+      fprintf('  - Rotating\n');
+      fprintf('  - Speed = %.0f [rpm]\n', 60/args.Rz_period);
+  end
+
+
+  fprintf('- Micro Hexapod:\n');
+  switch args.Dh_type
+    case 'constant'
+      fprintf('  - Constant Position\n');
+      fprintf('  - Dh = %.0f, %.0f, %.0f [mm]\n',  args.Dh_pos(1), args.Dh_pos(2), args.Dh_pos(3));
+      fprintf('  - Rh = %.0f, %.0f, %.0f [deg]\n', args.Dh_pos(4), args.Dh_pos(5), args.Dh_pos(6));
+  end
+
+  fprintf('\n');
+
+  load('./mat/nass_model_stages.mat', 'ground', 'granite', 'ty', 'ry', 'rz', 'micro_hexapod', 'axisc');
+
+  fprintf('Micro Station:\n');
+
+  if granite.type == 1 && ...
+          ty.type == 1 && ...
+          ry.type == 1 && ...
+          rz.type == 1 && ...
+          micro_hexapod.type == 1;
+      fprintf('- All stages are rigid\n');
+  elseif granite.type == 2 && ...
+          ty.type == 2 && ...
+          ry.type == 2 && ...
+          rz.type == 2 && ...
+          micro_hexapod.type == 2;
+      fprintf('- All stages are flexible\n');
+  else
+      if granite.type == 1 || granite.type == 4
+          fprintf('- Granite is rigid\n');
+      else
+          fprintf('- Granite is flexible\n');
+      end
+      if ty.type == 1 || ty.type == 4
+          fprintf('- Translation Stage is rigid\n');
+      else
+          fprintf('- Translation Stage is flexible\n');
+      end
+      if ry.type == 1 || ry.type == 4
+          fprintf('- Tilt Stage is rigid\n');
+      else
+          fprintf('- Tilt Stage is flexible\n');
+      end
+      if rz.type == 1 || rz.type == 4
+          fprintf('- Spindle is rigid\n');
+      else
+          fprintf('- Spindle is flexible\n');
+      end
+      if micro_hexapod.type == 1 || micro_hexapod.type == 4
+          fprintf('- Micro Hexapod is rigid\n');
+      else
+          fprintf('- Micro Hexapod is flexible\n');
+      end
+
+  end
+
+  fprintf('\n');

A4-simscape-micro-station/src/generateGeneralConfiguration.m

@@ -0,0 +1,39 @@
+  function [stewart] = generateGeneralConfiguration(stewart, args)
+  % generateGeneralConfiguration - Generate a Very General Configuration
+  %
+  % Syntax: [stewart] = generateGeneralConfiguration(stewart, args)
+  %
+  % Inputs:
+  %    - args - Can have the following fields:
+  %        - FH  [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m]
+  %        - FR  [1x1] - Radius of the position of the fixed joints in the X-Y [m]
+  %        - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad]
+  %        - MH  [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m]
+  %        - FR  [1x1] - Radius of the position of the mobile joints in the X-Y [m]
+  %        - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad]
+  %
+  % Outputs:
+  %    - stewart - updated Stewart structure with the added fields:
+  %        - platform_F.Fa  [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
+  %        - platform_M.Mb  [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
+
+  arguments
+      stewart
+      args.FH  (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
+      args.FR  (1,1) double {mustBeNumeric, mustBePositive} = 115e-3;
+      args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180);
+      args.MH  (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
+      args.MR  (1,1) double {mustBeNumeric, mustBePositive} = 90e-3;
+      args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
+  end
+
+  Fa = zeros(3,6);
+  Mb = zeros(3,6);
+
+  for i = 1:6
+    Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i));  args.FH];
+    Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH];
+  end
+
+  stewart.platform_F.Fa = Fa;
+  stewart.platform_M.Mb = Mb;

A4-simscape-micro-station/src/initializeCylindricalPlatforms.m

@@ -0,0 +1,59 @@
+  function [stewart] = initializeCylindricalPlatforms(stewart, args)
+  % initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms
+  %
+  % Syntax: [stewart] = initializeCylindricalPlatforms(args)
+  %
+  % Inputs:
+  %    - args - Structure with the following fields:
+  %        - Fpm [1x1] - Fixed Platform Mass [kg]
+  %        - Fph [1x1] - Fixed Platform Height [m]
+  %        - Fpr [1x1] - Fixed Platform Radius [m]
+  %        - Mpm [1x1] - Mobile Platform Mass [kg]
+  %        - Mph [1x1] - Mobile Platform Height [m]
+  %        - Mpr [1x1] - Mobile Platform Radius [m]
+  %
+  % Outputs:
+  %    - stewart - updated Stewart structure with the added fields:
+  %      - platform_F [struct] - structure with the following fields:
+  %        - type = 1
+  %        - M [1x1] - Fixed Platform Mass [kg]
+  %        - I [3x3] - Fixed Platform Inertia matrix [kg*m^2]
+  %        - H [1x1] - Fixed Platform Height [m]
+  %        - R [1x1] - Fixed Platform Radius [m]
+  %      - platform_M [struct] - structure with the following fields:
+  %        - M [1x1] - Mobile Platform Mass [kg]
+  %        - I [3x3] - Mobile Platform Inertia matrix [kg*m^2]
+  %        - H [1x1] - Mobile Platform Height [m]
+  %        - R [1x1] - Mobile Platform Radius [m]
+
+  arguments
+      stewart
+      args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
+      args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
+      args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e-3
+      args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
+      args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
+      args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
+  end
+
+  I_F = diag([1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
+              1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
+              1/2 *args.Fpm * args.Fpr^2]);
+
+  I_M = diag([1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
+              1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
+              1/2 *args.Mpm * args.Mpr^2]);
+
+  stewart.platform_F.type = 1;
+
+  stewart.platform_F.I = I_F;
+  stewart.platform_F.M = args.Fpm;
+  stewart.platform_F.R = args.Fpr;
+  stewart.platform_F.H = args.Fph;
+
+  stewart.platform_M.type = 1;
+
+  stewart.platform_M.I = I_M;
+  stewart.platform_M.M = args.Mpm;
+  stewart.platform_M.R = args.Mpr;
+  stewart.platform_M.H = args.Mph;

A4-simscape-micro-station/src/initializeCylindricalStruts.m

@@ -0,0 +1,71 @@
+  function [stewart] = initializeCylindricalStruts(stewart, args)
+  % initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts
+  %
+  % Syntax: [stewart] = initializeCylindricalStruts(args)
+  %
+  % Inputs:
+  %    - args - Structure with the following fields:
+  %        - Fsm [1x1] - Mass of the Fixed part of the struts [kg]
+  %        - Fsh [1x1] - Height of cylinder for the Fixed part of the struts [m]
+  %        - Fsr [1x1] - Radius of cylinder for the Fixed part of the struts [m]
+  %        - Msm [1x1] - Mass of the Mobile part of the struts [kg]
+  %        - Msh [1x1] - Height of cylinder for the Mobile part of the struts [m]
+  %        - Msr [1x1] - Radius of cylinder for the Mobile part of the struts [m]
+  %
+  % Outputs:
+  %    - stewart - updated Stewart structure with the added fields:
+  %      - struts_F [struct] - structure with the following fields:
+  %        - M [6x1]   - Mass of the Fixed part of the struts [kg]
+  %        - I [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2]
+  %        - H [6x1]   - Height of cylinder for the Fixed part of the struts [m]
+  %        - R [6x1]   - Radius of cylinder for the Fixed part of the struts [m]
+  %      - struts_M [struct] - structure with the following fields:
+  %        - M [6x1]   - Mass of the Mobile part of the struts [kg]
+  %        - I [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2]
+  %        - H [6x1]   - Height of cylinder for the Mobile part of the struts [m]
+  %        - R [6x1]   - Radius of cylinder for the Mobile part of the struts [m]
+
+  arguments
+      stewart
+      args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
+      args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
+      args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
+      args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
+      args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
+      args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
+  end
+
+  Fsm = ones(6,1).*args.Fsm;
+  Fsh = ones(6,1).*args.Fsh;
+  Fsr = ones(6,1).*args.Fsr;
+
+  Msm = ones(6,1).*args.Msm;
+  Msh = ones(6,1).*args.Msh;
+  Msr = ones(6,1).*args.Msr;
+
+  I_F = zeros(3, 3, 6); % Inertia of the "fixed" part of the strut
+  I_M = zeros(3, 3, 6); % Inertia of the "mobile" part of the strut
+
+  for i = 1:6
+    I_F(:,:,i) = diag([1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
+                       1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
+                       1/2  * Fsm(i) * Fsr(i)^2]);
+
+    I_M(:,:,i) = diag([1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
+                       1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
+                       1/2  * Msm(i) * Msr(i)^2]);
+  end
+
+  stewart.struts_M.type = 1;
+
+  stewart.struts_M.I = I_M;
+  stewart.struts_M.M = Msm;
+  stewart.struts_M.R = Msr;
+  stewart.struts_M.H = Msh;
+
+  stewart.struts_F.type = 1;
+
+  stewart.struts_F.I = I_F;
+  stewart.struts_F.M = Fsm;
+  stewart.struts_F.R = Fsr;
+  stewart.struts_F.H = Fsh;