Work on HAC-DVF control architecture

org/experiments.org

@@ -81,18 +81,12 @@ The nano-hexapod is considered to be a rigid body.
   initializeSample('mass', 1);
 #+end_src
 
-We initialize the reference path for all the stages.
-All stage is set to its zero position except the Spindle which is rotating at 60rpm.
-#+begin_src matlab
-  initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
-#+end_src
-
 No controller is used (Open Loop).
 #+begin_src matlab
   initializeController('type', 'open-loop');
 #+end_src
 
-And we put some gravity.
+We don't gravity.
 #+begin_src matlab
   initializeSimscapeConfiguration('gravity', false);
 #+end_src
@@ -121,6 +115,12 @@ And we initialize the disturbances to be equal to zero.
       );
 #+end_src
 
+We initialize the reference path for all the stages.
+All stage is set to its zero position except the Spindle which is rotating at 60rpm.
+#+begin_src matlab
+  initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
+#+end_src
+
 We simulate the model.
 #+begin_src matlab
   sim('nass_model');
@@ -202,19 +202,25 @@ And we save the obtained data.
 In this section, we also perform a tomography experiment with the sample's center of mass aligned with the rotation axis.
 However this time, we include perturbations such as ground motion and stage vibrations.
 
-** Simulation Setup
+** TODO Simulation Setup
 We now activate the disturbances.
 #+begin_src matlab
   initializeDisturbances(...
       'Dwx', true, ... % Ground Motion - X direction
       'Dwy', true, ... % Ground Motion - Y direction
       'Dwz', true, ... % Ground Motion - Z direction
-      'Fty_x', true, ... % Translation Stage - X direction
+      'Fty_x', false, ... % Translation Stage - X direction
-      'Fty_z', true, ... % Translation Stage - Z direction
+      'Fty_z', false, ... % Translation Stage - Z direction
       'Frz_z', true  ... % Spindle - Z direction
       );
 #+end_src
 
+We initialize the reference path for all the stages.
+All stage is set to its zero position except the Spindle which is rotating at 60rpm.
+#+begin_src matlab
+  initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
+#+end_src
+
 We simulate the model.
 #+begin_src matlab
   sim('nass_model');
@@ -292,11 +298,106 @@ And we save the obtained data.
 [[file:figs/exp_tomo_dist.png]]
 
 ** Conclusion
-
 #+begin_important
   Error motion is what expected from the disturbance measurements.
 #+end_important
 
+* Tomography Experiment with Ty raster scans
+<<sec:tomo_dist_ty_scans>>
+** Introduction                                                     :ignore:
+In this section, we also perform a tomography experiment with scans of the Translation stage.
+All the perturbations are included.
+
+** Simulation Setup
+We now activate the disturbances.
+#+begin_src matlab
+  initializeDisturbances(...
+      'Dwx', true, ... % Ground Motion - X direction
+      'Dwy', true, ... % Ground Motion - Y direction
+      'Dwz', true, ... % Ground Motion - Z direction
+      'Fty_x', true, ... % Translation Stage - X direction
+      'Fty_z', true, ... % Translation Stage - Z direction
+      'Frz_z', true  ... % Spindle - Z direction
+      );
+#+end_src
+We initialize the reference path for all the stages.
+The Spindle which is rotating at 60rpm and the translation stage is following a triangular path.
+#+begin_src matlab
+  initializeReferences('Rz_type', 'rotating', 'Rz_period', 1, ...
+                       'Dy_type', 'triangular', 'Dy_amplitude', 5e-3, 'Dy_period', 10);
+#+end_src
+
+We simulate the model.
+#+begin_src matlab
+  sim('nass_model');
+#+end_src
+
+And we save the obtained data.
+#+begin_src matlab
+  scans_rz_align_dist = simout;
+  save('./mat/experiment_tomography.mat', 'scans_rz_align_dist', '-append');
+#+end_src
+
+** Analysis
+#+begin_src matlab
+  load('./mat/experiment_tomography.mat', 'scans_rz_align_dist');
+#+end_src
+
+#+begin_src matlab :exports none
+  figure;
+  ax1 = subplot(2, 3, 1);
+  hold on;
+  plot(scans_rz_align_dist.Em.Eg.Time, scans_rz_align_dist.Em.Eg.Data(:, 1))
+  hold off;
+  ylabel('Displacement $\epsilon_x$ [m]');
+
+  ax2 = subplot(2, 3, 2);
+  hold on;
+  plot(scans_rz_align_dist.Em.Eg.Time, scans_rz_align_dist.Em.Eg.Data(:, 2))
+  hold off;
+  ylabel('Displacement $\epsilon_y$ [m]');
+
+  ax3 = subplot(2, 3, 3);
+  hold on;
+  plot(scans_rz_align_dist.Em.Eg.Time, scans_rz_align_dist.Em.Eg.Data(:, 3))
+  hold off;
+  ylabel('Displacement $\epsilon_z$ [m]');
+
+  ax4 = subplot(2, 3, 4);
+  hold on;
+  plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 4))
+  hold off;
+  ylabel('Rotation $\epsilon_{R_x}$ [rad]');
+
+  ax5 = subplot(2, 3, 5);
+  hold on;
+  plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 5))
+  hold off;
+  xlabel('Time [s]');
+  ylabel('Rotation $\epsilon_{R_y}$ [rad]');
+
+  ax6 = subplot(2, 3, 6);
+  hold on;
+  plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 6))
+  hold off;
+  ylabel('Rotation $\epsilon_{R_z}$ [rad]');
+
+
+  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/exp_scans_rz_dist.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  <<plt-matlab>>
+#+end_src
+
+#+NAME: fig:exp_scans_rz_dist
+#+CAPTION: X-Y-Z translations and rotations of the sample w.r.t. the granite when performing tomography experiment and scans with the translation stage at the same time ([[./figs/exp_scans_rz_dist.png][png]], [[./figs/exp_scans_rz_dist.pdf][pdf]])
+[[file:figs/exp_scans_rz_dist.png]]
+
+** Conclusion
+
 * Tomography when the micro-hexapod is not centered
 <<sec:tomo_hexa_trans>>
 ** Introduction                                                     :ignore:

org/optimal_stiffness_control.org

@@ -28,7 +28,7 @@
 
 * Introduction                                                        :ignore:
 
-* Low Authority Control - Decentralized Integral Force Feedback
+* Low Authority Control - Decentralized Direct Velocity Feedback
 ** Introduction                                                      :ignore:
 
 ** Matlab Init                                             :noexport:ignore:
@@ -62,30 +62,37 @@ We initialize all the stages with the default parameters.
   initializeMirror();
 #+end_src
 
-We set the references that corresponds to a tomography experiment.
 #+begin_src matlab
-  initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', 1);
   initializeSimscapeConfiguration();
   initializeDisturbances('enable', false);
   initializeLoggingConfiguration('log', 'none');
 #+end_src
 
 #+begin_src matlab
-  initializeController('type', 'hac-iff');
+  initializeController('type', 'hac-dvf');
+#+end_src
+
+We set the stiffness of the payload fixation:
+#+begin_src matlab
+  Kp = 1e8; % [N/m]
 #+end_src
 
 ** Identification
 #+begin_src matlab
-  Kx = tf(zeros(6));
+  K = tf(zeros(6));
-  Kiff = tf(zeros(6));
+  Kdvf = tf(zeros(6));
 #+end_src
 
+We identify the system for the following payload masses:
 #+begin_src matlab
   Ms = [1, 10, 50];
-
-  Gm_iff = {zeros(length(Ms), 1)};
 #+end_src
 
+#+begin_src matlab :exports none
+  Gm_dvf = {zeros(length(Ms), 1)};
+#+end_src
+
+The nano-hexapod has the following leg's stiffness and damping.
 #+begin_src matlab
   initializeNanoHexapod('k', 1e5, 'c', 2e2);
 #+end_src
@@ -97,25 +104,22 @@ We set the references that corresponds to a tomography experiment.
   %% 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
+  io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm');  io_i = io_i + 1; % Force Sensors
 #+end_src
 
 #+begin_src matlab :exports none
   for i = 1:length(Ms)
-    initializeSample('mass', Ms(i), 'freq', 200*ones(6,1));
+    initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
+    initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
 
     %% Run the linearization
-    G_iff = linearize(mdl, io);
+    G_dvf = linearize(mdl, io);
-    G_iff.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
+    G_dvf.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
-    G_iff.OutputName = {'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'};
+    G_dvf.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
-    Gm_iff(i) = {G_iff};
+    Gm_dvf(i) = {G_dvf};
   end
 #+end_src
 
-#+begin_src matlab :exports none
-  save('./mat/optimal_stiffness_control.mat', 'Gm_iff');
-#+end_src
-
 ** Controller Design
 #+begin_src matlab :exports none
   freqs = logspace(-1, 3, 1000);
@@ -125,17 +129,16 @@ We set the references that corresponds to a tomography experiment.
   ax1 = subplot(2, 1, 1);
   hold on;
   for i = 1:length(Ms)
-    plot(freqs, abs(squeeze(freqresp(Gm_iff{i}(1, 1), freqs, 'Hz'))));
+    plot(freqs, abs(squeeze(freqresp(Gm_dvf{i}(1, 1), freqs, 'Hz'))));
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
-  title('Diagonal elements of the Plant');
 
   ax2 = subplot(2, 1, 2);
   hold on;
   for i = 1:length(Ms)
-    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_iff{i}(1, 1), freqs, 'Hz')))), ...
+    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_dvf{i}(1, 1), freqs, 'Hz')))), ...
          'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
   end
   hold off;
@@ -152,19 +155,19 @@ Root Locus
 #+begin_src matlab :exports none :post
   figure;
 
-  gains = logspace(0, 3, 300);
+  gains = logspace(1, 4, 300);
 
   hold on;
   for i = 1:length(Ms)
       set(gca,'ColorOrderIndex',i);
-      plot(real(pole(Gm_iff{i})),  imag(pole(Gm_iff{i})), 'x', ...
+      plot(real(pole(Gm_dvf{i})),  imag(pole(Gm_dvf{i})), 'x', ...
            'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
       set(gca,'ColorOrderIndex',i);
-      plot(real(tzero(Gm_iff{i})),  imag(tzero(Gm_iff{i})), 'o', ...
+      plot(real(tzero(Gm_dvf{i})),  imag(tzero(Gm_dvf{i})), 'o', ...
            'HandleVisibility', 'off');
       for k = 1:length(gains)
           set(gca,'ColorOrderIndex',i);
-          cl_poles = pole(feedback(Gm_iff{i}, -(gains(k)/s)*eye(6)));
+          cl_poles = pole(feedback(Gm_dvf{i}, (gains(k)*s)*eye(6)));
           plot(real(cl_poles), imag(cl_poles), '.', ...
                'HandleVisibility', 'off');
       end
@@ -178,13 +181,13 @@ Root Locus
 #+end_src
 
 #+begin_src matlab :tangle no :exports results :results file replace
-  exportFig('figs/opt_stiff_iff_root_locus.pdf', 'width', 'wide', 'height', 'tall');
+  exportFig('figs/opt_stdvf_dvf_root_locus.pdf', 'width', 'wide', 'height', 'tall');
 #+end_src
 
-#+name: fig:opt_stiff_iff_root_locus
+#+name: fig:opt_stdvf_dvf_root_locus
 #+caption: Root Locus for the
 #+RESULTS:
-[[file:figs/opt_stiff_iff_root_locus.png]]
+[[file:figs/opt_stdvf_dvf_root_locus.png]]
 
 Damping as function of the gain
 #+begin_src matlab :exports none
@@ -199,48 +202,38 @@ Damping as function of the gain
 
   figure;
 
-  gains = logspace(0, 3, 100);
+  gains = logspace(1, 4, 100);
 
   hold on;
   for i = 1:length(Ms)
       for k = 1:length(gains)
-          cl_poles = pole(feedback(Gm_iff{i}, -(gains(k)/s)*eye(6)));
+          cl_poles = pole(feedback(Gm_dvf{i}, (gains(k)*s)*eye(6)));
           set(gca,'ColorOrderIndex',i);
           plot(gains(k), sin(-pi/2 + angle(cl_poles)), '.', 'color', colors(i, :));
       end
   end
   hold off;
-  xlabel('IFF Gain'); ylabel('Modal Damping');
+  xlabel('DVF Gain'); ylabel('Modal Damping');
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
   ylim([0, 1]);
 #+end_src
 
 #+begin_src matlab
-  Kiff = -200/s*eye(6);
+  Kdvf = 5e3*s/(1+s/2/pi/1e3)*eye(6);
 #+end_src
 
-* Primary Control
+* Identification of the dynamics for the Primary controller
 ** Introduction                                                      :ignore:
+Let's identify the dynamics from actuator forces $\bm{\tau}$ to displacement as measured by the metrology $\bm{\mathcal{X}}$:
+\[ \bm{G}(s) = \frac{\bm{\mathcal{X}}}{\bm{\tau}} \]
 
+Then, we compute both the transfer function from forces applied by the actuators $\bm{\mathcal{F}}$ to the measured position error in the frame of the nano-hexapod $\bm{\epsilon}_{\mathcal{X}_n}$:
+\[ \bm{G}_\mathcal{X}(s) = \frac{\bm{\epsilon}_{\mathcal{X}_n}}{\bm{\mathcal{F}}} = \bm{G}(s) \bm{J}^{-T} \]
 
-** Matlab Init                                             :noexport:ignore:
+and from actuator forces $\bm{\tau}$ to position error of each leg $\bm{\epsilon}_\mathcal{L}$:
-#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
+\[ \bm{G}_\mathcal{L} = \frac{\bm{\epsilon}_\mathcal{L}}{\bm{\tau}} = \bm{J} \bm{G}(s) \]
-<<matlab-dir>>
-#+end_src
 
-#+begin_src matlab :exports none :results silent :noweb yes
+We identify these dynamics with and without using the DVF controller.
-<<matlab-init>>
-#+end_src
-
-** Identification
-#+begin_src matlab
-  Gm_x = {zeros(length(Ms), 1)};
-  Gm_l = {zeros(length(Ms), 1)};
-#+end_src
-
-#+begin_src matlab
-  load('mat/stages.mat', 'nano_hexapod');
-#+end_src
 
 #+begin_src matlab :exports none
   %% Name of the Simulink File
@@ -252,9 +245,55 @@ Damping as function of the gain
   io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
 #+end_src
 
+#+begin_src matlab :exports none
+  load('mat/stages.mat', 'nano_hexapod');
+#+end_src
+
+** Identification of the undamped plant                              :ignore:
+#+begin_src matlab :exports none
+  Kdvf_backup = Kdvf;
+  Kdvf = tf(zeros(6));
+#+end_src
+
+#+begin_src matlab :exports none
+  G_x = {zeros(length(Ms), 1)};
+  G_l = {zeros(length(Ms), 1)};
+#+end_src
+
 #+begin_src matlab :exports none
   for i = 1:length(Ms)
-      initializeSample('mass', Ms(i), 'freq', 200*ones(6,1));
+      initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
+    initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
+
+      %% Run the linearization
+      G = linearize(mdl, io);
+      G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
+      G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
+
+      Gx = -G*inv(nano_hexapod.J');
+      Gx.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
+      G_x(i) = {Gx};
+
+      Gl = -nano_hexapod.J*G;
+      Gl.OutputName  = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
+      G_l(i) = {Gl};
+  end
+#+end_src
+
+#+begin_src matlab :exports none
+  Kdvf = Kdvf_backup;
+#+end_src
+
+** Identification of the damped plant                                :ignore:
+#+begin_src matlab :exports none
+  Gm_x = {zeros(length(Ms), 1)};
+  Gm_l = {zeros(length(Ms), 1)};
+#+end_src
+
+#+begin_src matlab :exports none
+  for i = 1:length(Ms)
+      initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
+      initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
 
       %% Run the linearization
       G = linearize(mdl, io);
@@ -271,71 +310,107 @@ Damping as function of the gain
   end
 #+end_src
 
-** Controller in the task space
+** Obtained dynamics for the Undamped plant                          :ignore:
 #+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
+  freqs = logspace(0, 3, 5000);
-
-  labels = {'$D_x/\mathcal{F}_x$', '$D_y/\mathcal{F}_y$', '$D_z/\mathcal{F}_z$', '$R_x/\mathcal{M}_x$', '$R_y/\mathcal{M}_y$', '$R_z/\mathcal{M}_z$'};
 
   figure;
 
   ax1 = subplot(2, 2, 1);
   hold on;
-  for i = 1:6
+  for i = 1:length(Ms)
-    plot(freqs, abs(squeeze(freqresp(Gx(i, i), freqs, 'Hz'))));
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, abs(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz'))));
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, abs(squeeze(freqresp(G_x{i}(2, 2), freqs, 'Hz'))));
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
   ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
-  title('Diagonal elements of the Plant');
+  title('$\mathcal{X}_x/\mathcal{F}_x$, $\mathcal{X}_y/\mathcal{F}_y$')
 
-  ax2 = subplot(2, 2, 3);
+  ax2 = subplot(2, 2, 2);
   hold on;
-  for i = 1:6
+  for i = 1:length(Ms)
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, i), freqs, 'Hz'))), 'DisplayName', labels{i});
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, abs(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz'))));
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+  title('$\mathcal{X}_z/\mathcal{F}_z$')
+
+  ax3 = subplot(2, 2, 3);
+  hold on;
+  for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz')))));
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(2, 2), freqs, 'Hz')))));
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
   ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
+  ylim([-270, 90]);
-  yticks([-180, -90, 0, 90, 180]);
+  yticks([-360:90:360]);
-  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 i = 1:length(Ms)
-    for j = i+1:6
+      set(gca,'ColorOrderIndex',i);
-      plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
+      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz')))), ...
+           'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
   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]);
+  ylim([-270, 90]);
-  yticks([-180, -90, 0, 90, 180]);
+  yticks([-360:90:360]);
+  legend('location', 'southwest');
 
   linkaxes([ax1,ax2,ax3,ax4],'x');
 #+end_src
 
-*** Translation
 #+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
+  freqs = logspace(0, 3, 5000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, abs(squeeze(freqresp(G_l{i}(1, 1), freqs, 'Hz'))));
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_l{i}(1, 1), freqs, 'Hz')))), ...
+           'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-270, 90]);
+  yticks([-360:90:360]);
+  legend('location', 'southwest');
+
+  linkaxes([ax1,ax2],'x');
+#+end_src
+
+* Primary Control in the task space
+** Introduction                                                      :ignore:
+
+** Plant in the task space
+Let's look $\bm{G}_\mathcal{X}(s)$.
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 5000);
 
   figure;
 
@@ -394,85 +469,6 @@ Damping as function of the gain
   linkaxes([ax1,ax2,ax3,ax4],'x');
 #+end_src
 
-#+begin_src matlab
-  Kx = tf(zeros(6));
-
-  h = 1.5;
-  Kx(1,1) = 2e6 * ...
-            1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
-            (s/2/pi/1 + 1)/(s/2/pi/1) * ...
-            (s/2/pi/10 + 1)/(s/2/pi/10);
-
-  Kx(2,2) = Kx(1,1);
-
-  h = 1.5;
-  Kx(3,3) = 1e7 * ...
-            1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
-            (s/2/pi/1 + 1)/(s/2/pi/1) * ...
-            (s/2/pi/10 + 1)/(s/2/pi/10);
-#+end_src
-
-#+begin_src matlab :exports none
-  freqs = logspace(0, 3, 1000);
-
-  figure;
-
-  ax1 = subplot(2, 2, 1);
-  hold on;
-  for i = 1:length(Ms)
-      set(gca,'ColorOrderIndex',i);
-      plot(freqs, abs(squeeze(freqresp(Gm_x{i}(1, 1)*Kx(1,1), freqs, 'Hz'))));
-      set(gca,'ColorOrderIndex',i);
-      plot(freqs, abs(squeeze(freqresp(Gm_x{i}(2, 2)*Kx(2,2), freqs, 'Hz'))));
-  end
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
-  title('$\mathcal{X}_x/\mathcal{F}_x$, $\mathcal{X}_y/\mathcal{F}_y$')
-
-  ax2 = subplot(2, 2, 2);
-  hold on;
-  for i = 1:length(Ms)
-      set(gca,'ColorOrderIndex',i);
-      plot(freqs, abs(squeeze(freqresp(Gm_x{i}(3, 3)*Kx(3,3), freqs, 'Hz'))));
-  end
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
-  title('$\mathcal{X}_z/\mathcal{F}_z$')
-
-  ax3 = subplot(2, 2, 3);
-  hold on;
-  for i = 1:length(Ms)
-      set(gca,'ColorOrderIndex',i);
-      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(1, 1)*Kx(1,1), freqs, 'Hz')))));
-      set(gca,'ColorOrderIndex',i);
-      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(2, 2)*Kx(2,2), freqs, 'Hz')))));
-  end
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-270, 90]);
-  yticks([-360:90:360]);
-
-  ax4 = subplot(2, 2, 4);
-  hold on;
-  for i = 1:length(Ms)
-      set(gca,'ColorOrderIndex',i);
-      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(3, 3)*Kx(3,3), freqs, 'Hz')))), ...
-           'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
-  end
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-270, 90]);
-  yticks([-360:90:360]);
-  legend('location', 'southwest');
-
-  linkaxes([ax1,ax2,ax3,ax4],'x');
-#+end_src
-
-*** Rotations
 #+begin_src matlab :exports none
   freqs = logspace(0, 3, 1000);
 
@@ -533,20 +529,95 @@ Damping as function of the gain
   linkaxes([ax1,ax2,ax3,ax4],'x');
 #+end_src
 
+** Control in the task space
+#+begin_src matlab
+  Kx = tf(zeros(6));
+
+  h = 2.5;
+  Kx(1,1) = 3e7 * ...
+            1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
+            (s/2/pi/1 + 1)/(s/2/pi/1);
+
+  Kx(2,2) = Kx(1,1);
+
+  h = 2.5;
+  Kx(3,3) = 3e7 * ...
+            1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
+            (s/2/pi/1 + 1)/(s/2/pi/1);
+#+end_src
+
 #+begin_src matlab
   h = 1.5;
-  Kx(4,4) = 1e5 * ...
+  Kx(4,4) = 5e5 * ...
             1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
-            (s/2/pi/1 + 1)/(s/2/pi/1) * ...
+            (s/2/pi/1 + 1)/(s/2/pi/1);
-            (s/2/pi/10 + 1)/(s/2/pi/10);
 
   Kx(5,5) = Kx(4,4);
 
   h = 1.5;
-  Kx(6,6) = 2e5 * ...
+  Kx(6,6) = 5e4 * ...
-            1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
+            1/h*(s/(2*pi*30/h) + 1)/(s/(2*pi*30*h) + 1) * ...
-            (s/2/pi/1 + 1)/(s/2/pi/1) * ...
+            (s/2/pi/1 + 1)/(s/2/pi/1);
-            (s/2/pi/10 + 1)/(s/2/pi/10);
+#+end_src
+
+#+begin_src matlab :exports none
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 2, 1);
+  hold on;
+  for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, abs(squeeze(freqresp(Gm_x{i}(1, 1)*Kx(1,1), freqs, 'Hz'))));
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, abs(squeeze(freqresp(Gm_x{i}(2, 2)*Kx(2,2), freqs, 'Hz'))));
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
+  title('Loop Gain $x$ and $y$')
+
+  ax2 = subplot(2, 2, 2);
+  hold on;
+  for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, abs(squeeze(freqresp(Gm_x{i}(3, 3)*Kx(3,3), freqs, 'Hz'))));
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
+  title('Loop Gain $z$')
+
+  ax3 = subplot(2, 2, 3);
+  hold on;
+  for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(1, 1)*Kx(1,1), freqs, 'Hz')))));
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(2, 2)*Kx(2,2), freqs, 'Hz')))));
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-270, 90]);
+  yticks([-360:90:360]);
+
+  ax4 = subplot(2, 2, 4);
+  hold on;
+  for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(3, 3)*Kx(3,3), freqs, 'Hz')))), ...
+           'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-270, 90]);
+  yticks([-360:90:360]);
+  legend('location', 'southwest');
+
+  linkaxes([ax1,ax2,ax3,ax4],'x');
 #+end_src
 
 #+begin_src matlab :exports none
@@ -618,67 +689,8 @@ Damping as function of the gain
 
 ** Simulation
 
-** Control in the leg space
+* Primary Control in the leg space
-#+begin_src matlab :exports none
+** Plant in the task space
-  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
-
 #+begin_src matlab :exports none
   freqs = logspace(0, 3, 1000);
 
@@ -702,24 +714,26 @@ Damping as function of the gain
   for i = 1:length(Ms)
       for j = 1:6
           set(gca,'ColorOrderIndex',i);
-          plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_l{i}(j, j), freqs, 'Hz'))));
+          plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_l{i}(j, j), freqs, 'Hz')))));
       end
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
   ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
+  ylim([-270, 90]);
-  yticks([-180, -90, 0, 90, 180]);
+  yticks([-360:90:360]);
 
   linkaxes([ax1,ax2],'x');
 #+end_src
 
+** Control in the leg space
 #+begin_src matlab
   h = 1.5;
-  Kl = 5e6 * eye(6) * ...
+  Kl = 2e7 * eye(6) * ...
+       1/h*(s/(2*pi*200/h) + 1)/(s/(2*pi*200*h) + 1) * ...
        1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
-       (s/2/pi/1 + 1)/(s/2/pi/1) * ...
+       (s/2/pi/10 + 1)/(s/2/pi/10) * ...
-       (s/2/pi/10 + 1)/(s/2/pi/10);
+       1/(1 + s/2/pi/500);
 #+end_src
 
 #+begin_src matlab
@@ -762,5 +776,281 @@ Damping as function of the gain
   linkaxes([ax1,ax2],'x');
 #+end_src
 
+** Simulations
+#+begin_src matlab
+  load('mat/stages.mat', 'nano_hexapod');
+  K = Kl*nano_hexapod.J;
+#+end_src
 
-* Simulations
+#+begin_src matlab
+  initializeDisturbances('Fty_x', false, 'Fty_z', false);
+  initializeSimscapeConfiguration('gravity', false);
+  initializeLoggingConfiguration('log', 'all');
+#+end_src
+
+#+begin_src matlab
+  load('mat/conf_simulink.mat');
+  set_param(conf_simulink, 'StopTime', '2');
+#+end_src
+
+#+begin_src matlab
+  hac_dvf_L = {zeros(length(Ms)), 1};
+
+  for i = 1:length(Ms)
+      initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
+      initializeReferences('Rz_type', 'rotating', 'Rz_period', Ms(i));
+
+      sim('nass_model');
+      hac_dvf_L(i) = {simout};
+  end
+#+end_src
+
+#+begin_src matlab
+  save('./mat/tomo_exp_hac_dvf.mat', 'hac_dvf_L');
+#+end_src
+
+** Results
+#+begin_src matlab
+  load('./mat/experiment_tomography.mat', 'tomo_align_dist');
+#+end_src
+
+#+begin_src matlab
+  n_av = 4;
+  han_win = hanning(ceil(length(simout.Em.En.Data(:,1))/n_av));
+#+end_src
+
+#+begin_src matlab
+  t = simout.Em.En.Time;
+  Ts = t(2)-t(1);
+
+  [pxx_ol, f] = pwelch(tomo_align_dist.Em.En.Data, han_win, [], [], 1/Ts);
+
+  pxx_dvf_L = zeros(length(f), 6, length(Ms));
+  for i = 1:length(Ms)
+      [pxx, ~] = pwelch(hac_dvf_L{i}.Em.En.Data(ceil(0.2/Ts):end,:), han_win, [], [], 1/Ts);
+      pxx_dvf_L(:, :, i) = pxx;
+  end
+#+end_src
+
+#+begin_src matlab :exports none
+  figure;
+  ax1 = subplot(2, 3, 1);
+  hold on;
+  plot(f, sqrt(pxx_ol(:, 1)))
+  for i = 1:length(Ms)
+      plot(f, sqrt(pxx_dvf_L(:, 1, i)))
+  end
+  hold off;
+  xlabel('Frequency [Hz]');
+  ylabel('$\Gamma_{D_x}$ [$m/\sqrt{Hz}$]');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+
+  ax2 = subplot(2, 3, 2);
+  hold on;
+  plot(f, sqrt(pxx_ol(:, 2)))
+  for i = 1:length(Ms)
+      plot(f, sqrt(pxx_dvf_L(:, 2, i)))
+  end
+  hold off;
+  xlabel('Frequency [Hz]');
+  ylabel('$\Gamma_{D_y}$ [$m/\sqrt{Hz}$]');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+
+  ax3 = subplot(2, 3, 3);
+  hold on;
+  plot(f, sqrt(pxx_ol(:, 3)))
+  for i = 1:length(Ms)
+      plot(f, sqrt(pxx_dvf_L(:, 3, i)))
+  end
+  hold off;
+  xlabel('Frequency [Hz]');
+  ylabel('$\Gamma_{D_z}$ [$m/\sqrt{Hz}$]');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+
+  ax4 = subplot(2, 3, 4);
+  hold on;
+  plot(f, sqrt(pxx_ol(:, 4)))
+  for i = 1:length(Ms)
+      plot(f, sqrt(pxx_dvf_L(:, 4, i)))
+  end
+  hold off;
+  xlabel('Frequency [Hz]');
+  ylabel('$\Gamma_{R_x}$ [$rad/\sqrt{Hz}$]');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+
+  ax5 = subplot(2, 3, 5);
+  hold on;
+  plot(f, sqrt(pxx_ol(:, 5)))
+  for i = 1:length(Ms)
+      plot(f, sqrt(pxx_dvf_L(:, 5, i)))
+  end
+  hold off;
+  xlabel('Frequency [Hz]');
+  ylabel('$\Gamma_{R_y}$ [$rad/\sqrt{Hz}$]');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+
+  ax6 = subplot(2, 3, 6);
+  hold on;
+  plot(f, sqrt(pxx_ol(:, 6)), 'DisplayName', '$\mu$-Station')
+  for i = 1:length(Ms)
+      plot(f, sqrt(pxx_dvf_L(:, 6, i)), ...
+           'DisplayName', sprintf('HAC-DVF $m = %.0f kg$', Ms(i)))
+  end
+  hold off;
+  xlabel('Frequency [Hz]');
+  ylabel('$\Gamma_{R_z}$ [$rad/\sqrt{Hz}$]');
+  legend('location', 'southwest');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+
+  linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
+  xlim([f(2), f(end)])
+#+end_src
+
+#+begin_src matlab :exports none
+  figure;
+  ax1 = subplot(2, 3, 1);
+  hold on;
+  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 1))))))
+  for i = 1:length(Ms)
+      plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 1, i))))));
+  end
+  hold off;
+  xlabel('Frequency [Hz]');
+  ylabel('CAS $D_x$ [$m$]');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylim([1e-11, 1e-5]);
+
+  ax2 = subplot(2, 3, 2);
+  hold on;
+  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 2))))))
+  for i = 1:length(Ms)
+      plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 2, i))))));
+  end
+  hold off;
+  xlabel('Frequency [Hz]');
+  ylabel('CAS $D_y$ [$m$]');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylim([1e-11, 1e-5]);
+
+  ax3 = subplot(2, 3, 3);
+  hold on;
+  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 3))))))
+  for i = 1:length(Ms)
+      plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 3, i))))));
+  end
+  hold off;
+  xlabel('Frequency [Hz]');
+  ylabel('CAS $D_z$ [$m$]');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylim([1e-11, 1e-5]);
+
+  ax4 = subplot(2, 3, 4);
+  hold on;
+  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 4))))))
+  for i = 1:length(Ms)
+      plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 4, i))))));
+  end
+  hold off;
+  xlabel('Frequency [Hz]');
+  ylabel('CAS $R_x$ [$rad$]');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylim([1e-11, 1e-5]);
+
+  ax5 = subplot(2, 3, 5);
+  hold on;
+  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 5))))))
+  for i = 1:length(Ms)
+      plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 5, i))))));
+  end
+  hold off;
+  xlabel('Frequency [Hz]');
+  ylabel('CAS $R_y$ [$rad$]');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylim([1e-11, 1e-5]);
+
+  ax6 = subplot(2, 3, 6);
+  hold on;
+  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 6))))), 'DisplayName', '$\mu$-Station')
+  for i = 1:length(Ms)
+      plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 6, i))))), ...
+           'DisplayName', sprintf('HAC-DVF $m = %.0f kg$', Ms(i)));
+  end
+  hold off;
+  xlabel('Frequency [Hz]');
+  ylabel('CAS $R_z$ [$rad$]');
+  legend('location', 'southwest');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylim([1e-11, 1e-5]);
+
+  linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
+  xlim([f(2), f(end)])
+#+end_src
+
+#+begin_src matlab :exports none
+  figure;
+  ax1 = subplot(2, 3, 1);
+  hold on;
+  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 1))
+  for i = 1:length(Ms)
+      plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 1));
+  end
+  hold off;
+  xlabel('Time [s]');
+  ylabel('Dx [m]');
+
+  ax2 = subplot(2, 3, 2);
+  hold on;
+  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 2))
+  for i = 1:length(Ms)
+      plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 2));
+  end
+  hold off;
+  xlabel('Time [s]');
+  ylabel('Dy [m]');
+
+  ax3 = subplot(2, 3, 3);
+  hold on;
+  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 3))
+  for i = 1:length(Ms)
+      plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 3));
+  end
+  hold off;
+  xlabel('Time [s]');
+  ylabel('Dz [m]');
+
+  ax4 = subplot(2, 3, 4);
+  hold on;
+  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 4))
+  for i = 1:length(Ms)
+      plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 4));
+  end
+  hold off;
+  xlabel('Time [s]');
+  ylabel('Rx [rad]');
+
+  ax5 = subplot(2, 3, 5);
+  hold on;
+  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 5))
+  for i = 1:length(Ms)
+      plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 5));
+  end
+  hold off;
+  xlabel('Time [s]');
+  ylabel('Ry [rad]');
+
+  ax6 = subplot(2, 3, 6);
+  hold on;
+  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 6), ...
+        'DisplayName', '$\mu$-Station')
+  for i = 1:length(Ms)
+      plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 6), ...
+           'DisplayName', sprintf('HAC-DVF $m = %.0f kg$', Ms(i)));
+  end
+  hold off;
+  xlabel('Time [s]');
+  ylabel('Rz [rad]');
+  legend();
+
+  linkaxes([ax1,ax2,ax3,ax4],'x');
+  xlim([0.5, inf]);
+#+end_src