#+TITLE: HAC-LAC applied on the Simscape Model
: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:
The position $\bm{\mathcal{X}}$ of the Sample with respect to the granite is measured.

It is then compare to the wanted position of the Sample $\bm{r}_\mathcal{X}$ in order to obtain the position error $\bm{\epsilon}_\mathcal{X}$ of the Sample with respect to a frame attached to the Stewart top platform.

#+begin_src latex :file hac_lac_control_schematic.pdf
  \begin{tikzpicture}
    \node[block={3.0cm}{3.0cm}] (G) {$G$};

    % Input and outputs coordinates
    \coordinate[] (outputX) at ($(G.south east)!0.25!(G.north east)$);
    \coordinate[] (outputL) at ($(G.south east)!0.75!(G.north east)$);

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

    % Blocs
    \node[addb, left= of G] (addF) {};
    \node[block, left=1.2 of addF] (Kx) {$\bm{K}_\mathcal{X}$};
    \node[block={2cm}{2cm}, align=center, left=1.2 of Kx] (subx) {Computes\\Position\\Error};

    \node[block, above= of addF] (Kl) {$\bm{K}_\mathcal{L}$};
    \node[addb={+}{}{}{-}{}, above= of Kl] (subl) {};

    \node[block, align=center, left=0.8 of subl] (invK) {Inverse\\Kinematics};

    % Connections and labels
    \draw[<-] (subx.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-0.8, 0);
    \draw[->] ($(subx.east) + (0.2, 0)$)node[branch]{} |- (invK.west);
    \draw[->] (invK.east) -- (subl.west) node[above left]{$\bm{r}_\mathcal{L}$};
    \draw[->] (subl.south) -- (Kl.north) node[above right]{$\bm{\epsilon}_\mathcal{L}$};
    \draw[->] (Kl.south) -- (addF.north);

    \draw[->] (subx.east) -- (Kx.west) node[above left]{$\bm{\epsilon}_\mathcal{X}$};
    \draw[->] (Kx.east) node[above right]{$\bm{\tau}_\mathcal{X}$} -- (addF.west);
    \draw[->] (addF.east) -- (G.west) node[above left]{$\bm{\tau}$};

    \draw[->] ($(outputL.east) + (0.4, 0)$)node[branch](L){} |- (subl.east);
    \draw[->] ($(outputX.east) + (1.2, 0)$)node[branch]{} -- ++(0, -1.6) -| (subx.south);

    \begin{scope}[on background layer]
      \node[fit={(G.south-|Kl.west) (L|-subl.north)}, fill=black!20!white, draw, dashed, inner sep=8pt] (Ktot) {};
    \end{scope}
  \end{tikzpicture}
#+end_src

#+name: fig:hac_lac_control_schematic
#+caption: HAC-LAC Control Architecture used for the Control of the NASS
#+RESULTS:
[[file:figs/hac_lac_control_schematic.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 piezoelectric hexapod and the sample has a mass of 50kg.
#+begin_src matlab
  initializeNanoHexapod('actuator', 'piezo');
  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

Open Loop.
#+begin_src matlab
  initializeController('type', 'open-loop');
#+end_src

And we put some gravity.
#+begin_src matlab
  initializeSimscapeConfiguration('gravity', true);
#+end_src

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

* Low Authority Control - Direct Velocity Feedback $\bm{K}_\mathcal{L}$
** Introduction                                                      :ignore:
The first loop closed corresponds to a direct velocity feedback loop.

The design of the associated decentralized controller is explained in [[file:active_damping.org][this]] file.

** 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'],    1, 'openinput');               io_i = io_i + 1; % Actuator Inputs
  io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm');  io_i = io_i + 1; % Relative Motion Outputs

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

** Plant
#+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(G_dvf(i,i), 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:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf(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

** Root Locus
#+begin_src matlab :exports none
  gains = logspace(0, 5, 500);

  figure;
  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, (gains(i)*s)*eye(6)));
    plot(real(cl_poles), imag(cl_poles), '.');
  end
  % ylim([0, 1.1*max(imag(pole(G_dvf)))]);
  % xlim([-1.1*max(imag(pole(G_dvf))),0]);
  xlabel('Real Part')
  ylabel('Imaginary Part')
  axis square
#+end_src

** Controller and Loop Gain
#+begin_src matlab
  K_dvf = s*15000/(1 + s/2/pi/10000);
#+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(K_dvf*G_dvf(i,i), 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:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(K_dvf*G_dvf(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
  K_dvf = -K_dvf*eye(6);
#+end_src

* High Authority Control - $\bm{K}_\mathcal{X}$
** Identification of the damped plant
#+begin_src matlab
  Kx = tf(zeros(6));
#+end_src

#+begin_src matlab
  initializeController('type', 'hac-dvf');
#+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, '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

  %% 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

The minus sine is put here because there is already a minus sign included due to the computation of the position error.
#+begin_src matlab
  load('mat/stages.mat', 'nano_hexapod');

  Gx = -G*inv(nano_hexapod.J');
  Gx.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
#+end_src

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

  labels = {'$D_x/\mathcal{F}_x$', '$D_y/\mathcal{F}_y$', '$D_z/\mathcal{F}_z$', '$R_x/\mathcal{M}_x$', '$R_y/\mathcal{M}_y$', '$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

** Controller Design
The controller consists of:
- A pure integrator
- A Second integrator up to half the wanted bandwidth
- A Lead around the cross-over frequency
- A low pass filter with a cut-off equal to two times the wanted bandwidth

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

  h = 1.5; % Lead parameter

  Kx = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * wc/s * ((s/wc*2 + 1)/(s/wc*2)) * (1/(1 + s/wc/2));

  % 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);

  labels = {'$D_x/\mathcal{F}_x$', '$D_y/\mathcal{F}_y$', '$D_z/\mathcal{F}_z$', '$R_x/\mathcal{M}_x$', '$R_y/\mathcal{M}_y$', '$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)*Kx(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)*Kx(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)*Kx(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

#+begin_src matlab
  isstable(feedback(Gx*Kx, eye(6), -1))
#+end_src

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

#+begin_src matlab
  isstable(feedback(G*Kx, eye(6), 1))
#+end_src

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

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

#+begin_src matlab
  save('./mat/tomo_exp_hac_lac.mat', 'simout');
#+end_src

* Results
#+begin_src matlab
  load('./mat/tomo_exp_hac_lac.mat', 'simout');
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(simout.Em.En.Data(:,1),     simout.Em.En.Data(:,2),     'DisplayName', '$\epsilon_{x,y}$ - OL')
  xlabel('X Motion [m]'); ylabel('Y Motion [m]');
  legend();
#+end_src


#+begin_src matlab :exports none
  figure;
  hold on;
  plot3(simout.Em.En.Data(:,1), simout.Em.En.Data(:,2), simout.Em.En.Data(:,3))
  xlabel('X Motion [m]'); ylabel('Y Motion [m]');
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot3(simout.Em.En.Data(:,4), simout.Em.En.Data(:,5), simout.Em.En.Data(:,3))
  xlabel('X Motion [m]'); ylabel('Y Motion [m]');
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(simout.Em.En.Time, simout.Em.En.Data(:,1), 'DisplayName', '$\epsilon_{x}$')
  plot(simout.Em.En.Time, simout.Em.En.Data(:,2), 'DisplayName', '$\epsilon_{y}$')
  plot(simout.Em.En.Time, simout.Em.En.Data(:,3), 'DisplayName', '$\epsilon_{z}$')
  hold off;
  legend();
  xlabel('Time [s]'); ylabel('Position Error [m]');
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(simout.Em.En.Time, simout.Em.En.Data(:,4), 'DisplayName', '$\epsilon_{R_x}$')
  plot(simout.Em.En.Time, simout.Em.En.Data(:,5), 'DisplayName', '$\epsilon_{R_y}$')
  plot(simout.Em.En.Time, simout.Em.En.Data(:,6), 'DisplayName', '$\epsilon_{R_z}$')
  hold off;
  legend();
  xlabel('Time [s]'); ylabel('Orientation Error [rad]');
#+end_src

* Undamped System                                                   :noexport:
<<sec:undamped_system>>

** Introduction                                                      :ignore:

** Matlab Init                                              :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
  <<matlab-dir>>
#+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

** Identification of the plant
*** Initialize the Simulation
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 piezoelectric hexapod and the sample has a mass of 50kg.
#+begin_src matlab
  initializeNanoHexapod('actuator', 'piezo');
  initializeSample('mass', 50);
#+end_src

No disturbances.
#+begin_src matlab
  initializeDisturbances();
#+end_src

We set the references to zero.
#+begin_src matlab
  initializeReferences();
#+end_src

And all the controllers are set to 0.
#+begin_src matlab
  initializeController('type', 'open-loop');
#+end_src

*** Identification
First, we identify the dynamics of the system using the =linearize= function.
#+begin_src matlab
  %% Options for Linearized
  options = linearizeOptions;
  options.SampleTime = 0;

  %% 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, '/Tracking Error'], 1, 'openoutput', [], 'En'); io_i = io_i + 1; % Metrology Outputs

  %% Run the linearization
  G = linearize(mdl, io, options);
  G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
#+end_src

#+begin_src matlab
  load('mat/stages.mat', 'nano_hexapod');
  G_cart = minreal(G*inv(nano_hexapod.J'));
  G_cart.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
#+end_src

#+begin_src matlab
  G_legs = minreal(inv(nano_hexapod.J)*G);
  G_legs.OutputName = {'e1', 'e2', 'e3', 'e4', 'e5', 'e6'};
#+end_src

*** Display TF
#+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(G_cart(i, i), 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:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart(i, i), freqs, 'Hz'))), 'DisplayName', [G_cart.InputName{i}, ' to ', G_cart.OutputName{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', 'southwest');

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

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

#+NAME: fig:plant_G_cart
#+CAPTION: Transfer Function from forces applied by the nano-hexapod to position error ([[./figs/plant_G_cart.png][png]], [[./figs/plant_G_cart.pdf][pdf]])
[[file:figs/plant_G_cart.png]]

#+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(G_legs(['e', num2str(i)], ['Fnl', num2str(i)]), 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:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_legs(['e', num2str(i)], ['Fnl', num2str(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

*** Obtained Plants for Active Damping
#+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(G_iff(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), 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:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff(['Fnlm', num2str(i)], ['Fnl', num2str(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/nass_active_damping_iff_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+NAME: fig:nass_active_damping_iff_plant
#+CAPTION: =G_iff=: IFF Plant ([[./figs/nass_active_damping_iff_plant.png][png]], [[./figs/nass_active_damping_iff_plant.pdf][pdf]])
[[file:figs/nass_active_damping_iff_plant.png]]

#+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(G_dvf(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), 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:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf(['Dnlm', num2str(i)], ['Fnl', num2str(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/nass_active_damping_dvf_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+NAME: fig:nass_active_damping_dvf_plant
#+CAPTION: =G_dvf=: Plant for Direct Velocity Feedback ([[./figs/nass_active_damping_dvf_plant.png][png]], [[./figs/nass_active_damping_dvf_plant.pdf][pdf]])
[[file:figs/nass_active_damping_ine_plant.png]]

#+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(G_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [$\frac{m/s}{N}$]'); set(gca, 'XTickLabel',[]);

  ax2 = subplot(2, 1, 2);
  hold on;
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine(['Vnlm', num2str(i)], ['Fnl', num2str(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/nass_active_damping_inertial_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+NAME: fig:nass_active_damping_inertial_plant
#+CAPTION: Inertial Feedback Plant ([[./figs/nass_active_damping_inertial_plant.png][png]], [[./figs/nass_active_damping_inertial_plant.pdf][pdf]])
[[file:figs/nass_active_damping_inertial_plant.png]]

** Tomography Experiment
*** Simulation
We initialize elements for the tomography experiment.
#+begin_src matlab
  prepareTomographyExperiment();
#+end_src

We change the simulation stop time.
#+begin_src matlab
  load('mat/conf_simulink.mat');
  set_param(conf_simulink, 'StopTime', '3');
#+end_src

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

Finally, we save the simulation results for further analysis
#+begin_src matlab
  save('./mat/active_damping_tomo_exp.mat', 'En', 'Eg', '-append');
#+end_src

*** Results
We load the results of tomography experiments.
#+begin_src matlab
  load('./mat/active_damping_tomo_exp.mat', 'En');
  t = linspace(0, 3, length(En(:,1)));
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(t, En(:,1), 'DisplayName', '$\epsilon_{x}$')
  plot(t, En(:,2), 'DisplayName', '$\epsilon_{y}$')
  plot(t, En(:,3), 'DisplayName', '$\epsilon_{z}$')
  hold off;
  legend();
  xlabel('Time [s]'); ylabel('Position Error [m]');
#+end_src

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

#+NAME: fig:nass_act_damp_undamped_sim_tomo_trans
#+CAPTION: Position Error during tomography experiment - Translations ([[./figs/nass_act_damp_undamped_sim_tomo_trans.png][png]], [[./figs/nass_act_damp_undamped_sim_tomo_trans.pdf][pdf]])
[[file:figs/nass_act_damp_undamped_sim_tomo_trans.png]]

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(t, En(:,4), 'DisplayName', '$\epsilon_{\theta_x}$')
  plot(t, En(:,5), 'DisplayName', '$\epsilon_{\theta_y}$')
  plot(t, En(:,6), 'DisplayName', '$\epsilon_{\theta_z}$')
  hold off;
  xlim([0.5,inf]);
  legend();
  xlabel('Time [s]'); ylabel('Position Error [rad]');
#+end_src

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

#+NAME: fig:nass_act_damp_undamped_sim_tomo_rot
#+CAPTION: Position Error during tomography experiment - Rotations ([[./figs/nass_act_damp_undamped_sim_tomo_rot.png][png]], [[./figs/nass_act_damp_undamped_sim_tomo_rot.pdf][pdf]])
[[file:figs/nass_act_damp_undamped_sim_tomo_rot.png]]

** Verification of the transfer function from nano hexapod to metrology
*** Initialize the Simulation
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 piezoelectric hexapod and the sample has a mass of 50kg.
#+begin_src matlab
  initializeNanoHexapod('actuator', 'piezo');
  initializeSample('mass', 50);
#+end_src

No disturbances.
#+begin_src matlab
  initializeDisturbances('enable', false);
#+end_src

We set the references to zero.
#+begin_src matlab
  initializeReferences();
#+end_src

And all the controllers are set to 0.
#+begin_src matlab
  initializeController('type', 'open-loop');
#+end_src

*** Identification
First, we identify the dynamics of the system using the =linearize= function.
#+begin_src matlab
  %% Options for Linearized
  options = linearizeOptions;
  options.SampleTime = 0;

  %% 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, '/Tracking Error'], 1, 'openoutput', [], 'En'); io_i = io_i + 1; % Metrology Outputs

  %% Run the linearization
  G = linearize(mdl, io, options);
  G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
  G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
#+end_src

#+begin_src matlab
  load('mat/stages.mat', 'nano_hexapod');
  G_cart = minreal(G*inv(nano_hexapod.J'));
  G_cart.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
#+end_src

#+begin_src matlab
  G_legs = minreal(inv(nano_hexapod.J)*G);
  G_legs.OutputName = {'e1', 'e2', 'e3', 'e4', 'e5', 'e6'};
#+end_src

*** Display TF
#+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(G_cart(i, i), 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:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart(i, i), freqs, 'Hz'))), 'DisplayName', [G_cart.InputName{i}, ' to ', G_cart.OutputName{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();

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

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

#+NAME: fig:plant_G_cart
#+CAPTION: Transfer Function from forces applied by the nano-hexapod to position error ([[./figs/plant_G_cart.png][png]], [[./figs/plant_G_cart.pdf][pdf]])
[[file:figs/plant_G_cart.png]]

#+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(G_legs(['e', num2str(i)], ['Fnl', num2str(i)]), 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:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_legs(['e', num2str(i)], ['Fnl', num2str(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

*** Obtained Plants for Active Damping
#+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(G_iff(['Fnlm', num2str(i)], ['Fnl', num2str(i)]), 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:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff(['Fnlm', num2str(i)], ['Fnl', num2str(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/nass_active_damping_iff_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+NAME: fig:nass_active_damping_iff_plant
#+CAPTION: =G_iff=: IFF Plant ([[./figs/nass_active_damping_iff_plant.png][png]], [[./figs/nass_active_damping_iff_plant.pdf][pdf]])
[[file:figs/nass_active_damping_iff_plant.png]]

#+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(G_dvf(['Dnlm', num2str(i)], ['Fnl', num2str(i)]), 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:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf(['Dnlm', num2str(i)], ['Fnl', num2str(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/nass_active_damping_dvf_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+NAME: fig:nass_active_damping_dvf_plant
#+CAPTION: =G_dvf=: Plant for Direct Velocity Feedback ([[./figs/nass_active_damping_dvf_plant.png][png]], [[./figs/nass_active_damping_dvf_plant.pdf][pdf]])
[[file:figs/nass_active_damping_ine_plant.png]]

#+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(G_ine(['Vnlm', num2str(i)], ['Fnl', num2str(i)]), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [$\frac{m/s}{N}$]'); set(gca, 'XTickLabel',[]);

  ax2 = subplot(2, 1, 2);
  hold on;
  for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_ine(['Vnlm', num2str(i)], ['Fnl', num2str(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/nass_active_damping_inertial_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+NAME: fig:nass_active_damping_inertial_plant
#+CAPTION: Inertial Feedback Plant ([[./figs/nass_active_damping_inertial_plant.png][png]], [[./figs/nass_active_damping_inertial_plant.pdf][pdf]])
[[file:figs/nass_active_damping_inertial_plant.png]]