Huge Change

- Add may folders
- Add IFF and HAC-LAC scripts

.SimulinkProject/Root.type.ProjectPath/1809d361-dc3d-4fcb-8b9b-833180e2f6fc.type.Reference.xml

@@ -0,0 +1,2 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<Info Ref="active_damping" Type="Relative" />

.SimulinkProject/Root.type.ProjectPath/2793b451-85f9-44a8-8d10-25d7657da4e0.type.Reference.xml

@@ -1,2 +0,0 @@
-<?xml version='1.0' encoding='UTF-8'?>
-<Info Ref="Identification" Type="Relative" />

.SimulinkProject/Root.type.ProjectPath/56b46427-db89-446e-ab1b-3a8188f5e1b3.type.Reference.xml

@@ -1,2 +1,2 @@
 <?xml version='1.0' encoding='UTF-8'?>
-<Info Ref="Control" Type="Relative" />
+<Info Ref="control" Type="Relative" />

.SimulinkProject/Root.type.ProjectPath/7e53593c-166d-46eb-8350-609f08763240.type.Reference.xml

@@ -1,2 +1,2 @@
 <?xml version='1.0' encoding='UTF-8'?>
-<Info Ref="Analysis" Type="Relative" />
+<Info Ref="analysis" Type="Relative" />

.SimulinkProject/Root.type.ProjectPath/83f0e724-9931-453a-b699-3bd293db000c.type.Reference.xml

@@ -0,0 +1,2 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<Info Ref="hac_lac" Type="Relative" />

.SimulinkProject/Root.type.ProjectPath/a1959b94-b957-42df-87b8-4945482358a6.type.Reference.xml

@@ -0,0 +1,2 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<Info Ref="identification" Type="Relative" />

.SimulinkProject/Root.type.ProjectPath/d03bc20c-98e3-4ed4-a859-51e1f5a18637.type.Reference.xml

@@ -0,0 +1,2 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<Info Ref="demonstration" Type="Relative" />

Control/cl_tf.m

@@ -1,23 +0,0 @@
-%%
-clear; close all; clc;
-
-%% Load Plant
-load('./mat/G_xg_to_d.mat', 'G_xg_to_d');
-load('./mat/G_f_to_d.mat', 'G_1', 'G_20', 'G_50');
-load('./mat/controller.mat', 'K');
-
-%%
-S = minreal(inv(tf(eye(6))+G_20*K));
-T = minreal((tf(eye(6))+G_20*K)\G_20*K);
-
-bodeFig({S(1,1), T(1,1)})
-legend({'$S_x$', '$T_x$'})
-
-bodeFig({S(2,2), T(2,2)})
-legend({'$S_y$', '$T_y$'})
-
-bodeFig({S(3,3), T(3,3)})
-legend({'$S_z$', '$T_z$'})
-
-%%
-save('./mat/T_S.mat', 'S', 'T');

Control/generate_control.m

@@ -1,55 +0,0 @@
-%%
-clear; close all; clc;
-
-%% Load Plant
-load('./mat/G_f_to_d.mat', 'G_20');
-
-%% Load previously generated controllers
-load('./mat/control_K_tx.mat', 'K_tx');
-load('./mat/control_K_ty.mat', 'K_ty');
-load('./mat/control_K_tz.mat', 'K_tz');
-load('./mat/control_K_rx.mat', 'K_rx');
-load('./mat/control_K_ry.mat', 'K_ry');
-load('./mat/control_K_rz.mat', 'K_rz');
-
-%%
-sisotool('bode', G_20(1, 1), K_tx);
-K_tx = C;
-save('./mat/control_K_tx.mat', 'K_tx');
-
-%%
-sisotool('bode', G_20(2, 2), K_ty);
-K_ty = C;
-save('./mat/control_K_ty.mat', 'K_ty');
-
-%%
-sisotool('bode', G_20(3, 3), K_tz);
-K_tz = C;
-save('./mat/control_K_tz.mat', 'K_tz');
-
-%%
-sisotool('bode', G_20(4, 4), K_rx);
-K_rx = C;
-save('./mat/control_K_rx.mat', 'K_rx');
-
-%%
-sisotool('bode', G_20(5, 5), K_ry);
-K_ry = C;
-save('./mat/control_K_ry.mat', 'K_ry');
-
-%%
-sisotool('bode', G_20(6, 6), K_rz);
-K_rz = C;
-save('./mat/control_K_rz.mat', 'K_rz');
-
-%%
-K = tf(zeros(6));
-K(1,1) = K_tx;
-K(2,2) = K_ty;
-K(3,3) = K_tz;
-K(4,4) = K_rx;
-K(5,5) = K_ry;
-K(6,6) = K_rz;
-
-%% Save the MIMO control
-save('./mat/controller.mat', 'K');

active_damping/act_damp_main.m

@@ -0,0 +1,28 @@
+%%
+clear; close all; clc;
+
+%% IFF: Integral Force Feedback Control
+% Generate the IFF Control Laws
+run iff_control.m
+
+% Identification of the TF of damped system
+run iff_identification.m
+
+% Compare undamped and damped system
+run iff_comp_tf.m
+
+% Generate Control Laws with the damped system
+run iff_fb_control.m
+
+% Plot Loop Gains for the new control laws
+run iff_fb_control_plots.m
+
+% Simulation of the damped system
+run iff_simulation.m
+
+% Plot results of the simulations
+run iff_results.m
+
+%% DVF: Direct Velocity Feedback
+% Generate the DVF Control Laws
+run dvf_control.m

active_damping/dvf_control.m

@@ -0,0 +1,22 @@
+%%
+clear; close all; clc;
+
+%% Load the identified transfer functions
+load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
+
+%% Load Configuration file
+load('./mat/config.mat', 'save_fig', 'freqs');
+
+%%
+s = tf('s');
+
+%%
+% sisotool(-G_heavy_pz.G_dvf('Vnx', 'Fnx'))
+
+K_dvf_light_vc = tf(eye(6));
+K_dvf_light_pz = tf(eye(6));
+K_dvf_heavy_vc = tf(eye(6));
+K_dvf_heavy_pz = tf(eye(6));
+
+%%
+save('./mat/K_dvf_crit.mat', 'K_dvf_light_vc', 'K_dvf_light_pz', 'K_dvf_heavy_vc', 'K_dvf_heavy_pz');

active_damping/iff_comp_tf.m

@@ -0,0 +1,74 @@
+%%
+clear; close all; clc;
+
+%% Load System and Damped System
+load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
+load('./mat/G_iff.mat', 'G_iff_light_vc', 'G_iff_light_pz', 'G_iff_heavy_vc', 'G_iff_heavy_pz');
+
+%% Load Configuration file
+load('./mat/config.mat', 'save_fig', 'freqs');
+
+%% New Plant damped
+figure;
+% Amplitude
+ax1 = subaxis(2,1,1);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '-', 'DisplayName', 'Light VC');
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '-', 'DisplayName', 'Light PZ');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_iff_light_vc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'DisplayName', 'Damped');
+plot(freqs, abs(squeeze(freqresp(G_iff_light_pz.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'DisplayName', 'Damped');
+set(gca,'xscale','log'); set(gca,'yscale','log');
+ylabel('Amplitude [m/N]');
+set(gca, 'XTickLabel',[]);
+legend('Location', 'southwest');
+hold off;
+% Phase
+ax2 = subaxis(2,1,2);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '-');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '-');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_light_vc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_light_pz.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
+set(gca,'xscale','log');
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1) freqs(end)]);
+
+if save_fig; exportFig('damping_comp_plant', 'normal-normal', struct('path', 'active_damping')); end
+
+%% From xw to d
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '-', 'DisplayName', 'Light VC');
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '-', 'DisplayName', 'Light PZ');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_iff_light_vc.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'DisplayName', 'Damped');
+plot(freqs, abs(squeeze(freqresp(G_iff_light_pz.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'DisplayName', 'Damped');
+hold off;
+xlim([freqs(1) freqs(end)]);
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
+legend('Location', 'southwest');
+
+if save_fig; exportFig('damping_comp_xw', 'normal-normal', struct('path', 'active_damping')); end
+
+%% From fi to d
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '-', 'DisplayName', 'Light VC');
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '-', 'DisplayName', 'Light PZ');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_iff_light_vc.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'DisplayName', 'Damped');
+plot(freqs, abs(squeeze(freqresp(G_iff_light_pz.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'DisplayName', 'Damped');
+hold off;
+xlim([freqs(1) freqs(end)]);
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+legend('Location', 'southwest');
+
+if save_fig; exportFig('damping_comp_fi', 'normal-normal', struct('path', 'active_damping')); end

active_damping/iff_control.m

@@ -0,0 +1,22 @@
+%%
+clear; close all; clc;
+
+%% Load the identified transfer functions
+load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
+
+%% Load Configuration file
+load('./mat/config.mat', 'save_fig', 'freqs');
+
+%%
+s = tf('s');
+
+%%
+% sisotool(-G_heavy_pz.G_iff('Fm1', 'F1')/s)
+
+K_iff_light_vc = 48/s*tf(eye(6));
+K_iff_light_pz = 1500/s*tf(eye(6));
+K_iff_heavy_vc = 20/s*tf(eye(6));
+K_iff_heavy_pz = 535/s*tf(eye(6));
+
+%%
+save('./mat/K_iff_crit.mat', 'K_iff_light_vc', 'K_iff_light_pz', 'K_iff_heavy_vc', 'K_iff_heavy_pz');

active_damping/iff_identification.m

@@ -0,0 +1,34 @@
+%%
+clear; close all; clc;
+
+%% Load IFF Controllers
+load('./mat/K_iff_crit.mat', 'K_iff_light_vc', 'K_iff_light_pz', 'K_iff_heavy_vc', 'K_iff_heavy_pz');
+
+%% Light Sample
+initializeSample(struct('mass', 1));
+
+initializeNanoHexapod(struct('actuator', 'lorentz'));
+K_iff = K_iff_light_vc; %#ok
+save('./mat/K_iff.mat', 'K_iff');
+G_iff_light_vc = identifyPlant();
+
+initializeNanoHexapod(struct('actuator', 'piezo'));
+K_iff = K_iff_light_pz; %#ok
+save('./mat/K_iff.mat', 'K_iff');
+G_iff_light_pz = identifyPlant();
+
+%% Heavy Sample
+initializeSample(struct('mass', 50));
+
+initializeNanoHexapod(struct('actuator', 'lorentz'));
+K_iff = K_iff_heavy_vc; %#ok
+save('./mat/K_iff.mat', 'K_iff');
+G_iff_heavy_vc = identifyPlant();
+
+initializeNanoHexapod(struct('actuator', 'piezo'));
+K_iff = K_iff_heavy_pz;
+save('./mat/K_iff.mat', 'K_iff');
+G_iff_heavy_pz = identifyPlant();
+
+%% Save the obtained transfer functions
+save('./mat/G_iff.mat', 'G_iff_light_vc', 'G_iff_light_pz', 'G_iff_heavy_vc', 'G_iff_heavy_pz');

active_damping/iff_results.m

@@ -0,0 +1,39 @@
+%%
+clear; close all; clc;
+
+%% Load Configuration file
+load('./mat/config.mat', 'save_fig', 'freqs');
+
+%% Load Simulation Results
+sim_light_vc_ol     = load('./mat/sim_light_vc_ol_none.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
+sim_light_pz_ol     = load('./mat/sim_light_pz_ol_none.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
+
+sim_light_vc_cl     = load('./mat/sim_light_vc_cl_none.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
+sim_light_pz_cl     = load('./mat/sim_light_pz_cl_none.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
+
+sim_light_vc_ol_iff = load('./mat/sim_light_vc_ol_iff.mat',  'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
+sim_light_pz_ol_iff = load('./mat/sim_light_pz_ol_iff.mat',  'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
+
+sim_light_vc_cl_iff = load('./mat/sim_light_vc_cl_iff.mat',  'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
+sim_light_pz_cl_iff = load('./mat/sim_light_pz_cl_iff.mat',  'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
+
+%%
+figure;
+hold on;
+plot(sim_light_vc_ol.Dx, sim_light_vc_ol.Dy);
+plot(sim_light_vc_cl.Dx, sim_light_vc_cl.Dy);
+plot(sim_light_vc_ol_iff.Dx, sim_light_vc_ol_iff.Dy);
+plot(sim_light_vc_cl_iff.Dx, sim_light_vc_cl_iff.Dy);
+hold off;
+
+%%
+rms(sqrt(sim_light_vc_ol.Dx.^2+sim_light_vc_ol.Dy.^2))
+rms(sqrt(sim_light_vc_cl.Dx.^2+sim_light_vc_cl.Dy.^2))
+rms(sqrt(sim_light_vc_ol_iff.Dx.^2+sim_light_vc_ol_iff.Dy.^2))
+rms(sqrt(sim_light_vc_cl_iff.Dx.^2+sim_light_vc_cl_iff.Dy.^2))
+
+%%
+rms(sqrt(sim_light_pz_ol.Dx.^2+sim_light_pz_ol.Dy.^2))
+rms(sqrt(sim_light_pz_cl.Dx.^2+sim_light_pz_cl.Dy.^2))
+rms(sqrt(sim_light_pz_ol_iff.Dx.^2+sim_light_pz_ol_iff.Dy.^2))
+rms(sqrt(sim_light_pz_cl_iff.Dx.^2+sim_light_pz_cl_iff.Dy.^2))

active_damping/iff_simulation.m

@@ -0,0 +1,11 @@
+%%
+clear; close all; clc;
+
+%% Initialize Simulation and Inputs
+initializeExperiment('tomography', 'light');
+
+%% Run Open Loop Simulations
+runSimulation('vc', 'light', 'ol', 'iff');
+runSimulation('pz', 'light', 'ol', 'iff');
+% runSimulation('vc', 'heavy', 'ol', 'iff');
+% runSimulation('pz', 'heavy', 'ol', 'iff');

config.m

@@ -1,10 +1,11 @@
 %% Add folders to Matlab Path
-% addpath('./Analysis/');
+% addpath('./analysis/');
-% addpath('./Control/');
+% addpath('./control/');
-% addpath('./Identification/');
+% addpath('./identification/');
 % addpath('./initialize/');
 % addpath('./src/');
 % addpath('./stewart-simscape/');
+% addpath('./active_damping/');
 
 %%
 freqs = logspace(-1, 3, 1000);

control/control_cl_ol_plots.m

@@ -0,0 +1,90 @@
+%%
+clear; close all; clc;
+
+%% Load Configuration file
+load('./mat/config.mat', 'save_fig', 'freqs');
+
+%% Load Simulation Results
+sim_light_vc_ol = load('./mat/sim_light_vc_ol.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
+sim_light_vc_cl = load('./mat/sim_light_vc_cl.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
+sim_light_pz_ol = load('./mat/sim_light_pz_ol.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
+sim_light_pz_cl = load('./mat/sim_light_pz_cl.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
+
+%% Start after few seconds
+T_init = 1;
+
+%% Plot X against Y in OL - Piezo and Voice Coil
+figure;
+hold on;
+plot(1e9*sim_light_vc_ol.Dx, 1e9*sim_light_vc_ol.Dy);
+plot(1e9*sim_light_pz_ol.Dx, 1e9*sim_light_pz_ol.Dy);
+hold off;
+xlabel('X Displacement [nm]'); ylabel('Y Displacement [nm]');
+xlim([-1000 1000]); ylim([-1000 1000]);
+xticks(-1000:200:1000); yticks(-1000:200:1000);
+legend({'VC - Light - OL', 'PZ - Light - OL'});
+
+if save_fig; exportFig('xy_ol_vc_pz', 'normal-normal', struct('path', 'control')); end
+
+%% Plot X against Y in CL - Piezo and Voice Coil
+figure;
+hold on;
+plot(1e9*sim_light_vc_cl.Dx, 1e9*sim_light_vc_cl.Dy);
+plot(1e9*sim_light_pz_cl.Dx, 1e9*sim_light_pz_cl.Dy);
+hold off;
+xlabel('X Displacement [nm]'); ylabel('Y Displacement [nm]');
+xlim([-500 500]); ylim([-500 500]);
+xticks(-500:100:500); yticks(-500:100:500);
+legend({'VC - Light - CL', 'PZ - Light - CL'});
+
+if save_fig; exportFig('xy_cl_vc_pz', 'normal-normal', struct('path', 'control')); end
+
+%% Compute the RMS Values
+i_init = find(sim_light_vc_ol.time > T_init, 1);
+
+rms_light_vc_ol = rms(sqrt(sim_light_vc_ol.Dx(i_init:end).^2+sim_light_vc_ol.Dy(i_init:end).^2));
+rms_light_pz_ol = rms(sqrt(sim_light_pz_ol.Dx(i_init:end).^2+sim_light_pz_ol.Dy(i_init:end).^2));
+rms_light_vc_cl = rms(sqrt(sim_light_vc_cl.Dx(i_init:end).^2+sim_light_vc_cl.Dy(i_init:end).^2));
+rms_light_pz_cl = rms(sqrt(sim_light_pz_cl.Dx(i_init:end).^2+sim_light_pz_cl.Dy(i_init:end).^2));
+
+fprintf('   \t OL   \t CL   [nm RMS]\n');
+fprintf('PZ \t %.0f \t %.0f \n',   1e9*rms_light_pz_ol, 1e9*rms_light_pz_cl);
+fprintf('VC \t %.0f \t %.0f \n\n', 1e9*rms_light_vc_ol, 1e9*rms_light_vc_cl);
+
+%% Compute the PSD
+sim_light_vc_ol.psd = computePsdDispl(sim_light_vc_ol, 1, 2);
+sim_light_pz_ol.psd = computePsdDispl(sim_light_pz_ol, 1, 2);
+sim_light_vc_cl.psd = computePsdDispl(sim_light_vc_cl, 1, 2);
+sim_light_pz_cl.psd = computePsdDispl(sim_light_pz_cl, 1, 2);
+
+%% PSD Open Loop and Close Loop for the X direction
+figure;
+hold on;
+plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.dx, 'DisplayName', 'VC - $T_x$ - OL');
+plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.dx, 'DisplayName', 'PZ - $T_x$ - OL');
+set(gca,'ColorOrderIndex',1);
+plot(sim_light_vc_cl.psd.f, sim_light_vc_cl.psd.dx, '--', 'DisplayName', 'VC - $T_x$ - CL');
+plot(sim_light_pz_cl.psd.f, sim_light_pz_cl.psd.dx, '--', 'DisplayName', 'PZ - $T_x$ - CL');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [$m^2/Hz$]'); xlabel('Frequency [Hz]');
+xlim([sim_light_vc_ol.psd.f(1), sim_light_vc_ol.psd.f(end)])
+hold off;
+legend('Location', 'southwest');
+
+if save_fig; exportFig('psd_ol_cl_pz_vc_light_tx', 'normal-normal', struct('path', 'control')); end
+
+%% PSD Open Loop and Close Loop for the Z direction
+figure;
+hold on;
+plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.dz, 'DisplayName', 'VC - $T_z$ - OL');
+plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.dz, 'DisplayName', 'PZ - $T_z$ - OL');
+set(gca,'ColorOrderIndex',1);
+plot(sim_light_vc_cl.psd.f, sim_light_vc_cl.psd.dz, '--', 'DisplayName', 'VC - $T_z$ - CL');
+plot(sim_light_pz_cl.psd.f, sim_light_pz_cl.psd.dz, '--', 'DisplayName', 'PZ - $T_z$ - CL');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [$m^2/Hz$]'); xlabel('Frequency [Hz]');
+xlim([sim_light_vc_ol.psd.f(1), sim_light_vc_ol.psd.f(end)])
+hold off;
+legend('Location', 'southwest');
+
+if save_fig; exportFig('psd_ol_cl_pz_vc_light_tz', 'normal-normal', struct('path', 'control')); end

control/control_cl_sim.m

@@ -0,0 +1,11 @@
+%%
+clear; close all; clc;
+
+%% Initialize Simulation and Inputs
+initializeExperiment('tomography', 'light');
+
+%% Run Close Loop Simulations
+runSimulation('vc', 'light', 'cl', 'none');
+runSimulation('pz', 'light', 'cl', 'none');
+% runSimulation('vc', 'heavy', 'cl', 'none');
+% runSimulation('pz', 'heavy', 'cl', 'none');

control/control_cl_tf.m

@@ -0,0 +1,34 @@
+%%
+clear; close all; clc;
+
+%% Load Controllers
+load('./mat/K_fb.mat', 'K_light_vc', 'K_light_pz', 'K_heavy_vc', 'K_heavy_pz');
+
+%% Closed Loop - Light Sample
+initializeSample(struct('mass', 1));
+
+initializeNanoHexapod(struct('actuator', 'lorentz'));
+K = K_light_vc; %#ok
+save('./mat/controller.mat', 'K');
+Gd_cl_light_vc = identifyPlant();
+
+initializeNanoHexapod(struct('actuator', 'piezo'));
+K = K_light_pz; %#ok
+save('./mat/controller.mat', 'K');
+Gd_cl_light_pz = identifyPlant();
+
+%% Closed Loop - Heavy Sample
+initializeSample(struct('mass', 50));
+
+initializeNanoHexapod(struct('actuator', 'lorentz'));
+K = K_heavy_vc; %#ok
+save('./mat/controller.mat', 'K');
+G_cl_heavy_vc = identifyPlant();
+
+initializeNanoHexapod(struct('actuator', 'piezo'));
+K = K_heavy_pz;
+save('./mat/controller.mat', 'K');
+G_cl_heavy_pz = identifyPlant();
+
+%% Save the identified transfer functions
+save('./mat/G_cl.mat', 'G_cl_light_vc', 'G_cl_light_pz', 'G_cl_heavy_vc', 'G_cl_heavy_pz');

control/control_cl_tf_comp.m

@@ -0,0 +1,41 @@
+%%
+clear; close all; clc;
+
+%% Load System and Damped System
+load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
+load('./mat/G_cl.mat', 'G_cl_light_vc', 'G_cl_light_pz', 'G_cl_heavy_vc', 'G_cl_heavy_pz');
+
+%% Load Configuration file
+load('./mat/config.mat', 'save_fig', 'freqs');
+
+%% From xw to d
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '-', 'DisplayName', 'Light VC - OL');
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '-', 'DisplayName', 'Light PZ - OL');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_cl_light_vc.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'DisplayName', 'Light VC - CL');
+plot(freqs, abs(squeeze(freqresp(G_cl_light_pz.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'DisplayName', 'Light PZ - CL');
+hold off;
+xlim([freqs(1) freqs(end)]);
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
+legend('Location', 'southwest');
+
+if save_fig; exportFig('damping_comp_xw', 'normal-normal', struct('path', 'active_damping')); end
+
+%% From fi to d
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '-', 'DisplayName', 'Light VC - OL');
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '-', 'DisplayName', 'Light PZ - OL');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_cl_light_vc.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'DisplayName', 'Light VC - CL');
+plot(freqs, abs(squeeze(freqresp(G_cl_light_pz.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'DisplayName', 'Light PZ - CL');
+hold off;
+xlim([freqs(1) freqs(end)]);
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+legend('Location', 'southwest');
+
+if save_fig; exportFig('damping_comp_fi', 'normal-normal', struct('path', 'active_damping')); end

control/control_generate.m

@@ -0,0 +1,17 @@
+%%
+clear; close all; clc;
+
+%% Load Plant
+load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
+
+%%
+fs = 10;
+
+K_light_vc = generateDiagPidControl(G_light_vc.G_cart, fs);
+K_light_pz = generateDiagPidControl(G_light_pz.G_cart, fs);
+
+K_heavy_vc = generateDiagPidControl(G_heavy_vc.G_cart, fs);
+K_heavy_pz = generateDiagPidControl(G_heavy_pz.G_cart, fs);
+
+%% Save the MIMO control
+save('./mat/K_fb.mat', 'K_light_vc', 'K_light_pz', 'K_heavy_vc', 'K_heavy_pz');

control/control_main.m

@@ -0,0 +1,25 @@
+%%
+clear; close all; clc;
+
+%% Generate Control Laws for the Undamped System
+run control_generate.m
+
+%% Run the simulation and save results
+% Run open loop simulations
+run control_ol_sim.m
+
+% Run closed loop simulations
+run control_cl_sim.m
+
+% Compute PSD in open loop
+run control_ol_psd.m
+
+% Plots to compare OL and CL for PZ and VC
+run control_cl_ol_plots.m
+
+%% Identify the Closed Loop Transfer Functions
+% Compute the closed loop transfer functions
+run control_cl_tf.m
+
+% Compare OL and CL transfer functions
+run control_cl_tf_comp.m

control/control_ol_psd.m

@@ -0,0 +1,87 @@
+%%
+clear; close all; clc;
+
+%% Load Configuration file
+load('./mat/config.mat', 'save_fig', 'freqs');
+
+%% Load Simulation Results
+sim_light_vc_ol = load('./mat/sim_light_vc_ol.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
+sim_light_pz_ol = load('./mat/sim_light_pz_ol.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
+sim_heavy_vc_ol = load('./mat/sim_heavy_vc_ol.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
+sim_heavy_pz_ol = load('./mat/sim_heavy_pz_ol.mat', 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
+
+%%
+sim_light_vc_ol.psd = computePsdDispl(sim_light_vc_ol, 1, 2);
+sim_light_pz_ol.psd = computePsdDispl(sim_light_pz_ol, 1, 2);
+
+sim_heavy_vc_ol.psd = computePsdDispl(sim_heavy_vc_ol, 1, 2);
+sim_heavy_pz_ol.psd = computePsdDispl(sim_heavy_pz_ol, 1, 2);
+
+%% PSD Plot of translations
+figure;
+hold on;
+plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.dx);
+plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.dy);
+plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.dz);
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [$m^2/Hz$]'); xlabel('Frequency [Hz]');
+hold off;
+legend({'PSD $Tx$', 'PSD $Tz$', 'PSD $Tz$'})
+
+if save_fig; exportFig('psd_ol_vc_light_trans', 'normal-normal', struct('path', 'control')); end
+
+%% PSD Plot of rotations
+figure;
+hold on;
+plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.rx);
+plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.ry);
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [$rad^2/Hz$]'); xlabel('Frequency [Hz]');
+hold off;
+legend({'PSD $Rx$', 'PSD $Rz$'})
+
+if save_fig; exportFig('psd_ol_vc_light_rot', 'normal-normal', struct('path', 'control')); end
+
+%% PSD Plot of translations
+figure;
+hold on;
+plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.dx);
+plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.dy);
+plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.dz);
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [$m^2/Hz$]'); xlabel('Frequency [Hz]');
+hold off;
+legend({'PSD $Tx$', 'PSD $Tz$', 'PSD $Tz$'})
+
+if save_fig; exportFig('psd_ol_pz_light_trans', 'normal-normal', struct('path', 'control')); end
+
+%% PSD Plot of rotations
+figure;
+hold on;
+plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.rx);
+plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.ry);
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [$rad^2/Hz$]'); xlabel('Frequency [Hz]');
+hold off;
+legend({'PSD $Rx$', 'PSD $Rz$'})
+
+if save_fig; exportFig('psd_ol_pz_light_rot', 'normal-normal', struct('path', 'control')); end
+
+
+%% PSD Plot of translations
+figure;
+hold on;
+plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.dx);
+plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.dy);
+plot(sim_light_vc_ol.psd.f, sim_light_vc_ol.psd.dz);
+set(gca,'ColorOrderIndex',1);
+plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.dx, '--');
+plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.dy, '--');
+plot(sim_light_pz_ol.psd.f, sim_light_pz_ol.psd.dz, '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [$m^2/Hz$]'); xlabel('Frequency [Hz]');
+xlim([sim_light_vc_ol.psd.f(1), sim_light_vc_ol.psd.f(end)])
+hold off;
+legend({'PSD $Tx$ - VC', 'PSD $Tz$ - VC', 'PSD $Tz$ - VC', 'PSD $Tx$ - PZ', 'PSD $Tz$ - PZ', 'PSD $Tz$ - PZ'})
+
+if save_fig; exportFig('psd_ol_pz_vc_light_trans', 'wide-tall', struct('path', 'control')); end

control/control_ol_sim.m

@@ -0,0 +1,11 @@
+%%
+clear; close all; clc;
+
+%% Initialize Simulation and Inputs
+initializeExperiment('tomography', 'light');
+
+%% Run Open Loop Simulations
+runSimulation('vc', 'light', 'ol', 'none');
+runSimulation('pz', 'light', 'ol', 'none');
+% runSimulation('vc', 'heavy', 'ol', 'none');
+% runSimulation('pz', 'heavy', 'ol', 'none');

demonstration/demonstration_main.m

@@ -0,0 +1,14 @@
+%%
+clear; close all; clc;
+
+%% Demonstration of stroke of each stage
+% Initalize data for demonstration
+run displacement_init.m
+
+% Run the simulation
+run displacement_sim.m
+
+%% Test the measurement of sample position
+run sample_pos_init.m
+
+run sample_pos_sim.m

demonstration/displacement_sim.m

@@ -0,0 +1,5 @@
+%%
+clear; close all; clc;
+
+%%
+sim('Micro_Station_Displacement.slx');

demonstration/sample_pos_init.m

@@ -0,0 +1,69 @@
+%%
+clear; close all; clc;
+
+%% Initialize simulation configuration
+opts_sim = struct(...
+    'Tsim', 2 ...
+);
+
+initializeSimConf(opts_sim);
+
+%% Initialize Inputs
+load('./mat/sim_conf.mat', 'sim_conf')
+
+time_vector = 0:sim_conf.Ts:sim_conf.Tsim;
+
+% Translation Stage
+ty = 0*ones(length(time_vector), 1);
+
+% Tilt Stage
+ry = 2*pi*(3/360)*ones(length(time_vector), 1);
+
+% Spindle
+rz = 2*pi*1*(time_vector);
+
+% Micro Hexapod
+u_hexa = zeros(length(time_vector), 6);
+
+% Gravity Compensator system
+mass = zeros(length(time_vector), 2);
+
+opts_inputs = struct(...
+    'ty', ty, ...
+    'ry', ry, ...
+    'rz', rz, ...
+    'u_hexa', u_hexa, ...
+    'mass', mass ...
+);
+
+initializeInputs(opts_inputs);
+
+%% Initialize SolidWorks Data
+initializeSmiData();
+
+%% Initialize Ground
+initializeGround();
+
+%% Initialize Granite
+initializeGranite();
+
+%% Initialize Translation stage
+initializeTy();
+
+%% Initialize Tilt Stage
+initializeRy();
+
+%% Initialize Spindle
+initializeRz();
+
+%% Initialize Hexapod Symétrie
+initializeMicroHexapod();
+
+%% Initialize Center of Gravity compensation
+initializeAxisc();
+
+%% Initialize NASS
+initializeNanoHexapod(struct('actuator', 'piezo'));
+
+%% Initialize Sample
+initializeSample(struct('mass', 20));

demonstration/sample_pos_sim.m

@@ -0,0 +1,5 @@
+%%
+clear; close all; clc;
+
+%%
+sim('Micro_Station_Displacement.slx');

hac_lac/hac_lac_iff_control.m

@@ -0,0 +1,17 @@
+%%
+clear; close all; clc;
+
+%% Load Plant
+load('./mat/G_iff.mat', 'G_iff_light_vc', 'G_iff_light_pz', 'G_iff_heavy_vc', 'G_iff_heavy_pz');
+
+%%
+fs = 10;
+
+K_light_vc_iff = generateDiagPidControl(G_iff_light_vc.G_cart, fs);
+K_light_pz_iff = generateDiagPidControl(G_iff_light_pz.G_cart, fs);
+
+K_heavy_vc_iff = generateDiagPidControl(G_iff_heavy_vc.G_cart, fs);
+K_heavy_pz_iff = generateDiagPidControl(G_iff_heavy_pz.G_cart, fs);
+
+%% Save the MIMO control
+save('./mat/K_fb_iff.mat', 'K_light_vc_iff', 'K_light_pz_iff', 'K_heavy_vc_iff', 'K_heavy_pz_iff');

hac_lac/hac_lac_iff_control_plots.m

@@ -0,0 +1,117 @@
+%%
+clear; close all; clc;
+
+%% Load plant and controller
+load('./mat/G_iff.mat', 'G_iff_light_vc', 'G_iff_light_pz', 'G_iff_heavy_vc', 'G_iff_heavy_pz');
+load('./mat/K_fb_iff.mat', 'K_light_vc_iff', 'K_light_pz_iff', 'K_heavy_vc_iff', 'K_heavy_pz_iff');
+
+%% Load Configuration
+load('./mat/config.mat', 'save_fig', 'freqs');
+
+%% Plot the Loop gain for Translations - Light VC
+figure;
+% Amplitude
+ax1 = subaxis(2,1,1);
+hold on;
+plot(freqs, abs(squeeze(freqresp(K_light_vc_iff(1, 1)*G_iff_light_vc.G_cart(1, 1), freqs, 'Hz'))), 'DisplayName', 'x');
+plot(freqs, abs(squeeze(freqresp(K_light_vc_iff(2, 2)*G_iff_light_vc.G_cart(2, 2), freqs, 'Hz'))), 'DisplayName', 'y');
+plot(freqs, abs(squeeze(freqresp(K_light_vc_iff(3, 3)*G_iff_light_vc.G_cart(3, 3), freqs, 'Hz'))), 'DisplayName', 'z');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]);
+ylabel('Amplitude [m/N]');
+hold off;
+% Phase
+ax2 = subaxis(2,1,2);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_vc_iff(1, 1)*G_iff_light_vc.G_cart(1, 1), freqs, 'Hz'))));
+plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_vc_iff(2, 2)*G_iff_light_vc.G_cart(2, 2), freqs, 'Hz'))));
+plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_vc_iff(3, 3)*G_iff_light_vc.G_cart(3, 3), freqs, 'Hz'))));
+set(gca,'xscale','log');
+yticks(-180:90:180);
+ylim([-180 180]);
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+legend('Location', 'southwest');
+hold off;
+linkaxes([ax1,ax2],'x');
+
+if save_fig; exportFig('loop_gain_fb_iff_light_vc_trans', 'normal-normal', struct('path', 'active_damping')); end
+
+%% Plot the Loop gain for Rotations - Light VC
+figure;
+% Amplitude
+ax1 = subaxis(2,1,1);
+hold on;
+plot(freqs, abs(squeeze(freqresp(K_light_vc_iff(4, 4)*G_iff_light_vc.G_cart(4, 4), freqs, 'Hz'))), 'DisplayName', 'Rx');
+plot(freqs, abs(squeeze(freqresp(K_light_vc_iff(5, 5)*G_iff_light_vc.G_cart(5, 5), freqs, 'Hz'))), 'DisplayName', 'Ry');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]);
+ylabel('Amplitude [m/N]');
+hold off;
+% Phase
+ax2 = subaxis(2,1,2);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_vc_iff(4, 4)*G_iff_light_vc.G_cart(4, 4), freqs, 'Hz'))));
+plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_vc_iff(5, 5)*G_iff_light_vc.G_cart(5, 5), freqs, 'Hz'))));
+set(gca,'xscale','log');
+yticks(-180:90:180);
+ylim([-180 180]);
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+legend('Location', 'southwest');
+hold off;
+linkaxes([ax1,ax2],'x');
+
+if save_fig; exportFig('loop_gain_fb_iff_light_vc_rot', 'normal-normal', struct('path', 'active_damping')); end
+
+%% Plot the Loop gain for Translations - Light PZ
+figure;
+% Amplitude
+ax1 = subaxis(2,1,1);
+hold on;
+plot(freqs, abs(squeeze(freqresp(K_light_pz_iff(1, 1)*G_iff_light_pz.G_cart(1, 1), freqs, 'Hz'))), 'DisplayName', 'x');
+plot(freqs, abs(squeeze(freqresp(K_light_pz_iff(2, 2)*G_iff_light_pz.G_cart(2, 2), freqs, 'Hz'))), 'DisplayName', 'y');
+plot(freqs, abs(squeeze(freqresp(K_light_pz_iff(3, 3)*G_iff_light_pz.G_cart(3, 3), freqs, 'Hz'))), 'DisplayName', 'z');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]);
+ylabel('Amplitude [m/N]');
+hold off;
+% Phase
+ax2 = subaxis(2,1,2);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_pz_iff(1, 1)*G_iff_light_pz.G_cart(1, 1), freqs, 'Hz'))));
+plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_pz_iff(2, 2)*G_iff_light_pz.G_cart(2, 2), freqs, 'Hz'))));
+plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_pz_iff(3, 3)*G_iff_light_pz.G_cart(3, 3), freqs, 'Hz'))));
+set(gca,'xscale','log');
+yticks(-180:90:180);
+ylim([-180 180]);
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+legend('Location', 'southwest');
+hold off;
+linkaxes([ax1,ax2],'x');
+
+if save_fig; exportFig('loop_gain_fb_iff_light_pz_trans', 'normal-normal', struct('path', 'active_damping')); end
+
+%% Plot the Loop gain for Rotations - Light PZ
+figure;
+% Amplitude
+ax1 = subaxis(2,1,1);
+hold on;
+plot(freqs, abs(squeeze(freqresp(K_light_pz_iff(4, 4)*G_iff_light_pz.G_cart(4, 4), freqs, 'Hz'))), 'DisplayName', 'Rx');
+plot(freqs, abs(squeeze(freqresp(K_light_pz_iff(5, 5)*G_iff_light_pz.G_cart(5, 5), freqs, 'Hz'))), 'DisplayName', 'Ry');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]);
+ylabel('Amplitude [m/N]');
+hold off;
+% Phase
+ax2 = subaxis(2,1,2);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_pz_iff(4, 4)*G_iff_light_pz.G_cart(4, 4), freqs, 'Hz'))));
+plot(freqs, 180/pi*angle(squeeze(freqresp(K_light_pz_iff(5, 5)*G_iff_light_pz.G_cart(5, 5), freqs, 'Hz'))));
+set(gca,'xscale','log');
+yticks(-180:90:180);
+ylim([-180 180]);
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+legend('Location', 'southwest');
+hold off;
+linkaxes([ax1,ax2],'x');
+
+if save_fig; exportFig('loop_gain_fb_iff_light_pz_rot', 'normal-normal', struct('path', 'active_damping')); end

hac_lac/hac_lac_iff_simulation.m

@@ -0,0 +1,11 @@
+%%
+clear; close all; clc;
+
+%% Initialize Simulation and Inputs
+initializeExperiment('tomography', 'light');
+
+%% Run Closed Loop Simulations
+runSimulation('vc', 'light', 'cl', 'iff');
+runSimulation('pz', 'light', 'cl', 'iff');
+% runSimulation('vc', 'heavy', 'cl', 'iff');
+% runSimulation('pz', 'heavy', 'cl', 'iff');

hac_lac/hac_lac_main.m

@@ -0,0 +1,2 @@
+%%
+clear; close all; clc;

identification/id_G_cart_plots.m

@@ -0,0 +1,115 @@
+%%
+clear; close all; clc;
+
+%% Load the transfer functions
+save('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
+
+%% Load Configuration file
+load('./mat/config.mat', 'save_fig', 'freqs');
+
+%% Plant
+figure;
+% Amplitude
+ax1 = subaxis(2,1,1);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]);
+ylabel('Amplitude [m/N]');
+hold off;
+
+% Phase
+ax2 = subaxis(2,1,2);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_vc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_pz.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
+set(gca,'xscale','log');
+yticks(-1800:90:1800);
+ylim([-180 180]);
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+legend('Location', 'southwest');
+hold off;
+
+linkaxes([ax1,ax2],'x');
+
+if save_fig; exportFig('comp_models_plant_x_x', 'normal-normal', struct('path', 'identification')); end
+
+%%
+figure;
+% Amplitude
+ax1 = subaxis(2,1,1);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]);
+ylabel('Amplitude [m/N]');
+hold off;
+
+% Phase
+ax2 = subaxis(2,1,2);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_vc.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_pz.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
+set(gca,'xscale','log');
+yticks(-1800:90:1800);
+ylim([-180 180]);
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+legend('Location', 'southwest');
+hold off;
+
+linkaxes([ax1,ax2],'x');
+
+if save_fig; exportFig('comp_models_plant_z_z', 'normal-normal', struct('path', 'identification')); end
+
+%% Plot all the coupling
+figure;
+
+for i_input = 1:3
+    for i_output = 1:3
+        subaxis(3,3,3*(i_input-1)+i_output);
+        hold on;
+        plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart(i_output, i_input), freqs, 'Hz'))));
+        plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart(i_output, i_input), freqs, 'Hz'))));
+        set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+        xlim([freqs(1) freqs(end)]); ylim([1e-12, 1e-2]);
+        yticks([1e-12, 1e-8, 1e-4]); xticks([0.1 1 10 100 1000]);
+        if i_output > 1; set(gca,'yticklabel',[]); end
+        if i_input < 3; set(gca,'xticklabel',[]); end
+        hold off;
+    end
+end
+
+if save_fig; exportFig('comp_models_plant_coupling_all', 'full-tall', struct('path', 'identification')); end
+
+%% Plot some coupling
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', 'VC - Light - $Fx \to Dx$');
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light - $Fx \to Dx$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart('Dy', 'Fnx'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy - $Fx \to Dy$');
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart('Dy', 'Fnx'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy - $Fx \to Dy$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_cart('Dz', 'Fnx'), freqs, 'Hz'))), '-.', 'DisplayName', 'VC - Heavy - $Fx \to Dz$');
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_cart('Dz', 'Fnx'), freqs, 'Hz'))), '-.', 'DisplayName', 'PZ - Heavy - $Fx \to Dz$');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Amplitude [m/m]');
+legend('Location', 'southwest');
+xticks('manual'); xlim([freqs(1) freqs(end)]);
+hold off;
+
+if save_fig; exportFig('comp_models_plant_coupling', 'normal-normal', struct('path', 'identification')); end

identification/id_G_d_plots.m

@@ -0,0 +1,77 @@
+%%
+clear; close all; clc;
+
+%% Load the identified transfer functions
+save('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
+
+%% Load Configuration file
+load('./mat/config.mat', 'save_fig', 'freqs');
+
+%% Transfer function from ground displacement to measured displacement
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_dg('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_dg('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_dg('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
+plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_dg('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/m]');
+hold off;
+legend('Location', 'southwest');
+
+if save_fig; exportFig('comp_models_xw_to_d', 'normal-normal', struct('path', 'identification')); end
+
+%%
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_dg('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_dg('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_dg('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
+plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_dg('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/m]');
+hold off;
+legend('Location', 'southwest');
+
+%% Transfer function from direct force to measured displacement
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
+plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]');
+hold off;
+legend('Location', 'southwest');
+
+if save_fig; exportFig('comp_models_fi_to_d', 'normal-normal', struct('path', 'identification')); end
+
+%%
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_fs('Ry', 'Fsx'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_fs('Ry', 'Fsx'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_fs('Ry', 'Fsx'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
+plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_fs('Ry', 'Fsx'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]');
+hold off;
+legend('Location', 'southwest');
+
+%%
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_fs('Rz', 'Fsx'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_fs('Rz', 'Fsx'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_fs('Rz', 'Fsx'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
+plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_fs('Rz', 'Fsx'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]');
+hold off;
+legend('Location', 'southwest');

identification/id_G_iff_plots.m

@@ -0,0 +1,57 @@
+%%
+clear; close all; clc;
+
+%% Load the identified transfer functions
+load('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');
+
+%% Load Configuration file
+load('./mat/config.mat', 'save_fig', 'freqs');
+
+%% 
+figure;
+% Amplitude
+ax1 = subaxis(2,1,1);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm1', 'F1'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G_light_pz.G_iff('Fm1', 'F1'), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_heavy_vc.G_iff('Fm1', 'F1'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_heavy_pz.G_iff('Fm1', 'F1'), freqs, 'Hz'))), '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]);
+ylabel('Amplitude [m/N]');
+hold off;
+
+% Phase
+ax2 = subaxis(2,1,2);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_vc.G_iff('Fm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_light_pz.G_iff('Fm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_vc.G_iff('Fm1', 'F1'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_heavy_pz.G_iff('Fm1', 'F1'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
+set(gca,'xscale','log');
+yticks(-180:90:180);
+ylim([-180 180]);
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+legend('Location', 'southwest');
+hold off;
+
+linkaxes([ax1,ax2],'x');
+
+if save_fig; exportFig('G_iff', 'normal-normal', struct('path', 'identification')); end
+
+%% Coupling
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm1', 'F1'), freqs, 'Hz'))), 'k-');
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm2', 'F1'), freqs, 'Hz'))), 'k--');
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm3', 'F1'), freqs, 'Hz'))), 'k--');
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm4', 'F1'), freqs, 'Hz'))), 'k--');
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm5', 'F1'), freqs, 'Hz'))), 'k--');
+plot(freqs, abs(squeeze(freqresp(G_light_vc.G_iff('Fm6', 'F1'), freqs, 'Hz'))), 'k--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+hold off;
+
+if save_fig; exportFig('G_iff_coupling', 'normal-normal', struct('path', 'identification')); end

identification/id_G_plots.m

@@ -1,115 +0,0 @@
-%%
-clear; close all; clc;
-
-%% Load the transfer functions
-load('./mat/G_f_to_d.mat', 'G_1_vc', 'G_1_pz', 'G_50_vc', 'G_50_pz');
-
-%% Load Configuration file
-load('./mat/config.mat', 'save_fig', 'freqs');
-
-%%
-figure;
-% Amplitude
-ax1 = subaxis(2,1,1);
-hold on;
-plot(freqs, abs(squeeze(freqresp(G_1_vc('Dx', 'Fnx'), freqs, 'Hz'))));
-plot(freqs, abs(squeeze(freqresp(G_1_pz('Dx', 'Fnx'), freqs, 'Hz'))));
-set(gca,'ColorOrderIndex',1);
-plot(freqs, abs(squeeze(freqresp(G_50_vc('Dx', 'Fnx'), freqs, 'Hz'))), '--');
-plot(freqs, abs(squeeze(freqresp(G_50_pz('Dx', 'Fnx'), freqs, 'Hz'))), '--');
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]);
-ylabel('Amplitude [m/N]');
-hold off;
-
-% Phase
-ax2 = subaxis(2,1,2);
-hold on;
-plot(freqs, 180/pi*angle(squeeze(freqresp(G_1_vc('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
-plot(freqs, 180/pi*angle(squeeze(freqresp(G_1_pz('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
-set(gca,'ColorOrderIndex',1)
-plot(freqs, 180/pi*angle(squeeze(freqresp(G_50_vc('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
-plot(freqs, 180/pi*angle(squeeze(freqresp(G_50_pz('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
-set(gca,'xscale','log');
-yticks(-1800:90:1800);
-ylim([-180 180]);
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-legend('Location', 'southwest');
-hold off;
-
-linkaxes([ax1,ax2],'x');
-
-if save_fig; exportFig('comp_models_plant_x_x', 'normal-normal', struct('path', 'identification')); end
-
-%%
-figure;
-% Amplitude
-ax1 = subaxis(2,1,1);
-hold on;
-plot(freqs, abs(squeeze(freqresp(G_1_vc('Dz', 'Fnz'), freqs, 'Hz'))));
-plot(freqs, abs(squeeze(freqresp(G_1_pz('Dz', 'Fnz'), freqs, 'Hz'))));
-set(gca,'ColorOrderIndex',1);
-plot(freqs, abs(squeeze(freqresp(G_50_vc('Dz', 'Fnz'), freqs, 'Hz'))), '--');
-plot(freqs, abs(squeeze(freqresp(G_50_pz('Dz', 'Fnz'), freqs, 'Hz'))), '--');
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]);
-ylabel('Amplitude [m/N]');
-hold off;
-
-% Phase
-ax2 = subaxis(2,1,2);
-hold on;
-plot(freqs, 180/pi*angle(squeeze(freqresp(G_1_vc('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
-plot(freqs, 180/pi*angle(squeeze(freqresp(G_1_pz('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
-set(gca,'ColorOrderIndex',1)
-plot(freqs, 180/pi*angle(squeeze(freqresp(G_50_vc('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
-plot(freqs, 180/pi*angle(squeeze(freqresp(G_50_pz('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
-set(gca,'xscale','log');
-yticks(-1800:90:1800);
-ylim([-180 180]);
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-legend('Location', 'southwest');
-hold off;
-
-linkaxes([ax1,ax2],'x');
-
-if save_fig; exportFig('comp_models_plant_z_z', 'normal-normal', struct('path', 'identification')); end
-
-%% Plot all the coupling
-figure;
-
-for i_input = 1:3
-    for i_output = 1:3
-        subaxis(3,3,3*(i_input-1)+i_output);
-        hold on;
-        plot(freqs, abs(squeeze(freqresp(G_1_vc(i_output, i_input), freqs, 'Hz'))));
-        plot(freqs, abs(squeeze(freqresp(G_1_pz(i_output, i_input), freqs, 'Hz'))));
-        set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-        xlim([freqs(1) freqs(end)]); ylim([1e-12, 1e-2]);
-        yticks([1e-12, 1e-8, 1e-4]); xticks([0.1 1 10 100 1000]);
-        if i_output > 1; set(gca,'yticklabel',[]); end
-        if i_input < 3; set(gca,'xticklabel',[]); end
-        hold off;
-    end
-end
-
-if save_fig; exportFig('comp_models_plant_coupling_all', 'full-tall', struct('path', 'identification')); end
-
-%% Plot some coupling
-figure;
-hold on;
-plot(freqs, abs(squeeze(freqresp(G_1_vc('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', 'VC - Light - $Fx \to Dx$');
-plot(freqs, abs(squeeze(freqresp(G_1_pz('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light - $Fx \to Dx$');
-set(gca,'ColorOrderIndex',1);
-plot(freqs, abs(squeeze(freqresp(G_1_vc('Dy', 'Fnx'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy - $Fx \to Dy$');
-plot(freqs, abs(squeeze(freqresp(G_1_pz('Dy', 'Fnx'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy - $Fx \to Dy$');
-set(gca,'ColorOrderIndex',1);
-plot(freqs, abs(squeeze(freqresp(G_1_vc('Dz', 'Fnx'), freqs, 'Hz'))), '-.', 'DisplayName', 'VC - Heavy - $Fx \to Dz$');
-plot(freqs, abs(squeeze(freqresp(G_1_pz('Dz', 'Fnx'), freqs, 'Hz'))), '-.', 'DisplayName', 'PZ - Heavy - $Fx \to Dz$');
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('Amplitude [m/m]');
-legend('Location', 'southwest');
-xticks('manual'); xlim([freqs(1) freqs(end)]);
-hold off;
-
-if save_fig; exportFig('comp_models_plant_coupling', 'normal-normal', struct('path', 'identification')); end

identification/id_Gd.m

@@ -1,28 +0,0 @@
-%% Script Description
-% Identification of the transfer function
-% from Ground Motion to measured displacement
-
-%%
-clear; close all; clc;
-
-%% Open Loop - Light Sample
-initializeSample(struct('mass', 1));
-
-initializeNanoHexapod(struct('actuator', 'lorentz'));
-Gd_ol_1_vc = identifyGd(struct('cl', false));
-
-initializeNanoHexapod(struct('actuator', 'piezo'));
-Gd_ol_1_pz = identifyGd(struct('cl', false));
-
-%% Open Loop - Heavy Sample
-initializeSample(struct('mass', 50));
-
-initializeNanoHexapod(struct('actuator', 'lorentz'));
-Gd_ol_50_vc = identifyGd(struct('cl', false));
-
-initializeNanoHexapod(struct('actuator', 'piezo'));
-Gd_ol_50_pz = identifyGd(struct('cl', false));
-
-%% Save the identified transfer functions
-save('./mat/G_xw_to_d.mat', ...
-    'Gd_ol_1_vc',  'Gd_ol_1_pz',  'Gd_ol_50_vc', 'Gd_ol_50_pz');

identification/id_Gd_plots.m

@@ -1,39 +0,0 @@
-%%
-clear; close all; clc;
-
-%% Load the identified transfer functions
-load('./mat/G_xw_to_d.mat', ...
-    'Gd_ol_1_vc',  'Gd_ol_1_pz',  'Gd_ol_50_vc', 'Gd_ol_50_pz');
-
-%% Load Configuration file
-load('./mat/config.mat', 'save_fig', 'freqs');
-
-%% Transfer function from ground displacement to measured displacement
-figure;
-hold on;
-plot(freqs, abs(squeeze(freqresp(Gd_ol_1_vc('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
-plot(freqs, abs(squeeze(freqresp(Gd_ol_1_pz('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
-set(gca,'ColorOrderIndex',1);
-plot(freqs, abs(squeeze(freqresp(Gd_ol_50_vc('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
-plot(freqs, abs(squeeze(freqresp(Gd_ol_50_pz('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [m/m]');
-hold off;
-legend('Location', 'southwest');
-
-if save_fig; exportFig('comp_models_xw_to_d', 'normal-normal', struct('path', 'identification')); end
-
-%% Transfer function from direct force to measured displacement
-figure;
-hold on;
-plot(freqs, abs(squeeze(freqresp(Gd_ol_1_vc('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', 'VC - Light');
-plot(freqs, abs(squeeze(freqresp(Gd_ol_1_pz('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', 'PZ - Light');
-set(gca,'ColorOrderIndex',1);
-plot(freqs, abs(squeeze(freqresp(Gd_ol_50_vc('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'DisplayName', 'VC - Heavy');
-plot(freqs, abs(squeeze(freqresp(Gd_ol_50_pz('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'DisplayName', 'PZ - Heavy');
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [m/N]');
-hold off;
-legend('Location', 'southwest');
-
-if save_fig; exportFig('comp_models_fi_to_d', 'normal-normal', struct('path', 'identification')); end

identification/id_main.m

@@ -1,20 +1,6 @@
 %%
 clear; close all; clc;
 
-%% Plant Identification
-% Compute the transfer function of G for multiple masses
-run id_G.m
-
-% Plot de obtained transfer functions
-run id_G_plots.m
-
-%% Identification of transfer function from disturbances to displacement
-% Compute the transfer function of Gd
-run id_Gd.m
-
-% Plot de obtained transfer functions
-run id_Gd_plots.m
-
 %% Identification of the micro-station
 % Compute the transfer functions
 run id_micro_station.m
@@ -25,6 +11,19 @@ run id_micro_station_plots.m
 % Compare the measurements of Marc with the model
 run id_micro_station_comp_meas.m
 
+%% Identification of the nano-station
+% Run the identification
+run id_nano_station.m
+
+% Plot the plant for feedback control
+run id_G_cart_plots.m
+
+% Plot the transfer function from disturbances to displacement
+run id_G_d_plots.m
+
+% Plot the transfer function for IFF control
+run id_G_iff_plots.m
+
 %% Identification of all the stages
 % Compute the transfer functions of each stage from act. to sens.
 run id_stages.m

identification/id_nano_station.m

@@ -1,28 +1,27 @@
-%% Script Description
-% Identification of a force injected into the NASS (in cartesian
-% coordinates) to the relative displacement of the sample
-% and granite.
-
 %%
 clear; close all; clc;
 
 %%
-initializeNanoHexapod(struct('actuator', 'lorentz'));
+K_iff = tf(zeros(6));
+save('./mat/K_iff.mat', 'K_iff');
+
+%% Light Sample
 initializeSample(struct('mass', 1));
 
-G_1_vc = identifyG();
+initializeNanoHexapod(struct('actuator', 'lorentz'));
+G_light_vc = identifyPlant();
 
 initializeNanoHexapod(struct('actuator', 'piezo'));
-G_1_pz = identifyG();
+G_light_pz = identifyPlant();
 
-%%
+%% Heavy Sample
-initializeNanoHexapod(struct('actuator', 'lorentz'));
 initializeSample(struct('mass', 50));
 
-G_50_vc = identifyG();
+initializeNanoHexapod(struct('actuator', 'lorentz'));
+G_heavy_vc = identifyPlant();
 
 initializeNanoHexapod(struct('actuator', 'piezo'));
-G_50_pz = identifyG();
+G_heavy_pz = identifyPlant();
 
 %% Save the obtained transfer functions
-save('./mat/G_f_to_d.mat', 'G_1_vc', 'G_1_pz', 'G_50_vc', 'G_50_pz');
+save('./mat/G.mat', 'G_light_vc', 'G_light_pz', 'G_heavy_vc', 'G_heavy_pz');

init_simulation.m

@@ -23,3 +23,4 @@ load('./mat/inputs.mat', 'inputs');
 
 %% Load Controller
 load('./mat/controller.mat', 'K');
+load('./mat/K_iff.mat', 'K_iff');

initialize/initializeExperiment.m

@@ -0,0 +1,25 @@
+function [] = initializeExperiment(exp_name, sys_mass)
+    if strcmp(exp_name, 'tomography')
+        opts_sim = struct(...
+            'Tsim',    5, ...
+            'cl_time', 5  ...
+        );
+        initializeSimConf(opts_sim);
+
+        if strcmp(sys_mass, 'light')
+            opts_inputs = struct(...
+                'ground_motion', true,  ...
+                'rz',            60     ... % rpm
+            );
+        elseif strcpm(sys_mass, 'heavy')
+            opts_inputs = struct(...
+                'ground_motion', true,  ...
+                'rz',            1     ... % rpm
+            );
+        else
+            error('sys_mass should be light or heavy');
+        end
+
+        initializeInputs(opts_inputs);
+    elseif
+end

initialize/initializeInputs.m

@@ -112,22 +112,14 @@ function [inputs] = initializeInputs(opts_param)
     %% Set point [m, rad]
     if islogical(opts.setpoint) && opts.setpoint == true
         setpoint = zeros(length(time_vector), 6);
-        setpoint(ceil(10/sim_conf.Ts):end, 2) = 1e-6; % Step of 1 micro-meter in y direction
     elseif islogical(opts.setpoint) && opts.setpoint == false
         setpoint = zeros(length(time_vector), 6);
     else
         setpoint = opts.setpoint;
     end
 
-    % The setpoint in rotation should be the same as the rotation of the Spindle
-    % Should change that. And think how to include all the setpoint of each stage in this
-    % global setpoint. Maybe do everything in simulink
-    setpoint(:, 6) = rz;
-
     inputs.setpoint = timeseries(setpoint, time_vector);
 
-    %% Save if no output argument
+    %% Save
-    if nargout == 0
     save('./mat/inputs.mat', 'inputs');
-    end
 end

initialize/initializeSimConf.m

@@ -3,7 +3,7 @@ function [] = initializeSimConf(opts_param)
     opts = struct('Ts',      1e-4,  ... % Sampling time [s]
                   'Tsim',    10,    ... % Simulation time [s]
                   'cl_time', 0,     ... % Close Loop time [s]
-                  'gravity', false  ... % Gravity along the z axis [m/s^2]
+                  'gravity', false  ... % Gravity along the z axis
     );
 
     %% Populate opts with input parameters
@@ -23,13 +23,11 @@ function [] = initializeSimConf(opts_param)
 
     %% Gravity
     if opts.gravity
-        sim_conf.g = -9.8;
+        sim_conf.g = -9.8; %#ok
     else
-        sim_conf.g = 0;
+        sim_conf.g = 0; %#ok
     end
 
     %% Save
     save('./mat/sim_conf.mat', 'sim_conf');
-
 end
-

initialize/initializeSmiData.m

@@ -1,6 +1,6 @@
 function [smiData] = initializeSmiData()
     %% Initialize the structure
-    smiData = struct;
+    smiData = struct();
 
     %% Rigid Transform
     smiData.RigidTransform = struct;
@@ -265,6 +265,30 @@ function [smiData] = initializeSmiData()
     smiData.RigidTransform(65).axis = [-0.57735026918962584 -0.57735026918962584 -0.57735026918962584];
     smiData.RigidTransform(65).ID = 'B[Guide_Tilt-2:-:Plateau_Tilt-1]';
     smiData.RigidTransform(66).translation = [-313.5 0 0];
+    smiData.RigidTransform(66).angle = 2.0943951023931962;
+    smiData.RigidTransform(66).axis = [-0.577350269189626 -0.57735026918962606 0.5773502691896254];
+    smiData.RigidTransform(66).ID = 'F[Guide_Tilt-2:-:Plateau_Tilt-1]';
+    smiData.RigidTransform(67).translation = [0 -5 600];
+    smiData.RigidTransform(67).angle = 2.0943951023931953;
+    smiData.RigidTransform(67).axis = [-0.57735026918962584 -0.57735026918962584 -0.57735026918962584];
+    smiData.RigidTransform(67).ID = 'B[Guide_Tilt-3:-:Plateau_Tilt-1]';
+    smiData.RigidTransform(68).translation = [313.5 0 0];
+    smiData.RigidTransform(68).angle = 2.0943951023931948;
+    smiData.RigidTransform(68).axis = [0.57735026918962551 0.57735026918962562 0.57735026918962629];
+    smiData.RigidTransform(68).ID = 'F[Guide_Tilt-3:-:Plateau_Tilt-1]';
+    smiData.RigidTransform(69).translation = [0 -5 600];
+    smiData.RigidTransform(69).angle = 2.0943951023931953;
+    smiData.RigidTransform(69).axis = [-0.57735026918962584 -0.57735026918962584 -0.57735026918962584];
+    smiData.RigidTransform(69).ID = 'B[Guide_Tilt-4:-:Plateau_Tilt-1]';
+    smiData.RigidTransform(70).translation = [313.5 0 0];
+    smiData.RigidTransform(70).angle = 2.0943951023931948;
+    smiData.RigidTransform(70).axis = [0.57735026918962551 0.57735026918962562 0.57735026918962629];
+    smiData.RigidTransform(70).ID = 'F[Guide_Tilt-4:-:Plateau_Tilt-1]';
+    smiData.RigidTransform(71).translation = [146.02 0 0];
+    smiData.RigidTransform(71).angle = 2.0943951023931953;
+    smiData.RigidTransform(71).axis = [0.57735026918962584 0.57735026918962584 0.57735026918962584];
+    smiData.RigidTransform(71).ID = 'B[Bati_Spindle-1:-:Axe_Spindle-1]';
+    smiData.RigidTransform(72).translation = [146.2 0 0];
     smiData.RigidTransform(72).angle = 2.0943951023931957;
     smiData.RigidTransform(72).axis = [0.57735026918962573 0.57735026918962584 0.57735026918962573];
     smiData.RigidTransform(72).ID = 'F[Bati_Spindle-1:-:Axe_Spindle-1]';
@@ -1348,8 +1372,6 @@ function [smiData] = initializeSmiData()
     smiData.SphericalJoint(24).S.Pos.Axis = [0.77690891930122863 0.35378802664966663 0.52081336706111148];
     smiData.SphericalJoint(24).ID = '[BrasHaut_Nano-6:-:Nacelle_Nano_Support-1]';
 
-    %% If no output argument, save the object
+    %% Save
-    if nargout == 0
     save('./mat/smiData.mat', 'smiData')
-    end
 end

main.m

@@ -4,9 +4,6 @@ clear; close all; clc;
 %% Open the project
 simulinkproject('./');
 
-%% General Configuration
-run config.m
-
 %% Initialization
 % Initialize the perturbations
 run init_perturbations.m
@@ -14,11 +11,17 @@ run init_perturbations.m
 % Initialize all the stages parameters
 run init_data.m
 
-%% Run the simulations
-run run_simulations.m
-
 %% Demonstration of displacement of all the stages
-run init_data_demonstration.m
+run demonstration_main.m
 
 %% Identification
 open id_main.m
+
+%% Active Damping Control
+open act_damp_main.m
+
+%% Control With the Undamped System
+open control_main.m
+
+%% HAC-LAC Control
+open hac_lac_main.m

src/computePsdDispl.m

@@ -0,0 +1,23 @@
+function [psd_object] = computePsdDispl(sys_data, t_init, n_av)
+    i_init = find(sys_data.time > t_init, 1);
+
+    han_win = hanning(ceil(length(sys_data.Dx(i_init:end, :))/n_av));
+    Fs = 1/sys_data.time(2);
+
+    [pdx, f] = pwelch(sys_data.Dx(i_init:end, :), han_win, [], [], Fs);
+    [pdy, ~] = pwelch(sys_data.Dy(i_init:end, :), han_win, [], [], Fs);
+    [pdz, ~] = pwelch(sys_data.Dz(i_init:end, :), han_win, [], [], Fs);
+
+    [prx, ~] = pwelch(sys_data.Rx(i_init:end, :), han_win, [], [], Fs);
+    [pry, ~] = pwelch(sys_data.Ry(i_init:end, :), han_win, [], [], Fs);
+    [prz, ~] = pwelch(sys_data.Rz(i_init:end, :), han_win, [], [], Fs);
+
+    psd_object = struct(...
+        'f',  f,   ...
+        'dx', pdx, ...
+        'dy', pdy, ...
+        'dz', pdz, ...
+        'rx', prx, ...
+        'ry', pry, ...
+        'rz', prz);
+end

src/generateDiagPidControl.m

@@ -0,0 +1,18 @@
+function [K] = generateDiagPidControl(G, fs)
+    %%
+    pid_opts = pidtuneOptions(...
+        'PhaseMargin', 50, ...
+        'DesignFocus', 'disturbance-rejection');
+
+    %%
+    K = tf(zeros(6));
+
+    for i = 1:5
+        input_name = G.InputName(i);
+        output_name = G.OutputName(i);
+        K(i, i) = tf(pidtune(minreal(G(output_name, input_name)), 'PIDF', 2*pi*fs, pid_opts));
+    end
+
+    K.InputName = G.OutputName;
+    K.OutputName = G.InputName;
+end

src/identifyG.m

@@ -1,28 +0,0 @@
-function [G, G_raw] = identifyG(opts_param)
-    %% Default values for opts
-    opts = struct();
-
-    %% Populate opts with input parameters
-    if exist('opts_param','var')
-        for opt = fieldnames(opts_param)'
-            opts.(opt{1}) = opts_param.(opt{1});
-        end
-    end
-
-    %% Options for Linearized
-    options = linearizeOptions;
-    options.SampleTime = 0;
-
-    %% Name of the Simulink File
-    mdl = 'sim_nano_station';
-
-    %% Centralized control (Cartesian coordinates)
-    % Input/Output definition
-    io(1) = linio([mdl, '/Micro-Station/Fn'],    1,'openinput');
-    io(2) = linio([mdl, '/Micro-Station/Sample'],1,'output');
-
-    % Run the linearization
-    G = linearize(mdl,io, 0);
-    G.InputName  = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
-    G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
-end

src/identifyGd.m

@@ -1,35 +0,0 @@
-function [Gd, Gd_raw] = identifyGd(opts_param)
-    %% Default values for opts
-    opts = struct('cl',  true);
-
-    %% Populate opts with input parameters
-    if exist('opts_param','var')
-        for opt = fieldnames(opts_param)'
-            opts.(opt{1}) = opts_param.(opt{1});
-        end
-    end
-
-    %% Options for Linearized
-    options = linearizeOptions;
-    options.SampleTime = 0;
-
-    %% Name of the Simulink File
-    if opts.cl
-        % Make sure that the loop is closed
-        initializeSimConf(struct('cl_time', 0));
-        mdl = 'Assemblage';
-    else
-        mdl = 'sim_nano_station';
-    end
-
-    %% Centralized control (Cartesian coordinates)
-    % Input/Output definition
-    io(1) = linio([mdl, '/Micro-Station/Gm'],     1,'openinput');
-    io(2) = linio([mdl, '/Micro-Station/Fs_ext'], 1,'openinput');
-    io(3) = linio([mdl, '/Micro-Station/Sample'], 1,'output');
-
-    % Run the linearization
-    Gd = linearize(mdl,io, 0);
-    Gd.InputName  = {'Dgx', 'Dgy', 'Dgz', 'Fsx', 'Fsy', 'Fsz'};
-    Gd.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
-end

src/identifyNass.m

@@ -1,30 +0,0 @@
-function [G_cart, G_cart_raw] = identifyNass(opts_param)
-    %% Default values for opts
-    opts = struct();
-
-    %% Populate opts with input parameters
-    if exist('opts_param','var')
-        for opt = fieldnames(opts_param)'
-            opts.(opt{1}) = opts_param.(opt{1});
-        end
-    end
-
-    %% Options for Linearized
-    options = linearizeOptions;
-    options.SampleTime = 0;
-
-    %% Name of the Simulink File
-    mdl = 'sim_nano_station';
-
-    %% Centralized control (Cartesian coordinates)
-    % Input/Output definition
-    io(1) = linio([mdl, '/Micro-Station/Fn'],           1, 'openinput');
-    io(2) = linio([mdl, '/Micro-Station/Nano_Hexapod'], 1, 'output');
-
-    % Run the linearization
-    G_cart = linearize(mdl,io, 0);
-
-    % Input/Output names
-    G_cart.InputName  = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
-    G_cart.OutputName = {'Dnx', 'Dny', 'Dnz', 'Rnx', 'Rny', 'Rnz'};
-end

src/identifyPlant.m

@@ -0,0 +1,42 @@
+function [sys] = identifyPlant(opts_param)
+    %% Default values for opts
+    opts = struct();
+
+    %% Populate opts with input parameters
+    if exist('opts_param','var')
+        for opt = fieldnames(opts_param)'
+            opts.(opt{1}) = opts_param.(opt{1});
+        end
+    end
+
+    %% Options for Linearized
+    options = linearizeOptions;
+    options.SampleTime = 0;
+
+    %% Name of the Simulink File
+    mdl = 'sim_nano_station';
+
+    %% Input/Output definition
+    io(1) = linio([mdl, '/Fn'],           1, 'input');
+    io(2) = linio([mdl, '/Gm'],           1, 'input');
+    io(3) = linio([mdl, '/Fs_ext'],       1, 'input');
+    io(4) = linio([mdl, '/F_legs'],       1, 'input');
+    io(5) = linio([mdl, '/Dsample_meas'], 1, 'output');
+    io(6) = linio([mdl, '/F_meas'],       1, 'output');
+
+    %% Run the linearization
+    G = linearize(mdl, io, 0);
+    G.InputName  = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz', ...
+                    'Dgx', 'Dgy', 'Dgz', ...
+                    'Fsx', 'Fsy', 'Fsz', ...
+                    'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
+    G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', ...
+                    'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'};
+
+    %% Create the sub transfer functions
+    sys.G_cart = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}));
+    sys.G_gm   = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Dgx', 'Dgy', 'Dgz'}));
+    sys.G_fs   = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fsx', 'Fsy', 'Fsz'}));
+    sys.G_iff  = minreal(G({'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}));
+end
+

src/runSimulation.m

@@ -0,0 +1,56 @@
+function [] = runSimulation(sys_name, sys_mass, ctrl_type, act_damp)
+    %% Load the controller and save it for the simulation
+    if strcmp(ctrl_type, 'cl') && strcmp(act_damp, 'none')
+        K_obj = load('./mat/K_fb.mat');
+        K = K_obj.(sprintf('K_%s_%s', sys_mass, sys_name)); %#ok
+        save('./mat/controller.mat', 'K');
+    elseif strcmp(ctrl_type, 'cl') && strcmp(act_damp, 'iff')
+        K_obj = load('./mat/K_fb_iff.mat');
+        K = K_obj.(sprintf('K_%s_%s_iff', sys_mass, sys_name)); %#ok
+        save('./mat/controller.mat', 'K');
+    elseif strcmp(ctrl_type, 'ol')
+        K = tf(zeros(6)); %#ok
+        save('./mat/controller.mat', 'K');
+    else
+        error('ctrl_type should be cl or ol');
+    end
+
+    %% Active Damping
+    if strcmp(act_damp, 'iff')
+        K_iff_crit = load('./mat/K_iff_crit.mat');
+        K_iff = K_iff_crit.(sprintf('K_iff_%s_%s', sys_mass, sys_name)); %#ok
+        save('./mat/K_iff.mat', 'K_iff');
+    elseif strcmp(act_damp, 'none')
+        K_iff = tf(zeros(6)); %#ok
+        save('./mat/K_iff.mat', 'K_iff');
+    end
+
+    %%
+    if strcmp(sys_name, 'pz')
+    	initializeNanoHexapod(struct('actuator', 'piezo'));
+    elseif strcmp(sys_name, 'vc')
+        initializeNanoHexapod(struct('actuator', 'lorentz'));
+    else
+        error('sys_name should be pz or vc');
+    end
+    
+    if strcmp(sys_mass, 'light')
+    	initializeSample(struct('mass', 1));
+    elseif strcmp(sys_mass, 'heavy')
+        initializeSample(struct('mass', 50));
+    else
+        error('sys_mass should be light or heavy');
+    end
+
+    %% Run the simulation
+    sim('Assemblage.slx');
+
+    %% Split the Dsample matrix into vectors
+    [Dx, Dy, Dz, Rx, Ry, Rz] = matSplit(Dsample.Data, 1); %#ok
+    time = Dsample.Time; %#ok
+
+    %% Save the result
+    filename = sprintf('sim_%s_%s_%s_%s', sys_mass, sys_name, ctrl_type, act_damp);
+    save(sprintf('./mat/%s.mat', filename), ...
+        'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
+end

stewart-simscape

@@ -1 +1 @@
-Subproject commit bef54b5bf7967d8eb61a2d24d39a729e5d34c76d
+Subproject commit 9d1250990137ddc246a1d7bf4084b36377cc55a3