#+TITLE: Control of the NASS with Voice coil actuators
:DRAWER:
#+STARTUP: overview

#+LANGUAGE: en
#+EMAIL: dehaeze.thomas@gmail.com
#+AUTHOR: Dehaeze Thomas

#+HTML_LINK_HOME: ./index.html
#+HTML_LINK_UP: ./index.html

#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/zenburn.css"/>
#+HTML_HEAD: <script type="text/javascript" src="./js/jquery.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="./js/bootstrap.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="./js/jquery.stickytableheaders.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="./js/readtheorg.js"></script>

#+HTML_MATHJAX: align: center tagside: right font: TeX

#+PROPERTY: header-args:matlab  :session *MATLAB*
#+PROPERTY: header-args:matlab+ :comments org
#+PROPERTY: header-args:matlab+ :results none
#+PROPERTY: header-args:matlab+ :exports both
#+PROPERTY: header-args:matlab+ :eval no-export
#+PROPERTY: header-args:matlab+ :output-dir figs
#+PROPERTY: header-args:matlab+ :tangle no
#+PROPERTY: header-args:matlab+ :mkdirp yes

#+PROPERTY: header-args:shell  :eval no-export

#+PROPERTY: header-args:latex  :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/org/}{config.tex}")
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
#+PROPERTY: header-args:latex+ :results file raw replace
#+PROPERTY: header-args:latex+ :buffer no
#+PROPERTY: header-args:latex+ :eval no-export
#+PROPERTY: header-args:latex+ :exports results
#+PROPERTY: header-args:latex+ :mkdirp yes
#+PROPERTY: header-args:latex+ :output-dir figs
#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png")
:END:

* Introduction                                                        :ignore:
* HAC-LAC + Cascade Control Topology
** Introduction                                                      :ignore:

#+name: fig:cascade_control_architecture
#+caption: Cascaded Control consisting of (from inner to outer loop): IFF, Linearization Loop, Tracking Control in the frame of the Legs
#+RESULTS:
[[file:figs/cascade_control_architecture.png]]

** 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
We initialize all the stages with the default parameters.
#+begin_src matlab
  initializeGround();
  initializeGranite();
  initializeTy();
  initializeRy();
  initializeRz();
  initializeMicroHexapod();
  initializeAxisc();
  initializeMirror();
#+end_src

The nano-hexapod is a voice coil based hexapod and the sample has a mass of 1kg.
#+begin_src matlab
  initializeNanoHexapod('actuator', 'lorentz');
  initializeSample('mass', 1);
#+end_src

We set the references that corresponds to a tomography experiment.
#+begin_src matlab
  initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
#+end_src

#+begin_src matlab
  initializeDisturbances();
#+end_src

#+begin_src matlab
  initializeController('type', 'cascade-hac-lac');
#+end_src

#+begin_src matlab
  initializeSimscapeConfiguration('gravity', true);
#+end_src

We log the signals.
#+begin_src matlab
  initializeLoggingConfiguration('log', 'all');
#+end_src

#+begin_src matlab
  Kp = tf(zeros(6));
  Kl = tf(zeros(6));
  Kiff = tf(zeros(6));
#+end_src

** Low Authority Control - Integral Force Feedback $\bm{K}_\text{IFF}$
<<sec:lac_iff>>
*** Identification
Let's first identify the plant for the IFF controller.
#+begin_src matlab
  %% 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

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

*** Plant
The obtained plant for IFF is shown in Figure [[fig:cascade_vc_iff_plant]].

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

  figure;

  ax1 = subplot(2, 2, 1);
  hold on;
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(G_iff(i, i), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
  title('Diagonal elements of the Plant');

  ax2 = subplot(2, 2, 3);
  hold on;
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff(i, i), freqs, 'Hz'))), 'DisplayName', sprintf('$\\tau_{m,%i}/\\tau_%i$', i, i));
  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');

  ax3 = subplot(2, 2, 2);
  hold on;
  for i = 1:5
    for j = i+1:6
      plot(freqs, abs(squeeze(freqresp(G_iff(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, abs(squeeze(freqresp(G_iff(1, 1), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
  title('Off-Diagonal elements of the Plant');

  ax4 = subplot(2, 2, 4);
  hold on;
  for i = 1:5
    for j = i+1:6
      plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff(1, 1), freqs, 'Hz'))));
  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,ax3,ax4],'x');
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/cascade_vc_iff_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:cascade_vc_iff_plant
#+caption: IFF Plant ([[./figs/cascade_vc_iff_plant.png][png]], [[./figs/cascade_vc_iff_plant.pdf][pdf]])
[[file:figs/cascade_vc_iff_plant.png]]

*** Root Locus
As seen in the root locus (Figure [[fig:cascade_vc_iff_root_locus]], no damping can be added to modes corresponding to the resonance of the micro-station.

However, critical damping can be achieve for the resonances of the nano-hexapod as shown in the zoomed part of the root (Figure [[fig:cascade_vc_iff_root_locus]], left part).
The maximum damping is obtained for a control gain of $\approx 70$.

#+begin_src matlab :exports none
  gains = logspace(0, 4, 500);

  figure;

  subplot(1, 2, 1);
  hold on;
  plot(real(pole(G_iff)),  imag(pole(G_iff)),  'x');
  set(gca,'ColorOrderIndex',1);
  plot(real(tzero(G_iff)),  imag(tzero(G_iff)),  'o');
  for i = 1:length(gains)
    set(gca,'ColorOrderIndex',1);
    cl_poles = pole(feedback(G_iff, -(gains(i)/s)*eye(6)));
    plot(real(cl_poles), imag(cl_poles), '.');
  end
  ylim([0, 2*pi*500]);
  xlim([-2*pi*500,0]);
  xlabel('Real Part')
  ylabel('Imaginary Part')
  axis square

  subplot(1, 2, 2);
  hold on;
  plot(real(pole(G_iff)),  imag(pole(G_iff)),  'x');
  set(gca,'ColorOrderIndex',1);
  plot(real(tzero(G_iff)),  imag(tzero(G_iff)),  'o');
  for i = 1:length(gains)
    set(gca,'ColorOrderIndex',1);
    cl_poles = pole(feedback(G_iff, -(gains(i)/s)*eye(6)));
    plot(real(cl_poles), imag(cl_poles), '.');
  end
  ylim([0, 2*pi*8]);
  xlim([-2*pi*8,0]);
  xlabel('Real Part')
  ylabel('Imaginary Part')
  axis square
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/cascade_vc_iff_root_locus.pdf" :var figsize="wide-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:cascade_vc_iff_root_locus
#+caption: Root Locus for the IFF control ([[./figs/cascade_vc_iff_root_locus.png][png]], [[./figs/cascade_vc_iff_root_locus.pdf][pdf]])
[[file:figs/cascade_vc_iff_root_locus.png]]

*** Controller and Loop Gain
We create the $6 \times 6$ diagonal Integral Force Feedback controller.
The obtained loop gain is shown in Figure [[fig:cascade_vc_iff_loop_gain]].
#+begin_src matlab
  Kiff = -70/s*eye(6);
#+end_src

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(Kiff(i,i)*G_iff(i,i), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);

  ax2 = subplot(2, 1, 2);
  hold on;
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(Kiff(i,i)*G_iff(i,i), 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');
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/cascade_vc_iff_loop_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:cascade_vc_iff_loop_gain
#+caption: Obtained Loop gain the IFF Control ([[./figs/cascade_vc_iff_loop_gain.png][png]], [[./figs/cascade_vc_iff_loop_gain.pdf][pdf]])
[[file:figs/cascade_vc_iff_loop_gain.png]]

** High Authority Control in the joint space - $\bm{K}_\mathcal{L}$
<<sec:hac_joint_space>>
*** Identification of the damped plant
Let's identify the dynamics from $\bm{\tau}^\prime$ to $d\bm{\mathcal{L}}$ as shown in Figure [[fig:cascade_control_architecture]].

#+begin_src matlab
  %% 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, '/Micro-Station'], 3, 'output', [], 'Dnlm');  io_i = io_i + 1; % Leg Displacement

  %% Run the linearization
  Gl = linearize(mdl, io, 0);
  Gl.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  Gl.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
#+end_src

There are some unstable poles in the Plant with very small imaginary parts.
These unstable poles are probably not physical, and they disappear when taking the minimum realization of the plant.
#+begin_src matlab
  isstable(Gl)
  Gl = minreal(Gl);
  isstable(Gl)
#+end_src

*** Obtained Plant
The obtained dynamics is shown in Figure [[fig:cascade_vc_hac_joint_plant]].

Few things can be said on the dynamics:
- the dynamics of the diagonal elements are almost all the same
- the system is well decoupled below the resonances of the nano-hexapod (1Hz)
- the dynamics of the diagonal elements are almost equivalent to a critically damped mass-spring-system with some spurious resonances above 50Hz corresponding to the resonances of the micro-station

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

  figure;

  ax1 = subplot(2, 2, 1);
  hold on;
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(Gl(i, i), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title('Diagonal elements of the Plant');

  ax2 = subplot(2, 2, 3);
  hold on;
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, i), freqs, 'Hz'))), 'DisplayName', sprintf('$d\\mathcal{L}_%i/\\tau_%i$', i, i));
  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();

  ax3 = subplot(2, 2, 2);
  hold on;
  for i = 1:5
    for j = i+1:6
      plot(freqs, abs(squeeze(freqresp(Gl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, abs(squeeze(freqresp(Gl(1, 1), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title('Off-Diagonal elements of the Plant');

  ax4 = subplot(2, 2, 4);
  hold on;
  for i = 1:5
    for j = i+1:6
      plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(1, 1), freqs, 'Hz'))));
  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,ax3,ax4],'x');
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/cascade_vc_hac_joint_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:cascade_vc_hac_joint_plant
#+caption: Plant for the High Authority Control in the Joint Space ([[./figs/cascade_vc_hac_joint_plant.png][png]], [[./figs/cascade_vc_hac_joint_plant.pdf][pdf]])
[[file:figs/cascade_vc_hac_joint_plant.png]]

*** Controller Design and Loop Gain
As the plant is well decoupled, a diagonal plant is designed.

#+begin_src matlab
  wc = 2*pi*10; % Bandwidth Bandwidth [rad/s]

  h = 2; % Lead parameter

  Kl = (s + 2*pi*1)/s;

  % Normalization of the gain of have a loop gain of 1 at frequency wc
  Kl = Kl.*diag(1./diag(abs(freqresp(Gl*Kl, wc))));
#+end_src

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(Gl(i, i)*Kl(i,i), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);

  ax2 = subplot(2, 1, 2);
  hold on;
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, i)*Kl(i,i), 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');
#+end_src

#+begin_src matlab :exports none :tangle no
  isstable(feedback(Gl*Kl, eye(6), -1))
#+end_src

** Primary Controller in the task space - $\bm{K}_\mathcal{X}$
<<sec:primary_controller>>
*** Identification of the linearized plant
We know identify the dynamics between $\bm{r}_{\mathcal{X}_n}$ and $\bm{r}_\mathcal{X}$.
#+begin_src matlab
  %% Name of the Simulink File
  mdl = 'nass_model';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Controller/Cascade-HAC-LAC/Kp'],  1, 'input'); io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En');      io_i = io_i + 1; % Position Errror

  %% Run the linearization
  Gp = linearize(mdl, io, 0);
  Gp.InputName  = {'rl1', 'rl2', 'rl3', 'rl4', 'rl5', 'rl6'};
  Gp.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
#+end_src

A minus sign is added because the minus sign is already included in the plant identification.
#+begin_src matlab
  isstable(Gp)
  Gp = -minreal(Gp);
  isstable(Gp)
#+end_src

#+begin_src matlab
  load('mat/stages.mat', 'nano_hexapod');
  Gpx = Gp*inv(nano_hexapod.J');
  Gpx.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};

  Gpl = nano_hexapod.J*Gp;
  Gpl.OutputName  = {'El1', 'El2', 'El3', 'El4', 'El5', 'El6'};
#+end_src

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

  labels = {'$\epsilon_x/r_{xn}$', '$\epsilon_y/r_{yn}$', '$\epsilon_z/r_{zn}$', '$\epsilon_{R_x}/r_{R_xn}$', '$\epsilon_{R_y}/r_{R_yn}$', '$\epsilon_{R_z}/r_{R_zn}$'};

  figure;

  ax1 = subplot(2, 2, 1);
  hold on;
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(Gpx(i, i), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title('Diagonal elements of the Plant');

  ax2 = subplot(2, 2, 3);
  hold on;
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(i, i), freqs, 'Hz'))), 'DisplayName', labels{i});
  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();

  ax3 = subplot(2, 2, 2);
  hold on;
  for i = 1:5
    for j = i+1:6
      plot(freqs, abs(squeeze(freqresp(Gpx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, abs(squeeze(freqresp(Gpx(1, 1), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title('Off-Diagonal elements of the Plant');

  ax4 = subplot(2, 2, 4);
  hold on;
  for i = 1:5
    for j = i+1:6
      plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(1, 1), freqs, 'Hz'))));
  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,ax3,ax4],'x');
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/primary_plant_voice_coil_X.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:primary_plant_voice_coil_X
#+caption: Obtained Primary plant in the Task space ([[./figs/primary_plant_voice_coil_X.png][png]], [[./figs/primary_plant_voice_coil_X.pdf][pdf]])
[[file:figs/primary_plant_voice_coil_X.png]]


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

  labels = {'$\epsilon_{\mathcal{L}_1}/\tau_{1}$', '$\epsilon_{\mathcal{L}_2}/\tau_{2}$', '$\epsilon_{\mathcal{L}_3}/\tau_{3}$', '$\epsilon_{\mathcal{L}_4}/\tau_{4}$', '$\epsilon_{\mathcal{L}_5}/\tau_{5}$', '$\epsilon_{\mathcal{L}_6}/\tau_{6}$'};

  figure;

  ax1 = subplot(2, 2, 1);
  hold on;
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(Gpl(i, i), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title('Diagonal elements of the Plant');

  ax2 = subplot(2, 2, 3);
  hold on;
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gpl(i, i), freqs, 'Hz'))), 'DisplayName', labels{i});
  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();

  ax3 = subplot(2, 2, 2);
  hold on;
  for i = 1:5
    for j = i+1:6
      plot(freqs, abs(squeeze(freqresp(Gpl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, abs(squeeze(freqresp(Gpl(1, 1), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title('Off-Diagonal elements of the Plant');

  ax4 = subplot(2, 2, 4);
  hold on;
  for i = 1:5
    for j = i+1:6
      plot(freqs, 180/pi*angle(squeeze(freqresp(Gpl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gpl(1, 1), freqs, 'Hz'))));
  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,ax3,ax4],'x');
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/primary_plant_voice_coil_L.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:primary_plant_voice_coil_L
#+caption: Obtained Primary plant in the frame of the legs ([[./figs/primary_plant_voice_coil_L.png][png]], [[./figs/primary_plant_voice_coil_L.pdf][pdf]])
[[file:figs/primary_plant_voice_coil_L.png]]


*** Controller Design
#+begin_src matlab
  wc = 2*pi*200; % Bandwidth Bandwidth [rad/s]

  h = 2; % Lead parameter

  Kp = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ...
       (1/h) * (1 + s/wc*h)/(1 + s/wc/h); % For Piezo
  % Kp = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * (s + 2*pi*10)/s * (s + 2*pi*1)/s ; % For voice coil

  % Normalization of the gain of have a loop gain of 1 at frequency wc
  Kp = Kp.*diag(1./diag(abs(freqresp(Gpx*Kp, wc))));
#+end_src

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(Gpx(i, i)*Kp(i,i), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Loop Gpain'); set(gca, 'XTickLabel',[]);

  ax2 = subplot(2, 1, 2);
  hold on;
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(i, i)*Kp(i,i), 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');
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/loop_gain_primary_voice_coil_X.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:loop_gain_primary_voice_coil_X
#+caption: Obtained Loop gain for the primary controller in the Task space ([[./figs/loop_gain_primary_voice_coil_X.png][png]], [[./figs/loop_gain_primary_voice_coil_X.pdf][pdf]])
[[file:figs/loop_gain_primary_voice_coil_X.png]]


#+begin_src matlab :exports none :tangle no
  isstable(feedback(Gpx*Kp, eye(6), -1))
#+end_src

And now we include the Jacobian inside the controller.
#+begin_src matlab
  Kp = inv(nano_hexapod.J')*Kp;
#+end_src

#+begin_src matlab :exports none :tangle no
  isstable(feedback(-Gp*Kp, eye(6), +1))
#+end_src

** Simulation
Let's first save the 3 controllers for further analysis:
#+begin_src matlab
  save('mat/hac_lac_cascade_vc_controllers.mat', 'Kiff', 'Kl', 'Kp')
#+end_src

#+begin_src matlab
  load('mat/conf_simulink.mat');
  set_param(conf_simulink, 'StopTime', '2');
#+end_src

And we simulate the system.
#+begin_src matlab
  sim('nass_model');
#+end_src

#+begin_src matlab
  cascade_hac_lac_lorentz = simout;
  save('./mat/cascade_hac_lac.mat', 'cascade_hac_lac_lorentz', '-append');
#+end_src

** Results
*** Load the simulation results
#+begin_src matlab
  load('./mat/experiment_tomography.mat', 'tomo_align_dist');
  load('./mat/cascade_hac_lac.mat', 'cascade_hac_lac', 'cascade_hac_lac_lorentz');
#+end_src

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

  F_pz = [nano_hexapod.J'*cascade_hac_lac.u.Data']';
  F_vc = [nano_hexapod.J'*cascade_hac_lac_lorentz.u.Data']';

  % F_pz = [-nano_hexapod.J'*cascade_hac_lac.yn.Fnlm.Data']';
  % F_vc = [-nano_hexapod.J'*cascade_hac_lac_lorentz.yn.Fnlm.Data']';
#+end_src

#+begin_src matlab :exports none
  labels = {'$\mathcal{F}_x$', '$\mathcal{F}_y$', '$\mathcal{F}_z$', '$\mathcal{M}_x$', '$\mathcal{M}_y$', '$\mathcal{M}_z$'};

  figure;
  ax1 = subplot(1, 2, 1);
  hold on;
  for i = 1:6
    plot(cascade_hac_lac.u.Time, F_pz(:, i), 'DisplayName', labels{i})
  end
  hold off;
  xlabel('Time [s]'); ylabel('Force/Torque Piezo [N, N$\cdot$m]');
  legend('location', 'northeast');

  ax2 = subplot(1, 2, 2);
  hold on;
  for i = 1:6
    plot(cascade_hac_lac_lorentz.u.Time, F_vc(:, i), 'DisplayName', labels{i})
  end
  hold off;
  xlabel('Time [s]'); ylabel('Force/Torque Lorentz [N, N$\cdot$m]');
  legend('location', 'northeast');
#+end_src

#+begin_src matlab :exports none
  labels = {'$\mathcal{F}_x$', '$\mathcal{F}_y$', '$\mathcal{F}_z$', '$\mathcal{M}_x$', '$\mathcal{M}_y$', '$\mathcal{M}_z$'};

  figure;
  hold on;
  for i = 1:6
    plot(cascade_hac_lac_lorentz.u.Time, F_vc(:, i), 'DisplayName', labels{i})
  end
  hold off;
  xlabel('Time [s]'); ylabel('Force/Torque Lorentz [N, N$\cdot$m]');
  xlim([0.5, inf]);
  legend('location', 'northeast');
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/actuator_force_torques_tomography_voice_coil.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:actuator_force_torques_tomography_voice_coil
#+caption: Actuator Action during a tomography experiment when using Voice Coil actuators ([[./figs/actuator_force_torques_tomography_voice_coil.png][png]], [[./figs/actuator_force_torques_tomography_voice_coil.pdf][pdf]])
[[file:figs/actuator_force_torques_tomography_voice_coil.png]]

*** Load the simulation results
#+begin_src matlab
  n_av = 4;
  han_win = hanning(ceil(length(cascade_hac_lac.Em.En.Data(:,1))/n_av));
#+end_src

#+begin_src matlab
  t = cascade_hac_lac.Em.En.Time;
  Ts = t(2)-t(1);

  [pxx_ol, f] = pwelch(tomo_align_dist.Em.En.Data, han_win, [], [], 1/Ts);
  [pxx_ca, ~] = pwelch(cascade_hac_lac.Em.En.Data, han_win, [], [], 1/Ts);
  [pxx_vc, ~] = pwelch(cascade_hac_lac_lorentz.Em.En.Data, han_win, [], [], 1/Ts);
#+end_src

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2, 3, 1);
  hold on;
  plot(f, sqrt(pxx_ol(:, 1)))
  plot(f, sqrt(pxx_vc(:, 1)))
  hold off;
  ylabel('$\Gamma_{D_x}$, $\Gamma_{D_y}$, $\Gamma_{D_z}$ [$m/\sqrt{Hz}$]');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');

  ax2 = subplot(2, 3, 2);
  hold on;
  plot(f, sqrt(pxx_ol(:, 2)))
  plot(f, sqrt(pxx_vc(:, 2)))
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');

  ax3 = subplot(2, 3, 3);
  hold on;
  plot(f, sqrt(pxx_ol(:, 3)))
  plot(f, sqrt(pxx_vc(:, 3)))
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');

  ax4 = subplot(2, 3, 4);
  hold on;
  plot(f, sqrt(pxx_ol(:, 4)))
  plot(f, sqrt(pxx_vc(:, 4)))
  hold off;
  ylabel('$\Gamma_{R_x}$, $\Gamma_{R_y}$, $\Gamma_{R_z}$ [$rad/\sqrt{Hz}$]');
  xlabel('Frequency [Hz]');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');

  ax5 = subplot(2, 3, 5);
  hold on;
  plot(f, sqrt(pxx_ol(:, 5)))
  plot(f, sqrt(pxx_vc(:, 5)))
  hold off;
  xlabel('Frequency [Hz]');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');

  ax6 = subplot(2, 3, 6);
  hold on;
  plot(f, sqrt(pxx_ol(:, 6)), 'DisplayName', '$\mu$-Station')
  plot(f, sqrt(pxx_vc(:, 6)), 'DisplayName', 'Voice-Coil')
  hold off;
  xlabel('Frequency [Hz]');
  legend('location', 'southwest');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');

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

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/exp_tomography_voice_coil_psd_pos_error.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:exp_tomography_voice_coil_psd_pos_error
#+caption: Power Spectral Density of the position error during a tomography experiment when using Voice Coil based nano-hexapod ([[./figs/exp_tomography_voice_coil_psd_pos_error.png][png]], [[./figs/exp_tomography_voice_coil_psd_pos_error.pdf][pdf]])
[[file:figs/exp_tomography_voice_coil_psd_pos_error.png]]

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2, 3, 1);
  hold on;
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 1))))))
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_vc(:, 1))))))
  hold off;
  ylabel('CAS $D_x$, $D_y$, $D_z$ [$m$]');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');

  ax2 = subplot(2, 3, 2);
  hold on;
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 2))))))
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_vc(:, 2))))))
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');

  ax3 = subplot(2, 3, 3);
  hold on;
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 3))))))
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_vc(:, 3))))))
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');

  ax4 = subplot(2, 3, 4);
  hold on;
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 4))))))
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_vc(:, 4))))))
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('CAS $R_x$, $R_y$, $R_z$ [$rad$]');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');

  ax5 = subplot(2, 3, 5);
  hold on;
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 5))))))
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_vc(:, 5))))))
  hold off;
  xlabel('Frequency [Hz]');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');

  ax6 = subplot(2, 3, 6);
  hold on;
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 6))))), 'DisplayName', '$\mu$-Station')
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_vc(:, 6))))), 'DisplayName', 'Voice Coil')
  hold off;
  xlabel('Frequency [Hz]');
  legend('location', 'southwest');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');

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

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/exp_tomography_voice_coil_cap_pos_error.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:exp_tomography_voice_coil_cap_pos_error
#+caption: Cumulative Amplitude Spectrum of the position error during a tomography experiment when using Voice Coil based nano-hexapod ([[./figs/exp_tomography_voice_coil_cap_pos_error.png][png]], [[./figs/exp_tomography_voice_coil_cap_pos_error.pdf][pdf]])
[[file:figs/exp_tomography_voice_coil_cap_pos_error.png]]

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2, 3, 1);
  hold on;
  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 1))
  plot(cascade_hac_lac_lorentz.Em.En.Time, cascade_hac_lac_lorentz.Em.En.Data(:, 1))
  hold off;
  ylabel('$D_x$, $D_y$, $D_z$ [m]');

  ax2 = subplot(2, 3, 2);
  hold on;
  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 2))
  plot(cascade_hac_lac_lorentz.Em.En.Time, cascade_hac_lac_lorentz.Em.En.Data(:, 2))
  hold off;

  ax3 = subplot(2, 3, 3);
  hold on;
  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 3))
  plot(cascade_hac_lac_lorentz.Em.En.Time, cascade_hac_lac_lorentz.Em.En.Data(:, 3))
  hold off;

  ax4 = subplot(2, 3, 4);
  hold on;
  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 4))
  plot(cascade_hac_lac_lorentz.Em.En.Time, cascade_hac_lac_lorentz.Em.En.Data(:, 4))
  hold off;
  xlabel('Time [s]');
  ylabel('$R_x$, $R_y$, $R_z$ [rad]');

  ax5 = subplot(2, 3, 5);
  hold on;
  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 5))
  plot(cascade_hac_lac_lorentz.Em.En.Time, cascade_hac_lac_lorentz.Em.En.Data(:, 5))
  hold off;
  xlabel('Time [s]');

  ax6 = subplot(2, 3, 6);
  hold on;
  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 6), 'DisplayName', '$\mu$-Station')
  plot(cascade_hac_lac_lorentz.Em.En.Time, cascade_hac_lac_lorentz.Em.En.Data(:, 6), 'DisplayName', 'Voice Coil')
  hold off;
  xlabel('Time [s]');
  legend();

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

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/exp_tomography_voice_coil_time_domain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:exp_tomography_voice_coil_time_domain
#+caption: Position error during a tomography experiment when using Voice Coil based nano-hexapod ([[./figs/exp_tomography_voice_coil_time_domain.png][png]], [[./figs/exp_tomography_voice_coil_time_domain.pdf][pdf]])
[[file:figs/exp_tomography_voice_coil_time_domain.png]]


** Robustness to Payload Variability
*** Initialization
Let's change the payload mass, and see if the controller design for a payload mass of 1 still gives good performance.
#+begin_src matlab
  initializeSample('mass', 50);
#+end_src

#+begin_src matlab
  Kp = tf(zeros(6));
  Kl = tf(zeros(6));
  Kiff = tf(zeros(6));
#+end_src

*** Low Authority Control
Let's first identify the transfer function for the Low Authority control.

#+begin_src matlab
  %% 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

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

The obtained dynamics is compared when using a payload of 1Kg in Figure [[fig:voice_coil_variability_mass_iff]].

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(G_iff(i, i), freqs, 'Hz'))));
  end
  set(gca,'ColorOrderIndex',1);
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(G_iff_m(i, i), freqs, 'Hz'))), '--');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
  title('Diagonal elements of the Plant');

  ax2 = subplot(2, 1, 2);
  hold on;
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff(i, i), freqs, 'Hz'))), 'DisplayName', sprintf('$\\tau_{m,%i}/\\tau_%i$', i, i));
  end
  set(gca,'ColorOrderIndex',1);
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m(i, i), 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');
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/voice_coil_variability_mass_iff.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:voice_coil_variability_mass_iff
#+caption: Dynamics of the LAC plant when using a 50Kg payload (dashed) and when using a 1Kg payload (solid) ([[./figs/voice_coil_variability_mass_iff.png][png]], [[./figs/voice_coil_variability_mass_iff.pdf][pdf]])
[[file:figs/voice_coil_variability_mass_iff.png]]

A gain of 50 will here suffice to obtain critical damping of the nano-hexapod modes.

Let's load the IFF controller designed when the payload has a mass of 1Kg.
#+begin_src matlab
  load('mat/hac_lac_cascade_vc_controllers.mat', 'Kiff')
#+end_src

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(Kiff(i,i)*G_iff(i,i), freqs, 'Hz'))), '-');
  end
  set(gca,'ColorOrderIndex',1);
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(Kiff(i,i)*G_iff_m(i,i), freqs, 'Hz'))), '--');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);

  ax2 = subplot(2, 1, 2);
  hold on;
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(Kiff(i,i)*G_iff(i,i), freqs, 'Hz'))), '-');
  end
  set(gca,'ColorOrderIndex',1);
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(Kiff(i,i)*G_iff_m(i,i), 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');
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/voice_coil_variability_mass_iff_loop_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:voice_coil_variability_mass_iff_loop_gain
#+caption: Loop gain for the IFF Control when using a 50Kg payload (dashed) and when using a 1Kg payload (solid) ([[./figs/voice_coil_variability_mass_iff_loop_gain.png][png]], [[./figs/voice_coil_variability_mass_iff_loop_gain.pdf][pdf]])
[[file:figs/voice_coil_variability_mass_iff_loop_gain.png]]

*** High Authority Control
We use the Integral Force Feedback developed with a mass of 1Kg and we identify the dynamics for the High Authority Controller in the case of the 50Kg payload

#+begin_src matlab
  %% 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, '/Micro-Station'], 3, 'output', [], 'Dnlm');  io_i = io_i + 1; % Leg Displacement

  %% Run the linearization
  Gl_m = linearize(mdl, io, 0);
  Gl_m.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  Gl_m.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
 
  isstable(Gl_m)
  Gl_m = minreal(Gl_m);
  isstable(Gl_m)
#+end_src

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(Gl(i, i), freqs, 'Hz'))));
  end
  set(gca,'ColorOrderIndex',1);
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(Gl_m(i, i), freqs, 'Hz'))), '--');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title('Diagonal elements of the Plant');

  ax2 = subplot(2, 1, 2);
  hold on;
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, i), freqs, 'Hz'))), 'DisplayName', sprintf('$d\\mathcal{L}_%i/\\tau_%i$', i, i));
  end
  set(gca,'ColorOrderIndex',1);
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gl_m(i, i), 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();

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

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/voice_coil_variability_mass_hac_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:voice_coil_variability_mass_hac_plant
#+caption: Dynamics of the HAC plant when using a 50Kg payload (dashed) and when using a 1Kg payload (solid) ([[./figs/voice_coil_variability_mass_hac_plant.png][png]], [[./figs/voice_coil_variability_mass_hac_plant.pdf][pdf]])
[[file:figs/voice_coil_variability_mass_hac_plant.png]]

We load the HAC controller design when the payload has a mass of 1Kg.
#+begin_src matlab
  load('mat/hac_lac_cascade_vc_controllers.mat', 'Kl')
#+end_src

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(Gl(i, i)*Kl(i,i), freqs, 'Hz'))), '-');
  end
  set(gca,'ColorOrderIndex',1);
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(Gl_m(i, i)*Kl(i,i), freqs, 'Hz'))), '--');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);

  ax2 = subplot(2, 1, 2);
  hold on;
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, i)*Kl(i,i), freqs, 'Hz'))), '-');
  end
  set(gca,'ColorOrderIndex',1);
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gl_m(i, i)*Kl(i,i), 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');
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/voice_coil_variability_mass_hac_lool_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:voice_coil_variability_mass_hac_lool_gain
#+caption: Loop Gain of the HAC when using a 50Kg payload (dashed) and when using a 1Kg payload (solid) ([[./figs/voice_coil_variability_mass_hac_lool_gain.png][png]], [[./figs/voice_coil_variability_mass_hac_lool_gain.pdf][pdf]])
[[file:figs/voice_coil_variability_mass_hac_lool_gain.png]]

#+begin_src matlab :exports none :tangle no
  isstable(feedback(Gl_m*Kl, eye(6), -1))
#+end_src

*** Primary Plant
We use the Low Authority Controller developed with a mass of 1Kg and we identify the dynamics for the Primary controller in the case of the 50Kg payload.
#+begin_src matlab
  %% Name of the Simulink File
  mdl = 'nass_model';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Controller/Cascade-HAC-LAC/Kp'],  1, 'input'); io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En');      io_i = io_i + 1; % Position Errror

  %% Run the linearization
  Gp_m = linearize(mdl, io, 0);
  Gp_m.InputName  = {'rl1', 'rl2', 'rl3', 'rl4', 'rl5', 'rl6'};
  Gp_m.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
#+end_src

A minus sign is added to cancel the minus sign already included in the identified plant.
#+begin_src matlab
  isstable(Gp_m)
  Gp_m = -minreal(Gp_m);
  isstable(Gp_m)
#+end_src

#+begin_src matlab
  load('mat/stages.mat', 'nano_hexapod');
  Gpx_m = Gp_m*inv(nano_hexapod.J');
  Gpx_m.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};

  Gpl_m = nano_hexapod.J*Gp_m;
  Gpl_m.OutputName  = {'El1', 'El2', 'El3', 'El4', 'El5', 'El6'};
#+end_src

#+begin_important
There are two zeros with positive real part for the plant in the y direction at about 100Hz.
This is problematic as it limits the bandwidth to be less than $\approx 50\ \text{Hz}$.

It is important here to physically understand why such "positive" zero appears.

If we make a "rigid" 50kg paylaod, the positive zero disappears.
#+end_important

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

  labels = {'$\epsilon_x/r_{xn}$', '$\epsilon_y/r_{yn}$', '$\epsilon_z/r_{zn}$', '$\epsilon_{R_x}/r_{R_xn}$', '$\epsilon_{R_y}/r_{R_yn}$', '$\epsilon_{R_z}/r_{R_zn}$'};

  figure;

  ax1 = subplot(2, 2, 1);
  hold on;
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(Gpx(i, i), freqs, 'Hz'))));
  end
  set(gca,'ColorOrderIndex',1);
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(Gpx_m(i, i), freqs, 'Hz'))), '--');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title('Diagonal elements of the Plant');

  ax2 = subplot(2, 2, 3);
  hold on;
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(i, i), freqs, 'Hz'))), 'DisplayName', labels{i});
  end
  set(gca,'ColorOrderIndex',1);
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx_m(i, i), 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();

  ax3 = subplot(2, 2, 2);
  hold on;
  for i = 1:5
    for j = i+1:6
      plot(freqs, abs(squeeze(freqresp(Gpx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
    end
  end
  for i = 1:5
    for j = i+1:6
      plot(freqs, abs(squeeze(freqresp(Gpx_m(i, j), freqs, 'Hz'))), '--', 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, abs(squeeze(freqresp(Gpx(1, 1), freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',1);
  plot(freqs, abs(squeeze(freqresp(Gpx_m(1, 1), freqs, 'Hz'))), '--');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title('Off-Diagonal elements of the Plant');

  ax4 = subplot(2, 2, 4);
  hold on;
  for i = 1:5
    for j = i+1:6
      plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(1, 1), freqs, 'Hz'))));
  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,ax3,ax4],'x');
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/voice_coil_variability_mass_primary_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:voice_coil_variability_mass_primary_plant
#+caption: Dynamics of the Primary plant when using a 50Kg payload (dashed) and when using a 1Kg payload (solid) ([[./figs/voice_coil_variability_mass_primary_plant.png][png]], [[./figs/voice_coil_variability_mass_primary_plant.pdf][pdf]])
[[file:figs/voice_coil_variability_mass_primary_plant.png]]

We load the primary controller that was design when the payload has a mass of 1Kg.

We load the HAC controller design when the payload has a mass of 1Kg.
#+begin_src matlab
  load('mat/hac_lac_cascade_vc_controllers.mat', 'Kp')
  Kp_x = nano_hexapod.J'*Kp;
#+end_src

#+begin_src matlab
  wc = 2*pi*50; % Bandwidth Bandwidth [rad/s]

  h = 2; % Lead parameter

  Kp = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ...
       (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ...
       (s + 2*pi*1)/s * ...
       1/(1+s/2/wc); % For Piezo

  % Normalization of the gain of have a loop gain of 1 at frequency wc
  Kp = Kp.*diag(1./diag(abs(freqresp(Gpx_m*Kp, wc))));
#+end_src

#+begin_src matlab :exports none
  labels = {'$L_{x}$', '$L_{y}$', '$L_{z}$', '$L_{R_x}$', '$L_{R_y}$', '$L_{R_z}$'};

  freqs = logspace(0, 3, 1000);

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(Gpx(i, i)*Kp_x(i,i), freqs, 'Hz'))));
  end
  set(gca,'ColorOrderIndex',1);
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(Gpx_m(i, i)*Kp_x(i,i), freqs, 'Hz'))), '--');
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Loop Gpain'); set(gca, 'XTickLabel',[]);

  ax2 = subplot(2, 1, 2);
  hold on;
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(i, i)*Kp_x(i,i), freqs, 'Hz'))), 'DisplayName', labels{i});
  end
  set(gca,'ColorOrderIndex',1);
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx_m(i, i)*Kp_x(i,i), 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', 'northwest')

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

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/voice_coil_variability_mass_primary_lool_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:voice_coil_variability_mass_primary_lool_gain
#+caption: Loop Gain of the Primary loop when using a 50Kg payload (dashed) and when using a 1Kg payload (solid) ([[./figs/voice_coil_variability_mass_primary_lool_gain.png][png]], [[./figs/voice_coil_variability_mass_primary_lool_gain.pdf][pdf]])
[[file:figs/voice_coil_variability_mass_primary_lool_gain.png]]

#+begin_src matlab :exports none :tangle no
  isstable(feedback(Gpx_m*Kp_x, eye(6), -1))
#+end_src

#+begin_src matlab :exports none :tangle no
  isstable(feedback(Gp_m*Kp, eye(6), -1))
#+end_src

*** Simulation
#+begin_src matlab
  load('mat/conf_simulink.mat');
  set_param(conf_simulink, 'StopTime', '2');
#+end_src

And we simulate the system.
#+begin_src matlab
  sim('nass_model');
#+end_src

#+begin_src matlab
  cascade_hac_lac_lorentz_high_mass = simout;
  save('./mat/cascade_hac_lac.mat', 'cascade_hac_lac_lorentz_high_mass', '-append');
#+end_src

#+begin_src matlab
  load('./mat/experiment_tomography.mat', 'tomo_align_dist');
#+end_src

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2, 3, 1);
  hold on;
  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 1))
  plot(cascade_hac_lac_lorentz_high_mass.Em.En.Time, cascade_hac_lac_lorentz_high_mass.Em.En.Data(:, 1))
  hold off;
  ylabel('$D_x$, $D_y$, $D_z$ [m]');

  ax2 = subplot(2, 3, 2);
  hold on;
  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 2))
  plot(cascade_hac_lac_lorentz_high_mass.Em.En.Time, cascade_hac_lac_lorentz_high_mass.Em.En.Data(:, 2))
  hold off;

  ax3 = subplot(2, 3, 3);
  hold on;
  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 3))
  plot(cascade_hac_lac_lorentz_high_mass.Em.En.Time, cascade_hac_lac_lorentz_high_mass.Em.En.Data(:, 3))
  hold off;

  ax4 = subplot(2, 3, 4);
  hold on;
  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 4))
  plot(cascade_hac_lac_lorentz_high_mass.Em.En.Time, cascade_hac_lac_lorentz_high_mass.Em.En.Data(:, 4))
  hold off;
  xlabel('Time [s]');
  ylabel('$R_x$, $R_y$, $R_z$ [rad]');

  ax5 = subplot(2, 3, 5);
  hold on;
  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 5))
  plot(cascade_hac_lac_lorentz_high_mass.Em.En.Time, cascade_hac_lac_lorentz_high_mass.Em.En.Data(:, 5))
  hold off;
  xlabel('Time [s]');

  ax6 = subplot(2, 3, 6);
  hold on;
  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 6), 'DisplayName', '$\mu$-Station')
  plot(cascade_hac_lac_lorentz_high_mass.Em.En.Time, cascade_hac_lac_lorentz_high_mass.Em.En.Data(:, 6), 'DisplayName', 'Voice Coil')
  hold off;
  xlabel('Time [s]');
  legend();

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

* Other analysis
** Robustness to Payload Variability
- [ ] For 3/masses (1kg, 10kg, 50kg), plot each of the 3 plants

** Direct HAC control in the task space - $\bm{K}_\mathcal{X}$
*** Introduction                                                    :ignore:

#+name: fig:control_architecture_hac_iff_pos_X
#+caption: Control Architecture containing an IFF controller and a Controller in the task space
[[file:figs/control_architecture_hac_iff_pos_X.png]]

*** Identification
#+begin_src matlab
  initializeController('type', 'hac-iff');
#+end_src

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

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Controller/HAC-IFF/Kx'],  1, 'input');    io_i = io_i + 1; % Control input
  io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror

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

#+begin_src matlab
  isstable(G)
  G = -minreal(G);
  isstable(G)
#+end_src

#+begin_src matlab
  load('mat/stages.mat', 'nano_hexapod');
  Gx = G*inv(nano_hexapod.J');
  Gx.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};

  Gl = nano_hexapod.J*G;
  Gl.OutputName  = {'El1', 'El2', 'El3', 'El4', 'El5', 'El6'};
#+end_src

*** Obtained Plant in the Task Space
#+begin_src matlab :exports none
  freqs = logspace(0, 4, 1000);

  labels = {'$\epsilon_x/F_{x}$', '$\epsilon_y/F_{y}$', '$\epsilon_z/F_{z}$', '$\epsilon_{R_x}/M_{x}$', '$\epsilon_{R_y}/M_{y}$', '$\epsilon_{R_z}/M_{z}$'};

  figure;

  ax1 = subplot(2, 2, 1);
  hold on;
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(Gx(i, i), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title('Diagonal elements of the Plant');

  ax2 = subplot(2, 2, 3);
  hold on;
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, i), freqs, 'Hz'))), 'DisplayName', labels{i});
  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();

  ax3 = subplot(2, 2, 2);
  hold on;
  for i = 1:5
    for j = i+1:6
      plot(freqs, abs(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, abs(squeeze(freqresp(Gx(1, 1), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title('Off-Diagonal elements of the Plant');

  ax4 = subplot(2, 2, 4);
  hold on;
  for i = 1:5
    for j = i+1:6
      plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(1, 1), freqs, 'Hz'))));
  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,ax3,ax4],'x');
#+end_src


*** Obtained Plant in the Joint Space
#+begin_src matlab :exports none
  freqs = logspace(0, 4, 1000);

  labels = {'$\epsilon_{\mathcal{L}_1}/\tau_{1}$', '$\epsilon_{\mathcal{L}_2}/\tau_{2}$', '$\epsilon_{\mathcal{L}_3}/\tau_{3}$', '$\epsilon_{\mathcal{L}_4}/\tau_{4}$', '$\epsilon_{\mathcal{L}_5}/\tau_{5}$', '$\epsilon_{\mathcal{L}_6}/\tau_{6}$'};

  figure;

  ax1 = subplot(2, 2, 1);
  hold on;
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(Gl(i, i), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title('Diagonal elements of the Plant');

  ax2 = subplot(2, 2, 3);
  hold on;
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, i), freqs, 'Hz'))), 'DisplayName', labels{i});
  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();

  ax3 = subplot(2, 2, 2);
  hold on;
  for i = 1:5
    for j = i+1:6
      plot(freqs, abs(squeeze(freqresp(Gl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, abs(squeeze(freqresp(Gl(1, 1), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title('Off-Diagonal elements of the Plant');

  ax4 = subplot(2, 2, 4);
  hold on;
  for i = 1:5
    for j = i+1:6
      plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(1, 1), freqs, 'Hz'))));
  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,ax3,ax4],'x');
#+end_src

*** Controller Design in the Joint Space
#+begin_src matlab
  wc = 2*pi*200; % Bandwidth Bandwidth [rad/s]

  h = 2; % Lead parameter

  Kx = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ... % Lead
       (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ... % Lead
       (s + 2*pi*10)/s * ... % Pseudo Integrator
       1/(1+s/2/pi/500); % Low pass Filter

  % Normalization of the gain of have a loop gain of 1 at frequency wc
  Kx = Kx.*diag(1./diag(abs(freqresp(Gx*Kx, wc))));
#+end_src

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(Gx(i, i)*Kx(i,i), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);

  ax2 = subplot(2, 1, 2);
  hold on;
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, i)*Kx(i,i), 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');
#+end_src

#+begin_src matlab :exports none :tangle no
  isstable(feedback(Gx*Kx, eye(6), -1))
#+end_src

#+begin_src matlab
  wc = 2*pi*200; % Bandwidth Bandwidth [rad/s]

  h = 2; % Lead parameter

  Kl = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ... % Lead
       (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ... % Lead
       (s + 2*pi*1)/s * ... % Pseudo Integrator
       (s + 2*pi*10)/s * ... % Pseudo Integrator
       1/(1+s/2/pi/500); % Low pass Filter

  % Normalization of the gain of have a loop gain of 1 at frequency wc
  Kl = Kl.*diag(1./diag(abs(freqresp(Gl*Kl, wc))));
#+end_src

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(Gl(i, i)*Kl(i,i), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);

  ax2 = subplot(2, 1, 2);
  hold on;
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, i)*Kl(i,i), 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');
#+end_src

#+begin_src matlab :exports none :tangle no
  isstable(feedback(Gl*Kl, eye(6), -1))
#+end_src

** On the usefulness of the High Authority Control loop / Linearization loop
*** Introduction                                                    :ignore:
Let's see what happens is we omit the HAC loop and we directly try to control the damped plant using the measurement of the sample with respect to the granite $\bm{\mathcal{X}}$.

We can do that in two different ways:
- in the task space as shown in Figure [[fig:control_architecture_iff_X]]
- in the space of the legs as shown in Figure [[fig:control_architecture_iff_L]]

#+begin_src latex :file control_architecture_iff_X.pdf
  \begin{tikzpicture}
    % Blocs
    \node[block={3.0cm}{3.0cm}] (P) {Plant};
    \coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$);
    \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
    \coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$);
    \coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$);

    \node[block, above=0.4 of P] (Kiff) {$\bm{K}_\text{IFF}$};
    \node[addb={+}{}{-}{}{}, left= of inputF] (addF) {};
    \node[block, left= of addF] (J) {$\bm{J}^{-T}$};
    \node[block, left= of J] (K) {$\bm{K}_\mathcal{X}$};
    \node[addb={+}{}{}{}{-}, left= of K] (subr) {};

    % Connections and labels
    \draw[->] (outputF) -- ++(1, 0) node[below left]{$\bm{\tau}_m$};
    \draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east);
    \draw[->] (Kiff.west) -| (addF.north);
    \draw[->] (addF.east) -- (inputF) node[above left]{$\bm{\tau}$};

    \draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$};

    \draw[->] (outputX) -- ++(1.6, 0);
    \draw[->] ($(outputX) + (1.2, 0)$)node[branch]{} node[above]{$\bm{\mathcal{X}}$} -- ++(0, -2) -| (subr.south);

    \draw[<-] (subr.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1, 0);
    \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{\mathcal{X}}$};
    \draw[->] (K.east) -- (J.west) node[above left]{$\bm{\mathcal{F}}$};
    \draw[->] (J.east) -- (addF.west) node[above left]{$\bm{\tau}^\prime$};
  \end{tikzpicture}
#+end_src

#+name: fig:control_architecture_iff_X
#+caption: IFF control + primary controller in the task space
#+RESULTS:
[[file:figs/control_architecture_iff_X.png]]

#+begin_src latex :file control_architecture_iff_L.pdf
  \begin{tikzpicture}
    % Blocs
    \node[block={3.0cm}{3.0cm}] (P) {Plant};
    \coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$);
    \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
    \coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$);
    \coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$);

    \node[block, above=0.4 of P] (Kiff) {$\bm{K}_\text{IFF}$};
    \node[addb={+}{}{-}{}{}, left= of inputF] (addF) {};
    \node[block, left= of addF] (K) {$\bm{K}_\mathcal{L}$};
    \node[block, left= of K] (J) {$\bm{J}$};
    \node[addb={+}{}{}{}{-}, left= of J] (subr) {};

    % Connections and labels
    \draw[->] (outputF) -- ++(1, 0) node[below left]{$\bm{\tau}_m$};
    \draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east);
    \draw[->] (Kiff.west) -| (addF.north);
    \draw[->] (addF.east) -- (inputF) node[above left]{$\bm{\tau}$};

    \draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$};

    \draw[->] (outputX) -- ++(1.6, 0);
    \draw[->] ($(outputX) + (1.2, 0)$)node[branch]{} node[above]{$\bm{\mathcal{X}}$} -- ++(0, -2) -| (subr.south);

    \draw[<-] (subr.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1, 0);
    \draw[->] (subr.east) -- (J.west) node[above left]{$\bm{\epsilon}_{\mathcal{X}}$};
    \draw[->] (J.east) -- (K.west) node[above left]{$\bm{\epsilon}_{\mathcal{L}}$};
    \draw[->] (K.east) -- (addF.west) node[above left]{$\bm{\tau}^\prime$};
  \end{tikzpicture}
#+end_src

#+name: fig:control_architecture_iff_L
#+caption: HAC-LAC control architecture in the frame of the legs
#+RESULTS:
[[file:figs/control_architecture_iff_L.png]]

*** Identification
#+begin_src matlab
  initializeController('type', 'hac-iff');
#+end_src

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

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Controller/HAC-IFF/Kx'],  1, 'input');    io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror

  %% Run the linearization
  G = linearize(mdl, io, 0);
  G.InputName  = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
  G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
#+end_src

#+begin_src matlab
  isstable(G)
  G = -minreal(G);
  isstable(G)
#+end_src

*** Plant in the Task space
The obtained plant is shown in Figure

#+begin_src matlab
  Gx = G*inv(nano_hexapod.J');
#+end_src

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

  labels = {'$\epsilon_x/\mathcal{F}_{x}$', '$\epsilon_y/\mathcal{F}_{y}$', '$\epsilon_z/\mathcal{F}_{z}$', '$\epsilon_{R_x}/\mathcal{M}_{x}$', '$\epsilon_{R_y}/\mathcal{M}_{y}$', '$\epsilon_{R_z}/\mathcal{M}_{z}$'};

  figure;

  ax1 = subplot(2, 2, 1);
  hold on;
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(Gx(i, i), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title('Diagonal elements of the Plant');

  ax2 = subplot(2, 2, 3);
  hold on;
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, i), freqs, 'Hz'))), 'DisplayName', labels{i});
  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();

  ax3 = subplot(2, 2, 2);
  hold on;
  for i = 1:5
    for j = i+1:6
      plot(freqs, abs(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, abs(squeeze(freqresp(Gx(1, 1), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title('Off-Diagonal elements of the Plant');

  ax4 = subplot(2, 2, 4);
  hold on;
  for i = 1:5
    for j = i+1:6
      plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(1, 1), freqs, 'Hz'))));
  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,ax3,ax4],'x');
#+end_src

*** Plant in the Leg's space
#+begin_src matlab
  Gl = nano_hexapod.J*G;
#+end_src

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

  figure;

  ax1 = subplot(2, 2, 1);
  hold on;
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(Gl(i, i), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title('Diagonal elements of the Plant');

  ax2 = subplot(2, 2, 3);
  hold on;
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, i), freqs, 'Hz'))), 'DisplayName', sprintf('$d\\mathcal{L}_%i/\\tau_%i$', i, i));
  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();

  ax3 = subplot(2, 2, 2);
  hold on;
  for i = 1:5
    for j = i+1:6
      plot(freqs, abs(squeeze(freqresp(Gl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, abs(squeeze(freqresp(Gl(1, 1), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
  title('Off-Diagonal elements of the Plant');

  ax4 = subplot(2, 2, 4);
  hold on;
  for i = 1:5
    for j = i+1:6
      plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(1, 1), freqs, 'Hz'))));
  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,ax3,ax4],'x');
#+end_src
** DVF instead of IFF?
*** Initialization and Identification
#+begin_src matlab
  initializeController('type', 'hac-dvf');
  Kdvf = tf(zeros(6));
#+end_src

#+begin_src matlab
  %% 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', [], 'Dnlm');  io_i = io_i + 1; % Displacement Sensors

  %% Run the linearization
  G_dvf = linearize(mdl, io, 0);
  G_dvf.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  G_dvf.OutputName = {'Dlm1', 'Dlm2', 'Dlm3', 'Dlm4', 'Dlm5', 'Dlm6'};
#+end_src

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

  figure;

  ax1 = subplot(2, 2, 1);
  hold on;
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(G_dvf(i, i), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
  title('Diagonal elements of the Plant');

  ax2 = subplot(2, 2, 3);
  hold on;
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf(i, i), freqs, 'Hz'))), 'DisplayName', sprintf('$d\\mathcal{L}_{%i}/\\tau_%i$', i, i));
  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');

  ax3 = subplot(2, 2, 2);
  hold on;
  for i = 1:5
    for j = i+1:6
      plot(freqs, abs(squeeze(freqresp(G_dvf(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, abs(squeeze(freqresp(G_dvf(1, 1), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
  title('Off-Diagonal elements of the Plant');

  ax4 = subplot(2, 2, 4);
  hold on;
  for i = 1:5
    for j = i+1:6
      plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf(1, 1), freqs, 'Hz'))));
  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,ax3,ax4],'x');
#+end_src

#+begin_src matlab :exports none
  gains = logspace(0, 4, 500);

  figure;

  subplot(1, 2, 1);
  hold on;
  plot(real(pole(G_dvf)),  imag(pole(G_dvf)),  'x');
  set(gca,'ColorOrderIndex',1);
  plot(real(tzero(G_dvf)),  imag(tzero(G_dvf)),  'o');
  for i = 1:length(gains)
    set(gca,'ColorOrderIndex',1);
    cl_poles = pole(feedback(G_dvf, (s*gains(i))*eye(6)));
    plot(real(cl_poles), imag(cl_poles), '.');
  end
  ylim([0, 2*pi*500]);
  xlim([-2*pi*500,0]);
  xlabel('Real Part')
  ylabel('Imaginary Part')
  axis square

  subplot(1, 2, 2);
  hold on;
  plot(real(pole(G_dvf)),  imag(pole(G_dvf)),  'x');
  set(gca,'ColorOrderIndex',1);
  plot(real(tzero(G_dvf)),  imag(tzero(G_dvf)),  'o');
  for i = 1:length(gains)
    set(gca,'ColorOrderIndex',1);
    cl_poles = pole(feedback(G_dvf, (s*gains(i))*eye(6)));
    plot(real(cl_poles), imag(cl_poles), '.');
  end
  ylim([0, 2*pi*8]);
  xlim([-2*pi*8,0]);
  xlabel('Real Part')
  ylabel('Imaginary Part')
  axis square
#+end_src

*** Controller
#+begin_src matlab
  Kdvf = -850*s/(1+s/2/pi/1000)*eye(6);
#+end_src


*** HAC Identification
#+begin_src matlab
  %% Name of the Simulink File
  mdl = 'nass_model';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Controller/HAC-DVF/Kx'],  1, 'input');    io_i = io_i + 1; % Control input
  io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror

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

#+begin_src matlab
  isstable(G)
  G = -minreal(G);
  isstable(G)
#+end_src

#+begin_src matlab
  load('mat/stages.mat', 'nano_hexapod');
  Gx = G*inv(nano_hexapod.J');
  Gx.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};

  Gl = nano_hexapod.J*G;
  Gl.OutputName  = {'El1', 'El2', 'El3', 'El4', 'El5', 'El6'};
#+end_src

*** Conclusion
#+begin_important
  DVF can be used instead of IFF.
#+end_important