#+TITLE: Amplified Piezoelectric Stack Actuator
#+SETUPFILE: ./setup/org-setup-file.org

* Introduction                                                        :ignore:
The presented model is based on cite:souleille18_concep_activ_mount_space_applic.

The model represents the amplified piezo APA100M from Cedrat-Technologies (Figure [[fig:souleille18_model_piezo]]).
The parameters are shown in the table below.

#+name: fig:souleille18_model_piezo
#+caption: Picture of an APA100M from Cedrat Technologies. Simplified model of a one DoF payload mounted on such isolator
[[file:./figs/souleille18_model_piezo.png]]

#+caption: Parameters used for the model of the APA 100M
|       | Value             | Meaning                                                        |
|-------+-------------------+----------------------------------------------------------------|
| $m$   | $1\,[kg]$         | Payload mass                                                   |
| $k_e$ | $4.8\,[N/\mu m]$  | Stiffness used to adjust the pole of the isolator              |
| $k_1$ | $0.96\,[N/\mu m]$ | Stiffness of the metallic suspension when the stack is removed |
| $k_a$ | $65\,[N/\mu m]$   | Stiffness of the actuator                                      |
| $c_1$ | $10\,[N/(m/s)]$   | Added viscous damping                                          |

* Simplified 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>>
#+end_src

#+begin_src matlab :exports none :results silent :noweb yes
  <<matlab-init>>
#+end_src

#+BEGIN_SRC matlab
  simulinkproject('../');
#+END_SRC

#+begin_src matlab
  open 'amplified_piezo_model.slx'
#+end_src

** Parameters
#+begin_src matlab
  m = 1; % [kg]

  ke = 4.8e6; % [N/m]
  ce = 5; % [N/(m/s)]
  me = 0.001; % [kg]

  k1 = 0.96e6; % [N/m]
  c1 = 10; % [N/(m/s)]

  ka = 65e6; % [N/m]
  ca = 5; % [N/(m/s)]
  ma = 0.001; % [kg]

  h = 0.2; % [m]
#+end_src

IFF Controller:
#+begin_src matlab
  Kiff = -8000/s;
#+end_src

** Identification
Identification in open-loop.
#+begin_src matlab
  %% Name of the Simulink File
  mdl = 'amplified_piezo_model';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/w'],  1, 'openinput');  io_i = io_i + 1; % Base Motion
  io(io_i) = linio([mdl, '/f'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
  io(io_i) = linio([mdl, '/F'],  1, 'openinput');  io_i = io_i + 1; % External Force

  io(io_i) = linio([mdl, '/Fs'], 3, 'openoutput'); io_i = io_i + 1; % Force Sensors
  io(io_i) = linio([mdl, '/x1'], 1, 'openoutput'); io_i = io_i + 1; % Mass displacement

  G = linearize(mdl, io, 0);
  G.InputName  = {'w', 'f', 'F'};
  G.OutputName = {'Fs', 'x1'};
#+end_src

Identification in closed-loop.
#+begin_src matlab
  %% Name of the Simulink File
  mdl = 'amplified_piezo_model';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/w'],  1, 'input');  io_i = io_i + 1; % Base Motion
  io(io_i) = linio([mdl, '/f'],  1, 'input');  io_i = io_i + 1; % Actuator Inputs
  io(io_i) = linio([mdl, '/F'],  1, 'input');  io_i = io_i + 1; % External Force

  io(io_i) = linio([mdl, '/Fs'], 3, 'output'); io_i = io_i + 1; % Force Sensors
  io(io_i) = linio([mdl, '/x1'], 1, 'output'); io_i = io_i + 1; % Mass displacement

  Giff = linearize(mdl, io, 0);
  Giff.InputName  = {'w', 'f', 'F'};
  Giff.OutputName = {'Fs', 'x1'};
#+end_src

#+begin_src matlab :exports none
  freqs = logspace(1, 3, 1000);

  figure;

  ax1 = subplot(2, 3, 1);
  title('$\displaystyle \frac{x_1}{w}$')
  hold on;
  plot(freqs, abs(squeeze(freqresp(G('x1', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Giff('x1', 'w'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/m]');xlabel('Frequency [Hz]');

  ax2 = subplot(2, 3, 2);
  title('$\displaystyle \frac{x_1}{f}$')
  hold on;
  plot(freqs, abs(squeeze(freqresp(G('x1', 'f'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Giff('x1', 'f'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]');

  ax3 = subplot(2, 3, 3);
  title('$\displaystyle \frac{x_1}{F}$')
  hold on;
  plot(freqs, abs(squeeze(freqresp(G('x1', 'F'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Giff('x1', 'F'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]');

  ax4 = subplot(2, 3, 4);
  title('$\displaystyle \frac{F_s}{w}$')
  hold on;
  plot(freqs, abs(squeeze(freqresp(G('Fs', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Giff('Fs', 'w'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/m]');xlabel('Frequency [Hz]');

  ax5 = subplot(2, 3, 5);
  title('$\displaystyle \frac{F_s}{f}$')
  hold on;
  plot(freqs, abs(squeeze(freqresp(G('Fs', 'f'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Giff('Fs', 'f'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]');

  ax6 = subplot(2, 3, 6);
  title('$\displaystyle \frac{F_s}{F}$')
  hold on;
  plot(freqs, abs(squeeze(freqresp(G('Fs', 'F'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Giff('Fs', 'F'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');

  linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_tf_ol_and_cl.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:amplified_piezo_tf_ol_and_cl
#+caption: Matrix of transfer functions from input to output in open loop (blue) and closed loop (red)
#+RESULTS:
[[file:figs/amplified_piezo_tf_ol_and_cl.png]]

** Root Locus
#+begin_src matlab :exports none :post
  figure;

  gains = logspace(1, 6, 500);

  hold on;
  plot(real(pole(G('Fs', 'f'))),  imag(pole(G('Fs', 'f'))), 'kx');
  plot(real(tzero(G('Fs', 'f'))),  imag(tzero(G('Fs', 'f'))), 'ko');
  for k = 1:length(gains)
      cl_poles = pole(feedback(G('Fs', 'f'), -gains(k)/s));
      plot(real(cl_poles), imag(cl_poles), 'k.');
  end
  hold off;
  axis square;
  xlim([-2500, 100]); ylim([0, 2600]);

  xlabel('Real Part'); ylabel('Imaginary Part');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_root_locus.pdf', 'width', 'wide', 'height', 'tall');
#+end_src

#+name: fig:amplified_piezo_root_locus
#+caption: Root Locus
#+RESULTS:
[[file:figs/amplified_piezo_root_locus.png]]

* Rotating X-Y platform
** Matlab Init                                              :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
  <<matlab-dir>>
#+end_src

#+begin_src matlab :exports none :results silent :noweb yes
  <<matlab-init>>
#+end_src

#+BEGIN_SRC matlab
  simulinkproject('../');
#+END_SRC

#+begin_src matlab
  open 'amplified_piezo_xy_rotating_stage.slx'
#+end_src

** Parameters
#+begin_src matlab
  m = 1; % [kg]

  ke = 4.8e6; % [N/m]
  ce = 5; % [N/(m/s)]
  me = 0.001; % [kg]

  k1 = 0.96e6; % [N/m]
  c1 = 10; % [N/(m/s)]

  ka = 65e6; % [N/m]
  ca = 5; % [N/(m/s)]
  ma = 0.001; % [kg]

  h = 0.2; % [m]
#+end_src

#+begin_src matlab
  Kiff = tf(0);
#+end_src

** Identification
Rotating speed in rad/s:
#+begin_src matlab
  Ws = 2*pi*[0, 1, 10, 100];
#+end_src

#+begin_src matlab
  Gs = {zeros(length(Ws), 1)};
#+end_src

Identification in open-loop.
#+begin_src matlab
  %% Name of the Simulink File
  mdl = 'amplified_piezo_xy_rotating_stage';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/fx'],  1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/fy'],  1, 'openinput');  io_i = io_i + 1;

  io(io_i) = linio([mdl, '/Fs'], 1, 'openoutput'); io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Fs'], 2, 'openoutput'); io_i = io_i + 1;

  for i = 1:length(Ws)
      ws = Ws(i);
      G = linearize(mdl, io, 0);
      G.InputName  = {'fx', 'fy'};
      G.OutputName = {'Fsx', 'Fsy'};
      Gs(i) = {G};
  end
#+end_src

#+begin_src matlab :exports none
  freqs = logspace(1, 3, 1000);

  figure;

  ax1 = subplot(2, 2, 1);
  title('$\displaystyle \frac{F_{s,x}}{f_x}$')
  hold on;
  for i = 1:length(Ws)
    plot(freqs, abs(squeeze(freqresp(Gs{i}('Fsx', 'fx'), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/m]');xlabel('Frequency [Hz]');

  ax2 = subplot(2, 2, 2);
  title('$\displaystyle \frac{F_{s,y}}{f_x}$')
  hold on;
  for i = 1:length(Ws)
    plot(freqs, abs(squeeze(freqresp(Gs{i}('Fsy', 'fx'), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]');

  ax3 = subplot(2, 2, 3);
  title('$\displaystyle \frac{F_{s,x}}{f_y}$')
  hold on;
  for i = 1:length(Ws)
    plot(freqs, abs(squeeze(freqresp(Gs{i}('Fsx', 'fy'), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]');

  ax4 = subplot(2, 2, 4);
  title('$\displaystyle \frac{F_{s,y}}{f_y}$')
  hold on;
  for i = 1:length(Ws)
    plot(freqs, abs(squeeze(freqresp(Gs{i}('Fsy', 'fy'), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/m]');xlabel('Frequency [Hz]');

  linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplitifed_piezo_xy_rotation_plant_iff.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:amplitifed_piezo_xy_rotation_plant_iff
#+caption: Transfer function matrix from forces to force sensors for multiple rotation speed
#+RESULTS:
[[file:figs/amplitifed_piezo_xy_rotation_plant_iff.png]]

** Root Locus
#+begin_src matlab :exports none :post
  figure;

  gains = logspace(1, 6, 500);

  hold on;
  for i = 1:length(Ws)
      set(gca,'ColorOrderIndex',i);
      plot(real(pole(Gs{i})),  imag(pole(Gs{i})), 'x');
      set(gca,'ColorOrderIndex',i);
      plot(real(tzero(Gs{i})),  imag(tzero(Gs{i})), 'o');
      for k = 1:length(gains)
          set(gca,'ColorOrderIndex',i);
          cl_poles = pole(feedback(Gs{i}, -gains(k)/s*eye(2)));
          plot(real(cl_poles), imag(cl_poles), '.');
      end
  end
  hold off;
  axis square;
  xlim([-2900, 100]); ylim([0, 3000]);

  xlabel('Real Part'); ylabel('Imaginary Part');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/amplified_piezo_xy_rotation_root_locus.pdf', 'width', 'tall', 'height', 'wide');
#+end_src

#+name: fig:amplified_piezo_xy_rotation_root_locus
#+caption: Root locus for 3 rotating speed
#+RESULTS:
[[file:figs/amplified_piezo_xy_rotation_root_locus.png]]

** Analysis
The negative stiffness induced by the rotation is equal to $m \omega_0^2$.
Thus, the maximum rotation speed where IFF can be applied is:
\[ \omega_\text{max} = \sqrt{\frac{k_1}{m}} \approx 156\,[Hz] \]

Let's verify that.
#+begin_src matlab
  Ws = 2*pi*[140, 160];
#+end_src

#+begin_src matlab :exports none
  Gs = {zeros(length(Ws), 1)};
#+end_src

Identification
#+begin_src matlab
  %% Name of the Simulink File
  mdl = 'amplified_piezo_xy_rotating_stage';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/fx'],  1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/fy'],  1, 'openinput');  io_i = io_i + 1;

  io(io_i) = linio([mdl, '/Fs'], 1, 'openoutput'); io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Fs'], 2, 'openoutput'); io_i = io_i + 1;

  for i = 1:length(Ws)
      ws = Ws(i);
      G = linearize(mdl, io, 0);
      G.InputName  = {'fx', 'fy'};
      G.OutputName = {'Fsx', 'Fsy'};
      Gs(i) = {G};
  end
#+end_src

#+begin_src matlab :exports none
  figure;

  gains = logspace(1, 6, 500);

  hold on;
  for i = 1:length(Ws)
      set(gca,'ColorOrderIndex',i);
      plot(real(pole(Gs{i})),  imag(pole(Gs{i})), 'x');
      set(gca,'ColorOrderIndex',i);
      plot(real(tzero(Gs{i})),  imag(tzero(Gs{i})), 'o');
      for k = 1:length(gains)
          set(gca,'ColorOrderIndex',i);
          cl_poles = pole(feedback(Gs{i}, -gains(k)/s*eye(2)));
          plot(real(cl_poles), imag(cl_poles), '.');
      end
  end
  hold off;
  axis square;
  xlim([-100, 50]); ylim([0, 150]);

  xlabel('Real Part'); ylabel('Imaginary Part');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/amplified_piezo_xy_rotating_unstable_root_locus.pdf', 'width', 'wide', 'height', 'tall');
#+end_src

#+name: fig:amplified_piezo_xy_rotating_unstable_root_locus
#+caption: Root Locus for the two considered rotation speed. For the red curve, the system is unstable.
#+RESULTS:
[[file:figs/amplified_piezo_xy_rotating_unstable_root_locus.png]]

* Stewart Platform with Amplified Actuators
** Matlab Init                                              :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
  <<matlab-dir>>
#+end_src

#+begin_src matlab :exports none :results silent :noweb yes
  <<matlab-init>>
#+end_src

#+begin_src matlab :tangle no
  simulinkproject('../');
#+end_src

#+begin_src matlab
  open('nass_model.slx')
#+end_src


** Initialization
#+begin_src matlab
  initializeGround();
  initializeGranite();
  initializeTy();
  initializeRy();
  initializeRz();
  initializeMicroHexapod();
  initializeAxisc();
  initializeMirror();

  initializeSimscapeConfiguration();
  initializeDisturbances('enable', false);
  initializeLoggingConfiguration('log', 'none');

  initializeController('type', 'hac-iff');
#+end_src

We set the stiffness of the payload fixation:
#+begin_src matlab
  Kp = 1e8; % [N/m]
#+end_src

** Identification
#+begin_src matlab
  K = tf(zeros(6));
  Kiff = tf(zeros(6));
#+end_src

We identify the system for the following payload masses:
#+begin_src matlab
  Ms = [1, 10, 50];
#+end_src

#+begin_src matlab :exports none
  Gm_iff = {zeros(length(Ms), 1)};
#+end_src

The nano-hexapod has the following leg's stiffness and damping.
#+begin_src matlab
  initializeNanoHexapod('actuator', 'amplified');
#+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; % Actuator Inputs
  io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm');  io_i = io_i + 1; % Force Sensors
#+end_src

#+begin_src matlab :exports none
  for i = 1:length(Ms)
    initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
    initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));

    %% Run the linearization
    G_iff = linearize(mdl, io);
    G_iff.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
    G_iff.OutputName = {'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'};
    Gm_iff(i) = {G_iff};
  end
#+end_src

** Controller Design

#+begin_src matlab :exports none
  freqs = logspace(-1, 3, 1000);

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:length(Ms)
    plot(freqs, abs(squeeze(freqresp(Gm_iff{i}(1, 1), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);

  ax2 = subplot(2, 1, 2);
  hold on;
  for i = 1:length(Ms)
    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_iff{i}(1, 1), freqs, 'Hz')))), ...
         'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-270, 90]);
  yticks([-360:90:360]);
  legend('location', 'northeast');

  linkaxes([ax1,ax2],'x');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_iff_loop_gain.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:amplified_piezo_iff_loop_gain
#+caption: Dynamics for the Integral Force Feedback for three payload masses
#+RESULTS:
[[file:figs/amplified_piezo_iff_loop_gain.png]]


#+begin_src matlab :exports none
  figure;

  gains = logspace(2, 5, 300);

  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(real(pole(Gm_iff{i})),  imag(pole(Gm_iff{i})), 'x', ...
           'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
      set(gca,'ColorOrderIndex',i);
      plot(real(tzero(Gm_iff{i})),  imag(tzero(Gm_iff{i})), 'o', ...
           'HandleVisibility', 'off');
      for k = 1:length(gains)
          set(gca,'ColorOrderIndex',i);
          cl_poles = pole(feedback(Gm_iff{i}, -(gains(k)/s)*eye(6)));
          plot(real(cl_poles), imag(cl_poles), '.', ...
               'HandleVisibility', 'off');
      end
  end
  hold off;
  axis square;
  xlim([-400, 10]); ylim([0, 500]);

  xlabel('Real Part'); ylabel('Imaginary Part');
  legend('location', 'northwest');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_iff_root_locus.pdf', 'width', 'wide', 'height', 'tall');
#+end_src

#+name: fig:amplified_piezo_iff_root_locus
#+caption: Root Locus for the IFF control for three payload masses
#+RESULTS:
[[file:figs/amplified_piezo_iff_root_locus.png]]

Damping as function of the gain
#+begin_src matlab :exports none
  c1 = [     0    0.4470    0.7410]; % Blue
  c2 = [0.8500    0.3250    0.0980]; % Orange
  c3 = [0.9290    0.6940    0.1250]; % Yellow
  c4 = [0.4940    0.1840    0.5560]; % Purple
  c5 = [0.4660    0.6740    0.1880]; % Green
  c6 = [0.3010    0.7450    0.9330]; % Light Blue
  c7 = [0.6350    0.0780    0.1840]; % Red
  colors = [c1; c2; c3; c4; c5; c6; c7];

  figure;

  gains = logspace(2, 5, 100);

  hold on;
  for i = 1:length(Ms)
      for k = 1:length(gains)
          cl_poles = pole(feedback(Gm_iff{i}, -(gains(k)/s)*eye(6)));
          set(gca,'ColorOrderIndex',i);
          plot(gains(k), sin(-pi/2 + angle(cl_poles)), '.', 'color', colors(i, :));
      end
  end
  hold off;
  xlabel('IFF Gain'); ylabel('Modal Damping');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylim([0, 1]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_iff_damping_gain.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:amplified_piezo_iff_damping_gain
#+caption: Damping ratio of the poles as a function of the IFF gain
#+RESULTS:
[[file:figs/amplified_piezo_iff_damping_gain.png]]

Finally, we use the following controller for the Decentralized Direct Velocity Feedback:
#+begin_src matlab
  Kiff = -1e4/s*eye(6);
#+end_src

** Effect of the Low Authority Control on the Primary Plant
*** Introduction                                                    :ignore:
#+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, 'input');            io_i = io_i + 1; % Actuator Inputs
  io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
#+end_src

#+begin_src matlab :exports none
  load('mat/stages.mat', 'nano_hexapod');
#+end_src

*** Identification of the undamped plant                            :ignore:
#+begin_src matlab :exports none
  Kiff_backup = Kiff;
  Kiff = tf(zeros(6));
#+end_src

#+begin_src matlab :exports none
  G_x = {zeros(length(Ms), 1)};
  G_l = {zeros(length(Ms), 1)};
#+end_src

#+begin_src matlab :exports none
  for i = 1:length(Ms)
      initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
      initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));

      %% Run the linearization
      G = linearize(mdl, io);
      G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
      G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};

      Gx = -G*inv(nano_hexapod.kinematics.J');
      Gx.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
      G_x(i) = {Gx};

      Gl = -nano_hexapod.kinematics.J*G;
      Gl.OutputName  = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
      G_l(i) = {Gl};
  end
#+end_src

#+begin_src matlab :exports none
  Kiff = Kiff_backup;
#+end_src

*** Identification of the damped plant                              :ignore:
#+begin_src matlab :exports none
  Gm_x = {zeros(length(Ms), 1)};
  Gm_l = {zeros(length(Ms), 1)};
#+end_src

#+begin_src matlab :exports none
  for i = 1:length(Ms)
      initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
      initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));

      %% Run the linearization
      G = linearize(mdl, io);
      G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
      G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};

      Gx = -G*inv(nano_hexapod.kinematics.J');
      Gx.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
      Gm_x(i) = {Gx};

      Gl = -nano_hexapod.kinematics.J*G;
      Gl.OutputName  = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
      Gm_l(i) = {Gl};
  end
#+end_src

*** Effect of the Damping on the plant diagonal dynamics            :ignore:
#+begin_src matlab :exports none
  freqs = logspace(0, 3, 5000);

  figure;

  ax1 = subplot(2, 2, 1);
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz'))));
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(G_x{i}(2, 2), freqs, 'Hz'))));
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gm_x{i}(1, 1), freqs, 'Hz'))), '--');
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gm_x{i}(2, 2), freqs, 'Hz'))), '--');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title('$\mathcal{X}_x/\mathcal{F}_x$, $\mathcal{X}_y/\mathcal{F}_y$')

  ax2 = subplot(2, 2, 2);
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz'))));
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gm_x{i}(3, 3), freqs, 'Hz'))), '--');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title('$\mathcal{X}_z/\mathcal{F}_z$')

  ax3 = subplot(2, 2, 3);
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz')))));
      set(gca,'ColorOrderIndex',i);
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(2, 2), freqs, 'Hz')))));
      set(gca,'ColorOrderIndex',i);
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(1, 1), freqs, 'Hz')))), '--');
      set(gca,'ColorOrderIndex',i);
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(2, 2), freqs, 'Hz')))), '--');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-270, 90]);
  yticks([-360:90:360]);

  ax4 = subplot(2, 2, 4);
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz')))), ...
           'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
      set(gca,'ColorOrderIndex',i);
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(3, 3), freqs, 'Hz')))), '--', ...
           'HandleVisibility', 'off');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-270, 90]);
  yticks([-360:90:360]);
  legend('location', 'southwest');

  linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_iff_plant_damped_X.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:amplified_piezo_iff_plant_damped_X
#+caption: Primary plant in the task space with (dashed) and without (solid) IFF
#+RESULTS:
[[file:figs/amplified_piezo_iff_plant_damped_X.png]]


#+begin_src matlab :exports none
  freqs = logspace(0, 3, 5000);

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(G_l{i}(1, 1), freqs, 'Hz'))));
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gm_l{i}(1, 1), 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(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_l{i}(1, 1), freqs, 'Hz')))), ...
           'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
      set(gca,'ColorOrderIndex',i);
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_l{i}(1, 1), freqs, 'Hz')))), '--', ...
           'HandleVisibility', 'off');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-270, 90]);
  yticks([-360:90:360]);
  legend('location', 'southwest');

  linkaxes([ax1,ax2],'x');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_iff_damped_plant_L.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:amplified_piezo_iff_damped_plant_L
#+caption: Primary plant in the space of the legs with (dashed) and without (solid) IFF
#+RESULTS:
[[file:figs/amplified_piezo_iff_damped_plant_L.png]]


*** Effect of the Damping on the coupling dynamics                  :ignore:
#+begin_src matlab :exports none
  freqs = logspace(0, 3, 1000);

  figure;
  hold on;
  for i = 1:5
    for j = i+1:6
      plot(freqs, abs(squeeze(freqresp(G_x{1}(i, j), freqs, 'Hz'))),        'color', [0, 0, 0, 0.2]);
      plot(freqs, abs(squeeze(freqresp(Gm_x{1}(i, j), freqs, 'Hz'))), '--', 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, abs(squeeze(freqresp(G_x{1}(1, 1), freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',1);
  plot(freqs, abs(squeeze(freqresp(Gm_x{1}(1, 1), freqs, 'Hz'))), '--');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  ylim([1e-12, inf]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_iff_damped_coupling_X.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:amplified_piezo_iff_damped_coupling_X
#+caption: Coupling in the primary plant in the task with (dashed) and without (solid) IFF
#+RESULTS:
[[file:figs/amplified_piezo_iff_damped_coupling_X.png]]


#+begin_src matlab :exports none
  freqs = logspace(0, 3, 1000);

  figure;
  hold on;
  for i = 1:5
    for j = i+1:6
      plot(freqs, abs(squeeze(freqresp(G_l{1}(i, j), freqs, 'Hz'))),        'color', [0, 0, 0, 0.2]);
      plot(freqs, abs(squeeze(freqresp(Gm_l{1}(i, j), freqs, 'Hz'))), '--', 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, abs(squeeze(freqresp(G_l{1}(1, 1), freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',1);
  plot(freqs, abs(squeeze(freqresp(Gm_l{1}(1, 1), freqs, 'Hz'))), '--');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  ylim([1e-9, inf]);
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_iff_damped_coupling_L.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:amplified_piezo_iff_damped_coupling_L
#+caption: Coupling in the primary plant in the space of the legs with (dashed) and without (solid) IFF
#+RESULTS:
[[file:figs/amplified_piezo_iff_damped_coupling_L.png]]

** Effect of the Low Authority Control on the Sensibility to Disturbances
*** Introduction                                                    :ignore:

*** Identification                                                  :ignore:
#+begin_src matlab :exports none
  %% Name of the Simulink File
  mdl = 'nass_model';

  %% Micro-Hexapod
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwz');   io_i = io_i + 1; % Z Ground motion
  io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fty_z'); io_i = io_i + 1; % Parasitic force Ty - Z
  io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Frz_z'); io_i = io_i + 1; % Parasitic force Rz - Z
  io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fd');    io_i = io_i + 1; % Direct forces

  io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
#+end_src

#+begin_src matlab :exports none
  Kiff_backup = Kiff;
  Kiff = tf(zeros(6));
#+end_src

#+begin_src matlab :exports none
  Gd = {zeros(length(Ms), 1)};

  for i = 1:length(Ms)
      initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
      initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));

      %% Run the linearization
      G = linearize(mdl, io);
      G.InputName  = {'Dwz', 'Fty_z', 'Frz_z', 'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
      G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};

      Gd(i) = {G};
  end
#+end_src

#+begin_src matlab :exports none
  Kiff = Kiff_backup;
#+end_src

#+begin_src matlab :exports none
  Gd_iff = {zeros(length(Ms), 1)};

  for i = 1:length(Ms)
      initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
      initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));

      %% Run the linearization
      G = linearize(mdl, io);
      G.InputName  = {'Dwz', 'Fty_z', 'Frz_z', 'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
      G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};

      Gd_iff(i) = {G};
  end
#+end_src

*** Results                                                         :ignore:
#+begin_src matlab :exports none
  freqs = logspace(0, 3, 5000);

  figure;

  subplot(2, 2, 1);
  title('$D_{w,z}$ to $E_z$');
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Dwz'), freqs, 'Hz'))), ...
           'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gd_iff{i}('Ez', 'Dwz'), freqs, 'Hz'))), '--', ...
           'HandleVisibility', 'off');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/m]'); set(gca, 'XTickLabel',[]);
  legend('location', 'southeast');

  subplot(2, 2, 2);
  title('$F_{dz}$ to $E_z$');
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Fdz'), freqs, 'Hz'))));
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gd_iff{i}('Ez', 'Fdz'), freqs, 'Hz'))), '--');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/N]');

  subplot(2, 2, 3);
  title('$F_{T_y,z}$ to $E_z$');
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Fty_z'), freqs, 'Hz'))));
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gd_iff{i}('Ez', 'Fty_z'), freqs, 'Hz'))), '--');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');

  subplot(2, 2, 4);
  title('$F_{R_z,z}$ to $E_z$');
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Frz_z'), freqs, 'Hz'))));
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gd_iff{i}('Ez', 'Frz_z'), freqs, 'Hz'))), '--');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/amplified_piezo_iff_disturbances.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:amplified_piezo_iff_disturbances
#+caption: Norm of the transfer function from vertical disturbances to vertical position error with (dashed) and without (solid) Integral Force Feedback applied
#+RESULTS:
[[file:figs/amplified_piezo_iff_disturbances.png]]

*** Conclusion                                                      :ignore:
#+begin_important
#+end_important