diff --git a/matlab/nass_model.slx b/matlab/nass_model.slx
index d6c1287..1282eb7 100644
Binary files a/matlab/nass_model.slx and b/matlab/nass_model.slx differ
diff --git a/org/optimal_stiffness.org b/org/optimal_stiffness.org
index 9927409..37c1f10 100644
--- a/org/optimal_stiffness.org
+++ b/org/optimal_stiffness.org
@@ -51,18 +51,21 @@ We wish here to see if we can determine an optimal stiffness of the nano-hexapod
 - Section [[sec:micro_station_compliance]]: the support compliance dynamics is not much present in the nano-hexapod dynamics
 - Section [[sec:payload_impedance]]: the change of payload impedance has acceptable effect on the plant dynamics
 
+The overall goal is to design a nano-hexapod that will allow the highest possible control bandwidth.
+
 * Spindle Rotation Speed
 <<sec:spindle_rotation_speed>>
 
 ** 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>>
+  <<matlab-dir>>
 #+end_src
 
 #+begin_src matlab :exports none :results silent :noweb yes
-<<matlab-init>>
+  <<matlab-init>>
 #+end_src
 
 #+begin_src matlab :tangle no
@@ -73,7 +76,318 @@ We wish here to see if we can determine an optimal stiffness of the nano-hexapod
   open('nass_model.slx')
 #+end_src
 
+** Initialization
+We initialize all the stages with the default parameters.
+#+begin_src matlab
+  initializeGround();
+  initializeGranite();
+  initializeTy();
+  initializeRy();
+  initializeRz();
+  initializeMicroHexapod();
+  initializeAxisc();
+  initializeMirror();
+#+end_src
+
+The worst case scenario is a rotation speed of 60rpm with a payload mass of 1Kg.
+#+begin_src matlab
+  initializeSample('mass', 10);
+#+end_src
+
+We don't include gravity nor disturbances in this model as it adds complexity to the simulations and does not alter the obtained dynamics.
+#+begin_src matlab
+  initializeSimscapeConfiguration('gravity', false);
+  initializeDisturbances('enable', false);
+#+end_src
+
+We set the controller type to Open-Loop, and we do not need to log any signal.
+#+begin_src matlab
+  initializeController('type', 'stabilizing');
+  initializeLoggingConfiguration('log', 'none');
+#+end_src
+
+#+begin_src matlab :exports none
+  %% Name of the Simulink File
+  mdl = 'nass_model';
+
+  %% Input/Output definition
+  clear io; io_i = 1;
+  io(io_i) = linio([mdl, '/Controller'],     1, 'openinput');              io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/Micro-Station'],  3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/Micro-Station'],  3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'En');   io_i = io_i + 1;
+#+end_src
+
+** Identification when not rotating
+We set the range of stiffness that we want to use.
+#+begin_src matlab
+  Ks = logspace(3,9,7)
+#+end_src
+
+#+begin_src matlab
+  initializeReferences();
+#+end_src
+
+#+begin_src matlab
+  Gk_iff = {zeros(length(Ks))};
+  Gk_dvf = {zeros(length(Ks))};
+  Gk_err = {zeros(length(Ks))};
+#+end_src
+
+#+begin_src matlab
+  for i = 1:length(Ks)
+    initializeNanoHexapod('k', Ks(i));
+
+    %% Run the linearization
+    G = linearize(mdl, io);
+    G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
+    G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
+                    'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
+                    'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
+
+    Gk_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
+    Gk_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
+
+    Jinvt = tf(inv(nano_hexapod.J)');
+    Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
+    Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
+    Gk_err(i) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'},                {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt};
+  end
+#+end_src
+
+** Identification when rotating at maximum speed
+#+begin_src matlab
+  Rz_rpm = 60;
+
+  initializeReferences('Rz_type', 'rotating', ...
+                       'Rz_period', 60/Rz_rpm, ... % Rotation period [s]
+                       'Rz_amplitude', -0.2*(2*pi*Rz_rpm/60)); % Angle offset [rad]
+
+  load('mat/nass_references.mat', 'Rz'); % We load the reference for the Spindle
+  [~, i_end] = min(abs(Rz.signals.values)); % Obtain the indice where the spindle angle is zero
+  t_sim = Rz.time(i_end); % Simulation time before identification [s]
+#+end_src
+
+#+begin_src matlab
+  k_sta = -1e8;
+#+end_src
+
+#+begin_src matlab
+  Gk_wz_iff = {zeros(length(Ks))};
+  Gk_wz_dvf = {zeros(length(Ks))};
+  Gk_wz_err = {zeros(length(Ks))};
+#+end_src
+
+#+begin_src matlab
+  for i = 1:length(Ks)
+    initializeNanoHexapod('k', Ks(i));
+
+    %% Run the linearization
+    G = linearize(mdl, io, t_sim);
+    G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
+    G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
+                    'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
+                    'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
+
+    Gk_wz_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
+    Gk_wz_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
+
+    Jinvt = tf(inv(nano_hexapod.J)');
+    Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
+    Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
+    Gk_wz_err(i) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'},                {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt};
+  end
+#+end_src
+
+#+begin_src matlab
+  save('mat/optimal_stiffness_Gk_wz.mat', 'Ks', ...
+       'Gk_iff',    'Gk_dvf',    'Gk_err', ...
+       'Gk_wz_iff', 'Gk_wz_dvf', 'Gk_wz_err');
+#+end_src
+
+** Change of dynamics
+#+begin_src matlab :exports none
+  load('mat/optimal_stiffness_Gk_wz.mat');
+#+end_src
+
+Change of dynamics for decentralized IFF control.
+#+begin_src matlab :exports none
+  freqs = logspace(-1, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  for i = 1:length(Gk_iff)
+    set(gca,'ColorOrderIndex',i);
+    plot(freqs, abs(squeeze(freqresp(Gk_iff{i}(   'Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-');
+    set(gca,'ColorOrderIndex',i);
+    plot(freqs, abs(squeeze(freqresp(Gk_wz_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '--');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
+  title('Soft Nano-Hexapod');
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  for i = 1:length(Gk_iff)
+    set(gca,'ColorOrderIndex',i);
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-', ...
+         'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
+    set(gca,'ColorOrderIndex',i);
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_wz_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '--', ...
+         'HandleVisibility', 'off');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+  legend('location', 'northeast');
+
+  linkaxes([ax1,ax2],'x');
+  xlim([freqs(1), freqs(end)]);
+#+end_src
+
+Change of dynamics from $F_x$ to $D_x$.
+#+begin_src matlab :exports none
+  freqs = logspace(-1, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  for i = 1:length(Gk_err)
+    set(gca,'ColorOrderIndex',i);
+    plot(freqs, abs(squeeze(freqresp(Gk_err{i}(   'Ex', 'Fx'), freqs, 'Hz'))), '-');
+    set(gca,'ColorOrderIndex',i);
+    plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '--');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  for i = 1:length(Gk_err)
+    set(gca,'ColorOrderIndex',i);
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
+         'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
+    set(gca,'ColorOrderIndex',i);
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_wz_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '--', ...
+         'HandleVisibility', 'off');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+  legend('location', 'northeast');
+
+  linkaxes([ax1,ax2],'x');
+  xlim([freqs(1), freqs(end)]);
+#+end_src
+
+Change of dynamics from $F_z$ to $D_z$.
+#+begin_src matlab :exports none
+  freqs = logspace(-1, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  for i = 1:length(Gk_err)
+    set(gca,'ColorOrderIndex',i);
+    plot(freqs, abs(squeeze(freqresp(Gk_err{i}(   'Ez', 'Fz'), freqs, 'Hz'))), '-');
+    set(gca,'ColorOrderIndex',i);
+    plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '--');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+  title('Soft Nano-Hexapod');
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  for i = 1:length(Gk_err)
+    set(gca,'ColorOrderIndex',i);
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '-', ...
+         'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
+    set(gca,'ColorOrderIndex',i);
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_wz_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '--', ...
+         'HandleVisibility', 'off');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+  legend('location', 'northeast');
+
+  linkaxes([ax1,ax2],'x');
+  xlim([freqs(1), freqs(end)]);
+#+end_src
+
+** Change of coupling
+#+begin_src matlab :exports none
+  load('mat/optimal_stiffness_Gk_wz.mat');
+#+end_src
+
+Change of coupling from $F_x$ to $D_y$ when not rotating and when rotating at 60rpm.
+#+begin_src matlab :exports none
+  freqs = logspace(-1, 3, 1000);
+
+  figure;
+
+  hold on;
+  for i = 1:length(Gk_err)
+    set(gca,'ColorOrderIndex',i);
+    plot(freqs, abs(squeeze(freqresp(Gk_err{i}(   'Ey', 'Fx'), freqs, 'Hz'))), '-', ...
+         'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
+    set(gca,'ColorOrderIndex',i);
+    plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i}('Ey', 'Fx'), freqs, 'Hz'))), '--', ...
+         'HandleVisibility', 'off');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+  xlim([freqs(1), freqs(end)]);
+  legend('location', 'northeast');
+#+end_src
+
+Comparison of the coupling from $F_x$ to $D_y$ when rotating at 60rpm to the direct term $F_x$ to $D_x$.
+#+begin_src matlab :exports none
+  freqs = logspace(-1, 3, 1000);
+
+  figure;
+
+  hold on;
+  for i = 1:length(Gk_err)
+    set(gca,'ColorOrderIndex',i);
+    plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
+         'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
+    set(gca,'ColorOrderIndex',i);
+    plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i}('Ey', 'Fx'), freqs, 'Hz'))), '--', ...
+         'HandleVisibility', 'off');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+  xlim([freqs(1), freqs(end)]);
+  legend('location', 'northeast');
+#+end_src
+
 ** Conclusion                                                        :ignore:
+#+begin_important
+  The leg stiffness should be at higher than $k_i = 10^4\ [N/m]$ such that the main resonance frequency does not shift too much when rotating.
+  For the coupling, it is more difficult to conclude about the minimum required leg stiffness.
+#+end_important
+
+#+begin_notes
+  Note that we can use very soft nano-hexapod if we limit the spindle rotating speed.
+#+end_notes
+
 * Micro-Station Compliance Effect
 <<sec:micro_station_compliance>>
 
@@ -121,5 +435,632 @@ We wish here to see if we can determine an optimal stiffness of the nano-hexapod
   open('nass_model.slx')
 #+end_src
 
+** Initialization
+We initialize all the stages with the default parameters.
+#+begin_src matlab
+  initializeGround();
+  initializeGranite();
+  initializeTy();
+  initializeRy();
+  initializeRz();
+  initializeMicroHexapod();
+  initializeAxisc();
+  initializeMirror();
+#+end_src
+
+We don't include gravity nor disturbances in this model as it adds complexity to the simulations and does not alter the obtained dynamics.
+#+begin_src matlab
+  initializeSimscapeConfiguration('gravity', false);
+  initializeDisturbances('enable', false);
+#+end_src
+
+We set the controller type to Open-Loop, and we do not need to log any signal.
+#+begin_src matlab
+  initializeController('type', 'stabilizing');
+  initializeLoggingConfiguration('log', 'none');
+#+end_src
+
+#+begin_src matlab :exports none
+  %% Name of the Simulink File
+  mdl = 'nass_model';
+
+  %% Input/Output definition
+  clear io; io_i = 1;
+  io(io_i) = linio([mdl, '/Controller'],     1, 'openinput');              io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/Micro-Station'],  3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/Micro-Station'],  3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1;
+  io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'En');   io_i = io_i + 1;
+#+end_src
+
+** Change of payload dynamics
+- Change of mass: from 1kg to 50kg
+- Change of resonance frequency: from 50Hz to 500Hz
+- The damping ratio of the payload is fixed to $\xi = 0.2$
+
+#+begin_src matlab
+  initializeReferences();
+  Ks = logspace(3,9,7) % [N/m]
+#+end_src
+
+#+begin_src matlab
+  Ms = [1, 20, 50]; % [Kg]
+#+end_src
+
+#+begin_src matlab
+  Gm_iff = {zeros(length(Ks), length(Ms))};
+  Gm_dvf = {zeros(length(Ks), length(Ms))};
+  Gm_err = {zeros(length(Ks), length(Ms))};
+#+end_src
+
+#+begin_src matlab
+  for i = 1:length(Ks)
+    for j = 1:length(Ms)
+      initializeNanoHexapod('k', Ks(i));
+      initializeSample('mass', Ms(j), 'freq', 100*ones(6,1));
+
+      G = linearize(mdl, io);
+      G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
+      G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
+                      'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
+                      'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
+
+      Gm_iff(i,j) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
+      Gm_dvf(i,j) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
+
+      Jinvt = tf(inv(nano_hexapod.J)');
+      Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
+      Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
+      Gm_err(i,j) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'},                {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt};
+    end
+  end
+#+end_src
+
+#+begin_src matlab
+  Fs = [50, 200, 500]; % [Hz]
+#+end_src
+
+#+begin_src matlab
+  Gf_iff = {zeros(length(Ks), length(Fs))};
+  Gf_dvf = {zeros(length(Ks), length(Fs))};
+  Gf_err = {zeros(length(Ks), length(Fs))};
+#+end_src
+
+#+begin_src matlab
+  for i = 1:length(Ks)
+    for j = 1:length(Fs)
+      initializeNanoHexapod('k', Ks(i));
+      initializeSample('mass', 20, 'freq', Fs(j)*ones(6,1));
+
+      G = linearize(mdl, io);
+      G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
+      G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
+                      'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
+                      'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
+
+      Gf_iff(i,j) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
+      Gf_dvf(i,j) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
+
+      Jinvt = tf(inv(nano_hexapod.J)');
+      Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
+      Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
+      Gf_err(i,j) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'},                {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt};
+    end
+  end
+#+end_src
+
+#+begin_src matlab
+  save('mat/optimal_stiffness_Gm_Gf.mat', 'Ks', 'Ms', 'Fs', ...
+       'Gm_iff',    'Gm_dvf',    'Gm_err', ...
+       'Gf_iff',    'Gf_dvf',    'Gf_err');
+#+end_src
+
+** Plots
+** Change of optimal gain for decentralized control
+For each payload, compute the optimal gain for the IFF control.
+The optimal value corresponds to critical damping to *all* the 6 modes of the nano-hexapod.
+
+#+begin_src matlab
+  load('mat/optimal_stiffness_Gm_Gf.mat');
+#+end_src
+
+Change of Mass
+#+begin_src matlab :exports none
+  freqs = logspace(-1, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  for i = 1:length(Ks)
+    for j = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, abs(squeeze(freqresp(Gm_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-');
+    end
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  for i = 1:length(Ks)
+    for j = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
+      if j == 1
+        plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-', ...
+            'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i)));
+      else
+        plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-', ...
+             'HandleVisibility', 'off');
+      end
+    end
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+  legend('location', 'northeast');
+
+  linkaxes([ax1,ax2],'x');
+  xlim([freqs(1), freqs(end)]);
+#+end_src
+
+Change of payload resonance frequency
+#+begin_src matlab :exports none
+  freqs = logspace(-1, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  for i = 1:length(Ks)
+    for j = 1:length(Fs)
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, abs(squeeze(freqresp(Gf_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-');
+    end
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  for i = 1:length(Ks)
+    for j = 1:length(Fs)
+      set(gca,'ColorOrderIndex',i);
+      if j == 1
+        plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-', ...
+            'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i)));
+      else
+        plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-', ...
+             'HandleVisibility', 'off');
+      end
+    end
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+  legend('location', 'northeast');
+
+  linkaxes([ax1,ax2],'x');
+  xlim([freqs(1), freqs(end)]);
+#+end_src
+
+** Change of dynamics for the primary controller
+For each stiffness, plot the total spread of dynamics.
+
+#+begin_src matlab
+  load('mat/optimal_stiffness_Gm_Gf.mat');
+#+end_src
+
+*** Frequency variation
+Same payload mass, but different stiffness resulting in different resonance frequency.
+
+All curves
+#+begin_src matlab :exports none
+  freqs = logspace(-1, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  for i = 1:length(Ks)
+    for j = 1:length(Fs)
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-');
+    end
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  for i = 1:length(Ks)
+    for j = 1:length(Fs)
+      set(gca,'ColorOrderIndex',i);
+      if j == 1
+        plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
+            'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i)));
+      else
+        plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
+             'HandleVisibility', 'off');
+      end
+    end
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+  legend('location', 'northeast');
+
+  linkaxes([ax1,ax2],'x');
+  xlim([freqs(1), freqs(end)]);
+#+end_src
+
+X direction
+#+begin_src matlab :exports none
+  i = 1;
+
+  freqs = logspace(-1, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 2, 1);
+  hold on;
+  for j = 1:length(Fs)
+    plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+  title(sprintf('$k = %.0e$ [N/m]', Ks(i)))
+
+  ax2 = subplot(2, 2, 3);
+  hold on;
+  for j = 1:length(Fs)
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
+         'DisplayName', sprintf('$\\omega_0 = %.0f$ [Hz]', Fs(j)));
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+  legend('location', 'northeast');
+
+  linkaxes([ax1,ax2],'x');
+  xlim([freqs(1), freqs(end)]);
+
+  i = 7;
+
+  ax1 = subplot(2, 2, 2);
+  hold on;
+  for j = 1:length(Fs)
+    plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+  title(sprintf('$k = %.0e$ [N/m]', Ks(i)))
+
+  ax2 = subplot(2, 2, 4);
+  hold on;
+  for j = 1:length(Fs)
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
+         'DisplayName', sprintf('$\\omega_0 = %.0f$ [Hz]', Fs(j)));
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+  legend('location', 'northeast');
+
+  linkaxes([ax1,ax2],'x');
+  xlim([freqs(1), freqs(end)]);
+#+end_src
+
+Z direction:
+We can see two mass lines for the soft nano-hexapod:
+- The first mass line corresponds to $\frac{1}{(m_n + m_p)s^2}$ where $m_p = 20\ [kg]$ is the mass of the payload and $m_n = 15\ [Kg]$ is the mass of the nano-hexapod top platform and attached mirror
+- The second mass line corresponds to $\frac{1}{m_n s^2}$
+
+#+begin_src matlab :exports none
+  i = 1;
+
+  freqs = logspace(-1, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 2, 1);
+  hold on;
+  for j = 1:length(Fs)
+    plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+  title(sprintf('$k = %.0e$ [N/m]', Ks(i)))
+
+  ax2 = subplot(2, 2, 3);
+  hold on;
+  for j = 1:length(Fs)
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-', ...
+         'DisplayName', sprintf('$\\omega_0 = %.0f$ [Hz]', Fs(j)));
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+  legend('location', 'northeast');
+
+  linkaxes([ax1,ax2],'x');
+  xlim([freqs(1), freqs(end)]);
+
+  i = 7;
+
+  ax1 = subplot(2, 2, 2);
+  hold on;
+  for j = 1:length(Fs)
+    plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+  title(sprintf('$k = %.0e$ [N/m]', Ks(i)))
+
+  ax2 = subplot(2, 2, 4);
+  hold on;
+  for j = 1:length(Fs)
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-', ...
+         'DisplayName', sprintf('$\\omega_0 = %.0f$ [Hz]', Fs(j)));
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+  legend('location', 'northeast');
+
+  linkaxes([ax1,ax2],'x');
+  xlim([freqs(1), freqs(end)]);
+#+end_src
+
+*** Mass variation
+All mixed, X direction
+#+begin_src matlab :exports none
+  freqs = logspace(-1, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  for i = 1:length(Ks)
+    for j = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-');
+    end
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  for i = 1:length(Ks)
+    for j = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
+      if j == 1
+        plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
+            'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i)));
+      else
+        plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
+             'HandleVisibility', 'off');
+      end
+    end
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+  legend('location', 'northeast');
+
+  linkaxes([ax1,ax2],'x');
+  xlim([freqs(1), freqs(end)]);
+#+end_src
+
+All mixed, Z direction
+#+begin_src matlab :exports none
+  freqs = logspace(-1, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  for i = 1:length(Ks)
+    for j = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-');
+    end
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  for i = 1:length(Ks)
+    for j = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
+      if j == 1
+        plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-', ...
+            'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i)));
+      else
+        plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-', ...
+             'HandleVisibility', 'off');
+      end
+    end
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+  legend('location', 'northeast');
+
+  linkaxes([ax1,ax2],'x');
+  xlim([freqs(1), freqs(end)]);
+#+end_src
+
+Z direction
+#+begin_src matlab :exports none
+  freqs = logspace(-1, 3, 1000);
+
+  figure;
+
+  i = 1;
+
+  ax1 = subplot(2, 2, 1);
+  hold on;
+  for j = 1:length(Ms)
+    plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+  title(sprintf('$k = %.0e$ [N/m]', Ks(i)))
+
+  ax2 = subplot(2, 2, 3);
+  hold on;
+  for j = 1:length(Ms)
+      plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+
+  linkaxes([ax1,ax2],'x');
+  xlim([freqs(1), freqs(end)]);
+
+  i = 7;
+
+  ax1 = subplot(2, 2, 2);
+  hold on;
+  for j = 1:length(Ms)
+    plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+  title(sprintf('$k = %.0e$ [N/m]', Ks(i)))
+
+  ax2 = subplot(2, 2, 4);
+  hold on;
+  for j = 1:length(Ms)
+      plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-', ...
+          'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(j)));
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+  legend('location', 'northeast');
+
+  linkaxes([ax1,ax2],'x');
+  xlim([freqs(1), freqs(end)]);
+#+end_src
+
+*** Total variation
+Total change of dynamics due to change of the payload:
+- mass from 1kg to 50kg
+- main resonance from 50Hz to 500Hz
+
+For a soft nano-hexapod
+#+begin_src matlab :exports none
+  i = 1;
+
+  freqs = logspace(-1, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  for j = 1:length(Fs)
+    plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
+  end
+  for j = 1:length(Ms)
+    plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  for j = 1:length(Fs)
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
+  for j = 1:length(Ms)
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
+  end
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+
+  linkaxes([ax1,ax2],'x');
+  xlim([freqs(1), freqs(end)]);
+#+end_src
+
+For a stiff nano-hexapod
+#+begin_src matlab :exports none
+  i = 7;
+
+  freqs = logspace(-1, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  for j = 1:length(Fs)
+    plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
+  end
+  for j = 1:length(Ms)
+    plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  for j = 1:length(Fs)
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
+  for j = 1:length(Ms)
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
+  end
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+
+  linkaxes([ax1,ax2],'x');
+  xlim([freqs(1), freqs(end)]);
+#+end_src
+
+
 ** Conclusion                                                        :ignore:
 
diff --git a/src/initializeController.m b/src/initializeController.m
index 2b6c628..33d7201 100644
--- a/src/initializeController.m
+++ b/src/initializeController.m
@@ -1,7 +1,7 @@
 function [] = initializeController(args)
 
 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'})} = 'open-loop'
+  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
 
 controller = struct();
@@ -23,6 +23,8 @@ switch args.type
     controller.type = 7;
   case 'hac-iff'
     controller.type = 8;
+  case 'stabilizing'
+    controller.type = 9;
 end
 
 save('./mat/controller.mat', 'controller');