Update analysis

matlab/run_test.m

@@ -1,25 +0,0 @@
-%%
-tg = slrt;
-
-f = SimulinkRealTime.openFTP(tg);
-mget(f, 'data/data.dat');
-close(f);
-
-%% Convert the Data
-data = SimulinkRealTime.utils.getFileScopeData('data/data.dat').data;
-
-V = data(:, 1);
-d = data(:, 2)*1000e-6/3.333;
-t = data(:, end);
-
-%% Save
-save('mat/stroke_apa_1stacks_2.mat', 't', 'V', 'd');
-
-%%
-d = d - mean(d(t > 1.9 & t < 2.0));
-
-figure;
-plot(t, d)
-
-figure;
-plot(V, d)

matlab/setup.m

@@ -1,24 +0,0 @@
-Fs = 10e3; % [Hz]
-Ts = 1/Fs; % [s]
-
-%%
-freq_LPF = 0.5; % [Hz]
-s = tf('s');
-G_lpf = (1)/(1 + s/2/pi/freq_LPF);
-Gz = c2d(G_lpf, Ts, 'tustin');
-
-%%
-t = 0:Ts:12;
-V = -1*ones(size(t));
-
-t0 = 2; t1 = t0 + 1;
-V(t < t0-1) = 0;
-V(t > t0 & t < t0 + pi) = 3.25 + 4.25*cos(t(t > t0 & t < t0 + pi) - t0 + pi);
-V(t > t0 + pi & t < t1 + pi) = 7.5;
-V(t > t1 + pi & t < t1 + 2*pi) = 3.25 + 4.25*cos(t(t > t1 + pi & t < t1 + 2*pi) - t1 + pi);
-V(t > t1 + 2*pi + 2) = 0;
-
-w0 = 2*pi*10; xi = 1;
-V = lsim(1/(1 + 2*xi*s/w0 + s^2/w0^2), V, t);
-
-Vin = [t', V];

matlab/src/initializeAPA.m

@@ -16,9 +16,9 @@ arguments
     args.Gs  (1,1) double {mustBeNumeric} = 0
 
     % For 2DoF
-    args.k   (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*0.35e6
-    args.ke  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*1.5e6
-    args.ka  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*43e6
+    args.k   (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*0.38e6
+    args.ke  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*1.75e6
+    args.ka  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*3e7
 
     args.c   (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*3e1
     args.ce  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*2e1
@@ -48,7 +48,7 @@ end
 if args.Ga == 0
     switch args.type
       case '2dof'
-        actuator.Ga = -46.4;
+        actuator.Ga = -30.0;
       case 'flexible frame'
         actuator.Ga = 1; % TODO
       case 'flexible'

test-bench-apa300ml.org

@@ -49,6 +49,8 @@
   <hr>
 #+end_export
 
+#+latex: \clearpage
+
 * Introduction                                                        :ignore:
 
 The goal of this test bench is to extract all the important parameters of the Amplified Piezoelectric Actuator APA300ML.
@@ -2555,6 +2557,58 @@ The resonances are indeed due to limited stiffness of the APA.
 #+end_important
 
 **** TODO Estimated Flexible Joint axial stiffness
+#+begin_src matlab
+load(sprintf('frf_data_leg_coder_%i_add_mass_closed_circuit.mat',    1), 't', 'Va', 'Vs', 'de', 'da');
+de = de - de(1);
+da = da - da(1);
+#+end_src
+
+#+begin_src matlab
+figure;
+hold on;
+plot(t, de, 'DisplayName', 'Encoder')
+plot(t, da, 'DisplayName', 'Interferometer')
+hold off;
+xlabel('Time [s]'); ylabel('Displacement [m]');
+legend('location', 'northeast');
+#+end_src
+
+#+begin_src matlab
+de_steps = [mean(de(t > 13 & t < 14));
+            mean(de(t > 19 & t < 20));
+            mean(de(t > 24 & t < 25));
+            mean(de(t > 28.5 & t < 29.5));
+            mean(de(t > 49 & t < 50))] - mean(de(t > 13 & t < 14));
+
+da_steps = [mean(da(t > 13 & t < 14));
+            mean(da(t > 19 & t < 20));
+            mean(da(t > 24 & t < 25));
+            mean(da(t > 28.5 & t < 29.5));
+            mean(da(t > 49 & t < 50))] - mean(da(t > 13 & t < 14));
+#+end_src
+
+#+begin_src matlab
+added_mass = [0; 1; 2; 3; 4];
+#+end_src
+
+#+begin_src matlab
+lin_fit = (added_mass\abs(de_steps-da_steps) - abs(de_steps(1)-da_steps(1)));
+#+end_src
+
+#+begin_src matlab
+figure;
+hold on;
+plot(added_mass, abs(de_steps-da_steps) - abs(de_steps(1)-da_steps(1)), 'ok')
+plot(added_mass, added_mass*lin_fit, '-r')
+hold off;
+#+end_src
+
+#+begin_question
+What is strange is that the encoder is measuring a larger displacement than the interferometer.
+That should be the opposite.
+Maybe is is caused by the fact that the APA is experiencing some bending and therefore a larger motion is measured on the encoder.
+#+end_question
+
 **** FRF Identification - IFF
 In this section, the dynamics from $V_a$ to $V_s$ is identified.
 
@@ -4050,6 +4104,7 @@ A simscape model (Figure [[fig:model_bench_apa]]) of the measurement bench is us
 #+end_src
 
 #+begin_src matlab :tangle no
+%% Add useful folders to the path
 addpath('matlab/');
 addpath('matlab/test_bench_apa300ml/');
 addpath('matlab/mat/');
@@ -4058,6 +4113,7 @@ addpath('matlab/png/');
 #+end_src
 
 #+begin_src matlab :eval no
+%% Add useful folders to the path
 addpath('test_bench_apa300ml/');
 addpath('png/');
 addpath('mat/');
@@ -4065,32 +4121,30 @@ addpath('src/');
 #+end_src
 
 #+begin_src matlab
-%% Options for Linearized
+%% Frequency vector used for many plots
+freqs = 2*logspace(0, 3, 1000);
+#+end_src
+
+#+begin_src matlab
+%% Open Simscape Model
 options = linearizeOptions;
 options.SampleTime = 0;
 
-%% Name of the Simulink File
+% Name of the Simulink File
 mdl = 'test_bench_apa300ml';
 
-%% Open the Simulink File
-open([mdl, '.slx'])
+open(mdl)
 #+end_src
 
 ** First Identification
 The APA is first initialized with default parameters and the transfer function from excitation voltage $V_a$ (before the amplification of 20 due to the PD200 amplifier) to the sensor stack voltage $V_s$, encoder $d_L$ and interferometer $z$ is identified.
 #+begin_src matlab
+%% Initialize the structure containing the data used in the model
 n_hexapod = struct();
 n_hexapod.actuator = initializeAPA('type', '2dof');
 #+end_src
 
 #+begin_src matlab
-%% Options for Linearized
-options = linearizeOptions;
-options.SampleTime = 0;
-
-%% Name of the Simulink File
-mdl = 'test_bench_apa300ml';
-
 %% Input/Output definition
 clear io; io_i = 1;
 io(io_i) = linio([mdl, '/Va'],  1, 'openinput');  io_i = io_i + 1; % Actuator Voltage
@@ -4107,6 +4161,7 @@ Ga.OutputName = {'Vs', 'dL', 'z'};
 The obtain dynamics are shown in Figure [[fig:apa_model_bench_bode_vs]] and [[fig:apa_model_bench_bode_dl_z]].
 
 #+begin_src matlab :exports none
+%% Bode plot of the transfer function from u to taum
 freqs = logspace(1, 3, 1000);
 
 figure;
@@ -4142,6 +4197,7 @@ exportFig('figs/apa_model_bench_bode_vs.pdf', 'width', 'wide', 'height', 'normal
 [[file:figs/apa_model_bench_bode_vs.png]]
 
 #+begin_src matlab :exports none
+%% Bode plot of the transfer function from u to dLm and dZm
 freqs = logspace(1, 3, 1000);
 
 figure;
@@ -4179,7 +4235,6 @@ exportFig('figs/apa_model_bench_bode_dl_z.pdf', 'width', 'wide', 'height', 'tall
 #+RESULTS:
 [[file:figs/apa_model_bench_bode_dl_z.png]]
 
-
 ** Identify Sensor/Actuator constants and compare with measured FRF
 *** How to identify these constants?
 **** Piezoelectric Actuator Constant
@@ -4222,6 +4277,7 @@ Then, the "sensor" constant is:
 
 *** Identification Data
 #+begin_src matlab
+%% Load the identification Data
 leg_sweep    = load(sprintf('frf_data_%i_sweep.mat',    1), 't', 'Va', 'Vs', 'de', 'da');
 leg_noise_hf = load(sprintf('frf_data_%i_noise_hf.mat', 1), 't', 'Va', 'Vs', 'de', 'da');
 #+end_src
@@ -4231,39 +4287,39 @@ The time is the same for all measurements.
 %% Time vector
 t = leg_sweep.t - leg_sweep.t(1) ; % Time vector [s]
 
-%% Sampling
+%% Sampling Time / Frequency
 Ts = (t(end) - t(1))/(length(t)-1); % Sampling Time [s]
 Fs = 1/Ts; % Sampling Frequency [Hz]
 #+end_src
 
 Then we defined a "Hanning" windows that will be used for the spectral analysis:
 #+begin_src matlab
+%% Window used for spectral analysis
 win = hanning(ceil(0.5*Fs)); % Hannning Windows
 #+end_src
 
 We get the frequency vector that will be the same for all the frequency domain analysis.
 #+begin_src matlab
-% Only used to have the frequency vector "f"
+%% Get the frequency vector "f" common to all further spectral data
 [~, f] = tfestimate(leg_sweep.Va, leg_sweep.de, win, [], [], 1/Ts);
 #+end_src
 
 #+begin_src matlab
+%% Estimate the transfer function from u to dLm
 [dvf_sweep, ~]    = tfestimate(leg_sweep.Va,    leg_sweep.da,    win, [], [], 1/Ts);
 [dvf_noise_hf, ~] = tfestimate(leg_noise_hf.Va, leg_noise_hf.da, win, [], [], 1/Ts);
 #+end_src
 
 #+begin_src matlab
+%% Estimate the transfer function from u to taum
 [iff_sweep, ~] = tfestimate(leg_sweep.Va, leg_sweep.Vs, win, [], [], 1/Ts);
 [iff_noise_hf, ~] = tfestimate(leg_noise_hf.Va, leg_noise_hf.Vs, win, [], [], 1/Ts);
 #+end_src
 
-#+begin_src matlab
-freqs = 2*logspace(0, 3, 1000);
-#+end_src
-
 *** 2DoF APA
 **** 2DoF APA
 #+begin_src matlab
+%% Initialize a 2DoF APA with Ga=Gs=1
 n_hexapod.actuator = initializeAPA('type', '2dof', 'ga', 1, 'gs', 1);
 #+end_src
 
@@ -4284,6 +4340,7 @@ Gs.OutputName = {'Vs', 'dL', 'z'};
 
 **** Actuator Constant
 #+begin_src matlab
+%% Estimated Actuator Constant
 ga = -mean(abs(dvf_sweep(f>10 & f<20)))./dcgain(Gs('dL', 'Va')); % [N/V]
 #+end_src
 
@@ -4292,7 +4349,7 @@ sprintf('ga = %.1f [N/V]', ga);
 #+end_src
 
 #+RESULTS:
-: ga = -46.4 [N/V]
+: ga = -32.1 [N/V]
 
 #+begin_src matlab
 n_hexapod.actuator.Ga = ones(6,1)*ga; % Actuator gain [N/V]
@@ -4300,6 +4357,7 @@ n_hexapod.actuator.Ga = ones(6,1)*ga; % Actuator gain [N/V]
 
 **** Sensor Constant
 #+begin_src matlab
+%% Estimated Sensor Constant
 gs = -mean(abs(iff_sweep(f>400 & f<500)))./(ga*abs(squeeze(freqresp(Gs('Vs', 'Va'), 1e3, 'Hz')))); % [V/m]
 #+end_src
 
@@ -4308,7 +4366,7 @@ sprintf('gs = %.3f [V/m]', gs);
 #+end_src
 
 #+RESULTS:
-: gs = 0.098 [V/m]
+: gs = 0.085 [V/m]
 
 #+begin_src matlab
 n_hexapod.actuator.Gs = ones(6,1)*gs; % Sensor gain [V/m]
@@ -4317,6 +4375,7 @@ n_hexapod.actuator.Gs = ones(6,1)*gs; % Sensor gain [V/m]
 **** Comparison
 Identify the dynamics with included constants.
 #+begin_src matlab
+%% Identify again the dynamics with correct Ga,Gs
 Gs = linearize(mdl, io, 0.0, options);
 Gs = Gs*exp(-Ts*s);
 Gs.InputName  = {'Va'};
@@ -4324,6 +4383,7 @@ Gs.OutputName = {'Vs', 'dL', 'z'};
 #+end_src
 
 #+begin_src matlab :exports none
+%% Bode plot of the transfer function from u to dLm (both Simscape and measured FRF)
 figure;
 tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
 
@@ -4334,9 +4394,9 @@ plot(f(f<=350), abs(dvf_sweep(   f<=350)), 'k-');
 plot(freqs, abs(squeeze(freqresp(Gs('dL', 'Va'), freqs, 'Hz'))))
 hold off;
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
+ylabel('Amplitude $d\mathcal{L}_m/u$ [m/V]'); set(gca, 'XTickLabel',[]);
 hold off;
-ylim([1e-9, 1e-3]);
+ylim([1e-8, 1e-3]);
 
 ax2 = nexttile;
 hold on;
@@ -4358,11 +4418,12 @@ exportFig('figs/apa_act_constant_comp.pdf', 'width', 'wide', 'height', 'tall');
 #+end_src
 
 #+name: fig:apa_act_constant_comp
-#+caption: Comparison of the experimental data and Simscape model ($V_a$ to $d_e$)
+#+caption: Comparison of the experimental data and Simscape model ($u$ to $d\mathcal{L}_m$)
 #+RESULTS:
 [[file:figs/apa_act_constant_comp.png]]
 
 #+begin_src matlab :exports none
+%% Bode plot of the transfer function from u to taum (both Simscape and measured FRF)
 figure;
 tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
 
@@ -4373,7 +4434,7 @@ plot(f(f<=350), abs(iff_sweep(   f<=350)), 'k-');
 plot(freqs, abs(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
 hold off;
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
+ylabel('Amplitude $\tau_m/u$ [V/V]'); set(gca, 'XTickLabel',[]);
 hold off;
 ylim([1e-2, 1e2]);
 
@@ -4404,11 +4465,13 @@ exportFig('figs/apa_sens_constant_comp.pdf', 'width', 'wide', 'height', 'tall');
 *** Flexible APA
 **** Flexible APA
 #+begin_src matlab
+%% Initialize the APA as a flexible body
 n_hexapod.actuator = initializeAPA('type', 'flexible', 'ga', 1, 'gs', 1);
 #+end_src
 
 **** Identification without actuator or sensor constants
 #+begin_src matlab
+%% Identify the dynamics
 Gs = linearize(mdl, io, 0.0, options);
 Gs.InputName  = {'Va'};
 Gs.OutputName = {'Vs', 'dL', 'z'};
@@ -4416,6 +4479,7 @@ Gs.OutputName = {'Vs', 'dL', 'z'};
 
 **** Actuator Constant
 #+begin_src matlab
+%% Actuator Constant
 ga = -mean(abs(dvf_sweep(f>10 & f<20)))./dcgain(Gs('dL', 'Va')); % [N/V]
 #+end_src
 
@@ -4432,6 +4496,7 @@ n_hexapod.actuator.Ga = ones(6,1)*ga; % Actuator gain [N/V]
 
 **** Sensor Constant
 #+begin_src matlab
+%% Sensor Constant
 gs = -mean(abs(iff_sweep(f>400 & f<500)))./(ga*abs(squeeze(freqresp(Gs('Vs', 'Va'), 1e3, 'Hz')))); % [V/m]
 #+end_src
 
@@ -4440,7 +4505,7 @@ sprintf('gs = %.3f [V/m]', gs);
 #+end_src
 
 #+RESULTS:
-: gs = -4674824.519 [V/m]
+: gs = -4674826.805 [V/m]
 
 #+begin_src matlab
 n_hexapod.actuator.Gs = ones(6,1)*gs; % Sensor gain [V/m]
@@ -4448,6 +4513,7 @@ n_hexapod.actuator.Gs = ones(6,1)*gs; % Sensor gain [V/m]
 
 **** Comparison
 #+begin_src matlab
+%% Identify with updated constants
 Gs = linearize(mdl, io, 0.0, options);
 Gs = Gs*exp(-Ts*s);
 Gs.InputName  = {'Va'};
@@ -4455,6 +4521,7 @@ Gs.OutputName = {'Vs', 'dL', 'z'};
 #+end_src
 
 #+begin_src matlab :exports none
+%% Bode plot of the transfer function from u to dLm (both Simscape and measured FRF)
 figure;
 tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
 
@@ -4465,7 +4532,7 @@ plot(f(f<=350), abs(dvf_sweep(   f<=350)), 'k-');
 plot(freqs, abs(squeeze(freqresp(Gs('dL', 'Va'), freqs, 'Hz'))))
 hold off;
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
+ylabel('Amplitude $d\mathcal{L}_m/u$ [m/V]'); set(gca, 'XTickLabel',[]);
 hold off;
 ylim([1e-9, 1e-3]);
 
@@ -4489,11 +4556,12 @@ exportFig('figs/apa_act_constant_comp_flex.pdf', 'width', 'wide', 'height', 'tal
 #+end_src
 
 #+name: fig:apa_act_constant_comp_flex
-#+caption: Comparison of the experimental data and Simscape model ($V_a$ to $d_e$)
+#+caption: Comparison of the experimental data and Simscape model ($u$ to $d\mathcal{L}_m$)
 #+RESULTS:
 [[file:figs/apa_act_constant_comp_flex.png]]
 
 #+begin_src matlab :exports none
+%% Bode plot of the transfer function from u to taum (both Simscape and measured FRF)
 figure;
 tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
 
@@ -4504,7 +4572,7 @@ plot(f(f<=350), abs(iff_sweep(   f<=350)), 'k-');
 plot(freqs, abs(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
 hold off;
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
+ylabel('Amplitude $\tau_m/u$ [V/V]'); set(gca, 'XTickLabel',[]);
 hold off;
 ylim([1e-2, 1e2]);
 
@@ -4528,11 +4596,167 @@ exportFig('figs/apa_sens_constant_comp_flex.pdf', 'width', 'wide', 'height', 'ta
 #+end_src
 
 #+name: fig:apa_sens_constant_comp_flex
-#+caption: Comparison of the experimental data and Simscape model ($V_a$ to $V_s$)
+#+caption: Comparison of the experimental data and Simscape model ($u$ to $\tau_m$)
 #+RESULTS:
 [[file:figs/apa_sens_constant_comp_flex.png]]
 
-** Compare 2-DoF with flexible
+** Optimize 2-DoF model to fit the experimental Data
+#+begin_src matlab
+%% Optimized parameters
+n_hexapod.actuator = initializeAPA('type', '2dof', ...
+                                   'Ga', -30.0, ...
+                                   'Gs', 0.098, ...
+                                   'k',  ones(6,1)*0.38e6, ...
+                                   'ke', ones(6,1)*1.75e6, ...
+                                   'ka', ones(6,1)*3e7, ...
+                                   'c',  ones(6,1)*1.3e2, ...
+                                   'ce', ones(6,1)*1e1, ...
+                                   'ca', ones(6,1)*1e1 ...
+                                   );
+#+end_src
+
+#+begin_src matlab :exports none
+%% Input/Output definition
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/Va'],  1, 'openinput');  io_i = io_i + 1; % Actuator Voltage
+io(io_i) = linio([mdl, '/Vs'],  1, 'openoutput'); io_i = io_i + 1; % Sensor Voltage
+io(io_i) = linio([mdl, '/dL'],  1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs
+
+%% Identification with optimized parameters
+Gs = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
+Gs.InputName  = {'Va'};
+Gs.OutputName = {'Vs', 'dL'};
+#+end_src
+
+#+begin_src matlab :exports none
+apa_nums = [1 2 4 5 6 8];
+
+%% Second identification
+apa_sweep = {};
+for i = 1:length(apa_nums)
+    apa_sweep(i) = {load(sprintf('frf_data_%i_sweep.mat', apa_nums(i)), 't', 'Va', 'Vs', 'de', 'da')};
+end
+
+%% Third identification
+apa_noise_hf = {};
+for i = 1:length(apa_nums)
+    apa_noise_hf(i) = {load(sprintf('frf_data_%i_noise_hf.mat', apa_nums(i)), 't', 'Va', 'Vs', 'de', 'da')};
+end
+
+%% Time vector
+t = apa_sweep{1}.t - apa_sweep{1}.t(1) ; % Time vector [s]
+
+%% Sampling
+Ts = (t(end) - t(1))/(length(t)-1); % Sampling Time [s]
+Fs = 1/Ts; % Sampling Frequency [Hz]
+
+win = hanning(ceil(0.5*Fs)); % Hannning Windows
+% Only used to have the frequency vector "f"
+[~, f] = tfestimate(apa_sweep{1}.Va, apa_sweep{1}.de, win, [], [], 1/Ts);
+
+%% Transfer function estimation
+dvf_sweep = zeros(length(f), length(apa_nums));
+for i = 1:length(apa_nums)
+    [frf, ~] = tfestimate(apa_sweep{i}.Va, apa_sweep{i}.de, win, [], [], 1/Ts);
+    dvf_sweep(:, i) = frf;
+end
+
+dvf_noise_hf = zeros(length(f), length(apa_nums));
+for i = 1:length(apa_nums)
+    [frf, ~] = tfestimate(apa_noise_hf{i}.Va, apa_noise_hf{i}.de, win, [], [], 1/Ts);
+    dvf_noise_hf(:, i) = frf;
+end
+
+%% FRF estimation of the transfer function from Va to Vs
+iff_sweep = zeros(length(f), length(apa_nums));
+for i = 1:length(apa_nums)
+    [frf, ~] = tfestimate(apa_sweep{i}.Va, apa_sweep{i}.Vs, win, [], [], 1/Ts);
+    iff_sweep(:, i) = frf;
+end
+
+iff_noise_hf = zeros(length(f), length(apa_nums));
+for i = 1:length(apa_nums)
+    [frf, ~] = tfestimate(apa_noise_hf{i}.Va, apa_noise_hf{i}.Vs, win, [], [], 1/Ts);
+    iff_noise_hf(:, i) = frf;
+end
+#+end_src
+
+#+begin_src matlab :exports none
+%% Comparison of the experimental data and Simscape Model
+freqs = 5*logspace(0, 3, 1000);
+figure;
+tiledlayout(3, 2, 'TileSpacing', 'None', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+for i = 1:length(apa_nums)
+    plot(f(f> 350), abs(dvf_noise_hf(f> 350, i)), 'color', [0,0,0,0.2]);
+    plot(f(f<=350), abs(dvf_sweep(   f<=350, i)), 'color', [0,0,0,0.2]);
+end
+set(gca,'ColorOrderIndex',2);
+plot(freqs, abs(squeeze(freqresp(Gs('dL', 'Va'), freqs, 'Hz'))))
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
+hold off;
+ylim([1e-8, 1e-3]);
+
+ax1b = nexttile([2,1]);
+hold on;
+for i = 1:length(apa_nums)
+    plot(f(f> 350), abs(iff_noise_hf(f> 350, i)), 'color', [0,0,0,0.2]);
+    plot(f(f<=350), abs(iff_sweep(   f<=350, i)), 'color', [0,0,0,0.2]);
+end
+set(gca,'ColorOrderIndex',2);
+plot(freqs, abs(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
+hold off;
+ylim([1e-2, 1e2]);
+
+ax2 = nexttile;
+hold on;
+for i = 1:length(apa_nums)
+    plot(f(f> 350), 180/pi*angle(dvf_noise_hf(f> 350, i)), 'color', [0,0,0,0.2]);
+    plot(f(f<=350), 180/pi*angle(dvf_sweep(   f<=350, i)), 'color', [0,0,0,0.2]);
+end
+set(gca,'ColorOrderIndex',2);
+plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('dL', 'Va'), 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]);
+
+ax2b = nexttile;
+hold on;
+for i = 1:length(apa_nums)
+    plot(f(f> 350), 180/pi*angle(iff_noise_hf(f> 350, i)), 'color', [0,0,0,0.2]);
+    plot(f(f<=350), 180/pi*angle(iff_sweep(   f<=350, i)), 'color', [0,0,0,0.2]);
+end
+set(gca,'ColorOrderIndex',2);
+plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('Vs', 'Va'), 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,ax1b,ax2b],'x');
+xlim([10, 2e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/comp_apa_plant_after_opt.pdf', 'width', 'full', 'height', 'tall');
+#+end_src
+
+#+name: fig:comp_apa_plant_after_opt
+#+caption: Comparison of the measured FRF and the optimized model
+#+RESULTS:
+[[file:figs/comp_apa_plant_after_opt.png]]
+
+** Compare 2-DoF with flexible                                     :noexport:
 APA - 2 DoF
 #+begin_src matlab
 n_hexapod.actuator = initializeAPA('type', '2dof');
@@ -4678,6 +4902,7 @@ exportFig('figs/apa_effect_joint_comp_z.pdf', 'width', 'wide', 'height', 'tall')
 #+end_src
 
 #+begin_src matlab :tangle no
+%% Add useful folders to the path
 addpath('matlab/');
 addpath('matlab/test_bench_struts/');
 addpath('matlab/mat/');
@@ -4686,12 +4911,18 @@ addpath('matlab/png/');
 #+end_src
 
 #+begin_src matlab :eval no
+%% Add useful folders to the path
 addpath('test_bench_struts/');
 addpath('png/');
 addpath('mat/');
 addpath('src/');
 #+end_src
 
+#+begin_src matlab
+%% Frequency vector used for many plots
+freqs = 2*logspace(0, 3, 1000);
+#+end_src
+
 #+begin_src matlab
 %% Options for Linearized
 options = linearizeOptions;
@@ -4701,12 +4932,13 @@ options.SampleTime = 0;
 mdl = 'test_bench_struts';
 
 %% Open the Simulink File
-open([mdl, '.slx'])
+open(mdl)
 #+end_src
 
 ** First Identification
 The object containing all the data is created.
 #+begin_src matlab
+%% Initialize structure containing data for the Simscape model
 n_hexapod = struct();
 n_hexapod.flex_bot = initializeBotFlexibleJoint('type', '2dof');
 n_hexapod.flex_top = initializeTopFlexibleJoint('type', '2dof');
@@ -4730,8 +4962,7 @@ Gs.OutputName = {'Vs', 'dL', 'z'};
 #+end_src
 
 #+begin_src matlab :exports none
-freqs = logspace(1, 3, 1000);
-
+%% Bode plot of the transfer function from u to taum
 figure;
 tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
 
@@ -4763,8 +4994,7 @@ exportFig('figs/strut_bench_model_iff_bode.pdf', 'width', 'wide', 'height', 'nor
 [[file:figs/strut_bench_model_iff_bode.png]]
 
 #+begin_src matlab :exports none
-freqs = logspace(1, 4, 1000);
-
+%% Bode plot of the transfer function from u to dLm and dZm
 figure;
 tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
 
@@ -4803,6 +5033,7 @@ exportFig('figs/strut_bench_model_dvf_bode.pdf', 'width', 'wide', 'height', 'tal
 ** Effect of flexible joints
 Perfect
 #+begin_src matlab
+%% Perfect Joints
 n_hexapod.flex_bot = initializeBotFlexibleJoint('type', '2dof');
 n_hexapod.flex_top = initializeTopFlexibleJoint('type', '3dof');
 
@@ -4813,6 +5044,7 @@ Gp.OutputName = {'Vs', 'dL', 'z'};
 
 Top Flexible
 #+begin_src matlab
+%% Top joint with Axial stiffness
 n_hexapod.flex_bot = initializeBotFlexibleJoint('type', '2dof');
 n_hexapod.flex_top = initializeTopFlexibleJoint('type', '4dof');
 
@@ -4823,6 +5055,7 @@ Gt.OutputName = {'Vs', 'dL', 'z'};
 
 Bottom Flexible
 #+begin_src matlab
+%% Bottom joint with Axial stiffness
 n_hexapod.flex_bot = initializeBotFlexibleJoint('type', '4dof');
 n_hexapod.flex_top = initializeTopFlexibleJoint('type', '3dof');
 
@@ -4833,6 +5066,7 @@ Gb.OutputName = {'Vs', 'dL', 'z'};
 
 Both Flexible
 #+begin_src matlab
+%% Both joints with Axial stiffness
 n_hexapod.flex_bot = initializeBotFlexibleJoint('type', '4dof');
 n_hexapod.flex_top = initializeTopFlexibleJoint('type', '4dof');
 
@@ -4844,8 +5078,7 @@ Gf.OutputName = {'Vs', 'dL', 'z'};
 Comparison in Figures [[fig:strut_effect_joint_comp_vs]], [[fig:strut_effect_joint_comp_dl]] and [[fig:strut_effect_joint_comp_z]].
 
 #+begin_src matlab :exports none
-freqs = logspace(1, 4, 1000);
-
+%% Transfer function from u to taum - Effect of flexible joints
 figure;
 tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
 
@@ -4886,8 +5119,7 @@ exportFig('figs/strut_effect_joint_comp_vs.pdf', 'width', 'wide', 'height', 'tal
 [[file:figs/strut_effect_joint_comp_vs.png]]
 
 #+begin_src matlab :exports none
-freqs = logspace(1, 4, 1000);
-
+%% Transfer function from u to dLm - Effect of flexible joints
 figure;
 tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
 
@@ -4928,8 +5160,7 @@ exportFig('figs/strut_effect_joint_comp_dl.pdf', 'width', 'wide', 'height', 'tal
 [[file:figs/strut_effect_joint_comp_dl.png]]
 
 #+begin_src matlab :exports none
-freqs = logspace(1, 4, 1000);
-
+%% Transfer function from u to dZm - Effect of flexible joints
 figure;
 tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
 
@@ -4977,6 +5208,7 @@ However, it has relatively small impact on the transfer functions from $V_a$ to
 ** Integral Force Feedback
 *** Initialize the system
 #+begin_src matlab
+%% Initialize typical system
 n_hexapod = struct();
 n_hexapod.flex_bot = initializeBotFlexibleJoint('type', '2dof');
 n_hexapod.flex_top = initializeTopFlexibleJoint('type', '3dof');
@@ -4985,12 +5217,14 @@ n_hexapod.actuator = initializeAPA('type', '2dof');
 
 *** Plant Identification
 #+begin_src matlab
+%% Identify th edynamics
 G = linearize(mdl, io, 0.0, options);
 G.InputName  = {'Va'};
 G.OutputName = {'Vs', 'dL', 'z'};
 #+end_src
 
 #+begin_src matlab
+%% IFF Plant (transfer function from u to taum)
 Giff = G('Vs', 'Va');
 #+end_src
 
@@ -5000,12 +5234,15 @@ K_{\text{IFF}} = \frac{g}{s + \omega_c}
 \end{equation}
 
 #+begin_src matlab
+%% Cut-off frequency of the LPF
 wc = 2*pi*20;
 #+end_src
 
 #+begin_src matlab :exports none
+% Gains used for the Root Locus plot
 gains = logspace(1, 5, 300);
 
+%% Root Locus for the IFF
 figure;
 
 hold on;
@@ -5020,16 +5257,164 @@ for g = gains
     plot(real(clpoles), imag(clpoles), '.', 'HandleVisibility', 'off');
 end
 
-g = 8e2;
+g = 2e2;
 clpoles = pole(feedback(Giff, (g/(s + wc))));
-set(gca,'ColorOrderIndex',1);
-plot(real(clpoles), imag(clpoles), 'rx', 'DisplayName', '$g = 1.5 \cdot 10^4$');
+set(gca,'ColorOrderIndex',2);
+plot(real(clpoles), imag(clpoles), 'x', 'DisplayName', sprintf('$g=%.0f$', g));
 axis square;
-xlim([-600, 5]); ylim([-5, 600]);
+xlim([-700, 0]); ylim([-0, 700]);
 xlabel('Real Part'); ylabel('Imaginary Part');
 legend('location', 'northwest');
 #+end_src
 
+** Comparison with the experimental Data
+#+begin_src matlab :exports none
+%% Measured dynamics
+leg_nums = [1 2 3 4 5];
+
+% Second identification
+leg_noise = {};
+for i = 1:length(leg_nums)
+    leg_noise(i) = {load(sprintf('frf_data_leg_coder_%i_noise.mat', leg_nums(i)), 't', 'Va', 'Vs', 'de', 'da')};
+end
+
+% Third identification
+leg_noise_hf = {};
+for i = 1:length(leg_nums)
+    leg_noise_hf(i) = {load(sprintf('frf_data_leg_coder_%i_noise_hf.mat', leg_nums(i)), 't', 'Va', 'Vs', 'de', 'da')};
+end
+
+t = leg_noise{1}.t - leg_noise{1}.t(1) ; % Time vector [s]
+Ts = (t(end) - t(1))/(length(t)-1); % Sampling Time [s]
+Fs = 1/Ts; % Sampling Frequency [Hz]
+
+win = hanning(ceil(0.5*Fs)); % Hannning Windows
+
+% Only used to have the frequency vector "f"
+[~, f] = tfestimate(leg_noise{1}.Va, leg_noise{1}.de, win, [], [], 1/Ts);
+
+% Transfer function estimation
+dvf_a_noise = zeros(length(f), length(leg_nums));
+for i = 1:length(leg_nums)
+    [frf, ~] = tfestimate(leg_noise{i}.Va, leg_noise{i}.da, win, [], [], 1/Ts);
+    dvf_a_noise(:, i) = frf;
+end
+
+dvf_a_noise_hf = zeros(length(f), length(leg_nums));
+for i = 1:length(leg_nums)
+    [frf, ~] = tfestimate(leg_noise_hf{i}.Va, leg_noise_hf{i}.da, win, [], [], 1/Ts);
+    dvf_a_noise_hf(:, i) = frf;
+end
+
+% FRF estimation of the transfer function from Va to Vs
+iff_noise = zeros(length(f), length(leg_nums));
+for i = 1:length(leg_nums)
+    [frf, ~] = tfestimate(leg_noise{i}.Va, leg_noise{i}.Vs, win, [], [], 1/Ts);
+    iff_noise(:, i) = frf;
+end
+
+iff_noise_hf = zeros(length(f), length(leg_nums));
+for i = 1:length(leg_nums)
+    [frf, ~] = tfestimate(leg_noise_hf{i}.Va, leg_noise_hf{i}.Vs, win, [], [], 1/Ts);
+    iff_noise_hf(:, i) = frf;
+end
+#+end_src
+
+#+begin_src matlab
+%% Initialize Simscape data
+n_hexapod.flex_bot = initializeBotFlexibleJoint('type', '4dof');
+n_hexapod.flex_top = initializeTopFlexibleJoint('type', '4dof');
+n_hexapod.actuator = initializeAPA('type', '2dof');
+#+end_src
+
+#+begin_src matlab
+%% Input/Output definition
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/Va'],  1, 'openinput');  io_i = io_i + 1; % Actuator Voltage
+io(io_i) = linio([mdl, '/Vs'],  1, 'openoutput'); io_i = io_i + 1; % Sensor Voltage
+io(io_i) = linio([mdl, '/z'],   1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs
+
+%% Identification
+Gs = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
+Gs.InputName  = {'Va'};
+Gs.OutputName = {'Vs', 'z'};
+#+end_src
+
+#+begin_src matlab :exports none
+freqs = 5*logspace(0, 3, 1000);
+
+figure;
+tiledlayout(3, 2, 'TileSpacing', 'None', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+for i = 1:length(leg_nums)
+    plot(f(f> 350), abs(dvf_a_noise_hf(f> 350, i)), 'color', [0,0,0,0.2]);
+    plot(f(f<=350), abs(dvf_a_noise(   f<=350, i)), 'color', [0,0,0,0.2]);
+end
+set(gca,'ColorOrderIndex',2);
+plot(freqs, abs(squeeze(freqresp(Gs('z', 'Va'), freqs, 'Hz'))), '-')
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
+hold off;
+ylim([1e-8, 1e-3]);
+
+ax1b = nexttile([2,1]);
+hold on;
+for i = 1:length(leg_nums)
+    plot(f(f> 350), abs(iff_noise_hf(f> 350, i)), 'color', [0,0,0,0.2]);
+    plot(f(f<=350), abs(iff_noise(   f<=350, i)), 'color', [0,0,0,0.2]);
+end
+set(gca,'ColorOrderIndex',2);
+plot(freqs, abs(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))), '-')
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
+hold off;
+ylim([1e-2, 1e2]);
+
+ax2 = nexttile;
+hold on;
+for i = 1:length(leg_nums)
+    plot(f(f> 350), 180/pi*angle(dvf_a_noise_hf(f> 350, i)), 'color', [0,0,0,0.2]);
+    plot(f(f<=350), 180/pi*angle(dvf_a_noise(   f<=350, i)), 'color', [0,0,0,0.2]);
+end
+set(gca,'ColorOrderIndex',2);
+plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('z', 'Va'), 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]);
+
+ax2b = nexttile;
+hold on;
+for i = 1:length(leg_nums)
+    plot(f(f> 350), 180/pi*angle(iff_noise_hf(f> 350, i)), 'color', [0,0,0,0.2]);
+    plot(f(f<=350), 180/pi*angle(iff_noise(   f<=350, i)), 'color', [0,0,0,0.2]);
+end
+set(gca,'ColorOrderIndex',2);
+plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('Vs', 'Va'), 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,ax1b,ax2b],'x');
+xlim([10, 2e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/comp_strut_plant_after_opt.pdf', 'width', 'full', 'height', 'tall');
+#+end_src
+
+#+name: fig:comp_strut_plant_after_opt
+#+caption: Comparison of the measured FRF and the optimized model
+#+RESULTS:
+[[file:figs/comp_strut_plant_after_opt.png]]
+
 * Function
 ** =initializeBotFlexibleJoint= - Initialize Flexible Joint
 :PROPERTIES:
@@ -5249,9 +5634,9 @@ arguments
     args.Gs  (1,1) double {mustBeNumeric} = 0
 
     % For 2DoF
-    args.k   (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*0.35e6
-    args.ke  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*1.5e6
-    args.ka  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*43e6
+    args.k   (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*0.38e6
+    args.ke  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*1.75e6
+    args.ka  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*3e7
 
     args.c   (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*3e1
     args.ce  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*2e1
@@ -5302,7 +5687,7 @@ end
 if args.Ga == 0
     switch args.type
       case '2dof'
-        actuator.Ga = -46.4;
+        actuator.Ga = -30.0;
       case 'flexible frame'
         actuator.Ga = 1; % TODO
       case 'flexible'