Add analysis about Cascade Control

org/control_cascade.org

@@ -0,0 +1,658 @@
+#+TITLE: Cascade Control applied on the Simscape Model
+:DRAWER:
+#+STARTUP: overview
+
+#+LANGUAGE: en
+#+EMAIL: dehaeze.thomas@gmail.com
+#+AUTHOR: Dehaeze Thomas
+
+#+HTML_LINK_HOME: ./index.html
+#+HTML_LINK_UP: ./index.html
+
+#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
+#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
+#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/zenburn.css"/>
+#+HTML_HEAD: <script type="text/javascript" src="./js/jquery.min.js"></script>
+#+HTML_HEAD: <script type="text/javascript" src="./js/bootstrap.min.js"></script>
+#+HTML_HEAD: <script type="text/javascript" src="./js/jquery.stickytableheaders.min.js"></script>
+#+HTML_HEAD: <script type="text/javascript" src="./js/readtheorg.js"></script>
+
+#+HTML_MATHJAX: align: center tagside: right font: TeX
+
+#+PROPERTY: header-args:matlab  :session *MATLAB*
+#+PROPERTY: header-args:matlab+ :comments org
+#+PROPERTY: header-args:matlab+ :results none
+#+PROPERTY: header-args:matlab+ :exports both
+#+PROPERTY: header-args:matlab+ :eval no-export
+#+PROPERTY: header-args:matlab+ :output-dir figs
+#+PROPERTY: header-args:matlab+ :tangle no
+#+PROPERTY: header-args:matlab+ :mkdirp yes
+
+#+PROPERTY: header-args:shell  :eval no-export
+
+#+PROPERTY: header-args:latex  :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/org/}{config.tex}")
+#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
+#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
+#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
+#+PROPERTY: header-args:latex+ :results file raw replace
+#+PROPERTY: header-args:latex+ :buffer no
+#+PROPERTY: header-args:latex+ :eval no-export
+#+PROPERTY: header-args:latex+ :exports results
+#+PROPERTY: header-args:latex+ :mkdirp yes
+#+PROPERTY: header-args:latex+ :output-dir figs
+#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png")
+:END:
+
+* Introduction                                                        :ignore:
+The 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] (Kx) {$\bm{K}_\mathcal{X}$};
+
+    \node[block, align=center, left= of Kx] (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) -- (Kx.west) node[above left]{$\bm{r}_{\mathcal{X}}$};
+    \draw[->] (Kx.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
+  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
+#+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/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
+
+#+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
+
+  Kx = (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
+  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
+
+#+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