#+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:
The goal here is to study the use of a voice coil based nano-hexapod.
That is to say a nano-hexapod with a very small stiffness.

#+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
  Kx = 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*5; % 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*10)/s * ... % Weak Integrator
       (s + 2*pi*1)/s * ... % Weak Integrator
       1/(1 + s/2/pi/10); % Low pass filter after crossover

  % 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

* 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/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
  Gx = linearize(mdl, io, 0);
  Gx.InputName  = {'rL1', 'rL2', 'rL3', 'rL4', 'rL5', 'rL6'};
  Gx.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
#+end_src

As before, we take the minimum realization.
#+begin_src matlab
  isstable(Gx)
  Gx = -minreal(Gx);
  isstable(Gx)
#+end_src

** Obtained Plant
#+begin_src matlab :exports none
  freqs = logspace(0, 4, 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(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
#+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) * ...
       (s + 2*pi*10)/s * ...
       (s + 2*pi*100)/s * ...
       1/(1+s/2/pi/500); % For Piezo
  % Kx = (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
  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

* 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 = 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
  figure;
  ax1 = subplot(1, 2, 1);
  hold on;
  for i = 1:6
    plot(cascade_hac_lac.u.Time, cascade_hac_lac.u.Data(:, i))
  end
  hold off;
  xlabel('Time [s]'); ylabel('Force applied by the Actuators [N]');

  ax2 = subplot(1, 2, 2);
  hold on;
  for i = 1:6
    plot(cascade_hac_lac_lorentz.u.Time, cascade_hac_lac_lorentz.u.Data(:, i))
  end
  hold off;
  xlabel('Time [s]'); ylabel('Force applied by the Actuators [N]');
#+end_src

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

** 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;
  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)))
  plot(f, sqrt(pxx_vc(:, 2)))
  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)))
  plot(f, sqrt(pxx_vc(:, 3)))
  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)))
  plot(f, sqrt(pxx_vc(:, 4)))
  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)))
  plot(f, sqrt(pxx_vc(:, 5)))
  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')
  plot(f, sqrt(pxx_vc(:, 6)), 'DisplayName', 'Cascade')
  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))))))
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_vc(:, 1))))))
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('CAS $D_x$ [$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;
  xlabel('Frequency [Hz]');
  ylabel('CAS $D_y$ [$m$]');
  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;
  xlabel('Frequency [Hz]');
  ylabel('CAS $D_z$ [$m$]');
  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$ [$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]');
  ylabel('CAS $R_y$ [$rad$]');
  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', 'Cascade')
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('CAS $R_z$ [$rad$]');
  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(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;
  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))
  plot(cascade_hac_lac_lorentz.Em.En.Time, cascade_hac_lac_lorentz.Em.En.Data(:, 2))
  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))
  plot(cascade_hac_lac_lorentz.Em.En.Time, cascade_hac_lac_lorentz.Em.En.Data(:, 3))
  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))
  plot(cascade_hac_lac_lorentz.Em.En.Time, cascade_hac_lac_lorentz.Em.En.Data(:, 4))
  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))
  plot(cascade_hac_lac_lorentz.Em.En.Time, cascade_hac_lac_lorentz.Em.En.Data(:, 5))
  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')
  plot(cascade_hac_lac_lorentz.Em.En.Time, cascade_hac_lac_lorentz.Em.En.Data(:, 6), 'DisplayName', 'Cascade')
  hold off;
  xlabel('Time [s]');
  ylabel('Rz [rad]');
  legend();

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