diff --git a/.gitattributes b/.gitattributes
new file mode 100644
index 0000000..a06e566
--- /dev/null
+++ b/.gitattributes
@@ -0,0 +1,3 @@
+*.pdf binary
+*.svg binary
+*.mat binary
diff --git a/.gitignore b/.gitignore
index 6b7e1a4..d7a02f8 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,5 +1,3 @@
-mat/
-figures/
 ltximg/
 slprj/
 matlab/slprj/
diff --git a/backup.org b/backup.org
new file mode 100644
index 0000000..61596e9
--- /dev/null
+++ b/backup.org
@@ -0,0 +1,15301 @@
+* Identified Open Loop Plant                                        :noexport:
+<<sec:test_id31_open_loop_plant>>
+** Introduction                                                      :ignore:
+
+** Matlab Init                                              :noexport:ignore:
+#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
+<<matlab-dir>>
+#+end_src
+
+#+begin_src matlab :exports none :results silent :noweb yes
+<<matlab-init>>
+#+end_src
+
+#+begin_src matlab :tangle no :noweb yes
+<<m-init-path>>
+#+end_src
+
+#+begin_src matlab :eval no :noweb yes
+<<m-init-path-tangle>>
+#+end_src
+
+#+begin_src matlab :noweb yes
+<<m-init-other>>
+#+end_src
+
+** Simscape Model
+<<sec:id31_simscape_model>>
+*** Introduction                                                    :ignore:
+
+*** Matlab Init                                            :noexport:ignore:
+#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
+<<matlab-dir>>
+#+end_src
+
+#+begin_src matlab :exports none :results silent :noweb yes
+<<matlab-init>>
+#+end_src
+
+#+begin_src matlab :tangle no :noweb yes
+<<m-init-path>>
+#+end_src
+
+#+begin_src matlab :eval no :noweb yes
+<<m-init-path-tangle>>
+#+end_src
+
+#+begin_src matlab :noweb yes
+<<m-init-other>>
+#+end_src
+
+*** Init model
+#+begin_src matlab
+%% Add directories to path for Simscape Model
+addpath('mat')
+addpath('matlab/subsystems')
+addpath('STEPS/nano_hexapod')
+addpath('STEPS/metrology')
+addpath('STEPS/png')
+#+end_src
+
+#+begin_src matlab
+%% Options for Linearized
+options = linearizeOptions;
+options.SampleTime = 0;
+
+%% Name of the Simulink File
+mdl = 'nass_model_id31';
+#+end_src
+
+#+begin_src matlab
+open(mdl)
+#+end_src
+
+#+begin_src matlab
+load('mat/conf_simulink.mat');
+
+%% Initialize each Simscape model elements
+initializeGround();
+initializeGranite();
+initializeTy();
+initializeRy();
+initializeRz();
+initializeMicroHexapod();
+
+initializeSimscapeConfiguration();
+initializeDisturbances('enable', false);
+initializeLoggingConfiguration('log', 'none');
+
+initializeController('type', 'open-loop');
+
+initializeNanoHexapod('flex_bot_type', '2dof', ...
+                      'flex_top_type', '3dof', ...
+                      'motion_sensor_type', 'plates', ...
+                      'actuator_type', '2dof');
+
+initializeSample('type', '0');
+
+initializePosError();
+
+initializeReferences();
+
+initializeRzEstimate('type', 'encoders');
+
+initializeLion();
+#+end_src
+
+*** Identify Transfer functions
+#+begin_src matlab
+%% Input/Output definition
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/Controller'],     1, 'openinput');               io_i = io_i + 1; % Actuator Inputs
+io(io_i) = linio([mdl, '/Micro-Station'],  3, 'openoutput', [], 'Fnlm');  io_i = io_i + 1; % Force Sensors
+io(io_i) = linio([mdl, '/Micro-Station'],  3, 'openoutput', [], 'Dnlm');  io_i = io_i + 1; % Encoders
+io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL');   io_i = io_i + 1; % Position Errors
+io(io_i) = linio([mdl, '/Micro-Station/metrology_5dof/Lion_Metrology'], 1, 'output');   io_i = io_i + 1; % Interferometers
+#+end_src
+
+#+begin_src matlab :exports none
+%% Identify the dynamics for IFF plant
+initializeSample('type', '0');
+Gm_m0 = linearize(mdl, io);
+Gm_m0.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
+Gm_m0.OutputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6', ...
+                    'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ...
+                    'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6', ...
+                    'd1', 'd2', 'd3', 'd4', 'd5'};
+
+initializeSample('type', '1');
+Gm_m1 = linearize(mdl, io);
+Gm_m1.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
+Gm_m1.OutputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6', ...
+                    'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ...
+                    'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6', ...
+                    'd1', 'd2', 'd3', 'd4', 'd5'};
+
+initializeSample('type', '2');
+Gm_m2 = linearize(mdl, io);
+Gm_m2.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
+Gm_m2.OutputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6', ...
+                    'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ...
+                    'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6', ...
+                    'd1', 'd2', 'd3', 'd4', 'd5'};
+
+initializeSample('type', '3');
+Gm_m3 = linearize(mdl, io);
+Gm_m3.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
+Gm_m3.OutputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6', ...
+                    'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ...
+                    'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6', ...
+                    'd1', 'd2', 'd3', 'd4', 'd5'};
+#+end_src
+
+#+begin_src matlab :exports none
+%% Save Identified Plants
+save('./matlab/mat/Gm.mat', 'Gm_m0', 'Gm_m1', 'Gm_m2', 'Gm_m3');
+#+end_src
+
+*** IFF Plant
+#+begin_src matlab :exports none :results none
+%% IFF transfer function - Simscape model
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('Fn%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
+    'DisplayName', '$\tau_{m,i}/u_i$ - $m_0$');
+for i = 2:6
+    plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
+        'HandleVisibility', 'off');
+end
+plot(freqs, abs(squeeze(freqresp(Gm_m1(sprintf('Fn%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
+    'DisplayName', '$\tau_{m,i}/u_i$ - $m_1$');
+for i = 2:6
+    plot(freqs, abs(squeeze(freqresp(Gm_m1(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
+        'HandleVisibility', 'off');
+end
+plot(freqs, abs(squeeze(freqresp(Gm_m2(sprintf('Fn%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
+    'DisplayName', '$\tau_{m,i}/u_i$ - $m_2$');
+for i = 2:6
+    plot(freqs, abs(squeeze(freqresp(Gm_m2(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
+        'HandleVisibility', 'off');
+end
+plot(freqs, abs(squeeze(freqresp(Gm_m3(sprintf('Fn%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
+    'DisplayName', '$\tau_{m,i}/u_i$ - $m_3$');
+for i = 2:6
+    plot(freqs, abs(squeeze(freqresp(Gm_m3(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
+        'HandleVisibility', 'off');
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
+
+ax2 = nexttile;
+hold on;
+for i =1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
+end
+for i =1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m1(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
+end
+for i =1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m2(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5]);
+end
+for i =1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m3(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-90, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_simscape_iff_ol_plant.pdf', 'width', 'wide', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_simscape_iff_ol_plant
+#+caption: IFF transfer function - Simscape model
+#+RESULTS:
+[[file:figs/id31_simscape_iff_ol_plant.png]]
+
+*** Encoder plant
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('dL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
+    'DisplayName', '$\tau_{m,i}/u_i$ - $m_0$');
+for i = 2:6
+    plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
+        'HandleVisibility', 'off');
+end
+plot(freqs, abs(squeeze(freqresp(Gm_m1(sprintf('dL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
+    'DisplayName', '$\tau_{m,i}/u_i$ - $m_1$');
+for i = 2:6
+    plot(freqs, abs(squeeze(freqresp(Gm_m1(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
+        'HandleVisibility', 'off');
+end
+plot(freqs, abs(squeeze(freqresp(Gm_m2(sprintf('dL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
+    'DisplayName', '$\tau_{m,i}/u_i$ - $m_2$');
+for i = 2:6
+    plot(freqs, abs(squeeze(freqresp(Gm_m2(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
+        'HandleVisibility', 'off');
+end
+plot(freqs, abs(squeeze(freqresp(Gm_m3(sprintf('dL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
+    'DisplayName', '$\tau_{m,i}/u_i$ - $m_3$');
+for i = 2:6
+    plot(freqs, abs(squeeze(freqresp(Gm_m3(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
+        'HandleVisibility', 'off');
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 1e-3]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
+
+ax2 = nexttile;
+hold on;
+for i =1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
+end
+for i =1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m1(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
+end
+for i =1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m2(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5]);
+end
+for i =1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m3(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-90, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_simscape_enc_ol_plant.pdf', 'width', 'wide', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_simscape_enc_ol_plant
+#+caption: ENC transfer function - Simscape model
+#+RESULTS:
+[[file:figs/id31_simscape_enc_ol_plant.png]]
+
+*** HAC Undamped plant
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
+    'DisplayName', '$\tau_{m,i}/u_i$ - $m_0$');
+for i = 2:6
+    plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
+        'HandleVisibility', 'off');
+end
+plot(freqs, abs(squeeze(freqresp(Gm_m1(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
+    'DisplayName', '$\tau_{m,i}/u_i$ - $m_1$');
+for i = 2:6
+    plot(freqs, abs(squeeze(freqresp(Gm_m1(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
+        'HandleVisibility', 'off');
+end
+plot(freqs, abs(squeeze(freqresp(Gm_m2(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
+    'DisplayName', '$\tau_{m,i}/u_i$ - $m_2$');
+for i = 2:6
+    plot(freqs, abs(squeeze(freqresp(Gm_m2(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
+        'HandleVisibility', 'off');
+end
+plot(freqs, abs(squeeze(freqresp(Gm_m3(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
+    'DisplayName', '$\tau_{m,i}/u_i$ - $m_3$');
+for i = 2:6
+    plot(freqs, abs(squeeze(freqresp(Gm_m3(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
+        'HandleVisibility', 'off');
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 1e-3]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
+
+ax2 = nexttile;
+hold on;
+for i =1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
+end
+for i =1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m1(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
+end
+for i =1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m2(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5]);
+end
+for i =1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m3(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-90, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_simscape_int_ol_plant.pdf', 'width', 'wide', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_simscape_int_ol_plant
+#+caption: INT transfer function - Simscape model
+#+RESULTS:
+[[file:figs/id31_simscape_int_ol_plant.png]]
+
+** Load Data                                                       :noexport:
+#+begin_src matlab
+%% Load identification data
+data_m0 = load(sprintf('%s/dynamics/2023-08-08_16-17_ol_plant_m0_Wz0.mat', mat_dir));
+data_m1 = load(sprintf('%s/dynamics/2023-08-08_18-57_ol_plant_m1_Wz0.mat', mat_dir));
+data_m2 = load(sprintf('%s/dynamics/2023-08-08_17-12_ol_plant_m2_Wz0.mat', mat_dir));
+data_m3 = load(sprintf('%s/dynamics/2023-08-08_18-20_ol_plant_m3_Wz0.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+% Sampling Time [s]
+Ts = 1e-4;
+
+% Hannning Windows
+Nfft = floor(2.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+#+end_src
+
+#+begin_src matlab :exports none
+% And we get the frequency vector
+[~, f] = tfestimate(data_m0.uL1.id_plant, data_m0.uL1.e_L1, win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+** IFF Plant
+#+begin_src matlab :exports none
+%% IFF Plant (transfer function from u to taum)
+G_iff_m0 = zeros(length(f), 6, 6);
+
+for i_strut = 1:6
+    eL = [data_m0.(sprintf("uL%i", i_strut)).Vs1 ; data_m0.(sprintf("uL%i", i_strut)).Vs2 ; data_m0.(sprintf("uL%i", i_strut)).Vs3 ; data_m0.(sprintf("uL%i", i_strut)).Vs4 ; data_m0.(sprintf("uL%i", i_strut)).Vs5 ; data_m0.(sprintf("uL%i", i_strut)).Vs6]';
+
+    G_iff_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% IFF Plant (transfer function from u to taum)
+G_iff_m1 = zeros(length(f), 6, 6);
+
+for i_strut = 1:6
+    eL = [data_m1.(sprintf("uL%i", i_strut)).Vs1 ; data_m1.(sprintf("uL%i", i_strut)).Vs2 ; data_m1.(sprintf("uL%i", i_strut)).Vs3 ; data_m1.(sprintf("uL%i", i_strut)).Vs4 ; data_m1.(sprintf("uL%i", i_strut)).Vs5 ; data_m1.(sprintf("uL%i", i_strut)).Vs6]';
+
+    G_iff_m1(:,:,i_strut) = tfestimate(data_m1.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% IFF Plant (transfer function from u to taum)
+G_iff_m2 = zeros(length(f), 6, 6);
+
+for i_strut = 1:6
+    eL = [data_m2.(sprintf("uL%i", i_strut)).Vs1 ; data_m2.(sprintf("uL%i", i_strut)).Vs2 ; data_m2.(sprintf("uL%i", i_strut)).Vs3 ; data_m2.(sprintf("uL%i", i_strut)).Vs4 ; data_m2.(sprintf("uL%i", i_strut)).Vs5 ; data_m2.(sprintf("uL%i", i_strut)).Vs6]';
+
+    G_iff_m2(:,:,i_strut) = tfestimate(data_m2.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% IFF Plant (transfer function from u to taum)
+G_iff_m3 = zeros(length(f), 6, 6);
+
+for i_strut = 1:6
+    eL = [data_m3.(sprintf("uL%i", i_strut)).Vs1 ; data_m3.(sprintf("uL%i", i_strut)).Vs2 ; data_m3.(sprintf("uL%i", i_strut)).Vs3 ; data_m3.(sprintf("uL%i", i_strut)).Vs4 ; data_m3.(sprintf("uL%i", i_strut)).Vs5 ; data_m3.(sprintf("uL%i", i_strut)).Vs6]';
+
+    G_iff_m3(:,:,i_strut) = tfestimate(data_m3.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% Save Identified Plants
+save('./matlab/mat/G_ol.mat', 'G_iff_m0', 'G_iff_m1', 'G_iff_m2', 'G_iff_m3', 'f');
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Measured transfer function from generated voltages to measured voltage on the force sensors
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_iff_m0(:, i, j)), 'color', [0, 0, 0, 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+for i = 1:6
+    plot(f, abs(G_iff_m0(:,i, i)), 'color', colors(i,:), ...
+        'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i));
+end
+plot(f, abs(G_iff_m0(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
+     'DisplayName', '$\tau_{m,i}/u_j$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-4, 1e2]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
+
+ax2 = nexttile;
+hold on;
+for i =1:6
+    plot(f, 180/pi*angle(G_iff_m0(:,i, i)));
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-90, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_iff_ol_plant_m0.pdf', 'width', 'full', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_iff_ol_plant_m0
+#+caption: Measured transfer function from generated voltages to measured voltage on the force sensors
+#+RESULTS:
+[[file:figs/id31_iff_ol_plant_m0.png]]
+
+The measured frequency response functions from DAC voltages $u_i$ to measured voltages on the force sensors $\tau_{m,i}$ are compared with the simscape model in Figure ref:fig:id31_comp_simscape_frf_ol_iff.
+
+#+begin_src matlab :exports none :results none
+%% Comparison of the Simscape model and identified IFF plant
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_iff_m0(:, i, j)), 'color', [colors(1,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+for i = 1:5
+    for j = i+1:6
+        plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('Fn%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(f, abs(G_iff_m0(:,1, 1)), 'color', [colors(1,:)], ...
+     'DisplayName', '$\tau_{m,i}/u_i$ - Identified');
+for i = 2:6
+    plot(f, abs(G_iff_m0(:,i, i)), 'color', [colors(1,:)], ...
+         'HandleVisibility', 'off');
+end
+plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('Fn%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
+     'DisplayName', '$\tau_{m,i}/u_i$ - Simscape');
+for i = 2:6
+    plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
+         'HandleVisibility', 'off');
+end
+plot(f, abs(G_iff_m0(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
+     'DisplayName', '$\tau_{m,i}/u_j$ - Identified');
+plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('Fn%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
+     'DisplayName', '$\tau_{m,i}/u_j$ - Simscape');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-4, 1e2]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
+
+ax2 = nexttile;
+hold on;
+for i = 1:6
+    plot(f, 180/pi*angle(G_iff_m0(:,i, i)), 'color', [colors(1,:)]);
+end
+for i = 1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-90, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_comp_simscape_frf_ol_iff.pdf', 'width', 'full', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_comp_simscape_frf_ol_iff
+#+caption: Comparison of the Simscape model and identified IFF plant
+#+RESULTS:
+[[file:figs/id31_comp_simscape_frf_ol_iff.png]]
+
+The effect of the payload mass on the diagonal elements are shown in Figure ref:fig:id31_effect_mass_frf_ol_iff.
+
+#+begin_src matlab :exports none :results none
+%% Effect of the payload mass on the IFF plant
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_iff_m0(:,1, 1)), 'color', [colors(1,:), 0.5], ...
+     'DisplayName', '$\tau_{m,i}/u_i$ - m0');
+for i = 2:6
+    plot(f, abs(G_iff_m0(:,i, i)), 'color', [colors(1,:), 0.5], ...
+         'HandleVisibility', 'off');
+end
+plot(f, abs(G_iff_m1(:,1, 1)), 'color', [colors(2,:), 0.5], ...
+     'DisplayName', '$\tau_{m,i}/u_i$ - m1');
+for i = 2:6
+    plot(f, abs(G_iff_m1(:,i, i)), 'color', [colors(2,:), 0.5], ...
+         'HandleVisibility', 'off');
+end
+plot(f, abs(G_iff_m2(:,1, 1)), 'color', [colors(3,:), 0.5], ...
+     'DisplayName', '$\tau_{m,i}/u_i$ - m2');
+for i = 2:6
+    plot(f, abs(G_iff_m2(:,i, i)), 'color', [colors(3,:), 0.5], ...
+         'HandleVisibility', 'off');
+end
+plot(f, abs(G_iff_m3(:,1, 1)), 'color', [colors(4,:), 0.5], ...
+     'DisplayName', '$\tau_{m,i}/u_i$ - m3');
+for i = 2:6
+    plot(f, abs(G_iff_m3(:,i, i)), 'color', [colors(4,:), 0.5], ...
+         'HandleVisibility', 'off');
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-2, 1e2]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
+
+ax2 = nexttile;
+hold on;
+for i = 1:6
+    plot(f, 180/pi*angle(G_iff_m0(:,i, i)), 'color', [colors(1,:), 0.5]);
+end
+for i = 1:6
+    plot(f, 180/pi*angle(G_iff_m1(:,i, i)), 'color', [colors(2,:), 0.5]);
+end
+for i = 1:6
+    plot(f, 180/pi*angle(G_iff_m2(:,i, i)), 'color', [colors(3,:), 0.5]);
+end
+for i = 1:6
+    plot(f, 180/pi*angle(G_iff_m3(:,i, i)), 'color', [colors(4,:), 0.5]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-90, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_effect_mass_frf_ol_iff.pdf', 'width', 'full', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_effect_mass_frf_ol_iff
+#+caption: Effect of the payload mass on the IFF plant
+#+RESULTS:
+[[file:figs/id31_effect_mass_frf_ol_iff.png]]
+
+** Encoder plant
+#+begin_src matlab :exports none
+%% ENC Plant (transfer function from u to taum)
+G_enc_m0 = zeros(length(f), 6, 6);
+
+for i_strut = 1:6
+    eL = [data_m0.(sprintf("uL%i", i_strut)).dL1 ; data_m0.(sprintf("uL%i", i_strut)).dL2 ; data_m0.(sprintf("uL%i", i_strut)).dL3 ; data_m0.(sprintf("uL%i", i_strut)).dL4 ; data_m0.(sprintf("uL%i", i_strut)).dL5 ; data_m0.(sprintf("uL%i", i_strut)).dL6]';
+
+    G_enc_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% ENC Plant (transfer function from u to taum)
+G_enc_m1 = zeros(length(f), 6, 6);
+
+for i_strut = 1:6
+    eL = [data_m1.(sprintf("uL%i", i_strut)).dL1 ; data_m1.(sprintf("uL%i", i_strut)).dL2 ; data_m1.(sprintf("uL%i", i_strut)).dL3 ; data_m1.(sprintf("uL%i", i_strut)).dL4 ; data_m1.(sprintf("uL%i", i_strut)).dL5 ; data_m1.(sprintf("uL%i", i_strut)).dL6]';
+
+    G_enc_m1(:,:,i_strut) = tfestimate(data_m1.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% ENC Plant (transfer function from u to taum)
+G_enc_m2 = zeros(length(f), 6, 6);
+
+for i_strut = 1:6
+    eL = [data_m2.(sprintf("uL%i", i_strut)).dL1 ; data_m2.(sprintf("uL%i", i_strut)).dL2 ; data_m2.(sprintf("uL%i", i_strut)).dL3 ; data_m2.(sprintf("uL%i", i_strut)).dL4 ; data_m2.(sprintf("uL%i", i_strut)).dL5 ; data_m2.(sprintf("uL%i", i_strut)).dL6]';
+
+    G_enc_m2(:,:,i_strut) = tfestimate(data_m2.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% ENC Plant (transfer function from u to taum)
+G_enc_m3 = zeros(length(f), 6, 6);
+
+for i_strut = 1:6
+    eL = [data_m3.(sprintf("uL%i", i_strut)).dL1 ; data_m3.(sprintf("uL%i", i_strut)).dL2 ; data_m3.(sprintf("uL%i", i_strut)).dL3 ; data_m3.(sprintf("uL%i", i_strut)).dL4 ; data_m3.(sprintf("uL%i", i_strut)).dL5 ; data_m3.(sprintf("uL%i", i_strut)).dL6]';
+
+    G_enc_m3(:,:,i_strut) = tfestimate(data_m3.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% Save Identified Plants
+save('./matlab/mat/G_ol.mat', 'G_enc_m0', 'G_enc_m1', 'G_enc_m2', 'G_enc_m3', '-append');
+#+end_src
+
+The identified frequency response functions from general voltages $u_i$ to measured displacement of the struts by the encoders $d\mathcal{L}_i$ are compared with the simscape model in Figure ref:fig:id31_comp_simscape_frf_ol_enc.
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_enc_m0(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
+     'DisplayName', '$d\mathcal{L}_i/u_j$ - Identified');
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_enc_m0(:, i, j)), 'color', [colors(1,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('dL%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
+     'DisplayName', '$d\mathcal{L}_i/u_j$ - Simscape');
+for i = 1:5
+    for j = i+1:6
+        plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('dL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(f, abs(G_enc_m0(:, 1, 1)), 'color', [colors(1,:)], ...
+     'DisplayName', '$d\mathcal{L}_i/u_i$ - Identified');
+for i = 2:6
+    plot(f, abs(G_enc_m0(:,i, i)), 'color', [colors(1,:)], ...
+            'HandleVisibility', 'off');
+end
+plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('dL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
+     'DisplayName', '$d\mathcal{L}_i/u_i$ - Simscape');
+for i = 2:6
+    plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
+            'HandleVisibility', 'off');
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 2e-4]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
+
+ax2 = nexttile;
+hold on;
+for i = 1:6
+    plot(f, 180/pi*angle(G_enc_m0(:,i, i)), 'color', [colors(1,:)]);
+end
+for i = 1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-90, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_comp_simscape_frf_ol_enc.pdf', 'width', 'wide', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_comp_simscape_frf_ol_enc
+#+caption: Comparison of the Simscape model and identified plant - Transfer functions from voltages to encoders
+#+RESULTS:
+[[file:figs/id31_comp_simscape_frf_ol_enc.png]]
+
+#+begin_src matlab :exports none :results none
+%% Effect of the payload mass on the transfer function from actuator voltage to encoder displacement
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_enc_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
+     'DisplayName', '$d\mathcal{L}_i/u_i$ - m0');
+for i = 2:6
+    plot(f, abs(G_enc_m0(:,i, i)), 'color', [colors(1,:), 0.5], ...
+            'HandleVisibility', 'off');
+end
+plot(f, abs(G_enc_m1(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
+     'DisplayName', '$d\mathcal{L}_i/u_i$ - m1');
+for i = 2:6
+    plot(f, abs(G_enc_m1(:,i, i)), 'color', [colors(2,:), 0.5], ...
+            'HandleVisibility', 'off');
+end
+plot(f, abs(G_enc_m2(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
+     'DisplayName', '$d\mathcal{L}_i/u_i$ - m2');
+for i = 2:6
+    plot(f, abs(G_enc_m2(:,i, i)), 'color', [colors(3,:), 0.5], ...
+            'HandleVisibility', 'off');
+end
+plot(f, abs(G_enc_m3(:, 1, 1)), 'color', [colors(4,:), 0.5], ...
+     'DisplayName', '$d\mathcal{L}_i/u_i$ - m3');
+for i = 2:6
+    plot(f, abs(G_enc_m3(:,i, i)), 'color', [colors(4,:), 0.5], ...
+            'HandleVisibility', 'off');
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 2e-4]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
+
+ax2 = nexttile;
+hold on;
+for i = 1:6
+    plot(f, 180/pi*angle(G_enc_m0(:,i, i)), 'color', [colors(1,:), 0.5]);
+end
+for i = 1:6
+    plot(f, 180/pi*angle(G_enc_m1(:,i, i)), 'color', [colors(2,:), 0.5]);
+end
+for i = 1:6
+    plot(f, 180/pi*angle(G_enc_m2(:,i, i)), 'color', [colors(3,:), 0.5]);
+end
+for i = 1:6
+    plot(f, 180/pi*angle(G_enc_m3(:,i, i)), 'color', [colors(4,:), 0.5]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-90, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_effect_mass_frf_ol_enc.pdf', 'width', 'wide', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_effect_mass_frf_ol_enc
+#+caption: Effect of the payload mass on the transfer function from actuator voltage to encoder displacement
+#+RESULTS:
+[[file:figs/id31_effect_mass_frf_ol_enc.png]]
+
+** HAC Undamped plant
+#+begin_src matlab :exports none
+%% INT Plant (transfer function from u to taum)
+G_int_m0 = zeros(length(f), 6, 6);
+
+for i_strut = 1:6
+    eL = [data_m0.(sprintf("uL%i", i_strut)).e_L1 ; data_m0.(sprintf("uL%i", i_strut)).e_L2 ; data_m0.(sprintf("uL%i", i_strut)).e_L3 ; data_m0.(sprintf("uL%i", i_strut)).e_L4 ; data_m0.(sprintf("uL%i", i_strut)).e_L5 ; data_m0.(sprintf("uL%i", i_strut)).e_L6]';
+
+    G_int_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% INT Plant (transfer function from u to taum)
+G_int_m1 = zeros(length(f), 6, 6);
+
+for i_strut = 1:6
+    eL = [data_m1.(sprintf("uL%i", i_strut)).e_L1 ; data_m1.(sprintf("uL%i", i_strut)).e_L2 ; data_m1.(sprintf("uL%i", i_strut)).e_L3 ; data_m1.(sprintf("uL%i", i_strut)).e_L4 ; data_m1.(sprintf("uL%i", i_strut)).e_L5 ; data_m1.(sprintf("uL%i", i_strut)).e_L6]';
+
+    G_int_m1(:,:,i_strut) = tfestimate(data_m1.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% INT Plant (transfer function from u to taum)
+G_int_m2 = zeros(length(f), 6, 6);
+
+for i_strut = 1:6
+    eL = [data_m2.(sprintf("uL%i", i_strut)).e_L1 ; data_m2.(sprintf("uL%i", i_strut)).e_L2 ; data_m2.(sprintf("uL%i", i_strut)).e_L3 ; data_m2.(sprintf("uL%i", i_strut)).e_L4 ; data_m2.(sprintf("uL%i", i_strut)).e_L5 ; data_m2.(sprintf("uL%i", i_strut)).e_L6]';
+
+    G_int_m2(:,:,i_strut) = tfestimate(data_m2.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% INT Plant (transfer function from u to taum)
+G_int_m3 = zeros(length(f), 6, 6);
+
+for i_strut = 1:6
+    eL = [data_m3.(sprintf("uL%i", i_strut)).e_L1 ; data_m3.(sprintf("uL%i", i_strut)).e_L2 ; data_m3.(sprintf("uL%i", i_strut)).e_L3 ; data_m3.(sprintf("uL%i", i_strut)).e_L4 ; data_m3.(sprintf("uL%i", i_strut)).e_L5 ; data_m3.(sprintf("uL%i", i_strut)).e_L6]';
+
+    G_int_m3(:,:,i_strut) = tfestimate(data_m3.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% Save Identified Plants
+save('./matlab/mat/G_ol.mat', 'G_int_m0', 'G_int_m1', 'G_int_m2', 'G_int_m3', '-append');
+#+end_src
+
+The identified frequency response functions from actuator voltages $u_i$ to measured strut motion from the external metrology (i.e. the interferometers) are compare with the simscape model in Figure ref:fig:id31_comp_simscape_frf_ol_int.
+
+#+begin_src matlab :exports none :results none
+%% Measured transfer function from generated voltages to measured voltage on the force sensors
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_int_m0(:, i, j)), 'color', [0, 0, 0, 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+for i = 1:6
+    plot(f, abs(G_int_m0(:,i, i)), 'color', colors(i,:), ...
+        'DisplayName', sprintf('$e\\mathcal{L}_%i/u_%i$', i, i));
+end
+plot(f, abs(G_int_m0(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
+     'DisplayName', '$-e\mathcal{L}_i/u_j$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 2e-4]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
+
+ax2 = nexttile;
+hold on;
+for i =1:6
+    plot(f, 180/pi*angle(G_int_m0(:,i, i)));
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-90, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_int_ol_plant_m0.pdf', 'width', 'full', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_int_ol_plant_m0
+#+caption: Measured transfer function from generated voltages to measured voltage on the force sensors
+#+RESULTS:
+[[file:figs/id31_int_ol_plant_m0.png]]
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_int_m0(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
+     'DisplayName', '$-e\mathcal{L}_i/u_j$ - Identified');
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_int_m0(:, i, j)), 'color', [colors(1,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('eL%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
+     'DisplayName', '$-e\mathcal{L}_i/u_j$ - Simscape');
+for i = 1:5
+    for j = i+1:6
+        plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(f, abs(G_int_m0(:, 1, 1)), 'color', [colors(1,:)], ...
+     'DisplayName', '$-e\mathcal{L}_i/u_i$ - Identified');
+for i = 2:6
+    plot(f, abs(G_int_m0(:,i, i)), 'color', [colors(1,:)], ...
+         'HandleVisibility', 'off');
+end
+plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
+     'DisplayName', '$-e\mathcal{L}_i/u_i$ - Simscape');
+for i = 2:6
+    plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
+         'HandleVisibility', 'off');
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 2e-4]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
+
+ax2 = nexttile;
+hold on;
+for i = 1:6
+    plot(f, 180/pi*angle(G_int_m0(:,i, i)), 'color', [colors(1,:)]);
+end
+for i = 1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-90, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_comp_simscape_frf_ol_int.pdf', 'width', 'full', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_comp_simscape_frf_ol_int
+#+caption: Comparison of the Simscape model and identified plant - Transfer functions from voltages to estimated strut motion from interferometers
+#+RESULTS:
+[[file:figs/id31_comp_simscape_frf_ol_int.png]]
+
+#+begin_src matlab :exports none :results none
+%% Effect of the payload mass on the transfer functions from actuator voltage to measured strut motion by the external metrology
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_int_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
+     'DisplayName', '$-e\mathcal{L}_i/u_i$ - m0');
+for i = 2:6
+    plot(f, abs(G_int_m0(:,i, i)), 'color', [colors(1,:), 0.5], ...
+         'HandleVisibility', 'off');
+end
+plot(f, abs(G_int_m1(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
+     'DisplayName', '$-e\mathcal{L}_i/u_i$ - m1');
+for i = 2:6
+    plot(f, abs(G_int_m1(:,i, i)), 'color', [colors(2,:), 0.5], ...
+         'HandleVisibility', 'off');
+end
+plot(f, abs(G_int_m2(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
+     'DisplayName', '$-e\mathcal{L}_i/u_i$ - m2');
+for i = 2:6
+    plot(f, abs(G_int_m2(:,i, i)), 'color', [colors(3,:), 0.5], ...
+         'HandleVisibility', 'off');
+end
+plot(f, abs(G_int_m3(:, 1, 1)), 'color', [colors(4,:), 0.5], ...
+     'DisplayName', '$-e\mathcal{L}_i/u_i$ - m3');
+for i = 2:6
+    plot(f, abs(G_int_m3(:,i, i)), 'color', [colors(4,:), 0.5], ...
+         'HandleVisibility', 'off');
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 2e-4]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
+
+ax2 = nexttile;
+hold on;
+for i = 1:6
+    plot(f, 180/pi*angle(G_int_m0(:,i, i)), 'color', [colors(1,:), 0.5]);
+end
+for i = 1:6
+    plot(f, 180/pi*angle(G_int_m1(:,i, i)), 'color', [colors(2,:), 0.5]);
+end
+for i = 1:6
+    plot(f, 180/pi*angle(G_int_m2(:,i, i)), 'color', [colors(3,:), 0.5]);
+end
+for i = 1:6
+    plot(f, 180/pi*angle(G_int_m3(:,i, i)), 'color', [colors(4,:), 0.5]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-90, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_effect_mass_frf_ol_int.pdf', 'width', 'wide', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_effect_mass_frf_ol_int
+#+caption: Effect of the payload mass on the transfer functions from actuator voltage to measured strut motion by the external metrology
+#+RESULTS:
+[[file:figs/id31_effect_mass_frf_ol_int.png]]
+
+** SRI Figures                                                     :noexport:
+*** Open Loop Figure / Effect of mass
+#+begin_src matlab :exports none
+%% Save Identified Plants
+load('G_ol.mat', 'G_int_m0', 'G_int_m1', 'G_int_m2', 'G_int_m3', 'f');
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Measured transfer function from generated voltages to measured voltage on the force sensors
+figure;
+t = tiledlayout(6, 6, 'TileSpacing', 'compact', 'Padding', 'None');
+
+for i = 1:6
+    for j = 1:6
+        nexttile();
+        if i == j
+            plot(f, abs(G_int_m0(:, i, j)), 'color', [colors(1,:), 1]);
+        else
+            plot(f, abs(G_int_m0(:, i, j)), 'color', [colors(1,:), 0.5]);
+        end
+        set(gca, 'XTickLabel',[]);
+        set(gca, 'YTickLabel',[]);
+        if j == 1
+            ylabel(sprintf('$dL_{%i}$', i))
+        end
+        if i == 6
+            xlabel(sprintf('$u_{%i}$', j))
+        end
+        set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+        ylim([1e-7, 2e-4]);
+        xlim([1, 1e3]);
+    end
+end
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/sri2024_6x6_plant.pdf', 'width', 1000, 'height', 1000, 'transparent', false);
+#+end_src
+
+#+RESULTS:
+[[file:figs/sri2024_6x6_plant.png]]
+
+#+begin_src matlab :exports none :results none
+%% Measured transfer function from generated voltages to measured voltage on the force sensors
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_int_m0(:, i, j)), 'color', [colors(1,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(f, abs(G_int_m0(:,1, 1)), 'color', [colors(1,:), 1.0], ...
+     'DisplayName', '$m = 0$ kg');
+for i = 2:6
+    plot(f, abs(G_int_m0(:,i, i)), 'color', [colors(1,:), 1.0], ...
+         'HandleVisibility', 'off');
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 2e-4]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+for i =1:6
+    plot(f, 180/pi*unwrapphase(angle(-G_int_m0(:,i, i)), f), 'color', [colors(1,:), 1.0]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-270, 20])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/sri2024_plant_m0.pdf', 'width', 1000, 'height', 1000, 'transparent', false);
+#+end_src
+
+#+RESULTS:
+[[file:figs/sri2024_plant_m0.png]]
+
+#+begin_src matlab :exports none :results none
+%% Measured transfer function from generated voltages to measured voltage on the force sensors
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_int_m0(:, i, j)), 'color', [colors(1,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(f, abs(G_int_m0(:,1, 1)), 'color', [colors(1,:), 1.0], ...
+     'DisplayName', '$m = 0$ kg');
+for i = 2:6
+    plot(f, abs(G_int_m0(:,i, i)), 'color', [colors(1,:), 1.0], ...
+         'HandleVisibility', 'off');
+end
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_int_m1(:, i, j)), 'color', [colors(2,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(f, abs(G_int_m1(:,1, 1)), 'color', [colors(2,:), 1.0], ...
+     'DisplayName', '$m = 15$ kg');
+for i = 2:6
+    plot(f, abs(G_int_m1(:,i, i)), 'color', [colors(2,:), 1.0], ...
+         'HandleVisibility', 'off');
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 2e-4]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+for i =1:6
+    plot(f, 180/pi*unwrapphase(angle(-G_int_m0(:,i, i)), f), 'color', [colors(1,:), 1.0]);
+    plot(f, 180/pi*unwrapphase(angle(-G_int_m1(:,i, i)), f), 'color', [colors(2,:), 1.0]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-270, 20])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/sri2024_plant_m1.pdf', 'width', 1000, 'height', 1000, 'transparent', false);
+#+end_src
+
+#+RESULTS:
+[[file:figs/sri2024_plant_m1.png]]
+
+#+begin_src matlab :exports none :results none
+%% Measured transfer function from generated voltages to measured voltage on the force sensors
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_int_m0(:, i, j)), 'color', [colors(1,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(f, abs(G_int_m0(:,1, 1)), 'color', [colors(1,:), 1.0], ...
+     'DisplayName', '$m = 0$ kg');
+for i = 2:6
+    plot(f, abs(G_int_m0(:,i, i)), 'color', [colors(1,:), 1.0], ...
+         'HandleVisibility', 'off');
+end
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_int_m1(:, i, j)), 'color', [colors(2,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(f, abs(G_int_m1(:,1, 1)), 'color', [colors(2,:), 1.0], ...
+     'DisplayName', '$m = 15$ kg');
+for i = 2:6
+    plot(f, abs(G_int_m1(:,i, i)), 'color', [colors(2,:), 1.0], ...
+         'HandleVisibility', 'off');
+end
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_int_m2(:, i, j)), 'color', [colors(3,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(f, abs(G_int_m2(:,1, 1)), 'color', [colors(3,:), 1.0], ...
+     'DisplayName', '$m = 30$ kg');
+for i = 2:6
+    plot(f, abs(G_int_m2(:,i, i)), 'color', [colors(3,:), 1.0], ...
+         'HandleVisibility', 'off');
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 2e-4]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+for i =1:6
+    plot(f, 180/pi*unwrapphase(angle(-G_int_m0(:,i, i)), f), 'color', [colors(1,:), 1.0]);
+    plot(f, 180/pi*unwrapphase(angle(-G_int_m1(:,i, i)), f), 'color', [colors(2,:), 1.0]);
+    plot(f, 180/pi*unwrapphase(angle(-G_int_m2(:,i, i)), f), 'color', [colors(3,:), 1.0]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-270, 20])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/sri2024_plant_m2.pdf', 'width', 1000, 'height', 1000, 'transparent', false);
+#+end_src
+
+#+RESULTS:
+[[file:figs/sri2024_plant_m2.png]]
+
+#+begin_src matlab :exports none :results none
+%% Measured transfer function from generated voltages to measured voltage on the force sensors
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_int_m0(:, i, j)), 'color', [colors(1,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(f, abs(G_int_m0(:,1, 1)), 'color', [colors(1,:), 1.0], ...
+     'DisplayName', '$m = 0$ kg');
+for i = 2:6
+    plot(f, abs(G_int_m0(:,i, i)), 'color', [colors(1,:), 1.0], ...
+         'HandleVisibility', 'off');
+end
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_int_m1(:, i, j)), 'color', [colors(2,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(f, abs(G_int_m1(:,1, 1)), 'color', [colors(2,:), 1.0], ...
+     'DisplayName', '$m = 15$ kg');
+for i = 2:6
+    plot(f, abs(G_int_m1(:,i, i)), 'color', [colors(2,:), 1.0], ...
+         'HandleVisibility', 'off');
+end
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_int_m2(:, i, j)), 'color', [colors(3,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(f, abs(G_int_m2(:,1, 1)), 'color', [colors(3,:), 1.0], ...
+     'DisplayName', '$m = 30$ kg');
+for i = 2:6
+    plot(f, abs(G_int_m2(:,i, i)), 'color', [colors(3,:), 1.0], ...
+         'HandleVisibility', 'off');
+end
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_int_m3(:, i, j)), 'color', [colors(4,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(f, abs(G_int_m3(:,1, 1)), 'color', [colors(4,:), 1.0], ...
+     'DisplayName', '$m = 45$ kg');
+for i = 2:6
+    plot(f, abs(G_int_m3(:,i, i)), 'color', [colors(4,:), 1.0], ...
+         'HandleVisibility', 'off');
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 2e-4]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+for i =1:6
+    plot(f, 180/pi*unwrapphase(angle(-G_int_m0(:,i, i)), f), 'color', [colors(1,:), 1.0]);
+    plot(f, 180/pi*unwrapphase(angle(-G_int_m1(:,i, i)), f), 'color', [colors(2,:), 1.0]);
+    plot(f, 180/pi*unwrapphase(angle(-G_int_m2(:,i, i)), f), 'color', [colors(3,:), 1.0]);
+    plot(f, 180/pi*unwrapphase(angle(-G_int_m3(:,i, i)), f), 'color', [colors(4,:), 1.0]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-270, 20])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/sri2024_plant_m3.pdf', 'width', 1000, 'height', 1000, 'transparent', false);
+#+end_src
+
+#+RESULTS:
+[[file:figs/sri2024_plant_m3.png]]
+
+*** Effect of Active Damping
+#+begin_src matlab :exports none
+%% Save Identified Plants
+load('G_hac.mat', 'G_hac_m0', 'G_hac_m1', 'G_hac_m2', 'G_hac_m3', 'f');
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% description
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_int_m0(:, 1, 1)), 'color', [colors(1,:)], ...
+     'DisplayName', '$m = 0$ - OL');
+for i = 2:6
+    plot(f, abs(G_int_m0(:,i, i)), 'color', [colors(1,:)], ...
+         'HandleVisibility', 'off')
+end
+plot(f, abs(G_hac_m0(:, 1, 1)), 'color', [colors(2,:)], ...
+     'DisplayName', '$m = 0$ - Damped');
+for i = 2:6
+    plot(f, abs(G_hac_m0(:,i, i)), 'color', [colors(2,:)], ...
+         'HandleVisibility', 'off')
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 2e-4]);
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+for i =1:6
+    plot(f, 180/pi*unwrapphase(angle(-G_int_m0(:,i, i)), f), 'color', [colors(1,:)]);
+end
+for i =1:6
+    plot(f, 180/pi*unwrapphase(angle(G_hac_m0(:,i, i)), f), 'color', [colors(2,:)]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-270, 20])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/sri2024_plant_m0_comp_damped.pdf', 'width', 1000, 'height', 1000, 'transparent', false);
+#+end_src
+
+#+RESULTS:
+[[file:figs/sri2024_plant_m0_comp_damped.png]]
+
+#+begin_src matlab :exports none :results none
+%% Measured transfer function from generated voltages to measured voltage on the force sensors
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_hac_m0(:, i, j)), 'color', [colors(1,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(f, abs(G_hac_m0(:,1, 1)), 'color', [colors(1,:), 1.0], ...
+     'DisplayName', '$m = 0$ kg');
+for i = 2:6
+    plot(f, abs(G_hac_m0(:,i, i)), 'color', [colors(1,:), 1.0], ...
+         'HandleVisibility', 'off');
+end
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_hac_m1(:, i, j)), 'color', [colors(2,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(f, abs(G_hac_m1(:,1, 1)), 'color', [colors(2,:), 1.0], ...
+     'DisplayName', '$m = 15$ kg');
+for i = 2:6
+    plot(f, abs(G_hac_m1(:,i, i)), 'color', [colors(2,:), 1.0], ...
+         'HandleVisibility', 'off');
+end
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_hac_m2(:, i, j)), 'color', [colors(3,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(f, abs(G_hac_m2(:,1, 1)), 'color', [colors(3,:), 1.0], ...
+     'DisplayName', '$m = 30$ kg');
+for i = 2:6
+    plot(f, abs(G_hac_m2(:,i, i)), 'color', [colors(3,:), 1.0], ...
+         'HandleVisibility', 'off');
+end
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_hac_m3(:, i, j)), 'color', [colors(4,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(f, abs(G_hac_m3(:,1, 1)), 'color', [colors(4,:), 1.0], ...
+     'DisplayName', '$m = 45$ kg');
+for i = 2:6
+    plot(f, abs(G_hac_m3(:,i, i)), 'color', [colors(4,:), 1.0], ...
+         'HandleVisibility', 'off');
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 2e-4]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+for i =1:6
+    plot(f, 180/pi*unwrapphase(angle(G_hac_m0(:,i, i)), f), 'color', [colors(1,:), 1.0]);
+    plot(f, 180/pi*unwrapphase(angle(G_hac_m1(:,i, i)), f), 'color', [colors(2,:), 1.0]);
+    plot(f, 180/pi*unwrapphase(angle(G_hac_m2(:,i, i)), f), 'color', [colors(3,:), 1.0]);
+    plot(f, 180/pi*unwrapphase(angle(G_hac_m3(:,i, i)), f), 'color', [colors(4,:), 1.0]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-270, 20])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/sri2024_plant_m3_damped.pdf', 'width', 1000, 'height', 1000, 'transparent', false);
+#+end_src
+
+#+RESULTS:
+[[file:figs/sri2024_plant_m3_damped.png]]
+
+#+begin_src matlab :exports none :results none
+%% Measured transfer function from generated voltages to measured voltage on the force sensors
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_int_m0(:,1,1)), 'color', [colors(1,:), 0.5], 'DisplayName', 'Undamped');
+plot(f, abs(G_hac_m0(:,1,1)), 'color', [colors(2,:), 0.5], 'DisplayName', 'damped');
+for i = 1:6
+    plot(f, abs(G_int_m0(:,i, i)), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
+    plot(f, abs(G_int_m1(:,i, i)), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
+    plot(f, abs(G_int_m2(:,i, i)), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
+    plot(f, abs(G_int_m3(:,i, i)), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
+end
+for i = 1:6
+    plot(f, abs(G_hac_m0(:,i, i)), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
+    plot(f, abs(G_hac_m1(:,i, i)), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
+    plot(f, abs(G_hac_m2(:,i, i)), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
+    plot(f, abs(G_hac_m3(:,i, i)), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+ylim([1e-8, 2e-4]);
+
+ax2 = nexttile;
+hold on;
+for i =1:6
+    plot(f, 180/pi*unwrapphase(angle(-G_int_m0(:,i, i)), f), 'color', [colors(1,:), 0.5]);
+    plot(f, 180/pi*unwrapphase(angle(-G_int_m1(:,i, i)), f), 'color', [colors(1,:), 0.5]);
+    plot(f, 180/pi*unwrapphase(angle(-G_int_m2(:,i, i)), f), 'color', [colors(1,:), 0.5]);
+    plot(f, 180/pi*unwrapphase(angle(-G_int_m3(:,i, i)), f), 'color', [colors(1,:), 0.5]);
+end
+for i = 1:6
+    plot(f, 180/pi*unwrapphase(angle(G_hac_m0(:,i, i)), f), 'color', [colors(2,:), 0.5]);
+    plot(f, 180/pi*unwrapphase(angle(G_hac_m1(:,i, i)), f), 'color', [colors(2,:), 0.5]);
+    plot(f, 180/pi*unwrapphase(angle(G_hac_m2(:,i, i)), f), 'color', [colors(2,:), 0.5]);
+    plot(f, 180/pi*unwrapphase(angle(G_hac_m3(:,i, i)), f), 'color', [colors(2,:), 0.5]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-270, 20])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/sri2024_plant_comp_damped_undamped.pdf', 'width', 1000, 'height', 1000, 'transparent', false);
+#+end_src
+
+#+RESULTS:
+[[file:figs/sri2024_plant_comp_damped_undamped.png]]
+
+*** Comparison with the Model
+#+begin_src matlab
+%% Save Damped Plants
+load('Gm.mat', 'Gm_hac_m0', 'Gm_hac_m1', 'Gm_hac_m2', 'Gm_hac_m3');
+#+end_src
+
+All elements:
+#+begin_src matlab :exports none :results none
+%% Measured transfer function from generated voltages to measured voltage on the force sensors
+figure;
+t = tiledlayout(6, 6, 'TileSpacing', 'compact', 'Padding', 'None');
+
+for i = 1:6
+    for j = 1:6
+        nexttile();
+        hold on;
+        if i == j
+            plot(f, abs(G_hac_m0(:, i, j)), 'color', [colors(i,:), 0.5]);
+            plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), '--', 'color', colors(i,:));
+        else
+            plot(f, abs(G_hac_m0(:, i, j)), 'color', [0, 0, 0, 0.5]);
+            plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), '--', 'color', [0, 0, 0, 1]);
+        end
+        hold off;
+        set(gca, 'XTickLabel',[]);
+        set(gca, 'YTickLabel',[]);
+        if j == 1
+            ylabel(sprintf('$dL_{%i}$', i))
+        end
+        if i == 6
+            xlabel(sprintf('$u_{%i}$', j))
+        end
+        set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+        ylim([1e-7, 1e-4]);
+        xlim([1, 1e3]);
+    end
+end
+#+end_src
+#+begin_src matlab :exports none :results none
+%% Measured transfer function from generated voltages to measured voltage on the force sensors
+figure;
+t = tiledlayout(6, 6, 'TileSpacing', 'compact', 'Padding', 'None');
+
+for i = 1:6
+    for j = 1:6
+        nexttile();
+        hold on;
+        if i == j
+            plot(f, abs(G_hac_m0(:, i, j)), 'color', [colors(1,:), 1]);
+            plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'k-');
+        else
+            plot(f, abs(G_hac_m0(:, i, j)), 'color', [colors(1,:), 0.5]);
+            plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), '-', 'color', [0, 0, 0, 0.5]);
+        end
+        hold off;
+        set(gca, 'XTickLabel',[]);
+        set(gca, 'YTickLabel',[]);
+        if j == 1
+            ylabel(sprintf('$dL_{%i}$', i))
+        end
+        if i == 6
+            xlabel(sprintf('$u_{%i}$', j))
+        end
+        set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+        ylim([1e-7, 2e-4]);
+        xlim([1, 1e3]);
+    end
+end
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/sri2024_comp_plant_model_6x6.pdf', 'width', 1000, 'height', 1000, 'transparent', false);
+#+end_src
+
+#+RESULTS:
+[[file:figs/sri2024_comp_plant_model_6x6.png]]
+
+Diagonal elements:
+#+begin_src matlab :exports none :results none
+%% Comparison of the identified HAC plant and the HAC plant extracted from the simscape model
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_hac_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
+     'DisplayName', '$m = 0$ kg');
+for i = 2:6
+    plot(f, abs(G_hac_m0(:,i, i)), 'color', [colors(1,:), 0.5], ...
+         'HandleVisibility', 'off')
+end
+plot(f, abs(G_hac_m1(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
+     'DisplayName', '$m = 15$ kg');
+for i = 2:6
+    plot(f, abs(G_hac_m1(:,i, i)), 'color', [colors(2,:), 0.5], ...
+         'HandleVisibility', 'off')
+end
+plot(f, abs(G_hac_m2(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
+     'DisplayName', '$m = 30$ kg');
+for i = 2:6
+    plot(f, abs(G_hac_m2(:,i, i)), 'color', [colors(3,:), 0.5], ...
+         'HandleVisibility', 'off')
+end
+plot(f, abs(G_hac_m3(:, 1, 1)), 'color', [colors(4,:), 0.5], ...
+     'DisplayName', '$m = 45$ kg');
+for i = 2:6
+    plot(f, abs(G_hac_m3(:,i, i)), 'color', [colors(4,:), 0.5], ...
+         'HandleVisibility', 'off')
+end
+plot(freqs, abs(squeeze(freqresp(Gm_hac_m0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:), ...
+     'DisplayName', '$m = 0$ kg (model)');
+plot(freqs, abs(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:), ...
+     'DisplayName', '$m = 15$ kg (model)');
+plot(freqs, abs(squeeze(freqresp(Gm_hac_m2('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:), ...
+     'DisplayName', '$m = 30$ kg (model)');
+plot(freqs, abs(squeeze(freqresp(Gm_hac_m3('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:), ...
+     'DisplayName', '$m = 45$ kg (model)');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 2e-4]);
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*unwrapphase(angle(G_hac_m0(:,1,1)), f), 'color', [colors(1,:), 0.5]);
+for i = 2:6
+    plot(f, 180/pi*unwrapphase(angle(G_hac_m0(:,i, i)), f), 'color', [colors(1,:), 0.5]);
+end
+plot(f, 180/pi*unwrapphase(angle(G_hac_m1(:,1,1)), f), 'color', [colors(2,:), 0.5]);
+for i = 2:6
+    plot(f, 180/pi*unwrapphase(angle(G_hac_m1(:,i, i)), f), 'color', [colors(2,:), 0.5]);
+end
+plot(f, 180/pi*unwrapphase(angle(G_hac_m2(:,1,1)), f), 'color', [colors(3,:), 0.5]);
+for i = 2:6
+    plot(f, 180/pi*unwrapphase(angle(G_hac_m2(:,i, i)), f), 'color', [colors(3,:), 0.5]);
+end
+plot(f, 180/pi*unwrapphase(angle(G_hac_m3(:,1,1)), f), 'color', [colors(4,:), 0.5]);
+for i = 2:6
+    plot(f, 180/pi*unwrapphase(angle(G_hac_m3(:,i, i)), f), 'color', [colors(4,:), 0.5]);
+end
+plot(freqs, 180/pi*unwrapphase(angle(squeeze(freqresp(exp(-3e-4*s)*Gm_hac_m0('eL1', 'u1'), freqs, 'Hz'))), f), '--', 'color', colors(1,:));
+plot(freqs, 180/pi*unwrapphase(angle(squeeze(freqresp(exp(-3e-4*s)*Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))), f), '--', 'color', colors(2,:));
+plot(freqs, 180/pi*unwrapphase(angle(squeeze(freqresp(exp(-3e-4*s)*Gm_hac_m2('eL1', 'u1'), freqs, 'Hz'))), f), '--', 'color', colors(3,:));
+plot(freqs, 180/pi*unwrapphase(angle(squeeze(freqresp(exp(-3e-4*s)*Gm_hac_m3('eL1', 'u1'), freqs, 'Hz'))), f), '--', 'color', colors(4,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-270, 20])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/sri2024_comp_plant_model_damped_diag.pdf', 'width', 1000, 'height', 1000, 'transparent', false);
+#+end_src
+
+#+RESULTS:
+[[file:figs/sri2024_comp_plant_model_damped_diag.png]]
+
+*** Tomography scans
+#+begin_src matlab :exports none
+data_tomo_30rpm_m0 = load(sprintf("%s/scans/2023-08-17_15-26_tomography_30rpm_m0_robust.mat", mat_dir));
+data_tomo_30rpm_m0.time = Ts*[0:length(data_tomo_30rpm_m0.Rz)-1];
+yztomographymoviesri('movies/sri2024_tomography_30rpm_m0_robust', data_tomo_30rpm_m0, 'xlim_ax1', [-3, 3], 'ylim_ax1', [-1.5, 1.5])
+#+end_src
+
+*** Dy scan
+#+begin_src matlab
+%% Slow Ty scan (10um/s)
+data_ty_ol_slow = load(sprintf("%s/scans/2023-08-21_20-05_ty_scan_m1_open_loop_slow.mat", mat_dir));
+data_ty_ol_slow.time = Ts*[0:length(data_ty_ol_slow.Dy_int)-1];
+data_ty_ol_slow.e_dy = detrend(data_ty_ol_slow.e_dy, 0);
+data_ty_ol_slow.e_dz = detrend(data_ty_ol_slow.e_dz, 0);
+#+end_src
+
+#+begin_src matlab
+fig = figure;
+tiledlayout(2, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile;
+hold on;
+plot(ax1, 1e6*data_ty_ol_slow.Ty, 1e6*data_ty_ol_slow.e_dy, '-', 'color', [colors(1,:)],'LineWidth',1);
+% plot(ax1, 1e6*data_ty_cl_slow.Ty, 1e6*data_ty_cl_slow.e_dy, '-', 'color', [colors(2,:)],'LineWidth',1);
+hold off;
+set(gca, 'XTickLabel',[]);
+ylabel("$D_y$ error [$\mu m$]");
+ax2 = nexttile;
+hold on;
+plot(ax2, 1e6*data_ty_ol_slow.Ty, 1e6*data_ty_ol_slow.e_dz, '-', 'color', [colors(1,:)],'LineWidth',1);
+% plot(ax2, 1e6*data_ty_cl_slow.Ty, 1e6*data_ty_cl_slow.e_dz, '-', 'color', [colors(2,:)],'LineWidth',1);
+hold off;
+xlabel("$D_y$ setpoint [$\mu m$]");
+ylabel("Z motion [$\mu m$]");
+linkaxes([ax1,ax2],'x');
+xlim([-100, 100])
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/sri2024_ty_scan_ol.pdf', 'width', 600, 'height', 1000, 'transparent', false);
+#+end_src
+
+#+name: fig:sri2024_ty_scan_ol
+#+caption: description
+#+RESULTS:
+[[file:figs/sri2024_ty_scan_ol.png]]
+
+#+begin_src matlab
+fig = figure;
+tiledlayout(2, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile;
+hold on;
+plot(ax1, 1e6*data_ty_ol_slow.Ty, 1e6*data_ty_ol_slow.e_dy, '-', 'color', [colors(1,:)],'LineWidth',1, 'DisplayName', 'OL');
+plot(ax1, 1e6*data_ty_cl_slow.Ty, 1e6*data_ty_cl_slow.e_dy, '-', 'color', [colors(2,:)],'LineWidth',1, 'DisplayName', sprintf('CL $\\epsilon_y = %.1f$ nm', 1e9*rms(data_ty_cl_slow.e_dy)));
+hold off;
+legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+set(gca, 'XTickLabel',[]);
+ylabel("$D_y$ error [$\mu m$]");
+ax2 = nexttile;
+hold on;
+plot(ax2, 1e6*data_ty_ol_slow.Ty, 1e6*data_ty_ol_slow.e_dz, '-', 'color', [colors(1,:)],'LineWidth',1, 'DisplayName', 'OL');
+plot(ax2, 1e6*data_ty_cl_slow.Ty, 1e6*data_ty_cl_slow.e_dz, '-', 'color', [colors(2,:)],'LineWidth',1, 'DisplayName', sprintf('CL $\\epsilon_z = %.1f$ nm', 1e9*rms(data_ty_cl_slow.e_dz)));
+hold off;
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+xlabel("$D_y$ setpoint [$\mu m$]");
+ylabel("Z motion [$\mu m$]");
+linkaxes([ax1,ax2],'x');
+xlim([-100, 100])
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/sri2024_ty_scan_cl.pdf', 'width', 600, 'height', 1000, 'transparent', false);
+#+end_src
+
+#+name: fig:sri2024_ty_scan_cl
+#+caption: description
+#+RESULTS:
+[[file:figs/sri2024_ty_scan_cl.png]]
+
+#+begin_src matlab
+fig = figure;
+tiledlayout(2, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile;
+hold([ax1,ax2], 'on');
+plot(ax1, 1e6*data_ol.Ty, 1e6*data_ol.e_dy, '-', 'color', [colors(1,:)],'LineWidth',1);
+plot(ax1, 1e6*data_cl.Ty, 1e6*data_cl.e_dy, '-', 'color', [colors(2,:)],'LineWidth',1);
+set(gca, 'XTickLabel',[]);
+ylabel("$D_y$ error [$\mu m$]");
+ax2 = nexttile;
+plot(ax2, 1e6*data_cl.Ty, 1e6*data_cl.e_dz, '-', 'color', [colors(2,:)],'LineWidth',1);
+plot(ax2, 1e6*data_ol.Ty, 1e6*data_ol.e_dz, '-', 'color', [colors(1,:)],'LineWidth',1);
+xlabel("$D_y$ setpoint [$\mu m$]");
+ylabel("Z motion [$\mu m$]");
+#+end_src
+
+#+begin_src matlab
+dyscanmoviesri('movies/sri2024_ty_ol_slow', data_ty_ol_slow, 'xlim_ax1', [-100, 100], 'ylim_ax1', [-1.5, 1.5], 'xlim_ax2', [-100, 100], 'ylim_ax2', [-1, 1])
+#+end_src
+
+#+begin_src matlab
+%% Slow Ty scan (10um/s) - CL
+data_ty_cl_slow = load(sprintf("%s/scans/2023-08-21_20-07_ty_scan_m1_cf_closed_loop_slow.mat", mat_dir));
+data_ty_cl_slow.time = Ts*[0:length(data_ty_cl_slow.Dy_int)-1];
+#+end_src
+
+#+begin_src matlab
+dyscanclmoviesri('movies/sri2024_ty_cl_slow', data_ty_ol_slow, data_ty_cl_slow, 'xlim_ax1', [-100, 100], 'ylim_ax1', [-1.5, 1.5], 'xlim_ax2', [-100, 100], 'ylim_ax2', [-0.5, 0.5])
+#+end_src
+
+*** Ry (reflectivity) scan
+#+begin_src matlab
+%% Load data for the reflectivity scan
+data_ry = load(sprintf("%s/scans/2023-08-18_15-24_first_reflectivity_m0.mat", mat_dir));
+data_ry.time = Ts*[0:length(data_ry.Ry_int)-1];
+#+end_src
+
+#+begin_src matlab
+fig = figure;
+tiledlayout(2, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile;
+hold on;
+plot(data_ry.time, 1e6*data_ry.Ry_int, 'color', colors(2,:), 'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$rad RMS', 1e6*rms(data_ry.e_ry)))
+plot(data_ry.time, 1e6*data_ry.m_hexa_ry, 'k--', 'DisplayName', '$R_y$ setpoint')
+hold off;
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+set(gca, 'XTickLabel',[]);
+ylabel("$R_y$ motion [$\mu$rad]");
+ylim([-310, 310])
+ax2 = nexttile;
+hold on;
+plot(data_ry.time, 1e9*data_ry.e_dy,   'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nm RMS', 1e9*rms(data_ry.e_dy)))
+plot(data_ry.time, 1e9*data_ry.e_dz,   'DisplayName', sprintf('$\\epsilon D_z = %.0f$ nm RMS', 1e9*rms(data_ry.e_dz)))
+hold off;
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+xlabel("Time [s]");
+ylabel("$D_y$, $D_z$ motion [nm]");
+ylim([-150, 150])
+linkaxes([ax1,ax2],'x');
+xlim([0, 6.2]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/sri2024_ry_scan_cl.pdf', 'width', 600, 'height', 1000, 'transparent', false);
+#+end_src
+
+#+name: fig:sri2024_ry_scan_cl
+#+caption: description
+#+RESULTS:
+[[file:figs/sri2024_ry_scan_cl.png]]
+
+
+*** Dz scans
+#+begin_src matlab
+data_dz_10ums = load(sprintf("%s/scans/2023-08-18_15-33_dirty_layer_m0_small.mat", mat_dir));
+data_dz_10ums.time = Ts*[0:length(data_dz_10ums.Dz_int)-1];
+#+end_src
+
+#+begin_src matlab
+data_dz_100ums = load(sprintf("%s/scans/2023-08-18_15-32_dirty_layer_m0.mat", mat_dir));
+data_dz_100ums.time = Ts*[0:length(data_dz_100ums.Dz_int)-1];
+#+end_src
+
+#+begin_src matlab
+fig = figure;
+tiledlayout(2, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile;
+hold on;
+plot(data_dz_10ums.time, 1e6*data_dz_10ums.e_ry, ...
+    'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$rad RMS', 1e6*rms(data_dz_10ums.e_ry)))
+plot(data_dz_10ums.time, 1e6*data_dz_10ums.e_dy, ...
+    'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nm RMS', 1e9*rms(data_dz_10ums.e_dy)))
+plot(data_dz_10ums.time, 1e6*data_dz_10ums.Dz_int, ...
+    'DisplayName', sprintf('$\\epsilon D_z = %.0f$ nm RMS', 1e9*rms(data_dz_10ums.e_dz)))
+plot(data_dz_10ums.time, 1e6*data_dz_10ums.m_hexa_dz, 'k--', ...
+    'DisplayName', 'Setpoint, $10\mu$m/s')
+hold off;
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+xlabel("Time [s]");
+ylabel("Measured motion [$\mu$m, $\mu$rad]");
+ylim([-11, 11])
+
+ax2 = nexttile;
+hold on;
+plot(data_dz_100ums.time, 1e6*data_dz_100ums.e_ry, ...
+    'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$rad RMS', 1e6*rms(data_dz_100ums.e_ry)))
+plot(data_dz_100ums.time, 1e6*data_dz_100ums.e_dy, ...
+    'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nm RMS', 1e9*rms(data_dz_100ums.e_dy)))
+plot(data_dz_100ums.time, 1e6*data_dz_100ums.Dz_int, ...
+    'DisplayName', sprintf('$\\epsilon D_z = %.0f$ nm RMS', 1e9*rms(data_dz_100ums.e_dz)))
+plot(data_dz_100ums.time, 1e6*data_dz_100ums.m_hexa_dz, 'k--', ...
+    'DisplayName', 'Setpoint, $10\mu$m/s')
+hold off;
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+xlabel("Time [s]");
+ylabel("Measured motion [$\mu$m, $\mu$rad]");
+ylim([-110, 110])
+linkaxes([ax1,ax2],'x');
+xlim([0, 2.2])
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/sri2024_tz_scan_cl.pdf', 'width', 600, 'height', 1000, 'transparent', false);
+#+end_src
+
+#+name: fig:sri2024_tz_scan_cl
+#+caption: description
+#+RESULTS:
+[[file:figs/sri2024_tz_scan_cl.png]]
+
+** Decoupling improvement thanks to better Rz alignment
+*** Alignment procedure
+
+- Control based on encoders
+- Slow moving in X and Y
+- Compare with X and Y from interf
+
+#+begin_src matlab
+%% Load Data
+data_1_dx = h5scan(data_dir, 'align_int_enc_Rz', 'tx_first_scan', 2);
+data_1_dy = h5scan(data_dir, 'align_int_enc_Rz', 'tx_first_scan', 3);
+data_2_dx = h5scan(data_dir, 'align_int_enc_Rz', 'verif-after-correct-offset', 1);
+data_2_dy = h5scan(data_dir, 'align_int_enc_Rz', 'verif-after-correct-offset', 2);
+#+end_src
+
+#+begin_src matlab
+figure;
+hold on;
+plot(1e6*data_1_dx.Dx_int_filtered, 1e6*data_1_dx.Dy_int_filtered, 'color', colors(1,:), 'DisplayName', 'Measurement')
+plot(1e6*data_1_dy.Dx_int_filtered, 1e6*data_1_dy.Dy_int_filtered, 'color', colors(1,:), 'HandleVisibility', 'off')
+plot(1e6*[-10:10]*cos(2.7*pi/180), -1e6*[-10:10]*sin(2.7*pi/180), '--', 'color', colors(2,:), 'DisplayName', '$R_z = 2.7$ deg')
+plot(1e6*[-10:10]*sin(2.7*pi/180), 1e6*[-10:10]*cos(2.7*pi/180), '--', 'color', colors(2,:), 'HandleVisibility', 'off')
+hold off;
+xlabel('Interf $D_x$ [$\mu$m]');
+ylabel('Interf $D_y$ [$\mu$m]');
+legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+axis equal
+xlim([-10, 10]); ylim([-10, 10]);
+xticks([-10:5:10]); yticks([-10:5:10]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_Rz_align_dx_dy_scans_before_calib.pdf', 'width', 'wide', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_Rz_align_dx_dy_scans_before_calib
+#+caption: description
+#+RESULTS:
+[[file:figs/id31_Rz_align_dx_dy_scans_before_calib.png]]
+
+#+begin_src matlab
+figure;
+hold on;
+plot(1e6*data_1_dx.Dx_int_filtered, 1e6*data_1_dx.Dy_int_filtered, 'color', colors(1,:), 'DisplayName', 'Initial')
+plot(1e6*data_1_dy.Dx_int_filtered, 1e6*data_1_dy.Dy_int_filtered, 'color', colors(1,:), 'HandleVisibility', 'off')
+plot(1e6*data_2_dx.Dx_int_filtered, 1e6*data_2_dx.Dy_int_filtered, 'color', colors(2,:), 'DisplayName', 'After $R_z$ calib')
+plot(1e6*data_2_dy.Dx_int_filtered, 1e6*data_2_dy.Dy_int_filtered, 'color', colors(2,:), 'HandleVisibility', 'off')
+hold off;
+xlabel('Interf $D_x$ [$\mu$m]');
+ylabel('Interf $D_y$ [$\mu$m]');
+legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+axis equal
+xlim([-10, 10]); ylim([-10, 10]);
+xticks([-10:5:10]); yticks([-10:5:10]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_Rz_align_dx_dy_scans.pdf', 'width', 'wide', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_Rz_align_dx_dy_scans
+#+caption: description
+#+RESULTS:
+[[file:figs/id31_Rz_align_dx_dy_scans.png]]
+
+
+*** m0
+#+begin_src matlab
+data = load(sprintf('%s/dynamics/2023-08-08_16-17_ol_plant_m0_Wz0.mat', mat_dir));
+data_align = load(sprintf('%s/dynamics/2023-08-17_17-37_ol_plant_m0_Wz0_new_Rz_align.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+%% INT Plant (transfer function from u to taum)
+G_int = zeros(length(f), 6, 6);
+
+for i_strut = 1:6
+    eL = [data.(sprintf("uL%i", i_strut)).e_L1 ; data.(sprintf("uL%i", i_strut)).e_L2 ; data.(sprintf("uL%i", i_strut)).e_L3 ; data.(sprintf("uL%i", i_strut)).e_L4 ; data.(sprintf("uL%i", i_strut)).e_L5 ; data.(sprintf("uL%i", i_strut)).e_L6]';
+
+    G_int(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% INT Plant (transfer function from u to taum)
+G_int_align = zeros(length(f), 6, 6);
+
+for i_strut = 1:6
+    eL = [data_align.(sprintf("uL%i", i_strut)).e_L1 ; data_align.(sprintf("uL%i", i_strut)).e_L2 ; data_align.(sprintf("uL%i", i_strut)).e_L3 ; data_align.(sprintf("uL%i", i_strut)).e_L4 ; data_align.(sprintf("uL%i", i_strut)).e_L5 ; data_align.(sprintf("uL%i", i_strut)).e_L6]';
+
+    G_int_align(:,:,i_strut) = tfestimate(data_align.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
+end
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Decrease of the coupling with better Rz alignment
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_int(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
+     'DisplayName', '$-e\mathcal{L}_i/u_j$');
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_int(:, i, j)), 'color', [colors(1,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(f, abs(G_int_align(:, 1, 2)), 'color', [colors(2,:), 0.2], ...
+     'DisplayName', '$-e\mathcal{L}_i/u_j$ - $R_z$ align');
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_int_align(:, i, j)), 'color', [colors(2,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(f, abs(G_int(:, 1, 1)), 'color', [colors(1,:)], ...
+     'DisplayName', '$-e\mathcal{L}_i/u_i$');
+for i = 2:6
+    plot(f, abs(G_int(:,i, i)), 'color', [colors(1,:)], ...
+         'HandleVisibility', 'off');
+end
+plot(f, abs(G_int_align(:, 1, 1)), 'color', [colors(2,:)], ...
+     'DisplayName', '$-e\mathcal{L}_i/u_i$ - $R_z$ align');
+for i = 2:6
+    plot(f, abs(G_int_align(:,i, i)), 'color', [colors(2,:)], ...
+         'HandleVisibility', 'off');
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 2e-4]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
+
+ax2 = nexttile;
+hold on;
+for i = 1:6
+    plot(f, 180/pi*angle(G_int(:,i, i)), 'color', [colors(1,:)]);
+end
+for i = 1:6
+    plot(f, 180/pi*angle(G_int_align(:,i, i)), 'color', [colors(2,:)]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-90, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_coupling_decrease_rz_align.pdf', 'width', 'full', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_coupling_decrease_rz_align
+#+caption: Decrease of the coupling with better Rz alignment
+#+RESULTS:
+[[file:figs/id31_coupling_decrease_rz_align.png]]
+
+
+*** m3
+#+begin_src matlab
+data = load(sprintf('%s/dynamics/2023-08-10_18-16_damp_plant_m3_Wz0.mat', mat_dir));
+data_align = load(sprintf('%s/dynamics/2023-08-21_15-52_damp_plant_m3_new_Rz.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+%% INT Plant (transfer function from u to taum)
+G_int = zeros(length(f), 6, 6);
+
+for i_strut = 1:6
+    eL = [data.(sprintf("uL%i", i_strut)).e_L1 ; data.(sprintf("uL%i", i_strut)).e_L2 ; data.(sprintf("uL%i", i_strut)).e_L3 ; data.(sprintf("uL%i", i_strut)).e_L4 ; data.(sprintf("uL%i", i_strut)).e_L5 ; data.(sprintf("uL%i", i_strut)).e_L6]';
+
+    G_int(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% INT Plant (transfer function from u to taum)
+G_int_align = zeros(length(f), 6, 6);
+
+for i_strut = 1:6
+    eL = [data_align.(sprintf("uL%i", i_strut)).e_L1 ; data_align.(sprintf("uL%i", i_strut)).e_L2 ; data_align.(sprintf("uL%i", i_strut)).e_L3 ; data_align.(sprintf("uL%i", i_strut)).e_L4 ; data_align.(sprintf("uL%i", i_strut)).e_L5 ; data_align.(sprintf("uL%i", i_strut)).e_L6]';
+
+    G_int_align(:,:,i_strut) = tfestimate(data_align.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
+end
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Decrease of the coupling with better Rz alignment
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_int(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
+     'DisplayName', '$-e\mathcal{L}_i/u_j$');
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_int(:, i, j)), 'color', [colors(1,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(f, abs(G_int_align(:, 1, 2)), 'color', [colors(2,:), 0.2], ...
+     'DisplayName', '$-e\mathcal{L}_i/u_j$ - $R_z$ align');
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_int_align(:, i, j)), 'color', [colors(2,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(f, abs(G_int(:, 1, 1)), 'color', [colors(1,:)], ...
+     'DisplayName', '$-e\mathcal{L}_i/u_i$');
+for i = 2:6
+    plot(f, abs(G_int(:,i, i)), 'color', [colors(1,:)], ...
+         'HandleVisibility', 'off');
+end
+plot(f, abs(G_int_align(:, 1, 1)), 'color', [colors(2,:)], ...
+     'DisplayName', '$-e\mathcal{L}_i/u_i$ - $R_z$ align');
+for i = 2:6
+    plot(f, abs(G_int_align(:,i, i)), 'color', [colors(2,:)], ...
+         'HandleVisibility', 'off');
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 2e-4]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
+
+ax2 = nexttile;
+hold on;
+for i = 1:6
+    plot(f, 180/pi*angle(G_int(:,i, i)), 'color', [colors(1,:)]);
+end
+for i = 1:6
+    plot(f, 180/pi*angle(G_int_align(:,i, i)), 'color', [colors(2,:)]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-90, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_coupling_decrease_rz_align_m3.pdf', 'width', 'full', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_coupling_decrease_rz_align_m3
+#+caption: Decrease of the coupling with better Rz alignment
+#+RESULTS:
+[[file:figs/id31_coupling_decrease_rz_align_m3.png]]
+
+** Conclusion
+
+- Good match between the model and experiment
+
+* TODO Noise Budget                                                 :noexport:
+<<sec:test_id31_noise_budget>>
+** Introduction                                                      :ignore:
+
+In this section, the noise budget is performed.
+The vibrations of the sample is measured in different conditions using the external metrology.
+
+
+** Matlab Init                                              :noexport:ignore:
+#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
+<<matlab-dir>>
+#+end_src
+
+#+begin_src matlab :exports none :results silent :noweb yes
+<<matlab-init>>
+#+end_src
+
+#+begin_src matlab :tangle no :noweb yes
+<<m-init-path>>
+#+end_src
+
+#+begin_src matlab :eval no :noweb yes
+<<m-init-path-tangle>>
+#+end_src
+
+#+begin_src matlab :noweb yes
+<<m-init-other>>
+#+end_src
+
+** Open-Loop Noise Budget
+First, the noise is measured while no motion is performed.
+
+#+begin_src matlab
+%% Load measured noise
+data_m0 = load(sprintf('%s/freq_analysis/2023-08-11_16-51_m0_lac_off.mat', mat_dir));
+#+end_src
+
+Noise budget in the cartesian frame
+#+begin_src matlab :exports none
+%% Coordinate transform
+J_int_to_X = [ 0                 0                -0.787401574803149 -0.212598425196851  0;
+               0.78740157480315  0.21259842519685  0                  0                  0;
+               0                 0                 0                  0                 -1;
+             -13.1233595800525  13.1233595800525   0                  0                  0;
+               0                 0               -13.1233595800525   13.1233595800525    0];
+
+a = J_int_to_X*[data_m0.d1; data_m0.d2; data_m0.d3; data_m0.d4; data_m0.d5];
+data_m0.t = Ts*[0:length(data_m0.d1)-1];
+data_m0.Dx_int = a(1,:);
+data_m0.Dy_int = a(2,:);
+data_m0.Dz_int = a(3,:);
+data_m0.Rx_int = a(4,:);
+data_m0.Ry_int = a(5,:);
+#+end_src
+
+Data in the time domain
+#+begin_src matlab :exports none :results none
+%% Measured vibration with the interferometers
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile();
+hold on;
+% plot(data_m0.t, 1e9*data_m0.Dx_int, '-', 'DisplayName', '$D_x$');
+plot(data_m0.t, 1e9*data_m0.Dy_int, '-', 'DisplayName', '$D_y$');
+plot(data_m0.t, 1e9*data_m0.Dz_int, '-', 'DisplayName', '$D_z$');
+% plot(data_m0.t, data_m0.Rx_int, 'DisplayName', '$R_x$');
+plot(data_m0.t, 1e9*data_m0.Ry_int, '-', 'DisplayName', '$R_y$');
+hold off;
+set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
+xlabel('Time [s]'); ylabel('Amplitude [nm]');
+legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+xlim([50, 54])
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_noise_budget_interf_time_domain_m0.pdf', 'width', 'full', 'height', 'normal');
+#+end_src
+
+#+name: fig:id31_noise_budget_interf_time_domain_m0
+#+caption: Measured vibration with the interferometers
+#+RESULTS:
+[[file:figs/id31_noise_budget_interf_time_domain_m0.png]]
+
+In the frequency domain
+#+begin_src matlab :exports none
+% Hannning Windows
+Nfft = floor(2.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+#+end_src
+
+#+begin_src matlab :exports none
+% And we get the frequency vector
+[pxx_Dx, f] = pwelch(detrend(data_m0.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[pxx_Dy, ~] = pwelch(detrend(data_m0.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
+[pxx_Dz, ~] = pwelch(detrend(data_m0.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
+[pxx_Rx, ~] = pwelch(detrend(data_m0.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[pxx_Ry, ~] = pwelch(detrend(data_m0.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile();
+hold on;
+% plot(f, sqrt(pxx_Dx), 'DisplayName', '$D_x$');
+plot(f, sqrt(pxx_Dy), 'DisplayName', '$D_y$');
+plot(f, sqrt(pxx_Dz), 'DisplayName', '$D_z$');
+% plot(f, sqrt(pxx_Rx), 'DisplayName', '$R_x$');
+plot(f, sqrt(pxx_Ry), 'DisplayName', '$R_y$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Amplitude [$V/\sqrt{Hz}$]');
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+xlim([1, 5e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_noise_budget_interf_freq_domain_m0.pdf', 'width', 'wide', 'height', 'normal');
+#+end_src
+
+#+name: fig:id31_noise_budget_interf_freq_domain_m0
+#+caption: Measured vibration with the interferometers
+#+RESULTS:
+[[file:figs/id31_noise_budget_interf_freq_domain_m0.png]]
+
+#+begin_src matlab :exports none :results none
+%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion
+figure;
+hold on;
+% plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_Dx)))), 'DisplayName', '$D_x$');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_Dy)))), 'DisplayName', sprintf('$D_y$ - %.0f nm', rms(1e9*detrend(data_m0.Dy_int, 0))));
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_Dz)))), 'DisplayName', sprintf('$D_z$ - %.0f nm', rms(1e9*detrend(data_m0.Dz_int, 0))));
+% plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_Rx)))), 'DisplayName', '$R_x$');
+plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_Ry)))), 'DisplayName', sprintf('$R_y$ - %.0f nrad', rms(1e9*detrend(data_m0.Ry_int, 0))));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('CAS [$m$]');
+legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+xlim([1, 5e3]); ylim([1e-10, 1e-7]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_noise_budget_open_loop_cas_m0.pdf', 'width', 'wide', 'height', 'normal');
+#+end_src
+
+#+name: fig:id31_noise_budget_open_loop_cas_m0
+#+caption: Measured vibration with the interferometers
+#+RESULTS:
+[[file:figs/id31_noise_budget_open_loop_cas_m0.png]]
+
+** Effect of LAC
+#+begin_src matlab
+%% Load measured noise
+data_ol = load(sprintf('%s/freq_analysis/2023-08-11_16-51_m0_lac_off.mat', mat_dir));
+data_lac = load(sprintf('%s/freq_analysis/2023-08-11_16-46_m0_lac_on.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+%% Coordinate transform
+J_int_to_X = [ 0                 0                -0.787401574803149 -0.212598425196851  0;
+               0.78740157480315  0.21259842519685  0                  0                  0;
+               0                 0                 0                  0                 -1;
+             -13.1233595800525  13.1233595800525   0                  0                  0;
+               0                 0               -13.1233595800525   13.1233595800525    0];
+
+a = J_int_to_X*[data_ol.d1; data_ol.d2; data_ol.d3; data_ol.d4; data_ol.d5];
+data_ol.Dx_int = a(1,:);
+data_ol.Dy_int = a(2,:);
+data_ol.Dz_int = a(3,:);
+data_ol.Rx_int = a(4,:);
+data_ol.Ry_int = a(5,:);
+
+a = J_int_to_X*[data_lac.d1; data_lac.d2; data_lac.d3; data_lac.d4; data_lac.d5];
+data_lac.Dx_int = a(1,:);
+data_lac.Dy_int = a(2,:);
+data_lac.Dz_int = a(3,:);
+data_lac.Rx_int = a(4,:);
+data_lac.Ry_int = a(5,:);
+#+end_src
+
+#+begin_src matlab :exports none
+% Hannning Windows
+Nfft = floor(2.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+
+[data_ol.pxx_Dx, data_ol.f] = pwelch(detrend(data_ol.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_ol.pxx_Dy, ~        ] = pwelch(detrend(data_ol.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_ol.pxx_Dz, ~        ] = pwelch(detrend(data_ol.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_ol.pxx_Rx, ~        ] = pwelch(detrend(data_ol.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_ol.pxx_Ry, ~        ] = pwelch(detrend(data_ol.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
+
+[data_lac.pxx_Dx, data_lac.f] = pwelch(detrend(data_lac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_lac.pxx_Dy, ~         ] = pwelch(detrend(data_lac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_lac.pxx_Dz, ~         ] = pwelch(detrend(data_lac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_lac.pxx_Rx, ~         ] = pwelch(detrend(data_lac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_lac.pxx_Ry, ~         ] = pwelch(detrend(data_lac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+Effect of LAC (Figure ref:fig:id31_noise_budget_effect_lac_m0):
+- reduce amplitude around 80Hz
+- Inject some noise between 200 and 700Hz?
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile();
+hold on;
+plot(data_ol.f , sqrt(data_ol.pxx_Dy ), 'DisplayName', '$D_y$ - OL');
+plot(data_lac.f, sqrt(data_lac.pxx_Dy), 'DisplayName', '$D_y$ - LAC');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Amplitude [$m/\sqrt{Hz}$]');
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+title('$D_y$')
+
+ax2 = nexttile();
+hold on;
+plot(data_ol.f , sqrt(data_ol.pxx_Dz ), 'DisplayName', '$D_z$ - OL');
+plot(data_lac.f, sqrt(data_lac.pxx_Dz), 'DisplayName', '$D_z$ - LAC');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+title('$D_z$')
+
+ax3 = nexttile();
+hold on;
+plot(data_ol.f , sqrt(data_ol.pxx_Ry ), 'DisplayName', '$R_y$ - OL');
+plot(data_lac.f, sqrt(data_lac.pxx_Ry), 'DisplayName', '$R_y$ - LAC');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+title('$R_y$')
+
+linkaxes([ax1,ax2,ax3],'xy');
+xlim([1, 5e2]); ylim([1e-11, 2e-8]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_noise_budget_effect_lac_m0.pdf', 'width', 'full', 'height', 'normal');
+#+end_src
+
+#+name: fig:id31_noise_budget_effect_lac_m0
+#+caption: Measured vibration with the interferometers
+#+RESULTS:
+[[file:figs/id31_noise_budget_effect_lac_m0.png]]
+
+#+begin_src matlab :exports none :results none
+%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion
+figure;
+tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile();
+hold on;
+plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dy)))), 'DisplayName', 'OL');
+plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dy)))), 'DisplayName', 'LAC');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('CAS [$m$]');
+title('$D_y$');
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile();
+hold on;
+plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dz)))), 'DisplayName', 'OL');
+plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dz)))), 'DisplayName', 'LAC');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+title('$D_z$');
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax3 = nexttile();
+hold on;
+plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Ry)))), 'DisplayName', 'OL');
+plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Ry)))), 'DisplayName', 'LAC');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+title('$R_y$');
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+
+linkaxes([ax1,ax2,ax3],'xy');
+xlim([1, 1e3]); ylim([1e-10, 1e-7]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_cas_effect_lac_m0.pdf', 'width', 'full', 'height', 'normal');
+#+end_src
+
+#+name: fig:id31_cas_effect_lac_m0
+#+caption: Measured vibration with the interferometers
+#+RESULTS:
+[[file:figs/id31_cas_effect_lac_m0.png]]
+
+** Effect of rotation
+#+begin_src matlab
+data_Wz0 = load(sprintf('%s/freq_analysis/2023-08-11_16-51_m0_lac_off.mat',       mat_dir));
+data_Wz1 = load(sprintf('%s/freq_analysis/2023-08-11_17-18_m0_lac_off_1rpm.mat',  mat_dir));
+data_Wz2 = load(sprintf('%s/freq_analysis/2023-08-11_17-39_m0_lac_off_30rpm.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+%% Coordinate transform
+J_int_to_X = [ 0                 0                -0.787401574803149 -0.212598425196851  0;
+               0.78740157480315  0.21259842519685  0                  0                  0;
+               0                 0                 0                  0                 -1;
+             -13.1233595800525  13.1233595800525   0                  0                  0;
+               0                 0               -13.1233595800525   13.1233595800525    0];
+
+a = J_int_to_X*[data_Wz0.d1; data_Wz0.d2; data_Wz0.d3; data_Wz0.d4; data_Wz0.d5];
+data_Wz0.Dx_int = a(1,:);
+data_Wz0.Dy_int = a(2,:);
+data_Wz0.Dz_int = a(3,:);
+data_Wz0.Rx_int = a(4,:);
+data_Wz0.Ry_int = a(5,:);
+
+a = J_int_to_X*[data_Wz1.d1; data_Wz1.d2; data_Wz1.d3; data_Wz1.d4; data_Wz1.d5];
+data_Wz1.Dx_int = a(1,:);
+data_Wz1.Dy_int = a(2,:);
+data_Wz1.Dz_int = a(3,:);
+data_Wz1.Rx_int = a(4,:);
+data_Wz1.Ry_int = a(5,:);
+
+a = J_int_to_X*[data_Wz2.d1; data_Wz2.d2; data_Wz2.d3; data_Wz2.d4; data_Wz2.d5];
+data_Wz2.Dx_int = a(1,:);
+data_Wz2.Dy_int = a(2,:);
+data_Wz2.Dz_int = a(3,:);
+data_Wz2.Rx_int = a(4,:);
+data_Wz2.Ry_int = a(5,:);
+#+end_src
+
+#+begin_src matlab :exports none
+% Hannning Windows
+Nfft = floor(20.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+
+[data_Wz0.pxx_Dx, data_Wz0.f] = pwelch(detrend(data_Wz0.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_Wz0.pxx_Dy, ~         ] = pwelch(detrend(data_Wz0.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_Wz0.pxx_Dz, ~         ] = pwelch(detrend(data_Wz0.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_Wz0.pxx_Rx, ~         ] = pwelch(detrend(data_Wz0.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_Wz0.pxx_Ry, ~         ] = pwelch(detrend(data_Wz0.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
+
+[data_Wz1.pxx_Dx, data_Wz1.f] = pwelch(detrend(data_Wz1.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_Wz1.pxx_Dy, ~          ] = pwelch(detrend(data_Wz1.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_Wz1.pxx_Dz, ~          ] = pwelch(detrend(data_Wz1.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_Wz1.pxx_Rx, ~          ] = pwelch(detrend(data_Wz1.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_Wz1.pxx_Ry, ~          ] = pwelch(detrend(data_Wz1.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
+
+[data_Wz2.pxx_Dx, data_Wz2.f] = pwelch(detrend(data_Wz2.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_Wz2.pxx_Dy, ~          ] = pwelch(detrend(data_Wz2.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_Wz2.pxx_Dz, ~          ] = pwelch(detrend(data_Wz2.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_Wz2.pxx_Rx, ~          ] = pwelch(detrend(data_Wz2.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_Wz2.pxx_Ry, ~          ] = pwelch(detrend(data_Wz2.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile();
+hold on;
+plot(data_Wz0.f, sqrt(data_Wz0.pxx_Dy), 'DisplayName', '$D_y$ - 0rpm');
+plot(data_Wz1.f, sqrt(data_Wz1.pxx_Dy), 'DisplayName', '$D_y$ - 1rpm');
+plot(data_Wz2.f, sqrt(data_Wz2.pxx_Dy), 'DisplayName', '$D_y$ - 30rpm');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Amplitude [$V/\sqrt{Hz}$]');
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+xlim([0.1, 5e3]);
+#+end_src
+
+Rotation induces lots of vibrations, especially at high velocity.
+
+#+begin_src matlab :exports none :results none
+%% description
+figure;
+tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile();
+hold on;
+plot(data_Wz0.f, sqrt(flip(-cumtrapz(flip(data_Wz0.f), flip(data_Wz0.pxx_Dy)))),    ...
+     'DisplayName', sprintf('0rpm: $%.0f nm$', 1e9*rms(detrend(data_Wz0.Dy_int, 0))));
+plot(data_Wz1.f, sqrt(flip(-cumtrapz(flip(data_Wz1.f), flip(data_Wz1.pxx_Dy)))), ...
+     'DisplayName', sprintf('6rpm: $%.0f nm$', 1e9*rms(detrend(data_Wz1.Dy_int, 0))));
+plot(data_Wz2.f, sqrt(flip(-cumtrapz(flip(data_Wz2.f), flip(data_Wz2.pxx_Dy)))), ...
+     'DisplayName', sprintf('30rpm: $%.1f \\mu m$', 1e6*rms(detrend(data_Wz2.Dy_int, 0))));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('CAS [m rms, rad RMS]');
+legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+xticks([1e0, 1e1, 1e2]);
+title('$D_y$')
+
+ax2 = nexttile();
+hold on;
+plot(data_Wz0.f, sqrt(flip(-cumtrapz(flip(data_Wz0.f), flip(data_Wz0.pxx_Dz)))),    ...
+     'DisplayName', sprintf('0rpm: $%.0f nm$', 1e9*rms(detrend(data_Wz0.Dz_int, 0))));
+plot(data_Wz1.f, sqrt(flip(-cumtrapz(flip(data_Wz1.f), flip(data_Wz1.pxx_Dz)))), ...
+     'DisplayName', sprintf('6rpm: $%.0f nm$', 1e9*rms(detrend(data_Wz1.Dz_int, 0))));
+plot(data_Wz2.f, sqrt(flip(-cumtrapz(flip(data_Wz2.f), flip(data_Wz2.pxx_Dz)))), ...
+     'DisplayName', sprintf('30rpm: $%.0f nm$', 1e9*rms(detrend(data_Wz2.Dz_int, 0))));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+xticks([1e0, 1e1, 1e2]);
+title('$D_z$')
+
+ax3 = nexttile();
+hold on;
+plot(data_Wz0.f, sqrt(flip(-cumtrapz(flip(data_Wz0.f), flip(data_Wz0.pxx_Ry)))),    ...
+     'DisplayName', sprintf('0rpm: $%.1f \\mu$rad', 1e6*rms(detrend(data_Wz0.Ry_int, 0))));
+plot(data_Wz1.f, sqrt(flip(-cumtrapz(flip(data_Wz1.f), flip(data_Wz1.pxx_Ry)))), ...
+     'DisplayName', sprintf('6rpm: $%.1f \\mu$rad', 1e6*rms(detrend(data_Wz1.Ry_int, 0))));
+plot(data_Wz2.f, sqrt(flip(-cumtrapz(flip(data_Wz2.f), flip(data_Wz2.pxx_Ry)))), ...
+     'DisplayName', sprintf('30rpm: $%.0f \\mu$rad', 1e6*rms(detrend(data_Wz2.Ry_int, 0))));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+xticks([1e0, 1e1, 1e2]);
+title('$R_y$')
+
+linkaxes([ax1,ax2,ax3],'xy');
+xlim([0.1, 5e2]); ylim([1e-10, 3e-5]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_noise_budget_effect_rotation.pdf', 'width', 'full', 'height', 'normal');
+#+end_src
+
+#+name: fig:id31_noise_budget_effect_rotation
+#+caption: Cumulative Amplitude Spectrum for the three important directions ($D_y$, $D_z$ and $R_y$). Three rotating velocities are shown. Integrated RMS values are shown in the legend.
+#+RESULTS:
+[[file:figs/id31_noise_budget_effect_rotation.png]]
+
+** Effect of HAC
+#+begin_src matlab
+%% Load measured noise
+data_ol  = load(sprintf('%s/freq_analysis/2023-08-11_16-51_m0_lac_off.mat', mat_dir));
+data_lac = load(sprintf('%s/freq_analysis/2023-08-11_16-46_m0_lac_on.mat' , mat_dir));
+data_hac = load(sprintf('%s/freq_analysis/2023-08-11_16-49_m0_hac_on.mat' , mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+%% Coordinate transform
+J_int_to_X = [ 0                 0                -0.787401574803149 -0.212598425196851  0;
+               0.78740157480315  0.21259842519685  0                  0                  0;
+               0                 0                 0                  0                 -1;
+             -13.1233595800525  13.1233595800525   0                  0                  0;
+               0                 0               -13.1233595800525   13.1233595800525    0];
+
+a = J_int_to_X*[data_ol.d1; data_ol.d2; data_ol.d3; data_ol.d4; data_ol.d5];
+data_ol.Dx_int = a(1,:);
+data_ol.Dy_int = a(2,:);
+data_ol.Dz_int = a(3,:);
+data_ol.Rx_int = a(4,:);
+data_ol.Ry_int = a(5,:);
+
+a = J_int_to_X*[data_lac.d1; data_lac.d2; data_lac.d3; data_lac.d4; data_lac.d5];
+data_lac.Dx_int = a(1,:);
+data_lac.Dy_int = a(2,:);
+data_lac.Dz_int = a(3,:);
+data_lac.Rx_int = a(4,:);
+data_lac.Ry_int = a(5,:);
+
+a = J_int_to_X*[data_hac.d1; data_hac.d2; data_hac.d3; data_hac.d4; data_hac.d5];
+data_hac.Dx_int = a(1,:);
+data_hac.Dy_int = a(2,:);
+data_hac.Dz_int = a(3,:);
+data_hac.Rx_int = a(4,:);
+data_hac.Ry_int = a(5,:);
+#+end_src
+
+#+begin_src matlab :exports none
+% Hannning Windows
+Nfft = floor(2.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+
+[data_ol.pxx_Dx, data_ol.f] = pwelch(detrend(data_ol.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_ol.pxx_Dy, ~        ] = pwelch(detrend(data_ol.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_ol.pxx_Dz, ~        ] = pwelch(detrend(data_ol.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_ol.pxx_Rx, ~        ] = pwelch(detrend(data_ol.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_ol.pxx_Ry, ~        ] = pwelch(detrend(data_ol.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
+
+[data_lac.pxx_Dx, data_lac.f] = pwelch(detrend(data_lac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_lac.pxx_Dy, ~         ] = pwelch(detrend(data_lac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_lac.pxx_Dz, ~         ] = pwelch(detrend(data_lac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_lac.pxx_Rx, ~         ] = pwelch(detrend(data_lac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_lac.pxx_Ry, ~         ] = pwelch(detrend(data_lac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
+
+[data_hac.pxx_Dx, data_hac.f] = pwelch(detrend(data_hac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_hac.pxx_Dy, ~         ] = pwelch(detrend(data_hac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_hac.pxx_Dz, ~         ] = pwelch(detrend(data_hac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_hac.pxx_Rx, ~         ] = pwelch(detrend(data_hac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_hac.pxx_Ry, ~         ] = pwelch(detrend(data_hac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+Bandwidth is approximately 10Hz.
+
+#+begin_src matlab :exports none :results none
+%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion
+figure;
+tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile();
+hold on;
+plot(data_ol.f,  sqrt(data_ol.pxx_Dy),  'DisplayName', 'OL');
+plot(data_lac.f, sqrt(data_lac.pxx_Dy), 'DisplayName', 'LAC');
+plot(data_hac.f, sqrt(data_hac.pxx_Dy), 'DisplayName', 'HAC');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]');
+title('$D_y$');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile();
+hold on;
+plot(data_ol.f,  sqrt(data_ol.pxx_Dz),  'DisplayName', 'OL');
+plot(data_lac.f, sqrt(data_lac.pxx_Dz), 'DisplayName', 'LAC');
+plot(data_hac.f, sqrt(data_hac.pxx_Dz), 'DisplayName', 'HAC');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+title('$D_z$');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax3 = nexttile();
+hold on;
+plot(data_ol.f,  sqrt(data_ol.pxx_Ry),  'DisplayName', 'OL');
+plot(data_lac.f, sqrt(data_lac.pxx_Ry), 'DisplayName', 'LAC');
+plot(data_hac.f, sqrt(data_hac.pxx_Ry), 'DisplayName', 'HAC');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+title('$R_y$');
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+
+linkaxes([ax1,ax2,ax3],'xy');
+xlim([1, 1e3]); ylim([1e-12, 1e-7]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_noise_budget_effect_hac_m0.pdf', 'width', 'full', 'height', 'normal');
+#+end_src
+
+#+name: fig:id31_noise_budget_effect_hac_m0
+#+caption: Measured vibration with the interferometers
+#+RESULTS:
+[[file:figs/id31_noise_budget_effect_hac_m0.png]]
+
+
+
+#+begin_src matlab :exports none :results none
+%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion
+figure;
+tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile();
+hold on;
+plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dy)))), 'DisplayName', 'OL');
+plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dy)))), 'DisplayName', 'LAC');
+plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Dy)))), 'DisplayName', 'HAC');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('CAS [$m$]');
+title('$D_y$');
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile();
+hold on;
+plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dz)))), 'DisplayName', 'OL');
+plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dz)))), 'DisplayName', 'LAC');
+plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Dz)))), 'DisplayName', 'HAC');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+title('$D_z$');
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax3 = nexttile();
+hold on;
+plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Ry)))), 'DisplayName', 'OL');
+plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Ry)))), 'DisplayName', 'LAC');
+plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Ry)))), 'DisplayName', 'HAC');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+title('$R_y$');
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+
+linkaxes([ax1,ax2,ax3],'xy');
+xlim([1, 1e3]); ylim([1e-10, 1e-6]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_cas_effect_hac_m0.pdf', 'width', 'full', 'height', 'normal');
+#+end_src
+
+#+name: fig:id31_cas_effect_hac_m0
+#+caption: Measured vibration with the interferometers
+#+RESULTS:
+[[file:figs/id31_cas_effect_hac_m0.png]]
+
+** TODO Noise coming from force sensor
+
+* Integral Force Feedback                                           :noexport:
+<<sec:test_id31_iff>>
+** Introduction                                                      :ignore:
+
+** Matlab Init                                              :noexport:ignore:
+#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
+<<matlab-dir>>
+#+end_src
+
+#+begin_src matlab :exports none :results silent :noweb yes
+<<matlab-init>>
+#+end_src
+
+#+begin_src matlab :tangle no :noweb yes
+<<m-init-path>>
+#+end_src
+
+#+begin_src matlab :eval no :noweb yes
+<<m-init-path-tangle>>
+#+end_src
+
+#+begin_src matlab :noweb yes
+<<m-init-other>>
+#+end_src
+
+** IFF Plants
+*** Introduction                                                    :ignore:
+
+*** 6x6 Plant
+#+begin_src matlab
+%% Load identification data
+data = load(sprintf('%s/dynamics/2023-08-08_16-17_ol_plant_m0_Wz0.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+% Hannning Windows
+Nfft = floor(1.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+#+end_src
+
+#+begin_src matlab :exports none
+% And we get the frequency vector
+[~, f] = tfestimate(data.uL1.id_plant, data.uL1.e_L1, win, [], [], 1/Ts);
+#+end_src
+
+#+begin_src matlab :exports none
+%% IFF Plant (transfer function from u to taum)
+G_iff = zeros(length(f), 6, 6);
+
+for i_strut = 1:6
+    eL = [data.(sprintf("uL%i", i_strut)).Vs1 ; data.(sprintf("uL%i", i_strut)).Vs2 ; data.(sprintf("uL%i", i_strut)).Vs3 ; data.(sprintf("uL%i", i_strut)).Vs4 ; data.(sprintf("uL%i", i_strut)).Vs5 ; data.(sprintf("uL%i", i_strut)).Vs6]';
+
+    G_iff(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, eL, win, [], [], 1/Ts);
+end
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_iff(:, i, j)), 'color', [0, 0, 0, 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+for i = 1:6
+    plot(f, abs(G_iff(:,i, i)), 'color', colors(i,:), ...
+        'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i));
+end
+plot(f, abs(G_iff(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
+     'DisplayName', '$\tau_{m,i}/u_j$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-4, 1e2]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
+
+ax2 = nexttile;
+hold on;
+for i =1:6
+    plot(f, 180/pi*angle(G_iff(:,i, i)));
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-90, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_Giff_plant.pdf', 'width', 'full', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_Giff_plant
+#+caption: Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+#+RESULTS:
+[[file:figs/id31_Giff_plant.png]]
+
+Compare with Model:
+#+begin_src matlab
+load('Gm_iff.mat');
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_iff(:, i, j)), 'color', [colors(3,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+for i = 1:5
+    for j = i+1:6
+        plot(freqs, abs(squeeze(freqresp(Gm_iff_m0(i, j), freqs, 'Hz'))), 'color', [colors(4,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+for i = 2:6
+    plot(f, abs(G_iff(:,i, i)), 'color', [colors(1,:), 0.5], ...
+            'HandleVisibility', 'off');
+end
+for i = 2:6
+    plot(freqs, abs(squeeze(freqresp(Gm_iff_m0(i, i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
+            'HandleVisibility', 'off');
+end
+% plot(f, abs(G_iff(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
+%      'DisplayName', '$\tau_{m,i}/u_j$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-4, 1e2]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
+
+% ax2 = nexttile;
+% hold on;
+% for i =1:6
+%     plot(f, 180/pi*angle(G_iff(:,i, i)));
+% end
+% hold off;
+% set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+% xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+% hold off;
+% yticks(-360:90:360);
+% ylim([-90, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+*** Effect of Rotation
+#+begin_src matlab
+%% Load identification data
+data_Wz0 = load(sprintf('%s/dynamics/2023-08-08_16-17_ol_plant_m0_Wz0.mat', mat_dir));
+data_Wz1 = load(sprintf('%s/dynamics/2023-08-08_16-28_ol_plant_m0_Wz36.mat', mat_dir));
+data_Wz2 = load(sprintf('%s/dynamics/2023-08-08_16-45_ol_plant_m0_Wz180.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+% Sampling Time [s]
+Ts = 1e-4;
+
+% Hannning Windows
+Nfft = floor(1.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+#+end_src
+
+#+begin_src matlab :exports none
+% And we get the frequency vector
+[~, f] = tfestimate(data.uL1.id_plant, data.uL1.e_L1, win, [], [], 1/Ts);
+#+end_src
+
+#+begin_src matlab :exports none
+%% IFF Plant (transfer function from u to taum)
+G_iff_Wz0 = zeros(length(f), 6, 6);
+
+for i_strut = 1:6
+    eL = [data_Wz0.(sprintf("uL%i", i_strut)).Vs1 ; data_Wz0.(sprintf("uL%i", i_strut)).Vs2 ; data_Wz0.(sprintf("uL%i", i_strut)).Vs3 ; data_Wz0.(sprintf("uL%i", i_strut)).Vs4 ; data_Wz0.(sprintf("uL%i", i_strut)).Vs5 ; data_Wz0.(sprintf("uL%i", i_strut)).Vs6]';
+
+    G_iff_Wz0(:,:,i_strut) = tfestimate(data_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, [], [], 1/Ts);
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% IFF Plant (transfer function from u to taum)
+G_iff_Wz1 = zeros(length(f), 6, 6);
+
+for i_strut = 1:6
+    eL = [data_Wz1.(sprintf("uL%i", i_strut)).Vs1 ; data_Wz1.(sprintf("uL%i", i_strut)).Vs2 ; data_Wz1.(sprintf("uL%i", i_strut)).Vs3 ; data_Wz1.(sprintf("uL%i", i_strut)).Vs4 ; data_Wz1.(sprintf("uL%i", i_strut)).Vs5 ; data_Wz1.(sprintf("uL%i", i_strut)).Vs6]';
+
+    G_iff_Wz1(:,:,i_strut) = tfestimate(data_Wz1.(sprintf("uL%i", i_strut)).id_plant, eL, win, [], [], 1/Ts);
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% IFF Plant (transfer function from u to taum)
+G_iff_Wz2 = zeros(length(f), 6, 6);
+
+for i_strut = 1:6
+    eL = [data_Wz2.(sprintf("uL%i", i_strut)).Vs1 ; data_Wz2.(sprintf("uL%i", i_strut)).Vs2 ; data_Wz2.(sprintf("uL%i", i_strut)).Vs3 ; data_Wz2.(sprintf("uL%i", i_strut)).Vs4 ; data_Wz2.(sprintf("uL%i", i_strut)).Vs5 ; data_Wz2.(sprintf("uL%i", i_strut)).Vs6]';
+
+    G_iff_Wz2(:,:,i_strut) = tfestimate(data_Wz2.(sprintf("uL%i", i_strut)).id_plant, eL, win, [], [], 1/Ts);
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% Save Identified Plants
+save('./mat/G_iff.mat', 'G_iff_Wz0', 'G_iff_Wz1', 'G_iff_Wz2', '-append');
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_iff_Wz0(:, 1, 1)), 'color', [colors(1,:), 0.2], ...
+     'DisplayName', '$\Omega_z = 0$');
+for i = 2:6
+    plot(f, abs(G_iff_Wz0(:,i, i)), 'color', [colors(1,:), 0.2], ...
+         'HandleVisibility', 'off')
+end
+plot(f, abs(G_iff_Wz1(:, 1, 1)), 'color', [colors(2,:), 0.2], ...
+     'DisplayName', '$\Omega_z = 36$ deg/s');
+for i = 2:6
+    plot(f, abs(G_iff_Wz1(:,i, i)), 'color', [colors(2,:), 0.2], ...
+         'HandleVisibility', 'off')
+end
+plot(f, abs(G_iff_Wz2(:, 1, 1)), 'color', [colors(3,:), 0.2], ...
+     'DisplayName', '$\Omega_z = 180$ deg/s');
+for i = 2:6
+    plot(f, abs(G_iff_Wz2(:,i, i)), 'color', [colors(3,:), 0.2], ...
+         'HandleVisibility', 'off')
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-2, 1e2]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+for i =1:6
+    plot(f, 180/pi*angle(G_iff_Wz0(:,i, i)), 'color', [colors(1,:), 0.2]);
+end
+for i =1:6
+    plot(f, 180/pi*angle(G_iff_Wz1(:,i, i)), 'color', [colors(2,:), 0.2]);
+end
+for i =1:6
+    plot(f, 180/pi*angle(G_iff_Wz2(:,i, i)), 'color', [colors(3,:), 0.2]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-90, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_Giff_effect_rotation.pdf', 'width', 'full', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_Giff_effect_rotation
+#+caption: Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+#+RESULTS:
+[[file:figs/id31_Giff_effect_rotation.png]]
+
+*** Effect of Mass
+#+begin_src matlab
+load('G_ol.mat', 'G_iff_m0', 'G_iff_m1', 'G_iff_m2', 'G_iff_m3', 'f');
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_iff_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
+     'DisplayName', '$m = 0$');
+for i = 2:6
+    plot(f, abs(G_iff_m0(:,i, i)), 'color', [colors(1,:), 0.5], ...
+         'HandleVisibility', 'off')
+end
+plot(f, abs(G_iff_m1(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
+     'DisplayName', '$m = 1$');
+for i = 2:6
+    plot(f, abs(G_iff_m1(:,i, i)), 'color', [colors(2,:), 0.5], ...
+         'HandleVisibility', 'off')
+end
+plot(f, abs(G_iff_m2(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
+     'DisplayName', '$m = 2$');
+for i = 2:6
+    plot(f, abs(G_iff_m2(:,i, i)), 'color', [colors(3,:), 0.5], ...
+         'HandleVisibility', 'off')
+end
+plot(f, abs(G_iff_m3(:, 1, 1)), 'color', [colors(4,:), 0.5], ...
+     'DisplayName', '$m = 3$');
+for i = 2:6
+    plot(f, abs(G_iff_m3(:,i, i)), 'color', [colors(4,:), 0.5], ...
+         'HandleVisibility', 'off')
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-2, 1e2]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+for i =1:6
+    plot(f, 180/pi*angle(G_iff_m0(:,i, i)), 'color', [colors(1,:), 0.5]);
+end
+for i =1:6
+    plot(f, 180/pi*angle(G_iff_m1(:,i, i)), 'color', [colors(2,:), 0.5]);
+end
+for i =1:6
+    plot(f, 180/pi*angle(G_iff_m2(:,i, i)), 'color', [colors(3,:), 0.5]);
+end
+for i =1:6
+    plot(f, 180/pi*angle(G_iff_m3(:,i, i)), 'color', [colors(4,:), 0.5]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-90, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_Giff_effect_mass.pdf', 'width', 'full', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_Giff_effect_mass
+#+caption: Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+#+RESULTS:
+[[file:figs/id31_Giff_effect_mass.png]]
+
+*** Compare with the model
+#+begin_src matlab
+load('Gm.mat')
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Comparison of the identified IFF plant and the IFF plant extracted from the simscape model
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_iff_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
+     'DisplayName', '$m = 0$');
+for i = 2:6
+    plot(f, abs(G_iff_m0(:,i, i)), 'color', [colors(1,:), 0.5], ...
+         'HandleVisibility', 'off')
+end
+plot(f, abs(G_iff_m1(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
+     'DisplayName', '$m = 1$');
+for i = 2:6
+    plot(f, abs(G_iff_m1(:,i, i)), 'color', [colors(2,:), 0.5], ...
+         'HandleVisibility', 'off')
+end
+plot(f, abs(G_iff_m2(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
+     'DisplayName', '$m = 2$');
+for i = 2:6
+    plot(f, abs(G_iff_m2(:,i, i)), 'color', [colors(3,:), 0.5], ...
+         'HandleVisibility', 'off')
+end
+plot(f, abs(G_iff_m3(:, 1, 1)), 'color', [colors(4,:), 0.5], ...
+     'DisplayName', '$m = 3$');
+for i = 2:6
+    plot(f, abs(G_iff_m3(:,i, i)), 'color', [colors(4,:), 0.5], ...
+         'HandleVisibility', 'off')
+end
+plot(freqs, abs(squeeze(freqresp(Gm_m0('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:), ...
+     'DisplayName', '$m = 0$ - Model');
+plot(freqs, abs(squeeze(freqresp(Gm_m1('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:), ...
+     'DisplayName', '$m = 1$ - Model');
+plot(freqs, abs(squeeze(freqresp(Gm_m2('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:), ...
+     'DisplayName', '$m = 2$ - Model');
+plot(freqs, abs(squeeze(freqresp(Gm_m3('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:), ...
+     'DisplayName', '$m = 3$ - Model');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-2, 1e2]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(G_iff_m0(:,1,1)), 'color', [colors(1,:), 0.5]);
+for i = 2:6
+    plot(f, 180/pi*angle(G_iff_m0(:,i, i)), 'color', [colors(1,:), 0.5]);
+end
+plot(f, 180/pi*angle(G_iff_m1(:,1,1)), 'color', [colors(2,:), 0.5]);
+for i = 2:6
+    plot(f, 180/pi*angle(G_iff_m1(:,i, i)), 'color', [colors(2,:), 0.5]);
+end
+plot(f, 180/pi*angle(G_iff_m2(:,1,1)), 'color', [colors(3,:), 0.5]);
+for i = 2:6
+    plot(f, 180/pi*angle(G_iff_m2(:,i, i)), 'color', [colors(3,:), 0.5]);
+end
+plot(f, 180/pi*angle(G_iff_m3(:,1,1)), 'color', [colors(4,:), 0.5]);
+for i = 2:6
+    plot(f, 180/pi*angle(G_iff_m3(:,i, i)), 'color', [colors(4,:), 0.5]);
+end
+plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:));
+plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m1('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:));
+plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m2('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:));
+plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m3('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-90, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_Giff_plant_comp_model.pdf', 'width', 'full', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_Giff_plant_comp_model
+#+caption: Comparison of the identified IFF plant and the IFF plant extracted from the simscape model
+#+RESULTS:
+[[file:figs/id31_Giff_plant_comp_model.png]]
+
+
+** IFF Controller
+*** Controller Design
+Test second order high pass filter:
+#+begin_src matlab
+wz = 2*pi*10;
+xiz = 0.7;
+Ghpf = (s^2/wz^2)/(s^2/wz^2 + 2*xiz*s/wz + 1)
+       % s/(2*pi*1)/(1 + s/(2*pi*1)) * ... % HPF: reduce gain at low frequency
+#+end_src
+
+We want integral action between 20Hz and 200Hz.
+#+begin_src matlab
+%% IFF Controller
+Kiff = -1e2 * ...                           % Gain
+       1/(0.01*2*pi + s) * ...               % LPF: provides integral action
+       Ghpf * ...
+       eye(6);                             % Diagonal 6x6 controller
+Kiff.InputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6'};
+Kiff.OutputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
+#+end_src
+
+Loop Gain:
+#+begin_src matlab :exports none :results none
+%% IFF Loop gain of the diagonal terms
+Kiff_frf = squeeze(freqresp(Kiff(1,1), f, 'Hz'));
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_iff_m0(:, 1, 1).*Kiff_frf), 'color', colors(1,:), ...
+     'DisplayName', '$m = 0$');
+plot(f, abs(G_iff_m1(:, 1, 1).*Kiff_frf), 'color', colors(2,:), ...
+     'DisplayName', '$m = 1$');
+plot(f, abs(G_iff_m2(:, 1, 1).*Kiff_frf), 'color', colors(3,:), ...
+     'DisplayName', '$m = 2$');
+plot(f, abs(G_iff_m3(:, 1, 1).*Kiff_frf), 'color', colors(4,:), ...
+     'DisplayName', '$m = 3$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-2, 1e1]);
+legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(-G_iff_m0(:,1,1).*Kiff_frf), 'color', colors(1,:));
+plot(f, 180/pi*angle(-G_iff_m1(:,1,1).*Kiff_frf), 'color', colors(2,:));
+plot(f, 180/pi*angle(-G_iff_m2(:,1,1).*Kiff_frf), 'color', colors(3,:));
+plot(f, 180/pi*angle(-G_iff_m3(:,1,1).*Kiff_frf), 'color', colors(4,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_iff_loop_gain_diagonal_terms.pdf', 'width', 'full', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_iff_loop_gain_diagonal_terms
+#+caption: IFF Loop gain of the diagonal terms
+#+RESULTS:
+[[file:figs/id31_iff_loop_gain_diagonal_terms.png]]
+
+Root Locus to obtain optimal gain.
+#+begin_src matlab :exports none :results none
+%% Root Locus for IFF
+gains = logspace(-2, 2, 100);
+Gm_iff_m0 = Gm_m0({'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'});
+Gm_iff_m1 = Gm_m1({'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'});
+Gm_iff_m2 = Gm_m2({'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'});
+Gm_iff_m3 = Gm_m3({'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'});
+
+figure;
+tiledlayout(1, 4, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile();
+hold on;
+plot(real(pole(Gm_iff_m0)),  imag(pole(Gm_iff_m0)),  'x', 'color', colors(1,:), ...
+    'DisplayName', '$g = 0$');
+plot(real(tzero(Gm_iff_m0)), imag(tzero(Gm_iff_m0)), 'o', 'color', colors(1,:), ...
+    'HandleVisibility', 'off');
+
+for g = gains
+    clpoles = pole(feedback(Gm_iff_m0, g*Kiff, +1));
+    plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
+        'HandleVisibility', 'off');
+end
+
+% Optimal gain
+clpoles = pole(feedback(Gm_iff_m0, Kiff, +1));
+plot(real(clpoles), imag(clpoles), 'x', 'color', colors(5,:), ...
+    'DisplayName', '$g_{opt}$');
+hold off;
+axis equal;
+xlim([-640, 0]); ylim([0, 1600]);
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+title('$m_0$');
+
+ax2 = nexttile();
+hold on;
+plot(real(pole(Gm_iff_m1)),  imag(pole(Gm_iff_m1)),  'x', 'color', colors(2,:), ...
+    'DisplayName', '$g = 0$');
+plot(real(tzero(Gm_iff_m1)), imag(tzero(Gm_iff_m1)), 'o', 'color', colors(2,:), ...
+    'HandleVisibility', 'off');
+
+for g = gains
+    clpoles = pole(feedback(Gm_iff_m1, g*Kiff, +1));
+    plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ...
+        'HandleVisibility', 'off');
+end
+
+% Optimal gain
+clpoles = pole(feedback(Gm_iff_m1, Kiff, +1));
+plot(real(clpoles), imag(clpoles), 'x', 'color', colors(5,:), ...
+    'DisplayName', '$g_{opt}$');
+hold off;
+axis equal;
+xlim([-320, 0]); ylim([0, 800]);
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+title('$m_1$');
+
+ax3 = nexttile();
+hold on;
+plot(real(pole(Gm_iff_m2)),  imag(pole(Gm_iff_m2)),  'x', 'color', colors(3,:), ...
+    'DisplayName', '$g = 0$');
+plot(real(tzero(Gm_iff_m2)), imag(tzero(Gm_iff_m2)), 'o', 'color', colors(3,:), ...
+    'HandleVisibility', 'off');
+
+for g = gains
+    clpoles = pole(feedback(Gm_iff_m2, g*Kiff, +1));
+    plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ...
+        'HandleVisibility', 'off');
+end
+
+% Optimal gain
+clpoles = pole(feedback(Gm_iff_m2, Kiff, +1));
+plot(real(clpoles), imag(clpoles), 'x', 'color', colors(5,:), ...
+    'DisplayName', '$g_{opt}$');
+hold off;
+axis equal;
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+xlim([-240, 0]); ylim([0, 600]);
+title('$m_2$');
+
+ax4 = nexttile();
+hold on;
+plot(real(pole(Gm_iff_m3)),  imag(pole(Gm_iff_m3)),  'x', 'color', colors(4,:), ...
+    'DisplayName', '$g = 0$');
+plot(real(tzero(Gm_iff_m3)), imag(tzero(Gm_iff_m3)), 'o', 'color', colors(4,:), ...
+    'HandleVisibility', 'off');
+
+for g = gains
+    clpoles = pole(feedback(Gm_iff_m3, g*Kiff, +1));
+    plot(real(clpoles), imag(clpoles), '.', 'color', colors(4,:), ...
+        'HandleVisibility', 'off');
+end
+
+% Optimal gain
+clpoles = pole(feedback(Gm_iff_m3, Kiff, +1));
+plot(real(clpoles), imag(clpoles), 'x', 'color', colors(5,:), ...
+    'DisplayName', '$g_{opt}$');
+hold off;
+axis equal;
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+xlim([-160, 0]); ylim([0, 400]);
+title('$m_3$');
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_iff_root_locus.pdf', 'width', 'full', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_iff_root_locus
+#+caption: Root Locus for IFF. Green crosses are closed-loop poles for the same choosen IFF gain.
+#+RESULTS:
+[[file:figs/id31_iff_root_locus.png]]
+
+*** TODO Verify Stability
+Verify Stability with Nyquist plot:
+
+- Why bad stability margins?
+
+#+begin_src matlab :exports none
+%% Compute the Eigenvalues of the loop gain
+Ldet = zeros(4, 6, length(f));
+
+% Loop gain
+Lmimo = pagemtimes(permute(G_iff_m0, [2,3,1]),squeeze(freqresp(Kiff, f, 'Hz')));
+for i_f = 2:length(f)
+    Ldet(1,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
+end
+
+Lmimo = pagemtimes(permute(G_iff_m1, [2,3,1]),squeeze(freqresp(Kiff, f, 'Hz')));
+for i_f = 2:length(f)
+    Ldet(2,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
+end
+
+Lmimo = pagemtimes(permute(G_iff_m2, [2,3,1]),squeeze(freqresp(Kiff, f, 'Hz')));
+for i_f = 2:length(f)
+    Ldet(3,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
+end
+
+Lmimo = pagemtimes(permute(G_iff_m3, [2,3,1]),squeeze(freqresp(Kiff, f, 'Hz')));
+for i_f = 2:length(f)
+    Ldet(4,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% Plot of the eigenvalues of L in the complex plane
+figure;
+hold on;
+for i_mass = 1:4
+    plot(real(squeeze(Ldet(i_mass, 1,:))), imag(squeeze(Ldet(i_mass, 1,:))), ...
+         '.', 'color', colors(i_mass, :), ...
+         'DisplayName', sprintf('%i masses', i_mass));
+    plot(real(squeeze(Ldet(i_mass, 1,:))), -imag(squeeze(Ldet(i_mass, 1,:))), ...
+         '.', 'color', colors(i_mass, :), ...
+         'HandleVisibility', 'off');
+    for i = 1:6
+        plot(real(squeeze(Ldet(i_mass, i,:))), imag(squeeze(Ldet(i_mass, i,:))), ...
+             '.', 'color', colors(i_mass, :), ...
+             'HandleVisibility', 'off');
+        plot(real(squeeze(Ldet(i_mass, i,:))), -imag(squeeze(Ldet(i_mass, i,:))), ...
+             '.', 'color', colors(i_mass, :), ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(-1, 0, 'kx', 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
+xlabel('Real'); ylabel('Imag');
+legend('location', 'southeast');
+xlim([-3, 1]); ylim([-2, 2]);
+#+end_src
+
+*** Save Controller
+#+begin_src matlab :exports none :tangle no
+K_order = order(Kiff(1,1));
+
+Kz = c2d(Kiff(1,1)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
+[num, den] = tfdata(Kz, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_iff.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+#+begin_src matlab
+save('./matlab/mat/K_iff.mat', 'Kiff')
+#+end_src
+
+** Estimated Damped Plant
+#+begin_src matlab
+%% Damped plant from Simscape model
+Gm_hac_m0 = -feedback(Gm_m0, Kiff, 'name', +1);
+Gm_hac_m1 = -feedback(Gm_m1, Kiff, 'name', +1);
+Gm_hac_m2 = -feedback(Gm_m2, Kiff, 'name', +1);
+Gm_hac_m3 = -feedback(Gm_m3, Kiff, 'name', +1);
+#+end_src
+
+#+begin_src matlab
+%% Verify Stability
+isstable(Gm_hac_m0)
+isstable(Gm_hac_m1)
+isstable(Gm_hac_m2)
+isstable(Gm_hac_m3)
+#+end_src
+
+#+begin_src matlab
+%% Save Damped Plants
+save('./matlab/mat/Gm.mat', 'Gm_hac_m0', 'Gm_hac_m1', 'Gm_hac_m2', 'Gm_hac_m3', '-append');
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Estimated damped plant from the Simscape model
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
+    'DisplayName', '$\tau_{m,i}/u_i$ - $m_0$');
+for i = 2:6
+    plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
+        'HandleVisibility', 'off');
+end
+plot(freqs, abs(squeeze(freqresp(Gm_hac_m1(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
+    'DisplayName', '$\tau_{m,i}/u_i$ - $m_1$');
+for i = 2:6
+    plot(freqs, abs(squeeze(freqresp(Gm_hac_m1(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
+        'HandleVisibility', 'off');
+end
+plot(freqs, abs(squeeze(freqresp(Gm_hac_m2(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
+    'DisplayName', '$\tau_{m,i}/u_i$ - $m_2$');
+for i = 2:6
+    plot(freqs, abs(squeeze(freqresp(Gm_hac_m2(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
+        'HandleVisibility', 'off');
+end
+plot(freqs, abs(squeeze(freqresp(Gm_hac_m3(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
+    'DisplayName', '$\tau_{m,i}/u_i$ - $m_3$');
+for i = 2:6
+    plot(freqs, abs(squeeze(freqresp(Gm_hac_m3(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
+        'HandleVisibility', 'off');
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 1e-3]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
+
+ax2 = nexttile;
+hold on;
+for i =1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
+end
+for i =1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m1(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
+end
+for i =1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m2(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5]);
+end
+for i =1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m3(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_hac_damped_plant_estimated_simscape.pdf', 'width', 'wide', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_hac_damped_plant_estimated_simscape
+#+caption: description
+#+RESULTS:
+[[file:figs/id31_hac_damped_plant_estimated_simscape.png]]
+
+
+* High Authority Control                                            :noexport:
+<<sec:test_id31_iff_hac>>
+** Introduction                                                      :ignore:
+
+** Matlab Init                                              :noexport:ignore:
+#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
+<<matlab-dir>>
+#+end_src
+
+#+begin_src matlab :exports none :results silent :noweb yes
+<<matlab-init>>
+#+end_src
+
+#+begin_src matlab :tangle no :noweb yes
+<<m-init-path>>
+#+end_src
+
+#+begin_src matlab :eval no :noweb yes
+<<m-init-path-tangle>>
+#+end_src
+
+#+begin_src matlab :noweb yes
+<<m-init-other>>
+#+end_src
+
+** Identify Spurious modes
+#+begin_src matlab
+%% Load identification data
+data = load(sprintf('%s/dynamics/2023-08-10_18-32_identify_spurious_modes.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+% Hannning Windows
+Nfft = floor(1.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+#+end_src
+
+#+begin_src matlab :exports none
+% And we get the frequency vector
+[G1, f] = tfestimate(data.id_plant, data.d1, win, Noverlap, Nfft, 1/Ts);
+[G2, ~] = tfestimate(data.id_plant, data.d2, win, Noverlap, Nfft, 1/Ts);
+[G3, ~] = tfestimate(data.id_plant, data.d3, win, Noverlap, Nfft, 1/Ts);
+[G4, ~] = tfestimate(data.id_plant, data.d4, win, Noverlap, Nfft, 1/Ts);
+[G5, ~] = tfestimate(data.id_plant, data.d5, win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile();
+hold on;
+plot(f, abs(G1), 'DisplayName', '1 - top');
+plot(f, abs(G2), 'DisplayName', '2 - bot');
+plot(f, abs(G3), 'DisplayName', '3 - top');
+plot(f, abs(G4), 'DisplayName', '4 - bot');
+plot(f, abs(G5), 'DisplayName', '5 - vertical');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
+xlim([500, 800])
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+#+end_src
+
+** HAC Plants
+*** Introduction                                                    :ignore:
+
+*** 6x6 Plant
+#+begin_src matlab
+%% Load identification data
+data = load(sprintf('%s/dynamics/2023-08-17_17-53_damp_plant_m0_Wz0.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+% Hannning Windows
+Nfft = floor(1.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+#+end_src
+
+#+begin_src matlab :exports none
+% And we get the frequency vector
+[~, f] = tfestimate(data.uL1.id_plant, data.uL1.e_L1, win, [], [], 1/Ts);
+#+end_src
+
+#+begin_src matlab :exports none
+%% HAC Plant
+G_hac = zeros(length(f), 6, 6);
+
+for i_strut = 1:6
+    eL = [data.(sprintf("uL%i", i_strut)).e_L1 ; data.(sprintf("uL%i", i_strut)).e_L2 ; data.(sprintf("uL%i", i_strut)).e_L3 ; data.(sprintf("uL%i", i_strut)).e_L4 ; data.(sprintf("uL%i", i_strut)).e_L5 ; data.(sprintf("uL%i", i_strut)).e_L6]';
+
+    G_hac(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, [], [], 1/Ts);
+end
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_hac(:, i, j)), 'color', [0, 0, 0, 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+for i = 1:6
+    plot(f, abs(G_hac(:,i, i)), 'color', colors(i,:), ...
+        'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i));
+end
+plot(f, abs(G_hac(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
+     'DisplayName', '$\tau_{m,i}/u_j$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 1e-3]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
+
+ax2 = nexttile;
+hold on;
+for i =1:6
+    plot(f, 180/pi*angle(G_hac(:,i, i)));
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+Compare with Model:
+#+begin_src matlab
+load('Gm.mat');
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% 6x6 plant from generated voltages to displacement of the struts as measured by the external metrology
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_hac(:,1,2)), 'color', [colors(3,:), 0.5], ...
+        'DisplayName', 'Identified, Off-Diagonal');
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_hac(:, i, j)), 'color', [colors(3,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
+        'DisplayName', 'Model, Off-Diagonal');
+for i = 1:5
+    for j = i+1:6
+        plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(4,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(f, abs(G_hac(:,1,1)), 'color', [colors(1,:), 0.5], ...
+        'DisplayName', 'Identified, Diagonal');
+for i = 2:6
+    plot(f, abs(G_hac(:,i, i)), 'color', [colors(1,:), 0.5], ...
+            'HandleVisibility', 'off');
+end
+plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
+        'DisplayName', 'Model, Diagonal');
+for i = 2:6
+    plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
+            'HandleVisibility', 'off');
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 1e-3]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
+
+ax2 = nexttile;
+hold on;
+for i = 1:6
+    plot(f, 180/pi*angle(G_hac(:,i, i)), 'color', [colors(1,:), 0.5])
+end
+for i = 1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5])
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_Ghac_6x6_plant_comp_model.pdf', 'width', 'full', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_Ghac_6x6_plant_comp_model
+#+caption: 6x6 plant from generated voltages to displacement of the struts as measured by the external metrology
+#+RESULTS:
+[[file:figs/id31_Ghac_6x6_plant_comp_model.png]]
+
+
+*** Effect of Mass
+#+begin_src matlab
+%% Load identification data
+data_m0 = load(sprintf('%s/dynamics/2023-08-17_17-53_damp_plant_m0_Wz0.mat', mat_dir));
+data_m1 = load(sprintf('%s/dynamics/2023-08-10_16-01_damp_plant_m1_Wz0.mat', mat_dir));
+data_m2 = load(sprintf('%s/dynamics/2023-08-10_17-28_damp_plant_m2_Wz0.mat', mat_dir));
+data_m3 = load(sprintf('%s/dynamics/2023-08-10_18-16_damp_plant_m3_Wz0.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+% Sampling Time [s]
+Ts = 1e-4;
+
+% Hannning Windows
+Nfft = floor(2.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+#+end_src
+
+#+begin_src matlab :exports none
+% And we get the frequency vector
+[~, f] = tfestimate(data_m0.uL1.id_plant, data_m0.uL1.e_L1, win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+#+begin_src matlab :exports none
+%% HAC Plant (transfer function from u to taum)
+G_hac_m0 = zeros(length(f), 6, 6);
+
+for i_strut = 1:6
+    eL = [data_m0.(sprintf("uL%i", i_strut)).e_L1 ; data_m0.(sprintf("uL%i", i_strut)).e_L2 ; data_m0.(sprintf("uL%i", i_strut)).e_L3 ; data_m0.(sprintf("uL%i", i_strut)).e_L4 ; data_m0.(sprintf("uL%i", i_strut)).e_L5 ; data_m0.(sprintf("uL%i", i_strut)).e_L6]';
+
+    G_hac_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts);
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% HAC Plant (transfer function from u to taum)
+G_hac_m1 = zeros(length(f), 6, 6);
+
+for i_strut = 1:6
+    eL = [data_m1.(sprintf("uL%i", i_strut)).e_L1 ; data_m1.(sprintf("uL%i", i_strut)).e_L2 ; data_m1.(sprintf("uL%i", i_strut)).e_L3 ; data_m1.(sprintf("uL%i", i_strut)).e_L4 ; data_m1.(sprintf("uL%i", i_strut)).e_L5 ; data_m1.(sprintf("uL%i", i_strut)).e_L6]';
+
+    G_hac_m1(:,:,i_strut) = tfestimate(data_m1.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts);
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% HAC Plant (transfer function from u to taum)
+G_hac_m2 = zeros(length(f), 6, 6);
+
+for i_strut = 1:6
+    eL = [data_m2.(sprintf("uL%i", i_strut)).e_L1 ; data_m2.(sprintf("uL%i", i_strut)).e_L2 ; data_m2.(sprintf("uL%i", i_strut)).e_L3 ; data_m2.(sprintf("uL%i", i_strut)).e_L4 ; data_m2.(sprintf("uL%i", i_strut)).e_L5 ; data_m2.(sprintf("uL%i", i_strut)).e_L6]';
+
+    G_hac_m2(:,:,i_strut) = tfestimate(data_m2.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts);
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% HAC Plant (transfer function from u to taum)
+G_hac_m3 = zeros(length(f), 6, 6);
+
+for i_strut = 1:6
+    eL = [data_m3.(sprintf("uL%i", i_strut)).e_L1 ; data_m3.(sprintf("uL%i", i_strut)).e_L2 ; data_m3.(sprintf("uL%i", i_strut)).e_L3 ; data_m3.(sprintf("uL%i", i_strut)).e_L4 ; data_m3.(sprintf("uL%i", i_strut)).e_L5 ; data_m3.(sprintf("uL%i", i_strut)).e_L6]';
+
+    G_hac_m3(:,:,i_strut) = tfestimate(data_m3.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts);
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% Save Identified Plants
+save('./matlab/mat/G_hac.mat', 'G_hac_m0', 'G_hac_m1', 'G_hac_m2', 'G_hac_m3', 'f');
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer functions
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_hac_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
+     'DisplayName', '$m = 0$');
+for i = 2:6
+    plot(f, abs(G_hac_m0(:,i, i)), 'color', [colors(1,:), 0.5], ...
+         'HandleVisibility', 'off')
+end
+plot(f, abs(G_hac_m1(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
+     'DisplayName', '$m = 1$');
+for i = 2:6
+    plot(f, abs(G_hac_m1(:,i, i)), 'color', [colors(2,:), 0.5], ...
+         'HandleVisibility', 'off')
+end
+plot(f, abs(G_hac_m2(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
+     'DisplayName', '$m = 2$');
+for i = 2:6
+    plot(f, abs(G_hac_m2(:,i, i)), 'color', [colors(3,:), 0.5], ...
+         'HandleVisibility', 'off')
+end
+plot(f, abs(G_hac_m3(:, 1, 1)), 'color', [colors(4,:), 0.5], ...
+     'DisplayName', '$m = 3$');
+for i = 2:6
+    plot(f, abs(G_hac_m3(:,i, i)), 'color', [colors(4,:), 0.5], ...
+         'HandleVisibility', 'off')
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 1e-3]);
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
+
+ax2 = nexttile;
+hold on;
+for i =1:6
+    plot(f, 180/pi*angle(G_hac_m0(:,i, i)), 'color', [colors(1,:), 0.5]);
+end
+for i =1:6
+    plot(f, 180/pi*angle(G_hac_m1(:,i, i)), 'color', [colors(2,:), 0.5]);
+end
+for i =1:6
+    plot(f, 180/pi*angle(G_hac_m2(:,i, i)), 'color', [colors(3,:), 0.5]);
+end
+for i =1:6
+    plot(f, 180/pi*angle(G_hac_m3(:,i, i)), 'color', [colors(4,:), 0.5]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_Ghac_effect_mass.pdf', 'width', 'full', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_Ghac_effect_mass
+#+caption: Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+#+RESULTS:
+[[file:figs/id31_Ghac_effect_mass.png]]
+
+*** Compare with the model
+#+begin_src matlab
+load('G_hac.mat')
+load('Gm.mat')
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Comparison of the identified HAC plant and the HAC plant extracted from the simscape model
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_hac_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
+     'DisplayName', '$m = 0$');
+for i = 2:6
+    plot(f, abs(G_hac_m0(:,i, i)), 'color', [colors(1,:), 0.5], ...
+         'HandleVisibility', 'off')
+end
+plot(f, abs(G_hac_m1(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
+     'DisplayName', '$m = 1$');
+for i = 2:6
+    plot(f, abs(G_hac_m1(:,i, i)), 'color', [colors(2,:), 0.5], ...
+         'HandleVisibility', 'off')
+end
+plot(f, abs(G_hac_m2(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
+     'DisplayName', '$m = 2$');
+for i = 2:6
+    plot(f, abs(G_hac_m2(:,i, i)), 'color', [colors(3,:), 0.5], ...
+         'HandleVisibility', 'off')
+end
+plot(f, abs(G_hac_m3(:, 1, 1)), 'color', [colors(4,:), 0.5], ...
+     'DisplayName', '$m = 3$');
+for i = 2:6
+    plot(f, abs(G_hac_m3(:,i, i)), 'color', [colors(4,:), 0.5], ...
+         'HandleVisibility', 'off')
+end
+plot(freqs, abs(squeeze(freqresp(Gm_hac_m0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:), ...
+     'DisplayName', '$m = 0$ - Model');
+plot(freqs, abs(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:), ...
+     'DisplayName', '$m = 1$ - Model');
+plot(freqs, abs(squeeze(freqresp(Gm_hac_m2('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:), ...
+     'DisplayName', '$m = 2$ - Model');
+plot(freqs, abs(squeeze(freqresp(Gm_hac_m3('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:), ...
+     'DisplayName', '$m = 3$ - Model');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 1e-3]);
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(G_hac_m0(:,1,1)), 'color', [colors(1,:), 0.5]);
+for i = 2:6
+    plot(f, 180/pi*angle(G_hac_m0(:,i, i)), 'color', [colors(1,:), 0.5]);
+end
+plot(f, 180/pi*angle(G_hac_m1(:,1,1)), 'color', [colors(2,:), 0.5]);
+for i = 2:6
+    plot(f, 180/pi*angle(G_hac_m1(:,i, i)), 'color', [colors(2,:), 0.5]);
+end
+plot(f, 180/pi*angle(G_hac_m2(:,1,1)), 'color', [colors(3,:), 0.5]);
+for i = 2:6
+    plot(f, 180/pi*angle(G_hac_m2(:,i, i)), 'color', [colors(3,:), 0.5]);
+end
+plot(f, 180/pi*angle(G_hac_m3(:,1,1)), 'color', [colors(4,:), 0.5]);
+for i = 2:6
+    plot(f, 180/pi*angle(G_hac_m3(:,i, i)), 'color', [colors(4,:), 0.5]);
+end
+plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:));
+plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:));
+plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m2('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:));
+plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m3('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_Ghac_plant_comp_model.pdf', 'width', 'full', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_Ghac_plant_comp_model
+#+caption: Comparison of the identified HAC plant and the HAC plant extracted from the simscape model
+#+RESULTS:
+[[file:figs/id31_Ghac_plant_comp_model.png]]
+
+
+*** Comparison with Undamped plant
+
+#+begin_src matlab
+load('G_ol.mat');
+load('G_hac.mat');
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% description
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_int_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
+     'DisplayName', '$m = 0$ - OL');
+for i = 2:6
+    plot(f, abs(G_int_m0(:,i, i)), 'color', [colors(1,:), 0.5], ...
+         'HandleVisibility', 'off')
+end
+plot(f, abs(G_hac_m0(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
+     'DisplayName', '$m = 0$ - Damped');
+for i = 2:6
+    plot(f, abs(G_hac_m0(:,i, i)), 'color', [colors(2,:), 0.5], ...
+         'HandleVisibility', 'off')
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 1e-3]);
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
+
+ax2 = nexttile;
+hold on;
+for i =1:6
+    plot(f, 180/pi*angle(-G_int_m0(:,i, i)), 'color', [colors(1,:), 0.5]);
+end
+for i =1:6
+    plot(f, 180/pi*angle(G_hac_m0(:,i, i)), 'color', [colors(2,:), 0.5]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_comp_undamped_damped_plant_m0.pdf', 'width', 'full', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_comp_undamped_damped_plant_m0
+#+caption: description
+#+RESULTS:
+[[file:figs/id31_comp_undamped_damped_plant_m0.png]]
+
+** Robust HAC
+#+begin_src matlab
+load('G_hac.mat')
+load('Gm.mat')
+#+end_src
+
+*** Controller design
+#+begin_src matlab
+%% Wanted crossover
+wc = 2*pi*5;
+
+%% Double Integrator
+% H_int = (wc^2)/(s + 1e-1*2*pi)^2;
+H_int = wc/s;
+
+%% Lead to increase phase margin
+a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
+
+H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
+
+%% Low Pass filter to increase robustness
+H_lpf = 1/(1 + s/2/pi/30);
+
+%% Notch at the top-plate resonance
+% gm = 0.02;
+% xi = 0.3;
+% wn = 2*pi*665;
+
+% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
+
+%% Gain to have unitary crossover at 30Hz
+[~, i_f] = min(abs(f - wc/2/pi));
+H_gain = 1./abs(G_hac_m0(i_f, 1, 1));
+
+%% Decentralized HAC
+Khac = H_gain  * ... % Gain
+       H_int   * ... % Integrator
+       H_lpf   * ... % Integrator
+       eye(6);            % 6x6 Diagonal
+#+end_src
+
+Loop gain
+#+begin_src matlab :exports none :results none
+%% description
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_hac_m0(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(1,:), 'DisplayName', '$m_0$');
+plot(f, abs(G_hac_m1(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(2,:), 'DisplayName', '$m_1$');
+plot(f, abs(G_hac_m2(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(3,:), 'DisplayName', '$m_2$');
+plot(f, abs(G_hac_m3(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(4,:), 'DisplayName', '$m_3$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
+legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 3);
+ylim([1e-5, 1e2]);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(G_hac_m0(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(1,:));
+plot(f, 180/pi*angle(G_hac_m1(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(2,:));
+plot(f, 180/pi*angle(G_hac_m2(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(3,:));
+plot(f, 180/pi*angle(G_hac_m3(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(4,:));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_hac_robust_loop_gain.pdf', 'width', 'full', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_hac_robust_loop_gain
+#+caption: description
+#+RESULTS:
+[[file:figs/id31_hac_robust_loop_gain.png]]
+
+
+*** Verify Stability
+#+begin_src matlab :exports none
+%% Compute the Eigenvalues of the loop gain
+Ldet = zeros(4, 6, length(f));
+
+% Loop gain
+Lmimo = pagemtimes(permute(G_hac_m0, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
+for i_f = 2:length(f)
+    Ldet(1,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
+end
+
+Lmimo = pagemtimes(permute(G_hac_m1, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
+for i_f = 2:length(f)
+    Ldet(2,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
+end
+
+Lmimo = pagemtimes(permute(G_hac_m2, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
+for i_f = 2:length(f)
+    Ldet(3,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
+end
+
+Lmimo = pagemtimes(permute(G_hac_m3, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
+for i_f = 2:length(f)
+    Ldet(4,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% Plot of the eigenvalues of L in the complex plane
+figure;
+hold on;
+for i_mass = 1:4
+    plot(real(squeeze(Ldet(i_mass, 1,:))), imag(squeeze(Ldet(i_mass, 1,:))), ...
+         '.', 'color', colors(i_mass, :), ...
+         'DisplayName', sprintf('$m_%i$', i_mass));
+    plot(real(squeeze(Ldet(i_mass, 1,:))), -imag(squeeze(Ldet(i_mass, 1,:))), ...
+         '.', 'color', colors(i_mass, :), ...
+         'HandleVisibility', 'off');
+    for i = 1:6
+        plot(real(squeeze(Ldet(i_mass, i,:))), imag(squeeze(Ldet(i_mass, i,:))), ...
+             '.', 'color', colors(i_mass, :), ...
+             'HandleVisibility', 'off');
+        plot(real(squeeze(Ldet(i_mass, i,:))), -imag(squeeze(Ldet(i_mass, i,:))), ...
+             '.', 'color', colors(i_mass, :), ...
+             'HandleVisibility', 'off');
+    end
+end
+plot(-1, 0, 'kx', 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
+xlabel('Real'); ylabel('Imag');
+legend('location', 'southeast');
+axis square
+xlim([-1.5, 0.5]); ylim([-1, 1]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_hac_robust_nyquist.pdf', 'width', 'wide', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_hac_robust_nyquist
+#+caption: description
+#+RESULTS:
+[[file:figs/id31_hac_robust_nyquist.png]]
+
+*** Estimated performances
+
+*** Save Controller
+#+begin_src matlab :exports none :tangle no
+K_order = order(Khac(1,1));
+
+Kz = c2d(Khac(1,1)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
+[num, den] = tfdata(Kz, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+#+begin_src matlab
+save('./matlab/mat/K_hac.mat', 'Khac')
+#+end_src
+
+
+** High Performance HAC
+*** Introduction                                                    :ignore:
+The goal is to make a controller specific for one mass in order to have high bandwidth.
+
+*** Mass 0
+**** Load Plant
+#+begin_src matlab
+load('G_hac.mat')
+load('Gm.mat')
+#+end_src
+
+**** Plant
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_hac_m0(:, i, j)), 'color', [0, 0, 0, 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+for i = 1:6
+    plot(f, abs(G_hac_m0(:,i, i)), 'color', colors(i,:), ...
+         'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i));
+end
+plot(f, abs(G_hac_m0(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
+     'DisplayName', '$\tau_{m,i}/u_j$');
+plot(freqs, abs(squeeze(freqresp(Gm_hac_m0('eL1', 'u1'), freqs, 'Hz'))), 'k--', ...
+     'DisplayName', '$\tau_{m,i}/u_i$ - Model');
+plot(freqs, abs(squeeze(freqresp(Gm_hac_m0('eL2', 'u1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
+     'DisplayName', '$\tau_{m,i}/u_j$ - Model');
+for i = 1:5
+    for j = i+1:6
+        plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 1e-3]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
+
+ax2 = nexttile;
+hold on;
+for i =1:6
+    plot(f, 180/pi*angle(G_hac_m0(:,i, i)));
+end
+plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m0('eL1', 'u1'), freqs, 'Hz'))), 'k--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+**** Controller design
+#+begin_src matlab
+%% Wanted crossover
+wc = 2*pi*50;
+
+%% Double Integrator
+H_int = 1/(s + 0.1*2*pi) * ...
+        1/(0.1*2*pi + s);
+H_int = H_int/abs(evalfr(H_int, 1j*wc));
+% H_int = wc/s;
+
+%% Lead to increase phase margin
+a  = 3;  % Amount of phase lead / width of the phase lead / high frequency gain
+H_lead = 1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a)));
+a  = 3;  % Amount of phase lead / width of the phase lead / high frequency gain
+H_lead = H_lead/sqrt(a)*(1 + s/(2.5*wc/sqrt(a)))/(1 + s/(2.5*wc*sqrt(a)));
+
+%% Low Pass filter to increase robustness
+H_lpf = 1/(1 + s/2/pi/500);
+
+%% Notch at the top-plate resonance
+% gm = 0.02;
+% xi = 0.3;
+% wn = 2*pi*665;
+
+% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
+
+%% Gain to have unitary crossover at 30Hz
+[~, i_f] = min(abs(f - wc/2/pi));
+H_gain = 1./abs(G_hac_m0(i_f, 1, 1));
+
+%% Decentralized HAC
+Khac = H_gain  * ... % Gain
+       H_int   * ... % Integrator
+       H_lpf   * ... % Low Pass Filter
+       H_lead  * ... % Integrator
+       eye(6);            % 6x6 Diagonal
+
+Khac.InputName = {'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
+Khac.OutputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
+#+end_src
+
+Loop gain
+#+begin_src matlab :exports none :results none
+%% Loop gain for the High Authority Control
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+for i = 1:6
+    plot(f, abs(G_hac_m0(:,i,i).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [colors(i,:), 0.5]);
+end
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_hac_m0(:,i,j).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [0, 0, 0, 0.2]);
+    end
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
+ylim([1e-4, 1e4]);
+
+ax2 = nexttile;
+hold on;
+for i = 1:6
+    plot(f, 180/pi*angle(G_hac_m0(:,i,i).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [colors(i,:), 0.5]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_high_perf_hac_m0_loop_gain.pdf', 'width', 'wide', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_high_perf_hac_m0_loop_gain
+#+caption: Loop gain for the High Authority Control
+#+RESULTS:
+[[file:figs/id31_high_perf_hac_m0_loop_gain.png]]
+
+**** Verify Stability
+#+begin_src matlab :exports none
+%% Compute the Eigenvalues of the loop gain
+Ldet = zeros(6, length(f));
+
+% Loop gain
+Lmimo = pagemtimes(permute(G_hac_m0, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
+for i_f = 2:length(f)
+    Ldet(:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% Plot of the eigenvalues of L in the complex plane
+figure;
+hold on;
+plot(real(squeeze(Ldet(1,:))),  imag(squeeze(Ldet(1,:))), 'k.');
+plot(real(squeeze(Ldet(1,:))), -imag(squeeze(Ldet(1,:))), 'k.');
+for i = 1:6
+    plot(real(squeeze(Ldet(i,:))),  imag(squeeze(Ldet(i,:))), 'k.');
+    plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.');
+end
+plot(-1, 0, 'kx', 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
+xlabel('Real'); ylabel('Imag');
+xlim([-3, 1]); ylim([-2, 2]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_high_perf_hac_m0_nyquist.pdf', 'width', 'wide', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_high_perf_hac_m0_nyquist
+#+caption: Nyquist plot for the High Authority Control
+#+RESULTS:
+[[file:figs/id31_high_perf_hac_m0_nyquist.png]]
+
+**** Estimated performances
+Loop gain with model
+#+begin_src matlab :exports none
+%% Loop Gain
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+for i = 1:6
+    plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i))*Khac(i,i), freqs, 'Hz'))), 'color', [colors(i,:), 0.5]);
+end
+for i = 1:5
+    for j = i+1:6
+        plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', j))*Khac(i,i), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
+    end
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
+ylim([1e-4, 1e4]);
+
+ax2 = nexttile;
+hold on;
+for i = 1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i))*Khac(i,i), freqs, 'Hz'))), 'color', [colors(i,:), 0.5]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab
+Gm_cl_m0 = feedback(Gm_hac_m0, 0.8*Khac, 'name', -1);
+#+end_src
+
+#+begin_src matlab
+isstable(Gm_cl_m0)
+#+end_src
+
+**** Save Controller
+#+begin_src matlab :exports none :tangle no
+K_order = order(Khac(1,1));
+
+Kz = c2d(Khac(1,1)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
+[num, den] = tfdata(Kz, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_m0.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+**** Experimental Validation
+
+#+begin_src matlab
+data_1rpm = load(sprintf('%s/scans/2023-08-18_10-43_m0_1rpm.mat', mat_dir));
+data_30rpm = load(sprintf('%s/scans/2023-08-18_10-45_m0_30rpm.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
+data2orgtable([rms(1e9*data_1rpm.Dy_int), rms(1e9*data_1rpm.Dz_int), rms(1e6*data_1rpm.Ry_int); rms(1e9*data_30rpm.Dy_int), rms(1e9*data_30rpm.Dz_int), rms(1e6*data_30rpm.Ry_int)], {'1rpm', '30rpm'}, {'Dy [nm]', 'Dz [nm]', 'Ry [urad]'}, ' %.1f ');
+#+end_src
+
+#+RESULTS:
+|       | Dy [nm] | Dz [nm] | Ry [urad] |
+|-------+---------+---------+-----------|
+| 1rpm  |    55.3 |     5.9 |       0.1 |
+| 30rpm |    85.2 |    12.5 |       0.3 |
+
+**** Closed-Loop identification
+#+begin_src matlab
+data_cl = load(sprintf('%s/dynamics/2023-08-18_11-03_m0_perf_hac.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+% Sampling Time [s]
+Ts = 1e-4;
+
+% Hannning Windows
+Nfft = floor(1.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+#+end_src
+
+#+begin_src matlab :exports none
+% And we get the frequency vector
+[S_dx, f] = tfestimate(data_cl.Dx.id_cl, data_cl.Dx.e_dx, win, Noverlap, Nfft, 1/Ts);
+[S_dy, ~] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
+[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
+[S_rx, ~] = tfestimate(data_cl.Rx.id_cl, data_cl.Rx.e_rx, win, Noverlap, Nfft, 1/Ts);
+[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
+[S_rz, ~] = tfestimate(data_cl.Rz.id_cl, data_cl.Rz.e_rz, win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile();
+hold on;
+plot(f, abs(S_dy), 'DisplayName', '$D_y$');
+plot(f, abs(S_dz), 'DisplayName', '$D_z$');
+plot(f, abs(S_ry), 'DisplayName', '$R_y$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Amplitude');
+ylim([1e-3, 1e1]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
+xlim([1, 1e3]);
+#+end_src
+
+
+*** Mass 1
+**** Load Plant
+#+begin_src matlab
+load('G_hac.mat')
+load('Gm.mat')
+#+end_src
+
+**** Plant
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_hac_m1(:, i, j)), 'color', [0, 0, 0, 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+for i = 1:6
+    plot(f, abs(G_hac_m1(:,i, i)), 'color', colors(i,:), ...
+         'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i));
+end
+plot(f, abs(G_hac_m1(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
+     'DisplayName', '$\tau_{m,i}/u_j$');
+plot(freqs, abs(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))), 'k--', ...
+     'DisplayName', '$\tau_{m,i}/u_i$ - Model');
+plot(freqs, abs(squeeze(freqresp(Gm_hac_m1('eL2', 'u1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
+     'DisplayName', '$\tau_{m,i}/u_j$ - Model');
+for i = 1:5
+    for j = i+1:6
+        plot(freqs, abs(squeeze(freqresp(Gm_hac_m1(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 1e-3]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
+
+ax2 = nexttile;
+hold on;
+for i =1:6
+    plot(f, 180/pi*angle(G_hac_m1(:,i, i)));
+end
+plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))), 'k--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+**** Plant Inverse
+#+begin_src matlab
+Gm_hac_red_m1 = flipRphZeros(-balred(Gm_hac_m1('eL1', 'u1'), 6, ...
+                                    balredOptions('StateProjection', 'MatchDC', ...
+                                                  'FreqIntervals', [0, 80])));
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(Gm_hac_red_m1, freqs, 'Hz'))));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-8, 1e-3]);
+
+ax2 = nexttile;
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))));
+plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_red_m1, 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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab
+%% Plant Inverse
+Gm_hac_inv_m1 = inv(Gm_hac_red_m1);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(inv(Gm_hac_m1('eL1', 'u1')), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(Gm_hac_inv_m1, freqs, 'Hz'))));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
+
+ax2 = nexttile;
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(inv(Gm_hac_m1('eL1', 'u1')), freqs, 'Hz'))));
+plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_inv_m1, 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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+**** Controller design
+#+begin_src matlab
+%% Wanted crossover
+wc = 2*pi*30;
+
+%% Double Integrator
+H_int = 1/(s + 0.1*2*pi) * ...
+        (50*2*pi + s)/(0.01*2*pi + s);
+H_int = H_int/abs(evalfr(H_int, 1j*wc));
+% H_int = wc/s;
+
+%% Lead to increase phase margin
+a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
+H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
+a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
+H_lead = H_lead*1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
+
+%% Low Pass filter to increase robustness
+H_lpf = 1/(1 + s/2/pi/200);
+
+%% Notch at the top-plate resonance
+% gm = 0.02;
+% xi = 0.3;
+% wn = 2*pi*665;
+
+% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
+
+%% Gain to have unitary crossover at 30Hz
+[~, i_f] = min(abs(f - wc/2/pi));
+H_gain = 1./abs(G_hac_m0(i_f, 1, 1));
+
+%% Decentralized HAC
+Khac = 0.8*H_gain  * ... % Gain
+       H_int   * ... % Integrator
+       H_lpf   * ... % Low Pass Filter
+       H_lead  * ... % Integrator
+       eye(6);            % 6x6 Diagonal
+
+Khac.InputName = {'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
+Khac.OutputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
+#+end_src
+
+Loop gain
+#+begin_src matlab :exports none
+%% Loop Gain
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+for i = 1:6
+    plot(f, abs(G_hac_m0(:,i,i).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [colors(i,:), 0.5]);
+end
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_hac_m0(:,i,j).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [0, 0, 0, 0.2]);
+    end
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
+ylim([1e-4, 1e4]);
+
+ax2 = nexttile;
+hold on;
+for i = 1:6
+    plot(f, 180/pi*angle(G_hac_m0(:,i,i).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [colors(i,:), 0.5]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+**** Verify Stability
+#+begin_src matlab :exports none
+%% Compute the Eigenvalues of the loop gain
+Ldet = zeros(6, length(f));
+
+% Loop gain
+Lmimo = pagemtimes(permute(G_hac_m0, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
+for i_f = 2:length(f)
+    Ldet(:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% Plot of the eigenvalues of L in the complex plane
+figure;
+hold on;
+plot(real(squeeze(Ldet(1,:))),  imag(squeeze(Ldet(1,:))), 'k.');
+plot(real(squeeze(Ldet(1,:))), -imag(squeeze(Ldet(1,:))), 'k.');
+for i = 1:6
+    plot(real(squeeze(Ldet(i,:))),  imag(squeeze(Ldet(i,:))), 'k.');
+    plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.');
+end
+plot(-1, 0, 'kx', 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
+xlabel('Real'); ylabel('Imag');
+xlim([-3, 1]); ylim([-2, 2]);
+#+end_src
+
+**** Estimated performances
+Loop gain with model
+#+begin_src matlab :exports none
+%% Loop Gain
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+for i = 1:6
+    plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i))*Khac(i,i), freqs, 'Hz'))), 'color', [colors(i,:), 0.5]);
+end
+for i = 1:5
+    for j = i+1:6
+        plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', j))*Khac(i,i), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
+    end
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
+ylim([1e-4, 1e4]);
+
+ax2 = nexttile;
+hold on;
+for i = 1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i))*Khac(i,i), freqs, 'Hz'))), 'color', [colors(i,:), 0.5]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab
+Gm_cl_m0 = feedback(Gm_hac_m0, Khac, 'name', +1);
+#+end_src
+
+#+begin_src matlab
+isstable(Gm_hac_m0)
+#+end_src
+
+**** Save Controller
+#+begin_src matlab :exports none :tangle no
+K_order = order(Khac(1,1));
+
+Kz = c2d(Khac(1,1)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
+[num, den] = tfdata(Kz, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_m0.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+** Tomography - Performances
+*** First scan with closed-loop at middle
+
+#+begin_src matlab
+data = load(sprintf('%s/scans/2023-08-17_15-22_tomography_30rpm_m0_robust.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none :results none
+writerObj = VideoWriter('test2.avi'); %// initialize the VideoWriter object
+open(writerObj) ;
+
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile();
+di = 500;
+hold on;
+for i = 1:floor(length(data.Dx_int)/di)-1
+    if data.hac_status(di*(i+1)-1) == 0
+        % Only open loop
+        plot(1e6*data.Dx_int(di*i:di*(i+1)-1), 1e6*data.Dy_int(di*i:di*(i+1)-1), '-', 'color', colors(1,:))
+    elseif data.hac_status(di*i) == 1
+        % Only closed loop
+        plot(1e6*data.Dx_int(di*i:di*(i+1)-1), 1e6*data.Dy_int(di*i:di*(i+1)-1), '-', 'color', colors(2,:))
+    else
+        % Both open and closed loop
+        Dx_int = data.Dx_int(di*i:di*(i+1)-1);
+        Dy_int = data.Dy_int(di*i:di*(i+1)-1);
+        plot(1e6*Dx_int(data.hac_status(di*i:di*(i+1)-1) == 0), 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 0), '-', 'color', colors(1,:))
+        plot(1e6*Dx_int(data.hac_status(di*i:di*(i+1)-1) == 1), 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 1), '-', 'color', colors(2,:))
+    end
+    axis square
+    xlim([-3, 3])
+    ylim([-3, 3])
+    xlabel("X motion [$\mu m$]");
+    ylabel("Y motion [$\mu m$]");
+    F = getframe;            %// Capture the frame
+    writeVideo(writerObj,F)  %// add the frame to the movie
+end
+close(writerObj);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% description
+figure;
+tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile;
+hold on;
+plot(1e6*data.Dx_int(data.hac_status == 0), 1e6*data.Dy_int(data.hac_status == 0), ...
+    'DisplayName', 'OL')
+plot(1e6*data.Dx_int(data.hac_status == 1), 1e6*data.Dy_int(data.hac_status == 1), ...
+    'DisplayName', 'CL')
+% Get first time where closed-loop ON
+[~, i] = find(data.hac_status == 1);
+plot(1e6*data.Dx_int(i+50000:end), 1e6*data.Dy_int(i+50000:end), ...
+    'DisplayName', 'CL, stabilized')
+hold off;
+axis equal
+xlim([-3, 3])
+ylim([-3, 3])
+xticks([-3:1:3])
+yticks([-3:1:3])
+xlabel("X motion [$\mu m$]");
+ylabel("Y motion [$\mu m$]");
+
+ax2 = nexttile;
+hold on;
+plot(1e6*data.Dx_int(data.hac_status == 0), 1e6*data.Dy_int(data.hac_status == 0), ...
+    'DisplayName', 'OL')
+plot(1e6*data.Dx_int(data.hac_status == 1), 1e6*data.Dy_int(data.hac_status == 1), ...
+    'DisplayName', 'CL')
+% Get first time where closed-loop ON
+[~, i] = find(data.hac_status == 1);
+plot(1e6*data.Dx_int(i+50000:end), 1e6*data.Dy_int(i+50000:end), ...
+    'DisplayName', 'CL, stabilized')
+legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+hold off;
+axis equal
+xlabel("X motion [$\mu m$]");
+ylabel("Y motion [$\mu m$]");
+xlim([-0.3, 0.3])
+ylim([-0.3, 0.3])
+xticks([-0.3:0.1:0.3])
+yticks([-0.3:0.1:0.3])
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_tomography_ol_cl_robust_hac_m0.pdf', 'width', 'full', 'height', 'normal');
+#+end_src
+
+#+name: fig:id31_tomography_ol_cl_robust_hac_m0
+#+caption: description
+#+RESULTS:
+[[file:figs/id31_tomography_ol_cl_robust_hac_m0.png]]
+
+
+*** Slow Rotation - 6RPM
+#+begin_src matlab
+%% Load measured noise
+data_ol  = load(sprintf('%s/freq_analysis/2023-08-11_17-18_m0_lac_off_1rpm.mat', mat_dir));
+data_lac = load(sprintf('%s/freq_analysis/2023-08-11_17-16_m0_lac_on_1rpm.mat', mat_dir));
+data_hac = load(sprintf('%s/freq_analysis/2023-08-11_17-14_m0_hac_on_1rpm.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab
+%% Coordinate transform
+J_int_to_X = [ 0                 0                -0.787401574803149 -0.212598425196851  0;
+               0.78740157480315  0.21259842519685  0                  0                  0;
+               0                 0                 0                  0                 -1;
+             -13.1233595800525  13.1233595800525   0                  0                  0;
+               0                 0               -13.1233595800525   13.1233595800525    0];
+
+a = J_int_to_X*[data_ol.d1; data_ol.d2; data_ol.d3; data_ol.d4; data_ol.d5];
+data_ol.Dx_int = a(1,:);
+data_ol.Dy_int = a(2,:);
+data_ol.Dz_int = a(3,:);
+data_ol.Rx_int = a(4,:);
+data_ol.Ry_int = a(5,:);
+
+a = J_int_to_X*[data_lac.d1; data_lac.d2; data_lac.d3; data_lac.d4; data_lac.d5];
+data_lac.Dx_int = a(1,:);
+data_lac.Dy_int = a(2,:);
+data_lac.Dz_int = a(3,:);
+data_lac.Rx_int = a(4,:);
+data_lac.Ry_int = a(5,:);
+
+a = J_int_to_X*[data_hac.d1; data_hac.d2; data_hac.d3; data_hac.d4; data_hac.d5];
+data_hac.Dx_int = a(1,:);
+data_hac.Dy_int = a(2,:);
+data_hac.Dz_int = a(3,:);
+data_hac.Rx_int = a(4,:);
+data_hac.Ry_int = a(5,:);
+#+end_src
+
+#+begin_src matlab :exports none
+% Hannning Windows
+Nfft = floor(30.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+
+[data_ol.pxx_Dx, data_ol.f] = pwelch(detrend(data_ol.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_ol.pxx_Dy, ~        ] = pwelch(detrend(data_ol.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_ol.pxx_Dz, ~        ] = pwelch(detrend(data_ol.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_ol.pxx_Rx, ~        ] = pwelch(detrend(data_ol.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_ol.pxx_Ry, ~        ] = pwelch(detrend(data_ol.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
+
+[data_lac.pxx_Dx, data_lac.f] = pwelch(detrend(data_lac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_lac.pxx_Dy, ~         ] = pwelch(detrend(data_lac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_lac.pxx_Dz, ~         ] = pwelch(detrend(data_lac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_lac.pxx_Rx, ~         ] = pwelch(detrend(data_lac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_lac.pxx_Ry, ~         ] = pwelch(detrend(data_lac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
+
+[data_hac.pxx_Dx, data_hac.f] = pwelch(detrend(data_hac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_hac.pxx_Dy, ~         ] = pwelch(detrend(data_hac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_hac.pxx_Dz, ~         ] = pwelch(detrend(data_hac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_hac.pxx_Rx, ~         ] = pwelch(detrend(data_hac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_hac.pxx_Ry, ~         ] = pwelch(detrend(data_hac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile();
+hold on;
+plot(data_ol.f , sqrt(data_ol.pxx_Dy ), 'DisplayName', '$D_y$ - OL');
+plot(data_lac.f, sqrt(data_lac.pxx_Dy), 'DisplayName', '$D_y$ - LAC');
+plot(data_hac.f, sqrt(data_hac.pxx_Dy), 'DisplayName', '$D_y$ - HAC-LAC');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Amplitude [$V/\sqrt{Hz}$]');
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+xlim([1, 5e3]);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion
+figure;
+hold on;
+plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dy)))), 'DisplayName', '$D_y$ - OL');
+plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dy)))), 'DisplayName', '$D_y$ - LAC');
+plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Dy)))), 'DisplayName', '$D_y$ - HAC-LAC');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('CAS [$m$]');
+legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+% xlim([0.1, 5e3]); ylim([1e-10, 1e-7]);
+#+end_src
+
+*** Rapid Rotation - 30RPM
+#+begin_src matlab
+%% Load measured noise
+data_ol  = load(sprintf('%s/freq_analysis/2023-08-11_17-39_m0_lac_off_30rpm.mat', mat_dir));
+data_lac = load(sprintf('%s/freq_analysis/2023-08-11_17-36_m0_lac_on_30rpm.mat', mat_dir));
+data_hac = load(sprintf('%s/freq_analysis/2023-08-11_17-34_m0_hac_on_30rpm.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab
+%% Coordinate transform
+J_int_to_X = [ 0                 0                -0.787401574803149 -0.212598425196851  0;
+               0.78740157480315  0.21259842519685  0                  0                  0;
+               0                 0                 0                  0                 -1;
+             -13.1233595800525  13.1233595800525   0                  0                  0;
+               0                 0               -13.1233595800525   13.1233595800525    0];
+
+a = J_int_to_X*[data_ol.d1; data_ol.d2; data_ol.d3; data_ol.d4; data_ol.d5];
+data_ol.Dx_int = a(1,:);
+data_ol.Dy_int = a(2,:);
+data_ol.Dz_int = a(3,:);
+data_ol.Rx_int = a(4,:);
+data_ol.Ry_int = a(5,:);
+
+a = J_int_to_X*[data_lac.d1; data_lac.d2; data_lac.d3; data_lac.d4; data_lac.d5];
+data_lac.Dx_int = a(1,:);
+data_lac.Dy_int = a(2,:);
+data_lac.Dz_int = a(3,:);
+data_lac.Rx_int = a(4,:);
+data_lac.Ry_int = a(5,:);
+
+a = J_int_to_X*[data_hac.d1; data_hac.d2; data_hac.d3; data_hac.d4; data_hac.d5];
+data_hac.Dx_int = a(1,:);
+data_hac.Dy_int = a(2,:);
+data_hac.Dz_int = a(3,:);
+data_hac.Rx_int = a(4,:);
+data_hac.Ry_int = a(5,:);
+#+end_src
+
+#+begin_src matlab :exports none
+% Hannning Windows
+Nfft = floor(20.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+
+[data_ol.pxx_Dx, data_ol.f] = pwelch(detrend(data_ol.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_ol.pxx_Dy, ~        ] = pwelch(detrend(data_ol.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_ol.pxx_Dz, ~        ] = pwelch(detrend(data_ol.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_ol.pxx_Rx, ~        ] = pwelch(detrend(data_ol.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_ol.pxx_Ry, ~        ] = pwelch(detrend(data_ol.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
+
+[data_lac.pxx_Dx, data_lac.f] = pwelch(detrend(data_lac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_lac.pxx_Dy, ~         ] = pwelch(detrend(data_lac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_lac.pxx_Dz, ~         ] = pwelch(detrend(data_lac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_lac.pxx_Rx, ~         ] = pwelch(detrend(data_lac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_lac.pxx_Ry, ~         ] = pwelch(detrend(data_lac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
+
+[data_hac.pxx_Dx, data_hac.f] = pwelch(detrend(data_hac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_hac.pxx_Dy, ~         ] = pwelch(detrend(data_hac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_hac.pxx_Dz, ~         ] = pwelch(detrend(data_hac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_hac.pxx_Rx, ~         ] = pwelch(detrend(data_hac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
+[data_hac.pxx_Ry, ~         ] = pwelch(detrend(data_hac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile();
+hold on;
+plot(data_ol.f , sqrt(data_ol.pxx_Dy ), 'DisplayName', '$D_y$ - OL');
+plot(data_lac.f, sqrt(data_lac.pxx_Dy), 'DisplayName', '$D_y$ - LAC');
+plot(data_hac.f, sqrt(data_hac.pxx_Dy), 'DisplayName', '$D_y$ - HAC-LAC');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Amplitude [$V/\sqrt{Hz}$]');
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+xlim([1, 5e3]);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion
+figure;
+tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile();
+hold on;
+plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dy)))),    ...
+     'DisplayName', sprintf('OL: $%.1f \\mu m$ RMS', 1e6*rms(detrend(data_ol.Dy_int, 0))));
+plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dy)))), ...
+     'DisplayName', sprintf('LAC: $%.1f \\mu m$ RMS', 1e6*rms(detrend(data_lac.Dy_int, 0))));
+plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Dy)))), ...
+     'DisplayName', sprintf('HAC: $%.0f nm$ RMS', 1e9*rms(detrend(data_hac.Dy_int, 0))));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('CAS [m rms, rad RMS]');
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+xticks([1e0, 1e1, 1e2]);
+title('$D_y$')
+
+ax2 = nexttile();
+hold on;
+plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dz)))),    ...
+     'DisplayName', sprintf('OL: $%.0f nm$ RMS', 1e9*rms(detrend(data_ol.Dz_int, 0))));
+plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dz)))), ...
+     'DisplayName', sprintf('LAC: $%.0f nm$ RMS', 1e9*rms(detrend(data_lac.Dz_int, 0))));
+plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Dz)))), ...
+     'DisplayName', sprintf('HAC: $%.0f nm$ RMS', 1e9*rms(detrend(data_hac.Dz_int, 0))));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+xticks([1e0, 1e1, 1e2]);
+title('$D_z$')
+
+ax3 = nexttile();
+hold on;
+plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Ry)))),    ...
+     'DisplayName', sprintf('OL: $%.1f \\mu$radRMS', 1e6*rms(detrend(data_ol.Ry_int, 0))));
+plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Ry)))), ...
+     'DisplayName', sprintf('LAC: $%.1f \\mu$radRMS', 1e6*rms(detrend(data_lac.Ry_int, 0))));
+plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Ry)))), ...
+     'DisplayName', sprintf('HAC: $%.1f \\mu$radRMS', 1e6*rms(detrend(data_hac.Ry_int, 0))));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+xticks([1e0, 1e1, 1e2]);
+title('$R_y$')
+
+linkaxes([ax1,ax2,ax3],'xy');
+xlim([0.1, 5e2]); ylim([1e-10, 3e-5]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_cas_m0_30rpm_ol_lac_hac.pdf', 'width', 'full', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_cas_m0_30rpm_ol_lac_hac
+#+caption: Cumulative Amplitude Spectrum of the errors in $D_y$, $D_z$ and $R_y$ during a tomography scan at 30RPM. Three control configuration are compared: Open-Loop, Low Authority Control, and High Authority Control
+#+RESULTS:
+[[file:figs/id31_cas_m0_30rpm_ol_lac_hac.png]]
+
+
+* 6DoF Control in Cartesian plane (rotating with the nano-hexapod)  :noexport:
+<<sec:id31_cart_hac_rot>>
+** Matlab Init                                              :noexport:ignore:
+#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
+<<matlab-dir>>
+#+end_src
+
+#+begin_src matlab :exports none :results silent :noweb yes
+<<matlab-init>>
+#+end_src
+
+#+begin_src matlab :tangle no :noweb yes
+<<m-init-path>>
+#+end_src
+
+#+begin_src matlab :eval no :noweb yes
+<<m-init-path-tangle>>
+#+end_src
+
+#+begin_src matlab :noweb yes
+<<m-init-other>>
+#+end_src
+
+** Introduction                                                      :ignore:
+As only Dy, Dz and Ry directions are important, we could only control them.
+This lead to a 3x3 plant that may be more decoupled than the 6x6 plant.
+
+** 5x5 plant in Cartesian plane
+#+begin_src matlab :exports none
+%% Load model
+load('Gm.mat')
+#+end_src
+
+#+begin_src matlab
+%% Jacobian for 3x3 plant
+n_hexapod = initializeNanoHexapod('flex_bot_type', '2dof', ...
+                                  'flex_top_type', '3dof', ...
+                                  'motion_sensor_type', 'plates', ...
+                                  'actuator_type', '2dof');
+
+Jt_inv = pinv(n_hexapod.geometry.J');
+Jt_inv = Jt_inv(:,[1,2,3,4,5]);
+
+J_int_to_X = [ 0                 0                -0.787401574803149 -0.212598425196851  0;
+               0.78740157480315  0.21259842519685  0                  0                  0;
+               0                 0                 0                  0                 -1;
+             -13.1233595800525  13.1233595800525   0                  0                  0;
+               0                 0               -13.1233595800525   13.1233595800525    0];
+#+end_src
+
+Compute identified plant in the Cartesian plane:
+#+begin_src matlab
+%% Load Data
+% data_m0 = load(sprintf('%s/dynamics/2023-08-17_10-44_lac_plant_m0_Wz0_interf.mat', mat_dir));
+%% New data after calibrating the Rz-offset
+data_m0 = load(sprintf('%s/dynamics/2023-08-17_16-23_lac_plant_m0_Wz0_interf_Rz_align.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+% Sampling Time [s]
+Ts = 1e-4;
+
+% Hannning Windows
+Nfft = floor(1.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+#+end_src
+
+#+begin_src matlab :exports none
+% And we get the frequency vector
+[~, f] = tfestimate(data_m0.uL1.id_plant, data_m0.uL1.d1, win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+#+begin_src matlab :exports none
+%% HAC Cartesian Plant
+G_hac_m0 = zeros(length(f), 5, 6);
+
+for i_strut = 1:6
+    Di = [data_m0.(sprintf("uL%i", i_strut)).d1 ; data_m0.(sprintf("uL%i", i_strut)).d2 ; data_m0.(sprintf("uL%i", i_strut)).d3 ; data_m0.(sprintf("uL%i", i_strut)).d4 ; data_m0.(sprintf("uL%i", i_strut)).d5]';
+
+    G_hac_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, detrend(Di, 0), win, Noverlap, Nfft, 1/Ts);
+end
+
+G_cart = permute(pagemtimes(J_int_to_X, pagemtimes(permute(G_hac_m0, [2,3,1]), Jt_inv)), [3,1,2]);
+#+end_src
+
+Compute plant model in the Cartesian plane:
+#+begin_src matlab
+Gm_cart = J_int_to_X*Gm_hac_m0({'d1', 'd2', 'd3', 'd4', 'd5'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'})*Jt_inv;
+Gm_cart.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My'};
+Gm_cart.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry'};
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% 5x5 plant
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_cart(:,1,1)), 'DisplayName', '$D_x$');
+plot(f, abs(G_cart(:,2,2)), 'DisplayName', '$D_y$');
+plot(f, abs(G_cart(:,3,3)), 'DisplayName', '$D_z$');
+plot(f, abs(G_cart(:,4,4)), 'DisplayName', '$R_x$');
+plot(f, abs(G_cart(:,5,5)), 'DisplayName', '$R_y$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+% ylim([1e-4, 1e1]);
+
+ax2 = nexttile();
+hold on;
+plot(f, 180/pi*angle(G_cart(:,1,1)));
+plot(f, 180/pi*angle(G_cart(:,2,2)));
+plot(f, 180/pi*angle(G_cart(:,3,3)));
+plot(f, 180/pi*angle(G_cart(:,4,4)));
+plot(f, 180/pi*angle(G_cart(:,5,5)));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+** Controller Design
+#+begin_src matlab
+%% Wanted crossover
+wc = 2*pi*30;
+
+%% Double Integrator
+H_int = 1/(s + 1*2*pi) * ...
+        1/(s + 0.01*2*pi);
+H_int = H_int/abs(evalfr(H_int, 1j*wc));
+
+%% Lead to increase phase margin
+a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
+H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
+a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
+H_lead = H_lead*1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a)));
+
+%% Low Pass filter to increase robustness
+H_lpf = 1/(1 + s/2/pi/300);
+
+%% Notch at the top-plate resonance
+% gm = 0.02;
+% xi = 0.3;
+% wn = 2*pi*665;
+
+% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
+
+%% Gain to have unitary crossover at 30Hz
+[~, i_f] = min(abs(f - wc/2/pi));
+H_gain = -1./abs(G_cart(i_f,1,1));
+
+%% Decentralized HAC
+Khac_Dx = H_gain  * ... % Gain
+          H_int   * ... % Integrator
+          H_lpf   * ... % Low Pass Filter
+          H_lead;       % Integrator
+#+end_src
+
+#+begin_src matlab
+%% Wanted crossover
+wc = 2*pi*30;
+
+%% Double Integrator
+H_int = 1/(s + 1*2*pi) * ...
+        1/(s + 0.01*2*pi);
+H_int = H_int/abs(evalfr(H_int, 1j*wc));
+
+%% Lead to increase phase margin
+a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
+H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
+a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
+H_lead = H_lead*1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a)));
+
+%% Low Pass filter to increase robustness
+H_lpf = 1/(1 + s/2/pi/300);
+
+%% Notch at the top-plate resonance
+% gm = 0.02;
+% xi = 0.3;
+% wn = 2*pi*665;
+
+% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
+
+%% Gain to have unitary crossover at 30Hz
+[~, i_f] = min(abs(f - wc/2/pi));
+H_gain = -1./abs(G_cart(i_f,2,2));
+
+%% Decentralized HAC
+Khac_Dy = H_gain  * ... % Gain
+          H_int   * ... % Integrator
+          H_lpf   * ... % Low Pass Filter
+          H_lead;       % Integrator
+#+end_src
+
+#+begin_src matlab
+%% Wanted crossover
+wc = 2*pi*40;
+
+%% Double Integrator
+H_int = 1/(s + 2*2*pi) * ...
+        1/(s + 0.01*2*pi);
+H_int = H_int/abs(evalfr(H_int, 1j*wc));
+
+%% Lead to increase phase margin
+a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
+H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
+a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
+H_lead = H_lead*1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a)));
+
+%% Low Pass filter to increase robustness
+H_lpf = 1/(1 + s/2/pi/300);
+
+%% Notch at the top-plate resonance
+% gm = 0.02;
+% xi = 0.3;
+% wn = 2*pi*665;
+
+% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
+
+%% Gain to have unitary crossover at 30Hz
+[~, i_f] = min(abs(f - wc/2/pi));
+H_gain = -1./abs(G_cart(i_f,3,3));
+
+%% Decentralized HAC
+Khac_Dz = H_gain  * ... % Gain
+          H_int   * ... % Integrator
+          H_lpf   * ... % Low Pass Filter
+          H_lead;       % Integrator
+#+end_src
+
+#+begin_src matlab
+%% Wanted crossover
+wc = 2*pi*10;
+
+%% Double Integrator
+H_int = 1/(s + 1.5*2*pi) * ...
+        1/(s + 0.01*2*pi);
+H_int = H_int/abs(evalfr(H_int, 1j*wc));
+
+%% Lead to increase phase margin
+a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
+H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
+
+%% Low Pass filter to increase robustness
+H_lpf = 1/(1 + s/2/pi/300);
+
+%% Notch at the top-plate resonance
+% gm = 0.02;
+% xi = 0.3;
+% wn = 2*pi*665;
+
+% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
+
+%% Gain to have unitary crossover at 30Hz
+[~, i_f] = min(abs(f - wc/2/pi));
+H_gain = -1./abs(G_cart(i_f,4,4));
+
+%% Decentralized HAC
+Khac_Rx = H_gain  * ... % Gain
+          H_int   * ... % Integrator
+          H_lpf   * ... % Low Pass Filter
+          H_lead;       % Integrator
+#+end_src
+
+#+begin_src matlab
+%% Wanted crossover
+wc = 2*pi*10;
+
+%% Double Integrator
+H_int = 1/(s + 1.5*2*pi) * ...
+        1/(s + 0.01*2*pi);
+H_int = H_int/abs(evalfr(H_int, 1j*wc));
+
+%% Lead to increase phase margin
+a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
+H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
+
+%% Low Pass filter to increase robustness
+H_lpf = 1/(1 + s/2/pi/300);
+
+%% Notch at the top-plate resonance
+% gm = 0.02;
+% xi = 0.3;
+% wn = 2*pi*665;
+
+% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
+
+%% Gain to have unitary crossover at 30Hz
+[~, i_f] = min(abs(f - wc/2/pi));
+H_gain = -1./abs(G_cart(i_f,5,5));
+
+%% Decentralized HAC
+Khac_Ry = H_gain  * ... % Gain
+          H_int   * ... % Integrator
+          H_lpf   * ... % Low Pass Filter
+          H_lead;       % Integrator
+#+end_src
+
+#+begin_src matlab
+Khac = blkdiag(Khac_Dx, Khac_Dy, Khac_Dz, Khac_Rx, Khac_Ry);
+#+end_src
+
+#+begin_src matlab :exports none
+%% Loop Gain
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_cart(:,1,1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))));
+plot(f, abs(G_cart(:,2,2).*squeeze(freqresp(Khac(2,2), f, 'Hz'))));
+plot(f, abs(G_cart(:,3,3).*squeeze(freqresp(Khac(3,3), f, 'Hz'))));
+plot(f, abs(G_cart(:,4,4).*squeeze(freqresp(Khac(4,4), f, 'Hz'))));
+plot(f, abs(G_cart(:,5,5).*squeeze(freqresp(Khac(5,5), f, 'Hz'))));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
+ylim([1e-4, 1e4]);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(G_cart(:,1,1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))));
+plot(f, 180/pi*angle(G_cart(:,2,2).*squeeze(freqresp(Khac(2,2), f, 'Hz'))));
+plot(f, 180/pi*angle(G_cart(:,3,3).*squeeze(freqresp(Khac(3,3), f, 'Hz'))));
+plot(f, 180/pi*angle(G_cart(:,4,4).*squeeze(freqresp(Khac(4,4), f, 'Hz'))));
+plot(f, 180/pi*angle(G_cart(:,5,5).*squeeze(freqresp(Khac(5,5), f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+** Check Stability
+#+begin_src matlab :exports none
+%% Compute the Eigenvalues of the loop gain
+Ldet = zeros(5, length(f));
+
+% Loop gain
+Lmimo = pagemtimes(permute(G_cart, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
+for i_f = 2:length(f)
+    Ldet(:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% Plot of the eigenvalues of L in the complex plane
+figure;
+hold on;
+plot(real(squeeze(Ldet(1,:))), imag(squeeze(Ldet(1,:))), 'k.')
+plot(real(squeeze(Ldet(1,:))), -imag(squeeze(Ldet(1,:))), 'k.')
+for i = 1:size(Ldet,1)
+    plot(real(squeeze(Ldet(i,:))), imag(squeeze(Ldet(i,:))), 'k.')
+    plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.')
+end
+plot(-1, 0, 'kx');
+hold off;
+set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
+xlabel('Real'); ylabel('Imag');
+xlim([-3, 1]); ylim([-2, 2]);
+#+end_src
+
+** Save controllers
+#+begin_src matlab :exports none :tangle no
+K_order = order(Khac(1,1));
+
+Kz = c2d(-Khac(1,1)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
+[num, den] = tfdata(Kz, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_Dx.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+#+begin_src matlab :exports none :tangle no
+K_order = order(Khac(2,2));
+
+Kz = c2d(-Khac(2,2)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
+[num, den] = tfdata(Kz, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_Dy.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+#+begin_src matlab :exports none :tangle no
+K_order = order(Khac(3,3));
+
+Kz = c2d(-Khac(3,3)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
+[num, den] = tfdata(Kz, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_Dz.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+#+begin_src matlab :exports none :tangle no
+K_order = order(Khac(4,4));
+
+Kz = c2d(-Khac(4,4)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
+[num, den] = tfdata(Kz, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_Rx.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+#+begin_src matlab :exports none :tangle no
+K_order = order(Khac(5,5));
+
+Kz = c2d(-Khac(5,5)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
+[num, den] = tfdata(Kz, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_Ry.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+** Performances
+2023-08-18_18-33_m0_1rpm_K_cart.mat
+
+* 3DoF Control in Cartesian plane (fixed)                           :noexport:
+<<sec:id31_cart_hac_fix>>
+** Matlab Init                                              :noexport:ignore:
+#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
+<<matlab-dir>>
+#+end_src
+
+#+begin_src matlab :exports none :results silent :noweb yes
+<<matlab-init>>
+#+end_src
+
+#+begin_src matlab :tangle no :noweb yes
+<<m-init-path>>
+#+end_src
+
+#+begin_src matlab :eval no :noweb yes
+<<m-init-path-tangle>>
+#+end_src
+
+#+begin_src matlab :noweb yes
+<<m-init-other>>
+#+end_src
+
+** Introduction                                                      :ignore:
+As only Dy, Dz and Ry directions are important, we could only control them.
+This lead to a 3x3 plant that may be more decoupled than the 6x6 plant.
+
+** 3x3 plant in Cartesian plane
+#+begin_src matlab :exports none
+%% Load model
+load('Gm.mat')
+#+end_src
+
+#+begin_src matlab
+%% Jacobian for 3x3 plant
+n_hexapod = initializeNanoHexapod('flex_bot_type', '2dof', ...
+                                  'flex_top_type', '3dof', ...
+                                  'motion_sensor_type', 'plates', ...
+                                  'actuator_type', '2dof');
+
+Jt_inv = pinv(n_hexapod.geometry.J');
+
+J_int_to_X = [0.78740157480315  0.21259842519685  0                  0                  0;
+              0                 0                 0                  0                 -1;
+              0                 0               -13.1233595800525   13.1233595800525    0];
+#+end_src
+
+Compute identified plant in the Cartesian plane:
+#+begin_src matlab
+%% Load Data
+% data_m0 = load(sprintf('%s/dynamics/2023-08-17_10-44_lac_plant_m0_Wz0_interf.mat', mat_dir));
+%% New data after calibrating the Rz-offset
+data_m0 = load(sprintf('%s/dynamics/2023-08-17_16-23_lac_plant_m0_Wz0_interf_Rz_align.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+% Sampling Time [s]
+Ts = 1e-4;
+
+% Hannning Windows
+Nfft = floor(1.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+#+end_src
+
+#+begin_src matlab :exports none
+% And we get the frequency vector
+[~, f] = tfestimate(data_m0.uL1.id_plant, data_m0.uL1.d1, win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+#+begin_src matlab :exports none
+%% HAC Cartesian Plant
+G_hac_m0 = zeros(length(f), 5, 6);
+
+for i_strut = 1:6
+    Di = [data_m0.(sprintf("uL%i", i_strut)).d1 ; data_m0.(sprintf("uL%i", i_strut)).d2 ; data_m0.(sprintf("uL%i", i_strut)).d3 ; data_m0.(sprintf("uL%i", i_strut)).d4 ; data_m0.(sprintf("uL%i", i_strut)).d5]';
+
+    G_hac_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, detrend(Di, 0), win, Noverlap, Nfft, 1/Ts);
+end
+
+G_cart = permute(pagemtimes(J_int_to_X, pagemtimes(permute(G_hac_m0, [2,3,1]), Jt_inv(:,[2,3,5]))), [3,1,2]);
+#+end_src
+
+Compute plant model in the Cartesian plane:
+#+begin_src matlab
+Gm_cart = J_int_to_X*Gm_hac_m0({'d1', 'd2', 'd3', 'd4', 'd5'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'})*Jt_inv(:,[2,3,5]);
+Gm_cart.InputName = {'Fy', 'Fz', 'My'};
+Gm_cart.OutputName = {'Dy', 'Dz', 'Ry'};
+#+end_src
+
+#+begin_important
+Diagonal elements are matching quite well, but off-diagonal elements are very different.
+
+Why so much more coupling than from the model?
+- Is it due to the metrology? The spheres could induce coupling as for instance X motion will also be seen as Z motion.
+  This is especially true if not well centered with the sphere (as seemed to be the case for the lateral interferometers).
+#+end_important
+
+#+begin_src matlab :exports none :results none
+%% 3x3 cartesian plant
+figure;
+tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax11 = nexttile();
+hold on;
+plot(f, abs(G_cart(:,1,1)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'Fy'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('$D_y$'); set(gca, 'XTickLabel',[]);
+title('$F_y$')
+
+ax12 = nexttile();
+hold on;
+plot(f, abs(G_cart(:,1,2)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'Fz'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+title('$F_z$')
+
+ax13 = nexttile();
+hold on;
+plot(f, abs(G_cart(:,1,3)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'My'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+title('$M_y$')
+
+linkaxes([ax11,ax12,ax13],'y');
+ylim([1e-8, 2e-4]);
+
+ax21 = nexttile();
+hold on;
+plot(f, abs(G_cart(:,2,1)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'Fy'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('$F_z$'); set(gca, 'XTickLabel',[]);
+
+ax22 = nexttile();
+hold on;
+plot(f, abs(G_cart(:,2,2)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'Fz'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+
+ax23 = nexttile();
+hold on;
+plot(f, abs(G_cart(:,2,3)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'My'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+
+linkaxes([ax21,ax22,ax23],'y');
+ylim([1e-8, 2e-4]);
+
+ax31 = nexttile();
+hold on;
+plot(f, abs(G_cart(:,3,1)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'Fy'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('$M_y$');
+
+ax32 = nexttile();
+hold on;
+plot(f, abs(G_cart(:,3,2)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'Fz'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+
+ax33 = nexttile();
+hold on;
+plot(f, abs(G_cart(:,3,3)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'My'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+
+linkaxes([ax31,ax32,ax33],'y');
+ylim([1e-9, 1e-2]);
+
+linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x');
+xlim([1, 5e2]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_cart_plant_3x3.pdf', 'width', 'full', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_cart_plant_3x3
+#+caption: 3x3 cartesian plant
+#+RESULTS:
+[[file:figs/id31_cart_plant_3x3.png]]
+
+
+Normalization of outputs:
+#+begin_src matlab
+Gm_cart_normalized = -diag(1./diag(dcgain(Gm_cart)))*Gm_cart;
+Gm_cart_normalized.InputName = {'Fy', 'Fz', 'My'};
+Gm_cart_normalized.OutputName = {'Dy', 'Dz', 'Ry'};
+
+G_cart_normalized = permute(pagemtimes(-diag(1./diag(dcgain(Gm_cart))), permute(G_cart, [2,3,1])), [3,1,2]);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% 3x3 plant
+figure;
+tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax11 = nexttile();
+hold on;
+plot(f, abs(G_cart_normalized(:,1,1)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Dy', 'Fy'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('$D_y$'); set(gca, 'XTickLabel',[]);
+title('$F_y$')
+
+ax12 = nexttile();
+hold on;
+plot(f, abs(G_cart_normalized(:,1,2)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Dy', 'Fz'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+title('$F_z$')
+
+ax13 = nexttile();
+hold on;
+plot(f, abs(G_cart_normalized(:,1,3)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Dy', 'My'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+title('$M_y$')
+
+linkaxes([ax11,ax12,ax13],'y');
+ylim([1e-4, 1e1]);
+
+ax21 = nexttile();
+hold on;
+plot(f, abs(G_cart_normalized(:,2,1)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Dz', 'Fy'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('$F_z$'); set(gca, 'XTickLabel',[]);
+
+ax22 = nexttile();
+hold on;
+plot(f, abs(G_cart_normalized(:,2,2)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Dz', 'Fz'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+
+ax23 = nexttile();
+hold on;
+plot(f, abs(G_cart_normalized(:,2,3)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Dz', 'My'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+
+linkaxes([ax21,ax22,ax23],'y');
+ylim([1e-4, 1e1]);
+
+ax31 = nexttile();
+hold on;
+plot(f, abs(G_cart_normalized(:,3,1)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Ry', 'Fy'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('$M_y$');
+
+ax32 = nexttile();
+hold on;
+plot(f, abs(G_cart_normalized(:,3,2)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Ry', 'Fz'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+
+ax33 = nexttile();
+hold on;
+plot(f, abs(G_cart_normalized(:,3,3)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Ry', 'My'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+
+linkaxes([ax31,ax32,ax33],'y');
+ylim([1e-4, 1e1]);
+
+linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x');
+xlim([1, 5e2]);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% 3x3 plant
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_cart_normalized(:,1,1)));
+plot(f, abs(G_cart_normalized(:,2,2)));
+plot(f, abs(G_cart_normalized(:,3,3)));
+plot(f, abs(G_cart_normalized(:,1,2)), 'color', [0,0,0,0.2]);
+plot(f, abs(G_cart_normalized(:,1,3)), 'color', [0,0,0,0.2]);
+plot(f, abs(G_cart_normalized(:,2,1)), 'color', [0,0,0,0.2]);
+plot(f, abs(G_cart_normalized(:,3,1)), 'color', [0,0,0,0.2]);
+plot(f, abs(G_cart_normalized(:,2,3)), 'color', [0,0,0,0.2]);
+plot(f, abs(G_cart_normalized(:,3,2)), 'color', [0,0,0,0.2]);
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+ylim([1e-4, 1e1]);
+
+ax2 = nexttile();
+hold on;
+plot(f, 180/pi*angle(G_cart_normalized(:,1,1)));
+plot(f, 180/pi*angle(G_cart_normalized(:,2,2)));
+plot(f, 180/pi*angle(G_cart_normalized(:,3,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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+** Controller Design
+*** Dy
+#+begin_src matlab
+%% Wanted crossover
+wc = 2*pi*30;
+
+%% Double Integrator
+H_int = 1/(s + 0.1*2*pi) * ...
+        1/(s + 0.01*2*pi);
+H_int = H_int/abs(evalfr(H_int, 1j*wc));
+
+%% Lead to increase phase margin
+a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
+H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
+a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
+H_lead = H_lead*1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a)));
+
+%% Low Pass filter to increase robustness
+H_lpf = 1/(1 + s/2/pi/300);
+
+%% Notch at the top-plate resonance
+% gm = 0.02;
+% xi = 0.3;
+% wn = 2*pi*665;
+
+% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
+
+%% Gain to have unitary crossover at 30Hz
+[~, i_f] = min(abs(f - wc/2/pi));
+H_gain = -1./abs(G_cart(i_f,1,1));
+
+%% Decentralized HAC
+Khac_Dy = H_gain  * ... % Gain
+          H_int   * ... % Integrator
+          H_lpf   * ... % Low Pass Filter
+          H_lead;       % Integrator
+#+end_src
+
+#+begin_src matlab :exports none
+%% Loop Gain
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_cart(:,1,1).*squeeze(freqresp(Khac_Dy, f, 'Hz'))));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
+ylim([1e-4, 1e4]);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(G_cart(:,1,1).*squeeze(freqresp(Khac_Dy, f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+*** Dz
+#+begin_src matlab
+%% Wanted crossover
+wc = 2*pi*50;
+
+%% Double Integrator
+H_int = 1/(s + 0.1*2*pi) * ...
+        1/(s + 0.01*2*pi);
+H_int = H_int/abs(evalfr(H_int, 1j*wc));
+
+%% Lead to increase phase margin
+a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
+H_lead = 1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a)));
+a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
+H_lead = H_lead*1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a)));
+
+%% Low Pass filter to increase robustness
+H_lpf = 1/(1 + s/2/pi/300);
+
+%% Notch at the top-plate resonance
+% gm = 0.02;
+% xi = 0.3;
+% wn = 2*pi*665;
+
+% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
+
+%% Gain to have unitary crossover at 30Hz
+[~, i_f] = min(abs(f - wc/2/pi));
+H_gain = -1./abs(G_cart(i_f,2,2));
+
+%% Decentralized HAC
+Khac_Dz = H_gain  * ... % Gain
+          H_int   * ... % Integrator
+          H_lpf   * ... % Low Pass Filter
+          H_lead;       % Integrator
+#+end_src
+
+#+begin_src matlab :exports none
+%% Loop Gain
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_cart(:,2,2).*squeeze(freqresp(Khac_Dz, f, 'Hz'))));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
+ylim([1e-4, 1e4]);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(G_cart(:,2,2).*squeeze(freqresp(Khac_Dz, f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+*** Ry
+#+begin_src matlab
+%% Wanted crossover
+wc = 2*pi*30;
+
+%% Double Integrator
+H_int = 1/(s + 0.1*2*pi) * ...
+        1/(s + 0.01*2*pi);
+H_int = H_int/abs(evalfr(H_int, 1j*wc));
+
+%% Lead to increase phase margin
+a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
+H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
+a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
+H_lead = H_lead*1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a)));
+
+%% Low Pass filter to increase robustness
+H_lpf = 1/(1 + s/2/pi/300);
+
+%% Notch at the top-plate resonance
+% gm = 0.02;
+% xi = 0.3;
+% wn = 2*pi*665;
+
+% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
+
+%% Gain to have unitary crossover at 30Hz
+[~, i_f] = min(abs(f - wc/2/pi));
+H_gain = -1./abs(G_cart(i_f,3,3));
+
+%% Decentralized HAC
+Khac_Ry = H_gain  * ... % Gain
+          H_int   * ... % Integrator
+          H_lpf   * ... % Low Pass Filter
+          H_lead;       % Integrator
+#+end_src
+
+#+begin_src matlab :exports none
+%% Loop Gain
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_cart(:,3,3).*squeeze(freqresp(Khac_Ry, f, 'Hz'))));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
+ylim([1e-4, 1e4]);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(G_cart(:,3,3).*squeeze(freqresp(Khac_Ry, f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+*** 3x3 controller
+#+begin_src matlab
+Khac = blkdiag(Khac_Dy, Khac_Dz, Khac_Ry);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% description
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_cart(:,1,1).*squeeze(freqresp(Khac_Dy, f, 'Hz'))), 'DisplayName', '$D_y$');
+plot(f, abs(G_cart(:,2,2).*squeeze(freqresp(Khac_Dz, f, 'Hz'))), 'DisplayName', '$D_z$');
+plot(f, abs(G_cart(:,3,3).*squeeze(freqresp(Khac_Ry, f, 'Hz'))), 'DisplayName', '$R_y$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
+ylim([1e-4, 1e4]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(G_cart(:,1,1).*squeeze(freqresp(Khac_Dy, f, 'Hz'))));
+plot(f, 180/pi*angle(G_cart(:,2,2).*squeeze(freqresp(Khac_Dz, f, 'Hz'))));
+plot(f, 180/pi*angle(G_cart(:,3,3).*squeeze(freqresp(Khac_Ry, f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/G_cart_loop_gain_diagonal_3dof.pdf', 'width', 'full', 'height', 'tall');
+#+end_src
+
+#+name: fig:G_cart_loop_gain_diagonal_3dof
+#+caption: description
+#+RESULTS:
+[[file:figs/G_cart_loop_gain_diagonal_3dof.png]]
+
+
+** Check Stability
+#+begin_src matlab :exports none
+%% Compute the Eigenvalues of the loop gain
+Ldet = zeros(3, length(f));
+
+% Loop gain
+Lmimo = pagemtimes(permute(G_cart, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
+for i_f = 2:length(f)
+    Ldet(:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
+end
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% description
+figure;
+hold on;
+plot(real(squeeze(Ldet(1,:))), imag(squeeze(Ldet(1,:))), 'k.')
+plot(real(squeeze(Ldet(1,:))), -imag(squeeze(Ldet(1,:))), 'k.')
+for i = 1:3
+    plot(real(squeeze(Ldet(i,:))), imag(squeeze(Ldet(i,:))), 'k.')
+    plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.')
+end
+plot(-1, 0, 'kx');
+hold off;
+set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
+xlabel('Real'); ylabel('Imag');
+xlim([-3, 1]); ylim([-2, 2]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/G_cart_nyquist_diagonal_3dof.pdf', 'width', 'wide', 'height', 'tall');
+#+end_src
+
+#+name: fig:G_cart_nyquist_diagonal_3dof
+#+caption: description
+#+RESULTS:
+[[file:figs/G_cart_nyquist_diagonal_3dof.png]]
+
+** Save controllers
+*** Save Controller
+#+begin_src matlab :exports none :tangle no
+K_order = order(Khac(1,1));
+
+Kz = c2d(-Khac(1,1)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
+[num, den] = tfdata(Kz, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_Dy_3x3.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+#+begin_src matlab :exports none :tangle no
+K_order = order(Khac(2,2));
+
+Kz = c2d(-Khac(2,2)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
+[num, den] = tfdata(Kz, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_Dz_3x3.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+#+begin_src matlab :exports none :tangle no
+K_order = order(Khac(3,3));
+
+Kz = c2d(-Khac(3,3)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
+[num, den] = tfdata(Kz, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_Ry_3x3.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+** Controller Design (normalized)
+#+begin_src matlab
+%% Wanted crossover
+wc = 2*pi*30;
+
+%% Double Integrator
+H_int = 1/(s + 0.1*2*pi) * ...
+        (50*2*pi + s)/(0.01*2*pi + s);
+H_int = H_int/abs(evalfr(H_int, 1j*wc));
+% H_int = wc/s;
+
+%% Lead to increase phase margin
+a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
+H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
+% a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
+% H_lead = H_lead*1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
+
+%% Low Pass filter to increase robustness
+H_lpf = 1/(1 + s/2/pi/200);
+
+%% Notch at the top-plate resonance
+% gm = 0.02;
+% xi = 0.3;
+% wn = 2*pi*665;
+
+% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
+
+%% Gain to have unitary crossover at 30Hz
+[~, i_f] = min(abs(f - wc/2/pi));
+H_gain = 1./abs(G_cart_normalized(i_f, 1, 1));
+
+%% Decentralized HAC
+Khac = H_gain  * ... % Gain
+       H_int   * ... % Integrator
+       H_lpf   * ... % Low Pass Filter
+       H_lead  * ... % Integrator
+       eye(3);            % 6x6 Diagonal
+
+% Khac.InputName = {'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
+% Khac.OutputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
+#+end_src
+
+#+begin_src matlab :exports none
+%% Loop Gain
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+for i = 1:3
+    plot(f, abs(G_cart_normalized(:,i,i).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [colors(i,:), 0.5]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
+ylim([1e-4, 1e4]);
+
+ax2 = nexttile;
+hold on;
+for i = 1:3
+    plot(f, 180/pi*angle(G_cart_normalized(:,i,i).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [colors(i,:), 0.5]);
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+** Verify Stability
+#+begin_src matlab :exports none
+%% Compute the Eigenvalues of the loop gain
+Ldet = zeros(3, length(f));
+
+% Loop gain
+Lmimo = pagemtimes(permute(G_cart_normalized, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
+for i_f = 2:length(f)
+    Ldet(:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% Plot of the eigenvalues of L in the complex plane
+figure;
+hold on;
+plot(real(squeeze(Ldet(1,:))),  imag(squeeze(Ldet(1,:))), 'k.');
+plot(real(squeeze(Ldet(1,:))), -imag(squeeze(Ldet(1,:))), 'k.');
+for i = 1:3
+    plot(real(squeeze(Ldet(i,:))),  imag(squeeze(Ldet(i,:))), 'k.');
+    plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.');
+end
+plot(-1, 0, 'kx', 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
+xlabel('Real'); ylabel('Imag');
+xlim([-3, 1]); ylim([-2, 2]);
+#+end_src
+
+
+** Control Performances
+#+begin_src matlab
+data_cl = load(sprintf('%s/dynamics/2023-08-21_10-36_m0_cart_fixed.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+% Sampling Time [s]
+Ts = 1e-4;
+
+% Hannning Windows
+Nfft = floor(1.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+#+end_src
+
+#+begin_src matlab :exports none
+% And we get the frequency vector
+[S_dx, f] = tfestimate(data_cl.Dx.id_cl, data_cl.Dx.e_dx, win, Noverlap, Nfft, 1/Ts);
+[S_dy, ~] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
+[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
+[S_rx, ~] = tfestimate(data_cl.Rx.id_cl, data_cl.Rx.e_rx, win, Noverlap, Nfft, 1/Ts);
+[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
+[S_rz, ~] = tfestimate(data_cl.Rz.id_cl, data_cl.Rz.e_rz, win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+- [ ] Compare with estimated performances
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile();
+hold on;
+plot(f, abs(S_dy), 'DisplayName', '$D_y$');
+plot(f, abs(S_dz), 'DisplayName', '$D_z$');
+plot(f, abs(S_ry), 'DisplayName', '$R_y$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Amplitude');
+ylim([1e-3, 1e1]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
+xlim([1, 1e3]);
+#+end_src
+
+* Complementary Filter Control                                      :noexport:
+<<sec:id31_cart_hac_complementary_filter>>
+** Matlab Init                                              :noexport:ignore:
+#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
+<<matlab-dir>>
+#+end_src
+
+#+begin_src matlab :exports none :results silent :noweb yes
+<<matlab-init>>
+#+end_src
+
+#+begin_src matlab :tangle no :noweb yes
+<<m-init-path>>
+#+end_src
+
+#+begin_src matlab :eval no :noweb yes
+<<m-init-path-tangle>>
+#+end_src
+
+#+begin_src matlab :noweb yes
+<<m-init-other>>
+#+end_src
+
+** m0
+*** 3x3 plant in Cartesian plane
+#+begin_src matlab :exports none
+%% Load model
+load('Gm.mat')
+#+end_src
+
+#+begin_src matlab
+%% Jacobian for 3x3 plant
+n_hexapod = initializeNanoHexapod('flex_bot_type', '2dof', ...
+                                  'flex_top_type', '3dof', ...
+                                  'motion_sensor_type', 'plates', ...
+                                  'actuator_type', '2dof');
+
+Jt_inv = pinv(n_hexapod.geometry.J');
+
+J_int_to_X = [0.78740157480315  0.21259842519685  0                  0                  0;
+              0                 0                 0                  0                 -1;
+              0                 0               -13.1233595800525   13.1233595800525    0];
+#+end_src
+
+Compute identified plant in the Cartesian plane:
+#+begin_src matlab
+%% New data after calibrating the Rz-offset
+data_m0 = load(sprintf('%s/dynamics/2023-08-21_13-32_damp_plant_m0.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+% Sampling Time [s]
+Ts = 1e-4;
+
+% Hannning Windows
+Nfft = floor(1.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+#+end_src
+
+#+begin_src matlab :exports none
+% And we get the frequency vector
+[~, f] = tfestimate(data_m0.uL1.id_plant, data_m0.uL1.d1, win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+#+begin_src matlab :exports none
+%% HAC Cartesian Plant
+G_hac_m0 = zeros(length(f), 5, 6);
+
+for i_strut = 1:6
+    Di = [data_m0.(sprintf("uL%i", i_strut)).d1 ; data_m0.(sprintf("uL%i", i_strut)).d2 ; data_m0.(sprintf("uL%i", i_strut)).d3 ; data_m0.(sprintf("uL%i", i_strut)).d4 ; data_m0.(sprintf("uL%i", i_strut)).d5]';
+
+    G_hac_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, detrend(Di, 0), win, Noverlap, Nfft, 1/Ts);
+end
+
+G_cart = permute(pagemtimes(J_int_to_X, pagemtimes(permute(G_hac_m0, [2,3,1]), Jt_inv(:,[2,3,5]))), [3,1,2]);
+#+end_src
+
+Compute plant model in the Cartesian plane:
+#+begin_src matlab
+Gm_cart = J_int_to_X*Gm_hac_m0({'d1', 'd2', 'd3', 'd4', 'd5'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'})*Jt_inv(:,[2,3,5]);
+Gm_cart.InputName = {'Fy', 'Fz', 'My'};
+Gm_cart.OutputName = {'Dy', 'Dz', 'Ry'};
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% 3x3 plant
+figure;
+tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax11 = nexttile();
+hold on;
+plot(f, abs(G_cart(:,1,1)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'Fy'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('$D_y$'); set(gca, 'XTickLabel',[]);
+title('$F_y$')
+
+ax12 = nexttile();
+hold on;
+plot(f, abs(G_cart(:,1,2)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'Fz'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+title('$F_z$')
+
+ax13 = nexttile();
+hold on;
+plot(f, abs(G_cart(:,1,3)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'My'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+title('$M_y$')
+
+linkaxes([ax11,ax12,ax13],'y');
+ylim([1e-8, 2e-4]);
+
+ax21 = nexttile();
+hold on;
+plot(f, abs(G_cart(:,2,1)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'Fy'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('$F_z$'); set(gca, 'XTickLabel',[]);
+
+ax22 = nexttile();
+hold on;
+plot(f, abs(G_cart(:,2,2)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'Fz'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+
+ax23 = nexttile();
+hold on;
+plot(f, abs(G_cart(:,2,3)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'My'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+
+linkaxes([ax21,ax22,ax23],'y');
+ylim([1e-8, 2e-4]);
+
+ax31 = nexttile();
+hold on;
+plot(f, abs(G_cart(:,3,1)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'Fy'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('$M_y$');
+
+ax32 = nexttile();
+hold on;
+plot(f, abs(G_cart(:,3,2)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'Fz'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+
+ax33 = nexttile();
+hold on;
+plot(f, abs(G_cart(:,3,3)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'My'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+
+linkaxes([ax31,ax32,ax33],'y');
+ylim([1e-9, 1e-2]);
+
+linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x');
+xlim([1, 5e2]);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% 3x3 plant
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile();
+hold on;
+plot(f, abs(G_cart(:,1,1)), 'color', [colors(1,:)]);
+plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'Fy'), freqs, 'Hz'))), '--', 'color', [colors(1,:)]);
+plot(f, abs(G_cart(:,2,2)), 'color', [colors(2,:)]);
+plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'Fz'), freqs, 'Hz'))), '--', 'color', [colors(2,:)]);
+plot(f, abs(G_cart(:,3,3)), 'color', [colors(3,:)]);
+plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'My'), freqs, 'Hz'))), '--', 'color', [colors(3,:)]);
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude');
+
+ylim([1e-9, 1e-2]);
+xlim([1, 5e2]);
+#+end_src
+
+*** Plant Invert
+Reduce model size
+#+begin_src matlab
+Gm_cart_dy = flipRphZeros(balred(-Gm_cart('Dy', 'Fy'), 10));
+Gm_cart_dz = flipRphZeros(balred(-Gm_cart('Dz', 'Fz'), 10));
+Gm_cart_ry = flipRphZeros(balred(-Gm_cart('Ry', 'My'), 10));
+#+end_src
+
+Add first resonance
+#+begin_src matlab :eval no
+% gm = 200;
+% xi = 0.003;
+% wn = 2*pi*670;
+
+% Gm_cart_dy = Gm_cart_dy*(s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
+
+% gm = 200;
+% xi = 0.003;
+% wn = 2*pi*1086;
+
+% Gm_cart_dz = Gm_cart_dz*(s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
+
+% gm = 200;
+% xi = 0.003;
+% wn = 2*pi*670;
+
+% Gm_cart_ry = Gm_cart_ry*(s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_cart(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
+plot(f, abs(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
+plot(f, abs(G_cart(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
+plot(f, abs(squeeze(freqresp(Gm_cart_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
+plot(f, abs(G_cart(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
+plot(f, abs(squeeze(freqresp(Gm_cart_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(G_cart(:,1,1)), 'color', [colors(1,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Gm_cart_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
+plot(f, 180/pi*angle(G_cart(:,2,2)), 'color', [colors(2,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Gm_cart_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
+plot(f, 180/pi*angle(G_cart(:,3,3)), 'color', [colors(3,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Gm_cart_ry, f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_comp_cart_plant_reduced_model.pdf', 'width', 'wide', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_comp_cart_plant_reduced_model
+#+caption: Comparaison of the measured direct terms and the reduced order models
+#+RESULTS:
+[[file:figs/id31_comp_cart_plant_reduced_model.png]]
+
+
+Invert and make realizable
+#+begin_src matlab
+Gm_cart_dy_inv = inv(Gm_cart_dy);
+Gm_cart_dz_inv = inv(Gm_cart_dz);
+Gm_cart_ry_inv = inv(Gm_cart_ry);
+#+end_src
+
+#+begin_src matlab
+isstable(Gm_cart_dy_inv)
+isstable(Gm_cart_dz_inv)
+isstable(Gm_cart_ry_inv)
+#+end_src
+
+*** Save Plant Inverse
+#+begin_src matlab :exports none :tangle no
+K = -Gm_cart_dy_inv;
+K_order = order(K);
+
+Kz = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
+[num, den] = tfdata(Kz, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dy_inv.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+#+begin_src matlab :exports none :tangle no
+K = -Gm_cart_dz_inv;
+K_order = order(K);
+
+Kz = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
+[num, den] = tfdata(Kz, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dz_inv.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+#+begin_src matlab :exports none :tangle no
+K = -Gm_cart_ry_inv;
+K_order = order(K);
+
+Kz = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
+[num, den] = tfdata(Kz, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_ry_inv.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+*** Control Performances
+**** 5Hz
+#+begin_src matlab
+data_cl = load(sprintf('%s/dynamics/2023-08-21_10-59_m0_cf_5Hz.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+% Sampling Time [s]
+Ts = 1e-4;
+
+% Hannning Windows
+Nfft = floor(1.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+#+end_src
+
+#+begin_src matlab :exports none
+% And we get the frequency vector
+[S_dx, f] = tfestimate(data_cl.Dx.id_cl, data_cl.Dx.e_dx, win, Noverlap, Nfft, 1/Ts);
+[S_dy, ~] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
+[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
+[S_rx, ~] = tfestimate(data_cl.Rx.id_cl, data_cl.Rx.e_rx, win, Noverlap, Nfft, 1/Ts);
+[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
+[S_rz, ~] = tfestimate(data_cl.Rz.id_cl, data_cl.Rz.e_rz, win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+- [ ] Compare with estimated performances
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile();
+hold on;
+plot(f, abs(S_dy), 'DisplayName', '$D_y$');
+plot(f, abs(S_dz), 'DisplayName', '$D_z$');
+plot(f, abs(S_ry), 'DisplayName', '$R_y$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Amplitude');
+ylim([1e-3, 1e1]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
+xlim([1, 1e3]);
+#+end_src
+
+**** 20Hz
+#+begin_src matlab
+data_cl = load(sprintf('%s/dynamics/2023-08-21_11-04_m0_cf_20Hz.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+% Sampling Time [s]
+Ts = 1e-4;
+
+% Hannning Windows
+Nfft = floor(1.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+#+end_src
+
+#+begin_src matlab :exports none
+% And we get the frequency vector
+[S_dx, f] = tfestimate(data_cl.Dx.id_cl, data_cl.Dx.e_dx, win, Noverlap, Nfft, 1/Ts);
+[S_dy, ~] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
+[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
+[S_rx, ~] = tfestimate(data_cl.Rx.id_cl, data_cl.Rx.e_rx, win, Noverlap, Nfft, 1/Ts);
+[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
+[S_rz, ~] = tfestimate(data_cl.Rz.id_cl, data_cl.Rz.e_rz, win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+- [ ] Compare with estimated performances
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile();
+hold on;
+plot(f, abs(S_dy), 'DisplayName', '$D_y$');
+plot(f, abs(S_dz), 'DisplayName', '$D_z$');
+plot(f, abs(S_ry), 'DisplayName', '$R_y$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Amplitude');
+ylim([1e-3, 1e1]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
+xlim([1, 1e3]);
+#+end_src
+
+**** Different bandwidth for different directions
+#+begin_src matlab
+data_cl = load(sprintf('%s/dynamics/2023-08-21_11-16_m0_cf_different.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+% Sampling Time [s]
+Ts = 1e-4;
+
+% Hannning Windows
+Nfft = floor(1.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+#+end_src
+
+#+begin_src matlab :exports none
+% And we get the frequency vector
+[S_dx, f] = tfestimate(data_cl.Dx.id_cl, data_cl.Dx.e_dx, win, Noverlap, Nfft, 1/Ts);
+[S_dy, ~] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
+[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
+[S_rx, ~] = tfestimate(data_cl.Rx.id_cl, data_cl.Rx.e_rx, win, Noverlap, Nfft, 1/Ts);
+[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
+[S_rz, ~] = tfestimate(data_cl.Rz.id_cl, data_cl.Rz.e_rz, win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+- [ ] Compare with estimated performances
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile();
+hold on;
+plot(f, abs(S_dy), 'DisplayName', '$D_y$');
+plot(f, abs(S_dz), 'DisplayName', '$D_z$');
+plot(f, abs(S_ry), 'DisplayName', '$R_y$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Amplitude');
+ylim([1e-3, 1e1]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
+xlim([1, 1e3]);
+#+end_src
+
+
+**** Dz 25Hz
+#+begin_src matlab
+data_cl = load(sprintf('%s/dynamics/', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+% Sampling Time [s]
+Ts = 1e-4;
+
+% Hannning Windows
+Nfft = floor(1.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+#+end_src
+
+#+begin_src matlab :exports none
+% And we get the frequency vector
+[S_dx, f] = tfestimate(data_cl.Dx.id_cl, data_cl.Dx.e_dx, win, Noverlap, Nfft, 1/Ts);
+[S_dy, ~] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
+[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
+[S_rx, ~] = tfestimate(data_cl.Rx.id_cl, data_cl.Rx.e_rx, win, Noverlap, Nfft, 1/Ts);
+[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
+[S_rz, ~] = tfestimate(data_cl.Rz.id_cl, data_cl.Rz.e_rz, win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+- [ ] Compare with estimated performances
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile();
+hold on;
+plot(f, abs(S_dy), 'DisplayName', '$D_y$');
+plot(f, abs(S_dz), 'DisplayName', '$D_z$');
+plot(f, abs(S_ry), 'DisplayName', '$R_y$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Amplitude');
+ylim([1e-3, 1e1]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
+xlim([1, 1e3]);
+#+end_src
+
+
+*** Better plant invert
+**** Dy
+#+begin_src matlab :exports none
+opts = struct();
+
+opts.stable = 1;    % Enforce stable poles
+opts.asymp = 1;     % Force D matrix to be null
+opts.relax = 1;     % Use vector fitting with relaxed non-triviality constraint
+opts.skip_pole = 0; % Do NOT skip pole identification
+opts.skip_res = 0;  % Do NOT skip identification of residues (C,D,E)
+opts.cmplx_ss = 0;  % Create real state space model with block diagonal A
+
+opts.spy1 = 0;      % No plotting for first stage of vector fitting
+opts.spy2 = 0;      % Create magnitude plot for fitting of f(s)
+
+%% We define the number of iteration.
+Niter = 100;
+#+end_src
+
+#+begin_src matlab :exports none
+N = 9; %Order of approximation
+
+%Complex conjugate pairs, linearly spaced:
+bet=linspace(f(1),f(end),N/2);
+poles=[];
+for n=1:length(bet)
+  alf=-bet(n)*1e-1;
+  poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
+end
+#+end_src
+
+#+begin_src matlab
+%% Estimate resonance frequency and damping
+for iter =1:Niter
+    [G_est, poles, ~, frf_est] = vectfit3(G_cart(f>2&f<800,1,1).', 1i*2*pi*f(f>2&f<800)', poles, 1./(f(f>2&f<800))', opts);
+end
+
+Gfit_dy = ss(G_est.A, G_est.B, G_est.C, G_est.D);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_cart(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
+plot(f, abs(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(G_cart(:,1,1)), 'color', [colors(1,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dy, f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+Stable Inverse
+#+begin_src matlab
+Ginv_dy = inv(-flipRphZeros(Gfit_dy))/(1 + s/2/pi/1e3);
+isstable(Ginv_dy)
+#+end_src
+
+**** Dz
+#+begin_src matlab :exports none
+N = 9; %Order of approximation
+
+%Complex conjugate pairs, linearly spaced:
+bet=linspace(f(1),f(end),N/2);
+poles=[];
+for n=1:length(bet)
+  alf=-bet(n)*1e-1;
+  poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
+end
+#+end_src
+
+#+begin_src matlab
+%% Estimate resonance frequency and damping
+for iter =1:Niter
+    [G_est, poles, ~, frf_est] = vectfit3(G_cart(f>2&f<1500,2,2).', 1i*2*pi*f(f>2&f<1500)', poles, 1./(f(f>2&f<1500))', opts);
+end
+
+Gfit_dz = ss(G_est.A, G_est.B, G_est.C, G_est.D);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_cart(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
+plot(f, abs(squeeze(freqresp(Gfit_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(G_cart(:,2,2)), 'color', [colors(2,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dz, f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+Stable Inverse
+#+begin_src matlab
+Ginv_dz = inv(-flipRphZeros(Gfit_dz))/(1 + s/2/pi/1e3);
+isstable(Ginv_dz)
+#+end_src
+
+**** Ry
+#+begin_src matlab :exports none
+N = 9; %Order of approximation
+
+%Complex conjugate pairs, linearly spaced:
+bet=linspace(f(1),f(end),N/2);
+poles=[];
+for n=1:length(bet)
+  alf=-bet(n)*1e-1;
+  poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
+end
+#+end_src
+
+#+begin_src matlab
+%% Estimate resonance frequency and damping
+for iter =1:Niter
+    [G_est, poles, ~, frf_est] = vectfit3(G_cart(f>2&f<800,3,3).', 1i*2*pi*f(f>2&f<800)', poles, 1./(f(f>2&f<800))', opts);
+end
+
+Gfit_ry = ss(G_est.A, G_est.B, G_est.C, G_est.D);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_cart(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
+plot(f, abs(squeeze(freqresp(Gfit_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(G_cart(:,3,3)), 'color', [colors(3,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Gfit_ry, f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+Stable Inverse
+#+begin_src matlab
+Ginv_ry = inv(flipRphZeros(Gfit_ry))/(1 + s/2/pi/1e3);
+isstable(Ginv_ry)
+#+end_src
+
+**** Compare Invert plants
+
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, 1./abs(G_cart(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
+plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
+plot(f, 1./abs(G_cart(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
+plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
+plot(f, 1./abs(G_cart(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
+plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(1./G_cart(:,1,1)), 'color', [colors(1,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
+plot(f, 180/pi*angle(1./G_cart(:,2,2)), 'color', [colors(2,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
+plot(f, 180/pi*angle(1./G_cart(:,3,3)), 'color', [colors(3,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz')).*abs(G_cart(:,1,1))), '-', 'DisplayName', '$D_y$');
+plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz')).*abs(G_cart(:,2,2))), '-', 'DisplayName', '$D_z$');
+plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz')).*abs(G_cart(:,3,3))), '-', 'DisplayName', '$R_y$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz')).*G_cart(:,1,1)), '-');
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz')).*G_cart(:,2,2)), '-');
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz')).*G_cart(:,3,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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+**** Save plant inverse
+#+begin_src matlab :exports none :tangle no
+K = -Ginv_dy;
+K_order = order(K);
+
+Kz_dy = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
+[num, den] = tfdata(Kz_dy, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dy_inv_fit.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+#+begin_src matlab :exports none :tangle no
+K = -Ginv_dz;
+K_order = order(K);
+
+Kz_dz = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
+[num, den] = tfdata(Kz_dz, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dz_inv_fit.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+#+begin_src matlab :exports none :tangle no
+K = -Ginv_ry;
+K_order = order(K);
+
+Kz_ry = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
+[num, den] = tfdata(Kz_ry, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_ry_inv_fit.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+**** Compare Digital Invert plants
+
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, 1./abs(G_cart(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
+plot(f, abs(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
+plot(f, 1./abs(G_cart(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
+plot(f, abs(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
+plot(f, 1./abs(G_cart(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
+plot(f, abs(squeeze(freqresp(Kz_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(1./G_cart(:,1,1)), 'color', [colors(1,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
+plot(f, 180/pi*angle(1./G_cart(:,2,2)), 'color', [colors(2,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
+plot(f, 180/pi*angle(1./G_cart(:,3,3)), 'color', [colors(3,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Kz_ry, f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz')).*abs(G_cart(:,1,1))), '-', 'DisplayName', '$D_y$');
+plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz')).*abs(G_cart(:,2,2))), '-', 'DisplayName', '$D_z$');
+plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz')).*abs(G_cart(:,3,3))), '-', 'DisplayName', '$R_y$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz')).*G_cart(:,1,1)), '-');
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz')).*G_cart(:,2,2)), '-');
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz')).*G_cart(:,3,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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+*** Control Performances
+**** Better plant invert
+#+begin_src matlab
+data_cl = load(sprintf('%s/dynamics/2023-08-21_13-36_m0_cf_inv_fit.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+% Sampling Time [s]
+Ts = 1e-4;
+
+% Hannning Windows
+Nfft = floor(1.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+#+end_src
+
+#+begin_src matlab :exports none
+% And we get the frequency vector
+[S_dy, f] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
+[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
+[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile();
+hold on;
+plot(f, abs(S_dy), 'DisplayName', '$D_y$');
+plot(f, abs(S_dz), 'DisplayName', '$D_z$');
+plot(f, abs(S_ry), 'DisplayName', '$R_y$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Amplitude');
+ylim([1e-3, 1e1]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
+xlim([1, 1e3]);
+#+end_src
+
+*** Scans with good controller
+**** 1rpm
+1RPM scans are performed for all the masses with the same controller.
+
+#+begin_src matlab
+data_m0 = load(sprintf("%s/scans/2023-08-21_14-28_m0_1rpm_K_cf.mat", mat_dir));
+data_m0.time = Ts*[0:length(data_m0.Rz)-1];
+#+end_src
+
+#+begin_src matlab
+%% Compute RMS values while in closed-loop
+data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0));
+data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0));
+data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0));
+data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0));
+data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0));
+#+end_src
+
+#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
+data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ...
+              {'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f ');
+#+end_src
+
+#+RESULTS:
+|    | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] |
+|----+---------+---------+---------+-----------+-----------|
+| m0 |     796 |      20 |       8 |      8209 |        73 |
+
+#+begin_src matlab :exports none :results none
+%% description
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile;
+hold on;
+plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int);
+hold off;
+axis square
+xlabel("Y motion [$nm$]");
+ylabel("Z motion [$nm$]");
+#+end_src
+
+
+**** 30rpm
+1RPM scans are performed for all the masses with the same controller.
+
+#+begin_src matlab
+data_m0 = load(sprintf("%s/scans/2023-08-21_14-33_m0_30rpm_cf_control.mat", mat_dir));
+data_m0.time = Ts*[0:length(data_m0.Rz)-1];
+#+end_src
+
+#+begin_src matlab
+%% Compute RMS values while in closed-loop
+data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0));
+data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0));
+data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0));
+data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0));
+data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0));
+#+end_src
+
+#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
+data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ...
+              {'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f ');
+#+end_src
+
+#+RESULTS:
+|    | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] |
+|----+---------+---------+---------+-----------+-----------|
+| m0 |     820 |      39 |      13 |      7790 |       156 |
+
+#+begin_src matlab :exports none :results none
+%% description
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile;
+hold on;
+plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int);
+hold off;
+axis square
+xlabel("Y motion [$nm$]");
+ylabel("Z motion [$nm$]");
+#+end_src
+
+** m1
+*** 3x3 plant in Cartesian plane
+#+begin_src matlab :exports none
+%% Load model
+load('Gm.mat')
+#+end_src
+
+#+begin_src matlab
+%% Jacobian for 3x3 plant
+n_hexapod = initializeNanoHexapod('flex_bot_type', '2dof', ...
+                                  'flex_top_type', '3dof', ...
+                                  'motion_sensor_type', 'plates', ...
+                                  'actuator_type', '2dof');
+
+Jt_inv = pinv(n_hexapod.geometry.J');
+
+J_int_to_X = [0.78740157480315  0.21259842519685  0                  0                  0;
+              0                 0                 0                  0                 -1;
+              0                 0               -13.1233595800525   13.1233595800525    0];
+#+end_src
+
+Compute identified plant in the Cartesian plane:
+#+begin_src matlab
+%% New data after calibrating the Rz-offset
+data_m1 = load(sprintf('%s/dynamics/2023-08-21_19-05_damp_plant_m1_new_Rz.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+% Sampling Time [s]
+Ts = 1e-4;
+
+% Hannning Windows
+Nfft = floor(1.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+#+end_src
+
+#+begin_src matlab :exports none
+% And we get the frequency vector
+[~, f] = tfestimate(data_m1.uL1.id_plant, data_m1.uL1.d1, win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+#+begin_src matlab :exports none
+%% HAC Cartesian Plant
+G_hac_m1 = zeros(length(f), 5, 6);
+
+for i_strut = 1:6
+    Di = [data_m1.(sprintf("uL%i", i_strut)).d1 ; data_m1.(sprintf("uL%i", i_strut)).d2 ; data_m1.(sprintf("uL%i", i_strut)).d3 ; data_m1.(sprintf("uL%i", i_strut)).d4 ; data_m1.(sprintf("uL%i", i_strut)).d5]';
+
+    G_hac_m1(:,:,i_strut) = tfestimate(data_m1.(sprintf("uL%i", i_strut)).id_plant, detrend(Di, 0), win, Noverlap, Nfft, 1/Ts);
+end
+
+G_cart_m1 = permute(pagemtimes(J_int_to_X, pagemtimes(permute(G_hac_m1, [2,3,1]), Jt_inv(:,[2,3,5]))), [3,1,2]);
+#+end_src
+
+Compute plant model in the Cartesian plane:
+#+begin_src matlab
+Gm_cart_m1 = J_int_to_X*Gm_hac_m1({'d1', 'd2', 'd3', 'd4', 'd5'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'})*Jt_inv(:,[2,3,5]);
+Gm_cart_m1.InputName = {'Fy', 'Fz', 'My'};
+Gm_cart_m1.OutputName = {'Dy', 'Dz', 'Ry'};
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% 3x3 plant
+figure;
+tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax11 = nexttile();
+hold on;
+plot(f, abs(G_cart_m1(:,1,1)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Dy', 'Fy'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('$D_y$'); set(gca, 'XTickLabel',[]);
+title('$F_y$')
+
+ax12 = nexttile();
+hold on;
+plot(f, abs(G_cart_m1(:,1,2)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Dy', 'Fz'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+title('$F_z$')
+
+ax13 = nexttile();
+hold on;
+plot(f, abs(G_cart_m1(:,1,3)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Dy', 'My'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+title('$M_y$')
+
+linkaxes([ax11,ax12,ax13],'y');
+ylim([1e-8, 2e-4]);
+
+ax21 = nexttile();
+hold on;
+plot(f, abs(G_cart_m1(:,2,1)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Dz', 'Fy'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('$F_z$'); set(gca, 'XTickLabel',[]);
+
+ax22 = nexttile();
+hold on;
+plot(f, abs(G_cart_m1(:,2,2)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Dz', 'Fz'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+
+ax23 = nexttile();
+hold on;
+plot(f, abs(G_cart_m1(:,2,3)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Dz', 'My'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+
+linkaxes([ax21,ax22,ax23],'y');
+ylim([1e-8, 2e-4]);
+
+ax31 = nexttile();
+hold on;
+plot(f, abs(G_cart_m1(:,3,1)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Ry', 'Fy'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('$M_y$');
+
+ax32 = nexttile();
+hold on;
+plot(f, abs(G_cart_m1(:,3,2)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Ry', 'Fz'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+
+ax33 = nexttile();
+hold on;
+plot(f, abs(G_cart_m1(:,3,3)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Ry', 'My'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+
+linkaxes([ax31,ax32,ax33],'y');
+ylim([1e-9, 1e-2]);
+
+linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x');
+xlim([1, 5e2]);
+#+end_src
+
+Normalization of outputs:
+#+begin_src matlab
+Gm_cart_m1_normalized = -diag(1./diag(dcgain(Gm_cart_m1)))*Gm_cart_m1;
+Gm_cart_m1_normalized.InputName = {'Fy', 'Fz', 'My'};
+Gm_cart_m1_normalized.OutputName = {'Dy', 'Dz', 'Ry'};
+
+G_cart_m1_normalized = permute(pagemtimes(-diag(1./diag(dcgain(Gm_cart_m1))), permute(G_cart_m1, [2,3,1])), [3,1,2]);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% 3x3 plant
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_cart_m1_normalized(:,1,1)));
+plot(f, abs(G_cart_m1_normalized(:,2,2)));
+plot(f, abs(G_cart_m1_normalized(:,3,3)));
+plot(f, abs(G_cart_m1_normalized(:,1,2)), 'color', [0,0,0,0.2]);
+plot(f, abs(G_cart_m1_normalized(:,1,3)), 'color', [0,0,0,0.2]);
+plot(f, abs(G_cart_m1_normalized(:,2,1)), 'color', [0,0,0,0.2]);
+plot(f, abs(G_cart_m1_normalized(:,3,1)), 'color', [0,0,0,0.2]);
+plot(f, abs(G_cart_m1_normalized(:,2,3)), 'color', [0,0,0,0.2]);
+plot(f, abs(G_cart_m1_normalized(:,3,2)), 'color', [0,0,0,0.2]);
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+ylim([1e-4, 1e1]);
+
+ax2 = nexttile();
+hold on;
+plot(f, 180/pi*angle(G_cart_m1_normalized(:,1,1)));
+plot(f, 180/pi*angle(G_cart_m1_normalized(:,2,2)));
+plot(f, 180/pi*angle(G_cart_m1_normalized(:,3,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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+*** Better plant invert
+#+begin_src matlab :exports none
+opts = struct();
+
+opts.stable = 1;    % Enforce stable poles
+opts.asymp = 1;     % Force D matrix to be null
+opts.relax = 1;     % Use vector fitting with relaxed non-triviality constraint
+opts.skip_pole = 0; % Do NOT skip pole identification
+opts.skip_res = 0;  % Do NOT skip identification of residues (C,D,E)
+opts.cmplx_ss = 0;  % Create real state space model with block diagonal A
+
+opts.spy1 = 0;      % No plotting for first stage of vector fitting
+opts.spy2 = 0;      % Create magnitude plot for fitting of f(s)
+
+%% We define the number of iteration.
+Niter = 100;
+#+end_src
+
+#+begin_src matlab
+N = 9; %Order of approximation
+#+end_src
+
+**** Dy
+#+begin_src matlab :exports none
+%Complex conjugate pairs, linearly spaced:
+bet=linspace(f(1),f(end),N/2);
+poles=[];
+for n=1:length(bet)
+  alf=-bet(n)*1e-2;
+  poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
+end
+#+end_src
+
+#+begin_src matlab
+%% Estimate resonance frequency and damping
+for iter =1:Niter
+    [G_est, poles, ~, frf_est] = vectfit3(G_cart_m1(f>1&f<1500,1,1).', 1i*2*pi*f(f>1&f<1500)', poles, 1./sqrt(f(f>1&f<1500))', opts);
+end
+
+Gfit_dy = ss(G_est.A, G_est.B, G_est.C, G_est.D);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_cart_m1(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
+plot(f, abs(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(G_cart_m1(:,1,1)), 'color', [colors(1,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dy, f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+Stable Inverse
+#+begin_src matlab
+Ginv_dy = inv(flipRphZeros(Gfit_dy))/(1 + s/2/pi/1e3);
+isstable(Ginv_dy)
+#+end_src
+
+**** Dz
+#+begin_src matlab :exports none
+%Complex conjugate pairs, linearly spaced:
+bet=linspace(f(1),f(end),N/2);
+poles=[];
+for n=1:length(bet)
+  alf=-bet(n)*1e-1;
+  poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
+end
+#+end_src
+
+#+begin_src matlab
+%% Estimate resonance frequency and damping
+for iter =1:Niter
+    [G_est, poles, ~, frf_est] = vectfit3(G_cart_m1(f>2&f<1500,2,2).', 1i*2*pi*f(f>2&f<1500)', poles, 1./sqrt(f(f>2&f<1500))', opts);
+end
+
+Gfit_dz = ss(G_est.A, G_est.B, G_est.C, G_est.D);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_cart_m1(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
+plot(f, abs(squeeze(freqresp(Gfit_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(G_cart_m1(:,2,2)), 'color', [colors(2,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dz, f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+Stable Inverse
+#+begin_src matlab
+Ginv_dz = inv(-flipRphZeros(Gfit_dz))/(1 + s/2/pi/1e3);
+isstable(Ginv_dz)
+#+end_src
+
+**** Ry
+#+begin_src matlab :exports none
+%Complex conjugate pairs, linearly spaced:
+bet=linspace(f(1),f(end),N/2);
+poles=[];
+for n=1:length(bet)
+  alf=-bet(n)*1e-1;
+  poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
+end
+#+end_src
+
+#+begin_src matlab
+%% Estimate resonance frequency and damping
+for iter =1:Niter
+    [G_est, poles, ~, frf_est] = vectfit3(G_cart_m1(f>2&f<800,3,3).', 1i*2*pi*f(f>2&f<800)', poles, 1./(f(f>2&f<800))', opts);
+end
+
+Gfit_ry = ss(G_est.A, G_est.B, G_est.C, G_est.D);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_cart_m1(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
+plot(f, abs(squeeze(freqresp(Gfit_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(G_cart_m1(:,3,3)), 'color', [colors(3,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Gfit_ry, f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+Stable Inverse
+#+begin_src matlab
+Ginv_ry = inv(flipRphZeros(Gfit_ry))/(1 + s/2/pi/1e3);
+isstable(Ginv_ry)
+#+end_src
+
+**** Compare Invert plants
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, 1./abs(G_cart_m1(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
+plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
+plot(f, 1./abs(G_cart_m1(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
+plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
+plot(f, 1./abs(G_cart_m1(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
+plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(1./G_cart_m1(:,1,1)), 'color', [colors(1,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
+plot(f, 180/pi*angle(1./G_cart_m1(:,2,2)), 'color', [colors(2,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
+plot(f, 180/pi*angle(1./G_cart_m1(:,3,3)), 'color', [colors(3,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz')).*abs(G_cart_m1(:,1,1))), '-', 'DisplayName', '$D_y$');
+plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz')).*abs(G_cart_m1(:,2,2))), '-', 'DisplayName', '$D_z$');
+plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz')).*abs(G_cart_m1(:,3,3))), '-', 'DisplayName', '$R_y$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz')).*G_cart_m1(:,1,1)), '-');
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz')).*G_cart_m1(:,2,2)), '-');
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz')).*G_cart_m1(:,3,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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+**** Save plant inverse
+#+begin_src matlab :exports none :tangle no
+K = -Ginv_dy;
+K_order = order(K);
+
+Kz_dy = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
+[num, den] = tfdata(Kz_dy, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dy_inv_m1.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+#+begin_src matlab :exports none :tangle no
+K = -Ginv_dz;
+K_order = order(K);
+
+Kz_dz = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
+[num, den] = tfdata(Kz_dz, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dz_inv_m1.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+#+begin_src matlab :exports none :tangle no
+K = -Ginv_ry;
+K_order = order(K);
+
+Kz_ry = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
+[num, den] = tfdata(Kz_ry, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_ry_inv_m1.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+**** Compare Digital Invert plants
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, 1./abs(G_cart_m1(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
+plot(f, abs(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
+plot(f, 1./abs(G_cart_m1(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
+plot(f, abs(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
+plot(f, 1./abs(G_cart_m1(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
+plot(f, abs(squeeze(freqresp(Kz_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(1./G_cart_m1(:,1,1)), 'color', [colors(1,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
+plot(f, 180/pi*angle(1./G_cart_m1(:,2,2)), 'color', [colors(2,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
+plot(f, 180/pi*angle(1./G_cart_m1(:,3,3)), 'color', [colors(3,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Kz_ry, f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+*** TODO Control Performances
+**** Better plant invert
+#+begin_src matlab
+data_cl = load(sprintf('%s/dynamics/2023-08-21_13-36_m0_cf_inv_fit.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+% Sampling Time [s]
+Ts = 1e-4;
+
+% Hannning Windows
+Nfft = floor(1.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+#+end_src
+
+#+begin_src matlab :exports none
+% And we get the frequency vector
+[S_dy, f] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
+[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
+[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile();
+hold on;
+plot(f, abs(S_dy), 'DisplayName', '$D_y$');
+plot(f, abs(S_dz), 'DisplayName', '$D_z$');
+plot(f, abs(S_ry), 'DisplayName', '$R_y$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Amplitude');
+ylim([1e-3, 1e1]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
+xlim([1, 1e3]);
+#+end_src
+
+*** TODO Scans with good controller
+**** 1rpm
+1RPM scans are performed for all the masses with the same controller.
+
+#+begin_src matlab
+data_m0 = load(sprintf("%s/scans/2023-08-21_14-28_m0_1rpm_K_cf.mat", mat_dir));
+data_m0.time = Ts*[0:length(data_m0.Rz)-1];
+#+end_src
+
+#+begin_src matlab
+%% Compute RMS values while in closed-loop
+data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0));
+data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0));
+data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0));
+data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0));
+data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0));
+#+end_src
+
+#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
+data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ...
+              {'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f ');
+#+end_src
+
+#+RESULTS:
+|    | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] |
+|----+---------+---------+---------+-----------+-----------|
+| m0 |     796 |      20 |       8 |      8209 |        73 |
+
+#+begin_src matlab :exports none :results none
+%% description
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile;
+hold on;
+plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int);
+hold off;
+axis square
+xlabel("Y motion [$nm$]");
+ylabel("Z motion [$nm$]");
+#+end_src
+
+
+**** 30rpm
+1RPM scans are performed for all the masses with the same controller.
+
+#+begin_src matlab
+data_m0 = load(sprintf("%s/scans/2023-08-21_14-33_m0_30rpm_cf_control.mat", mat_dir));
+data_m0.time = Ts*[0:length(data_m0.Rz)-1];
+#+end_src
+
+#+begin_src matlab
+%% Compute RMS values while in closed-loop
+data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0));
+data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0));
+data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0));
+data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0));
+data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0));
+#+end_src
+
+#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
+data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ...
+              {'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f ');
+#+end_src
+
+#+RESULTS:
+|    | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] |
+|----+---------+---------+---------+-----------+-----------|
+| m0 |     820 |      39 |      13 |      7790 |       156 |
+
+#+begin_src matlab :exports none :results none
+%% description
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile;
+hold on;
+plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int);
+hold off;
+axis square
+xlabel("Y motion [$nm$]");
+ylabel("Z motion [$nm$]");
+#+end_src
+
+** m2
+*** 3x3 plant in Cartesian plane
+#+begin_src matlab :exports none
+%% Load model
+load('Gm.mat')
+#+end_src
+
+#+begin_src matlab
+%% Jacobian for 3x3 plant
+n_hexapod = initializeNanoHexapod('flex_bot_type', '2dof', ...
+                                  'flex_top_type', '3dof', ...
+                                  'motion_sensor_type', 'plates', ...
+                                  'actuator_type', '2dof');
+
+Jt_inv = pinv(n_hexapod.geometry.J');
+
+J_int_to_X = [0.78740157480315  0.21259842519685  0                  0                  0;
+              0                 0                 0                  0                 -1;
+              0                 0               -13.1233595800525   13.1233595800525    0];
+#+end_src
+
+Compute identified plant in the Cartesian plane:
+#+begin_src matlab
+%% New data after calibrating the Rz-offset
+data_m2 = load(sprintf('%s/dynamics/2023-08-21_17-32_damp_plant_m2_new_Rz.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+% Sampling Time [s]
+Ts = 1e-4;
+
+% Hannning Windows
+Nfft = floor(1.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+#+end_src
+
+#+begin_src matlab :exports none
+% And we get the frequency vector
+[~, f] = tfestimate(data_m2.uL1.id_plant, data_m2.uL1.d1, win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+#+begin_src matlab :exports none
+%% HAC Cartesian Plant
+G_hac_m2 = zeros(length(f), 5, 6);
+
+for i_strut = 1:6
+    Di = [data_m2.(sprintf("uL%i", i_strut)).d1 ; data_m2.(sprintf("uL%i", i_strut)).d2 ; data_m2.(sprintf("uL%i", i_strut)).d3 ; data_m2.(sprintf("uL%i", i_strut)).d4 ; data_m2.(sprintf("uL%i", i_strut)).d5]';
+
+    G_hac_m2(:,:,i_strut) = tfestimate(data_m2.(sprintf("uL%i", i_strut)).id_plant, detrend(Di, 0), win, Noverlap, Nfft, 1/Ts);
+end
+
+G_cart_m2 = permute(pagemtimes(J_int_to_X, pagemtimes(permute(G_hac_m2, [2,3,1]), Jt_inv(:,[2,3,5]))), [3,1,2]);
+#+end_src
+
+Compute plant model in the Cartesian plane:
+#+begin_src matlab
+Gm_cart_m2 = J_int_to_X*Gm_hac_m2({'d1', 'd2', 'd3', 'd4', 'd5'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'})*Jt_inv(:,[2,3,5]);
+Gm_cart_m2.InputName = {'Fy', 'Fz', 'My'};
+Gm_cart_m2.OutputName = {'Dy', 'Dz', 'Ry'};
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% 3x3 plant
+figure;
+tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax11 = nexttile();
+hold on;
+plot(f, abs(G_cart_m2(:,1,1)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Dy', 'Fy'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('$D_y$'); set(gca, 'XTickLabel',[]);
+title('$F_y$')
+
+ax12 = nexttile();
+hold on;
+plot(f, abs(G_cart_m2(:,1,2)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Dy', 'Fz'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+title('$F_z$')
+
+ax13 = nexttile();
+hold on;
+plot(f, abs(G_cart_m2(:,1,3)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Dy', 'My'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+title('$M_y$')
+
+linkaxes([ax11,ax12,ax13],'y');
+ylim([1e-8, 2e-4]);
+
+ax21 = nexttile();
+hold on;
+plot(f, abs(G_cart_m2(:,2,1)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Dz', 'Fy'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('$F_z$'); set(gca, 'XTickLabel',[]);
+
+ax22 = nexttile();
+hold on;
+plot(f, abs(G_cart_m2(:,2,2)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Dz', 'Fz'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+
+ax23 = nexttile();
+hold on;
+plot(f, abs(G_cart_m2(:,2,3)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Dz', 'My'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+
+linkaxes([ax21,ax22,ax23],'y');
+ylim([1e-8, 2e-4]);
+
+ax31 = nexttile();
+hold on;
+plot(f, abs(G_cart_m2(:,3,1)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Ry', 'Fy'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('$M_y$');
+
+ax32 = nexttile();
+hold on;
+plot(f, abs(G_cart_m2(:,3,2)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Ry', 'Fz'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+
+ax33 = nexttile();
+hold on;
+plot(f, abs(G_cart_m2(:,3,3)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Ry', 'My'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+
+linkaxes([ax31,ax32,ax33],'y');
+ylim([1e-9, 1e-2]);
+
+linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x');
+xlim([1, 5e2]);
+#+end_src
+
+Normalization of outputs:
+#+begin_src matlab
+Gm_cart_m2_normalized = -diag(1./diag(dcgain(Gm_cart_m2)))*Gm_cart_m2;
+Gm_cart_m2_normalized.InputName = {'Fy', 'Fz', 'My'};
+Gm_cart_m2_normalized.OutputName = {'Dy', 'Dz', 'Ry'};
+
+G_cart_m2_normalized = permute(pagemtimes(-diag(1./diag(dcgain(Gm_cart_m2))), permute(G_cart_m2, [2,3,1])), [3,1,2]);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% 3x3 plant
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_cart_m2_normalized(:,1,1)));
+plot(f, abs(G_cart_m2_normalized(:,2,2)));
+plot(f, abs(G_cart_m2_normalized(:,3,3)));
+plot(f, abs(G_cart_m2_normalized(:,1,2)), 'color', [0,0,0,0.2]);
+plot(f, abs(G_cart_m2_normalized(:,1,3)), 'color', [0,0,0,0.2]);
+plot(f, abs(G_cart_m2_normalized(:,2,1)), 'color', [0,0,0,0.2]);
+plot(f, abs(G_cart_m2_normalized(:,3,1)), 'color', [0,0,0,0.2]);
+plot(f, abs(G_cart_m2_normalized(:,2,3)), 'color', [0,0,0,0.2]);
+plot(f, abs(G_cart_m2_normalized(:,3,2)), 'color', [0,0,0,0.2]);
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+ylim([1e-4, 1e1]);
+
+ax2 = nexttile();
+hold on;
+plot(f, 180/pi*angle(G_cart_m2_normalized(:,1,1)));
+plot(f, 180/pi*angle(G_cart_m2_normalized(:,2,2)));
+plot(f, 180/pi*angle(G_cart_m2_normalized(:,3,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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+*** Better plant invert
+#+begin_src matlab :exports none
+opts = struct();
+
+opts.stable = 1;    % Enforce stable poles
+opts.asymp = 1;     % Force D matrix to be null
+opts.relax = 1;     % Use vector fitting with relaxed non-triviality constraint
+opts.skip_pole = 0; % Do NOT skip pole identification
+opts.skip_res = 0;  % Do NOT skip identification of residues (C,D,E)
+opts.cmplx_ss = 0;  % Create real state space model with block diagonal A
+
+opts.spy1 = 0;      % No plotting for first stage of vector fitting
+opts.spy2 = 0;      % Create magnitude plot for fitting of f(s)
+
+%% We define the number of iteration.
+Niter = 100;
+#+end_src
+
+#+begin_src matlab
+N = 9; %Order of approximation
+#+end_src
+
+**** Dy
+#+begin_src matlab :exports none
+%Complex conjugate pairs, linearly spaced:
+bet=linspace(f(1),f(end),N/2);
+poles=[];
+for n=1:length(bet)
+  alf=-bet(n)*1e-2;
+  poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
+end
+#+end_src
+
+#+begin_src matlab
+%% Estimate resonance frequency and damping
+for iter =1:Niter
+    [G_est, poles, ~, frf_est] = vectfit3(G_cart_m2(f>1&f<1500,1,1).', 1i*2*pi*f(f>1&f<1500)', poles, 1./sqrt(f(f>1&f<1500))', opts);
+end
+
+Gfit_dy = ss(G_est.A, G_est.B, G_est.C, G_est.D);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_cart_m2(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
+plot(f, abs(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(G_cart_m2(:,1,1)), 'color', [colors(1,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dy, f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+Stable Inverse
+#+begin_src matlab
+Ginv_dy = inv(-flipRphZeros(Gfit_dy))/(1 + s/2/pi/1e3);
+isstable(Ginv_dy)
+#+end_src
+
+**** Dz
+#+begin_src matlab :exports none
+%Complex conjugate pairs, linearly spaced:
+bet=linspace(f(1),f(end),N/2);
+poles=[];
+for n=1:length(bet)
+  alf=-bet(n)*1e-1;
+  poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
+end
+#+end_src
+
+#+begin_src matlab
+%% Estimate resonance frequency and damping
+for iter =1:Niter
+    [G_est, poles, ~, frf_est] = vectfit3(G_cart_m2(f>2&f<1500,2,2).', 1i*2*pi*f(f>2&f<1500)', poles, 1./(f(f>2&f<1500))', opts);
+end
+
+Gfit_dz = ss(G_est.A, G_est.B, G_est.C, G_est.D);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_cart_m2(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
+plot(f, abs(squeeze(freqresp(Gfit_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(G_cart_m2(:,2,2)), 'color', [colors(2,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dz, f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+Stable Inverse
+#+begin_src matlab
+Ginv_dz = inv(-flipRphZeros(Gfit_dz))/(1 + s/2/pi/1e3);
+isstable(Ginv_dz)
+#+end_src
+
+**** Ry
+#+begin_src matlab :exports none
+%Complex conjugate pairs, linearly spaced:
+bet=linspace(f(1),f(end),N/2);
+poles=[];
+for n=1:length(bet)
+  alf=-bet(n)*1e-1;
+  poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
+end
+#+end_src
+
+#+begin_src matlab
+%% Estimate resonance frequency and damping
+for iter =1:Niter
+    [G_est, poles, ~, frf_est] = vectfit3(G_cart_m2(f>2&f<800,3,3).', 1i*2*pi*f(f>2&f<800)', poles, 1./(f(f>2&f<800))', opts);
+end
+
+Gfit_ry = ss(G_est.A, G_est.B, G_est.C, G_est.D);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_cart_m2(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
+plot(f, abs(squeeze(freqresp(Gfit_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(G_cart_m2(:,3,3)), 'color', [colors(3,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Gfit_ry, f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+Stable Inverse
+#+begin_src matlab
+Ginv_ry = inv(-flipRphZeros(Gfit_ry))/(1 + s/2/pi/1e3);
+isstable(Ginv_ry)
+#+end_src
+
+**** Compare Invert plants
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, 1./abs(G_cart_m2(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
+plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
+plot(f, 1./abs(G_cart_m2(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
+plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
+plot(f, 1./abs(G_cart_m2(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
+plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(1./G_cart_m2(:,1,1)), 'color', [colors(1,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
+plot(f, 180/pi*angle(1./G_cart_m2(:,2,2)), 'color', [colors(2,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
+plot(f, 180/pi*angle(1./G_cart_m2(:,3,3)), 'color', [colors(3,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz')).*abs(G_cart_m2(:,1,1))), '-', 'DisplayName', '$D_y$');
+plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz')).*abs(G_cart_m2(:,2,2))), '-', 'DisplayName', '$D_z$');
+plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz')).*abs(G_cart_m2(:,3,3))), '-', 'DisplayName', '$R_y$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz')).*G_cart_m2(:,1,1)), '-');
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz')).*G_cart_m2(:,2,2)), '-');
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz')).*G_cart_m2(:,3,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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+**** Save plant inverse
+#+begin_src matlab :exports none :tangle no
+K = -Ginv_dy;
+K_order = order(K);
+
+Kz_dy = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
+[num, den] = tfdata(Kz_dy, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dy_inv_m2.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+#+begin_src matlab :exports none :tangle no
+K = -Ginv_dz;
+K_order = order(K);
+
+Kz_dz = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
+[num, den] = tfdata(Kz_dz, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dz_inv_m2.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+#+begin_src matlab :exports none :tangle no
+K = -Ginv_ry;
+K_order = order(K);
+
+Kz_ry = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
+[num, den] = tfdata(Kz_ry, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_ry_inv_m2.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+**** Compare Digital Invert plants
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, 1./abs(G_cart_m2(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
+plot(f, abs(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
+plot(f, 1./abs(G_cart_m2(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
+plot(f, abs(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
+plot(f, 1./abs(G_cart_m2(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
+plot(f, abs(squeeze(freqresp(Kz_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(1./G_cart_m2(:,1,1)), 'color', [colors(1,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
+plot(f, 180/pi*angle(1./G_cart_m2(:,2,2)), 'color', [colors(2,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
+plot(f, 180/pi*angle(1./G_cart_m2(:,3,3)), 'color', [colors(3,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Kz_ry, f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+*** TODO Control Performances
+**** Better plant invert
+#+begin_src matlab
+data_cl = load(sprintf('%s/dynamics/2023-08-21_13-36_m0_cf_inv_fit.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+% Sampling Time [s]
+Ts = 1e-4;
+
+% Hannning Windows
+Nfft = floor(1.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+#+end_src
+
+#+begin_src matlab :exports none
+% And we get the frequency vector
+[S_dy, f] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
+[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
+[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile();
+hold on;
+plot(f, abs(S_dy), 'DisplayName', '$D_y$');
+plot(f, abs(S_dz), 'DisplayName', '$D_z$');
+plot(f, abs(S_ry), 'DisplayName', '$R_y$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Amplitude');
+ylim([1e-3, 1e1]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
+xlim([1, 1e3]);
+#+end_src
+
+*** TODO Scans with good controller
+**** 1rpm
+1RPM scans are performed for all the masses with the same controller.
+
+#+begin_src matlab
+data_m0 = load(sprintf("%s/scans/2023-08-21_14-28_m0_1rpm_K_cf.mat", mat_dir));
+data_m0.time = Ts*[0:length(data_m0.Rz)-1];
+#+end_src
+
+#+begin_src matlab
+%% Compute RMS values while in closed-loop
+data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0));
+data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0));
+data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0));
+data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0));
+data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0));
+#+end_src
+
+#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
+data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ...
+              {'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f ');
+#+end_src
+
+#+RESULTS:
+|    | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] |
+|----+---------+---------+---------+-----------+-----------|
+| m0 |     796 |      20 |       8 |      8209 |        73 |
+
+#+begin_src matlab :exports none :results none
+%% description
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile;
+hold on;
+plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int);
+hold off;
+axis square
+xlabel("Y motion [$nm$]");
+ylabel("Z motion [$nm$]");
+#+end_src
+
+
+**** 30rpm
+1RPM scans are performed for all the masses with the same controller.
+
+#+begin_src matlab
+data_m0 = load(sprintf("%s/scans/2023-08-21_14-33_m0_30rpm_cf_control.mat", mat_dir));
+data_m0.time = Ts*[0:length(data_m0.Rz)-1];
+#+end_src
+
+#+begin_src matlab
+%% Compute RMS values while in closed-loop
+data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0));
+data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0));
+data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0));
+data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0));
+data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0));
+#+end_src
+
+#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
+data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ...
+              {'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f ');
+#+end_src
+
+#+RESULTS:
+|    | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] |
+|----+---------+---------+---------+-----------+-----------|
+| m0 |     820 |      39 |      13 |      7790 |       156 |
+
+#+begin_src matlab :exports none :results none
+%% description
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile;
+hold on;
+plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int);
+hold off;
+axis square
+xlabel("Y motion [$nm$]");
+ylabel("Z motion [$nm$]");
+#+end_src
+
+** m3
+*** 3x3 plant in Cartesian plane
+#+begin_src matlab :exports none
+%% Load model
+load('Gm.mat')
+#+end_src
+
+#+begin_src matlab
+%% Jacobian for 3x3 plant
+n_hexapod = initializeNanoHexapod('flex_bot_type', '2dof', ...
+                                  'flex_top_type', '3dof', ...
+                                  'motion_sensor_type', 'plates', ...
+                                  'actuator_type', '2dof');
+
+Jt_inv = pinv(n_hexapod.geometry.J');
+
+J_int_to_X = [0.78740157480315  0.21259842519685  0                  0                  0;
+              0                 0                 0                  0                 -1;
+              0                 0               -13.1233595800525   13.1233595800525    0];
+#+end_src
+
+Compute identified plant in the Cartesian plane:
+#+begin_src matlab
+%% New data after calibrating the Rz-offset
+data_m3 = load(sprintf('%s/dynamics/2023-08-21_16-33_damp_plant_m3_new_Rz_fast.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+% Sampling Time [s]
+Ts = 1e-4;
+
+% Hannning Windows
+Nfft = floor(1.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+#+end_src
+
+#+begin_src matlab :exports none
+% And we get the frequency vector
+[~, f] = tfestimate(data_m3.uL1.id_plant, data_m3.uL1.d1, win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+#+begin_src matlab :exports none
+%% HAC Cartesian Plant
+G_hac_m3 = zeros(length(f), 5, 6);
+
+for i_strut = 1:6
+    Di = [data_m3.(sprintf("uL%i", i_strut)).d1 ; data_m3.(sprintf("uL%i", i_strut)).d2 ; data_m3.(sprintf("uL%i", i_strut)).d3 ; data_m3.(sprintf("uL%i", i_strut)).d4 ; data_m3.(sprintf("uL%i", i_strut)).d5]';
+
+    G_hac_m3(:,:,i_strut) = tfestimate(data_m3.(sprintf("uL%i", i_strut)).id_plant, detrend(Di, 0), win, Noverlap, Nfft, 1/Ts);
+end
+
+G_cart_m3 = permute(pagemtimes(J_int_to_X, pagemtimes(permute(G_hac_m3, [2,3,1]), Jt_inv(:,[2,3,5]))), [3,1,2]);
+#+end_src
+
+Compute plant model in the Cartesian plane:
+#+begin_src matlab
+Gm_cart_m3 = J_int_to_X*Gm_hac_m3({'d1', 'd2', 'd3', 'd4', 'd5'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'})*Jt_inv(:,[2,3,5]);
+Gm_cart_m3.InputName = {'Fy', 'Fz', 'My'};
+Gm_cart_m3.OutputName = {'Dy', 'Dz', 'Ry'};
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% 3x3 plant
+figure;
+tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax11 = nexttile();
+hold on;
+plot(f, abs(G_cart_m3(:,1,1)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Dy', 'Fy'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('$D_y$'); set(gca, 'XTickLabel',[]);
+title('$F_y$')
+
+ax12 = nexttile();
+hold on;
+plot(f, abs(G_cart_m3(:,1,2)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Dy', 'Fz'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+title('$F_z$')
+
+ax13 = nexttile();
+hold on;
+plot(f, abs(G_cart_m3(:,1,3)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Dy', 'My'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+title('$M_y$')
+
+linkaxes([ax11,ax12,ax13],'y');
+ylim([1e-8, 2e-4]);
+
+ax21 = nexttile();
+hold on;
+plot(f, abs(G_cart_m3(:,2,1)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Dz', 'Fy'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('$F_z$'); set(gca, 'XTickLabel',[]);
+
+ax22 = nexttile();
+hold on;
+plot(f, abs(G_cart_m3(:,2,2)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Dz', 'Fz'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+
+ax23 = nexttile();
+hold on;
+plot(f, abs(G_cart_m3(:,2,3)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Dz', 'My'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+
+linkaxes([ax21,ax22,ax23],'y');
+ylim([1e-8, 2e-4]);
+
+ax31 = nexttile();
+hold on;
+plot(f, abs(G_cart_m3(:,3,1)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Ry', 'Fy'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('$M_y$');
+
+ax32 = nexttile();
+hold on;
+plot(f, abs(G_cart_m3(:,3,2)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Ry', 'Fz'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+
+ax33 = nexttile();
+hold on;
+plot(f, abs(G_cart_m3(:,3,3)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Ry', 'My'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+
+linkaxes([ax31,ax32,ax33],'y');
+ylim([1e-9, 1e-2]);
+
+linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x');
+xlim([1, 5e2]);
+#+end_src
+
+Normalization of outputs:
+#+begin_src matlab
+Gm_cart_m3_normalized = -diag(1./diag(dcgain(Gm_cart_m3)))*Gm_cart_m3;
+Gm_cart_m3_normalized.InputName = {'Fy', 'Fz', 'My'};
+Gm_cart_m3_normalized.OutputName = {'Dy', 'Dz', 'Ry'};
+
+G_cart_m3_normalized = permute(pagemtimes(-diag(1./diag(dcgain(Gm_cart_m3))), permute(G_cart_m3, [2,3,1])), [3,1,2]);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% 3x3 plant
+figure;
+tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax11 = nexttile();
+hold on;
+plot(f, abs(G_cart_m3_normalized(:,1,1)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Dy', 'Fy'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('$D_y$'); set(gca, 'XTickLabel',[]);
+title('$F_y$')
+
+ax12 = nexttile();
+hold on;
+plot(f, abs(G_cart_m3_normalized(:,1,2)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Dy', 'Fz'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+title('$F_z$')
+
+ax13 = nexttile();
+hold on;
+plot(f, abs(G_cart_m3_normalized(:,1,3)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Dy', 'My'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+title('$M_y$')
+
+linkaxes([ax11,ax12,ax13],'y');
+ylim([1e-4, 1e1]);
+
+ax21 = nexttile();
+hold on;
+plot(f, abs(G_cart_m3_normalized(:,2,1)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Dz', 'Fy'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('$F_z$'); set(gca, 'XTickLabel',[]);
+
+ax22 = nexttile();
+hold on;
+plot(f, abs(G_cart_m3_normalized(:,2,2)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Dz', 'Fz'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+
+ax23 = nexttile();
+hold on;
+plot(f, abs(G_cart_m3_normalized(:,2,3)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Dz', 'My'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
+
+linkaxes([ax21,ax22,ax23],'y');
+ylim([1e-4, 1e1]);
+
+ax31 = nexttile();
+hold on;
+plot(f, abs(G_cart_m3_normalized(:,3,1)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Ry', 'Fy'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('$M_y$');
+
+ax32 = nexttile();
+hold on;
+plot(f, abs(G_cart_m3_normalized(:,3,2)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Ry', 'Fz'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+
+ax33 = nexttile();
+hold on;
+plot(f, abs(G_cart_m3_normalized(:,3,3)));
+plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Ry', 'My'), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+
+linkaxes([ax31,ax32,ax33],'y');
+ylim([1e-4, 1e1]);
+
+linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x');
+xlim([1, 5e2]);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% 3x3 plant
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_cart_m3_normalized(:,1,1)));
+plot(f, abs(G_cart_m3_normalized(:,2,2)));
+plot(f, abs(G_cart_m3_normalized(:,3,3)));
+plot(f, abs(G_cart_m3_normalized(:,1,2)), 'color', [0,0,0,0.2]);
+plot(f, abs(G_cart_m3_normalized(:,1,3)), 'color', [0,0,0,0.2]);
+plot(f, abs(G_cart_m3_normalized(:,2,1)), 'color', [0,0,0,0.2]);
+plot(f, abs(G_cart_m3_normalized(:,3,1)), 'color', [0,0,0,0.2]);
+plot(f, abs(G_cart_m3_normalized(:,2,3)), 'color', [0,0,0,0.2]);
+plot(f, abs(G_cart_m3_normalized(:,3,2)), 'color', [0,0,0,0.2]);
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+ylim([1e-4, 1e1]);
+
+ax2 = nexttile();
+hold on;
+plot(f, 180/pi*angle(G_cart_m3_normalized(:,1,1)));
+plot(f, 180/pi*angle(G_cart_m3_normalized(:,2,2)));
+plot(f, 180/pi*angle(G_cart_m3_normalized(:,3,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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 1e3]);
+#+end_src
+
+*** Better plant invert
+#+begin_src matlab :exports none
+opts = struct();
+
+opts.stable = 1;    % Enforce stable poles
+opts.asymp = 1;     % Force D matrix to be null
+opts.relax = 1;     % Use vector fitting with relaxed non-triviality constraint
+opts.skip_pole = 0; % Do NOT skip pole identification
+opts.skip_res = 0;  % Do NOT skip identification of residues (C,D,E)
+opts.cmplx_ss = 0;  % Create real state space model with block diagonal A
+
+opts.spy1 = 0;      % No plotting for first stage of vector fitting
+opts.spy2 = 0;      % Create magnitude plot for fitting of f(s)
+
+%% We define the number of iteration.
+Niter = 100;
+#+end_src
+
+#+begin_src matlab
+N = 9; %Order of approximation
+#+end_src
+
+**** Dy
+#+begin_src matlab :exports none
+%Complex conjugate pairs, linearly spaced:
+bet=linspace(f(1),f(end),N/2);
+poles=[];
+for n=1:length(bet)
+  alf=-bet(n)*1e-1;
+  poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
+end
+#+end_src
+
+#+begin_src matlab
+%% Estimate resonance frequency and damping
+for iter =1:Niter
+    [G_est, poles, ~, frf_est] = vectfit3(G_cart_m3(f>2&f<800,1,1).', 1i*2*pi*f(f>2&f<800)', poles, 1./(f(f>2&f<800))', opts);
+end
+
+Gfit_dy = ss(G_est.A, G_est.B, G_est.C, G_est.D);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_cart_m3(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
+plot(f, abs(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(G_cart_m3(:,1,1)), 'color', [colors(1,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dy, f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+Stable Inverse
+#+begin_src matlab
+Ginv_dy = inv(flipRphZeros(Gfit_dy))/(1 + s/2/pi/1e3);
+isstable(Ginv_dy)
+#+end_src
+
+**** Dz
+#+begin_src matlab :exports none
+%Complex conjugate pairs, linearly spaced:
+bet=linspace(f(1),f(end),N/2);
+poles=[];
+for n=1:length(bet)
+  alf=-bet(n)*1e-1;
+  poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
+end
+#+end_src
+
+#+begin_src matlab
+%% Estimate resonance frequency and damping
+for iter =1:Niter
+    [G_est, poles, ~, frf_est] = vectfit3(G_cart_m3(f>2&f<1500,2,2).', 1i*2*pi*f(f>2&f<1500)', poles, 1./(f(f>2&f<1500))', opts);
+end
+
+Gfit_dz = ss(G_est.A, G_est.B, G_est.C, G_est.D);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_cart_m3(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
+plot(f, abs(squeeze(freqresp(Gfit_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(G_cart_m3(:,2,2)), 'color', [colors(2,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dz, f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+Stable Inverse
+#+begin_src matlab
+Ginv_dz = inv(-flipRphZeros(Gfit_dz))/(1 + s/2/pi/1e3);
+isstable(Ginv_dz)
+#+end_src
+
+**** Ry
+#+begin_src matlab :exports none
+%Complex conjugate pairs, linearly spaced:
+bet=linspace(f(1),f(end),N/2);
+poles=[];
+for n=1:length(bet)
+  alf=-bet(n)*1e-1;
+  poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
+end
+#+end_src
+
+#+begin_src matlab
+%% Estimate resonance frequency and damping
+for iter =1:Niter
+    [G_est, poles, ~, frf_est] = vectfit3(G_cart_m3(f>2&f<800,3,3).', 1i*2*pi*f(f>2&f<800)', poles, 1./(f(f>2&f<800))', opts);
+end
+
+Gfit_ry = ss(G_est.A, G_est.B, G_est.C, G_est.D);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(G_cart_m3(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
+plot(f, abs(squeeze(freqresp(Gfit_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(G_cart_m3(:,3,3)), 'color', [colors(3,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Gfit_ry, f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+Stable Inverse
+#+begin_src matlab
+Ginv_ry = inv(-flipRphZeros(Gfit_ry))/(1 + s/2/pi/1e3);
+isstable(Ginv_ry)
+#+end_src
+
+**** Compare Invert plants
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, 1./abs(G_cart_m3(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
+plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
+plot(f, 1./abs(G_cart_m3(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
+plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
+plot(f, 1./abs(G_cart_m3(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
+plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(1./G_cart_m3(:,1,1)), 'color', [colors(1,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
+plot(f, 180/pi*angle(1./G_cart_m3(:,2,2)), 'color', [colors(2,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
+plot(f, 180/pi*angle(1./G_cart_m3(:,3,3)), 'color', [colors(3,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz')).*abs(G_cart_m3(:,1,1))), '-', 'DisplayName', '$D_y$');
+plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz')).*abs(G_cart_m3(:,2,2))), '-', 'DisplayName', '$D_z$');
+plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz')).*abs(G_cart_m3(:,3,3))), '-', 'DisplayName', '$R_y$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz')).*G_cart_m3(:,1,1)), '-');
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz')).*G_cart_m3(:,2,2)), '-');
+plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz')).*G_cart_m3(:,3,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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+**** Save plant inverse
+#+begin_src matlab :exports none :tangle no
+K = -Ginv_dy;
+K_order = order(K);
+
+Kz_dy = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
+[num, den] = tfdata(Kz_dy, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dy_inv_m3.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+#+begin_src matlab :exports none :tangle no
+K = -Ginv_dz;
+K_order = order(K);
+
+Kz_dz = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
+[num, den] = tfdata(Kz_dz, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dz_inv_m3.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+#+begin_src matlab :exports none :tangle no
+K = -Ginv_ry;
+K_order = order(K);
+
+Kz_ry = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
+[num, den] = tfdata(Kz_ry, 'v');
+
+formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
+fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_ry_inv_m3.dat', 'w');
+fprintf(fileID, formatSpec, [num; den]');
+fclose(fileID);
+#+end_src
+
+**** Compare Digital Invert plants
+#+begin_src matlab :exports none :results none
+%% Comparaison of the measured direct terms and the reduced order models
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(f, 1./abs(G_cart_m3(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
+plot(f, abs(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
+plot(f, 1./abs(G_cart_m3(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
+plot(f, abs(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
+plot(f, 1./abs(G_cart_m3(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
+plot(f, abs(squeeze(freqresp(Kz_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]); ylabel('Magnitude');
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(f, 180/pi*angle(1./G_cart_m3(:,1,1)), 'color', [colors(1,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
+plot(f, 180/pi*angle(1./G_cart_m3(:,2,2)), 'color', [colors(2,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
+plot(f, 180/pi*angle(1./G_cart_m3(:,3,3)), 'color', [colors(3,:)]);
+plot(f, 180/pi*angle(squeeze(freqresp(Kz_ry, f, '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, 180])
+
+linkaxes([ax1,ax2],'x');
+xlim([1, 2e3]);
+#+end_src
+
+*** TODO Control Performances
+**** Better plant invert
+#+begin_src matlab
+data_cl = load(sprintf('%s/dynamics/2023-08-21_13-36_m0_cf_inv_fit.mat', mat_dir));
+#+end_src
+
+#+begin_src matlab :exports none
+% Sampling Time [s]
+Ts = 1e-4;
+
+% Hannning Windows
+Nfft = floor(1.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+#+end_src
+
+#+begin_src matlab :exports none
+% And we get the frequency vector
+[S_dy, f] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
+[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
+[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile();
+hold on;
+plot(f, abs(S_dy), 'DisplayName', '$D_y$');
+plot(f, abs(S_dz), 'DisplayName', '$D_z$');
+plot(f, abs(S_ry), 'DisplayName', '$R_y$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Amplitude');
+ylim([1e-3, 1e1]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
+xlim([1, 1e3]);
+#+end_src
+
+*** TODO Scans with good controller
+**** 1rpm
+1RPM scans are performed for all the masses with the same controller.
+
+#+begin_src matlab
+data_m0 = load(sprintf("%s/scans/2023-08-21_14-28_m0_1rpm_K_cf.mat", mat_dir));
+data_m0.time = Ts*[0:length(data_m0.Rz)-1];
+#+end_src
+
+#+begin_src matlab
+%% Compute RMS values while in closed-loop
+data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0));
+data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0));
+data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0));
+data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0));
+data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0));
+#+end_src
+
+#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
+data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ...
+              {'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f ');
+#+end_src
+
+#+RESULTS:
+|    | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] |
+|----+---------+---------+---------+-----------+-----------|
+| m0 |     796 |      20 |       8 |      8209 |        73 |
+
+#+begin_src matlab :exports none :results none
+%% description
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile;
+hold on;
+plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int);
+hold off;
+axis square
+xlabel("Y motion [$nm$]");
+ylabel("Z motion [$nm$]");
+#+end_src
+
+
+**** 30rpm
+1RPM scans are performed for all the masses with the same controller.
+
+#+begin_src matlab
+data_m0 = load(sprintf("%s/scans/2023-08-21_14-33_m0_30rpm_cf_control.mat", mat_dir));
+data_m0.time = Ts*[0:length(data_m0.Rz)-1];
+#+end_src
+
+#+begin_src matlab
+%% Compute RMS values while in closed-loop
+data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0));
+data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0));
+data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0));
+data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0));
+data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0));
+#+end_src
+
+#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
+data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ...
+              {'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f ');
+#+end_src
+
+#+RESULTS:
+|    | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] |
+|----+---------+---------+---------+-----------+-----------|
+| m0 |     820 |      39 |      13 |      7790 |       156 |
+
+#+begin_src matlab :exports none :results none
+%% description
+figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile;
+hold on;
+plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int);
+hold off;
+axis square
+xlabel("Y motion [$nm$]");
+ylabel("Z motion [$nm$]");
+#+end_src
+
+* Scans                                                             :noexport:
+<<sec:test_id31_scans>>
+** Introduction                                                      :ignore:
+
+- Section ref:sec:id31_scans_tomography
+- Section ref:sec:id31_scans_dz
+- Section ref:sec:id31_scans_reflectivity
+- Section ref:sec:id31_scans_dy
+- Section ref:sec:id31_scans_diffraction_tomo
+
+** Matlab Init                                              :noexport:ignore:
+#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
+<<matlab-dir>>
+#+end_src
+
+#+begin_src matlab :exports none :results silent :noweb yes
+<<matlab-init>>
+#+end_src
+
+#+begin_src matlab :tangle no :noweb yes
+<<m-init-path>>
+#+end_src
+
+#+begin_src matlab :eval no :noweb yes
+<<m-init-path-tangle>>
+#+end_src
+
+#+begin_src matlab :noweb yes
+<<m-init-other>>
+#+end_src
+
+** $R_z$ scans: Tomography
+<<sec:id31_scans_tomography>>
+*** Introduction                                                    :ignore:
+
+m0: 30rpm, 6rpm, 1rpm
+m1: 6rpm, 1rpm
+m2: 6rpm, 1rpm
+m3: 1rpm
+
+*** Robust Control - 1rpm
+1RPM scans are performed for all the masses with the same robust controller.
+
+#+begin_src matlab
+%% Load Tomography scans with robust controller
+data_tomo_1rpm_m0 = load(sprintf("%s/scans/2023-08-11_11-37_tomography_1rpm_m0.mat", mat_dir));
+data_tomo_1rpm_m0.time = Ts*[0:length(data_tomo_1rpm_m0.Rz)-1];
+
+data_tomo_1rpm_m1 = load(sprintf("%s/scans/2023-08-11_11-15_tomography_1rpm_m1.mat", mat_dir));
+data_tomo_1rpm_m1.time = Ts*[0:length(data_tomo_1rpm_m1.Rz)-1];
+
+data_tomo_1rpm_m2 = load(sprintf("%s/scans/2023-08-11_10-59_tomography_1rpm_m2.mat", mat_dir));
+data_tomo_1rpm_m2.time = Ts*[0:length(data_tomo_1rpm_m2.Rz)-1];
+
+data_tomo_1rpm_m3 = load(sprintf("%s/scans/2023-08-11_10-24_tomography_1rpm_m3.mat", mat_dir));
+data_tomo_1rpm_m3.time = Ts*[0:length(data_tomo_1rpm_m3.Rz)-1];
+#+end_src
+
+The problem for these scans is that the position initialization was not make properly, so the open-loop errors are quite large (see Figure ref:fig:id31_tomo_1rpm_robust_m0).
+
+#+begin_src matlab :exports none :results none
+%% $D_x$, $D_y$ and $D_z$ motion during a slow (1RPM) tomography experiment. Open Loop data is shown in blue and closed-loop data in red
+figure;
+tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile;
+hold on;
+plot(1e6*data_tomo_1rpm_m0.Dx_int(data_tomo_1rpm_m0.hac_status == 0), 1e6*data_tomo_1rpm_m0.Dy_int(data_tomo_1rpm_m0.hac_status == 0), ...
+    'DisplayName', 'OL')
+plot(1e6*data_tomo_1rpm_m0.Dx_int(data_tomo_1rpm_m0.hac_status == 1), 1e6*data_tomo_1rpm_m0.Dy_int(data_tomo_1rpm_m0.hac_status == 1), ...
+    'DisplayName', 'CL')
+hold off;
+axis equal
+xlabel("X motion [$\mu m$]");
+ylabel("Y motion [$\mu m$]");
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+% xlim([-3, 3])
+% ylim([-3, 3])
+
+ax2 = nexttile;
+hold on;
+plot(1e6*data_tomo_1rpm_m0.Dy_int(data_tomo_1rpm_m0.hac_status == 0), 1e6*data_tomo_1rpm_m0.Dz_int(data_tomo_1rpm_m0.hac_status == 0), ...
+    'DisplayName', 'OL')
+plot(1e6*data_tomo_1rpm_m0.Dy_int(data_tomo_1rpm_m0.hac_status == 1), 1e6*data_tomo_1rpm_m0.Dz_int(data_tomo_1rpm_m0.hac_status == 1), ...
+    'DisplayName', 'CL')
+hold off;
+% axis equal
+% xlim([-3, 3])
+ylim([-0.2, 1.1])
+xlabel("Y motion [$\mu m$]");
+ylabel("Z motion [$\mu m$]");
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_tomo_1rpm_robust_m0.pdf', 'width', 'full', 'height', 'normal');
+#+end_src
+
+#+name: fig:id31_tomo_1rpm_robust_m0
+#+caption: $D_x$, $D_y$ and $D_z$ motion during a slow (1RPM) tomography experiment. Open Loop data is shown in blue and closed-loop data in red
+#+RESULTS:
+[[file:figs/id31_tomo_1rpm_robust_m0.png]]
+
+#+begin_src matlab
+yztomographymovie('movies/tomography_1rpm_m0', data_tomo_1rpm_m0, 'xlim_ax1', [-10, 15], 'ylim_ax1', [-0.2, 1.2], 'xlim_ax2', [-300, 300], 'ylim_ax2', [-100, 100])
+yztomographymovie('movies/tomography_1rpm_m0_di_5000', data_tomo_1rpm_m0, 'xlim_ax1', [-10, 15], 'ylim_ax1', [-0.2, 1.2], 'xlim_ax2', [-300, 300], 'ylim_ax2', [-100, 100], 'di', 5000)
+#+end_src
+
+The obtained open-loop and closed-loop errors are shown in tables ref:tab:id31_tomo_1rpm_robust_ol_errors and ref:tab:id31_tomo_1rpm_robust_cl_errors respectively.
+
+#+begin_src matlab
+%% Compute RMS values while in closed-loop and open-loop
+[~, i_m0] = find(data_tomo_1rpm_m0.hac_status == 1);
+data_tomo_1rpm_m0.Dx_rms_cl = rms(detrend(data_tomo_1rpm_m0.Dx_int(i_m0+50000:end), 0));
+data_tomo_1rpm_m0.Dy_rms_cl = rms(detrend(data_tomo_1rpm_m0.Dy_int(i_m0+50000:end), 0));
+data_tomo_1rpm_m0.Dz_rms_cl = rms(detrend(data_tomo_1rpm_m0.Dz_int(i_m0+50000:end), 0));
+data_tomo_1rpm_m0.Rx_rms_cl = rms(detrend(data_tomo_1rpm_m0.Rx_int(i_m0+50000:end), 0));
+data_tomo_1rpm_m0.Ry_rms_cl = rms(detrend(data_tomo_1rpm_m0.Ry_int(i_m0+50000:end), 0));
+
+data_tomo_1rpm_m0.Dx_rms_ol = rms(detrend(data_tomo_1rpm_m0.Dx_int(1:i_m0), 0));
+data_tomo_1rpm_m0.Dy_rms_ol = rms(detrend(data_tomo_1rpm_m0.Dy_int(1:i_m0), 0));
+data_tomo_1rpm_m0.Dz_rms_ol = rms(detrend(data_tomo_1rpm_m0.Dz_int(1:i_m0), 0));
+data_tomo_1rpm_m0.Rx_rms_ol = rms(detrend(data_tomo_1rpm_m0.Rx_int(1:i_m0), 0));
+data_tomo_1rpm_m0.Ry_rms_ol = rms(detrend(data_tomo_1rpm_m0.Ry_int(1:i_m0), 0));
+
+%% Compute RMS values while in closed-loop and open-loop
+[~, i_m1] = find(data_tomo_1rpm_m1.hac_status == 1);
+data_tomo_1rpm_m1.Dx_rms_cl = rms(detrend(data_tomo_1rpm_m1.Dx_int(i_m1+50000:end), 0));
+data_tomo_1rpm_m1.Dy_rms_cl = rms(detrend(data_tomo_1rpm_m1.Dy_int(i_m1+50000:end), 0));
+data_tomo_1rpm_m1.Dz_rms_cl = rms(detrend(data_tomo_1rpm_m1.Dz_int(i_m1+50000:end), 0));
+data_tomo_1rpm_m1.Rx_rms_cl = rms(detrend(data_tomo_1rpm_m1.Rx_int(i_m1+50000:end), 0));
+data_tomo_1rpm_m1.Ry_rms_cl = rms(detrend(data_tomo_1rpm_m1.Ry_int(i_m1+50000:end), 0));
+
+data_tomo_1rpm_m1.Dx_rms_ol = rms(detrend(data_tomo_1rpm_m1.Dx_int(1:i_m1), 0));
+data_tomo_1rpm_m1.Dy_rms_ol = rms(detrend(data_tomo_1rpm_m1.Dy_int(1:i_m1), 0));
+data_tomo_1rpm_m1.Dz_rms_ol = rms(detrend(data_tomo_1rpm_m1.Dz_int(1:i_m1), 0));
+data_tomo_1rpm_m1.Rx_rms_ol = rms(detrend(data_tomo_1rpm_m1.Rx_int(1:i_m1), 0));
+data_tomo_1rpm_m1.Ry_rms_ol = rms(detrend(data_tomo_1rpm_m1.Ry_int(1:i_m1), 0));
+
+%% Compute RMS values while in closed-loop and open-loop
+[~, i_m2] = find(data_tomo_1rpm_m2.hac_status == 1);
+data_tomo_1rpm_m2.Dx_rms_cl = rms(detrend(data_tomo_1rpm_m2.Dx_int(i_m2+50000:end), 0));
+data_tomo_1rpm_m2.Dy_rms_cl = rms(detrend(data_tomo_1rpm_m2.Dy_int(i_m2+50000:end), 0));
+data_tomo_1rpm_m2.Dz_rms_cl = rms(detrend(data_tomo_1rpm_m2.Dz_int(i_m2+50000:end), 0));
+data_tomo_1rpm_m2.Rx_rms_cl = rms(detrend(data_tomo_1rpm_m2.Rx_int(i_m2+50000:end), 0));
+data_tomo_1rpm_m2.Ry_rms_cl = rms(detrend(data_tomo_1rpm_m2.Ry_int(i_m2+50000:end), 0));
+
+data_tomo_1rpm_m2.Dx_rms_ol = rms(detrend(data_tomo_1rpm_m2.Dx_int(1:i_m2), 0));
+data_tomo_1rpm_m2.Dy_rms_ol = rms(detrend(data_tomo_1rpm_m2.Dy_int(1:i_m2), 0));
+data_tomo_1rpm_m2.Dz_rms_ol = rms(detrend(data_tomo_1rpm_m2.Dz_int(1:i_m2), 0));
+data_tomo_1rpm_m2.Rx_rms_ol = rms(detrend(data_tomo_1rpm_m2.Rx_int(1:i_m2), 0));
+data_tomo_1rpm_m2.Ry_rms_ol = rms(detrend(data_tomo_1rpm_m2.Ry_int(1:i_m2), 0));
+
+%% Compute RMS values while in closed-loop and open-loop
+[~, i_m3] = find(data_tomo_1rpm_m3.hac_status == 1);
+data_tomo_1rpm_m3.Dx_rms_cl = rms(detrend(data_tomo_1rpm_m3.Dx_int(i_m3+50000:end), 0));
+data_tomo_1rpm_m3.Dy_rms_cl = rms(detrend(data_tomo_1rpm_m3.Dy_int(i_m3+50000:end), 0));
+data_tomo_1rpm_m3.Dz_rms_cl = rms(detrend(data_tomo_1rpm_m3.Dz_int(i_m3+50000:end), 0));
+data_tomo_1rpm_m3.Rx_rms_cl = rms(detrend(data_tomo_1rpm_m3.Rx_int(i_m3+50000:end), 0));
+data_tomo_1rpm_m3.Ry_rms_cl = rms(detrend(data_tomo_1rpm_m3.Ry_int(i_m3+50000:end), 0));
+
+data_tomo_1rpm_m3.Dx_rms_ol = rms(detrend(data_tomo_1rpm_m3.Dx_int(1:i_m3), 0));
+data_tomo_1rpm_m3.Dy_rms_ol = rms(detrend(data_tomo_1rpm_m3.Dy_int(1:i_m3), 0));
+data_tomo_1rpm_m3.Dz_rms_ol = rms(detrend(data_tomo_1rpm_m3.Dz_int(1:i_m3), 0));
+data_tomo_1rpm_m3.Rx_rms_ol = rms(detrend(data_tomo_1rpm_m3.Rx_int(1:i_m3), 0));
+data_tomo_1rpm_m3.Ry_rms_ol = rms(detrend(data_tomo_1rpm_m3.Ry_int(1:i_m3), 0));
+#+end_src
+
+#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
+data2orgtable([1e6*data_tomo_1rpm_m0.Dx_rms_ol, 1e6*data_tomo_1rpm_m0.Dy_rms_ol, 1e9*data_tomo_1rpm_m0.Dz_rms_ol, 1e6*data_tomo_1rpm_m0.Rx_rms_ol, 1e6*data_tomo_1rpm_m0.Ry_rms_ol; ...
+               1e6*data_tomo_1rpm_m1.Dx_rms_ol, 1e6*data_tomo_1rpm_m1.Dy_rms_ol, 1e9*data_tomo_1rpm_m1.Dz_rms_ol, 1e6*data_tomo_1rpm_m1.Rx_rms_ol, 1e6*data_tomo_1rpm_m1.Ry_rms_ol; ...
+               1e6*data_tomo_1rpm_m2.Dx_rms_ol, 1e6*data_tomo_1rpm_m2.Dy_rms_ol, 1e9*data_tomo_1rpm_m2.Dz_rms_ol, 1e6*data_tomo_1rpm_m2.Rx_rms_ol, 1e6*data_tomo_1rpm_m2.Ry_rms_ol; ...
+               1e6*data_tomo_1rpm_m3.Dx_rms_ol, 1e6*data_tomo_1rpm_m3.Dy_rms_ol, 1e9*data_tomo_1rpm_m3.Dz_rms_ol, 1e6*data_tomo_1rpm_m3.Rx_rms_ol, 1e6*data_tomo_1rpm_m3.Ry_rms_ol], ...
+              {'$m_0$', '$m_1$', '$m_2$', '$m_3$'}, {'$D_x$ [$\mu m$]', '$D_y$ [$\mu m$]', '$D_z$ [$nm$]', '$R_x$ [$\mu\text{rad}$]', '$R_y$ [$\mu\text{rad}$]'}, ' %.0f ');
+#+end_src
+
+#+name: tab:id31_tomo_1rpm_robust_ol_errors
+#+caption: Measured error during open-loop tomography scans (1rpm)
+#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
+#+attr_latex: :center t :booktabs t
+#+RESULTS:
+|       | $D_x$ [$\mu m$] | $D_y$ [$\mu m$] | $D_z$ [$nm$] | $R_x$ [$\mu\text{rad}$] | $R_y$ [$\mu\text{rad}$] |
+|-------+-----------------+-----------------+--------------+-------------------------+-------------------------|
+| $m_0$ |               6 |               6 |           32 |                      34 |                      34 |
+| $m_1$ |               6 |               7 |           26 |                      51 |                      55 |
+| $m_2$ |              36 |              38 |           36 |                     259 |                     253 |
+| $m_3$ |              31 |              33 |           38 |                     214 |                     203 |
+
+#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
+data2orgtable([1e9*data_tomo_1rpm_m0.Dx_rms_cl, 1e9*data_tomo_1rpm_m0.Dy_rms_cl, 1e9*data_tomo_1rpm_m0.Dz_rms_cl, 1e9*data_tomo_1rpm_m0.Rx_rms_cl, 1e9*data_tomo_1rpm_m0.Ry_rms_cl; ...
+               1e9*data_tomo_1rpm_m1.Dx_rms_cl, 1e9*data_tomo_1rpm_m1.Dy_rms_cl, 1e9*data_tomo_1rpm_m1.Dz_rms_cl, 1e9*data_tomo_1rpm_m1.Rx_rms_cl, 1e9*data_tomo_1rpm_m1.Ry_rms_cl; ...
+               1e9*data_tomo_1rpm_m2.Dx_rms_cl, 1e9*data_tomo_1rpm_m2.Dy_rms_cl, 1e9*data_tomo_1rpm_m2.Dz_rms_cl, 1e9*data_tomo_1rpm_m2.Rx_rms_cl, 1e9*data_tomo_1rpm_m2.Ry_rms_cl; ...
+               1e9*data_tomo_1rpm_m3.Dx_rms_cl, 1e9*data_tomo_1rpm_m3.Dy_rms_cl, 1e9*data_tomo_1rpm_m3.Dz_rms_cl, 1e9*data_tomo_1rpm_m3.Rx_rms_cl, 1e9*data_tomo_1rpm_m3.Ry_rms_cl], ...
+              {'$m_0$', '$m_1$', '$m_2$', '$m_3$'}, {'$D_x$ [nm]', '$D_y$ [nm]', '$D_z$ [nm]', '$R_x$ [nrad]', '$R_y$ [nrad]'}, ' %.0f ');
+#+end_src
+
+#+name: tab:id31_tomo_1rpm_robust_cl_errors
+#+caption: Measured error during closed-loop tomography scans (1rpm, robust controller)
+#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
+#+attr_latex: :center t :booktabs t
+#+RESULTS:
+|       | $D_x$ [nm] | $D_y$ [nm] | $D_z$ [nm] | $R_x$ [nrad] | $R_y$ [nrad] |
+|-------+------------+------------+------------+--------------+--------------|
+| $m_0$ |         13 |         15 |          5 |           57 |           55 |
+| $m_1$ |         16 |         25 |          6 |          102 |           55 |
+| $m_2$ |         25 |         25 |          7 |          120 |          103 |
+| $m_3$ |         40 |         53 |          9 |          225 |          169 |
+
+*** TODO Slow Tomography Scans Comparison of control performances :noexport:
+#+begin_src matlab
+% Decentralized in the frame of the struts
+data = load(sprintf("%s/scans/2023-08-18_10-43_m0_1rpm.mat", mat_dir));
+data.time = Ts*[0:length(data.Rz)-1];
+
+% Rotating cartesian frame
+data_cart = load(sprintf("%s/scans/2023-08-18_18-33_m0_1rpm_K_cart.mat", mat_dir));
+data_cart.time = Ts*[0:length(data_cart.Rz)-1];
+
+% Fixed cartesian frame
+data_cart_fixed = load(sprintf("%s/scans/2023-08-18_19-08_m0_1rpm_K_cart_fixed.mat", mat_dir));
+data_cart_fixed.time = Ts*[0:length(data_cart_fixed.Rz)-1];
+
+% Fixed cartesian frame with Complementary Filters
+data_cf = load(sprintf("%s/scans/2023-08-21_14-28_m0_1rpm_K_cf.mat", mat_dir));
+data_cf.time = Ts*[0:length(data_cf.Rz)-1];
+#+end_src
+
+#+begin_src matlab
+1e9*rms(data.Dx_int(data.time<45))
+1e9*rms(data_cart.Dx_int(data_cart.time<45))
+1e9*rms(data_cart_fixed.Dx_int(data_cart_fixed.time<45))
+1e9*rms(data_cf.Dx_int(data_cf.time<45))
+#+end_src
+
+#+begin_src matlab
+1e9*rms(data.Dy_int(data.time<45))
+1e9*rms(data_cart.Dy_int(data_cart.time<45))
+1e9*rms(data_cart_fixed.Dy_int(data_cart_fixed.time<45))
+1e9*rms(data_cf.Dy_int(data_cf.time<45))
+#+end_src
+
+#+begin_src matlab
+1e9*rms(data.Dz_int(data.time<45))
+1e9*rms(data_cart.Dz_int(data_cart.time<45))
+1e9*rms(data_cart_fixed.Dz_int(data_cart_fixed.time<45))
+1e9*rms(data_cf.Dz_int(data_cf.time<45))
+#+end_src
+
+#+begin_src matlab
+1e9*rms(data.Rx_int(data.time<45))
+1e9*rms(data_cart.Rx_int(data_cart.time<45))
+1e9*rms(data_cart_fixed.Rx_int(data_cart_fixed.time<45))
+1e9*rms(data_cf.Rx_int(data_cf.time<45))
+#+end_src
+
+#+begin_src matlab
+1e9*rms(data.Ry_int(data.time<45))
+1e9*rms(data_cart.Ry_int(data_cart.time<45))
+1e9*rms(data_cart_fixed.Ry_int(data_cart_fixed.time<45))
+1e9*rms(data_cf.Ry_int(data_cf.time<45))
+#+end_src
+
+#+begin_src matlab
+figure;
+
+hold on;
+plot(data.time, data.Dy_int)
+plot(data_cart.time, data_cart.Dy_int)
+plot(data_cart_fixed.time, data_cart_fixed.Dy_int)
+plot(data_cf.time, data_cf.Dy_int)
+hold off;
+#+end_src
+
+#+begin_src matlab :exports none
+% Hannning Windows
+Nfft = floor(10.0/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+
+[data.pxx_Dx, data.f] = pwelch(detrend(data.Dx_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts);
+[data.pxx_Dy, ~     ] = pwelch(detrend(data.Dy_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts);
+[data.pxx_Dz, ~     ] = pwelch(detrend(data.Dz_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts);
+[data.pxx_Rx, ~     ] = pwelch(detrend(data.Rx_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts);
+[data.pxx_Ry, ~     ] = pwelch(detrend(data.Ry_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts);
+
+[data_cart.pxx_Dx, data_cart.f] = pwelch(detrend(data_cart.Dx_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts);
+[data_cart.pxx_Dy, ~          ] = pwelch(detrend(data_cart.Dy_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts);
+[data_cart.pxx_Dz, ~          ] = pwelch(detrend(data_cart.Dz_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts);
+[data_cart.pxx_Rx, ~          ] = pwelch(detrend(data_cart.Rx_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts);
+[data_cart.pxx_Ry, ~          ] = pwelch(detrend(data_cart.Ry_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts);
+
+[data_cart_fixed.pxx_Dx, data_cart_fixed.f] = pwelch(detrend(data_cart_fixed.Dx_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts);
+[data_cart_fixed.pxx_Dy, ~                ] = pwelch(detrend(data_cart_fixed.Dy_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts);
+[data_cart_fixed.pxx_Dz, ~                ] = pwelch(detrend(data_cart_fixed.Dz_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts);
+[data_cart_fixed.pxx_Rx, ~                ] = pwelch(detrend(data_cart_fixed.Rx_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts);
+[data_cart_fixed.pxx_Ry, ~                ] = pwelch(detrend(data_cart_fixed.Ry_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts);
+
+[data_cf.pxx_Dx, data_cf.f] = pwelch(detrend(data_cf.Dx_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts);
+[data_cf.pxx_Dy, ~        ] = pwelch(detrend(data_cf.Dy_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts);
+[data_cf.pxx_Dz, ~        ] = pwelch(detrend(data_cf.Dz_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts);
+[data_cf.pxx_Rx, ~        ] = pwelch(detrend(data_cf.Rx_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts);
+[data_cf.pxx_Ry, ~        ] = pwelch(detrend(data_cf.Ry_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts);
+#+end_src
+
+#+begin_src matlab
+figure;
+
+hold on;
+plot(data.f, data.pxx_Dy)
+plot(data_cart.f, data_cart.pxx_Dy)
+plot(data_cart_fixed.f, data_cart_fixed.pxx_Dy)
+plot(data_cf.f, data_cf.pxx_Dy)
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+% legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+xlim([0.1, 5e2]);
+#+end_src
+
+#+begin_src matlab
+figure;
+
+hold on;
+plot(data.f, sqrt(flip(-cumtrapz(flip(data.f), flip(data.pxx_Dy)))))
+plot(data_cart.f, sqrt(flip(-cumtrapz(flip(data_cart.f), flip(data_cart.pxx_Dy)))))
+plot(data_cart_fixed.f, sqrt(flip(-cumtrapz(flip(data_cart_fixed.f), flip(data_cart_fixed.pxx_Dy)))))
+plot(data_cf.f, sqrt(flip(-cumtrapz(flip(data_cf.f), flip(data_cf.pxx_Dy)))))
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
+% legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
+xlim([0.1, 5e2]);
+#+end_src
+
+*** Robust Control - 6rpm
+#+begin_src matlab
+data_tomo_6rpm_m0 = load(sprintf("%s/scans/2023-08-11_11-31_tomography_6rpm_m0.mat", mat_dir));
+data_tomo_6rpm_m0.time = Ts*[0:length(data_tomo_6rpm_m0.Rz)-1];
+#+end_src
+
+#+begin_src matlab
+data_tomo_6rpm_m1 = load(sprintf("%s/scans/2023-08-11_11-23_tomography_6rpm_m1.mat", mat_dir));
+data_tomo_6rpm_m1.time = Ts*[0:length(data_tomo_6rpm_m1.Rz)-1];
+#+end_src
+
+#+begin_src matlab
+%% Compute RMS values while in closed-loop
+[~, i_m0] = find(data_tomo_6rpm_m0.hac_status == 1);
+data_tomo_6rpm_m0.Dx_rms_cl = rms(detrend(data_tomo_6rpm_m0.Dx_int(i_m0+50000:end), 0));
+data_tomo_6rpm_m0.Dy_rms_cl = rms(detrend(data_tomo_6rpm_m0.Dy_int(i_m0+50000:end), 0));
+data_tomo_6rpm_m0.Dz_rms_cl = rms(detrend(data_tomo_6rpm_m0.Dz_int(i_m0+50000:end), 0));
+data_tomo_6rpm_m0.Rx_rms_cl = rms(detrend(data_tomo_6rpm_m0.Rx_int(i_m0+50000:end), 0));
+data_tomo_6rpm_m0.Ry_rms_cl = rms(detrend(data_tomo_6rpm_m0.Ry_int(i_m0+50000:end), 0));
+
+data_tomo_6rpm_m0.Dx_rms_ol = rms(detrend(data_tomo_6rpm_m0.Dx_int(1:i_m0), 0));
+data_tomo_6rpm_m0.Dy_rms_ol = rms(detrend(data_tomo_6rpm_m0.Dy_int(1:i_m0), 0));
+data_tomo_6rpm_m0.Dz_rms_ol = rms(detrend(data_tomo_6rpm_m0.Dz_int(1:i_m0), 0));
+data_tomo_6rpm_m0.Rx_rms_ol = rms(detrend(data_tomo_6rpm_m0.Rx_int(1:i_m0), 0));
+data_tomo_6rpm_m0.Ry_rms_ol = rms(detrend(data_tomo_6rpm_m0.Ry_int(1:i_m0), 0));
+
+%% Compute RMS values while in closed-loop
+[~, i_m1] = find(data_tomo_6rpm_m1.hac_status == 1);
+data_tomo_6rpm_m1.Dx_rms_cl = rms(detrend(data_tomo_6rpm_m1.Dx_int(i_m1+50000:end), 0));
+data_tomo_6rpm_m1.Dy_rms_cl = rms(detrend(data_tomo_6rpm_m1.Dy_int(i_m1+50000:end), 0));
+data_tomo_6rpm_m1.Dz_rms_cl = rms(detrend(data_tomo_6rpm_m1.Dz_int(i_m1+50000:end), 0));
+data_tomo_6rpm_m1.Rx_rms_cl = rms(detrend(data_tomo_6rpm_m1.Rx_int(i_m1+50000:end), 0));
+data_tomo_6rpm_m1.Ry_rms_cl = rms(detrend(data_tomo_6rpm_m1.Ry_int(i_m1+50000:end), 0));
+
+data_tomo_6rpm_m1.Dx_rms_ol = rms(detrend(data_tomo_6rpm_m1.Dx_int(1:i_m1), 0));
+data_tomo_6rpm_m1.Dy_rms_ol = rms(detrend(data_tomo_6rpm_m1.Dy_int(1:i_m1), 0));
+data_tomo_6rpm_m1.Dz_rms_ol = rms(detrend(data_tomo_6rpm_m1.Dz_int(1:i_m1), 0));
+data_tomo_6rpm_m1.Rx_rms_ol = rms(detrend(data_tomo_6rpm_m1.Rx_int(1:i_m1), 0));
+data_tomo_6rpm_m1.Ry_rms_ol = rms(detrend(data_tomo_6rpm_m1.Ry_int(1:i_m1), 0));
+#+end_src
+
+#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
+data2orgtable([1e6*data_tomo_6rpm_m0.Dx_rms_ol, 1e6*data_tomo_6rpm_m0.Dy_rms_ol, 1e9*data_tomo_6rpm_m0.Dz_rms_ol, 1e6*data_tomo_6rpm_m0.Rx_rms_ol, 1e6*data_tomo_6rpm_m0.Ry_rms_ol; ...
+               1e6*data_tomo_6rpm_m1.Dx_rms_ol, 1e6*data_tomo_6rpm_m1.Dy_rms_ol, 1e9*data_tomo_6rpm_m1.Dz_rms_ol, 1e6*data_tomo_6rpm_m1.Rx_rms_ol, 1e6*data_tomo_6rpm_m1.Ry_rms_ol], ...
+              {'$m_0$', '$m_1$'}, {'$D_x$ [$\mu m$]', '$D_y$ [$\mu m$]', '$D_z$ [$nm$]', '$R_x$ [$\mu\text{rad}$]', '$R_y$ [$\mu\text{rad}$]'}, ' %.0f ');
+#+end_src
+
+#+name: tab:id31_tomo_6rpm_robust_ol_errors
+#+caption: Measured error during open-loop tomography scans (6rpm)
+#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
+#+attr_latex: :center t :booktabs t
+#+RESULTS:
+|       | $D_x$ [$\mu m$] | $D_y$ [$\mu m$] | $D_z$ [$nm$] | $R_x$ [$\mu\text{rad}$] | $R_y$ [$\mu\text{rad}$] |
+|-------+-----------------+-----------------+--------------+-------------------------+-------------------------|
+| $m_0$ |               8 |               7 |           20 |                      41 |                      41 |
+| $m_1$ |               4 |               4 |           21 |                      39 |                      39 |
+
+#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
+data2orgtable([1e9*data_tomo_6rpm_m0.Dx_rms_cl, 1e9*data_tomo_6rpm_m0.Dy_rms_cl, 1e9*data_tomo_6rpm_m0.Dz_rms_cl, 1e9*data_tomo_6rpm_m0.Rx_rms_cl, 1e9*data_tomo_6rpm_m0.Ry_rms_cl; ...
+               1e9*data_tomo_6rpm_m1.Dx_rms_cl, 1e9*data_tomo_6rpm_m1.Dy_rms_cl, 1e9*data_tomo_6rpm_m1.Dz_rms_cl, 1e9*data_tomo_6rpm_m1.Rx_rms_cl, 1e9*data_tomo_6rpm_m1.Ry_rms_cl], ...
+              {'$m_0$', '$m_1$'}, {'$D_x$ [nm]', '$D_y$ [nm]', '$D_z$ [nm]', '$R_x$ [nrad]', '$R_y$ [nrad]'}, ' %.0f ');
+#+end_src
+
+#+name: tab:id31_tomo_6rpm_robust_cl_errors
+#+caption: Measured error during closed-loop tomography scans (6rpm, robust controller)
+#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
+#+attr_latex: :center t :booktabs t
+#+RESULTS:
+|       | $D_x$ [nm] | $D_y$ [nm] | $D_z$ [nm] | $R_x$ [nrad] | $R_y$ [nrad] |
+|-------+------------+------------+------------+--------------+--------------|
+| $m_0$ |         17 |         19 |          5 |           70 |           73 |
+| $m_1$ |         20 |         26 |          7 |          110 |           77 |
+
+*** TODO Medium velocity tomography scans with CF control         :noexport:
+#+begin_src matlab
+data_m1_cf = load(sprintf("%s/scans/2023-08-21_19-18_m1_6rpm_cf_control.mat", mat_dir));
+data_m1_cf.time = Ts*[0:length(data_m1_cf.Rz)-1];
+#+end_src
+
+#+begin_src matlab
+data_m2_cf = load(sprintf("%s/scans/2023-08-21_18-07_m2_6rpm_cf_control.mat", mat_dir));
+data_m2_cf.time = Ts*[0:length(data_m2_cf.Rz)-1];
+#+end_src
+
+And higher bandwidth:
+#+begin_src matlab
+data_m1_cf_high_fb = load(sprintf("%s/scans/2023-08-21_19-24_m1_6rpm_cf_control_60Hz.mat", mat_dir));
+data_m1_cf_high_fb.time = Ts*[0:length(data_m1_cf_high_fb.Rz)-1];
+#+end_src
+
+#+begin_src matlab
+figure;
+hold on;
+plot(data_m1_cf.Dy_int, detrend(data_m1_cf.Dz_int, 0))
+plot(data_m2_cf.Dy_int, detrend(data_m2_cf.Dz_int, 0))
+plot(data_m1_cf_high_fb.Dy_int, detrend(data_m1_cf_high_fb.Dz_int, 0))
+axis equal
+#+end_src
+
+#+begin_src matlab
+1e9*rms(detrend(data_m1.Dz_int(i_m1+50000:end), 0))
+1e9*rms(detrend(data_m1.Dy_int(i_m1+50000:end), 0))
+1e9*rms(detrend(data_m1.Ry_int(i_m1+50000:end), 0))
+#+end_src
+
+#+begin_src matlab
+1e9*rms(detrend(data_m1_cf.Dz_int, 0))
+1e9*rms(detrend(data_m1_cf.Dy_int, 0))
+1e9*rms(detrend(data_m1_cf.Ry_int, 0))
+#+end_src
+
+#+begin_src matlab
+1e9*rms(detrend(data_m2.Dz_int, 0))
+1e9*rms(detrend(data_m2.Dy_int, 0))
+1e9*rms(detrend(data_m2.Ry_int, 0))
+#+end_src
+
+#+begin_src matlab
+1e9*rms(detrend(data_m1_high_fb.Dz_int, 0))
+1e9*rms(detrend(data_m1_high_fb.Dy_int, 0))
+1e9*rms(detrend(data_m1_high_fb.Ry_int, 0))
+#+end_src
+
+*** Robust Control - 30rpm
+#+begin_src matlab
+%% Load Data
+data_tomo_30rpm_m0 = load(sprintf("%s/scans/2023-08-17_15-26_tomography_30rpm_m0_robust.mat", mat_dir));
+data_tomo_30rpm_m0.time = Ts*[0:length(data_tomo_30rpm_m0.Rz)-1];
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Measured motion during tomography scan at 30RPM with a robust controller
+figure;
+tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile;
+hold on;
+plot(1e6*data_tomo_30rpm_m0.Dx_int(data_tomo_30rpm_m0.hac_status == 0), 1e6*data_tomo_30rpm_m0.Dy_int(data_tomo_30rpm_m0.hac_status == 0), ...
+    'DisplayName', 'OL')
+plot(1e6*data_tomo_30rpm_m0.Dx_int(data_tomo_30rpm_m0.hac_status == 1), 1e6*data_tomo_30rpm_m0.Dy_int(data_tomo_30rpm_m0.hac_status == 1), ...
+    'DisplayName', 'CL')
+hold off;
+axis square
+xlabel("X motion [$\mu m$]");
+ylabel("Y motion [$\mu m$]");
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+xlim([-3, 3])
+ylim([-3, 3])
+
+ax2 = nexttile;
+hold on;
+plot(1e6*data_tomo_30rpm_m0.Dy_int(data_tomo_30rpm_m0.hac_status == 0), 1e6*data_tomo_30rpm_m0.Dz_int(data_tomo_30rpm_m0.hac_status == 0), ...
+    'DisplayName', 'OL')
+plot(1e6*data_tomo_30rpm_m0.Dy_int(data_tomo_30rpm_m0.hac_status == 1), 1e6*data_tomo_30rpm_m0.Dz_int(data_tomo_30rpm_m0.hac_status == 1), ...
+    'DisplayName', 'CL')
+hold off;
+axis equal
+xlim([-3, 3])
+ylim([-3, 3])
+xlabel("Y motion [$\mu m$]");
+ylabel("Z motion [$\mu m$]");
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_tomography_m0_30rpm_robust_xyz.pdf', 'width', 'full', 'height', 'tall');
+#+end_src
+
+#+name: fig:id31_tomography_m0_30rpm_robust_xyz
+#+caption: Measured motion during tomography scan at 30RPM with a robust controller
+#+RESULTS:
+[[file:figs/id31_tomography_m0_30rpm_robust_xyz.png]]
+
+#+begin_src matlab
+%% Compute RMS values while in closed-loop
+[~, i_m0] = find(data_tomo_30rpm_m0.hac_status == 1);
+data_tomo_30rpm_m0.Dx_rms_cl = rms(detrend(data_tomo_30rpm_m0.Dx_int(i_m0+50000:end), 0));
+data_tomo_30rpm_m0.Dy_rms_cl = rms(detrend(data_tomo_30rpm_m0.Dy_int(i_m0+50000:end), 0));
+data_tomo_30rpm_m0.Dz_rms_cl = rms(detrend(data_tomo_30rpm_m0.Dz_int(i_m0+50000:end), 0));
+data_tomo_30rpm_m0.Rx_rms_cl = rms(detrend(data_tomo_30rpm_m0.Rx_int(i_m0+50000:end), 0));
+data_tomo_30rpm_m0.Ry_rms_cl = rms(detrend(data_tomo_30rpm_m0.Ry_int(i_m0+50000:end), 0));
+
+data_tomo_30rpm_m0.Dx_rms_ol = rms(detrend(data_tomo_30rpm_m0.Dx_int(1:i_m0), 0));
+data_tomo_30rpm_m0.Dy_rms_ol = rms(detrend(data_tomo_30rpm_m0.Dy_int(1:i_m0), 0));
+data_tomo_30rpm_m0.Dz_rms_ol = rms(detrend(data_tomo_30rpm_m0.Dz_int(1:i_m0), 0));
+data_tomo_30rpm_m0.Rx_rms_ol = rms(detrend(data_tomo_30rpm_m0.Rx_int(1:i_m0), 0));
+data_tomo_30rpm_m0.Ry_rms_ol = rms(detrend(data_tomo_30rpm_m0.Ry_int(1:i_m0), 0));
+#+end_src
+
+#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
+data2orgtable([1e6*data_tomo_30rpm_m0.Dx_rms_ol, 1e6*data_tomo_30rpm_m0.Dy_rms_ol, 1e9*data_tomo_30rpm_m0.Dz_rms_ol, 1e6*data_tomo_30rpm_m0.Rx_rms_ol, 1e6*data_tomo_30rpm_m0.Ry_rms_ol], ...
+              {'$m_0$'}, {'$D_x$ [$\mu m$]', '$D_y$ [$\mu m$]', '$D_z$ [$nm$]', '$R_x$ [$\mu\text{rad}$]', '$R_y$ [$\mu\text{rad}$]'}, ' %.0f ');
+#+end_src
+
+#+name: tab:id31_tomo_30rpm_robust_ol_errors
+#+caption: Measured error during open-loop tomography scans (30rpm)
+#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
+#+attr_latex: :center t :booktabs t
+#+RESULTS:
+|       | $D_x$ [$\mu m$] | $D_y$ [$\mu m$] | $D_z$ [$nm$] | $R_x$ [$\mu\text{rad}$] | $R_y$ [$\mu\text{rad}$] |
+|-------+-----------------+-----------------+--------------+-------------------------+-------------------------|
+| $m_0$ |               2 |               2 |           24 |                      10 |                      10 |
+
+#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
+data2orgtable([1e9*data_tomo_30rpm_m0.Dx_rms_cl, 1e9*data_tomo_30rpm_m0.Dy_rms_cl, 1e9*data_tomo_30rpm_m0.Dz_rms_cl, 1e9*data_tomo_30rpm_m0.Rx_rms_cl, 1e9*data_tomo_30rpm_m0.Ry_rms_cl], ...
+              {'$m_0$'}, {'$D_x$ [nm]', '$D_y$ [nm]', '$D_z$ [nm]', '$R_x$ [nrad]', '$R_y$ [nrad]'}, ' %.0f ');
+#+end_src
+
+#+name: tab:id31_tomo_30rpm_robust_cl_errors
+#+caption: Measured error during closed-loop tomography scans (30rpm, robust controller)
+#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
+#+attr_latex: :center t :booktabs t
+#+RESULTS:
+|       | $D_x$ [nm] | $D_y$ [nm] | $D_z$ [nm] | $R_x$ [nrad] | $R_y$ [nrad] |
+|-------+------------+------------+------------+--------------+--------------|
+| $m_0$ |         34 |         38 |         10 |          127 |          129 |
+
+#+begin_src matlab :exports none
+yztomography3dmovie('movies/id31_tomography_m0_30rpm_robust_xyz.avi', data_tomo_30rpm_m0, 'di', 300);
+#+end_src
+
+#+begin_src matlab :exports none
+yztomographymovie('movies/tomography_30rpm_m0_robust', data_tomo_30rpm_m0, 'xlim_ax1', [-3, 3], 'ylim_ax1', [-3, 3], 'xlim_ax2', [-300, 300], 'ylim_ax2', [-300, 300])
+#+end_src
+
+*** TODO Fast Tomography Scan with Complementary Filter Controller :noexport:
+#+begin_src matlab
+data_cf = load(sprintf("%s/scans/2023-08-21_14-33_m0_30rpm_cf_control.mat", mat_dir));
+data_cf.time = Ts*[0:length(data_cf.Rz)-1];
+#+end_src
+
+#+begin_src matlab
+[~, i0] = find(data.hac_status == 1);
+1e9*rms(data.Dy_int(i0(1)+5000:end))
+1e9*rms(data.Dz_int(i0(1)+5000:end))
+
+1e9*rms(data_cf.Dy_int)
+1e9*rms(data_cf.Dz_int)
+#+end_src
+
+Same performance than the robust controller in terms of RMS residual motion.
+
+#+begin_src matlab
+figure; plot3(data.Dx_int, data.Dy_int, data.Dz_int)
+#+end_src
+
+*** Tomography - Effect of the scanning velocity                  :noexport:
+- [ ] What are the controller used here? Why worst results than with the robust controller?
+
+#+begin_src matlab
+data_1rpm = load(sprintf("%s/scans/2023-08-18_10-43_m0_1rpm.mat", mat_dir));
+data_1rpm.time = Ts*[0:length(data_1rpm.Rz)-1];
+#+end_src
+
+#+begin_src matlab
+data_30rpm = load(sprintf("%s/scans/2023-08-18_10-45_m0_30rpm.mat", mat_dir));
+data_30rpm.time = Ts*[0:length(data_30rpm.Rz)-1];
+#+end_src
+
+#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
+data2orgtable([1e9*rms(detrend(data_1rpm.Dy_int, 0)), 1e9*rms(detrend(data_30rpm.Dy_int, 0)); 1e9*rms(detrend(data_1rpm.Dz_int, 0)), 1e9*rms(detrend(data_30rpm.Dz_int, 0)); 1e9*rms(detrend(data_1rpm.Ry_int, 0)), 1e9*rms(detrend(data_30rpm.Ry_int, 0))]', {'1RPM', '30RPM'}, {'$D_y$', '$D_z$', '$R_y$'}, ' %.1f ');
+#+end_src
+
+#+name: tab:id31_tomography_effect_velocity_rms
+#+caption: RMS values of the errors during tomography scan - Effect of $R_z$ velocity
+#+attr_latex: :environment tabularx :width 0.5\linewidth :align lXXX
+#+attr_latex: :center t :booktabs t
+#+RESULTS:
+|       | $D_y$ | $D_z$ | $R_y$ |
+|-------+-------+-------+-------|
+| 1RPM  |  30.9 |   5.9 |  92.4 |
+| 30RPM |  71.7 |  12.5 | 301.3 |
+
+
+#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
+data2orgtable([1e9*data_tomo_1rpm_m0.Dx_rms_cl, 1e9*data_tomo_1rpm_m0.Dy_rms_cl, 1e9*data_tomo_1rpm_m0.Dz_rms_cl, 1e9*data_tomo_1rpm_m0.Rx_rms_cl, 1e9*data_tomo_1rpm_m0.Ry_rms_cl; ...
+               1e9*data_tomo_6rpm_m0.Dx_rms_cl, 1e9*data_tomo_6rpm_m0.Dy_rms_cl, 1e9*data_tomo_6rpm_m0.Dz_rms_cl, 1e9*data_tomo_6rpm_m0.Rx_rms_cl, 1e9*data_tomo_6rpm_m0.Ry_rms_cl; ...
+               1e9*data_tomo_30rpm_m0.Dx_rms_cl, 1e9*data_tomo_30rpm_m0.Dy_rms_cl, 1e9*data_tomo_30rpm_m0.Dz_rms_cl, 1e9*data_tomo_30rpm_m0.Rx_rms_cl, 1e9*data_tomo_30rpm_m0.Ry_rms_cl], ...
+              {'1rpm', '6rpm', '30rpm'}, {'$D_x$ [$\mu m$]', '$D_y$ [$\mu m$]', '$D_z$ [$nm$]', '$R_x$ [$\mu\text{rad}$]', '$R_y$ [$\mu\text{rad}$]'}, ' %.0f ');
+#+end_src
+
+#+RESULTS:
+|       | $D_x$ [$\mu m$] | $D_y$ [$\mu m$] | $D_z$ [$nm$] | $R_x$ [$\mu\text{rad}$] | $R_y$ [$\mu\text{rad}$] |
+|-------+-----------------+-----------------+--------------+-------------------------+-------------------------|
+| 1rpm  |              13 |              15 |            5 |                      57 |                      55 |
+| 6rpm  |              17 |              19 |            5 |                      70 |                      73 |
+| 30rpm |              34 |              38 |           10 |                     127 |                     129 |
+
+** $D_z$ scans: Dirty Layer Scans
+<<sec:id31_scans_dz>>
+*** Step by Step $D_z$ motion
+#+begin_src matlab
+%% Load Dz MIM data
+data_dz_steps_3nm = load(sprintf("%s/scans/2023-08-18_14-57_dz_mim_3_nm.mat", mat_dir));
+data_dz_steps_3nm.time = Ts*[0:length(data_dz_steps_3nm.Dz_int)-1];
+
+data_dz_steps_10nm = load(sprintf("%s/scans/2023-08-18_14-57_dz_mim_10_nm.mat", mat_dir));
+data_dz_steps_10nm.time = Ts*[0:length(data_dz_steps_10nm.Dz_int)-1];
+
+data_dz_steps_100nm = load(sprintf("%s/scans/2023-08-18_14-57_dz_mim_100_nm.mat", mat_dir));
+data_dz_steps_100nm.time = Ts*[0:length(data_dz_steps_100nm.Dz_int)-1];
+
+data_dz_steps_1000nm = load(sprintf("%s/scans/2023-08-18_14-57_dz_mim_1000_nm.mat", mat_dir));
+data_dz_steps_1000nm.time = Ts*[0:length(data_dz_steps_1000nm.Dz_int)-1];
+#+end_src
+
+Three step sizes are tested:
+- $10\,nm$ steps (Figure ref:fig:id31_dz_mim_10nm_steps)
+- $100\,nm$ steps (Figure ref:fig:id31_dz_mim_100nm_steps)
+- $1\,\mu m$ steps (Figure ref:fig:id31_dz_steps_response)
+
+#+begin_src matlab :exports none :results none
+%% Dz MIM test with 10nm steps
+figure;
+hold on;
+plot(data_dz_steps_10nm.time, 1e9*(data_dz_steps_10nm.Dz_int - mean(data_dz_steps_10nm.Dz_int(1:1000))))
+hold off;
+xlabel('Time [s]');
+ylabel('$D_z$ Motion [nm]');
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_dz_mim_10nm_steps.pdf', 'width', 'wide', 'height', 'normal');
+#+end_src
+
+#+name: fig:id31_dz_mim_10nm_steps
+#+caption: Dz MIM test with 10nm steps (low pass filter with cut-off frequency of 10Hz is applied)
+#+RESULTS:
+[[file:figs/id31_dz_mim_10nm_steps.png]]
+
+#+begin_src matlab :exports none :results none
+%% Dz MIM test with 10nm steps
+figure;
+hold on;
+plot(data_dz_steps_100nm.time, 1e9*(data_dz_steps_100nm.Dz_int - mean(data_dz_steps_100nm.Dz_int(1:1000))))
+hold off;
+xlabel('Time [s]');
+ylabel('$D_z$ Motion [nm]');
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_dz_mim_100nm_steps.pdf', 'width', 'wide', 'height', 'normal');
+#+end_src
+
+#+name: fig:id31_dz_mim_100nm_steps
+#+caption: Dz MIM test with 100nm steps
+#+RESULTS:
+[[file:figs/id31_dz_mim_100nm_steps.png]]
+
+#+begin_src matlab :exports none :results none
+%% Dz step response - Stabilization time is around 70ms
+figure;
+[~, i] = find(data_dz_steps_1000nm.m_hexa_dz>data_dz_steps_1000nm.m_hexa_dz(1));
+i0 = i(1);
+
+figure;
+hold on;
+plot(data_dz_steps_1000nm.time-data_dz_steps_1000nm.time(i0), 1e9*(data_dz_steps_1000nm.Dz_int - mean(data_dz_steps_1000nm.Dz_int(1:1000))))
+plot([-1, 1], [1000-20, 1000-20], 'k--')
+plot([-1, 1], [1000+20, 1000+20], 'k--')
+xline(0, 'k--', 'LineWidth', 1.5)
+xline(0.07, 'k--', 'LineWidth', 1.5)
+hold off;
+xlabel('Time [s]');
+ylabel('$D_z$ Motion [nm]');
+xlim([-0.1, 0.2]);
+ylim([-100, 1600])
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_dz_steps_response.pdf', 'width', 'wide', 'height', 'normal');
+#+end_src
+
+#+name: fig:id31_dz_steps_response
+#+caption: $D_z$ step response - Stabilization time is around 70ms
+#+RESULTS:
+[[file:figs/id31_dz_steps_response.png]]
+
+
+*** Continuous $D_z$ motion: Dirty Layer Scans
+#+begin_src matlab
+data_dz_10ums = load(sprintf("%s/scans/2023-08-18_15-33_dirty_layer_m0_small.mat", mat_dir));
+data_dz_10ums.time = Ts*[0:length(data_dz_10ums.Dz_int)-1];
+#+end_src
+
+#+begin_src matlab
+data_dz_100ums = load(sprintf("%s/scans/2023-08-18_15-32_dirty_layer_m0.mat", mat_dir));
+data_dz_100ums.time = Ts*[0:length(data_dz_100ums.Dz_int)-1];
+#+end_src
+
+Two $D_z$ scans are performed:
+- at $10\,\mu m/s$ in Figure ref:fig:id31_dirty_layer_scan_m0
+- at $100\,\mu m/s$ in Figure ref:fig:id31_dirty_layer_scan_m0_large
+
+#+begin_src matlab :exports none :results none
+%% Dirty layer scan: Dz motion
+figure;
+hold on;
+plot(data_dz_10ums.time, 1e6*data_dz_10ums.Dz_int, ...
+    'DisplayName', sprintf('$\\epsilon D_z = %.0f$ nm RMS', 1e9*rms(data_dz_10ums.e_dz)))
+plot(data_dz_10ums.time, 1e6*data_dz_10ums.e_dy, ...
+    'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nm RMS', 1e9*rms(data_dz_10ums.e_dy)))
+plot(data_dz_10ums.time, 1e6*data_dz_10ums.e_ry, ...
+    'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$rad RMS', 1e6*rms(data_dz_10ums.e_ry)))
+hold off;
+xlabel('Time [s]');
+ylabel('Motion [$\mu$m]');
+xlim([0, 2.2])
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_dirty_layer_scan_m0.pdf', 'width', 'wide', 'height', 'normal');
+#+end_src
+
+#+name: fig:id31_dirty_layer_scan_m0
+#+caption: Dirty layer scan: $D_z$ motion at $10\,\mu m/s$
+#+RESULTS:
+[[file:figs/id31_dirty_layer_scan_m0.png]]
+
+#+begin_src matlab :exports none :results none
+%% Dirty layer scan: Dz motion
+figure;
+hold on;
+plot(data_dz_100ums.time, 1e6*data_dz_100ums.Dz_int, ...
+    'DisplayName', sprintf('$\\epsilon D_z = %.0f$ nm RMS', 1e9*rms(data_dz_100ums.e_dz)))
+plot(data_dz_100ums.time, 1e6*data_dz_100ums.e_dy, ...
+    'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nm RMS', 1e9*rms(data_dz_100ums.e_dy)))
+plot(data_dz_100ums.time, 1e6*data_dz_100ums.e_ry, ...
+    'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$rad RMS', 1e6*rms(data_dz_100ums.e_ry)))
+hold off;
+xlabel('Time [s]');
+ylabel('Motion [$\mu$m]');
+xlim([0, 2.2])
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_dirty_layer_scan_m0_large.pdf', 'width', 'wide', 'height', 'normal');
+#+end_src
+
+#+name: fig:id31_dirty_layer_scan_m0_large
+#+caption: Dirty layer scan: $D_z$ motion at $100\,\mu m/s$
+#+RESULTS:
+[[file:figs/id31_dirty_layer_scan_m0_large.png]]
+
+#+begin_src matlab
+%% Not so good results with the CF controller
+data_cf = load(sprintf("%s/scans/2023-08-21_19-20_dirty_layer_m1_cf.mat", mat_dir));
+data_cf.time = Ts*[0:length(data_cf.Dz_int)-1];
+#+end_src
+
+** $R_y$ scans: Reflectivity
+<<sec:id31_scans_reflectivity>>
+#+begin_src matlab
+%% Load data for the reflectivity scan
+data_ry = load(sprintf("%s/scans/2023-08-18_15-24_first_reflectivity_m0.mat", mat_dir));
+data_ry.time = Ts*[0:length(data_ry.Ry_int)-1];
+#+end_src
+
+An $R_y$ scan is performed at $100\,\mu rad/s$ velocity (Figure ref:fig:id31_reflectivity_scan_Ry_100urads).
+During the $R_y$ scan, the errors in $D_y$ are $D_z$ are kept small.
+
+#+begin_src matlab :exports none :results none
+%% Ry reflectivity scan
+figure;
+hold on;
+plot(data_ry.time, 1e6*data_ry.Ry_int, 'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$rad RMS', 1e6*rms(data_ry.e_ry)))
+plot(data_ry.time, 1e6*data_ry.e_dy, 'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nm RMS', 1e9*rms(data_ry.e_dy)))
+plot(data_ry.time, 1e6*data_ry.e_dz, 'DisplayName', sprintf('$\\epsilon D_z = %.0f$ nm RMS', 1e9*rms(data_ry.e_dz)))
+hold off;
+xlabel('Time [s]');
+ylabel('$R_y$ motion [$\mu$rad]')
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+xlim([0, 6.2]);
+ylim([-310, 310])
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_reflectivity_scan_Ry_100urads.pdf', 'width', 'wide', 'height', 'normal');
+#+end_src
+
+#+name: fig:id31_reflectivity_scan_Ry_100urads
+#+caption: $R_y$ reflecitivity scan at $100\,\mu\text{rad}/s$ velocity
+#+RESULTS:
+[[file:figs/id31_reflectivity_scan_Ry_100urads.png]]
+
+
+** $D_y$ Scans
+<<sec:id31_scans_dy>>
+*** Introduction                                                    :ignore:
+The steps generated by the IcePAP for the $T_y$ stage are send to the Speedgoat.
+Then, we can know in real time what is the wanted position in $D_y$ during $T_y$ scans.
+
+*** Open Loop
+#+begin_src matlab
+%% Slow Ty scan (10um/s)
+data_ty_ol_slow = load(sprintf("%s/scans/2023-08-21_20-05_ty_scan_m1_open_loop_slow.mat", mat_dir));
+data_ty_ol_slow.time = Ts*[0:length(data_ty_ol_slow.Dy_int)-1];
+#+end_src
+
+We can clearly see micro-stepping errors of the stepper motor used for the $T_y$ stage.
+The errors have a period of $10\,\mu m$ with an amplitude of $\pm 100\,nm$.
+
+#+begin_src matlab :exports none :results none
+%% Ty scan (at 10um/s) - Dy errors
+figure;
+plot(1e6*data_ty_ol_slow.Ty, 1e6*detrend(data_ty_ol_slow.e_dy, 0))
+xlabel('Ty position [$\mu$m]');
+ylabel('$D_y$ error [$\mu$m]');
+xlim([-100, 100])
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_ty_scan_10ums_ol_dy_errors.pdf', 'width', 'wide', 'height', 'normal');
+#+end_src
+
+#+name: fig:id31_ty_scan_10ums_ol_dy_errors
+#+caption: $T_y$ scan (at $10\,\mu m/s$) - $D_y$ errors. The micro-stepping errors can clearly be seen with a period of $10\,\mu m$ and an amplitude of $\pm 100\,nm$
+#+RESULTS:
+[[file:figs/id31_ty_scan_10ums_ol_dy_errors.png]]
+
+#+begin_src matlab :exports none :results none
+%% Ty scan (at 10um/s) - Dz and Ry errors
+figure;
+tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile;
+hold on;
+plot(1e6*data_ty_ol_slow.Ty, 1e6*detrend(data_ty_ol_slow.e_dz, 0))
+hold off;
+xlabel('Ty position [$\mu$m]');
+ylabel('$D_z$ error [$\mu$m]');
+xlim([-100, 100])
+
+ax2 = nexttile;
+hold on;
+plot(1e6*data_ty_ol_slow.Ty, 1e6*data_ty_ol_slow.e_ry)
+hold off;
+xlabel('Ty position [$\mu$m]');
+ylabel('$R_y$ error [$\mu$rad]');
+xlim([-100, 100])
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_ty_scan_10ums_ol_dz_ry_errors.pdf', 'width', 'full', 'height', 'normal');
+#+end_src
+
+#+name: fig:id31_ty_scan_10ums_ol_dz_ry_errors
+#+caption: $T_y$ scan (at $10\,\mu m/s$) - $D_z$ and $R_y$ errors. The $D_z$ error is most likely due to having the top interferometer pointing to a sphere. The large $R_y$ errors might also be due to the metrology system
+#+RESULTS:
+[[file:figs/id31_ty_scan_10ums_ol_dz_ry_errors.png]]
+
+*** Closed Loop
+#+begin_src matlab
+%% Slow Ty scan (10um/s) - CL
+data_ty_cl_slow = load(sprintf("%s/scans/2023-08-21_20-07_ty_scan_m1_cf_closed_loop_slow.mat", mat_dir));
+data_ty_cl_slow.time = Ts*[0:length(data_ty_cl_slow.Dy_int)-1];
+#+end_src
+
+#+begin_src matlab :exports none :results none
+%% Ty scan (at 10um/s) - Dy errors
+figure;
+hold on;
+plot(1e6*data_ty_ol_slow.Ty, 1e6*detrend(data_ty_ol_slow.e_dy, 0), ...
+     'DisplayName', 'OL')
+plot(1e6*data_ty_cl_slow.Ty, 1e6*detrend(data_ty_cl_slow.e_dy, 0), ...
+     'DisplayName', sprintf('CL - $\\epsilon D_y = %.0f$ nmRMS', 1e9*rms(detrend(data_ty_cl_slow.e_dy, 0))))
+hold off;
+xlabel('Ty position [$\mu$m]');
+ylabel('$D_y$ error [$\mu$m]');
+xlim([-100, 100])
+legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_ty_scan_10ums_cl_dy_errors.pdf', 'width', 'wide', 'height', 'normal');
+#+end_src
+
+#+name: fig:id31_ty_scan_10ums_cl_dy_errors
+#+caption: $T_y$ scan (at $10\,\mu m/s$) - $D_y$ errors. Open-loop and Closed-loop scans
+#+RESULTS:
+[[file:figs/id31_ty_scan_10ums_cl_dy_errors.png]]
+
+#+begin_src matlab :exports none :results none
+%% Ty scan (at 10um/s) - Dz and Ry errors
+figure;
+tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile;
+hold on;
+plot(1e6*data_ty_ol_slow.Ty, 1e6*detrend(data_ty_ol_slow.e_dz, 0), ...
+     'DisplayName', 'OL')
+plot(1e6*data_ty_cl_slow.Ty, 1e6*detrend(data_ty_cl_slow.e_dz, 0), ...
+     'DisplayName', sprintf('Cl - $\\epsilon D_z = %.0f$ nmRMS', 1e9*rms(detrend(data_ty_cl_slow.e_dz, 0))))
+hold off;
+xlabel('Ty position [$\mu$m]');
+ylabel('$D_z$ error [$\mu$m]');
+xlim([-100, 100])
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(1e6*data_ty_ol_slow.Ty, 1e6*data_ty_ol_slow.e_ry, ...
+     'DisplayName', 'OL')
+plot(1e6*data_ty_cl_slow.Ty, 1e6*data_ty_cl_slow.e_ry, ...
+     'DisplayName', sprintf('Cl - $\\epsilon R_y = %.2f$ uradRMS', 1e6*rms(detrend(data_ty_cl_slow.e_ry, 0))))
+hold off;
+xlabel('Ty position [$\mu$m]');
+ylabel('$R_y$ error [$\mu$rad]');
+xlim([-100, 100])
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_ty_scan_10ums_cl_dz_ry_errors.pdf', 'width', 'full', 'height', 'normal');
+#+end_src
+
+#+name: fig:id31_ty_scan_10ums_cl_dz_ry_errors
+#+caption: $T_y$ scan (at $10\,\mu m/s$) - $D_z$ and $R_y$ errors. Open-loop and Closed-loop scans
+#+RESULTS:
+[[file:figs/id31_ty_scan_10ums_cl_dz_ry_errors.png]]
+
+*** Faster Scan
+#+begin_src matlab
+%% Fast Ty scan (100um/s) - OL
+data_ty_ol_fast = load(sprintf("%s/scans/2023-08-21_20-05_ty_scan_m1_open_loop.mat", mat_dir));
+data_ty_ol_fast.time = Ts*[0:length(data_ty_ol_fast.Dy_int)-1];
+#+end_src
+
+#+begin_src matlab
+%% Fast Ty scan (10um/s) - CL
+data_ty_cl_fast = load(sprintf("%s/scans/2023-08-21_20-07_ty_scan_m1_cf_closed_loop.mat", mat_dir));
+data_ty_cl_fast.time = Ts*[0:length(data_ty_cl_fast.Dy_int)-1];
+#+end_src
+
+Because of micro-stepping errors of the Ty stepper motor, when scanning at high velocity this induce high frequency vibration that are outside the bandwidth of the feedback controller.
+
+At $100\,\mu m/s$, the micro-stepping errors with a period of $10\,\mu m$ (see Figure ref:fig:id31_ty_scan_10ums_ol_dy_errors) are at 10Hz.
+These errors are them amplified by some resonances in the system.
+
+This could be easily solved by changing the stepper motor for a torque motor for instance.
+
+#+begin_src matlab :exports none :results none
+%% Ty scan (at 100um/s) - Dy errors
+figure;
+hold on;
+plot(1e6*data_ty_ol_fast.Ty, 1e6*detrend(data_ty_ol_fast.e_dy, 0), ...
+     'DisplayName', 'OL')
+plot(1e6*data_ty_cl_fast.Ty, 1e6*detrend(data_ty_cl_fast.e_dy, 0), ...
+     'DisplayName', sprintf('CL - $\\epsilon D_y = %.0f$ nmRMS', 1e9*rms(detrend(data_ty_cl_fast.e_dy, 0))))
+hold off;
+xlabel('Ty position [$\mu$m]');
+ylabel('$D_y$ error [$\mu$m]');
+xlim([-100, 100])
+legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_ty_scan_100ums_cl_dy_errors.pdf', 'width', 'wide', 'height', 'normal');
+#+end_src
+
+#+name: fig:id31_ty_scan_100ums_cl_dy_errors
+#+caption: $T_y$ scan (at $100\,\mu m/s$) - $D_y$ errors. Open-loop and Closed-loop scans
+#+RESULTS:
+[[file:figs/id31_ty_scan_100ums_cl_dy_errors.png]]
+
+#+begin_src matlab :exports none :results none
+%% Ty scan (at 100um/s) - Dz and Ry errors
+figure;
+tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile;
+hold on;
+plot(1e6*data_ty_ol_fast.Ty, 1e6*detrend(data_ty_ol_fast.e_dz, 0), ...
+     'DisplayName', 'OL')
+plot(1e6*data_ty_cl_fast.Ty, 1e6*detrend(data_ty_cl_fast.e_dz, 0), ...
+     'DisplayName', sprintf('Cl - $\\epsilon D_z = %.0f$ nmRMS', 1e9*rms(detrend(data_ty_cl_fast.e_dz, 0))))
+hold off;
+xlabel('Ty position [$\mu$m]');
+ylabel('$D_z$ error [$\mu$m]');
+xlim([-100, 100])
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+
+ax2 = nexttile;
+hold on;
+plot(1e6*data_ty_ol_fast.Ty, 1e6*data_ty_ol_fast.e_ry, ...
+     'DisplayName', 'OL')
+plot(1e6*data_ty_cl_fast.Ty, 1e6*data_ty_cl_fast.e_ry, ...
+     'DisplayName', sprintf('Cl - $\\epsilon R_y = %.2f$ uradRMS', 1e6*rms(detrend(data_ty_cl_fast.e_ry, 0))))
+hold off;
+xlabel('Ty position [$\mu$m]');
+ylabel('$R_y$ error [$\mu$rad]');
+xlim([-100, 100])
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_ty_scan_100ums_cl_dz_ry_errors.pdf', 'width', 'full', 'height', 'normal');
+#+end_src
+
+#+name: fig:id31_ty_scan_100ums_cl_dz_ry_errors
+#+caption: $T_y$ scan (at $100\,\mu m/s$) - $D_z$ and $R_y$ errors. Open-loop and Closed-loop scans
+#+RESULTS:
+[[file:figs/id31_ty_scan_100ums_cl_dz_ry_errors.png]]
+
+** Combined $R_z$ and $D_y$: Diffraction Tomography
+<<sec:id31_scans_diffraction_tomo>>
+Instead of doing a fast $R_z$ motion a slow $D_y$, the idea is to perform slow $R_z$ (here 1rpm) and fast $D_y$ scans with the nano-hexapod.
+
+#+begin_src matlab
+%% 100um/s - Robust controller
+data_dt_100ums = load(sprintf("%s/scans/2023-08-18_17-12_diffraction_tomo_m0.mat", mat_dir));
+data_dt_100ums.time = Ts*[0:length(data_dt_100ums.Dy_int)-1];
+
+%% 500um/s - Robust controller (Not used)
+% data_dt_500ums = load(sprintf("%s/scans/2023-08-18_17-19_diffraction_tomo_m0_fast.mat", mat_dir));
+% data_dt_500ums.time = Ts*[0:length(data_dt_500ums.Dy_int)-1];
+
+%% 500um/s - Complementary filters
+data_dt_500ums = load(sprintf("%s/scans/2023-08-21_15-15_diffraction_tomo_m0_fast_cf.mat", mat_dir));
+data_dt_500ums.time = Ts*[0:length(data_dt_500ums.Dy_int)-1];
+
+%% 1mm/s - Complementary filters
+data_dt_1000ums = load(sprintf("%s/scans/2023-08-21_15-16_diffraction_tomo_m0_fast_cf.mat", mat_dir));
+data_dt_1000ums.time = Ts*[0:length(data_dt_1000ums.Dy_int)-1];
+
+%% 5mm/s - Complementary filters
+% data_dt_5000ums = load(sprintf("%s/scans/2023-08-21_18-03_diffraction_tomo_m2_fast_cf.mat", mat_dir));
+% data_dt_5000ums.time = Ts*[0:length(data_dt_5000ums.Dy_int)-1];
+
+%% 10mm/s - Complementary filters
+data_dt_10000ums = load(sprintf("%s/scans/2023-08-21_15-17_diffraction_tomo_m0_fast_cf.mat", mat_dir));
+data_dt_10000ums.time = Ts*[0:length(data_dt_10000ums.Dy_int)-1];
+#+end_src
+
+Here, the $D_y$ scans are performed only with the nano-hexapod (the Ty stage is not moving), so we are limited to $\pm 100\,\mu m$.
+
+Several $D_y$ velocities are tested: $0.1\,mm/s$, $0.5\,mm/s$, $1\,mm/s$ and $10\,mm/s$ (see Figure ref:fig:id31_diffraction_tomo_velocities).
+
+#+begin_src matlab :exports none :results none
+%% Dy motion for several configured velocities
+figure;
+hold on;
+plot(data_dt_10000ums.time, 1e6*data_dt_10000ums.Dy_int, ...
+     'DisplayName', '$10 mm/s$')
+plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.Dy_int, ...
+     'DisplayName', '$1 mm/s$')
+plot(data_dt_500ums.time, 1e6*data_dt_500ums.Dy_int, ...
+     'DisplayName', '$0.5 mm/s$')
+plot(data_dt_100ums.time, 1e6*data_dt_100ums.Dy_int, ...
+     'DisplayName', '$0.1 mm/s$')
+hold off;
+xlim([0, 4]);
+xlabel('Time [s]');
+ylabel('$D_y$ position [$\mu$m]')
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_diffraction_tomo_velocities.pdf', 'width', 'wide', 'height', 'normal');
+#+end_src
+
+#+name: fig:id31_diffraction_tomo_velocities
+#+caption: Dy motion for several configured velocities
+#+RESULTS:
+[[file:figs/id31_diffraction_tomo_velocities.png]]
+
+The corresponding "repetition rate" and $D_y$ scan per spindle turn are shown in Table ref:tab:diffraction_tomo_velocities.
+
+The main issue here is the "waiting" time between two scans that is in the order of 50ms.
+By removing this waiting time (fairly easily), we can double the repetition rate at 10mm/s.
+
+#+name: tab:diffraction_tomo_velocities
+#+caption: $D_y$ scaning repetition rate
+#+attr_latex: :environment tabularx :width 0.6\linewidth :align lXX
+#+attr_latex: :center t :booktabs t
+| $D_y$ Velocity | Repetition rate | Scans per turn (at 1RPM) |
+|----------------+-----------------+--------------------------|
+| 0.1 mm/s       | 4 s             |                       15 |
+| 0.5 mm/s       | 0.9 s           |                       65 |
+| 1 mm/s         | 0.5 s           |                      120 |
+| 10 mm/s        | 0.18 s          |                      330 |
+
+
+The scan results for a velocity of 1mm/s is shown in Figure ref:fig:id31_diffraction_tomo_1mms.
+The $D_z$ and $R_y$ errors are quite small during the scan.
+
+The $D_y$ errors are quite large as the velocity is increased.
+This type of scan can probably be massively improved by using feed-forward and optimizing the trajectory.
+Also, if the detectors are triggered in position (the Speedgoat could generate an encoder signal for instance), we don't care about the $D_y$ errors.
+
+#+begin_src matlab :exports none :results none
+%% Diffraction tomography with Dy velocity of 1mm/s and Rz velocity of 1RPM
+figure;
+hold on;
+plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.Dz_int, ...
+     'DisplayName', sprintf('$\\epsilon D_z = %.0f$ nmRMS', 1e9*rms(data_dt_1000ums.Dz_int)))
+plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.Ry_int, ...
+     'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$radRMS', 1e6*rms(data_dt_1000ums.Ry_int)))
+plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.Dy_int, ...
+     'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nmRMS', 1e9*rms(data_dt_1000ums.Dy_int - data_dt_1000ums.m_hexa_dy)))
+plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.m_hexa_dy, 'k--', 'HandleVisibility', 'off')
+hold off;
+xlim([0, 1])
+xlabel('Time [s]');
+ylabel('Measurement [nm,nrad]')
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
+ylim([-110, 110])
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/id31_diffraction_tomo_1mms.pdf', 'width', 'full', 'height', 'normal');
+#+end_src
+
+#+name: fig:id31_diffraction_tomo_1mms
+#+caption: Diffraction tomography with Dy velocity of 1mm/s and Rz velocity of 1RPM
+#+RESULTS:
+[[file:figs/id31_diffraction_tomo_1mms.png]]
+
+#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
+data2orgtable([1e9*rms(data_dt_100ums.Dy_int   - data_dt_100ums.m_hexa_dy), 1e9*rms(data_dt_500ums.Dy_int   - data_dt_500ums.m_hexa_dy), 1e9*rms(data_dt_1000ums.Dy_int  - data_dt_1000ums.m_hexa_dy), 1e9*rms(data_dt_10000ums.Dy_int - data_dt_10000ums.m_hexa_dy);
+1e9*rms(data_dt_100ums.Dz_int), 1e9*rms(data_dt_500ums.Dz_int), 1e9*rms(data_dt_1000ums.Dz_int), 1e9*rms(data_dt_10000ums.Dz_int);
+1e6*rms(data_dt_100ums.Ry_int), 1e6*rms(data_dt_500ums.Ry_int), 1e6*rms(data_dt_1000ums.Ry_int), 1e6*rms(data_dt_10000ums.Ry_int)]', {'0.1 mm/s' ,'0.5 mm/s', '1 mm/s', '10 mm/s'}, {'Velocity', '$D_y$ [nmRMS]', '$D_z$ [nmRMS]', '$R_y$ [$\mu\text{radRMS}$]'}, ' %.1f ');
+#+end_src
+
+#+name: tab:id31_diffraction_tomo_results
+#+caption: Obtained errors for several $D_y$ velocities
+#+attr_latex: :environment tabularx :width \linewidth :align lXX
+#+attr_latex: :center t :booktabs t
+#+RESULTS:
+| Velocity | $D_y$ [nmRMS] | $D_z$ [nmRMS] | $R_y$ [$\mu\text{radRMS}$] |
+|----------+---------------+---------------+----------------------------|
+| 0.1 mm/s |          75.5 |           9.1 |                        0.1 |
+| 0.5 mm/s |         190.5 |          10.0 |                        0.1 |
+| 1 mm/s   |         428.0 |          11.2 |                        0.2 |
+| 10 mm/s  |        4639.9 |          55.9 |                        1.4 |
+
+** Summary of experiments
+For each conducted experiments, the $D_y$, $D_z$ and $R_y$ errors are computed and summarized in Table ref:tab:id31_experiments_results_summary.
+
+#+begin_src matlab
+%% Summary of results
+data_results = [...
+    1e9*data_tomo_1rpm_m0.Dy_rms_cl,                                      1e9*data_tomo_1rpm_m0.Dz_rms_cl,            1e9*data_tomo_1rpm_m0.Ry_rms_cl           ; ... % Tomo 1rpm
+    1e9*data_tomo_6rpm_m0.Dy_rms_cl,                                      1e9*data_tomo_6rpm_m0.Dz_rms_cl,            1e9*data_tomo_6rpm_m0.Ry_rms_cl           ; ... % Tomo 6rpm
+    1e9*data_tomo_30rpm_m0.Dy_rms_cl,                                     1e9*data_tomo_30rpm_m0.Dz_rms_cl,           1e9*data_tomo_30rpm_m0.Ry_rms_cl          ; ... % Tomo 30rpm
+    1e9*rms(detrend(data_dz_10ums.e_dy, 0)),                              1e9*rms(detrend(data_dz_10ums.e_dz, 0)),    1e9*rms(detrend(data_dz_10ums.e_ry, 0))   ; ... % Dz 10um/s
+    1e9*rms(detrend(data_dz_100ums.e_dy,0)),                              1e9*rms(detrend(data_dz_100ums.e_dz,0)),    1e9*rms(detrend(data_dz_100ums.e_ry,0))   ; ... % Dz 100um/s
+    1e9*rms(detrend(data_ry.e_dy,0)),                                     1e9*rms(detrend(data_ry.e_dz,0)),           1e9*rms(detrend(data_ry.e_ry,0))          ; ... % Ry 100urad/s
+    1e9*rms(detrend(data_ty_cl_slow.e_dy, 0)),                            1e9*rms(detrend(data_ty_cl_slow.e_dz, 0)),  1e9*rms(detrend(data_ty_cl_slow.e_rz, 0)) ; ... % Dy 10 um/s
+    1e9*rms(detrend(data_dt_100ums.Dy_int-data_dt_100ums.m_hexa_dy,  0)), 1e9*rms(detrend(data_dt_100ums.Dz_int, 0)), 1e9*rms(detrend(data_dt_100ums.Ry_int, 0)); ... % Diffraction tomo 0.1mm/s
+    1e9*rms(detrend(data_dt_1000ums.Dy_int-data_dt_1000ums.m_hexa_dy,0)), 1e9*rms(detrend(data_dt_1000ums.Dz_int,0)), 1e9*rms(detrend(data_dt_1000ums.Ry_int,0))  ... % Diffraction tomo 1mm/s
+];
+#+end_src
+
+#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
+data2orgtable(data_results, {'Tomography ($R_z$ 1rpm)', 'Tomography ($R_z$ 6rpm)', 'Tomography ($R_z$ 30rpm)', 'Dirty Layer ($D_z$ $10\,\mu m/s$)', 'Dirty Layer ($D_z$ $100\,\mu m/s$)', 'Reflectivity ($R_y$ $100\,\mu\text{rad}/s$)', 'Lateral Scan ($D_y$ $10\,\mu m/s$)', 'Diffraction Tomography ($R_z$ 1rpm, $D_y$ 0.1mm/s)', 'Diffraction Tomography ($R_z$ 1rpm, $D_y$ 1mm/s)'}, {'$D_y$ [nmRMS]', '$D_z$ [nmRMS]', '$R_y$ [nradRMS]'}, ' %.0f ');
+#+end_src
+
+#+name: tab:id31_experiments_results_summary
+#+caption: Table caption
+#+attr_latex: :environment tabularx :width \linewidth :align Xccc
+#+attr_latex: :center t :booktabs t
+#+RESULTS:
+|                                                    | $D_y$ [nmRMS] | $D_z$ [nmRMS] | $R_y$ [nradRMS] |
+|----------------------------------------------------+---------------+---------------+-----------------|
+| Tomography ($R_z$ 1rpm)                            |            15 |             5 |              55 |
+| Tomography ($R_z$ 6rpm)                            |            19 |             5 |              73 |
+| Tomography ($R_z$ 30rpm)                           |            38 |            10 |             129 |
+| Dirty Layer ($D_z$ $10\,\mu m/s$)                  |            25 |             5 |             114 |
+| Dirty Layer ($D_z$ $100\,\mu m/s$)                 |            34 |            15 |             130 |
+| Reflectivity ($R_y$ $100\,\mu\text{rad}/s$)        |            28 |             6 |             118 |
+| Lateral Scan ($D_y$ $10\,\mu m/s$)                 |            21 |            10 |              37 |
+| Diffraction Tomography ($R_z$ 1rpm, $D_y$ 0.1mm/s) |            75 |             9 |             118 |
+| Diffraction Tomography ($R_z$ 1rpm, $D_y$ 1mm/s)   |           428 |            11 |             169 |
+
+* Helping Functions                                                 :noexport:
+** =unwrapphase=
+:PROPERTIES:
+:header-args:matlab+: :tangle matlab/src/unwrapphase.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+
+#+begin_src matlab
+function [unwraped_phase] = unwrapphase(frf, f, args)
+#+end_src
+
+#+begin_src matlab
+arguments
+  frf
+  f
+  args.f0 (1,1) double {mustBeNumeric} = 1
+end
+#+end_src
+
+#+begin_src matlab
+unwraped_phase = unwrap(frf);
+[~,i] = min(abs(f - args.f0));
+unwraped_phase = unwraped_phase - 2*pi*round(unwraped_phase(i)./(2*pi));
+#+end_src
+
+** =circlefit=
+:PROPERTIES:
+:header-args:matlab+: :tangle matlab/src/circlefit.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:h5scan>>
+
+#+begin_src matlab
+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));
+#+end_src
+
+** =yztomography3dmovie=
+:PROPERTIES:
+:header-args:matlab+: :tangle matlab/src/yztomography3dmovie.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:yztomography3dmovie>>
+
+#+begin_src matlab
+function   [] = yztomography3dmovie(filename, data, args)
+#+end_src
+
+#+begin_src matlab
+  arguments
+    filename
+    data
+    args.di      (1,1) double {mustBeNumeric} = 500
+    args.xlim    (2,1) double {mustBeNumeric} = [-3, 3]
+    args.ylim    (2,1) double {mustBeNumeric} = [-3, 3]
+    args.zlim    (2,1) double {mustBeNumeric} = [-0.4, 0.4]
+    args.view_az (1,1) double {mustBeNumeric} = -70
+    args.view_el (1,1) double {mustBeNumeric} = 5
+  end
+#+end_src
+
+#+begin_src matlab
+colors = colororder;
+#+end_src
+
+#+begin_src matlab
+writerObj = VideoWriter(filename); % initialize the VideoWriter object
+open(writerObj);
+#+end_src
+
+#+begin_src matlab
+fig = figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile;
+#+end_src
+
+#+begin_src matlab
+di = args.di;
+for i = 1:floor(length(data.Dx_int)/di)-1
+    if data.hac_status(di*(i+1)-1) == 0
+        % Only open loop
+        plot3(ax1, 1e6*data.Dx_int(di*i:di*(i+1)-1), 1e6*data.Dy_int(di*i:di*(i+1)-1), 1e6*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(1,:))
+    elseif data.hac_status(di*i) == 1
+        % Only closed loop
+        plot3(ax1, 1e6*data.Dx_int(di*i:di*(i+1)-1), 1e6*data.Dy_int(di*i:di*(i+1)-1), 1e6*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(2,:))
+    else
+        % Both open and closed loop
+        Dx_int = data.Dx_int(di*i:di*(i+1)-1);
+        Dy_int = data.Dy_int(di*i:di*(i+1)-1);
+        Dz_int = data.Dz_int(di*i:di*(i+1)-1);
+        plot3(ax1, 1e6*Dx_int(data.hac_status(di*i:di*(i+1)-1) == 0), 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 0), 1e6*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 0), '-', 'color', colors(1,:))
+        plot3(ax1, 1e6*Dx_int(data.hac_status(di*i:di*(i+1)-1) == 1), 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 1), 1e6*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 1), '-', 'color', colors(2,:))
+    end
+    axis(ax1, 'equal')
+    xlim(ax1, args.xlim)
+    ylim(ax1, args.ylim)
+    zlim(ax1, args.zlim)
+    view(ax1, args.view_az, args.view_el)
+    xlabel(ax1, "X motion [$\mu$m]");
+    ylabel(ax1, "Y motion [$\mu$m]");
+    zlabel(ax1, "Z motion [$\mu$m]");
+    drawnow()
+    writeVideo(writerObj,getframe(fig))  % add the frame to the movie
+end
+close(writerObj);
+#+end_src
+
+** =yztomographymovie=
+:PROPERTIES:
+:header-args:matlab+: :tangle matlab/src/yztomographymovie.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:h5scan>>
+
+#+begin_src matlab
+function   [] = yztomographymovie(filename, data, args)
+#+end_src
+
+#+begin_src matlab
+  arguments
+    filename
+    data
+    args.di      (1,1) double {mustBeNumeric} = 500
+    args.xlim_ax1 (2,1) double {mustBeNumeric} = [-3, 3]
+    args.ylim_ax1 (2,1) double {mustBeNumeric} = [-3, 3]
+    args.xlim_ax2 (2,1) double {mustBeNumeric} = [-3, 3]
+    args.ylim_ax2 (2,1) double {mustBeNumeric} = [-3, 3]
+  end
+#+end_src
+
+#+begin_src matlab
+colors = colororder;
+#+end_src
+
+#+begin_src matlab
+writerObj = VideoWriter(filename); % initialize the VideoWriter object
+open(writerObj);
+#+end_src
+
+#+begin_src matlab
+fig = figure;
+tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile;
+ax2 = nexttile;
+#+end_src
+
+#+begin_src matlab
+di = args.di;
+for i = 1:floor(length(data.Dx_int)/di)-1
+    if data.hac_status(di*(i+1)-1) == 0
+        % Only open loop
+        plot(ax1, 1e6*data.Dy_int(di*i:di*(i+1)-1), 1e6*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(1,:))
+        plot(ax2, 1e9*data.Dy_int(di*i:di*(i+1)-1), 1e9*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(1,:))
+    elseif data.hac_status(di*i) == 1
+        % Only closed loop
+        plot(ax1, 1e6*data.Dy_int(di*i:di*(i+1)-1), 1e6*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(2,:))
+        plot(ax2, 1e9*data.Dy_int(di*i:di*(i+1)-1), 1e9*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(2,:))
+    else
+        % Both open and closed loop
+        Dy_int = data.Dy_int(di*i:di*(i+1)-1);
+        Dz_int = data.Dz_int(di*i:di*(i+1)-1);
+        plot(ax1, 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 0), 1e6*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 0), '-', 'color', colors(1,:))
+        plot(ax1, 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 1), 1e6*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 1), '-', 'color', colors(2,:))
+        plot(ax2, 1e9*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 0), 1e9*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 0), '-', 'color', colors(1,:))
+        plot(ax2, 1e9*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 1), 1e9*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 1), '-', 'color', colors(2,:))
+    end
+    axis(ax1, 'square')
+    axis(ax2, 'square')
+    xlim(ax1, args.xlim_ax1)
+    ylim(ax1, args.ylim_ax1)
+    xlim(ax2, args.xlim_ax2)
+    ylim(ax2, args.ylim_ax2)
+    xlabel(ax1, "Y motion [$\mu m$]");
+    ylabel(ax1, "Z motion [$\mu m$]");
+    xlabel(ax2, "Y motion [$nm$]");
+    ylabel(ax2, "Z motion [$nm$]");
+    F = getframe(fig);            %// Capture the frame
+    writeVideo(writerObj,F)  %// add the frame to the movie
+end
+close(writerObj);
+#+end_src
+
+** =yztomographymoviesri=
+:PROPERTIES:
+:header-args:matlab+: :tangle matlab/src/yztomographymoviesri.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:h5scan>>
+
+#+begin_src matlab
+function   [] = yztomographymoviesri(filename, data, args)
+#+end_src
+
+#+begin_src matlab
+  arguments
+    filename
+    data
+    args.di      (1,1) double {mustBeNumeric} = 500
+    args.xlim_ax1 (2,1) double {mustBeNumeric} = [-3, 3]
+    args.ylim_ax1 (2,1) double {mustBeNumeric} = [-3, 3]
+  end
+#+end_src
+
+#+begin_src matlab
+colors = colororder;
+#+end_src
+
+#+begin_src matlab
+writerObj = VideoWriter(filename); % initialize the VideoWriter object
+open(writerObj);
+#+end_src
+
+#+begin_src matlab
+fig = figure;
+tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile;
+#+end_src
+
+#+begin_src matlab
+di = args.di;
+for i = 1:floor(length(data.Dx_int)/di)-1
+    if data.hac_status(di*(i+1)-1) == 0
+        % Only open loop
+        plot(ax1, 1e6*data.Dy_int(di*i:di*(i+1)-1), 1e6*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(1,:))
+    elseif data.hac_status(di*i) == 1
+        % Only closed loop
+        plot(ax1, 1e6*data.Dy_int(di*i:di*(i+1)-1), 1e6*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(2,:))
+    else
+        % Both open and closed loop
+        Dy_int = data.Dy_int(di*i:di*(i+1)-1);
+        Dz_int = data.Dz_int(di*i:di*(i+1)-1);
+        plot(ax1, 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 0), 1e6*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 0), '-', 'color', colors(1,:))
+        plot(ax1, 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 1), 1e6*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 1), '-', 'color', colors(2,:))
+    end
+    axis(ax1, 'square')
+    xlim(ax1, args.xlim_ax1)
+    ylim(ax1, args.ylim_ax1)
+    xlabel(ax1, "Y motion [$\mu m$]");
+    ylabel(ax1, "Z motion [$\mu m$]");
+    F = getframe(fig);            %// Capture the frame
+    writeVideo(writerObj,F)  %// add the frame to the movie
+end
+close(writerObj);
+#+end_src
+
+** =dyscanmoviesri=
+:PROPERTIES:
+:header-args:matlab+: :tangle matlab/src/dyscanmoviesri.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:h5scan>>
+
+#+begin_src matlab
+function   [] = dyscanmoviesri(filename, data, args)
+#+end_src
+
+#+begin_src matlab
+  arguments
+    filename
+    data
+    args.di      (1,1) double {mustBeNumeric} = 500
+    args.xlim_ax1 (2,1) double {mustBeNumeric} = [-3, 3]
+    args.ylim_ax1 (2,1) double {mustBeNumeric} = [-3, 3]
+    args.xlim_ax2 (2,1) double {mustBeNumeric} = [-3, 3]
+    args.ylim_ax2 (2,1) double {mustBeNumeric} = [-3, 3]
+  end
+#+end_src
+
+#+begin_src matlab
+colors = colororder;
+#+end_src
+
+#+begin_src matlab
+writerObj = VideoWriter(filename); % initialize the VideoWriter object
+open(writerObj);
+#+end_src
+
+#+begin_src matlab
+fig = figure;
+tiledlayout(2, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile;
+ax2 = nexttile;
+hold([ax1,ax2], 'on');
+plot(ax1, 1e6*data.Ty, 1e6*data.e_dy, '-', 'color', [colors(1,:), 0.2],'LineWidth',1);
+plot(ax2, 1e6*data.Ty, 1e6*data.e_dz, '-', 'color', [colors(1,:), 0.2],'LineWidth',1);
+#+end_src
+
+#+begin_src matlab
+di = args.di;
+for i = 1:floor(length(data.e_dy)/di)-1
+    plt1 = plot(ax1, 1e6*data.Ty(di*i:di*(i+1)-1), 1e6*data.e_dy(di*i:di*(i+1)-1), '-', 'color', colors(1,:));
+    plt2 = plot(ax2, 1e6*data.Ty(di*i:di*(i+1)-1), 1e6*data.e_dz(di*i:di*(i+1)-1), '-', 'color', colors(1,:));
+    % axis(ax1, 'square')
+    xlim(ax1, args.xlim_ax1);
+    ylim(ax1, args.ylim_ax1);
+    xlim(ax2, args.xlim_ax2);
+    ylim(ax2, args.ylim_ax2);
+    set(ax1, 'XTickLabel',[]);
+    ylabel(ax1, "$D_y$ error [$\mu m$]");
+    xlabel(ax2, "$D_y$ setpoint [$\mu m$]");
+    ylabel(ax2, "Z motion [$\mu m$]");
+    F = getframe(fig);            %// Capture the frame
+    writeVideo(writerObj,F)  %// add the frame to the movie
+    delete(plt1);
+    delete(plt2);
+end
+hold([ax1,ax2], 'off');
+close(writerObj);
+#+end_src
+
+** =dyscanclmoviesri=
+:PROPERTIES:
+:header-args:matlab+: :tangle matlab/src/dyscanclmoviesri.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:h5scan>>
+
+#+begin_src matlab
+function   [] = dyscanclmoviesri(filename, data_ol, data_cl, args)
+#+end_src
+
+#+begin_src matlab
+  arguments
+    filename
+    data_ol
+    data_cl
+    args.di      (1,1) double {mustBeNumeric} = 500
+    args.xlim_ax1 (2,1) double {mustBeNumeric} = [-3, 3]
+    args.ylim_ax1 (2,1) double {mustBeNumeric} = [-3, 3]
+    args.xlim_ax2 (2,1) double {mustBeNumeric} = [-3, 3]
+    args.ylim_ax2 (2,1) double {mustBeNumeric} = [-3, 3]
+  end
+#+end_src
+
+#+begin_src matlab
+colors = colororder;
+#+end_src
+
+#+begin_src matlab
+writerObj = VideoWriter(filename); % initialize the VideoWriter object
+open(writerObj);
+#+end_src
+
+#+begin_src matlab
+fig = figure;
+tiledlayout(2, 1, 'TileSpacing', 'compact', 'Padding', 'None');
+
+ax1 = nexttile;
+ax2 = nexttile;
+hold([ax1,ax2], 'on');
+plot(ax1, 1e6*data_ol.Ty, 1e6*data_ol.e_dy, '-', 'color', [colors(1,:), 0.2],'LineWidth',1);
+plot(ax2, 1e6*data_ol.Ty, 1e6*data_ol.e_dz, '-', 'color', [colors(1,:), 0.2],'LineWidth',1);
+plot(ax1, 1e6*data_cl.Ty, 1e6*data_cl.e_dy, '-', 'color', [colors(2,:), 0.2],'LineWidth',1);
+plot(ax2, 1e6*data_cl.Ty, 1e6*data_cl.e_dz, '-', 'color', [colors(2,:), 0.2],'LineWidth',1);
+#+end_src
+
+#+begin_src matlab
+di = args.di;
+for i = 1:floor(length(data_cl.e_dy)/di)-1
+    plt1 = plot(ax1, 1e6*data_cl.Ty(di*i:di*(i+1)-1), 1e6*data_cl.e_dy(di*i:di*(i+1)-1), '-', 'color', colors(2,:));
+    plt2 = plot(ax2, 1e6*data_cl.Ty(di*i:di*(i+1)-1), 1e6*data_cl.e_dz(di*i:di*(i+1)-1), '-', 'color', colors(2,:));
+    % axis(ax1, 'square')
+    xlim(ax1, args.xlim_ax1);
+    ylim(ax1, args.ylim_ax1);
+    xlim(ax2, args.xlim_ax2);
+    ylim(ax2, args.ylim_ax2);
+    set(ax1, 'XTickLabel',[]);
+    ylabel(ax1, "$D_y$ error [$\mu m$]");
+    xlabel(ax2, "$D_y$ setpoint [$\mu m$]");
+    ylabel(ax2, "Z motion [$\mu m$]");
+    F = getframe(fig);            %// Capture the frame
+    writeVideo(writerObj,F)  %// add the frame to the movie
+    delete(plt1);
+    delete(plt2);
+end
+hold([ax1,ax2], 'off');
+close(writerObj);
+#+end_src
+
+** Simscape Configuration
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/initializeSimscapeConfiguration.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:initializeSimscapeConfiguration>>
+
+*** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [] = initializeSimscapeConfiguration(args)
+#+end_src
+
+*** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+    args.gravity logical {mustBeNumericOrLogical} = true
+  end
+#+end_src
+
+*** Structure initialization
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  conf_simscape = struct();
+#+end_src
+
+*** Add Type
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  if args.gravity
+    conf_simscape.type = 1;
+  else
+    conf_simscape.type = 2;
+  end
+#+end_src
+
+*** Save the Structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  save('./mat/conf_simscape.mat', 'conf_simscape');
+#+end_src
+
+** Logging Configuration
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/initializeLoggingConfiguration.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:initializeLoggingConfiguration>>
+
+*** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [] = initializeLoggingConfiguration(args)
+#+end_src
+
+*** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+    args.log      char   {mustBeMember(args.log,{'none', 'all', 'forces'})} = 'none'
+    args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
+  end
+#+end_src
+
+*** Structure initialization
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  conf_log = struct();
+#+end_src
+
+*** Add Type
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  switch args.log
+    case 'none'
+      conf_log.type = 0;
+    case 'all'
+      conf_log.type = 1;
+    case 'forces'
+      conf_log.type = 2;
+  end
+#+end_src
+
+*** Sampling Time
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  conf_log.Ts = args.Ts;
+#+end_src
+
+*** Save the Structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  save('./mat/conf_log.mat', 'conf_log');
+#+end_src
+
+** Ground
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/initializeGround.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:initializeGround>>
+
+*** Simscape Model
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+The model of the Ground is composed of:
+- A *Cartesian* joint that is used to simulation the ground motion
+- A solid that represents the ground on which the granite is located
+
+#+name: fig:simscape_model_ground
+#+attr_org: :width 800
+#+caption: Simscape model for the Ground
+[[file:figs/images/simscape_model_ground.png]]
+
+#+name: fig:simscape_picture_ground
+#+attr_org: :width 800
+#+caption: Simscape picture for the Ground
+[[file:figs/images/simscape_picture_ground.png]]
+
+*** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [ground] = initializeGround(args)
+#+end_src
+
+*** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+    args.type char {mustBeMember(args.type,{'none', 'rigid'})} = 'rigid'
+    args.rot_point (3,1) double {mustBeNumeric} = zeros(3,1) % Rotation point for the ground motion [m]
+  end
+#+end_src
+
+*** Structure initialization
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+First, we initialize the =granite= structure.
+#+begin_src matlab
+  ground = struct();
+#+end_src
+
+*** Add Type
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  switch args.type
+    case 'none'
+      ground.type = 0;
+    case 'rigid'
+      ground.type = 1;
+  end
+#+end_src
+
+*** Ground Solid properties
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+We set the shape and density of the ground solid element.
+#+begin_src matlab
+  ground.shape   = [2, 2, 0.5]; % [m]
+  ground.density = 2800;        % [kg/m3]
+#+end_src
+
+*** Rotation Point
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+#+begin_src matlab
+  ground.rot_point = args.rot_point;
+#+end_src
+
+*** Save the Structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+The =ground= structure is saved.
+#+begin_src matlab
+  save('./mat/stages.mat', 'ground', '-append');
+#+end_src
+
+** Granite
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/initializeGranite.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:initializeGranite>>
+
+*** Simscape Model
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+The Simscape model of the granite is composed of:
+- A cartesian joint such that the granite can vibrations along x, y and z axis
+- A solid
+
+The output =sample_pos= corresponds to the impact point of the X-ray.
+
+#+name: fig:simscape_model_granite
+#+attr_org: :width 800
+#+caption: Simscape model for the Granite
+[[file:figs/images/simscape_model_granite.png]]
+
+#+name: fig:simscape_picture_granite
+#+attr_org: :width 800
+#+caption: Simscape picture for the Granite
+[[file:figs/images/simscape_picture_granite.png]]
+
+*** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [granite] = initializeGranite(args)
+#+end_src
+
+*** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+    args.type          char    {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'modal-analysis', 'init'})} = 'flexible'
+    args.Foffset       logical {mustBeNumericOrLogical} = false
+    args.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 % Density [kg/m3]
+    args.K  (3,1) double {mustBeNumeric, mustBeNonnegative} = [4e9; 3e8; 8e8] % [N/m]
+    args.C  (3,1) double {mustBeNumeric, mustBeNonnegative} = [4.0e5; 1.1e5; 9.0e5] % [N/(m/s)]
+    args.x0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the X direction [m]
+    args.y0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Y direction [m]
+    args.z0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Z direction [m]
+    args.sample_pos (1,1) double {mustBeNumeric} = 0.8-25e-3 % Height of the measurment point [m]
+  end
+#+end_src
+
+*** Structure initialization
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+First, we initialize the =granite= structure.
+#+begin_src matlab
+  granite = struct();
+#+end_src
+
+*** Add Granite Type
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  switch args.type
+    case 'none'
+      granite.type = 0;
+    case 'rigid'
+      granite.type = 1;
+    case 'flexible'
+      granite.type = 2;
+    case 'modal-analysis'
+      granite.type = 3;
+    case 'init'
+      granite.type = 4;
+  end
+#+end_src
+
+*** Material and Geometry
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+Properties of the Material and link to the geometry of the granite.
+#+begin_src matlab
+  granite.density = args.density; % [kg/m3]
+  granite.STEP    = './STEPS/granite/granite.STEP';
+#+end_src
+
+Z-offset for the initial position of the sample with respect to the granite top surface.
+#+begin_src matlab
+  granite.sample_pos = args.sample_pos; % [m]
+#+end_src
+
+*** Stiffness and Damping properties
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+#+begin_src matlab
+  granite.K = args.K; % [N/m]
+  granite.C = args.C; % [N/(m/s)]
+#+end_src
+
+*** Equilibrium position of the each joint.
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+#+begin_src matlab
+  if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
+    load('mat/Foffset.mat', 'Fgm');
+    granite.Deq = -Fgm'./granite.K;
+  else
+    granite.Deq = zeros(6,1);
+  end
+#+end_src
+
+*** Save the Structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+The =granite= structure is saved.
+#+begin_src matlab
+  save('./mat/stages.mat', 'granite', '-append');
+#+end_src
+
+** Translation Stage
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/initializeTy.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:initializeTy>>
+
+*** Simscape Model
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+The Simscape model of the Translation stage consist of:
+- One rigid body for the fixed part of the translation stage
+- One rigid body for the moving part of the translation stage
+- Four 6-DOF Joints that only have some rigidity in the X and Z directions.
+  The rigidity in rotation comes from the fact that we use multiple joints that are located at different points
+- One 6-DOF joint that represent the Actuator.
+  It is used to impose the motion in the Y direction
+- One 6-DOF joint to inject force disturbance in the X and Z directions
+
+#+name: fig:simscape_model_ty
+#+ATTR_ORG: :width 800
+#+caption: Simscape model for the Translation Stage
+[[file:figs/images/simscape_model_ty.png]]
+
+#+name: fig:simscape_picture_ty
+#+attr_org: :width 800
+#+caption: Simscape picture for the Translation Stage
+[[file:figs/images/simscape_picture_ty.png]]
+
+*** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [ty] = initializeTy(args)
+#+end_src
+
+*** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+    args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
+    args.Foffset logical {mustBeNumericOrLogical} = false
+  end
+#+end_src
+
+*** Structure initialization
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+First, we initialize the =ty= structure.
+#+begin_src matlab
+  ty = struct();
+#+end_src
+
+*** Add Translation Stage Type
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  switch args.type
+    case 'none'
+      ty.type = 0;
+    case 'rigid'
+      ty.type = 1;
+    case 'flexible'
+      ty.type = 2;
+    case 'modal-analysis'
+      ty.type = 3;
+    case 'init'
+      ty.type = 4;
+  end
+#+end_src
+
+*** Material and Geometry
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+Define the density of the materials as well as the geometry (STEP files).
+#+begin_src matlab
+  % Ty Granite frame
+  ty.granite_frame.density = 7800; % [kg/m3] => 43kg
+  ty.granite_frame.STEP    = './STEPS/Ty/Ty_Granite_Frame.STEP';
+
+  % Guide Translation Ty
+  ty.guide.density         = 7800; % [kg/m3] => 76kg
+  ty.guide.STEP            = './STEPS/ty/Ty_Guide.STEP';
+
+  % Ty - Guide_Translation12
+  ty.guide12.density       = 7800; % [kg/m3]
+  ty.guide12.STEP          = './STEPS/Ty/Ty_Guide_12.STEP';
+
+  % Ty - Guide_Translation11
+  ty.guide11.density       = 7800; % [kg/m3]
+  ty.guide11.STEP          = './STEPS/ty/Ty_Guide_11.STEP';
+
+  % Ty - Guide_Translation22
+  ty.guide22.density       = 7800; % [kg/m3]
+  ty.guide22.STEP          = './STEPS/ty/Ty_Guide_22.STEP';
+
+  % Ty - Guide_Translation21
+  ty.guide21.density       = 7800; % [kg/m3]
+  ty.guide21.STEP          = './STEPS/Ty/Ty_Guide_21.STEP';
+
+  % Ty - Plateau translation
+  ty.frame.density         = 7800; % [kg/m3]
+  ty.frame.STEP            = './STEPS/ty/Ty_Stage.STEP';
+
+  % Ty Stator Part
+  ty.stator.density        = 5400; % [kg/m3]
+  ty.stator.STEP           = './STEPS/ty/Ty_Motor_Stator.STEP';
+
+  % Ty Rotor Part
+  ty.rotor.density         = 5400; % [kg/m3]
+  ty.rotor.STEP            = './STEPS/ty/Ty_Motor_Rotor.STEP';
+#+end_src
+
+*** Stiffness and Damping properties
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+#+begin_src matlab
+  ty.K = [2e8; 1e8; 2e8; 6e7; 9e7; 6e7]; % [N/m, N*m/rad]
+  ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 2e4]; % [N/(m/s), N*m/(rad/s)]
+#+end_src
+
+*** Equilibrium position of the each joint.
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+#+begin_src matlab
+  if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
+    load('mat/Foffset.mat', 'Ftym');
+    ty.Deq = -Ftym'./ty.K;
+  else
+    ty.Deq = zeros(6,1);
+  end
+#+end_src
+
+*** Save the Structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+The =ty= structure is saved.
+#+begin_src matlab
+  save('./mat/stages.mat', 'ty', '-append');
+#+end_src
+
+** Tilt Stage
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/initializeRy.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:initializeRy>>
+
+*** Simscape Model
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+The Simscape model of the Tilt stage is composed of:
+- Two solid bodies for the two part of the stage
+- *Four* 6-DOF joints to model the flexibility of the stage.
+  These joints are virtually located along the rotation axis and are connecting the two solid bodies.
+  These joints have some translation stiffness in the u-v-w directions aligned with the joint.
+  The stiffness in rotation between the two solids is due to the fact that the 4 joints are connecting the two solids are different locations
+- A Bushing Joint used for the Actuator.
+  The Ry motion is imposed by the input.
+
+#+name: fig:simscape_model_ry
+#+attr_org: :width 800
+#+caption: Simscape model for the Tilt Stage
+[[file:figs/images/simscape_model_ry.png]]
+
+#+name: fig:simscape_picture_ry
+#+attr_org: :width 800
+#+caption: Simscape picture for the Tilt Stage
+[[file:figs/images/simscape_picture_ry.png]]
+
+*** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [ry] = initializeRy(args)
+#+end_src
+
+*** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+    args.type          char    {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
+    args.Foffset       logical {mustBeNumericOrLogical} = false
+    args.Ry_init (1,1) double  {mustBeNumeric} = 0
+  end
+#+end_src
+
+*** Structure initialization
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+First, we initialize the =ry= structure.
+#+begin_src matlab
+  ry = struct();
+#+end_src
+
+
+*** Add Tilt Type
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  switch args.type
+    case 'none'
+      ry.type = 0;
+    case 'rigid'
+      ry.type = 1;
+    case 'flexible'
+      ry.type = 2;
+    case 'modal-analysis'
+      ry.type = 3;
+    case 'init'
+      ry.type = 4;
+  end
+#+end_src
+
+*** Material and Geometry
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+Properties of the Material and link to the geometry of the Tilt stage.
+#+begin_src matlab
+  % Ry - Guide for the tilt stage
+  ry.guide.density = 7800; % [kg/m3]
+  ry.guide.STEP    = './STEPS/ry/Tilt_Guide.STEP';
+
+  % Ry - Rotor of the motor
+  ry.rotor.density = 2400; % [kg/m3]
+  ry.rotor.STEP    = './STEPS/ry/Tilt_Motor_Axis.STEP';
+
+  % Ry - Motor
+  ry.motor.density = 3200; % [kg/m3]
+  ry.motor.STEP    = './STEPS/ry/Tilt_Motor.STEP';
+
+  % Ry - Plateau Tilt
+  ry.stage.density = 7800; % [kg/m3]
+  ry.stage.STEP    = './STEPS/ry/Tilt_Stage.STEP';
+#+end_src
+
+Z-Offset so that the center of rotation matches the sample center;
+#+begin_src matlab
+  ry.z_offset = 0.58178; % [m]
+#+end_src
+
+#+begin_src matlab
+  ry.Ry_init = args.Ry_init; % [rad]
+#+end_src
+
+*** Stiffness and Damping properties
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+#+begin_src matlab
+  ry.K = [3.8e8; 4e8; 3.8e8; 1.2e8; 6e4; 1.2e8];
+  ry.C = [1e5;   1e5; 1e5;   3e4;   1e3; 3e4];
+#+end_src
+
+*** Equilibrium position of the each joint.
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+#+begin_src matlab
+  if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
+    load('mat/Foffset.mat', 'Fym');
+    ry.Deq = -Fym'./ry.K;
+  else
+    ry.Deq = zeros(6,1);
+  end
+#+end_src
+
+*** Save the Structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+The =ry= structure is saved.
+#+begin_src matlab
+  save('./mat/stages.mat', 'ry', '-append');
+#+end_src
+
+** Spindle
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/initializeRz.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:initializeRz>>
+
+*** Simscape Model
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+The Simscape model of the Spindle is composed of:
+- Two rigid bodies: the stator and the rotor
+- A Bushing Joint that is used both as the actuator (the Rz motion is imposed by the input) and as the force perturbation in the Z direction.
+- The Bushing joint has some flexibility in the X-Y-Z directions as well as in Rx and Ry rotations
+
+#+name: fig:simscape_model_rz
+#+attr_org: :width 800
+#+caption: Simscape model for the Spindle
+[[file:figs/images/simscape_model_rz.png]]
+
+#+name: fig:simscape_picture_rz
+#+attr_org: :width 800
+#+caption: Simscape picture for the Spindle
+[[file:figs/images/simscape_picture_rz.png]]
+
+*** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [rz] = initializeRz(args)
+#+end_src
+
+*** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+    args.type    char    {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
+    args.Foffset logical {mustBeNumericOrLogical} = false
+  end
+#+end_src
+
+*** Structure initialization
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+First, we initialize the =rz= structure.
+#+begin_src matlab
+  rz = struct();
+#+end_src
+
+*** Add Spindle Type
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  switch args.type
+    case 'none'
+      rz.type = 0;
+    case 'rigid'
+      rz.type = 1;
+    case 'flexible'
+      rz.type = 2;
+    case 'modal-analysis'
+      rz.type = 3;
+    case 'init'
+      rz.type = 4;
+  end
+#+end_src
+
+*** Material and Geometry
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+Properties of the Material and link to the geometry of the spindle.
+#+begin_src matlab
+  % Spindle - Slip Ring
+  rz.slipring.density = 7800; % [kg/m3]
+  rz.slipring.STEP    = './STEPS/rz/Spindle_Slip_Ring.STEP';
+
+  % Spindle - Rotor
+  rz.rotor.density    = 7800; % [kg/m3]
+  rz.rotor.STEP       = './STEPS/rz/Spindle_Rotor.STEP';
+
+  % Spindle - Stator
+  rz.stator.density   = 7800; % [kg/m3]
+  rz.stator.STEP      = './STEPS/rz/Spindle_Stator.STEP';
+#+end_src
+
+*** Stiffness and Damping properties
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+#+begin_src matlab
+  rz.K = [7e8; 7e8; 2e9; 1e7; 1e7; 1e7];
+  rz.C = [4e4; 4e4; 7e4; 1e4; 1e4; 1e4];
+#+end_src
+
+*** Equilibrium position of the each joint.
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+#+begin_src matlab
+  if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
+    load('mat/Foffset.mat', 'Fzm');
+    rz.Deq = -Fzm'./rz.K;
+  else
+    rz.Deq = zeros(6,1);
+  end
+#+end_src
+
+*** Save the Structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+The =rz= structure is saved.
+#+begin_src matlab
+  save('./mat/stages.mat', 'rz', '-append');
+#+end_src
+
+** Micro Hexapod
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/initializeMicroHexapod.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:initializeMicroHexapod>>
+
+*** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [micro_hexapod] = initializeMicroHexapod(args)
+#+end_src
+
+*** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+      args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init', 'compliance'})} = 'flexible'
+      % initializeFramesPositions
+      args.H    (1,1) double {mustBeNumeric, mustBePositive} = 350e-3
+      args.MO_B (1,1) double {mustBeNumeric} = 270e-3
+      % generateGeneralConfiguration
+      args.FH  (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
+      args.FR  (1,1) double {mustBeNumeric, mustBePositive} = 175.5e-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} = 45e-3
+      args.MR  (1,1) double {mustBeNumeric, mustBePositive} = 118e-3
+      args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180)
+      % initializeStrutDynamics
+      args.Ki (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e7*ones(6,1)
+      args.Ci (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.4e3*ones(6,1)
+      % initializeCylindricalPlatforms
+      args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 10
+      args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
+      args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 207.5e-3
+      args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 10
+      args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
+      args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3
+      % initializeCylindricalStruts
+      args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1
+      args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
+      args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
+      args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1
+      args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
+      args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
+      % inverseKinematics
+      args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
+      args.ARB (3,3) double {mustBeNumeric} = eye(3)
+      % Force that stiffness of each joint should apply at t=0
+      args.Foffset      logical {mustBeNumericOrLogical} = false
+  end
+#+end_src
+
+*** Function content
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  stewart = initializeStewartPlatform();
+
+  stewart = initializeFramesPositions(stewart, ...
+                                      'H', args.H, ...
+                                      'MO_B', args.MO_B);
+
+  stewart = generateGeneralConfiguration(stewart, ...
+                                         'FH', args.FH, ...
+                                         'FR', args.FR, ...
+                                         'FTh', args.FTh, ...
+                                         'MH', args.MH, ...
+                                         'MR', args.MR, ...
+                                         'MTh', args.MTh);
+
+  stewart = computeJointsPose(stewart);
+#+end_src
+
+#+begin_src matlab
+  stewart = initializeStrutDynamics(stewart, ...
+                                    'K', args.Ki, ...
+                                    'C', args.Ci);
+
+  stewart = initializeJointDynamics(stewart, ...
+                                    'type_F', 'universal_p', ...
+                                    'type_M', 'spherical_p');
+#+end_src
+
+#+begin_src matlab
+  stewart = initializeCylindricalPlatforms(stewart, ...
+                                           'Fpm', args.Fpm, ...
+                                           'Fph', args.Fph, ...
+                                           'Fpr', args.Fpr, ...
+                                           'Mpm', args.Mpm, ...
+                                           'Mph', args.Mph, ...
+                                           'Mpr', args.Mpr);
+
+  stewart = initializeCylindricalStruts(stewart, ...
+                                        'Fsm', args.Fsm, ...
+                                        'Fsh', args.Fsh, ...
+                                        'Fsr', args.Fsr, ...
+                                        'Msm', args.Msm, ...
+                                        'Msh', args.Msh, ...
+                                        'Msr', args.Msr);
+
+  stewart = computeJacobian(stewart);
+
+  stewart = initializeStewartPose(stewart, ...
+                                'AP', args.AP, ...
+                                'ARB', args.ARB);
+#+end_src
+
+#+begin_src matlab
+  stewart = initializeInertialSensor(stewart, 'type', 'none');
+#+end_src
+
+Equilibrium position of the each joint.
+#+begin_src matlab
+  if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
+    load('mat/Foffset.mat', 'Fhm');
+    stewart.actuators.dLeq = -Fhm'./args.Ki;
+  else
+    stewart.actuators.dLeq = zeros(6,1);
+  end
+#+end_src
+
+*** Add Type
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  switch args.type
+    case 'none'
+      stewart.type = 0;
+    case 'rigid'
+      stewart.type = 1;
+    case 'flexible'
+      stewart.type = 2;
+    case 'modal-analysis'
+      stewart.type = 3;
+    case 'init'
+      stewart.type = 4;
+    case 'compliance'
+      stewart.type = 5;
+  end
+#+end_src
+
+*** Save the Structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+The =micro_hexapod= structure is saved.
+#+begin_src matlab
+  micro_hexapod = stewart;
+  save('./mat/stages.mat', 'micro_hexapod', '-append');
+#+end_src
+
+** Nano Hexapod
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/initializeNanoHexapod.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:initializeNanoHexapod>>
+
+*** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+function [nano_hexapod] = initializeNanoHexapod(args)
+#+end_src
+
+*** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+arguments
+    %% Bottom Flexible Joints
+    args.flex_bot_type      char {mustBeMember(args.flex_bot_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof'
+    args.flex_bot_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad]
+    args.flex_bot_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad]
+    args.flex_bot_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad]
+    args.flex_bot_kz  (6,1) double {mustBeNumeric} = ones(6,1)*7e7 % Axial Stiffness [N/m]
+    args.flex_bot_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % X bending Damping [Nm/(rad/s)]
+    args.flex_bot_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Y bending Damping [Nm/(rad/s)]
+    args.flex_bot_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Torsionnal Damping [Nm/(rad/s)]
+    args.flex_bot_cz  (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Axial Damping [N/(m/s)]
+
+    %% Top Flexible Joints
+    args.flex_top_type      char {mustBeMember(args.flex_top_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof'
+    args.flex_top_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad]
+    args.flex_top_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad]
+    args.flex_top_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad]
+    args.flex_top_kz  (6,1) double {mustBeNumeric} = ones(6,1)*7e7 % Axial Stiffness [N/m]
+    args.flex_top_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % X bending Damping [Nm/(rad/s)]
+    args.flex_top_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Y bending Damping [Nm/(rad/s)]
+    args.flex_top_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Torsionnal Damping [Nm/(rad/s)]
+    args.flex_top_cz  (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Axial Damping [N/(m/s)]
+
+    %% Jacobian - Location of frame {A} and {B}
+    args.MO_B (1,1) double {mustBeNumeric} = 150e-3 % Height of {B} w.r.t. {M} [m]
+
+    %% Relative Motion Sensor
+    args.motion_sensor_type char {mustBeMember(args.motion_sensor_type,{'struts', 'plates'})} = 'struts'
+
+    %% Top Plate
+    args.top_plate_type char {mustBeMember(args.top_plate_type,{'rigid', 'flexible'})} = 'rigid'
+    args.top_plate_xi  (1,1) double {mustBeNumeric} = 0.01 % Damping Ratio
+
+    %% Actuators
+    args.actuator_type char {mustBeMember(args.actuator_type,{'2dof', 'flexible frame', 'flexible'})} = 'flexible'
+    args.actuator_Ga  (6,1) double {mustBeNumeric} = zeros(6,1) % Actuator gain [N/V]
+    args.actuator_Gs  (6,1) double {mustBeNumeric} = zeros(6,1) % Sensor gain [V/m]
+    % For 2DoF
+    args.actuator_k   (6,1) double {mustBeNumeric} = ones(6,1)*0.38e6 % [N/m]
+    args.actuator_ke  (6,1) double {mustBeNumeric} = ones(6,1)*1.75e6 % [N/m]
+    args.actuator_ka  (6,1) double {mustBeNumeric} = ones(6,1)*3e7 % [N/m]
+    args.actuator_c   (6,1) double {mustBeNumeric} = ones(6,1)*3e1 % [N/(m/s)]
+    args.actuator_ce  (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % [N/(m/s)]
+    args.actuator_ca  (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % [N/(m/s)]
+    args.actuator_Leq (6,1) double {mustBeNumeric} = ones(6,1)*0.056 % [m]
+    % For Flexible Frame
+    args.actuator_ks (6,1) double {mustBeNumeric} = ones(6,1)*235e6 % Stiffness of one stack [N/m]
+    args.actuator_cs (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % Stiffness of one stack [N/m]
+    % Misalignment
+    args.actuator_d_align (6,3) double {mustBeNumeric} = zeros(6,3) % [m]
+
+    args.actuator_xi  (1,1) double {mustBeNumeric} = 0.01 % Damping Ratio
+end
+#+end_src
+
+*** Nano Hexapod Object
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+#+begin_src matlab
+nano_hexapod = struct();
+#+end_src
+
+*** Flexible Joints - Bot
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+#+begin_src matlab
+nano_hexapod.flex_bot = struct();
+
+switch args.flex_bot_type
+  case '2dof'
+    nano_hexapod.flex_bot.type = 1;
+  case '3dof'
+    nano_hexapod.flex_bot.type = 2;
+  case '4dof'
+    nano_hexapod.flex_bot.type = 3;
+  case 'flexible'
+    nano_hexapod.flex_bot.type = 4;
+end
+
+nano_hexapod.flex_bot.kRx = args.flex_bot_kRx; % X bending stiffness [Nm/rad]
+nano_hexapod.flex_bot.kRy = args.flex_bot_kRy; % Y bending stiffness [Nm/rad]
+nano_hexapod.flex_bot.kRz = args.flex_bot_kRz; % Torsionnal stiffness [Nm/rad]
+nano_hexapod.flex_bot.kz  = args.flex_bot_kz;  % Axial stiffness [N/m]
+
+nano_hexapod.flex_bot.cRx = args.flex_bot_cRx; % [Nm/(rad/s)]
+nano_hexapod.flex_bot.cRy = args.flex_bot_cRy; % [Nm/(rad/s)]
+nano_hexapod.flex_bot.cRz = args.flex_bot_cRz; % [Nm/(rad/s)]
+nano_hexapod.flex_bot.cz  = args.flex_bot_cz;  %[N/(m/s)]
+#+end_src
+
+*** Flexible Joints - Top
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+#+begin_src matlab
+nano_hexapod.flex_top = struct();
+
+switch args.flex_top_type
+  case '2dof'
+    nano_hexapod.flex_top.type = 1;
+  case '3dof'
+    nano_hexapod.flex_top.type = 2;
+  case '4dof'
+    nano_hexapod.flex_top.type = 3;
+  case 'flexible'
+    nano_hexapod.flex_top.type = 4;
+end
+
+nano_hexapod.flex_top.kRx = args.flex_top_kRx; % X bending stiffness [Nm/rad]
+nano_hexapod.flex_top.kRy = args.flex_top_kRy; % Y bending stiffness [Nm/rad]
+nano_hexapod.flex_top.kRz = args.flex_top_kRz; % Torsionnal stiffness [Nm/rad]
+nano_hexapod.flex_top.kz  = args.flex_top_kz;  % Axial stiffness [N/m]
+
+nano_hexapod.flex_top.cRx = args.flex_top_cRx; % [Nm/(rad/s)]
+nano_hexapod.flex_top.cRy = args.flex_top_cRy; % [Nm/(rad/s)]
+nano_hexapod.flex_top.cRz = args.flex_top_cRz; % [Nm/(rad/s)]
+nano_hexapod.flex_top.cz  = args.flex_top_cz;  %[N/(m/s)]
+#+end_src
+
+*** Relative Motion Sensor
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+#+begin_src matlab
+nano_hexapod.motion_sensor = struct();
+
+switch args.motion_sensor_type
+  case 'struts'
+    nano_hexapod.motion_sensor.type = 1;
+  case 'plates'
+    nano_hexapod.motion_sensor.type = 2;
+end
+#+end_src
+
+*** Amplified Piezoelectric Actuator
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+#+begin_src matlab
+nano_hexapod.actuator = struct();
+
+switch args.actuator_type
+  case '2dof'
+    nano_hexapod.actuator.type = 1;
+  case 'flexible frame'
+    nano_hexapod.actuator.type = 2;
+  case 'flexible'
+    nano_hexapod.actuator.type = 3;
+end
+#+end_src
+
+#+begin_src matlab
+%% Actuator gain [N/V]
+if all(args.actuator_Ga == 0)
+    switch args.actuator_type
+      case '2dof'
+        nano_hexapod.actuator.Ga = ones(6,1)*(-30.0);
+      case 'flexible frame'
+        nano_hexapod.actuator.Ga = ones(6,1); % TODO
+      case 'flexible'
+        nano_hexapod.actuator.Ga = ones(6,1)*23.4;
+    end
+else
+    nano_hexapod.actuator.Ga = args.actuator_Ga; % Actuator gain [N/V]
+end
+
+%% Sensor gain [V/m]
+if all(args.actuator_Gs == 0)
+    switch args.actuator_type
+      case '2dof'
+        nano_hexapod.actuator.Gs = ones(6,1)*0.098;
+      case 'flexible frame'
+        nano_hexapod.actuator.Gs = ones(6,1); % TODO
+      case 'flexible'
+        nano_hexapod.actuator.Gs = ones(6,1)*(-4674824);
+    end
+else
+    nano_hexapod.actuator.Gs = args.actuator_Gs; % Sensor gain [V/m]
+end
+#+end_src
+
+Mechanical characteristics:
+#+begin_src matlab
+switch args.actuator_type
+  case '2dof'
+    nano_hexapod.actuator.k  = args.actuator_k;  % [N/m]
+    nano_hexapod.actuator.ke = args.actuator_ke; % [N/m]
+    nano_hexapod.actuator.ka = args.actuator_ka; % [N/m]
+
+    nano_hexapod.actuator.c  = args.actuator_c;  % [N/(m/s)]
+    nano_hexapod.actuator.ce = args.actuator_ce; % [N/(m/s)]
+    nano_hexapod.actuator.ca = args.actuator_ca; % [N/(m/s)]
+
+    nano_hexapod.actuator.Leq = args.actuator_Leq; % [m]
+
+  case 'flexible frame'
+    nano_hexapod.actuator.K = readmatrix('APA300ML_b_mat_K.CSV'); % Stiffness Matrix
+    nano_hexapod.actuator.M = readmatrix('APA300ML_b_mat_M.CSV'); % Mass Matrix
+    nano_hexapod.actuator.P = extractNodes('APA300ML_b_out_nodes_3D.txt'); % Node coordinates [m]
+
+    nano_hexapod.actuator.ks = args.actuator_ks; % Stiffness of one stack [N/m]
+    nano_hexapod.actuator.cs = args.actuator_cs; % Damping of one stack [N/m]
+    nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio
+
+  case 'flexible'
+    nano_hexapod.actuator.K = readmatrix('full_APA300ML_K.CSV'); % Stiffness Matrix
+    nano_hexapod.actuator.M = readmatrix('full_APA300ML_M.CSV'); % Mass Matrix
+    nano_hexapod.actuator.P = extractNodes('full_APA300ML_out_nodes_3D.txt'); % Node coordiantes [m]
+
+    nano_hexapod.actuator.d_align = args.actuator_d_align; % Misalignment
+    nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio
+
+end
+
+#+end_src
+
+*** Geometry
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+#+begin_src matlab
+nano_hexapod.geometry = struct();
+#+end_src
+
+Center of joints $a_i$ with respect to {F}:
+#+begin_src matlab
+Fa = [[-21.74,  111.91, 22.49],
+      [-107.79, -37.13, 22.49],
+      [-86.05,  -74.78, 22.49],
+      [ 86.05,  -74.78, 22.49],
+      [ 107.79, -37.13, 22.49],
+      [ 21.74,  111.91, 22.49]]'*1e-3; % Ai w.r.t. {F} [m]
+#+end_src
+
+Center of joints $b_i$ with respect to {M}:
+#+begin_src matlab
+Mb = [[-77.78,  77.78,  -22.50],
+      [-106.25, 28.47,  -22.50],
+      [-28.47, -106.25, -22.50],
+      [ 28.47, -106.25, -22.50],
+      [ 106.25, 28.47,  -22.50],
+      [ 77.78,  77.78,  -22.50]]'*1e-3; % Bi w.r.t. {M} [m]
+#+end_src
+
+Now compute the positions $b_i$ with respect to {F}:
+#+begin_src matlab
+Fb = Mb + [0; 0; 95e-3]; % Bi w.r.t. {F} [m]
+#+end_src
+
+The unit vector representing the orientation of the struts can then be computed:
+#+begin_src matlab
+si = Fb - Fa;
+si = si./vecnorm(si); % Normalize
+#+end_src
+
+Location of encoder measurement points when fixed on the plates:
+#+begin_src matlab
+Fc = [[-76.914,  78.31,   52.605]
+      [-106.276, 27.454,  52.605]
+      [-29.362, -105.765, 52.605]
+      [ 29.362, -105.765, 52.605]
+      [ 106.276, 27.454,  52.605]
+      [ 76.914,  78.31,   52.605]]'*1e-3; % Meas pos w.r.t. {F}
+Mc = Fc - [0; 0; 95e-3]; % Meas pos w.r.t. {M}
+#+end_src
+
+#+begin_src matlab
+nano_hexapod.geometry.Fa = Fa;
+nano_hexapod.geometry.Fb = Fb;
+nano_hexapod.geometry.Fc = Fc;
+nano_hexapod.geometry.Mb = Mb;
+nano_hexapod.geometry.Mc = Mc;
+nano_hexapod.geometry.si = si;
+nano_hexapod.geometry.MO_B = args.MO_B;
+#+end_src
+
+*** Jacobian for Actuators
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+#+begin_src matlab
+Bb = Mb - [0; 0; args.MO_B];
+
+nano_hexapod.geometry.J = [nano_hexapod.geometry.si', cross(Bb, nano_hexapod.geometry.si)'];
+#+end_src
+
+*** Jacobian for Sensors
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+switch args.motion_sensor_type
+  case 'struts'
+    nano_hexapod.geometry.Js = nano_hexapod.geometry.J;
+  case 'plates'
+    Bc = Mc - [0; 0; args.MO_B];
+    nano_hexapod.geometry.Js = [nano_hexapod.geometry.si', cross(Bc, nano_hexapod.geometry.si)'];
+end
+#+end_src
+
+*** Top Plate
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+#+begin_src matlab
+nano_hexapod.top_plate = struct();
+
+switch args.top_plate_type
+  case 'rigid'
+    nano_hexapod.top_plate.type = 1;
+  case 'flexible'
+    nano_hexapod.top_plate.type = 2;
+
+    nano_hexapod.top_plate.R_flex = ...
+       {[-0.51771438594671  0.5201563363052 -0.67927108019211
+         -0.31530446231304 -0.8540710660135 -0.41369760724945
+         -0.79533320729697  0                0.60617249143351 ],
+        [-0.01420448131632  0.9997254079576  0.01863709726679
+         -0.60600604129104 -0.0234330681729  0.79511481512719
+          0.79533320729697  0                0.60617249143351 ],
+        [ 0.53191886726305  0.4795690716524  0.69790817745892
+         -0.29070157897799  0.8775041341865 -0.38141720787774
+         -0.79533320729697  0                0.60617249143351 ],
+        [ 0.53191886726305 -0.4795690716524 -0.69790817745892
+          0.29070157897799  0.8775041341865 -0.38141720787774
+          0.79533320729697  0                0.60617249143351 ],
+        [-0.01420448131633 -0.9997254079576 -0.01863709726680
+          0.60600604129104 -0.0234330681729  0.79511481512719
+         -0.79533320729697  0                0.60617249143351 ],
+        [-0.51771438594672 -0.5201563363051  0.67927108019212
+          0.31530446231304 -0.8540710660135 -0.41369760724945
+          0.79533320729697  0                0.60617249143351 ]};
+
+
+    nano_hexapod.top_plate.R_enc = ...
+      { [ 0.854071066013574 -0.520156336305191 0
+          0.520156336305191  0.854071066013574 0
+                          0                  0 1 ],
+        [-0.023433068172958  0.999725407957606 0
+         -0.999725407957606 -0.023433068172958 0
+                          0                  0 1 ],
+        [-0.877504134186525 -0.479569071652412 0
+         0.479569071652412 -0.877504134186525 0
+                         0                  0 1 ],
+        [ 0.877504134186525 -0.479569071652413 0
+          0.479569071652413  0.877504134186525 0
+                          0                  0 1 ],
+        [ 0.023433068172945  0.999725407957606 0
+         -0.999725407957606  0.023433068172945 0
+                          0                  0 1 ],
+        [-0.854071066013566 -0.520156336305202 0
+          0.520156336305202 -0.854071066013566 0
+                          0                  0 1 ]};
+
+    nano_hexapod.top_plate.K = readmatrix('top_plate_K_6.CSV'); % Stiffness Matrix
+    nano_hexapod.top_plate.M = readmatrix('top_plate_M_6.CSV'); % Mass Matrix
+    nano_hexapod.top_plate.P = extractNodes('top_plate_out_nodes_3D_qua.txt'); % Node coordiantes [m]
+    nano_hexapod.top_plate.xi = args.top_plate_xi; % Damping ratio
+
+end
+#+end_src
+
+*** Save the Structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+#+begin_src matlab
+if nargout == 0
+  save('./mat/stages.mat', 'nano_hexapod', '-append');
+end
+#+end_src
+
+** Sample
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/initializeSample.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:initializeSample>>
+
+*** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [sample] = initializeSample(args)
+#+end_src
+
+*** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+    args.type         char    {mustBeMember(args.type,{'0', '1', '2', '3'})} = '0'
+  end
+#+end_src
+
+*** Structure initialization
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+First, we initialize the =sample= structure.
+#+begin_src matlab
+  sample = struct();
+#+end_src
+
+*** Add Sample Type
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  switch args.type
+    case '0'
+      sample.type = 0;
+    case '1'
+      sample.type = 1;
+    case '2'
+      sample.type = 2;
+    case '3'
+      sample.type = 3;
+  end
+#+end_src
+
+*** Save the Structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+The =sample= structure is saved.
+#+begin_src matlab
+  save('./mat/stages.mat', 'sample', '-append');
+#+end_src
+
+** Initialize Controller
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/initializeController.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:initializeController>>
+
+*** Function Declaration and Documentation
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [] = initializeController(args)
+#+end_src
+
+*** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+    args.type char {mustBeMember(args.type,{'open-loop', 'iff', 'dvf', 'hac-dvf', 'ref-track-L', 'ref-track-iff-L', 'cascade-hac-lac', 'hac-iff', 'stabilizing'})} = 'open-loop'
+  end
+#+end_src
+
+*** Structure initialization
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+First, we initialize the =controller= structure.
+#+begin_src matlab
+  controller = struct();
+#+end_src
+
+*** Controller Type
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  switch args.type
+    case 'open-loop'
+      controller.type = 1;
+      controller.name = 'Open-Loop';
+    case 'dvf'
+      controller.type = 2;
+      controller.name = 'Decentralized Direct Velocity Feedback';
+    case 'iff'
+      controller.type = 3;
+      controller.name = 'Decentralized Integral Force Feedback';
+    case 'hac-dvf'
+      controller.type = 4;
+      controller.name = 'HAC-DVF';
+    case 'ref-track-L'
+      controller.type = 5;
+      controller.name = 'Reference Tracking in the frame of the legs';
+    case 'ref-track-iff-L'
+      controller.type = 6;
+      controller.name = 'Reference Tracking in the frame of the legs + IFF';
+    case 'cascade-hac-lac'
+      controller.type = 7;
+      controller.name = 'Cascade Control + HAC-LAC';
+    case 'hac-iff'
+      controller.type = 8;
+      controller.name = 'HAC-IFF';
+    case 'stabilizing'
+      controller.type = 9;
+      controller.name = 'Stabilizing Controller';
+  end
+#+end_src
+
+*** Save the Structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+The =controller= structure is saved.
+#+begin_src matlab
+  save('./mat/controller.mat', 'controller');
+#+end_src
+
+** Generate Reference Signals
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/initializeReferences.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:initializeReferences>>
+
+*** Function Declaration and Documentation
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [ref] = initializeReferences(args)
+#+end_src
+
+*** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+      % Sampling Frequency [s]
+      args.Ts           (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
+      % Maximum simulation time [s]
+      args.Tmax         (1,1) double {mustBeNumeric, mustBePositive} = 100
+      % Either "constant" / "triangular" / "sinusoidal"
+      args.Dy_type      char {mustBeMember(args.Dy_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
+      % Amplitude of the displacement [m]
+      args.Dy_amplitude (1,1) double {mustBeNumeric} = 0
+      % Period of the displacement [s]
+      args.Dy_period    (1,1) double {mustBeNumeric, mustBePositive} = 1
+      % Either "constant" / "triangular" / "sinusoidal"
+      args.Ry_type      char {mustBeMember(args.Ry_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
+      % Amplitude [rad]
+      args.Ry_amplitude (1,1) double {mustBeNumeric} = 0
+      % Period of the displacement [s]
+      args.Ry_period    (1,1) double {mustBeNumeric, mustBePositive} = 1
+      % Either "constant" / "rotating"
+      args.Rz_type      char {mustBeMember(args.Rz_type,{'constant', 'rotating', 'rotating-not-filtered'})} = 'constant'
+      % Initial angle [rad]
+      args.Rz_amplitude (1,1) double {mustBeNumeric} = 0
+      % Period of the rotating [s]
+      args.Rz_period    (1,1) double {mustBeNumeric, mustBePositive} = 1
+      % For now, only constant is implemented
+      args.Dh_type      char {mustBeMember(args.Dh_type,{'constant'})} = 'constant'
+      % Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
+      args.Dh_pos       (6,1) double {mustBeNumeric} = zeros(6, 1), ...
+      % For now, only constant is implemented
+      args.Rm_type      char {mustBeMember(args.Rm_type,{'constant'})} = 'constant'
+      % Initial position of the two masses
+      args.Rm_pos       (2,1) double {mustBeNumeric} = [0; pi]
+      % For now, only constant is implemented
+      args.Dn_type      char {mustBeMember(args.Dn_type,{'constant'})} = 'constant'
+      % Initial position [m,m,m,rad,rad,rad] of the top platform
+      args.Dn_pos       (6,1) double {mustBeNumeric} = zeros(6,1)
+  end
+#+end_src
+
+
+*** Initialize Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  %% Set Sampling Time
+  Ts = args.Ts;
+  Tmax = args.Tmax;
+
+  %% Low Pass Filter to filter out the references
+  s = zpk('s');
+  w0 = 2*pi*10;
+  xi = 1;
+  H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2);
+#+end_src
+
+*** Translation Stage
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  %% Translation stage - Dy
+  t = 0:Ts:Tmax; % Time Vector [s]
+  Dy   = zeros(length(t), 1);
+  Dyd  = zeros(length(t), 1);
+  Dydd = zeros(length(t), 1);
+  switch args.Dy_type
+    case 'constant'
+      Dy(:)   = args.Dy_amplitude;
+      Dyd(:)  = 0;
+      Dydd(:) = 0;
+    case 'triangular'
+      % This is done to unsure that we start with no displacement
+      Dy_raw = args.Dy_amplitude*sawtooth(2*pi*t/args.Dy_period,1/2);
+      i0 = find(t>=args.Dy_period/4,1);
+      Dy(1:end-i0+1) = Dy_raw(i0:end);
+      Dy(end-i0+2:end) = Dy_raw(end); % we fix the last value
+
+      % The signal is filtered out
+      Dy   = lsim(H_lpf,     Dy, t);
+      Dyd  = lsim(H_lpf*s,   Dy, t);
+      Dydd = lsim(H_lpf*s^2, Dy, t);
+    case 'sinusoidal'
+      Dy(:) = args.Dy_amplitude*sin(2*pi/args.Dy_period*t);
+      Dyd   = args.Dy_amplitude*2*pi/args.Dy_period*cos(2*pi/args.Dy_period*t);
+      Dydd  = -args.Dy_amplitude*(2*pi/args.Dy_period)^2*sin(2*pi/args.Dy_period*t);
+    otherwise
+      warning('Dy_type is not set correctly');
+  end
+
+  Dy = struct('time', t, 'signals', struct('values', Dy), 'deriv', Dyd, 'dderiv', Dydd);
+#+end_src
+
+*** Tilt Stage
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  %% Tilt Stage - Ry
+  t = 0:Ts:Tmax; % Time Vector [s]
+  Ry   = zeros(length(t), 1);
+  Ryd  = zeros(length(t), 1);
+  Rydd = zeros(length(t), 1);
+
+  switch args.Ry_type
+    case 'constant'
+      Ry(:) = args.Ry_amplitude;
+      Ryd(:)   = 0;
+      Rydd(:)  = 0;
+    case 'triangular'
+      Ry_raw = args.Ry_amplitude*sawtooth(2*pi*t/args.Ry_period,1/2);
+      i0 = find(t>=args.Ry_period/4,1);
+      Ry(1:end-i0+1) = Ry_raw(i0:end);
+      Ry(end-i0+2:end) = Ry_raw(end); % we fix the last value
+
+      % The signal is filtered out
+      Ry   = lsim(H_lpf,     Ry, t);
+      Ryd  = lsim(H_lpf*s,   Ry, t);
+      Rydd = lsim(H_lpf*s^2, Ry, t);
+    case 'sinusoidal'
+      Ry(:) = args.Ry_amplitude*sin(2*pi/args.Ry_period*t);
+
+      Ryd  = args.Ry_amplitude*2*pi/args.Ry_period*cos(2*pi/args.Ry_period*t);
+      Rydd = -args.Ry_amplitude*(2*pi/args.Ry_period)^2*sin(2*pi/args.Ry_period*t);
+    otherwise
+      warning('Ry_type is not set correctly');
+  end
+
+  Ry = struct('time', t, 'signals', struct('values', Ry), 'deriv', Ryd, 'dderiv', Rydd);
+#+end_src
+
+*** Spindle
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  %% Spindle - Rz
+  t = 0:Ts:Tmax; % Time Vector [s]
+  Rz   = zeros(length(t), 1);
+  Rzd  = zeros(length(t), 1);
+  Rzdd = zeros(length(t), 1);
+
+  switch args.Rz_type
+    case 'constant'
+      Rz(:)   = args.Rz_amplitude;
+      Rzd(:)  = 0;
+      Rzdd(:) = 0;
+    case 'rotating-not-filtered'
+      Rz(:) = 2*pi/args.Rz_period*t;
+
+      % The signal is filtered out
+      Rz(:)   = 2*pi/args.Rz_period*t;
+      Rzd(:)  = 2*pi/args.Rz_period;
+      Rzdd(:) = 0;
+
+      % We add the angle offset
+      Rz = Rz + args.Rz_amplitude;
+
+    case 'rotating'
+      Rz(:) = 2*pi/args.Rz_period*t;
+
+      % The signal is filtered out
+      Rz   = lsim(H_lpf,     Rz, t);
+      Rzd  = lsim(H_lpf*s,   Rz, t);
+      Rzdd = lsim(H_lpf*s^2, Rz, t);
+
+      % We add the angle offset
+      Rz = Rz + args.Rz_amplitude;
+    otherwise
+      warning('Rz_type is not set correctly');
+  end
+
+  Rz = struct('time', t, 'signals', struct('values', Rz), 'deriv', Rzd, 'dderiv', Rzdd);
+#+end_src
+
+*** Micro Hexapod
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  %% Micro-Hexapod
+  t = [0, Ts];
+  Dh = zeros(length(t), 6);
+  Dhl = zeros(length(t), 6);
+
+  switch args.Dh_type
+    case 'constant'
+      Dh = [args.Dh_pos, args.Dh_pos];
+
+      load('mat/stages.mat', 'micro_hexapod');
+
+      AP = [args.Dh_pos(1) ; args.Dh_pos(2) ; args.Dh_pos(3)];
+
+      tx = args.Dh_pos(4);
+      ty = args.Dh_pos(5);
+      tz = args.Dh_pos(6);
+
+      ARB = [cos(tz) -sin(tz) 0;
+             sin(tz)  cos(tz) 0;
+             0        0       1]*...
+            [ cos(ty) 0 sin(ty);
+              0       1 0;
+             -sin(ty) 0 cos(ty)]*...
+            [1 0        0;
+             0 cos(tx) -sin(tx);
+             0 sin(tx)  cos(tx)];
+
+      [~, Dhl] = inverseKinematics(micro_hexapod, 'AP', AP, 'ARB', ARB);
+      Dhl = [Dhl, Dhl];
+    otherwise
+      warning('Dh_type is not set correctly');
+  end
+
+  Dh = struct('time', t, 'signals', struct('values', Dh));
+  Dhl = struct('time', t, 'signals', struct('values', Dhl));
+#+end_src
+
+*** Save
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+      %% Save
+      save('./mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts');
+  end
+#+end_src
+
+** Initialize Position Errors
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/initializePosError.m
+:header-args:matlab+: :comments none :mkdirp yes
+:header-args:matlab+: :eval no :results none
+:END:
+<<sec:initializePosError>>
+
+*** Function Declaration and Documentation
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [] = initializePosError(args)
+  % initializePosError - Initialize the position errors
+  %
+  % Syntax: [] = initializePosError(args)
+  %
+  % Inputs:
+  %    - args -
+
+#+end_src
+
+*** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+      args.error    logical {mustBeNumericOrLogical} = false
+      args.Dy (1,1) double  {mustBeNumeric} = 0 % [m]
+      args.Ry (1,1) double  {mustBeNumeric} = 0 % [m]
+      args.Rz (1,1) double  {mustBeNumeric} = 0 % [m]
+  end
+#+end_src
+
+*** Structure initialization
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+First, we initialize the =pos_error= structure.
+#+begin_src matlab
+  pos_error = struct();
+#+end_src
+
+*** Type
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  if args.error
+    pos_error.type = 1;
+  else
+    pos_error.type = 0;
+  end
+#+end_src
+
+*** Position Errors
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  pos_error.Dy = args.Dy;
+  pos_error.Ry = args.Ry;
+  pos_error.Rz = args.Rz;
+#+end_src
+
+*** Save
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  save('./mat/pos_error.mat', 'pos_error');
+#+end_src
+** Initialize Rz Estimate
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/initializeRzEstimate.m
+:header-args:matlab+: :comments none :mkdirp yes
+:header-args:matlab+: :eval no :results none
+:END:
+<<sec:initializeRzEstimate>>
+
+*** Function Declaration and Documentation
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+function [] = initializeRzEstimate(args)
+% initializeRzEstimate - Initialize the position errors
+%
+% Syntax: [] = initializeRzEstimate(args)
+%
+% Inputs:
+%    - args -
+
+#+end_src
+
+*** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+arguments
+  args.type         char    {mustBeMember(args.type,{'encoders', 'voltages'})} = 'encoders'
+end
+#+end_src
+
+*** Structure initialization
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+First, we initialize the =rz_estimate= structure.
+#+begin_src matlab
+rz_estimate = struct();
+#+end_src
+
+*** Type
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+switch args.type
+  case 'encoders'
+    rz_estimate.type = 0;
+  case 'voltages'
+    rz_estimate.type = 1;
+end
+#+end_src
+
+*** Position Errors
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+load('mat/stages.mat', 'nano_hexapod');
+rz_estimate.J_L_to_X = pinv(nano_hexapod.geometry.J);
+rz_estimate.pz_sensitivity = -240e-6/8.5; % [m/V]
+#+end_src
+
+*** Save
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+save('./mat/stages.mat', 'rz_estimate', '-append');
+#+end_src
+
+** Initialize Lion Metrology
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/initializeLion.m
+:header-args:matlab+: :comments none :mkdirp yes
+:header-args:matlab+: :eval no :results none
+:END:
+<<sec:initializeLion>>
+
+*** Function Declaration and Documentation
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [] = initializeLion(args)
+  % initializeLion - Initialize the position errors
+  %
+  % Syntax: [] = initializeLion(args)
+  %
+  % Inputs:
+  %    - args -
+
+#+end_src
+
+*** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+    args.type         char    {mustBeMember(args.type,{'rigid'})} = 'rigid'
+  end
+#+end_src
+
+*** Structure initialization
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+First, we initialize the =rz_estimate= structure.
+#+begin_src matlab
+lion = struct();
+#+end_src
+
+*** Jacobian
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+lion.J_int_to_X = [ 0                 0                -0.787401574803149 -0.212598425196851  0;
+                    0.78740157480315  0.21259842519685  0                  0                  0;
+                    0                 0                 0                  0                 -1;
+                  -13.1233595800525  13.1233595800525   0                  0                  0;
+                    0                 0               -13.1233595800525   13.1233595800525    0];
+#+end_src
+
+*** Save
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+save('./mat/stages.mat', 'lion', '-append');
+#+end_src
+** Initialize Disturbances
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/initializeDisturbances.m
+:header-args:matlab+: :comments none :mkdirp yes
+:header-args:matlab+: :eval no :results none
+:END:
+<<sec:initializeDisturbances>>
+
+*** Function Declaration and Documentation
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [] = initializeDisturbances(args)
+  % initializeDisturbances - Initialize the disturbances
+  %
+  % Syntax: [] = initializeDisturbances(args)
+  %
+  % Inputs:
+  %    - args -
+
+#+end_src
+
+*** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+      % Global parameter to enable or disable the disturbances
+      args.enable logical {mustBeNumericOrLogical} = true
+      % Ground Motion - X direction
+      args.Dwx logical {mustBeNumericOrLogical} = true
+      % Ground Motion - Y direction
+      args.Dwy logical {mustBeNumericOrLogical} = true
+      % Ground Motion - Z direction
+      args.Dwz logical {mustBeNumericOrLogical} = true
+      % Translation Stage - X direction
+      args.Fty_x logical {mustBeNumericOrLogical} = true
+      % Translation Stage - Z direction
+      args.Fty_z logical {mustBeNumericOrLogical} = true
+      % Spindle - Z direction
+      args.Frz_z logical {mustBeNumericOrLogical} = true
+  end
+#+end_src
+
+
+*** Load Data
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  load('./mat/dist_psd.mat', 'dist_f');
+#+end_src
+
+We remove the first frequency point that usually is very large.
+#+begin_src matlab :exports none
+  dist_f.f = dist_f.f(2:end);
+  dist_f.psd_gm = dist_f.psd_gm(2:end);
+  dist_f.psd_ty = dist_f.psd_ty(2:end);
+  dist_f.psd_rz = dist_f.psd_rz(2:end);
+#+end_src
+
+*** Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+We define some parameters that will be used in the algorithm.
+#+begin_src matlab
+  Fs = 2*dist_f.f(end);      % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
+  N  = 2*length(dist_f.f);   % Number of Samples match the one of the wanted PSD
+  T0 = N/Fs;                 % Signal Duration [s]
+  df = 1/T0;                 % Frequency resolution of the DFT [Hz]
+                             % Also equal to (dist_f.f(2)-dist_f.f(1))
+  t = linspace(0, T0, N+1)'; % Time Vector [s]
+  Ts = 1/Fs;                 % Sampling Time [s]
+#+end_src
+
+*** Ground Motion
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  phi = dist_f.psd_gm;
+  C = zeros(N/2,1);
+  for i = 1:N/2
+    C(i) = sqrt(phi(i)*df);
+  end
+#+end_src
+
+#+begin_src matlab
+  if args.Dwx && args.enable
+    rng(111);
+    theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
+    Cx = [0 ; C.*complex(cos(theta),sin(theta))];
+    Cx = [Cx; flipud(conj(Cx(2:end)))];;
+    Dwx = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m]
+  else
+    Dwx = zeros(length(t), 1);
+  end
+#+end_src
+
+#+begin_src matlab
+  if args.Dwy && args.enable
+    rng(112);
+    theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
+    Cx = [0 ; C.*complex(cos(theta),sin(theta))];
+    Cx = [Cx; flipud(conj(Cx(2:end)))];;
+    Dwy = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m]
+  else
+    Dwy = zeros(length(t), 1);
+  end
+#+end_src
+
+#+begin_src matlab
+  if args.Dwy && args.enable
+    rng(113);
+    theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
+    Cx = [0 ; C.*complex(cos(theta),sin(theta))];
+    Cx = [Cx; flipud(conj(Cx(2:end)))];;
+    Dwz = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m]
+  else
+    Dwz = zeros(length(t), 1);
+  end
+#+end_src
+
+*** Translation Stage - X direction
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  if args.Fty_x && args.enable
+    phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate
+    C = zeros(N/2,1);
+    for i = 1:N/2
+      C(i) = sqrt(phi(i)*df);
+    end
+    rng(121);
+    theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
+    Cx = [0 ; C.*complex(cos(theta),sin(theta))];
+    Cx = [Cx; flipud(conj(Cx(2:end)))];;
+    u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty x [N]
+    Fty_x = u;
+  else
+    Fty_x = zeros(length(t), 1);
+  end
+#+end_src
+
+*** Translation Stage - Z direction
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  if args.Fty_z && args.enable
+    phi = dist_f.psd_ty;
+    C = zeros(N/2,1);
+    for i = 1:N/2
+      C(i) = sqrt(phi(i)*df);
+    end
+    rng(122);
+    theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
+    Cx = [0 ; C.*complex(cos(theta),sin(theta))];
+    Cx = [Cx; flipud(conj(Cx(2:end)))];;
+    u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty z [N]
+    Fty_z = u;
+  else
+    Fty_z = zeros(length(t), 1);
+  end
+#+end_src
+
+*** Spindle - Z direction
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  if args.Frz_z && args.enable
+    phi = dist_f.psd_rz;
+    C = zeros(N/2,1);
+    for i = 1:N/2
+      C(i) = sqrt(phi(i)*df);
+    end
+    rng(131);
+    theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
+    Cx = [0 ; C.*complex(cos(theta),sin(theta))];
+    Cx = [Cx; flipud(conj(Cx(2:end)))];;
+    u = N/sqrt(2)*ifft(Cx); % Disturbance Force Rz z [N]
+    Frz_z = u;
+  else
+    Frz_z = zeros(length(t), 1);
+  end
+#+end_src
+
+*** Direct Forces
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  u = zeros(length(t), 6);
+  Fd = u;
+#+end_src
+
+*** Set initial value to zero
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  Dwx    = Dwx   - Dwx(1);
+  Dwy    = Dwy   - Dwy(1);
+  Dwz    = Dwz   - Dwz(1);
+  Fty_x  = Fty_x - Fty_x(1);
+  Fty_z  = Fty_z - Fty_z(1);
+  Frz_z  = Frz_z - Frz_z(1);
+#+end_src
+
+*** Save
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  save('./mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't', 'args');
+#+end_src
+
+** Initialize Stewart Platform
+*** =initializeStewartPlatform=: Initialize the Stewart Platform structure
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/initializeStewartPlatform.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:initializeStewartPlatform>>
+
+This Matlab function is accessible [[file:./matlab/src/initializeStewartPlatform.m][here]].
+
+**** Documentation
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+#+name: fig:stewart-frames-position
+#+caption: Definition of the position of the frames
+[[file:figs/stewart-frames-position.png]]
+
+**** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [stewart] = initializeStewartPlatform()
+  % initializeStewartPlatform - Initialize the stewart structure
+  %
+  % Syntax: [stewart] = initializeStewartPlatform(args)
+  %
+  % Outputs:
+  %    - stewart - A structure with the following sub-structures:
+  %      - platform_F -
+  %      - platform_M -
+  %      - joints_F   -
+  %      - joints_M   -
+  %      - struts_F   -
+  %      - struts_M   -
+  %      - actuators  -
+  %      - geometry   -
+  %      - properties -
+#+end_src
+
+**** Initialize the Stewart structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  stewart = struct();
+  stewart.platform_F = struct();
+  stewart.platform_M = struct();
+  stewart.joints_F   = struct();
+  stewart.joints_M   = struct();
+  stewart.struts_F   = struct();
+  stewart.struts_M   = struct();
+  stewart.actuators  = struct();
+  stewart.sensors    = struct();
+  stewart.sensors.inertial = struct();
+  stewart.sensors.force    = struct();
+  stewart.sensors.relative = struct();
+  stewart.geometry   = struct();
+  stewart.kinematics = struct();
+#+end_src
+
+*** =initializeFramesPositions=: Initialize the positions of frames {A}, {B}, {F} and {M}
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/initializeFramesPositions.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:initializeFramesPositions>>
+
+This Matlab function is accessible [[file:./matlab/src/initializeFramesPositions.m][here]].
+
+**** Documentation
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+#+name: fig:stewart-frames-position
+#+caption: Definition of the position of the frames
+[[file:figs/stewart-frames-position.png]]
+
+**** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [stewart] = initializeFramesPositions(stewart, args)
+  % initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
+  %
+  % Syntax: [stewart] = initializeFramesPositions(stewart, args)
+  %
+  % Inputs:
+  %    - args - Can have the following fields:
+  %        - H    [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m]
+  %        - MO_B [1x1] - Height of the frame {B} with respect to {M} [m]
+  %
+  % Outputs:
+  %    - stewart - A structure with the following fields:
+  %        - geometry.H      [1x1] - Total Height of the Stewart Platform [m]
+  %        - geometry.FO_M   [3x1] - Position of {M} with respect to {F} [m]
+  %        - platform_M.MO_B [3x1] - Position of {B} with respect to {M} [m]
+  %        - platform_F.FO_A [3x1] - Position of {A} with respect to {F} [m]
+#+end_src
+
+**** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+      stewart
+      args.H    (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
+      args.MO_B (1,1) double {mustBeNumeric} = 50e-3
+  end
+#+end_src
+
+**** Compute the position of each frame
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  H = args.H; % Total Height of the Stewart Platform [m]
+
+  FO_M = [0; 0; H]; % Position of {M} with respect to {F} [m]
+
+  MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m]
+
+  FO_A = MO_B + FO_M; % Position of {A} with respect to {F} [m]
+#+end_src
+
+**** Populate the =stewart= structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  stewart.geometry.H      = H;
+  stewart.geometry.FO_M   = FO_M;
+  stewart.platform_M.MO_B = MO_B;
+  stewart.platform_F.FO_A = FO_A;
+#+end_src
+
+*** =generateGeneralConfiguration=: Generate a Very General Configuration
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/generateGeneralConfiguration.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:generateGeneralConfiguration>>
+
+This Matlab function is accessible [[file:./matlab/src/generateGeneralConfiguration.m][here]].
+
+**** Documentation
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+Joints are positions on a circle centered with the Z axis of {F} and {M} and at a chosen distance from {F} and {M}.
+The radius of the circles can be chosen as well as the angles where the joints are located (see Figure ref:fig:joint_position_general).
+
+#+begin_src latex :file stewart_bottom_plate.pdf :tangle no
+  \begin{tikzpicture}
+    % Internal and external limit
+    \draw[fill=white!80!black] (0, 0) circle [radius=3];
+    % Circle where the joints are located
+    \draw[dashed] (0, 0) circle [radius=2.5];
+
+    % Bullets for the positions of the joints
+    \node[] (J1) at ( 80:2.5){$\bullet$};
+    \node[] (J2) at (100:2.5){$\bullet$};
+    \node[] (J3) at (200:2.5){$\bullet$};
+    \node[] (J4) at (220:2.5){$\bullet$};
+    \node[] (J5) at (320:2.5){$\bullet$};
+    \node[] (J6) at (340:2.5){$\bullet$};
+
+    % Name of the points
+    \node[above right] at (J1) {$a_{1}$};
+    \node[above left]  at (J2) {$a_{2}$};
+    \node[above left]  at (J3) {$a_{3}$};
+    \node[right     ]  at (J4) {$a_{4}$};
+    \node[left      ]  at (J5) {$a_{5}$};
+    \node[above right] at (J6) {$a_{6}$};
+
+    % First 2 angles
+    \draw[dashed, ->] (0:1)   arc [start angle=0, end angle=80, radius=1]    node[below right]{$\theta_{1}$};
+    \draw[dashed, ->] (0:1.5) arc [start angle=0, end angle=100, radius=1.5] node[left       ]{$\theta_{2}$};
+
+    % Division of 360 degrees by 3
+    \draw[dashed] (0, 0) -- ( 80:3.2);
+    \draw[dashed] (0, 0) -- (100:3.2);
+    \draw[dashed] (0, 0) -- (200:3.2);
+    \draw[dashed] (0, 0) -- (220:3.2);
+    \draw[dashed] (0, 0) -- (320:3.2);
+    \draw[dashed] (0, 0) -- (340:3.2);
+
+    % Radius for the position of the joints
+    \draw[<->] (0, 0) --node[near end, above]{$R$} (180:2.5);
+
+    \draw[->] (0, 0) -- ++(3.4, 0) node[above]{$x$};
+    \draw[->] (0, 0) -- ++(0, 3.4) node[left]{$y$};
+  \end{tikzpicture}
+#+end_src
+
+#+name: fig:joint_position_general
+#+caption: Position of the joints
+#+RESULTS:
+[[file:figs/stewart_bottom_plate.png]]
+
+**** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  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}
+#+end_src
+
+**** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  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
+#+end_src
+
+**** Compute the pose
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  Fa = zeros(3,6);
+  Mb = zeros(3,6);
+#+end_src
+
+#+begin_src matlab
+  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
+#+end_src
+
+**** Populate the =stewart= structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  stewart.platform_F.Fa = Fa;
+  stewart.platform_M.Mb = Mb;
+#+end_src
+
+*** =computeJointsPose=: Compute the Pose of the Joints
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/computeJointsPose.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:computeJointsPose>>
+
+This Matlab function is accessible [[file:./matlab/src/computeJointsPose.m][here]].
+
+**** Documentation
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+#+name: fig:stewart-struts
+#+caption: Position and orientation of the struts
+[[file:figs/stewart-struts.png]]
+
+**** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  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}
+#+end_src
+
+**** Check the =stewart= structure elements
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  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;
+#+end_src
+
+**** Compute the position of the Joints
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  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]);
+#+end_src
+
+**** Compute the strut length and orientation
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As
+
+  l = vecnorm(Ab - Aa)';
+#+end_src
+
+#+begin_src matlab
+  Bs = (Bb - Ba)./vecnorm(Bb - Ba);
+#+end_src
+
+**** Compute the orientation of the Joints
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  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
+#+end_src
+
+**** Populate the =stewart= structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  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;
+#+end_src
+
+*** =initializeStewartPose=: Determine the initial stroke in each leg to have the wanted pose
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/initializeStewartPose.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:initializeStewartPose>>
+
+This Matlab function is accessible [[file:./matlab/src/initializeStewartPose.m][here]].
+
+**** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [stewart] = initializeStewartPose(stewart, args)
+  % initializeStewartPose - Determine the initial stroke in each leg to have the wanted pose
+  %                         It uses the inverse kinematic
+  %
+  % Syntax: [stewart] = initializeStewartPose(stewart, args)
+  %
+  % Inputs:
+  %    - stewart - A structure with the following fields
+  %        - Aa   [3x6] - The positions ai expressed in {A}
+  %        - Bb   [3x6] - The positions bi expressed in {B}
+  %    - args - Can have the following fields:
+  %        - AP   [3x1] - The wanted position of {B} with respect to {A}
+  %        - ARB  [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
+  %
+  % Outputs:
+  %    - stewart - updated Stewart structure with the added fields:
+  %      - actuators.Leq [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
+#+end_src
+
+**** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+      stewart
+      args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
+      args.ARB (3,3) double {mustBeNumeric} = eye(3)
+  end
+#+end_src
+
+**** Use the Inverse Kinematic function
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  [Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);
+#+end_src
+
+**** Populate the =stewart= structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  stewart.actuators.Leq = dLi;
+#+end_src
+
+*** =initializeCylindricalPlatforms=: Initialize the geometry of the Fixed and Mobile Platforms
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/initializeCylindricalPlatforms.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:initializeCylindricalPlatforms>>
+
+This Matlab function is accessible [[file:./matlab/src/initializeCylindricalPlatforms.m][here]].
+
+**** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  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]
+#+end_src
+
+**** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  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
+#+end_src
+
+**** Compute the Inertia matrices of platforms
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  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]);
+#+end_src
+
+#+begin_src matlab
+  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]);
+#+end_src
+
+**** Populate the =stewart= structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  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;
+#+end_src
+
+#+begin_src matlab
+  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;
+#+end_src
+
+*** =initializeCylindricalStruts=: Define the inertia of cylindrical struts
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/initializeCylindricalStruts.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:initializeCylindricalStruts>>
+
+This Matlab function is accessible [[file:./matlab/src/initializeCylindricalStruts.m][here]].
+
+**** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  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]
+#+end_src
+
+**** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  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
+#+end_src
+
+**** Compute the properties of the cylindrical struts
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+#+begin_src matlab
+  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;
+#+end_src
+
+#+begin_src matlab
+  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
+#+end_src
+
+**** Populate the =stewart= structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  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;
+#+end_src
+
+#+begin_src matlab
+  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;
+#+end_src
+
+*** =initializeStrutDynamics=: Add Stiffness and Damping properties of each strut
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/initializeStrutDynamics.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:initializeStrutDynamics>>
+
+This Matlab function is accessible [[file:./matlab/src/initializeStrutDynamics.m][here]].
+
+**** Documentation
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+#+name: fig:piezoelectric_stack
+#+attr_html: :width 500px
+#+caption: Example of a piezoelectric stach actuator (PI)
+[[file:figs/piezoelectric_stack.jpg]]
+
+A simplistic model of such amplified actuator is shown in Figure ref:fig:actuator_model_simple where:
+- $K$ represent the vertical stiffness of the actuator
+- $C$ represent the vertical damping of the actuator
+- $F$ represents the force applied by the actuator
+- $F_{m}$ represents the total measured force
+- $v_{m}$ represents the absolute velocity of the top part of the actuator
+- $d_{m}$ represents the total relative displacement of the actuator
+
+#+begin_src latex :file actuator_model_simple.pdf :tangle no
+  \begin{tikzpicture}
+    \draw (-1, 0) -- (1, 0);
+
+    % Spring, Damper, and Actuator
+    \draw[spring]   (-1, 0) -- (-1, 1.5) node[midway, left=0.1]{$K$};
+    \draw[damper]   ( 0, 0) -- ( 0, 1.5) node[midway, left=0.2]{$C$};
+    \draw[actuator] ( 1, 0) -- ( 1, 1.5) node[midway, left=0.1](F){$F$};
+
+    \node[forcesensor={2}{0.2}] (fsens) at (0, 1.5){};
+
+    \node[left] at (fsens.west) {$F_{m}$};
+
+    \draw[dashed] (1, 0) -- ++(0.4, 0);
+    \draw[dashed] (1, 1.7) -- ++(0.4, 0);
+
+    \draw[->] (0, 1.7)node[]{$\bullet$} -- ++(0, 0.5) node[right]{$v_{m}$};
+
+    \draw[<->] (1.4, 0) -- ++(0, 1.7) node[midway, right]{$d_{m}$};
+  \end{tikzpicture}
+#+end_src
+
+#+name: fig:actuator_model_simple
+#+caption: Simple model of an Actuator
+#+RESULTS:
+[[file:figs/actuator_model_simple.png]]
+
+**** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [stewart] = initializeStrutDynamics(stewart, args)
+  % initializeStrutDynamics - Add Stiffness and Damping properties of each strut
+  %
+  % Syntax: [stewart] = initializeStrutDynamics(args)
+  %
+  % Inputs:
+  %    - args - Structure with the following fields:
+  %        - K [6x1] - Stiffness of each strut [N/m]
+  %        - C [6x1] - Damping of each strut [N/(m/s)]
+  %
+  % Outputs:
+  %    - stewart - updated Stewart structure with the added fields:
+  %      - actuators.type = 1
+  %      - actuators.K [6x1] - Stiffness of each strut [N/m]
+  %      - actuators.C [6x1] - Damping of each strut [N/(m/s)]
+#+end_src
+
+**** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+      stewart
+      args.type char {mustBeMember(args.type,{'classical', 'amplified'})} = 'classical'
+      args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = 20e6*ones(6,1)
+      args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e1*ones(6,1)
+      args.k1 (6,1) double {mustBeNumeric} = 1e6*ones(6,1)
+      args.ke (6,1) double {mustBeNumeric} = 5e6*ones(6,1)
+      args.ka (6,1) double {mustBeNumeric} = 60e6*ones(6,1)
+      args.c1 (6,1) double {mustBeNumeric} = 10*ones(6,1)
+      args.F_gain (6,1) double {mustBeNumeric} = 1*ones(6,1)
+      args.me (6,1) double {mustBeNumeric} = 0.01*ones(6,1)
+      args.ma (6,1) double {mustBeNumeric} = 0.01*ones(6,1)
+  end
+#+end_src
+
+**** Add Stiffness and Damping properties of each strut
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  if strcmp(args.type, 'classical')
+      stewart.actuators.type = 1;
+  elseif strcmp(args.type, 'amplified')
+      stewart.actuators.type = 2;
+  end
+
+  stewart.actuators.K = args.K;
+  stewart.actuators.C = args.C;
+
+  stewart.actuators.k1 = args.k1;
+  stewart.actuators.c1 = args.c1;
+
+  stewart.actuators.ka = args.ka;
+  stewart.actuators.ke = args.ke;
+
+  stewart.actuators.F_gain = args.F_gain;
+
+  stewart.actuators.ma = args.ma;
+  stewart.actuators.me = args.me;
+#+end_src
+
+*** =initializeJointDynamics=: Add Stiffness and Damping properties for spherical joints
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/initializeJointDynamics.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:initializeJointDynamics>>
+
+This Matlab function is accessible [[file:./matlab/src/initializeJointDynamics.m][here]].
+
+**** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [stewart] = initializeJointDynamics(stewart, args)
+  % initializeJointDynamics - Add Stiffness and Damping properties for the spherical joints
+  %
+  % Syntax: [stewart] = initializeJointDynamics(args)
+  %
+  % Inputs:
+  %    - args - Structure with the following fields:
+  %        - type_F - 'universal', 'spherical', 'universal_p', 'spherical_p'
+  %        - type_M - 'universal', 'spherical', 'universal_p', 'spherical_p'
+  %        - Kf_M [6x1] - Bending (Rx, Ry) Stiffness for each top joints [(N.m)/rad]
+  %        - Kt_M [6x1] - Torsion (Rz) Stiffness for each top joints [(N.m)/rad]
+  %        - Cf_M [6x1] - Bending (Rx, Ry) Damping of each top joint [(N.m)/(rad/s)]
+  %        - Ct_M [6x1] - Torsion (Rz) Damping of each top joint [(N.m)/(rad/s)]
+  %        - Kf_F [6x1] - Bending (Rx, Ry) Stiffness for each bottom joints [(N.m)/rad]
+  %        - Kt_F [6x1] - Torsion (Rz) Stiffness for each bottom joints [(N.m)/rad]
+  %        - Cf_F [6x1] - Bending (Rx, Ry) Damping of each bottom joint [(N.m)/(rad/s)]
+  %        - Cf_F [6x1] - Torsion (Rz) Damping of each bottom joint [(N.m)/(rad/s)]
+  %
+  % Outputs:
+  %    - stewart - updated Stewart structure with the added fields:
+  %      - stewart.joints_F and stewart.joints_M:
+  %        - type - 1 (universal), 2 (spherical), 3 (universal perfect), 4 (spherical perfect)
+  %        - Kx, Ky, Kz [6x1] - Translation (Tx, Ty, Tz) Stiffness [N/m]
+  %        - Kf [6x1] - Flexion (Rx, Ry) Stiffness [(N.m)/rad]
+  %        - Kt [6x1] - Torsion (Rz) Stiffness [(N.m)/rad]
+  %        - Cx, Cy, Cz [6x1] - Translation (Rx, Ry) Damping [N/(m/s)]
+  %        - Cf [6x1] - Flexion (Rx, Ry) Damping [(N.m)/(rad/s)]
+  %        - Cb [6x1] - Torsion (Rz) Damping [(N.m)/(rad/s)]
+#+end_src
+
+**** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+      stewart
+      args.type_F     char   {mustBeMember(args.type_F,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'universal'
+      args.type_M     char   {mustBeMember(args.type_M,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'spherical'
+      args.Kf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1)
+      args.Cf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
+      args.Kt_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1)
+      args.Ct_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
+      args.Kf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1)
+      args.Cf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
+      args.Kt_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1)
+      args.Ct_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
+      args.Ka_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1)
+      args.Ca_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
+      args.Kr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1)
+      args.Cr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
+      args.Ka_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1)
+      args.Ca_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
+      args.Kr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1)
+      args.Cr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
+      args.K_M        double {mustBeNumeric} = zeros(6,6)
+      args.M_M        double {mustBeNumeric} = zeros(6,6)
+      args.n_xyz_M    double {mustBeNumeric} = zeros(2,3)
+      args.xi_M       double {mustBeNumeric} = 0.1
+      args.step_file_M char {} = ''
+      args.K_F        double {mustBeNumeric} = zeros(6,6)
+      args.M_F        double {mustBeNumeric} = zeros(6,6)
+      args.n_xyz_F    double {mustBeNumeric} = zeros(2,3)
+      args.xi_F       double {mustBeNumeric} = 0.1
+      args.step_file_F char {} = ''
+  end
+#+end_src
+
+**** Add Actuator Type
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  switch args.type_F
+    case 'universal'
+      stewart.joints_F.type = 1;
+    case 'spherical'
+      stewart.joints_F.type = 2;
+    case 'universal_p'
+      stewart.joints_F.type = 3;
+    case 'spherical_p'
+      stewart.joints_F.type = 4;
+    case 'flexible'
+      stewart.joints_F.type = 5;
+    case 'universal_3dof'
+      stewart.joints_F.type = 6;
+    case 'spherical_3dof'
+      stewart.joints_F.type = 7;
+  end
+
+  switch args.type_M
+    case 'universal'
+      stewart.joints_M.type = 1;
+    case 'spherical'
+      stewart.joints_M.type = 2;
+    case 'universal_p'
+      stewart.joints_M.type = 3;
+    case 'spherical_p'
+      stewart.joints_M.type = 4;
+    case 'flexible'
+      stewart.joints_M.type = 5;
+    case 'universal_3dof'
+      stewart.joints_M.type = 6;
+    case 'spherical_3dof'
+      stewart.joints_M.type = 7;
+  end
+#+end_src
+
+**** Add Stiffness and Damping in Translation of each strut
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+Axial and Radial (shear) Stiffness
+#+begin_src matlab
+  stewart.joints_M.Ka = args.Ka_M;
+  stewart.joints_M.Kr = args.Kr_M;
+
+  stewart.joints_F.Ka = args.Ka_F;
+  stewart.joints_F.Kr = args.Kr_F;
+#+end_src
+
+Translation Damping
+#+begin_src matlab
+  stewart.joints_M.Ca = args.Ca_M;
+  stewart.joints_M.Cr = args.Cr_M;
+
+  stewart.joints_F.Ca = args.Ca_F;
+  stewart.joints_F.Cr = args.Cr_F;
+#+end_src
+
+**** Add Stiffness and Damping in Rotation of each strut
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+Rotational Stiffness
+#+begin_src matlab
+  stewart.joints_M.Kf = args.Kf_M;
+  stewart.joints_M.Kt = args.Kt_M;
+
+  stewart.joints_F.Kf = args.Kf_F;
+  stewart.joints_F.Kt = args.Kt_F;
+#+end_src
+
+Rotational Damping
+#+begin_src matlab
+  stewart.joints_M.Cf = args.Cf_M;
+  stewart.joints_M.Ct = args.Ct_M;
+
+  stewart.joints_F.Cf = args.Cf_F;
+  stewart.joints_F.Ct = args.Ct_F;
+#+end_src
+
+**** Stiffness and Mass matrices for flexible joint
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+#+begin_src matlab
+  stewart.joints_F.M = args.M_F;
+  stewart.joints_F.K = args.K_F;
+  stewart.joints_F.n_xyz = args.n_xyz_F;
+  stewart.joints_F.xi = args.xi_F;
+  stewart.joints_F.xi = args.xi_F;
+  stewart.joints_F.step_file = args.step_file_F;
+
+  stewart.joints_M.M = args.M_M;
+  stewart.joints_M.K = args.K_M;
+  stewart.joints_M.n_xyz = args.n_xyz_M;
+  stewart.joints_M.xi = args.xi_M;
+  stewart.joints_M.step_file = args.step_file_M;
+#+end_src
+
+*** =initializeInertialSensor=: Initialize the inertial sensor in each strut
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/initializeInertialSensor.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:initializeInertialSensor>>
+
+This Matlab function is accessible [[file:./matlab/src/initializeInertialSensor.m][here]].
+
+**** Geophone - Working Principle
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+From the schematic of the Z-axis geophone shown in Figure ref:fig:z_axis_geophone, we can write the transfer function from the support velocity $\dot{w}$ to the relative velocity of the inertial mass $\dot{d}$:
+\[ \frac{\dot{d}}{\dot{w}} = \frac{-\frac{s^2}{{\omega_0}^2}}{\frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1} \]
+with:
+- $\omega_0 = \sqrt{\frac{k}{m}}$
+- $\xi = \frac{1}{2} \sqrt{\frac{m}{k}}$
+
+#+name: fig:z_axis_geophone
+#+caption: Schematic of a Z-Axis geophone
+[[file:figs/inertial_sensor.png]]
+
+We see that at frequencies above $\omega_0$:
+\[ \frac{\dot{d}}{\dot{w}} \approx -1 \]
+
+And thus, the measurement of the relative velocity of the mass with respect to its support gives the absolute velocity of the support.
+
+We generally want to have the smallest resonant frequency $\omega_0$ to measure low frequency absolute velocity, however there is a trade-off between $\omega_0$ and the mass of the inertial mass.
+
+**** Accelerometer - Working Principle
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+From the schematic of the Z-axis accelerometer shown in Figure ref:fig:z_axis_accelerometer, we can write the transfer function from the support acceleration $\ddot{w}$ to the relative position of the inertial mass $d$:
+\[ \frac{d}{\ddot{w}} = \frac{-\frac{1}{{\omega_0}^2}}{\frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1} \]
+with:
+- $\omega_0 = \sqrt{\frac{k}{m}}$
+- $\xi = \frac{1}{2} \sqrt{\frac{m}{k}}$
+
+#+name: fig:z_axis_accelerometer
+#+caption: Schematic of a Z-Axis geophone
+[[file:figs/inertial_sensor.png]]
+
+We see that at frequencies below $\omega_0$:
+\[ \frac{d}{\ddot{w}} \approx -\frac{1}{{\omega_0}^2} \]
+
+And thus, the measurement of the relative displacement of the mass with respect to its support gives the absolute acceleration of the support.
+
+Note that there is trade-off between:
+- the highest measurable acceleration $\omega_0$
+- the sensitivity of the accelerometer which is equal to $-\frac{1}{{\omega_0}^2}$
+
+**** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [stewart] = initializeInertialSensor(stewart, args)
+  % initializeInertialSensor - Initialize the inertial sensor in each strut
+  %
+  % Syntax: [stewart] = initializeInertialSensor(args)
+  %
+  % Inputs:
+  %    - args - Structure with the following fields:
+  %        - type       - 'geophone', 'accelerometer', 'none'
+  %        - mass [1x1] - Weight of the inertial mass [kg]
+  %        - freq [1x1] - Cutoff frequency [Hz]
+  %
+  % Outputs:
+  %    - stewart - updated Stewart structure with the added fields:
+  %      - stewart.sensors.inertial
+  %        - type    - 1 (geophone), 2 (accelerometer), 3 (none)
+  %        - K [1x1] - Stiffness [N/m]
+  %        - C [1x1] - Damping [N/(m/s)]
+  %        - M [1x1] - Inertial Mass [kg]
+  %        - G [1x1] - Gain
+#+end_src
+
+**** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+      stewart
+      args.type       char   {mustBeMember(args.type,{'geophone', 'accelerometer', 'none'})} = 'none'
+      args.mass (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e-2
+      args.freq (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e3
+  end
+#+end_src
+
+**** Compute the properties of the sensor
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  sensor = struct();
+
+  switch args.type
+    case 'geophone'
+      sensor.type = 1;
+
+      sensor.M = args.mass;
+      sensor.K = sensor.M * (2*pi*args.freq)^2;
+      sensor.C = 2*sqrt(sensor.M * sensor.K);
+    case 'accelerometer'
+      sensor.type = 2;
+
+      sensor.M = args.mass;
+      sensor.K = sensor.M * (2*pi*args.freq)^2;
+      sensor.C = 2*sqrt(sensor.M * sensor.K);
+      sensor.G = -sensor.K/sensor.M;
+    case 'none'
+      sensor.type = 3;
+  end
+#+end_src
+
+**** Populate the =stewart= structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  stewart.sensors.inertial = sensor;
+#+end_src
+
+*** =displayArchitecture=: 3D plot of the Stewart platform architecture
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/displayArchitecture.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:displayArchitecture>>
+
+This Matlab function is accessible [[file:./matlab/src/displayArchitecture.m][here]].
+
+**** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [] = displayArchitecture(stewart, args)
+  % displayArchitecture - 3D plot of the Stewart platform architecture
+  %
+  % Syntax: [] = displayArchitecture(args)
+  %
+  % Inputs:
+  %    - stewart
+  %    - args - Structure with the following fields:
+  %        - AP   [3x1] - The wanted position of {B} with respect to {A}
+  %        - ARB  [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
+  %        - ARB  [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
+  %        - F_color [color] - Color used for the Fixed elements
+  %        - M_color [color] - Color used for the Mobile elements
+  %        - L_color [color] - Color used for the Legs elements
+  %        - frames    [true/false] - Display the Frames
+  %        - legs      [true/false] - Display the Legs
+  %        - joints    [true/false] - Display the Joints
+  %        - labels    [true/false] - Display the Labels
+  %        - platforms [true/false] - Display the Platforms
+  %        - views     ['all', 'xy', 'yz', 'xz', 'default'] -
+  %
+  % Outputs:
+#+end_src
+
+**** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+      stewart
+      args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
+      args.ARB (3,3) double {mustBeNumeric} = eye(3)
+      args.F_color = [0 0.4470 0.7410]
+      args.M_color = [0.8500 0.3250 0.0980]
+      args.L_color = [0 0 0]
+      args.frames    logical {mustBeNumericOrLogical} = true
+      args.legs      logical {mustBeNumericOrLogical} = true
+      args.joints    logical {mustBeNumericOrLogical} = true
+      args.labels    logical {mustBeNumericOrLogical} = true
+      args.platforms logical {mustBeNumericOrLogical} = true
+      args.views     char    {mustBeMember(args.views,{'all', 'xy', 'xz', 'yz', 'default'})} = 'default'
+  end
+#+end_src
+
+**** Check the =stewart= structure elements
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  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, 'H'),   'stewart.geometry should have attribute H')
+  H = stewart.geometry.H;
+
+  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;
+#+end_src
+
+
+**** Figure Creation, Frames and Homogeneous transformations
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+The reference frame of the 3d plot corresponds to the frame $\{F\}$.
+#+begin_src matlab
+  if ~strcmp(args.views, 'all')
+    figure;
+  else
+    f = figure('visible', 'off');
+  end
+
+  hold on;
+#+end_src
+
+We first compute homogeneous matrices that will be useful to position elements on the figure where the reference frame is $\{F\}$.
+#+begin_src matlab
+  FTa = [eye(3), FO_A; ...
+         zeros(1,3), 1];
+  ATb = [args.ARB, args.AP; ...
+         zeros(1,3), 1];
+  BTm = [eye(3), -MO_B; ...
+         zeros(1,3), 1];
+
+  FTm = FTa*ATb*BTm;
+#+end_src
+
+Let's define a parameter that define the length of the unit vectors used to display the frames.
+#+begin_src matlab
+  d_unit_vector = H/4;
+#+end_src
+
+Let's define a parameter used to position the labels with respect to the center of the element.
+#+begin_src matlab
+  d_label = H/20;
+#+end_src
+
+**** Fixed Base elements
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+Let's first plot the frame $\{F\}$.
+#+begin_src matlab
+  Ff = [0, 0, 0];
+  if args.frames
+    quiver3(Ff(1)*ones(1,3), Ff(2)*ones(1,3), Ff(3)*ones(1,3), ...
+            [d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color)
+
+    if args.labels
+      text(Ff(1) + d_label, ...
+          Ff(2) + d_label, ...
+          Ff(3) + d_label, '$\{F\}$', 'Color', args.F_color);
+    end
+  end
+#+end_src
+
+Now plot the frame $\{A\}$ fixed to the Base.
+#+begin_src matlab
+  if args.frames
+    quiver3(FO_A(1)*ones(1,3), FO_A(2)*ones(1,3), FO_A(3)*ones(1,3), ...
+            [d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color)
+
+    if args.labels
+      text(FO_A(1) + d_label, ...
+           FO_A(2) + d_label, ...
+           FO_A(3) + d_label, '$\{A\}$', 'Color', args.F_color);
+    end
+  end
+#+end_src
+
+Let's then plot the circle corresponding to the shape of the Fixed base.
+#+begin_src matlab
+  if args.platforms && stewart.platform_F.type == 1
+    theta = [0:0.01:2*pi+0.01]; % Angles [rad]
+    v = null([0; 0; 1]'); % Two vectors that are perpendicular to the circle normal
+    center = [0; 0; 0]; % Center of the circle
+    radius = stewart.platform_F.R; % Radius of the circle [m]
+
+    points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));
+
+    plot3(points(1,:), ...
+          points(2,:), ...
+          points(3,:), '-', 'Color', args.F_color);
+  end
+#+end_src
+
+Let's now plot the position and labels of the Fixed Joints
+#+begin_src matlab
+  if args.joints
+    scatter3(Fa(1,:), ...
+             Fa(2,:), ...
+             Fa(3,:), 'MarkerEdgeColor', args.F_color);
+    if args.labels
+      for i = 1:size(Fa,2)
+        text(Fa(1,i) + d_label, ...
+             Fa(2,i), ...
+             Fa(3,i), sprintf('$a_{%i}$', i), 'Color', args.F_color);
+      end
+    end
+  end
+#+end_src
+
+**** Mobile Platform elements
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+Plot the frame $\{M\}$.
+#+begin_src matlab
+  Fm = FTm*[0; 0; 0; 1]; % Get the position of frame {M} w.r.t. {F}
+
+  if args.frames
+    FM_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
+    quiver3(Fm(1)*ones(1,3), Fm(2)*ones(1,3), Fm(3)*ones(1,3), ...
+            FM_uv(1,1:3), FM_uv(2,1:3), FM_uv(3,1:3), '-', 'Color', args.M_color)
+
+    if args.labels
+      text(Fm(1) + d_label, ...
+           Fm(2) + d_label, ...
+           Fm(3) + d_label, '$\{M\}$', 'Color', args.M_color);
+    end
+  end
+#+end_src
+
+Plot the frame $\{B\}$.
+#+begin_src matlab
+  FB = FO_A + args.AP;
+
+  if args.frames
+    FB_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
+    quiver3(FB(1)*ones(1,3), FB(2)*ones(1,3), FB(3)*ones(1,3), ...
+            FB_uv(1,1:3), FB_uv(2,1:3), FB_uv(3,1:3), '-', 'Color', args.M_color)
+
+    if args.labels
+      text(FB(1) - d_label, ...
+           FB(2) + d_label, ...
+           FB(3) + d_label, '$\{B\}$', 'Color', args.M_color);
+    end
+  end
+#+end_src
+
+Let's then plot the circle corresponding to the shape of the Mobile platform.
+#+begin_src matlab
+  if args.platforms && stewart.platform_M.type == 1
+    theta = [0:0.01:2*pi+0.01]; % Angles [rad]
+    v = null((FTm(1:3,1:3)*[0;0;1])'); % Two vectors that are perpendicular to the circle normal
+    center = Fm(1:3); % Center of the circle
+    radius = stewart.platform_M.R; % Radius of the circle [m]
+
+    points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));
+
+    plot3(points(1,:), ...
+          points(2,:), ...
+          points(3,:), '-', 'Color', args.M_color);
+  end
+#+end_src
+
+Plot the position and labels of the rotation joints fixed to the mobile platform.
+#+begin_src matlab
+  if args.joints
+    Fb = FTm*[Mb;ones(1,6)];
+
+    scatter3(Fb(1,:), ...
+             Fb(2,:), ...
+             Fb(3,:), 'MarkerEdgeColor', args.M_color);
+
+    if args.labels
+      for i = 1:size(Fb,2)
+        text(Fb(1,i) + d_label, ...
+             Fb(2,i), ...
+             Fb(3,i), sprintf('$b_{%i}$', i), 'Color', args.M_color);
+      end
+    end
+  end
+#+end_src
+
+**** Legs
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+Plot the legs connecting the joints of the fixed base to the joints of the mobile platform.
+#+begin_src matlab
+  if args.legs
+    for i = 1:6
+      plot3([Fa(1,i), Fb(1,i)], ...
+            [Fa(2,i), Fb(2,i)], ...
+            [Fa(3,i), Fb(3,i)], '-', 'Color', args.L_color);
+
+      if args.labels
+        text((Fa(1,i)+Fb(1,i))/2 + d_label, ...
+             (Fa(2,i)+Fb(2,i))/2, ...
+             (Fa(3,i)+Fb(3,i))/2, sprintf('$%i$', i), 'Color', args.L_color);
+      end
+    end
+  end
+#+end_src
+
+**** Figure parameters
+#+begin_src matlab
+  switch args.views
+    case 'default'
+        view([1 -0.6 0.4]);
+    case 'xy'
+        view([0 0 1]);
+    case 'xz'
+        view([0 -1 0]);
+    case 'yz'
+        view([1 0 0]);
+  end
+  axis equal;
+  axis off;
+#+end_src
+
+**** Subplots
+#+begin_src matlab
+  if strcmp(args.views, 'all')
+    hAx = findobj('type', 'axes');
+
+    figure;
+    s1 = subplot(2,2,1);
+    copyobj(get(hAx(1), 'Children'), s1);
+    view([0 0 1]);
+    axis equal;
+    axis off;
+    title('Top')
+
+    s2 = subplot(2,2,2);
+    copyobj(get(hAx(1), 'Children'), s2);
+    view([1 -0.6 0.4]);
+    axis equal;
+    axis off;
+
+    s3 = subplot(2,2,3);
+    copyobj(get(hAx(1), 'Children'), s3);
+    view([1 0 0]);
+    axis equal;
+    axis off;
+    title('Front')
+
+    s4 = subplot(2,2,4);
+    copyobj(get(hAx(1), 'Children'), s4);
+    view([0 -1 0]);
+    axis equal;
+    axis off;
+    title('Side')
+
+    close(f);
+  end
+#+end_src
+
+
+*** =describeStewartPlatform=: Display some text describing the current defined Stewart Platform
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/describeStewartPlatform.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:describeStewartPlatform>>
+
+This Matlab function is accessible [[file:./matlab/src/describeStewartPlatform.m][here]].
+
+**** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [] = describeStewartPlatform(stewart)
+  % describeStewartPlatform - Display some text describing the current defined Stewart Platform
+  %
+  % Syntax: [] = describeStewartPlatform(args)
+  %
+  % Inputs:
+  %    - stewart
+  %
+  % Outputs:
+#+end_src
+
+**** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+      stewart
+  end
+#+end_src
+
+**** Geometry
+#+begin_src matlab
+  fprintf('GEOMETRY:\n')
+  fprintf('- The height between the fixed based and the top platform is %.3g [mm].\n', 1e3*stewart.geometry.H)
+
+  if stewart.platform_M.MO_B(3) > 0
+    fprintf('- Frame {A} is located %.3g [mm] above the top platform.\n',  1e3*stewart.platform_M.MO_B(3))
+  else
+    fprintf('- Frame {A} is located %.3g [mm] below the top platform.\n', - 1e3*stewart.platform_M.MO_B(3))
+  end
+
+  fprintf('- The initial length of the struts are:\n')
+  fprintf('\t %.3g, %.3g, %.3g, %.3g, %.3g, %.3g [mm]\n', 1e3*stewart.geometry.l)
+  fprintf('\n')
+#+end_src
+
+**** Actuators
+#+begin_src matlab
+  fprintf('ACTUATORS:\n')
+  if stewart.actuators.type == 1
+      fprintf('- The actuators are classical.\n')
+      fprintf('- The Stiffness and Damping of each actuators is:\n')
+      fprintf('\t k = %.0e [N/m] \t c = %.0e [N/(m/s)]\n', stewart.actuators.K(1), stewart.actuators.C(1))
+  elseif stewart.actuators.type == 2
+      fprintf('- The actuators are mechanicaly amplified.\n')
+      fprintf('- The vertical stiffness and damping contribution of the piezoelectric stack is:\n')
+      fprintf('\t ka = %.0e [N/m] \t ca = %.0e [N/(m/s)]\n', stewart.actuators.Ka(1), stewart.actuators.Ca(1))
+      fprintf('- Vertical stiffness when the piezoelectric stack is removed is:\n')
+      fprintf('\t kr = %.0e [N/m] \t cr = %.0e [N/(m/s)]\n', stewart.actuators.Kr(1), stewart.actuators.Cr(1))
+  end
+  fprintf('\n')
+#+end_src
+
+**** Joints
+#+begin_src matlab
+  fprintf('JOINTS:\n')
+#+end_src
+
+Type of the joints on the fixed base.
+#+begin_src matlab
+  switch stewart.joints_F.type
+    case 1
+      fprintf('- The joints on the fixed based are universal joints\n')
+    case 2
+      fprintf('- The joints on the fixed based are spherical joints\n')
+    case 3
+      fprintf('- The joints on the fixed based are perfect universal joints\n')
+    case 4
+      fprintf('- The joints on the fixed based are perfect spherical joints\n')
+  end
+#+end_src
+
+Type of the joints on the mobile platform.
+#+begin_src matlab
+  switch stewart.joints_M.type
+    case 1
+      fprintf('- The joints on the mobile based are universal joints\n')
+    case 2
+      fprintf('- The joints on the mobile based are spherical joints\n')
+    case 3
+      fprintf('- The joints on the mobile based are perfect universal joints\n')
+    case 4
+      fprintf('- The joints on the mobile based are perfect spherical joints\n')
+  end
+#+end_src
+
+Position of the fixed joints
+#+begin_src matlab
+  fprintf('- The position of the joints on the fixed based with respect to {F} are (in [mm]):\n')
+  fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_F.Fa)
+#+end_src
+
+Position of the mobile joints
+#+begin_src matlab
+  fprintf('- The position of the joints on the mobile based with respect to {M} are (in [mm]):\n')
+  fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_M.Mb)
+  fprintf('\n')
+#+end_src
+
+**** Kinematics
+#+begin_src matlab
+  fprintf('KINEMATICS:\n')
+
+  if isfield(stewart.kinematics, 'K')
+    fprintf('- The Stiffness matrix K is (in [N/m]):\n')
+    fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.K)
+  end
+
+  if isfield(stewart.kinematics, 'C')
+    fprintf('- The Damping matrix C is (in [m/N]):\n')
+    fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.C)
+  end
+#+end_src
+
+*** =generateCubicConfiguration=: Generate a Cubic Configuration
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/generateCubicConfiguration.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:generateCubicConfiguration>>
+
+This Matlab function is accessible [[file:./matlab/src/generateCubicConfiguration.m][here]].
+
+**** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [stewart] = generateCubicConfiguration(stewart, args)
+  % generateCubicConfiguration - Generate a Cubic Configuration
+  %
+  % Syntax: [stewart] = generateCubicConfiguration(stewart, args)
+  %
+  % Inputs:
+  %    - stewart - A structure with the following fields
+  %        - geometry.H [1x1] - Total height of the platform [m]
+  %    - args - Can have the following fields:
+  %        - Hc  [1x1] - Height of the "useful" part of the cube [m]
+  %        - FOc [1x1] - Height of the center of the cube with respect to {F} [m]
+  %        - FHa [1x1] - Height of the plane joining the points ai with respect to the frame {F} [m]
+  %        - MHb [1x1] - Height of the plane joining the points bi with respect to the frame {M} [m]
+  %
+  % 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}
+#+end_src
+
+**** Documentation
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+name: fig:cubic-configuration-definition
+#+caption: Cubic Configuration
+[[file:figs/cubic-configuration-definition.png]]
+
+**** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+      stewart
+      args.Hc  (1,1) double {mustBeNumeric, mustBePositive} = 60e-3
+      args.FOc (1,1) double {mustBeNumeric} = 50e-3
+      args.FHa (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3
+      args.MHb (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3
+  end
+#+end_src
+
+**** Check the =stewart= structure elements
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  assert(isfield(stewart.geometry, 'H'),   'stewart.geometry should have attribute H')
+  H = stewart.geometry.H;
+#+end_src
+
+**** Position of the Cube
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+We define the useful points of the cube with respect to the Cube's center.
+${}^{C}C$ are the 6 vertices of the cubes expressed in a frame {C} which is located at the center of the cube and aligned with {F} and {M}.
+
+#+begin_src matlab
+  sx = [ 2; -1; -1];
+  sy = [ 0;  1; -1];
+  sz = [ 1;  1;  1];
+
+  R = [sx, sy, sz]./vecnorm([sx, sy, sz]);
+
+  L = args.Hc*sqrt(3);
+
+  Cc = R'*[[0;0;L],[L;0;L],[L;0;0],[L;L;0],[0;L;0],[0;L;L]] - [0;0;1.5*args.Hc];
+
+  CCf = [Cc(:,1), Cc(:,3), Cc(:,3), Cc(:,5), Cc(:,5), Cc(:,1)]; % CCf(:,i) corresponds to the bottom cube's vertice corresponding to the i'th leg
+  CCm = [Cc(:,2), Cc(:,2), Cc(:,4), Cc(:,4), Cc(:,6), Cc(:,6)]; % CCm(:,i) corresponds to the top cube's vertice corresponding to the i'th leg
+#+end_src
+
+**** Compute the pose
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+We can compute the vector of each leg ${}^{C}\hat{\bm{s}}_{i}$ (unit vector from ${}^{C}C_{f}$ to ${}^{C}C_{m}$).
+#+begin_src matlab
+  CSi = (CCm - CCf)./vecnorm(CCm - CCf);
+#+end_src
+
+We now which to compute the position of the joints $a_{i}$ and $b_{i}$.
+#+begin_src matlab
+  Fa = CCf + [0; 0; args.FOc] + ((args.FHa-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
+  Mb = CCf + [0; 0; args.FOc-H] + ((H-args.MHb-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
+#+end_src
+
+**** Populate the =stewart= structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  stewart.platform_F.Fa = Fa;
+  stewart.platform_M.Mb = Mb;
+#+end_src
+
+*** =computeJacobian=: Compute the Jacobian Matrix
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/computeJacobian.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:computeJacobian>>
+
+This Matlab function is accessible [[file:./matlab/src/computeJacobian.m][here]].
+
+**** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  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
+#+end_src
+
+**** Check the =stewart= structure elements
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  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;
+#+end_src
+
+
+**** Compute Jacobian Matrix
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  J = [As' , cross(Ab, As)'];
+#+end_src
+
+**** Compute Stiffness Matrix
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  K = J'*diag(Ki)*J;
+#+end_src
+
+**** Compute Compliance Matrix
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  C = inv(K);
+#+end_src
+
+**** Populate the =stewart= structure
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  stewart.kinematics.J = J;
+  stewart.kinematics.K = K;
+  stewart.kinematics.C = C;
+#+end_src
+
+
+*** =inverseKinematics=: Compute Inverse Kinematics
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/inverseKinematics.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:inverseKinematics>>
+
+This Matlab function is accessible [[file:./matlab/src/inverseKinematics.m][here]].
+
+**** Theory
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+For inverse kinematic analysis, it is assumed that the position ${}^A\bm{P}$ and orientation of the moving platform ${}^A\bm{R}_B$ are given and the problem is to obtain the joint variables, namely, $\bm{L} = [l_1, l_2, \dots, l_6]^T$.
+
+From the geometry of the manipulator, the loop closure for each limb, $i = 1, 2, \dots, 6$ can be written as
+\begin{align*}
+  l_i {}^A\hat{\bm{s}}_i &= {}^A\bm{A} + {}^A\bm{b}_i - {}^A\bm{a}_i \\
+                         &= {}^A\bm{A} + {}^A\bm{R}_b {}^B\bm{b}_i - {}^A\bm{a}_i
+\end{align*}
+
+To obtain the length of each actuator and eliminate $\hat{\bm{s}}_i$, it is sufficient to dot multiply each side by itself:
+\begin{equation}
+  l_i^2 \left[ {}^A\hat{\bm{s}}_i^T {}^A\hat{\bm{s}}_i \right] = \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]^T \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]
+\end{equation}
+
+Hence, for $i = 1, 2, \dots, 6$, each limb length can be uniquely determined by:
+\begin{equation}
+  l_i = \sqrt{{}^A\bm{P}^T {}^A\bm{P} + {}^B\bm{b}_i^T {}^B\bm{b}_i + {}^A\bm{a}_i^T {}^A\bm{a}_i - 2 {}^A\bm{P}^T {}^A\bm{a}_i + 2 {}^A\bm{P}^T \left[{}^A\bm{R}_B {}^B\bm{b}_i\right] - 2 \left[{}^A\bm{R}_B {}^B\bm{b}_i\right]^T {}^A\bm{a}_i}
+\end{equation}
+
+If the position and orientation of the moving platform lie in the feasible workspace of the manipulator, one unique solution to the limb length is determined by the above equation.
+Otherwise, when the limbs' lengths derived yield complex numbers, then the position or orientation of the moving platform is not reachable.
+
+**** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [Li, dLi] = inverseKinematics(stewart, args)
+  % inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}
+  %
+  % Syntax: [stewart] = inverseKinematics(stewart)
+  %
+  % Inputs:
+  %    - stewart - A structure with the following fields
+  %        - geometry.Aa   [3x6] - The positions ai expressed in {A}
+  %        - geometry.Bb   [3x6] - The positions bi expressed in {B}
+  %        - geometry.l    [6x1] - Length of each strut
+  %    - args - Can have the following fields:
+  %        - AP   [3x1] - The wanted position of {B} with respect to {A}
+  %        - ARB  [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
+  %
+  % Outputs:
+  %    - Li   [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}
+  %    - dLi  [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
+#+end_src
+
+**** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+      stewart
+      args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
+      args.ARB (3,3) double {mustBeNumeric} = eye(3)
+  end
+#+end_src
+
+**** Check the =stewart= structure elements
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  assert(isfield(stewart.geometry, 'Aa'),   'stewart.geometry should have attribute Aa')
+  Aa = stewart.geometry.Aa;
+
+  assert(isfield(stewart.geometry, 'Bb'),   'stewart.geometry should have attribute Bb')
+  Bb = stewart.geometry.Bb;
+
+  assert(isfield(stewart.geometry, 'l'),   'stewart.geometry should have attribute l')
+  l = stewart.geometry.l;
+#+end_src
+
+
+**** Compute
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  Li = sqrt(args.AP'*args.AP + diag(Bb'*Bb) + diag(Aa'*Aa) - (2*args.AP'*Aa)' + (2*args.AP'*(args.ARB*Bb))' - diag(2*(args.ARB*Bb)'*Aa));
+#+end_src
+
+#+begin_src matlab
+  dLi = Li-l;
+#+end_src
+
+*** =forwardKinematicsApprox=: Compute the Approximate Forward Kinematics
+:PROPERTIES:
+:header-args:matlab+: :tangle ./matlab/src/forwardKinematicsApprox.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<<sec:forwardKinematicsApprox>>
+
+This Matlab function is accessible [[file:./matlab/src/forwardKinematicsApprox.m][here]].
+
+**** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [P, R] = forwardKinematicsApprox(stewart, args)
+  % forwardKinematicsApprox - Computed the approximate pose of {B} with respect to {A} from the length of each strut and using
+  %                           the Jacobian Matrix
+  %
+  % Syntax: [P, R] = forwardKinematicsApprox(stewart, args)
+  %
+  % Inputs:
+  %    - stewart - A structure with the following fields
+  %        - kinematics.J  [6x6] - The Jacobian Matrix
+  %    - args - Can have the following fields:
+  %        - dL [6x1] - Displacement of each strut [m]
+  %
+  % Outputs:
+  %    - P  [3x1] - The estimated position of {B} with respect to {A}
+  %    - R  [3x3] - The estimated rotation matrix that gives the orientation of {B} with respect to {A}
+#+end_src
+
+**** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+      stewart
+      args.dL (6,1) double {mustBeNumeric} = zeros(6,1)
+  end
+#+end_src
+
+**** Check the =stewart= structure elements
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  assert(isfield(stewart.kinematics, 'J'),   'stewart.kinematics should have attribute J')
+  J = stewart.kinematics.J;
+#+end_src
+
+**** Computation
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+From a small displacement of each strut $d\bm{\mathcal{L}}$, we can compute the
+position and orientation of {B} with respect to {A} using the following formula:
+\[ d \bm{\mathcal{X}} = \bm{J}^{-1} d\bm{\mathcal{L}} \]
+#+begin_src matlab
+  X = J\args.dL;
+#+end_src
+
+The position vector corresponds to the first 3 elements.
+#+begin_src matlab
+  P = X(1:3);
+#+end_src
+
+The next 3 elements are the orientation of {B} with respect to {A} expressed
+using the screw axis.
+#+begin_src matlab
+  theta = norm(X(4:6));
+  s = X(4:6)/theta;
+#+end_src
+
+We then compute the corresponding rotation matrix.
+#+begin_src matlab
+  R = [s(1)^2*(1-cos(theta)) + cos(theta) ,        s(1)*s(2)*(1-cos(theta)) - s(3)*sin(theta), s(1)*s(3)*(1-cos(theta)) + s(2)*sin(theta);
+       s(2)*s(1)*(1-cos(theta)) + s(3)*sin(theta), s(2)^2*(1-cos(theta)) + cos(theta),         s(2)*s(3)*(1-cos(theta)) - s(1)*sin(theta);
+       s(3)*s(1)*(1-cos(theta)) - s(2)*sin(theta), s(3)*s(2)*(1-cos(theta)) + s(1)*sin(theta), s(3)^2*(1-cos(theta)) + cos(theta)];
+#+end_src
diff --git a/figs/G_cart_loop_gain_diagonal_3dof.pdf b/figs/G_cart_loop_gain_diagonal_3dof.pdf
deleted file mode 100644
index 6e80c1c..0000000
Binary files a/figs/G_cart_loop_gain_diagonal_3dof.pdf and /dev/null differ
diff --git a/figs/G_cart_loop_gain_diagonal_3dof.png b/figs/G_cart_loop_gain_diagonal_3dof.png
deleted file mode 100644
index e036ab4..0000000
Binary files a/figs/G_cart_loop_gain_diagonal_3dof.png and /dev/null differ
diff --git a/figs/G_cart_nyquist_diagonal_3dof.pdf b/figs/G_cart_nyquist_diagonal_3dof.pdf
deleted file mode 100644
index 4a04bf2..0000000
Binary files a/figs/G_cart_nyquist_diagonal_3dof.pdf and /dev/null differ
diff --git a/figs/G_cart_nyquist_diagonal_3dof.png b/figs/G_cart_nyquist_diagonal_3dof.png
deleted file mode 100644
index 5cb1f5b..0000000
Binary files a/figs/G_cart_nyquist_diagonal_3dof.png and /dev/null differ
diff --git a/figs/LION_metrology_interferometers.pdf b/figs/LION_metrology_interferometers.pdf
deleted file mode 100644
index 1e7e2de..0000000
Binary files a/figs/LION_metrology_interferometers.pdf and /dev/null differ
diff --git a/figs/LION_metrology_interferometers.png b/figs/LION_metrology_interferometers.png
deleted file mode 100644
index ef2cac5..0000000
Binary files a/figs/LION_metrology_interferometers.png and /dev/null differ
diff --git a/figs/LION_picture_close.jpg b/figs/LION_picture_close.jpg
deleted file mode 100644
index 842ecc5..0000000
Binary files a/figs/LION_picture_close.jpg and /dev/null differ
diff --git a/figs/align_top_sphere_comparators.jpg b/figs/align_top_sphere_comparators.jpg
deleted file mode 100644
index 1809bbb..0000000
Binary files a/figs/align_top_sphere_comparators.jpg and /dev/null differ
diff --git a/figs/id31_Ghac_6x6_plant_comp_model.pdf b/figs/id31_Ghac_6x6_plant_comp_model.pdf
deleted file mode 100644
index 865fc0b..0000000
Binary files a/figs/id31_Ghac_6x6_plant_comp_model.pdf and /dev/null differ
diff --git a/figs/id31_Ghac_6x6_plant_comp_model.png b/figs/id31_Ghac_6x6_plant_comp_model.png
deleted file mode 100644
index 7a7a054..0000000
Binary files a/figs/id31_Ghac_6x6_plant_comp_model.png and /dev/null differ
diff --git a/figs/id31_Ghac_effect_mass.pdf b/figs/id31_Ghac_effect_mass.pdf
deleted file mode 100644
index f56cb46..0000000
Binary files a/figs/id31_Ghac_effect_mass.pdf and /dev/null differ
diff --git a/figs/id31_Ghac_effect_mass.png b/figs/id31_Ghac_effect_mass.png
deleted file mode 100644
index c51ab5e..0000000
Binary files a/figs/id31_Ghac_effect_mass.png and /dev/null differ
diff --git a/figs/id31_Ghac_plant_comp_model.pdf b/figs/id31_Ghac_plant_comp_model.pdf
deleted file mode 100644
index 19f7862..0000000
Binary files a/figs/id31_Ghac_plant_comp_model.pdf and /dev/null differ
diff --git a/figs/id31_Ghac_plant_comp_model.png b/figs/id31_Ghac_plant_comp_model.png
deleted file mode 100644
index 20472e4..0000000
Binary files a/figs/id31_Ghac_plant_comp_model.png and /dev/null differ
diff --git a/figs/id31_Giff_effect_mass.pdf b/figs/id31_Giff_effect_mass.pdf
deleted file mode 100644
index 5ce994b..0000000
Binary files a/figs/id31_Giff_effect_mass.pdf and /dev/null differ
diff --git a/figs/id31_Giff_effect_mass.png b/figs/id31_Giff_effect_mass.png
deleted file mode 100644
index df3d1ae..0000000
Binary files a/figs/id31_Giff_effect_mass.png and /dev/null differ
diff --git a/figs/id31_Giff_effect_rotation.pdf b/figs/id31_Giff_effect_rotation.pdf
deleted file mode 100644
index aec5a48..0000000
Binary files a/figs/id31_Giff_effect_rotation.pdf and /dev/null differ
diff --git a/figs/id31_Giff_effect_rotation.png b/figs/id31_Giff_effect_rotation.png
deleted file mode 100644
index e29e0fd..0000000
Binary files a/figs/id31_Giff_effect_rotation.png and /dev/null differ
diff --git a/figs/id31_Giff_plant.pdf b/figs/id31_Giff_plant.pdf
deleted file mode 100644
index 86099a3..0000000
Binary files a/figs/id31_Giff_plant.pdf and /dev/null differ
diff --git a/figs/id31_Giff_plant.png b/figs/id31_Giff_plant.png
deleted file mode 100644
index 2214a96..0000000
Binary files a/figs/id31_Giff_plant.png and /dev/null differ
diff --git a/figs/id31_Giff_plant_comp_model.pdf b/figs/id31_Giff_plant_comp_model.pdf
deleted file mode 100644
index 3413652..0000000
Binary files a/figs/id31_Giff_plant_comp_model.pdf and /dev/null differ
diff --git a/figs/id31_Giff_plant_comp_model.png b/figs/id31_Giff_plant_comp_model.png
deleted file mode 100644
index 2afbdc1..0000000
Binary files a/figs/id31_Giff_plant_comp_model.png and /dev/null differ
diff --git a/figs/id31_Rz_align_dx_dy_scans.pdf b/figs/id31_Rz_align_dx_dy_scans.pdf
deleted file mode 100644
index bac276a..0000000
Binary files a/figs/id31_Rz_align_dx_dy_scans.pdf and /dev/null differ
diff --git a/figs/id31_Rz_align_dx_dy_scans.png b/figs/id31_Rz_align_dx_dy_scans.png
deleted file mode 100644
index fa68e90..0000000
Binary files a/figs/id31_Rz_align_dx_dy_scans.png and /dev/null differ
diff --git a/figs/id31_Rz_align_dx_dy_scans_before_calib.pdf b/figs/id31_Rz_align_dx_dy_scans_before_calib.pdf
deleted file mode 100644
index fc723fd..0000000
Binary files a/figs/id31_Rz_align_dx_dy_scans_before_calib.pdf and /dev/null differ
diff --git a/figs/id31_Rz_align_dx_dy_scans_before_calib.png b/figs/id31_Rz_align_dx_dy_scans_before_calib.png
deleted file mode 100644
index a9f1698..0000000
Binary files a/figs/id31_Rz_align_dx_dy_scans_before_calib.png and /dev/null differ
diff --git a/figs/id31_align_metorlogy_errors_after_align.pdf b/figs/id31_align_metorlogy_errors_after_align.pdf
deleted file mode 100644
index 063fd85..0000000
Binary files a/figs/id31_align_metorlogy_errors_after_align.pdf and /dev/null differ
diff --git a/figs/id31_align_metorlogy_errors_after_align.png b/figs/id31_align_metorlogy_errors_after_align.png
deleted file mode 100644
index ed42799..0000000
Binary files a/figs/id31_align_metorlogy_errors_after_align.png and /dev/null differ
diff --git a/figs/id31_cart_plant_3x3.pdf b/figs/id31_cart_plant_3x3.pdf
deleted file mode 100644
index 4bde41e..0000000
Binary files a/figs/id31_cart_plant_3x3.pdf and /dev/null differ
diff --git a/figs/id31_cart_plant_3x3.png b/figs/id31_cart_plant_3x3.png
deleted file mode 100644
index ac8865e..0000000
Binary files a/figs/id31_cart_plant_3x3.png and /dev/null differ
diff --git a/figs/id31_cas_effect_hac_m0.pdf b/figs/id31_cas_effect_hac_m0.pdf
deleted file mode 100644
index cff451f..0000000
Binary files a/figs/id31_cas_effect_hac_m0.pdf and /dev/null differ
diff --git a/figs/id31_cas_effect_hac_m0.png b/figs/id31_cas_effect_hac_m0.png
deleted file mode 100644
index 77e812b..0000000
Binary files a/figs/id31_cas_effect_hac_m0.png and /dev/null differ
diff --git a/figs/id31_cas_effect_lac_m0.pdf b/figs/id31_cas_effect_lac_m0.pdf
deleted file mode 100644
index c2a791a..0000000
Binary files a/figs/id31_cas_effect_lac_m0.pdf and /dev/null differ
diff --git a/figs/id31_cas_effect_lac_m0.png b/figs/id31_cas_effect_lac_m0.png
deleted file mode 100644
index bcf31dd..0000000
Binary files a/figs/id31_cas_effect_lac_m0.png and /dev/null differ
diff --git a/figs/id31_cas_m0_30rpm_ol_lac_hac.pdf b/figs/id31_cas_m0_30rpm_ol_lac_hac.pdf
deleted file mode 100644
index b36ee0e..0000000
Binary files a/figs/id31_cas_m0_30rpm_ol_lac_hac.pdf and /dev/null differ
diff --git a/figs/id31_cas_m0_30rpm_ol_lac_hac.png b/figs/id31_cas_m0_30rpm_ol_lac_hac.png
deleted file mode 100644
index fcc090f..0000000
Binary files a/figs/id31_cas_m0_30rpm_ol_lac_hac.png and /dev/null differ
diff --git a/figs/id31_comp_cart_plant_reduced_model.pdf b/figs/id31_comp_cart_plant_reduced_model.pdf
deleted file mode 100644
index 990e287..0000000
Binary files a/figs/id31_comp_cart_plant_reduced_model.pdf and /dev/null differ
diff --git a/figs/id31_comp_cart_plant_reduced_model.png b/figs/id31_comp_cart_plant_reduced_model.png
deleted file mode 100644
index ccabb70..0000000
Binary files a/figs/id31_comp_cart_plant_reduced_model.png and /dev/null differ
diff --git a/figs/id31_comp_simscape_frf_ol_enc.pdf b/figs/id31_comp_simscape_frf_ol_enc.pdf
deleted file mode 100644
index 9fcccae..0000000
Binary files a/figs/id31_comp_simscape_frf_ol_enc.pdf and /dev/null differ
diff --git a/figs/id31_comp_simscape_frf_ol_enc.png b/figs/id31_comp_simscape_frf_ol_enc.png
deleted file mode 100644
index 4f8f441..0000000
Binary files a/figs/id31_comp_simscape_frf_ol_enc.png and /dev/null differ
diff --git a/figs/id31_comp_simscape_frf_ol_iff.pdf b/figs/id31_comp_simscape_frf_ol_iff.pdf
deleted file mode 100644
index 8f0de37..0000000
Binary files a/figs/id31_comp_simscape_frf_ol_iff.pdf and /dev/null differ
diff --git a/figs/id31_comp_simscape_frf_ol_iff.png b/figs/id31_comp_simscape_frf_ol_iff.png
deleted file mode 100644
index c536cfc..0000000
Binary files a/figs/id31_comp_simscape_frf_ol_iff.png and /dev/null differ
diff --git a/figs/id31_comp_simscape_frf_ol_int.pdf b/figs/id31_comp_simscape_frf_ol_int.pdf
deleted file mode 100644
index 13fc09f..0000000
Binary files a/figs/id31_comp_simscape_frf_ol_int.pdf and /dev/null differ
diff --git a/figs/id31_comp_simscape_frf_ol_int.png b/figs/id31_comp_simscape_frf_ol_int.png
deleted file mode 100644
index 033204e..0000000
Binary files a/figs/id31_comp_simscape_frf_ol_int.png and /dev/null differ
diff --git a/figs/id31_comp_undamped_damped_plant_m0.pdf b/figs/id31_comp_undamped_damped_plant_m0.pdf
deleted file mode 100644
index 3d665a1..0000000
Binary files a/figs/id31_comp_undamped_damped_plant_m0.pdf and /dev/null differ
diff --git a/figs/id31_comp_undamped_damped_plant_m0.png b/figs/id31_comp_undamped_damped_plant_m0.png
deleted file mode 100644
index a5d653a..0000000
Binary files a/figs/id31_comp_undamped_damped_plant_m0.png and /dev/null differ
diff --git a/figs/id31_coupling_decrease_rz_align.pdf b/figs/id31_coupling_decrease_rz_align.pdf
deleted file mode 100644
index 2d3b1f0..0000000
Binary files a/figs/id31_coupling_decrease_rz_align.pdf and /dev/null differ
diff --git a/figs/id31_coupling_decrease_rz_align.png b/figs/id31_coupling_decrease_rz_align.png
deleted file mode 100644
index 7ae22ca..0000000
Binary files a/figs/id31_coupling_decrease_rz_align.png and /dev/null differ
diff --git a/figs/id31_coupling_decrease_rz_align_m3.pdf b/figs/id31_coupling_decrease_rz_align_m3.pdf
deleted file mode 100644
index cbf8a75..0000000
Binary files a/figs/id31_coupling_decrease_rz_align_m3.pdf and /dev/null differ
diff --git a/figs/id31_coupling_decrease_rz_align_m3.png b/figs/id31_coupling_decrease_rz_align_m3.png
deleted file mode 100644
index ebd8277..0000000
Binary files a/figs/id31_coupling_decrease_rz_align_m3.png and /dev/null differ
diff --git a/figs/id31_diffraction_tomo_1mms.pdf b/figs/id31_diffraction_tomo_1mms.pdf
deleted file mode 100644
index 109faf3..0000000
Binary files a/figs/id31_diffraction_tomo_1mms.pdf and /dev/null differ
diff --git a/figs/id31_diffraction_tomo_1mms.png b/figs/id31_diffraction_tomo_1mms.png
deleted file mode 100644
index a367bd9..0000000
Binary files a/figs/id31_diffraction_tomo_1mms.png and /dev/null differ
diff --git a/figs/id31_diffraction_tomo_velocities.pdf b/figs/id31_diffraction_tomo_velocities.pdf
deleted file mode 100644
index b7a0240..0000000
Binary files a/figs/id31_diffraction_tomo_velocities.pdf and /dev/null differ
diff --git a/figs/id31_diffraction_tomo_velocities.png b/figs/id31_diffraction_tomo_velocities.png
deleted file mode 100644
index 3c0dec5..0000000
Binary files a/figs/id31_diffraction_tomo_velocities.png and /dev/null differ
diff --git a/figs/id31_dirty_layer_scan_m0.pdf b/figs/id31_dirty_layer_scan_m0.pdf
deleted file mode 100644
index 21bb4a3..0000000
Binary files a/figs/id31_dirty_layer_scan_m0.pdf and /dev/null differ
diff --git a/figs/id31_dirty_layer_scan_m0.png b/figs/id31_dirty_layer_scan_m0.png
deleted file mode 100644
index da991a9..0000000
Binary files a/figs/id31_dirty_layer_scan_m0.png and /dev/null differ
diff --git a/figs/id31_dirty_layer_scan_m0_large.pdf b/figs/id31_dirty_layer_scan_m0_large.pdf
deleted file mode 100644
index 6523edb..0000000
Binary files a/figs/id31_dirty_layer_scan_m0_large.pdf and /dev/null differ
diff --git a/figs/id31_dirty_layer_scan_m0_large.png b/figs/id31_dirty_layer_scan_m0_large.png
deleted file mode 100644
index 2fdbfa4..0000000
Binary files a/figs/id31_dirty_layer_scan_m0_large.png and /dev/null differ
diff --git a/figs/id31_dz_mim_100nm_steps.pdf b/figs/id31_dz_mim_100nm_steps.pdf
deleted file mode 100644
index 2d7828c..0000000
Binary files a/figs/id31_dz_mim_100nm_steps.pdf and /dev/null differ
diff --git a/figs/id31_dz_mim_100nm_steps.png b/figs/id31_dz_mim_100nm_steps.png
deleted file mode 100644
index 3e24980..0000000
Binary files a/figs/id31_dz_mim_100nm_steps.png and /dev/null differ
diff --git a/figs/id31_dz_mim_10nm_steps.pdf b/figs/id31_dz_mim_10nm_steps.pdf
deleted file mode 100644
index 0dc3892..0000000
Binary files a/figs/id31_dz_mim_10nm_steps.pdf and /dev/null differ
diff --git a/figs/id31_dz_mim_10nm_steps.png b/figs/id31_dz_mim_10nm_steps.png
deleted file mode 100644
index 479dd83..0000000
Binary files a/figs/id31_dz_mim_10nm_steps.png and /dev/null differ
diff --git a/figs/id31_dz_steps_response.pdf b/figs/id31_dz_steps_response.pdf
deleted file mode 100644
index c49f380..0000000
Binary files a/figs/id31_dz_steps_response.pdf and /dev/null differ
diff --git a/figs/id31_dz_steps_response.png b/figs/id31_dz_steps_response.png
deleted file mode 100644
index 12c9e08..0000000
Binary files a/figs/id31_dz_steps_response.png and /dev/null differ
diff --git a/figs/id31_effect_mass_frf_ol_enc.pdf b/figs/id31_effect_mass_frf_ol_enc.pdf
deleted file mode 100644
index 7fee6aa..0000000
Binary files a/figs/id31_effect_mass_frf_ol_enc.pdf and /dev/null differ
diff --git a/figs/id31_effect_mass_frf_ol_enc.png b/figs/id31_effect_mass_frf_ol_enc.png
deleted file mode 100644
index abd98e4..0000000
Binary files a/figs/id31_effect_mass_frf_ol_enc.png and /dev/null differ
diff --git a/figs/id31_effect_mass_frf_ol_iff.pdf b/figs/id31_effect_mass_frf_ol_iff.pdf
deleted file mode 100644
index 6857ab5..0000000
Binary files a/figs/id31_effect_mass_frf_ol_iff.pdf and /dev/null differ
diff --git a/figs/id31_effect_mass_frf_ol_iff.png b/figs/id31_effect_mass_frf_ol_iff.png
deleted file mode 100644
index e3b13dc..0000000
Binary files a/figs/id31_effect_mass_frf_ol_iff.png and /dev/null differ
diff --git a/figs/id31_effect_mass_frf_ol_int.pdf b/figs/id31_effect_mass_frf_ol_int.pdf
deleted file mode 100644
index c2343bd..0000000
Binary files a/figs/id31_effect_mass_frf_ol_int.pdf and /dev/null differ
diff --git a/figs/id31_effect_mass_frf_ol_int.png b/figs/id31_effect_mass_frf_ol_int.png
deleted file mode 100644
index 00832c1..0000000
Binary files a/figs/id31_effect_mass_frf_ol_int.png and /dev/null differ
diff --git a/figs/id31_hac_damped_plant_estimated_simscape.pdf b/figs/id31_hac_damped_plant_estimated_simscape.pdf
deleted file mode 100644
index e4cc430..0000000
Binary files a/figs/id31_hac_damped_plant_estimated_simscape.pdf and /dev/null differ
diff --git a/figs/id31_hac_damped_plant_estimated_simscape.png b/figs/id31_hac_damped_plant_estimated_simscape.png
deleted file mode 100644
index da57566..0000000
Binary files a/figs/id31_hac_damped_plant_estimated_simscape.png and /dev/null differ
diff --git a/figs/id31_hac_robust_loop_gain.pdf b/figs/id31_hac_robust_loop_gain.pdf
deleted file mode 100644
index 845e171..0000000
Binary files a/figs/id31_hac_robust_loop_gain.pdf and /dev/null differ
diff --git a/figs/id31_hac_robust_loop_gain.png b/figs/id31_hac_robust_loop_gain.png
deleted file mode 100644
index 42629e0..0000000
Binary files a/figs/id31_hac_robust_loop_gain.png and /dev/null differ
diff --git a/figs/id31_hac_robust_nyquist.pdf b/figs/id31_hac_robust_nyquist.pdf
deleted file mode 100644
index 5786797..0000000
Binary files a/figs/id31_hac_robust_nyquist.pdf and /dev/null differ
diff --git a/figs/id31_hac_robust_nyquist.png b/figs/id31_hac_robust_nyquist.png
deleted file mode 100644
index 013476b..0000000
Binary files a/figs/id31_hac_robust_nyquist.png and /dev/null differ
diff --git a/figs/id31_high_perf_hac_m0_loop_gain.pdf b/figs/id31_high_perf_hac_m0_loop_gain.pdf
deleted file mode 100644
index 4da774b..0000000
Binary files a/figs/id31_high_perf_hac_m0_loop_gain.pdf and /dev/null differ
diff --git a/figs/id31_high_perf_hac_m0_loop_gain.png b/figs/id31_high_perf_hac_m0_loop_gain.png
deleted file mode 100644
index a6186e8..0000000
Binary files a/figs/id31_high_perf_hac_m0_loop_gain.png and /dev/null differ
diff --git a/figs/id31_high_perf_hac_m0_nyquist.pdf b/figs/id31_high_perf_hac_m0_nyquist.pdf
deleted file mode 100644
index 0115979..0000000
Binary files a/figs/id31_high_perf_hac_m0_nyquist.pdf and /dev/null differ
diff --git a/figs/id31_high_perf_hac_m0_nyquist.png b/figs/id31_high_perf_hac_m0_nyquist.png
deleted file mode 100644
index 016c1c5..0000000
Binary files a/figs/id31_high_perf_hac_m0_nyquist.png and /dev/null differ
diff --git a/figs/id31_iff_loop_gain_diagonal_terms.pdf b/figs/id31_iff_loop_gain_diagonal_terms.pdf
deleted file mode 100644
index 02d9b59..0000000
Binary files a/figs/id31_iff_loop_gain_diagonal_terms.pdf and /dev/null differ
diff --git a/figs/id31_iff_loop_gain_diagonal_terms.png b/figs/id31_iff_loop_gain_diagonal_terms.png
deleted file mode 100644
index ba3330d..0000000
Binary files a/figs/id31_iff_loop_gain_diagonal_terms.png and /dev/null differ
diff --git a/figs/id31_iff_ol_plant_m0.pdf b/figs/id31_iff_ol_plant_m0.pdf
deleted file mode 100644
index 5be69c4..0000000
Binary files a/figs/id31_iff_ol_plant_m0.pdf and /dev/null differ
diff --git a/figs/id31_iff_ol_plant_m0.png b/figs/id31_iff_ol_plant_m0.png
deleted file mode 100644
index 890c94a..0000000
Binary files a/figs/id31_iff_ol_plant_m0.png and /dev/null differ
diff --git a/figs/id31_iff_root_locus.pdf b/figs/id31_iff_root_locus.pdf
deleted file mode 100644
index 9da6f05..0000000
Binary files a/figs/id31_iff_root_locus.pdf and /dev/null differ
diff --git a/figs/id31_iff_root_locus.png b/figs/id31_iff_root_locus.png
deleted file mode 100644
index b7a45a1..0000000
Binary files a/figs/id31_iff_root_locus.png and /dev/null differ
diff --git a/figs/id31_int_ol_plant_m0.pdf b/figs/id31_int_ol_plant_m0.pdf
deleted file mode 100644
index bef88e7..0000000
Binary files a/figs/id31_int_ol_plant_m0.pdf and /dev/null differ
diff --git a/figs/id31_int_ol_plant_m0.png b/figs/id31_int_ol_plant_m0.png
deleted file mode 100644
index b413010..0000000
Binary files a/figs/id31_int_ol_plant_m0.png and /dev/null differ
diff --git a/figs/id31_metrology_align_dx_dy.pdf b/figs/id31_metrology_align_dx_dy.pdf
deleted file mode 100644
index 3f05f02..0000000
Binary files a/figs/id31_metrology_align_dx_dy.pdf and /dev/null differ
diff --git a/figs/id31_metrology_align_dx_dy.png b/figs/id31_metrology_align_dx_dy.png
deleted file mode 100644
index 471a06f..0000000
Binary files a/figs/id31_metrology_align_dx_dy.png and /dev/null differ
diff --git a/figs/id31_metrology_align_rx_ry.pdf b/figs/id31_metrology_align_rx_ry.pdf
deleted file mode 100644
index d985b3b..0000000
Binary files a/figs/id31_metrology_align_rx_ry.pdf and /dev/null differ
diff --git a/figs/id31_metrology_align_rx_ry.png b/figs/id31_metrology_align_rx_ry.png
deleted file mode 100644
index 9c2e045..0000000
Binary files a/figs/id31_metrology_align_rx_ry.png and /dev/null differ
diff --git a/figs/id31_noise_budget_effect_hac_m0.pdf b/figs/id31_noise_budget_effect_hac_m0.pdf
deleted file mode 100644
index 068a9b4..0000000
Binary files a/figs/id31_noise_budget_effect_hac_m0.pdf and /dev/null differ
diff --git a/figs/id31_noise_budget_effect_hac_m0.png b/figs/id31_noise_budget_effect_hac_m0.png
deleted file mode 100644
index 1246fd3..0000000
Binary files a/figs/id31_noise_budget_effect_hac_m0.png and /dev/null differ
diff --git a/figs/id31_noise_budget_effect_lac_m0.pdf b/figs/id31_noise_budget_effect_lac_m0.pdf
deleted file mode 100644
index 564a314..0000000
Binary files a/figs/id31_noise_budget_effect_lac_m0.pdf and /dev/null differ
diff --git a/figs/id31_noise_budget_effect_lac_m0.png b/figs/id31_noise_budget_effect_lac_m0.png
deleted file mode 100644
index 1c4fdaa..0000000
Binary files a/figs/id31_noise_budget_effect_lac_m0.png and /dev/null differ
diff --git a/figs/id31_noise_budget_effect_rotation.pdf b/figs/id31_noise_budget_effect_rotation.pdf
deleted file mode 100644
index fd04abc..0000000
Binary files a/figs/id31_noise_budget_effect_rotation.pdf and /dev/null differ
diff --git a/figs/id31_noise_budget_effect_rotation.png b/figs/id31_noise_budget_effect_rotation.png
deleted file mode 100644
index b82b1f1..0000000
Binary files a/figs/id31_noise_budget_effect_rotation.png and /dev/null differ
diff --git a/figs/id31_noise_budget_interf_freq_domain_m0.pdf b/figs/id31_noise_budget_interf_freq_domain_m0.pdf
deleted file mode 100644
index a207f70..0000000
Binary files a/figs/id31_noise_budget_interf_freq_domain_m0.pdf and /dev/null differ
diff --git a/figs/id31_noise_budget_interf_freq_domain_m0.png b/figs/id31_noise_budget_interf_freq_domain_m0.png
deleted file mode 100644
index 63cb08d..0000000
Binary files a/figs/id31_noise_budget_interf_freq_domain_m0.png and /dev/null differ
diff --git a/figs/id31_noise_budget_interf_time_domain_m0.pdf b/figs/id31_noise_budget_interf_time_domain_m0.pdf
deleted file mode 100644
index 9fceaf9..0000000
Binary files a/figs/id31_noise_budget_interf_time_domain_m0.pdf and /dev/null differ
diff --git a/figs/id31_noise_budget_interf_time_domain_m0.png b/figs/id31_noise_budget_interf_time_domain_m0.png
deleted file mode 100644
index b01e3fc..0000000
Binary files a/figs/id31_noise_budget_interf_time_domain_m0.png and /dev/null differ
diff --git a/figs/id31_noise_budget_open_loop_cas_m0.pdf b/figs/id31_noise_budget_open_loop_cas_m0.pdf
deleted file mode 100644
index f45495b..0000000
Binary files a/figs/id31_noise_budget_open_loop_cas_m0.pdf and /dev/null differ
diff --git a/figs/id31_noise_budget_open_loop_cas_m0.png b/figs/id31_noise_budget_open_loop_cas_m0.png
deleted file mode 100644
index c1d3685..0000000
Binary files a/figs/id31_noise_budget_open_loop_cas_m0.png and /dev/null differ
diff --git a/figs/id31_reflectivity_scan_Ry_100urads.pdf b/figs/id31_reflectivity_scan_Ry_100urads.pdf
deleted file mode 100644
index d6fd238..0000000
Binary files a/figs/id31_reflectivity_scan_Ry_100urads.pdf and /dev/null differ
diff --git a/figs/id31_reflectivity_scan_Ry_100urads.png b/figs/id31_reflectivity_scan_Ry_100urads.png
deleted file mode 100644
index 08765b6..0000000
Binary files a/figs/id31_reflectivity_scan_Ry_100urads.png and /dev/null differ
diff --git a/figs/id31_simscape_enc_ol_plant.pdf b/figs/id31_simscape_enc_ol_plant.pdf
deleted file mode 100644
index 925e1b0..0000000
Binary files a/figs/id31_simscape_enc_ol_plant.pdf and /dev/null differ
diff --git a/figs/id31_simscape_enc_ol_plant.png b/figs/id31_simscape_enc_ol_plant.png
deleted file mode 100644
index 7e64345..0000000
Binary files a/figs/id31_simscape_enc_ol_plant.png and /dev/null differ
diff --git a/figs/id31_simscape_iff_ol_plant.pdf b/figs/id31_simscape_iff_ol_plant.pdf
deleted file mode 100644
index 8a711ba..0000000
Binary files a/figs/id31_simscape_iff_ol_plant.pdf and /dev/null differ
diff --git a/figs/id31_simscape_iff_ol_plant.png b/figs/id31_simscape_iff_ol_plant.png
deleted file mode 100644
index 78adc4e..0000000
Binary files a/figs/id31_simscape_iff_ol_plant.png and /dev/null differ
diff --git a/figs/id31_simscape_int_ol_plant.pdf b/figs/id31_simscape_int_ol_plant.pdf
deleted file mode 100644
index 4c3c040..0000000
Binary files a/figs/id31_simscape_int_ol_plant.pdf and /dev/null differ
diff --git a/figs/id31_simscape_int_ol_plant.png b/figs/id31_simscape_int_ol_plant.png
deleted file mode 100644
index 5a94460..0000000
Binary files a/figs/id31_simscape_int_ol_plant.png and /dev/null differ
diff --git a/figs/id31_tomo_1rpm_robust_m0.pdf b/figs/id31_tomo_1rpm_robust_m0.pdf
deleted file mode 100644
index 395a3fc..0000000
Binary files a/figs/id31_tomo_1rpm_robust_m0.pdf and /dev/null differ
diff --git a/figs/id31_tomo_1rpm_robust_m0.png b/figs/id31_tomo_1rpm_robust_m0.png
deleted file mode 100644
index cd92eb4..0000000
Binary files a/figs/id31_tomo_1rpm_robust_m0.png and /dev/null differ
diff --git a/figs/id31_tomo_robust_slow_m0.pdf b/figs/id31_tomo_robust_slow_m0.pdf
deleted file mode 100644
index 5f7642f..0000000
Binary files a/figs/id31_tomo_robust_slow_m0.pdf and /dev/null differ
diff --git a/figs/id31_tomo_robust_slow_m0.png b/figs/id31_tomo_robust_slow_m0.png
deleted file mode 100644
index cd92eb4..0000000
Binary files a/figs/id31_tomo_robust_slow_m0.png and /dev/null differ
diff --git a/figs/id31_tomography_m0_30rpm.pdf b/figs/id31_tomography_m0_30rpm.pdf
deleted file mode 100644
index a49629a..0000000
Binary files a/figs/id31_tomography_m0_30rpm.pdf and /dev/null differ
diff --git a/figs/id31_tomography_m0_30rpm.png b/figs/id31_tomography_m0_30rpm.png
deleted file mode 100644
index f094c20..0000000
Binary files a/figs/id31_tomography_m0_30rpm.png and /dev/null differ
diff --git a/figs/id31_tomography_m0_30rpm_robust_xyz.pdf b/figs/id31_tomography_m0_30rpm_robust_xyz.pdf
deleted file mode 100644
index a8ea1d5..0000000
Binary files a/figs/id31_tomography_m0_30rpm_robust_xyz.pdf and /dev/null differ
diff --git a/figs/id31_tomography_m0_30rpm_robust_xyz.png b/figs/id31_tomography_m0_30rpm_robust_xyz.png
deleted file mode 100644
index a1a2f05..0000000
Binary files a/figs/id31_tomography_m0_30rpm_robust_xyz.png and /dev/null differ
diff --git a/figs/id31_tomography_m0_30rpm_yz_errors.pdf b/figs/id31_tomography_m0_30rpm_yz_errors.pdf
deleted file mode 100644
index a819af9..0000000
Binary files a/figs/id31_tomography_m0_30rpm_yz_errors.pdf and /dev/null differ
diff --git a/figs/id31_tomography_m0_30rpm_yz_errors.png b/figs/id31_tomography_m0_30rpm_yz_errors.png
deleted file mode 100644
index 0b591cc..0000000
Binary files a/figs/id31_tomography_m0_30rpm_yz_errors.png and /dev/null differ
diff --git a/figs/id31_tomography_m3_1rpm.pdf b/figs/id31_tomography_m3_1rpm.pdf
deleted file mode 100644
index 2d5f6ef..0000000
Binary files a/figs/id31_tomography_m3_1rpm.pdf and /dev/null differ
diff --git a/figs/id31_tomography_m3_1rpm.png b/figs/id31_tomography_m3_1rpm.png
deleted file mode 100644
index 0f8ccdf..0000000
Binary files a/figs/id31_tomography_m3_1rpm.png and /dev/null differ
diff --git a/figs/id31_tomography_ol_cl_robust_hac_m0.pdf b/figs/id31_tomography_ol_cl_robust_hac_m0.pdf
deleted file mode 100644
index a14e183..0000000
Binary files a/figs/id31_tomography_ol_cl_robust_hac_m0.pdf and /dev/null differ
diff --git a/figs/id31_tomography_ol_cl_robust_hac_m0.png b/figs/id31_tomography_ol_cl_robust_hac_m0.png
deleted file mode 100644
index ab77f1e..0000000
Binary files a/figs/id31_tomography_ol_cl_robust_hac_m0.png and /dev/null differ
diff --git a/figs/id31_ty_scan_100ums_cl_dy_errors.pdf b/figs/id31_ty_scan_100ums_cl_dy_errors.pdf
deleted file mode 100644
index 2e827ed..0000000
Binary files a/figs/id31_ty_scan_100ums_cl_dy_errors.pdf and /dev/null differ
diff --git a/figs/id31_ty_scan_100ums_cl_dy_errors.png b/figs/id31_ty_scan_100ums_cl_dy_errors.png
deleted file mode 100644
index 9e064cf..0000000
Binary files a/figs/id31_ty_scan_100ums_cl_dy_errors.png and /dev/null differ
diff --git a/figs/id31_ty_scan_100ums_cl_dz_ry_errors.pdf b/figs/id31_ty_scan_100ums_cl_dz_ry_errors.pdf
deleted file mode 100644
index 8baae2b..0000000
Binary files a/figs/id31_ty_scan_100ums_cl_dz_ry_errors.pdf and /dev/null differ
diff --git a/figs/id31_ty_scan_100ums_cl_dz_ry_errors.png b/figs/id31_ty_scan_100ums_cl_dz_ry_errors.png
deleted file mode 100644
index 6d6ec2d..0000000
Binary files a/figs/id31_ty_scan_100ums_cl_dz_ry_errors.png and /dev/null differ
diff --git a/figs/id31_ty_scan_10ums_cl_dy_errors.pdf b/figs/id31_ty_scan_10ums_cl_dy_errors.pdf
deleted file mode 100644
index 49b069c..0000000
Binary files a/figs/id31_ty_scan_10ums_cl_dy_errors.pdf and /dev/null differ
diff --git a/figs/id31_ty_scan_10ums_cl_dy_errors.png b/figs/id31_ty_scan_10ums_cl_dy_errors.png
deleted file mode 100644
index f981d54..0000000
Binary files a/figs/id31_ty_scan_10ums_cl_dy_errors.png and /dev/null differ
diff --git a/figs/id31_ty_scan_10ums_cl_dz_ry_errors.pdf b/figs/id31_ty_scan_10ums_cl_dz_ry_errors.pdf
deleted file mode 100644
index 182af9e..0000000
Binary files a/figs/id31_ty_scan_10ums_cl_dz_ry_errors.pdf and /dev/null differ
diff --git a/figs/id31_ty_scan_10ums_cl_dz_ry_errors.png b/figs/id31_ty_scan_10ums_cl_dz_ry_errors.png
deleted file mode 100644
index 119b024..0000000
Binary files a/figs/id31_ty_scan_10ums_cl_dz_ry_errors.png and /dev/null differ
diff --git a/figs/id31_ty_scan_10ums_ol_dy_errors.pdf b/figs/id31_ty_scan_10ums_ol_dy_errors.pdf
deleted file mode 100644
index 1d7b8c5..0000000
Binary files a/figs/id31_ty_scan_10ums_ol_dy_errors.pdf and /dev/null differ
diff --git a/figs/id31_ty_scan_10ums_ol_dy_errors.png b/figs/id31_ty_scan_10ums_ol_dy_errors.png
deleted file mode 100644
index 1fc61ef..0000000
Binary files a/figs/id31_ty_scan_10ums_ol_dy_errors.png and /dev/null differ
diff --git a/figs/id31_ty_scan_10ums_ol_dz_ry_errors.pdf b/figs/id31_ty_scan_10ums_ol_dz_ry_errors.pdf
deleted file mode 100644
index 04f072a..0000000
Binary files a/figs/id31_ty_scan_10ums_ol_dz_ry_errors.pdf and /dev/null differ
diff --git a/figs/id31_ty_scan_10ums_ol_dz_ry_errors.png b/figs/id31_ty_scan_10ums_ol_dz_ry_errors.png
deleted file mode 100644
index 918e495..0000000
Binary files a/figs/id31_ty_scan_10ums_ol_dz_ry_errors.png and /dev/null differ
diff --git a/figs/metrology_alignment_xy_map.pdf b/figs/metrology_alignment_xy_map.pdf
deleted file mode 100644
index 8436293..0000000
Binary files a/figs/metrology_alignment_xy_map.pdf and /dev/null differ
diff --git a/figs/metrology_alignment_xy_map.png b/figs/metrology_alignment_xy_map.png
deleted file mode 100644
index 794b414..0000000
Binary files a/figs/metrology_alignment_xy_map.png and /dev/null differ
diff --git a/figs/metrology_alignment_xz_map.pdf b/figs/metrology_alignment_xz_map.pdf
deleted file mode 100644
index f89b255..0000000
Binary files a/figs/metrology_alignment_xz_map.pdf and /dev/null differ
diff --git a/figs/metrology_alignment_xz_map.png b/figs/metrology_alignment_xz_map.png
deleted file mode 100644
index eba5afd..0000000
Binary files a/figs/metrology_alignment_xz_map.png and /dev/null differ
diff --git a/figs/metrology_alignment_yz_map.pdf b/figs/metrology_alignment_yz_map.pdf
deleted file mode 100644
index 58d1670..0000000
Binary files a/figs/metrology_alignment_yz_map.pdf and /dev/null differ
diff --git a/figs/metrology_alignment_yz_map.png b/figs/metrology_alignment_yz_map.png
deleted file mode 100644
index ccc7363..0000000
Binary files a/figs/metrology_alignment_yz_map.png and /dev/null differ
diff --git a/figs/short_stroke_metrology_overview.jpg b/figs/short_stroke_metrology_overview.jpg
deleted file mode 100644
index eae7784..0000000
Binary files a/figs/short_stroke_metrology_overview.jpg and /dev/null differ
diff --git a/figs/test_id31_align_top_sphere_comparators.jpg b/figs/test_id31_align_top_sphere_comparators.jpg
new file mode 100644
index 0000000..82f4f71
Binary files /dev/null and b/figs/test_id31_align_top_sphere_comparators.jpg differ
diff --git a/figs/test_id31_fixed_nano_hexapod.jpg b/figs/test_id31_fixed_nano_hexapod.jpg
new file mode 100644
index 0000000..147f8a6
Binary files /dev/null and b/figs/test_id31_fixed_nano_hexapod.jpg differ
diff --git a/figs/test_id31_interf.jpg b/figs/test_id31_interf.jpg
new file mode 100644
index 0000000..90d181f
Binary files /dev/null and b/figs/test_id31_interf.jpg differ
diff --git a/figs/test_id31_interf_head.jpg b/figs/test_id31_interf_head.jpg
new file mode 100644
index 0000000..a5807ee
Binary files /dev/null and b/figs/test_id31_interf_head.jpg differ
diff --git a/figs/test_id31_interf_noise.pdf b/figs/test_id31_interf_noise.pdf
new file mode 100644
index 0000000..7c4b2fc
Binary files /dev/null and b/figs/test_id31_interf_noise.pdf differ
diff --git a/figs/test_id31_interf_noise.png b/figs/test_id31_interf_noise.png
new file mode 100644
index 0000000..c5eb314
Binary files /dev/null and b/figs/test_id31_interf_noise.png differ
diff --git a/figs/test_id31_lion.jpg b/figs/test_id31_lion.jpg
new file mode 100644
index 0000000..6a39e89
Binary files /dev/null and b/figs/test_id31_lion.jpg differ
diff --git a/figs/test_id31_metrology_align_dx_dy.pdf b/figs/test_id31_metrology_align_dx_dy.pdf
new file mode 100644
index 0000000..7b39e30
Binary files /dev/null and b/figs/test_id31_metrology_align_dx_dy.pdf differ
diff --git a/figs/test_id31_metrology_align_dx_dy.png b/figs/test_id31_metrology_align_dx_dy.png
new file mode 100644
index 0000000..faf1132
Binary files /dev/null and b/figs/test_id31_metrology_align_dx_dy.png differ
diff --git a/figs/test_id31_metrology_align_rx_ry.pdf b/figs/test_id31_metrology_align_rx_ry.pdf
new file mode 100644
index 0000000..3508627
Binary files /dev/null and b/figs/test_id31_metrology_align_rx_ry.pdf differ
diff --git a/figs/test_id31_metrology_align_rx_ry.png b/figs/test_id31_metrology_align_rx_ry.png
new file mode 100644
index 0000000..7cd92ef
Binary files /dev/null and b/figs/test_id31_metrology_align_rx_ry.png differ
diff --git a/figs/test_id31_metrology_kinematics.pdf b/figs/test_id31_metrology_kinematics.pdf
new file mode 100644
index 0000000..a369e24
Binary files /dev/null and b/figs/test_id31_metrology_kinematics.pdf differ
diff --git a/figs/test_id31_metrology_kinematics.png b/figs/test_id31_metrology_kinematics.png
new file mode 100644
index 0000000..6ca8bbb
Binary files /dev/null and b/figs/test_id31_metrology_kinematics.png differ
diff --git a/figs/LION_metrology_interferometers.svg b/figs/test_id31_metrology_kinematics.svg
similarity index 95%
rename from figs/LION_metrology_interferometers.svg
rename to figs/test_id31_metrology_kinematics.svg
index 8c047db..3f0281b 100644
Binary files a/figs/LION_metrology_interferometers.svg and b/figs/test_id31_metrology_kinematics.svg differ
diff --git a/figs/test_id31_micro_station_cables.jpg b/figs/test_id31_micro_station_cables.jpg
new file mode 100644
index 0000000..0f85c40
Binary files /dev/null and b/figs/test_id31_micro_station_cables.jpg differ
diff --git a/figs/test_id31_short_stroke_metrology_overview.jpg b/figs/test_id31_short_stroke_metrology_overview.jpg
new file mode 100644
index 0000000..01f5835
Binary files /dev/null and b/figs/test_id31_short_stroke_metrology_overview.jpg differ
diff --git a/figs/test_id31_xy_map_sphere.pdf b/figs/test_id31_xy_map_sphere.pdf
new file mode 100644
index 0000000..c12a32f
Binary files /dev/null and b/figs/test_id31_xy_map_sphere.pdf differ
diff --git a/figs/test_id31_xy_map_sphere.png b/figs/test_id31_xy_map_sphere.png
new file mode 100644
index 0000000..679f6ee
Binary files /dev/null and b/figs/test_id31_xy_map_sphere.png differ
diff --git a/preamble.tex b/preamble.tex
index d18dbd9..adafd1c 100644
--- a/preamble.tex
+++ b/preamble.tex
@@ -1,137 +1,16 @@
-\usepackage{float}
+\usepackage[      %
+    acronym,      % Separate acronyms and glossary
+    toc,          % appear in ToC
+    automake,     % auto-use the makeglossaries command (requires shell-escape)
+    nonumberlist, % don't back reference pages
+    nogroupskip,  % don't group by letter
+    nopostdot     % don't add a dot at the end of each element
+]{glossaries}
 
-\usepackage{caption,tabularx,booktabs}
-\usepackage{bm}
+\usepackage[stylemods=longextra]{glossaries-extra}
 
-\usepackage{xpatch} % Recommanded for biblatex
-\usepackage[        % use biblatex for bibliography
-    backend=biber,  % use biber backend (bibtex replacement) or bibtex
-    style=ieee,     % bib style
-    hyperref=true,  % activate hyperref support
-    backref=true,   % activate backrefs
-    isbn=false,     % don't show isbn tags
-    url=false,      % don't show url tags
-    doi=false,      % don't show doi tags
-    urldate=long,   % display type for dates
-    maxnames=3,     %
-    minnames=1,     %
-    maxbibnames=5,  %
-    minbibnames=3,  %
-    maxcitenames=2, %
-    mincitenames=1  %
-    ]{biblatex}
+\setabbreviationstyle[acronym]{long-short}
+\setglossarystyle{long-name-desc}
 
-\setlength\bibitemsep{1.1\itemsep}
-
-% \renewcommand*{\bibfont}{\footnotesize}
-
-\usepackage{fontawesome}
-
-\usepackage{caption}
-\usepackage{subcaption}
-
-\captionsetup[figure]{labelfont=bf}
-\captionsetup[subfigure]{labelfont=bf}
-\captionsetup[listing]{labelfont=bf}
-\captionsetup[table]{labelfont=bf}
-
-\usepackage{xcolor}
-
-\definecolor{my-blue}{HTML}{6b7adb}
-\definecolor{my-pale-blue}{HTML}{e6e9f9}
-\definecolor{my-red}{HTML}{db6b6b}
-\definecolor{my-pale-red}{HTML}{f9e6e6}
-\definecolor{my-green}{HTML}{6bdbb6}
-\definecolor{my-pale-green}{HTML}{e6f9f3}
-\definecolor{my-yellow}{HTML}{dbd26b}
-\definecolor{my-pale-yellow}{HTML}{f9f7e6}
-\definecolor{my-orange}{HTML}{dba76b}
-\definecolor{my-pale-orange}{HTML}{f9f0e6}
-\definecolor{my-grey}{HTML}{a3a3a3}
-\definecolor{my-pale-grey}{HTML}{f0f0f0}
-\definecolor{my-turq}{HTML}{6bc7db}
-\definecolor{my-pale-turq}{HTML}{e6f6f9}
-
-\usepackage{inconsolata}
-
-\usepackage[newfloat=true, chapter]{minted}
-\usemintedstyle{autumn}
-
-\setminted{frame=lines,breaklines=true,tabsize=4,fontsize=\scriptsize,autogobble=true,labelposition=topline,bgcolor=my-pale-grey}
-\setminted[matlab]{label=Matlab}
-\setminted[latex]{label=LaTeX}
-\setminted[bash]{label=Bash}
-\setminted[python]{label=Python}
-\setminted[text]{label=Results}
-\setminted[md]{label=Org Mode}
-
-\setmintedinline{fontsize=\normalsize,bgcolor=my-pale-grey}
-
-\usepackage[most]{tcolorbox}
-
-\tcbuselibrary{minted}
-
-\newtcolorbox{seealso}{   enhanced,breakable,colback=my-pale-grey,colframe=my-grey,fonttitle=\bfseries,title=See Also}
-\newtcolorbox{hint}{      enhanced,breakable,colback=my-pale-grey,colframe=my-grey,fonttitle=\bfseries,title=Hint}
-\newtcolorbox{definition}{enhanced,breakable,colback=my-pale-red, colframe=my-red, fonttitle=\bfseries,title=Definition}
-\newtcolorbox{important}{ enhanced,breakable,colback=my-pale-red, colframe=my-red, fonttitle=\bfseries,title=Important}
-\newtcolorbox{exampl}[1][]{    enhanced,breakable,colback=my-pale-green,colframe=my-green,fonttitle=\bfseries,title=Example,#1}
-\newtcolorbox{exercice}{  enhanced,breakable,colback=my-pale-yellow,colframe=my-yellow,fonttitle=\bfseries,title=Exercice}
-\newtcolorbox{question}{  enhanced,breakable,colback=my-pale-yellow,colframe=my-yellow,fonttitle=\bfseries,title=Question}
-\newtcolorbox{answer}{    enhanced,breakable,colback=my-pale-turq,colframe=my-turq,fonttitle=\bfseries,title=Answer}
-\newtcolorbox{summary}{   enhanced,breakable,colback=my-pale-blue,colframe=my-blue,fonttitle=\bfseries,title=Summary}
-\newtcolorbox{note}{      enhanced,breakable,colback=my-pale-blue,colframe=my-blue,fonttitle=\bfseries,title=Note}
-\newtcolorbox{caution}{   enhanced,breakable,colback=my-pale-orange,colframe=my-orange,fonttitle=\bfseries,title=Caution}
-\newtcolorbox{warning}{   enhanced,breakable,colback=my-pale-orange,colframe=my-orange,fonttitle=\bfseries,title=Warning}
-
-\newtcolorbox{my-quote}[1]{%
-  colback=my-pale-grey,
-  grow to right by=-10mm,
-  grow to left by=-10mm,
-  boxrule=0pt,
-  boxsep=0pt,
-  breakable,
-  enhanced jigsaw,
-  borderline west={4pt}{0pt}{my-grey}}
-
-\renewenvironment{quote}{\begin{my-quote}}{\end{my-quote}}
-
-\newtcolorbox{my-verse}[1]{%
-  colback=my-pale-grey,
-  grow to right by=-10mm,
-  grow to left by=-10mm,
-  boxrule=0pt,
-  boxsep=0pt,
-  breakable,
-  enhanced jigsaw,
-  borderline west={4pt}{0pt}{my-grey}}
-
-\renewenvironment{verse}{\begin{my-verse}}{\end{my-verse}}
-
-\usepackage{environ}% http://ctan.org/pkg/environ
-\NewEnviron{aside}{%
-  \marginpar{\BODY}
-}
-
-\renewenvironment{verbatim}{\VerbatimEnvironment\begin{minted}[]{text}}{\end{minted}}
-
-\usepackage{soul}
-\sethlcolor{my-pale-grey}
-
-\let\OldTexttt\texttt
-\renewcommand{\texttt}[1]{{\ttfamily\hl{\mbox{\,#1\,}}}}
-
-\makeatletter
-\preto\Gin@extensions{png,}
-\DeclareGraphicsRule{.png}{pdf}{.pdf}{\noexpand\Gin@base.pdf}
-\preto\Gin@extensions{gif,}
-\DeclareGraphicsRule{.gif}{png}{.png}{\noexpand\Gin@base.png}
-\makeatother
-
-\usepackage{hyperref}
-\hypersetup{
-  colorlinks = true,
-  allcolors = my-blue
-}
-
-\usepackage{hypcap}
+\makeindex
+\makeglossaries
diff --git a/test-bench-id31.bib b/test-bench-id31.bib
index e69de29..2f2c7ce 100644
--- a/test-bench-id31.bib
+++ b/test-bench-id31.bib
@@ -0,0 +1,11 @@
+@article{watchi18_review_compac_inter,
+  author          = {Watchi, Jennifer and Cooper, Sam and Ding, Binlei and
+                  Mow-Lowry, Conor M. and Collette, Christophe},
+  title           = {A Review of Compact Interferometers},
+  journal         = {CoRR},
+  year            = 2018,
+  url             = {http://arxiv.org/abs/1808.04175v1},
+  eprint          = {1808.04175},
+  keywords        = {metrology},
+}
+
diff --git a/test-bench-id31.org b/test-bench-id31.org
index 1fb0c64..9ba7273 100644
--- a/test-bench-id31.org
+++ b/test-bench-id31.org
@@ -15,7 +15,8 @@
 
 #+LaTeX_CLASS: scrreprt
 #+LaTeX_CLASS_OPTIONS: [a4paper, 10pt, DIV=12, parskip=full, bibliography=totoc]
-#+LaTeX_HEADER_EXTRA: \input{preamble.tex}
+#+LATEX_HEADER: \input{preamble.tex}
+#+LATEX_HEADER_EXTRA: \input{preamble_extra.tex}
 #+LATEX_HEADER_EXTRA: \bibliography{test-bench-id31.bib}
 
 #+BIND: org-latex-bib-compiler "biber"
@@ -64,6 +65,7 @@
                ("\\paragraph{%s}" . "\\paragraph*{%s}")
                ))
 
+
 ;; Remove automatic org heading labels
 (defun my-latex-filter-removeOrgAutoLabels (text backend info)
   "Org-mode automatically generates labels for headings despite explicit use of `#+LABEL`. This filter forcibly removes all automatically generated org-labels in headings."
@@ -95,37 +97,126 @@
 #+END_SRC
 
 * Notes                                                             :noexport:
+** Notes
+Prefix is =test_id31=
+
+*Goals*:
+- Short stroke metrology
+- Complete validation of the concept + nano-hexapod + instrumentation + control system
+- Experimental validation of complementary filter control
+
+*Outline*:
+- Short stroke metrology system
+  - Objective: 5DoF measurement while rotation
+  - Estimation of angular acceptance
+  - Do not care too much about accuracy here
+  - Mounting and alignment
+  - Jacobian etc...
+- Plant identification:
+  - Comparison with Simscape
+  - Better Rz alignment: use of the model
+  - Effect of payload mass
+- Robust IFF
+- High Authority Control
+  - Classical Loop Shaping:
+    - Control in the frame of the struts
+    - Control in the cartesian frame
+  - Complementary Filter Control
+  - S/T, noise budget
+- Scientific experiments
+  - Tomography
+  - Lateral Ty scans
+  - Dirty layer scans
+  - Reflectivity
+  - Diffraction tomography
+
+** CANC [#C] Find identification where Rz was not taken into account
+CLOSED: [2024-11-12 Tue 16:03]
+
+- State "CANC"       from "TODO"       [2024-11-12 Tue 16:03]
+- Much larger coupling than estimated from the model
+- In the model, suppose Rz = 0?
+- Then how to estimate Rz?
+  - From voltages
+  - From encoders
+  - Results and comparison with model
+
+*Maybe not talk about this here as it is not too interesting*
 
 * Introduction                                                        :ignore:
 
+Now that the nano-hexapod is mounted and that a good multi-body model of the nano-hexapod
+The system is validated on the ID31 beamline.
+
+At the beginning of the project, it was planned to develop a long stroke 5-DoF metrology system to measure the pose of the sample with respect to the granite.
+The development of such system was complex, and was not completed at the time of the experimental tests on ID31.
+To still validate the developed nano active platform and the associated instrumentation and control architecture, a 5-DoF short stroke metrology system was developed (Section ref:sec:test_id31_metrology).
+
+The identify dynamics of the nano-hexapod fixed on top of the micro-station was identified for different experimental conditions (payload masses, rotational velocities) and compared with the model (Section ref:sec:test_id31_open_loop_plant).
+
+Decentralized Integral Force Feedback is then applied to actively damp the plant in a robust way (Section ref:sec:test_id31_iff).
+
+High authority control is then applied
+
+#+name: fig:test_id31_micro_station_nano_hexapod
+#+caption: Picture of the micro-station without the nano-hexapod (\subref{fig:test_id31_micro_station_cables}) and with the nano-hexapod (\subref{fig:test_id31_fixed_nano_hexapod})
+#+attr_latex: :options [htbp]
+#+begin_figure
+#+attr_latex: :caption \subcaption{\label{fig:test_id31_micro_station_cables}Micro-station and nano-hexapod cables}
+#+attr_latex: :options {0.49\textwidth}
+#+begin_subfigure
+#+attr_latex: :width 0.95\linewidth
+[[file:figs/test_id31_micro_station_cables.jpg]]
+#+end_subfigure
+#+attr_latex: :caption \subcaption{\label{fig:test_id31_fixed_nano_hexapod}Nano-hexapod fixed on top of the micro-station}
+#+attr_latex: :options {0.49\textwidth}
+#+begin_subfigure
+#+attr_latex: :width 0.95\linewidth
+[[file:figs/test_id31_fixed_nano_hexapod.jpg]]
+#+end_subfigure
+#+end_figure
+
 * Short Stroke Metrology System
-<<sec:id31_short_stroke_metrology>>
+<<sec:test_id31_metrology>>
 ** Introduction                                                      :ignore:
 
 The control of the nano-hexapod requires an external metrology system measuring the relative position of the nano-hexapod top platform with respect to the granite.
-As the long-stroke ($\approx 1\,cm^3$) metrology system is not developed yet, a stroke stroke ($> 100\,\mu m^3$) can be used instead to validate the nano-hexapod control.
+As the long-stroke ($\approx 1 \,cm^3$) metrology system was not developed yet, a stroke stroke ($> 100\,\mu m^3$) was used instead to validate the nano-hexapod control.
 
-This short stroke metrology system consists of 5 interferometers pointing at 2 spheres fixed on top of the nano-hexapod (Figure ref:fig:LION_picture_close).
+A first considered option was to use the "Spindle error analyzer" shown in Figure ref:fig:test_id31_lion.
+This system comprises 5 capacitive sensors which are facing two reference spheres.
+As the gap between the capacitive sensors and the spheres is very small[fn:1], the risk of damaging the spheres and the capacitive sensors is high.
 
-#+name: fig:LION_picture_close
-#+caption: Metrology system with LION spheres (1 inch diameter) and 5 interferometers fixed to their individual tip-tilts
-#+attr_latex: :width 0.5\linewidth
-[[file:figs/LION_picture_close.jpg]]
+#+name: fig:test_id31_short_stroke_metrology
+#+caption: Short stroke metrology system used to measure the sample position with respect to the granite in 5DoF. The system is based on a "Spindle error analyzer" (\subref{fig:test_id31_lion}), but the capacitive sensors are replaced with fibered interferometers (\subref{fig:test_id31_interf}). Interferometer heads are shown in (\subref{fig:test_id31_interf_head})
+#+attr_latex: :options [htbp]
+#+begin_figure
+#+attr_latex: :caption \subcaption{\label{fig:test_id31_lion}Capacitive Sensors}
+#+attr_latex: :options {0.33\textwidth}
+#+begin_subfigure
+#+attr_latex: :width 0.9\linewidth
+[[file:figs/test_id31_lion.jpg]]
+#+end_subfigure
+#+attr_latex: :caption \subcaption{\label{fig:test_id31_interf}Short-Stroke metrology}
+#+attr_latex: :options {0.33\textwidth}
+#+begin_subfigure
+#+attr_latex: :width 0.9\linewidth
+[[file:figs/test_id31_interf.jpg]]
+#+end_subfigure
+#+attr_latex: :caption \subcaption{\label{fig:test_id31_interf_head}Interferometer head}
+#+attr_latex: :options {0.33\textwidth}
+#+begin_subfigure
+#+attr_latex: :width 0.9\linewidth
+[[file:figs/test_id31_interf_head.jpg]]
+#+end_subfigure
+#+end_figure
 
-This short stroke metrology system is fixed to the main granite using a gantry made of granite blocs to have good vibration and thermal stability (see Figure ref:fig:short_stroke_metrology_overview).
+Instead of using capacitive sensors, 5 fibered interferometers were used in a similar way (Figure ref:fig:test_id31_interf).
+At the end of each fiber, a sensor head[fn:2] (Figure ref:fig:test_id31_interf_head) is used, which consists of a lens precisely positioned with respect to the fiber's end.
+The lens is focusing the light on the surface of the sphere, such that it comes back to the fiber and produces an interference.
+This way, the gap between the sensor and the reference sphere is much larger (here around $40\,mm$), removing the risk of collision.
 
-#+name: fig:short_stroke_metrology_overview
-#+caption: Granite gantry used to fix the short-stroke metrology system
-#+attr_latex: :width \linewidth
-[[file:figs/short_stroke_metrology_overview.jpg]]
-
-As the metrology system as limited stroke (estimated to be in the order of hundreds of micro-meters in x-y-z), it has to be well aligned in the rest position.
-
-The alignment procedure is as follows:
-1. The granite is aligned to be perpendicular to gravity (using inclinometer and adjusting airlocks)
-2. The height of micro-hexapod is tuned to be able to position the short stroke metrology without additional shim
-3. It is verified that the spindle axis is well perpendicular to the granite using the laser tracker
-4. The micro hexapod is then used to align the two spheres with the spindle axis.
+Nevertheless, the metrology system still has limited measurement range, as when the spheres are moving perpendicularly to the beam axis, the reflected light does not coincide with the incident light, and for some perpendicular displacement, the interference is too small to be detected.
 
 ** Matlab Init                                              :noexport:ignore:
 #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
@@ -148,41 +239,39 @@ The alignment procedure is as follows:
 <<m-init-other>>
 #+end_src
 
-** Kinematics
+** Metrology Kinematics
+<<ssec:test_id31_metrology_kinematics>>
 
-#+name: fig:LION_metrology_interferometers
-#+caption: Schematic of the measurement system
-#+attr_latex: :width 0.5\linewidth
-[[file:figs/LION_metrology_interferometers.png]]
+The developed short-stroke metrology system is schematically shown in Figure ref:fig:test_id31_metrology_kinematics.
+The point of interest is indicated by the blue frame $\{B\}$, which is located $H = 150\,mm$ above the nano-hexapod's top platform.
+The spheres have a diameter $d = 25.4\,mm$, and indicated dimensions are $l_1 = 60\,mm$ and $l_2 = 16.2\,mm$.
+In order to compute the pose of the $\{B\}$ frame with respect to the granite (i.e. with respect to the fixed interferometer heads), the measured small displacements $[d_1,\ d_2,\ d_3,\ d_4,\ d_5]$ by the interferometers are first written as a function of the small linear and angular motion of the $\{B\}$ frame $[D_x,\ D_y,\ D_z,\ R_x,\ R_y]$ eqref:eq:test_id31_metrology_kinematics.
 
-We have the following set of equations:
-\begin{align}
-d_1 &= +D_y - l_2 R_x \\
-d_2 &= +D_y + l_1 R_x \\
-d_3 &= -D_x - l_2 R_y \\
-d_4 &= -D_x + l_1 R_y \\
-d_5 &= -D_z
-\end{align}
-
-That can be written as a linear transformation:
-\begin{equation}
-\begin{bmatrix}
-  d_1 \\ d_2 \\ d_3 \\ d_4 \\ d_5
-\end{bmatrix} = \begin{bmatrix}
-   0 &  1 &  0 & -l_2 &  0   \\
-   0 &  1 &  0 &  l_1 &  0   \\
-  -1 &  0 &  0 &  0   & -l_2 \\
-  -1 &  0 &  0 &  0   &  l_1 \\
-   0 &  0 & -1 &  0   &  0
-\end{bmatrix} \cdot \begin{bmatrix}
-  D_x \\ D_y \\ D_z \\ R_x \\ R_y
-\end{bmatrix}
+\begin{equation}\label{eq:test_id31_metrology_kinematics}
+d_1 = D_y - l_2 R_x, \quad d_2 = D_y + l_1 R_x, \quad d_3 = -D_x - l_2 R_y, \quad d_4 = -D_x + l_1 R_y, \quad d_5 = -D_z
 \end{equation}
 
-By inverting the matrix, we obtain the Jacobian relation:
-\begin{equation}
+#+attr_latex: :options [b]{0.48\linewidth}
+#+begin_minipage
+#+name: fig:test_id31_metrology_kinematics
+#+caption: Schematic of the measurement system. Measured distances are indicated by red arrows.
+#+attr_latex: :scale 1 :float nil
+[[file:figs/test_id31_metrology_kinematics.png]]
+#+end_minipage
+\hfill
+#+attr_latex: :options [b]{0.48\linewidth}
+#+begin_minipage
+#+name: fig:align_top_sphere_comparators
+#+attr_latex: :width \linewidth :float nil
+#+caption: The top sphere is aligned with the rotation axis of the spindle using two probes.
+[[file:figs/test_id31_align_top_sphere_comparators.jpg]]
+#+end_minipage
+
+The five equations eqref:eq:test_id31_metrology_kinematics can be written in a matrix form, and then inverted to have the pose of $\{B\}$ frame as a linear combination of the measured five distances by the interferometers eqref:eq:test_id31_metrology_kinematics_inverse.
+
+\begin{equation}\label{eq:test_id31_metrology_kinematics_inverse}
 \begin{bmatrix}
-  D_x \\ D_y \\ D_z \\ R_x \\ R_y
+   D_x \\ D_y \\ D_z \\ R_x \\ R_y
 \end{bmatrix} = \begin{bmatrix}
    0 &  1 &  0 & -l_2 &  0   \\
    0 &  1 &  0 &  l_1 &  0   \\
@@ -195,14 +284,12 @@ By inverting the matrix, we obtain the Jacobian relation:
 \end{equation}
 
 #+begin_src matlab
-%% Parameters
+%% Geometrical parameters of the metrology system
 H = 150e-3;
 l1 = (150-48-42)*1e-3;
 l2 = (76.2+48+42-150)*1e-3;
-#+end_src
 
-#+begin_src matlab
-%% Transformation matrix
+% Computation of the Transformation matrix
 Hm = [ 0  1  0 -l2  0;
        0  1  0  l1  0;
       -1  0  0  0  -l2;
@@ -210,55 +297,67 @@ Hm = [ 0  1  0 -l2  0;
        0  0 -1  0   0];
 #+end_src
 
-#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-data2orgtable(pinv(Hm), {'*Dx*', '*Dy*', '*Dz*', '*Rx*', '*Ry*'}, {'*d1*', '*d2*', '*d3*', '*d4*', '*d5*'}, ' %.2f ');
-#+end_src
+** Rough alignment of the reference spheres
+<<ssec:test_id31_metrology_sphere_rought_alignment>>
 
-#+name: tab:jacobian_metrology
-#+caption: Jacobian matrix for the metrology system
-#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
-#+attr_latex: :center t :booktabs t
-#+RESULTS:
-|      |   *d1* |  *d2* |   *d3* |  *d4* | *d5* |
-|------+--------+-------+--------+-------+------|
-| *Dx* |    0.0 |   0.0 |  -0.79 | -0.21 |  0.0 |
-| *Dy* |   0.79 |  0.21 |    0.0 |   0.0 |  0.0 |
-| *Dz* |    0.0 |   0.0 |    0.0 |   0.0 | -1.0 |
-| *Rx* | -13.12 | 13.12 |   -0.0 |   0.0 |  0.0 |
-| *Ry* |   -0.0 |  -0.0 | -13.12 | 13.12 |  0.0 |
+The two reference spheres are aligned with the rotation axis of the spindle.
+To do so, two measuring probes are used as shown in Figure ref:fig:align_top_sphere_comparators.
 
-** Rough alignment of spheres using comparators
-Bottom Sphere, then top sphere.
+To not damage the sensitive sphere surface, the probes are instead positioned on the cylinder on which the sphere is mounted.
+First, the probes are fixed to the bottom (fixed) cylinder to align its axis with the spindle axis.
+Then, the probes are fixed to the top (adjustable) cylinder, and the same alignment is performed.
 
-Alignment better than 10um.
-But the coaxiality between the cylinder and the sphere might not be good.
+With this setup, the precision of the alignment of both sphere better with the spindle axis is expected to limited to $\approx 10\,\mu m$.
+This is probably limited due to the poor coaxiality between the cylinders and the spheres.
+However, the alignment precision should be enough to stay in the acceptance of the interferometers.
 
-#+name: fig:align_top_sphere_comparators
-#+attr_latex: :width \linewidth
-#+caption: Two mechanical comparators used to align the top sphere with the rotation axis of the spindle
-[[file:figs/align_top_sphere_comparators.jpg]]
+** Tip-Tilt adjustment of the interferometers
+<<ssec:test_id31_metrology_alignment>>
+
+The short stroke metrology system is placed on top of the main granite using a gantry made of granite blocs to have good vibration and thermal stability (Figure ref:fig:short_stroke_metrology_overview).
+
+#+name: fig:short_stroke_metrology_overview
+#+caption: Granite gantry used to fix the short-stroke metrology system
+#+attr_latex: :width 0.8\linewidth
+[[file:figs/test_id31_short_stroke_metrology_overview.jpg]]
+
+The interferometers need to be aligned with respect to the two reference spheres to approach as much as possible the ideal case shown in Figure ref:fig:test_id31_metrology_kinematics.
+The vertical position of the spheres is adjusted using the micro-hexapod to match the height of the interferometers.
+Then, the horizontal position of the gantry is adjusted such that the coupling efficiency (i.e. the intensity of the light reflected back in the fiber) of the top interferometer is maximized.
+This is equivalent as to optimize the perpendicularity between the interferometer beam and the sphere surface (i.e. the concentricity between the beam and the sphere center).
+
+The lateral sensor heads (i.e. all except the top one), which are each fixed to a custom tip-tilt adjustment mechanism, are individually oriented such that the coupling efficient is maximized.
+
+** Fine Alignment of reference spheres using interferometers
+<<ssec:test_id31_metrology_sphere_fine_alignment>>
+
+Thanks to the good alignment of the two reference spheres with the spindle axis and to the fine adjustment of the interferometers orientations, the interferometer measurement is made possible during complete spindle rotation.
+This metrology and therefore be used to better align the axis defined by the two spheres' center with the spindle axis.
+
+The alignment process is made by few iterations.
+First, the spindle is scanned and the alignment errors are recorded.
+From the errors, the motion of the micro-hexapod to better align the spheres is determined and the micro-hexapod is moved.
+Then, the spindle is scanned again, and the new alignment errors are recorded.
+
+This iterative process is first perform for angular errors (Figure ref:fig:test_id31_metrology_align_rx_ry) and then for lateral errors (Figure ref:fig:test_id31_metrology_align_dx_dy).
+Remaining error after alignment is in the order of $\pm5\,\mu\text{rad}$ for angular errors, $\pm 1\,\mu m$ laterally and less than $0.1\,\mu m$ vertically.
 
-** Alignment of spheres using interferometers
-*** Angular alignment
 #+begin_src matlab
-%% Load Data
+%% Angular alignment
+% Load Data
 data_it0 = h5scan(data_dir, 'alignment', 'h1rx_h1ry', 1);
 data_it1 = h5scan(data_dir, 'alignment', 'h1rx_h1ry_0002', 3);
 data_it2 = h5scan(data_dir, 'alignment', 'h1rx_h1ry_0002', 5);
-#+end_src
 
-#+begin_src matlab
-%% Offset wrong points
+% Offset wrong points
 i_it0 = find(abs(data_it0.Rx_int_filtered(2:end)-data_it0.Rx_int_filtered(1:end-1))>1e-5);
 data_it0.Rx_int_filtered(i_it0+1:end) = data_it0.Rx_int_filtered(i_it0+1:end) + data_it0.Rx_int_filtered(i_it0) - data_it0.Rx_int_filtered(i_it0+1);
 i_it1 = find(abs(data_it1.Rx_int_filtered(2:end)-data_it1.Rx_int_filtered(1:end-1))>1e-5);
 data_it1.Rx_int_filtered(i_it1+1:end) = data_it1.Rx_int_filtered(i_it1+1:end) + data_it1.Rx_int_filtered(i_it1) - data_it1.Rx_int_filtered(i_it1+1);
 i_it2 = find(abs(data_it2.Rx_int_filtered(2:end)-data_it2.Rx_int_filtered(1:end-1))>1e-5);
 data_it2.Rx_int_filtered(i_it2+1:end) = data_it2.Rx_int_filtered(i_it2+1:end) + data_it2.Rx_int_filtered(i_it2) - data_it2.Rx_int_filtered(i_it2+1);
-#+end_src
 
-#+begin_src matlab
-%% Compute circle fit and get radius
+% Compute circle fit and get radius
 [~, ~, R_it0, ~] = circlefit(1e6*data_it0.Rx_int_filtered, 1e6*data_it0.Ry_int_filtered);
 [~, ~, R_it1, ~] = circlefit(1e6*data_it1.Rx_int_filtered, 1e6*data_it1.Ry_int_filtered);
 [~, ~, R_it2, ~] = circlefit(1e6*data_it2.Rx_int_filtered, 1e6*data_it2.Ry_int_filtered);
@@ -269,41 +368,34 @@ data_it2.Rx_int_filtered(i_it2+1:end) = data_it2.Rx_int_filtered(i_it2+1:end) +
 figure;
 hold on;
 plot(1e6*data_it0.Rx_int_filtered, 1e6*data_it0.Ry_int_filtered, '-', ...
-     'DisplayName', sprintf('It. 0: $R = %.0f \\mu$rad', R_it0))
+     'DisplayName', sprintf('$R_0 = %.0f \\mu$rad', R_it0))
 plot(1e6*data_it1.Rx_int_filtered, 1e6*data_it1.Ry_int_filtered, '-', ...
-     'DisplayName', sprintf('It. 1: $R = %.0f \\mu$rad', R_it1))
-plot(1e6*data_it2.Rx_int_filtered, 1e6*data_it2.Ry_int_filtered, '-', ...
-     'DisplayName', sprintf('It. 2: $R = %.0f \\mu$rad', R_it2))
+     'DisplayName', sprintf('$R_1 = %.0f \\mu$rad', R_it1))
+plot(1e6*data_it2.Rx_int_filtered, 1e6*data_it2.Ry_int_filtered, '-', 'color', colors(5,:), ...
+     'DisplayName', sprintf('$R_2 = %.0f \\mu$rad', R_it2))
 hold off;
 xlabel('$R_x$ [$\mu$rad]'); ylabel('$R_y$ [$\mu$rad]');
 axis equal
 legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+xlim([-600, 300]);
+ylim([-100, 800]);
 #+end_src
 
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_metrology_align_rx_ry.pdf', 'width', 'wide', 'height', 'normal');
+#+begin_src matlab :tangle no :exports results :results file none
+exportFig('figs/test_id31_metrology_align_rx_ry.pdf', 'width', 'half', 'height', 'normal');
 #+end_src
 
-#+name: fig:id31_metrology_align_rx_ry
-#+caption: Rx/Ry alignment of the spheres using the micro-station
-#+RESULTS:
-[[file:figs/id31_metrology_align_rx_ry.png]]
-
-*** Eccentricity alignment
 #+begin_src matlab
-%% Load Data
+%% Eccentricity alignment
+% Load Data
 data_it0 = h5scan(data_dir, 'alignment', 'h1rx_h1ry_0002', 5);
 data_it1 = h5scan(data_dir, 'alignment', 'h1dx_h1dy', 1);
-#+end_src
 
-#+begin_src matlab
-%% Offset wrong points
+% Offset wrong points
 i_it0 = find(abs(data_it0.Dy_int_filtered(2:end)-data_it0.Dy_int_filtered(1:end-1))>1e-5);
 data_it0.Dy_int_filtered(i_it0+1:end) = data_it0.Dy_int_filtered(i_it0+1:end) + data_it0.Dy_int_filtered(i_it0) - data_it0.Dy_int_filtered(i_it0+1);
-#+end_src
 
-#+begin_src matlab
-%% Compute circle fit and get radius
+% Compute circle fit and get radius
 [~, ~, R_it0, ~] = circlefit(1e6*data_it0.Dx_int_filtered, 1e6*data_it0.Dy_int_filtered);
 [~, ~, R_it1, ~] = circlefit(1e6*data_it1.Dx_int_filtered, 1e6*data_it1.Dy_int_filtered);
 #+end_src
@@ -313,63 +405,182 @@ data_it0.Dy_int_filtered(i_it0+1:end) = data_it0.Dy_int_filtered(i_it0+1:end) +
 figure;
 hold on;
 plot(1e6*data_it0.Dx_int_filtered, 1e6*data_it0.Dy_int_filtered, '-', ...
-     'DisplayName', sprintf('It. 0: $R = %.0f \\mu$m', R_it0))
+     'DisplayName', sprintf('$R_0 = %.0f \\mu$m', R_it0))
 plot(1e6*data_it1.Dx_int_filtered, 1e6*data_it1.Dy_int_filtered, '-', ...
-     'DisplayName', sprintf('It. 1: $R = %.0f \\mu$m', R_it1))
+     'DisplayName', sprintf('$R_1 = %.0f \\mu$m', R_it1))
 hold off;
 xlabel('$D_x$ [$\mu$m]'); ylabel('$D_y$ [$\mu$m]');
 axis equal
 legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+xlim([-1, 21]);
+ylim([-8, 14]);
 #+end_src
 
 #+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_metrology_align_dx_dy.pdf', 'width', 'wide', 'height', 'normal');
+exportFig('figs/test_id31_metrology_align_dx_dy.pdf', 'width', 'half', 'height', 'normal');
 #+end_src
 
-#+name: fig:id31_metrology_align_dx_dy
-#+caption: Dx/Dy alignment of the spheres using the micro-station
-#+RESULTS:
-[[file:figs/id31_metrology_align_dx_dy.png]]
+#+name: fig:test_id31_metrology_align
+#+caption: Measured angular (\subref{fig:test_id31_metrology_align_rx_ry}) and lateral (\subref{fig:test_id31_metrology_align_dx_dy}) errors during a full spindle rotation. Between two rotations, the micro-hexapod is adjusted to better align the two spheres with the rotation axis.
+#+attr_latex: :options [htbp]
+#+begin_figure
+#+attr_latex: :caption \subcaption{\label{fig:test_id31_metrology_align_rx_ry}Angular alignment}
+#+attr_latex: :options {0.49\textwidth}
+#+begin_subfigure
+#+attr_latex: :scale 1
+[[file:figs/test_id31_metrology_align_rx_ry.png]]
+#+end_subfigure
+#+attr_latex: :caption \subcaption{\label{fig:test_id31_metrology_align_dx_dy}Lateral alignment}
+#+attr_latex: :options {0.49\textwidth}
+#+begin_subfigure
+#+attr_latex: :scale 1
+[[file:figs/test_id31_metrology_align_dx_dy.png]]
+#+end_subfigure
+#+end_figure
+
+
+** Estimated measurement volume
+<<ssec:test_id31_metrology_acceptance>>
+
+Because the interferometers are pointing to spheres and not flat surfaces, the lateral acceptance is limited.
+In order to estimate the metrology acceptance, the micro-hexapod is used to perform three accurate scans of $\pm 1\,mm$, respectively along the the $x$, $y$ and $z$ axes.
+During these scans, the 5 interferometers are recorded, and the ranges in which each interferometer has enough coupling efficiency for measurement are estimated.
+Results are summarized in Table ref:tab:test_id31_metrology_acceptance.
+The obtained lateral acceptance for pure displacements in any direction is estimated to be around $+/-0.5\,mm$, which is enough for the current application as it is well above the micro-station errors to be actively corrected.
 
-** Residual error after alignment
 #+begin_src matlab
-%% Load Data
-data = h5scan(data_dir, 'alignment', 'h1dx_h1dy', 1);
+%% Estimated acceptance of the metrology
+% This is estimated by moving the spheres using the micro-hexapod
+
+% Dx
+data_dx = h5scan(data_dir, 'metrology_acceptance_new_align', 'dx', 1);
+
+dx_acceptance = zeros(5,1);
+
+for i = [1:size(dx_acceptance, 1)]
+    % Find range in which the interferometers are measuring displacement
+    dx_di = diff(data_dx.(sprintf('d%i', i))) == 0;
+    if sum(dx_di) > 0
+        dx_acceptance(i) = data_dx.h1tx(find(dx_di(501:end), 1) + 500) - ...
+                           data_dx.h1tx(find(flip(dx_di(1:500)), 1));
+    else
+        dx_acceptance(i) = data_dx.h1tx(end) - data_dx.h1tx(1);
+    end
+end
+
+% Dy
+data_dy = h5scan(data_dir, 'metrology_acceptance_new_align', 'dy', 1);
+
+dy_acceptance = zeros(5,1);
+
+for i = [1:size(dy_acceptance, 1)]
+    % Find range in which the interferometers are measuring displacement
+    dy_di = diff(data_dy.(sprintf('d%i', i))) == 0;
+    if sum(dy_di) > 0
+        dy_acceptance(i) = data_dy.h1ty(find(dy_di(501:end), 1) + 500) - ...
+                           data_dy.h1ty(find(flip(dy_di(1:500)), 1));
+    else
+        dy_acceptance(i) = data_dy.h1ty(end) - data_dy.h1ty(1);
+    end
+end
+
+% Dz
+data_dz = h5scan(data_dir, 'metrology_acceptance_new_align', 'dz', 1);
+
+dz_acceptance = zeros(5,1);
+
+for i = [1:size(dz_acceptance, 1)]
+    % Find range in which the interferometers are measuring displacement
+    dz_di = diff(data_dz.(sprintf('d%i', i))) == 0;
+    if sum(dz_di) > 0
+        dz_acceptance(i) = data_dz.h1tz(find(dz_di(501:end), 1) + 500) - ...
+                           data_dz.h1tz(find(flip(dz_di(1:500)), 1));
+    else
+        dz_acceptance(i) = data_dz.h1tz(end) - data_dz.h1tz(1);
+    end
+end
 #+end_src
 
-- Dx and Dy are less than 1um.
-- Dz less than 0.1um.
-- Rx and Ry less than 4urad.
+#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
+% data2orgtable([dx_acceptance, dy_acceptance, dz_acceptance], {'$d_1$ (y)', '$d_2$ (y)', '$d_3$ (x)', '$d_4$ (x)', '$d_5$ (z)'}, {'$D_x$', '$D_y$', '$D_z$'}, ' %.2f ');
+#+end_src
 
-#+begin_src matlab :exports none :results none
-%% Remaining errors after aligning the metrology using the interferometers
+#+name: tab:test_id31_metrology_acceptance
+#+caption: Estimated measurement range for each interferometer, and for three different directions.
+#+attr_latex: :environment tabularx :width 0.45\linewidth :align Xccc
+#+attr_latex: :center t :booktabs t
+#+RESULTS:
+|           | $D_x$       | $D_y$      | $D_z$ |
+|-----------+-------------+------------+-------|
+| $d_1$ (y) | $1.0\,mm$   | $>2\,mm$   |  $1.35\,mm$ |
+| $d_2$ (y) | $0.8\,mm$   | $>2\,mm$   |  $1.01\,mm$ |
+| $d_3$ (x) | $>2\,mm$    | $1.06\,mm$ |  $1.38\,mm$ |
+| $d_4$ (x) | $>2\,mm$    | $0.99\,mm$ |  $0.94\,mm$ |
+| $d_5$ (z) | $1.33\, mm$ | $1.06\,mm$ |  $>2\,mm$   |
+
+
+** Estimated measurement errors
+<<ssec:test_id31_metrology_errors>>
+
+When using the NASS, the accuracy of the sample's positioning is linked to the accuracy of the external metrology.
+However, to validate the nano-hexapod with the associated instrumentation and control architecture, the accuracy of the metrology is not an issue.
+Only the bandwidth and noise characteristics of the external metrology are important.
+Yet, some elements effecting the accuracy of the metrology are discussed here.
+
+First, the "metrology kinematics" (discussed in Section ref:ssec:test_id31_metrology_kinematics) is only approximate (i.e. valid for very small displacements).
+This can be seen when performing lateral $[D_x,\,D_y]$ scans using the micro-hexapod while recording the vertical interferometer (Figure ref:fig:test_id31_xy_map_sphere).
+As the interferometer is pointing to a sphere and not to a plane, lateral motion of the sphere is seen as a vertical motion by the top interferometer.
+
+Then, the reference spheres have some deviations with respect to an ideal sphere.
+They are meant to be used with capacitive sensors which are integrating the shape errors over large surfaces.
+When using interferometers, the size of the "light spot" on the sphere surface is a circle with a diameter $\approx 50\,\mu m$, therefore the system is more sensitive to shape errors with small features.
+
+As the interferometer light is travelling in air, the measured distance is sensitive to any variation in the refractive index of the air.
+Therefore, any variation of air temperature, pressure or humidity will induce measurement errors.
+For a measurement length of $40\,mm$, a temperature variation of $0.1\,{}^oC$ induces an errors in the distance measurement of $\approx 4\,nm$.
+
+Finally, even in vacuum and in the absence of target motion, the interferometers are affected by noise [[cite:&watchi18_review_compac_inter]].
+The effect of the noise on the translation and rotation measurements is estimated in Figure ref:fig:test_id31_interf_noise.
+
+#+begin_src matlab
+%% Interferometer noise estimation
+data = load("test_id31_interf_noise.mat");
+
+Ts = 1e-4;
+Nfft = floor(5/Ts);
+win = hanning(Nfft);
+Noverlap = floor(Nfft/2);
+
+[pxx_int, f] = pwelch(detrend(data.d, 0), win, Noverlap, Nfft, 1/Ts);
+
+% Uncorrelated noise: square root of the sum of the squares
+pxx_cart = pxx_int*sum(inv(Hm).^2, 2)';
+
+rms_dxy = sqrt(trapz(f(f>1), pxx_cart((f>1),1))); % < 0.3 nm RMS
+rms_dz  = sqrt(trapz(f(f>1), pxx_cart((f>1),3))); % < 0.3 nm RMS
+rms_rxy = sqrt(trapz(f(f>1), pxx_cart((f>1),4))); % 5 nrad RMS
+#+end_src
+
+#+begin_src matlab
 figure;
 hold on;
-plot(data.nth, 1e6*data.Dx_int_filtered, 'DisplayName', '$D_x$')
-plot(data.nth, 1e6*data.Dy_int_filtered, 'DisplayName', '$D_y$')
-plot(data.nth, 1e6*data.Dz_int_filtered, 'DisplayName', '$D_z$')
-plot(data.nth, 1e6*data.Rx_int_filtered, 'DisplayName', '$R_x$')
-plot(data.nth, 1e6*data.Ry_int_filtered, 'DisplayName', '$R_y$')
-hold off;
-xlabel('$R_z$ [deg]'); ylabel('Remaining Errors [$\mu$m, $\mu$rad]')
-xlim([0, 360]); xticks(0:90:360);
-ylim([-5, 5]); yticks(-5:1:5);
+plot(f, sqrt(pxx_cart(:,1)), 'DisplayName', sprintf('$D_{x,y}$, %.1f nmRMS', rms_dxy));
+plot(f, sqrt(pxx_cart(:,3)), 'DisplayName', sprintf('$D_{z}$, %.1f nmRMS', rms_dz));
+plot(f, sqrt(pxx_cart(:,4)), 'DisplayName', sprintf('$R_{x,y}$, %.1f nradRMS', rms_rxy));
+set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
+xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{nm,\ nrad}{\sqrt{Hz}}\right]$')
+xlim([1, 1e3]); ylim([1e-3, 1]);
+leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
+leg.ItemTokenSize(1) = 15;
 #+end_src
 
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_align_metorlogy_errors_after_align.pdf', 'width', 'wide', 'height', 'normal');
+#+begin_src matlab :tangle no :exports results :results file none
+exportFig('figs/test_id31_interf_noise.pdf', 'width', 'half', 'height', 'normal');
 #+end_src
 
-#+name: fig:id31_align_metorlogy_errors_after_align
-#+caption: Remaining errors after aligning the metrology using the interferometers
-#+RESULTS:
-[[file:figs/id31_align_metorlogy_errors_after_align.png]]
-
-
-** Metrology acceptance
-Because the interferometers are pointing to spheres and not flat surfaces, the lateral acceptance is limited.
 
 #+begin_src matlab
+%% X-Y scan with the micro-hexapod, and record of the vertical interferometer
 data = h5scan(data_dir, 'metrology_acceptance', 'after_int_align_meshXY', 1);
 
 x = 1e3*detrend(data.h1tx, 0); % [um]
@@ -379,6 +590,9 @@ z = 1e6*data.Dz_int_filtered - max(data.Dz_int_filtered); % [um]
 mdl = scatteredInterpolant(x, y, z);
 [xg, yg] = meshgrid(unique(x), unique(y));
 zg = mdl(xg, yg);
+
+% Fit a sphere to the data
+[sphere_center,sphere_radius] = sphereFit(1e-3*[x, y, z]);
 #+end_src
 
 #+begin_src matlab :exports none :results none
@@ -386,9950 +600,34 @@ zg = mdl(xg, yg);
 figure;
 [~,c] = contour3(xg,yg,zg,30);
 c.LineWidth = 3;
-xlabel('$\mu$-hexapod $D_x$ [$\mu$m]');
-ylabel('$\mu$-hexapod $D_y$ [$\mu$m]');
-zlabel('Interf $D_z$ [$\mu$m]');
+xlabel('$D_x$ [$\mu$m]');
+ylabel('$D_y$ [$\mu$m]');
+zlabel('$D_z$ [$\mu$m]');
 zlim([-1, 0]);
-xticks(-100:50:100);
-yticks(-100:50:100);
-zticks(-1:0.2:0);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/metrology_alignment_xy_map.pdf', 'width', 'wide', 'height', 'tall');
-#+end_src
-
-#+name: fig:metrology_alignment_xy_map
-#+caption: XY mapping of the Z measurement by the interferometer
-#+RESULTS:
-[[file:figs/metrology_alignment_xy_map.png]]
-
-
-* Simscape Model
-<<sec:id31_simscape_model>>
-** Introduction                                                      :ignore:
-
-** Matlab Init                                              :noexport:ignore:
-#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
-<<matlab-dir>>
-#+end_src
-
-#+begin_src matlab :exports none :results silent :noweb yes
-<<matlab-init>>
-#+end_src
-
-#+begin_src matlab :tangle no :noweb yes
-<<m-init-path>>
-#+end_src
-
-#+begin_src matlab :eval no :noweb yes
-<<m-init-path-tangle>>
-#+end_src
-
-#+begin_src matlab :noweb yes
-<<m-init-other>>
-#+end_src
-
-** Init model
-#+begin_src matlab
-%% Add directories to path for Simscape Model
-addpath('mat')
-addpath('matlab/subsystems')
-addpath('STEPS/nano_hexapod')
-addpath('STEPS/metrology')
-addpath('STEPS/png')
-#+end_src
-
-#+begin_src matlab
-%% Options for Linearized
-options = linearizeOptions;
-options.SampleTime = 0;
-
-%% Name of the Simulink File
-mdl = 'nass_model_id31';
-#+end_src
-
-#+begin_src matlab
-open(mdl)
-#+end_src
-
-#+begin_src matlab
-load('mat/conf_simulink.mat');
-
-%% Initialize each Simscape model elements
-initializeGround();
-initializeGranite();
-initializeTy();
-initializeRy();
-initializeRz();
-initializeMicroHexapod();
-
-initializeSimscapeConfiguration();
-initializeDisturbances('enable', false);
-initializeLoggingConfiguration('log', 'none');
-
-initializeController('type', 'open-loop');
-
-initializeNanoHexapod('flex_bot_type', '2dof', ...
-                      'flex_top_type', '3dof', ...
-                      'motion_sensor_type', 'plates', ...
-                      'actuator_type', '2dof');
-
-initializeSample('type', '0');
-
-initializePosError();
-
-initializeReferences();
-
-initializeRzEstimate('type', 'encoders');
-
-initializeLion();
-#+end_src
-
-** Identify Transfer functions
-#+begin_src matlab
-%% Input/Output definition
-clear io; io_i = 1;
-io(io_i) = linio([mdl, '/Controller'],     1, 'openinput');               io_i = io_i + 1; % Actuator Inputs
-io(io_i) = linio([mdl, '/Micro-Station'],  3, 'openoutput', [], 'Fnlm');  io_i = io_i + 1; % Force Sensors
-io(io_i) = linio([mdl, '/Micro-Station'],  3, 'openoutput', [], 'Dnlm');  io_i = io_i + 1; % Encoders
-io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL');   io_i = io_i + 1; % Position Errors
-io(io_i) = linio([mdl, '/Micro-Station/metrology_5dof/Lion_Metrology'], 1, 'output');   io_i = io_i + 1; % Interferometers
-#+end_src
-
-#+begin_src matlab :exports none
-%% Identify the dynamics for IFF plant
-initializeSample('type', '0');
-Gm_m0 = linearize(mdl, io);
-Gm_m0.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
-Gm_m0.OutputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6', ...
-                    'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ...
-                    'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6', ...
-                    'd1', 'd2', 'd3', 'd4', 'd5'};
-
-initializeSample('type', '1');
-Gm_m1 = linearize(mdl, io);
-Gm_m1.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
-Gm_m1.OutputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6', ...
-                    'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ...
-                    'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6', ...
-                    'd1', 'd2', 'd3', 'd4', 'd5'};
-
-initializeSample('type', '2');
-Gm_m2 = linearize(mdl, io);
-Gm_m2.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
-Gm_m2.OutputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6', ...
-                    'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ...
-                    'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6', ...
-                    'd1', 'd2', 'd3', 'd4', 'd5'};
-
-initializeSample('type', '3');
-Gm_m3 = linearize(mdl, io);
-Gm_m3.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
-Gm_m3.OutputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6', ...
-                    'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ...
-                    'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6', ...
-                    'd1', 'd2', 'd3', 'd4', 'd5'};
-#+end_src
-
-#+begin_src matlab :exports none
-%% Save Identified Plants
-save('./matlab/mat/Gm.mat', 'Gm_m0', 'Gm_m1', 'Gm_m2', 'Gm_m3');
-#+end_src
-
-** IFF Plant
-#+begin_src matlab :exports none :results none
-%% IFF transfer function - Simscape model
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('Fn%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
-    'DisplayName', '$\tau_{m,i}/u_i$ - $m_0$');
-for i = 2:6
-    plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
-        'HandleVisibility', 'off');
-end
-plot(freqs, abs(squeeze(freqresp(Gm_m1(sprintf('Fn%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
-    'DisplayName', '$\tau_{m,i}/u_i$ - $m_1$');
-for i = 2:6
-    plot(freqs, abs(squeeze(freqresp(Gm_m1(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
-        'HandleVisibility', 'off');
-end
-plot(freqs, abs(squeeze(freqresp(Gm_m2(sprintf('Fn%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
-    'DisplayName', '$\tau_{m,i}/u_i$ - $m_2$');
-for i = 2:6
-    plot(freqs, abs(squeeze(freqresp(Gm_m2(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
-        'HandleVisibility', 'off');
-end
-plot(freqs, abs(squeeze(freqresp(Gm_m3(sprintf('Fn%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
-    'DisplayName', '$\tau_{m,i}/u_i$ - $m_3$');
-for i = 2:6
-    plot(freqs, abs(squeeze(freqresp(Gm_m3(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
-        'HandleVisibility', 'off');
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
-
-ax2 = nexttile;
-hold on;
-for i =1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
-end
-for i =1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m1(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
-end
-for i =1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m2(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5]);
-end
-for i =1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m3(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5]);
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-90, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_simscape_iff_ol_plant.pdf', 'width', 'wide', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_simscape_iff_ol_plant
-#+caption: IFF transfer function - Simscape model
-#+RESULTS:
-[[file:figs/id31_simscape_iff_ol_plant.png]]
-
-** Encoder plant
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('dL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
-    'DisplayName', '$\tau_{m,i}/u_i$ - $m_0$');
-for i = 2:6
-    plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
-        'HandleVisibility', 'off');
-end
-plot(freqs, abs(squeeze(freqresp(Gm_m1(sprintf('dL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
-    'DisplayName', '$\tau_{m,i}/u_i$ - $m_1$');
-for i = 2:6
-    plot(freqs, abs(squeeze(freqresp(Gm_m1(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
-        'HandleVisibility', 'off');
-end
-plot(freqs, abs(squeeze(freqresp(Gm_m2(sprintf('dL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
-    'DisplayName', '$\tau_{m,i}/u_i$ - $m_2$');
-for i = 2:6
-    plot(freqs, abs(squeeze(freqresp(Gm_m2(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
-        'HandleVisibility', 'off');
-end
-plot(freqs, abs(squeeze(freqresp(Gm_m3(sprintf('dL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
-    'DisplayName', '$\tau_{m,i}/u_i$ - $m_3$');
-for i = 2:6
-    plot(freqs, abs(squeeze(freqresp(Gm_m3(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
-        'HandleVisibility', 'off');
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-8, 1e-3]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
-
-ax2 = nexttile;
-hold on;
-for i =1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
-end
-for i =1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m1(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
-end
-for i =1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m2(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5]);
-end
-for i =1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m3(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5]);
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-90, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_simscape_enc_ol_plant.pdf', 'width', 'wide', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_simscape_enc_ol_plant
-#+caption: ENC transfer function - Simscape model
-#+RESULTS:
-[[file:figs/id31_simscape_enc_ol_plant.png]]
-
-** HAC Undamped plant
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
-    'DisplayName', '$\tau_{m,i}/u_i$ - $m_0$');
-for i = 2:6
-    plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
-        'HandleVisibility', 'off');
-end
-plot(freqs, abs(squeeze(freqresp(Gm_m1(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
-    'DisplayName', '$\tau_{m,i}/u_i$ - $m_1$');
-for i = 2:6
-    plot(freqs, abs(squeeze(freqresp(Gm_m1(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
-        'HandleVisibility', 'off');
-end
-plot(freqs, abs(squeeze(freqresp(Gm_m2(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
-    'DisplayName', '$\tau_{m,i}/u_i$ - $m_2$');
-for i = 2:6
-    plot(freqs, abs(squeeze(freqresp(Gm_m2(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
-        'HandleVisibility', 'off');
-end
-plot(freqs, abs(squeeze(freqresp(Gm_m3(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
-    'DisplayName', '$\tau_{m,i}/u_i$ - $m_3$');
-for i = 2:6
-    plot(freqs, abs(squeeze(freqresp(Gm_m3(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
-        'HandleVisibility', 'off');
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-8, 1e-3]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
-
-ax2 = nexttile;
-hold on;
-for i =1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
-end
-for i =1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m1(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
-end
-for i =1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m2(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5]);
-end
-for i =1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m3(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5]);
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-90, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_simscape_int_ol_plant.pdf', 'width', 'wide', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_simscape_int_ol_plant
-#+caption: INT transfer function - Simscape model
-#+RESULTS:
-[[file:figs/id31_simscape_int_ol_plant.png]]
-
-* Identified Open Loop Plant
-<<sec:id31_open_loop_plant>>
-** Introduction                                                      :ignore:
-
-** Matlab Init                                              :noexport:ignore:
-#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
-<<matlab-dir>>
-#+end_src
-
-#+begin_src matlab :exports none :results silent :noweb yes
-<<matlab-init>>
-#+end_src
-
-#+begin_src matlab :tangle no :noweb yes
-<<m-init-path>>
-#+end_src
-
-#+begin_src matlab :eval no :noweb yes
-<<m-init-path-tangle>>
-#+end_src
-
-#+begin_src matlab :noweb yes
-<<m-init-other>>
-#+end_src
-
-#+begin_src matlab
-%% Load Simscape Model
-load('Gm.mat', 'Gm_m0', 'Gm_m1', 'Gm_m2', 'Gm_m3');
-#+end_src
-
-** Load Data                                                       :noexport:
-#+begin_src matlab
-%% Load identification data
-data_m0 = load(sprintf('%s/dynamics/2023-08-08_16-17_ol_plant_m0_Wz0.mat', mat_dir));
-data_m1 = load(sprintf('%s/dynamics/2023-08-08_18-57_ol_plant_m1_Wz0.mat', mat_dir));
-data_m2 = load(sprintf('%s/dynamics/2023-08-08_17-12_ol_plant_m2_Wz0.mat', mat_dir));
-data_m3 = load(sprintf('%s/dynamics/2023-08-08_18-20_ol_plant_m3_Wz0.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-% Sampling Time [s]
-Ts = 1e-4;
-
-% Hannning Windows
-Nfft = floor(2.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-#+end_src
-
-#+begin_src matlab :exports none
-% And we get the frequency vector
-[~, f] = tfestimate(data_m0.uL1.id_plant, data_m0.uL1.e_L1, win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-** IFF Plant
-#+begin_src matlab :exports none
-%% IFF Plant (transfer function from u to taum)
-G_iff_m0 = zeros(length(f), 6, 6);
-
-for i_strut = 1:6
-    eL = [data_m0.(sprintf("uL%i", i_strut)).Vs1 ; data_m0.(sprintf("uL%i", i_strut)).Vs2 ; data_m0.(sprintf("uL%i", i_strut)).Vs3 ; data_m0.(sprintf("uL%i", i_strut)).Vs4 ; data_m0.(sprintf("uL%i", i_strut)).Vs5 ; data_m0.(sprintf("uL%i", i_strut)).Vs6]';
-
-    G_iff_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% IFF Plant (transfer function from u to taum)
-G_iff_m1 = zeros(length(f), 6, 6);
-
-for i_strut = 1:6
-    eL = [data_m1.(sprintf("uL%i", i_strut)).Vs1 ; data_m1.(sprintf("uL%i", i_strut)).Vs2 ; data_m1.(sprintf("uL%i", i_strut)).Vs3 ; data_m1.(sprintf("uL%i", i_strut)).Vs4 ; data_m1.(sprintf("uL%i", i_strut)).Vs5 ; data_m1.(sprintf("uL%i", i_strut)).Vs6]';
-
-    G_iff_m1(:,:,i_strut) = tfestimate(data_m1.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% IFF Plant (transfer function from u to taum)
-G_iff_m2 = zeros(length(f), 6, 6);
-
-for i_strut = 1:6
-    eL = [data_m2.(sprintf("uL%i", i_strut)).Vs1 ; data_m2.(sprintf("uL%i", i_strut)).Vs2 ; data_m2.(sprintf("uL%i", i_strut)).Vs3 ; data_m2.(sprintf("uL%i", i_strut)).Vs4 ; data_m2.(sprintf("uL%i", i_strut)).Vs5 ; data_m2.(sprintf("uL%i", i_strut)).Vs6]';
-
-    G_iff_m2(:,:,i_strut) = tfestimate(data_m2.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% IFF Plant (transfer function from u to taum)
-G_iff_m3 = zeros(length(f), 6, 6);
-
-for i_strut = 1:6
-    eL = [data_m3.(sprintf("uL%i", i_strut)).Vs1 ; data_m3.(sprintf("uL%i", i_strut)).Vs2 ; data_m3.(sprintf("uL%i", i_strut)).Vs3 ; data_m3.(sprintf("uL%i", i_strut)).Vs4 ; data_m3.(sprintf("uL%i", i_strut)).Vs5 ; data_m3.(sprintf("uL%i", i_strut)).Vs6]';
-
-    G_iff_m3(:,:,i_strut) = tfestimate(data_m3.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% Save Identified Plants
-save('./matlab/mat/G_ol.mat', 'G_iff_m0', 'G_iff_m1', 'G_iff_m2', 'G_iff_m3', 'f');
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Measured transfer function from generated voltages to measured voltage on the force sensors
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-for i = 1:5
-    for j = i+1:6
-        plot(f, abs(G_iff_m0(:, i, j)), 'color', [0, 0, 0, 0.2], ...
-             'HandleVisibility', 'off');
-    end
-end
-for i = 1:6
-    plot(f, abs(G_iff_m0(:,i, i)), 'color', colors(i,:), ...
-        'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i));
-end
-plot(f, abs(G_iff_m0(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
-     'DisplayName', '$\tau_{m,i}/u_j$');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-4, 1e2]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
-
-ax2 = nexttile;
-hold on;
-for i =1:6
-    plot(f, 180/pi*angle(G_iff_m0(:,i, i)));
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-90, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_iff_ol_plant_m0.pdf', 'width', 'full', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_iff_ol_plant_m0
-#+caption: Measured transfer function from generated voltages to measured voltage on the force sensors
-#+RESULTS:
-[[file:figs/id31_iff_ol_plant_m0.png]]
-
-The measured frequency response functions from DAC voltages $u_i$ to measured voltages on the force sensors $\tau_{m,i}$ are compared with the simscape model in Figure ref:fig:id31_comp_simscape_frf_ol_iff.
-
-#+begin_src matlab :exports none :results none
-%% Comparison of the Simscape model and identified IFF plant
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-for i = 1:5
-    for j = i+1:6
-        plot(f, abs(G_iff_m0(:, i, j)), 'color', [colors(1,:), 0.2], ...
-             'HandleVisibility', 'off');
-    end
-end
-for i = 1:5
-    for j = i+1:6
-        plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('Fn%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
-             'HandleVisibility', 'off');
-    end
-end
-plot(f, abs(G_iff_m0(:,1, 1)), 'color', [colors(1,:)], ...
-     'DisplayName', '$\tau_{m,i}/u_i$ - Identified');
-for i = 2:6
-    plot(f, abs(G_iff_m0(:,i, i)), 'color', [colors(1,:)], ...
-         'HandleVisibility', 'off');
-end
-plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('Fn%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
-     'DisplayName', '$\tau_{m,i}/u_i$ - Simscape');
-for i = 2:6
-    plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
-         'HandleVisibility', 'off');
-end
-plot(f, abs(G_iff_m0(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
-     'DisplayName', '$\tau_{m,i}/u_j$ - Identified');
-plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('Fn%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
-     'DisplayName', '$\tau_{m,i}/u_j$ - Simscape');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-4, 1e2]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
-
-ax2 = nexttile;
-hold on;
-for i = 1:6
-    plot(f, 180/pi*angle(G_iff_m0(:,i, i)), 'color', [colors(1,:)]);
-end
-for i = 1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)]);
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-90, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_comp_simscape_frf_ol_iff.pdf', 'width', 'full', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_comp_simscape_frf_ol_iff
-#+caption: Comparison of the Simscape model and identified IFF plant
-#+RESULTS:
-[[file:figs/id31_comp_simscape_frf_ol_iff.png]]
-
-The effect of the payload mass on the diagonal elements are shown in Figure ref:fig:id31_effect_mass_frf_ol_iff.
-
-#+begin_src matlab :exports none :results none
-%% Effect of the payload mass on the IFF plant
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_iff_m0(:,1, 1)), 'color', [colors(1,:), 0.5], ...
-     'DisplayName', '$\tau_{m,i}/u_i$ - m0');
-for i = 2:6
-    plot(f, abs(G_iff_m0(:,i, i)), 'color', [colors(1,:), 0.5], ...
-         'HandleVisibility', 'off');
-end
-plot(f, abs(G_iff_m1(:,1, 1)), 'color', [colors(2,:), 0.5], ...
-     'DisplayName', '$\tau_{m,i}/u_i$ - m1');
-for i = 2:6
-    plot(f, abs(G_iff_m1(:,i, i)), 'color', [colors(2,:), 0.5], ...
-         'HandleVisibility', 'off');
-end
-plot(f, abs(G_iff_m2(:,1, 1)), 'color', [colors(3,:), 0.5], ...
-     'DisplayName', '$\tau_{m,i}/u_i$ - m2');
-for i = 2:6
-    plot(f, abs(G_iff_m2(:,i, i)), 'color', [colors(3,:), 0.5], ...
-         'HandleVisibility', 'off');
-end
-plot(f, abs(G_iff_m3(:,1, 1)), 'color', [colors(4,:), 0.5], ...
-     'DisplayName', '$\tau_{m,i}/u_i$ - m3');
-for i = 2:6
-    plot(f, abs(G_iff_m3(:,i, i)), 'color', [colors(4,:), 0.5], ...
-         'HandleVisibility', 'off');
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-2, 1e2]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
-
-ax2 = nexttile;
-hold on;
-for i = 1:6
-    plot(f, 180/pi*angle(G_iff_m0(:,i, i)), 'color', [colors(1,:), 0.5]);
-end
-for i = 1:6
-    plot(f, 180/pi*angle(G_iff_m1(:,i, i)), 'color', [colors(2,:), 0.5]);
-end
-for i = 1:6
-    plot(f, 180/pi*angle(G_iff_m2(:,i, i)), 'color', [colors(3,:), 0.5]);
-end
-for i = 1:6
-    plot(f, 180/pi*angle(G_iff_m3(:,i, i)), 'color', [colors(4,:), 0.5]);
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-90, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_effect_mass_frf_ol_iff.pdf', 'width', 'full', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_effect_mass_frf_ol_iff
-#+caption: Effect of the payload mass on the IFF plant
-#+RESULTS:
-[[file:figs/id31_effect_mass_frf_ol_iff.png]]
-
-** Encoder plant
-#+begin_src matlab :exports none
-%% ENC Plant (transfer function from u to taum)
-G_enc_m0 = zeros(length(f), 6, 6);
-
-for i_strut = 1:6
-    eL = [data_m0.(sprintf("uL%i", i_strut)).dL1 ; data_m0.(sprintf("uL%i", i_strut)).dL2 ; data_m0.(sprintf("uL%i", i_strut)).dL3 ; data_m0.(sprintf("uL%i", i_strut)).dL4 ; data_m0.(sprintf("uL%i", i_strut)).dL5 ; data_m0.(sprintf("uL%i", i_strut)).dL6]';
-
-    G_enc_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% ENC Plant (transfer function from u to taum)
-G_enc_m1 = zeros(length(f), 6, 6);
-
-for i_strut = 1:6
-    eL = [data_m1.(sprintf("uL%i", i_strut)).dL1 ; data_m1.(sprintf("uL%i", i_strut)).dL2 ; data_m1.(sprintf("uL%i", i_strut)).dL3 ; data_m1.(sprintf("uL%i", i_strut)).dL4 ; data_m1.(sprintf("uL%i", i_strut)).dL5 ; data_m1.(sprintf("uL%i", i_strut)).dL6]';
-
-    G_enc_m1(:,:,i_strut) = tfestimate(data_m1.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% ENC Plant (transfer function from u to taum)
-G_enc_m2 = zeros(length(f), 6, 6);
-
-for i_strut = 1:6
-    eL = [data_m2.(sprintf("uL%i", i_strut)).dL1 ; data_m2.(sprintf("uL%i", i_strut)).dL2 ; data_m2.(sprintf("uL%i", i_strut)).dL3 ; data_m2.(sprintf("uL%i", i_strut)).dL4 ; data_m2.(sprintf("uL%i", i_strut)).dL5 ; data_m2.(sprintf("uL%i", i_strut)).dL6]';
-
-    G_enc_m2(:,:,i_strut) = tfestimate(data_m2.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% ENC Plant (transfer function from u to taum)
-G_enc_m3 = zeros(length(f), 6, 6);
-
-for i_strut = 1:6
-    eL = [data_m3.(sprintf("uL%i", i_strut)).dL1 ; data_m3.(sprintf("uL%i", i_strut)).dL2 ; data_m3.(sprintf("uL%i", i_strut)).dL3 ; data_m3.(sprintf("uL%i", i_strut)).dL4 ; data_m3.(sprintf("uL%i", i_strut)).dL5 ; data_m3.(sprintf("uL%i", i_strut)).dL6]';
-
-    G_enc_m3(:,:,i_strut) = tfestimate(data_m3.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% Save Identified Plants
-save('./matlab/mat/G_ol.mat', 'G_enc_m0', 'G_enc_m1', 'G_enc_m2', 'G_enc_m3', '-append');
-#+end_src
-
-The identified frequency response functions from general voltages $u_i$ to measured displacement of the struts by the encoders $d\mathcal{L}_i$ are compared with the simscape model in Figure ref:fig:id31_comp_simscape_frf_ol_enc.
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_enc_m0(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
-     'DisplayName', '$d\mathcal{L}_i/u_j$ - Identified');
-for i = 1:5
-    for j = i+1:6
-        plot(f, abs(G_enc_m0(:, i, j)), 'color', [colors(1,:), 0.2], ...
-             'HandleVisibility', 'off');
-    end
-end
-plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('dL%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
-     'DisplayName', '$d\mathcal{L}_i/u_j$ - Simscape');
-for i = 1:5
-    for j = i+1:6
-        plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('dL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
-             'HandleVisibility', 'off');
-    end
-end
-plot(f, abs(G_enc_m0(:, 1, 1)), 'color', [colors(1,:)], ...
-     'DisplayName', '$d\mathcal{L}_i/u_i$ - Identified');
-for i = 2:6
-    plot(f, abs(G_enc_m0(:,i, i)), 'color', [colors(1,:)], ...
-            'HandleVisibility', 'off');
-end
-plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('dL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
-     'DisplayName', '$d\mathcal{L}_i/u_i$ - Simscape');
-for i = 2:6
-    plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
-            'HandleVisibility', 'off');
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-8, 2e-4]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
-
-ax2 = nexttile;
-hold on;
-for i = 1:6
-    plot(f, 180/pi*angle(G_enc_m0(:,i, i)), 'color', [colors(1,:)]);
-end
-for i = 1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)]);
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-90, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_comp_simscape_frf_ol_enc.pdf', 'width', 'wide', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_comp_simscape_frf_ol_enc
-#+caption: Comparison of the Simscape model and identified plant - Transfer functions from voltages to encoders
-#+RESULTS:
-[[file:figs/id31_comp_simscape_frf_ol_enc.png]]
-
-#+begin_src matlab :exports none :results none
-%% Effect of the payload mass on the transfer function from actuator voltage to encoder displacement
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_enc_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
-     'DisplayName', '$d\mathcal{L}_i/u_i$ - m0');
-for i = 2:6
-    plot(f, abs(G_enc_m0(:,i, i)), 'color', [colors(1,:), 0.5], ...
-            'HandleVisibility', 'off');
-end
-plot(f, abs(G_enc_m1(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
-     'DisplayName', '$d\mathcal{L}_i/u_i$ - m1');
-for i = 2:6
-    plot(f, abs(G_enc_m1(:,i, i)), 'color', [colors(2,:), 0.5], ...
-            'HandleVisibility', 'off');
-end
-plot(f, abs(G_enc_m2(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
-     'DisplayName', '$d\mathcal{L}_i/u_i$ - m2');
-for i = 2:6
-    plot(f, abs(G_enc_m2(:,i, i)), 'color', [colors(3,:), 0.5], ...
-            'HandleVisibility', 'off');
-end
-plot(f, abs(G_enc_m3(:, 1, 1)), 'color', [colors(4,:), 0.5], ...
-     'DisplayName', '$d\mathcal{L}_i/u_i$ - m3');
-for i = 2:6
-    plot(f, abs(G_enc_m3(:,i, i)), 'color', [colors(4,:), 0.5], ...
-            'HandleVisibility', 'off');
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-8, 2e-4]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
-
-ax2 = nexttile;
-hold on;
-for i = 1:6
-    plot(f, 180/pi*angle(G_enc_m0(:,i, i)), 'color', [colors(1,:), 0.5]);
-end
-for i = 1:6
-    plot(f, 180/pi*angle(G_enc_m1(:,i, i)), 'color', [colors(2,:), 0.5]);
-end
-for i = 1:6
-    plot(f, 180/pi*angle(G_enc_m2(:,i, i)), 'color', [colors(3,:), 0.5]);
-end
-for i = 1:6
-    plot(f, 180/pi*angle(G_enc_m3(:,i, i)), 'color', [colors(4,:), 0.5]);
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-90, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_effect_mass_frf_ol_enc.pdf', 'width', 'wide', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_effect_mass_frf_ol_enc
-#+caption: Effect of the payload mass on the transfer function from actuator voltage to encoder displacement
-#+RESULTS:
-[[file:figs/id31_effect_mass_frf_ol_enc.png]]
-
-** HAC Undamped plant
-#+begin_src matlab :exports none
-%% INT Plant (transfer function from u to taum)
-G_int_m0 = zeros(length(f), 6, 6);
-
-for i_strut = 1:6
-    eL = [data_m0.(sprintf("uL%i", i_strut)).e_L1 ; data_m0.(sprintf("uL%i", i_strut)).e_L2 ; data_m0.(sprintf("uL%i", i_strut)).e_L3 ; data_m0.(sprintf("uL%i", i_strut)).e_L4 ; data_m0.(sprintf("uL%i", i_strut)).e_L5 ; data_m0.(sprintf("uL%i", i_strut)).e_L6]';
-
-    G_int_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% INT Plant (transfer function from u to taum)
-G_int_m1 = zeros(length(f), 6, 6);
-
-for i_strut = 1:6
-    eL = [data_m1.(sprintf("uL%i", i_strut)).e_L1 ; data_m1.(sprintf("uL%i", i_strut)).e_L2 ; data_m1.(sprintf("uL%i", i_strut)).e_L3 ; data_m1.(sprintf("uL%i", i_strut)).e_L4 ; data_m1.(sprintf("uL%i", i_strut)).e_L5 ; data_m1.(sprintf("uL%i", i_strut)).e_L6]';
-
-    G_int_m1(:,:,i_strut) = tfestimate(data_m1.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% INT Plant (transfer function from u to taum)
-G_int_m2 = zeros(length(f), 6, 6);
-
-for i_strut = 1:6
-    eL = [data_m2.(sprintf("uL%i", i_strut)).e_L1 ; data_m2.(sprintf("uL%i", i_strut)).e_L2 ; data_m2.(sprintf("uL%i", i_strut)).e_L3 ; data_m2.(sprintf("uL%i", i_strut)).e_L4 ; data_m2.(sprintf("uL%i", i_strut)).e_L5 ; data_m2.(sprintf("uL%i", i_strut)).e_L6]';
-
-    G_int_m2(:,:,i_strut) = tfestimate(data_m2.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% INT Plant (transfer function from u to taum)
-G_int_m3 = zeros(length(f), 6, 6);
-
-for i_strut = 1:6
-    eL = [data_m3.(sprintf("uL%i", i_strut)).e_L1 ; data_m3.(sprintf("uL%i", i_strut)).e_L2 ; data_m3.(sprintf("uL%i", i_strut)).e_L3 ; data_m3.(sprintf("uL%i", i_strut)).e_L4 ; data_m3.(sprintf("uL%i", i_strut)).e_L5 ; data_m3.(sprintf("uL%i", i_strut)).e_L6]';
-
-    G_int_m3(:,:,i_strut) = tfestimate(data_m3.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% Save Identified Plants
-save('./matlab/mat/G_ol.mat', 'G_int_m0', 'G_int_m1', 'G_int_m2', 'G_int_m3', '-append');
-#+end_src
-
-The identified frequency response functions from actuator voltages $u_i$ to measured strut motion from the external metrology (i.e. the interferometers) are compare with the simscape model in Figure ref:fig:id31_comp_simscape_frf_ol_int.
-
-
-#+begin_src matlab :exports none :results none
-%% Measured transfer function from generated voltages to measured voltage on the force sensors
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-for i = 1:5
-    for j = i+1:6
-        plot(f, abs(G_int_m0(:, i, j)), 'color', [0, 0, 0, 0.2], ...
-             'HandleVisibility', 'off');
-    end
-end
-for i = 1:6
-    plot(f, abs(G_int_m0(:,i, i)), 'color', colors(i,:), ...
-        'DisplayName', sprintf('$e\\mathcal{L}_%i/u_%i$', i, i));
-end
-plot(f, abs(G_int_m0(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
-     'DisplayName', '$-e\mathcal{L}_i/u_j$');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-8, 2e-4]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
-
-ax2 = nexttile;
-hold on;
-for i =1:6
-    plot(f, 180/pi*angle(G_int_m0(:,i, i)));
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-90, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_int_ol_plant_m0.pdf', 'width', 'full', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_int_ol_plant_m0
-#+caption: Measured transfer function from generated voltages to measured voltage on the force sensors
-#+RESULTS:
-[[file:figs/id31_int_ol_plant_m0.png]]
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_int_m0(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
-     'DisplayName', '$-e\mathcal{L}_i/u_j$ - Identified');
-for i = 1:5
-    for j = i+1:6
-        plot(f, abs(G_int_m0(:, i, j)), 'color', [colors(1,:), 0.2], ...
-             'HandleVisibility', 'off');
-    end
-end
-plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('eL%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
-     'DisplayName', '$-e\mathcal{L}_i/u_j$ - Simscape');
-for i = 1:5
-    for j = i+1:6
-        plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
-             'HandleVisibility', 'off');
-    end
-end
-plot(f, abs(G_int_m0(:, 1, 1)), 'color', [colors(1,:)], ...
-     'DisplayName', '$-e\mathcal{L}_i/u_i$ - Identified');
-for i = 2:6
-    plot(f, abs(G_int_m0(:,i, i)), 'color', [colors(1,:)], ...
-         'HandleVisibility', 'off');
-end
-plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
-     'DisplayName', '$-e\mathcal{L}_i/u_i$ - Simscape');
-for i = 2:6
-    plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
-         'HandleVisibility', 'off');
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-8, 2e-4]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
-
-ax2 = nexttile;
-hold on;
-for i = 1:6
-    plot(f, 180/pi*angle(G_int_m0(:,i, i)), 'color', [colors(1,:)]);
-end
-for i = 1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)]);
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-90, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_comp_simscape_frf_ol_int.pdf', 'width', 'full', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_comp_simscape_frf_ol_int
-#+caption: Comparison of the Simscape model and identified plant - Transfer functions from voltages to estimated strut motion from interferometers
-#+RESULTS:
-[[file:figs/id31_comp_simscape_frf_ol_int.png]]
-
-#+begin_src matlab :exports none :results none
-%% Effect of the payload mass on the transfer functions from actuator voltage to measured strut motion by the external metrology
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_int_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
-     'DisplayName', '$-e\mathcal{L}_i/u_i$ - m0');
-for i = 2:6
-    plot(f, abs(G_int_m0(:,i, i)), 'color', [colors(1,:), 0.5], ...
-         'HandleVisibility', 'off');
-end
-plot(f, abs(G_int_m1(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
-     'DisplayName', '$-e\mathcal{L}_i/u_i$ - m1');
-for i = 2:6
-    plot(f, abs(G_int_m1(:,i, i)), 'color', [colors(2,:), 0.5], ...
-         'HandleVisibility', 'off');
-end
-plot(f, abs(G_int_m2(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
-     'DisplayName', '$-e\mathcal{L}_i/u_i$ - m2');
-for i = 2:6
-    plot(f, abs(G_int_m2(:,i, i)), 'color', [colors(3,:), 0.5], ...
-         'HandleVisibility', 'off');
-end
-plot(f, abs(G_int_m3(:, 1, 1)), 'color', [colors(4,:), 0.5], ...
-     'DisplayName', '$-e\mathcal{L}_i/u_i$ - m3');
-for i = 2:6
-    plot(f, abs(G_int_m3(:,i, i)), 'color', [colors(4,:), 0.5], ...
-         'HandleVisibility', 'off');
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-8, 2e-4]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
-
-ax2 = nexttile;
-hold on;
-for i = 1:6
-    plot(f, 180/pi*angle(G_int_m0(:,i, i)), 'color', [colors(1,:), 0.5]);
-end
-for i = 1:6
-    plot(f, 180/pi*angle(G_int_m1(:,i, i)), 'color', [colors(2,:), 0.5]);
-end
-for i = 1:6
-    plot(f, 180/pi*angle(G_int_m2(:,i, i)), 'color', [colors(3,:), 0.5]);
-end
-for i = 1:6
-    plot(f, 180/pi*angle(G_int_m3(:,i, i)), 'color', [colors(4,:), 0.5]);
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-90, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_effect_mass_frf_ol_int.pdf', 'width', 'wide', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_effect_mass_frf_ol_int
-#+caption: Effect of the payload mass on the transfer functions from actuator voltage to measured strut motion by the external metrology
-#+RESULTS:
-[[file:figs/id31_effect_mass_frf_ol_int.png]]
-
-** Decoupling improvement thanks to better Rz alignment
-*** Alignment procedure
-
-- Control based on encoders
-- Slow moving in X and Y
-- Compare with X and Y from interf
-
-#+begin_src matlab
-%% Load Data
-data_1_dx = h5scan(data_dir, 'align_int_enc_Rz', 'tx_first_scan', 2);
-data_1_dy = h5scan(data_dir, 'align_int_enc_Rz', 'tx_first_scan', 3);
-data_2_dx = h5scan(data_dir, 'align_int_enc_Rz', 'verif-after-correct-offset', 1);
-data_2_dy = h5scan(data_dir, 'align_int_enc_Rz', 'verif-after-correct-offset', 2);
-#+end_src
-
-#+begin_src matlab
-figure;
-hold on;
-plot(1e6*data_1_dx.Dx_int_filtered, 1e6*data_1_dx.Dy_int_filtered, 'color', colors(1,:), 'DisplayName', 'Measurement')
-plot(1e6*data_1_dy.Dx_int_filtered, 1e6*data_1_dy.Dy_int_filtered, 'color', colors(1,:), 'HandleVisibility', 'off')
-plot(1e6*[-10:10]*cos(2.7*pi/180), -1e6*[-10:10]*sin(2.7*pi/180), '--', 'color', colors(2,:), 'DisplayName', '$R_z = 2.7$ deg')
-plot(1e6*[-10:10]*sin(2.7*pi/180), 1e6*[-10:10]*cos(2.7*pi/180), '--', 'color', colors(2,:), 'HandleVisibility', 'off')
-hold off;
-xlabel('Interf $D_x$ [$\mu$m]');
-ylabel('Interf $D_y$ [$\mu$m]');
-legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
-axis equal
-xlim([-10, 10]); ylim([-10, 10]);
-xticks([-10:5:10]); yticks([-10:5:10]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_Rz_align_dx_dy_scans_before_calib.pdf', 'width', 'wide', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_Rz_align_dx_dy_scans_before_calib
-#+caption: description
-#+RESULTS:
-[[file:figs/id31_Rz_align_dx_dy_scans_before_calib.png]]
-
-#+begin_src matlab
-figure;
-hold on;
-plot(1e6*data_1_dx.Dx_int_filtered, 1e6*data_1_dx.Dy_int_filtered, 'color', colors(1,:), 'DisplayName', 'Initial')
-plot(1e6*data_1_dy.Dx_int_filtered, 1e6*data_1_dy.Dy_int_filtered, 'color', colors(1,:), 'HandleVisibility', 'off')
-plot(1e6*data_2_dx.Dx_int_filtered, 1e6*data_2_dx.Dy_int_filtered, 'color', colors(2,:), 'DisplayName', 'After $R_z$ calib')
-plot(1e6*data_2_dy.Dx_int_filtered, 1e6*data_2_dy.Dy_int_filtered, 'color', colors(2,:), 'HandleVisibility', 'off')
-hold off;
-xlabel('Interf $D_x$ [$\mu$m]');
-ylabel('Interf $D_y$ [$\mu$m]');
-legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
-axis equal
-xlim([-10, 10]); ylim([-10, 10]);
-xticks([-10:5:10]); yticks([-10:5:10]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_Rz_align_dx_dy_scans.pdf', 'width', 'wide', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_Rz_align_dx_dy_scans
-#+caption: description
-#+RESULTS:
-[[file:figs/id31_Rz_align_dx_dy_scans.png]]
-
-
-*** m0
-#+begin_src matlab
-data = load(sprintf('%s/dynamics/2023-08-08_16-17_ol_plant_m0_Wz0.mat', mat_dir));
-data_align = load(sprintf('%s/dynamics/2023-08-17_17-37_ol_plant_m0_Wz0_new_Rz_align.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-%% INT Plant (transfer function from u to taum)
-G_int = zeros(length(f), 6, 6);
-
-for i_strut = 1:6
-    eL = [data.(sprintf("uL%i", i_strut)).e_L1 ; data.(sprintf("uL%i", i_strut)).e_L2 ; data.(sprintf("uL%i", i_strut)).e_L3 ; data.(sprintf("uL%i", i_strut)).e_L4 ; data.(sprintf("uL%i", i_strut)).e_L5 ; data.(sprintf("uL%i", i_strut)).e_L6]';
-
-    G_int(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% INT Plant (transfer function from u to taum)
-G_int_align = zeros(length(f), 6, 6);
-
-for i_strut = 1:6
-    eL = [data_align.(sprintf("uL%i", i_strut)).e_L1 ; data_align.(sprintf("uL%i", i_strut)).e_L2 ; data_align.(sprintf("uL%i", i_strut)).e_L3 ; data_align.(sprintf("uL%i", i_strut)).e_L4 ; data_align.(sprintf("uL%i", i_strut)).e_L5 ; data_align.(sprintf("uL%i", i_strut)).e_L6]';
-
-    G_int_align(:,:,i_strut) = tfestimate(data_align.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
-end
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Decrease of the coupling with better Rz alignment
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_int(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
-     'DisplayName', '$-e\mathcal{L}_i/u_j$');
-for i = 1:5
-    for j = i+1:6
-        plot(f, abs(G_int(:, i, j)), 'color', [colors(1,:), 0.2], ...
-             'HandleVisibility', 'off');
-    end
-end
-plot(f, abs(G_int_align(:, 1, 2)), 'color', [colors(2,:), 0.2], ...
-     'DisplayName', '$-e\mathcal{L}_i/u_j$ - $R_z$ align');
-for i = 1:5
-    for j = i+1:6
-        plot(f, abs(G_int_align(:, i, j)), 'color', [colors(2,:), 0.2], ...
-             'HandleVisibility', 'off');
-    end
-end
-plot(f, abs(G_int(:, 1, 1)), 'color', [colors(1,:)], ...
-     'DisplayName', '$-e\mathcal{L}_i/u_i$');
-for i = 2:6
-    plot(f, abs(G_int(:,i, i)), 'color', [colors(1,:)], ...
-         'HandleVisibility', 'off');
-end
-plot(f, abs(G_int_align(:, 1, 1)), 'color', [colors(2,:)], ...
-     'DisplayName', '$-e\mathcal{L}_i/u_i$ - $R_z$ align');
-for i = 2:6
-    plot(f, abs(G_int_align(:,i, i)), 'color', [colors(2,:)], ...
-         'HandleVisibility', 'off');
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-8, 2e-4]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
-
-ax2 = nexttile;
-hold on;
-for i = 1:6
-    plot(f, 180/pi*angle(G_int(:,i, i)), 'color', [colors(1,:)]);
-end
-for i = 1:6
-    plot(f, 180/pi*angle(G_int_align(:,i, i)), 'color', [colors(2,:)]);
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-90, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_coupling_decrease_rz_align.pdf', 'width', 'full', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_coupling_decrease_rz_align
-#+caption: Decrease of the coupling with better Rz alignment
-#+RESULTS:
-[[file:figs/id31_coupling_decrease_rz_align.png]]
-
-
-*** m3
-#+begin_src matlab
-data = load(sprintf('%s/dynamics/2023-08-10_18-16_damp_plant_m3_Wz0.mat', mat_dir));
-data_align = load(sprintf('%s/dynamics/2023-08-21_15-52_damp_plant_m3_new_Rz.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-%% INT Plant (transfer function from u to taum)
-G_int = zeros(length(f), 6, 6);
-
-for i_strut = 1:6
-    eL = [data.(sprintf("uL%i", i_strut)).e_L1 ; data.(sprintf("uL%i", i_strut)).e_L2 ; data.(sprintf("uL%i", i_strut)).e_L3 ; data.(sprintf("uL%i", i_strut)).e_L4 ; data.(sprintf("uL%i", i_strut)).e_L5 ; data.(sprintf("uL%i", i_strut)).e_L6]';
-
-    G_int(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% INT Plant (transfer function from u to taum)
-G_int_align = zeros(length(f), 6, 6);
-
-for i_strut = 1:6
-    eL = [data_align.(sprintf("uL%i", i_strut)).e_L1 ; data_align.(sprintf("uL%i", i_strut)).e_L2 ; data_align.(sprintf("uL%i", i_strut)).e_L3 ; data_align.(sprintf("uL%i", i_strut)).e_L4 ; data_align.(sprintf("uL%i", i_strut)).e_L5 ; data_align.(sprintf("uL%i", i_strut)).e_L6]';
-
-    G_int_align(:,:,i_strut) = tfestimate(data_align.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
-end
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Decrease of the coupling with better Rz alignment
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_int(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
-     'DisplayName', '$-e\mathcal{L}_i/u_j$');
-for i = 1:5
-    for j = i+1:6
-        plot(f, abs(G_int(:, i, j)), 'color', [colors(1,:), 0.2], ...
-             'HandleVisibility', 'off');
-    end
-end
-plot(f, abs(G_int_align(:, 1, 2)), 'color', [colors(2,:), 0.2], ...
-     'DisplayName', '$-e\mathcal{L}_i/u_j$ - $R_z$ align');
-for i = 1:5
-    for j = i+1:6
-        plot(f, abs(G_int_align(:, i, j)), 'color', [colors(2,:), 0.2], ...
-             'HandleVisibility', 'off');
-    end
-end
-plot(f, abs(G_int(:, 1, 1)), 'color', [colors(1,:)], ...
-     'DisplayName', '$-e\mathcal{L}_i/u_i$');
-for i = 2:6
-    plot(f, abs(G_int(:,i, i)), 'color', [colors(1,:)], ...
-         'HandleVisibility', 'off');
-end
-plot(f, abs(G_int_align(:, 1, 1)), 'color', [colors(2,:)], ...
-     'DisplayName', '$-e\mathcal{L}_i/u_i$ - $R_z$ align');
-for i = 2:6
-    plot(f, abs(G_int_align(:,i, i)), 'color', [colors(2,:)], ...
-         'HandleVisibility', 'off');
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-8, 2e-4]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
-
-ax2 = nexttile;
-hold on;
-for i = 1:6
-    plot(f, 180/pi*angle(G_int(:,i, i)), 'color', [colors(1,:)]);
-end
-for i = 1:6
-    plot(f, 180/pi*angle(G_int_align(:,i, i)), 'color', [colors(2,:)]);
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-90, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_coupling_decrease_rz_align_m3.pdf', 'width', 'full', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_coupling_decrease_rz_align_m3
-#+caption: Decrease of the coupling with better Rz alignment
-#+RESULTS:
-[[file:figs/id31_coupling_decrease_rz_align_m3.png]]
-
-** Conclusion
-
-- Good match between the model and experiment
-
-* TODO Noise Budget
-<<sec:id31_noise_budget>>
-** Introduction                                                      :ignore:
-
-In this section, the noise budget is performed.
-The vibrations of the sample is measured in different conditions using the external metrology.
-
-
-** Matlab Init                                              :noexport:ignore:
-#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
-<<matlab-dir>>
-#+end_src
-
-#+begin_src matlab :exports none :results silent :noweb yes
-<<matlab-init>>
-#+end_src
-
-#+begin_src matlab :tangle no :noweb yes
-<<m-init-path>>
-#+end_src
-
-#+begin_src matlab :eval no :noweb yes
-<<m-init-path-tangle>>
-#+end_src
-
-#+begin_src matlab :noweb yes
-<<m-init-other>>
-#+end_src
-
-** Open-Loop Noise Budget
-First, the noise is measured while no motion is performed.
-
-#+begin_src matlab
-%% Load measured noise
-data_m0 = load(sprintf('%s/freq_analysis/2023-08-11_16-51_m0_lac_off.mat', mat_dir));
-#+end_src
-
-Noise budget in the cartesian frame
-#+begin_src matlab :exports none
-%% Coordinate transform
-J_int_to_X = [ 0                 0                -0.787401574803149 -0.212598425196851  0;
-               0.78740157480315  0.21259842519685  0                  0                  0;
-               0                 0                 0                  0                 -1;
-             -13.1233595800525  13.1233595800525   0                  0                  0;
-               0                 0               -13.1233595800525   13.1233595800525    0];
-
-a = J_int_to_X*[data_m0.d1; data_m0.d2; data_m0.d3; data_m0.d4; data_m0.d5];
-data_m0.t = Ts*[0:length(data_m0.d1)-1];
-data_m0.Dx_int = a(1,:);
-data_m0.Dy_int = a(2,:);
-data_m0.Dz_int = a(3,:);
-data_m0.Rx_int = a(4,:);
-data_m0.Ry_int = a(5,:);
-#+end_src
-
-Data in the time domain
-#+begin_src matlab :exports none :results none
-%% Measured vibration with the interferometers
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile();
-hold on;
-% plot(data_m0.t, 1e9*data_m0.Dx_int, '-', 'DisplayName', '$D_x$');
-plot(data_m0.t, 1e9*data_m0.Dy_int, '-', 'DisplayName', '$D_y$');
-plot(data_m0.t, 1e9*data_m0.Dz_int, '-', 'DisplayName', '$D_z$');
-% plot(data_m0.t, data_m0.Rx_int, 'DisplayName', '$R_x$');
-plot(data_m0.t, 1e9*data_m0.Ry_int, '-', 'DisplayName', '$R_y$');
-hold off;
-set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
-xlabel('Time [s]'); ylabel('Amplitude [nm]');
-legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
-xlim([50, 54])
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_noise_budget_interf_time_domain_m0.pdf', 'width', 'full', 'height', 'normal');
-#+end_src
-
-#+name: fig:id31_noise_budget_interf_time_domain_m0
-#+caption: Measured vibration with the interferometers
-#+RESULTS:
-[[file:figs/id31_noise_budget_interf_time_domain_m0.png]]
-
-In the frequency domain
-#+begin_src matlab :exports none
-% Hannning Windows
-Nfft = floor(2.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-#+end_src
-
-#+begin_src matlab :exports none
-% And we get the frequency vector
-[pxx_Dx, f] = pwelch(detrend(data_m0.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[pxx_Dy, ~] = pwelch(detrend(data_m0.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
-[pxx_Dz, ~] = pwelch(detrend(data_m0.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
-[pxx_Rx, ~] = pwelch(detrend(data_m0.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[pxx_Ry, ~] = pwelch(detrend(data_m0.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile();
-hold on;
-% plot(f, sqrt(pxx_Dx), 'DisplayName', '$D_x$');
-plot(f, sqrt(pxx_Dy), 'DisplayName', '$D_y$');
-plot(f, sqrt(pxx_Dz), 'DisplayName', '$D_z$');
-% plot(f, sqrt(pxx_Rx), 'DisplayName', '$R_x$');
-plot(f, sqrt(pxx_Ry), 'DisplayName', '$R_y$');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('Amplitude [$V/\sqrt{Hz}$]');
-legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
-xlim([1, 5e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_noise_budget_interf_freq_domain_m0.pdf', 'width', 'wide', 'height', 'normal');
-#+end_src
-
-#+name: fig:id31_noise_budget_interf_freq_domain_m0
-#+caption: Measured vibration with the interferometers
-#+RESULTS:
-[[file:figs/id31_noise_budget_interf_freq_domain_m0.png]]
-
-#+begin_src matlab :exports none :results none
-%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion
-figure;
-hold on;
-% plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_Dx)))), 'DisplayName', '$D_x$');
-plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_Dy)))), 'DisplayName', sprintf('$D_y$ - %.0f nm', rms(1e9*detrend(data_m0.Dy_int, 0))));
-plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_Dz)))), 'DisplayName', sprintf('$D_z$ - %.0f nm', rms(1e9*detrend(data_m0.Dz_int, 0))));
-% plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_Rx)))), 'DisplayName', '$R_x$');
-plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_Ry)))), 'DisplayName', sprintf('$R_y$ - %.0f nrad', rms(1e9*detrend(data_m0.Ry_int, 0))));
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('CAS [$m$]');
-legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
-xlim([1, 5e3]); ylim([1e-10, 1e-7]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_noise_budget_open_loop_cas_m0.pdf', 'width', 'wide', 'height', 'normal');
-#+end_src
-
-#+name: fig:id31_noise_budget_open_loop_cas_m0
-#+caption: Measured vibration with the interferometers
-#+RESULTS:
-[[file:figs/id31_noise_budget_open_loop_cas_m0.png]]
-
-** Effect of LAC
-#+begin_src matlab
-%% Load measured noise
-data_ol = load(sprintf('%s/freq_analysis/2023-08-11_16-51_m0_lac_off.mat', mat_dir));
-data_lac = load(sprintf('%s/freq_analysis/2023-08-11_16-46_m0_lac_on.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-%% Coordinate transform
-J_int_to_X = [ 0                 0                -0.787401574803149 -0.212598425196851  0;
-               0.78740157480315  0.21259842519685  0                  0                  0;
-               0                 0                 0                  0                 -1;
-             -13.1233595800525  13.1233595800525   0                  0                  0;
-               0                 0               -13.1233595800525   13.1233595800525    0];
-
-a = J_int_to_X*[data_ol.d1; data_ol.d2; data_ol.d3; data_ol.d4; data_ol.d5];
-data_ol.Dx_int = a(1,:);
-data_ol.Dy_int = a(2,:);
-data_ol.Dz_int = a(3,:);
-data_ol.Rx_int = a(4,:);
-data_ol.Ry_int = a(5,:);
-
-a = J_int_to_X*[data_lac.d1; data_lac.d2; data_lac.d3; data_lac.d4; data_lac.d5];
-data_lac.Dx_int = a(1,:);
-data_lac.Dy_int = a(2,:);
-data_lac.Dz_int = a(3,:);
-data_lac.Rx_int = a(4,:);
-data_lac.Ry_int = a(5,:);
-#+end_src
-
-#+begin_src matlab :exports none
-% Hannning Windows
-Nfft = floor(2.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-
-[data_ol.pxx_Dx, data_ol.f] = pwelch(detrend(data_ol.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_ol.pxx_Dy, ~        ] = pwelch(detrend(data_ol.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_ol.pxx_Dz, ~        ] = pwelch(detrend(data_ol.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_ol.pxx_Rx, ~        ] = pwelch(detrend(data_ol.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_ol.pxx_Ry, ~        ] = pwelch(detrend(data_ol.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
-
-[data_lac.pxx_Dx, data_lac.f] = pwelch(detrend(data_lac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_lac.pxx_Dy, ~         ] = pwelch(detrend(data_lac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_lac.pxx_Dz, ~         ] = pwelch(detrend(data_lac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_lac.pxx_Rx, ~         ] = pwelch(detrend(data_lac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_lac.pxx_Ry, ~         ] = pwelch(detrend(data_lac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-Effect of LAC (Figure ref:fig:id31_noise_budget_effect_lac_m0):
-- reduce amplitude around 80Hz
-- Inject some noise between 200 and 700Hz?
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile();
-hold on;
-plot(data_ol.f , sqrt(data_ol.pxx_Dy ), 'DisplayName', '$D_y$ - OL');
-plot(data_lac.f, sqrt(data_lac.pxx_Dy), 'DisplayName', '$D_y$ - LAC');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('Amplitude [$m/\sqrt{Hz}$]');
-legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
-title('$D_y$')
-
-ax2 = nexttile();
-hold on;
-plot(data_ol.f , sqrt(data_ol.pxx_Dz ), 'DisplayName', '$D_z$ - OL');
-plot(data_lac.f, sqrt(data_lac.pxx_Dz), 'DisplayName', '$D_z$ - LAC');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
-title('$D_z$')
-
-ax3 = nexttile();
-hold on;
-plot(data_ol.f , sqrt(data_ol.pxx_Ry ), 'DisplayName', '$R_y$ - OL');
-plot(data_lac.f, sqrt(data_lac.pxx_Ry), 'DisplayName', '$R_y$ - LAC');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
-title('$R_y$')
-
-linkaxes([ax1,ax2,ax3],'xy');
-xlim([1, 5e2]); ylim([1e-11, 2e-8]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_noise_budget_effect_lac_m0.pdf', 'width', 'full', 'height', 'normal');
-#+end_src
-
-#+name: fig:id31_noise_budget_effect_lac_m0
-#+caption: Measured vibration with the interferometers
-#+RESULTS:
-[[file:figs/id31_noise_budget_effect_lac_m0.png]]
-
-#+begin_src matlab :exports none :results none
-%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion
-figure;
-tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile();
-hold on;
-plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dy)))), 'DisplayName', 'OL');
-plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dy)))), 'DisplayName', 'LAC');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('CAS [$m$]');
-title('$D_y$');
-legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile();
-hold on;
-plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dz)))), 'DisplayName', 'OL');
-plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dz)))), 'DisplayName', 'LAC');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-title('$D_z$');
-legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax3 = nexttile();
-hold on;
-plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Ry)))), 'DisplayName', 'OL');
-plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Ry)))), 'DisplayName', 'LAC');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-title('$R_y$');
-legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
-
-linkaxes([ax1,ax2,ax3],'xy');
-xlim([1, 1e3]); ylim([1e-10, 1e-7]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_cas_effect_lac_m0.pdf', 'width', 'full', 'height', 'normal');
-#+end_src
-
-#+name: fig:id31_cas_effect_lac_m0
-#+caption: Measured vibration with the interferometers
-#+RESULTS:
-[[file:figs/id31_cas_effect_lac_m0.png]]
-
-** Effect of rotation
-#+begin_src matlab
-data_Wz0 = load(sprintf('%s/freq_analysis/2023-08-11_16-51_m0_lac_off.mat',       mat_dir));
-data_Wz1 = load(sprintf('%s/freq_analysis/2023-08-11_17-18_m0_lac_off_1rpm.mat',  mat_dir));
-data_Wz2 = load(sprintf('%s/freq_analysis/2023-08-11_17-39_m0_lac_off_30rpm.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-%% Coordinate transform
-J_int_to_X = [ 0                 0                -0.787401574803149 -0.212598425196851  0;
-               0.78740157480315  0.21259842519685  0                  0                  0;
-               0                 0                 0                  0                 -1;
-             -13.1233595800525  13.1233595800525   0                  0                  0;
-               0                 0               -13.1233595800525   13.1233595800525    0];
-
-a = J_int_to_X*[data_Wz0.d1; data_Wz0.d2; data_Wz0.d3; data_Wz0.d4; data_Wz0.d5];
-data_Wz0.Dx_int = a(1,:);
-data_Wz0.Dy_int = a(2,:);
-data_Wz0.Dz_int = a(3,:);
-data_Wz0.Rx_int = a(4,:);
-data_Wz0.Ry_int = a(5,:);
-
-a = J_int_to_X*[data_Wz1.d1; data_Wz1.d2; data_Wz1.d3; data_Wz1.d4; data_Wz1.d5];
-data_Wz1.Dx_int = a(1,:);
-data_Wz1.Dy_int = a(2,:);
-data_Wz1.Dz_int = a(3,:);
-data_Wz1.Rx_int = a(4,:);
-data_Wz1.Ry_int = a(5,:);
-
-a = J_int_to_X*[data_Wz2.d1; data_Wz2.d2; data_Wz2.d3; data_Wz2.d4; data_Wz2.d5];
-data_Wz2.Dx_int = a(1,:);
-data_Wz2.Dy_int = a(2,:);
-data_Wz2.Dz_int = a(3,:);
-data_Wz2.Rx_int = a(4,:);
-data_Wz2.Ry_int = a(5,:);
-#+end_src
-
-#+begin_src matlab :exports none
-% Hannning Windows
-Nfft = floor(20.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-
-[data_Wz0.pxx_Dx, data_Wz0.f] = pwelch(detrend(data_Wz0.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_Wz0.pxx_Dy, ~         ] = pwelch(detrend(data_Wz0.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_Wz0.pxx_Dz, ~         ] = pwelch(detrend(data_Wz0.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_Wz0.pxx_Rx, ~         ] = pwelch(detrend(data_Wz0.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_Wz0.pxx_Ry, ~         ] = pwelch(detrend(data_Wz0.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
-
-[data_Wz1.pxx_Dx, data_Wz1.f] = pwelch(detrend(data_Wz1.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_Wz1.pxx_Dy, ~          ] = pwelch(detrend(data_Wz1.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_Wz1.pxx_Dz, ~          ] = pwelch(detrend(data_Wz1.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_Wz1.pxx_Rx, ~          ] = pwelch(detrend(data_Wz1.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_Wz1.pxx_Ry, ~          ] = pwelch(detrend(data_Wz1.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
-
-[data_Wz2.pxx_Dx, data_Wz2.f] = pwelch(detrend(data_Wz2.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_Wz2.pxx_Dy, ~          ] = pwelch(detrend(data_Wz2.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_Wz2.pxx_Dz, ~          ] = pwelch(detrend(data_Wz2.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_Wz2.pxx_Rx, ~          ] = pwelch(detrend(data_Wz2.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_Wz2.pxx_Ry, ~          ] = pwelch(detrend(data_Wz2.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile();
-hold on;
-plot(data_Wz0.f, sqrt(data_Wz0.pxx_Dy), 'DisplayName', '$D_y$ - 0rpm');
-plot(data_Wz1.f, sqrt(data_Wz1.pxx_Dy), 'DisplayName', '$D_y$ - 1rpm');
-plot(data_Wz2.f, sqrt(data_Wz2.pxx_Dy), 'DisplayName', '$D_y$ - 30rpm');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('Amplitude [$V/\sqrt{Hz}$]');
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
-xlim([0.1, 5e3]);
-#+end_src
-
-Rotation induces lots of vibrations, especially at high velocity.
-
-#+begin_src matlab :exports none :results none
-%% description
-figure;
-tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile();
-hold on;
-plot(data_Wz0.f, sqrt(flip(-cumtrapz(flip(data_Wz0.f), flip(data_Wz0.pxx_Dy)))),    ...
-     'DisplayName', sprintf('0rpm: $%.0f nm$', 1e9*rms(detrend(data_Wz0.Dy_int, 0))));
-plot(data_Wz1.f, sqrt(flip(-cumtrapz(flip(data_Wz1.f), flip(data_Wz1.pxx_Dy)))), ...
-     'DisplayName', sprintf('6rpm: $%.0f nm$', 1e9*rms(detrend(data_Wz1.Dy_int, 0))));
-plot(data_Wz2.f, sqrt(flip(-cumtrapz(flip(data_Wz2.f), flip(data_Wz2.pxx_Dy)))), ...
-     'DisplayName', sprintf('30rpm: $%.1f \\mu m$', 1e6*rms(detrend(data_Wz2.Dy_int, 0))));
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('CAS [m rms, rad RMS]');
-legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
-xticks([1e0, 1e1, 1e2]);
-title('$D_y$')
-
-ax2 = nexttile();
-hold on;
-plot(data_Wz0.f, sqrt(flip(-cumtrapz(flip(data_Wz0.f), flip(data_Wz0.pxx_Dz)))),    ...
-     'DisplayName', sprintf('0rpm: $%.0f nm$', 1e9*rms(detrend(data_Wz0.Dz_int, 0))));
-plot(data_Wz1.f, sqrt(flip(-cumtrapz(flip(data_Wz1.f), flip(data_Wz1.pxx_Dz)))), ...
-     'DisplayName', sprintf('6rpm: $%.0f nm$', 1e9*rms(detrend(data_Wz1.Dz_int, 0))));
-plot(data_Wz2.f, sqrt(flip(-cumtrapz(flip(data_Wz2.f), flip(data_Wz2.pxx_Dz)))), ...
-     'DisplayName', sprintf('30rpm: $%.0f nm$', 1e9*rms(detrend(data_Wz2.Dz_int, 0))));
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
-xticks([1e0, 1e1, 1e2]);
-title('$D_z$')
-
-ax3 = nexttile();
-hold on;
-plot(data_Wz0.f, sqrt(flip(-cumtrapz(flip(data_Wz0.f), flip(data_Wz0.pxx_Ry)))),    ...
-     'DisplayName', sprintf('0rpm: $%.1f \\mu$rad', 1e6*rms(detrend(data_Wz0.Ry_int, 0))));
-plot(data_Wz1.f, sqrt(flip(-cumtrapz(flip(data_Wz1.f), flip(data_Wz1.pxx_Ry)))), ...
-     'DisplayName', sprintf('6rpm: $%.1f \\mu$rad', 1e6*rms(detrend(data_Wz1.Ry_int, 0))));
-plot(data_Wz2.f, sqrt(flip(-cumtrapz(flip(data_Wz2.f), flip(data_Wz2.pxx_Ry)))), ...
-     'DisplayName', sprintf('30rpm: $%.0f \\mu$rad', 1e6*rms(detrend(data_Wz2.Ry_int, 0))));
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
-xticks([1e0, 1e1, 1e2]);
-title('$R_y$')
-
-linkaxes([ax1,ax2,ax3],'xy');
-xlim([0.1, 5e2]); ylim([1e-10, 3e-5]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_noise_budget_effect_rotation.pdf', 'width', 'full', 'height', 'normal');
-#+end_src
-
-#+name: fig:id31_noise_budget_effect_rotation
-#+caption: Cumulative Amplitude Spectrum for the three important directions ($D_y$, $D_z$ and $R_y$). Three rotating velocities are shown. Integrated RMS values are shown in the legend.
-#+RESULTS:
-[[file:figs/id31_noise_budget_effect_rotation.png]]
-
-** Effect of HAC
-#+begin_src matlab
-%% Load measured noise
-data_ol  = load(sprintf('%s/freq_analysis/2023-08-11_16-51_m0_lac_off.mat', mat_dir));
-data_lac = load(sprintf('%s/freq_analysis/2023-08-11_16-46_m0_lac_on.mat' , mat_dir));
-data_hac = load(sprintf('%s/freq_analysis/2023-08-11_16-49_m0_hac_on.mat' , mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-%% Coordinate transform
-J_int_to_X = [ 0                 0                -0.787401574803149 -0.212598425196851  0;
-               0.78740157480315  0.21259842519685  0                  0                  0;
-               0                 0                 0                  0                 -1;
-             -13.1233595800525  13.1233595800525   0                  0                  0;
-               0                 0               -13.1233595800525   13.1233595800525    0];
-
-a = J_int_to_X*[data_ol.d1; data_ol.d2; data_ol.d3; data_ol.d4; data_ol.d5];
-data_ol.Dx_int = a(1,:);
-data_ol.Dy_int = a(2,:);
-data_ol.Dz_int = a(3,:);
-data_ol.Rx_int = a(4,:);
-data_ol.Ry_int = a(5,:);
-
-a = J_int_to_X*[data_lac.d1; data_lac.d2; data_lac.d3; data_lac.d4; data_lac.d5];
-data_lac.Dx_int = a(1,:);
-data_lac.Dy_int = a(2,:);
-data_lac.Dz_int = a(3,:);
-data_lac.Rx_int = a(4,:);
-data_lac.Ry_int = a(5,:);
-
-a = J_int_to_X*[data_hac.d1; data_hac.d2; data_hac.d3; data_hac.d4; data_hac.d5];
-data_hac.Dx_int = a(1,:);
-data_hac.Dy_int = a(2,:);
-data_hac.Dz_int = a(3,:);
-data_hac.Rx_int = a(4,:);
-data_hac.Ry_int = a(5,:);
-#+end_src
-
-#+begin_src matlab :exports none
-% Hannning Windows
-Nfft = floor(2.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-
-[data_ol.pxx_Dx, data_ol.f] = pwelch(detrend(data_ol.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_ol.pxx_Dy, ~        ] = pwelch(detrend(data_ol.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_ol.pxx_Dz, ~        ] = pwelch(detrend(data_ol.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_ol.pxx_Rx, ~        ] = pwelch(detrend(data_ol.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_ol.pxx_Ry, ~        ] = pwelch(detrend(data_ol.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
-
-[data_lac.pxx_Dx, data_lac.f] = pwelch(detrend(data_lac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_lac.pxx_Dy, ~         ] = pwelch(detrend(data_lac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_lac.pxx_Dz, ~         ] = pwelch(detrend(data_lac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_lac.pxx_Rx, ~         ] = pwelch(detrend(data_lac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_lac.pxx_Ry, ~         ] = pwelch(detrend(data_lac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
-
-[data_hac.pxx_Dx, data_hac.f] = pwelch(detrend(data_hac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_hac.pxx_Dy, ~         ] = pwelch(detrend(data_hac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_hac.pxx_Dz, ~         ] = pwelch(detrend(data_hac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_hac.pxx_Rx, ~         ] = pwelch(detrend(data_hac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_hac.pxx_Ry, ~         ] = pwelch(detrend(data_hac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-Bandwidth is approximately 10Hz.
-
-#+begin_src matlab :exports none :results none
-%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion
-figure;
-tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile();
-hold on;
-plot(data_ol.f,  sqrt(data_ol.pxx_Dy),  'DisplayName', 'OL');
-plot(data_lac.f, sqrt(data_lac.pxx_Dy), 'DisplayName', 'LAC');
-plot(data_hac.f, sqrt(data_hac.pxx_Dy), 'DisplayName', 'HAC');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]');
-title('$D_y$');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile();
-hold on;
-plot(data_ol.f,  sqrt(data_ol.pxx_Dz),  'DisplayName', 'OL');
-plot(data_lac.f, sqrt(data_lac.pxx_Dz), 'DisplayName', 'LAC');
-plot(data_hac.f, sqrt(data_hac.pxx_Dz), 'DisplayName', 'HAC');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-title('$D_z$');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax3 = nexttile();
-hold on;
-plot(data_ol.f,  sqrt(data_ol.pxx_Ry),  'DisplayName', 'OL');
-plot(data_lac.f, sqrt(data_lac.pxx_Ry), 'DisplayName', 'LAC');
-plot(data_hac.f, sqrt(data_hac.pxx_Ry), 'DisplayName', 'HAC');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-title('$R_y$');
-legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
-
-linkaxes([ax1,ax2,ax3],'xy');
-xlim([1, 1e3]); ylim([1e-12, 1e-7]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_noise_budget_effect_hac_m0.pdf', 'width', 'full', 'height', 'normal');
-#+end_src
-
-#+name: fig:id31_noise_budget_effect_hac_m0
-#+caption: Measured vibration with the interferometers
-#+RESULTS:
-[[file:figs/id31_noise_budget_effect_hac_m0.png]]
-
-
-
-#+begin_src matlab :exports none :results none
-%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion
-figure;
-tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile();
-hold on;
-plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dy)))), 'DisplayName', 'OL');
-plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dy)))), 'DisplayName', 'LAC');
-plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Dy)))), 'DisplayName', 'HAC');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('CAS [$m$]');
-title('$D_y$');
-legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile();
-hold on;
-plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dz)))), 'DisplayName', 'OL');
-plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dz)))), 'DisplayName', 'LAC');
-plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Dz)))), 'DisplayName', 'HAC');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-title('$D_z$');
-legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax3 = nexttile();
-hold on;
-plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Ry)))), 'DisplayName', 'OL');
-plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Ry)))), 'DisplayName', 'LAC');
-plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Ry)))), 'DisplayName', 'HAC');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-title('$R_y$');
-legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
-
-linkaxes([ax1,ax2,ax3],'xy');
-xlim([1, 1e3]); ylim([1e-10, 1e-6]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_cas_effect_hac_m0.pdf', 'width', 'full', 'height', 'normal');
-#+end_src
-
-#+name: fig:id31_cas_effect_hac_m0
-#+caption: Measured vibration with the interferometers
-#+RESULTS:
-[[file:figs/id31_cas_effect_hac_m0.png]]
-
-** TODO Noise coming from force sensor
-
-* Integral Force Feedback
-<<sec:id31_iff>>
-** Introduction                                                      :ignore:
-
-** Matlab Init                                              :noexport:ignore:
-#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
-<<matlab-dir>>
-#+end_src
-
-#+begin_src matlab :exports none :results silent :noweb yes
-<<matlab-init>>
-#+end_src
-
-#+begin_src matlab :tangle no :noweb yes
-<<m-init-path>>
-#+end_src
-
-#+begin_src matlab :eval no :noweb yes
-<<m-init-path-tangle>>
-#+end_src
-
-#+begin_src matlab :noweb yes
-<<m-init-other>>
-#+end_src
-
-** IFF Plants
-*** Introduction                                                    :ignore:
-
-*** 6x6 Plant
-#+begin_src matlab
-%% Load identification data
-data = load(sprintf('%s/dynamics/2023-08-08_16-17_ol_plant_m0_Wz0.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-% Hannning Windows
-Nfft = floor(1.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-#+end_src
-
-#+begin_src matlab :exports none
-% And we get the frequency vector
-[~, f] = tfestimate(data.uL1.id_plant, data.uL1.e_L1, win, [], [], 1/Ts);
-#+end_src
-
-#+begin_src matlab :exports none
-%% IFF Plant (transfer function from u to taum)
-G_iff = zeros(length(f), 6, 6);
-
-for i_strut = 1:6
-    eL = [data.(sprintf("uL%i", i_strut)).Vs1 ; data.(sprintf("uL%i", i_strut)).Vs2 ; data.(sprintf("uL%i", i_strut)).Vs3 ; data.(sprintf("uL%i", i_strut)).Vs4 ; data.(sprintf("uL%i", i_strut)).Vs5 ; data.(sprintf("uL%i", i_strut)).Vs6]';
-
-    G_iff(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, eL, win, [], [], 1/Ts);
-end
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-for i = 1:5
-    for j = i+1:6
-        plot(f, abs(G_iff(:, i, j)), 'color', [0, 0, 0, 0.2], ...
-             'HandleVisibility', 'off');
-    end
-end
-for i = 1:6
-    plot(f, abs(G_iff(:,i, i)), 'color', colors(i,:), ...
-        'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i));
-end
-plot(f, abs(G_iff(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
-     'DisplayName', '$\tau_{m,i}/u_j$');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-4, 1e2]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
-
-ax2 = nexttile;
-hold on;
-for i =1:6
-    plot(f, 180/pi*angle(G_iff(:,i, i)));
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-90, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_Giff_plant.pdf', 'width', 'full', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_Giff_plant
-#+caption: Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-#+RESULTS:
-[[file:figs/id31_Giff_plant.png]]
-
-Compare with Model:
-#+begin_src matlab
-load('Gm_iff.mat');
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-for i = 1:5
-    for j = i+1:6
-        plot(f, abs(G_iff(:, i, j)), 'color', [colors(3,:), 0.2], ...
-             'HandleVisibility', 'off');
-    end
-end
-for i = 1:5
-    for j = i+1:6
-        plot(freqs, abs(squeeze(freqresp(Gm_iff_m0(i, j), freqs, 'Hz'))), 'color', [colors(4,:), 0.2], ...
-             'HandleVisibility', 'off');
-    end
-end
-for i = 2:6
-    plot(f, abs(G_iff(:,i, i)), 'color', [colors(1,:), 0.5], ...
-            'HandleVisibility', 'off');
-end
-for i = 2:6
-    plot(freqs, abs(squeeze(freqresp(Gm_iff_m0(i, i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
-            'HandleVisibility', 'off');
-end
-% plot(f, abs(G_iff(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
-%      'DisplayName', '$\tau_{m,i}/u_j$');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-4, 1e2]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
-
-% ax2 = nexttile;
-% hold on;
-% for i =1:6
-%     plot(f, 180/pi*angle(G_iff(:,i, i)));
-% end
-% hold off;
-% set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-% xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-% hold off;
-% yticks(-360:90:360);
-% ylim([-90, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-*** Effect of Rotation
-#+begin_src matlab
-%% Load identification data
-data_Wz0 = load(sprintf('%s/dynamics/2023-08-08_16-17_ol_plant_m0_Wz0.mat', mat_dir));
-data_Wz1 = load(sprintf('%s/dynamics/2023-08-08_16-28_ol_plant_m0_Wz36.mat', mat_dir));
-data_Wz2 = load(sprintf('%s/dynamics/2023-08-08_16-45_ol_plant_m0_Wz180.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-% Sampling Time [s]
-Ts = 1e-4;
-
-% Hannning Windows
-Nfft = floor(1.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-#+end_src
-
-#+begin_src matlab :exports none
-% And we get the frequency vector
-[~, f] = tfestimate(data.uL1.id_plant, data.uL1.e_L1, win, [], [], 1/Ts);
-#+end_src
-
-#+begin_src matlab :exports none
-%% IFF Plant (transfer function from u to taum)
-G_iff_Wz0 = zeros(length(f), 6, 6);
-
-for i_strut = 1:6
-    eL = [data_Wz0.(sprintf("uL%i", i_strut)).Vs1 ; data_Wz0.(sprintf("uL%i", i_strut)).Vs2 ; data_Wz0.(sprintf("uL%i", i_strut)).Vs3 ; data_Wz0.(sprintf("uL%i", i_strut)).Vs4 ; data_Wz0.(sprintf("uL%i", i_strut)).Vs5 ; data_Wz0.(sprintf("uL%i", i_strut)).Vs6]';
-
-    G_iff_Wz0(:,:,i_strut) = tfestimate(data_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, [], [], 1/Ts);
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% IFF Plant (transfer function from u to taum)
-G_iff_Wz1 = zeros(length(f), 6, 6);
-
-for i_strut = 1:6
-    eL = [data_Wz1.(sprintf("uL%i", i_strut)).Vs1 ; data_Wz1.(sprintf("uL%i", i_strut)).Vs2 ; data_Wz1.(sprintf("uL%i", i_strut)).Vs3 ; data_Wz1.(sprintf("uL%i", i_strut)).Vs4 ; data_Wz1.(sprintf("uL%i", i_strut)).Vs5 ; data_Wz1.(sprintf("uL%i", i_strut)).Vs6]';
-
-    G_iff_Wz1(:,:,i_strut) = tfestimate(data_Wz1.(sprintf("uL%i", i_strut)).id_plant, eL, win, [], [], 1/Ts);
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% IFF Plant (transfer function from u to taum)
-G_iff_Wz2 = zeros(length(f), 6, 6);
-
-for i_strut = 1:6
-    eL = [data_Wz2.(sprintf("uL%i", i_strut)).Vs1 ; data_Wz2.(sprintf("uL%i", i_strut)).Vs2 ; data_Wz2.(sprintf("uL%i", i_strut)).Vs3 ; data_Wz2.(sprintf("uL%i", i_strut)).Vs4 ; data_Wz2.(sprintf("uL%i", i_strut)).Vs5 ; data_Wz2.(sprintf("uL%i", i_strut)).Vs6]';
-
-    G_iff_Wz2(:,:,i_strut) = tfestimate(data_Wz2.(sprintf("uL%i", i_strut)).id_plant, eL, win, [], [], 1/Ts);
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% Save Identified Plants
-save('./mat/G_iff.mat', 'G_iff_Wz0', 'G_iff_Wz1', 'G_iff_Wz2', '-append');
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_iff_Wz0(:, 1, 1)), 'color', [colors(1,:), 0.2], ...
-     'DisplayName', '$\Omega_z = 0$');
-for i = 2:6
-    plot(f, abs(G_iff_Wz0(:,i, i)), 'color', [colors(1,:), 0.2], ...
-         'HandleVisibility', 'off')
-end
-plot(f, abs(G_iff_Wz1(:, 1, 1)), 'color', [colors(2,:), 0.2], ...
-     'DisplayName', '$\Omega_z = 36$ deg/s');
-for i = 2:6
-    plot(f, abs(G_iff_Wz1(:,i, i)), 'color', [colors(2,:), 0.2], ...
-         'HandleVisibility', 'off')
-end
-plot(f, abs(G_iff_Wz2(:, 1, 1)), 'color', [colors(3,:), 0.2], ...
-     'DisplayName', '$\Omega_z = 180$ deg/s');
-for i = 2:6
-    plot(f, abs(G_iff_Wz2(:,i, i)), 'color', [colors(3,:), 0.2], ...
-         'HandleVisibility', 'off')
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-2, 1e2]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-for i =1:6
-    plot(f, 180/pi*angle(G_iff_Wz0(:,i, i)), 'color', [colors(1,:), 0.2]);
-end
-for i =1:6
-    plot(f, 180/pi*angle(G_iff_Wz1(:,i, i)), 'color', [colors(2,:), 0.2]);
-end
-for i =1:6
-    plot(f, 180/pi*angle(G_iff_Wz2(:,i, i)), 'color', [colors(3,:), 0.2]);
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-90, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_Giff_effect_rotation.pdf', 'width', 'full', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_Giff_effect_rotation
-#+caption: Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-#+RESULTS:
-[[file:figs/id31_Giff_effect_rotation.png]]
-
-*** Effect of Mass
-#+begin_src matlab
-load('G_ol.mat', 'G_iff_m0', 'G_iff_m1', 'G_iff_m2', 'G_iff_m3', 'f');
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_iff_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
-     'DisplayName', '$m = 0$');
-for i = 2:6
-    plot(f, abs(G_iff_m0(:,i, i)), 'color', [colors(1,:), 0.5], ...
-         'HandleVisibility', 'off')
-end
-plot(f, abs(G_iff_m1(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
-     'DisplayName', '$m = 1$');
-for i = 2:6
-    plot(f, abs(G_iff_m1(:,i, i)), 'color', [colors(2,:), 0.5], ...
-         'HandleVisibility', 'off')
-end
-plot(f, abs(G_iff_m2(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
-     'DisplayName', '$m = 2$');
-for i = 2:6
-    plot(f, abs(G_iff_m2(:,i, i)), 'color', [colors(3,:), 0.5], ...
-         'HandleVisibility', 'off')
-end
-plot(f, abs(G_iff_m3(:, 1, 1)), 'color', [colors(4,:), 0.5], ...
-     'DisplayName', '$m = 3$');
-for i = 2:6
-    plot(f, abs(G_iff_m3(:,i, i)), 'color', [colors(4,:), 0.5], ...
-         'HandleVisibility', 'off')
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-2, 1e2]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-for i =1:6
-    plot(f, 180/pi*angle(G_iff_m0(:,i, i)), 'color', [colors(1,:), 0.5]);
-end
-for i =1:6
-    plot(f, 180/pi*angle(G_iff_m1(:,i, i)), 'color', [colors(2,:), 0.5]);
-end
-for i =1:6
-    plot(f, 180/pi*angle(G_iff_m2(:,i, i)), 'color', [colors(3,:), 0.5]);
-end
-for i =1:6
-    plot(f, 180/pi*angle(G_iff_m3(:,i, i)), 'color', [colors(4,:), 0.5]);
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-90, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_Giff_effect_mass.pdf', 'width', 'full', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_Giff_effect_mass
-#+caption: Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-#+RESULTS:
-[[file:figs/id31_Giff_effect_mass.png]]
-
-*** Compare with the model
-#+begin_src matlab
-load('Gm.mat')
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Comparison of the identified IFF plant and the IFF plant extracted from the simscape model
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_iff_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
-     'DisplayName', '$m = 0$');
-for i = 2:6
-    plot(f, abs(G_iff_m0(:,i, i)), 'color', [colors(1,:), 0.5], ...
-         'HandleVisibility', 'off')
-end
-plot(f, abs(G_iff_m1(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
-     'DisplayName', '$m = 1$');
-for i = 2:6
-    plot(f, abs(G_iff_m1(:,i, i)), 'color', [colors(2,:), 0.5], ...
-         'HandleVisibility', 'off')
-end
-plot(f, abs(G_iff_m2(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
-     'DisplayName', '$m = 2$');
-for i = 2:6
-    plot(f, abs(G_iff_m2(:,i, i)), 'color', [colors(3,:), 0.5], ...
-         'HandleVisibility', 'off')
-end
-plot(f, abs(G_iff_m3(:, 1, 1)), 'color', [colors(4,:), 0.5], ...
-     'DisplayName', '$m = 3$');
-for i = 2:6
-    plot(f, abs(G_iff_m3(:,i, i)), 'color', [colors(4,:), 0.5], ...
-         'HandleVisibility', 'off')
-end
-plot(freqs, abs(squeeze(freqresp(Gm_m0('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:), ...
-     'DisplayName', '$m = 0$ - Model');
-plot(freqs, abs(squeeze(freqresp(Gm_m1('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:), ...
-     'DisplayName', '$m = 1$ - Model');
-plot(freqs, abs(squeeze(freqresp(Gm_m2('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:), ...
-     'DisplayName', '$m = 2$ - Model');
-plot(freqs, abs(squeeze(freqresp(Gm_m3('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:), ...
-     'DisplayName', '$m = 3$ - Model');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-2, 1e2]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(G_iff_m0(:,1,1)), 'color', [colors(1,:), 0.5]);
-for i = 2:6
-    plot(f, 180/pi*angle(G_iff_m0(:,i, i)), 'color', [colors(1,:), 0.5]);
-end
-plot(f, 180/pi*angle(G_iff_m1(:,1,1)), 'color', [colors(2,:), 0.5]);
-for i = 2:6
-    plot(f, 180/pi*angle(G_iff_m1(:,i, i)), 'color', [colors(2,:), 0.5]);
-end
-plot(f, 180/pi*angle(G_iff_m2(:,1,1)), 'color', [colors(3,:), 0.5]);
-for i = 2:6
-    plot(f, 180/pi*angle(G_iff_m2(:,i, i)), 'color', [colors(3,:), 0.5]);
-end
-plot(f, 180/pi*angle(G_iff_m3(:,1,1)), 'color', [colors(4,:), 0.5]);
-for i = 2:6
-    plot(f, 180/pi*angle(G_iff_m3(:,i, i)), 'color', [colors(4,:), 0.5]);
-end
-plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:));
-plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m1('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:));
-plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m2('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:));
-plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m3('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:));
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-90, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_Giff_plant_comp_model.pdf', 'width', 'full', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_Giff_plant_comp_model
-#+caption: Comparison of the identified IFF plant and the IFF plant extracted from the simscape model
-#+RESULTS:
-[[file:figs/id31_Giff_plant_comp_model.png]]
-
-
-** IFF Controller
-*** Controller Design
-Test second order high pass filter:
-#+begin_src matlab
-wz = 2*pi*10;
-xiz = 0.7;
-Ghpf = (s^2/wz^2)/(s^2/wz^2 + 2*xiz*s/wz + 1)
-       % s/(2*pi*1)/(1 + s/(2*pi*1)) * ... % HPF: reduce gain at low frequency
-#+end_src
-
-We want integral action between 20Hz and 200Hz.
-#+begin_src matlab
-%% IFF Controller
-Kiff = -1e2 * ...                           % Gain
-       1/(0.01*2*pi + s) * ...               % LPF: provides integral action
-       Ghpf * ...
-       eye(6);                             % Diagonal 6x6 controller
-Kiff.InputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6'};
-Kiff.OutputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
-#+end_src
-
-Loop Gain:
-#+begin_src matlab :exports none :results none
-%% IFF Loop gain of the diagonal terms
-Kiff_frf = squeeze(freqresp(Kiff(1,1), f, 'Hz'));
-
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_iff_m0(:, 1, 1).*Kiff_frf), 'color', colors(1,:), ...
-     'DisplayName', '$m = 0$');
-plot(f, abs(G_iff_m1(:, 1, 1).*Kiff_frf), 'color', colors(2,:), ...
-     'DisplayName', '$m = 1$');
-plot(f, abs(G_iff_m2(:, 1, 1).*Kiff_frf), 'color', colors(3,:), ...
-     'DisplayName', '$m = 2$');
-plot(f, abs(G_iff_m3(:, 1, 1).*Kiff_frf), 'color', colors(4,:), ...
-     'DisplayName', '$m = 3$');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-2, 1e1]);
-legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(-G_iff_m0(:,1,1).*Kiff_frf), 'color', colors(1,:));
-plot(f, 180/pi*angle(-G_iff_m1(:,1,1).*Kiff_frf), 'color', colors(2,:));
-plot(f, 180/pi*angle(-G_iff_m2(:,1,1).*Kiff_frf), 'color', colors(3,:));
-plot(f, 180/pi*angle(-G_iff_m3(:,1,1).*Kiff_frf), 'color', colors(4,:));
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-180, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_iff_loop_gain_diagonal_terms.pdf', 'width', 'full', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_iff_loop_gain_diagonal_terms
-#+caption: IFF Loop gain of the diagonal terms
-#+RESULTS:
-[[file:figs/id31_iff_loop_gain_diagonal_terms.png]]
-
-Root Locus to obtain optimal gain.
-#+begin_src matlab :exports none :results none
-%% Root Locus for IFF
-gains = logspace(-2, 2, 100);
-Gm_iff_m0 = Gm_m0({'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'});
-Gm_iff_m1 = Gm_m1({'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'});
-Gm_iff_m2 = Gm_m2({'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'});
-Gm_iff_m3 = Gm_m3({'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'});
-
-figure;
-tiledlayout(1, 4, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile();
-hold on;
-plot(real(pole(Gm_iff_m0)),  imag(pole(Gm_iff_m0)),  'x', 'color', colors(1,:), ...
-    'DisplayName', '$g = 0$');
-plot(real(tzero(Gm_iff_m0)), imag(tzero(Gm_iff_m0)), 'o', 'color', colors(1,:), ...
-    'HandleVisibility', 'off');
-
-for g = gains
-    clpoles = pole(feedback(Gm_iff_m0, g*Kiff, +1));
-    plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
-        'HandleVisibility', 'off');
-end
-
-% Optimal gain
-clpoles = pole(feedback(Gm_iff_m0, Kiff, +1));
-plot(real(clpoles), imag(clpoles), 'x', 'color', colors(5,:), ...
-    'DisplayName', '$g_{opt}$');
-hold off;
-axis equal;
-xlim([-640, 0]); ylim([0, 1600]);
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-title('$m_0$');
-
-ax2 = nexttile();
-hold on;
-plot(real(pole(Gm_iff_m1)),  imag(pole(Gm_iff_m1)),  'x', 'color', colors(2,:), ...
-    'DisplayName', '$g = 0$');
-plot(real(tzero(Gm_iff_m1)), imag(tzero(Gm_iff_m1)), 'o', 'color', colors(2,:), ...
-    'HandleVisibility', 'off');
-
-for g = gains
-    clpoles = pole(feedback(Gm_iff_m1, g*Kiff, +1));
-    plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ...
-        'HandleVisibility', 'off');
-end
-
-% Optimal gain
-clpoles = pole(feedback(Gm_iff_m1, Kiff, +1));
-plot(real(clpoles), imag(clpoles), 'x', 'color', colors(5,:), ...
-    'DisplayName', '$g_{opt}$');
-hold off;
-axis equal;
-xlim([-320, 0]); ylim([0, 800]);
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-title('$m_1$');
-
-ax3 = nexttile();
-hold on;
-plot(real(pole(Gm_iff_m2)),  imag(pole(Gm_iff_m2)),  'x', 'color', colors(3,:), ...
-    'DisplayName', '$g = 0$');
-plot(real(tzero(Gm_iff_m2)), imag(tzero(Gm_iff_m2)), 'o', 'color', colors(3,:), ...
-    'HandleVisibility', 'off');
-
-for g = gains
-    clpoles = pole(feedback(Gm_iff_m2, g*Kiff, +1));
-    plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ...
-        'HandleVisibility', 'off');
-end
-
-% Optimal gain
-clpoles = pole(feedback(Gm_iff_m2, Kiff, +1));
-plot(real(clpoles), imag(clpoles), 'x', 'color', colors(5,:), ...
-    'DisplayName', '$g_{opt}$');
-hold off;
-axis equal;
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-xlim([-240, 0]); ylim([0, 600]);
-title('$m_2$');
-
-ax4 = nexttile();
-hold on;
-plot(real(pole(Gm_iff_m3)),  imag(pole(Gm_iff_m3)),  'x', 'color', colors(4,:), ...
-    'DisplayName', '$g = 0$');
-plot(real(tzero(Gm_iff_m3)), imag(tzero(Gm_iff_m3)), 'o', 'color', colors(4,:), ...
-    'HandleVisibility', 'off');
-
-for g = gains
-    clpoles = pole(feedback(Gm_iff_m3, g*Kiff, +1));
-    plot(real(clpoles), imag(clpoles), '.', 'color', colors(4,:), ...
-        'HandleVisibility', 'off');
-end
-
-% Optimal gain
-clpoles = pole(feedback(Gm_iff_m3, Kiff, +1));
-plot(real(clpoles), imag(clpoles), 'x', 'color', colors(5,:), ...
-    'DisplayName', '$g_{opt}$');
-hold off;
-axis equal;
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-xlim([-160, 0]); ylim([0, 400]);
-title('$m_3$');
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_iff_root_locus.pdf', 'width', 'full', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_iff_root_locus
-#+caption: Root Locus for IFF. Green crosses are closed-loop poles for the same choosen IFF gain.
-#+RESULTS:
-[[file:figs/id31_iff_root_locus.png]]
-
-*** TODO Verify Stability
-Verify Stability with Nyquist plot:
-
-- Why bad stability margins?
-
-#+begin_src matlab :exports none
-%% Compute the Eigenvalues of the loop gain
-Ldet = zeros(4, 6, length(f));
-
-% Loop gain
-Lmimo = pagemtimes(permute(G_iff_m0, [2,3,1]),squeeze(freqresp(Kiff, f, 'Hz')));
-for i_f = 2:length(f)
-    Ldet(1,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
-end
-
-Lmimo = pagemtimes(permute(G_iff_m1, [2,3,1]),squeeze(freqresp(Kiff, f, 'Hz')));
-for i_f = 2:length(f)
-    Ldet(2,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
-end
-
-Lmimo = pagemtimes(permute(G_iff_m2, [2,3,1]),squeeze(freqresp(Kiff, f, 'Hz')));
-for i_f = 2:length(f)
-    Ldet(3,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
-end
-
-Lmimo = pagemtimes(permute(G_iff_m3, [2,3,1]),squeeze(freqresp(Kiff, f, 'Hz')));
-for i_f = 2:length(f)
-    Ldet(4,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% Plot of the eigenvalues of L in the complex plane
-figure;
-hold on;
-for i_mass = 1:4
-    plot(real(squeeze(Ldet(i_mass, 1,:))), imag(squeeze(Ldet(i_mass, 1,:))), ...
-         '.', 'color', colors(i_mass, :), ...
-         'DisplayName', sprintf('%i masses', i_mass));
-    plot(real(squeeze(Ldet(i_mass, 1,:))), -imag(squeeze(Ldet(i_mass, 1,:))), ...
-         '.', 'color', colors(i_mass, :), ...
-         'HandleVisibility', 'off');
-    for i = 1:6
-        plot(real(squeeze(Ldet(i_mass, i,:))), imag(squeeze(Ldet(i_mass, i,:))), ...
-             '.', 'color', colors(i_mass, :), ...
-             'HandleVisibility', 'off');
-        plot(real(squeeze(Ldet(i_mass, i,:))), -imag(squeeze(Ldet(i_mass, i,:))), ...
-             '.', 'color', colors(i_mass, :), ...
-             'HandleVisibility', 'off');
-    end
-end
-plot(-1, 0, 'kx', 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
-xlabel('Real'); ylabel('Imag');
-legend('location', 'southeast');
-xlim([-3, 1]); ylim([-2, 2]);
-#+end_src
-
-*** Save Controller
-#+begin_src matlab :exports none :tangle no
-K_order = order(Kiff(1,1));
-
-Kz = c2d(Kiff(1,1)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
-[num, den] = tfdata(Kz, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_iff.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-#+begin_src matlab
-save('./matlab/mat/K_iff.mat', 'Kiff')
-#+end_src
-
-** Estimated Damped Plant
-#+begin_src matlab
-%% Damped plant from Simscape model
-Gm_hac_m0 = -feedback(Gm_m0, Kiff, 'name', +1);
-Gm_hac_m1 = -feedback(Gm_m1, Kiff, 'name', +1);
-Gm_hac_m2 = -feedback(Gm_m2, Kiff, 'name', +1);
-Gm_hac_m3 = -feedback(Gm_m3, Kiff, 'name', +1);
-#+end_src
-
-#+begin_src matlab
-%% Verify Stability
-isstable(Gm_hac_m0)
-isstable(Gm_hac_m1)
-isstable(Gm_hac_m2)
-isstable(Gm_hac_m3)
-#+end_src
-
-#+begin_src matlab
-%% Save Damped Plants
-save('./matlab/mat/Gm.mat', 'Gm_hac_m0', 'Gm_hac_m1', 'Gm_hac_m2', 'Gm_hac_m3', '-append');
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Estimated damped plant from the Simscape model
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
-    'DisplayName', '$\tau_{m,i}/u_i$ - $m_0$');
-for i = 2:6
-    plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
-        'HandleVisibility', 'off');
-end
-plot(freqs, abs(squeeze(freqresp(Gm_hac_m1(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
-    'DisplayName', '$\tau_{m,i}/u_i$ - $m_1$');
-for i = 2:6
-    plot(freqs, abs(squeeze(freqresp(Gm_hac_m1(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
-        'HandleVisibility', 'off');
-end
-plot(freqs, abs(squeeze(freqresp(Gm_hac_m2(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
-    'DisplayName', '$\tau_{m,i}/u_i$ - $m_2$');
-for i = 2:6
-    plot(freqs, abs(squeeze(freqresp(Gm_hac_m2(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
-        'HandleVisibility', 'off');
-end
-plot(freqs, abs(squeeze(freqresp(Gm_hac_m3(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
-    'DisplayName', '$\tau_{m,i}/u_i$ - $m_3$');
-for i = 2:6
-    plot(freqs, abs(squeeze(freqresp(Gm_hac_m3(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
-        'HandleVisibility', 'off');
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-8, 1e-3]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
-
-ax2 = nexttile;
-hold on;
-for i =1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
-end
-for i =1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m1(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
-end
-for i =1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m2(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5]);
-end
-for i =1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m3(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5]);
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-180, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_hac_damped_plant_estimated_simscape.pdf', 'width', 'wide', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_hac_damped_plant_estimated_simscape
-#+caption: description
-#+RESULTS:
-[[file:figs/id31_hac_damped_plant_estimated_simscape.png]]
-
-
-* High Authority Control
-<<sec:id31_iff_hac>>
-** Introduction                                                      :ignore:
-
-** Matlab Init                                              :noexport:ignore:
-#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
-<<matlab-dir>>
-#+end_src
-
-#+begin_src matlab :exports none :results silent :noweb yes
-<<matlab-init>>
-#+end_src
-
-#+begin_src matlab :tangle no :noweb yes
-<<m-init-path>>
-#+end_src
-
-#+begin_src matlab :eval no :noweb yes
-<<m-init-path-tangle>>
-#+end_src
-
-#+begin_src matlab :noweb yes
-<<m-init-other>>
-#+end_src
-
-** Identify Spurious modes
-#+begin_src matlab
-%% Load identification data
-data = load(sprintf('%s/dynamics/2023-08-10_18-32_identify_spurious_modes.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-% Hannning Windows
-Nfft = floor(1.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-#+end_src
-
-#+begin_src matlab :exports none
-% And we get the frequency vector
-[G1, f] = tfestimate(data.id_plant, data.d1, win, Noverlap, Nfft, 1/Ts);
-[G2, ~] = tfestimate(data.id_plant, data.d2, win, Noverlap, Nfft, 1/Ts);
-[G3, ~] = tfestimate(data.id_plant, data.d3, win, Noverlap, Nfft, 1/Ts);
-[G4, ~] = tfestimate(data.id_plant, data.d4, win, Noverlap, Nfft, 1/Ts);
-[G5, ~] = tfestimate(data.id_plant, data.d5, win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile();
-hold on;
-plot(f, abs(G1), 'DisplayName', '1 - top');
-plot(f, abs(G2), 'DisplayName', '2 - bot');
-plot(f, abs(G3), 'DisplayName', '3 - top');
-plot(f, abs(G4), 'DisplayName', '4 - bot');
-plot(f, abs(G5), 'DisplayName', '5 - vertical');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
-xlim([500, 800])
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
-#+end_src
-
-** HAC Plants
-*** Introduction                                                    :ignore:
-
-*** 6x6 Plant
-#+begin_src matlab
-%% Load identification data
-data = load(sprintf('%s/dynamics/2023-08-17_17-53_damp_plant_m0_Wz0.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-% Hannning Windows
-Nfft = floor(1.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-#+end_src
-
-#+begin_src matlab :exports none
-% And we get the frequency vector
-[~, f] = tfestimate(data.uL1.id_plant, data.uL1.e_L1, win, [], [], 1/Ts);
-#+end_src
-
-#+begin_src matlab :exports none
-%% HAC Plant
-G_hac = zeros(length(f), 6, 6);
-
-for i_strut = 1:6
-    eL = [data.(sprintf("uL%i", i_strut)).e_L1 ; data.(sprintf("uL%i", i_strut)).e_L2 ; data.(sprintf("uL%i", i_strut)).e_L3 ; data.(sprintf("uL%i", i_strut)).e_L4 ; data.(sprintf("uL%i", i_strut)).e_L5 ; data.(sprintf("uL%i", i_strut)).e_L6]';
-
-    G_hac(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, [], [], 1/Ts);
-end
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-for i = 1:5
-    for j = i+1:6
-        plot(f, abs(G_hac(:, i, j)), 'color', [0, 0, 0, 0.2], ...
-             'HandleVisibility', 'off');
-    end
-end
-for i = 1:6
-    plot(f, abs(G_hac(:,i, i)), 'color', colors(i,:), ...
-        'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i));
-end
-plot(f, abs(G_hac(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
-     'DisplayName', '$\tau_{m,i}/u_j$');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-8, 1e-3]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
-
-ax2 = nexttile;
-hold on;
-for i =1:6
-    plot(f, 180/pi*angle(G_hac(:,i, i)));
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-180, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-Compare with Model:
-#+begin_src matlab
-load('Gm.mat');
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% 6x6 plant from generated voltages to displacement of the struts as measured by the external metrology
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_hac(:,1,2)), 'color', [colors(3,:), 0.5], ...
-        'DisplayName', 'Identified, Off-Diagonal');
-for i = 1:5
-    for j = i+1:6
-        plot(f, abs(G_hac(:, i, j)), 'color', [colors(3,:), 0.2], ...
-             'HandleVisibility', 'off');
-    end
-end
-plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
-        'DisplayName', 'Model, Off-Diagonal');
-for i = 1:5
-    for j = i+1:6
-        plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(4,:), 0.2], ...
-             'HandleVisibility', 'off');
-    end
-end
-plot(f, abs(G_hac(:,1,1)), 'color', [colors(1,:), 0.5], ...
-        'DisplayName', 'Identified, Diagonal');
-for i = 2:6
-    plot(f, abs(G_hac(:,i, i)), 'color', [colors(1,:), 0.5], ...
-            'HandleVisibility', 'off');
-end
-plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
-        'DisplayName', 'Model, Diagonal');
-for i = 2:6
-    plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
-            'HandleVisibility', 'off');
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-8, 1e-3]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
-
-ax2 = nexttile;
-hold on;
-for i = 1:6
-    plot(f, 180/pi*angle(G_hac(:,i, i)), 'color', [colors(1,:), 0.5])
-end
-for i = 1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5])
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-180, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_Ghac_6x6_plant_comp_model.pdf', 'width', 'full', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_Ghac_6x6_plant_comp_model
-#+caption: 6x6 plant from generated voltages to displacement of the struts as measured by the external metrology
-#+RESULTS:
-[[file:figs/id31_Ghac_6x6_plant_comp_model.png]]
-
-
-*** Effect of Mass
-#+begin_src matlab
-%% Load identification data
-data_m0 = load(sprintf('%s/dynamics/2023-08-17_17-53_damp_plant_m0_Wz0.mat', mat_dir));
-data_m1 = load(sprintf('%s/dynamics/2023-08-10_16-01_damp_plant_m1_Wz0.mat', mat_dir));
-data_m2 = load(sprintf('%s/dynamics/2023-08-10_17-28_damp_plant_m2_Wz0.mat', mat_dir));
-data_m3 = load(sprintf('%s/dynamics/2023-08-10_18-16_damp_plant_m3_Wz0.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-% Sampling Time [s]
-Ts = 1e-4;
-
-% Hannning Windows
-Nfft = floor(2.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-#+end_src
-
-#+begin_src matlab :exports none
-% And we get the frequency vector
-[~, f] = tfestimate(data_m0.uL1.id_plant, data_m0.uL1.e_L1, win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-#+begin_src matlab :exports none
-%% HAC Plant (transfer function from u to taum)
-G_hac_m0 = zeros(length(f), 6, 6);
-
-for i_strut = 1:6
-    eL = [data_m0.(sprintf("uL%i", i_strut)).e_L1 ; data_m0.(sprintf("uL%i", i_strut)).e_L2 ; data_m0.(sprintf("uL%i", i_strut)).e_L3 ; data_m0.(sprintf("uL%i", i_strut)).e_L4 ; data_m0.(sprintf("uL%i", i_strut)).e_L5 ; data_m0.(sprintf("uL%i", i_strut)).e_L6]';
-
-    G_hac_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts);
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% HAC Plant (transfer function from u to taum)
-G_hac_m1 = zeros(length(f), 6, 6);
-
-for i_strut = 1:6
-    eL = [data_m1.(sprintf("uL%i", i_strut)).e_L1 ; data_m1.(sprintf("uL%i", i_strut)).e_L2 ; data_m1.(sprintf("uL%i", i_strut)).e_L3 ; data_m1.(sprintf("uL%i", i_strut)).e_L4 ; data_m1.(sprintf("uL%i", i_strut)).e_L5 ; data_m1.(sprintf("uL%i", i_strut)).e_L6]';
-
-    G_hac_m1(:,:,i_strut) = tfestimate(data_m1.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts);
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% HAC Plant (transfer function from u to taum)
-G_hac_m2 = zeros(length(f), 6, 6);
-
-for i_strut = 1:6
-    eL = [data_m2.(sprintf("uL%i", i_strut)).e_L1 ; data_m2.(sprintf("uL%i", i_strut)).e_L2 ; data_m2.(sprintf("uL%i", i_strut)).e_L3 ; data_m2.(sprintf("uL%i", i_strut)).e_L4 ; data_m2.(sprintf("uL%i", i_strut)).e_L5 ; data_m2.(sprintf("uL%i", i_strut)).e_L6]';
-
-    G_hac_m2(:,:,i_strut) = tfestimate(data_m2.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts);
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% HAC Plant (transfer function from u to taum)
-G_hac_m3 = zeros(length(f), 6, 6);
-
-for i_strut = 1:6
-    eL = [data_m3.(sprintf("uL%i", i_strut)).e_L1 ; data_m3.(sprintf("uL%i", i_strut)).e_L2 ; data_m3.(sprintf("uL%i", i_strut)).e_L3 ; data_m3.(sprintf("uL%i", i_strut)).e_L4 ; data_m3.(sprintf("uL%i", i_strut)).e_L5 ; data_m3.(sprintf("uL%i", i_strut)).e_L6]';
-
-    G_hac_m3(:,:,i_strut) = tfestimate(data_m3.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts);
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% Save Identified Plants
-save('./matlab/mat/G_hac.mat', 'G_hac_m0', 'G_hac_m1', 'G_hac_m2', 'G_hac_m3', 'f');
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer functions
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_hac_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
-     'DisplayName', '$m = 0$');
-for i = 2:6
-    plot(f, abs(G_hac_m0(:,i, i)), 'color', [colors(1,:), 0.5], ...
-         'HandleVisibility', 'off')
-end
-plot(f, abs(G_hac_m1(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
-     'DisplayName', '$m = 1$');
-for i = 2:6
-    plot(f, abs(G_hac_m1(:,i, i)), 'color', [colors(2,:), 0.5], ...
-         'HandleVisibility', 'off')
-end
-plot(f, abs(G_hac_m2(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
-     'DisplayName', '$m = 2$');
-for i = 2:6
-    plot(f, abs(G_hac_m2(:,i, i)), 'color', [colors(3,:), 0.5], ...
-         'HandleVisibility', 'off')
-end
-plot(f, abs(G_hac_m3(:, 1, 1)), 'color', [colors(4,:), 0.5], ...
-     'DisplayName', '$m = 3$');
-for i = 2:6
-    plot(f, abs(G_hac_m3(:,i, i)), 'color', [colors(4,:), 0.5], ...
-         'HandleVisibility', 'off')
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-8, 1e-3]);
-legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
-
-ax2 = nexttile;
-hold on;
-for i =1:6
-    plot(f, 180/pi*angle(G_hac_m0(:,i, i)), 'color', [colors(1,:), 0.5]);
-end
-for i =1:6
-    plot(f, 180/pi*angle(G_hac_m1(:,i, i)), 'color', [colors(2,:), 0.5]);
-end
-for i =1:6
-    plot(f, 180/pi*angle(G_hac_m2(:,i, i)), 'color', [colors(3,:), 0.5]);
-end
-for i =1:6
-    plot(f, 180/pi*angle(G_hac_m3(:,i, i)), 'color', [colors(4,:), 0.5]);
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-180, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_Ghac_effect_mass.pdf', 'width', 'full', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_Ghac_effect_mass
-#+caption: Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-#+RESULTS:
-[[file:figs/id31_Ghac_effect_mass.png]]
-
-*** Compare with the model
-#+begin_src matlab
-load('G_hac.mat')
-load('Gm.mat')
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Comparison of the identified HAC plant and the HAC plant extracted from the simscape model
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_hac_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
-     'DisplayName', '$m = 0$');
-for i = 2:6
-    plot(f, abs(G_hac_m0(:,i, i)), 'color', [colors(1,:), 0.5], ...
-         'HandleVisibility', 'off')
-end
-plot(f, abs(G_hac_m1(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
-     'DisplayName', '$m = 1$');
-for i = 2:6
-    plot(f, abs(G_hac_m1(:,i, i)), 'color', [colors(2,:), 0.5], ...
-         'HandleVisibility', 'off')
-end
-plot(f, abs(G_hac_m2(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
-     'DisplayName', '$m = 2$');
-for i = 2:6
-    plot(f, abs(G_hac_m2(:,i, i)), 'color', [colors(3,:), 0.5], ...
-         'HandleVisibility', 'off')
-end
-plot(f, abs(G_hac_m3(:, 1, 1)), 'color', [colors(4,:), 0.5], ...
-     'DisplayName', '$m = 3$');
-for i = 2:6
-    plot(f, abs(G_hac_m3(:,i, i)), 'color', [colors(4,:), 0.5], ...
-         'HandleVisibility', 'off')
-end
-plot(freqs, abs(squeeze(freqresp(Gm_hac_m0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:), ...
-     'DisplayName', '$m = 0$ - Model');
-plot(freqs, abs(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:), ...
-     'DisplayName', '$m = 1$ - Model');
-plot(freqs, abs(squeeze(freqresp(Gm_hac_m2('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:), ...
-     'DisplayName', '$m = 2$ - Model');
-plot(freqs, abs(squeeze(freqresp(Gm_hac_m3('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:), ...
-     'DisplayName', '$m = 3$ - Model');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-8, 1e-3]);
-legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(G_hac_m0(:,1,1)), 'color', [colors(1,:), 0.5]);
-for i = 2:6
-    plot(f, 180/pi*angle(G_hac_m0(:,i, i)), 'color', [colors(1,:), 0.5]);
-end
-plot(f, 180/pi*angle(G_hac_m1(:,1,1)), 'color', [colors(2,:), 0.5]);
-for i = 2:6
-    plot(f, 180/pi*angle(G_hac_m1(:,i, i)), 'color', [colors(2,:), 0.5]);
-end
-plot(f, 180/pi*angle(G_hac_m2(:,1,1)), 'color', [colors(3,:), 0.5]);
-for i = 2:6
-    plot(f, 180/pi*angle(G_hac_m2(:,i, i)), 'color', [colors(3,:), 0.5]);
-end
-plot(f, 180/pi*angle(G_hac_m3(:,1,1)), 'color', [colors(4,:), 0.5]);
-for i = 2:6
-    plot(f, 180/pi*angle(G_hac_m3(:,i, i)), 'color', [colors(4,:), 0.5]);
-end
-plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:));
-plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:));
-plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m2('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:));
-plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m3('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:));
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-180, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_Ghac_plant_comp_model.pdf', 'width', 'full', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_Ghac_plant_comp_model
-#+caption: Comparison of the identified HAC plant and the HAC plant extracted from the simscape model
-#+RESULTS:
-[[file:figs/id31_Ghac_plant_comp_model.png]]
-
-
-*** Comparison with Undamped plant
-
-#+begin_src matlab
-load('G_ol.mat');
-load('G_hac.mat');
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% description
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_int_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
-     'DisplayName', '$m = 0$ - OL');
-for i = 2:6
-    plot(f, abs(G_int_m0(:,i, i)), 'color', [colors(1,:), 0.5], ...
-         'HandleVisibility', 'off')
-end
-plot(f, abs(G_hac_m0(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
-     'DisplayName', '$m = 0$ - Damped');
-for i = 2:6
-    plot(f, abs(G_hac_m0(:,i, i)), 'color', [colors(2,:), 0.5], ...
-         'HandleVisibility', 'off')
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-8, 1e-3]);
-legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
-
-ax2 = nexttile;
-hold on;
-for i =1:6
-    plot(f, 180/pi*angle(-G_int_m0(:,i, i)), 'color', [colors(1,:), 0.5]);
-end
-for i =1:6
-    plot(f, 180/pi*angle(G_hac_m0(:,i, i)), 'color', [colors(2,:), 0.5]);
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-180, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_comp_undamped_damped_plant_m0.pdf', 'width', 'full', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_comp_undamped_damped_plant_m0
-#+caption: description
-#+RESULTS:
-[[file:figs/id31_comp_undamped_damped_plant_m0.png]]
-
-** Robust HAC
-#+begin_src matlab
-load('G_hac.mat')
-load('Gm.mat')
-#+end_src
-
-*** Controller design
-#+begin_src matlab
-%% Wanted crossover
-wc = 2*pi*5;
-
-%% Double Integrator
-% H_int = (wc^2)/(s + 1e-1*2*pi)^2;
-H_int = wc/s;
-
-%% Lead to increase phase margin
-a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
-
-H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
-
-%% Low Pass filter to increase robustness
-H_lpf = 1/(1 + s/2/pi/30);
-
-%% Notch at the top-plate resonance
-% gm = 0.02;
-% xi = 0.3;
-% wn = 2*pi*665;
-
-% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
-
-%% Gain to have unitary crossover at 30Hz
-[~, i_f] = min(abs(f - wc/2/pi));
-H_gain = 1./abs(G_hac_m0(i_f, 1, 1));
-
-%% Decentralized HAC
-Khac = H_gain  * ... % Gain
-       H_int   * ... % Integrator
-       H_lpf   * ... % Integrator
-       eye(6);            % 6x6 Diagonal
-#+end_src
-
-Loop gain
-#+begin_src matlab :exports none :results none
-%% description
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_hac_m0(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(1,:), 'DisplayName', '$m_0$');
-plot(f, abs(G_hac_m1(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(2,:), 'DisplayName', '$m_1$');
-plot(f, abs(G_hac_m2(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(3,:), 'DisplayName', '$m_2$');
-plot(f, abs(G_hac_m3(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(4,:), 'DisplayName', '$m_3$');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
-legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 3);
-ylim([1e-5, 1e2]);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(G_hac_m0(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(1,:));
-plot(f, 180/pi*angle(G_hac_m1(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(2,:));
-plot(f, 180/pi*angle(G_hac_m2(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(3,:));
-plot(f, 180/pi*angle(G_hac_m3(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(4,:));
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-180, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_hac_robust_loop_gain.pdf', 'width', 'full', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_hac_robust_loop_gain
-#+caption: description
-#+RESULTS:
-[[file:figs/id31_hac_robust_loop_gain.png]]
-
-
-*** Verify Stability
-#+begin_src matlab :exports none
-%% Compute the Eigenvalues of the loop gain
-Ldet = zeros(4, 6, length(f));
-
-% Loop gain
-Lmimo = pagemtimes(permute(G_hac_m0, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
-for i_f = 2:length(f)
-    Ldet(1,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
-end
-
-Lmimo = pagemtimes(permute(G_hac_m1, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
-for i_f = 2:length(f)
-    Ldet(2,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
-end
-
-Lmimo = pagemtimes(permute(G_hac_m2, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
-for i_f = 2:length(f)
-    Ldet(3,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
-end
-
-Lmimo = pagemtimes(permute(G_hac_m3, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
-for i_f = 2:length(f)
-    Ldet(4,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% Plot of the eigenvalues of L in the complex plane
-figure;
-hold on;
-for i_mass = 1:4
-    plot(real(squeeze(Ldet(i_mass, 1,:))), imag(squeeze(Ldet(i_mass, 1,:))), ...
-         '.', 'color', colors(i_mass, :), ...
-         'DisplayName', sprintf('$m_%i$', i_mass));
-    plot(real(squeeze(Ldet(i_mass, 1,:))), -imag(squeeze(Ldet(i_mass, 1,:))), ...
-         '.', 'color', colors(i_mass, :), ...
-         'HandleVisibility', 'off');
-    for i = 1:6
-        plot(real(squeeze(Ldet(i_mass, i,:))), imag(squeeze(Ldet(i_mass, i,:))), ...
-             '.', 'color', colors(i_mass, :), ...
-             'HandleVisibility', 'off');
-        plot(real(squeeze(Ldet(i_mass, i,:))), -imag(squeeze(Ldet(i_mass, i,:))), ...
-             '.', 'color', colors(i_mass, :), ...
-             'HandleVisibility', 'off');
-    end
-end
-plot(-1, 0, 'kx', 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
-xlabel('Real'); ylabel('Imag');
-legend('location', 'southeast');
-axis square
-xlim([-1.5, 0.5]); ylim([-1, 1]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_hac_robust_nyquist.pdf', 'width', 'wide', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_hac_robust_nyquist
-#+caption: description
-#+RESULTS:
-[[file:figs/id31_hac_robust_nyquist.png]]
-
-*** Estimated performances
-
-*** Save Controller
-#+begin_src matlab :exports none :tangle no
-K_order = order(Khac(1,1));
-
-Kz = c2d(Khac(1,1)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
-[num, den] = tfdata(Kz, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-#+begin_src matlab
-save('./matlab/mat/K_hac.mat', 'Khac')
-#+end_src
-
-
-** High Performance HAC
-*** Introduction                                                    :ignore:
-The goal is to make a controller specific for one mass in order to have high bandwidth.
-
-*** Mass 0
-**** Load Plant
-#+begin_src matlab
-load('G_hac.mat')
-load('Gm.mat')
-#+end_src
-
-**** Plant
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-for i = 1:5
-    for j = i+1:6
-        plot(f, abs(G_hac_m0(:, i, j)), 'color', [0, 0, 0, 0.2], ...
-             'HandleVisibility', 'off');
-    end
-end
-for i = 1:6
-    plot(f, abs(G_hac_m0(:,i, i)), 'color', colors(i,:), ...
-         'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i));
-end
-plot(f, abs(G_hac_m0(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
-     'DisplayName', '$\tau_{m,i}/u_j$');
-plot(freqs, abs(squeeze(freqresp(Gm_hac_m0('eL1', 'u1'), freqs, 'Hz'))), 'k--', ...
-     'DisplayName', '$\tau_{m,i}/u_i$ - Model');
-plot(freqs, abs(squeeze(freqresp(Gm_hac_m0('eL2', 'u1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
-     'DisplayName', '$\tau_{m,i}/u_j$ - Model');
-for i = 1:5
-    for j = i+1:6
-        plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
-             'HandleVisibility', 'off');
-    end
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-8, 1e-3]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
-
-ax2 = nexttile;
-hold on;
-for i =1:6
-    plot(f, 180/pi*angle(G_hac_m0(:,i, i)));
-end
-plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m0('eL1', 'u1'), freqs, 'Hz'))), 'k--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-180, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-**** Controller design
-#+begin_src matlab
-%% Wanted crossover
-wc = 2*pi*50;
-
-%% Double Integrator
-H_int = 1/(s + 0.1*2*pi) * ...
-        1/(0.1*2*pi + s);
-H_int = H_int/abs(evalfr(H_int, 1j*wc));
-% H_int = wc/s;
-
-%% Lead to increase phase margin
-a  = 3;  % Amount of phase lead / width of the phase lead / high frequency gain
-H_lead = 1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a)));
-a  = 3;  % Amount of phase lead / width of the phase lead / high frequency gain
-H_lead = H_lead/sqrt(a)*(1 + s/(2.5*wc/sqrt(a)))/(1 + s/(2.5*wc*sqrt(a)));
-
-%% Low Pass filter to increase robustness
-H_lpf = 1/(1 + s/2/pi/500);
-
-%% Notch at the top-plate resonance
-% gm = 0.02;
-% xi = 0.3;
-% wn = 2*pi*665;
-
-% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
-
-%% Gain to have unitary crossover at 30Hz
-[~, i_f] = min(abs(f - wc/2/pi));
-H_gain = 1./abs(G_hac_m0(i_f, 1, 1));
-
-%% Decentralized HAC
-Khac = H_gain  * ... % Gain
-       H_int   * ... % Integrator
-       H_lpf   * ... % Low Pass Filter
-       H_lead  * ... % Integrator
-       eye(6);            % 6x6 Diagonal
-
-Khac.InputName = {'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
-Khac.OutputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
-#+end_src
-
-Loop gain
-#+begin_src matlab :exports none :results none
-%% Loop gain for the High Authority Control
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-for i = 1:6
-    plot(f, abs(G_hac_m0(:,i,i).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [colors(i,:), 0.5]);
-end
-for i = 1:5
-    for j = i+1:6
-        plot(f, abs(G_hac_m0(:,i,j).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [0, 0, 0, 0.2]);
-    end
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
-ylim([1e-4, 1e4]);
-
-ax2 = nexttile;
-hold on;
-for i = 1:6
-    plot(f, 180/pi*angle(G_hac_m0(:,i,i).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [colors(i,:), 0.5]);
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-180, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_high_perf_hac_m0_loop_gain.pdf', 'width', 'wide', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_high_perf_hac_m0_loop_gain
-#+caption: Loop gain for the High Authority Control
-#+RESULTS:
-[[file:figs/id31_high_perf_hac_m0_loop_gain.png]]
-
-**** Verify Stability
-#+begin_src matlab :exports none
-%% Compute the Eigenvalues of the loop gain
-Ldet = zeros(6, length(f));
-
-% Loop gain
-Lmimo = pagemtimes(permute(G_hac_m0, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
-for i_f = 2:length(f)
-    Ldet(:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% Plot of the eigenvalues of L in the complex plane
-figure;
-hold on;
-plot(real(squeeze(Ldet(1,:))),  imag(squeeze(Ldet(1,:))), 'k.');
-plot(real(squeeze(Ldet(1,:))), -imag(squeeze(Ldet(1,:))), 'k.');
-for i = 1:6
-    plot(real(squeeze(Ldet(i,:))),  imag(squeeze(Ldet(i,:))), 'k.');
-    plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.');
-end
-plot(-1, 0, 'kx', 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
-xlabel('Real'); ylabel('Imag');
-xlim([-3, 1]); ylim([-2, 2]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_high_perf_hac_m0_nyquist.pdf', 'width', 'wide', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_high_perf_hac_m0_nyquist
-#+caption: Nyquist plot for the High Authority Control
-#+RESULTS:
-[[file:figs/id31_high_perf_hac_m0_nyquist.png]]
-
-**** Estimated performances
-Loop gain with model
-#+begin_src matlab :exports none
-%% Loop Gain
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-for i = 1:6
-    plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i))*Khac(i,i), freqs, 'Hz'))), 'color', [colors(i,:), 0.5]);
-end
-for i = 1:5
-    for j = i+1:6
-        plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', j))*Khac(i,i), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
-    end
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
-ylim([1e-4, 1e4]);
-
-ax2 = nexttile;
-hold on;
-for i = 1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i))*Khac(i,i), freqs, 'Hz'))), 'color', [colors(i,:), 0.5]);
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-180, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab
-Gm_cl_m0 = feedback(Gm_hac_m0, 0.8*Khac, 'name', -1);
-#+end_src
-
-#+begin_src matlab
-isstable(Gm_cl_m0)
-#+end_src
-
-**** Save Controller
-#+begin_src matlab :exports none :tangle no
-K_order = order(Khac(1,1));
-
-Kz = c2d(Khac(1,1)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
-[num, den] = tfdata(Kz, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_m0.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-**** Experimental Validation
-
-#+begin_src matlab
-data_1rpm = load(sprintf('%s/scans/2023-08-18_10-43_m0_1rpm.mat', mat_dir));
-data_30rpm = load(sprintf('%s/scans/2023-08-18_10-45_m0_30rpm.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-data2orgtable([rms(1e9*data_1rpm.Dy_int), rms(1e9*data_1rpm.Dz_int), rms(1e6*data_1rpm.Ry_int); rms(1e9*data_30rpm.Dy_int), rms(1e9*data_30rpm.Dz_int), rms(1e6*data_30rpm.Ry_int)], {'1rpm', '30rpm'}, {'Dy [nm]', 'Dz [nm]', 'Ry [urad]'}, ' %.1f ');
-#+end_src
-
-#+RESULTS:
-|       | Dy [nm] | Dz [nm] | Ry [urad] |
-|-------+---------+---------+-----------|
-| 1rpm  |    55.3 |     5.9 |       0.1 |
-| 30rpm |    85.2 |    12.5 |       0.3 |
-
-**** Closed-Loop identification
-#+begin_src matlab
-data_cl = load(sprintf('%s/dynamics/2023-08-18_11-03_m0_perf_hac.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-% Sampling Time [s]
-Ts = 1e-4;
-
-% Hannning Windows
-Nfft = floor(1.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-#+end_src
-
-#+begin_src matlab :exports none
-% And we get the frequency vector
-[S_dx, f] = tfestimate(data_cl.Dx.id_cl, data_cl.Dx.e_dx, win, Noverlap, Nfft, 1/Ts);
-[S_dy, ~] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
-[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
-[S_rx, ~] = tfestimate(data_cl.Rx.id_cl, data_cl.Rx.e_rx, win, Noverlap, Nfft, 1/Ts);
-[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
-[S_rz, ~] = tfestimate(data_cl.Rz.id_cl, data_cl.Rz.e_rz, win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile();
-hold on;
-plot(f, abs(S_dy), 'DisplayName', '$D_y$');
-plot(f, abs(S_dz), 'DisplayName', '$D_z$');
-plot(f, abs(S_ry), 'DisplayName', '$R_y$');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('Amplitude');
-ylim([1e-3, 1e1]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
-xlim([1, 1e3]);
-#+end_src
-
-
-*** Mass 1
-**** Load Plant
-#+begin_src matlab
-load('G_hac.mat')
-load('Gm.mat')
-#+end_src
-
-**** Plant
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-for i = 1:5
-    for j = i+1:6
-        plot(f, abs(G_hac_m1(:, i, j)), 'color', [0, 0, 0, 0.2], ...
-             'HandleVisibility', 'off');
-    end
-end
-for i = 1:6
-    plot(f, abs(G_hac_m1(:,i, i)), 'color', colors(i,:), ...
-         'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i));
-end
-plot(f, abs(G_hac_m1(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
-     'DisplayName', '$\tau_{m,i}/u_j$');
-plot(freqs, abs(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))), 'k--', ...
-     'DisplayName', '$\tau_{m,i}/u_i$ - Model');
-plot(freqs, abs(squeeze(freqresp(Gm_hac_m1('eL2', 'u1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
-     'DisplayName', '$\tau_{m,i}/u_j$ - Model');
-for i = 1:5
-    for j = i+1:6
-        plot(freqs, abs(squeeze(freqresp(Gm_hac_m1(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
-             'HandleVisibility', 'off');
-    end
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-8, 1e-3]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
-
-ax2 = nexttile;
-hold on;
-for i =1:6
-    plot(f, 180/pi*angle(G_hac_m1(:,i, i)));
-end
-plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))), 'k--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-180, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-**** Plant Inverse
-#+begin_src matlab
-Gm_hac_red_m1 = flipRphZeros(-balred(Gm_hac_m1('eL1', 'u1'), 6, ...
-                                    balredOptions('StateProjection', 'MatchDC', ...
-                                                  'FreqIntervals', [0, 80])));
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(freqs, abs(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))));
-plot(freqs, abs(squeeze(freqresp(Gm_hac_red_m1, freqs, 'Hz'))));
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
-ylim([1e-8, 1e-3]);
-
-ax2 = nexttile;
-hold on;
-plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))));
-plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_red_m1, 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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab
-%% Plant Inverse
-Gm_hac_inv_m1 = inv(Gm_hac_red_m1);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(freqs, abs(squeeze(freqresp(inv(Gm_hac_m1('eL1', 'u1')), freqs, 'Hz'))));
-plot(freqs, abs(squeeze(freqresp(Gm_hac_inv_m1, freqs, 'Hz'))));
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
-
-ax2 = nexttile;
-hold on;
-plot(freqs, 180/pi*angle(squeeze(freqresp(inv(Gm_hac_m1('eL1', 'u1')), freqs, 'Hz'))));
-plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_inv_m1, 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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-**** Controller design
-#+begin_src matlab
-%% Wanted crossover
-wc = 2*pi*30;
-
-%% Double Integrator
-H_int = 1/(s + 0.1*2*pi) * ...
-        (50*2*pi + s)/(0.01*2*pi + s);
-H_int = H_int/abs(evalfr(H_int, 1j*wc));
-% H_int = wc/s;
-
-%% Lead to increase phase margin
-a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
-H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
-a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
-H_lead = H_lead*1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
-
-%% Low Pass filter to increase robustness
-H_lpf = 1/(1 + s/2/pi/200);
-
-%% Notch at the top-plate resonance
-% gm = 0.02;
-% xi = 0.3;
-% wn = 2*pi*665;
-
-% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
-
-%% Gain to have unitary crossover at 30Hz
-[~, i_f] = min(abs(f - wc/2/pi));
-H_gain = 1./abs(G_hac_m0(i_f, 1, 1));
-
-%% Decentralized HAC
-Khac = 0.8*H_gain  * ... % Gain
-       H_int   * ... % Integrator
-       H_lpf   * ... % Low Pass Filter
-       H_lead  * ... % Integrator
-       eye(6);            % 6x6 Diagonal
-
-Khac.InputName = {'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
-Khac.OutputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
-#+end_src
-
-Loop gain
-#+begin_src matlab :exports none
-%% Loop Gain
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-for i = 1:6
-    plot(f, abs(G_hac_m0(:,i,i).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [colors(i,:), 0.5]);
-end
-for i = 1:5
-    for j = i+1:6
-        plot(f, abs(G_hac_m0(:,i,j).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [0, 0, 0, 0.2]);
-    end
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
-ylim([1e-4, 1e4]);
-
-ax2 = nexttile;
-hold on;
-for i = 1:6
-    plot(f, 180/pi*angle(G_hac_m0(:,i,i).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [colors(i,:), 0.5]);
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-180, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-**** Verify Stability
-#+begin_src matlab :exports none
-%% Compute the Eigenvalues of the loop gain
-Ldet = zeros(6, length(f));
-
-% Loop gain
-Lmimo = pagemtimes(permute(G_hac_m0, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
-for i_f = 2:length(f)
-    Ldet(:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% Plot of the eigenvalues of L in the complex plane
-figure;
-hold on;
-plot(real(squeeze(Ldet(1,:))),  imag(squeeze(Ldet(1,:))), 'k.');
-plot(real(squeeze(Ldet(1,:))), -imag(squeeze(Ldet(1,:))), 'k.');
-for i = 1:6
-    plot(real(squeeze(Ldet(i,:))),  imag(squeeze(Ldet(i,:))), 'k.');
-    plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.');
-end
-plot(-1, 0, 'kx', 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
-xlabel('Real'); ylabel('Imag');
-xlim([-3, 1]); ylim([-2, 2]);
-#+end_src
-
-**** Estimated performances
-Loop gain with model
-#+begin_src matlab :exports none
-%% Loop Gain
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-for i = 1:6
-    plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i))*Khac(i,i), freqs, 'Hz'))), 'color', [colors(i,:), 0.5]);
-end
-for i = 1:5
-    for j = i+1:6
-        plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', j))*Khac(i,i), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
-    end
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
-ylim([1e-4, 1e4]);
-
-ax2 = nexttile;
-hold on;
-for i = 1:6
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i))*Khac(i,i), freqs, 'Hz'))), 'color', [colors(i,:), 0.5]);
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-180, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab
-Gm_cl_m0 = feedback(Gm_hac_m0, Khac, 'name', +1);
-#+end_src
-
-#+begin_src matlab
-isstable(Gm_hac_m0)
-#+end_src
-
-**** Save Controller
-#+begin_src matlab :exports none :tangle no
-K_order = order(Khac(1,1));
-
-Kz = c2d(Khac(1,1)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
-[num, den] = tfdata(Kz, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_m0.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-** Tomography - Performances
-*** First scan with closed-loop at middle
-
-#+begin_src matlab
-data = load(sprintf('%s/scans/2023-08-17_15-22_tomography_30rpm_m0_robust.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none :results none
-writerObj = VideoWriter('test2.avi'); %// initialize the VideoWriter object
-open(writerObj) ;
-
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile();
-di = 500;
-hold on;
-for i = 1:floor(length(data.Dx_int)/di)-1
-    if data.hac_status(di*(i+1)-1) == 0
-        % Only open loop
-        plot(1e6*data.Dx_int(di*i:di*(i+1)-1), 1e6*data.Dy_int(di*i:di*(i+1)-1), '-', 'color', colors(1,:))
-    elseif data.hac_status(di*i) == 1
-        % Only closed loop
-        plot(1e6*data.Dx_int(di*i:di*(i+1)-1), 1e6*data.Dy_int(di*i:di*(i+1)-1), '-', 'color', colors(2,:))
-    else
-        % Both open and closed loop
-        Dx_int = data.Dx_int(di*i:di*(i+1)-1);
-        Dy_int = data.Dy_int(di*i:di*(i+1)-1);
-        plot(1e6*Dx_int(data.hac_status(di*i:di*(i+1)-1) == 0), 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 0), '-', 'color', colors(1,:))
-        plot(1e6*Dx_int(data.hac_status(di*i:di*(i+1)-1) == 1), 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 1), '-', 'color', colors(2,:))
-    end
-    axis square
-    xlim([-3, 3])
-    ylim([-3, 3])
-    xlabel("X motion [$\mu m$]");
-    ylabel("Y motion [$\mu m$]");
-    F = getframe;            %// Capture the frame
-    writeVideo(writerObj,F)  %// add the frame to the movie
-end
-close(writerObj);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% description
-figure;
-tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile;
-hold on;
-plot(1e6*data.Dx_int(data.hac_status == 0), 1e6*data.Dy_int(data.hac_status == 0), ...
-    'DisplayName', 'OL')
-plot(1e6*data.Dx_int(data.hac_status == 1), 1e6*data.Dy_int(data.hac_status == 1), ...
-    'DisplayName', 'CL')
-% Get first time where closed-loop ON
-[~, i] = find(data.hac_status == 1);
-plot(1e6*data.Dx_int(i+50000:end), 1e6*data.Dy_int(i+50000:end), ...
-    'DisplayName', 'CL, stabilized')
-hold off;
-axis equal
-xlim([-3, 3])
-ylim([-3, 3])
-xticks([-3:1:3])
-yticks([-3:1:3])
-xlabel("X motion [$\mu m$]");
-ylabel("Y motion [$\mu m$]");
-
-ax2 = nexttile;
-hold on;
-plot(1e6*data.Dx_int(data.hac_status == 0), 1e6*data.Dy_int(data.hac_status == 0), ...
-    'DisplayName', 'OL')
-plot(1e6*data.Dx_int(data.hac_status == 1), 1e6*data.Dy_int(data.hac_status == 1), ...
-    'DisplayName', 'CL')
-% Get first time where closed-loop ON
-[~, i] = find(data.hac_status == 1);
-plot(1e6*data.Dx_int(i+50000:end), 1e6*data.Dy_int(i+50000:end), ...
-    'DisplayName', 'CL, stabilized')
-legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
-hold off;
-axis equal
-xlabel("X motion [$\mu m$]");
-ylabel("Y motion [$\mu m$]");
-xlim([-0.3, 0.3])
-ylim([-0.3, 0.3])
-xticks([-0.3:0.1:0.3])
-yticks([-0.3:0.1:0.3])
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_tomography_ol_cl_robust_hac_m0.pdf', 'width', 'full', 'height', 'normal');
-#+end_src
-
-#+name: fig:id31_tomography_ol_cl_robust_hac_m0
-#+caption: description
-#+RESULTS:
-[[file:figs/id31_tomography_ol_cl_robust_hac_m0.png]]
-
-
-*** Slow Rotation - 6RPM
-#+begin_src matlab
-%% Load measured noise
-data_ol  = load(sprintf('%s/freq_analysis/2023-08-11_17-18_m0_lac_off_1rpm.mat', mat_dir));
-data_lac = load(sprintf('%s/freq_analysis/2023-08-11_17-16_m0_lac_on_1rpm.mat', mat_dir));
-data_hac = load(sprintf('%s/freq_analysis/2023-08-11_17-14_m0_hac_on_1rpm.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab
-%% Coordinate transform
-J_int_to_X = [ 0                 0                -0.787401574803149 -0.212598425196851  0;
-               0.78740157480315  0.21259842519685  0                  0                  0;
-               0                 0                 0                  0                 -1;
-             -13.1233595800525  13.1233595800525   0                  0                  0;
-               0                 0               -13.1233595800525   13.1233595800525    0];
-
-a = J_int_to_X*[data_ol.d1; data_ol.d2; data_ol.d3; data_ol.d4; data_ol.d5];
-data_ol.Dx_int = a(1,:);
-data_ol.Dy_int = a(2,:);
-data_ol.Dz_int = a(3,:);
-data_ol.Rx_int = a(4,:);
-data_ol.Ry_int = a(5,:);
-
-a = J_int_to_X*[data_lac.d1; data_lac.d2; data_lac.d3; data_lac.d4; data_lac.d5];
-data_lac.Dx_int = a(1,:);
-data_lac.Dy_int = a(2,:);
-data_lac.Dz_int = a(3,:);
-data_lac.Rx_int = a(4,:);
-data_lac.Ry_int = a(5,:);
-
-a = J_int_to_X*[data_hac.d1; data_hac.d2; data_hac.d3; data_hac.d4; data_hac.d5];
-data_hac.Dx_int = a(1,:);
-data_hac.Dy_int = a(2,:);
-data_hac.Dz_int = a(3,:);
-data_hac.Rx_int = a(4,:);
-data_hac.Ry_int = a(5,:);
-#+end_src
-
-#+begin_src matlab :exports none
-% Hannning Windows
-Nfft = floor(30.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-
-[data_ol.pxx_Dx, data_ol.f] = pwelch(detrend(data_ol.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_ol.pxx_Dy, ~        ] = pwelch(detrend(data_ol.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_ol.pxx_Dz, ~        ] = pwelch(detrend(data_ol.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_ol.pxx_Rx, ~        ] = pwelch(detrend(data_ol.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_ol.pxx_Ry, ~        ] = pwelch(detrend(data_ol.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
-
-[data_lac.pxx_Dx, data_lac.f] = pwelch(detrend(data_lac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_lac.pxx_Dy, ~         ] = pwelch(detrend(data_lac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_lac.pxx_Dz, ~         ] = pwelch(detrend(data_lac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_lac.pxx_Rx, ~         ] = pwelch(detrend(data_lac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_lac.pxx_Ry, ~         ] = pwelch(detrend(data_lac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
-
-[data_hac.pxx_Dx, data_hac.f] = pwelch(detrend(data_hac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_hac.pxx_Dy, ~         ] = pwelch(detrend(data_hac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_hac.pxx_Dz, ~         ] = pwelch(detrend(data_hac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_hac.pxx_Rx, ~         ] = pwelch(detrend(data_hac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_hac.pxx_Ry, ~         ] = pwelch(detrend(data_hac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile();
-hold on;
-plot(data_ol.f , sqrt(data_ol.pxx_Dy ), 'DisplayName', '$D_y$ - OL');
-plot(data_lac.f, sqrt(data_lac.pxx_Dy), 'DisplayName', '$D_y$ - LAC');
-plot(data_hac.f, sqrt(data_hac.pxx_Dy), 'DisplayName', '$D_y$ - HAC-LAC');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('Amplitude [$V/\sqrt{Hz}$]');
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
-xlim([1, 5e3]);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion
-figure;
-hold on;
-plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dy)))), 'DisplayName', '$D_y$ - OL');
-plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dy)))), 'DisplayName', '$D_y$ - LAC');
-plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Dy)))), 'DisplayName', '$D_y$ - HAC-LAC');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('CAS [$m$]');
-legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
-% xlim([0.1, 5e3]); ylim([1e-10, 1e-7]);
-#+end_src
-
-*** Rapid Rotation - 30RPM
-#+begin_src matlab
-%% Load measured noise
-data_ol  = load(sprintf('%s/freq_analysis/2023-08-11_17-39_m0_lac_off_30rpm.mat', mat_dir));
-data_lac = load(sprintf('%s/freq_analysis/2023-08-11_17-36_m0_lac_on_30rpm.mat', mat_dir));
-data_hac = load(sprintf('%s/freq_analysis/2023-08-11_17-34_m0_hac_on_30rpm.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab
-%% Coordinate transform
-J_int_to_X = [ 0                 0                -0.787401574803149 -0.212598425196851  0;
-               0.78740157480315  0.21259842519685  0                  0                  0;
-               0                 0                 0                  0                 -1;
-             -13.1233595800525  13.1233595800525   0                  0                  0;
-               0                 0               -13.1233595800525   13.1233595800525    0];
-
-a = J_int_to_X*[data_ol.d1; data_ol.d2; data_ol.d3; data_ol.d4; data_ol.d5];
-data_ol.Dx_int = a(1,:);
-data_ol.Dy_int = a(2,:);
-data_ol.Dz_int = a(3,:);
-data_ol.Rx_int = a(4,:);
-data_ol.Ry_int = a(5,:);
-
-a = J_int_to_X*[data_lac.d1; data_lac.d2; data_lac.d3; data_lac.d4; data_lac.d5];
-data_lac.Dx_int = a(1,:);
-data_lac.Dy_int = a(2,:);
-data_lac.Dz_int = a(3,:);
-data_lac.Rx_int = a(4,:);
-data_lac.Ry_int = a(5,:);
-
-a = J_int_to_X*[data_hac.d1; data_hac.d2; data_hac.d3; data_hac.d4; data_hac.d5];
-data_hac.Dx_int = a(1,:);
-data_hac.Dy_int = a(2,:);
-data_hac.Dz_int = a(3,:);
-data_hac.Rx_int = a(4,:);
-data_hac.Ry_int = a(5,:);
-#+end_src
-
-#+begin_src matlab :exports none
-% Hannning Windows
-Nfft = floor(20.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-
-[data_ol.pxx_Dx, data_ol.f] = pwelch(detrend(data_ol.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_ol.pxx_Dy, ~        ] = pwelch(detrend(data_ol.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_ol.pxx_Dz, ~        ] = pwelch(detrend(data_ol.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_ol.pxx_Rx, ~        ] = pwelch(detrend(data_ol.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_ol.pxx_Ry, ~        ] = pwelch(detrend(data_ol.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
-
-[data_lac.pxx_Dx, data_lac.f] = pwelch(detrend(data_lac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_lac.pxx_Dy, ~         ] = pwelch(detrend(data_lac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_lac.pxx_Dz, ~         ] = pwelch(detrend(data_lac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_lac.pxx_Rx, ~         ] = pwelch(detrend(data_lac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_lac.pxx_Ry, ~         ] = pwelch(detrend(data_lac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
-
-[data_hac.pxx_Dx, data_hac.f] = pwelch(detrend(data_hac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_hac.pxx_Dy, ~         ] = pwelch(detrend(data_hac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_hac.pxx_Dz, ~         ] = pwelch(detrend(data_hac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_hac.pxx_Rx, ~         ] = pwelch(detrend(data_hac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
-[data_hac.pxx_Ry, ~         ] = pwelch(detrend(data_hac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile();
-hold on;
-plot(data_ol.f , sqrt(data_ol.pxx_Dy ), 'DisplayName', '$D_y$ - OL');
-plot(data_lac.f, sqrt(data_lac.pxx_Dy), 'DisplayName', '$D_y$ - LAC');
-plot(data_hac.f, sqrt(data_hac.pxx_Dy), 'DisplayName', '$D_y$ - HAC-LAC');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('Amplitude [$V/\sqrt{Hz}$]');
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
-xlim([1, 5e3]);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion
-figure;
-tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile();
-hold on;
-plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dy)))),    ...
-     'DisplayName', sprintf('OL: $%.1f \\mu m$ RMS', 1e6*rms(detrend(data_ol.Dy_int, 0))));
-plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dy)))), ...
-     'DisplayName', sprintf('LAC: $%.1f \\mu m$ RMS', 1e6*rms(detrend(data_lac.Dy_int, 0))));
-plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Dy)))), ...
-     'DisplayName', sprintf('HAC: $%.0f nm$ RMS', 1e9*rms(detrend(data_hac.Dy_int, 0))));
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('CAS [m rms, rad RMS]');
-legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
-xticks([1e0, 1e1, 1e2]);
-title('$D_y$')
-
-ax2 = nexttile();
-hold on;
-plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dz)))),    ...
-     'DisplayName', sprintf('OL: $%.0f nm$ RMS', 1e9*rms(detrend(data_ol.Dz_int, 0))));
-plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dz)))), ...
-     'DisplayName', sprintf('LAC: $%.0f nm$ RMS', 1e9*rms(detrend(data_lac.Dz_int, 0))));
-plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Dz)))), ...
-     'DisplayName', sprintf('HAC: $%.0f nm$ RMS', 1e9*rms(detrend(data_hac.Dz_int, 0))));
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
-xticks([1e0, 1e1, 1e2]);
-title('$D_z$')
-
-ax3 = nexttile();
-hold on;
-plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Ry)))),    ...
-     'DisplayName', sprintf('OL: $%.1f \\mu$radRMS', 1e6*rms(detrend(data_ol.Ry_int, 0))));
-plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Ry)))), ...
-     'DisplayName', sprintf('LAC: $%.1f \\mu$radRMS', 1e6*rms(detrend(data_lac.Ry_int, 0))));
-plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Ry)))), ...
-     'DisplayName', sprintf('HAC: $%.1f \\mu$radRMS', 1e6*rms(detrend(data_hac.Ry_int, 0))));
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
-xticks([1e0, 1e1, 1e2]);
-title('$R_y$')
-
-linkaxes([ax1,ax2,ax3],'xy');
-xlim([0.1, 5e2]); ylim([1e-10, 3e-5]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_cas_m0_30rpm_ol_lac_hac.pdf', 'width', 'full', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_cas_m0_30rpm_ol_lac_hac
-#+caption: Cumulative Amplitude Spectrum of the errors in $D_y$, $D_z$ and $R_y$ during a tomography scan at 30RPM. Three control configuration are compared: Open-Loop, Low Authority Control, and High Authority Control
-#+RESULTS:
-[[file:figs/id31_cas_m0_30rpm_ol_lac_hac.png]]
-
-
-* 6DoF Control in Cartesian plane (rotating with the nano-hexapod)
-<<sec:id31_cart_hac_rot>>
-** Matlab Init                                              :noexport:ignore:
-#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
-<<matlab-dir>>
-#+end_src
-
-#+begin_src matlab :exports none :results silent :noweb yes
-<<matlab-init>>
-#+end_src
-
-#+begin_src matlab :tangle no :noweb yes
-<<m-init-path>>
-#+end_src
-
-#+begin_src matlab :eval no :noweb yes
-<<m-init-path-tangle>>
-#+end_src
-
-#+begin_src matlab :noweb yes
-<<m-init-other>>
-#+end_src
-
-** Introduction                                                      :ignore:
-As only Dy, Dz and Ry directions are important, we could only control them.
-This lead to a 3x3 plant that may be more decoupled than the 6x6 plant.
-
-** 5x5 plant in Cartesian plane
-#+begin_src matlab :exports none
-%% Load model
-load('Gm.mat')
-#+end_src
-
-#+begin_src matlab
-%% Jacobian for 3x3 plant
-n_hexapod = initializeNanoHexapod('flex_bot_type', '2dof', ...
-                                  'flex_top_type', '3dof', ...
-                                  'motion_sensor_type', 'plates', ...
-                                  'actuator_type', '2dof');
-
-Jt_inv = pinv(n_hexapod.geometry.J');
-Jt_inv = Jt_inv(:,[1,2,3,4,5]);
-
-J_int_to_X = [ 0                 0                -0.787401574803149 -0.212598425196851  0;
-               0.78740157480315  0.21259842519685  0                  0                  0;
-               0                 0                 0                  0                 -1;
-             -13.1233595800525  13.1233595800525   0                  0                  0;
-               0                 0               -13.1233595800525   13.1233595800525    0];
-#+end_src
-
-Compute identified plant in the Cartesian plane:
-#+begin_src matlab
-%% Load Data
-% data_m0 = load(sprintf('%s/dynamics/2023-08-17_10-44_lac_plant_m0_Wz0_interf.mat', mat_dir));
-%% New data after calibrating the Rz-offset
-data_m0 = load(sprintf('%s/dynamics/2023-08-17_16-23_lac_plant_m0_Wz0_interf_Rz_align.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-% Sampling Time [s]
-Ts = 1e-4;
-
-% Hannning Windows
-Nfft = floor(1.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-#+end_src
-
-#+begin_src matlab :exports none
-% And we get the frequency vector
-[~, f] = tfestimate(data_m0.uL1.id_plant, data_m0.uL1.d1, win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-#+begin_src matlab :exports none
-%% HAC Cartesian Plant
-G_hac_m0 = zeros(length(f), 5, 6);
-
-for i_strut = 1:6
-    Di = [data_m0.(sprintf("uL%i", i_strut)).d1 ; data_m0.(sprintf("uL%i", i_strut)).d2 ; data_m0.(sprintf("uL%i", i_strut)).d3 ; data_m0.(sprintf("uL%i", i_strut)).d4 ; data_m0.(sprintf("uL%i", i_strut)).d5]';
-
-    G_hac_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, detrend(Di, 0), win, Noverlap, Nfft, 1/Ts);
-end
-
-G_cart = permute(pagemtimes(J_int_to_X, pagemtimes(permute(G_hac_m0, [2,3,1]), Jt_inv)), [3,1,2]);
-#+end_src
-
-Compute plant model in the Cartesian plane:
-#+begin_src matlab
-Gm_cart = J_int_to_X*Gm_hac_m0({'d1', 'd2', 'd3', 'd4', 'd5'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'})*Jt_inv;
-Gm_cart.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My'};
-Gm_cart.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry'};
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% 5x5 plant
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_cart(:,1,1)), 'DisplayName', '$D_x$');
-plot(f, abs(G_cart(:,2,2)), 'DisplayName', '$D_y$');
-plot(f, abs(G_cart(:,3,3)), 'DisplayName', '$D_z$');
-plot(f, abs(G_cart(:,4,4)), 'DisplayName', '$R_x$');
-plot(f, abs(G_cart(:,5,5)), 'DisplayName', '$R_y$');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-% ylim([1e-4, 1e1]);
-
-ax2 = nexttile();
-hold on;
-plot(f, 180/pi*angle(G_cart(:,1,1)));
-plot(f, 180/pi*angle(G_cart(:,2,2)));
-plot(f, 180/pi*angle(G_cart(:,3,3)));
-plot(f, 180/pi*angle(G_cart(:,4,4)));
-plot(f, 180/pi*angle(G_cart(:,5,5)));
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-180, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-** Controller Design
-#+begin_src matlab
-%% Wanted crossover
-wc = 2*pi*30;
-
-%% Double Integrator
-H_int = 1/(s + 1*2*pi) * ...
-        1/(s + 0.01*2*pi);
-H_int = H_int/abs(evalfr(H_int, 1j*wc));
-
-%% Lead to increase phase margin
-a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
-H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
-a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
-H_lead = H_lead*1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a)));
-
-%% Low Pass filter to increase robustness
-H_lpf = 1/(1 + s/2/pi/300);
-
-%% Notch at the top-plate resonance
-% gm = 0.02;
-% xi = 0.3;
-% wn = 2*pi*665;
-
-% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
-
-%% Gain to have unitary crossover at 30Hz
-[~, i_f] = min(abs(f - wc/2/pi));
-H_gain = -1./abs(G_cart(i_f,1,1));
-
-%% Decentralized HAC
-Khac_Dx = H_gain  * ... % Gain
-          H_int   * ... % Integrator
-          H_lpf   * ... % Low Pass Filter
-          H_lead;       % Integrator
-#+end_src
-
-#+begin_src matlab
-%% Wanted crossover
-wc = 2*pi*30;
-
-%% Double Integrator
-H_int = 1/(s + 1*2*pi) * ...
-        1/(s + 0.01*2*pi);
-H_int = H_int/abs(evalfr(H_int, 1j*wc));
-
-%% Lead to increase phase margin
-a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
-H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
-a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
-H_lead = H_lead*1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a)));
-
-%% Low Pass filter to increase robustness
-H_lpf = 1/(1 + s/2/pi/300);
-
-%% Notch at the top-plate resonance
-% gm = 0.02;
-% xi = 0.3;
-% wn = 2*pi*665;
-
-% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
-
-%% Gain to have unitary crossover at 30Hz
-[~, i_f] = min(abs(f - wc/2/pi));
-H_gain = -1./abs(G_cart(i_f,2,2));
-
-%% Decentralized HAC
-Khac_Dy = H_gain  * ... % Gain
-          H_int   * ... % Integrator
-          H_lpf   * ... % Low Pass Filter
-          H_lead;       % Integrator
-#+end_src
-
-#+begin_src matlab
-%% Wanted crossover
-wc = 2*pi*40;
-
-%% Double Integrator
-H_int = 1/(s + 2*2*pi) * ...
-        1/(s + 0.01*2*pi);
-H_int = H_int/abs(evalfr(H_int, 1j*wc));
-
-%% Lead to increase phase margin
-a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
-H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
-a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
-H_lead = H_lead*1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a)));
-
-%% Low Pass filter to increase robustness
-H_lpf = 1/(1 + s/2/pi/300);
-
-%% Notch at the top-plate resonance
-% gm = 0.02;
-% xi = 0.3;
-% wn = 2*pi*665;
-
-% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
-
-%% Gain to have unitary crossover at 30Hz
-[~, i_f] = min(abs(f - wc/2/pi));
-H_gain = -1./abs(G_cart(i_f,3,3));
-
-%% Decentralized HAC
-Khac_Dz = H_gain  * ... % Gain
-          H_int   * ... % Integrator
-          H_lpf   * ... % Low Pass Filter
-          H_lead;       % Integrator
-#+end_src
-
-#+begin_src matlab
-%% Wanted crossover
-wc = 2*pi*10;
-
-%% Double Integrator
-H_int = 1/(s + 1.5*2*pi) * ...
-        1/(s + 0.01*2*pi);
-H_int = H_int/abs(evalfr(H_int, 1j*wc));
-
-%% Lead to increase phase margin
-a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
-H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
-
-%% Low Pass filter to increase robustness
-H_lpf = 1/(1 + s/2/pi/300);
-
-%% Notch at the top-plate resonance
-% gm = 0.02;
-% xi = 0.3;
-% wn = 2*pi*665;
-
-% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
-
-%% Gain to have unitary crossover at 30Hz
-[~, i_f] = min(abs(f - wc/2/pi));
-H_gain = -1./abs(G_cart(i_f,4,4));
-
-%% Decentralized HAC
-Khac_Rx = H_gain  * ... % Gain
-          H_int   * ... % Integrator
-          H_lpf   * ... % Low Pass Filter
-          H_lead;       % Integrator
-#+end_src
-
-#+begin_src matlab
-%% Wanted crossover
-wc = 2*pi*10;
-
-%% Double Integrator
-H_int = 1/(s + 1.5*2*pi) * ...
-        1/(s + 0.01*2*pi);
-H_int = H_int/abs(evalfr(H_int, 1j*wc));
-
-%% Lead to increase phase margin
-a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
-H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
-
-%% Low Pass filter to increase robustness
-H_lpf = 1/(1 + s/2/pi/300);
-
-%% Notch at the top-plate resonance
-% gm = 0.02;
-% xi = 0.3;
-% wn = 2*pi*665;
-
-% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
-
-%% Gain to have unitary crossover at 30Hz
-[~, i_f] = min(abs(f - wc/2/pi));
-H_gain = -1./abs(G_cart(i_f,5,5));
-
-%% Decentralized HAC
-Khac_Ry = H_gain  * ... % Gain
-          H_int   * ... % Integrator
-          H_lpf   * ... % Low Pass Filter
-          H_lead;       % Integrator
-#+end_src
-
-#+begin_src matlab
-Khac = blkdiag(Khac_Dx, Khac_Dy, Khac_Dz, Khac_Rx, Khac_Ry);
-#+end_src
-
-#+begin_src matlab :exports none
-%% Loop Gain
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_cart(:,1,1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))));
-plot(f, abs(G_cart(:,2,2).*squeeze(freqresp(Khac(2,2), f, 'Hz'))));
-plot(f, abs(G_cart(:,3,3).*squeeze(freqresp(Khac(3,3), f, 'Hz'))));
-plot(f, abs(G_cart(:,4,4).*squeeze(freqresp(Khac(4,4), f, 'Hz'))));
-plot(f, abs(G_cart(:,5,5).*squeeze(freqresp(Khac(5,5), f, 'Hz'))));
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
-ylim([1e-4, 1e4]);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(G_cart(:,1,1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))));
-plot(f, 180/pi*angle(G_cart(:,2,2).*squeeze(freqresp(Khac(2,2), f, 'Hz'))));
-plot(f, 180/pi*angle(G_cart(:,3,3).*squeeze(freqresp(Khac(3,3), f, 'Hz'))));
-plot(f, 180/pi*angle(G_cart(:,4,4).*squeeze(freqresp(Khac(4,4), f, 'Hz'))));
-plot(f, 180/pi*angle(G_cart(:,5,5).*squeeze(freqresp(Khac(5,5), f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-** Check Stability
-#+begin_src matlab :exports none
-%% Compute the Eigenvalues of the loop gain
-Ldet = zeros(5, length(f));
-
-% Loop gain
-Lmimo = pagemtimes(permute(G_cart, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
-for i_f = 2:length(f)
-    Ldet(:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% Plot of the eigenvalues of L in the complex plane
-figure;
-hold on;
-plot(real(squeeze(Ldet(1,:))), imag(squeeze(Ldet(1,:))), 'k.')
-plot(real(squeeze(Ldet(1,:))), -imag(squeeze(Ldet(1,:))), 'k.')
-for i = 1:size(Ldet,1)
-    plot(real(squeeze(Ldet(i,:))), imag(squeeze(Ldet(i,:))), 'k.')
-    plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.')
-end
-plot(-1, 0, 'kx');
-hold off;
-set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
-xlabel('Real'); ylabel('Imag');
-xlim([-3, 1]); ylim([-2, 2]);
-#+end_src
-
-** Save controllers
-#+begin_src matlab :exports none :tangle no
-K_order = order(Khac(1,1));
-
-Kz = c2d(-Khac(1,1)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
-[num, den] = tfdata(Kz, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_Dx.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-#+begin_src matlab :exports none :tangle no
-K_order = order(Khac(2,2));
-
-Kz = c2d(-Khac(2,2)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
-[num, den] = tfdata(Kz, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_Dy.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-#+begin_src matlab :exports none :tangle no
-K_order = order(Khac(3,3));
-
-Kz = c2d(-Khac(3,3)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
-[num, den] = tfdata(Kz, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_Dz.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-#+begin_src matlab :exports none :tangle no
-K_order = order(Khac(4,4));
-
-Kz = c2d(-Khac(4,4)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
-[num, den] = tfdata(Kz, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_Rx.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-#+begin_src matlab :exports none :tangle no
-K_order = order(Khac(5,5));
-
-Kz = c2d(-Khac(5,5)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
-[num, den] = tfdata(Kz, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_Ry.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-** Performances
-2023-08-18_18-33_m0_1rpm_K_cart.mat
-
-* 3DoF Control in Cartesian plane (fixed)
-<<sec:id31_cart_hac_fix>>
-** Matlab Init                                              :noexport:ignore:
-#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
-<<matlab-dir>>
-#+end_src
-
-#+begin_src matlab :exports none :results silent :noweb yes
-<<matlab-init>>
-#+end_src
-
-#+begin_src matlab :tangle no :noweb yes
-<<m-init-path>>
-#+end_src
-
-#+begin_src matlab :eval no :noweb yes
-<<m-init-path-tangle>>
-#+end_src
-
-#+begin_src matlab :noweb yes
-<<m-init-other>>
-#+end_src
-
-** Introduction                                                      :ignore:
-As only Dy, Dz and Ry directions are important, we could only control them.
-This lead to a 3x3 plant that may be more decoupled than the 6x6 plant.
-
-** 3x3 plant in Cartesian plane
-#+begin_src matlab :exports none
-%% Load model
-load('Gm.mat')
-#+end_src
-
-#+begin_src matlab
-%% Jacobian for 3x3 plant
-n_hexapod = initializeNanoHexapod('flex_bot_type', '2dof', ...
-                                  'flex_top_type', '3dof', ...
-                                  'motion_sensor_type', 'plates', ...
-                                  'actuator_type', '2dof');
-
-Jt_inv = pinv(n_hexapod.geometry.J');
-
-J_int_to_X = [0.78740157480315  0.21259842519685  0                  0                  0;
-              0                 0                 0                  0                 -1;
-              0                 0               -13.1233595800525   13.1233595800525    0];
-#+end_src
-
-Compute identified plant in the Cartesian plane:
-#+begin_src matlab
-%% Load Data
-% data_m0 = load(sprintf('%s/dynamics/2023-08-17_10-44_lac_plant_m0_Wz0_interf.mat', mat_dir));
-%% New data after calibrating the Rz-offset
-data_m0 = load(sprintf('%s/dynamics/2023-08-17_16-23_lac_plant_m0_Wz0_interf_Rz_align.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-% Sampling Time [s]
-Ts = 1e-4;
-
-% Hannning Windows
-Nfft = floor(1.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-#+end_src
-
-#+begin_src matlab :exports none
-% And we get the frequency vector
-[~, f] = tfestimate(data_m0.uL1.id_plant, data_m0.uL1.d1, win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-#+begin_src matlab :exports none
-%% HAC Cartesian Plant
-G_hac_m0 = zeros(length(f), 5, 6);
-
-for i_strut = 1:6
-    Di = [data_m0.(sprintf("uL%i", i_strut)).d1 ; data_m0.(sprintf("uL%i", i_strut)).d2 ; data_m0.(sprintf("uL%i", i_strut)).d3 ; data_m0.(sprintf("uL%i", i_strut)).d4 ; data_m0.(sprintf("uL%i", i_strut)).d5]';
-
-    G_hac_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, detrend(Di, 0), win, Noverlap, Nfft, 1/Ts);
-end
-
-G_cart = permute(pagemtimes(J_int_to_X, pagemtimes(permute(G_hac_m0, [2,3,1]), Jt_inv(:,[2,3,5]))), [3,1,2]);
-#+end_src
-
-Compute plant model in the Cartesian plane:
-#+begin_src matlab
-Gm_cart = J_int_to_X*Gm_hac_m0({'d1', 'd2', 'd3', 'd4', 'd5'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'})*Jt_inv(:,[2,3,5]);
-Gm_cart.InputName = {'Fy', 'Fz', 'My'};
-Gm_cart.OutputName = {'Dy', 'Dz', 'Ry'};
-#+end_src
-
-#+begin_important
-Diagonal elements are matching quite well, but off-diagonal elements are very different.
-
-Why so much more coupling than from the model?
-- Is it due to the metrology? The spheres could induce coupling as for instance X motion will also be seen as Z motion.
-  This is especially true if not well centered with the sphere (as seemed to be the case for the lateral interferometers).
-#+end_important
-
-#+begin_src matlab :exports none :results none
-%% 3x3 cartesian plant
-figure;
-tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax11 = nexttile();
-hold on;
-plot(f, abs(G_cart(:,1,1)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'Fy'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('$D_y$'); set(gca, 'XTickLabel',[]);
-title('$F_y$')
-
-ax12 = nexttile();
-hold on;
-plot(f, abs(G_cart(:,1,2)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'Fz'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-title('$F_z$')
-
-ax13 = nexttile();
-hold on;
-plot(f, abs(G_cart(:,1,3)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'My'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-title('$M_y$')
-
-linkaxes([ax11,ax12,ax13],'y');
-ylim([1e-8, 2e-4]);
-
-ax21 = nexttile();
-hold on;
-plot(f, abs(G_cart(:,2,1)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'Fy'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('$F_z$'); set(gca, 'XTickLabel',[]);
-
-ax22 = nexttile();
-hold on;
-plot(f, abs(G_cart(:,2,2)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'Fz'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-
-ax23 = nexttile();
-hold on;
-plot(f, abs(G_cart(:,2,3)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'My'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-
-linkaxes([ax21,ax22,ax23],'y');
-ylim([1e-8, 2e-4]);
-
-ax31 = nexttile();
-hold on;
-plot(f, abs(G_cart(:,3,1)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'Fy'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('$M_y$');
-
-ax32 = nexttile();
-hold on;
-plot(f, abs(G_cart(:,3,2)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'Fz'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-
-ax33 = nexttile();
-hold on;
-plot(f, abs(G_cart(:,3,3)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'My'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-
-linkaxes([ax31,ax32,ax33],'y');
-ylim([1e-9, 1e-2]);
-
-linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x');
-xlim([1, 5e2]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_cart_plant_3x3.pdf', 'width', 'full', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_cart_plant_3x3
-#+caption: 3x3 cartesian plant
-#+RESULTS:
-[[file:figs/id31_cart_plant_3x3.png]]
-
-
-Normalization of outputs:
-#+begin_src matlab
-Gm_cart_normalized = -diag(1./diag(dcgain(Gm_cart)))*Gm_cart;
-Gm_cart_normalized.InputName = {'Fy', 'Fz', 'My'};
-Gm_cart_normalized.OutputName = {'Dy', 'Dz', 'Ry'};
-
-G_cart_normalized = permute(pagemtimes(-diag(1./diag(dcgain(Gm_cart))), permute(G_cart, [2,3,1])), [3,1,2]);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% 3x3 plant
-figure;
-tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax11 = nexttile();
-hold on;
-plot(f, abs(G_cart_normalized(:,1,1)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Dy', 'Fy'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('$D_y$'); set(gca, 'XTickLabel',[]);
-title('$F_y$')
-
-ax12 = nexttile();
-hold on;
-plot(f, abs(G_cart_normalized(:,1,2)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Dy', 'Fz'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-title('$F_z$')
-
-ax13 = nexttile();
-hold on;
-plot(f, abs(G_cart_normalized(:,1,3)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Dy', 'My'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-title('$M_y$')
-
-linkaxes([ax11,ax12,ax13],'y');
-ylim([1e-4, 1e1]);
-
-ax21 = nexttile();
-hold on;
-plot(f, abs(G_cart_normalized(:,2,1)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Dz', 'Fy'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('$F_z$'); set(gca, 'XTickLabel',[]);
-
-ax22 = nexttile();
-hold on;
-plot(f, abs(G_cart_normalized(:,2,2)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Dz', 'Fz'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-
-ax23 = nexttile();
-hold on;
-plot(f, abs(G_cart_normalized(:,2,3)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Dz', 'My'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-
-linkaxes([ax21,ax22,ax23],'y');
-ylim([1e-4, 1e1]);
-
-ax31 = nexttile();
-hold on;
-plot(f, abs(G_cart_normalized(:,3,1)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Ry', 'Fy'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('$M_y$');
-
-ax32 = nexttile();
-hold on;
-plot(f, abs(G_cart_normalized(:,3,2)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Ry', 'Fz'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-
-ax33 = nexttile();
-hold on;
-plot(f, abs(G_cart_normalized(:,3,3)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Ry', 'My'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-
-linkaxes([ax31,ax32,ax33],'y');
-ylim([1e-4, 1e1]);
-
-linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x');
-xlim([1, 5e2]);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% 3x3 plant
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_cart_normalized(:,1,1)));
-plot(f, abs(G_cart_normalized(:,2,2)));
-plot(f, abs(G_cart_normalized(:,3,3)));
-plot(f, abs(G_cart_normalized(:,1,2)), 'color', [0,0,0,0.2]);
-plot(f, abs(G_cart_normalized(:,1,3)), 'color', [0,0,0,0.2]);
-plot(f, abs(G_cart_normalized(:,2,1)), 'color', [0,0,0,0.2]);
-plot(f, abs(G_cart_normalized(:,3,1)), 'color', [0,0,0,0.2]);
-plot(f, abs(G_cart_normalized(:,2,3)), 'color', [0,0,0,0.2]);
-plot(f, abs(G_cart_normalized(:,3,2)), 'color', [0,0,0,0.2]);
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-ylim([1e-4, 1e1]);
-
-ax2 = nexttile();
-hold on;
-plot(f, 180/pi*angle(G_cart_normalized(:,1,1)));
-plot(f, 180/pi*angle(G_cart_normalized(:,2,2)));
-plot(f, 180/pi*angle(G_cart_normalized(:,3,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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-** Controller Design
-*** Dy
-#+begin_src matlab
-%% Wanted crossover
-wc = 2*pi*30;
-
-%% Double Integrator
-H_int = 1/(s + 0.1*2*pi) * ...
-        1/(s + 0.01*2*pi);
-H_int = H_int/abs(evalfr(H_int, 1j*wc));
-
-%% Lead to increase phase margin
-a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
-H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
-a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
-H_lead = H_lead*1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a)));
-
-%% Low Pass filter to increase robustness
-H_lpf = 1/(1 + s/2/pi/300);
-
-%% Notch at the top-plate resonance
-% gm = 0.02;
-% xi = 0.3;
-% wn = 2*pi*665;
-
-% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
-
-%% Gain to have unitary crossover at 30Hz
-[~, i_f] = min(abs(f - wc/2/pi));
-H_gain = -1./abs(G_cart(i_f,1,1));
-
-%% Decentralized HAC
-Khac_Dy = H_gain  * ... % Gain
-          H_int   * ... % Integrator
-          H_lpf   * ... % Low Pass Filter
-          H_lead;       % Integrator
-#+end_src
-
-#+begin_src matlab :exports none
-%% Loop Gain
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_cart(:,1,1).*squeeze(freqresp(Khac_Dy, f, 'Hz'))));
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
-ylim([1e-4, 1e4]);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(G_cart(:,1,1).*squeeze(freqresp(Khac_Dy, f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-*** Dz
-#+begin_src matlab
-%% Wanted crossover
-wc = 2*pi*50;
-
-%% Double Integrator
-H_int = 1/(s + 0.1*2*pi) * ...
-        1/(s + 0.01*2*pi);
-H_int = H_int/abs(evalfr(H_int, 1j*wc));
-
-%% Lead to increase phase margin
-a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
-H_lead = 1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a)));
-a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
-H_lead = H_lead*1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a)));
-
-%% Low Pass filter to increase robustness
-H_lpf = 1/(1 + s/2/pi/300);
-
-%% Notch at the top-plate resonance
-% gm = 0.02;
-% xi = 0.3;
-% wn = 2*pi*665;
-
-% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
-
-%% Gain to have unitary crossover at 30Hz
-[~, i_f] = min(abs(f - wc/2/pi));
-H_gain = -1./abs(G_cart(i_f,2,2));
-
-%% Decentralized HAC
-Khac_Dz = H_gain  * ... % Gain
-          H_int   * ... % Integrator
-          H_lpf   * ... % Low Pass Filter
-          H_lead;       % Integrator
-#+end_src
-
-#+begin_src matlab :exports none
-%% Loop Gain
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_cart(:,2,2).*squeeze(freqresp(Khac_Dz, f, 'Hz'))));
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
-ylim([1e-4, 1e4]);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(G_cart(:,2,2).*squeeze(freqresp(Khac_Dz, f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-*** Ry
-#+begin_src matlab
-%% Wanted crossover
-wc = 2*pi*30;
-
-%% Double Integrator
-H_int = 1/(s + 0.1*2*pi) * ...
-        1/(s + 0.01*2*pi);
-H_int = H_int/abs(evalfr(H_int, 1j*wc));
-
-%% Lead to increase phase margin
-a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
-H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
-a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
-H_lead = H_lead*1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a)));
-
-%% Low Pass filter to increase robustness
-H_lpf = 1/(1 + s/2/pi/300);
-
-%% Notch at the top-plate resonance
-% gm = 0.02;
-% xi = 0.3;
-% wn = 2*pi*665;
-
-% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
-
-%% Gain to have unitary crossover at 30Hz
-[~, i_f] = min(abs(f - wc/2/pi));
-H_gain = -1./abs(G_cart(i_f,3,3));
-
-%% Decentralized HAC
-Khac_Ry = H_gain  * ... % Gain
-          H_int   * ... % Integrator
-          H_lpf   * ... % Low Pass Filter
-          H_lead;       % Integrator
-#+end_src
-
-#+begin_src matlab :exports none
-%% Loop Gain
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_cart(:,3,3).*squeeze(freqresp(Khac_Ry, f, 'Hz'))));
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
-ylim([1e-4, 1e4]);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(G_cart(:,3,3).*squeeze(freqresp(Khac_Ry, f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-*** 3x3 controller
-#+begin_src matlab
-Khac = blkdiag(Khac_Dy, Khac_Dz, Khac_Ry);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% description
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_cart(:,1,1).*squeeze(freqresp(Khac_Dy, f, 'Hz'))), 'DisplayName', '$D_y$');
-plot(f, abs(G_cart(:,2,2).*squeeze(freqresp(Khac_Dz, f, 'Hz'))), 'DisplayName', '$D_z$');
-plot(f, abs(G_cart(:,3,3).*squeeze(freqresp(Khac_Ry, f, 'Hz'))), 'DisplayName', '$R_y$');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
-ylim([1e-4, 1e4]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(G_cart(:,1,1).*squeeze(freqresp(Khac_Dy, f, 'Hz'))));
-plot(f, 180/pi*angle(G_cart(:,2,2).*squeeze(freqresp(Khac_Dz, f, 'Hz'))));
-plot(f, 180/pi*angle(G_cart(:,3,3).*squeeze(freqresp(Khac_Ry, f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/G_cart_loop_gain_diagonal_3dof.pdf', 'width', 'full', 'height', 'tall');
-#+end_src
-
-#+name: fig:G_cart_loop_gain_diagonal_3dof
-#+caption: description
-#+RESULTS:
-[[file:figs/G_cart_loop_gain_diagonal_3dof.png]]
-
-
-** Check Stability
-#+begin_src matlab :exports none
-%% Compute the Eigenvalues of the loop gain
-Ldet = zeros(3, length(f));
-
-% Loop gain
-Lmimo = pagemtimes(permute(G_cart, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
-for i_f = 2:length(f)
-    Ldet(:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
-end
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% description
-figure;
-hold on;
-plot(real(squeeze(Ldet(1,:))), imag(squeeze(Ldet(1,:))), 'k.')
-plot(real(squeeze(Ldet(1,:))), -imag(squeeze(Ldet(1,:))), 'k.')
-for i = 1:3
-    plot(real(squeeze(Ldet(i,:))), imag(squeeze(Ldet(i,:))), 'k.')
-    plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.')
-end
-plot(-1, 0, 'kx');
-hold off;
-set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
-xlabel('Real'); ylabel('Imag');
-xlim([-3, 1]); ylim([-2, 2]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/G_cart_nyquist_diagonal_3dof.pdf', 'width', 'wide', 'height', 'tall');
-#+end_src
-
-#+name: fig:G_cart_nyquist_diagonal_3dof
-#+caption: description
-#+RESULTS:
-[[file:figs/G_cart_nyquist_diagonal_3dof.png]]
-
-** Save controllers
-*** Save Controller
-#+begin_src matlab :exports none :tangle no
-K_order = order(Khac(1,1));
-
-Kz = c2d(-Khac(1,1)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
-[num, den] = tfdata(Kz, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_Dy_3x3.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-#+begin_src matlab :exports none :tangle no
-K_order = order(Khac(2,2));
-
-Kz = c2d(-Khac(2,2)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
-[num, den] = tfdata(Kz, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_Dz_3x3.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-#+begin_src matlab :exports none :tangle no
-K_order = order(Khac(3,3));
-
-Kz = c2d(-Khac(3,3)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
-[num, den] = tfdata(Kz, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_Ry_3x3.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-** Controller Design (normalized)
-#+begin_src matlab
-%% Wanted crossover
-wc = 2*pi*30;
-
-%% Double Integrator
-H_int = 1/(s + 0.1*2*pi) * ...
-        (50*2*pi + s)/(0.01*2*pi + s);
-H_int = H_int/abs(evalfr(H_int, 1j*wc));
-% H_int = wc/s;
-
-%% Lead to increase phase margin
-a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
-H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
-% a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
-% H_lead = H_lead*1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
-
-%% Low Pass filter to increase robustness
-H_lpf = 1/(1 + s/2/pi/200);
-
-%% Notch at the top-plate resonance
-% gm = 0.02;
-% xi = 0.3;
-% wn = 2*pi*665;
-
-% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
-
-%% Gain to have unitary crossover at 30Hz
-[~, i_f] = min(abs(f - wc/2/pi));
-H_gain = 1./abs(G_cart_normalized(i_f, 1, 1));
-
-%% Decentralized HAC
-Khac = H_gain  * ... % Gain
-       H_int   * ... % Integrator
-       H_lpf   * ... % Low Pass Filter
-       H_lead  * ... % Integrator
-       eye(3);            % 6x6 Diagonal
-
-% Khac.InputName = {'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
-% Khac.OutputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
-#+end_src
-
-#+begin_src matlab :exports none
-%% Loop Gain
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-for i = 1:3
-    plot(f, abs(G_cart_normalized(:,i,i).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [colors(i,:), 0.5]);
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
-ylim([1e-4, 1e4]);
-
-ax2 = nexttile;
-hold on;
-for i = 1:3
-    plot(f, 180/pi*angle(G_cart_normalized(:,i,i).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [colors(i,:), 0.5]);
-end
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-yticks(-360:90:360);
-ylim([-180, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-** Verify Stability
-#+begin_src matlab :exports none
-%% Compute the Eigenvalues of the loop gain
-Ldet = zeros(3, length(f));
-
-% Loop gain
-Lmimo = pagemtimes(permute(G_cart_normalized, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
-for i_f = 2:length(f)
-    Ldet(:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
-end
-#+end_src
-
-#+begin_src matlab :exports none
-%% Plot of the eigenvalues of L in the complex plane
-figure;
-hold on;
-plot(real(squeeze(Ldet(1,:))),  imag(squeeze(Ldet(1,:))), 'k.');
-plot(real(squeeze(Ldet(1,:))), -imag(squeeze(Ldet(1,:))), 'k.');
-for i = 1:3
-    plot(real(squeeze(Ldet(i,:))),  imag(squeeze(Ldet(i,:))), 'k.');
-    plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.');
-end
-plot(-1, 0, 'kx', 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
-xlabel('Real'); ylabel('Imag');
-xlim([-3, 1]); ylim([-2, 2]);
-#+end_src
-
-
-** Control Performances
-#+begin_src matlab
-data_cl = load(sprintf('%s/dynamics/2023-08-21_10-36_m0_cart_fixed.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-% Sampling Time [s]
-Ts = 1e-4;
-
-% Hannning Windows
-Nfft = floor(1.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-#+end_src
-
-#+begin_src matlab :exports none
-% And we get the frequency vector
-[S_dx, f] = tfestimate(data_cl.Dx.id_cl, data_cl.Dx.e_dx, win, Noverlap, Nfft, 1/Ts);
-[S_dy, ~] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
-[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
-[S_rx, ~] = tfestimate(data_cl.Rx.id_cl, data_cl.Rx.e_rx, win, Noverlap, Nfft, 1/Ts);
-[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
-[S_rz, ~] = tfestimate(data_cl.Rz.id_cl, data_cl.Rz.e_rz, win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-- [ ] Compare with estimated performances
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile();
-hold on;
-plot(f, abs(S_dy), 'DisplayName', '$D_y$');
-plot(f, abs(S_dz), 'DisplayName', '$D_z$');
-plot(f, abs(S_ry), 'DisplayName', '$R_y$');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('Amplitude');
-ylim([1e-3, 1e1]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
-xlim([1, 1e3]);
-#+end_src
-
-* Complementary Filter Control
-<<sec:id31_cart_hac_complementary_filter>>
-** Matlab Init                                              :noexport:ignore:
-#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
-<<matlab-dir>>
-#+end_src
-
-#+begin_src matlab :exports none :results silent :noweb yes
-<<matlab-init>>
-#+end_src
-
-#+begin_src matlab :tangle no :noweb yes
-<<m-init-path>>
-#+end_src
-
-#+begin_src matlab :eval no :noweb yes
-<<m-init-path-tangle>>
-#+end_src
-
-#+begin_src matlab :noweb yes
-<<m-init-other>>
-#+end_src
-
-** m0
-*** 3x3 plant in Cartesian plane
-#+begin_src matlab :exports none
-%% Load model
-load('Gm.mat')
-#+end_src
-
-#+begin_src matlab
-%% Jacobian for 3x3 plant
-n_hexapod = initializeNanoHexapod('flex_bot_type', '2dof', ...
-                                  'flex_top_type', '3dof', ...
-                                  'motion_sensor_type', 'plates', ...
-                                  'actuator_type', '2dof');
-
-Jt_inv = pinv(n_hexapod.geometry.J');
-
-J_int_to_X = [0.78740157480315  0.21259842519685  0                  0                  0;
-              0                 0                 0                  0                 -1;
-              0                 0               -13.1233595800525   13.1233595800525    0];
-#+end_src
-
-Compute identified plant in the Cartesian plane:
-#+begin_src matlab
-%% New data after calibrating the Rz-offset
-data_m0 = load(sprintf('%s/dynamics/2023-08-21_13-32_damp_plant_m0.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-% Sampling Time [s]
-Ts = 1e-4;
-
-% Hannning Windows
-Nfft = floor(1.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-#+end_src
-
-#+begin_src matlab :exports none
-% And we get the frequency vector
-[~, f] = tfestimate(data_m0.uL1.id_plant, data_m0.uL1.d1, win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-#+begin_src matlab :exports none
-%% HAC Cartesian Plant
-G_hac_m0 = zeros(length(f), 5, 6);
-
-for i_strut = 1:6
-    Di = [data_m0.(sprintf("uL%i", i_strut)).d1 ; data_m0.(sprintf("uL%i", i_strut)).d2 ; data_m0.(sprintf("uL%i", i_strut)).d3 ; data_m0.(sprintf("uL%i", i_strut)).d4 ; data_m0.(sprintf("uL%i", i_strut)).d5]';
-
-    G_hac_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, detrend(Di, 0), win, Noverlap, Nfft, 1/Ts);
-end
-
-G_cart = permute(pagemtimes(J_int_to_X, pagemtimes(permute(G_hac_m0, [2,3,1]), Jt_inv(:,[2,3,5]))), [3,1,2]);
-#+end_src
-
-Compute plant model in the Cartesian plane:
-#+begin_src matlab
-Gm_cart = J_int_to_X*Gm_hac_m0({'d1', 'd2', 'd3', 'd4', 'd5'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'})*Jt_inv(:,[2,3,5]);
-Gm_cart.InputName = {'Fy', 'Fz', 'My'};
-Gm_cart.OutputName = {'Dy', 'Dz', 'Ry'};
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% 3x3 plant
-figure;
-tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax11 = nexttile();
-hold on;
-plot(f, abs(G_cart(:,1,1)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'Fy'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('$D_y$'); set(gca, 'XTickLabel',[]);
-title('$F_y$')
-
-ax12 = nexttile();
-hold on;
-plot(f, abs(G_cart(:,1,2)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'Fz'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-title('$F_z$')
-
-ax13 = nexttile();
-hold on;
-plot(f, abs(G_cart(:,1,3)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'My'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-title('$M_y$')
-
-linkaxes([ax11,ax12,ax13],'y');
-ylim([1e-8, 2e-4]);
-
-ax21 = nexttile();
-hold on;
-plot(f, abs(G_cart(:,2,1)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'Fy'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('$F_z$'); set(gca, 'XTickLabel',[]);
-
-ax22 = nexttile();
-hold on;
-plot(f, abs(G_cart(:,2,2)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'Fz'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-
-ax23 = nexttile();
-hold on;
-plot(f, abs(G_cart(:,2,3)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'My'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-
-linkaxes([ax21,ax22,ax23],'y');
-ylim([1e-8, 2e-4]);
-
-ax31 = nexttile();
-hold on;
-plot(f, abs(G_cart(:,3,1)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'Fy'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('$M_y$');
-
-ax32 = nexttile();
-hold on;
-plot(f, abs(G_cart(:,3,2)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'Fz'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-
-ax33 = nexttile();
-hold on;
-plot(f, abs(G_cart(:,3,3)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'My'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-
-linkaxes([ax31,ax32,ax33],'y');
-ylim([1e-9, 1e-2]);
-
-linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x');
-xlim([1, 5e2]);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% 3x3 plant
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile();
-hold on;
-plot(f, abs(G_cart(:,1,1)), 'color', [colors(1,:)]);
-plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'Fy'), freqs, 'Hz'))), '--', 'color', [colors(1,:)]);
-plot(f, abs(G_cart(:,2,2)), 'color', [colors(2,:)]);
-plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'Fz'), freqs, 'Hz'))), '--', 'color', [colors(2,:)]);
-plot(f, abs(G_cart(:,3,3)), 'color', [colors(3,:)]);
-plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'My'), freqs, 'Hz'))), '--', 'color', [colors(3,:)]);
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('Magnitude');
-
-ylim([1e-9, 1e-2]);
-xlim([1, 5e2]);
-#+end_src
-
-*** Plant Invert
-Reduce model size
-#+begin_src matlab
-Gm_cart_dy = flipRphZeros(balred(-Gm_cart('Dy', 'Fy'), 10));
-Gm_cart_dz = flipRphZeros(balred(-Gm_cart('Dz', 'Fz'), 10));
-Gm_cart_ry = flipRphZeros(balred(-Gm_cart('Ry', 'My'), 10));
-#+end_src
-
-Add first resonance
-#+begin_src matlab :eval no
-% gm = 200;
-% xi = 0.003;
-% wn = 2*pi*670;
-
-% Gm_cart_dy = Gm_cart_dy*(s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
-
-% gm = 200;
-% xi = 0.003;
-% wn = 2*pi*1086;
-
-% Gm_cart_dz = Gm_cart_dz*(s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
-
-% gm = 200;
-% xi = 0.003;
-% wn = 2*pi*670;
-
-% Gm_cart_ry = Gm_cart_ry*(s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_cart(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
-plot(f, abs(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
-plot(f, abs(G_cart(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
-plot(f, abs(squeeze(freqresp(Gm_cart_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
-plot(f, abs(G_cart(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
-plot(f, abs(squeeze(freqresp(Gm_cart_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(G_cart(:,1,1)), 'color', [colors(1,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Gm_cart_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
-plot(f, 180/pi*angle(G_cart(:,2,2)), 'color', [colors(2,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Gm_cart_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
-plot(f, 180/pi*angle(G_cart(:,3,3)), 'color', [colors(3,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Gm_cart_ry, f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_comp_cart_plant_reduced_model.pdf', 'width', 'wide', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_comp_cart_plant_reduced_model
-#+caption: Comparaison of the measured direct terms and the reduced order models
-#+RESULTS:
-[[file:figs/id31_comp_cart_plant_reduced_model.png]]
-
-
-Invert and make realizable
-#+begin_src matlab
-Gm_cart_dy_inv = inv(Gm_cart_dy);
-Gm_cart_dz_inv = inv(Gm_cart_dz);
-Gm_cart_ry_inv = inv(Gm_cart_ry);
-#+end_src
-
-#+begin_src matlab
-isstable(Gm_cart_dy_inv)
-isstable(Gm_cart_dz_inv)
-isstable(Gm_cart_ry_inv)
-#+end_src
-
-*** Save Plant Inverse
-#+begin_src matlab :exports none :tangle no
-K = -Gm_cart_dy_inv;
-K_order = order(K);
-
-Kz = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
-[num, den] = tfdata(Kz, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dy_inv.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-#+begin_src matlab :exports none :tangle no
-K = -Gm_cart_dz_inv;
-K_order = order(K);
-
-Kz = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
-[num, den] = tfdata(Kz, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dz_inv.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-#+begin_src matlab :exports none :tangle no
-K = -Gm_cart_ry_inv;
-K_order = order(K);
-
-Kz = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
-[num, den] = tfdata(Kz, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_ry_inv.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-*** Control Performances
-**** 5Hz
-#+begin_src matlab
-data_cl = load(sprintf('%s/dynamics/2023-08-21_10-59_m0_cf_5Hz.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-% Sampling Time [s]
-Ts = 1e-4;
-
-% Hannning Windows
-Nfft = floor(1.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-#+end_src
-
-#+begin_src matlab :exports none
-% And we get the frequency vector
-[S_dx, f] = tfestimate(data_cl.Dx.id_cl, data_cl.Dx.e_dx, win, Noverlap, Nfft, 1/Ts);
-[S_dy, ~] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
-[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
-[S_rx, ~] = tfestimate(data_cl.Rx.id_cl, data_cl.Rx.e_rx, win, Noverlap, Nfft, 1/Ts);
-[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
-[S_rz, ~] = tfestimate(data_cl.Rz.id_cl, data_cl.Rz.e_rz, win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-- [ ] Compare with estimated performances
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile();
-hold on;
-plot(f, abs(S_dy), 'DisplayName', '$D_y$');
-plot(f, abs(S_dz), 'DisplayName', '$D_z$');
-plot(f, abs(S_ry), 'DisplayName', '$R_y$');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('Amplitude');
-ylim([1e-3, 1e1]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
-xlim([1, 1e3]);
-#+end_src
-
-**** 20Hz
-#+begin_src matlab
-data_cl = load(sprintf('%s/dynamics/2023-08-21_11-04_m0_cf_20Hz.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-% Sampling Time [s]
-Ts = 1e-4;
-
-% Hannning Windows
-Nfft = floor(1.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-#+end_src
-
-#+begin_src matlab :exports none
-% And we get the frequency vector
-[S_dx, f] = tfestimate(data_cl.Dx.id_cl, data_cl.Dx.e_dx, win, Noverlap, Nfft, 1/Ts);
-[S_dy, ~] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
-[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
-[S_rx, ~] = tfestimate(data_cl.Rx.id_cl, data_cl.Rx.e_rx, win, Noverlap, Nfft, 1/Ts);
-[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
-[S_rz, ~] = tfestimate(data_cl.Rz.id_cl, data_cl.Rz.e_rz, win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-- [ ] Compare with estimated performances
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile();
-hold on;
-plot(f, abs(S_dy), 'DisplayName', '$D_y$');
-plot(f, abs(S_dz), 'DisplayName', '$D_z$');
-plot(f, abs(S_ry), 'DisplayName', '$R_y$');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('Amplitude');
-ylim([1e-3, 1e1]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
-xlim([1, 1e3]);
-#+end_src
-
-**** Different bandwidth for different directions
-#+begin_src matlab
-data_cl = load(sprintf('%s/dynamics/2023-08-21_11-16_m0_cf_different.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-% Sampling Time [s]
-Ts = 1e-4;
-
-% Hannning Windows
-Nfft = floor(1.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-#+end_src
-
-#+begin_src matlab :exports none
-% And we get the frequency vector
-[S_dx, f] = tfestimate(data_cl.Dx.id_cl, data_cl.Dx.e_dx, win, Noverlap, Nfft, 1/Ts);
-[S_dy, ~] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
-[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
-[S_rx, ~] = tfestimate(data_cl.Rx.id_cl, data_cl.Rx.e_rx, win, Noverlap, Nfft, 1/Ts);
-[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
-[S_rz, ~] = tfestimate(data_cl.Rz.id_cl, data_cl.Rz.e_rz, win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-- [ ] Compare with estimated performances
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile();
-hold on;
-plot(f, abs(S_dy), 'DisplayName', '$D_y$');
-plot(f, abs(S_dz), 'DisplayName', '$D_z$');
-plot(f, abs(S_ry), 'DisplayName', '$R_y$');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('Amplitude');
-ylim([1e-3, 1e1]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
-xlim([1, 1e3]);
-#+end_src
-
-
-**** Dz 25Hz
-#+begin_src matlab
-data_cl = load(sprintf('%s/dynamics/', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-% Sampling Time [s]
-Ts = 1e-4;
-
-% Hannning Windows
-Nfft = floor(1.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-#+end_src
-
-#+begin_src matlab :exports none
-% And we get the frequency vector
-[S_dx, f] = tfestimate(data_cl.Dx.id_cl, data_cl.Dx.e_dx, win, Noverlap, Nfft, 1/Ts);
-[S_dy, ~] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
-[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
-[S_rx, ~] = tfestimate(data_cl.Rx.id_cl, data_cl.Rx.e_rx, win, Noverlap, Nfft, 1/Ts);
-[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
-[S_rz, ~] = tfestimate(data_cl.Rz.id_cl, data_cl.Rz.e_rz, win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-- [ ] Compare with estimated performances
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile();
-hold on;
-plot(f, abs(S_dy), 'DisplayName', '$D_y$');
-plot(f, abs(S_dz), 'DisplayName', '$D_z$');
-plot(f, abs(S_ry), 'DisplayName', '$R_y$');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('Amplitude');
-ylim([1e-3, 1e1]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
-xlim([1, 1e3]);
-#+end_src
-
-
-*** Better plant invert
-**** Dy
-#+begin_src matlab :exports none
-opts = struct();
-
-opts.stable = 1;    % Enforce stable poles
-opts.asymp = 1;     % Force D matrix to be null
-opts.relax = 1;     % Use vector fitting with relaxed non-triviality constraint
-opts.skip_pole = 0; % Do NOT skip pole identification
-opts.skip_res = 0;  % Do NOT skip identification of residues (C,D,E)
-opts.cmplx_ss = 0;  % Create real state space model with block diagonal A
-
-opts.spy1 = 0;      % No plotting for first stage of vector fitting
-opts.spy2 = 0;      % Create magnitude plot for fitting of f(s)
-
-%% We define the number of iteration.
-Niter = 100;
-#+end_src
-
-#+begin_src matlab :exports none
-N = 9; %Order of approximation
-
-%Complex conjugate pairs, linearly spaced:
-bet=linspace(f(1),f(end),N/2);
-poles=[];
-for n=1:length(bet)
-  alf=-bet(n)*1e-1;
-  poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
-end
-#+end_src
-
-#+begin_src matlab
-%% Estimate resonance frequency and damping
-for iter =1:Niter
-    [G_est, poles, ~, frf_est] = vectfit3(G_cart(f>2&f<800,1,1).', 1i*2*pi*f(f>2&f<800)', poles, 1./(f(f>2&f<800))', opts);
-end
-
-Gfit_dy = ss(G_est.A, G_est.B, G_est.C, G_est.D);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_cart(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
-plot(f, abs(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(G_cart(:,1,1)), 'color', [colors(1,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dy, f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-Stable Inverse
-#+begin_src matlab
-Ginv_dy = inv(-flipRphZeros(Gfit_dy))/(1 + s/2/pi/1e3);
-isstable(Ginv_dy)
-#+end_src
-
-**** Dz
-#+begin_src matlab :exports none
-N = 9; %Order of approximation
-
-%Complex conjugate pairs, linearly spaced:
-bet=linspace(f(1),f(end),N/2);
-poles=[];
-for n=1:length(bet)
-  alf=-bet(n)*1e-1;
-  poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
-end
-#+end_src
-
-#+begin_src matlab
-%% Estimate resonance frequency and damping
-for iter =1:Niter
-    [G_est, poles, ~, frf_est] = vectfit3(G_cart(f>2&f<1500,2,2).', 1i*2*pi*f(f>2&f<1500)', poles, 1./(f(f>2&f<1500))', opts);
-end
-
-Gfit_dz = ss(G_est.A, G_est.B, G_est.C, G_est.D);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_cart(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
-plot(f, abs(squeeze(freqresp(Gfit_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(G_cart(:,2,2)), 'color', [colors(2,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dz, f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-Stable Inverse
-#+begin_src matlab
-Ginv_dz = inv(-flipRphZeros(Gfit_dz))/(1 + s/2/pi/1e3);
-isstable(Ginv_dz)
-#+end_src
-
-**** Ry
-#+begin_src matlab :exports none
-N = 9; %Order of approximation
-
-%Complex conjugate pairs, linearly spaced:
-bet=linspace(f(1),f(end),N/2);
-poles=[];
-for n=1:length(bet)
-  alf=-bet(n)*1e-1;
-  poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
-end
-#+end_src
-
-#+begin_src matlab
-%% Estimate resonance frequency and damping
-for iter =1:Niter
-    [G_est, poles, ~, frf_est] = vectfit3(G_cart(f>2&f<800,3,3).', 1i*2*pi*f(f>2&f<800)', poles, 1./(f(f>2&f<800))', opts);
-end
-
-Gfit_ry = ss(G_est.A, G_est.B, G_est.C, G_est.D);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_cart(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
-plot(f, abs(squeeze(freqresp(Gfit_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(G_cart(:,3,3)), 'color', [colors(3,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Gfit_ry, f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-Stable Inverse
-#+begin_src matlab
-Ginv_ry = inv(flipRphZeros(Gfit_ry))/(1 + s/2/pi/1e3);
-isstable(Ginv_ry)
-#+end_src
-
-**** Compare Invert plants
-
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, 1./abs(G_cart(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
-plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
-plot(f, 1./abs(G_cart(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
-plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
-plot(f, 1./abs(G_cart(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
-plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(1./G_cart(:,1,1)), 'color', [colors(1,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
-plot(f, 180/pi*angle(1./G_cart(:,2,2)), 'color', [colors(2,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
-plot(f, 180/pi*angle(1./G_cart(:,3,3)), 'color', [colors(3,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz')).*abs(G_cart(:,1,1))), '-', 'DisplayName', '$D_y$');
-plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz')).*abs(G_cart(:,2,2))), '-', 'DisplayName', '$D_z$');
-plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz')).*abs(G_cart(:,3,3))), '-', 'DisplayName', '$R_y$');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz')).*G_cart(:,1,1)), '-');
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz')).*G_cart(:,2,2)), '-');
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz')).*G_cart(:,3,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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-**** Save plant inverse
-#+begin_src matlab :exports none :tangle no
-K = -Ginv_dy;
-K_order = order(K);
-
-Kz_dy = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
-[num, den] = tfdata(Kz_dy, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dy_inv_fit.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-#+begin_src matlab :exports none :tangle no
-K = -Ginv_dz;
-K_order = order(K);
-
-Kz_dz = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
-[num, den] = tfdata(Kz_dz, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dz_inv_fit.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-#+begin_src matlab :exports none :tangle no
-K = -Ginv_ry;
-K_order = order(K);
-
-Kz_ry = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
-[num, den] = tfdata(Kz_ry, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_ry_inv_fit.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-**** Compare Digital Invert plants
-
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, 1./abs(G_cart(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
-plot(f, abs(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
-plot(f, 1./abs(G_cart(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
-plot(f, abs(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
-plot(f, 1./abs(G_cart(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
-plot(f, abs(squeeze(freqresp(Kz_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(1./G_cart(:,1,1)), 'color', [colors(1,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
-plot(f, 180/pi*angle(1./G_cart(:,2,2)), 'color', [colors(2,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
-plot(f, 180/pi*angle(1./G_cart(:,3,3)), 'color', [colors(3,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Kz_ry, f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz')).*abs(G_cart(:,1,1))), '-', 'DisplayName', '$D_y$');
-plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz')).*abs(G_cart(:,2,2))), '-', 'DisplayName', '$D_z$');
-plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz')).*abs(G_cart(:,3,3))), '-', 'DisplayName', '$R_y$');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz')).*G_cart(:,1,1)), '-');
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz')).*G_cart(:,2,2)), '-');
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz')).*G_cart(:,3,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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-*** Control Performances
-**** Better plant invert
-#+begin_src matlab
-data_cl = load(sprintf('%s/dynamics/2023-08-21_13-36_m0_cf_inv_fit.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-% Sampling Time [s]
-Ts = 1e-4;
-
-% Hannning Windows
-Nfft = floor(1.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-#+end_src
-
-#+begin_src matlab :exports none
-% And we get the frequency vector
-[S_dy, f] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
-[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
-[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile();
-hold on;
-plot(f, abs(S_dy), 'DisplayName', '$D_y$');
-plot(f, abs(S_dz), 'DisplayName', '$D_z$');
-plot(f, abs(S_ry), 'DisplayName', '$R_y$');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('Amplitude');
-ylim([1e-3, 1e1]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
-xlim([1, 1e3]);
-#+end_src
-
-*** Scans with good controller
-**** 1rpm
-1RPM scans are performed for all the masses with the same controller.
-
-#+begin_src matlab
-data_m0 = load(sprintf("%s/scans/2023-08-21_14-28_m0_1rpm_K_cf.mat", mat_dir));
-data_m0.time = Ts*[0:length(data_m0.Rz)-1];
-#+end_src
-
-#+begin_src matlab
-%% Compute RMS values while in closed-loop
-data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0));
-data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0));
-data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0));
-data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0));
-data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0));
-#+end_src
-
-#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ...
-              {'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f ');
-#+end_src
-
-#+RESULTS:
-|    | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] |
-|----+---------+---------+---------+-----------+-----------|
-| m0 |     796 |      20 |       8 |      8209 |        73 |
-
-#+begin_src matlab :exports none :results none
-%% description
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile;
-hold on;
-plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int);
-hold off;
-axis square
-xlabel("Y motion [$nm$]");
-ylabel("Z motion [$nm$]");
-#+end_src
-
-
-**** 30rpm
-1RPM scans are performed for all the masses with the same controller.
-
-#+begin_src matlab
-data_m0 = load(sprintf("%s/scans/2023-08-21_14-33_m0_30rpm_cf_control.mat", mat_dir));
-data_m0.time = Ts*[0:length(data_m0.Rz)-1];
-#+end_src
-
-#+begin_src matlab
-%% Compute RMS values while in closed-loop
-data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0));
-data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0));
-data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0));
-data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0));
-data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0));
-#+end_src
-
-#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ...
-              {'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f ');
-#+end_src
-
-#+RESULTS:
-|    | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] |
-|----+---------+---------+---------+-----------+-----------|
-| m0 |     820 |      39 |      13 |      7790 |       156 |
-
-#+begin_src matlab :exports none :results none
-%% description
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile;
-hold on;
-plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int);
-hold off;
-axis square
-xlabel("Y motion [$nm$]");
-ylabel("Z motion [$nm$]");
-#+end_src
-
-** m1
-*** 3x3 plant in Cartesian plane
-#+begin_src matlab :exports none
-%% Load model
-load('Gm.mat')
-#+end_src
-
-#+begin_src matlab
-%% Jacobian for 3x3 plant
-n_hexapod = initializeNanoHexapod('flex_bot_type', '2dof', ...
-                                  'flex_top_type', '3dof', ...
-                                  'motion_sensor_type', 'plates', ...
-                                  'actuator_type', '2dof');
-
-Jt_inv = pinv(n_hexapod.geometry.J');
-
-J_int_to_X = [0.78740157480315  0.21259842519685  0                  0                  0;
-              0                 0                 0                  0                 -1;
-              0                 0               -13.1233595800525   13.1233595800525    0];
-#+end_src
-
-Compute identified plant in the Cartesian plane:
-#+begin_src matlab
-%% New data after calibrating the Rz-offset
-data_m1 = load(sprintf('%s/dynamics/2023-08-21_19-05_damp_plant_m1_new_Rz.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-% Sampling Time [s]
-Ts = 1e-4;
-
-% Hannning Windows
-Nfft = floor(1.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-#+end_src
-
-#+begin_src matlab :exports none
-% And we get the frequency vector
-[~, f] = tfestimate(data_m1.uL1.id_plant, data_m1.uL1.d1, win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-#+begin_src matlab :exports none
-%% HAC Cartesian Plant
-G_hac_m1 = zeros(length(f), 5, 6);
-
-for i_strut = 1:6
-    Di = [data_m1.(sprintf("uL%i", i_strut)).d1 ; data_m1.(sprintf("uL%i", i_strut)).d2 ; data_m1.(sprintf("uL%i", i_strut)).d3 ; data_m1.(sprintf("uL%i", i_strut)).d4 ; data_m1.(sprintf("uL%i", i_strut)).d5]';
-
-    G_hac_m1(:,:,i_strut) = tfestimate(data_m1.(sprintf("uL%i", i_strut)).id_plant, detrend(Di, 0), win, Noverlap, Nfft, 1/Ts);
-end
-
-G_cart_m1 = permute(pagemtimes(J_int_to_X, pagemtimes(permute(G_hac_m1, [2,3,1]), Jt_inv(:,[2,3,5]))), [3,1,2]);
-#+end_src
-
-Compute plant model in the Cartesian plane:
-#+begin_src matlab
-Gm_cart_m1 = J_int_to_X*Gm_hac_m1({'d1', 'd2', 'd3', 'd4', 'd5'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'})*Jt_inv(:,[2,3,5]);
-Gm_cart_m1.InputName = {'Fy', 'Fz', 'My'};
-Gm_cart_m1.OutputName = {'Dy', 'Dz', 'Ry'};
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% 3x3 plant
-figure;
-tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax11 = nexttile();
-hold on;
-plot(f, abs(G_cart_m1(:,1,1)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Dy', 'Fy'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('$D_y$'); set(gca, 'XTickLabel',[]);
-title('$F_y$')
-
-ax12 = nexttile();
-hold on;
-plot(f, abs(G_cart_m1(:,1,2)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Dy', 'Fz'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-title('$F_z$')
-
-ax13 = nexttile();
-hold on;
-plot(f, abs(G_cart_m1(:,1,3)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Dy', 'My'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-title('$M_y$')
-
-linkaxes([ax11,ax12,ax13],'y');
-ylim([1e-8, 2e-4]);
-
-ax21 = nexttile();
-hold on;
-plot(f, abs(G_cart_m1(:,2,1)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Dz', 'Fy'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('$F_z$'); set(gca, 'XTickLabel',[]);
-
-ax22 = nexttile();
-hold on;
-plot(f, abs(G_cart_m1(:,2,2)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Dz', 'Fz'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-
-ax23 = nexttile();
-hold on;
-plot(f, abs(G_cart_m1(:,2,3)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Dz', 'My'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-
-linkaxes([ax21,ax22,ax23],'y');
-ylim([1e-8, 2e-4]);
-
-ax31 = nexttile();
-hold on;
-plot(f, abs(G_cart_m1(:,3,1)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Ry', 'Fy'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('$M_y$');
-
-ax32 = nexttile();
-hold on;
-plot(f, abs(G_cart_m1(:,3,2)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Ry', 'Fz'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-
-ax33 = nexttile();
-hold on;
-plot(f, abs(G_cart_m1(:,3,3)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Ry', 'My'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-
-linkaxes([ax31,ax32,ax33],'y');
-ylim([1e-9, 1e-2]);
-
-linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x');
-xlim([1, 5e2]);
-#+end_src
-
-Normalization of outputs:
-#+begin_src matlab
-Gm_cart_m1_normalized = -diag(1./diag(dcgain(Gm_cart_m1)))*Gm_cart_m1;
-Gm_cart_m1_normalized.InputName = {'Fy', 'Fz', 'My'};
-Gm_cart_m1_normalized.OutputName = {'Dy', 'Dz', 'Ry'};
-
-G_cart_m1_normalized = permute(pagemtimes(-diag(1./diag(dcgain(Gm_cart_m1))), permute(G_cart_m1, [2,3,1])), [3,1,2]);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% 3x3 plant
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_cart_m1_normalized(:,1,1)));
-plot(f, abs(G_cart_m1_normalized(:,2,2)));
-plot(f, abs(G_cart_m1_normalized(:,3,3)));
-plot(f, abs(G_cart_m1_normalized(:,1,2)), 'color', [0,0,0,0.2]);
-plot(f, abs(G_cart_m1_normalized(:,1,3)), 'color', [0,0,0,0.2]);
-plot(f, abs(G_cart_m1_normalized(:,2,1)), 'color', [0,0,0,0.2]);
-plot(f, abs(G_cart_m1_normalized(:,3,1)), 'color', [0,0,0,0.2]);
-plot(f, abs(G_cart_m1_normalized(:,2,3)), 'color', [0,0,0,0.2]);
-plot(f, abs(G_cart_m1_normalized(:,3,2)), 'color', [0,0,0,0.2]);
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-ylim([1e-4, 1e1]);
-
-ax2 = nexttile();
-hold on;
-plot(f, 180/pi*angle(G_cart_m1_normalized(:,1,1)));
-plot(f, 180/pi*angle(G_cart_m1_normalized(:,2,2)));
-plot(f, 180/pi*angle(G_cart_m1_normalized(:,3,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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-*** Better plant invert
-#+begin_src matlab :exports none
-opts = struct();
-
-opts.stable = 1;    % Enforce stable poles
-opts.asymp = 1;     % Force D matrix to be null
-opts.relax = 1;     % Use vector fitting with relaxed non-triviality constraint
-opts.skip_pole = 0; % Do NOT skip pole identification
-opts.skip_res = 0;  % Do NOT skip identification of residues (C,D,E)
-opts.cmplx_ss = 0;  % Create real state space model with block diagonal A
-
-opts.spy1 = 0;      % No plotting for first stage of vector fitting
-opts.spy2 = 0;      % Create magnitude plot for fitting of f(s)
-
-%% We define the number of iteration.
-Niter = 100;
-#+end_src
-
-#+begin_src matlab
-N = 9; %Order of approximation
-#+end_src
-
-**** Dy
-#+begin_src matlab :exports none
-%Complex conjugate pairs, linearly spaced:
-bet=linspace(f(1),f(end),N/2);
-poles=[];
-for n=1:length(bet)
-  alf=-bet(n)*1e-2;
-  poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
-end
-#+end_src
-
-#+begin_src matlab
-%% Estimate resonance frequency and damping
-for iter =1:Niter
-    [G_est, poles, ~, frf_est] = vectfit3(G_cart_m1(f>1&f<1500,1,1).', 1i*2*pi*f(f>1&f<1500)', poles, 1./sqrt(f(f>1&f<1500))', opts);
-end
-
-Gfit_dy = ss(G_est.A, G_est.B, G_est.C, G_est.D);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_cart_m1(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
-plot(f, abs(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(G_cart_m1(:,1,1)), 'color', [colors(1,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dy, f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-Stable Inverse
-#+begin_src matlab
-Ginv_dy = inv(flipRphZeros(Gfit_dy))/(1 + s/2/pi/1e3);
-isstable(Ginv_dy)
-#+end_src
-
-**** Dz
-#+begin_src matlab :exports none
-%Complex conjugate pairs, linearly spaced:
-bet=linspace(f(1),f(end),N/2);
-poles=[];
-for n=1:length(bet)
-  alf=-bet(n)*1e-1;
-  poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
-end
-#+end_src
-
-#+begin_src matlab
-%% Estimate resonance frequency and damping
-for iter =1:Niter
-    [G_est, poles, ~, frf_est] = vectfit3(G_cart_m1(f>2&f<1500,2,2).', 1i*2*pi*f(f>2&f<1500)', poles, 1./sqrt(f(f>2&f<1500))', opts);
-end
-
-Gfit_dz = ss(G_est.A, G_est.B, G_est.C, G_est.D);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_cart_m1(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
-plot(f, abs(squeeze(freqresp(Gfit_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(G_cart_m1(:,2,2)), 'color', [colors(2,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dz, f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-Stable Inverse
-#+begin_src matlab
-Ginv_dz = inv(-flipRphZeros(Gfit_dz))/(1 + s/2/pi/1e3);
-isstable(Ginv_dz)
-#+end_src
-
-**** Ry
-#+begin_src matlab :exports none
-%Complex conjugate pairs, linearly spaced:
-bet=linspace(f(1),f(end),N/2);
-poles=[];
-for n=1:length(bet)
-  alf=-bet(n)*1e-1;
-  poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
-end
-#+end_src
-
-#+begin_src matlab
-%% Estimate resonance frequency and damping
-for iter =1:Niter
-    [G_est, poles, ~, frf_est] = vectfit3(G_cart_m1(f>2&f<800,3,3).', 1i*2*pi*f(f>2&f<800)', poles, 1./(f(f>2&f<800))', opts);
-end
-
-Gfit_ry = ss(G_est.A, G_est.B, G_est.C, G_est.D);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_cart_m1(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
-plot(f, abs(squeeze(freqresp(Gfit_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(G_cart_m1(:,3,3)), 'color', [colors(3,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Gfit_ry, f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-Stable Inverse
-#+begin_src matlab
-Ginv_ry = inv(flipRphZeros(Gfit_ry))/(1 + s/2/pi/1e3);
-isstable(Ginv_ry)
-#+end_src
-
-**** Compare Invert plants
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, 1./abs(G_cart_m1(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
-plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
-plot(f, 1./abs(G_cart_m1(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
-plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
-plot(f, 1./abs(G_cart_m1(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
-plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(1./G_cart_m1(:,1,1)), 'color', [colors(1,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
-plot(f, 180/pi*angle(1./G_cart_m1(:,2,2)), 'color', [colors(2,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
-plot(f, 180/pi*angle(1./G_cart_m1(:,3,3)), 'color', [colors(3,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz')).*abs(G_cart_m1(:,1,1))), '-', 'DisplayName', '$D_y$');
-plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz')).*abs(G_cart_m1(:,2,2))), '-', 'DisplayName', '$D_z$');
-plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz')).*abs(G_cart_m1(:,3,3))), '-', 'DisplayName', '$R_y$');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz')).*G_cart_m1(:,1,1)), '-');
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz')).*G_cart_m1(:,2,2)), '-');
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz')).*G_cart_m1(:,3,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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-**** Save plant inverse
-#+begin_src matlab :exports none :tangle no
-K = -Ginv_dy;
-K_order = order(K);
-
-Kz_dy = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
-[num, den] = tfdata(Kz_dy, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dy_inv_m1.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-#+begin_src matlab :exports none :tangle no
-K = -Ginv_dz;
-K_order = order(K);
-
-Kz_dz = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
-[num, den] = tfdata(Kz_dz, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dz_inv_m1.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-#+begin_src matlab :exports none :tangle no
-K = -Ginv_ry;
-K_order = order(K);
-
-Kz_ry = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
-[num, den] = tfdata(Kz_ry, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_ry_inv_m1.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-**** Compare Digital Invert plants
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, 1./abs(G_cart_m1(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
-plot(f, abs(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
-plot(f, 1./abs(G_cart_m1(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
-plot(f, abs(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
-plot(f, 1./abs(G_cart_m1(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
-plot(f, abs(squeeze(freqresp(Kz_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(1./G_cart_m1(:,1,1)), 'color', [colors(1,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
-plot(f, 180/pi*angle(1./G_cart_m1(:,2,2)), 'color', [colors(2,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
-plot(f, 180/pi*angle(1./G_cart_m1(:,3,3)), 'color', [colors(3,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Kz_ry, f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-*** TODO Control Performances
-**** Better plant invert
-#+begin_src matlab
-data_cl = load(sprintf('%s/dynamics/2023-08-21_13-36_m0_cf_inv_fit.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-% Sampling Time [s]
-Ts = 1e-4;
-
-% Hannning Windows
-Nfft = floor(1.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-#+end_src
-
-#+begin_src matlab :exports none
-% And we get the frequency vector
-[S_dy, f] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
-[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
-[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile();
-hold on;
-plot(f, abs(S_dy), 'DisplayName', '$D_y$');
-plot(f, abs(S_dz), 'DisplayName', '$D_z$');
-plot(f, abs(S_ry), 'DisplayName', '$R_y$');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('Amplitude');
-ylim([1e-3, 1e1]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
-xlim([1, 1e3]);
-#+end_src
-
-*** TODO Scans with good controller
-**** 1rpm
-1RPM scans are performed for all the masses with the same controller.
-
-#+begin_src matlab
-data_m0 = load(sprintf("%s/scans/2023-08-21_14-28_m0_1rpm_K_cf.mat", mat_dir));
-data_m0.time = Ts*[0:length(data_m0.Rz)-1];
-#+end_src
-
-#+begin_src matlab
-%% Compute RMS values while in closed-loop
-data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0));
-data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0));
-data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0));
-data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0));
-data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0));
-#+end_src
-
-#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ...
-              {'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f ');
-#+end_src
-
-#+RESULTS:
-|    | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] |
-|----+---------+---------+---------+-----------+-----------|
-| m0 |     796 |      20 |       8 |      8209 |        73 |
-
-#+begin_src matlab :exports none :results none
-%% description
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile;
-hold on;
-plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int);
-hold off;
-axis square
-xlabel("Y motion [$nm$]");
-ylabel("Z motion [$nm$]");
-#+end_src
-
-
-**** 30rpm
-1RPM scans are performed for all the masses with the same controller.
-
-#+begin_src matlab
-data_m0 = load(sprintf("%s/scans/2023-08-21_14-33_m0_30rpm_cf_control.mat", mat_dir));
-data_m0.time = Ts*[0:length(data_m0.Rz)-1];
-#+end_src
-
-#+begin_src matlab
-%% Compute RMS values while in closed-loop
-data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0));
-data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0));
-data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0));
-data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0));
-data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0));
-#+end_src
-
-#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ...
-              {'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f ');
-#+end_src
-
-#+RESULTS:
-|    | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] |
-|----+---------+---------+---------+-----------+-----------|
-| m0 |     820 |      39 |      13 |      7790 |       156 |
-
-#+begin_src matlab :exports none :results none
-%% description
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile;
-hold on;
-plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int);
-hold off;
-axis square
-xlabel("Y motion [$nm$]");
-ylabel("Z motion [$nm$]");
-#+end_src
-
-** m2
-*** 3x3 plant in Cartesian plane
-#+begin_src matlab :exports none
-%% Load model
-load('Gm.mat')
-#+end_src
-
-#+begin_src matlab
-%% Jacobian for 3x3 plant
-n_hexapod = initializeNanoHexapod('flex_bot_type', '2dof', ...
-                                  'flex_top_type', '3dof', ...
-                                  'motion_sensor_type', 'plates', ...
-                                  'actuator_type', '2dof');
-
-Jt_inv = pinv(n_hexapod.geometry.J');
-
-J_int_to_X = [0.78740157480315  0.21259842519685  0                  0                  0;
-              0                 0                 0                  0                 -1;
-              0                 0               -13.1233595800525   13.1233595800525    0];
-#+end_src
-
-Compute identified plant in the Cartesian plane:
-#+begin_src matlab
-%% New data after calibrating the Rz-offset
-data_m2 = load(sprintf('%s/dynamics/2023-08-21_17-32_damp_plant_m2_new_Rz.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-% Sampling Time [s]
-Ts = 1e-4;
-
-% Hannning Windows
-Nfft = floor(1.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-#+end_src
-
-#+begin_src matlab :exports none
-% And we get the frequency vector
-[~, f] = tfestimate(data_m2.uL1.id_plant, data_m2.uL1.d1, win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-#+begin_src matlab :exports none
-%% HAC Cartesian Plant
-G_hac_m2 = zeros(length(f), 5, 6);
-
-for i_strut = 1:6
-    Di = [data_m2.(sprintf("uL%i", i_strut)).d1 ; data_m2.(sprintf("uL%i", i_strut)).d2 ; data_m2.(sprintf("uL%i", i_strut)).d3 ; data_m2.(sprintf("uL%i", i_strut)).d4 ; data_m2.(sprintf("uL%i", i_strut)).d5]';
-
-    G_hac_m2(:,:,i_strut) = tfestimate(data_m2.(sprintf("uL%i", i_strut)).id_plant, detrend(Di, 0), win, Noverlap, Nfft, 1/Ts);
-end
-
-G_cart_m2 = permute(pagemtimes(J_int_to_X, pagemtimes(permute(G_hac_m2, [2,3,1]), Jt_inv(:,[2,3,5]))), [3,1,2]);
-#+end_src
-
-Compute plant model in the Cartesian plane:
-#+begin_src matlab
-Gm_cart_m2 = J_int_to_X*Gm_hac_m2({'d1', 'd2', 'd3', 'd4', 'd5'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'})*Jt_inv(:,[2,3,5]);
-Gm_cart_m2.InputName = {'Fy', 'Fz', 'My'};
-Gm_cart_m2.OutputName = {'Dy', 'Dz', 'Ry'};
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% 3x3 plant
-figure;
-tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax11 = nexttile();
-hold on;
-plot(f, abs(G_cart_m2(:,1,1)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Dy', 'Fy'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('$D_y$'); set(gca, 'XTickLabel',[]);
-title('$F_y$')
-
-ax12 = nexttile();
-hold on;
-plot(f, abs(G_cart_m2(:,1,2)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Dy', 'Fz'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-title('$F_z$')
-
-ax13 = nexttile();
-hold on;
-plot(f, abs(G_cart_m2(:,1,3)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Dy', 'My'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-title('$M_y$')
-
-linkaxes([ax11,ax12,ax13],'y');
-ylim([1e-8, 2e-4]);
-
-ax21 = nexttile();
-hold on;
-plot(f, abs(G_cart_m2(:,2,1)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Dz', 'Fy'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('$F_z$'); set(gca, 'XTickLabel',[]);
-
-ax22 = nexttile();
-hold on;
-plot(f, abs(G_cart_m2(:,2,2)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Dz', 'Fz'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-
-ax23 = nexttile();
-hold on;
-plot(f, abs(G_cart_m2(:,2,3)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Dz', 'My'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-
-linkaxes([ax21,ax22,ax23],'y');
-ylim([1e-8, 2e-4]);
-
-ax31 = nexttile();
-hold on;
-plot(f, abs(G_cart_m2(:,3,1)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Ry', 'Fy'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('$M_y$');
-
-ax32 = nexttile();
-hold on;
-plot(f, abs(G_cart_m2(:,3,2)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Ry', 'Fz'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-
-ax33 = nexttile();
-hold on;
-plot(f, abs(G_cart_m2(:,3,3)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Ry', 'My'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-
-linkaxes([ax31,ax32,ax33],'y');
-ylim([1e-9, 1e-2]);
-
-linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x');
-xlim([1, 5e2]);
-#+end_src
-
-Normalization of outputs:
-#+begin_src matlab
-Gm_cart_m2_normalized = -diag(1./diag(dcgain(Gm_cart_m2)))*Gm_cart_m2;
-Gm_cart_m2_normalized.InputName = {'Fy', 'Fz', 'My'};
-Gm_cart_m2_normalized.OutputName = {'Dy', 'Dz', 'Ry'};
-
-G_cart_m2_normalized = permute(pagemtimes(-diag(1./diag(dcgain(Gm_cart_m2))), permute(G_cart_m2, [2,3,1])), [3,1,2]);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% 3x3 plant
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_cart_m2_normalized(:,1,1)));
-plot(f, abs(G_cart_m2_normalized(:,2,2)));
-plot(f, abs(G_cart_m2_normalized(:,3,3)));
-plot(f, abs(G_cart_m2_normalized(:,1,2)), 'color', [0,0,0,0.2]);
-plot(f, abs(G_cart_m2_normalized(:,1,3)), 'color', [0,0,0,0.2]);
-plot(f, abs(G_cart_m2_normalized(:,2,1)), 'color', [0,0,0,0.2]);
-plot(f, abs(G_cart_m2_normalized(:,3,1)), 'color', [0,0,0,0.2]);
-plot(f, abs(G_cart_m2_normalized(:,2,3)), 'color', [0,0,0,0.2]);
-plot(f, abs(G_cart_m2_normalized(:,3,2)), 'color', [0,0,0,0.2]);
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-ylim([1e-4, 1e1]);
-
-ax2 = nexttile();
-hold on;
-plot(f, 180/pi*angle(G_cart_m2_normalized(:,1,1)));
-plot(f, 180/pi*angle(G_cart_m2_normalized(:,2,2)));
-plot(f, 180/pi*angle(G_cart_m2_normalized(:,3,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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-*** Better plant invert
-#+begin_src matlab :exports none
-opts = struct();
-
-opts.stable = 1;    % Enforce stable poles
-opts.asymp = 1;     % Force D matrix to be null
-opts.relax = 1;     % Use vector fitting with relaxed non-triviality constraint
-opts.skip_pole = 0; % Do NOT skip pole identification
-opts.skip_res = 0;  % Do NOT skip identification of residues (C,D,E)
-opts.cmplx_ss = 0;  % Create real state space model with block diagonal A
-
-opts.spy1 = 0;      % No plotting for first stage of vector fitting
-opts.spy2 = 0;      % Create magnitude plot for fitting of f(s)
-
-%% We define the number of iteration.
-Niter = 100;
-#+end_src
-
-#+begin_src matlab
-N = 9; %Order of approximation
-#+end_src
-
-**** Dy
-#+begin_src matlab :exports none
-%Complex conjugate pairs, linearly spaced:
-bet=linspace(f(1),f(end),N/2);
-poles=[];
-for n=1:length(bet)
-  alf=-bet(n)*1e-2;
-  poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
-end
-#+end_src
-
-#+begin_src matlab
-%% Estimate resonance frequency and damping
-for iter =1:Niter
-    [G_est, poles, ~, frf_est] = vectfit3(G_cart_m2(f>1&f<1500,1,1).', 1i*2*pi*f(f>1&f<1500)', poles, 1./sqrt(f(f>1&f<1500))', opts);
-end
-
-Gfit_dy = ss(G_est.A, G_est.B, G_est.C, G_est.D);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_cart_m2(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
-plot(f, abs(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(G_cart_m2(:,1,1)), 'color', [colors(1,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dy, f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-Stable Inverse
-#+begin_src matlab
-Ginv_dy = inv(-flipRphZeros(Gfit_dy))/(1 + s/2/pi/1e3);
-isstable(Ginv_dy)
-#+end_src
-
-**** Dz
-#+begin_src matlab :exports none
-%Complex conjugate pairs, linearly spaced:
-bet=linspace(f(1),f(end),N/2);
-poles=[];
-for n=1:length(bet)
-  alf=-bet(n)*1e-1;
-  poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
-end
-#+end_src
-
-#+begin_src matlab
-%% Estimate resonance frequency and damping
-for iter =1:Niter
-    [G_est, poles, ~, frf_est] = vectfit3(G_cart_m2(f>2&f<1500,2,2).', 1i*2*pi*f(f>2&f<1500)', poles, 1./(f(f>2&f<1500))', opts);
-end
-
-Gfit_dz = ss(G_est.A, G_est.B, G_est.C, G_est.D);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_cart_m2(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
-plot(f, abs(squeeze(freqresp(Gfit_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(G_cart_m2(:,2,2)), 'color', [colors(2,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dz, f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-Stable Inverse
-#+begin_src matlab
-Ginv_dz = inv(-flipRphZeros(Gfit_dz))/(1 + s/2/pi/1e3);
-isstable(Ginv_dz)
-#+end_src
-
-**** Ry
-#+begin_src matlab :exports none
-%Complex conjugate pairs, linearly spaced:
-bet=linspace(f(1),f(end),N/2);
-poles=[];
-for n=1:length(bet)
-  alf=-bet(n)*1e-1;
-  poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
-end
-#+end_src
-
-#+begin_src matlab
-%% Estimate resonance frequency and damping
-for iter =1:Niter
-    [G_est, poles, ~, frf_est] = vectfit3(G_cart_m2(f>2&f<800,3,3).', 1i*2*pi*f(f>2&f<800)', poles, 1./(f(f>2&f<800))', opts);
-end
-
-Gfit_ry = ss(G_est.A, G_est.B, G_est.C, G_est.D);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_cart_m2(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
-plot(f, abs(squeeze(freqresp(Gfit_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(G_cart_m2(:,3,3)), 'color', [colors(3,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Gfit_ry, f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-Stable Inverse
-#+begin_src matlab
-Ginv_ry = inv(-flipRphZeros(Gfit_ry))/(1 + s/2/pi/1e3);
-isstable(Ginv_ry)
-#+end_src
-
-**** Compare Invert plants
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, 1./abs(G_cart_m2(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
-plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
-plot(f, 1./abs(G_cart_m2(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
-plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
-plot(f, 1./abs(G_cart_m2(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
-plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(1./G_cart_m2(:,1,1)), 'color', [colors(1,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
-plot(f, 180/pi*angle(1./G_cart_m2(:,2,2)), 'color', [colors(2,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
-plot(f, 180/pi*angle(1./G_cart_m2(:,3,3)), 'color', [colors(3,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz')).*abs(G_cart_m2(:,1,1))), '-', 'DisplayName', '$D_y$');
-plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz')).*abs(G_cart_m2(:,2,2))), '-', 'DisplayName', '$D_z$');
-plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz')).*abs(G_cart_m2(:,3,3))), '-', 'DisplayName', '$R_y$');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz')).*G_cart_m2(:,1,1)), '-');
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz')).*G_cart_m2(:,2,2)), '-');
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz')).*G_cart_m2(:,3,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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-**** Save plant inverse
-#+begin_src matlab :exports none :tangle no
-K = -Ginv_dy;
-K_order = order(K);
-
-Kz_dy = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
-[num, den] = tfdata(Kz_dy, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dy_inv_m2.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-#+begin_src matlab :exports none :tangle no
-K = -Ginv_dz;
-K_order = order(K);
-
-Kz_dz = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
-[num, den] = tfdata(Kz_dz, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dz_inv_m2.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-#+begin_src matlab :exports none :tangle no
-K = -Ginv_ry;
-K_order = order(K);
-
-Kz_ry = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
-[num, den] = tfdata(Kz_ry, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_ry_inv_m2.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-**** Compare Digital Invert plants
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, 1./abs(G_cart_m2(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
-plot(f, abs(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
-plot(f, 1./abs(G_cart_m2(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
-plot(f, abs(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
-plot(f, 1./abs(G_cart_m2(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
-plot(f, abs(squeeze(freqresp(Kz_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(1./G_cart_m2(:,1,1)), 'color', [colors(1,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
-plot(f, 180/pi*angle(1./G_cart_m2(:,2,2)), 'color', [colors(2,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
-plot(f, 180/pi*angle(1./G_cart_m2(:,3,3)), 'color', [colors(3,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Kz_ry, f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-*** TODO Control Performances
-**** Better plant invert
-#+begin_src matlab
-data_cl = load(sprintf('%s/dynamics/2023-08-21_13-36_m0_cf_inv_fit.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-% Sampling Time [s]
-Ts = 1e-4;
-
-% Hannning Windows
-Nfft = floor(1.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-#+end_src
-
-#+begin_src matlab :exports none
-% And we get the frequency vector
-[S_dy, f] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
-[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
-[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile();
-hold on;
-plot(f, abs(S_dy), 'DisplayName', '$D_y$');
-plot(f, abs(S_dz), 'DisplayName', '$D_z$');
-plot(f, abs(S_ry), 'DisplayName', '$R_y$');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('Amplitude');
-ylim([1e-3, 1e1]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
-xlim([1, 1e3]);
-#+end_src
-
-*** TODO Scans with good controller
-**** 1rpm
-1RPM scans are performed for all the masses with the same controller.
-
-#+begin_src matlab
-data_m0 = load(sprintf("%s/scans/2023-08-21_14-28_m0_1rpm_K_cf.mat", mat_dir));
-data_m0.time = Ts*[0:length(data_m0.Rz)-1];
-#+end_src
-
-#+begin_src matlab
-%% Compute RMS values while in closed-loop
-data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0));
-data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0));
-data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0));
-data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0));
-data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0));
-#+end_src
-
-#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ...
-              {'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f ');
-#+end_src
-
-#+RESULTS:
-|    | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] |
-|----+---------+---------+---------+-----------+-----------|
-| m0 |     796 |      20 |       8 |      8209 |        73 |
-
-#+begin_src matlab :exports none :results none
-%% description
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile;
-hold on;
-plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int);
-hold off;
-axis square
-xlabel("Y motion [$nm$]");
-ylabel("Z motion [$nm$]");
-#+end_src
-
-
-**** 30rpm
-1RPM scans are performed for all the masses with the same controller.
-
-#+begin_src matlab
-data_m0 = load(sprintf("%s/scans/2023-08-21_14-33_m0_30rpm_cf_control.mat", mat_dir));
-data_m0.time = Ts*[0:length(data_m0.Rz)-1];
-#+end_src
-
-#+begin_src matlab
-%% Compute RMS values while in closed-loop
-data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0));
-data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0));
-data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0));
-data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0));
-data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0));
-#+end_src
-
-#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ...
-              {'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f ');
-#+end_src
-
-#+RESULTS:
-|    | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] |
-|----+---------+---------+---------+-----------+-----------|
-| m0 |     820 |      39 |      13 |      7790 |       156 |
-
-#+begin_src matlab :exports none :results none
-%% description
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile;
-hold on;
-plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int);
-hold off;
-axis square
-xlabel("Y motion [$nm$]");
-ylabel("Z motion [$nm$]");
-#+end_src
-
-** m3
-*** 3x3 plant in Cartesian plane
-#+begin_src matlab :exports none
-%% Load model
-load('Gm.mat')
-#+end_src
-
-#+begin_src matlab
-%% Jacobian for 3x3 plant
-n_hexapod = initializeNanoHexapod('flex_bot_type', '2dof', ...
-                                  'flex_top_type', '3dof', ...
-                                  'motion_sensor_type', 'plates', ...
-                                  'actuator_type', '2dof');
-
-Jt_inv = pinv(n_hexapod.geometry.J');
-
-J_int_to_X = [0.78740157480315  0.21259842519685  0                  0                  0;
-              0                 0                 0                  0                 -1;
-              0                 0               -13.1233595800525   13.1233595800525    0];
-#+end_src
-
-Compute identified plant in the Cartesian plane:
-#+begin_src matlab
-%% New data after calibrating the Rz-offset
-data_m3 = load(sprintf('%s/dynamics/2023-08-21_16-33_damp_plant_m3_new_Rz_fast.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-% Sampling Time [s]
-Ts = 1e-4;
-
-% Hannning Windows
-Nfft = floor(1.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-#+end_src
-
-#+begin_src matlab :exports none
-% And we get the frequency vector
-[~, f] = tfestimate(data_m3.uL1.id_plant, data_m3.uL1.d1, win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-#+begin_src matlab :exports none
-%% HAC Cartesian Plant
-G_hac_m3 = zeros(length(f), 5, 6);
-
-for i_strut = 1:6
-    Di = [data_m3.(sprintf("uL%i", i_strut)).d1 ; data_m3.(sprintf("uL%i", i_strut)).d2 ; data_m3.(sprintf("uL%i", i_strut)).d3 ; data_m3.(sprintf("uL%i", i_strut)).d4 ; data_m3.(sprintf("uL%i", i_strut)).d5]';
-
-    G_hac_m3(:,:,i_strut) = tfestimate(data_m3.(sprintf("uL%i", i_strut)).id_plant, detrend(Di, 0), win, Noverlap, Nfft, 1/Ts);
-end
-
-G_cart_m3 = permute(pagemtimes(J_int_to_X, pagemtimes(permute(G_hac_m3, [2,3,1]), Jt_inv(:,[2,3,5]))), [3,1,2]);
-#+end_src
-
-Compute plant model in the Cartesian plane:
-#+begin_src matlab
-Gm_cart_m3 = J_int_to_X*Gm_hac_m3({'d1', 'd2', 'd3', 'd4', 'd5'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'})*Jt_inv(:,[2,3,5]);
-Gm_cart_m3.InputName = {'Fy', 'Fz', 'My'};
-Gm_cart_m3.OutputName = {'Dy', 'Dz', 'Ry'};
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% 3x3 plant
-figure;
-tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax11 = nexttile();
-hold on;
-plot(f, abs(G_cart_m3(:,1,1)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Dy', 'Fy'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('$D_y$'); set(gca, 'XTickLabel',[]);
-title('$F_y$')
-
-ax12 = nexttile();
-hold on;
-plot(f, abs(G_cart_m3(:,1,2)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Dy', 'Fz'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-title('$F_z$')
-
-ax13 = nexttile();
-hold on;
-plot(f, abs(G_cart_m3(:,1,3)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Dy', 'My'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-title('$M_y$')
-
-linkaxes([ax11,ax12,ax13],'y');
-ylim([1e-8, 2e-4]);
-
-ax21 = nexttile();
-hold on;
-plot(f, abs(G_cart_m3(:,2,1)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Dz', 'Fy'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('$F_z$'); set(gca, 'XTickLabel',[]);
-
-ax22 = nexttile();
-hold on;
-plot(f, abs(G_cart_m3(:,2,2)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Dz', 'Fz'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-
-ax23 = nexttile();
-hold on;
-plot(f, abs(G_cart_m3(:,2,3)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Dz', 'My'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-
-linkaxes([ax21,ax22,ax23],'y');
-ylim([1e-8, 2e-4]);
-
-ax31 = nexttile();
-hold on;
-plot(f, abs(G_cart_m3(:,3,1)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Ry', 'Fy'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('$M_y$');
-
-ax32 = nexttile();
-hold on;
-plot(f, abs(G_cart_m3(:,3,2)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Ry', 'Fz'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-
-ax33 = nexttile();
-hold on;
-plot(f, abs(G_cart_m3(:,3,3)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Ry', 'My'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-
-linkaxes([ax31,ax32,ax33],'y');
-ylim([1e-9, 1e-2]);
-
-linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x');
-xlim([1, 5e2]);
-#+end_src
-
-Normalization of outputs:
-#+begin_src matlab
-Gm_cart_m3_normalized = -diag(1./diag(dcgain(Gm_cart_m3)))*Gm_cart_m3;
-Gm_cart_m3_normalized.InputName = {'Fy', 'Fz', 'My'};
-Gm_cart_m3_normalized.OutputName = {'Dy', 'Dz', 'Ry'};
-
-G_cart_m3_normalized = permute(pagemtimes(-diag(1./diag(dcgain(Gm_cart_m3))), permute(G_cart_m3, [2,3,1])), [3,1,2]);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% 3x3 plant
-figure;
-tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax11 = nexttile();
-hold on;
-plot(f, abs(G_cart_m3_normalized(:,1,1)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Dy', 'Fy'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('$D_y$'); set(gca, 'XTickLabel',[]);
-title('$F_y$')
-
-ax12 = nexttile();
-hold on;
-plot(f, abs(G_cart_m3_normalized(:,1,2)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Dy', 'Fz'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-title('$F_z$')
-
-ax13 = nexttile();
-hold on;
-plot(f, abs(G_cart_m3_normalized(:,1,3)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Dy', 'My'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-title('$M_y$')
-
-linkaxes([ax11,ax12,ax13],'y');
-ylim([1e-4, 1e1]);
-
-ax21 = nexttile();
-hold on;
-plot(f, abs(G_cart_m3_normalized(:,2,1)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Dz', 'Fy'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('$F_z$'); set(gca, 'XTickLabel',[]);
-
-ax22 = nexttile();
-hold on;
-plot(f, abs(G_cart_m3_normalized(:,2,2)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Dz', 'Fz'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-
-ax23 = nexttile();
-hold on;
-plot(f, abs(G_cart_m3_normalized(:,2,3)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Dz', 'My'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
-
-linkaxes([ax21,ax22,ax23],'y');
-ylim([1e-4, 1e1]);
-
-ax31 = nexttile();
-hold on;
-plot(f, abs(G_cart_m3_normalized(:,3,1)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Ry', 'Fy'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('$M_y$');
-
-ax32 = nexttile();
-hold on;
-plot(f, abs(G_cart_m3_normalized(:,3,2)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Ry', 'Fz'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-
-ax33 = nexttile();
-hold on;
-plot(f, abs(G_cart_m3_normalized(:,3,3)));
-plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Ry', 'My'), freqs, 'Hz'))), '--');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-
-linkaxes([ax31,ax32,ax33],'y');
-ylim([1e-4, 1e1]);
-
-linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x');
-xlim([1, 5e2]);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% 3x3 plant
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_cart_m3_normalized(:,1,1)));
-plot(f, abs(G_cart_m3_normalized(:,2,2)));
-plot(f, abs(G_cart_m3_normalized(:,3,3)));
-plot(f, abs(G_cart_m3_normalized(:,1,2)), 'color', [0,0,0,0.2]);
-plot(f, abs(G_cart_m3_normalized(:,1,3)), 'color', [0,0,0,0.2]);
-plot(f, abs(G_cart_m3_normalized(:,2,1)), 'color', [0,0,0,0.2]);
-plot(f, abs(G_cart_m3_normalized(:,3,1)), 'color', [0,0,0,0.2]);
-plot(f, abs(G_cart_m3_normalized(:,2,3)), 'color', [0,0,0,0.2]);
-plot(f, abs(G_cart_m3_normalized(:,3,2)), 'color', [0,0,0,0.2]);
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-ylim([1e-4, 1e1]);
-
-ax2 = nexttile();
-hold on;
-plot(f, 180/pi*angle(G_cart_m3_normalized(:,1,1)));
-plot(f, 180/pi*angle(G_cart_m3_normalized(:,2,2)));
-plot(f, 180/pi*angle(G_cart_m3_normalized(:,3,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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 1e3]);
-#+end_src
-
-*** Better plant invert
-#+begin_src matlab :exports none
-opts = struct();
-
-opts.stable = 1;    % Enforce stable poles
-opts.asymp = 1;     % Force D matrix to be null
-opts.relax = 1;     % Use vector fitting with relaxed non-triviality constraint
-opts.skip_pole = 0; % Do NOT skip pole identification
-opts.skip_res = 0;  % Do NOT skip identification of residues (C,D,E)
-opts.cmplx_ss = 0;  % Create real state space model with block diagonal A
-
-opts.spy1 = 0;      % No plotting for first stage of vector fitting
-opts.spy2 = 0;      % Create magnitude plot for fitting of f(s)
-
-%% We define the number of iteration.
-Niter = 100;
-#+end_src
-
-#+begin_src matlab
-N = 9; %Order of approximation
-#+end_src
-
-**** Dy
-#+begin_src matlab :exports none
-%Complex conjugate pairs, linearly spaced:
-bet=linspace(f(1),f(end),N/2);
-poles=[];
-for n=1:length(bet)
-  alf=-bet(n)*1e-1;
-  poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
-end
-#+end_src
-
-#+begin_src matlab
-%% Estimate resonance frequency and damping
-for iter =1:Niter
-    [G_est, poles, ~, frf_est] = vectfit3(G_cart_m3(f>2&f<800,1,1).', 1i*2*pi*f(f>2&f<800)', poles, 1./(f(f>2&f<800))', opts);
-end
-
-Gfit_dy = ss(G_est.A, G_est.B, G_est.C, G_est.D);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_cart_m3(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
-plot(f, abs(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(G_cart_m3(:,1,1)), 'color', [colors(1,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dy, f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-Stable Inverse
-#+begin_src matlab
-Ginv_dy = inv(flipRphZeros(Gfit_dy))/(1 + s/2/pi/1e3);
-isstable(Ginv_dy)
-#+end_src
-
-**** Dz
-#+begin_src matlab :exports none
-%Complex conjugate pairs, linearly spaced:
-bet=linspace(f(1),f(end),N/2);
-poles=[];
-for n=1:length(bet)
-  alf=-bet(n)*1e-1;
-  poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
-end
-#+end_src
-
-#+begin_src matlab
-%% Estimate resonance frequency and damping
-for iter =1:Niter
-    [G_est, poles, ~, frf_est] = vectfit3(G_cart_m3(f>2&f<1500,2,2).', 1i*2*pi*f(f>2&f<1500)', poles, 1./(f(f>2&f<1500))', opts);
-end
-
-Gfit_dz = ss(G_est.A, G_est.B, G_est.C, G_est.D);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_cart_m3(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
-plot(f, abs(squeeze(freqresp(Gfit_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(G_cart_m3(:,2,2)), 'color', [colors(2,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dz, f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-Stable Inverse
-#+begin_src matlab
-Ginv_dz = inv(-flipRphZeros(Gfit_dz))/(1 + s/2/pi/1e3);
-isstable(Ginv_dz)
-#+end_src
-
-**** Ry
-#+begin_src matlab :exports none
-%Complex conjugate pairs, linearly spaced:
-bet=linspace(f(1),f(end),N/2);
-poles=[];
-for n=1:length(bet)
-  alf=-bet(n)*1e-1;
-  poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
-end
-#+end_src
-
-#+begin_src matlab
-%% Estimate resonance frequency and damping
-for iter =1:Niter
-    [G_est, poles, ~, frf_est] = vectfit3(G_cart_m3(f>2&f<800,3,3).', 1i*2*pi*f(f>2&f<800)', poles, 1./(f(f>2&f<800))', opts);
-end
-
-Gfit_ry = ss(G_est.A, G_est.B, G_est.C, G_est.D);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(G_cart_m3(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
-plot(f, abs(squeeze(freqresp(Gfit_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(G_cart_m3(:,3,3)), 'color', [colors(3,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Gfit_ry, f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-Stable Inverse
-#+begin_src matlab
-Ginv_ry = inv(-flipRphZeros(Gfit_ry))/(1 + s/2/pi/1e3);
-isstable(Ginv_ry)
-#+end_src
-
-**** Compare Invert plants
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, 1./abs(G_cart_m3(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
-plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
-plot(f, 1./abs(G_cart_m3(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
-plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
-plot(f, 1./abs(G_cart_m3(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
-plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(1./G_cart_m3(:,1,1)), 'color', [colors(1,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
-plot(f, 180/pi*angle(1./G_cart_m3(:,2,2)), 'color', [colors(2,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
-plot(f, 180/pi*angle(1./G_cart_m3(:,3,3)), 'color', [colors(3,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz')).*abs(G_cart_m3(:,1,1))), '-', 'DisplayName', '$D_y$');
-plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz')).*abs(G_cart_m3(:,2,2))), '-', 'DisplayName', '$D_z$');
-plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz')).*abs(G_cart_m3(:,3,3))), '-', 'DisplayName', '$R_y$');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz')).*G_cart_m3(:,1,1)), '-');
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz')).*G_cart_m3(:,2,2)), '-');
-plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz')).*G_cart_m3(:,3,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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-**** Save plant inverse
-#+begin_src matlab :exports none :tangle no
-K = -Ginv_dy;
-K_order = order(K);
-
-Kz_dy = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
-[num, den] = tfdata(Kz_dy, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dy_inv_m3.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-#+begin_src matlab :exports none :tangle no
-K = -Ginv_dz;
-K_order = order(K);
-
-Kz_dz = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
-[num, den] = tfdata(Kz_dz, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dz_inv_m3.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-#+begin_src matlab :exports none :tangle no
-K = -Ginv_ry;
-K_order = order(K);
-
-Kz_ry = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
-[num, den] = tfdata(Kz_ry, 'v');
-
-formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
-fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_ry_inv_m3.dat', 'w');
-fprintf(fileID, formatSpec, [num; den]');
-fclose(fileID);
-#+end_src
-
-**** Compare Digital Invert plants
-#+begin_src matlab :exports none :results none
-%% Comparaison of the measured direct terms and the reduced order models
-figure;
-tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile([2,1]);
-hold on;
-plot(f, 1./abs(G_cart_m3(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
-plot(f, abs(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
-plot(f, 1./abs(G_cart_m3(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
-plot(f, abs(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
-plot(f, 1./abs(G_cart_m3(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
-plot(f, abs(squeeze(freqresp(Kz_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]); ylabel('Magnitude');
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(f, 180/pi*angle(1./G_cart_m3(:,1,1)), 'color', [colors(1,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
-plot(f, 180/pi*angle(1./G_cart_m3(:,2,2)), 'color', [colors(2,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
-plot(f, 180/pi*angle(1./G_cart_m3(:,3,3)), 'color', [colors(3,:)]);
-plot(f, 180/pi*angle(squeeze(freqresp(Kz_ry, f, '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, 180])
-
-linkaxes([ax1,ax2],'x');
-xlim([1, 2e3]);
-#+end_src
-
-*** TODO Control Performances
-**** Better plant invert
-#+begin_src matlab
-data_cl = load(sprintf('%s/dynamics/2023-08-21_13-36_m0_cf_inv_fit.mat', mat_dir));
-#+end_src
-
-#+begin_src matlab :exports none
-% Sampling Time [s]
-Ts = 1e-4;
-
-% Hannning Windows
-Nfft = floor(1.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-#+end_src
-
-#+begin_src matlab :exports none
-% And we get the frequency vector
-[S_dy, f] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
-[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
-[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile();
-hold on;
-plot(f, abs(S_dy), 'DisplayName', '$D_y$');
-plot(f, abs(S_dz), 'DisplayName', '$D_z$');
-plot(f, abs(S_ry), 'DisplayName', '$R_y$');
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('Amplitude');
-ylim([1e-3, 1e1]);
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
-xlim([1, 1e3]);
-#+end_src
-
-*** TODO Scans with good controller
-**** 1rpm
-1RPM scans are performed for all the masses with the same controller.
-
-#+begin_src matlab
-data_m0 = load(sprintf("%s/scans/2023-08-21_14-28_m0_1rpm_K_cf.mat", mat_dir));
-data_m0.time = Ts*[0:length(data_m0.Rz)-1];
-#+end_src
-
-#+begin_src matlab
-%% Compute RMS values while in closed-loop
-data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0));
-data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0));
-data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0));
-data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0));
-data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0));
-#+end_src
-
-#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ...
-              {'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f ');
-#+end_src
-
-#+RESULTS:
-|    | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] |
-|----+---------+---------+---------+-----------+-----------|
-| m0 |     796 |      20 |       8 |      8209 |        73 |
-
-#+begin_src matlab :exports none :results none
-%% description
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile;
-hold on;
-plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int);
-hold off;
-axis square
-xlabel("Y motion [$nm$]");
-ylabel("Z motion [$nm$]");
-#+end_src
-
-
-**** 30rpm
-1RPM scans are performed for all the masses with the same controller.
-
-#+begin_src matlab
-data_m0 = load(sprintf("%s/scans/2023-08-21_14-33_m0_30rpm_cf_control.mat", mat_dir));
-data_m0.time = Ts*[0:length(data_m0.Rz)-1];
-#+end_src
-
-#+begin_src matlab
-%% Compute RMS values while in closed-loop
-data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0));
-data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0));
-data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0));
-data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0));
-data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0));
-#+end_src
-
-#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ...
-              {'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f ');
-#+end_src
-
-#+RESULTS:
-|    | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] |
-|----+---------+---------+---------+-----------+-----------|
-| m0 |     820 |      39 |      13 |      7790 |       156 |
-
-#+begin_src matlab :exports none :results none
-%% description
-figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile;
-hold on;
-plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int);
-hold off;
-axis square
-xlabel("Y motion [$nm$]");
-ylabel("Z motion [$nm$]");
-#+end_src
-
-* Scans
-<<sec:id31_scans>>
-** Introduction                                                      :ignore:
-
-- Section ref:sec:id31_scans_tomography
-- Section ref:sec:id31_scans_dz
-- Section ref:sec:id31_scans_reflectivity
-- Section ref:sec:id31_scans_dy
-- Section ref:sec:id31_scans_diffraction_tomo
-
-** Matlab Init                                              :noexport:ignore:
-#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
-<<matlab-dir>>
-#+end_src
-
-#+begin_src matlab :exports none :results silent :noweb yes
-<<matlab-init>>
-#+end_src
-
-#+begin_src matlab :tangle no :noweb yes
-<<m-init-path>>
-#+end_src
-
-#+begin_src matlab :eval no :noweb yes
-<<m-init-path-tangle>>
-#+end_src
-
-#+begin_src matlab :noweb yes
-<<m-init-other>>
-#+end_src
-
-** $R_z$ scans: Tomography
-<<sec:id31_scans_tomography>>
-*** Introduction                                                    :ignore:
-
-m0: 30rpm, 6rpm, 1rpm
-m1: 6rpm, 1rpm
-m2: 6rpm, 1rpm
-m3: 1rpm
-
-*** Robust Control - 1rpm
-1RPM scans are performed for all the masses with the same robust controller.
-
-#+begin_src matlab
-%% Load Tomography scans with robust controller
-data_tomo_1rpm_m0 = load(sprintf("%s/scans/2023-08-11_11-37_tomography_1rpm_m0.mat", mat_dir));
-data_tomo_1rpm_m0.time = Ts*[0:length(data_tomo_1rpm_m0.Rz)-1];
-
-data_tomo_1rpm_m1 = load(sprintf("%s/scans/2023-08-11_11-15_tomography_1rpm_m1.mat", mat_dir));
-data_tomo_1rpm_m1.time = Ts*[0:length(data_tomo_1rpm_m1.Rz)-1];
-
-data_tomo_1rpm_m2 = load(sprintf("%s/scans/2023-08-11_10-59_tomography_1rpm_m2.mat", mat_dir));
-data_tomo_1rpm_m2.time = Ts*[0:length(data_tomo_1rpm_m2.Rz)-1];
-
-data_tomo_1rpm_m3 = load(sprintf("%s/scans/2023-08-11_10-24_tomography_1rpm_m3.mat", mat_dir));
-data_tomo_1rpm_m3.time = Ts*[0:length(data_tomo_1rpm_m3.Rz)-1];
-#+end_src
-
-The problem for these scans is that the position initialization was not make properly, so the open-loop errors are quite large (see Figure ref:fig:id31_tomo_1rpm_robust_m0).
-
-#+begin_src matlab :exports none :results none
-%% $D_x$, $D_y$ and $D_z$ motion during a slow (1RPM) tomography experiment. Open Loop data is shown in blue and closed-loop data in red
-figure;
-tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile;
-hold on;
-plot(1e6*data_tomo_1rpm_m0.Dx_int(data_tomo_1rpm_m0.hac_status == 0), 1e6*data_tomo_1rpm_m0.Dy_int(data_tomo_1rpm_m0.hac_status == 0), ...
-    'DisplayName', 'OL')
-plot(1e6*data_tomo_1rpm_m0.Dx_int(data_tomo_1rpm_m0.hac_status == 1), 1e6*data_tomo_1rpm_m0.Dy_int(data_tomo_1rpm_m0.hac_status == 1), ...
-    'DisplayName', 'CL')
-hold off;
-axis equal
-xlabel("X motion [$\mu m$]");
-ylabel("Y motion [$\mu m$]");
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-% xlim([-3, 3])
-% ylim([-3, 3])
-
-ax2 = nexttile;
-hold on;
-plot(1e6*data_tomo_1rpm_m0.Dy_int(data_tomo_1rpm_m0.hac_status == 0), 1e6*data_tomo_1rpm_m0.Dz_int(data_tomo_1rpm_m0.hac_status == 0), ...
-    'DisplayName', 'OL')
-plot(1e6*data_tomo_1rpm_m0.Dy_int(data_tomo_1rpm_m0.hac_status == 1), 1e6*data_tomo_1rpm_m0.Dz_int(data_tomo_1rpm_m0.hac_status == 1), ...
-    'DisplayName', 'CL')
-hold off;
-% axis equal
-% xlim([-3, 3])
-ylim([-0.2, 1.1])
-xlabel("Y motion [$\mu m$]");
-ylabel("Z motion [$\mu m$]");
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_tomo_1rpm_robust_m0.pdf', 'width', 'full', 'height', 'normal');
-#+end_src
-
-#+name: fig:id31_tomo_1rpm_robust_m0
-#+caption: $D_x$, $D_y$ and $D_z$ motion during a slow (1RPM) tomography experiment. Open Loop data is shown in blue and closed-loop data in red
-#+RESULTS:
-[[file:figs/id31_tomo_1rpm_robust_m0.png]]
-
-#+begin_src matlab
-yztomographymovie('movies/tomography_1rpm_m0', data_tomo_1rpm_m0, 'xlim_ax1', [-10, 15], 'ylim_ax1', [-0.2, 1.2], 'xlim_ax2', [-300, 300], 'ylim_ax2', [-100, 100])
-yztomographymovie('movies/tomography_1rpm_m0_di_5000', data_tomo_1rpm_m0, 'xlim_ax1', [-10, 15], 'ylim_ax1', [-0.2, 1.2], 'xlim_ax2', [-300, 300], 'ylim_ax2', [-100, 100], 'di', 5000)
-#+end_src
-
-The obtained open-loop and closed-loop errors are shown in tables ref:tab:id31_tomo_1rpm_robust_ol_errors and ref:tab:id31_tomo_1rpm_robust_cl_errors respectively.
-
-#+begin_src matlab
-%% Compute RMS values while in closed-loop and open-loop
-[~, i_m0] = find(data_tomo_1rpm_m0.hac_status == 1);
-data_tomo_1rpm_m0.Dx_rms_cl = rms(detrend(data_tomo_1rpm_m0.Dx_int(i_m0+50000:end), 0));
-data_tomo_1rpm_m0.Dy_rms_cl = rms(detrend(data_tomo_1rpm_m0.Dy_int(i_m0+50000:end), 0));
-data_tomo_1rpm_m0.Dz_rms_cl = rms(detrend(data_tomo_1rpm_m0.Dz_int(i_m0+50000:end), 0));
-data_tomo_1rpm_m0.Rx_rms_cl = rms(detrend(data_tomo_1rpm_m0.Rx_int(i_m0+50000:end), 0));
-data_tomo_1rpm_m0.Ry_rms_cl = rms(detrend(data_tomo_1rpm_m0.Ry_int(i_m0+50000:end), 0));
-
-data_tomo_1rpm_m0.Dx_rms_ol = rms(detrend(data_tomo_1rpm_m0.Dx_int(1:i_m0), 0));
-data_tomo_1rpm_m0.Dy_rms_ol = rms(detrend(data_tomo_1rpm_m0.Dy_int(1:i_m0), 0));
-data_tomo_1rpm_m0.Dz_rms_ol = rms(detrend(data_tomo_1rpm_m0.Dz_int(1:i_m0), 0));
-data_tomo_1rpm_m0.Rx_rms_ol = rms(detrend(data_tomo_1rpm_m0.Rx_int(1:i_m0), 0));
-data_tomo_1rpm_m0.Ry_rms_ol = rms(detrend(data_tomo_1rpm_m0.Ry_int(1:i_m0), 0));
-
-%% Compute RMS values while in closed-loop and open-loop
-[~, i_m1] = find(data_tomo_1rpm_m1.hac_status == 1);
-data_tomo_1rpm_m1.Dx_rms_cl = rms(detrend(data_tomo_1rpm_m1.Dx_int(i_m1+50000:end), 0));
-data_tomo_1rpm_m1.Dy_rms_cl = rms(detrend(data_tomo_1rpm_m1.Dy_int(i_m1+50000:end), 0));
-data_tomo_1rpm_m1.Dz_rms_cl = rms(detrend(data_tomo_1rpm_m1.Dz_int(i_m1+50000:end), 0));
-data_tomo_1rpm_m1.Rx_rms_cl = rms(detrend(data_tomo_1rpm_m1.Rx_int(i_m1+50000:end), 0));
-data_tomo_1rpm_m1.Ry_rms_cl = rms(detrend(data_tomo_1rpm_m1.Ry_int(i_m1+50000:end), 0));
-
-data_tomo_1rpm_m1.Dx_rms_ol = rms(detrend(data_tomo_1rpm_m1.Dx_int(1:i_m1), 0));
-data_tomo_1rpm_m1.Dy_rms_ol = rms(detrend(data_tomo_1rpm_m1.Dy_int(1:i_m1), 0));
-data_tomo_1rpm_m1.Dz_rms_ol = rms(detrend(data_tomo_1rpm_m1.Dz_int(1:i_m1), 0));
-data_tomo_1rpm_m1.Rx_rms_ol = rms(detrend(data_tomo_1rpm_m1.Rx_int(1:i_m1), 0));
-data_tomo_1rpm_m1.Ry_rms_ol = rms(detrend(data_tomo_1rpm_m1.Ry_int(1:i_m1), 0));
-
-%% Compute RMS values while in closed-loop and open-loop
-[~, i_m2] = find(data_tomo_1rpm_m2.hac_status == 1);
-data_tomo_1rpm_m2.Dx_rms_cl = rms(detrend(data_tomo_1rpm_m2.Dx_int(i_m2+50000:end), 0));
-data_tomo_1rpm_m2.Dy_rms_cl = rms(detrend(data_tomo_1rpm_m2.Dy_int(i_m2+50000:end), 0));
-data_tomo_1rpm_m2.Dz_rms_cl = rms(detrend(data_tomo_1rpm_m2.Dz_int(i_m2+50000:end), 0));
-data_tomo_1rpm_m2.Rx_rms_cl = rms(detrend(data_tomo_1rpm_m2.Rx_int(i_m2+50000:end), 0));
-data_tomo_1rpm_m2.Ry_rms_cl = rms(detrend(data_tomo_1rpm_m2.Ry_int(i_m2+50000:end), 0));
-
-data_tomo_1rpm_m2.Dx_rms_ol = rms(detrend(data_tomo_1rpm_m2.Dx_int(1:i_m2), 0));
-data_tomo_1rpm_m2.Dy_rms_ol = rms(detrend(data_tomo_1rpm_m2.Dy_int(1:i_m2), 0));
-data_tomo_1rpm_m2.Dz_rms_ol = rms(detrend(data_tomo_1rpm_m2.Dz_int(1:i_m2), 0));
-data_tomo_1rpm_m2.Rx_rms_ol = rms(detrend(data_tomo_1rpm_m2.Rx_int(1:i_m2), 0));
-data_tomo_1rpm_m2.Ry_rms_ol = rms(detrend(data_tomo_1rpm_m2.Ry_int(1:i_m2), 0));
-
-%% Compute RMS values while in closed-loop and open-loop
-[~, i_m3] = find(data_tomo_1rpm_m3.hac_status == 1);
-data_tomo_1rpm_m3.Dx_rms_cl = rms(detrend(data_tomo_1rpm_m3.Dx_int(i_m3+50000:end), 0));
-data_tomo_1rpm_m3.Dy_rms_cl = rms(detrend(data_tomo_1rpm_m3.Dy_int(i_m3+50000:end), 0));
-data_tomo_1rpm_m3.Dz_rms_cl = rms(detrend(data_tomo_1rpm_m3.Dz_int(i_m3+50000:end), 0));
-data_tomo_1rpm_m3.Rx_rms_cl = rms(detrend(data_tomo_1rpm_m3.Rx_int(i_m3+50000:end), 0));
-data_tomo_1rpm_m3.Ry_rms_cl = rms(detrend(data_tomo_1rpm_m3.Ry_int(i_m3+50000:end), 0));
-
-data_tomo_1rpm_m3.Dx_rms_ol = rms(detrend(data_tomo_1rpm_m3.Dx_int(1:i_m3), 0));
-data_tomo_1rpm_m3.Dy_rms_ol = rms(detrend(data_tomo_1rpm_m3.Dy_int(1:i_m3), 0));
-data_tomo_1rpm_m3.Dz_rms_ol = rms(detrend(data_tomo_1rpm_m3.Dz_int(1:i_m3), 0));
-data_tomo_1rpm_m3.Rx_rms_ol = rms(detrend(data_tomo_1rpm_m3.Rx_int(1:i_m3), 0));
-data_tomo_1rpm_m3.Ry_rms_ol = rms(detrend(data_tomo_1rpm_m3.Ry_int(1:i_m3), 0));
-#+end_src
-
-#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-data2orgtable([1e6*data_tomo_1rpm_m0.Dx_rms_ol, 1e6*data_tomo_1rpm_m0.Dy_rms_ol, 1e9*data_tomo_1rpm_m0.Dz_rms_ol, 1e6*data_tomo_1rpm_m0.Rx_rms_ol, 1e6*data_tomo_1rpm_m0.Ry_rms_ol; ...
-               1e6*data_tomo_1rpm_m1.Dx_rms_ol, 1e6*data_tomo_1rpm_m1.Dy_rms_ol, 1e9*data_tomo_1rpm_m1.Dz_rms_ol, 1e6*data_tomo_1rpm_m1.Rx_rms_ol, 1e6*data_tomo_1rpm_m1.Ry_rms_ol; ...
-               1e6*data_tomo_1rpm_m2.Dx_rms_ol, 1e6*data_tomo_1rpm_m2.Dy_rms_ol, 1e9*data_tomo_1rpm_m2.Dz_rms_ol, 1e6*data_tomo_1rpm_m2.Rx_rms_ol, 1e6*data_tomo_1rpm_m2.Ry_rms_ol; ...
-               1e6*data_tomo_1rpm_m3.Dx_rms_ol, 1e6*data_tomo_1rpm_m3.Dy_rms_ol, 1e9*data_tomo_1rpm_m3.Dz_rms_ol, 1e6*data_tomo_1rpm_m3.Rx_rms_ol, 1e6*data_tomo_1rpm_m3.Ry_rms_ol], ...
-              {'$m_0$', '$m_1$', '$m_2$', '$m_3$'}, {'$D_x$ [$\mu m$]', '$D_y$ [$\mu m$]', '$D_z$ [$nm$]', '$R_x$ [$\mu\text{rad}$]', '$R_y$ [$\mu\text{rad}$]'}, ' %.0f ');
-#+end_src
-
-#+name: tab:id31_tomo_1rpm_robust_ol_errors
-#+caption: Measured error during open-loop tomography scans (1rpm)
-#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
-#+attr_latex: :center t :booktabs t
-#+RESULTS:
-|       | $D_x$ [$\mu m$] | $D_y$ [$\mu m$] | $D_z$ [$nm$] | $R_x$ [$\mu\text{rad}$] | $R_y$ [$\mu\text{rad}$] |
-|-------+-----------------+-----------------+--------------+-------------------------+-------------------------|
-| $m_0$ |               6 |               6 |           32 |                      34 |                      34 |
-| $m_1$ |               6 |               7 |           26 |                      51 |                      55 |
-| $m_2$ |              36 |              38 |           36 |                     259 |                     253 |
-| $m_3$ |              31 |              33 |           38 |                     214 |                     203 |
-
-#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-data2orgtable([1e9*data_tomo_1rpm_m0.Dx_rms_cl, 1e9*data_tomo_1rpm_m0.Dy_rms_cl, 1e9*data_tomo_1rpm_m0.Dz_rms_cl, 1e9*data_tomo_1rpm_m0.Rx_rms_cl, 1e9*data_tomo_1rpm_m0.Ry_rms_cl; ...
-               1e9*data_tomo_1rpm_m1.Dx_rms_cl, 1e9*data_tomo_1rpm_m1.Dy_rms_cl, 1e9*data_tomo_1rpm_m1.Dz_rms_cl, 1e9*data_tomo_1rpm_m1.Rx_rms_cl, 1e9*data_tomo_1rpm_m1.Ry_rms_cl; ...
-               1e9*data_tomo_1rpm_m2.Dx_rms_cl, 1e9*data_tomo_1rpm_m2.Dy_rms_cl, 1e9*data_tomo_1rpm_m2.Dz_rms_cl, 1e9*data_tomo_1rpm_m2.Rx_rms_cl, 1e9*data_tomo_1rpm_m2.Ry_rms_cl; ...
-               1e9*data_tomo_1rpm_m3.Dx_rms_cl, 1e9*data_tomo_1rpm_m3.Dy_rms_cl, 1e9*data_tomo_1rpm_m3.Dz_rms_cl, 1e9*data_tomo_1rpm_m3.Rx_rms_cl, 1e9*data_tomo_1rpm_m3.Ry_rms_cl], ...
-              {'$m_0$', '$m_1$', '$m_2$', '$m_3$'}, {'$D_x$ [nm]', '$D_y$ [nm]', '$D_z$ [nm]', '$R_x$ [nrad]', '$R_y$ [nrad]'}, ' %.0f ');
-#+end_src
-
-#+name: tab:id31_tomo_1rpm_robust_cl_errors
-#+caption: Measured error during closed-loop tomography scans (1rpm, robust controller)
-#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
-#+attr_latex: :center t :booktabs t
-#+RESULTS:
-|       | $D_x$ [nm] | $D_y$ [nm] | $D_z$ [nm] | $R_x$ [nrad] | $R_y$ [nrad] |
-|-------+------------+------------+------------+--------------+--------------|
-| $m_0$ |         13 |         15 |          5 |           57 |           55 |
-| $m_1$ |         16 |         25 |          6 |          102 |           55 |
-| $m_2$ |         25 |         25 |          7 |          120 |          103 |
-| $m_3$ |         40 |         53 |          9 |          225 |          169 |
-
-*** TODO Slow Tomography Scans Comparison of control performances :noexport:
-#+begin_src matlab
-% Decentralized in the frame of the struts
-data = load(sprintf("%s/scans/2023-08-18_10-43_m0_1rpm.mat", mat_dir));
-data.time = Ts*[0:length(data.Rz)-1];
-
-% Rotating cartesian frame
-data_cart = load(sprintf("%s/scans/2023-08-18_18-33_m0_1rpm_K_cart.mat", mat_dir));
-data_cart.time = Ts*[0:length(data_cart.Rz)-1];
-
-% Fixed cartesian frame
-data_cart_fixed = load(sprintf("%s/scans/2023-08-18_19-08_m0_1rpm_K_cart_fixed.mat", mat_dir));
-data_cart_fixed.time = Ts*[0:length(data_cart_fixed.Rz)-1];
-
-% Fixed cartesian frame with Complementary Filters
-data_cf = load(sprintf("%s/scans/2023-08-21_14-28_m0_1rpm_K_cf.mat", mat_dir));
-data_cf.time = Ts*[0:length(data_cf.Rz)-1];
-#+end_src
-
-#+begin_src matlab
-1e9*rms(data.Dx_int(data.time<45))
-1e9*rms(data_cart.Dx_int(data_cart.time<45))
-1e9*rms(data_cart_fixed.Dx_int(data_cart_fixed.time<45))
-1e9*rms(data_cf.Dx_int(data_cf.time<45))
-#+end_src
-
-#+begin_src matlab
-1e9*rms(data.Dy_int(data.time<45))
-1e9*rms(data_cart.Dy_int(data_cart.time<45))
-1e9*rms(data_cart_fixed.Dy_int(data_cart_fixed.time<45))
-1e9*rms(data_cf.Dy_int(data_cf.time<45))
-#+end_src
-
-#+begin_src matlab
-1e9*rms(data.Dz_int(data.time<45))
-1e9*rms(data_cart.Dz_int(data_cart.time<45))
-1e9*rms(data_cart_fixed.Dz_int(data_cart_fixed.time<45))
-1e9*rms(data_cf.Dz_int(data_cf.time<45))
-#+end_src
-
-#+begin_src matlab
-1e9*rms(data.Rx_int(data.time<45))
-1e9*rms(data_cart.Rx_int(data_cart.time<45))
-1e9*rms(data_cart_fixed.Rx_int(data_cart_fixed.time<45))
-1e9*rms(data_cf.Rx_int(data_cf.time<45))
-#+end_src
-
-#+begin_src matlab
-1e9*rms(data.Ry_int(data.time<45))
-1e9*rms(data_cart.Ry_int(data_cart.time<45))
-1e9*rms(data_cart_fixed.Ry_int(data_cart_fixed.time<45))
-1e9*rms(data_cf.Ry_int(data_cf.time<45))
-#+end_src
-
-#+begin_src matlab
-figure;
-
-hold on;
-plot(data.time, data.Dy_int)
-plot(data_cart.time, data_cart.Dy_int)
-plot(data_cart_fixed.time, data_cart_fixed.Dy_int)
-plot(data_cf.time, data_cf.Dy_int)
-hold off;
-#+end_src
-
-#+begin_src matlab :exports none
-% Hannning Windows
-Nfft = floor(10.0/Ts);
-win = hanning(Nfft);
-Noverlap = floor(Nfft/2);
-
-[data.pxx_Dx, data.f] = pwelch(detrend(data.Dx_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts);
-[data.pxx_Dy, ~     ] = pwelch(detrend(data.Dy_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts);
-[data.pxx_Dz, ~     ] = pwelch(detrend(data.Dz_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts);
-[data.pxx_Rx, ~     ] = pwelch(detrend(data.Rx_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts);
-[data.pxx_Ry, ~     ] = pwelch(detrend(data.Ry_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts);
-
-[data_cart.pxx_Dx, data_cart.f] = pwelch(detrend(data_cart.Dx_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts);
-[data_cart.pxx_Dy, ~          ] = pwelch(detrend(data_cart.Dy_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts);
-[data_cart.pxx_Dz, ~          ] = pwelch(detrend(data_cart.Dz_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts);
-[data_cart.pxx_Rx, ~          ] = pwelch(detrend(data_cart.Rx_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts);
-[data_cart.pxx_Ry, ~          ] = pwelch(detrend(data_cart.Ry_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts);
-
-[data_cart_fixed.pxx_Dx, data_cart_fixed.f] = pwelch(detrend(data_cart_fixed.Dx_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts);
-[data_cart_fixed.pxx_Dy, ~                ] = pwelch(detrend(data_cart_fixed.Dy_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts);
-[data_cart_fixed.pxx_Dz, ~                ] = pwelch(detrend(data_cart_fixed.Dz_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts);
-[data_cart_fixed.pxx_Rx, ~                ] = pwelch(detrend(data_cart_fixed.Rx_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts);
-[data_cart_fixed.pxx_Ry, ~                ] = pwelch(detrend(data_cart_fixed.Ry_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts);
-
-[data_cf.pxx_Dx, data_cf.f] = pwelch(detrend(data_cf.Dx_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts);
-[data_cf.pxx_Dy, ~        ] = pwelch(detrend(data_cf.Dy_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts);
-[data_cf.pxx_Dz, ~        ] = pwelch(detrend(data_cf.Dz_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts);
-[data_cf.pxx_Rx, ~        ] = pwelch(detrend(data_cf.Rx_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts);
-[data_cf.pxx_Ry, ~        ] = pwelch(detrend(data_cf.Ry_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts);
-#+end_src
-
-#+begin_src matlab
-figure;
-
-hold on;
-plot(data.f, data.pxx_Dy)
-plot(data_cart.f, data_cart.pxx_Dy)
-plot(data_cart_fixed.f, data_cart_fixed.pxx_Dy)
-plot(data_cf.f, data_cf.pxx_Dy)
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-% legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
-xlim([0.1, 5e2]);
-#+end_src
-
-#+begin_src matlab
-figure;
-
-hold on;
-plot(data.f, sqrt(flip(-cumtrapz(flip(data.f), flip(data.pxx_Dy)))))
-plot(data_cart.f, sqrt(flip(-cumtrapz(flip(data_cart.f), flip(data_cart.pxx_Dy)))))
-plot(data_cart_fixed.f, sqrt(flip(-cumtrapz(flip(data_cart_fixed.f), flip(data_cart_fixed.pxx_Dy)))))
-plot(data_cf.f, sqrt(flip(-cumtrapz(flip(data_cf.f), flip(data_cf.pxx_Dy)))))
-hold off;
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
-% legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
-xlim([0.1, 5e2]);
-#+end_src
-
-*** Robust Control - 6rpm
-#+begin_src matlab
-data_tomo_6rpm_m0 = load(sprintf("%s/scans/2023-08-11_11-31_tomography_6rpm_m0.mat", mat_dir));
-data_tomo_6rpm_m0.time = Ts*[0:length(data_tomo_6rpm_m0.Rz)-1];
-#+end_src
-
-#+begin_src matlab
-data_tomo_6rpm_m1 = load(sprintf("%s/scans/2023-08-11_11-23_tomography_6rpm_m1.mat", mat_dir));
-data_tomo_6rpm_m1.time = Ts*[0:length(data_tomo_6rpm_m1.Rz)-1];
-#+end_src
-
-#+begin_src matlab
-%% Compute RMS values while in closed-loop
-[~, i_m0] = find(data_tomo_6rpm_m0.hac_status == 1);
-data_tomo_6rpm_m0.Dx_rms_cl = rms(detrend(data_tomo_6rpm_m0.Dx_int(i_m0+50000:end), 0));
-data_tomo_6rpm_m0.Dy_rms_cl = rms(detrend(data_tomo_6rpm_m0.Dy_int(i_m0+50000:end), 0));
-data_tomo_6rpm_m0.Dz_rms_cl = rms(detrend(data_tomo_6rpm_m0.Dz_int(i_m0+50000:end), 0));
-data_tomo_6rpm_m0.Rx_rms_cl = rms(detrend(data_tomo_6rpm_m0.Rx_int(i_m0+50000:end), 0));
-data_tomo_6rpm_m0.Ry_rms_cl = rms(detrend(data_tomo_6rpm_m0.Ry_int(i_m0+50000:end), 0));
-
-data_tomo_6rpm_m0.Dx_rms_ol = rms(detrend(data_tomo_6rpm_m0.Dx_int(1:i_m0), 0));
-data_tomo_6rpm_m0.Dy_rms_ol = rms(detrend(data_tomo_6rpm_m0.Dy_int(1:i_m0), 0));
-data_tomo_6rpm_m0.Dz_rms_ol = rms(detrend(data_tomo_6rpm_m0.Dz_int(1:i_m0), 0));
-data_tomo_6rpm_m0.Rx_rms_ol = rms(detrend(data_tomo_6rpm_m0.Rx_int(1:i_m0), 0));
-data_tomo_6rpm_m0.Ry_rms_ol = rms(detrend(data_tomo_6rpm_m0.Ry_int(1:i_m0), 0));
-
-%% Compute RMS values while in closed-loop
-[~, i_m1] = find(data_tomo_6rpm_m1.hac_status == 1);
-data_tomo_6rpm_m1.Dx_rms_cl = rms(detrend(data_tomo_6rpm_m1.Dx_int(i_m1+50000:end), 0));
-data_tomo_6rpm_m1.Dy_rms_cl = rms(detrend(data_tomo_6rpm_m1.Dy_int(i_m1+50000:end), 0));
-data_tomo_6rpm_m1.Dz_rms_cl = rms(detrend(data_tomo_6rpm_m1.Dz_int(i_m1+50000:end), 0));
-data_tomo_6rpm_m1.Rx_rms_cl = rms(detrend(data_tomo_6rpm_m1.Rx_int(i_m1+50000:end), 0));
-data_tomo_6rpm_m1.Ry_rms_cl = rms(detrend(data_tomo_6rpm_m1.Ry_int(i_m1+50000:end), 0));
-
-data_tomo_6rpm_m1.Dx_rms_ol = rms(detrend(data_tomo_6rpm_m1.Dx_int(1:i_m1), 0));
-data_tomo_6rpm_m1.Dy_rms_ol = rms(detrend(data_tomo_6rpm_m1.Dy_int(1:i_m1), 0));
-data_tomo_6rpm_m1.Dz_rms_ol = rms(detrend(data_tomo_6rpm_m1.Dz_int(1:i_m1), 0));
-data_tomo_6rpm_m1.Rx_rms_ol = rms(detrend(data_tomo_6rpm_m1.Rx_int(1:i_m1), 0));
-data_tomo_6rpm_m1.Ry_rms_ol = rms(detrend(data_tomo_6rpm_m1.Ry_int(1:i_m1), 0));
-#+end_src
-
-#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-data2orgtable([1e6*data_tomo_6rpm_m0.Dx_rms_ol, 1e6*data_tomo_6rpm_m0.Dy_rms_ol, 1e9*data_tomo_6rpm_m0.Dz_rms_ol, 1e6*data_tomo_6rpm_m0.Rx_rms_ol, 1e6*data_tomo_6rpm_m0.Ry_rms_ol; ...
-               1e6*data_tomo_6rpm_m1.Dx_rms_ol, 1e6*data_tomo_6rpm_m1.Dy_rms_ol, 1e9*data_tomo_6rpm_m1.Dz_rms_ol, 1e6*data_tomo_6rpm_m1.Rx_rms_ol, 1e6*data_tomo_6rpm_m1.Ry_rms_ol], ...
-              {'$m_0$', '$m_1$'}, {'$D_x$ [$\mu m$]', '$D_y$ [$\mu m$]', '$D_z$ [$nm$]', '$R_x$ [$\mu\text{rad}$]', '$R_y$ [$\mu\text{rad}$]'}, ' %.0f ');
-#+end_src
-
-#+name: tab:id31_tomo_6rpm_robust_ol_errors
-#+caption: Measured error during open-loop tomography scans (6rpm)
-#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
-#+attr_latex: :center t :booktabs t
-#+RESULTS:
-|       | $D_x$ [$\mu m$] | $D_y$ [$\mu m$] | $D_z$ [$nm$] | $R_x$ [$\mu\text{rad}$] | $R_y$ [$\mu\text{rad}$] |
-|-------+-----------------+-----------------+--------------+-------------------------+-------------------------|
-| $m_0$ |               8 |               7 |           20 |                      41 |                      41 |
-| $m_1$ |               4 |               4 |           21 |                      39 |                      39 |
-
-#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-data2orgtable([1e9*data_tomo_6rpm_m0.Dx_rms_cl, 1e9*data_tomo_6rpm_m0.Dy_rms_cl, 1e9*data_tomo_6rpm_m0.Dz_rms_cl, 1e9*data_tomo_6rpm_m0.Rx_rms_cl, 1e9*data_tomo_6rpm_m0.Ry_rms_cl; ...
-               1e9*data_tomo_6rpm_m1.Dx_rms_cl, 1e9*data_tomo_6rpm_m1.Dy_rms_cl, 1e9*data_tomo_6rpm_m1.Dz_rms_cl, 1e9*data_tomo_6rpm_m1.Rx_rms_cl, 1e9*data_tomo_6rpm_m1.Ry_rms_cl], ...
-              {'$m_0$', '$m_1$'}, {'$D_x$ [nm]', '$D_y$ [nm]', '$D_z$ [nm]', '$R_x$ [nrad]', '$R_y$ [nrad]'}, ' %.0f ');
-#+end_src
-
-#+name: tab:id31_tomo_6rpm_robust_cl_errors
-#+caption: Measured error during closed-loop tomography scans (6rpm, robust controller)
-#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
-#+attr_latex: :center t :booktabs t
-#+RESULTS:
-|       | $D_x$ [nm] | $D_y$ [nm] | $D_z$ [nm] | $R_x$ [nrad] | $R_y$ [nrad] |
-|-------+------------+------------+------------+--------------+--------------|
-| $m_0$ |         17 |         19 |          5 |           70 |           73 |
-| $m_1$ |         20 |         26 |          7 |          110 |           77 |
-
-*** TODO Medium velocity tomography scans with CF control         :noexport:
-#+begin_src matlab
-data_m1_cf = load(sprintf("%s/scans/2023-08-21_19-18_m1_6rpm_cf_control.mat", mat_dir));
-data_m1_cf.time = Ts*[0:length(data_m1_cf.Rz)-1];
-#+end_src
-
-#+begin_src matlab
-data_m2_cf = load(sprintf("%s/scans/2023-08-21_18-07_m2_6rpm_cf_control.mat", mat_dir));
-data_m2_cf.time = Ts*[0:length(data_m2_cf.Rz)-1];
-#+end_src
-
-And higher bandwidth:
-#+begin_src matlab
-data_m1_cf_high_fb = load(sprintf("%s/scans/2023-08-21_19-24_m1_6rpm_cf_control_60Hz.mat", mat_dir));
-data_m1_cf_high_fb.time = Ts*[0:length(data_m1_cf_high_fb.Rz)-1];
-#+end_src
-
-#+begin_src matlab
-figure;
-hold on;
-plot(data_m1_cf.Dy_int, detrend(data_m1_cf.Dz_int, 0))
-plot(data_m2_cf.Dy_int, detrend(data_m2_cf.Dz_int, 0))
-plot(data_m1_cf_high_fb.Dy_int, detrend(data_m1_cf_high_fb.Dz_int, 0))
-axis equal
-#+end_src
-
-#+begin_src matlab
-1e9*rms(detrend(data_m1.Dz_int(i_m1+50000:end), 0))
-1e9*rms(detrend(data_m1.Dy_int(i_m1+50000:end), 0))
-1e9*rms(detrend(data_m1.Ry_int(i_m1+50000:end), 0))
-#+end_src
-
-#+begin_src matlab
-1e9*rms(detrend(data_m1_cf.Dz_int, 0))
-1e9*rms(detrend(data_m1_cf.Dy_int, 0))
-1e9*rms(detrend(data_m1_cf.Ry_int, 0))
-#+end_src
-
-#+begin_src matlab
-1e9*rms(detrend(data_m2.Dz_int, 0))
-1e9*rms(detrend(data_m2.Dy_int, 0))
-1e9*rms(detrend(data_m2.Ry_int, 0))
-#+end_src
-
-#+begin_src matlab
-1e9*rms(detrend(data_m1_high_fb.Dz_int, 0))
-1e9*rms(detrend(data_m1_high_fb.Dy_int, 0))
-1e9*rms(detrend(data_m1_high_fb.Ry_int, 0))
-#+end_src
-
-*** Robust Control - 30rpm
-#+begin_src matlab
-%% Load Data
-data_tomo_30rpm_m0 = load(sprintf("%s/scans/2023-08-17_15-26_tomography_30rpm_m0_robust.mat", mat_dir));
-data_tomo_30rpm_m0.time = Ts*[0:length(data_tomo_30rpm_m0.Rz)-1];
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Measured motion during tomography scan at 30RPM with a robust controller
-figure;
-tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile;
-hold on;
-plot(1e6*data_tomo_30rpm_m0.Dx_int(data_tomo_30rpm_m0.hac_status == 0), 1e6*data_tomo_30rpm_m0.Dy_int(data_tomo_30rpm_m0.hac_status == 0), ...
-    'DisplayName', 'OL')
-plot(1e6*data_tomo_30rpm_m0.Dx_int(data_tomo_30rpm_m0.hac_status == 1), 1e6*data_tomo_30rpm_m0.Dy_int(data_tomo_30rpm_m0.hac_status == 1), ...
-    'DisplayName', 'CL')
-hold off;
-axis square
-xlabel("X motion [$\mu m$]");
-ylabel("Y motion [$\mu m$]");
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-xlim([-3, 3])
-ylim([-3, 3])
-
-ax2 = nexttile;
-hold on;
-plot(1e6*data_tomo_30rpm_m0.Dy_int(data_tomo_30rpm_m0.hac_status == 0), 1e6*data_tomo_30rpm_m0.Dz_int(data_tomo_30rpm_m0.hac_status == 0), ...
-    'DisplayName', 'OL')
-plot(1e6*data_tomo_30rpm_m0.Dy_int(data_tomo_30rpm_m0.hac_status == 1), 1e6*data_tomo_30rpm_m0.Dz_int(data_tomo_30rpm_m0.hac_status == 1), ...
-    'DisplayName', 'CL')
-hold off;
-axis equal
-xlim([-3, 3])
-ylim([-3, 3])
-xlabel("Y motion [$\mu m$]");
-ylabel("Z motion [$\mu m$]");
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_tomography_m0_30rpm_robust_xyz.pdf', 'width', 'full', 'height', 'tall');
-#+end_src
-
-#+name: fig:id31_tomography_m0_30rpm_robust_xyz
-#+caption: Measured motion during tomography scan at 30RPM with a robust controller
-#+RESULTS:
-[[file:figs/id31_tomography_m0_30rpm_robust_xyz.png]]
-
-#+begin_src matlab
-%% Compute RMS values while in closed-loop
-[~, i_m0] = find(data_tomo_30rpm_m0.hac_status == 1);
-data_tomo_30rpm_m0.Dx_rms_cl = rms(detrend(data_tomo_30rpm_m0.Dx_int(i_m0+50000:end), 0));
-data_tomo_30rpm_m0.Dy_rms_cl = rms(detrend(data_tomo_30rpm_m0.Dy_int(i_m0+50000:end), 0));
-data_tomo_30rpm_m0.Dz_rms_cl = rms(detrend(data_tomo_30rpm_m0.Dz_int(i_m0+50000:end), 0));
-data_tomo_30rpm_m0.Rx_rms_cl = rms(detrend(data_tomo_30rpm_m0.Rx_int(i_m0+50000:end), 0));
-data_tomo_30rpm_m0.Ry_rms_cl = rms(detrend(data_tomo_30rpm_m0.Ry_int(i_m0+50000:end), 0));
-
-data_tomo_30rpm_m0.Dx_rms_ol = rms(detrend(data_tomo_30rpm_m0.Dx_int(1:i_m0), 0));
-data_tomo_30rpm_m0.Dy_rms_ol = rms(detrend(data_tomo_30rpm_m0.Dy_int(1:i_m0), 0));
-data_tomo_30rpm_m0.Dz_rms_ol = rms(detrend(data_tomo_30rpm_m0.Dz_int(1:i_m0), 0));
-data_tomo_30rpm_m0.Rx_rms_ol = rms(detrend(data_tomo_30rpm_m0.Rx_int(1:i_m0), 0));
-data_tomo_30rpm_m0.Ry_rms_ol = rms(detrend(data_tomo_30rpm_m0.Ry_int(1:i_m0), 0));
-#+end_src
-
-#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-data2orgtable([1e6*data_tomo_30rpm_m0.Dx_rms_ol, 1e6*data_tomo_30rpm_m0.Dy_rms_ol, 1e9*data_tomo_30rpm_m0.Dz_rms_ol, 1e6*data_tomo_30rpm_m0.Rx_rms_ol, 1e6*data_tomo_30rpm_m0.Ry_rms_ol], ...
-              {'$m_0$'}, {'$D_x$ [$\mu m$]', '$D_y$ [$\mu m$]', '$D_z$ [$nm$]', '$R_x$ [$\mu\text{rad}$]', '$R_y$ [$\mu\text{rad}$]'}, ' %.0f ');
-#+end_src
-
-#+name: tab:id31_tomo_30rpm_robust_ol_errors
-#+caption: Measured error during open-loop tomography scans (30rpm)
-#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
-#+attr_latex: :center t :booktabs t
-#+RESULTS:
-|       | $D_x$ [$\mu m$] | $D_y$ [$\mu m$] | $D_z$ [$nm$] | $R_x$ [$\mu\text{rad}$] | $R_y$ [$\mu\text{rad}$] |
-|-------+-----------------+-----------------+--------------+-------------------------+-------------------------|
-| $m_0$ |               2 |               2 |           24 |                      10 |                      10 |
-
-#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-data2orgtable([1e9*data_tomo_30rpm_m0.Dx_rms_cl, 1e9*data_tomo_30rpm_m0.Dy_rms_cl, 1e9*data_tomo_30rpm_m0.Dz_rms_cl, 1e9*data_tomo_30rpm_m0.Rx_rms_cl, 1e9*data_tomo_30rpm_m0.Ry_rms_cl], ...
-              {'$m_0$'}, {'$D_x$ [nm]', '$D_y$ [nm]', '$D_z$ [nm]', '$R_x$ [nrad]', '$R_y$ [nrad]'}, ' %.0f ');
-#+end_src
-
-#+name: tab:id31_tomo_30rpm_robust_cl_errors
-#+caption: Measured error during closed-loop tomography scans (30rpm, robust controller)
-#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
-#+attr_latex: :center t :booktabs t
-#+RESULTS:
-|       | $D_x$ [nm] | $D_y$ [nm] | $D_z$ [nm] | $R_x$ [nrad] | $R_y$ [nrad] |
-|-------+------------+------------+------------+--------------+--------------|
-| $m_0$ |         34 |         38 |         10 |          127 |          129 |
-
-#+begin_src matlab :exports none
-yztomography3dmovie('movies/id31_tomography_m0_30rpm_robust_xyz.avi', data_tomo_30rpm_m0, 'di', 300);
-#+end_src
-
-#+begin_src matlab :exports none
-yztomographymovie('movies/tomography_30rpm_m0_robust', data_tomo_30rpm_m0, 'xlim_ax1', [-3, 3], 'ylim_ax1', [-3, 3], 'xlim_ax2', [-300, 300], 'ylim_ax2', [-300, 300])
-#+end_src
-
-*** TODO Fast Tomography Scan with Complementary Filter Controller :noexport:
-#+begin_src matlab
-data_cf = load(sprintf("%s/scans/2023-08-21_14-33_m0_30rpm_cf_control.mat", mat_dir));
-data_cf.time = Ts*[0:length(data_cf.Rz)-1];
-#+end_src
-
-#+begin_src matlab
-[~, i0] = find(data.hac_status == 1);
-1e9*rms(data.Dy_int(i0(1)+5000:end))
-1e9*rms(data.Dz_int(i0(1)+5000:end))
-
-1e9*rms(data_cf.Dy_int)
-1e9*rms(data_cf.Dz_int)
-#+end_src
-
-Same performance than the robust controller in terms of RMS residual motion.
-
-#+begin_src matlab
-figure; plot3(data.Dx_int, data.Dy_int, data.Dz_int)
-#+end_src
-
-*** Tomography - Effect of the scanning velocity                  :noexport:
-- [ ] What are the controller used here? Why worst results than with the robust controller?
-
-#+begin_src matlab
-data_1rpm = load(sprintf("%s/scans/2023-08-18_10-43_m0_1rpm.mat", mat_dir));
-data_1rpm.time = Ts*[0:length(data_1rpm.Rz)-1];
-#+end_src
-
-#+begin_src matlab
-data_30rpm = load(sprintf("%s/scans/2023-08-18_10-45_m0_30rpm.mat", mat_dir));
-data_30rpm.time = Ts*[0:length(data_30rpm.Rz)-1];
-#+end_src
-
-#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-data2orgtable([1e9*rms(detrend(data_1rpm.Dy_int, 0)), 1e9*rms(detrend(data_30rpm.Dy_int, 0)); 1e9*rms(detrend(data_1rpm.Dz_int, 0)), 1e9*rms(detrend(data_30rpm.Dz_int, 0)); 1e9*rms(detrend(data_1rpm.Ry_int, 0)), 1e9*rms(detrend(data_30rpm.Ry_int, 0))]', {'1RPM', '30RPM'}, {'$D_y$', '$D_z$', '$R_y$'}, ' %.1f ');
-#+end_src
-
-#+name: tab:id31_tomography_effect_velocity_rms
-#+caption: RMS values of the errors during tomography scan - Effect of $R_z$ velocity
-#+attr_latex: :environment tabularx :width 0.5\linewidth :align lXXX
-#+attr_latex: :center t :booktabs t
-#+RESULTS:
-|       | $D_y$ | $D_z$ | $R_y$ |
-|-------+-------+-------+-------|
-| 1RPM  |  30.9 |   5.9 |  92.4 |
-| 30RPM |  71.7 |  12.5 | 301.3 |
-
-
-#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-data2orgtable([1e9*data_tomo_1rpm_m0.Dx_rms_cl, 1e9*data_tomo_1rpm_m0.Dy_rms_cl, 1e9*data_tomo_1rpm_m0.Dz_rms_cl, 1e9*data_tomo_1rpm_m0.Rx_rms_cl, 1e9*data_tomo_1rpm_m0.Ry_rms_cl; ...
-               1e9*data_tomo_6rpm_m0.Dx_rms_cl, 1e9*data_tomo_6rpm_m0.Dy_rms_cl, 1e9*data_tomo_6rpm_m0.Dz_rms_cl, 1e9*data_tomo_6rpm_m0.Rx_rms_cl, 1e9*data_tomo_6rpm_m0.Ry_rms_cl; ...
-               1e9*data_tomo_30rpm_m0.Dx_rms_cl, 1e9*data_tomo_30rpm_m0.Dy_rms_cl, 1e9*data_tomo_30rpm_m0.Dz_rms_cl, 1e9*data_tomo_30rpm_m0.Rx_rms_cl, 1e9*data_tomo_30rpm_m0.Ry_rms_cl], ...
-              {'1rpm', '6rpm', '30rpm'}, {'$D_x$ [$\mu m$]', '$D_y$ [$\mu m$]', '$D_z$ [$nm$]', '$R_x$ [$\mu\text{rad}$]', '$R_y$ [$\mu\text{rad}$]'}, ' %.0f ');
-#+end_src
-
-#+RESULTS:
-|       | $D_x$ [$\mu m$] | $D_y$ [$\mu m$] | $D_z$ [$nm$] | $R_x$ [$\mu\text{rad}$] | $R_y$ [$\mu\text{rad}$] |
-|-------+-----------------+-----------------+--------------+-------------------------+-------------------------|
-| 1rpm  |              13 |              15 |            5 |                      57 |                      55 |
-| 6rpm  |              17 |              19 |            5 |                      70 |                      73 |
-| 30rpm |              34 |              38 |           10 |                     127 |                     129 |
-
-** $D_z$ scans: Dirty Layer Scans
-<<sec:id31_scans_dz>>
-*** Step by Step $D_z$ motion
-#+begin_src matlab
-%% Load Dz MIM data
-data_dz_steps_3nm = load(sprintf("%s/scans/2023-08-18_14-57_dz_mim_3_nm.mat", mat_dir));
-data_dz_steps_3nm.time = Ts*[0:length(data_dz_steps_3nm.Dz_int)-1];
-
-data_dz_steps_10nm = load(sprintf("%s/scans/2023-08-18_14-57_dz_mim_10_nm.mat", mat_dir));
-data_dz_steps_10nm.time = Ts*[0:length(data_dz_steps_10nm.Dz_int)-1];
-
-data_dz_steps_100nm = load(sprintf("%s/scans/2023-08-18_14-57_dz_mim_100_nm.mat", mat_dir));
-data_dz_steps_100nm.time = Ts*[0:length(data_dz_steps_100nm.Dz_int)-1];
-
-data_dz_steps_1000nm = load(sprintf("%s/scans/2023-08-18_14-57_dz_mim_1000_nm.mat", mat_dir));
-data_dz_steps_1000nm.time = Ts*[0:length(data_dz_steps_1000nm.Dz_int)-1];
-#+end_src
-
-Three step sizes are tested:
-- $10\,nm$ steps (Figure ref:fig:id31_dz_mim_10nm_steps)
-- $100\,nm$ steps (Figure ref:fig:id31_dz_mim_100nm_steps)
-- $1\,\mu m$ steps (Figure ref:fig:id31_dz_steps_response)
-
-#+begin_src matlab :exports none :results none
-%% Dz MIM test with 10nm steps
-figure;
-hold on;
-plot(data_dz_steps_10nm.time, 1e9*(data_dz_steps_10nm.Dz_int_filtered - mean(data_dz_steps_10nm.Dz_int_filtered(1:1000))))
-hold off;
-xlabel('Time [s]');
-ylabel('$D_z$ Motion [nm]');
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_dz_mim_10nm_steps.pdf', 'width', 'wide', 'height', 'normal');
-#+end_src
-
-#+name: fig:id31_dz_mim_10nm_steps
-#+caption: Dz MIM test with 10nm steps (low pass filter with cut-off frequency of 10Hz is applied)
-#+RESULTS:
-[[file:figs/id31_dz_mim_10nm_steps.png]]
-
-#+begin_src matlab :exports none :results none
-%% Dz MIM test with 10nm steps
-figure;
-hold on;
-plot(data_dz_steps_100nm.time, 1e9*(data_dz_steps_100nm.Dz_int - mean(data_dz_steps_100nm.Dz_int(1:1000))))
-hold off;
-xlabel('Time [s]');
-ylabel('$D_z$ Motion [nm]');
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_dz_mim_100nm_steps.pdf', 'width', 'wide', 'height', 'normal');
-#+end_src
-
-#+name: fig:id31_dz_mim_100nm_steps
-#+caption: Dz MIM test with 100nm steps
-#+RESULTS:
-[[file:figs/id31_dz_mim_100nm_steps.png]]
-
-#+begin_src matlab :exports none :results none
-%% Dz step response - Stabilization time is around 70ms
-figure;
-[~, i] = find(data_dz_steps_1000nm.m_hexa_dz>data_dz_steps_1000nm.m_hexa_dz(1));
-i0 = i(1);
-
-figure;
-hold on;
-plot(data_dz_steps_1000nm.time-data_dz_steps_1000nm.time(i0), 1e9*(data_dz_steps_1000nm.Dz_int - mean(data_dz_steps_1000nm.Dz_int(1:1000))))
-plot([-1, 1], [1000-20, 1000-20], 'k--')
-plot([-1, 1], [1000+20, 1000+20], 'k--')
-xline(0, 'k--', 'LineWidth', 1.5)
-xline(0.07, 'k--', 'LineWidth', 1.5)
-hold off;
-xlabel('Time [s]');
-ylabel('$D_z$ Motion [nm]');
-xlim([-0.1, 0.2]);
-ylim([-100, 1600])
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_dz_steps_response.pdf', 'width', 'wide', 'height', 'normal');
-#+end_src
-
-#+name: fig:id31_dz_steps_response
-#+caption: $D_z$ step response - Stabilization time is around 70ms
-#+RESULTS:
-[[file:figs/id31_dz_steps_response.png]]
-
-
-*** Continuous $D_z$ motion: Dirty Layer Scans
-#+begin_src matlab
-data_dz_10ums = load(sprintf("%s/scans/2023-08-18_15-33_dirty_layer_m0_small.mat", mat_dir));
-data_dz_10ums.time = Ts*[0:length(data_dz_10ums.Dz_int)-1];
-#+end_src
-
-#+begin_src matlab
-data_dz_100ums = load(sprintf("%s/scans/2023-08-18_15-32_dirty_layer_m0.mat", mat_dir));
-data_dz_100ums.time = Ts*[0:length(data_dz_100ums.Dz_int)-1];
-#+end_src
-
-Two $D_z$ scans are performed:
-- at $10\,\mu m/s$ in Figure ref:fig:id31_dirty_layer_scan_m0
-- at $100\,\mu m/s$ in Figure ref:fig:id31_dirty_layer_scan_m0_large
-
-#+begin_src matlab :exports none :results none
-%% Dirty layer scan: Dz motion
-figure;
-hold on;
-plot(data_dz_10ums.time, 1e6*data_dz_10ums.Dz_int, ...
-    'DisplayName', sprintf('$\\epsilon D_z = %.0f$ nm RMS', 1e9*rms(data_dz_10ums.e_dz)))
-plot(data_dz_10ums.time, 1e6*data_dz_10ums.e_dy, ...
-    'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nm RMS', 1e9*rms(data_dz_10ums.e_dy)))
-plot(data_dz_10ums.time, 1e6*data_dz_10ums.e_ry, ...
-    'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$rad RMS', 1e6*rms(data_dz_10ums.e_ry)))
-hold off;
-xlabel('Time [s]');
-ylabel('Motion [$\mu$m]');
-xlim([0, 2.2])
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_dirty_layer_scan_m0.pdf', 'width', 'wide', 'height', 'normal');
-#+end_src
-
-#+name: fig:id31_dirty_layer_scan_m0
-#+caption: Dirty layer scan: $D_z$ motion at $10\,\mu m/s$
-#+RESULTS:
-[[file:figs/id31_dirty_layer_scan_m0.png]]
-
-#+begin_src matlab :exports none :results none
-%% Dirty layer scan: Dz motion
-figure;
-hold on;
-plot(data_dz_100ums.time, 1e6*data_dz_100ums.Dz_int, ...
-    'DisplayName', sprintf('$\\epsilon D_z = %.0f$ nm RMS', 1e9*rms(data_dz_100ums.e_dz)))
-plot(data_dz_100ums.time, 1e6*data_dz_100ums.e_dy, ...
-    'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nm RMS', 1e9*rms(data_dz_100ums.e_dy)))
-plot(data_dz_100ums.time, 1e6*data_dz_100ums.e_ry, ...
-    'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$rad RMS', 1e6*rms(data_dz_100ums.e_ry)))
-hold off;
-xlabel('Time [s]');
-ylabel('Motion [$\mu$m]');
-xlim([0, 2.2])
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_dirty_layer_scan_m0_large.pdf', 'width', 'wide', 'height', 'normal');
-#+end_src
-
-#+name: fig:id31_dirty_layer_scan_m0_large
-#+caption: Dirty layer scan: $D_z$ motion at $100\,\mu m/s$
-#+RESULTS:
-[[file:figs/id31_dirty_layer_scan_m0_large.png]]
-
-#+begin_src matlab
-%% Not so good results with the CF controller
-data_cf = load(sprintf("%s/scans/2023-08-21_19-20_dirty_layer_m1_cf.mat", mat_dir));
-data_cf.time = Ts*[0:length(data_cf.Dz_int)-1];
-#+end_src
-
-** $R_y$ scans: Reflectivity
-<<sec:id31_scans_reflectivity>>
-#+begin_src matlab
-%% Load data for the reflectivity scan
-data_ry = load(sprintf("%s/scans/2023-08-18_15-24_first_reflectivity_m0.mat", mat_dir));
-data_ry.time = Ts*[0:length(data_ry.Ry_int)-1];
-#+end_src
-
-An $R_y$ scan is performed at $100\,\mu rad/s$ velocity (Figure ref:fig:id31_reflectivity_scan_Ry_100urads).
-During the $R_y$ scan, the errors in $D_y$ are $D_z$ are kept small.
-
-#+begin_src matlab :exports none :results none
-%% Ry reflectivity scan
-figure;
-hold on;
-plot(data_ry.time, 1e6*data_ry.Ry_int, 'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$rad RMS', 1e6*rms(data_ry.e_ry)))
-plot(data_ry.time, 1e6*data_ry.e_dy, 'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nm RMS', 1e9*rms(data_ry.e_dy)))
-plot(data_ry.time, 1e6*data_ry.e_dz, 'DisplayName', sprintf('$\\epsilon D_z = %.0f$ nm RMS', 1e9*rms(data_ry.e_dz)))
-hold off;
-xlabel('Time [s]');
-ylabel('$R_y$ motion [$\mu$rad]')
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
-xlim([0, 6.2]);
-ylim([-310, 310])
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_reflectivity_scan_Ry_100urads.pdf', 'width', 'wide', 'height', 'normal');
-#+end_src
-
-#+name: fig:id31_reflectivity_scan_Ry_100urads
-#+caption: $R_y$ reflecitivity scan at $100\,\mu\text{rad}/s$ velocity
-#+RESULTS:
-[[file:figs/id31_reflectivity_scan_Ry_100urads.png]]
-
-
-** $D_y$ Scans
-<<sec:id31_scans_dy>>
-*** Introduction                                                    :ignore:
-The steps generated by the IcePAP for the $T_y$ stage are send to the Speedgoat.
-Then, we can know in real time what is the wanted position in $D_y$ during $T_y$ scans.
-
-*** Open Loop
-#+begin_src matlab
-%% Slow Ty scan (10um/s)
-data_ty_ol_slow = load(sprintf("%s/scans/2023-08-21_20-05_ty_scan_m1_open_loop_slow.mat", mat_dir));
-data_ty_ol_slow.time = Ts*[0:length(data_ty_ol_slow.Dy_int)-1];
-#+end_src
-
-We can clearly see micro-stepping errors of the stepper motor used for the $T_y$ stage.
-The errors have a period of $10\,\mu m$ with an amplitude of $\pm 100\,nm$.
-
-#+begin_src matlab :exports none :results none
-%% Ty scan (at 10um/s) - Dy errors
-figure;
-plot(1e6*data_ty_ol_slow.Ty, 1e6*detrend(data_ty_ol_slow.e_dy, 0))
-xlabel('Ty position [$\mu$m]');
-ylabel('$D_y$ error [$\mu$m]');
-xlim([-100, 100])
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_ty_scan_10ums_ol_dy_errors.pdf', 'width', 'wide', 'height', 'normal');
-#+end_src
-
-#+name: fig:id31_ty_scan_10ums_ol_dy_errors
-#+caption: $T_y$ scan (at $10\,\mu m/s$) - $D_y$ errors. The micro-stepping errors can clearly be seen with a period of $10\,\mu m$ and an amplitude of $\pm 100\,nm$
-#+RESULTS:
-[[file:figs/id31_ty_scan_10ums_ol_dy_errors.png]]
-
-#+begin_src matlab :exports none :results none
-%% Ty scan (at 10um/s) - Dz and Ry errors
-figure;
-tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile;
-hold on;
-plot(1e6*data_ty_ol_slow.Ty, 1e6*detrend(data_ty_ol_slow.e_dz, 0))
-hold off;
-xlabel('Ty position [$\mu$m]');
-ylabel('$D_z$ error [$\mu$m]');
-xlim([-100, 100])
-
-ax2 = nexttile;
-hold on;
-plot(1e6*data_ty_ol_slow.Ty, 1e6*data_ty_ol_slow.e_ry)
-hold off;
-xlabel('Ty position [$\mu$m]');
-ylabel('$R_y$ error [$\mu$rad]');
-xlim([-100, 100])
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_ty_scan_10ums_ol_dz_ry_errors.pdf', 'width', 'full', 'height', 'normal');
-#+end_src
-
-#+name: fig:id31_ty_scan_10ums_ol_dz_ry_errors
-#+caption: $T_y$ scan (at $10\,\mu m/s$) - $D_z$ and $R_y$ errors. The $D_z$ error is most likely due to having the top interferometer pointing to a sphere. The large $R_y$ errors might also be due to the metrology system
-#+RESULTS:
-[[file:figs/id31_ty_scan_10ums_ol_dz_ry_errors.png]]
-
-*** Closed Loop
-#+begin_src matlab
-%% Slow Ty scan (10um/s) - CL
-data_ty_cl_slow = load(sprintf("%s/scans/2023-08-21_20-07_ty_scan_m1_cf_closed_loop_slow.mat", mat_dir));
-data_ty_cl_slow.time = Ts*[0:length(data_ty_cl_slow.Dy_int)-1];
-#+end_src
-
-#+begin_src matlab :exports none :results none
-%% Ty scan (at 10um/s) - Dy errors
-figure;
-hold on;
-plot(1e6*data_ty_ol_slow.Ty, 1e6*detrend(data_ty_ol_slow.e_dy, 0), ...
-     'DisplayName', 'OL')
-plot(1e6*data_ty_cl_slow.Ty, 1e6*detrend(data_ty_cl_slow.e_dy, 0), ...
-     'DisplayName', sprintf('CL - $\\epsilon D_y = %.0f$ nmRMS', 1e9*rms(detrend(data_ty_cl_slow.e_dy, 0))))
-hold off;
-xlabel('Ty position [$\mu$m]');
-ylabel('$D_y$ error [$\mu$m]');
-xlim([-100, 100])
-legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_ty_scan_10ums_cl_dy_errors.pdf', 'width', 'wide', 'height', 'normal');
-#+end_src
-
-#+name: fig:id31_ty_scan_10ums_cl_dy_errors
-#+caption: $T_y$ scan (at $10\,\mu m/s$) - $D_y$ errors. Open-loop and Closed-loop scans
-#+RESULTS:
-[[file:figs/id31_ty_scan_10ums_cl_dy_errors.png]]
-
-#+begin_src matlab :exports none :results none
-%% Ty scan (at 10um/s) - Dz and Ry errors
-figure;
-tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile;
-hold on;
-plot(1e6*data_ty_ol_slow.Ty, 1e6*detrend(data_ty_ol_slow.e_dz, 0), ...
-     'DisplayName', 'OL')
-plot(1e6*data_ty_cl_slow.Ty, 1e6*detrend(data_ty_cl_slow.e_dz, 0), ...
-     'DisplayName', sprintf('Cl - $\\epsilon D_z = %.0f$ nmRMS', 1e9*rms(detrend(data_ty_cl_slow.e_dz, 0))))
-hold off;
-xlabel('Ty position [$\mu$m]');
-ylabel('$D_z$ error [$\mu$m]');
-xlim([-100, 100])
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(1e6*data_ty_ol_slow.Ty, 1e6*data_ty_ol_slow.e_ry, ...
-     'DisplayName', 'OL')
-plot(1e6*data_ty_cl_slow.Ty, 1e6*data_ty_cl_slow.e_ry, ...
-     'DisplayName', sprintf('Cl - $\\epsilon R_y = %.2f$ uradRMS', 1e6*rms(detrend(data_ty_cl_slow.e_ry, 0))))
-hold off;
-xlabel('Ty position [$\mu$m]');
-ylabel('$R_y$ error [$\mu$rad]');
-xlim([-100, 100])
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_ty_scan_10ums_cl_dz_ry_errors.pdf', 'width', 'full', 'height', 'normal');
-#+end_src
-
-#+name: fig:id31_ty_scan_10ums_cl_dz_ry_errors
-#+caption: $T_y$ scan (at $10\,\mu m/s$) - $D_z$ and $R_y$ errors. Open-loop and Closed-loop scans
-#+RESULTS:
-[[file:figs/id31_ty_scan_10ums_cl_dz_ry_errors.png]]
-
-*** Faster Scan
-#+begin_src matlab
-%% Fast Ty scan (100um/s) - OL
-data_ty_ol_fast = load(sprintf("%s/scans/2023-08-21_20-05_ty_scan_m1_open_loop.mat", mat_dir));
-data_ty_ol_fast.time = Ts*[0:length(data_ty_ol_fast.Dy_int)-1];
-#+end_src
-
-#+begin_src matlab
-%% Fast Ty scan (10um/s) - CL
-data_ty_cl_fast = load(sprintf("%s/scans/2023-08-21_20-07_ty_scan_m1_cf_closed_loop.mat", mat_dir));
-data_ty_cl_fast.time = Ts*[0:length(data_ty_cl_fast.Dy_int)-1];
-#+end_src
-
-Because of micro-stepping errors of the Ty stepper motor, when scanning at high velocity this induce high frequency vibration that are outside the bandwidth of the feedback controller.
-
-At $100\,\mu m/s$, the micro-stepping errors with a period of $10\,\mu m$ (see Figure ref:fig:id31_ty_scan_10ums_ol_dy_errors) are at 10Hz.
-These errors are them amplified by some resonances in the system.
-
-This could be easily solved by changing the stepper motor for a torque motor for instance.
-
-#+begin_src matlab :exports none :results none
-%% Ty scan (at 100um/s) - Dy errors
-figure;
-hold on;
-plot(1e6*data_ty_ol_fast.Ty, 1e6*detrend(data_ty_ol_fast.e_dy, 0), ...
-     'DisplayName', 'OL')
-plot(1e6*data_ty_cl_fast.Ty, 1e6*detrend(data_ty_cl_fast.e_dy, 0), ...
-     'DisplayName', sprintf('CL - $\\epsilon D_y = %.0f$ nmRMS', 1e9*rms(detrend(data_ty_cl_fast.e_dy, 0))))
-hold off;
-xlabel('Ty position [$\mu$m]');
-ylabel('$D_y$ error [$\mu$m]');
-xlim([-100, 100])
-legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_ty_scan_100ums_cl_dy_errors.pdf', 'width', 'wide', 'height', 'normal');
-#+end_src
-
-#+name: fig:id31_ty_scan_100ums_cl_dy_errors
-#+caption: $T_y$ scan (at $100\,\mu m/s$) - $D_y$ errors. Open-loop and Closed-loop scans
-#+RESULTS:
-[[file:figs/id31_ty_scan_100ums_cl_dy_errors.png]]
-
-#+begin_src matlab :exports none :results none
-%% Ty scan (at 100um/s) - Dz and Ry errors
-figure;
-tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile;
-hold on;
-plot(1e6*data_ty_ol_fast.Ty, 1e6*detrend(data_ty_ol_fast.e_dz, 0), ...
-     'DisplayName', 'OL')
-plot(1e6*data_ty_cl_fast.Ty, 1e6*detrend(data_ty_cl_fast.e_dz, 0), ...
-     'DisplayName', sprintf('Cl - $\\epsilon D_z = %.0f$ nmRMS', 1e9*rms(detrend(data_ty_cl_fast.e_dz, 0))))
-hold off;
-xlabel('Ty position [$\mu$m]');
-ylabel('$D_z$ error [$\mu$m]');
-xlim([-100, 100])
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
-
-ax2 = nexttile;
-hold on;
-plot(1e6*data_ty_ol_fast.Ty, 1e6*data_ty_ol_fast.e_ry, ...
-     'DisplayName', 'OL')
-plot(1e6*data_ty_cl_fast.Ty, 1e6*data_ty_cl_fast.e_ry, ...
-     'DisplayName', sprintf('Cl - $\\epsilon R_y = %.2f$ uradRMS', 1e6*rms(detrend(data_ty_cl_fast.e_ry, 0))))
-hold off;
-xlabel('Ty position [$\mu$m]');
-ylabel('$R_y$ error [$\mu$rad]');
-xlim([-100, 100])
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_ty_scan_100ums_cl_dz_ry_errors.pdf', 'width', 'full', 'height', 'normal');
-#+end_src
-
-#+name: fig:id31_ty_scan_100ums_cl_dz_ry_errors
-#+caption: $T_y$ scan (at $100\,\mu m/s$) - $D_z$ and $R_y$ errors. Open-loop and Closed-loop scans
-#+RESULTS:
-[[file:figs/id31_ty_scan_100ums_cl_dz_ry_errors.png]]
-
-** Combined $R_z$ and $D_y$: Diffraction Tomography
-<<sec:id31_scans_diffraction_tomo>>
-Instead of doing a fast $R_z$ motion a slow $D_y$, the idea is to perform slow $R_z$ (here 1rpm) and fast $D_y$ scans with the nano-hexapod.
-
-#+begin_src matlab
-%% 100um/s - Robust controller
-data_dt_100ums = load(sprintf("%s/scans/2023-08-18_17-12_diffraction_tomo_m0.mat", mat_dir));
-data_dt_100ums.time = Ts*[0:length(data_dt_100ums.Dy_int)-1];
-
-%% 500um/s - Robust controller (Not used)
-% data_dt_500ums = load(sprintf("%s/scans/2023-08-18_17-19_diffraction_tomo_m0_fast.mat", mat_dir));
-% data_dt_500ums.time = Ts*[0:length(data_dt_500ums.Dy_int)-1];
-
-%% 500um/s - Complementary filters
-data_dt_500ums = load(sprintf("%s/scans/2023-08-21_15-15_diffraction_tomo_m0_fast_cf.mat", mat_dir));
-data_dt_500ums.time = Ts*[0:length(data_dt_500ums.Dy_int)-1];
-
-%% 1mm/s - Complementary filters
-data_dt_1000ums = load(sprintf("%s/scans/2023-08-21_15-16_diffraction_tomo_m0_fast_cf.mat", mat_dir));
-data_dt_1000ums.time = Ts*[0:length(data_dt_1000ums.Dy_int)-1];
-
-%% 5mm/s - Complementary filters
-% data_dt_5000ums = load(sprintf("%s/scans/2023-08-21_18-03_diffraction_tomo_m2_fast_cf.mat", mat_dir));
-% data_dt_5000ums.time = Ts*[0:length(data_dt_5000ums.Dy_int)-1];
-
-%% 10mm/s - Complementary filters
-data_dt_10000ums = load(sprintf("%s/scans/2023-08-21_15-17_diffraction_tomo_m0_fast_cf.mat", mat_dir));
-data_dt_10000ums.time = Ts*[0:length(data_dt_10000ums.Dy_int)-1];
-#+end_src
-
-Here, the $D_y$ scans are performed only with the nano-hexapod (the Ty stage is not moving), so we are limited to $\pm 100\,\mu m$.
-
-Several $D_y$ velocities are tested: $0.1\,mm/s$, $0.5\,mm/s$, $1\,mm/s$ and $10\,mm/s$ (see Figure ref:fig:id31_diffraction_tomo_velocities).
-
-#+begin_src matlab :exports none :results none
-%% Dy motion for several configured velocities
-figure;
-hold on;
-plot(data_dt_10000ums.time, 1e6*data_dt_10000ums.Dy_int, ...
-     'DisplayName', '$10 mm/s$')
-plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.Dy_int, ...
-     'DisplayName', '$1 mm/s$')
-plot(data_dt_500ums.time, 1e6*data_dt_500ums.Dy_int, ...
-     'DisplayName', '$0.5 mm/s$')
-plot(data_dt_100ums.time, 1e6*data_dt_100ums.Dy_int, ...
-     'DisplayName', '$0.1 mm/s$')
-hold off;
-xlim([0, 4]);
-xlabel('Time [s]');
-ylabel('$D_y$ position [$\mu$m]')
-legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_diffraction_tomo_velocities.pdf', 'width', 'wide', 'height', 'normal');
-#+end_src
-
-#+name: fig:id31_diffraction_tomo_velocities
-#+caption: Dy motion for several configured velocities
-#+RESULTS:
-[[file:figs/id31_diffraction_tomo_velocities.png]]
-
-The corresponding "repetition rate" and $D_y$ scan per spindle turn are shown in Table ref:tab:diffraction_tomo_velocities.
-
-The main issue here is the "waiting" time between two scans that is in the order of 50ms.
-By removing this waiting time (fairly easily), we can double the repetition rate at 10mm/s.
-
-#+name: tab:diffraction_tomo_velocities
-#+caption: $D_y$ scaning repetition rate
-#+attr_latex: :environment tabularx :width 0.6\linewidth :align lXX
-#+attr_latex: :center t :booktabs t
-| $D_y$ Velocity | Repetition rate | Scans per turn (at 1RPM) |
-|----------------+-----------------+--------------------------|
-| 0.1 mm/s       | 4 s             |                       15 |
-| 0.5 mm/s       | 0.9 s           |                       65 |
-| 1 mm/s         | 0.5 s           |                      120 |
-| 10 mm/s        | 0.18 s          |                      330 |
-
-
-The scan results for a velocity of 1mm/s is shown in Figure ref:fig:id31_diffraction_tomo_1mms.
-The $D_z$ and $R_y$ errors are quite small during the scan.
-
-The $D_y$ errors are quite large as the velocity is increased.
-This type of scan can probably be massively improved by using feed-forward and optimizing the trajectory.
-Also, if the detectors are triggered in position (the Speedgoat could generate an encoder signal for instance), we don't care about the $D_y$ errors.
-
-#+begin_src matlab :exports none :results none
-%% Diffraction tomography with Dy velocity of 1mm/s and Rz velocity of 1RPM
-figure;
-hold on;
-plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.Dz_int, ...
-     'DisplayName', sprintf('$\\epsilon D_z = %.0f$ nmRMS', 1e9*rms(data_dt_1000ums.Dz_int)))
-plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.Ry_int, ...
-     'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$radRMS', 1e6*rms(data_dt_1000ums.Ry_int)))
-plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.Dy_int, ...
-     'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nmRMS', 1e9*rms(data_dt_1000ums.Dy_int - data_dt_1000ums.m_hexa_dy)))
-plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.m_hexa_dy, 'k--', 'HandleVisibility', 'off')
-hold off;
-xlim([0, 1])
-xlabel('Time [s]');
-ylabel('Measurement [nm,nrad]')
-legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
-ylim([-110, 110])
-#+end_src
-
-#+begin_src matlab :tangle no :exports results :results file replace
-exportFig('figs/id31_diffraction_tomo_1mms.pdf', 'width', 'full', 'height', 'normal');
-#+end_src
-
-#+name: fig:id31_diffraction_tomo_1mms
-#+caption: Diffraction tomography with Dy velocity of 1mm/s and Rz velocity of 1RPM
-#+RESULTS:
-[[file:figs/id31_diffraction_tomo_1mms.png]]
-
-#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-data2orgtable([1e9*rms(data_dt_100ums.Dy_int   - data_dt_100ums.m_hexa_dy), 1e9*rms(data_dt_500ums.Dy_int   - data_dt_500ums.m_hexa_dy), 1e9*rms(data_dt_1000ums.Dy_int  - data_dt_1000ums.m_hexa_dy), 1e9*rms(data_dt_10000ums.Dy_int - data_dt_10000ums.m_hexa_dy);
-1e9*rms(data_dt_100ums.Dz_int), 1e9*rms(data_dt_500ums.Dz_int), 1e9*rms(data_dt_1000ums.Dz_int), 1e9*rms(data_dt_10000ums.Dz_int);
-1e6*rms(data_dt_100ums.Ry_int), 1e6*rms(data_dt_500ums.Ry_int), 1e6*rms(data_dt_1000ums.Ry_int), 1e6*rms(data_dt_10000ums.Ry_int)]', {'0.1 mm/s' ,'0.5 mm/s', '1 mm/s', '10 mm/s'}, {'Velocity', '$D_y$ [nmRMS]', '$D_z$ [nmRMS]', '$R_y$ [$\mu\text{radRMS}$]'}, ' %.1f ');
-#+end_src
-
-#+name: tab:id31_diffraction_tomo_results
-#+caption: Obtained errors for several $D_y$ velocities
-#+attr_latex: :environment tabularx :width \linewidth :align lXX
-#+attr_latex: :center t :booktabs t
-#+RESULTS:
-| Velocity | $D_y$ [nmRMS] | $D_z$ [nmRMS] | $R_y$ [$\mu\text{radRMS}$] |
-|----------+---------------+---------------+----------------------------|
-| 0.1 mm/s |          75.5 |           9.1 |                        0.1 |
-| 0.5 mm/s |         190.5 |          10.0 |                        0.1 |
-| 1 mm/s   |         428.0 |          11.2 |                        0.2 |
-| 10 mm/s  |        4639.9 |          55.9 |                        1.4 |
-
-** Summary of experiments
-For each conducted experiments, the $D_y$, $D_z$ and $R_y$ errors are computed and summarized in Table ref:tab:id31_experiments_results_summary.
-
-#+begin_src matlab
-%% Summary of results
-data_results = [...
-    1e9*data_tomo_1rpm_m0.Dy_rms_cl,                                      1e9*data_tomo_1rpm_m0.Dz_rms_cl,            1e9*data_tomo_1rpm_m0.Ry_rms_cl           ; ... % Tomo 1rpm
-    1e9*data_tomo_6rpm_m0.Dy_rms_cl,                                      1e9*data_tomo_6rpm_m0.Dz_rms_cl,            1e9*data_tomo_6rpm_m0.Ry_rms_cl           ; ... % Tomo 6rpm
-    1e9*data_tomo_30rpm_m0.Dy_rms_cl,                                     1e9*data_tomo_30rpm_m0.Dz_rms_cl,           1e9*data_tomo_30rpm_m0.Ry_rms_cl          ; ... % Tomo 30rpm
-    1e9*rms(detrend(data_dz_10ums.e_dy, 0)),                              1e9*rms(detrend(data_dz_10ums.e_dz, 0)),    1e9*rms(detrend(data_dz_10ums.e_ry, 0))   ; ... % Dz 10um/s
-    1e9*rms(detrend(data_dz_100ums.e_dy,0)),                              1e9*rms(detrend(data_dz_100ums.e_dz,0)),    1e9*rms(detrend(data_dz_100ums.e_ry,0))   ; ... % Dz 100um/s
-    1e9*rms(detrend(data_ry.e_dy,0)),                                     1e9*rms(detrend(data_ry.e_dz,0)),           1e9*rms(detrend(data_ry.e_ry,0))          ; ... % Ry 100urad/s
-    1e9*rms(detrend(data_ty_cl_slow.e_dy, 0)),                            1e9*rms(detrend(data_ty_cl_slow.e_dz, 0)),  1e9*rms(detrend(data_ty_cl_slow.e_rz, 0)) ; ... % Dy 10 um/s
-    1e9*rms(detrend(data_dt_100ums.Dy_int-data_dt_100ums.m_hexa_dy,  0)), 1e9*rms(detrend(data_dt_100ums.Dz_int, 0)), 1e9*rms(detrend(data_dt_100ums.Ry_int, 0)); ... % Diffraction tomo 0.1mm/s
-    1e9*rms(detrend(data_dt_1000ums.Dy_int-data_dt_1000ums.m_hexa_dy,0)), 1e9*rms(detrend(data_dt_1000ums.Dz_int,0)), 1e9*rms(detrend(data_dt_1000ums.Ry_int,0))  ... % Diffraction tomo 1mm/s
-];
-#+end_src
-
-#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-data2orgtable(data_results, {'Tomography ($R_z$ 1rpm)', 'Tomography ($R_z$ 6rpm)', 'Tomography ($R_z$ 30rpm)', 'Dirty Layer ($D_z$ $10\,\mu m/s$)', 'Dirty Layer ($D_z$ $100\,\mu m/s$)', 'Reflectivity ($R_y$ $100\,\mu\text{rad}/s$)', 'Lateral Scan ($D_y$ $10\,\mu m/s$)', 'Diffraction Tomography ($R_z$ 1rpm, $D_y$ 0.1mm/s)', 'Diffraction Tomography ($R_z$ 1rpm, $D_y$ 1mm/s)'}, {'$D_y$ [nmRMS]', '$D_z$ [nmRMS]', '$R_y$ [nradRMS]'}, ' %.0f ');
-#+end_src
-
-#+name: tab:id31_experiments_results_summary
-#+caption: Table caption
-#+attr_latex: :environment tabularx :width \linewidth :align Xccc
-#+attr_latex: :center t :booktabs t
-#+RESULTS:
-|                                                    | $D_y$ [nmRMS] | $D_z$ [nmRMS] | $R_y$ [nradRMS] |
-|----------------------------------------------------+---------------+---------------+-----------------|
-| Tomography ($R_z$ 1rpm)                            |            15 |             5 |              55 |
-| Tomography ($R_z$ 6rpm)                            |            19 |             5 |              73 |
-| Tomography ($R_z$ 30rpm)                           |            38 |            10 |             129 |
-| Dirty Layer ($D_z$ $10\,\mu m/s$)                  |            25 |             5 |             114 |
-| Dirty Layer ($D_z$ $100\,\mu m/s$)                 |            34 |            15 |             130 |
-| Reflectivity ($R_y$ $100\,\mu\text{rad}/s$)        |            28 |             6 |             118 |
-| Lateral Scan ($D_y$ $10\,\mu m/s$)                 |            21 |            10 |              37 |
-| Diffraction Tomography ($R_z$ 1rpm, $D_y$ 0.1mm/s) |            75 |             9 |             118 |
-| Diffraction Tomography ($R_z$ 1rpm, $D_y$ 1mm/s)   |           428 |            11 |             169 |
+xticks(-100:50:100); yticks(-100:50:100); zticks(-1:0.2:0);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file none
+exportFig('figs/test_id31_xy_map_sphere.pdf', 'width', 'half', 'height', 'normal');
+#+end_src
+
+#+name: fig:test_id31_metrology_errors
+#+caption: Estimated measurement errors of the metrology. Cross-coupling between lateral motion and vertical measurement is shown in (\subref{fig:test_id31_xy_map_sphere}). Effect of interferometer noise on the measured translations and rotations is shown in (\subref{fig:test_id31_interf_noise}).
+#+attr_latex: :options [htbp]
+#+begin_figure
+#+attr_latex: :caption \subcaption{\label{fig:test_id31_xy_map_sphere}Z measurement during an XY mapping}
+#+attr_latex: :options {0.49\textwidth}
+#+begin_subfigure
+#+attr_latex: :width 0.95\linewidth
+[[file:figs/test_id31_xy_map_sphere.png]]
+#+end_subfigure
+#+attr_latex: :caption \subcaption{\label{fig:test_id31_interf_noise}Interferometer noise}
+#+attr_latex: :options {0.49\textwidth}
+#+begin_subfigure
+#+attr_latex: :width 0.95\linewidth
+[[file:figs/test_id31_interf_noise.png]]
+#+end_subfigure
+#+end_figure
 
 * Bibliography                                                        :ignore:
 #+latex: \printbibliography[heading=bibintoc,title={Bibliography}]
@@ -10338,21 +636,28 @@ data2orgtable(data_results, {'Tomography ($R_z$ 1rpm)', 'Tomography ($R_z$ 6rpm)
 ** Initialize Path
 #+NAME: m-init-path
 #+BEGIN_SRC matlab
-%% Path for functions, data and scripts
-addpath('./matlab/STEPS/'); % Path for STEPS files
-addpath('./matlab/subsystems/'); % Path for Simulink subsystems
-addpath('./matlab/mat/'); % Path for data
-addpath('./matlab/src/'); % Path for functions
 addpath('./matlab/');     % Path for scripts
+
+%% Path for functions, data and scripts
+addpath('./matlab/mat/'); % Path for Computed FRF
+addpath('./matlab/src/'); % Path for functions
+addpath('./matlab/STEPS/'); % Path for STEPS
+addpath('./matlab/subsystems/'); % Path for Subsystems Simulink files
+
+%% Data directory
+data_dir = './matlab/mat/'
 #+END_SRC
 
 #+NAME: m-init-path-tangle
 #+BEGIN_SRC matlab
 %% Path for functions, data and scripts
-addpath('./STEPS/'); % Path for STEPS files
-addpath('./subsystems/'); % Path for Simulink subsystems
-addpath('./mat/'); % Path for data
+addpath('./mat/'); % Path for Data
 addpath('./src/'); % Path for functions
+addpath('./STEPS/'); % Path for STEPS
+addpath('./subsystems/'); % Path for Subsystems Simulink files
+
+%% Data directory
+data_dir = './mat/'
 #+END_SRC
 
 ** Initialize other elements
@@ -10362,8 +667,9 @@ addpath('./src/'); % Path for functions
 colors = colororder;
 freqs = logspace(0, 3, 1000);
 Ts = 1e-4;
-data_dir = "/home/thomas/mnt/data_id31/id31nass/id31/20230801/RAW_DATA"
-mat_dir = "/home/thomas/mnt/data_id31/nass"
+
+% data_dir = "/home/thomas/mnt/data_id31/id31nass/id31/20230801/RAW_DATA"
+% mat_dir = "/home/thomas/mnt/data_id31/nass"
 #+END_SRC
 
 ** =h5scan= - Easily load h5 files
@@ -10371,8 +677,6 @@ mat_dir = "/home/thomas/mnt/data_id31/nass"
 :header-args:matlab+: :tangle matlab/src/h5scan.m
 :header-args:matlab+: :comments none :mkdirp yes :eval no
 :END:
-<<sec:h5scan>>
-
 #+begin_src matlab
 function [cntrs,tp] = h5scan(pth,smp,ds,sn,varargin)
 
@@ -10478,4269 +782,46 @@ function A = vrtlds(f,nm,det)
 % H5F.close(fid);
 #+end_src
 
-** =circlefit=
+** =sphereFit= - Fit sphere from x,y,z points
 :PROPERTIES:
-:header-args:matlab+: :tangle matlab/src/circlefit.m
+:header-args:matlab+: :tangle matlab/src/sphereFit.m
 :header-args:matlab+: :comments none :mkdirp yes :eval no
 :END:
-<<sec:h5scan>>
 
 #+begin_src matlab
-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));
-#+end_src
-
-** =yztomography3dmovie=
-:PROPERTIES:
-:header-args:matlab+: :tangle matlab/src/yztomography3dmovie.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:yztomography3dmovie>>
-
-#+begin_src matlab
-function   [] = yztomography3dmovie(filename, data, args)
-#+end_src
-
-#+begin_src matlab
-  arguments
-    filename
-    data
-    args.di      (1,1) double {mustBeNumeric} = 500
-    args.xlim    (2,1) double {mustBeNumeric} = [-3, 3]
-    args.ylim    (2,1) double {mustBeNumeric} = [-3, 3]
-    args.zlim    (2,1) double {mustBeNumeric} = [-0.4, 0.4]
-    args.view_az (1,1) double {mustBeNumeric} = -70
-    args.view_el (1,1) double {mustBeNumeric} = 5
-  end
-#+end_src
-
-#+begin_src matlab
-colors = colororder;
-#+end_src
-
-#+begin_src matlab
-writerObj = VideoWriter(filename); % initialize the VideoWriter object
-open(writerObj);
-#+end_src
-
-#+begin_src matlab
-fig = figure;
-tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile;
-#+end_src
-
-#+begin_src matlab
-di = args.di;
-for i = 1:floor(length(data.Dx_int)/di)-1
-    if data.hac_status(di*(i+1)-1) == 0
-        % Only open loop
-        plot3(ax1, 1e6*data.Dx_int(di*i:di*(i+1)-1), 1e6*data.Dy_int(di*i:di*(i+1)-1), 1e6*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(1,:))
-    elseif data.hac_status(di*i) == 1
-        % Only closed loop
-        plot3(ax1, 1e6*data.Dx_int(di*i:di*(i+1)-1), 1e6*data.Dy_int(di*i:di*(i+1)-1), 1e6*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(2,:))
-    else
-        % Both open and closed loop
-        Dx_int = data.Dx_int(di*i:di*(i+1)-1);
-        Dy_int = data.Dy_int(di*i:di*(i+1)-1);
-        Dz_int = data.Dz_int(di*i:di*(i+1)-1);
-        plot3(ax1, 1e6*Dx_int(data.hac_status(di*i:di*(i+1)-1) == 0), 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 0), 1e6*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 0), '-', 'color', colors(1,:))
-        plot3(ax1, 1e6*Dx_int(data.hac_status(di*i:di*(i+1)-1) == 1), 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 1), 1e6*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 1), '-', 'color', colors(2,:))
-    end
-    axis(ax1, 'equal')
-    xlim(ax1, args.xlim)
-    ylim(ax1, args.ylim)
-    zlim(ax1, args.zlim)
-    view(ax1, args.view_az, args.view_el)
-    xlabel(ax1, "X motion [$\mu$m]");
-    ylabel(ax1, "Y motion [$\mu$m]");
-    zlabel(ax1, "Z motion [$\mu$m]");
-    drawnow()
-    writeVideo(writerObj,getframe(fig))  % add the frame to the movie
-end
-close(writerObj);
-#+end_src
-
-** =yztomographymovie=
-:PROPERTIES:
-:header-args:matlab+: :tangle matlab/src/yztomographymovie.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:h5scan>>
-
-#+begin_src matlab
-function   [] = yztomographymovie(filename, data, args)
-#+end_src
-
-#+begin_src matlab
-  arguments
-    filename
-    data
-    args.di      (1,1) double {mustBeNumeric} = 500
-    args.xlim_ax1 (2,1) double {mustBeNumeric} = [-3, 3]
-    args.ylim_ax1 (2,1) double {mustBeNumeric} = [-3, 3]
-    args.xlim_ax2 (2,1) double {mustBeNumeric} = [-3, 3]
-    args.ylim_ax2 (2,1) double {mustBeNumeric} = [-3, 3]
-  end
-#+end_src
-
-#+begin_src matlab
-colors = colororder;
-#+end_src
-
-#+begin_src matlab
-writerObj = VideoWriter(filename); % initialize the VideoWriter object
-open(writerObj);
-#+end_src
-
-#+begin_src matlab
-fig = figure;
-tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');
-
-ax1 = nexttile;
-ax2 = nexttile;
-#+end_src
-
-#+begin_src matlab
-di = args.di;
-for i = 1:floor(length(data.Dx_int)/di)-1
-    if data.hac_status(di*(i+1)-1) == 0
-        % Only open loop
-        plot(ax1, 1e6*data.Dy_int(di*i:di*(i+1)-1), 1e6*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(1,:))
-        plot(ax2, 1e9*data.Dy_int(di*i:di*(i+1)-1), 1e9*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(1,:))
-    elseif data.hac_status(di*i) == 1
-        % Only closed loop
-        plot(ax1, 1e6*data.Dy_int(di*i:di*(i+1)-1), 1e6*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(2,:))
-        plot(ax2, 1e9*data.Dy_int(di*i:di*(i+1)-1), 1e9*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(2,:))
-    else
-        % Both open and closed loop
-        Dy_int = data.Dy_int(di*i:di*(i+1)-1);
-        Dz_int = data.Dz_int(di*i:di*(i+1)-1);
-        plot(ax1, 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 0), 1e6*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 0), '-', 'color', colors(1,:))
-        plot(ax1, 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 1), 1e6*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 1), '-', 'color', colors(2,:))
-        plot(ax2, 1e9*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 0), 1e9*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 0), '-', 'color', colors(1,:))
-        plot(ax2, 1e9*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 1), 1e9*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 1), '-', 'color', colors(2,:))
-    end
-    axis(ax1, 'square')
-    axis(ax2, 'square')
-    xlim(ax1, args.xlim_ax1)
-    ylim(ax1, args.ylim_ax1)
-    xlim(ax2, args.xlim_ax2)
-    ylim(ax2, args.ylim_ax2)
-    xlabel(ax1, "Y motion [$\mu m$]");
-    ylabel(ax1, "Z motion [$\mu m$]");
-    xlabel(ax2, "Y motion [$nm$]");
-    ylabel(ax2, "Z motion [$nm$]");
-    F = getframe(fig);            %// Capture the frame
-    writeVideo(writerObj,F)  %// add the frame to the movie
-end
-close(writerObj);
-#+end_src
-
-** Simscape Configuration
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/initializeSimscapeConfiguration.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:initializeSimscapeConfiguration>>
-
-*** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  function [] = initializeSimscapeConfiguration(args)
-#+end_src
-
-*** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  arguments
-    args.gravity logical {mustBeNumericOrLogical} = true
-  end
-#+end_src
-
-*** Structure initialization
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  conf_simscape = struct();
-#+end_src
-
-*** Add Type
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  if args.gravity
-    conf_simscape.type = 1;
-  else
-    conf_simscape.type = 2;
-  end
-#+end_src
-
-*** Save the Structure
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  save('./mat/conf_simscape.mat', 'conf_simscape');
-#+end_src
-
-** Logging Configuration
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/initializeLoggingConfiguration.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:initializeLoggingConfiguration>>
-
-*** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  function [] = initializeLoggingConfiguration(args)
-#+end_src
-
-*** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  arguments
-    args.log      char   {mustBeMember(args.log,{'none', 'all', 'forces'})} = 'none'
-    args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
-  end
-#+end_src
-
-*** Structure initialization
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  conf_log = struct();
-#+end_src
-
-*** Add Type
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  switch args.log
-    case 'none'
-      conf_log.type = 0;
-    case 'all'
-      conf_log.type = 1;
-    case 'forces'
-      conf_log.type = 2;
-  end
-#+end_src
-
-*** Sampling Time
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  conf_log.Ts = args.Ts;
-#+end_src
-
-*** Save the Structure
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  save('./mat/conf_log.mat', 'conf_log');
-#+end_src
-
-** Ground
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/initializeGround.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:initializeGround>>
-
-*** Simscape Model
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-The model of the Ground is composed of:
-- A *Cartesian* joint that is used to simulation the ground motion
-- A solid that represents the ground on which the granite is located
-
-#+name: fig:simscape_model_ground
-#+attr_org: :width 800
-#+caption: Simscape model for the Ground
-[[file:figs/images/simscape_model_ground.png]]
-
-#+name: fig:simscape_picture_ground
-#+attr_org: :width 800
-#+caption: Simscape picture for the Ground
-[[file:figs/images/simscape_picture_ground.png]]
-
-*** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  function [ground] = initializeGround(args)
-#+end_src
-
-*** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  arguments
-    args.type char {mustBeMember(args.type,{'none', 'rigid'})} = 'rigid'
-    args.rot_point (3,1) double {mustBeNumeric} = zeros(3,1) % Rotation point for the ground motion [m]
-  end
-#+end_src
-
-*** Structure initialization
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-First, we initialize the =granite= structure.
-#+begin_src matlab
-  ground = struct();
-#+end_src
-
-*** Add Type
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  switch args.type
-    case 'none'
-      ground.type = 0;
-    case 'rigid'
-      ground.type = 1;
-  end
-#+end_src
-
-*** Ground Solid properties
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-We set the shape and density of the ground solid element.
-#+begin_src matlab
-  ground.shape   = [2, 2, 0.5]; % [m]
-  ground.density = 2800;        % [kg/m3]
-#+end_src
-
-*** Rotation Point
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-#+begin_src matlab
-  ground.rot_point = args.rot_point;
-#+end_src
-
-*** Save the Structure
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-The =ground= structure is saved.
-#+begin_src matlab
-  save('./mat/stages.mat', 'ground', '-append');
-#+end_src
-
-** Granite
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/initializeGranite.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:initializeGranite>>
-
-*** Simscape Model
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-The Simscape model of the granite is composed of:
-- A cartesian joint such that the granite can vibrations along x, y and z axis
-- A solid
-
-The output =sample_pos= corresponds to the impact point of the X-ray.
-
-#+name: fig:simscape_model_granite
-#+attr_org: :width 800
-#+caption: Simscape model for the Granite
-[[file:figs/images/simscape_model_granite.png]]
-
-#+name: fig:simscape_picture_granite
-#+attr_org: :width 800
-#+caption: Simscape picture for the Granite
-[[file:figs/images/simscape_picture_granite.png]]
-
-*** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  function [granite] = initializeGranite(args)
-#+end_src
-
-*** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  arguments
-    args.type          char    {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'modal-analysis', 'init'})} = 'flexible'
-    args.Foffset       logical {mustBeNumericOrLogical} = false
-    args.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 % Density [kg/m3]
-    args.K  (3,1) double {mustBeNumeric, mustBeNonnegative} = [4e9; 3e8; 8e8] % [N/m]
-    args.C  (3,1) double {mustBeNumeric, mustBeNonnegative} = [4.0e5; 1.1e5; 9.0e5] % [N/(m/s)]
-    args.x0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the X direction [m]
-    args.y0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Y direction [m]
-    args.z0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Z direction [m]
-    args.sample_pos (1,1) double {mustBeNumeric} = 0.8-25e-3 % Height of the measurment point [m]
-  end
-#+end_src
-
-*** Structure initialization
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-First, we initialize the =granite= structure.
-#+begin_src matlab
-  granite = struct();
-#+end_src
-
-*** Add Granite Type
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  switch args.type
-    case 'none'
-      granite.type = 0;
-    case 'rigid'
-      granite.type = 1;
-    case 'flexible'
-      granite.type = 2;
-    case 'modal-analysis'
-      granite.type = 3;
-    case 'init'
-      granite.type = 4;
-  end
-#+end_src
-
-*** Material and Geometry
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-Properties of the Material and link to the geometry of the granite.
-#+begin_src matlab
-  granite.density = args.density; % [kg/m3]
-  granite.STEP    = './STEPS/granite/granite.STEP';
-#+end_src
-
-Z-offset for the initial position of the sample with respect to the granite top surface.
-#+begin_src matlab
-  granite.sample_pos = args.sample_pos; % [m]
-#+end_src
-
-*** Stiffness and Damping properties
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-#+begin_src matlab
-  granite.K = args.K; % [N/m]
-  granite.C = args.C; % [N/(m/s)]
-#+end_src
-
-*** Equilibrium position of the each joint.
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-#+begin_src matlab
-  if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
-    load('mat/Foffset.mat', 'Fgm');
-    granite.Deq = -Fgm'./granite.K;
-  else
-    granite.Deq = zeros(6,1);
-  end
-#+end_src
-
-*** Save the Structure
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-The =granite= structure is saved.
-#+begin_src matlab
-  save('./mat/stages.mat', 'granite', '-append');
-#+end_src
-
-** Translation Stage
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/initializeTy.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:initializeTy>>
-
-*** Simscape Model
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-The Simscape model of the Translation stage consist of:
-- One rigid body for the fixed part of the translation stage
-- One rigid body for the moving part of the translation stage
-- Four 6-DOF Joints that only have some rigidity in the X and Z directions.
-  The rigidity in rotation comes from the fact that we use multiple joints that are located at different points
-- One 6-DOF joint that represent the Actuator.
-  It is used to impose the motion in the Y direction
-- One 6-DOF joint to inject force disturbance in the X and Z directions
-
-#+name: fig:simscape_model_ty
-#+ATTR_ORG: :width 800
-#+caption: Simscape model for the Translation Stage
-[[file:figs/images/simscape_model_ty.png]]
-
-#+name: fig:simscape_picture_ty
-#+attr_org: :width 800
-#+caption: Simscape picture for the Translation Stage
-[[file:figs/images/simscape_picture_ty.png]]
-
-*** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  function [ty] = initializeTy(args)
-#+end_src
-
-*** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  arguments
-    args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
-    args.Foffset logical {mustBeNumericOrLogical} = false
-  end
-#+end_src
-
-*** Structure initialization
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-First, we initialize the =ty= structure.
-#+begin_src matlab
-  ty = struct();
-#+end_src
-
-*** Add Translation Stage Type
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  switch args.type
-    case 'none'
-      ty.type = 0;
-    case 'rigid'
-      ty.type = 1;
-    case 'flexible'
-      ty.type = 2;
-    case 'modal-analysis'
-      ty.type = 3;
-    case 'init'
-      ty.type = 4;
-  end
-#+end_src
-
-*** Material and Geometry
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-Define the density of the materials as well as the geometry (STEP files).
-#+begin_src matlab
-  % Ty Granite frame
-  ty.granite_frame.density = 7800; % [kg/m3] => 43kg
-  ty.granite_frame.STEP    = './STEPS/Ty/Ty_Granite_Frame.STEP';
-
-  % Guide Translation Ty
-  ty.guide.density         = 7800; % [kg/m3] => 76kg
-  ty.guide.STEP            = './STEPS/ty/Ty_Guide.STEP';
-
-  % Ty - Guide_Translation12
-  ty.guide12.density       = 7800; % [kg/m3]
-  ty.guide12.STEP          = './STEPS/Ty/Ty_Guide_12.STEP';
-
-  % Ty - Guide_Translation11
-  ty.guide11.density       = 7800; % [kg/m3]
-  ty.guide11.STEP          = './STEPS/ty/Ty_Guide_11.STEP';
-
-  % Ty - Guide_Translation22
-  ty.guide22.density       = 7800; % [kg/m3]
-  ty.guide22.STEP          = './STEPS/ty/Ty_Guide_22.STEP';
-
-  % Ty - Guide_Translation21
-  ty.guide21.density       = 7800; % [kg/m3]
-  ty.guide21.STEP          = './STEPS/Ty/Ty_Guide_21.STEP';
-
-  % Ty - Plateau translation
-  ty.frame.density         = 7800; % [kg/m3]
-  ty.frame.STEP            = './STEPS/ty/Ty_Stage.STEP';
-
-  % Ty Stator Part
-  ty.stator.density        = 5400; % [kg/m3]
-  ty.stator.STEP           = './STEPS/ty/Ty_Motor_Stator.STEP';
-
-  % Ty Rotor Part
-  ty.rotor.density         = 5400; % [kg/m3]
-  ty.rotor.STEP            = './STEPS/ty/Ty_Motor_Rotor.STEP';
-#+end_src
-
-*** Stiffness and Damping properties
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-#+begin_src matlab
-  ty.K = [2e8; 1e8; 2e8; 6e7; 9e7; 6e7]; % [N/m, N*m/rad]
-  ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 2e4]; % [N/(m/s), N*m/(rad/s)]
-#+end_src
-
-*** Equilibrium position of the each joint.
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-#+begin_src matlab
-  if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
-    load('mat/Foffset.mat', 'Ftym');
-    ty.Deq = -Ftym'./ty.K;
-  else
-    ty.Deq = zeros(6,1);
-  end
-#+end_src
-
-*** Save the Structure
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-The =ty= structure is saved.
-#+begin_src matlab
-  save('./mat/stages.mat', 'ty', '-append');
-#+end_src
-
-** Tilt Stage
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/initializeRy.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:initializeRy>>
-
-*** Simscape Model
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-The Simscape model of the Tilt stage is composed of:
-- Two solid bodies for the two part of the stage
-- *Four* 6-DOF joints to model the flexibility of the stage.
-  These joints are virtually located along the rotation axis and are connecting the two solid bodies.
-  These joints have some translation stiffness in the u-v-w directions aligned with the joint.
-  The stiffness in rotation between the two solids is due to the fact that the 4 joints are connecting the two solids are different locations
-- A Bushing Joint used for the Actuator.
-  The Ry motion is imposed by the input.
-
-#+name: fig:simscape_model_ry
-#+attr_org: :width 800
-#+caption: Simscape model for the Tilt Stage
-[[file:figs/images/simscape_model_ry.png]]
-
-#+name: fig:simscape_picture_ry
-#+attr_org: :width 800
-#+caption: Simscape picture for the Tilt Stage
-[[file:figs/images/simscape_picture_ry.png]]
-
-*** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  function [ry] = initializeRy(args)
-#+end_src
-
-*** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  arguments
-    args.type          char    {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
-    args.Foffset       logical {mustBeNumericOrLogical} = false
-    args.Ry_init (1,1) double  {mustBeNumeric} = 0
-  end
-#+end_src
-
-*** Structure initialization
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-First, we initialize the =ry= structure.
-#+begin_src matlab
-  ry = struct();
-#+end_src
-
-
-*** Add Tilt Type
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  switch args.type
-    case 'none'
-      ry.type = 0;
-    case 'rigid'
-      ry.type = 1;
-    case 'flexible'
-      ry.type = 2;
-    case 'modal-analysis'
-      ry.type = 3;
-    case 'init'
-      ry.type = 4;
-  end
-#+end_src
-
-*** Material and Geometry
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-Properties of the Material and link to the geometry of the Tilt stage.
-#+begin_src matlab
-  % Ry - Guide for the tilt stage
-  ry.guide.density = 7800; % [kg/m3]
-  ry.guide.STEP    = './STEPS/ry/Tilt_Guide.STEP';
-
-  % Ry - Rotor of the motor
-  ry.rotor.density = 2400; % [kg/m3]
-  ry.rotor.STEP    = './STEPS/ry/Tilt_Motor_Axis.STEP';
-
-  % Ry - Motor
-  ry.motor.density = 3200; % [kg/m3]
-  ry.motor.STEP    = './STEPS/ry/Tilt_Motor.STEP';
-
-  % Ry - Plateau Tilt
-  ry.stage.density = 7800; % [kg/m3]
-  ry.stage.STEP    = './STEPS/ry/Tilt_Stage.STEP';
-#+end_src
-
-Z-Offset so that the center of rotation matches the sample center;
-#+begin_src matlab
-  ry.z_offset = 0.58178; % [m]
-#+end_src
-
-#+begin_src matlab
-  ry.Ry_init = args.Ry_init; % [rad]
-#+end_src
-
-*** Stiffness and Damping properties
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-#+begin_src matlab
-  ry.K = [3.8e8; 4e8; 3.8e8; 1.2e8; 6e4; 1.2e8];
-  ry.C = [1e5;   1e5; 1e5;   3e4;   1e3; 3e4];
-#+end_src
-
-*** Equilibrium position of the each joint.
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-#+begin_src matlab
-  if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
-    load('mat/Foffset.mat', 'Fym');
-    ry.Deq = -Fym'./ry.K;
-  else
-    ry.Deq = zeros(6,1);
-  end
-#+end_src
-
-*** Save the Structure
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-The =ry= structure is saved.
-#+begin_src matlab
-  save('./mat/stages.mat', 'ry', '-append');
-#+end_src
-
-** Spindle
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/initializeRz.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:initializeRz>>
-
-*** Simscape Model
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-The Simscape model of the Spindle is composed of:
-- Two rigid bodies: the stator and the rotor
-- A Bushing Joint that is used both as the actuator (the Rz motion is imposed by the input) and as the force perturbation in the Z direction.
-- The Bushing joint has some flexibility in the X-Y-Z directions as well as in Rx and Ry rotations
-
-#+name: fig:simscape_model_rz
-#+attr_org: :width 800
-#+caption: Simscape model for the Spindle
-[[file:figs/images/simscape_model_rz.png]]
-
-#+name: fig:simscape_picture_rz
-#+attr_org: :width 800
-#+caption: Simscape picture for the Spindle
-[[file:figs/images/simscape_picture_rz.png]]
-
-*** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  function [rz] = initializeRz(args)
-#+end_src
-
-*** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  arguments
-    args.type    char    {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
-    args.Foffset logical {mustBeNumericOrLogical} = false
-  end
-#+end_src
-
-*** Structure initialization
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-First, we initialize the =rz= structure.
-#+begin_src matlab
-  rz = struct();
-#+end_src
-
-*** Add Spindle Type
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  switch args.type
-    case 'none'
-      rz.type = 0;
-    case 'rigid'
-      rz.type = 1;
-    case 'flexible'
-      rz.type = 2;
-    case 'modal-analysis'
-      rz.type = 3;
-    case 'init'
-      rz.type = 4;
-  end
-#+end_src
-
-*** Material and Geometry
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-Properties of the Material and link to the geometry of the spindle.
-#+begin_src matlab
-  % Spindle - Slip Ring
-  rz.slipring.density = 7800; % [kg/m3]
-  rz.slipring.STEP    = './STEPS/rz/Spindle_Slip_Ring.STEP';
-
-  % Spindle - Rotor
-  rz.rotor.density    = 7800; % [kg/m3]
-  rz.rotor.STEP       = './STEPS/rz/Spindle_Rotor.STEP';
-
-  % Spindle - Stator
-  rz.stator.density   = 7800; % [kg/m3]
-  rz.stator.STEP      = './STEPS/rz/Spindle_Stator.STEP';
-#+end_src
-
-*** Stiffness and Damping properties
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-#+begin_src matlab
-  rz.K = [7e8; 7e8; 2e9; 1e7; 1e7; 1e7];
-  rz.C = [4e4; 4e4; 7e4; 1e4; 1e4; 1e4];
-#+end_src
-
-*** Equilibrium position of the each joint.
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-#+begin_src matlab
-  if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
-    load('mat/Foffset.mat', 'Fzm');
-    rz.Deq = -Fzm'./rz.K;
-  else
-    rz.Deq = zeros(6,1);
-  end
-#+end_src
-
-*** Save the Structure
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-The =rz= structure is saved.
-#+begin_src matlab
-  save('./mat/stages.mat', 'rz', '-append');
-#+end_src
-
-** Micro Hexapod
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/initializeMicroHexapod.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:initializeMicroHexapod>>
-
-*** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  function [micro_hexapod] = initializeMicroHexapod(args)
-#+end_src
-
-*** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  arguments
-      args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init', 'compliance'})} = 'flexible'
-      % initializeFramesPositions
-      args.H    (1,1) double {mustBeNumeric, mustBePositive} = 350e-3
-      args.MO_B (1,1) double {mustBeNumeric} = 270e-3
-      % generateGeneralConfiguration
-      args.FH  (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
-      args.FR  (1,1) double {mustBeNumeric, mustBePositive} = 175.5e-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} = 45e-3
-      args.MR  (1,1) double {mustBeNumeric, mustBePositive} = 118e-3
-      args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180)
-      % initializeStrutDynamics
-      args.Ki (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e7*ones(6,1)
-      args.Ci (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.4e3*ones(6,1)
-      % initializeCylindricalPlatforms
-      args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 10
-      args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
-      args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 207.5e-3
-      args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 10
-      args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
-      args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3
-      % initializeCylindricalStruts
-      args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1
-      args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
-      args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
-      args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1
-      args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
-      args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
-      % inverseKinematics
-      args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
-      args.ARB (3,3) double {mustBeNumeric} = eye(3)
-      % Force that stiffness of each joint should apply at t=0
-      args.Foffset      logical {mustBeNumericOrLogical} = false
-  end
-#+end_src
-
-*** Function content
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  stewart = initializeStewartPlatform();
-
-  stewart = initializeFramesPositions(stewart, ...
-                                      'H', args.H, ...
-                                      'MO_B', args.MO_B);
-
-  stewart = generateGeneralConfiguration(stewart, ...
-                                         'FH', args.FH, ...
-                                         'FR', args.FR, ...
-                                         'FTh', args.FTh, ...
-                                         'MH', args.MH, ...
-                                         'MR', args.MR, ...
-                                         'MTh', args.MTh);
-
-  stewart = computeJointsPose(stewart);
-#+end_src
-
-#+begin_src matlab
-  stewart = initializeStrutDynamics(stewart, ...
-                                    'K', args.Ki, ...
-                                    'C', args.Ci);
-
-  stewart = initializeJointDynamics(stewart, ...
-                                    'type_F', 'universal_p', ...
-                                    'type_M', 'spherical_p');
-#+end_src
-
-#+begin_src matlab
-  stewart = initializeCylindricalPlatforms(stewart, ...
-                                           'Fpm', args.Fpm, ...
-                                           'Fph', args.Fph, ...
-                                           'Fpr', args.Fpr, ...
-                                           'Mpm', args.Mpm, ...
-                                           'Mph', args.Mph, ...
-                                           'Mpr', args.Mpr);
-
-  stewart = initializeCylindricalStruts(stewart, ...
-                                        'Fsm', args.Fsm, ...
-                                        'Fsh', args.Fsh, ...
-                                        'Fsr', args.Fsr, ...
-                                        'Msm', args.Msm, ...
-                                        'Msh', args.Msh, ...
-                                        'Msr', args.Msr);
-
-  stewart = computeJacobian(stewart);
-
-  stewart = initializeStewartPose(stewart, ...
-                                'AP', args.AP, ...
-                                'ARB', args.ARB);
-#+end_src
-
-#+begin_src matlab
-  stewart = initializeInertialSensor(stewart, 'type', 'none');
-#+end_src
-
-Equilibrium position of the each joint.
-#+begin_src matlab
-  if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
-    load('mat/Foffset.mat', 'Fhm');
-    stewart.actuators.dLeq = -Fhm'./args.Ki;
-  else
-    stewart.actuators.dLeq = zeros(6,1);
-  end
-#+end_src
-
-*** Add Type
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  switch args.type
-    case 'none'
-      stewart.type = 0;
-    case 'rigid'
-      stewart.type = 1;
-    case 'flexible'
-      stewart.type = 2;
-    case 'modal-analysis'
-      stewart.type = 3;
-    case 'init'
-      stewart.type = 4;
-    case 'compliance'
-      stewart.type = 5;
-  end
-#+end_src
-
-*** Save the Structure
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-The =micro_hexapod= structure is saved.
-#+begin_src matlab
-  micro_hexapod = stewart;
-  save('./mat/stages.mat', 'micro_hexapod', '-append');
-#+end_src
-
-** Nano Hexapod
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/initializeNanoHexapod.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:initializeNanoHexapod>>
-
-*** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-function [nano_hexapod] = initializeNanoHexapod(args)
-#+end_src
-
-*** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-arguments
-    %% Bottom Flexible Joints
-    args.flex_bot_type      char {mustBeMember(args.flex_bot_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof'
-    args.flex_bot_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad]
-    args.flex_bot_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad]
-    args.flex_bot_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad]
-    args.flex_bot_kz  (6,1) double {mustBeNumeric} = ones(6,1)*7e7 % Axial Stiffness [N/m]
-    args.flex_bot_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % X bending Damping [Nm/(rad/s)]
-    args.flex_bot_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Y bending Damping [Nm/(rad/s)]
-    args.flex_bot_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Torsionnal Damping [Nm/(rad/s)]
-    args.flex_bot_cz  (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Axial Damping [N/(m/s)]
-
-    %% Top Flexible Joints
-    args.flex_top_type      char {mustBeMember(args.flex_top_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof'
-    args.flex_top_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad]
-    args.flex_top_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad]
-    args.flex_top_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad]
-    args.flex_top_kz  (6,1) double {mustBeNumeric} = ones(6,1)*7e7 % Axial Stiffness [N/m]
-    args.flex_top_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % X bending Damping [Nm/(rad/s)]
-    args.flex_top_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Y bending Damping [Nm/(rad/s)]
-    args.flex_top_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Torsionnal Damping [Nm/(rad/s)]
-    args.flex_top_cz  (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Axial Damping [N/(m/s)]
-
-    %% Jacobian - Location of frame {A} and {B}
-    args.MO_B (1,1) double {mustBeNumeric} = 150e-3 % Height of {B} w.r.t. {M} [m]
-
-    %% Relative Motion Sensor
-    args.motion_sensor_type char {mustBeMember(args.motion_sensor_type,{'struts', 'plates'})} = 'struts'
-
-    %% Top Plate
-    args.top_plate_type char {mustBeMember(args.top_plate_type,{'rigid', 'flexible'})} = 'rigid'
-    args.top_plate_xi  (1,1) double {mustBeNumeric} = 0.01 % Damping Ratio
-
-    %% Actuators
-    args.actuator_type char {mustBeMember(args.actuator_type,{'2dof', 'flexible frame', 'flexible'})} = 'flexible'
-    args.actuator_Ga  (6,1) double {mustBeNumeric} = zeros(6,1) % Actuator gain [N/V]
-    args.actuator_Gs  (6,1) double {mustBeNumeric} = zeros(6,1) % Sensor gain [V/m]
-    % For 2DoF
-    args.actuator_k   (6,1) double {mustBeNumeric} = ones(6,1)*0.38e6 % [N/m]
-    args.actuator_ke  (6,1) double {mustBeNumeric} = ones(6,1)*1.75e6 % [N/m]
-    args.actuator_ka  (6,1) double {mustBeNumeric} = ones(6,1)*3e7 % [N/m]
-    args.actuator_c   (6,1) double {mustBeNumeric} = ones(6,1)*3e1 % [N/(m/s)]
-    args.actuator_ce  (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % [N/(m/s)]
-    args.actuator_ca  (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % [N/(m/s)]
-    args.actuator_Leq (6,1) double {mustBeNumeric} = ones(6,1)*0.056 % [m]
-    % For Flexible Frame
-    args.actuator_ks (6,1) double {mustBeNumeric} = ones(6,1)*235e6 % Stiffness of one stack [N/m]
-    args.actuator_cs (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % Stiffness of one stack [N/m]
-    % Misalignment
-    args.actuator_d_align (6,3) double {mustBeNumeric} = zeros(6,3) % [m]
-
-    args.actuator_xi  (1,1) double {mustBeNumeric} = 0.01 % Damping Ratio
-end
-#+end_src
-
-*** Nano Hexapod Object
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-#+begin_src matlab
-nano_hexapod = struct();
-#+end_src
-
-*** Flexible Joints - Bot
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-#+begin_src matlab
-nano_hexapod.flex_bot = struct();
-
-switch args.flex_bot_type
-  case '2dof'
-    nano_hexapod.flex_bot.type = 1;
-  case '3dof'
-    nano_hexapod.flex_bot.type = 2;
-  case '4dof'
-    nano_hexapod.flex_bot.type = 3;
-  case 'flexible'
-    nano_hexapod.flex_bot.type = 4;
-end
-
-nano_hexapod.flex_bot.kRx = args.flex_bot_kRx; % X bending stiffness [Nm/rad]
-nano_hexapod.flex_bot.kRy = args.flex_bot_kRy; % Y bending stiffness [Nm/rad]
-nano_hexapod.flex_bot.kRz = args.flex_bot_kRz; % Torsionnal stiffness [Nm/rad]
-nano_hexapod.flex_bot.kz  = args.flex_bot_kz;  % Axial stiffness [N/m]
-
-nano_hexapod.flex_bot.cRx = args.flex_bot_cRx; % [Nm/(rad/s)]
-nano_hexapod.flex_bot.cRy = args.flex_bot_cRy; % [Nm/(rad/s)]
-nano_hexapod.flex_bot.cRz = args.flex_bot_cRz; % [Nm/(rad/s)]
-nano_hexapod.flex_bot.cz  = args.flex_bot_cz;  %[N/(m/s)]
-#+end_src
-
-*** Flexible Joints - Top
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-#+begin_src matlab
-nano_hexapod.flex_top = struct();
-
-switch args.flex_top_type
-  case '2dof'
-    nano_hexapod.flex_top.type = 1;
-  case '3dof'
-    nano_hexapod.flex_top.type = 2;
-  case '4dof'
-    nano_hexapod.flex_top.type = 3;
-  case 'flexible'
-    nano_hexapod.flex_top.type = 4;
-end
-
-nano_hexapod.flex_top.kRx = args.flex_top_kRx; % X bending stiffness [Nm/rad]
-nano_hexapod.flex_top.kRy = args.flex_top_kRy; % Y bending stiffness [Nm/rad]
-nano_hexapod.flex_top.kRz = args.flex_top_kRz; % Torsionnal stiffness [Nm/rad]
-nano_hexapod.flex_top.kz  = args.flex_top_kz;  % Axial stiffness [N/m]
-
-nano_hexapod.flex_top.cRx = args.flex_top_cRx; % [Nm/(rad/s)]
-nano_hexapod.flex_top.cRy = args.flex_top_cRy; % [Nm/(rad/s)]
-nano_hexapod.flex_top.cRz = args.flex_top_cRz; % [Nm/(rad/s)]
-nano_hexapod.flex_top.cz  = args.flex_top_cz;  %[N/(m/s)]
-#+end_src
-
-*** Relative Motion Sensor
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-#+begin_src matlab
-nano_hexapod.motion_sensor = struct();
-
-switch args.motion_sensor_type
-  case 'struts'
-    nano_hexapod.motion_sensor.type = 1;
-  case 'plates'
-    nano_hexapod.motion_sensor.type = 2;
-end
-#+end_src
-
-*** Amplified Piezoelectric Actuator
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-#+begin_src matlab
-nano_hexapod.actuator = struct();
-
-switch args.actuator_type
-  case '2dof'
-    nano_hexapod.actuator.type = 1;
-  case 'flexible frame'
-    nano_hexapod.actuator.type = 2;
-  case 'flexible'
-    nano_hexapod.actuator.type = 3;
-end
-#+end_src
-
-#+begin_src matlab
-%% Actuator gain [N/V]
-if all(args.actuator_Ga == 0)
-    switch args.actuator_type
-      case '2dof'
-        nano_hexapod.actuator.Ga = ones(6,1)*(-30.0);
-      case 'flexible frame'
-        nano_hexapod.actuator.Ga = ones(6,1); % TODO
-      case 'flexible'
-        nano_hexapod.actuator.Ga = ones(6,1)*23.4;
-    end
-else
-    nano_hexapod.actuator.Ga = args.actuator_Ga; % Actuator gain [N/V]
-end
-
-%% Sensor gain [V/m]
-if all(args.actuator_Gs == 0)
-    switch args.actuator_type
-      case '2dof'
-        nano_hexapod.actuator.Gs = ones(6,1)*0.098;
-      case 'flexible frame'
-        nano_hexapod.actuator.Gs = ones(6,1); % TODO
-      case 'flexible'
-        nano_hexapod.actuator.Gs = ones(6,1)*(-4674824);
-    end
-else
-    nano_hexapod.actuator.Gs = args.actuator_Gs; % Sensor gain [V/m]
-end
-#+end_src
-
-Mechanical characteristics:
-#+begin_src matlab
-switch args.actuator_type
-  case '2dof'
-    nano_hexapod.actuator.k  = args.actuator_k;  % [N/m]
-    nano_hexapod.actuator.ke = args.actuator_ke; % [N/m]
-    nano_hexapod.actuator.ka = args.actuator_ka; % [N/m]
-
-    nano_hexapod.actuator.c  = args.actuator_c;  % [N/(m/s)]
-    nano_hexapod.actuator.ce = args.actuator_ce; % [N/(m/s)]
-    nano_hexapod.actuator.ca = args.actuator_ca; % [N/(m/s)]
-
-    nano_hexapod.actuator.Leq = args.actuator_Leq; % [m]
-
-  case 'flexible frame'
-    nano_hexapod.actuator.K = readmatrix('APA300ML_b_mat_K.CSV'); % Stiffness Matrix
-    nano_hexapod.actuator.M = readmatrix('APA300ML_b_mat_M.CSV'); % Mass Matrix
-    nano_hexapod.actuator.P = extractNodes('APA300ML_b_out_nodes_3D.txt'); % Node coordinates [m]
-
-    nano_hexapod.actuator.ks = args.actuator_ks; % Stiffness of one stack [N/m]
-    nano_hexapod.actuator.cs = args.actuator_cs; % Damping of one stack [N/m]
-    nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio
-
-  case 'flexible'
-    nano_hexapod.actuator.K = readmatrix('full_APA300ML_K.CSV'); % Stiffness Matrix
-    nano_hexapod.actuator.M = readmatrix('full_APA300ML_M.CSV'); % Mass Matrix
-    nano_hexapod.actuator.P = extractNodes('full_APA300ML_out_nodes_3D.txt'); % Node coordiantes [m]
-
-    nano_hexapod.actuator.d_align = args.actuator_d_align; % Misalignment
-    nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio
-
-end
-
-#+end_src
-
-*** Geometry
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-#+begin_src matlab
-nano_hexapod.geometry = struct();
-#+end_src
-
-Center of joints $a_i$ with respect to {F}:
-#+begin_src matlab
-Fa = [[-21.74,  111.91, 22.49],
-      [-107.79, -37.13, 22.49],
-      [-86.05,  -74.78, 22.49],
-      [ 86.05,  -74.78, 22.49],
-      [ 107.79, -37.13, 22.49],
-      [ 21.74,  111.91, 22.49]]'*1e-3; % Ai w.r.t. {F} [m]
-#+end_src
-
-Center of joints $b_i$ with respect to {M}:
-#+begin_src matlab
-Mb = [[-77.78,  77.78,  -22.50],
-      [-106.25, 28.47,  -22.50],
-      [-28.47, -106.25, -22.50],
-      [ 28.47, -106.25, -22.50],
-      [ 106.25, 28.47,  -22.50],
-      [ 77.78,  77.78,  -22.50]]'*1e-3; % Bi w.r.t. {M} [m]
-#+end_src
-
-Now compute the positions $b_i$ with respect to {F}:
-#+begin_src matlab
-Fb = Mb + [0; 0; 95e-3]; % Bi w.r.t. {F} [m]
-#+end_src
-
-The unit vector representing the orientation of the struts can then be computed:
-#+begin_src matlab
-si = Fb - Fa;
-si = si./vecnorm(si); % Normalize
-#+end_src
-
-Location of encoder measurement points when fixed on the plates:
-#+begin_src matlab
-Fc = [[-76.914,  78.31,   52.605]
-      [-106.276, 27.454,  52.605]
-      [-29.362, -105.765, 52.605]
-      [ 29.362, -105.765, 52.605]
-      [ 106.276, 27.454,  52.605]
-      [ 76.914,  78.31,   52.605]]'*1e-3; % Meas pos w.r.t. {F}
-Mc = Fc - [0; 0; 95e-3]; % Meas pos w.r.t. {M}
-#+end_src
-
-#+begin_src matlab
-nano_hexapod.geometry.Fa = Fa;
-nano_hexapod.geometry.Fb = Fb;
-nano_hexapod.geometry.Fc = Fc;
-nano_hexapod.geometry.Mb = Mb;
-nano_hexapod.geometry.Mc = Mc;
-nano_hexapod.geometry.si = si;
-nano_hexapod.geometry.MO_B = args.MO_B;
-#+end_src
-
-*** Jacobian for Actuators
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-#+begin_src matlab
-Bb = Mb - [0; 0; args.MO_B];
-
-nano_hexapod.geometry.J = [nano_hexapod.geometry.si', cross(Bb, nano_hexapod.geometry.si)'];
-#+end_src
-
-*** Jacobian for Sensors
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-switch args.motion_sensor_type
-  case 'struts'
-    nano_hexapod.geometry.Js = nano_hexapod.geometry.J;
-  case 'plates'
-    Bc = Mc - [0; 0; args.MO_B];
-    nano_hexapod.geometry.Js = [nano_hexapod.geometry.si', cross(Bc, nano_hexapod.geometry.si)'];
-end
-#+end_src
-
-*** Top Plate
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-#+begin_src matlab
-nano_hexapod.top_plate = struct();
-
-switch args.top_plate_type
-  case 'rigid'
-    nano_hexapod.top_plate.type = 1;
-  case 'flexible'
-    nano_hexapod.top_plate.type = 2;
-
-    nano_hexapod.top_plate.R_flex = ...
-       {[-0.51771438594671  0.5201563363052 -0.67927108019211
-         -0.31530446231304 -0.8540710660135 -0.41369760724945
-         -0.79533320729697  0                0.60617249143351 ],
-        [-0.01420448131632  0.9997254079576  0.01863709726679
-         -0.60600604129104 -0.0234330681729  0.79511481512719
-          0.79533320729697  0                0.60617249143351 ],
-        [ 0.53191886726305  0.4795690716524  0.69790817745892
-         -0.29070157897799  0.8775041341865 -0.38141720787774
-         -0.79533320729697  0                0.60617249143351 ],
-        [ 0.53191886726305 -0.4795690716524 -0.69790817745892
-          0.29070157897799  0.8775041341865 -0.38141720787774
-          0.79533320729697  0                0.60617249143351 ],
-        [-0.01420448131633 -0.9997254079576 -0.01863709726680
-          0.60600604129104 -0.0234330681729  0.79511481512719
-         -0.79533320729697  0                0.60617249143351 ],
-        [-0.51771438594672 -0.5201563363051  0.67927108019212
-          0.31530446231304 -0.8540710660135 -0.41369760724945
-          0.79533320729697  0                0.60617249143351 ]};
-
-
-    nano_hexapod.top_plate.R_enc = ...
-      { [ 0.854071066013574 -0.520156336305191 0
-          0.520156336305191  0.854071066013574 0
-                          0                  0 1 ],
-        [-0.023433068172958  0.999725407957606 0
-         -0.999725407957606 -0.023433068172958 0
-                          0                  0 1 ],
-        [-0.877504134186525 -0.479569071652412 0
-         0.479569071652412 -0.877504134186525 0
-                         0                  0 1 ],
-        [ 0.877504134186525 -0.479569071652413 0
-          0.479569071652413  0.877504134186525 0
-                          0                  0 1 ],
-        [ 0.023433068172945  0.999725407957606 0
-         -0.999725407957606  0.023433068172945 0
-                          0                  0 1 ],
-        [-0.854071066013566 -0.520156336305202 0
-          0.520156336305202 -0.854071066013566 0
-                          0                  0 1 ]};
-
-    nano_hexapod.top_plate.K = readmatrix('top_plate_K_6.CSV'); % Stiffness Matrix
-    nano_hexapod.top_plate.M = readmatrix('top_plate_M_6.CSV'); % Mass Matrix
-    nano_hexapod.top_plate.P = extractNodes('top_plate_out_nodes_3D_qua.txt'); % Node coordiantes [m]
-    nano_hexapod.top_plate.xi = args.top_plate_xi; % Damping ratio
-
-end
-#+end_src
-
-*** Save the Structure
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-#+begin_src matlab
-if nargout == 0
-  save('./mat/stages.mat', 'nano_hexapod', '-append');
-end
-#+end_src
-
-** Sample
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/initializeSample.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:initializeSample>>
-
-*** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  function [sample] = initializeSample(args)
-#+end_src
-
-*** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  arguments
-    args.type         char    {mustBeMember(args.type,{'0', '1', '2', '3'})} = '0'
-  end
-#+end_src
-
-*** Structure initialization
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-First, we initialize the =sample= structure.
-#+begin_src matlab
-  sample = struct();
-#+end_src
-
-*** Add Sample Type
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  switch args.type
-    case '0'
-      sample.type = 0;
-    case '1'
-      sample.type = 1;
-    case '2'
-      sample.type = 2;
-    case '3'
-      sample.type = 3;
-  end
-#+end_src
-
-*** Save the Structure
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-The =sample= structure is saved.
-#+begin_src matlab
-  save('./mat/stages.mat', 'sample', '-append');
-#+end_src
-
-** Initialize Controller
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/initializeController.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:initializeController>>
-
-*** Function Declaration and Documentation
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  function [] = initializeController(args)
-#+end_src
-
-*** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  arguments
-    args.type char {mustBeMember(args.type,{'open-loop', 'iff', 'dvf', 'hac-dvf', 'ref-track-L', 'ref-track-iff-L', 'cascade-hac-lac', 'hac-iff', 'stabilizing'})} = 'open-loop'
-  end
-#+end_src
-
-*** Structure initialization
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-First, we initialize the =controller= structure.
-#+begin_src matlab
-  controller = struct();
-#+end_src
-
-*** Controller Type
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  switch args.type
-    case 'open-loop'
-      controller.type = 1;
-      controller.name = 'Open-Loop';
-    case 'dvf'
-      controller.type = 2;
-      controller.name = 'Decentralized Direct Velocity Feedback';
-    case 'iff'
-      controller.type = 3;
-      controller.name = 'Decentralized Integral Force Feedback';
-    case 'hac-dvf'
-      controller.type = 4;
-      controller.name = 'HAC-DVF';
-    case 'ref-track-L'
-      controller.type = 5;
-      controller.name = 'Reference Tracking in the frame of the legs';
-    case 'ref-track-iff-L'
-      controller.type = 6;
-      controller.name = 'Reference Tracking in the frame of the legs + IFF';
-    case 'cascade-hac-lac'
-      controller.type = 7;
-      controller.name = 'Cascade Control + HAC-LAC';
-    case 'hac-iff'
-      controller.type = 8;
-      controller.name = 'HAC-IFF';
-    case 'stabilizing'
-      controller.type = 9;
-      controller.name = 'Stabilizing Controller';
-  end
-#+end_src
-
-*** Save the Structure
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-The =controller= structure is saved.
-#+begin_src matlab
-  save('./mat/controller.mat', 'controller');
-#+end_src
-
-** Generate Reference Signals
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/initializeReferences.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:initializeReferences>>
-
-*** Function Declaration and Documentation
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  function [ref] = initializeReferences(args)
-#+end_src
-
-*** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  arguments
-      % Sampling Frequency [s]
-      args.Ts           (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
-      % Maximum simulation time [s]
-      args.Tmax         (1,1) double {mustBeNumeric, mustBePositive} = 100
-      % Either "constant" / "triangular" / "sinusoidal"
-      args.Dy_type      char {mustBeMember(args.Dy_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
-      % Amplitude of the displacement [m]
-      args.Dy_amplitude (1,1) double {mustBeNumeric} = 0
-      % Period of the displacement [s]
-      args.Dy_period    (1,1) double {mustBeNumeric, mustBePositive} = 1
-      % Either "constant" / "triangular" / "sinusoidal"
-      args.Ry_type      char {mustBeMember(args.Ry_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
-      % Amplitude [rad]
-      args.Ry_amplitude (1,1) double {mustBeNumeric} = 0
-      % Period of the displacement [s]
-      args.Ry_period    (1,1) double {mustBeNumeric, mustBePositive} = 1
-      % Either "constant" / "rotating"
-      args.Rz_type      char {mustBeMember(args.Rz_type,{'constant', 'rotating', 'rotating-not-filtered'})} = 'constant'
-      % Initial angle [rad]
-      args.Rz_amplitude (1,1) double {mustBeNumeric} = 0
-      % Period of the rotating [s]
-      args.Rz_period    (1,1) double {mustBeNumeric, mustBePositive} = 1
-      % For now, only constant is implemented
-      args.Dh_type      char {mustBeMember(args.Dh_type,{'constant'})} = 'constant'
-      % Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
-      args.Dh_pos       (6,1) double {mustBeNumeric} = zeros(6, 1), ...
-      % For now, only constant is implemented
-      args.Rm_type      char {mustBeMember(args.Rm_type,{'constant'})} = 'constant'
-      % Initial position of the two masses
-      args.Rm_pos       (2,1) double {mustBeNumeric} = [0; pi]
-      % For now, only constant is implemented
-      args.Dn_type      char {mustBeMember(args.Dn_type,{'constant'})} = 'constant'
-      % Initial position [m,m,m,rad,rad,rad] of the top platform
-      args.Dn_pos       (6,1) double {mustBeNumeric} = zeros(6,1)
-  end
-#+end_src
-
-
-*** Initialize Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  %% Set Sampling Time
-  Ts = args.Ts;
-  Tmax = args.Tmax;
-
-  %% Low Pass Filter to filter out the references
-  s = zpk('s');
-  w0 = 2*pi*10;
-  xi = 1;
-  H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2);
-#+end_src
-
-*** Translation Stage
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  %% Translation stage - Dy
-  t = 0:Ts:Tmax; % Time Vector [s]
-  Dy   = zeros(length(t), 1);
-  Dyd  = zeros(length(t), 1);
-  Dydd = zeros(length(t), 1);
-  switch args.Dy_type
-    case 'constant'
-      Dy(:)   = args.Dy_amplitude;
-      Dyd(:)  = 0;
-      Dydd(:) = 0;
-    case 'triangular'
-      % This is done to unsure that we start with no displacement
-      Dy_raw = args.Dy_amplitude*sawtooth(2*pi*t/args.Dy_period,1/2);
-      i0 = find(t>=args.Dy_period/4,1);
-      Dy(1:end-i0+1) = Dy_raw(i0:end);
-      Dy(end-i0+2:end) = Dy_raw(end); % we fix the last value
-
-      % The signal is filtered out
-      Dy   = lsim(H_lpf,     Dy, t);
-      Dyd  = lsim(H_lpf*s,   Dy, t);
-      Dydd = lsim(H_lpf*s^2, Dy, t);
-    case 'sinusoidal'
-      Dy(:) = args.Dy_amplitude*sin(2*pi/args.Dy_period*t);
-      Dyd   = args.Dy_amplitude*2*pi/args.Dy_period*cos(2*pi/args.Dy_period*t);
-      Dydd  = -args.Dy_amplitude*(2*pi/args.Dy_period)^2*sin(2*pi/args.Dy_period*t);
-    otherwise
-      warning('Dy_type is not set correctly');
-  end
-
-  Dy = struct('time', t, 'signals', struct('values', Dy), 'deriv', Dyd, 'dderiv', Dydd);
-#+end_src
-
-*** Tilt Stage
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  %% Tilt Stage - Ry
-  t = 0:Ts:Tmax; % Time Vector [s]
-  Ry   = zeros(length(t), 1);
-  Ryd  = zeros(length(t), 1);
-  Rydd = zeros(length(t), 1);
-
-  switch args.Ry_type
-    case 'constant'
-      Ry(:) = args.Ry_amplitude;
-      Ryd(:)   = 0;
-      Rydd(:)  = 0;
-    case 'triangular'
-      Ry_raw = args.Ry_amplitude*sawtooth(2*pi*t/args.Ry_period,1/2);
-      i0 = find(t>=args.Ry_period/4,1);
-      Ry(1:end-i0+1) = Ry_raw(i0:end);
-      Ry(end-i0+2:end) = Ry_raw(end); % we fix the last value
-
-      % The signal is filtered out
-      Ry   = lsim(H_lpf,     Ry, t);
-      Ryd  = lsim(H_lpf*s,   Ry, t);
-      Rydd = lsim(H_lpf*s^2, Ry, t);
-    case 'sinusoidal'
-      Ry(:) = args.Ry_amplitude*sin(2*pi/args.Ry_period*t);
-
-      Ryd  = args.Ry_amplitude*2*pi/args.Ry_period*cos(2*pi/args.Ry_period*t);
-      Rydd = -args.Ry_amplitude*(2*pi/args.Ry_period)^2*sin(2*pi/args.Ry_period*t);
-    otherwise
-      warning('Ry_type is not set correctly');
-  end
-
-  Ry = struct('time', t, 'signals', struct('values', Ry), 'deriv', Ryd, 'dderiv', Rydd);
-#+end_src
-
-*** Spindle
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  %% Spindle - Rz
-  t = 0:Ts:Tmax; % Time Vector [s]
-  Rz   = zeros(length(t), 1);
-  Rzd  = zeros(length(t), 1);
-  Rzdd = zeros(length(t), 1);
-
-  switch args.Rz_type
-    case 'constant'
-      Rz(:)   = args.Rz_amplitude;
-      Rzd(:)  = 0;
-      Rzdd(:) = 0;
-    case 'rotating-not-filtered'
-      Rz(:) = 2*pi/args.Rz_period*t;
-
-      % The signal is filtered out
-      Rz(:)   = 2*pi/args.Rz_period*t;
-      Rzd(:)  = 2*pi/args.Rz_period;
-      Rzdd(:) = 0;
-
-      % We add the angle offset
-      Rz = Rz + args.Rz_amplitude;
-
-    case 'rotating'
-      Rz(:) = 2*pi/args.Rz_period*t;
-
-      % The signal is filtered out
-      Rz   = lsim(H_lpf,     Rz, t);
-      Rzd  = lsim(H_lpf*s,   Rz, t);
-      Rzdd = lsim(H_lpf*s^2, Rz, t);
-
-      % We add the angle offset
-      Rz = Rz + args.Rz_amplitude;
-    otherwise
-      warning('Rz_type is not set correctly');
-  end
-
-  Rz = struct('time', t, 'signals', struct('values', Rz), 'deriv', Rzd, 'dderiv', Rzdd);
-#+end_src
-
-*** Micro Hexapod
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  %% Micro-Hexapod
-  t = [0, Ts];
-  Dh = zeros(length(t), 6);
-  Dhl = zeros(length(t), 6);
-
-  switch args.Dh_type
-    case 'constant'
-      Dh = [args.Dh_pos, args.Dh_pos];
-
-      load('mat/stages.mat', 'micro_hexapod');
-
-      AP = [args.Dh_pos(1) ; args.Dh_pos(2) ; args.Dh_pos(3)];
-
-      tx = args.Dh_pos(4);
-      ty = args.Dh_pos(5);
-      tz = args.Dh_pos(6);
-
-      ARB = [cos(tz) -sin(tz) 0;
-             sin(tz)  cos(tz) 0;
-             0        0       1]*...
-            [ cos(ty) 0 sin(ty);
-              0       1 0;
-             -sin(ty) 0 cos(ty)]*...
-            [1 0        0;
-             0 cos(tx) -sin(tx);
-             0 sin(tx)  cos(tx)];
-
-      [~, Dhl] = inverseKinematics(micro_hexapod, 'AP', AP, 'ARB', ARB);
-      Dhl = [Dhl, Dhl];
-    otherwise
-      warning('Dh_type is not set correctly');
-  end
-
-  Dh = struct('time', t, 'signals', struct('values', Dh));
-  Dhl = struct('time', t, 'signals', struct('values', Dhl));
-#+end_src
-
-*** Save
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-      %% Save
-      save('./mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts');
-  end
-#+end_src
-
-** Initialize Position Errors
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/initializePosError.m
-:header-args:matlab+: :comments none :mkdirp yes
-:header-args:matlab+: :eval no :results none
-:END:
-<<sec:initializePosError>>
-
-*** Function Declaration and Documentation
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  function [] = initializePosError(args)
-  % initializePosError - Initialize the position errors
-  %
-  % Syntax: [] = initializePosError(args)
-  %
-  % Inputs:
-  %    - args -
-
-#+end_src
-
-*** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  arguments
-      args.error    logical {mustBeNumericOrLogical} = false
-      args.Dy (1,1) double  {mustBeNumeric} = 0 % [m]
-      args.Ry (1,1) double  {mustBeNumeric} = 0 % [m]
-      args.Rz (1,1) double  {mustBeNumeric} = 0 % [m]
-  end
-#+end_src
-
-*** Structure initialization
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-First, we initialize the =pos_error= structure.
-#+begin_src matlab
-  pos_error = struct();
-#+end_src
-
-*** Type
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  if args.error
-    pos_error.type = 1;
-  else
-    pos_error.type = 0;
-  end
-#+end_src
-
-*** Position Errors
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  pos_error.Dy = args.Dy;
-  pos_error.Ry = args.Ry;
-  pos_error.Rz = args.Rz;
-#+end_src
-
-*** Save
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  save('./mat/pos_error.mat', 'pos_error');
-#+end_src
-** Initialize Rz Estimate
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/initializeRzEstimate.m
-:header-args:matlab+: :comments none :mkdirp yes
-:header-args:matlab+: :eval no :results none
-:END:
-<<sec:initializeRzEstimate>>
-
-*** Function Declaration and Documentation
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-function [] = initializeRzEstimate(args)
-% initializeRzEstimate - Initialize the position errors
-%
-% Syntax: [] = initializeRzEstimate(args)
-%
-% Inputs:
-%    - args -
-
-#+end_src
-
-*** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-arguments
-  args.type         char    {mustBeMember(args.type,{'encoders', 'voltages'})} = 'encoders'
-end
-#+end_src
-
-*** Structure initialization
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-First, we initialize the =rz_estimate= structure.
-#+begin_src matlab
-rz_estimate = struct();
-#+end_src
-
-*** Type
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-switch args.type
-  case 'encoders'
-    rz_estimate.type = 0;
-  case 'voltages'
-    rz_estimate.type = 1;
-end
-#+end_src
-
-*** Position Errors
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-load('mat/stages.mat', 'nano_hexapod');
-rz_estimate.J_L_to_X = pinv(nano_hexapod.geometry.J);
-rz_estimate.pz_sensitivity = -240e-6/8.5; % [m/V]
-#+end_src
-
-*** Save
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-save('./mat/stages.mat', 'rz_estimate', '-append');
-#+end_src
-
-** Initialize Lion Metrology
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/initializeLion.m
-:header-args:matlab+: :comments none :mkdirp yes
-:header-args:matlab+: :eval no :results none
-:END:
-<<sec:initializeLion>>
-
-*** Function Declaration and Documentation
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  function [] = initializeLion(args)
-  % initializeLion - Initialize the position errors
-  %
-  % Syntax: [] = initializeLion(args)
-  %
-  % Inputs:
-  %    - args -
-
-#+end_src
-
-*** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  arguments
-    args.type         char    {mustBeMember(args.type,{'rigid'})} = 'rigid'
-  end
-#+end_src
-
-*** Structure initialization
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-First, we initialize the =rz_estimate= structure.
-#+begin_src matlab
-lion = struct();
-#+end_src
-
-*** Jacobian
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-lion.J_int_to_X = [ 0                 0                -0.787401574803149 -0.212598425196851  0;
-                    0.78740157480315  0.21259842519685  0                  0                  0;
-                    0                 0                 0                  0                 -1;
-                  -13.1233595800525  13.1233595800525   0                  0                  0;
-                    0                 0               -13.1233595800525   13.1233595800525    0];
-#+end_src
-
-*** Save
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-save('./mat/stages.mat', 'lion', '-append');
-#+end_src
-** Initialize Disturbances
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/initializeDisturbances.m
-:header-args:matlab+: :comments none :mkdirp yes
-:header-args:matlab+: :eval no :results none
-:END:
-<<sec:initializeDisturbances>>
-
-*** Function Declaration and Documentation
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  function [] = initializeDisturbances(args)
-  % initializeDisturbances - Initialize the disturbances
-  %
-  % Syntax: [] = initializeDisturbances(args)
-  %
-  % Inputs:
-  %    - args -
-
-#+end_src
-
-*** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  arguments
-      % Global parameter to enable or disable the disturbances
-      args.enable logical {mustBeNumericOrLogical} = true
-      % Ground Motion - X direction
-      args.Dwx logical {mustBeNumericOrLogical} = true
-      % Ground Motion - Y direction
-      args.Dwy logical {mustBeNumericOrLogical} = true
-      % Ground Motion - Z direction
-      args.Dwz logical {mustBeNumericOrLogical} = true
-      % Translation Stage - X direction
-      args.Fty_x logical {mustBeNumericOrLogical} = true
-      % Translation Stage - Z direction
-      args.Fty_z logical {mustBeNumericOrLogical} = true
-      % Spindle - Z direction
-      args.Frz_z logical {mustBeNumericOrLogical} = true
-  end
-#+end_src
-
-
-*** Load Data
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  load('./mat/dist_psd.mat', 'dist_f');
-#+end_src
-
-We remove the first frequency point that usually is very large.
-#+begin_src matlab :exports none
-  dist_f.f = dist_f.f(2:end);
-  dist_f.psd_gm = dist_f.psd_gm(2:end);
-  dist_f.psd_ty = dist_f.psd_ty(2:end);
-  dist_f.psd_rz = dist_f.psd_rz(2:end);
-#+end_src
-
-*** Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-We define some parameters that will be used in the algorithm.
-#+begin_src matlab
-  Fs = 2*dist_f.f(end);      % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
-  N  = 2*length(dist_f.f);   % Number of Samples match the one of the wanted PSD
-  T0 = N/Fs;                 % Signal Duration [s]
-  df = 1/T0;                 % Frequency resolution of the DFT [Hz]
-                             % Also equal to (dist_f.f(2)-dist_f.f(1))
-  t = linspace(0, T0, N+1)'; % Time Vector [s]
-  Ts = 1/Fs;                 % Sampling Time [s]
-#+end_src
-
-*** Ground Motion
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  phi = dist_f.psd_gm;
-  C = zeros(N/2,1);
-  for i = 1:N/2
-    C(i) = sqrt(phi(i)*df);
-  end
-#+end_src
-
-#+begin_src matlab
-  if args.Dwx && args.enable
-    rng(111);
-    theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
-    Cx = [0 ; C.*complex(cos(theta),sin(theta))];
-    Cx = [Cx; flipud(conj(Cx(2:end)))];;
-    Dwx = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m]
-  else
-    Dwx = zeros(length(t), 1);
-  end
-#+end_src
-
-#+begin_src matlab
-  if args.Dwy && args.enable
-    rng(112);
-    theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
-    Cx = [0 ; C.*complex(cos(theta),sin(theta))];
-    Cx = [Cx; flipud(conj(Cx(2:end)))];;
-    Dwy = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m]
-  else
-    Dwy = zeros(length(t), 1);
-  end
-#+end_src
-
-#+begin_src matlab
-  if args.Dwy && args.enable
-    rng(113);
-    theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
-    Cx = [0 ; C.*complex(cos(theta),sin(theta))];
-    Cx = [Cx; flipud(conj(Cx(2:end)))];;
-    Dwz = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m]
-  else
-    Dwz = zeros(length(t), 1);
-  end
-#+end_src
-
-*** Translation Stage - X direction
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  if args.Fty_x && args.enable
-    phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate
-    C = zeros(N/2,1);
-    for i = 1:N/2
-      C(i) = sqrt(phi(i)*df);
-    end
-    rng(121);
-    theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
-    Cx = [0 ; C.*complex(cos(theta),sin(theta))];
-    Cx = [Cx; flipud(conj(Cx(2:end)))];;
-    u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty x [N]
-    Fty_x = u;
-  else
-    Fty_x = zeros(length(t), 1);
-  end
-#+end_src
-
-*** Translation Stage - Z direction
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  if args.Fty_z && args.enable
-    phi = dist_f.psd_ty;
-    C = zeros(N/2,1);
-    for i = 1:N/2
-      C(i) = sqrt(phi(i)*df);
-    end
-    rng(122);
-    theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
-    Cx = [0 ; C.*complex(cos(theta),sin(theta))];
-    Cx = [Cx; flipud(conj(Cx(2:end)))];;
-    u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty z [N]
-    Fty_z = u;
-  else
-    Fty_z = zeros(length(t), 1);
-  end
-#+end_src
-
-*** Spindle - Z direction
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  if args.Frz_z && args.enable
-    phi = dist_f.psd_rz;
-    C = zeros(N/2,1);
-    for i = 1:N/2
-      C(i) = sqrt(phi(i)*df);
-    end
-    rng(131);
-    theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
-    Cx = [0 ; C.*complex(cos(theta),sin(theta))];
-    Cx = [Cx; flipud(conj(Cx(2:end)))];;
-    u = N/sqrt(2)*ifft(Cx); % Disturbance Force Rz z [N]
-    Frz_z = u;
-  else
-    Frz_z = zeros(length(t), 1);
-  end
-#+end_src
-
-*** Direct Forces
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  u = zeros(length(t), 6);
-  Fd = u;
-#+end_src
-
-*** Set initial value to zero
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  Dwx    = Dwx   - Dwx(1);
-  Dwy    = Dwy   - Dwy(1);
-  Dwz    = Dwz   - Dwz(1);
-  Fty_x  = Fty_x - Fty_x(1);
-  Fty_z  = Fty_z - Fty_z(1);
-  Frz_z  = Frz_z - Frz_z(1);
-#+end_src
-
-*** Save
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  save('./mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't', 'args');
-#+end_src
-
-** Initialize Stewart Platform
-*** =initializeStewartPlatform=: Initialize the Stewart Platform structure
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/initializeStewartPlatform.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:initializeStewartPlatform>>
-
-This Matlab function is accessible [[file:./matlab/src/initializeStewartPlatform.m][here]].
-
-**** Documentation
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-#+name: fig:stewart-frames-position
-#+caption: Definition of the position of the frames
-[[file:figs/stewart-frames-position.png]]
-
-**** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  function [stewart] = initializeStewartPlatform()
-  % initializeStewartPlatform - Initialize the stewart structure
-  %
-  % Syntax: [stewart] = initializeStewartPlatform(args)
-  %
-  % Outputs:
-  %    - stewart - A structure with the following sub-structures:
-  %      - platform_F -
-  %      - platform_M -
-  %      - joints_F   -
-  %      - joints_M   -
-  %      - struts_F   -
-  %      - struts_M   -
-  %      - actuators  -
-  %      - geometry   -
-  %      - properties -
-#+end_src
-
-**** Initialize the Stewart structure
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  stewart = struct();
-  stewart.platform_F = struct();
-  stewart.platform_M = struct();
-  stewart.joints_F   = struct();
-  stewart.joints_M   = struct();
-  stewart.struts_F   = struct();
-  stewart.struts_M   = struct();
-  stewart.actuators  = struct();
-  stewart.sensors    = struct();
-  stewart.sensors.inertial = struct();
-  stewart.sensors.force    = struct();
-  stewart.sensors.relative = struct();
-  stewart.geometry   = struct();
-  stewart.kinematics = struct();
-#+end_src
-
-*** =initializeFramesPositions=: Initialize the positions of frames {A}, {B}, {F} and {M}
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/initializeFramesPositions.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:initializeFramesPositions>>
-
-This Matlab function is accessible [[file:./matlab/src/initializeFramesPositions.m][here]].
-
-**** Documentation
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-#+name: fig:stewart-frames-position
-#+caption: Definition of the position of the frames
-[[file:figs/stewart-frames-position.png]]
-
-**** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  function [stewart] = initializeFramesPositions(stewart, args)
-  % initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
-  %
-  % Syntax: [stewart] = initializeFramesPositions(stewart, args)
-  %
-  % Inputs:
-  %    - args - Can have the following fields:
-  %        - H    [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m]
-  %        - MO_B [1x1] - Height of the frame {B} with respect to {M} [m]
-  %
-  % Outputs:
-  %    - stewart - A structure with the following fields:
-  %        - geometry.H      [1x1] - Total Height of the Stewart Platform [m]
-  %        - geometry.FO_M   [3x1] - Position of {M} with respect to {F} [m]
-  %        - platform_M.MO_B [3x1] - Position of {B} with respect to {M} [m]
-  %        - platform_F.FO_A [3x1] - Position of {A} with respect to {F} [m]
-#+end_src
-
-**** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  arguments
-      stewart
-      args.H    (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
-      args.MO_B (1,1) double {mustBeNumeric} = 50e-3
-  end
-#+end_src
-
-**** Compute the position of each frame
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  H = args.H; % Total Height of the Stewart Platform [m]
-
-  FO_M = [0; 0; H]; % Position of {M} with respect to {F} [m]
-
-  MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m]
-
-  FO_A = MO_B + FO_M; % Position of {A} with respect to {F} [m]
-#+end_src
-
-**** Populate the =stewart= structure
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  stewart.geometry.H      = H;
-  stewart.geometry.FO_M   = FO_M;
-  stewart.platform_M.MO_B = MO_B;
-  stewart.platform_F.FO_A = FO_A;
-#+end_src
-
-*** =generateGeneralConfiguration=: Generate a Very General Configuration
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/generateGeneralConfiguration.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:generateGeneralConfiguration>>
-
-This Matlab function is accessible [[file:./matlab/src/generateGeneralConfiguration.m][here]].
-
-**** Documentation
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-Joints are positions on a circle centered with the Z axis of {F} and {M} and at a chosen distance from {F} and {M}.
-The radius of the circles can be chosen as well as the angles where the joints are located (see Figure [[fig:joint_position_general]]).
-
-#+begin_src latex :file stewart_bottom_plate.pdf :tangle no
-  \begin{tikzpicture}
-    % Internal and external limit
-    \draw[fill=white!80!black] (0, 0) circle [radius=3];
-    % Circle where the joints are located
-    \draw[dashed] (0, 0) circle [radius=2.5];
-
-    % Bullets for the positions of the joints
-    \node[] (J1) at ( 80:2.5){$\bullet$};
-    \node[] (J2) at (100:2.5){$\bullet$};
-    \node[] (J3) at (200:2.5){$\bullet$};
-    \node[] (J4) at (220:2.5){$\bullet$};
-    \node[] (J5) at (320:2.5){$\bullet$};
-    \node[] (J6) at (340:2.5){$\bullet$};
-
-    % Name of the points
-    \node[above right] at (J1) {$a_{1}$};
-    \node[above left]  at (J2) {$a_{2}$};
-    \node[above left]  at (J3) {$a_{3}$};
-    \node[right     ]  at (J4) {$a_{4}$};
-    \node[left      ]  at (J5) {$a_{5}$};
-    \node[above right] at (J6) {$a_{6}$};
-
-    % First 2 angles
-    \draw[dashed, ->] (0:1)   arc [start angle=0, end angle=80, radius=1]    node[below right]{$\theta_{1}$};
-    \draw[dashed, ->] (0:1.5) arc [start angle=0, end angle=100, radius=1.5] node[left       ]{$\theta_{2}$};
-
-    % Division of 360 degrees by 3
-    \draw[dashed] (0, 0) -- ( 80:3.2);
-    \draw[dashed] (0, 0) -- (100:3.2);
-    \draw[dashed] (0, 0) -- (200:3.2);
-    \draw[dashed] (0, 0) -- (220:3.2);
-    \draw[dashed] (0, 0) -- (320:3.2);
-    \draw[dashed] (0, 0) -- (340:3.2);
-
-    % Radius for the position of the joints
-    \draw[<->] (0, 0) --node[near end, above]{$R$} (180:2.5);
-
-    \draw[->] (0, 0) -- ++(3.4, 0) node[above]{$x$};
-    \draw[->] (0, 0) -- ++(0, 3.4) node[left]{$y$};
-  \end{tikzpicture}
-#+end_src
-
-#+name: fig:joint_position_general
-#+caption: Position of the joints
-#+RESULTS:
-[[file:figs/stewart_bottom_plate.png]]
-
-**** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  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}
-#+end_src
-
-**** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  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
-#+end_src
-
-**** Compute the pose
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  Fa = zeros(3,6);
-  Mb = zeros(3,6);
-#+end_src
-
-#+begin_src matlab
-  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
-#+end_src
-
-**** Populate the =stewart= structure
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  stewart.platform_F.Fa = Fa;
-  stewart.platform_M.Mb = Mb;
-#+end_src
-
-*** =computeJointsPose=: Compute the Pose of the Joints
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/computeJointsPose.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:computeJointsPose>>
-
-This Matlab function is accessible [[file:./matlab/src/computeJointsPose.m][here]].
-
-**** Documentation
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-#+name: fig:stewart-struts
-#+caption: Position and orientation of the struts
-[[file:figs/stewart-struts.png]]
-
-**** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  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}
-#+end_src
-
-**** Check the =stewart= structure elements
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  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;
-#+end_src
-
-**** Compute the position of the Joints
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  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]);
-#+end_src
-
-**** Compute the strut length and orientation
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As
-
-  l = vecnorm(Ab - Aa)';
-#+end_src
-
-#+begin_src matlab
-  Bs = (Bb - Ba)./vecnorm(Bb - Ba);
-#+end_src
-
-**** Compute the orientation of the Joints
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  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
-#+end_src
-
-**** Populate the =stewart= structure
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  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;
-#+end_src
-
-*** =initializeStewartPose=: Determine the initial stroke in each leg to have the wanted pose
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/initializeStewartPose.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:initializeStewartPose>>
-
-This Matlab function is accessible [[file:./matlab/src/initializeStewartPose.m][here]].
-
-**** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  function [stewart] = initializeStewartPose(stewart, args)
-  % initializeStewartPose - Determine the initial stroke in each leg to have the wanted pose
-  %                         It uses the inverse kinematic
-  %
-  % Syntax: [stewart] = initializeStewartPose(stewart, args)
-  %
-  % Inputs:
-  %    - stewart - A structure with the following fields
-  %        - Aa   [3x6] - The positions ai expressed in {A}
-  %        - Bb   [3x6] - The positions bi expressed in {B}
-  %    - args - Can have the following fields:
-  %        - AP   [3x1] - The wanted position of {B} with respect to {A}
-  %        - ARB  [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
-  %
-  % Outputs:
-  %    - stewart - updated Stewart structure with the added fields:
-  %      - actuators.Leq [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
-#+end_src
-
-**** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  arguments
-      stewart
-      args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
-      args.ARB (3,3) double {mustBeNumeric} = eye(3)
-  end
-#+end_src
-
-**** Use the Inverse Kinematic function
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  [Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);
-#+end_src
-
-**** Populate the =stewart= structure
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  stewart.actuators.Leq = dLi;
-#+end_src
-
-*** =initializeCylindricalPlatforms=: Initialize the geometry of the Fixed and Mobile Platforms
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/initializeCylindricalPlatforms.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:initializeCylindricalPlatforms>>
-
-This Matlab function is accessible [[file:./matlab/src/initializeCylindricalPlatforms.m][here]].
-
-**** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  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]
-#+end_src
-
-**** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  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
-#+end_src
-
-**** Compute the Inertia matrices of platforms
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  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]);
-#+end_src
-
-#+begin_src matlab
-  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]);
-#+end_src
-
-**** Populate the =stewart= structure
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  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;
-#+end_src
-
-#+begin_src matlab
-  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;
-#+end_src
-
-*** =initializeCylindricalStruts=: Define the inertia of cylindrical struts
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/initializeCylindricalStruts.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:initializeCylindricalStruts>>
-
-This Matlab function is accessible [[file:./matlab/src/initializeCylindricalStruts.m][here]].
-
-**** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  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]
-#+end_src
-
-**** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  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
-#+end_src
-
-**** Compute the properties of the cylindrical struts
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-#+begin_src matlab
-  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;
-#+end_src
-
-#+begin_src matlab
-  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
-#+end_src
-
-**** Populate the =stewart= structure
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  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;
-#+end_src
-
-#+begin_src matlab
-  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;
-#+end_src
-
-*** =initializeStrutDynamics=: Add Stiffness and Damping properties of each strut
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/initializeStrutDynamics.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:initializeStrutDynamics>>
-
-This Matlab function is accessible [[file:./matlab/src/initializeStrutDynamics.m][here]].
-
-**** Documentation
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-#+name: fig:piezoelectric_stack
-#+attr_html: :width 500px
-#+caption: Example of a piezoelectric stach actuator (PI)
-[[file:figs/piezoelectric_stack.jpg]]
-
-A simplistic model of such amplified actuator is shown in Figure [[fig:actuator_model_simple]] where:
-- $K$ represent the vertical stiffness of the actuator
-- $C$ represent the vertical damping of the actuator
-- $F$ represents the force applied by the actuator
-- $F_{m}$ represents the total measured force
-- $v_{m}$ represents the absolute velocity of the top part of the actuator
-- $d_{m}$ represents the total relative displacement of the actuator
-
-#+begin_src latex :file actuator_model_simple.pdf :tangle no
-  \begin{tikzpicture}
-    \draw (-1, 0) -- (1, 0);
-
-    % Spring, Damper, and Actuator
-    \draw[spring]   (-1, 0) -- (-1, 1.5) node[midway, left=0.1]{$K$};
-    \draw[damper]   ( 0, 0) -- ( 0, 1.5) node[midway, left=0.2]{$C$};
-    \draw[actuator] ( 1, 0) -- ( 1, 1.5) node[midway, left=0.1](F){$F$};
-
-    \node[forcesensor={2}{0.2}] (fsens) at (0, 1.5){};
-
-    \node[left] at (fsens.west) {$F_{m}$};
-
-    \draw[dashed] (1, 0) -- ++(0.4, 0);
-    \draw[dashed] (1, 1.7) -- ++(0.4, 0);
-
-    \draw[->] (0, 1.7)node[]{$\bullet$} -- ++(0, 0.5) node[right]{$v_{m}$};
-
-    \draw[<->] (1.4, 0) -- ++(0, 1.7) node[midway, right]{$d_{m}$};
-  \end{tikzpicture}
-#+end_src
-
-#+name: fig:actuator_model_simple
-#+caption: Simple model of an Actuator
-#+RESULTS:
-[[file:figs/actuator_model_simple.png]]
-
-**** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  function [stewart] = initializeStrutDynamics(stewart, args)
-  % initializeStrutDynamics - Add Stiffness and Damping properties of each strut
-  %
-  % Syntax: [stewart] = initializeStrutDynamics(args)
-  %
-  % Inputs:
-  %    - args - Structure with the following fields:
-  %        - K [6x1] - Stiffness of each strut [N/m]
-  %        - C [6x1] - Damping of each strut [N/(m/s)]
-  %
-  % Outputs:
-  %    - stewart - updated Stewart structure with the added fields:
-  %      - actuators.type = 1
-  %      - actuators.K [6x1] - Stiffness of each strut [N/m]
-  %      - actuators.C [6x1] - Damping of each strut [N/(m/s)]
-#+end_src
-
-**** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  arguments
-      stewart
-      args.type char {mustBeMember(args.type,{'classical', 'amplified'})} = 'classical'
-      args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = 20e6*ones(6,1)
-      args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e1*ones(6,1)
-      args.k1 (6,1) double {mustBeNumeric} = 1e6*ones(6,1)
-      args.ke (6,1) double {mustBeNumeric} = 5e6*ones(6,1)
-      args.ka (6,1) double {mustBeNumeric} = 60e6*ones(6,1)
-      args.c1 (6,1) double {mustBeNumeric} = 10*ones(6,1)
-      args.F_gain (6,1) double {mustBeNumeric} = 1*ones(6,1)
-      args.me (6,1) double {mustBeNumeric} = 0.01*ones(6,1)
-      args.ma (6,1) double {mustBeNumeric} = 0.01*ones(6,1)
-  end
-#+end_src
-
-**** Add Stiffness and Damping properties of each strut
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  if strcmp(args.type, 'classical')
-      stewart.actuators.type = 1;
-  elseif strcmp(args.type, 'amplified')
-      stewart.actuators.type = 2;
-  end
-
-  stewart.actuators.K = args.K;
-  stewart.actuators.C = args.C;
-
-  stewart.actuators.k1 = args.k1;
-  stewart.actuators.c1 = args.c1;
-
-  stewart.actuators.ka = args.ka;
-  stewart.actuators.ke = args.ke;
-
-  stewart.actuators.F_gain = args.F_gain;
-
-  stewart.actuators.ma = args.ma;
-  stewart.actuators.me = args.me;
-#+end_src
-
-*** =initializeJointDynamics=: Add Stiffness and Damping properties for spherical joints
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/initializeJointDynamics.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:initializeJointDynamics>>
-
-This Matlab function is accessible [[file:./matlab/src/initializeJointDynamics.m][here]].
-
-**** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  function [stewart] = initializeJointDynamics(stewart, args)
-  % initializeJointDynamics - Add Stiffness and Damping properties for the spherical joints
-  %
-  % Syntax: [stewart] = initializeJointDynamics(args)
-  %
-  % Inputs:
-  %    - args - Structure with the following fields:
-  %        - type_F - 'universal', 'spherical', 'universal_p', 'spherical_p'
-  %        - type_M - 'universal', 'spherical', 'universal_p', 'spherical_p'
-  %        - Kf_M [6x1] - Bending (Rx, Ry) Stiffness for each top joints [(N.m)/rad]
-  %        - Kt_M [6x1] - Torsion (Rz) Stiffness for each top joints [(N.m)/rad]
-  %        - Cf_M [6x1] - Bending (Rx, Ry) Damping of each top joint [(N.m)/(rad/s)]
-  %        - Ct_M [6x1] - Torsion (Rz) Damping of each top joint [(N.m)/(rad/s)]
-  %        - Kf_F [6x1] - Bending (Rx, Ry) Stiffness for each bottom joints [(N.m)/rad]
-  %        - Kt_F [6x1] - Torsion (Rz) Stiffness for each bottom joints [(N.m)/rad]
-  %        - Cf_F [6x1] - Bending (Rx, Ry) Damping of each bottom joint [(N.m)/(rad/s)]
-  %        - Cf_F [6x1] - Torsion (Rz) Damping of each bottom joint [(N.m)/(rad/s)]
-  %
-  % Outputs:
-  %    - stewart - updated Stewart structure with the added fields:
-  %      - stewart.joints_F and stewart.joints_M:
-  %        - type - 1 (universal), 2 (spherical), 3 (universal perfect), 4 (spherical perfect)
-  %        - Kx, Ky, Kz [6x1] - Translation (Tx, Ty, Tz) Stiffness [N/m]
-  %        - Kf [6x1] - Flexion (Rx, Ry) Stiffness [(N.m)/rad]
-  %        - Kt [6x1] - Torsion (Rz) Stiffness [(N.m)/rad]
-  %        - Cx, Cy, Cz [6x1] - Translation (Rx, Ry) Damping [N/(m/s)]
-  %        - Cf [6x1] - Flexion (Rx, Ry) Damping [(N.m)/(rad/s)]
-  %        - Cb [6x1] - Torsion (Rz) Damping [(N.m)/(rad/s)]
-#+end_src
-
-**** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  arguments
-      stewart
-      args.type_F     char   {mustBeMember(args.type_F,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'universal'
-      args.type_M     char   {mustBeMember(args.type_M,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'spherical'
-      args.Kf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1)
-      args.Cf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
-      args.Kt_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1)
-      args.Ct_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
-      args.Kf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1)
-      args.Cf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
-      args.Kt_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1)
-      args.Ct_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
-      args.Ka_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1)
-      args.Ca_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
-      args.Kr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1)
-      args.Cr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
-      args.Ka_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1)
-      args.Ca_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
-      args.Kr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1)
-      args.Cr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
-      args.K_M        double {mustBeNumeric} = zeros(6,6)
-      args.M_M        double {mustBeNumeric} = zeros(6,6)
-      args.n_xyz_M    double {mustBeNumeric} = zeros(2,3)
-      args.xi_M       double {mustBeNumeric} = 0.1
-      args.step_file_M char {} = ''
-      args.K_F        double {mustBeNumeric} = zeros(6,6)
-      args.M_F        double {mustBeNumeric} = zeros(6,6)
-      args.n_xyz_F    double {mustBeNumeric} = zeros(2,3)
-      args.xi_F       double {mustBeNumeric} = 0.1
-      args.step_file_F char {} = ''
-  end
-#+end_src
-
-**** Add Actuator Type
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  switch args.type_F
-    case 'universal'
-      stewart.joints_F.type = 1;
-    case 'spherical'
-      stewart.joints_F.type = 2;
-    case 'universal_p'
-      stewart.joints_F.type = 3;
-    case 'spherical_p'
-      stewart.joints_F.type = 4;
-    case 'flexible'
-      stewart.joints_F.type = 5;
-    case 'universal_3dof'
-      stewart.joints_F.type = 6;
-    case 'spherical_3dof'
-      stewart.joints_F.type = 7;
-  end
-
-  switch args.type_M
-    case 'universal'
-      stewart.joints_M.type = 1;
-    case 'spherical'
-      stewart.joints_M.type = 2;
-    case 'universal_p'
-      stewart.joints_M.type = 3;
-    case 'spherical_p'
-      stewart.joints_M.type = 4;
-    case 'flexible'
-      stewart.joints_M.type = 5;
-    case 'universal_3dof'
-      stewart.joints_M.type = 6;
-    case 'spherical_3dof'
-      stewart.joints_M.type = 7;
-  end
-#+end_src
-
-**** Add Stiffness and Damping in Translation of each strut
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-Axial and Radial (shear) Stiffness
-#+begin_src matlab
-  stewart.joints_M.Ka = args.Ka_M;
-  stewart.joints_M.Kr = args.Kr_M;
-
-  stewart.joints_F.Ka = args.Ka_F;
-  stewart.joints_F.Kr = args.Kr_F;
-#+end_src
-
-Translation Damping
-#+begin_src matlab
-  stewart.joints_M.Ca = args.Ca_M;
-  stewart.joints_M.Cr = args.Cr_M;
-
-  stewart.joints_F.Ca = args.Ca_F;
-  stewart.joints_F.Cr = args.Cr_F;
-#+end_src
-
-**** Add Stiffness and Damping in Rotation of each strut
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-Rotational Stiffness
-#+begin_src matlab
-  stewart.joints_M.Kf = args.Kf_M;
-  stewart.joints_M.Kt = args.Kt_M;
-
-  stewart.joints_F.Kf = args.Kf_F;
-  stewart.joints_F.Kt = args.Kt_F;
-#+end_src
-
-Rotational Damping
-#+begin_src matlab
-  stewart.joints_M.Cf = args.Cf_M;
-  stewart.joints_M.Ct = args.Ct_M;
-
-  stewart.joints_F.Cf = args.Cf_F;
-  stewart.joints_F.Ct = args.Ct_F;
-#+end_src
-
-**** Stiffness and Mass matrices for flexible joint
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-#+begin_src matlab
-  stewart.joints_F.M = args.M_F;
-  stewart.joints_F.K = args.K_F;
-  stewart.joints_F.n_xyz = args.n_xyz_F;
-  stewart.joints_F.xi = args.xi_F;
-  stewart.joints_F.xi = args.xi_F;
-  stewart.joints_F.step_file = args.step_file_F;
-
-  stewart.joints_M.M = args.M_M;
-  stewart.joints_M.K = args.K_M;
-  stewart.joints_M.n_xyz = args.n_xyz_M;
-  stewart.joints_M.xi = args.xi_M;
-  stewart.joints_M.step_file = args.step_file_M;
-#+end_src
-
-*** =initializeInertialSensor=: Initialize the inertial sensor in each strut
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/initializeInertialSensor.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:initializeInertialSensor>>
-
-This Matlab function is accessible [[file:./matlab/src/initializeInertialSensor.m][here]].
-
-**** Geophone - Working Principle
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-From the schematic of the Z-axis geophone shown in Figure [[fig:z_axis_geophone]], we can write the transfer function from the support velocity $\dot{w}$ to the relative velocity of the inertial mass $\dot{d}$:
-\[ \frac{\dot{d}}{\dot{w}} = \frac{-\frac{s^2}{{\omega_0}^2}}{\frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1} \]
-with:
-- $\omega_0 = \sqrt{\frac{k}{m}}$
-- $\xi = \frac{1}{2} \sqrt{\frac{m}{k}}$
-
-#+name: fig:z_axis_geophone
-#+caption: Schematic of a Z-Axis geophone
-[[file:figs/inertial_sensor.png]]
-
-We see that at frequencies above $\omega_0$:
-\[ \frac{\dot{d}}{\dot{w}} \approx -1 \]
-
-And thus, the measurement of the relative velocity of the mass with respect to its support gives the absolute velocity of the support.
-
-We generally want to have the smallest resonant frequency $\omega_0$ to measure low frequency absolute velocity, however there is a trade-off between $\omega_0$ and the mass of the inertial mass.
-
-**** Accelerometer - Working Principle
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-From the schematic of the Z-axis accelerometer shown in Figure [[fig:z_axis_accelerometer]], we can write the transfer function from the support acceleration $\ddot{w}$ to the relative position of the inertial mass $d$:
-\[ \frac{d}{\ddot{w}} = \frac{-\frac{1}{{\omega_0}^2}}{\frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1} \]
-with:
-- $\omega_0 = \sqrt{\frac{k}{m}}$
-- $\xi = \frac{1}{2} \sqrt{\frac{m}{k}}$
-
-#+name: fig:z_axis_accelerometer
-#+caption: Schematic of a Z-Axis geophone
-[[file:figs/inertial_sensor.png]]
-
-We see that at frequencies below $\omega_0$:
-\[ \frac{d}{\ddot{w}} \approx -\frac{1}{{\omega_0}^2} \]
-
-And thus, the measurement of the relative displacement of the mass with respect to its support gives the absolute acceleration of the support.
-
-Note that there is trade-off between:
-- the highest measurable acceleration $\omega_0$
-- the sensitivity of the accelerometer which is equal to $-\frac{1}{{\omega_0}^2}$
-
-**** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  function [stewart] = initializeInertialSensor(stewart, args)
-  % initializeInertialSensor - Initialize the inertial sensor in each strut
-  %
-  % Syntax: [stewart] = initializeInertialSensor(args)
-  %
-  % Inputs:
-  %    - args - Structure with the following fields:
-  %        - type       - 'geophone', 'accelerometer', 'none'
-  %        - mass [1x1] - Weight of the inertial mass [kg]
-  %        - freq [1x1] - Cutoff frequency [Hz]
-  %
-  % Outputs:
-  %    - stewart - updated Stewart structure with the added fields:
-  %      - stewart.sensors.inertial
-  %        - type    - 1 (geophone), 2 (accelerometer), 3 (none)
-  %        - K [1x1] - Stiffness [N/m]
-  %        - C [1x1] - Damping [N/(m/s)]
-  %        - M [1x1] - Inertial Mass [kg]
-  %        - G [1x1] - Gain
-#+end_src
-
-**** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  arguments
-      stewart
-      args.type       char   {mustBeMember(args.type,{'geophone', 'accelerometer', 'none'})} = 'none'
-      args.mass (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e-2
-      args.freq (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e3
-  end
-#+end_src
-
-**** Compute the properties of the sensor
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  sensor = struct();
-
-  switch args.type
-    case 'geophone'
-      sensor.type = 1;
-
-      sensor.M = args.mass;
-      sensor.K = sensor.M * (2*pi*args.freq)^2;
-      sensor.C = 2*sqrt(sensor.M * sensor.K);
-    case 'accelerometer'
-      sensor.type = 2;
-
-      sensor.M = args.mass;
-      sensor.K = sensor.M * (2*pi*args.freq)^2;
-      sensor.C = 2*sqrt(sensor.M * sensor.K);
-      sensor.G = -sensor.K/sensor.M;
-    case 'none'
-      sensor.type = 3;
-  end
-#+end_src
-
-**** Populate the =stewart= structure
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  stewart.sensors.inertial = sensor;
-#+end_src
-
-*** =displayArchitecture=: 3D plot of the Stewart platform architecture
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/displayArchitecture.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:displayArchitecture>>
-
-This Matlab function is accessible [[file:./matlab/src/displayArchitecture.m][here]].
-
-**** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  function [] = displayArchitecture(stewart, args)
-  % displayArchitecture - 3D plot of the Stewart platform architecture
-  %
-  % Syntax: [] = displayArchitecture(args)
-  %
-  % Inputs:
-  %    - stewart
-  %    - args - Structure with the following fields:
-  %        - AP   [3x1] - The wanted position of {B} with respect to {A}
-  %        - ARB  [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
-  %        - ARB  [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
-  %        - F_color [color] - Color used for the Fixed elements
-  %        - M_color [color] - Color used for the Mobile elements
-  %        - L_color [color] - Color used for the Legs elements
-  %        - frames    [true/false] - Display the Frames
-  %        - legs      [true/false] - Display the Legs
-  %        - joints    [true/false] - Display the Joints
-  %        - labels    [true/false] - Display the Labels
-  %        - platforms [true/false] - Display the Platforms
-  %        - views     ['all', 'xy', 'yz', 'xz', 'default'] -
-  %
-  % Outputs:
-#+end_src
-
-**** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  arguments
-      stewart
-      args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
-      args.ARB (3,3) double {mustBeNumeric} = eye(3)
-      args.F_color = [0 0.4470 0.7410]
-      args.M_color = [0.8500 0.3250 0.0980]
-      args.L_color = [0 0 0]
-      args.frames    logical {mustBeNumericOrLogical} = true
-      args.legs      logical {mustBeNumericOrLogical} = true
-      args.joints    logical {mustBeNumericOrLogical} = true
-      args.labels    logical {mustBeNumericOrLogical} = true
-      args.platforms logical {mustBeNumericOrLogical} = true
-      args.views     char    {mustBeMember(args.views,{'all', 'xy', 'xz', 'yz', 'default'})} = 'default'
-  end
-#+end_src
-
-**** Check the =stewart= structure elements
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  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, 'H'),   'stewart.geometry should have attribute H')
-  H = stewart.geometry.H;
-
-  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;
-#+end_src
-
-
-**** Figure Creation, Frames and Homogeneous transformations
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-The reference frame of the 3d plot corresponds to the frame $\{F\}$.
-#+begin_src matlab
-  if ~strcmp(args.views, 'all')
-    figure;
-  else
-    f = figure('visible', 'off');
-  end
-
-  hold on;
-#+end_src
-
-We first compute homogeneous matrices that will be useful to position elements on the figure where the reference frame is $\{F\}$.
-#+begin_src matlab
-  FTa = [eye(3), FO_A; ...
-         zeros(1,3), 1];
-  ATb = [args.ARB, args.AP; ...
-         zeros(1,3), 1];
-  BTm = [eye(3), -MO_B; ...
-         zeros(1,3), 1];
-
-  FTm = FTa*ATb*BTm;
-#+end_src
-
-Let's define a parameter that define the length of the unit vectors used to display the frames.
-#+begin_src matlab
-  d_unit_vector = H/4;
-#+end_src
-
-Let's define a parameter used to position the labels with respect to the center of the element.
-#+begin_src matlab
-  d_label = H/20;
-#+end_src
-
-**** Fixed Base elements
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-Let's first plot the frame $\{F\}$.
-#+begin_src matlab
-  Ff = [0, 0, 0];
-  if args.frames
-    quiver3(Ff(1)*ones(1,3), Ff(2)*ones(1,3), Ff(3)*ones(1,3), ...
-            [d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color)
-
-    if args.labels
-      text(Ff(1) + d_label, ...
-          Ff(2) + d_label, ...
-          Ff(3) + d_label, '$\{F\}$', 'Color', args.F_color);
-    end
-  end
-#+end_src
-
-Now plot the frame $\{A\}$ fixed to the Base.
-#+begin_src matlab
-  if args.frames
-    quiver3(FO_A(1)*ones(1,3), FO_A(2)*ones(1,3), FO_A(3)*ones(1,3), ...
-            [d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color)
-
-    if args.labels
-      text(FO_A(1) + d_label, ...
-           FO_A(2) + d_label, ...
-           FO_A(3) + d_label, '$\{A\}$', 'Color', args.F_color);
-    end
-  end
-#+end_src
-
-Let's then plot the circle corresponding to the shape of the Fixed base.
-#+begin_src matlab
-  if args.platforms && stewart.platform_F.type == 1
-    theta = [0:0.01:2*pi+0.01]; % Angles [rad]
-    v = null([0; 0; 1]'); % Two vectors that are perpendicular to the circle normal
-    center = [0; 0; 0]; % Center of the circle
-    radius = stewart.platform_F.R; % Radius of the circle [m]
-
-    points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));
-
-    plot3(points(1,:), ...
-          points(2,:), ...
-          points(3,:), '-', 'Color', args.F_color);
-  end
-#+end_src
-
-Let's now plot the position and labels of the Fixed Joints
-#+begin_src matlab
-  if args.joints
-    scatter3(Fa(1,:), ...
-             Fa(2,:), ...
-             Fa(3,:), 'MarkerEdgeColor', args.F_color);
-    if args.labels
-      for i = 1:size(Fa,2)
-        text(Fa(1,i) + d_label, ...
-             Fa(2,i), ...
-             Fa(3,i), sprintf('$a_{%i}$', i), 'Color', args.F_color);
-      end
-    end
-  end
-#+end_src
-
-**** Mobile Platform elements
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-
-Plot the frame $\{M\}$.
-#+begin_src matlab
-  Fm = FTm*[0; 0; 0; 1]; % Get the position of frame {M} w.r.t. {F}
-
-  if args.frames
-    FM_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
-    quiver3(Fm(1)*ones(1,3), Fm(2)*ones(1,3), Fm(3)*ones(1,3), ...
-            FM_uv(1,1:3), FM_uv(2,1:3), FM_uv(3,1:3), '-', 'Color', args.M_color)
-
-    if args.labels
-      text(Fm(1) + d_label, ...
-           Fm(2) + d_label, ...
-           Fm(3) + d_label, '$\{M\}$', 'Color', args.M_color);
-    end
-  end
-#+end_src
-
-Plot the frame $\{B\}$.
-#+begin_src matlab
-  FB = FO_A + args.AP;
-
-  if args.frames
-    FB_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
-    quiver3(FB(1)*ones(1,3), FB(2)*ones(1,3), FB(3)*ones(1,3), ...
-            FB_uv(1,1:3), FB_uv(2,1:3), FB_uv(3,1:3), '-', 'Color', args.M_color)
-
-    if args.labels
-      text(FB(1) - d_label, ...
-           FB(2) + d_label, ...
-           FB(3) + d_label, '$\{B\}$', 'Color', args.M_color);
-    end
-  end
-#+end_src
-
-Let's then plot the circle corresponding to the shape of the Mobile platform.
-#+begin_src matlab
-  if args.platforms && stewart.platform_M.type == 1
-    theta = [0:0.01:2*pi+0.01]; % Angles [rad]
-    v = null((FTm(1:3,1:3)*[0;0;1])'); % Two vectors that are perpendicular to the circle normal
-    center = Fm(1:3); % Center of the circle
-    radius = stewart.platform_M.R; % Radius of the circle [m]
-
-    points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));
-
-    plot3(points(1,:), ...
-          points(2,:), ...
-          points(3,:), '-', 'Color', args.M_color);
-  end
-#+end_src
-
-Plot the position and labels of the rotation joints fixed to the mobile platform.
-#+begin_src matlab
-  if args.joints
-    Fb = FTm*[Mb;ones(1,6)];
-
-    scatter3(Fb(1,:), ...
-             Fb(2,:), ...
-             Fb(3,:), 'MarkerEdgeColor', args.M_color);
-
-    if args.labels
-      for i = 1:size(Fb,2)
-        text(Fb(1,i) + d_label, ...
-             Fb(2,i), ...
-             Fb(3,i), sprintf('$b_{%i}$', i), 'Color', args.M_color);
-      end
-    end
-  end
-#+end_src
-
-**** Legs
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-Plot the legs connecting the joints of the fixed base to the joints of the mobile platform.
-#+begin_src matlab
-  if args.legs
-    for i = 1:6
-      plot3([Fa(1,i), Fb(1,i)], ...
-            [Fa(2,i), Fb(2,i)], ...
-            [Fa(3,i), Fb(3,i)], '-', 'Color', args.L_color);
-
-      if args.labels
-        text((Fa(1,i)+Fb(1,i))/2 + d_label, ...
-             (Fa(2,i)+Fb(2,i))/2, ...
-             (Fa(3,i)+Fb(3,i))/2, sprintf('$%i$', i), 'Color', args.L_color);
-      end
-    end
-  end
-#+end_src
-
-**** Figure parameters
-#+begin_src matlab
-  switch args.views
-    case 'default'
-        view([1 -0.6 0.4]);
-    case 'xy'
-        view([0 0 1]);
-    case 'xz'
-        view([0 -1 0]);
-    case 'yz'
-        view([1 0 0]);
-  end
-  axis equal;
-  axis off;
-#+end_src
-
-**** Subplots
-#+begin_src matlab
-  if strcmp(args.views, 'all')
-    hAx = findobj('type', 'axes');
-
-    figure;
-    s1 = subplot(2,2,1);
-    copyobj(get(hAx(1), 'Children'), s1);
-    view([0 0 1]);
-    axis equal;
-    axis off;
-    title('Top')
-
-    s2 = subplot(2,2,2);
-    copyobj(get(hAx(1), 'Children'), s2);
-    view([1 -0.6 0.4]);
-    axis equal;
-    axis off;
-
-    s3 = subplot(2,2,3);
-    copyobj(get(hAx(1), 'Children'), s3);
-    view([1 0 0]);
-    axis equal;
-    axis off;
-    title('Front')
-
-    s4 = subplot(2,2,4);
-    copyobj(get(hAx(1), 'Children'), s4);
-    view([0 -1 0]);
-    axis equal;
-    axis off;
-    title('Side')
-
-    close(f);
-  end
-#+end_src
-
-
-*** =describeStewartPlatform=: Display some text describing the current defined Stewart Platform
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/describeStewartPlatform.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:describeStewartPlatform>>
-
-This Matlab function is accessible [[file:./matlab/src/describeStewartPlatform.m][here]].
-
-**** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  function [] = describeStewartPlatform(stewart)
-  % describeStewartPlatform - Display some text describing the current defined Stewart Platform
-  %
-  % Syntax: [] = describeStewartPlatform(args)
-  %
-  % Inputs:
-  %    - stewart
-  %
-  % Outputs:
-#+end_src
-
-**** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  arguments
-      stewart
-  end
-#+end_src
-
-**** Geometry
-#+begin_src matlab
-  fprintf('GEOMETRY:\n')
-  fprintf('- The height between the fixed based and the top platform is %.3g [mm].\n', 1e3*stewart.geometry.H)
-
-  if stewart.platform_M.MO_B(3) > 0
-    fprintf('- Frame {A} is located %.3g [mm] above the top platform.\n',  1e3*stewart.platform_M.MO_B(3))
-  else
-    fprintf('- Frame {A} is located %.3g [mm] below the top platform.\n', - 1e3*stewart.platform_M.MO_B(3))
-  end
-
-  fprintf('- The initial length of the struts are:\n')
-  fprintf('\t %.3g, %.3g, %.3g, %.3g, %.3g, %.3g [mm]\n', 1e3*stewart.geometry.l)
-  fprintf('\n')
-#+end_src
-
-**** Actuators
-#+begin_src matlab
-  fprintf('ACTUATORS:\n')
-  if stewart.actuators.type == 1
-      fprintf('- The actuators are classical.\n')
-      fprintf('- The Stiffness and Damping of each actuators is:\n')
-      fprintf('\t k = %.0e [N/m] \t c = %.0e [N/(m/s)]\n', stewart.actuators.K(1), stewart.actuators.C(1))
-  elseif stewart.actuators.type == 2
-      fprintf('- The actuators are mechanicaly amplified.\n')
-      fprintf('- The vertical stiffness and damping contribution of the piezoelectric stack is:\n')
-      fprintf('\t ka = %.0e [N/m] \t ca = %.0e [N/(m/s)]\n', stewart.actuators.Ka(1), stewart.actuators.Ca(1))
-      fprintf('- Vertical stiffness when the piezoelectric stack is removed is:\n')
-      fprintf('\t kr = %.0e [N/m] \t cr = %.0e [N/(m/s)]\n', stewart.actuators.Kr(1), stewart.actuators.Cr(1))
-  end
-  fprintf('\n')
-#+end_src
-
-**** Joints
-#+begin_src matlab
-  fprintf('JOINTS:\n')
-#+end_src
-
-Type of the joints on the fixed base.
-#+begin_src matlab
-  switch stewart.joints_F.type
-    case 1
-      fprintf('- The joints on the fixed based are universal joints\n')
-    case 2
-      fprintf('- The joints on the fixed based are spherical joints\n')
-    case 3
-      fprintf('- The joints on the fixed based are perfect universal joints\n')
-    case 4
-      fprintf('- The joints on the fixed based are perfect spherical joints\n')
-  end
-#+end_src
-
-Type of the joints on the mobile platform.
-#+begin_src matlab
-  switch stewart.joints_M.type
-    case 1
-      fprintf('- The joints on the mobile based are universal joints\n')
-    case 2
-      fprintf('- The joints on the mobile based are spherical joints\n')
-    case 3
-      fprintf('- The joints on the mobile based are perfect universal joints\n')
-    case 4
-      fprintf('- The joints on the mobile based are perfect spherical joints\n')
-  end
-#+end_src
-
-Position of the fixed joints
-#+begin_src matlab
-  fprintf('- The position of the joints on the fixed based with respect to {F} are (in [mm]):\n')
-  fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_F.Fa)
-#+end_src
-
-Position of the mobile joints
-#+begin_src matlab
-  fprintf('- The position of the joints on the mobile based with respect to {M} are (in [mm]):\n')
-  fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_M.Mb)
-  fprintf('\n')
-#+end_src
-
-**** Kinematics
-#+begin_src matlab
-  fprintf('KINEMATICS:\n')
-
-  if isfield(stewart.kinematics, 'K')
-    fprintf('- The Stiffness matrix K is (in [N/m]):\n')
-    fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.K)
-  end
-
-  if isfield(stewart.kinematics, 'C')
-    fprintf('- The Damping matrix C is (in [m/N]):\n')
-    fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.C)
-  end
-#+end_src
-
-*** =generateCubicConfiguration=: Generate a Cubic Configuration
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/generateCubicConfiguration.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:generateCubicConfiguration>>
-
-This Matlab function is accessible [[file:./matlab/src/generateCubicConfiguration.m][here]].
-
-**** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  function [stewart] = generateCubicConfiguration(stewart, args)
-  % generateCubicConfiguration - Generate a Cubic Configuration
-  %
-  % Syntax: [stewart] = generateCubicConfiguration(stewart, args)
-  %
-  % Inputs:
-  %    - stewart - A structure with the following fields
-  %        - geometry.H [1x1] - Total height of the platform [m]
-  %    - args - Can have the following fields:
-  %        - Hc  [1x1] - Height of the "useful" part of the cube [m]
-  %        - FOc [1x1] - Height of the center of the cube with respect to {F} [m]
-  %        - FHa [1x1] - Height of the plane joining the points ai with respect to the frame {F} [m]
-  %        - MHb [1x1] - Height of the plane joining the points bi with respect to the frame {M} [m]
-  %
-  % 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}
-#+end_src
-
-**** Documentation
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+name: fig:cubic-configuration-definition
-#+caption: Cubic Configuration
-[[file:figs/cubic-configuration-definition.png]]
-
-**** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  arguments
-      stewart
-      args.Hc  (1,1) double {mustBeNumeric, mustBePositive} = 60e-3
-      args.FOc (1,1) double {mustBeNumeric} = 50e-3
-      args.FHa (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3
-      args.MHb (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3
-  end
-#+end_src
-
-**** Check the =stewart= structure elements
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  assert(isfield(stewart.geometry, 'H'),   'stewart.geometry should have attribute H')
-  H = stewart.geometry.H;
-#+end_src
-
-**** Position of the Cube
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-We define the useful points of the cube with respect to the Cube's center.
-${}^{C}C$ are the 6 vertices of the cubes expressed in a frame {C} which is located at the center of the cube and aligned with {F} and {M}.
-
-#+begin_src matlab
-  sx = [ 2; -1; -1];
-  sy = [ 0;  1; -1];
-  sz = [ 1;  1;  1];
-
-  R = [sx, sy, sz]./vecnorm([sx, sy, sz]);
-
-  L = args.Hc*sqrt(3);
-
-  Cc = R'*[[0;0;L],[L;0;L],[L;0;0],[L;L;0],[0;L;0],[0;L;L]] - [0;0;1.5*args.Hc];
-
-  CCf = [Cc(:,1), Cc(:,3), Cc(:,3), Cc(:,5), Cc(:,5), Cc(:,1)]; % CCf(:,i) corresponds to the bottom cube's vertice corresponding to the i'th leg
-  CCm = [Cc(:,2), Cc(:,2), Cc(:,4), Cc(:,4), Cc(:,6), Cc(:,6)]; % CCm(:,i) corresponds to the top cube's vertice corresponding to the i'th leg
-#+end_src
-
-**** Compute the pose
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-We can compute the vector of each leg ${}^{C}\hat{\bm{s}}_{i}$ (unit vector from ${}^{C}C_{f}$ to ${}^{C}C_{m}$).
-#+begin_src matlab
-  CSi = (CCm - CCf)./vecnorm(CCm - CCf);
-#+end_src
-
-We now which to compute the position of the joints $a_{i}$ and $b_{i}$.
-#+begin_src matlab
-  Fa = CCf + [0; 0; args.FOc] + ((args.FHa-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
-  Mb = CCf + [0; 0; args.FOc-H] + ((H-args.MHb-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
-#+end_src
-
-**** Populate the =stewart= structure
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  stewart.platform_F.Fa = Fa;
-  stewart.platform_M.Mb = Mb;
-#+end_src
-
-*** =computeJacobian=: Compute the Jacobian Matrix
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/computeJacobian.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:computeJacobian>>
-
-This Matlab function is accessible [[file:./matlab/src/computeJacobian.m][here]].
-
-**** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  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
-#+end_src
-
-**** Check the =stewart= structure elements
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  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;
-#+end_src
-
-
-**** Compute Jacobian Matrix
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  J = [As' , cross(Ab, As)'];
-#+end_src
-
-**** Compute Stiffness Matrix
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  K = J'*diag(Ki)*J;
-#+end_src
-
-**** Compute Compliance Matrix
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  C = inv(K);
-#+end_src
-
-**** Populate the =stewart= structure
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  stewart.kinematics.J = J;
-  stewart.kinematics.K = K;
-  stewart.kinematics.C = C;
-#+end_src
-
-
-*** =inverseKinematics=: Compute Inverse Kinematics
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/inverseKinematics.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:inverseKinematics>>
-
-This Matlab function is accessible [[file:./matlab/src/inverseKinematics.m][here]].
-
-**** Theory
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-For inverse kinematic analysis, it is assumed that the position ${}^A\bm{P}$ and orientation of the moving platform ${}^A\bm{R}_B$ are given and the problem is to obtain the joint variables, namely, $\bm{L} = [l_1, l_2, \dots, l_6]^T$.
-
-From the geometry of the manipulator, the loop closure for each limb, $i = 1, 2, \dots, 6$ can be written as
-\begin{align*}
-  l_i {}^A\hat{\bm{s}}_i &= {}^A\bm{A} + {}^A\bm{b}_i - {}^A\bm{a}_i \\
-                         &= {}^A\bm{A} + {}^A\bm{R}_b {}^B\bm{b}_i - {}^A\bm{a}_i
-\end{align*}
-
-To obtain the length of each actuator and eliminate $\hat{\bm{s}}_i$, it is sufficient to dot multiply each side by itself:
-\begin{equation}
-  l_i^2 \left[ {}^A\hat{\bm{s}}_i^T {}^A\hat{\bm{s}}_i \right] = \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]^T \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]
-\end{equation}
-
-Hence, for $i = 1, 2, \dots, 6$, each limb length can be uniquely determined by:
-\begin{equation}
-  l_i = \sqrt{{}^A\bm{P}^T {}^A\bm{P} + {}^B\bm{b}_i^T {}^B\bm{b}_i + {}^A\bm{a}_i^T {}^A\bm{a}_i - 2 {}^A\bm{P}^T {}^A\bm{a}_i + 2 {}^A\bm{P}^T \left[{}^A\bm{R}_B {}^B\bm{b}_i\right] - 2 \left[{}^A\bm{R}_B {}^B\bm{b}_i\right]^T {}^A\bm{a}_i}
-\end{equation}
-
-If the position and orientation of the moving platform lie in the feasible workspace of the manipulator, one unique solution to the limb length is determined by the above equation.
-Otherwise, when the limbs' lengths derived yield complex numbers, then the position or orientation of the moving platform is not reachable.
-
-**** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  function [Li, dLi] = inverseKinematics(stewart, args)
-  % inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}
-  %
-  % Syntax: [stewart] = inverseKinematics(stewart)
-  %
-  % Inputs:
-  %    - stewart - A structure with the following fields
-  %        - geometry.Aa   [3x6] - The positions ai expressed in {A}
-  %        - geometry.Bb   [3x6] - The positions bi expressed in {B}
-  %        - geometry.l    [6x1] - Length of each strut
-  %    - args - Can have the following fields:
-  %        - AP   [3x1] - The wanted position of {B} with respect to {A}
-  %        - ARB  [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
-  %
-  % Outputs:
-  %    - Li   [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}
-  %    - dLi  [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
-#+end_src
-
-**** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  arguments
-      stewart
-      args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
-      args.ARB (3,3) double {mustBeNumeric} = eye(3)
-  end
-#+end_src
-
-**** Check the =stewart= structure elements
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  assert(isfield(stewart.geometry, 'Aa'),   'stewart.geometry should have attribute Aa')
-  Aa = stewart.geometry.Aa;
-
-  assert(isfield(stewart.geometry, 'Bb'),   'stewart.geometry should have attribute Bb')
-  Bb = stewart.geometry.Bb;
-
-  assert(isfield(stewart.geometry, 'l'),   'stewart.geometry should have attribute l')
-  l = stewart.geometry.l;
-#+end_src
-
-
-**** Compute
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  Li = sqrt(args.AP'*args.AP + diag(Bb'*Bb) + diag(Aa'*Aa) - (2*args.AP'*Aa)' + (2*args.AP'*(args.ARB*Bb))' - diag(2*(args.ARB*Bb)'*Aa));
-#+end_src
-
-#+begin_src matlab
-  dLi = Li-l;
-#+end_src
-
-*** =forwardKinematicsApprox=: Compute the Approximate Forward Kinematics
-:PROPERTIES:
-:header-args:matlab+: :tangle ./matlab/src/forwardKinematicsApprox.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<<sec:forwardKinematicsApprox>>
-
-This Matlab function is accessible [[file:./matlab/src/forwardKinematicsApprox.m][here]].
-
-**** Function description
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  function [P, R] = forwardKinematicsApprox(stewart, args)
-  % forwardKinematicsApprox - Computed the approximate pose of {B} with respect to {A} from the length of each strut and using
-  %                           the Jacobian Matrix
-  %
-  % Syntax: [P, R] = forwardKinematicsApprox(stewart, args)
-  %
-  % Inputs:
-  %    - stewart - A structure with the following fields
-  %        - kinematics.J  [6x6] - The Jacobian Matrix
-  %    - args - Can have the following fields:
-  %        - dL [6x1] - Displacement of each strut [m]
-  %
-  % Outputs:
-  %    - P  [3x1] - The estimated position of {B} with respect to {A}
-  %    - R  [3x3] - The estimated rotation matrix that gives the orientation of {B} with respect to {A}
-#+end_src
-
-**** Optional Parameters
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  arguments
-      stewart
-      args.dL (6,1) double {mustBeNumeric} = zeros(6,1)
-  end
-#+end_src
-
-**** Check the =stewart= structure elements
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-#+begin_src matlab
-  assert(isfield(stewart.kinematics, 'J'),   'stewart.kinematics should have attribute J')
-  J = stewart.kinematics.J;
-#+end_src
-
-**** Computation
-:PROPERTIES:
-:UNNUMBERED: t
-:END:
-From a small displacement of each strut $d\bm{\mathcal{L}}$, we can compute the
-position and orientation of {B} with respect to {A} using the following formula:
-\[ d \bm{\mathcal{X}} = \bm{J}^{-1} d\bm{\mathcal{L}} \]
-#+begin_src matlab
-  X = J\args.dL;
-#+end_src
-
-The position vector corresponds to the first 3 elements.
-#+begin_src matlab
-  P = X(1:3);
-#+end_src
-
-The next 3 elements are the orientation of {B} with respect to {A} expressed
-using the screw axis.
-#+begin_src matlab
-  theta = norm(X(4:6));
-  s = X(4:6)/theta;
-#+end_src
-
-We then compute the corresponding rotation matrix.
-#+begin_src matlab
-  R = [s(1)^2*(1-cos(theta)) + cos(theta) ,        s(1)*s(2)*(1-cos(theta)) - s(3)*sin(theta), s(1)*s(3)*(1-cos(theta)) + s(2)*sin(theta);
-       s(2)*s(1)*(1-cos(theta)) + s(3)*sin(theta), s(2)^2*(1-cos(theta)) + cos(theta),         s(2)*s(3)*(1-cos(theta)) - s(1)*sin(theta);
-       s(3)*s(1)*(1-cos(theta)) - s(2)*sin(theta), s(3)*s(2)*(1-cos(theta)) + s(1)*sin(theta), s(3)^2*(1-cos(theta)) + cos(theta)];
-#+end_src
+function [Center,Radius] = sphereFit(X)
+% this fits a sphere to a collection of data using a closed form for the
+% solution (opposed to using an array the size of the data set).
+% Minimizes Sum((x-xc)^2+(y-yc)^2+(z-zc)^2-r^2)^2
+% x,y,z are the data, xc,yc,zc are the sphere's center, and r is the radius
+% Assumes that points are not in a singular configuration, real numbers, ...
+% if you have coplanar data, use a circle fit with svd for determining the
+% plane, recommended Circle Fit (Pratt method), by Nikolai Chernov
+% http://www.mathworks.com/matlabcentral/fileexchange/22643
+% Input:
+% X: n x 3 matrix of cartesian data
+% Outputs:
+% Center: Center of sphere
+% Radius: Radius of sphere
+% Author:
+% Alan Jennings, University of Dayton
+A=[mean(X(:,1).*(X(:,1)-mean(X(:,1)))), ...
+    2*mean(X(:,1).*(X(:,2)-mean(X(:,2)))), ...
+    2*mean(X(:,1).*(X(:,3)-mean(X(:,3)))); ...
+    0, ...
+    mean(X(:,2).*(X(:,2)-mean(X(:,2)))), ...
+    2*mean(X(:,2).*(X(:,3)-mean(X(:,3)))); ...
+    0, ...
+    0, ...
+    mean(X(:,3).*(X(:,3)-mean(X(:,3))))];
+A=A+A.';
+B=[mean((X(:,1).^2+X(:,2).^2+X(:,3).^2).*(X(:,1)-mean(X(:,1))));...
+    mean((X(:,1).^2+X(:,2).^2+X(:,3).^2).*(X(:,2)-mean(X(:,2))));...
+    mean((X(:,1).^2+X(:,2).^2+X(:,3).^2).*(X(:,3)-mean(X(:,3))))];
+Center=(A\B).';
+Radius=sqrt(mean(sum([X(:,1)-Center(1),X(:,2)-Center(2),X(:,3)-Center(3)].^2,2)));
+#+end_src
+
+* Footnotes
+[fn:2]M12/F40 model from Attocube
+[fn:1]Depending on the measuring range, gap can range from $\approx 1\,\mu m$ to $\approx 100\,\mu m$
diff --git a/test-bench-id31.pdf b/test-bench-id31.pdf
index 7f74555..8cd81ec 100644
Binary files a/test-bench-id31.pdf and b/test-bench-id31.pdf differ
diff --git a/test-bench-id31.tex b/test-bench-id31.tex
index 011e3e7..2ff06b6 100644
--- a/test-bench-id31.tex
+++ b/test-bench-id31.tex
@@ -1,8 +1,9 @@
-% Created 2024-03-19 Tue 11:22
+% Created 2024-11-13 Wed 10:25
 % Intended LaTeX compiler: pdflatex
 \documentclass[a4paper, 10pt, DIV=12, parskip=full, bibliography=totoc]{scrreprt}
 
 \input{preamble.tex}
+\input{preamble_extra.tex}
 \bibliography{test-bench-id31.bib}
 \author{Dehaeze Thomas}
 \date{\today}
@@ -12,7 +13,7 @@
  pdftitle={Nano-Hexapod on the micro-station},
  pdfkeywords={},
  pdfsubject={},
- pdfcreator={Emacs 29.2 (Org mode 9.7)}, 
+ pdfcreator={Emacs 29.4 (Org mode 9.6)}, 
  pdflang={English}}
 \usepackage{biblatex}
 
@@ -22,72 +23,104 @@
 \tableofcontents
 
 \clearpage
+
+Now that the nano-hexapod is mounted and that a good multi-body model of the nano-hexapod
+The system is validated on the ID31 beamline.
+
+At the beginning of the project, it was planned to develop a long stroke 5-DoF metrology system to measure the pose of the sample with respect to the granite.
+The development of such system was complex, and was not completed at the time of the experimental tests on ID31.
+To still validate the developed nano active platform and the associated instrumentation and control architecture, a 5-DoF short stroke metrology system was developed (Section \ref{sec:test_id31_metrology}).
+
+The identify dynamics of the nano-hexapod fixed on top of the micro-station was identified for different experimental conditions (payload masses, rotational velocities) and compared with the model (Section \ref{sec:test_id31_open_loop_plant}).
+
+Decentralized Integral Force Feedback is then applied to actively damp the plant in a robust way (Section \ref{sec:test_id31_iff}).
+
+High authority control is then applied
+
+\begin{figure}[htbp]
+\begin{subfigure}{0.49\textwidth}
+\begin{center}
+\includegraphics[scale=1,width=0.95\linewidth]{figs/test_id31_micro_station_cables.jpg}
+\end{center}
+\subcaption{\label{fig:test_id31_micro_station_cables}Micro-station and nano-hexapod cables}
+\end{subfigure}
+\begin{subfigure}{0.49\textwidth}
+\begin{center}
+\includegraphics[scale=1,width=0.95\linewidth]{figs/test_id31_fixed_nano_hexapod.jpg}
+\end{center}
+\subcaption{\label{fig:test_id31_fixed_nano_hexapod}Nano-hexapod fixed on top of the micro-station}
+\end{subfigure}
+\caption{\label{fig:test_id31_micro_station_nano_hexapod}Picture of the micro-station without the nano-hexapod (\subref{fig:test_id31_micro_station_cables}) and with the nano-hexapod (\subref{fig:test_id31_fixed_nano_hexapod})}
+\end{figure}
+
 \chapter{Short Stroke Metrology System}
-\label{sec:id31_short_stroke_metrology}
+\label{sec:test_id31_metrology}
 The control of the nano-hexapod requires an external metrology system measuring the relative position of the nano-hexapod top platform with respect to the granite.
-As the long-stroke (\(\approx 1\,cm^3\)) metrology system is not developed yet, a stroke stroke (\(> 100\,\mu m^3\)) can be used instead to validate the nano-hexapod control.
+As the long-stroke (\(\approx 1 \,cm^3\)) metrology system was not developed yet, a stroke stroke (\(> 100\,\mu m^3\)) was used instead to validate the nano-hexapod control.
 
-This short stroke metrology system consists of 5 interferometers pointing at 2 spheres fixed on top of the nano-hexapod (Figure \ref{fig:LION_picture_close}).
+A first considered option was to use the ``Spindle error analyzer'' shown in Figure \ref{fig:test_id31_lion}.
+This system comprises 5 capacitive sensors which are facing two reference spheres.
+As the gap between the capacitive sensors and the spheres is very small\footnote{Depending on the measuring range, gap can range from \(\approx 1\,\mu m\) to \(\approx 100\,\mu m\)}, the risk of damaging the spheres and the capacitive sensors is high.
 
 \begin{figure}[htbp]
-\centering
-\includegraphics[scale=1,width=0.5\linewidth]{figs/LION_picture_close.jpg}
-\caption{\label{fig:LION_picture_close}Metrology system with LION spheres (1 inch diameter) and 5 interferometers fixed to their individual tip-tilts}
+\begin{subfigure}{0.33\textwidth}
+\begin{center}
+\includegraphics[scale=1,width=0.9\linewidth]{figs/test_id31_lion.jpg}
+\end{center}
+\subcaption{\label{fig:test_id31_lion}Capacitive Sensors}
+\end{subfigure}
+\begin{subfigure}{0.33\textwidth}
+\begin{center}
+\includegraphics[scale=1,width=0.9\linewidth]{figs/test_id31_interf.jpg}
+\end{center}
+\subcaption{\label{fig:test_id31_interf}Short-Stroke metrology}
+\end{subfigure}
+\begin{subfigure}{0.33\textwidth}
+\begin{center}
+\includegraphics[scale=1,width=0.9\linewidth]{figs/test_id31_interf_head.jpg}
+\end{center}
+\subcaption{\label{fig:test_id31_interf_head}Interferometer head}
+\end{subfigure}
+\caption{\label{fig:test_id31_short_stroke_metrology}Short stroke metrology system used to measure the sample position with respect to the granite in 5DoF. The system is based on a ``Spindle error analyzer'' (\subref{fig:test_id31_lion}), but the capacitive sensors are replaced with fibered interferometers (\subref{fig:test_id31_interf}). Interferometer heads are shown in (\subref{fig:test_id31_interf_head})}
 \end{figure}
 
-This short stroke metrology system is fixed to the main granite using a gantry made of granite blocs to have good vibration and thermal stability (see Figure \ref{fig:short_stroke_metrology_overview}).
+Instead of using capacitive sensors, 5 fibered interferometers were used in a similar way (Figure \ref{fig:test_id31_interf}).
+At the end of each fiber, a sensor head\footnote{M12/F40 model from Attocube} (Figure \ref{fig:test_id31_interf_head}) is used, which consists of a lens precisely positioned with respect to the fiber's end.
+The lens is focusing the light on the surface of the sphere, such that it comes back to the fiber and produces an interference.
+This way, the gap between the sensor and the reference sphere is much larger (here around \(40\,mm\)), removing the risk of collision.
 
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1,width=\linewidth]{figs/short_stroke_metrology_overview.jpg}
-\caption{\label{fig:short_stroke_metrology_overview}Granite gantry used to fix the short-stroke metrology system}
-\end{figure}
+Nevertheless, the metrology system still has limited measurement range, as when the spheres are moving perpendicularly to the beam axis, the reflected light does not coincide with the incident light, and for some perpendicular displacement, the interference is too small to be detected.
+\section{Metrology Kinematics}
+\label{ssec:test_id31_metrology_kinematics}
 
-As the metrology system as limited stroke (estimated to be in the order of hundreds of micro-meters in x-y-z), it has to be well aligned in the rest position.
+The developed short-stroke metrology system is schematically shown in Figure \ref{fig:test_id31_metrology_kinematics}.
+The point of interest is indicated by the blue frame \(\{B\}\), which is located \(H = 150\,mm\) above the nano-hexapod's top platform.
+The spheres have a diameter \(d = 25.4\,mm\), and indicated dimensions are \(l_1 = 60\,mm\) and \(l_2 = 16.2\,mm\).
+In order to compute the pose of the \(\{B\}\) frame with respect to the granite (i.e. with respect to the fixed interferometer heads), the measured small displacements \([d_1,\ d_2,\ d_3,\ d_4,\ d_5]\) by the interferometers are first written as a function of the small linear and angular motion of the \(\{B\}\) frame \([D_x,\ D_y,\ D_z,\ R_x,\ R_y]\) \eqref{eq:test_id31_metrology_kinematics}.
 
-The alignment procedure is as follows:
-\begin{enumerate}
-\item The granite is aligned to be perpendicular to gravity (using inclinometer and adjusting airlocks)
-\item The height of micro-hexapod is tuned to be able to position the short stroke metrology without additional shim
-\item It is verified that the spindle axis is well perpendicular to the granite using the laser tracker
-\item The micro hexapod is then used to align the two spheres with the spindle axis.
-\end{enumerate}
-\section{Kinematics}
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1,width=0.5\linewidth]{figs/LION_metrology_interferometers.png}
-\caption{\label{fig:LION_metrology_interferometers}Schematic of the measurement system}
-\end{figure}
-
-We have the following set of equations:
-\begin{align}
-d_1 &= +D_y - l_2 R_x \\
-d_2 &= +D_y + l_1 R_x \\
-d_3 &= -D_x - l_2 R_y \\
-d_4 &= -D_x + l_1 R_y \\
-d_5 &= -D_z
-\end{align}
-
-That can be written as a linear transformation:
-\begin{equation}
-\begin{bmatrix}
-  d_1 \\ d_2 \\ d_3 \\ d_4 \\ d_5
-\end{bmatrix} = \begin{bmatrix}
-   0 &  1 &  0 & -l_2 &  0   \\
-   0 &  1 &  0 &  l_1 &  0   \\
-  -1 &  0 &  0 &  0   & -l_2 \\
-  -1 &  0 &  0 &  0   &  l_1 \\
-   0 &  0 & -1 &  0   &  0
-\end{bmatrix} \cdot \begin{bmatrix}
-  D_x \\ D_y \\ D_z \\ R_x \\ R_y
-\end{bmatrix}
+\begin{equation}\label{eq:test_id31_metrology_kinematics}
+d_1 = D_y - l_2 R_x, \quad d_2 = D_y + l_1 R_x, \quad d_3 = -D_x - l_2 R_y, \quad d_4 = -D_x + l_1 R_y, \quad d_5 = -D_z
 \end{equation}
 
-By inverting the matrix, we obtain the Jacobian relation:
-\begin{equation}
+\begin{minipage}[b]{0.48\linewidth}
+\begin{center}
+\includegraphics[scale=1,scale=1]{figs/test_id31_metrology_kinematics.png}
+\captionof{figure}{\label{fig:test_id31_metrology_kinematics}Schematic of the measurement system. Measured distances are indicated by red arrows.}
+\end{center}
+\end{minipage}
+\hfill
+\begin{minipage}[b]{0.48\linewidth}
+\begin{center}
+\includegraphics[scale=1,width=\linewidth]{figs/test_id31_align_top_sphere_comparators.jpg}
+\captionof{figure}{\label{fig:align_top_sphere_comparators}The top sphere is aligned with the rotation axis of the spindle using two probes.}
+\end{center}
+\end{minipage}
+
+The five equations \eqref{eq:test_id31_metrology_kinematics} can be written in a matrix form, and then inverted to have the pose of \(\{B\}\) frame as a linear combination of the measured five distances by the interferometers \eqref{eq:test_id31_metrology_kinematics_inverse}.
+
+\begin{equation}\label{eq:test_id31_metrology_kinematics_inverse}
 \begin{bmatrix}
-  D_x \\ D_y \\ D_z \\ R_x \\ R_y
+   D_x \\ D_y \\ D_z \\ R_x \\ R_y
 \end{bmatrix} = \begin{bmatrix}
    0 &  1 &  0 & -l_2 &  0   \\
    0 &  1 &  0 &  l_1 &  0   \\
@@ -99,961 +132,132 @@ By inverting the matrix, we obtain the Jacobian relation:
 \end{bmatrix}
 \end{equation}
 
-\begin{table}[htbp]
-\caption{\label{tab:jacobian_metrology}Jacobian matrix for the metrology system}
-\centering
-\begin{tabularx}{\linewidth}{cXXXXX}
-\toprule
- & \textbf{d1} & \textbf{d2} & \textbf{d3} & \textbf{d4} & \textbf{d5}\\
-\midrule
-\textbf{Dx} & 0.0 & 0.0 & -0.79 & -0.21 & 0.0\\
-\textbf{Dy} & 0.79 & 0.21 & 0.0 & 0.0 & 0.0\\
-\textbf{Dz} & 0.0 & 0.0 & 0.0 & 0.0 & -1.0\\
-\textbf{Rx} & -13.12 & 13.12 & -0.0 & 0.0 & 0.0\\
-\textbf{Ry} & -0.0 & -0.0 & -13.12 & 13.12 & 0.0\\
-\bottomrule
-\end{tabularx}
-\end{table}
-\section{Rough alignment of spheres using comparators}
-Bottom Sphere, then top sphere.
+\section{Rough alignment of the reference spheres}
+\label{ssec:test_id31_metrology_sphere_rought_alignment}
 
-Alignment better than 10um.
-But the coaxiality between the cylinder and the sphere might not be good.
+The two reference spheres are aligned with the rotation axis of the spindle.
+To do so, two measuring probes are used as shown in Figure \ref{fig:align_top_sphere_comparators}.
+
+To not damage the sensitive sphere surface, the probes are instead positioned on the cylinder on which the sphere is mounted.
+First, the probes are fixed to the bottom (fixed) cylinder to align its axis with the spindle axis.
+Then, the probes are fixed to the top (adjustable) cylinder, and the same alignment is performed.
+
+With this setup, the precision of the alignment of both sphere better with the spindle axis is expected to limited to \(\approx 10\,\mu m\).
+This is probably limited due to the poor coaxiality between the cylinders and the spheres.
+However, the alignment precision should be enough to stay in the acceptance of the interferometers.
+
+\section{Tip-Tilt adjustment of the interferometers}
+\label{ssec:test_id31_metrology_alignment}
+
+The short stroke metrology system is placed on top of the main granite using a gantry made of granite blocs to have good vibration and thermal stability (Figure \ref{fig:short_stroke_metrology_overview}).
 
 \begin{figure}[htbp]
 \centering
-\includegraphics[scale=1,width=\linewidth]{figs/align_top_sphere_comparators.jpg}
-\caption{\label{fig:align_top_sphere_comparators}Two mechanical comparators used to align the top sphere with the rotation axis of the spindle}
+\includegraphics[scale=1,width=0.8\linewidth]{figs/test_id31_short_stroke_metrology_overview.jpg}
+\caption{\label{fig:short_stroke_metrology_overview}Granite gantry used to fix the short-stroke metrology system}
 \end{figure}
-\section{Alignment of spheres using interferometers}
-\subsection{Angular alignment}
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_metrology_align_rx_ry.png}
-\caption{\label{fig:id31_metrology_align_rx_ry}Rx/Ry alignment of the spheres using the micro-station}
-\end{figure}
-\subsection{Eccentricity alignment}
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_metrology_align_dx_dy.png}
-\caption{\label{fig:id31_metrology_align_dx_dy}Dx/Dy alignment of the spheres using the micro-station}
-\end{figure}
-\section{Residual error after alignment}
-\begin{itemize}
-\item Dx and Dy are less than 1um.
-\item Dz less than 0.1um.
-\item Rx and Ry less than 4urad.
-\end{itemize}
+
+The interferometers need to be aligned with respect to the two reference spheres to approach as much as possible the ideal case shown in Figure \ref{fig:test_id31_metrology_kinematics}.
+The vertical position of the spheres is adjusted using the micro-hexapod to match the height of the interferometers.
+Then, the horizontal position of the gantry is adjusted such that the coupling efficiency (i.e. the intensity of the light reflected back in the fiber) of the top interferometer is maximized.
+This is equivalent as to optimize the perpendicularity between the interferometer beam and the sphere surface (i.e. the concentricity between the beam and the sphere center).
+
+The lateral sensor heads (i.e. all except the top one), which are each fixed to a custom tip-tilt adjustment mechanism, are individually oriented such that the coupling efficient is maximized.
+
+\section{Fine Alignment of reference spheres using interferometers}
+\label{ssec:test_id31_metrology_sphere_fine_alignment}
+
+Thanks to the good alignment of the two reference spheres with the spindle axis and to the fine adjustment of the interferometers orientations, the interferometer measurement is made possible during complete spindle rotation.
+This metrology and therefore be used to better align the axis defined by the two spheres' center with the spindle axis.
+
+The alignment process is made by few iterations.
+First, the spindle is scanned and the alignment errors are recorded.
+From the errors, the motion of the micro-hexapod to better align the spheres is determined and the micro-hexapod is moved.
+Then, the spindle is scanned again, and the new alignment errors are recorded.
+
+This iterative process is first perform for angular errors (Figure \ref{fig:test_id31_metrology_align_rx_ry}) and then for lateral errors (Figure \ref{fig:test_id31_metrology_align_dx_dy}).
+Remaining error after alignment is in the order of \(\pm5\,\mu\text{rad}\) for angular errors, \(\pm 1\,\mu m\) laterally and less than \(0.1\,\mu m\) vertically.
 
 \begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_align_metorlogy_errors_after_align.png}
-\caption{\label{fig:id31_align_metorlogy_errors_after_align}Remaining errors after aligning the metrology using the interferometers}
+\begin{subfigure}{0.49\textwidth}
+\begin{center}
+\includegraphics[scale=1,scale=1]{figs/test_id31_metrology_align_rx_ry.png}
+\end{center}
+\subcaption{\label{fig:test_id31_metrology_align_rx_ry}Angular alignment}
+\end{subfigure}
+\begin{subfigure}{0.49\textwidth}
+\begin{center}
+\includegraphics[scale=1,scale=1]{figs/test_id31_metrology_align_dx_dy.png}
+\end{center}
+\subcaption{\label{fig:test_id31_metrology_align_dx_dy}Lateral alignment}
+\end{subfigure}
+\caption{\label{fig:test_id31_metrology_align}Measured angular (\subref{fig:test_id31_metrology_align_rx_ry}) and lateral (\subref{fig:test_id31_metrology_align_dx_dy}) errors during a full spindle rotation. Between two rotations, the micro-hexapod is adjusted to better align the two spheres with the rotation axis.}
 \end{figure}
-\section{Metrology acceptance}
+
+
+\section{Estimated measurement volume}
+\label{ssec:test_id31_metrology_acceptance}
+
 Because the interferometers are pointing to spheres and not flat surfaces, the lateral acceptance is limited.
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/metrology_alignment_xy_map.png}
-\caption{\label{fig:metrology_alignment_xy_map}XY mapping of the Z measurement by the interferometer}
-\end{figure}
-\chapter{Simscape Model}
-\label{sec:id31_simscape_model}
-\section{Init model}
-\section{Identify Transfer functions}
-\section{IFF Plant}
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_simscape_iff_ol_plant.png}
-\caption{\label{fig:id31_simscape_iff_ol_plant}IFF transfer function - Simscape model}
-\end{figure}
-\section{Encoder plant}
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_simscape_enc_ol_plant.png}
-\caption{\label{fig:id31_simscape_enc_ol_plant}ENC transfer function - Simscape model}
-\end{figure}
-\section{HAC Undamped plant}
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_simscape_int_ol_plant.png}
-\caption{\label{fig:id31_simscape_int_ol_plant}INT transfer function - Simscape model}
-\end{figure}
-\chapter{Identified Open Loop Plant}
-\label{sec:id31_open_loop_plant}
-\section{IFF Plant}
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_iff_ol_plant_m0.png}
-\caption{\label{fig:id31_iff_ol_plant_m0}Measured transfer function from generated voltages to measured voltage on the force sensors}
-\end{figure}
-
-The measured frequency response functions from DAC voltages \(u_i\) to measured voltages on the force sensors \(\tau_{m,i}\) are compared with the simscape model in Figure \ref{fig:id31_comp_simscape_frf_ol_iff}.
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_comp_simscape_frf_ol_iff.png}
-\caption{\label{fig:id31_comp_simscape_frf_ol_iff}Comparison of the Simscape model and identified IFF plant}
-\end{figure}
-
-The effect of the payload mass on the diagonal elements are shown in Figure \ref{fig:id31_effect_mass_frf_ol_iff}.
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_effect_mass_frf_ol_iff.png}
-\caption{\label{fig:id31_effect_mass_frf_ol_iff}Effect of the payload mass on the IFF plant}
-\end{figure}
-\section{Encoder plant}
-The identified frequency response functions from general voltages \(u_i\) to measured displacement of the struts by the encoders \(d\mathcal{L}_i\) are compared with the simscape model in Figure \ref{fig:id31_comp_simscape_frf_ol_enc}.
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_comp_simscape_frf_ol_enc.png}
-\caption{\label{fig:id31_comp_simscape_frf_ol_enc}Comparison of the Simscape model and identified plant - Transfer functions from voltages to encoders}
-\end{figure}
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_effect_mass_frf_ol_enc.png}
-\caption{\label{fig:id31_effect_mass_frf_ol_enc}Effect of the payload mass on the transfer function from actuator voltage to encoder displacement}
-\end{figure}
-\section{HAC Undamped plant}
-The identified frequency response functions from actuator voltages \(u_i\) to measured strut motion from the external metrology (i.e. the interferometers) are compare with the simscape model in Figure \ref{fig:id31_comp_simscape_frf_ol_int}.
-
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_int_ol_plant_m0.png}
-\caption{\label{fig:id31_int_ol_plant_m0}Measured transfer function from generated voltages to measured voltage on the force sensors}
-\end{figure}
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_comp_simscape_frf_ol_int.png}
-\caption{\label{fig:id31_comp_simscape_frf_ol_int}Comparison of the Simscape model and identified plant - Transfer functions from voltages to estimated strut motion from interferometers}
-\end{figure}
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_effect_mass_frf_ol_int.png}
-\caption{\label{fig:id31_effect_mass_frf_ol_int}Effect of the payload mass on the transfer functions from actuator voltage to measured strut motion by the external metrology}
-\end{figure}
-\section{Decoupling improvement thanks to better Rz alignment}
-\subsection{Alignment procedure}
-
-\begin{itemize}
-\item Control based on encoders
-\item Slow moving in X and Y
-\item Compare with X and Y from interf
-\end{itemize}
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_Rz_align_dx_dy_scans_before_calib.png}
-\caption{\label{fig:id31_Rz_align_dx_dy_scans_before_calib}description}
-\end{figure}
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_Rz_align_dx_dy_scans.png}
-\caption{\label{fig:id31_Rz_align_dx_dy_scans}description}
-\end{figure}
-\subsection{m0}
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_coupling_decrease_rz_align.png}
-\caption{\label{fig:id31_coupling_decrease_rz_align}Decrease of the coupling with better Rz alignment}
-\end{figure}
-\subsection{m3}
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_coupling_decrease_rz_align_m3.png}
-\caption{\label{fig:id31_coupling_decrease_rz_align_m3}Decrease of the coupling with better Rz alignment}
-\end{figure}
-\section{Conclusion}
-
-\begin{itemize}
-\item Good match between the model and experiment
-\end{itemize}
-\chapter{Noise Budget}
-\label{sec:id31_noise_budget}
-In this section, the noise budget is performed.
-The vibrations of the sample is measured in different conditions using the external metrology.
-\section{Open-Loop Noise Budget}
-First, the noise is measured while no motion is performed.
-
-Noise budget in the cartesian frame
-Data in the time domain
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_noise_budget_interf_time_domain_m0.png}
-\caption{\label{fig:id31_noise_budget_interf_time_domain_m0}Measured vibration with the interferometers}
-\end{figure}
-
-In the frequency domain
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_noise_budget_interf_freq_domain_m0.png}
-\caption{\label{fig:id31_noise_budget_interf_freq_domain_m0}Measured vibration with the interferometers}
-\end{figure}
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_noise_budget_open_loop_cas_m0.png}
-\caption{\label{fig:id31_noise_budget_open_loop_cas_m0}Measured vibration with the interferometers}
-\end{figure}
-\section{Effect of LAC}
-Effect of LAC (Figure \ref{fig:id31_noise_budget_effect_lac_m0}):
-\begin{itemize}
-\item reduce amplitude around 80Hz
-\item Inject some noise between 200 and 700Hz?
-\end{itemize}
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_noise_budget_effect_lac_m0.png}
-\caption{\label{fig:id31_noise_budget_effect_lac_m0}Measured vibration with the interferometers}
-\end{figure}
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_cas_effect_lac_m0.png}
-\caption{\label{fig:id31_cas_effect_lac_m0}Measured vibration with the interferometers}
-\end{figure}
-\section{Effect of rotation}
-Rotation induces lots of vibrations, especially at high velocity.
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_noise_budget_effect_rotation.png}
-\caption{\label{fig:id31_noise_budget_effect_rotation}Cumulative Amplitude Spectrum for the three important directions (\(D_y\), \(D_z\) and \(R_y\)). Three rotating velocities are shown. Integrated RMS values are shown in the legend.}
-\end{figure}
-\section{Effect of HAC}
-Bandwidth is approximately 10Hz.
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_noise_budget_effect_hac_m0.png}
-\caption{\label{fig:id31_noise_budget_effect_hac_m0}Measured vibration with the interferometers}
-\end{figure}
-
-
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_cas_effect_hac_m0.png}
-\caption{\label{fig:id31_cas_effect_hac_m0}Measured vibration with the interferometers}
-\end{figure}
-\section{Noise coming from force sensor}
-\chapter{Integral Force Feedback}
-\label{sec:id31_iff}
-\section{IFF Plants}
-\subsection{6x6 Plant}
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_Giff_plant.png}
-\caption{\label{fig:id31_Giff_plant}Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor}
-\end{figure}
-
-Compare with Model:
-\subsection{Effect of Rotation}
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_Giff_effect_rotation.png}
-\caption{\label{fig:id31_Giff_effect_rotation}Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor}
-\end{figure}
-\subsection{Effect of Mass}
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_Giff_effect_mass.png}
-\caption{\label{fig:id31_Giff_effect_mass}Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor}
-\end{figure}
-\subsection{Compare with the model}
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_Giff_plant_comp_model.png}
-\caption{\label{fig:id31_Giff_plant_comp_model}Comparison of the identified IFF plant and the IFF plant extracted from the simscape model}
-\end{figure}
-\section{IFF Controller}
-\subsection{Controller Design}
-Test second order high pass filter:
-We want integral action between 20Hz and 200Hz.
-Loop Gain:
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_iff_loop_gain_diagonal_terms.png}
-\caption{\label{fig:id31_iff_loop_gain_diagonal_terms}IFF Loop gain of the diagonal terms}
-\end{figure}
-
-Root Locus to obtain optimal gain.
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_iff_root_locus.png}
-\caption{\label{fig:id31_iff_root_locus}Root Locus for IFF. Green crosses are closed-loop poles for the same choosen IFF gain.}
-\end{figure}
-\subsection{Verify Stability}
-Verify Stability with Nyquist plot:
-
-\begin{itemize}
-\item Why bad stability margins?
-\end{itemize}
-\subsection{Save Controller}
-\section{Estimated Damped Plant}
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_hac_damped_plant_estimated_simscape.png}
-\caption{\label{fig:id31_hac_damped_plant_estimated_simscape}description}
-\end{figure}
-\chapter{High Authority Control}
-\label{sec:id31_iff_hac}
-\section{Identify Spurious modes}
-\section{HAC Plants}
-\subsection{6x6 Plant}
-Compare with Model:
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_Ghac_6x6_plant_comp_model.png}
-\caption{\label{fig:id31_Ghac_6x6_plant_comp_model}6x6 plant from generated voltages to displacement of the struts as measured by the external metrology}
-\end{figure}
-\subsection{Effect of Mass}
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_Ghac_effect_mass.png}
-\caption{\label{fig:id31_Ghac_effect_mass}Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor}
-\end{figure}
-\subsection{Compare with the model}
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_Ghac_plant_comp_model.png}
-\caption{\label{fig:id31_Ghac_plant_comp_model}Comparison of the identified HAC plant and the HAC plant extracted from the simscape model}
-\end{figure}
-\subsection{Comparison with Undamped plant}
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_comp_undamped_damped_plant_m0.png}
-\caption{\label{fig:id31_comp_undamped_damped_plant_m0}description}
-\end{figure}
-\section{Robust HAC}
-\subsection{Controller design}
-Loop gain
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_hac_robust_loop_gain.png}
-\caption{\label{fig:id31_hac_robust_loop_gain}description}
-\end{figure}
-\subsection{Verify Stability}
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_hac_robust_nyquist.png}
-\caption{\label{fig:id31_hac_robust_nyquist}description}
-\end{figure}
-\subsection{Estimated performances}
-
-\subsection{Save Controller}
-\section{High Performance HAC}
-The goal is to make a controller specific for one mass in order to have high bandwidth.
-\subsection{Mass 0}
-\paragraph{Load Plant}
-\paragraph{Plant}
-\paragraph{Controller design}
-Loop gain
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_high_perf_hac_m0_loop_gain.png}
-\caption{\label{fig:id31_high_perf_hac_m0_loop_gain}Loop gain for the High Authority Control}
-\end{figure}
-\paragraph{Verify Stability}
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_high_perf_hac_m0_nyquist.png}
-\caption{\label{fig:id31_high_perf_hac_m0_nyquist}Nyquist plot for the High Authority Control}
-\end{figure}
-\paragraph{Estimated performances}
-Loop gain with model
-\paragraph{Save Controller}
-\paragraph{Experimental Validation}
-
-\begin{center}
-\begin{tabular}{lrrr}
- & Dy [nm] & Dz [nm] & Ry [urad]\\
-\hline
-1rpm & 55.3 & 5.9 & 0.1\\
-30rpm & 85.2 & 12.5 & 0.3\\
-\end{tabular}
-\end{center}
-\paragraph{Closed-Loop identification}
-\subsection{Mass 1}
-\paragraph{Load Plant}
-\paragraph{Plant}
-\paragraph{Plant Inverse}
-\paragraph{Controller design}
-Loop gain
-\paragraph{Verify Stability}
-\paragraph{Estimated performances}
-Loop gain with model
-\paragraph{Save Controller}
-\section{Tomography - Performances}
-\subsection{First scan with closed-loop at middle}
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_tomography_ol_cl_robust_hac_m0.png}
-\caption{\label{fig:id31_tomography_ol_cl_robust_hac_m0}description}
-\end{figure}
-\subsection{Slow Rotation - 6RPM}
-\subsection{Rapid Rotation - 30RPM}
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_cas_m0_30rpm_ol_lac_hac.png}
-\caption{\label{fig:id31_cas_m0_30rpm_ol_lac_hac}Cumulative Amplitude Spectrum of the errors in \(D_y\), \(D_z\) and \(R_y\) during a tomography scan at 30RPM. Three control configuration are compared: Open-Loop, Low Authority Control, and High Authority Control}
-\end{figure}
-\chapter{6DoF Control in Cartesian plane (rotating with the nano-hexapod)}
-\label{sec:id31_cart_hac_rot}
-As only Dy, Dz and Ry directions are important, we could only control them.
-This lead to a 3x3 plant that may be more decoupled than the 6x6 plant.
-\section{5x5 plant in Cartesian plane}
-Compute identified plant in the Cartesian plane:
-Compute plant model in the Cartesian plane:
-\section{Controller Design}
-\section{Check Stability}
-\section{Save controllers}
-\section{Performances}
-2023-08-18\_18-33\_m0\_1rpm\_K\_cart.mat
-\chapter{3DoF Control in Cartesian plane (fixed)}
-\label{sec:id31_cart_hac_fix}
-As only Dy, Dz and Ry directions are important, we could only control them.
-This lead to a 3x3 plant that may be more decoupled than the 6x6 plant.
-\section{3x3 plant in Cartesian plane}
-Compute identified plant in the Cartesian plane:
-Compute plant model in the Cartesian plane:
-\begin{important}
-Diagonal elements are matching quite well, but off-diagonal elements are very different.
-
-Why so much more coupling than from the model?
-\begin{itemize}
-\item Is it due to the metrology? The spheres could induce coupling as for instance X motion will also be seen as Z motion.
-This is especially true if not well centered with the sphere (as seemed to be the case for the lateral interferometers).
-\end{itemize}
-\end{important}
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_cart_plant_3x3.png}
-\caption{\label{fig:id31_cart_plant_3x3}3x3 cartesian plant}
-\end{figure}
-
-
-Normalization of outputs:
-\section{Controller Design}
-\subsection{Dy}
-\subsection{Dz}
-\subsection{Ry}
-\subsection{3x3 controller}
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/G_cart_loop_gain_diagonal_3dof.png}
-\caption{\label{fig:G_cart_loop_gain_diagonal_3dof}description}
-\end{figure}
-\section{Check Stability}
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/G_cart_nyquist_diagonal_3dof.png}
-\caption{\label{fig:G_cart_nyquist_diagonal_3dof}description}
-\end{figure}
-\section{Save controllers}
-\subsection{Save Controller}
-\section{Controller Design (normalized)}
-\section{Verify Stability}
-\section{Control Performances}
-\begin{itemize}
-\item[{$\square$}] Compare with estimated performances
-\end{itemize}
-\chapter{Complementary Filter Control}
-\label{sec:id31_cart_hac_complementary_filter}
-\section{m0}
-\subsection{3x3 plant in Cartesian plane}
-Compute identified plant in the Cartesian plane:
-Compute plant model in the Cartesian plane:
-\subsection{Plant Invert}
-Reduce model size
-Add first resonance
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_comp_cart_plant_reduced_model.png}
-\caption{\label{fig:id31_comp_cart_plant_reduced_model}Comparaison of the measured direct terms and the reduced order models}
-\end{figure}
-
-
-Invert and make realizable
-\subsection{Save Plant Inverse}
-\subsection{Control Performances}
-\paragraph{5Hz}
-\begin{itemize}
-\item[{$\square$}] Compare with estimated performances
-\end{itemize}
-\paragraph{20Hz}
-\begin{itemize}
-\item[{$\square$}] Compare with estimated performances
-\end{itemize}
-\paragraph{Different bandwidth for different directions}
-\begin{itemize}
-\item[{$\square$}] Compare with estimated performances
-\end{itemize}
-\paragraph{Dz 25Hz}
-\begin{itemize}
-\item[{$\square$}] Compare with estimated performances
-\end{itemize}
-\subsection{Better plant invert}
-\paragraph{Dy}
-Stable Inverse
-\paragraph{Dz}
-Stable Inverse
-\paragraph{Ry}
-Stable Inverse
-\paragraph{Compare Invert plants}
-
-\paragraph{Save plant inverse}
-\paragraph{Compare Digital Invert plants}
-\subsection{Control Performances}
-\paragraph{Better plant invert}
-\subsection{Scans with good controller}
-\paragraph{1rpm}
-1RPM scans are performed for all the masses with the same controller.
-
-\begin{center}
-\begin{tabular}{lrrrrr}
- & Dx [nm] & Dy [nm] & Dz [nm] & Rx [nrad] & Ry [nrad]\\
-\hline
-m0 & 796 & 20 & 8 & 8209 & 73\\
-\end{tabular}
-\end{center}
-\paragraph{30rpm}
-1RPM scans are performed for all the masses with the same controller.
-
-\begin{center}
-\begin{tabular}{lrrrrr}
- & Dx [nm] & Dy [nm] & Dz [nm] & Rx [nrad] & Ry [nrad]\\
-\hline
-m0 & 820 & 39 & 13 & 7790 & 156\\
-\end{tabular}
-\end{center}
-\section{m1}
-\subsection{3x3 plant in Cartesian plane}
-Compute identified plant in the Cartesian plane:
-Compute plant model in the Cartesian plane:
-Normalization of outputs:
-\subsection{Better plant invert}
-\paragraph{Dy}
-Stable Inverse
-\paragraph{Dz}
-Stable Inverse
-\paragraph{Ry}
-Stable Inverse
-\paragraph{Compare Invert plants}
-\paragraph{Save plant inverse}
-\paragraph{Compare Digital Invert plants}
-\subsection{Control Performances}
-\paragraph{Better plant invert}
-\subsection{Scans with good controller}
-\paragraph{1rpm}
-1RPM scans are performed for all the masses with the same controller.
-
-\begin{center}
-\begin{tabular}{lrrrrr}
- & Dx [nm] & Dy [nm] & Dz [nm] & Rx [nrad] & Ry [nrad]\\
-\hline
-m0 & 796 & 20 & 8 & 8209 & 73\\
-\end{tabular}
-\end{center}
-\paragraph{30rpm}
-1RPM scans are performed for all the masses with the same controller.
-
-\begin{center}
-\begin{tabular}{lrrrrr}
- & Dx [nm] & Dy [nm] & Dz [nm] & Rx [nrad] & Ry [nrad]\\
-\hline
-m0 & 820 & 39 & 13 & 7790 & 156\\
-\end{tabular}
-\end{center}
-\section{m2}
-\subsection{3x3 plant in Cartesian plane}
-Compute identified plant in the Cartesian plane:
-Compute plant model in the Cartesian plane:
-Normalization of outputs:
-\subsection{Better plant invert}
-\paragraph{Dy}
-Stable Inverse
-\paragraph{Dz}
-Stable Inverse
-\paragraph{Ry}
-Stable Inverse
-\paragraph{Compare Invert plants}
-\paragraph{Save plant inverse}
-\paragraph{Compare Digital Invert plants}
-\subsection{Control Performances}
-\paragraph{Better plant invert}
-\subsection{Scans with good controller}
-\paragraph{1rpm}
-1RPM scans are performed for all the masses with the same controller.
-
-\begin{center}
-\begin{tabular}{lrrrrr}
- & Dx [nm] & Dy [nm] & Dz [nm] & Rx [nrad] & Ry [nrad]\\
-\hline
-m0 & 796 & 20 & 8 & 8209 & 73\\
-\end{tabular}
-\end{center}
-\paragraph{30rpm}
-1RPM scans are performed for all the masses with the same controller.
-
-\begin{center}
-\begin{tabular}{lrrrrr}
- & Dx [nm] & Dy [nm] & Dz [nm] & Rx [nrad] & Ry [nrad]\\
-\hline
-m0 & 820 & 39 & 13 & 7790 & 156\\
-\end{tabular}
-\end{center}
-\section{m3}
-\subsection{3x3 plant in Cartesian plane}
-Compute identified plant in the Cartesian plane:
-Compute plant model in the Cartesian plane:
-Normalization of outputs:
-\subsection{Better plant invert}
-\paragraph{Dy}
-Stable Inverse
-\paragraph{Dz}
-Stable Inverse
-\paragraph{Ry}
-Stable Inverse
-\paragraph{Compare Invert plants}
-\paragraph{Save plant inverse}
-\paragraph{Compare Digital Invert plants}
-\subsection{Control Performances}
-\paragraph{Better plant invert}
-\subsection{Scans with good controller}
-\paragraph{1rpm}
-1RPM scans are performed for all the masses with the same controller.
-
-\begin{center}
-\begin{tabular}{lrrrrr}
- & Dx [nm] & Dy [nm] & Dz [nm] & Rx [nrad] & Ry [nrad]\\
-\hline
-m0 & 796 & 20 & 8 & 8209 & 73\\
-\end{tabular}
-\end{center}
-\paragraph{30rpm}
-1RPM scans are performed for all the masses with the same controller.
-
-\begin{center}
-\begin{tabular}{lrrrrr}
- & Dx [nm] & Dy [nm] & Dz [nm] & Rx [nrad] & Ry [nrad]\\
-\hline
-m0 & 820 & 39 & 13 & 7790 & 156\\
-\end{tabular}
-\end{center}
-\chapter{Scans}
-\label{sec:id31_scans}
-\begin{itemize}
-\item Section \ref{sec:id31_scans_tomography}
-\item Section \ref{sec:id31_scans_dz}
-\item Section \ref{sec:id31_scans_reflectivity}
-\item Section \ref{sec:id31_scans_dy}
-\item Section \ref{sec:id31_scans_diffraction_tomo}
-\end{itemize}
-\section{\(R_z\) scans: Tomography}
-\label{sec:id31_scans_tomography}
-m0: 30rpm, 6rpm, 1rpm
-m1: 6rpm, 1rpm
-m2: 6rpm, 1rpm
-m3: 1rpm
-\subsection{Robust Control - 1rpm}
-1RPM scans are performed for all the masses with the same robust controller.
-
-The problem for these scans is that the position initialization was not make properly, so the open-loop errors are quite large (see Figure \ref{fig:id31_tomo_1rpm_robust_m0}).
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_tomo_1rpm_robust_m0.png}
-\caption{\label{fig:id31_tomo_1rpm_robust_m0}\(D_x\), \(D_y\) and \(D_z\) motion during a slow (1RPM) tomography experiment. Open Loop data is shown in blue and closed-loop data in red}
-\end{figure}
-
-The obtained open-loop and closed-loop errors are shown in tables \ref{tab:id31_tomo_1rpm_robust_ol_errors} and \ref{tab:id31_tomo_1rpm_robust_cl_errors} respectively.
+In order to estimate the metrology acceptance, the micro-hexapod is used to perform three accurate scans of \(\pm 1\,mm\), respectively along the the \(x\), \(y\) and \(z\) axes.
+During these scans, the 5 interferometers are recorded, and the ranges in which each interferometer has enough coupling efficiency for measurement are estimated.
+Results are summarized in Table \ref{tab:test_id31_metrology_acceptance}.
+The obtained lateral acceptance for pure displacements in any direction is estimated to be around \(+/-0.5\,mm\), which is enough for the current application as it is well above the micro-station errors to be actively corrected.
 
 \begin{table}[htbp]
-\caption{\label{tab:id31_tomo_1rpm_robust_ol_errors}Measured error during open-loop tomography scans (1rpm)}
+\caption{\label{tab:test_id31_metrology_acceptance}Estimated measurement range for each interferometer, and for three different directions.}
 \centering
-\begin{tabularx}{\linewidth}{cXXXXX}
+\begin{tabularx}{0.45\linewidth}{Xccc}
 \toprule
- & \(D_x\) {[}\(\mu m\)] & \(D_y\) {[}\(\mu m\)] & \(D_z\) {[}\(nm\)] & \(R_x\) {[}\(\mu\text{rad}\)] & \(R_y\) {[}\(\mu\text{rad}\)]\\
+ & \(D_x\) & \(D_y\) & \(D_z\)\\
 \midrule
-\(m_0\) & 6 & 6 & 32 & 34 & 34\\
-\(m_1\) & 6 & 7 & 26 & 51 & 55\\
-\(m_2\) & 36 & 38 & 36 & 259 & 253\\
-\(m_3\) & 31 & 33 & 38 & 214 & 203\\
-\bottomrule
-\end{tabularx}
-\end{table}
-
-\begin{table}[htbp]
-\caption{\label{tab:id31_tomo_1rpm_robust_cl_errors}Measured error during closed-loop tomography scans (1rpm, robust controller)}
-\centering
-\begin{tabularx}{\linewidth}{cXXXXX}
-\toprule
- & \(D_x\) {[}nm] & \(D_y\) {[}nm] & \(D_z\) {[}nm] & \(R_x\) {[}nrad] & \(R_y\) {[}nrad]\\
-\midrule
-\(m_0\) & 13 & 15 & 5 & 57 & 55\\
-\(m_1\) & 16 & 25 & 6 & 102 & 55\\
-\(m_2\) & 25 & 25 & 7 & 120 & 103\\
-\(m_3\) & 40 & 53 & 9 & 225 & 169\\
-\bottomrule
-\end{tabularx}
-\end{table}
-\subsection{Robust Control - 6rpm}
-\begin{table}[htbp]
-\caption{\label{tab:id31_tomo_6rpm_robust_ol_errors}Measured error during open-loop tomography scans (6rpm)}
-\centering
-\begin{tabularx}{\linewidth}{cXXXXX}
-\toprule
- & \(D_x\) {[}\(\mu m\)] & \(D_y\) {[}\(\mu m\)] & \(D_z\) {[}\(nm\)] & \(R_x\) {[}\(\mu\text{rad}\)] & \(R_y\) {[}\(\mu\text{rad}\)]\\
-\midrule
-\(m_0\) & 8 & 7 & 20 & 41 & 41\\
-\(m_1\) & 4 & 4 & 21 & 39 & 39\\
-\bottomrule
-\end{tabularx}
-\end{table}
-
-\begin{table}[htbp]
-\caption{\label{tab:id31_tomo_6rpm_robust_cl_errors}Measured error during closed-loop tomography scans (6rpm, robust controller)}
-\centering
-\begin{tabularx}{\linewidth}{cXXXXX}
-\toprule
- & \(D_x\) {[}nm] & \(D_y\) {[}nm] & \(D_z\) {[}nm] & \(R_x\) {[}nrad] & \(R_y\) {[}nrad]\\
-\midrule
-\(m_0\) & 17 & 19 & 5 & 70 & 73\\
-\(m_1\) & 20 & 26 & 7 & 110 & 77\\
-\bottomrule
-\end{tabularx}
-\end{table}
-\subsection{Robust Control - 30rpm}
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_tomography_m0_30rpm_robust_xyz.png}
-\caption{\label{fig:id31_tomography_m0_30rpm_robust_xyz}Measured motion during tomography scan at 30RPM with a robust controller}
-\end{figure}
-
-\begin{table}[htbp]
-\caption{\label{tab:id31_tomo_30rpm_robust_ol_errors}Measured error during open-loop tomography scans (30rpm)}
-\centering
-\begin{tabularx}{\linewidth}{cXXXXX}
-\toprule
- & \(D_x\) {[}\(\mu m\)] & \(D_y\) {[}\(\mu m\)] & \(D_z\) {[}\(nm\)] & \(R_x\) {[}\(\mu\text{rad}\)] & \(R_y\) {[}\(\mu\text{rad}\)]\\
-\midrule
-\(m_0\) & 2 & 2 & 24 & 10 & 10\\
-\bottomrule
-\end{tabularx}
-\end{table}
-
-\begin{table}[htbp]
-\caption{\label{tab:id31_tomo_30rpm_robust_cl_errors}Measured error during closed-loop tomography scans (30rpm, robust controller)}
-\centering
-\begin{tabularx}{\linewidth}{cXXXXX}
-\toprule
- & \(D_x\) {[}nm] & \(D_y\) {[}nm] & \(D_z\) {[}nm] & \(R_x\) {[}nrad] & \(R_y\) {[}nrad]\\
-\midrule
-\(m_0\) & 34 & 38 & 10 & 127 & 129\\
-\bottomrule
-\end{tabularx}
-\end{table}
-\section{\(D_z\) scans: Dirty Layer Scans}
-\label{sec:id31_scans_dz}
-\subsection{Step by Step \(D_z\) motion}
-Three step sizes are tested:
-\begin{itemize}
-\item \(10\,nm\) steps (Figure \ref{fig:id31_dz_mim_10nm_steps})
-\item \(100\,nm\) steps (Figure \ref{fig:id31_dz_mim_100nm_steps})
-\item \(1\,\mu m\) steps (Figure \ref{fig:id31_dz_steps_response})
-\end{itemize}
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_dz_mim_10nm_steps.png}
-\caption{\label{fig:id31_dz_mim_10nm_steps}Dz MIM test with 10nm steps (low pass filter with cut-off frequency of 10Hz is applied)}
-\end{figure}
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_dz_mim_100nm_steps.png}
-\caption{\label{fig:id31_dz_mim_100nm_steps}Dz MIM test with 100nm steps}
-\end{figure}
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_dz_steps_response.png}
-\caption{\label{fig:id31_dz_steps_response}\(D_z\) step response - Stabilization time is around 70ms}
-\end{figure}
-\subsection{Continuous \(D_z\) motion: Dirty Layer Scans}
-Two \(D_z\) scans are performed:
-\begin{itemize}
-\item at \(10\,\mu m/s\) in Figure \ref{fig:id31_dirty_layer_scan_m0}
-\item at \(100\,\mu m/s\) in Figure \ref{fig:id31_dirty_layer_scan_m0_large}
-\end{itemize}
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_dirty_layer_scan_m0.png}
-\caption{\label{fig:id31_dirty_layer_scan_m0}Dirty layer scan: \(D_z\) motion at \(10\,\mu m/s\)}
-\end{figure}
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_dirty_layer_scan_m0_large.png}
-\caption{\label{fig:id31_dirty_layer_scan_m0_large}Dirty layer scan: \(D_z\) motion at \(100\,\mu m/s\)}
-\end{figure}
-\section{\(R_y\) scans: Reflectivity}
-\label{sec:id31_scans_reflectivity}
-An \(R_y\) scan is performed at \(100\,\mu rad/s\) velocity (Figure \ref{fig:id31_reflectivity_scan_Ry_100urads}).
-During the \(R_y\) scan, the errors in \(D_y\) are \(D_z\) are kept small.
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_reflectivity_scan_Ry_100urads.png}
-\caption{\label{fig:id31_reflectivity_scan_Ry_100urads}\(R_y\) reflecitivity scan at \(100\,\mu\text{rad}/s\) velocity}
-\end{figure}
-\section{\(D_y\) Scans}
-\label{sec:id31_scans_dy}
-The steps generated by the IcePAP for the \(T_y\) stage are send to the Speedgoat.
-Then, we can know in real time what is the wanted position in \(D_y\) during \(T_y\) scans.
-\subsection{Open Loop}
-We can clearly see micro-stepping errors of the stepper motor used for the \(T_y\) stage.
-The errors have a period of \(10\,\mu m\) with an amplitude of \(\pm 100\,nm\).
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_ty_scan_10ums_ol_dy_errors.png}
-\caption{\label{fig:id31_ty_scan_10ums_ol_dy_errors}\(T_y\) scan (at \(10\,\mu m/s\)) - \(D_y\) errors. The micro-stepping errors can clearly be seen with a period of \(10\,\mu m\) and an amplitude of \(\pm 100\,nm\)}
-\end{figure}
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_ty_scan_10ums_ol_dz_ry_errors.png}
-\caption{\label{fig:id31_ty_scan_10ums_ol_dz_ry_errors}\(T_y\) scan (at \(10\,\mu m/s\)) - \(D_z\) and \(R_y\) errors. The \(D_z\) error is most likely due to having the top interferometer pointing to a sphere. The large \(R_y\) errors might also be due to the metrology system}
-\end{figure}
-\subsection{Closed Loop}
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_ty_scan_10ums_cl_dy_errors.png}
-\caption{\label{fig:id31_ty_scan_10ums_cl_dy_errors}\(T_y\) scan (at \(10\,\mu m/s\)) - \(D_y\) errors. Open-loop and Closed-loop scans}
-\end{figure}
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_ty_scan_10ums_cl_dz_ry_errors.png}
-\caption{\label{fig:id31_ty_scan_10ums_cl_dz_ry_errors}\(T_y\) scan (at \(10\,\mu m/s\)) - \(D_z\) and \(R_y\) errors. Open-loop and Closed-loop scans}
-\end{figure}
-\subsection{Faster Scan}
-Because of micro-stepping errors of the Ty stepper motor, when scanning at high velocity this induce high frequency vibration that are outside the bandwidth of the feedback controller.
-
-At \(100\,\mu m/s\), the micro-stepping errors with a period of \(10\,\mu m\) (see Figure \ref{fig:id31_ty_scan_10ums_ol_dy_errors}) are at 10Hz.
-These errors are them amplified by some resonances in the system.
-
-This could be easily solved by changing the stepper motor for a torque motor for instance.
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_ty_scan_100ums_cl_dy_errors.png}
-\caption{\label{fig:id31_ty_scan_100ums_cl_dy_errors}\(T_y\) scan (at \(100\,\mu m/s\)) - \(D_y\) errors. Open-loop and Closed-loop scans}
-\end{figure}
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_ty_scan_100ums_cl_dz_ry_errors.png}
-\caption{\label{fig:id31_ty_scan_100ums_cl_dz_ry_errors}\(T_y\) scan (at \(100\,\mu m/s\)) - \(D_z\) and \(R_y\) errors. Open-loop and Closed-loop scans}
-\end{figure}
-\section{Combined \(R_z\) and \(D_y\): Diffraction Tomography}
-\label{sec:id31_scans_diffraction_tomo}
-Instead of doing a fast \(R_z\) motion a slow \(D_y\), the idea is to perform slow \(R_z\) (here 1rpm) and fast \(D_y\) scans with the nano-hexapod.
-
-Here, the \(D_y\) scans are performed only with the nano-hexapod (the Ty stage is not moving), so we are limited to \(\pm 100\,\mu m\).
-
-Several \(D_y\) velocities are tested: \(0.1\,mm/s\), \(0.5\,mm/s\), \(1\,mm/s\) and \(10\,mm/s\) (see Figure \ref{fig:id31_diffraction_tomo_velocities}).
-
-\begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_diffraction_tomo_velocities.png}
-\caption{\label{fig:id31_diffraction_tomo_velocities}Dy motion for several configured velocities}
-\end{figure}
-
-The corresponding ``repetition rate'' and \(D_y\) scan per spindle turn are shown in Table \ref{tab:diffraction_tomo_velocities}.
-
-The main issue here is the ``waiting'' time between two scans that is in the order of 50ms.
-By removing this waiting time (fairly easily), we can double the repetition rate at 10mm/s.
-
-\begin{table}[htbp]
-\caption{\label{tab:diffraction_tomo_velocities}\(D_y\) scaning repetition rate}
-\centering
-\begin{tabularx}{0.6\linewidth}{lXX}
-\toprule
-\(D_y\) Velocity & Repetition rate & Scans per turn (at 1RPM)\\
-\midrule
-0.1 mm/s & 4 s & 15\\
-0.5 mm/s & 0.9 s & 65\\
-1 mm/s & 0.5 s & 120\\
-10 mm/s & 0.18 s & 330\\
+\(d_1\) (y) & \(1.0\,mm\) & \(>2\,mm\) & \(1.35\,mm\)\\
+\(d_2\) (y) & \(0.8\,mm\) & \(>2\,mm\) & \(1.01\,mm\)\\
+\(d_3\) (x) & \(>2\,mm\) & \(1.06\,mm\) & \(1.38\,mm\)\\
+\(d_4\) (x) & \(>2\,mm\) & \(0.99\,mm\) & \(0.94\,mm\)\\
+\(d_5\) (z) & \(1.33\, mm\) & \(1.06\,mm\) & \(>2\,mm\)\\
 \bottomrule
 \end{tabularx}
 \end{table}
 
 
-The scan results for a velocity of 1mm/s is shown in Figure \ref{fig:id31_diffraction_tomo_1mms}.
-The \(D_z\) and \(R_y\) errors are quite small during the scan.
+\section{Estimated measurement errors}
+\label{ssec:test_id31_metrology_errors}
 
-The \(D_y\) errors are quite large as the velocity is increased.
-This type of scan can probably be massively improved by using feed-forward and optimizing the trajectory.
-Also, if the detectors are triggered in position (the Speedgoat could generate an encoder signal for instance), we don't care about the \(D_y\) errors.
+When using the NASS, the accuracy of the sample's positioning is linked to the accuracy of the external metrology.
+However, to validate the nano-hexapod with the associated instrumentation and control architecture, the accuracy of the metrology is not an issue.
+Only the bandwidth and noise characteristics of the external metrology are important.
+Yet, some elements effecting the accuracy of the metrology are discussed here.
+
+First, the ``metrology kinematics'' (discussed in Section \ref{ssec:test_id31_metrology_kinematics}) is only approximate (i.e. valid for very small displacements).
+This can be seen when performing lateral \([D_x,\,D_y]\) scans using the micro-hexapod while recording the vertical interferometer (Figure \ref{fig:test_id31_xy_map_sphere}).
+As the interferometer is pointing to a sphere and not to a plane, lateral motion of the sphere is seen as a vertical motion by the top interferometer.
+
+Then, the reference spheres have some deviations with respect to an ideal sphere.
+They are meant to be used with capacitive sensors which are integrating the shape errors over large surfaces.
+When using interferometers, the size of the ``light spot'' on the sphere surface is a circle with a diameter \(\approx 50\,\mu m\), therefore the system is more sensitive to shape errors with small features.
+As the interferometer light is travelling in air, the measured distance is sensitive to any variation in the refractive index of the air.
+Therefore, any variation of air temperature, pressure or humidity will induce measurement errors.
+For a measurement length of \(40\,mm\), a temperature variation of \(0.1\,{}^oC\) induces an errors in the distance measurement of \(\approx 4\,nm\).
+
+Finally, even in vacuum and in the absence of target motion, the interferometers are affected by noise \cite{watchi18_review_compac_inter}.
+The effect of the noise on the translation and rotation measurements is estimated in Figure \ref{fig:test_id31_interf_noise}.
 
 \begin{figure}[htbp]
-\centering
-\includegraphics[scale=1]{figs/id31_diffraction_tomo_1mms.png}
-\caption{\label{fig:id31_diffraction_tomo_1mms}Diffraction tomography with Dy velocity of 1mm/s and Rz velocity of 1RPM}
+\begin{subfigure}{0.49\textwidth}
+\begin{center}
+\includegraphics[scale=1,width=0.95\linewidth]{figs/test_id31_xy_map_sphere.png}
+\end{center}
+\subcaption{\label{fig:test_id31_xy_map_sphere}Z measurement during an XY mapping}
+\end{subfigure}
+\begin{subfigure}{0.49\textwidth}
+\begin{center}
+\includegraphics[scale=1,width=0.95\linewidth]{figs/test_id31_interf_noise.png}
+\end{center}
+\subcaption{\label{fig:test_id31_interf_noise}Interferometer noise}
+\end{subfigure}
+\caption{\label{fig:test_id31_metrology_errors}Estimated measurement errors of the metrology. Cross-coupling between lateral motion and vertical measurement is shown in (\subref{fig:test_id31_xy_map_sphere}). Effect of interferometer noise on the measured translations and rotations is shown in (\subref{fig:test_id31_interf_noise}).}
 \end{figure}
 
-\begin{table}[htbp]
-\caption{\label{tab:id31_diffraction_tomo_results}Obtained errors for several \(D_y\) velocities}
-\centering
-\begin{tabularx}{\linewidth}{lXX}
-\toprule
-Velocity & \(D_y\) {[}nmRMS] & \(D_z\) {[}nmRMS] & \(R_y\) {[}\(\mu\text{radRMS}\)]\\
-\midrule
-0.1 mm/s & 75.5 & 9.1 & 0.1\\
-0.5 mm/s & 190.5 & 10.0 & 0.1\\
-1 mm/s & 428.0 & 11.2 & 0.2\\
-10 mm/s & 4639.9 & 55.9 & 1.4\\
-\bottomrule
-\end{tabularx}
-\end{table}
-\section{Summary of experiments}
-For each conducted experiments, the \(D_y\), \(D_z\) and \(R_y\) errors are computed and summarized in Table \ref{tab:id31_experiments_results_summary}.
-
-\begin{table}[htbp]
-\caption{\label{tab:id31_experiments_results_summary}Table caption}
-\centering
-\begin{tabularx}{\linewidth}{Xccc}
-\toprule
- & \(D_y\) {[}nmRMS] & \(D_z\) {[}nmRMS] & \(R_y\) {[}nradRMS]\\
-\midrule
-Tomography (\(R_z\) 1rpm) & 15 & 5 & 55\\
-Tomography (\(R_z\) 6rpm) & 19 & 5 & 73\\
-Tomography (\(R_z\) 30rpm) & 38 & 10 & 129\\
-Dirty Layer (\(D_z\) \(10\,\mu m/s\)) & 25 & 5 & 114\\
-Dirty Layer (\(D_z\) \(100\,\mu m/s\)) & 34 & 15 & 130\\
-Reflectivity (\(R_y\) \(100\,\mu\text{rad}/s\)) & 28 & 6 & 118\\
-Lateral Scan (\(D_y\) \(10\,\mu m/s\)) & 21 & 10 & 37\\
-Diffraction Tomography (\(R_z\) 1rpm, \(D_y\) 0.1mm/s) & 75 & 9 & 118\\
-Diffraction Tomography (\(R_z\) 1rpm, \(D_y\) 1mm/s) & 428 & 11 & 169\\
-\bottomrule
-\end{tabularx}
-\end{table}
 \printbibliography[heading=bibintoc,title={Bibliography}]
 \end{document}