Rework analysis of encoders fixed to the struts

test-bench-nano-hexapod.org

@@ -142,10 +142,10 @@ This document is divided in the following sections:
 - Section [[sec:encoders_struts]]: the encoders are fixed to the struts
 - Section [[sec:encoders_plates]]: the encoders are fixed to the plates
 
-* Encoders fixed to the Struts
+* Encoders fixed to the Struts - Dynamics
 <<sec:encoders_struts>>
 
-** Introduction
+** Introduction                                                      :ignore:
 In this section, the encoders are fixed to the struts.
 
 It is divided in the following sections:
@@ -1556,9 +1556,6 @@ linkaxes([ax1,ax2],'x');
 xlim([freqs(1), freqs(end)]);
 #+end_src
 
-#+begin_src matlab
-#+end_src
-
 *** Flexible model + encoders fixed to the plates
 #+begin_src matlab
 %% Identify the IFF Plant (transfer function from u to taum)
@@ -1661,6 +1658,47 @@ exportFig('figs/dvf_plant_comp_struts_plates.pdf', 'width', 'wide', 'height', 't
 <<sec:enc_struts_iff>>
 *** Introduction                                                    :ignore:
 
+In this section, the Integral Force Feedback (IFF) control strategy is applied to the nano-hexapod.
+The main goal of this to add damping to the nano-hexapod's modes.
+
+The control architecture is shown in Figure [[fig:control_architecture_iff_struts]] where $\bm{K}_\text{IFF}$ is a diagonal $6 \times 6$ controller.
+
+The system as then a new input $\bm{u}^\prime$, and the transfer function from $\bm{u}^\prime$ to $d\bm{\mathcal{L}}_m$ should be easier to control than the initial transfer function from $\bm{u}$ to $d\bm{\mathcal{L}}_m$.
+
+#+begin_src latex :file control_architecture_iff_struts.pdf
+  \begin{tikzpicture}
+    % Blocs
+    \node[block={3.0cm}{2.0cm}] (P) {Plant};
+    \coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$);
+    \coordinate[] (outputF) at ($(P.south east)!0.7!(P.north east)$);
+    \coordinate[] (outputL) at ($(P.south east)!0.3!(P.north east)$);
+
+    \node[block, above=0.4 of P] (Kiff) {$\bm{K}_\text{IFF}$};
+    \node[addb, left= of inputF] (addF) {};
+
+    % Connections and labels
+    \draw[->] (outputF) -- ++(1, 0) node[below left]{$\bm{\tau}_m$};
+    \draw[->] (outputL) -- ++(1, 0) node[below left]{$d\bm{\mathcal{L}}_m$};
+
+    \draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east);
+    \draw[->] (Kiff.west) -| (addF.north);
+    \draw[->] (addF.east) -- (inputF) node[above left]{$\bm{u}$};
+    \draw[<-] (addF.west) -- ++(-1, 0) node[above right]{$\bm{u}^\prime$};
+  \end{tikzpicture}
+#+end_src
+
+#+name: fig:control_architecture_iff_struts
+#+caption: Integral Force Feedback Strategy
+#+RESULTS:
+[[file:figs/control_architecture_iff_struts.png]]
+
+This section is structured as follow:
+- Section [[sec:iff_struts_plant_id]]: Using the Simscape model (APA taken as 2DoF model), the transfer function from $\bm{u}$ to $\bm{\tau}_m$ is identified. Based on the obtained dynamics, the control law is developed and the optimal gain is estimated using the Root Locus.
+- Section [[sec:iff_struts_effect_plant]]: Still using the Simscape model, the effect of the IFF gain on the the transfer function from $\bm{u}^\prime$ to $d\bm{\mathcal{L}}_m$ is studied.
+- Section [[sec:iff_struts_effect_plant_exp]]: The same is performed experimentally: several IFF gains are used and the damped plant is identified each time.
+- Section [[sec:iff_struts_opt_gain]]: The damped model and the identified damped system are compared for the optimal IFF gain. It is found that IFF indeed adds a lot of damping into the system. However it is not efficient in damping the spurious struts modes.
+- Section [[sec:iff_struts_comp_flex_model]]: Finally, a "flexible" model of the APA is used in the Simscape model and the optimally damped model is compared with the measurements.
+
 *** 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>>
@@ -1715,7 +1753,10 @@ Rx = zeros(1, 7);
 open(mdl)
 #+end_src
 
-*** Identification of the IFF Plant
+*** IFF Control Law and Optimal Gain
+<<sec:iff_struts_plant_id>>
+
+Let's use a model of the Nano-Hexapod with the encoders fixed to the struts and the APA taken as 2DoF model.
 #+begin_src matlab
 %% Initialize Nano-Hexapod
 n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
@@ -1724,6 +1765,7 @@ n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
                                        'actuator_type', '2dof');
 #+end_src
 
+The transfer function from $\bm{u}$ to $\bm{\tau}_m$ is identified.
 #+begin_src matlab
 %% Identify the IFF Plant (transfer function from u to taum)
 clear io; io_i = 1;
@@ -1733,15 +1775,18 @@ io(io_i) = linio([mdl, '/dum'],  1, 'openoutput'); io_i = io_i + 1; % Force Sens
 Giff = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
 #+end_src
 
-*** Root Locus and Decentralized Loop gain
+The IFF controller is defined as shown below:
 #+begin_src matlab
 %% IFF Controller
-Kiff_g1 = -(1/(s + 2*pi*40))*... % Low pass filter (provides integral action above 40Hz)
+Kiff_g1 = -(1/(s + 2*pi*40))*...    % LPF: provides integral action above 40Hz
-           (s/(s + 2*pi*30))*... % High pass filter to limit low frequency gain
+           (s/(s + 2*pi*30))*...    % HPF: limit low frequency gain
-           (1/(1 + s/2/pi/500))*... % Low pass filter to be more robust to high frequency resonances
+           (1/(1 + s/2/pi/500))*... % LPF: more robust to high frequency resonances
-           eye(6); % Diagonal 6x6 controller
+           eye(6);                  % Diagonal 6x6 controller
 #+end_src
 
+Then, the poles of the system are shown in the complex plane as a function of the controller gain (i.e. Root Locus plot) in Figure [[fig:enc_struts_iff_root_locus]].
+A gain of $400$ is chosen as the "optimal" gain as it visually seems to be the gain that adds the maximum damping to all the suspension modes simultaneously.
+
 #+begin_src matlab :exports none
 %% Root Locus for IFF
 gains = logspace(1, 4, 100);
@@ -1784,17 +1829,20 @@ exportFig('figs/enc_struts_iff_root_locus.pdf', 'width', 'wide', 'height', 'tall
 Then the "optimal" IFF controller is:
 #+begin_src matlab
 %% IFF controller with Optimal gain
-Kiff = g*Kiff_g1;
+Kiff = 400*Kiff_g1;
 #+end_src
 
-#+begin_src matlab :tangle no
+And it is saved for further use.
+#+begin_src matlab :exports none :tangle no
 save('matlab/mat/Kiff.mat', 'Kiff')
 #+end_src
 
-#+begin_src matlab :exports none :eval no
+#+begin_src matlab :eval no
 save('mat/Kiff.mat', 'Kiff')
 #+end_src
 
+The bode plots of the "diagonal" elements of the loop gain are shown in Figure [[fig:enc_struts_iff_opt_loop_gain]].
+It is shown that the phase and gain margins are quite high and the loop gain is large arround the resonances.
 #+begin_src matlab :exports none
 %% Bode plot of the "decentralized loop gain"
 freqs = 2*logspace(1, 3, 1000);
@@ -1852,12 +1900,10 @@ exportFig('figs/enc_struts_iff_opt_loop_gain.pdf', 'width', 'wide', 'height', 't
 #+RESULTS:
 [[file:figs/enc_struts_iff_opt_loop_gain.png]]
 
-*** Multiple Gains - Simulation
+*** Effect of IFF on the plant - Simulations
-#+begin_src matlab
+<<sec:iff_struts_effect_plant>>
-%% Tested IFF gains
-iff_gains = [4, 10, 20, 40, 100, 200, 400];
-#+end_src
 
+Still using the Simscape model with encoders fixed to the struts and 2DoF APA, the IFF strategy is tested.
 #+begin_src matlab
 %% Initialize the Simscape model in closed loop
 n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
@@ -1867,13 +1913,20 @@ n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
                                        'controller_type', 'iff');
 #+end_src
 
+The following IFF gains are tried:
+#+begin_src matlab
+%% Tested IFF gains
+iff_gains = [4, 10, 20, 40, 100, 200, 400];
+#+end_src
+
+And the transfer functions from $\bm{u}^\prime$ to $d\bm{\mathcal{L}}_m$ are identified for all the IFF gains.
 #+begin_src matlab
 %% Identify the (damped) transfer function from u to dLm for different values of the IFF gain
 Gd_iff = {zeros(1, length(iff_gains))};
 
 clear io; io_i = 1;
-io(io_i) = linio([mdl, '/du'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
+io(io_i) = linio([mdl, '/du'], 1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
-io(io_i) = linio([mdl, '/D'],  1, 'openoutput'); io_i = io_i + 1; % Strut Displacement (encoder)
+io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Strut Displacement (encoder)
 
 for i = 1:length(iff_gains)
     Kiff = iff_gains(i)*Kiff_g1*eye(6); % IFF Controller
@@ -1883,6 +1936,7 @@ for i = 1:length(iff_gains)
 end
 #+end_src
 
+The obtained dynamics are shown in Figure [[fig:enc_struts_iff_gains_effect_dvf_plant]].
 #+begin_src matlab :exports none
 %% Bode plot of the transfer function from u to dLm for tested values of the IFF gain
 freqs = 2*logspace(1, 3, 1000);
@@ -1925,11 +1979,14 @@ exportFig('figs/enc_struts_iff_gains_effect_dvf_plant.pdf', 'width', 'wide', 'he
 #+RESULTS:
 [[file:figs/enc_struts_iff_gains_effect_dvf_plant.png]]
 
-*** Experimental Results - Gains
+*** Effect of IFF on the plant - Experimental Results
+<<sec:iff_struts_effect_plant_exp>>
+
 **** Introduction                                                  :ignore:
-Let's look at the damping introduced by IFF as a function of the IFF gain and compare that with the results obtained using the Simscape model.
+The IFF strategy is applied experimentally and the transfer function from $\bm{u}^\prime$ to $d\bm{\mathcal{L}}_m$ is identified for all the defined values of the gain.
 
 **** Load Data
+First load the identification data.
 #+begin_src matlab
 %% Load Identification Data
 meas_iff_gains = {};
@@ -1940,6 +1997,7 @@ end
 #+end_src
 
 **** Spectral Analysis - Setup
+And define the useful variables that will be used for the identification using the =tfestimate= function.
 #+begin_src matlab
 %% Setup useful variables
 % Sampling Time [s]
@@ -1956,6 +2014,7 @@ win = hanning(ceil(1*Fs));
 #+end_src
 
 **** DVF Plant
+The transfer functions are estimated for all the values of the gain.
 #+begin_src matlab
 %% DVF Plant (transfer function from u to dLm)
 G_iff_gains = {};
@@ -1965,6 +2024,8 @@ for i = 1:length(iff_gains)
 end
 #+end_src
 
+The obtained dynamics as shown in the bode plot in Figure [[fig:comp_iff_gains_dvf_plant]].
+The dashed curves are the results obtained using the model, and the solid curves the results from the experimental identification.
 #+begin_src matlab :exports none
 %% Bode plot of the transfer function from u to dLm for tested values of the IFF gain
 freqs = 2*logspace(1, 3, 1000);
@@ -1976,7 +2037,7 @@ ax1 = nexttile([2,1]);
 hold on;
 for i = 1:length(iff_gains)
     plot(f, abs(G_iff_gains{i}), '-', ...
-         'DisplayName', sprintf('$g_{iff} = %.0f$', iff_gains(i)));
+         'DisplayName', sprintf('$g = %.0f$', iff_gains(i)));
 end
 set(gca,'ColorOrderIndex',1)
 for i = 1:length(iff_gains)
@@ -2016,6 +2077,7 @@ exportFig('figs/comp_iff_gains_dvf_plant.pdf', 'width', 'wide', 'height', 'tall'
 #+RESULTS:
 [[file:figs/comp_iff_gains_dvf_plant.png]]
 
+The bode plot is then zoomed on the suspension modes of the nano-hexapod in Figure [[fig:comp_iff_gains_dvf_plant_zoom]].
 #+begin_src matlab :exports none
 xlim([20, 200]);
 #+end_src
@@ -2032,12 +2094,16 @@ exportFig('figs/comp_iff_gains_dvf_plant_zoom.pdf', 'width', 'wide', 'height', '
 #+begin_important
 The IFF control strategy is very effective for the damping of the suspension modes.
 It however does not damp the modes at 200Hz, 300Hz and 400Hz (flexible modes of the APA).
-This is very logical.
 
-Also, the experimental results and the models obtained from the Simscape model are in agreement.
+Also, the experimental results and the models obtained from the Simscape model are in agreement concerning the damped system (up to the flexible modes).
 #+end_important
 
 **** Experimental Results - Comparison of the un-damped and fully damped system
+The un-damped and damped experimental plants are compared in Figure [[fig:comp_undamped_opt_iff_gain_diagonal]] (diagonal terms).
+
+It is very clear that all the suspension modes are very well damped thanks to IFF.
+However, there is little to no effect on the flexible modes of the struts and of the plate.
+
 #+begin_src matlab :exports none
 %% Bode plot for the transfer function from u to dLm
 freqs = 2*logspace(1, 3, 1000);
@@ -2099,17 +2165,14 @@ exportFig('figs/comp_undamped_opt_iff_gain_diagonal.pdf', 'width', 'wide', 'heig
 #+RESULTS:
 [[file:figs/comp_undamped_opt_iff_gain_diagonal.png]]
 
-#+begin_question
-A series of modes at around 205Hz are also damped.
-
-Are these damped modes at 205Hz additional "suspension" modes or flexible modes of the struts?
-#+end_question
-
 *** Experimental Results - Damped Plant with Optimal gain
+<<sec:iff_struts_opt_gain>>
+
 **** Introduction                                                  :ignore:
 Let's now look at the $6 \times 6$ damped plant with the optimal gain $g = 400$.
 
 **** Load Data
+The experimental data are loaded.
 #+begin_src matlab
 %% Load Identification Data
 meas_iff_struts = {};
@@ -2120,6 +2183,7 @@ end
 #+end_src
 
 **** Spectral Analysis - Setup
+And the parameters useful for the spectral analysis are defined.
 #+begin_src matlab
 %% Setup useful variables
 % Sampling Time [s]
@@ -2136,6 +2200,7 @@ win = hanning(ceil(1*Fs));
 #+end_src
 
 **** DVF Plant
+Finally, the $6 \times 6$ plant is identified using the =tfestimate= function.
 #+begin_src matlab
 %% DVF Plant (transfer function from u to dLm)
 G_iff_opt = {};
@@ -2145,6 +2210,7 @@ for i = 1:6
 end
 #+end_src
 
+The obtained diagonal elements are compared with the model in Figure [[fig:damped_iff_plant_comp_diagonal]].
 #+begin_src matlab :exports none
 %% Bode plot for the transfer function from u to dLm
 freqs = 2*logspace(1, 3, 1000);
@@ -2204,6 +2270,7 @@ exportFig('figs/damped_iff_plant_comp_diagonal.pdf', 'width', 'wide', 'height',
 #+RESULTS:
 [[file:figs/damped_iff_plant_comp_diagonal.png]]
 
+And all the off-diagonal elements are compared with the model in Figure [[fig:damped_iff_plant_comp_off_diagonal]].
 #+begin_src matlab :exports none
 %% Bode plot for the transfer function from u to dLm
 freqs = 2*logspace(1, 3, 1000);
@@ -2278,13 +2345,203 @@ exportFig('figs/damped_iff_plant_comp_off_diagonal.pdf', 'width', 'wide', 'heigh
 
 #+begin_important
 With the IFF control strategy applied and the optimal gain used, the suspension modes are very well damped.
-Remains the undamped flexible modes of the APA (200Hz, 300Hz, 400Hz), and the modes of the plates (700Hz).
+Remains the un-damped flexible modes of the APA (200Hz, 300Hz, 400Hz), and the modes of the plates (700Hz).
 
 The Simscape model and the experimental results are in very good agreement.
 #+end_important
 
+*** Comparison with the Flexible model
+<<sec:iff_struts_comp_flex_model>>
+
+When using the 2-DoF model for the APA, the flexible modes of the struts were not modelled, and it was the main limitation of the model.
+Now, let's use a flexible model for the APA, and see if the obtained damped plant using the model is similar to the measured dynamics.
+
+First, the nano-hexapod is initialized.
+#+begin_src matlab
+%% Estimated misalignement of the struts
+d_aligns = [[-0.05,  -0.3,  0];
+            [ 0,      0.5,  0];
+            [-0.1,   -0.3,  0];
+            [ 0,      0.3,  0];
+            [-0.05,   0.05, 0];
+            [0,       0,    0]]*1e-3;
+
+
+%% Initialize Nano-Hexapod
+n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
+                                       'flex_top_type', '4dof', ...
+                                       'motion_sensor_type', 'struts', ...
+                                       'actuator_type', 'flexible', ...
+                                       'actuator_d_align', d_aligns, ...
+                                       'controller_type', 'iff');
+#+end_src
+
+And the "optimal" controller is loaded.
+#+begin_src matlab
+%% Optimal IFF controller
+load('Kiff.mat', 'Kiff');
+#+end_src
+
+The transfer function from $\bm{u}^\prime$ to $d\bm{\mathcal{L}}_m$ is identified using the Simscape model.
+#+begin_src matlab
+%% Linearized inputs/outputs
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/du'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
+io(io_i) = linio([mdl, '/dL'],  1, 'openoutput'); io_i = io_i + 1; % Strut Displacement (encoder)
+
+%% Identification of the plant
+Gd_iff = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
+#+end_src
+
+The obtained diagonal elements are shown in Figure [[fig:enc_struts_iff_opt_damp_comp_flex_model_diag]] while the off-diagonal elements are shown in Figure [[fig:enc_struts_iff_opt_damp_comp_flex_model_off_diag]].
+#+begin_src matlab :exports none
+%% Bode plot for the transfer function from u to dLm
+freqs = 2*logspace(1, 3, 1000);
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+% Diagonal Elements FRF
+plot(f, abs(G_iff_opt{1}(:,1)), 'color', [0,0,0,0.2], ...
+     'DisplayName', '$d\mathcal{L}_{m,i}/u^\prime_i$ - FRF')
+for i = 2:6
+    plot(f, abs(G_iff_opt{i}(:,i)), 'color', [0,0,0,0.2], ...
+         'HandleVisibility', 'off');
+end
+
+% Diagonal Elements Model
+set(gca,'ColorOrderIndex',2)
+plot(freqs, abs(squeeze(freqresp(Gd_iff(1,1), freqs, 'Hz'))), '-', ...
+     'DisplayName', '$d\mathcal{L}_{m,i}/u^\prime_i$ - Model')
+for i = 2:6
+    set(gca,'ColorOrderIndex',2)
+    plot(freqs, abs(squeeze(freqresp(Gd_iff(i,i), freqs, 'Hz'))), '-', ...
+         'HandleVisibility', 'off');
+end
+
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude $d\mathcal{L}_m/u^\prime$ [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-9, 1e-3]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
+
+ax2 = nexttile;
+hold on;
+for i =1:6
+    plot(f, 180/pi*angle(G_iff_opt{i}(:,i)), 'color', [0,0,0,0.2]);
+    set(gca,'ColorOrderIndex',2)
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gd_iff(i,i), freqs, 'Hz'))));
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+
+linkaxes([ax1,ax2],'x');
+xlim([20, 2e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/enc_struts_iff_opt_damp_comp_flex_model_diag.pdf', 'width', 'wide', 'height', 'tall');
+#+end_src
+
+#+name: fig:enc_struts_iff_opt_damp_comp_flex_model_diag
+#+caption: Diagonal elements of the transfer function from $\bm{u}^\prime$ to $d\bm{\mathcal{L}}_m$ - comparison of the measured FRF and the identified dynamics using the flexible model
+#+RESULTS:
+[[file:figs/enc_struts_iff_opt_damp_comp_flex_model_diag.png]]
+
+
+#+begin_src matlab :exports none
+%% Bode plot for the transfer function from u to dLm
+freqs = 2*logspace(1, 3, 1000);
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+% Off diagonal FRF
+plot(f, abs(G_iff_opt{1}(:,2)), 'color', [0,0,0,0.2], ...
+     'DisplayName', '$d\mathcal{L}_{m,i}/u^\prime_j$ - FRF')
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_iff_opt{i}(:,j)), 'color', [0, 0, 0, 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+
+% Off diagonal Model
+set(gca,'ColorOrderIndex',2)
+plot(freqs, abs(squeeze(freqresp(Gd_iff(1,2), freqs, 'Hz'))), '-', ...
+     'DisplayName', '$d\mathcal{L}_{m,i}/u^\prime_j$ - Model')
+for i = 1:5
+    for j = i+1:6
+        set(gca,'ColorOrderIndex',2)
+        plot(freqs, abs(squeeze(freqresp(Gd_iff(i,j), freqs, 'Hz'))), ...
+             'HandleVisibility', 'off');
+    end
+end
+
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude $d\mathcal{L}_m/u^\prime$ [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-9, 1e-3]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
+
+ax2 = nexttile;
+hold on;
+% Off diagonal FRF
+for i = 1:5
+    for j = i+1:6
+        plot(f, 180/pi*angle(G_iff_opt{i}(:,j)), 'color', [0, 0, 0, 0.2]);
+    end
+end
+
+% Off diagonal Model
+for i = 1:5
+    for j = i+1:6
+        set(gca,'ColorOrderIndex',2)
+        plot(freqs, 180/pi*angle(squeeze(freqresp(Gd_iff(i,j), freqs, 'Hz'))));
+    end
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+
+linkaxes([ax1,ax2],'x');
+xlim([20, 2e3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/enc_struts_iff_opt_damp_comp_flex_model_off_diag.pdf', 'width', 'wide', 'height', 'tall');
+#+end_src
+
+#+name: fig:enc_struts_iff_opt_damp_comp_flex_model_off_diag
+#+caption: Off-diagonal elements of the transfer function from $\bm{u}^\prime$ to $d\bm{\mathcal{L}}_m$ - comparison of the measured FRF and the identified dynamics using the flexible model
+#+RESULTS:
+[[file:figs/enc_struts_iff_opt_damp_comp_flex_model_off_diag.png]]
+
+#+begin_important
+Using flexible models for the APA, the agreement between the Simscape model of the nano-hexapod and the measured FRF is very good.
+
+Only the flexible mode of the top-plate is not appearing in the model which is very logical as the top plate is taken as a solid body.
+#+end_important
+
+*** Conclusion
+#+begin_important
+The decentralized Integral Force Feedback strategy applied on the nano-hexapod is very effective in damping all the suspension modes.
+
+The Simscape model (especially when using a flexible model for the APA) is shown to be very accurate, even when IFF is applied.
+#+end_important
+
 ** Modal Analysis
 <<sec:enc_struts_modal_analysis>>
+
 *** Introduction                                                    :ignore:
 Several 3-axis accelerometers are fixed on the top platform of the nano-hexapod as shown in Figure [[fig:compliance_vertical_comp_iff]].
 
@@ -2300,6 +2557,10 @@ The top platform is then excited using an instrumented hammer as shown in Figure
 #+attr_latex: :width \linewidth
 [[file:figs/hammer_excitation_compliance_meas.jpg]]
 
+From this experiment, the resonance frequencies and the associated mode shapes can be computed (Section [[sec:modal_analysis_mode_shapes]]).
+Then, in Section [[sec:compliance_effect_iff]], the vertical compliance of the nano-hexapod is experimentally estimated.
+Finally, in Section [[sec:compliance_effect_iff_comp_model]], the measured compliance is compare with the estimated one from the Simscape model.
+
 *** 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>>
@@ -2350,7 +2611,42 @@ Rx = zeros(1, 7);
 open(mdl)
 #+end_src
 
-*** Effectiveness of the IFF Strategy - Compliance
+*** Obtained Mode Shapes
+<<sec:modal_analysis_mode_shapes>>
+
+We can observe the mode shapes of the first 6 modes that are the suspension modes (the plate is behaving as a solid body) in Figure [[fig:mode_shapes_annotated]].
+
+#+name: fig:mode_shapes_annotated
+#+caption: Measured mode shapes for the first six modes
+#+attr_latex: :width \linewidth
+[[file:figs/mode_shapes_annotated.gif]]
+
+Then, there is a mode at 692Hz which corresponds to a flexible mode of the top plate (Figure [[fig:mode_shapes_flexible_annotated]]).
+
+#+name: fig:mode_shapes_flexible_annotated
+#+caption: First flexible mode at 692Hz
+#+attr_latex: :width 0.3\linewidth
+[[file:figs/ModeShapeFlex1_crop.gif]]
+
+The obtained modes are summarized in Table [[tab:description_modes]].
+
+#+name: tab:description_modes
+#+caption: Description of the identified modes
+#+attr_latex: :environment tabularx :width 0.7\linewidth :align ccX
+#+attr_latex: :center t :booktabs t :float t
+| Mode | Freq. [Hz] | Description                                  |
+|------+------------+----------------------------------------------|
+|    1 |        105 | Suspension Mode: Y-translation               |
+|    2 |        107 | Suspension Mode: X-translation               |
+|    3 |        131 | Suspension Mode: Z-translation               |
+|    4 |        161 | Suspension Mode: Y-tilt                      |
+|    5 |        162 | Suspension Mode: X-tilt                      |
+|    6 |        180 | Suspension Mode: Z-rotation                  |
+|    7 |        692 | (flexible) Membrane mode of the top platform |
+
+*** Nano-Hexapod Compliance - Effect of IFF
+<<sec:compliance_effect_iff>>
+
 In this section, we wish to estimated the effectiveness of the IFF strategy concerning the compliance.
 
 The top plate is excited vertically using the instrumented hammer two times:
@@ -2371,7 +2667,6 @@ d_frf_iff = 10/5*(frf_iff.FFT1_H1_4_1_RMS_Y_Mod + frf_iff.FFT1_H1_7_1_RMS_Y_Mod
 #+end_src
 
 The vertical compliance (magnitude of the transfer function from a vertical force applied on the top plate to the vertical motion of the top plate) is shown in Figure [[fig:compliance_vertical_comp_iff]].
-
 #+begin_src matlab :exports none
 figure;
 hold on;
@@ -2394,13 +2689,15 @@ exportFig('figs/compliance_vertical_comp_iff.pdf', 'width', 'wide', 'height', 'n
 [[file:figs/compliance_vertical_comp_iff.png]]
 
 #+begin_important
-From Figure [[fig:compliance_vertical_comp_iff]], it is clear that the IFF control strategy is very effective in damping the suspensions modes of the nano-hexapode.
+From Figure [[fig:compliance_vertical_comp_iff]], it is clear that the IFF control strategy is very effective in damping the suspensions modes of the nano-hexapod.
-It also has the effect of degrading (slightly) the vertical compliance at low frequency.
+It also has the effect of (slightly) degrading the vertical compliance at low frequency.
 
 It also seems some damping can be added to the modes at around 205Hz which are flexible modes of the struts.
 #+end_important
 
 *** Comparison with the Simscape Model
+<<sec:compliance_effect_iff_comp_model>>
+
 Let's now compare the measured vertical compliance with the vertical compliance as estimated from the Simscape model.
 
 The transfer function from a vertical external force to the absolute motion of the top platform is identified (with and without IFF) using the Simscape model.
@@ -2462,40 +2759,7 @@ exportFig('figs/compliance_vertical_comp_model_iff.pdf', 'width', 'wide', 'heigh
 #+RESULTS:
 [[file:figs/compliance_vertical_comp_model_iff.png]]
 
-*** Obtained Mode Shapes
+** TODO Accelerometers fixed on the top platform                   :noexport:
-Then, several excitation are performed using the instrumented Hammer and the mode shapes are extracted.
-
-We can observe the mode shapes of the first 6 modes that are the suspension modes (the plate is behaving as a solid body) in Figure [[fig:mode_shapes_annotated]].
-
-#+name: fig:mode_shapes_annotated
-#+caption: Measured mode shapes for the first six modes
-#+attr_latex: :width \linewidth
-[[file:figs/mode_shapes_annotated.gif]]
-
-Then, there is a mode at 692Hz which corresponds to a flexible mode of the top plate (Figure [[fig:mode_shapes_flexible_annotated]]).
-
-#+name: fig:mode_shapes_flexible_annotated
-#+caption: First flexible mode at 692Hz
-#+attr_latex: :width 0.3\linewidth
-[[file:figs/ModeShapeFlex1_crop.gif]]
-
-The obtained modes are summarized in Table [[tab:description_modes]].
-
-#+name: tab:description_modes
-#+caption: Description of the identified modes
-#+attr_latex: :environment tabularx :width 0.7\linewidth :align ccX
-#+attr_latex: :center t :booktabs t :float t
-| Mode | Freq. [Hz] | Description                                  |
-|------+------------+----------------------------------------------|
-|    1 |        105 | Suspension Mode: Y-translation               |
-|    2 |        107 | Suspension Mode: X-translation               |
-|    3 |        131 | Suspension Mode: Z-translation               |
-|    4 |        161 | Suspension Mode: Y-tilt                      |
-|    5 |        162 | Suspension Mode: X-tilt                      |
-|    6 |        180 | Suspension Mode: Z-rotation                  |
-|    7 |        692 | (flexible) Membrane mode of the top platform |
-
-** Accelerometers fixed on the top platform
 *** Introduction                                                    :ignore:
 
 #+name: fig:acc_top_plat_top_view
@@ -2839,7 +3103,18 @@ xlim([50, 5e2]); ylim([1e-7, 1e-1]);
 legend('location', 'southwest');
 #+end_src
 
-* Encoders fixed to the plates
+** Conclusion
+#+begin_important
+From the previous analysis, several conclusions can be drawn:
+- Decentralized IFF is very effective in damping the "suspension" modes of the nano-hexapod (Figure [[fig:comp_undamped_opt_iff_gain_diagonal]])
+- Decentralized IFF does not damp the "spurious" modes of the struts nor the flexible modes of the top plate (Figure [[fig:comp_undamped_opt_iff_gain_diagonal]])
+- Even though the Simscape model and the experimentally measured FRF are in good agreement (Figures [[fig:enc_struts_iff_opt_damp_comp_flex_model_diag]] and [[fig:enc_struts_iff_opt_damp_comp_flex_model_off_diag]]), the obtain dynamics from the control inputs $\bm{u}$ and the encoders $d\bm{\mathcal{L}}_m$ is very difficult to control
+
+Therefore, in the following sections, the encoders will be fixed to the plates.
+The goal is to be less sensitive to the flexible modes of the struts.
+#+end_important
+
+* Encoders fixed to the plates - Dynamics
 <<sec:encoders_plates>>
 
 ** Introduction                                                      :ignore:
@@ -3587,7 +3862,6 @@ exportFig('figs/enc_plates_dvf_comp_offdiag_simscape.pdf', 'width', 'wide', 'hei
 <<sec:enc_plates_iff>>
 *** Introduction                                                    :ignore:
 
-
 #+begin_src latex :file control_architecture_iff.pdf
   \begin{tikzpicture}
     % Blocs
@@ -4172,6 +4446,11 @@ save('matlab/mat/damped_plant_enc_plates.mat', 'f', 'Ts', 'G_enc_iff_opt')
 save('mat/damped_plant_enc_plates.mat', 'f', 'Ts', 'G_enc_iff_opt')
 #+end_src
 
+** Conclusion
+* HAC-IFF
+<<sec:hac_iff>>
+
+** Introduction                                                      :ignore:
 ** HAC Control - Frame of the struts
 <<sec:hac_iff_struts>>
 *** Introduction                                                    :ignore:
@@ -4530,6 +4809,78 @@ isstable(Gd_iff_hac_opt)
 #+RESULTS:
 : 1
 
+*** Experimental Measurements
+#+begin_src matlab
+load('hac_iff_more_lead_huddle.mat', 't', 'Va', 'Vs', 'de')
+#+end_src
+
+#+begin_src matlab
+rms(de)
+#+end_src
+
+#+begin_src matlab
+Xe = [inv(n_hexapod.geometry.J)*de']';
+#+end_src
+
+#+begin_src matlab
+figure;
+plot3(Xe(:,1), Xe(:,2), Xe(:,3))
+#+end_src
+
+** HAC Control - Cartesian Frame
+#+begin_src matlab
+load('damped_plant_enc_plates.mat', 'f', 'Ts', 'G_enc_iff_opt')
+load('jacobian.mat', 'J');
+#+end_src
+
+From the transfer function from $\bm{\tau}$ to $d\bm{\mathcal{L}}_m$, let's compute the transfer function from $\bm{\mathcal{F}}$ to $d\bm{\mathcal{X}}_m$.
+#+begin_src matlab
+G_dvf_J = permute(pagemtimes(inv(J), pagemtimes(permute(G_enc_iff_opt, [2 3 1]), inv(J'))), [3 1 2]);
+#+end_src
+
+#+begin_src matlab :exports none
+labels = {'$D_x/F_{x}$', '$D_y/F_{y}$', '$D_z/F_{z}$', '$R_{x}/M_{x}$', '$R_{y}/M_{y}$', '$R_{R}/M_{z}$'};
+
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+for i = 1:5
+    for j = i+1:6
+        plot(f, abs(G_dvf_J(:, i, j)), 'color', [0, 0, 0, 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+for i =1:6
+    set(gca,'ColorOrderIndex',i)
+    plot(f, abs(G_dvf_J(:,i , i)), ...
+         'DisplayName', labels{i});
+end
+plot(f, abs(G_dvf_J(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
+     'DisplayName', '$D_i/F_j$');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
+ylim([1e-7, 1e-1]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
+
+ax2 = nexttile;
+hold on;
+for i =1:6
+    set(gca,'ColorOrderIndex',i)
+    plot(f, 180/pi*angle(G_dvf_J(:,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);
+
+linkaxes([ax1,ax2],'x');
+xlim([20, 2e3]);
+#+end_src
+
 ** Reference Tracking
 <<sec:hac_iff_struts_ref_track>>
 *** Introduction                                                    :ignore:
@@ -4875,10 +5226,91 @@ save('matlab/mat/reference_path.mat', 'Rx_yz', 'Rx_nass')
 save('mat/reference_path.mat', 'Rx_yz', 'Rx_nass')
 #+end_src
 
-*** Experimental Results
+*** Experimental Measurements
+#+begin_src matlab
+load('hac_iff_more_lead_nass_scan.mat', 't', 'Va', 'Vs', 'de')
+t = t - t(1);
+#+end_src
 
-** Feedforward (Open-Loop) Control
+#+begin_src matlab
-*** Introduction
+load('reference_path.mat', 'Rx_yz', 'Rx_nass')
+#+end_src
+
+#+begin_src matlab
+Xe = [inv(n_hexapod.geometry.J)*de']';
+#+end_src
+
+#+begin_src matlab
+figure;
+hold on;
+plot3(Xe(:,1), Xe(:,2), Xe(:,3))
+plot3(Rx_nass(:,2), Rx_nass(:,3), Rx_nass(:,4))
+hold off;
+#+end_src
+
+#+begin_src matlab
+i_top = Xe(:,3) > 1.9e-6;
+i_rx = Rx_nass(:,4) > 0;
+#+end_src
+
+#+begin_src matlab :exports none
+figure;
+hold on;
+scatter(1e6*Xe(i_top,1),  1e6*Xe(i_top,2),'o','MarkerEdgeAlpha',0.2);
+plot(1e6*Rx_nass(i_rx,2), 1e6*Rx_nass(i_rx,3), '--');
+hold off;
+xlabel('X [$\mu m$]'); ylabel('Y [$\mu m$]');
+axis equal;
+xlim([-10.5, 10.5]); ylim([-4.5, 4.5]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/ref_track_nass_exp_hac_iff_struts.pdf', 'width', 'wide', 'height', 'normal');
+#+end_src
+
+#+name: fig:ref_track_nass_exp_hac_iff_struts
+#+caption: XY trajectory
+#+RESULTS:
+[[file:figs/ref_track_nass_exp_hac_iff_struts.png]]
+
+#+begin_src matlab :exports none
+axis equal;
+xlim([4.5, 4.7]); ylim([-0.15, 0.05]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/ref_track_nass_exp_hac_iff_struts_zoom.pdf', 'width', 'normal', 'height', 'tall');
+#+end_src
+
+#+name: fig:ref_track_nass_exp_hac_iff_struts_zoom
+#+caption:
+#+RESULTS:
+[[file:figs/ref_track_nass_exp_hac_iff_struts_zoom.png]]
+
+
+Positioning Errors:
+#+begin_src matlab
+
+#+end_src
+
+#+begin_src matlab
+figure;
+hold on;
+plot(t, 1e6*Xe(:,4), '-', 'DisplayName', '$\epsilon_{\theta_x}$');
+plot(t, 1e6*Xe(:,5), '-', 'DisplayName', '$\epsilon_{\theta_y}$');
+plot(t, 1e6*Xe(:,6), '-', 'DisplayName', '$\epsilon_{\theta_z}$');
+hold off;
+xlabel('Time [s]'); ylabel('Z error [$\mu$ rad]');
+legend('location', 'northeast');
+#+end_src
+
+** Huddle test
+- [ ] Compare signals without control and with control but no reference tracking
+
+* Feedforward Control
+<<sec:feedforward>>
+
+** Introduction                                                      :ignore:
 
 #+begin_src latex :file control_architecture_iff_feedforward.pdf
 \begin{tikzpicture}
@@ -4914,7 +5346,11 @@ save('mat/reference_path.mat', 'Rx_yz', 'Rx_nass')
 #+RESULTS:
 [[file:figs/control_architecture_iff_feedforward.png]]
 
-*** Matlab Init                                            :noexport:ignore:
+Main problems:
+- Non-linearity: Creep, Hysteresis
+- Variability of the plant
+
+** 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
@@ -4970,7 +5406,7 @@ Rx = zeros(1, 7);
 colors = colororder;
 #+end_src
 
-*** Simple Feedforward Controller
+** Simple Feedforward Controller
 Let's estimate the mean DC gain for the damped plant (diagonal elements:)
 #+begin_src matlab :results value replace :exports results :tangle no
 mean(diag(abs(squeeze(mean(G_enc_iff_opt(f>2 & f<4,:,:))))))
@@ -5041,11 +5477,13 @@ save('matlab/mat/feedforward_iff.mat', 'Kff_iff_L')
 save('mat/feedforward_iff.mat', 'Kff_iff_L')
 #+end_src
 
-*** Test with Simscape Model
+** Test with Simscape Model
 #+begin_src matlab
 load('reference_path.mat', 'Rx_yz');
 #+end_src
 
+
+* Further work                                                      :noexport:
 ** Feedback/Feedforward control in the frame of the struts
 *** Introduction                                                    :ignore:
 
@@ -5092,6 +5530,7 @@ load('reference_path.mat', 'Rx_yz');
 [[file:figs/control_architecture_hac_iff_L_feedforward.png]]
 
 
+
 * Functions
 ** =generateXYZTrajectory=
 :PROPERTIES: