#+TITLE: Cascade Control applied on the Simscape Model
#+SETUPFILE: ./setup/org-setup-file.org

* Introduction                                                        :ignore:
The control architecture we wish here to study is shown in Figure [[fig:cascade_control_architecture]].

#+begin_src latex :file cascade_control_architecture.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[addb={+}{}{}{}{-}, left= of K] (subr) {};
    \node[block, align=center, left= of subr] (J) {Inverse\\Kinematics};
    \node[block, left= of J] (Kp) {$\bm{K}_\mathcal{X}$};

    \node[block, align=center, left= of Kp] (Ex) {Compute\\Pos. Error};

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

    \draw[->] (outputL) -- ++(1.8, 0);
    \draw[->] ($(outputL) + (1.4, 0)$)node[branch]{} node[above]{$d\bm{\mathcal{L}}$} -- ++(0, -1.2) node(Plinse){} -| (subr.south);
    \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{d\mathcal{L}}$};
    \draw[->] (K.east) -- (addF.west) node[above left=0 and 8pt]{$\bm{\tau}^\prime$};

    \draw[->] (outputX) -- ++(2.6, 0);
    \draw[->] ($(outputX) + (2.2, 0)$)node[branch]{} node[above]{$\bm{\mathcal{X}}$} -- ++(0, -3.0) -| (Ex.south);

    \draw[<-] (Ex.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1, 0);
    \draw[->] (Ex.east) -- (Kp.west) node[above left]{$\bm{r}_{\mathcal{X}}$};
    \draw[->] (Kp.east) -- (J.west) node[above left=0 and 6pt]{$\bm{r}_{\mathcal{X}_n}$};
    \draw[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{d\mathcal{L}}$};

    \begin{scope}[on background layer]
      \node[fit={(P.south-|addF.west) (taum.east|-Kiff.north)}, opacity=0, inner sep=10pt] (Pdamped) {};

      \node[fit={(Pdamped.north-|J.west) (Plinse)}, fill=black!20!white, draw, dashed, inner sep=8pt] (Plin) {};
      \node[anchor={north west}] at (Plin.north west){$P_\text{lin}$};

      \node[fit={(P.south-|addF.west) (taum.east|-Kiff.north)}, fill=black!40!white, draw, dashed, inner sep=10pt] (Pdamped) {};
      \node[anchor={north west}] at (Pdamped.north west){$P_\text{damped}$};
    \end{scope}

  \end{tikzpicture}
#+end_src

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

This cascade control is designed in three steps:
- In section [[sec:lac_iff]]: an active damping controller is designed.
  This is based on the Integral Force Feedback and applied in a decentralized way
- In section [[sec:hac_joint_space]]: a decentralized tracking control is designed in the frame of the legs.
  This controller is based on the displacement of each of the legs
- In section [[sec:primary_controller]]: a controller is designed in the task space in order to follow the wanted reference path corresponding to the sample position with respect to the granite

* 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', 'cascade-hac-lac');
#+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

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

* Low Authority Control - Integral Force Feedback $\bm{K}_\text{IFF}$
<<sec:lac_iff>>
** Identification
Let's first identify the plant for the IFF controller.
#+begin_src matlab
  %% Name of the Simulink File
  mdl = 'nass_model';

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

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

** Plant
#+begin_src matlab :exports none
  freqs = logspace(0, 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', 'northwest');

  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_iff_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

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

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

  figure;
  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
#+end_src

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

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

The maximum damping is obtained for a control gain of $\approx 3000$.

** 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_iff_loop_gain]].
#+begin_src matlab
  w0 = 2*pi*50;
  Kiff = -3000/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_iff_loop_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

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

* High Authority Control in the joint space - $\bm{K}_\mathcal{L}$
<<sec:hac_joint_space>>
** Identification of the damped plant
We now identify the transfer function from $\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 obtain plant is shown in Figure [[fig:cascade_hac_joint_plant]].

We can see that the plant is quite well decoupled.

#+begin_src matlab :exports none
  freqs = logspace(0, 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_hac_joint_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

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


** Controller Design and Loop Gain
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*400; % Bandwidth Bandwidth [rad/s]

  h = 2; % Lead parameter

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

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

#+begin_src matlab :exports none
  freqs = logspace(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

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

#+name: fig:cascade_hac_joint_loop_gain
#+caption: Loop Gain for the High Autority Control in the joint space ([[./figs/cascade_hac_joint_loop_gain.png][png]], [[./figs/cascade_hac_joint_loop_gain.pdf][pdf]])
[[file:figs/cascade_hac_joint_loop_gain.png]]

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

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

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

  %% Run the linearization
  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

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

#+name: fig:cascade_primary_plant
#+caption: Plant for the Primary Controller ([[./figs/cascade_primary_plant.png][png]], [[./figs/cascade_primary_plant.pdf][pdf]])
[[file:figs/cascade_primary_plant.png]]

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

  h = 2; % Lead parameter

  Kp = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * wc/s * (s + 2*pi*5)/s * 1/(1+s/2/pi/20);

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

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:6
    plot(freqs, abs(squeeze(freqresp(Gx(i, i)*Kp(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)*Kp(i,i), freqs, 'Hz'))));
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-180, 180]);
  yticks([-180, -90, 0, 90, 180]);

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

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

#+name: fig:cascade_primary_loop_gain
#+caption: Loop Gain for the primary controller (outer loop) ([[./figs/cascade_primary_loop_gain.png][png]], [[./figs/cascade_primary_loop_gain.pdf][pdf]])
[[file:figs/cascade_primary_loop_gain.png]]

* 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 = simout;
  save('./mat/cascade_hac_lac.mat', 'cascade_hac_lac');
#+end_src

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

#+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);
#+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_ca(:, 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_ca(:, 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_ca(:, 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_ca(:, 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_ca(:, 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_ca(:, 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

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

#+name: fig:cascade_hac_lac_tomography_psd
#+caption: ASD of the position error ([[./figs/cascade_hac_lac_tomography_psd.png][png]], [[./figs/cascade_hac_lac_tomography_psd.pdf][pdf]])
[[file:figs/cascade_hac_lac_tomography_psd.png]]

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2, 3, 1);
  hold on;
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 1))))))
  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ca(:, 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_ca(:, 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_ca(:, 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_ca(:, 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_ca(:, 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_ca(:, 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

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

#+name: fig:cascade_hac_lac_tomography_cas
#+caption: Cumulative Amplitude Spectrum of the position error ([[./figs/cascade_hac_lac_tomography_cas.png][png]], [[./figs/cascade_hac_lac_tomography_cas.pdf][pdf]])
[[file:figs/cascade_hac_lac_tomography_cas.png]]

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2, 3, 1);
  hold on;
  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 1))
  plot(cascade_hac_lac.Em.En.Time, cascade_hac_lac.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.Em.En.Time, cascade_hac_lac.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.Em.En.Time, cascade_hac_lac.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.Em.En.Time, cascade_hac_lac.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.Em.En.Time, cascade_hac_lac.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.Em.En.Time, cascade_hac_lac.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

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

#+name: fig:cascade_hac_lac_tomography
#+caption: Results of the Tomography Experiment ([[./figs/cascade_hac_lac_tomography.png][png]], [[./figs/cascade_hac_lac_tomography.pdf][pdf]])
[[file:figs/cascade_hac_lac_tomography.png]]