#+TITLE: Control of the NASS with optimal stiffness
:DRAWER:
#+STARTUP: overview

#+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:

* Low Authority Control - Decentralized Direct Velocity Feedback
** 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
  load('mat/conf_simulink.mat');
  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

#+begin_src matlab
  initializeSimscapeConfiguration();
  initializeDisturbances('enable', false);
  initializeLoggingConfiguration('log', 'none');
#+end_src

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

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

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

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

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

The nano-hexapod has the following leg's stiffness and damping.
#+begin_src matlab
  initializeNanoHexapod('k', 1e5, 'c', 2e2);
#+end_src

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

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Controller'],    1, 'openinput');               io_i = io_i + 1; % Actuator Inputs
  io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm');  io_i = io_i + 1; % Force Sensors
#+end_src

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

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

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

  figure;

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

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

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

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

  gains = logspace(1, 4, 300);

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

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

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

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

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

  figure;

  gains = logspace(1, 4, 100);

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

#+begin_src matlab
  Kdvf = 5e3*s/(1+s/2/pi/1e3)*eye(6);
#+end_src

* Identification of the dynamics for the Primary controller
** Introduction                                                      :ignore:
Let's identify the dynamics from actuator forces $\bm{\tau}$ to displacement as measured by the metrology $\bm{\mathcal{X}}$:
\[ \bm{G}(s) = \frac{\bm{\mathcal{X}}}{\bm{\tau}} \]

Then, we compute both the transfer function from forces applied by the actuators $\bm{\mathcal{F}}$ to the measured position error in the frame of the nano-hexapod $\bm{\epsilon}_{\mathcal{X}_n}$:
\[ \bm{G}_\mathcal{X}(s) = \frac{\bm{\epsilon}_{\mathcal{X}_n}}{\bm{\mathcal{F}}} = \bm{G}(s) \bm{J}^{-T} \]

and from actuator forces $\bm{\tau}$ to position error of each leg $\bm{\epsilon}_\mathcal{L}$:
\[ \bm{G}_\mathcal{L} = \frac{\bm{\epsilon}_\mathcal{L}}{\bm{\tau}} = \bm{J} \bm{G}(s) \]

We identify these dynamics with and without using the DVF controller.

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

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Controller'],     1, 'input');            io_i = io_i + 1; % Actuator Inputs
  io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
#+end_src

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

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

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

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

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

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

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

#+begin_src matlab :exports none
  Kdvf = Kdvf_backup;
#+end_src

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

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

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

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

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

** Obtained dynamics for the Undamped plant                          :ignore:
#+begin_src matlab :exports none
  freqs = logspace(0, 3, 5000);

  figure;

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

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

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

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

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

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

  figure;

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

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

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

* Primary Control in the task space
** Introduction                                                      :ignore:

** Plant in the task space
Let's look $\bm{G}_\mathcal{X}(s)$.

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

  figure;

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

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

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

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

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

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

  figure;

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

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

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

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

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

** Control in the task space
#+begin_src matlab
  Kx = tf(zeros(6));

  h = 2.5;
  Kx(1,1) = 3e7 * ...
            1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
            (s/2/pi/1 + 1)/(s/2/pi/1);

  Kx(2,2) = Kx(1,1);

  h = 2.5;
  Kx(3,3) = 3e7 * ...
            1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
            (s/2/pi/1 + 1)/(s/2/pi/1);
#+end_src

#+begin_src matlab
  h = 1.5;
  Kx(4,4) = 5e5 * ...
            1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
            (s/2/pi/1 + 1)/(s/2/pi/1);

  Kx(5,5) = Kx(4,4);

  h = 1.5;
  Kx(6,6) = 5e4 * ...
            1/h*(s/(2*pi*30/h) + 1)/(s/(2*pi*30*h) + 1) * ...
            (s/2/pi/1 + 1)/(s/2/pi/1);
#+end_src

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

  figure;

  ax1 = subplot(2, 2, 1);
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gm_x{i}(1, 1)*Kx(1,1), freqs, 'Hz'))));
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gm_x{i}(2, 2)*Kx(2,2), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
  title('Loop Gain $x$ and $y$')

  ax2 = subplot(2, 2, 2);
  hold on;
  for i = 1:length(Ms)
      set(gca,'ColorOrderIndex',i);
      plot(freqs, abs(squeeze(freqresp(Gm_x{i}(3, 3)*Kx(3,3), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
  title('Loop Gain $z$')

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

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

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

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

  figure;

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

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

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

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

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

*** Stability
#+begin_src matlab
  for i = 1:length(Ms)
      isstable(feedback(Gm_x{i}*Kx, eye(6), -1))
  end
#+end_src

** Simulation

* Primary Control in the leg space
** Plant in the task space
#+begin_src matlab :exports none
  freqs = logspace(0, 3, 1000);

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:length(Ms)
      for j = 1:6
          set(gca,'ColorOrderIndex',i);
          plot(freqs, abs(squeeze(freqresp(Gm_l{i}(j, j), freqs, 'Hz'))));
      end
  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:length(Ms)
      for j = 1:6
          set(gca,'ColorOrderIndex',i);
          plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_l{i}(j, j), freqs, 'Hz')))));
      end
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-270, 90]);
  yticks([-360:90:360]);

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

** Control in the leg space
#+begin_src matlab
  h = 1.5;
  Kl = 2e7 * eye(6) * ...
       1/h*(s/(2*pi*200/h) + 1)/(s/(2*pi*200*h) + 1) * ...
       1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
       (s/2/pi/10 + 1)/(s/2/pi/10) * ...
       1/(1 + s/2/pi/500);
#+end_src

#+begin_src matlab
  for i = 1:length(Ms)
      isstable(feedback(Gm_l{i}(1,1)*Kl(1,1), 1, -1))
  end
#+end_src

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

  figure;

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

  ax2 = subplot(2, 1, 2);
  hold on;
  for i = 1:length(Ms)
      for j = 1:6
          set(gca,'ColorOrderIndex',i);
          plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_l{i}(j, j)*Kl(j,j), freqs, 'Hz'))));
      end
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-180, 180]);
  yticks([-180, -90, 0, 90, 180]);

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

** Simulations
#+begin_src matlab
  load('mat/stages.mat', 'nano_hexapod');
  K = Kl*nano_hexapod.J;
#+end_src

#+begin_src matlab
  initializeDisturbances('Fty_x', false, 'Fty_z', false);
  initializeSimscapeConfiguration('gravity', false);
  initializeLoggingConfiguration('log', 'all');
#+end_src

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

#+begin_src matlab
  hac_dvf_L = {zeros(length(Ms)), 1};

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

      sim('nass_model');
      hac_dvf_L(i) = {simout};
  end
#+end_src

#+begin_src matlab
  save('./mat/tomo_exp_hac_dvf.mat', 'hac_dvf_L');
#+end_src

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

#+begin_src matlab
  n_av = 4;
  han_win = hanning(ceil(length(simout.Em.En.Data(:,1))/n_av));
#+end_src

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

  [pxx_ol, f] = pwelch(tomo_align_dist.Em.En.Data, han_win, [], [], 1/Ts);

  pxx_dvf_L = zeros(length(f), 6, length(Ms));
  for i = 1:length(Ms)
      [pxx, ~] = pwelch(hac_dvf_L{i}.Em.En.Data(ceil(0.2/Ts):end,:), han_win, [], [], 1/Ts);
      pxx_dvf_L(:, :, i) = pxx;
  end
#+end_src

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2, 3, 1);
  hold on;
  plot(f, sqrt(pxx_ol(:, 1)))
  for i = 1:length(Ms)
      plot(f, sqrt(pxx_dvf_L(:, 1, i)))
  end
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('$\Gamma_{D_x}$ [$m/\sqrt{Hz}$]');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');

  ax2 = subplot(2, 3, 2);
  hold on;
  plot(f, sqrt(pxx_ol(:, 2)))
  for i = 1:length(Ms)
      plot(f, sqrt(pxx_dvf_L(:, 2, i)))
  end
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('$\Gamma_{D_y}$ [$m/\sqrt{Hz}$]');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');

  ax3 = subplot(2, 3, 3);
  hold on;
  plot(f, sqrt(pxx_ol(:, 3)))
  for i = 1:length(Ms)
      plot(f, sqrt(pxx_dvf_L(:, 3, i)))
  end
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('$\Gamma_{D_z}$ [$m/\sqrt{Hz}$]');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');

  ax4 = subplot(2, 3, 4);
  hold on;
  plot(f, sqrt(pxx_ol(:, 4)))
  for i = 1:length(Ms)
      plot(f, sqrt(pxx_dvf_L(:, 4, i)))
  end
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('$\Gamma_{R_x}$ [$rad/\sqrt{Hz}$]');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');

  ax5 = subplot(2, 3, 5);
  hold on;
  plot(f, sqrt(pxx_ol(:, 5)))
  for i = 1:length(Ms)
      plot(f, sqrt(pxx_dvf_L(:, 5, i)))
  end
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('$\Gamma_{R_y}$ [$rad/\sqrt{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')
  for i = 1:length(Ms)
      plot(f, sqrt(pxx_dvf_L(:, 6, i)), ...
           'DisplayName', sprintf('HAC-DVF $m = %.0f kg$', Ms(i)))
  end
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('$\Gamma_{R_z}$ [$rad/\sqrt{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

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2, 3, 1);
  hold on;
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 1))))))
  for i = 1:length(Ms)
      plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 1, i))))));
  end
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('CAS $D_x$ [$m$]');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylim([1e-11, 1e-5]);

  ax2 = subplot(2, 3, 2);
  hold on;
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 2))))))
  for i = 1:length(Ms)
      plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 2, i))))));
  end
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('CAS $D_y$ [$m$]');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylim([1e-11, 1e-5]);

  ax3 = subplot(2, 3, 3);
  hold on;
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 3))))))
  for i = 1:length(Ms)
      plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 3, i))))));
  end
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('CAS $D_z$ [$m$]');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylim([1e-11, 1e-5]);

  ax4 = subplot(2, 3, 4);
  hold on;
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 4))))))
  for i = 1:length(Ms)
      plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 4, i))))));
  end
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('CAS $R_x$ [$rad$]');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylim([1e-11, 1e-5]);

  ax5 = subplot(2, 3, 5);
  hold on;
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 5))))))
  for i = 1:length(Ms)
      plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 5, i))))));
  end
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('CAS $R_y$ [$rad$]');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylim([1e-11, 1e-5]);

  ax6 = subplot(2, 3, 6);
  hold on;
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 6))))), 'DisplayName', '$\mu$-Station')
  for i = 1:length(Ms)
      plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 6, i))))), ...
           'DisplayName', sprintf('HAC-DVF $m = %.0f kg$', Ms(i)));
  end
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('CAS $R_z$ [$rad$]');
  legend('location', 'southwest');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylim([1e-11, 1e-5]);

  linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
  xlim([f(2), f(end)])
#+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))
  for i = 1:length(Ms)
      plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 1));
  end
  hold off;
  xlabel('Time [s]');
  ylabel('Dx [m]');

  ax2 = subplot(2, 3, 2);
  hold on;
  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 2))
  for i = 1:length(Ms)
      plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 2));
  end
  hold off;
  xlabel('Time [s]');
  ylabel('Dy [m]');

  ax3 = subplot(2, 3, 3);
  hold on;
  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 3))
  for i = 1:length(Ms)
      plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 3));
  end
  hold off;
  xlabel('Time [s]');
  ylabel('Dz [m]');

  ax4 = subplot(2, 3, 4);
  hold on;
  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 4))
  for i = 1:length(Ms)
      plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 4));
  end
  hold off;
  xlabel('Time [s]');
  ylabel('Rx [rad]');

  ax5 = subplot(2, 3, 5);
  hold on;
  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 5))
  for i = 1:length(Ms)
      plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 5));
  end
  hold off;
  xlabel('Time [s]');
  ylabel('Ry [rad]');

  ax6 = subplot(2, 3, 6);
  hold on;
  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 6), ...
        'DisplayName', '$\mu$-Station')
  for i = 1:length(Ms)
      plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 6), ...
           'DisplayName', sprintf('HAC-DVF $m = %.0f kg$', Ms(i)));
  end
  hold off;
  xlabel('Time [s]');
  ylabel('Rz [rad]');
  legend();

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