* 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