Add decentralized control file

org/control_decentralized.org

@@ -0,0 +1,888 @@
+#+TITLE: Control in the Frame of the Legs 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: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:
+In this document, we apply some decentralized control to the NASS and see what level of performance can be obtained.
+
+* Decentralized Control
+** Matlab Init                                                     :noexport:
+#+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
+  simulinkproject('../');
+#+end_src
+
+#+begin_src matlab
+  open('nass_model.slx');
+#+end_src
+
+** Control Schematic
+The control architecture is shown in Figure [[fig:decentralized_reference_tracking_L]].
+
+The signals are:
+- $\bm{r}_\mathcal{X}$: wanted position of the sample with respect to the granite
+- $\bm{r}_{\mathcal{X}_n}$: wanted position of the sample with respect to the nano-hexapod
+- $\bm{r}_\mathcal{L}$: wanted length of each of the nano-hexapod's legs
+- $\bm{\tau}$: forces applied in each actuator
+- $\bm{\mathcal{L}}$: measured displacement of each leg
+- $\bm{\mathcal{X}}$: measured position of the sample with respect to the granite
+
+#+begin_src latex :file decentralized_reference_tracking_L.pdf
+  \begin{tikzpicture}
+    % Blocs
+    \node[block={2.0cm}{2.0cm}] (P) {};
+    \coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$);
+    \coordinate[] (outputX) at ($(P.south east)!0.7!(P.north east)$);
+    \coordinate[] (outputL) at ($(P.south east)!0.3!(P.north east)$);
+
+    \node[block, left= of inputF] (K) {$\bm{K}_\mathcal{L}$};
+    \node[addb={+}{}{}{}{-}, left= of K] (subr) {};
+    \node[block, align=center, left= of subr] (J) {Inverse\\Kinematics};
+
+    \node[block, align=center, left= of J] (Ex) {Compute\\Pos. Error};
+
+    % Connections and labels
+    \draw[->] (outputL) -- ++(1, 0) node[above left]{$\bm{\mathcal{L}}$};
+    \draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} -- ++(0, -1) -| (subr.south);
+    \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_\mathcal{L}$};
+    \draw[->] (K.east) -- (inputF) node[above left]{$\bm{\tau}$};
+
+    \draw[->] (outputX) -- ++(1.8, 0) node[above left]{$\bm{\mathcal{X}}$};
+    \draw[->] ($(outputX) + (1.4, 0)$)node[branch]{} -- ++(0, -2.5) -| (Ex.south);
+
+    \draw[->] (Ex.east) -- (J.west) node[above left]{$\bm{r}_{\mathcal{X}_n}$};
+    \draw[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{\mathcal{L}}$};
+    \draw[<-] (Ex.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1, 0);
+
+    % \draw[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{\mathcal{L}}$};
+    % \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_\mathcal{L}$};
+    % \draw[->] (G.east) node[above right]{$\bm{\mathcal{L}}$} -| ($(G.east)+(1, -1)$) -| (subr.south);
+  \end{tikzpicture}
+#+end_src
+
+#+name: fig:decentralized_reference_tracking_L
+#+caption: Decentralized control for reference tracking
+#+RESULTS:
+[[file:figs/decentralized_reference_tracking_L.png]]
+
+** Initialize the Simscape Model
+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', 'ref-track-L');
+  Kl = tf(zeros(6));
+#+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
+
+** Identification of the plant
+Let's identify the transfer function from $\bm{\tau}$ to $\bm{\mathcal{L}}$.
+#+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, '/Controller/Reference-Tracking-L/Sum'], 1, 'openoutput'); io_i = io_i + 1; % Leg length error
+
+  %% Run the linearization
+  G = linearize(mdl, io, 0);
+  G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
+  G.OutputName = {'El1', 'El2', 'El3', 'El4', 'El5', 'El6'};
+#+end_src
+
+** Plant Analysis
+The diagonal and off-diagonal terms of the plant are shown in Figure [[fig:decentralized_control_plant_L]].
+
+We can see that:
+- the diagonal terms have similar dynamics
+- the plant is decoupled at low frequency
+
+#+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(G(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(G(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('location', 'northwest');
+
+  ax3 = subplot(2, 2, 2);
+  hold on;
+  for i = 1:5
+    for j = i+1:6
+      plot(freqs, abs(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
+    end
+  end
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, abs(squeeze(freqresp(G(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(G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
+    end
+  end
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G(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/decentralized_control_plant_L.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+<<plt-matlab>>
+#+end_src
+
+#+name: fig:decentralized_control_plant_L
+#+caption: Transfer Functions from forces applied in each actuator $\tau_i$ to the relative motion of each leg $d\mathcal{L}_i$ ([[./figs/decentralized_control_plant_L.png][png]], [[./figs/decentralized_control_plant_L.pdf][pdf]])
+[[file:figs/decentralized_control_plant_L.png]]
+
+** Controller Design
+The controller consists of:
+- A pure integrator
+- An integrator up to little before the crossover
+- A lead around the crossover
+- A low pass filter with a cut-off frequency 3 times the crossover to increase the gain margin
+
+The obtained loop gains corresponding to the diagonal elements are shown in Figure [[fig:decentralized_control_L_loop_gain]].
+
+#+begin_src matlab
+  wc = 2*pi*20;
+  h = 1.5;
+
+  Kl = diag(1./diag(abs(freqresp(G, wc)))) * ...
+       wc/s * ... % Pure Integrator
+       ((s/wc*2 + 1)/(s/wc*2)) * ... % Integrator up to wc/2
+       1/h * (1 + s/wc*h)/(1 + s/wc/h) * ... % Lead
+       1/(1 + s/3/wc) * ... % Low pass Filter
+       1/(1 + s/3/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(Kl(i, i)*G(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(Kl(i, i)*G(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/decentralized_control_L_loop_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+<<plt-matlab>>
+#+end_src
+
+#+name: fig:decentralized_control_L_loop_gain
+#+caption: Obtained Loop Gain ([[./figs/decentralized_control_L_loop_gain.png][png]], [[./figs/decentralized_control_L_loop_gain.pdf][pdf]])
+[[file:figs/decentralized_control_L_loop_gain.png]]
+
+#+begin_src matlab :exports none :tangle no
+  isstable(feedback(G*Kl, eye(6), -1))
+#+end_src
+
+We add a minus sign to the controller as it is not included in the Simscape model.
+#+begin_src matlab
+  Kl = -Kl;
+#+end_src
+
+** Simulation
+#+begin_src matlab
+  initializeController('type', 'ref-track-L');
+#+end_src
+
+#+begin_src matlab
+  load('mat/conf_simulink.mat');
+  set_param(conf_simulink, 'StopTime', '2');
+#+end_src
+
+#+begin_src matlab
+  sim('nass_model');
+#+end_src
+
+#+begin_src matlab
+  decentralized_L = simout;
+  save('./mat/tomo_exp_decentalized.mat', 'decentralized_L', '-append');
+#+end_src
+
+** Results
+The reference path and the position of the mobile platform are shown in Figure [[fig:decentralized_L_position_errors]].
+
+#+begin_src matlab
+  load('./mat/experiment_tomography.mat', 'tomo_align_dist');
+  load('./mat/tomo_exp_decentalized.mat', 'decentralized_L');
+#+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(decentralized_L.Em.En.Time, decentralized_L.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(decentralized_L.Em.En.Time, decentralized_L.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(decentralized_L.Em.En.Time, decentralized_L.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(decentralized_L.Em.En.Time, decentralized_L.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(decentralized_L.Em.En.Time, decentralized_L.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(decentralized_L.Em.En.Time, decentralized_L.Em.En.Data(:, 6), 'DisplayName', 'HAC-DVF')
+  hold off;
+  xlabel('Time [s]');
+  ylabel('Rz [rad]');
+  legend();
+
+  linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
+  xlim([0.5, inf]);
+#+end_src
+
+#+header: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/decentralized_L_position_errors.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+<<plt-matlab>>
+#+end_src
+
+#+name: fig:decentralized_L_position_errors
+#+caption: Position Errors when using the Decentralized Control Architecture ([[./figs/decentralized_L_position_errors.png][png]], [[./figs/decentralized_L_position_errors.pdf][pdf]])
+[[file:figs/decentralized_L_position_errors.png]]
+
+* HAC-LAC (IFF) Decentralized Control
+** Introduction                                                      :ignore:
+We here add an Active Damping Loop (Integral Force Feedback) prior to using the Decentralized control architecture using $\bm{\mathcal{L}}$.
+
+** Control Schematic
+The control architecture is shown in Figure [[fig:decentralized_reference_tracking_L]].
+
+The signals are:
+- $\bm{r}_\mathcal{X}$: wanted position of the sample with respect to the granite
+- $\bm{r}_{\mathcal{X}_n}$: wanted position of the sample with respect to the nano-hexapod
+- $\bm{r}_\mathcal{L}$: wanted length of each of the nano-hexapod's legs
+- $\bm{\tau}$: forces applied in each actuator
+- $\bm{\mathcal{L}}$: measured displacement of each leg
+- $\bm{\mathcal{X}}$: measured position of the sample with respect to the granite
+
+#+begin_src latex :file decentralized_reference_tracking_iff_L.pdf
+  \begin{tikzpicture}
+    % Blocs
+    \node[block={3.0cm}{3.0cm}] (P) {};
+    \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= 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, align=center, left= of J] (Ex) {Compute\\Pos. Error};
+
+    % 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]{$\bm{\mathcal{L}}$};
+    \draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} -- ++(0, -1) -| (subr.south);
+    \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_\mathcal{L}$};
+    \draw[->] (K.east) -- (addF.west);
+
+    \draw[->] (outputX) -- ++(1.8, 0) node[above left]{$\bm{\mathcal{X}}$};
+    \draw[->] ($(outputX) + (1.4, 0)$)node[branch]{} -- ++(0, -2.5) -| (Ex.south);
+
+    \draw[->] (Ex.east) -- (J.west) node[above left]{$\bm{r}_{\mathcal{X}_n}$};
+    \draw[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{\mathcal{L}}$};
+    \draw[<-] (Ex.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1, 0);
+  \end{tikzpicture}
+#+end_src
+
+#+name: fig:decentralized_reference_tracking_L
+#+caption: Decentralized control for reference tracking
+#+RESULTS:
+[[file:figs/decentralized_reference_tracking_L.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
+  simulinkproject('../');
+#+end_src
+
+#+begin_src matlab
+  open('nass_model.slx');
+#+end_src
+
+** Initialize the Simscape Model
+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', 'ref-track-L');
+  Kl = tf(zeros(6));
+#+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
+
+** Initialization
+#+begin_src matlab
+  initializeController('type', 'ref-track-iff-L');
+  K_iff = tf(zeros(6));
+  Kl = tf(zeros(6));
+#+end_src
+
+** Identification for IFF
+#+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
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  for i = 1:6
+    plot(freqs, abs(squeeze(freqresp(G_iff(i, i), freqs, 'Hz'))));
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
+
+  ax2 = subplot(2, 1, 2);
+  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', 'southwest');
+
+  linkaxes([ax1,ax2],'x');
+#+end_src
+
+** Integral Force Feedback Controller
+#+begin_src matlab
+  w0 = 2*pi*50;
+  K_iff = -5000/s * (s/w0)/(1 + s/w0) * 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(K_iff(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(K_iff(i,i)*G_iff(i, i), freqs, 'Hz'))), 'DisplayName', sprintf('$L_{\\tau,%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', 'southwest');
+
+  linkaxes([ax1,ax2],'x');
+#+end_src
+
+#+begin_src matlab
+  K_iff = -K_iff;
+#+end_src
+
+** Identification of the damped plant
+#+begin_src matlab
+  %% Name of the Simulink  DehaezeFile
+  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, '/Controller/Reference-Tracking-IFF-L/Sum'], 1, 'openoutput'); io_i = io_i + 1; % Leg length error
+
+  %% Run the linearization
+  Gd = linearize(mdl, io, 0);
+  Gd.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
+  Gd.OutputName = {'El1', 'El2', 'El3', 'El4', 'El5', 'El6'};
+#+end_src
+
+#+begin_src matlab :exports none
+  freqs = logspace(1, 4, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 2, 1);
+  hold on;
+  for i = 1:6
+    set(gca,'ColorOrderIndex',i);
+    plot(freqs, abs(squeeze(freqresp(G( i, i), freqs, 'Hz'))));
+    set(gca,'ColorOrderIndex',i);
+    plot(freqs, abs(squeeze(freqresp(Gd(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
+    set(gca,'ColorOrderIndex',i);
+    plot(freqs, 180/pi*angle(squeeze(freqresp(G( i, i), freqs, 'Hz'))), 'DisplayName', sprintf('$d\\mathcal{L}_%i/\\tau_%i$', i, i));
+    set(gca,'ColorOrderIndex',i);
+    plot(freqs, 180/pi*angle(squeeze(freqresp(Gd(i, i), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+  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(i, j), freqs, 'Hz'))),      'color', [0, 0, 0, 0.2]);
+      plot(freqs, abs(squeeze(freqresp(Gd(i, j), freqs, 'Hz'))), '--', 'color', [0, 0, 0, 0.2]);
+    end
+  end
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, abs(squeeze(freqresp(G(1, 1), freqs, 'Hz'))));
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, abs(squeeze(freqresp(Gd(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(G(i, j), freqs, 'Hz'))),        'color', [0, 0, 0, 0.2]);
+      plot(freqs, 180/pi*angle(squeeze(freqresp(Gd(i, j), freqs, 'Hz'))), '--', 'color', [0, 0, 0, 0.2]);
+    end
+  end
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G(1, 1), freqs, 'Hz'))));
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, 180/pi*angle(squeeze(freqresp(Gd(1, 1), freqs, 'Hz'))), '--');
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+
+  linkaxes([ax1,ax2,ax3,ax4],'x');
+#+end_src
+
+
+#+begin_src matlab :exports none
+  freqs = logspace(1, 4, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 2, 1);
+  hold on;
+  for i = 1:6
+    plot(freqs, abs(squeeze(freqresp(G(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(G(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('location', 'northwest');
+
+  ax3 = subplot(2, 2, 2);
+  hold on;
+  for i = 1:5
+    for j = i+1:6
+      plot(freqs, abs(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
+    end
+  end
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, abs(squeeze(freqresp(G(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(G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
+    end
+  end
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G(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*300;
+  h = 3;
+
+  Kl = diag(1./diag(abs(freqresp(Gd, wc)))) * ...
+       ((s/(2*pi*20) + 1)/(s/(2*pi*20))) * ... % Pure Integrator
+       ((s/(2*pi*50) + 1)/(s/(2*pi*50))) * ... % Integrator up to wc/2
+       1/h * (1 + s/wc*h)/(1 + s/wc/h) * ...
+       1/(1 + s/(2*wc)) * ...
+       1/(1 + s/(3*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(Kl(i, i)*Gd(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(Kl(i, i)*Gd(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
+  isstable(feedback(Gd*Kl, eye(6), -1))
+#+end_src
+
+#+begin_src matlab
+  Kl = -Kl;
+#+end_src
+
+
+** Simulation
+#+begin_src matlab
+  initializeController('type', 'ref-track-iff-L');
+#+end_src
+
+#+begin_src matlab
+  load('mat/conf_simulink.mat');
+  set_param(conf_simulink, 'StopTime', '2');
+#+end_src
+
+#+begin_src matlab
+  sim('nass_model');
+#+end_src
+
+#+begin_src matlab
+  decentralized_iff_L = simout;
+  save('./mat/tomo_exp_decentalized.mat', 'decentralized_iff_L', '-append');
+#+end_src
+
+** Results
+#+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(decentralized_iff_L.Em.En.Time, decentralized_iff_L.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(decentralized_iff_L.Em.En.Time, decentralized_iff_L.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(decentralized_iff_L.Em.En.Time, decentralized_iff_L.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(decentralized_iff_L.Em.En.Time, decentralized_iff_L.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(decentralized_iff_L.Em.En.Time, decentralized_iff_L.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(decentralized_iff_L.Em.En.Time, decentralized_iff_L.Em.En.Data(:, 6), 'DisplayName', 'IFF + Decentralized')
+  hold off;
+  xlabel('Time [s]');
+  ylabel('Rz [rad]');
+  legend();
+
+  linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
+  xlim([0.5, inf]);
+#+end_src
+* Conclusion