phd-test-bench-id31/test-bench-id31.org

#+TITLE: Nano-Hexapod on the micro-station
:DRAWER:
#+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="https://research.tdehaeze.xyz/css/style.css"/>
#+HTML_HEAD: <script type="text/javascript" src="https://research.tdehaeze.xyz/js/script.js"></script>

#+BIND: org-latex-image-default-option "scale=1"
#+BIND: org-latex-image-default-width ""

#+LaTeX_CLASS: scrreprt
#+LaTeX_CLASS_OPTIONS: [a4paper, 10pt, DIV=12, parskip=full, bibliography=totoc]
#+LATEX_HEADER: \input{preamble.tex}
#+LATEX_HEADER_EXTRA: \input{preamble_extra.tex}
#+LATEX_HEADER_EXTRA: \bibliography{test-bench-id31.bib}

#+BIND: org-latex-bib-compiler "biber"

#+PROPERTY: header-args:matlab  :session *MATLAB*
#+PROPERTY: header-args:matlab+ :comments org
#+PROPERTY: header-args:matlab+ :exports none
#+PROPERTY: header-args:matlab+ :results none
#+PROPERTY: header-args:matlab+ :eval no-export
#+PROPERTY: header-args:matlab+ :noweb yes
#+PROPERTY: header-args:matlab+ :mkdirp yes
#+PROPERTY: header-args:matlab+ :output-dir figs
#+PROPERTY: header-args:matlab+ :tangle no

#+PROPERTY: header-args:latex  :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/tikz/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+ :tangle 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:

#+begin_export html
  <hr>
  <p>This report is also available as a <a href="./test-bench-id31.pdf">pdf</a>.</p>
  <hr>
#+end_export

#+latex: \clearpage

* Build                                                             :noexport:
#+NAME: startblock
#+BEGIN_SRC emacs-lisp :results none :tangle no
(add-to-list 'org-latex-classes
             '("scrreprt"
               "\\documentclass{scrreprt}"
               ("\\chapter{%s}" . "\\chapter*{%s}")
               ("\\section{%s}" . "\\section*{%s}")
               ("\\subsection{%s}" . "\\subsection*{%s}")
               ("\\paragraph{%s}" . "\\paragraph*{%s}")
               ))


;; Remove automatic org heading labels
(defun my-latex-filter-removeOrgAutoLabels (text backend info)
  "Org-mode automatically generates labels for headings despite explicit use of `#+LABEL`. This filter forcibly removes all automatically generated org-labels in headings."
  (when (org-export-derived-backend-p backend 'latex)
    (replace-regexp-in-string "\\\\label{sec:org[a-f0-9]+}\n" "" text)))
(add-to-list 'org-export-filter-headline-functions
             'my-latex-filter-removeOrgAutoLabels)

;; Remove all org comments in the output LaTeX file
(defun delete-org-comments (backend)
  (loop for comment in (reverse (org-element-map (org-element-parse-buffer)
                                    'comment 'identity))
        do
        (setf (buffer-substring (org-element-property :begin comment)
                                (org-element-property :end comment))
              "")))
(add-hook 'org-export-before-processing-hook 'delete-org-comments)

;; Use no package by default
(setq org-latex-packages-alist nil)
(setq org-latex-default-packages-alist nil)

;; Do not include the subtitle inside the title
(setq org-latex-subtitle-separate t)
(setq org-latex-subtitle-format "\\subtitle{%s}")

(setq org-export-before-parsing-hook '(org-ref-glossary-before-parsing
                                       org-ref-acronyms-before-parsing))
#+END_SRC

* Notes                                                             :noexport:
** Notes
Prefix is =test_id31=

data_dir = "/home/thomas/mnt/data_id31/id31nass/id31/20230801/RAW_DATA"
mat_dir = "/home/thomas/mnt/data_id31/nass"

*Goals*:
- Short stroke metrology
- Complete validation of the concept + nano-hexapod + instrumentation + control system
- Experimental validation of complementary filter control

*Outline*:
- Short stroke metrology system
  - Objective: 5DoF measurement while rotation
  - Estimation of angular acceptance
  - Do not care too much about accuracy here
  - Mounting and alignment
  - Jacobian etc...
- Plant identification:
  - Comparison with Simscape
  - Better Rz alignment: use of the model
  - Effect of payload mass
- Robust IFF
- High Authority Control
  - Classical Loop Shaping:
    - Control in the frame of the struts
    - Control in the cartesian frame
  - Complementary Filter Control
  - S/T, noise budget
- Scientific experiments
  - Tomography
  - Lateral Ty scans
  - Dirty layer scans
  - Reflectivity
  - Diffraction tomography

** TODO [#C] Should I speak about higher bandwidth controllers even though they give lower performances?
[[file:backup.org::*High Performance HAC][High Performance HAC]]

** DONE [#A] Copy all necessary .mat files
CLOSED: [2024-11-13 Wed 21:20]

** DONE [#B] Change the name of the I/O in the Simscape model to match the notations
CLOSED: [2024-11-14 Thu 11:32]

Fn => Vs

** TODO [#B] Coherent notation/description of spindle rotation

- RPM
- rpm
- Wz (deg/s)

Make a choice, and then adapt all notations.
Also, change the =initializeReferences= to accept the chosen description instead of =period=.

** TODO [#A] Check why simulation gives worst performances than reality

- [X] Check if low pass filtering the disturbances resolves the issue
  No but it makes the simulation faster!
- [ ] Maybe because the disturbances where estimated at 60RPM and not at 1RPM !

** TODO [#A] Where to discuss the necessity of estimated Rz?

One big advantage of doing the control in the cartesian plane, is that we don't need the estimation of nano-hexapod Rz, therefore we don't need the encoders anymore!

Maybe this should be done in A6 (simscape-nass).
Here it can be reminded when doing the control in the cartesian frame.

** TODO [#B] Explain that RMS values are not filtered

Maybe say that the detectors are integrating the signals (maybe 10ms?).
Then update some RMS values to show that it can be considered better than the values given.

** TODO [#A] Find specifications for each experiment

*Specifications for different directions* (Check this [[file:~/Cloud/work-projects/ID31-NASS/documents/work-package-1/work-package-1.pdf][document]].)
- [ ] Add these specifications in CAS plots (RMS values).

|             | Dx | Dy    | Dz    | Rx | Ry    | Rz       |
|-------------+----+-------+-------+----+-------+----------|
| RMS         |    | 30nm  | 15nm  |    |       | 250 nrad |
| peak 2 peak |    | 200nm | 100nm |    |       | 1.7 urad |
|-------------+----+-------+-------+----+-------+----------|
| MIM         |    | 20nm  | 10nm  |    | 2urad | 1urad    |

- [ ] Also check what was used for the uniaxial model and NASS Simscape model
 *20nmRMS* from the uniaxial model

*Tomography*:
- Beam size: 200nm x 100nm
- Keep the PoI in the beam: peak to peak errors of 200nm in Dy and 100nm in Dz
- RMS errors (/ by 6.6) gives 30nmRMS in Dy and 15nmRMS in Dz.
- Ry error <1.7urad, 250nrad RMS

** TODO [#B] Analyze the observed modes

Added mode compared to Nano-Hexapod test bench:
- Modes of the metrology spheres (between 600 and 700Hz)

- *Sphere modes*
  To identify this mode, look at the transfer function from actuator to individual interferometers.
  It may be possible that this mode does not appear on the bottom interferometers but only on the top one (because of limited stiffness of the top sphere support).

Look at =2023-08-10_18-32_identify_spurious_modes.mat=

** TODO [#A] Explain why position error does not converges to zero during tomography experiments

In closed-loop, the position does not converges to zeros but to a stable position.

This is due to the limited loop-gain at a frequency corresponding to the eccentricity.

Even though there is an integrator in the controller, this integrator is in the frame of the struts, and not in the cartesian frame.

The eccentricity is seen as DC disturbance in the cartesian frame, but at "Wz" frequency in the frame of the strut, hence the non-convergence to zero position.

- *Before 2023-08-11_11-41_tomography_30rpm_m0.mat*: Static errors
- *After 2023-08-11_14-16_m0_1rpm.mat*: No more static errors

Simulation: 30rpm experiment with large off-axis errors (to see if it converges to zero)
#+begin_src matlab
%% Tomography experiment
% Sample is not centered with the rotation axis
% This is done by offsetfing the micro-hexapod by 0.9um
P_micro_hexapod = [10e-6; 0; 0]; % [m]

set_param(mdl, 'StopTime', '3');

initializeMicroHexapod('AP', P_micro_hexapod);
initializeSample('type', '0');

initializeDisturbances(...
    'Dw_x',  true, ... % Ground Motion - X direction
    'Dw_y',  true, ... % Ground Motion - Y direction
    'Dw_z',  true, ... % Ground Motion - Z direction
    'Fdy_x', false, ... % Translation Stage - X direction
    'Fdy_z', false, ... % Translation Stage - Z direction
    'Frz_x', true, ... % Spindle - X direction
    'Frz_y', true, ... % Spindle - Y direction
    'Frz_z', true);    % Spindle - Z direction

initializeReferences(...
    'Rz_type', 'rotating', ...
    'Rz_period', 360/180, ... % 180deg/s, 30rpm
    'Dh_pos', [P_micro_hexapod; 0; 0; 0]);

% Closed-Loop Simulation
load('test_id31_K_iff.mat', 'Kiff');
load('test_id31_K_hac_robust.mat', 'Khac');
initializeController('type', 'hac-iff');
sim(mdl);
exp_tomo_cl_m0_Wz180_offset = simout;
#+end_src

#+begin_src matlab :exports none :results none
%% Measured radial errors of the Spindle
figure;
hold on;
plot(1e6*exp_tomo_cl_m0_Wz180_offset.y.x.Data, 1e6*exp_tomo_cl_m0_Wz180_offset.y.y.Data, 'DisplayName', 'Simulation')
hold off;
xlabel('X displacement [$\mu$m]'); ylabel('Y displacement [$\mu$m]');
axis equal
% xlim([-1, 1]); ylim([-1, 1]);
% xticks([-1, -0.5, 0, 0.5, 1]);
% yticks([-1, -0.5, 0, 0.5, 1]);
leg = legend('location', 'none', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
#+end_src

Yes it converges to zero.

** TODO [#A] Make detailed outline

- [ ] Where to put *noise budget*?
  - Separated (OL, IFF, HAC-IFF)?
  - Or all at once?
  - It was made specifically for tomography experiments
  - Each time, 3 figures (Dy, Dz, Ry)
- [ ] Separate sections for different control strategies?

*Outline*:
- Short stroke metrology
- Open-Loop plant
  - Effect of poor Rz alignment
  - Effect of payload mass, effect of rotation
- IFF
  - Controller design
  - Check robustness
  - Estimated damped plant?
- Robust HAC (frame of the struts)
  - Damped plants
  - Loop Shaping
  - Check stability
  - Noise budget

** DONE [#C] Talk about additional delay observed in the plant from u to d (interf)
CLOSED: [2024-11-15 Fri 18:22]

Explain that this is due to digital conversions using additional electronics, but this is inducing additional delays.
This was latter resolved by directly decoding the correct digital protocol in the Speedgoat.

** TODO [#B] Verify sign of plants

Should be inverse compare to encoder output

** DONE [#A] Make a nice schematic with all the signal names
CLOSED: [2024-11-15 Fri 15:03]

Should probably be similar than the one used in A6 report (Simscape with NASS).

- [X] Make a first schematic with all the signals going in and out of the Speedgoat.
  In this figure, there is nothing about control, kinematics, etc...
  But it would be nice to represent the micro-station + metrology
- [X] Make other figures for LAC, HAC-LAC, but this time really bloc diagrams

- Force sensors: $\bm{V}_s = [V_{s1},\ V_{s2},\ V_{s3},\ V_{s4},\ V_{s5},\ V_{s6}]$
- Encoders: $\bm{d}_e = [d_{e1},\ d_{e2},\ d_{e3},\ d_{e4},\ d_{e5},\ d_{e6}]$
- Interferometers: $\bm{d} = [d_{1},\ d_{2},\ d_{3},\ d_{4},\ d_{5}]$
- Command signal: $\bm{u} = [u_1,\ u_2,\ u_3,\ u_4,\ u_5,\ u_6]$
- Voltage across the piezoelectric stack actuator: $\bm{V}_a = [V_{a1},\ V_{a2},\ V_{a3},\ V_{a4},\ V_{a5},\ V_{a6}]$
- Motion of the sample measured by external metrology: $\bm{\mathcal{X}} = [D_x,\,D_y,\,D_z,\,R_x,\,R_y,\,R_z]$
- Motion of the struts measured by external metrology: $\bm{\mathcal{L}} = [\mathcal{L}_1,\,\mathcal{L}_2,\,\mathcal{L}_3,\,\mathcal{L}_4,\,\mathcal{L}_5,\,\mathcal{L}_6]$
- Error of the sample measured by external metrology: $\bm{\epsilon}_{\mathcal{X}} = [\epsilon_{D_x},\,\epsilon_{D_y},\,\epsilon_{D_z},\,\epsilon_{R_x},\,\epsilon_{R_y},\,\epsilon_{R_z}]$
- Error of the struts measured by external metrology: $\bm{\epsilon}_{\mathcal{L}} = [\epsilon_{\mathcal{L}_1},\,\epsilon_{\mathcal{L}_2},\,\epsilon_{\mathcal{L}_3},\,\epsilon_{\mathcal{L}_4},\,\epsilon_{\mathcal{L}_5},\,\epsilon_{\mathcal{L}_6}]$
- Spindle angle setpoint (or encoder):
- Translation stage setpoint:

** DONE [#A] Resolve height issue in Simscape model
CLOSED: [2024-11-13 Wed 11:53]

For the current (ID31) model, the beam is 175mm above the nano-hexapod top platform.
It should be 150mm (25mm offset).


Check the micro-station model.


Height granite <=> micro-heapod = 530mm (OK Model/Solidworks)
Height of nano-heaxapod = 95mm
Height granite <=> beam = 800 mm

This means that height of nano-hexapod <=> beam is 800 - 530 - 95 = *175mm and not 150mm*.


- [X] *I need to know what was used during the experiments!*
 *150mm* was used during the experiments
- [X] It should be compatible for the Jacobian used for the short stroke metrology
  it seems 150mm was used for the metrology jacobian!
- [X] If something is change, update the previous Simscape models

** CANC [#B] Should the micro-hexapod position be adjusted to match the experiment
CLOSED: [2024-11-13 Wed 18:05]

- State "CANC"       from "TODO"       [2024-11-13 Wed 18:05]
After alignment, the micro-hexapod position was *h1tz = -17.72101mm*.
I suppose compared to the initial height of 350mm

** DONE [#A] Maybe just need one mass for the first identification
CLOSED: [2024-11-13 Wed 18:05]

First identification:
- compare with Simscape
- High coupling
- Check Rz alignment
- Correct Rz alignment
- New identification for all masses
- Better match with Simscape model!

** QUES [#B] Why now we have minimum phase zero for IFF Plant?
** CANC [#C] Find identification where Rz was not taken into account
CLOSED: [2024-11-12 Tue 16:03]

- State "CANC"       from "TODO"       [2024-11-12 Tue 16:03]
- Much larger coupling than estimated from the model
- In the model, suppose Rz = 0?
- Then how to estimate Rz?
  - From voltages
  - From encoders
  - Results and comparison with model

*Maybe not talk about this here as it is not too interesting*

* Introduction                                                        :ignore:

Now that the nano-hexapod is mounted and that a good multi-body model of the nano-hexapod
The system is validated on the ID31 beamline.

At the beginning of the project, it was planned to develop a long stroke 5-DoF metrology system to measure the pose of the sample with respect to the granite.
The development of such system was complex, and was not completed at the time of the experimental tests on ID31.
To still validate the developed nano active platform and the associated instrumentation and control architecture, a 5-DoF short stroke metrology system was developed (Section ref:sec:test_id31_metrology).

The identify dynamics of the nano-hexapod fixed on top of the micro-station was identified for different experimental conditions (payload masses, rotational velocities) and compared with the model (Section ref:sec:test_id31_open_loop_plant).

Decentralized Integral Force Feedback is then applied to actively damp the plant in a robust way (Section ref:sec:test_id31_iff).

High authority control is then applied (Section ref:sec:test_id31_iff_hac).

#+name: fig:test_id31_micro_station_nano_hexapod
#+caption: Picture of the micro-station without the nano-hexapod (\subref{fig:test_id31_micro_station_cables}) and with the nano-hexapod (\subref{fig:test_id31_fixed_nano_hexapod})
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_micro_station_cables}Micro-station and nano-hexapod cables}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_id31_micro_station_cables.jpg]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_fixed_nano_hexapod}Nano-hexapod fixed on top of the micro-station}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_id31_fixed_nano_hexapod.jpg]]
#+end_subfigure
#+end_figure

* Short Stroke Metrology System
:PROPERTIES:
:header-args:matlab+: :tangle matlab/test_id31_1_metrology.m
:END:
<<sec:test_id31_metrology>>
** Introduction                                                      :ignore:

The control of the nano-hexapod requires an external metrology system measuring the relative position of the nano-hexapod top platform with respect to the granite.
As the long-stroke ($\approx 1 \,cm^3$) metrology system was not developed yet, a stroke stroke ($> 100\,\mu m^3$) was used instead to validate the nano-hexapod control.

A first considered option was to use the "Spindle error analyzer" shown in Figure ref:fig:test_id31_lion.
This system comprises 5 capacitive sensors which are facing two reference spheres.
As the gap between the capacitive sensors and the spheres is very small[fn:1], the risk of damaging the spheres and the capacitive sensors is high.

#+name: fig:test_id31_short_stroke_metrology
#+caption: Short stroke metrology system used to measure the sample position with respect to the granite in 5DoF. The system is based on a "Spindle error analyzer" (\subref{fig:test_id31_lion}), but the capacitive sensors are replaced with fibered interferometers (\subref{fig:test_id31_interf}). Interferometer heads are shown in (\subref{fig:test_id31_interf_head})
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_lion}Capacitive Sensors}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.9\linewidth
[[file:figs/test_id31_lion.jpg]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_interf}Short-Stroke metrology}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.9\linewidth
[[file:figs/test_id31_interf.jpg]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_interf_head}Interferometer head}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.9\linewidth
[[file:figs/test_id31_interf_head.jpg]]
#+end_subfigure
#+end_figure

Instead of using capacitive sensors, 5 fibered interferometers were used in a similar way (Figure ref:fig:test_id31_interf).
At the end of each fiber, a sensor head[fn:2] (Figure ref:fig:test_id31_interf_head) is used, which consists of a lens precisely positioned with respect to the fiber's end.
The lens is focusing the light on the surface of the sphere, such that it comes back to the fiber and produces an interference.
This way, the gap between the sensor and the reference sphere is much larger (here around $40\,mm$), removing the risk of collision.

Nevertheless, the metrology system still has limited measurement range, as when the spheres are moving perpendicularly to the beam axis, the reflected light does not coincide with the incident light, and for some perpendicular displacement, the interference is too small to be detected.

** 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 :noweb yes
<<m-init-path>>
#+end_src

#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle>>
#+end_src

#+begin_src matlab :noweb yes
<<m-init-other>>
#+end_src

** Metrology Kinematics
<<ssec:test_id31_metrology_kinematics>>

The developed short-stroke metrology system is schematically shown in Figure ref:fig:test_id31_metrology_kinematics.
The point of interest is indicated by the blue frame $\{B\}$, which is located $H = 150\,mm$ above the nano-hexapod's top platform.
The spheres have a diameter $d = 25.4\,mm$, and indicated dimensions are $l_1 = 60\,mm$ and $l_2 = 16.2\,mm$.
In order to compute the pose of the $\{B\}$ frame with respect to the granite (i.e. with respect to the fixed interferometer heads), the measured small displacements $[d_1,\ d_2,\ d_3,\ d_4,\ d_5]$ by the interferometers are first written as a function of the small linear and angular motion of the $\{B\}$ frame $[D_x,\ D_y,\ D_z,\ R_x,\ R_y]$ eqref:eq:test_id31_metrology_kinematics.

\begin{equation}\label{eq:test_id31_metrology_kinematics}
d_1 = D_y - l_2 R_x, \quad d_2 = D_y + l_1 R_x, \quad d_3 = -D_x - l_2 R_y, \quad d_4 = -D_x + l_1 R_y, \quad d_5 = -D_z
\end{equation}

#+attr_latex: :options [b]{0.48\linewidth}
#+begin_minipage
#+name: fig:test_id31_metrology_kinematics
#+caption: Schematic of the measurement system. Measured distances are indicated by red arrows.
#+attr_latex: :scale 1 :float nil
[[file:figs/test_id31_metrology_kinematics.png]]
#+end_minipage
\hfill
#+attr_latex: :options [b]{0.48\linewidth}
#+begin_minipage
#+name: fig:align_top_sphere_comparators
#+attr_latex: :width \linewidth :float nil
#+caption: The top sphere is aligned with the rotation axis of the spindle using two probes.
[[file:figs/test_id31_align_top_sphere_comparators.jpg]]
#+end_minipage

The five equations eqref:eq:test_id31_metrology_kinematics can be written in a matrix form, and then inverted to have the pose of $\{B\}$ frame as a linear combination of the measured five distances by the interferometers eqref:eq:test_id31_metrology_kinematics_inverse.

\begin{equation}\label{eq:test_id31_metrology_kinematics_inverse}
\begin{bmatrix}
   D_x \\ D_y \\ D_z \\ R_x \\ R_y
\end{bmatrix} = \begin{bmatrix}
   0 &  1 &  0 & -l_2 &  0   \\
   0 &  1 &  0 &  l_1 &  0   \\
  -1 &  0 &  0 &  0   & -l_2 \\
  -1 &  0 &  0 &  0   &  l_1 \\
   0 &  0 & -1 &  0   &  0
\end{bmatrix}^{-1} \cdot \begin{bmatrix}
  d_1 \\ d_2 \\ d_3 \\ d_4 \\ d_5
\end{bmatrix}
\end{equation}

#+begin_src matlab
%% Geometrical parameters of the metrology system
H = 150e-3;
l1 = (150-48-42)*1e-3;
l2 = (76.2+48+42-150)*1e-3;

% Computation of the Transformation matrix
Hm = [ 0  1  0 -l2  0;
       0  1  0  l1  0;
      -1  0  0  0  -l2;
      -1  0  0  0   l1;
       0  0 -1  0   0];
#+end_src

** Rough alignment of the reference spheres
<<ssec:test_id31_metrology_sphere_rought_alignment>>

The two reference spheres are aligned with the rotation axis of the spindle.
To do so, two measuring probes are used as shown in Figure ref:fig:align_top_sphere_comparators.

To not damage the sensitive sphere surface, the probes are instead positioned on the cylinder on which the sphere is mounted.
First, the probes are fixed to the bottom (fixed) cylinder to align its axis with the spindle axis.
Then, the probes are fixed to the top (adjustable) cylinder, and the same alignment is performed.

With this setup, the precision of the alignment of both sphere better with the spindle axis is expected to limited to $\approx 10\,\mu m$.
This is probably limited due to the poor coaxiality between the cylinders and the spheres.
However, the alignment precision should be enough to stay in the acceptance of the interferometers.

** Tip-Tilt adjustment of the interferometers
<<ssec:test_id31_metrology_alignment>>

The short stroke metrology system is placed on top of the main granite using a gantry made of granite blocs to have good vibration and thermal stability (Figure ref:fig:short_stroke_metrology_overview).

#+name: fig:short_stroke_metrology_overview
#+caption: Granite gantry used to fix the short-stroke metrology system
#+attr_latex: :width 0.8\linewidth
[[file:figs/test_id31_short_stroke_metrology_overview.jpg]]

The interferometers need to be aligned with respect to the two reference spheres to approach as much as possible the ideal case shown in Figure ref:fig:test_id31_metrology_kinematics.
The vertical position of the spheres is adjusted using the micro-hexapod to match the height of the interferometers.
Then, the horizontal position of the gantry is adjusted such that the coupling efficiency (i.e. the intensity of the light reflected back in the fiber) of the top interferometer is maximized.
This is equivalent as to optimize the perpendicularity between the interferometer beam and the sphere surface (i.e. the concentricity between the beam and the sphere center).

The lateral sensor heads (i.e. all except the top one), which are each fixed to a custom tip-tilt adjustment mechanism, are individually oriented such that the coupling efficient is maximized.

** Fine Alignment of reference spheres using interferometers
<<ssec:test_id31_metrology_sphere_fine_alignment>>

Thanks to the good alignment of the two reference spheres with the spindle axis and to the fine adjustment of the interferometers orientations, the interferometer measurement is made possible during complete spindle rotation.
This metrology and therefore be used to better align the axis defined by the two spheres' center with the spindle axis.

The alignment process is made by few iterations.
First, the spindle is scanned and the alignment errors are recorded.
From the errors, the motion of the micro-hexapod to better align the spheres is determined and the micro-hexapod is moved.
Then, the spindle is scanned again, and the new alignment errors are recorded.

This iterative process is first perform for angular errors (Figure ref:fig:test_id31_metrology_align_rx_ry) and then for lateral errors (Figure ref:fig:test_id31_metrology_align_dx_dy).
Remaining error after alignment is in the order of $\pm5\,\mu\text{rad}$ for angular errors, $\pm 1\,\mu m$ laterally and less than $0.1\,\mu m$ vertically.

#+begin_src matlab
%% Angular alignment
% Load Data
data_it0 = h5scan(data_dir, 'alignment', 'h1rx_h1ry', 1);
data_it1 = h5scan(data_dir, 'alignment', 'h1rx_h1ry_0002', 3);
data_it2 = h5scan(data_dir, 'alignment', 'h1rx_h1ry_0002', 5);

% Offset wrong points
i_it0 = find(abs(data_it0.Rx_int_filtered(2:end)-data_it0.Rx_int_filtered(1:end-1))>1e-5);
data_it0.Rx_int_filtered(i_it0+1:end) = data_it0.Rx_int_filtered(i_it0+1:end) + data_it0.Rx_int_filtered(i_it0) - data_it0.Rx_int_filtered(i_it0+1);
i_it1 = find(abs(data_it1.Rx_int_filtered(2:end)-data_it1.Rx_int_filtered(1:end-1))>1e-5);
data_it1.Rx_int_filtered(i_it1+1:end) = data_it1.Rx_int_filtered(i_it1+1:end) + data_it1.Rx_int_filtered(i_it1) - data_it1.Rx_int_filtered(i_it1+1);
i_it2 = find(abs(data_it2.Rx_int_filtered(2:end)-data_it2.Rx_int_filtered(1:end-1))>1e-5);
data_it2.Rx_int_filtered(i_it2+1:end) = data_it2.Rx_int_filtered(i_it2+1:end) + data_it2.Rx_int_filtered(i_it2) - data_it2.Rx_int_filtered(i_it2+1);

% Compute circle fit and get radius
[~, ~, R_it0, ~] = circlefit(1e6*data_it0.Rx_int_filtered, 1e6*data_it0.Ry_int_filtered);
[~, ~, R_it1, ~] = circlefit(1e6*data_it1.Rx_int_filtered, 1e6*data_it1.Ry_int_filtered);
[~, ~, R_it2, ~] = circlefit(1e6*data_it2.Rx_int_filtered, 1e6*data_it2.Ry_int_filtered);
#+end_src

#+begin_src matlab :exports none :results none
%% Rx/Ry alignment of the spheres using the micro-station
figure;
hold on;
plot(1e6*data_it0.Rx_int_filtered, 1e6*data_it0.Ry_int_filtered, '-', ...
     'DisplayName', sprintf('$R_0 = %.0f \\mu$rad', R_it0))
plot(1e6*data_it1.Rx_int_filtered, 1e6*data_it1.Ry_int_filtered, '-', ...
     'DisplayName', sprintf('$R_1 = %.0f \\mu$rad', R_it1))
plot(1e6*data_it2.Rx_int_filtered, 1e6*data_it2.Ry_int_filtered, '-', 'color', colors(5,:), ...
     'DisplayName', sprintf('$R_2 = %.0f \\mu$rad', R_it2))
hold off;
xlabel('$R_x$ [$\mu$rad]'); ylabel('$R_y$ [$\mu$rad]');
axis equal
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
xlim([-600, 300]);
ylim([-100, 800]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/test_id31_metrology_align_rx_ry.pdf', 'width', 'half', 'height', 'normal');
#+end_src

#+begin_src matlab
%% Eccentricity alignment
% Load Data
data_it0 = h5scan(data_dir, 'alignment', 'h1rx_h1ry_0002', 5);
data_it1 = h5scan(data_dir, 'alignment', 'h1dx_h1dy', 1);

% Offset wrong points
i_it0 = find(abs(data_it0.Dy_int_filtered(2:end)-data_it0.Dy_int_filtered(1:end-1))>1e-5);
data_it0.Dy_int_filtered(i_it0+1:end) = data_it0.Dy_int_filtered(i_it0+1:end) + data_it0.Dy_int_filtered(i_it0) - data_it0.Dy_int_filtered(i_it0+1);

% Compute circle fit and get radius
[~, ~, R_it0, ~] = circlefit(1e6*data_it0.Dx_int_filtered, 1e6*data_it0.Dy_int_filtered);
[~, ~, R_it1, ~] = circlefit(1e6*data_it1.Dx_int_filtered, 1e6*data_it1.Dy_int_filtered);
#+end_src

#+begin_src matlab :exports none :results none
%% Dx/Dy alignment of the spheres using the micro-station
figure;
hold on;
plot(1e6*data_it0.Dx_int_filtered, 1e6*data_it0.Dy_int_filtered, '-', ...
     'DisplayName', sprintf('$R_0 = %.0f \\mu$m', R_it0))
plot(1e6*data_it1.Dx_int_filtered, 1e6*data_it1.Dy_int_filtered, '-', ...
     'DisplayName', sprintf('$R_1 = %.0f \\mu$m', R_it1))
hold off;
xlabel('$D_x$ [$\mu$m]'); ylabel('$D_y$ [$\mu$m]');
axis equal
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
xlim([-1, 21]);
ylim([-8, 14]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/test_id31_metrology_align_dx_dy.pdf', 'width', 'half', 'height', 'normal');
#+end_src

#+name: fig:test_id31_metrology_align
#+caption: Measured angular (\subref{fig:test_id31_metrology_align_rx_ry}) and lateral (\subref{fig:test_id31_metrology_align_dx_dy}) errors during a full spindle rotation. Between two rotations, the micro-hexapod is adjusted to better align the two spheres with the rotation axis.
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_metrology_align_rx_ry}Angular alignment}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :scale 1
[[file:figs/test_id31_metrology_align_rx_ry.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_metrology_align_dx_dy}Lateral alignment}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :scale 1
[[file:figs/test_id31_metrology_align_dx_dy.png]]
#+end_subfigure
#+end_figure


** Estimated measurement volume
<<ssec:test_id31_metrology_acceptance>>

Because the interferometers are pointing to spheres and not flat surfaces, the lateral acceptance is limited.
In order to estimate the metrology acceptance, the micro-hexapod is used to perform three accurate scans of $\pm 1\,mm$, respectively along the the $x$, $y$ and $z$ axes.
During these scans, the 5 interferometers are recorded, and the ranges in which each interferometer has enough coupling efficiency for measurement are estimated.
Results are summarized in Table ref:tab:test_id31_metrology_acceptance.
The obtained lateral acceptance for pure displacements in any direction is estimated to be around $+/-0.5\,mm$, which is enough for the current application as it is well above the micro-station errors to be actively corrected.

#+begin_src matlab
%% Estimated acceptance of the metrology
% This is estimated by moving the spheres using the micro-hexapod

% Dx
data_dx = h5scan(data_dir, 'metrology_acceptance_new_align', 'dx', 1);

dx_acceptance = zeros(5,1);

for i = [1:size(dx_acceptance, 1)]
    % Find range in which the interferometers are measuring displacement
    dx_di = diff(data_dx.(sprintf('d%i', i))) == 0;
    if sum(dx_di) > 0
        dx_acceptance(i) = data_dx.h1tx(find(dx_di(501:end), 1) + 500) - ...
                           data_dx.h1tx(find(flip(dx_di(1:500)), 1));
    else
        dx_acceptance(i) = data_dx.h1tx(end) - data_dx.h1tx(1);
    end
end

% Dy
data_dy = h5scan(data_dir, 'metrology_acceptance_new_align', 'dy', 1);

dy_acceptance = zeros(5,1);

for i = [1:size(dy_acceptance, 1)]
    % Find range in which the interferometers are measuring displacement
    dy_di = diff(data_dy.(sprintf('d%i', i))) == 0;
    if sum(dy_di) > 0
        dy_acceptance(i) = data_dy.h1ty(find(dy_di(501:end), 1) + 500) - ...
                           data_dy.h1ty(find(flip(dy_di(1:500)), 1));
    else
        dy_acceptance(i) = data_dy.h1ty(end) - data_dy.h1ty(1);
    end
end

% Dz
data_dz = h5scan(data_dir, 'metrology_acceptance_new_align', 'dz', 1);

dz_acceptance = zeros(5,1);

for i = [1:size(dz_acceptance, 1)]
    % Find range in which the interferometers are measuring displacement
    dz_di = diff(data_dz.(sprintf('d%i', i))) == 0;
    if sum(dz_di) > 0
        dz_acceptance(i) = data_dz.h1tz(find(dz_di(501:end), 1) + 500) - ...
                           data_dz.h1tz(find(flip(dz_di(1:500)), 1));
    else
        dz_acceptance(i) = data_dz.h1tz(end) - data_dz.h1tz(1);
    end
end
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
% data2orgtable([dx_acceptance, dy_acceptance, dz_acceptance], {'$d_1$ (y)', '$d_2$ (y)', '$d_3$ (x)', '$d_4$ (x)', '$d_5$ (z)'}, {'$D_x$', '$D_y$', '$D_z$'}, ' %.2f ');
#+end_src

#+name: tab:test_id31_metrology_acceptance
#+caption: Estimated measurement range for each interferometer, and for three different directions.
#+attr_latex: :environment tabularx :width 0.45\linewidth :align Xccc
#+attr_latex: :center t :booktabs t
#+RESULTS:
|           | $D_x$       | $D_y$      | $D_z$ |
|-----------+-------------+------------+-------|
| $d_1$ (y) | $1.0\,mm$   | $>2\,mm$   |  $1.35\,mm$ |
| $d_2$ (y) | $0.8\,mm$   | $>2\,mm$   |  $1.01\,mm$ |
| $d_3$ (x) | $>2\,mm$    | $1.06\,mm$ |  $1.38\,mm$ |
| $d_4$ (x) | $>2\,mm$    | $0.99\,mm$ |  $0.94\,mm$ |
| $d_5$ (z) | $1.33\, mm$ | $1.06\,mm$ |  $>2\,mm$   |


** Estimated measurement errors
<<ssec:test_id31_metrology_errors>>

When using the NASS, the accuracy of the sample's positioning is linked to the accuracy of the external metrology.
However, to validate the nano-hexapod with the associated instrumentation and control architecture, the accuracy of the metrology is not an issue.
Only the bandwidth and noise characteristics of the external metrology are important.
Yet, some elements effecting the accuracy of the metrology are discussed here.

First, the "metrology kinematics" (discussed in Section ref:ssec:test_id31_metrology_kinematics) is only approximate (i.e. valid for very small displacements).
This can be seen when performing lateral $[D_x,\,D_y]$ scans using the micro-hexapod while recording the vertical interferometer (Figure ref:fig:test_id31_xy_map_sphere).
As the interferometer is pointing to a sphere and not to a plane, lateral motion of the sphere is seen as a vertical motion by the top interferometer.

Then, the reference spheres have some deviations with respect to an ideal sphere.
They are meant to be used with capacitive sensors which are integrating the shape errors over large surfaces.
When using interferometers, the size of the "light spot" on the sphere surface is a circle with a diameter $\approx 50\,\mu m$, therefore the system is more sensitive to shape errors with small features.

As the interferometer light is travelling in air, the measured distance is sensitive to any variation in the refractive index of the air.
Therefore, any variation of air temperature, pressure or humidity will induce measurement errors.
For a measurement length of $40\,mm$, a temperature variation of $0.1\,{}^oC$ induces an errors in the distance measurement of $\approx 4\,nm$.

Finally, even in vacuum and in the absence of target motion, the interferometers are affected by noise [[cite:&watchi18_review_compac_inter]].
The effect of the noise on the translation and rotation measurements is estimated in Figure ref:fig:test_id31_interf_noise.

#+begin_src matlab
%% Interferometer noise estimation
data = load("test_id31_interf_noise.mat");

Ts = 1e-4;
Nfft = floor(5/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

[pxx_int, f] = pwelch(detrend(data.d, 0), win, Noverlap, Nfft, 1/Ts);

% Uncorrelated noise: square root of the sum of the squares
pxx_cart = pxx_int*sum(inv(Hm).^2, 2)';

rms_dxy = sqrt(trapz(f(f>1), pxx_cart((f>1),1))); % < 0.3 nm RMS
rms_dz  = sqrt(trapz(f(f>1), pxx_cart((f>1),3))); % < 0.3 nm RMS
rms_rxy = sqrt(trapz(f(f>1), pxx_cart((f>1),4))); % 5 nrad RMS
#+end_src

#+begin_src matlab
figure;
hold on;
plot(f, sqrt(pxx_cart(:,1)), 'DisplayName', sprintf('$D_{x,y}$, %.1f nmRMS', rms_dxy));
plot(f, sqrt(pxx_cart(:,3)), 'DisplayName', sprintf('$D_{z}$, %.1f nmRMS', rms_dz));
plot(f, sqrt(pxx_cart(:,4)), 'DisplayName', sprintf('$R_{x,y}$, %.1f nradRMS', rms_rxy));
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{nm,\ nrad}{\sqrt{Hz}}\right]$')
xlim([1, 1e3]); ylim([1e-3, 1]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/test_id31_interf_noise.pdf', 'width', 'half', 'height', 'normal');
#+end_src


#+begin_src matlab
%% X-Y scan with the micro-hexapod, and record of the vertical interferometer
data = h5scan(data_dir, 'metrology_acceptance', 'after_int_align_meshXY', 1);

x = 1e3*detrend(data.h1tx, 0); % [um]
y = 1e3*detrend(data.h1ty, 0); % [um]
z = 1e6*data.Dz_int_filtered - max(data.Dz_int_filtered); % [um]

mdl = scatteredInterpolant(x, y, z);
[xg, yg] = meshgrid(unique(x), unique(y));
zg = mdl(xg, yg);

% Fit a sphere to the data
[sphere_center,sphere_radius] = sphereFit(1e-3*[x, y, z]);
#+end_src

#+begin_src matlab :exports none :results none
%% XY mapping of the Z measurement by the interferometer
figure;
[~,c] = contour3(xg,yg,zg,30);
c.LineWidth = 3;
xlabel('$D_x$ [$\mu$m]');
ylabel('$D_y$ [$\mu$m]');
zlabel('$D_z$ [$\mu$m]');
zlim([-1, 0]);
xticks(-100:50:100); yticks(-100:50:100); zticks(-1:0.2:0);
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/test_id31_xy_map_sphere.pdf', 'width', 'half', 'height', 'normal');
#+end_src

#+name: fig:test_id31_metrology_errors
#+caption: Estimated measurement errors of the metrology. Cross-coupling between lateral motion and vertical measurement is shown in (\subref{fig:test_id31_xy_map_sphere}). Effect of interferometer noise on the measured translations and rotations is shown in (\subref{fig:test_id31_interf_noise}).
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_xy_map_sphere}Z measurement during an XY mapping}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_id31_xy_map_sphere.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_interf_noise}Interferometer noise}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_id31_interf_noise.png]]
#+end_subfigure
#+end_figure

* Identified Open Loop Plant
:PROPERTIES:
:header-args:matlab+: :tangle matlab/test_id31_2_open_loop_plant.m
:END:
<<sec:test_id31_open_loop_plant>>
** Introduction                                                      :ignore:

- Force sensors: $\bm{V}_s = [V_{s1},\ V_{s2},\ V_{s3},\ V_{s4},\ V_{s5},\ V_{s6}]$
- Encoders: $\bm{d}_e = [d_{e1},\ d_{e2},\ d_{e3},\ d_{e4},\ d_{e5},\ d_{e6}]$
- Interferometers: $\bm{d} = [d_{1},\ d_{2},\ d_{3},\ d_{4},\ d_{5}]$
- Command signal: $\bm{u} = [u_1,\ u_2,\ u_3,\ u_4,\ u_5,\ u_6]$
- Voltage across the piezoelectric stack actuator: $\bm{V}_a = [V_{a1},\ V_{a2},\ V_{a3},\ V_{a4},\ V_{a5},\ V_{a6}]$
- Motion of the sample measured by external metrology: $\bm{y}_\mathcal{X} = [D_x,\,D_y,\,D_z,\,R_x,\,R_y,\,R_z]$
# - Sample motion expressed in the nano-hexapod frame: $\bm{\mathcal{X}} = [\epsilon_{D_x},\,\epsilon_{D_y},\,\epsilon_{D_z},\,\epsilon_{R_x},\,\epsilon_{R_y},\,\epsilon_{R_z}]$
# - Motion of the struts measured by external metrology: $\bm{\mathcal{L}} = [\mathcal{L}_1,\,\mathcal{L}_2,\,\mathcal{L}_3,\,\mathcal{L}_4,\,\mathcal{L}_5,\,\mathcal{L}_6]$
- Error of the sample measured by external metrology: $\bm{\epsilon\mathcal{X}} = [\epsilon_{D_x},\,\epsilon_{D_y},\,\epsilon_{D_z},\,\epsilon_{R_x},\,\epsilon_{R_y},\,\epsilon_{R_z}]$
- Error of the struts measured by external metrology: $\bm{\epsilon\mathcal{L}} = [\epsilon_{\mathcal{L}_1},\,\epsilon_{\mathcal{L}_2},\,\epsilon_{\mathcal{L}_3},\,\epsilon_{\mathcal{L}_4},\,\epsilon_{\mathcal{L}_5},\,\epsilon_{\mathcal{L}_6}]$
- Spindle angle setpoint (or encoder): $r_{R_z}$
- Translation stage setpoint: $r_{D_y}$
- Tilt stage setpoint: $r_{R_y}$

#+begin_src latex :file test_id31_block_schematic_plant.pdf
\begin{tikzpicture}
  % Blocs
  \node[block={2.0cm}{1.0cm}] (metrology) {Metrology};
  \node[block={2.0cm}{2.0cm}, below=0.1 of metrology, align=center] (nhexa) {Nano\\Hexapod};
  \node[block={4.0cm}{1.5cm}, below=0.1 of nhexa, align=center] (ustation) {Micro\\Station};

  \coordinate[] (inputVa)  at ($(nhexa.south west)!0.5!(nhexa.north west)$);
  \coordinate[] (outputVs) at ($(nhexa.south east)!0.3!(nhexa.north east)$);
  \coordinate[] (outputde) at ($(nhexa.south east)!0.7!(nhexa.north east)$);

  \coordinate[] (outputDy) at ($(ustation.south east)!0.1!(ustation.north east)$);
  \coordinate[] (outputRy) at ($(ustation.south east)!0.5!(ustation.north east)$);
  \coordinate[] (outputRz) at ($(ustation.south east)!0.9!(ustation.north east)$);

  \node[block={1.0cm}{1.0cm}, left=0.8 of inputVa] (pd200) {PD200};
  \node[block={1.0cm}{1.0cm}, right=0.8 of outputde] (Rz_kinematics) {$\bm{J}_{R_z}^{-1}$};
  \node[block={2.0cm}{2.0cm}, right=2.2 of ustation, align=center] (ustation_kinematics) {Compute\\Reference\\Position};
  \node[block={2.0cm}{2.0cm}, right=0.8 of ustation_kinematics, align=center] (compute_error) {Compute\\Error\\Position};
  \node[block={2.0cm}{2.0cm}, above=0.8 of compute_error, align=center] (compute_pos) {Compute\\Sample\\Position};
  \node[block={1.0cm}{1.0cm}, right=0.8 of compute_error] (hexa_jacobian) {$\bm{J}$};
  \node[block={1.0cm}{1.0cm}, right=0.8 of metrology] (metrology_kinematics) {$\bm{J}_d^{-1}$};

  \coordinate[] (inputMetrology) at ($(compute_error.north east)!0.3!(compute_error.north west)$);
  \coordinate[] (inputRz) at ($(compute_error.north east)!0.7!(compute_error.north west)$);

  \node[addb={+}{}{}{}{}, right=0.4 of Rz_kinematics] (addRz) {};
  \draw[->] (Rz_kinematics.east) -- (addRz.west);
  \draw[->] (outputRz-|addRz)node[branch]{} -- (addRz.south);

  \draw[->] (outputDy) node[above right]{$r_{D_y}$} -- (outputDy-|ustation_kinematics.west);
  \draw[->] (outputRy) node[above right]{$r_{R_y}$} -- (outputRy-|ustation_kinematics.west);
  \draw[->] (outputRz) node[above right]{$r_{R_z}$} -- (outputRz-|ustation_kinematics.west);

  \draw[->] (outputVs) -- ++(0.8, 0) node[above left]{$\bm{V}_s$};

  \draw[->] (metrology.east) -- (metrology_kinematics.west) node[above left]{$\bm{d}$};

  \draw[->] (metrology_kinematics.east)node[above right]{$[D_x,\,D_y,\,D_z,\,R_x,\,R_y]$} -- (compute_pos.west|-metrology_kinematics);
  \draw[->] (addRz.east)node[above right]{$R_z$} -- (compute_pos.west|-addRz);
  \draw[->] (compute_pos.south)node -- (compute_error.north)node[above right]{$\bm{y}_{\mathcal{X}}$};

  \draw[->] (outputde) -- (Rz_kinematics.west) node[above left]{$\bm{d}_{e}$};
  \draw[->] (ustation_kinematics.east) -- (compute_error.west) node[above left]{$\bm{r}_{\mathcal{X}}$};
  \draw[->] (compute_error.east) -- (hexa_jacobian.west) node[above left]{$\bm{\epsilon\mathcal{X}}$};
  \draw[->] (hexa_jacobian.east) -- ++(0.8, 0) node[above left]{$\bm{\epsilon\mathcal{L}}$};
  \draw[->] (pd200.east) -- (inputVa) node[above left]{$\bm{V}_a$};
  \draw[<-] (pd200.west) -- ++(-0.8, 0) node[above right]{$\bm{u}$};
\end{tikzpicture}
#+end_src

#+name: fig:test_id31_block_schematic_plant
#+caption: Schematic of the
#+RESULTS:
[[file:figs/test_id31_block_schematic_plant.png]]

** Matlab Init                                              :noexport:ignore:
#+begin_src matlab
%% test_id31_2_open_loop_plant.m
#+end_src

#+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 :noweb yes
<<m-init-path>>
#+end_src

#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle>>
#+end_src

#+begin_src matlab :noweb yes
<<m-init-simscape>>
#+end_src

#+begin_src matlab :noweb yes
<<m-init-other>>
#+end_src

** First Open-Loop Plant Identification
<<ssec:test_id31_open_loop_plant_first_id>>

The plant dynamics is first identified for a fixed spindle angle (at $0\,\text{deg}$) and without any payload.
The model dynamics is also identified in the same conditions.

A first comparison between the model and the measured dynamics is done in Figure ref:fig:test_id31_first_id.
A good match can be observed for the diagonal dynamics (except the high frequency modes which are not modeled).
However, the coupling for the transfer function from command signals $\bm{u}$ to estimated strut motion from the external metrology $e\bm{\mathcal{L}}$ is larger than expected (Figure ref:fig:test_id31_first_id_int).

The experimental time delay estimated from the FRF (Figure ref:fig:test_id31_first_id_int) is larger than expected.
After investigation, it was found that the additional delay was due to digital processing unit[fn:3] that was used to read the interferometers in the Speedgoat.
This issue was later solved.

#+begin_src matlab
%% Identify the plant dynamics using the Simscape model

% Initialize each Simscape model elements
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeNanoHexapod('flex_bot_type', '2dof', ...
                      'flex_top_type', '3dof', ...
                      'motion_sensor_type', 'plates', ...
                      'actuator_type', '2dof');
initializeSample('type', '0');

initializeSimscapeConfiguration('gravity', false);
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController('type', 'open-loop');
initializeReferences();

% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],     1, 'openinput');             io_i = io_i + 1; % Actuator Inputs [V]
io(io_i) = linio([mdl, '/Micro-Station'],  3, 'openoutput', [], 'Vs');  io_i = io_i + 1; % Force Sensors voltages [V]
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Position Errors [m]

% With no payload
Gm = exp(-1e-4*s)*linearize(mdl, io);
Gm.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
Gm.OutputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6', ...
                 'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
#+end_src

#+begin_src matlab
%% Identify the plant from experimental data

% Load identification data
data = load('2023-08-08_16-17_ol_plant_m0_Wz0.mat');

% Frequency analysis parameters
Ts = 1e-4; % Sampling Time [s]
Nfft = floor(2.0/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);
[~, f] = tfestimate(data.uL1.id_plant, data.uL1.e_L1, win, Noverlap, Nfft, 1/Ts);

G_iff = zeros(length(f), 6, 6); % Force sensor outputs
G_int = zeros(length(f), 6, 6); % Estimated strut errors
for i_strut = 1:6
    Vs = [data.(sprintf("uL%i", i_strut)).Vs1 ; data.(sprintf("uL%i", i_strut)).Vs2 ; data.(sprintf("uL%i", i_strut)).Vs3 ; data.(sprintf("uL%i", i_strut)).Vs4 ; data.(sprintf("uL%i", i_strut)).Vs5 ; data.(sprintf("uL%i", i_strut)).Vs6]';
    eL = [data.(sprintf("uL%i", i_strut)).e_L1 ; data.(sprintf("uL%i", i_strut)).e_L2 ; data.(sprintf("uL%i", i_strut)).e_L3 ; data.(sprintf("uL%i", i_strut)).e_L4 ; data.(sprintf("uL%i", i_strut)).e_L5 ; data.(sprintf("uL%i", i_strut)).e_L6]';
    dL = [data.(sprintf("uL%i", i_strut)).dL1 ; data.(sprintf("uL%i", i_strut)).dL2 ; data.(sprintf("uL%i", i_strut)).dL3 ; data.(sprintf("uL%i", i_strut)).dL4 ; data.(sprintf("uL%i", i_strut)).dL5 ; data.(sprintf("uL%i", i_strut)).dL6]';

    G_iff(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, Vs, win, Noverlap, Nfft, 1/Ts);
    G_int(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end
#+end_src

#+begin_src matlab :exports none :results none
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:5
    for j = i+1:6
        plot(f, abs(G_int(:, i, j)), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
plot(f, abs(G_int(:, 1, 1)), 'color', [colors(1,:)], ...
     'DisplayName', '$-e\mathcal{L}_i/u_i$ meas');
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
     'DisplayName', '$-e\mathcal{L}_i/u_i$ model');
for i = 2:6
    plot(f, abs(G_int(:,i, i)), 'color', [colors(1,:)], ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
         'HandleVisibility', 'off');
end
plot(f, abs(G_int(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
     'DisplayName', '$-e\mathcal{L}_i/u_j$ meas');
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
     'DisplayName', '$-e\mathcal{L}_i/u_j$ model');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([2e-9, 2e-4]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(f, 180/pi*angle(G_int(:,i, i)), 'color', [colors(1,:)]);
end
for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)]);
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-90, 180])

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

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/test_id31_first_id_int.pdf', 'width', 'half', 'height', 600);
#+end_src

#+begin_src matlab :exports none :results none
%% Comparison between the measured dynamics and the model dynamics - Force Sensors
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:5
    for j = i+1:6
        plot(f, abs(G_iff(:, i, j)), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(Gm(sprintf('Vs%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
plot(f, abs(G_iff(:,1, 1)), 'color', [colors(1,:)], ...
     'DisplayName', '$V_{s,i}/u_i$ meas');
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('Vs%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
     'DisplayName', '$V_{s,i}/u_i$ model');
for i = 2:6
    plot(f, abs(G_iff(:,i, i)), 'color', [colors(1,:)], ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(Gm(sprintf('Vs%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
         'HandleVisibility', 'off');
end
plot(f, abs(G_iff(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
     'DisplayName', '$V_{s,i}/u_j$ meas');
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('Vs%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
     'DisplayName', '$V_{s,i}/u_j$ model');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
ylim([5e-5, 4e1]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i = 1:6
    plot(f, 180/pi*angle(G_iff(:,i, i)), 'color', [colors(1,:)]);
end
for i = 1:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(sprintf('Vs%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)]);
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-90, 180])

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

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/test_id31_first_id_iff.pdf', 'width', 'half', 'height', 600);
#+end_src

#+name: fig:test_id31_first_id
#+caption: Comparison between the measured dynamics and the multi-body model dynamics. Both for the external metrology (\subref{fig:test_id31_first_id_int}) and force sensors (\subref{fig:test_id31_first_id_iff}).
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_first_id_int}External Metrology}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_id31_first_id_int.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_first_id_iff}Force Sensors}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_id31_first_id_iff.png]]
#+end_subfigure
#+end_figure

** Better Angular Alignment
<<ssec:test_id31_open_loop_plant_rz_alignment>>

One possible explanation of the increased coupling observed in Figure ref:fig:test_id31_first_id_int is the poor alignment between the external metrology axes (i.e. the interferometer supports) and the nano-hexapod axes.
To estimate this alignment, a decentralized low-bandwidth feedback controller based on the nano-hexapod encoders is implemented.
This allowed to perform two straight movements of the nano-hexapod along the $x$ and $y$ axes in the frame of the nano-hexapod.
During these two movements, the external metrology measurement is recorded and shown in Figure ref:fig:test_id31_Rz_align_error.
It was found that there is a misalignment of 2.7 degrees (rotation along the vertical axis) between the interferometer axes and nano-hexapod axes.
This was corrected by adding an offset to the spindle angle.
To check that the alignment has improved, the same movement was performed using the nano-hexapod while recording the signal of the external metrology.
Results shown in Figure ref:fig:test_id31_Rz_align_correct are indeed indicating much better alignment.

#+begin_src matlab
%% Load Data
data_1_dx = h5scan(data_dir, 'align_int_enc_Rz', 'tx_first_scan', 2);
data_1_dy = h5scan(data_dir, 'align_int_enc_Rz', 'tx_first_scan', 3);
data_2_dx = h5scan(data_dir, 'align_int_enc_Rz', 'verif-after-correct-offset', 1);
data_2_dy = h5scan(data_dir, 'align_int_enc_Rz', 'verif-after-correct-offset', 2);
#+end_src

#+begin_src matlab
% Estimation of Rz misalignment
p1 = polyfit(data_1_dx.Dx_int_filtered, data_1_dx.Dy_int_filtered, 1);
p2 = polyfit(data_1_dy.Dx_int_filtered, data_1_dy.Dy_int_filtered, 1);

Rz_error = (atan(p1(1)) + atan(p2(1))-pi/2)/2; % ~3 degrees
#+end_src

#+begin_src matlab
%% Estimation of the Rz misalignment
figure;
hold on;
plot(1e6*data_1_dx.Dx_int_filtered, 1e6*data_1_dx.Dy_int_filtered, 'color', colors(2,:), 'DisplayName', 'Measurement')
plot(1e6*data_1_dy.Dx_int_filtered, 1e6*data_1_dy.Dy_int_filtered, 'color', colors(2,:), 'HandleVisibility', 'off')
plot( 1e6*[-10:10]*cos(Rz_error), 1e6*[-10:10]*sin(Rz_error), 'k--', 'DisplayName', sprintf('$\\epsilon_{R_z} = %.1f$ deg', Rz_error*180/pi))
plot(-1e6*[-10:10]*sin(Rz_error), 1e6*[-10:10]*cos(Rz_error), 'k--', 'HandleVisibility', 'off')
hold off;
xlabel('Interf $D_x$ [$\mu$m]');
ylabel('Interf $D_y$ [$\mu$m]');
axis equal
xlim([-10, 10]); ylim([-10, 10]);
xticks([-10:5:10]); yticks([-10:5:10]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/test_id31_Rz_align_error.pdf', 'width', 'half', 'height', 'normal');
#+end_src

#+begin_src matlab
% Estimation of Rz misalignment after correcting the Rz angle
p1 = polyfit(data_2_dx.Dx_int_filtered, data_2_dx.Dy_int_filtered, 1);
p2 = polyfit(data_2_dy.Dx_int_filtered, data_2_dy.Dy_int_filtered, 1);

Rz_error = (atan(p1(1)) + atan(p2(1))-pi/2)/2; % ~0.2 degrees
#+end_src

#+begin_src matlab
%% Estimation of the Rz misalignment after correcting the Rz offset
figure;
hold on;
plot(1e6*data_2_dx.Dx_int_filtered, 1e6*data_2_dx.Dy_int_filtered, 'color', colors(5,:), 'DisplayName', 'Measurement')
plot(1e6*data_2_dy.Dx_int_filtered, 1e6*data_2_dy.Dy_int_filtered, 'color', colors(5,:), 'HandleVisibility', 'off')
plot( 1e6*[-10:10]*cos(Rz_error), 1e6*[-10:10]*sin(Rz_error), 'k--', 'DisplayName', sprintf('$\\epsilon_{R_z} = %.1f$ deg', Rz_error*180/pi))
plot(-1e6*[-10:10]*sin(Rz_error), 1e6*[-10:10]*cos(Rz_error), 'k--', 'HandleVisibility', 'off')
hold off;
xlabel('Interf $D_x$ [$\mu$m]');
ylabel('Interf $D_y$ [$\mu$m]');
axis equal
xlim([-10, 10]); ylim([-10, 10]);
xticks([-10:5:10]); yticks([-10:5:10]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/test_id31_Rz_align_correct.pdf', 'width', 'half', 'height', 'normal');
#+end_src

#+name: fig:test_id31_Rz_align_error
#+caption: Measurement of the Nano-Hexapod axes in the frame of the external metrology. Before alignment (\subref{fig:test_id31_Rz_align_error}) and after alignment (\subref{fig:test_id31_Rz_align_correct}).
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_Rz_align_error}Before alignment}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :scale 1
[[file:figs/test_id31_Rz_align_error.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_Rz_align_correct}After alignment}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :scale 1
[[file:figs/test_id31_Rz_align_correct.png]]
#+end_subfigure
#+end_figure

** Open-Loop Identification after alignment
<<ssec:test_id31_open_loop_plant_after_alignment>>

The plant dynamics is identified after the fine alignment and is compared with the model dynamics in Figure ref:fig:test_id31_first_id_int_better_rz_align.
Compared to the initial identification shown in Figure ref:fig:test_id31_first_id_int, the obtained coupling has decreased and is now close to the coupling obtained with the multi-body model.
At low frequency (below $10\,\text{Hz}$) all the off-diagonal elements have an amplitude $\approx 100$ times lower compared to the diagonal elements, indicating that a low bandwidth feedback controller can be implemented in a decentralized way (i.e. $6$ SISO controllers).

#+begin_src matlab
%% Identification of the plant after Rz alignment
data_align = load('2023-08-17_17-37_ol_plant_m0_Wz0_new_Rz_align.mat');

G_int_align = zeros(length(f), 6, 6);
for i_strut = 1:6
    eL = [data_align.(sprintf("uL%i", i_strut)).e_L1 ; data_align.(sprintf("uL%i", i_strut)).e_L2 ; data_align.(sprintf("uL%i", i_strut)).e_L3 ; data_align.(sprintf("uL%i", i_strut)).e_L4 ; data_align.(sprintf("uL%i", i_strut)).e_L5 ; data_align.(sprintf("uL%i", i_strut)).e_L6]';

    G_int_align(:,:,i_strut) = tfestimate(data_align.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end
#+end_src

#+begin_src matlab :exports none :results none
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');

nexttile();
hold on;
for i = 1:5
    for j = i+1:6
        plot(f, abs(G_int_align(:, i, j)), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
plot(f, abs(G_int_align(:, 1, 1)), 'color', [colors(1,:)], ...
     'DisplayName', '$e\mathcal{L}_i/u_i$ meas');
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
     'DisplayName', '$e\mathcal{L}_i/u_i$ model');
for i = 2:6
    plot(f, abs(G_int_align(:,i, i)), 'color', [colors(1,:)], ...
         'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
         'HandleVisibility', 'off');
end
plot(f, abs(G_int_align(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
     'DisplayName', '$e\mathcal{L}_i/u_j$ meas');
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
     'DisplayName', '$e\mathcal{L}_i/u_j$ model');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
xlim([1, 1e3]); ylim([2e-9, 2e-4]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/test_id31_first_id_int_better_rz_align.pdf', 'width', 'wide', 'height', 'normal');
#+end_src

#+name: fig:test_id31_first_id_int_better_rz_align
#+caption: Decrease of the coupling with better Rz alignment
#+RESULTS:
[[file:figs/test_id31_first_id_int_better_rz_align.png]]

** Effect of Payload Mass
<<ssec:test_id31_open_loop_plant_mass>>

The system dynamics was identified with four payload conditions that are shown in Figure ref:fig:test_id31_picture_masses.
The obtained direct terms are compared with the model dynamics in Figure ref:fig:test_nhexa_comp_simscape_diag_masses.

It is interesting to note that the anti-resonances in the force sensor plant are now appearing as minimum-phase, as the model predicts (Figure ref:fig:test_id31_comp_simscape_iff_diag_masses).

#+name: fig:test_id31_picture_masses
#+caption: The four tested payload conditions. (\subref{fig:test_id31_picture_mass_m0}) without payload. (\subref{fig:test_id31_picture_mass_m1}) with $13\,\text{kg}$ payload. (\subref{fig:test_id31_picture_mass_m2}) with $26\,\text{kg}$ payload. (\subref{fig:test_id31_picture_mass_m3}) with $39\,\text{kg}$ payload.
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_picture_mass_m0}$m=0\,\text{kg}$}
#+attr_latex: :options {0.24\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.99\linewidth
[[file:figs/test_id31_picture_mass_m0.jpg]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_picture_mass_m1}$m=13\,\text{kg}$}
#+attr_latex: :options {0.24\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.99\linewidth
[[file:figs/test_id31_picture_mass_m1.jpg]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_picture_mass_m2}$m=26\,\text{kg}$}
#+attr_latex: :options {0.24\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.99\linewidth
[[file:figs/test_id31_picture_mass_m2.jpg]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_picture_mass_m3}$m=39\,\text{kg}$}
#+attr_latex: :options {0.24\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.99\linewidth
[[file:figs/test_id31_picture_mass_m3.jpg]]
#+end_subfigure
#+end_figure

#+begin_src matlab :exports none
%% Identify the model dynamics for all payload conditions
% Initialize each Simscape model elements
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeNanoHexapod('flex_bot_type', '2dof', ...
                      'flex_top_type', '3dof', ...
                      'motion_sensor_type', 'plates', ...
                      'actuator_type', '2dof');
initializeSample('type', '0');

initializeSimscapeConfiguration('gravity', false);
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController('type', 'open-loop');
initializeReferences();

% 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', [], 'Vs');  io_i = io_i + 1; % Force Sensors
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Position Errors

initializeSample('type', '0');
Gm_m0_Wz0 = linearize(mdl, io);
Gm_m0_Wz0.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
Gm_m0_Wz0.OutputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6', ...
                        'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};

initializeSample('type', '1');
Gm_m1_Wz0 = linearize(mdl, io);
Gm_m1_Wz0.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
Gm_m1_Wz0.OutputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6', ...
                        'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};

initializeSample('type', '2');
Gm_m2_Wz0 = linearize(mdl, io);
Gm_m2_Wz0.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
Gm_m2_Wz0.OutputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6', ...
                        'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};

initializeSample('type', '3');
Gm_m3_Wz0 = linearize(mdl, io);
Gm_m3_Wz0.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
Gm_m3_Wz0.OutputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6', ...
                        'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
#+end_src

#+begin_src matlab
%% Identify the plant from experimental data - All payloads

% Load identification data
data_m0_Wz0 = load('2023-08-08_16-17_ol_plant_m0_Wz0.mat');
data_m1_Wz0 = load('2023-08-08_18-57_ol_plant_m1_Wz0.mat');
data_m2_Wz0 = load('2023-08-08_17-12_ol_plant_m2_Wz0.mat');
data_m3_Wz0 = load('2023-08-08_18-20_ol_plant_m3_Wz0.mat');

% Sampling Time [s]
Ts = 1e-4;

% Hannning Windows
Nfft = floor(2.0/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

% And we get the frequency vector
[~, f] = tfestimate(data_m0_Wz0.uL1.id_plant, data_m0_Wz0.uL1.e_L1, win, Noverlap, Nfft, 1/Ts);

% No payload
G_iff_m0_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
    eL = [data_m0_Wz0.(sprintf("uL%i", i_strut)).Vs1 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).Vs2 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).Vs3 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).Vs4 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).Vs5 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).Vs6]';

    G_iff_m0_Wz0(:,:,i_strut) = tfestimate(data_m0_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end

G_int_m0_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
    eL = [data_m0_Wz0.(sprintf("uL%i", i_strut)).e_L1 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).e_L2 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).e_L3 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).e_L4 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).e_L5 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).e_L6]';

    G_int_m0_Wz0(:,:,i_strut) = tfestimate(data_m0_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end

% 1 "payload layer"
G_iff_m1_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
    eL = [data_m1_Wz0.(sprintf("uL%i", i_strut)).Vs1 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).Vs2 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).Vs3 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).Vs4 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).Vs5 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).Vs6]';

    G_iff_m1_Wz0(:,:,i_strut) = tfestimate(data_m1_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end

G_int_m1_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
    eL = [data_m1_Wz0.(sprintf("uL%i", i_strut)).e_L1 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).e_L2 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).e_L3 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).e_L4 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).e_L5 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).e_L6]';

    G_int_m1_Wz0(:,:,i_strut) = tfestimate(data_m1_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end

% 2 "payload layers"
G_iff_m2_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
    eL = [data_m2_Wz0.(sprintf("uL%i", i_strut)).Vs1 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).Vs2 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).Vs3 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).Vs4 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).Vs5 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).Vs6]';

    G_iff_m2_Wz0(:,:,i_strut) = tfestimate(data_m2_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end

G_int_m2_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
    eL = [data_m2_Wz0.(sprintf("uL%i", i_strut)).e_L1 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).e_L2 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).e_L3 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).e_L4 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).e_L5 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).e_L6]';

    G_int_m2_Wz0(:,:,i_strut) = tfestimate(data_m2_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end

% 3 "payload layers"
G_iff_m3_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
    eL = [data_m3_Wz0.(sprintf("uL%i", i_strut)).Vs1 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).Vs2 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).Vs3 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).Vs4 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).Vs5 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).Vs6]';

    G_iff_m3_Wz0(:,:,i_strut) = tfestimate(data_m3_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end

G_int_m3_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
    eL = [data_m3_Wz0.(sprintf("uL%i", i_strut)).e_L1 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).e_L2 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).e_L3 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).e_L4 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).e_L5 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).e_L6]';

    G_int_m3_Wz0(:,:,i_strut) = tfestimate(data_m3_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end
#+end_src

#+begin_src matlab :exports none :results none
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_int_m0_Wz0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
     'DisplayName', 'Meas (0kg)');
for i = 2:6
    plot(f, abs(G_int_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], ...
         'HandleVisibility', 'off')
end
plot(f, abs(G_int_m1_Wz0(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
     'DisplayName', 'Meas (13kg)');
for i = 2:6
    plot(f, abs(G_int_m1_Wz0(:,i, i)), 'color', [colors(2,:), 0.5], ...
         'HandleVisibility', 'off')
end
plot(f, abs(G_int_m2_Wz0(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
     'DisplayName', 'Meas (26kg)');
for i = 2:6
    plot(f, abs(G_int_m2_Wz0(:,i, i)), 'color', [colors(3,:), 0.5], ...
         'HandleVisibility', 'off')
end
plot(f, abs(G_int_m3_Wz0(:, 1, 1)), 'color', [colors(4,:), 0.5], ...
     'DisplayName', 'Meas (39kg)');
for i = 2:6
    plot(f, abs(G_int_m3_Wz0(:,i, i)), 'color', [colors(4,:), 0.5], ...
         'HandleVisibility', 'off')
end
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:), ...
     'DisplayName', 'Model (0kg)');
plot(freqs, abs(squeeze(freqresp(Gm_m1_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:), ...
     'DisplayName', 'Model (13kg)');
plot(freqs, abs(squeeze(freqresp(Gm_m2_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:), ...
     'DisplayName', 'Model (26kg)');
plot(freqs, abs(squeeze(freqresp(Gm_m3_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:), ...
     'DisplayName', 'Model (39kg)');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-8, 5e-4]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i =1:6
    plot(f, 180/pi*angle(G_int_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.5]);
end
for i =1:6
    plot(f, 180/pi*angle(G_int_m1_Wz0(:,i, i)), 'color', [colors(2,:), 0.5]);
end
for i =1:6
    plot(f, 180/pi*angle(G_int_m2_Wz0(:,i, i)), 'color', [colors(3,:), 0.5]);
end
for i =1:6
    plot(f, 180/pi*angle(G_int_m3_Wz0(:,i, i)), 'color', [colors(4,:), 0.5]);
end
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m1_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m2_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m3_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-90, 180])

linkaxes([ax1,ax2],'x');
xlim([10, 5e2]);
xticks([10, 20, 50, 100, 200, 500])
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/test_id31_comp_simscape_int_diag_masses.pdf', 'width', 'half', 'height', 600);
#+end_src

#+begin_src matlab :exports none :results none
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_iff_m0_Wz0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
     'DisplayName', 'Meas (0kg)');
for i = 2:6
    plot(f, abs(G_iff_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], ...
         'HandleVisibility', 'off')
end
plot(f, abs(G_iff_m1_Wz0(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
     'DisplayName', 'Meas (13kg)');
for i = 2:6
    plot(f, abs(G_iff_m1_Wz0(:,i, i)), 'color', [colors(2,:), 0.5], ...
         'HandleVisibility', 'off')
end
plot(f, abs(G_iff_m2_Wz0(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
     'DisplayName', 'Meas (26kg)');
for i = 2:6
    plot(f, abs(G_iff_m2_Wz0(:,i, i)), 'color', [colors(3,:), 0.5], ...
         'HandleVisibility', 'off')
end
plot(f, abs(G_iff_m3_Wz0(:, 1, 1)), 'color', [colors(4,:), 0.5], ...
     'DisplayName', 'Meas (39kg)');
for i = 2:6
    plot(f, abs(G_iff_m3_Wz0(:,i, i)), 'color', [colors(4,:), 0.5], ...
         'HandleVisibility', 'off')
end
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('Vs1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:), ...
     'DisplayName', 'Model (0kg)');
plot(freqs, abs(squeeze(freqresp(Gm_m1_Wz0('Vs1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:), ...
     'DisplayName', 'Model (13kg)');
plot(freqs, abs(squeeze(freqresp(Gm_m2_Wz0('Vs1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:), ...
     'DisplayName', 'Model (26kg)');
plot(freqs, abs(squeeze(freqresp(Gm_m3_Wz0('Vs1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:), ...
     'DisplayName', 'Model (39kg)');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-2, 4e1]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
for i =1:6
    plot(f, 180/pi*angle(G_iff_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.5]);
end
for i =1:6
    plot(f, 180/pi*angle(G_iff_m1_Wz0(:,i, i)), 'color', [colors(2,:), 0.5]);
end
for i =1:6
    plot(f, 180/pi*angle(G_iff_m2_Wz0(:,i, i)), 'color', [colors(3,:), 0.5]);
end
for i =1:6
    plot(f, 180/pi*angle(G_iff_m3_Wz0(:,i, i)), 'color', [colors(4,:), 0.5]);
end
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0_Wz0('Vs1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m1_Wz0('Vs1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m2_Wz0('Vs1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m3_Wz0('Vs1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-90, 180])

linkaxes([ax1,ax2],'x');
xlim([10, 5e2]);
xticks([10, 20, 50, 100, 200, 500])
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/test_id31_comp_simscape_iff_diag_masses.pdf', 'width', 'half', 'height', 600);
#+end_src

#+name: fig:test_nhexa_comp_simscape_diag_masses
#+caption: Comparison of the diagonal elements (i.e. "direct" terms) of the measured FRF matrix and the dynamics identified from the Simscape model. Both for the dynamics from $u$ to $e\mathcal{L}$ (\subref{fig:test_id31_comp_simscape_int_diag_masses}) and from $u$ to $V_s$ (\subref{fig:test_id31_comp_simscape_iff_diag_masses})
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_comp_simscape_int_diag_masses}from $u$ to $e\mathcal{L}$}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_id31_comp_simscape_int_diag_masses.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_comp_simscape_iff_diag_masses}from $u$ to $V_s$}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_id31_comp_simscape_iff_diag_masses.png]]
#+end_subfigure
#+end_figure

** Effect of Spindle Rotation
<<ssec:test_id31_open_loop_plant_rotation>>

The dynamics was then identified while the Spindle was rotating at constant velocity.
Three identification experiments were performed: no spindle rotation, spindle rotation at $36\,\text{deg}/s$ and at $180\,\text{deg}/s$.

The comparison of the obtained dynamics from command signal $u$ to estimated strut error $e\mathcal{L}$ is done in Figure ref:fig:test_id31_effect_rotation.
Both direct terms (Figure ref:fig:test_id31_effect_rotation_direct) and coupling terms (Figure ref:fig:test_id31_effect_rotation_coupling) are unaffected by the rotation.
The same can be observed for the dynamics from the command signal to the encoders and to the force sensors.
This confirms that the rotation has no significant effect on the plant dynamics.
This also indicates that the metrology kinematics is correct and is working in real time.

#+begin_src matlab
%% Identify the model dynamics with Spindle rotation
initializeSample('type', '0');
initializeReferences(...
    'Rz_type', 'rotating', ...
    'Rz_period', 360/36); % 36 deg/s, 6rpm
Gm_m0_Wz36 = linearize(mdl, io, 0.1);
Gm_m0_Wz36.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
Gm_m0_Wz36.OutputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6', ...
                         'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};

initializeReferences(...
    'Rz_type', 'rotating', ...
    'Rz_period', 360/180); % 180 deg/s, 30rpm
Gm_m0_Wz180 = linearize(mdl, io, 0.1);
Gm_m0_Wz180.InputName  = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
Gm_m0_Wz180.OutputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6', ...
                          'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
#+end_src

#+begin_src matlab
%% Identify the plant from experimental data - Effect of rotation

% Load identification data
data_m0_Wz36  = load('2023-08-08_16-28_ol_plant_m0_Wz36.mat');
data_m0_Wz180 = load('2023-08-08_16-45_ol_plant_m0_Wz180.mat');

% Spindle Rotation at 36 deg/s
G_iff_m0_Wz36 = zeros(length(f), 6, 6);
for i_strut = 1:6
    eL = [data_m0_Wz36.(sprintf("uL%i", i_strut)).Vs1 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).Vs2 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).Vs3 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).Vs4 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).Vs5 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).Vs6]';

    G_iff_m0_Wz36(:,:,i_strut) = tfestimate(data_m0_Wz36.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end

G_int_m0_Wz36 = zeros(length(f), 6, 6);
for i_strut = 1:6
    eL = [data_m0_Wz36.(sprintf("uL%i", i_strut)).e_L1 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).e_L2 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).e_L3 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).e_L4 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).e_L5 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).e_L6]';

    G_int_m0_Wz36(:,:,i_strut) = tfestimate(data_m0_Wz36.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end

% Spindle Rotation at 180 deg/s
G_iff_m0_Wz180 = zeros(length(f), 6, 6);
for i_strut = 1:6
    eL = [data_m0_Wz180.(sprintf("uL%i", i_strut)).Vs1 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).Vs2 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).Vs3 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).Vs4 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).Vs5 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).Vs6]';

    G_iff_m0_Wz180(:,:,i_strut) = tfestimate(data_m0_Wz180.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end

G_int_m0_Wz180 = zeros(length(f), 6, 6);
for i_strut = 1:6
    eL = [data_m0_Wz180.(sprintf("uL%i", i_strut)).e_L1 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).e_L2 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).e_L3 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).e_L4 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).e_L5 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).e_L6]';

    G_int_m0_Wz180(:,:,i_strut) = tfestimate(data_m0_Wz180.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
end
#+end_src

#+begin_src matlab :exports none :tangle no
% The identified dynamics are then saved for further use.
save('./matlab/mat/test_id31_simscape_open_loop_plants.mat', 'Gm_m0_Wz0', 'Gm_m0_Wz36', 'Gm_m0_Wz180', 'Gm_m1_Wz0', 'Gm_m2_Wz0', 'Gm_m3_Wz0');
save('./matlab/mat/test_id31_identified_open_loop_plants.mat', 'G_int_m0_Wz0', 'G_int_m0_Wz36', 'G_int_m0_Wz180', 'G_int_m1_Wz0', 'G_int_m2_Wz0', 'G_int_m3_Wz0', ...
                                                               'G_iff_m0_Wz0', 'G_iff_m0_Wz36', 'G_iff_m0_Wz180', 'G_iff_m1_Wz0', 'G_iff_m2_Wz0', 'G_iff_m3_Wz0', 'f');
#+end_src

#+begin_src matlab :eval no
% The identified dynamics are then saved for further use.
save('./mat/test_id31_simscape_open_loop_plants.mat', 'Gm_m0_Wz0', 'Gm_m0_Wz36', 'Gm_m0_Wz180', 'Gm_m1_Wz0', 'Gm_m2_Wz0', 'Gm_m3_Wz0');
save('./mat/test_id31_identified_open_loop_plants.mat', 'G_int_m0_Wz0', 'G_int_m0_Wz36', 'G_int_m0_Wz180', 'G_int_m1_Wz0', 'G_int_m2_Wz0', 'G_int_m3_Wz0', ...
                                                        'G_iff_m0_Wz0', 'G_iff_m0_Wz36', 'G_iff_m0_Wz180', 'G_iff_m1_Wz0', 'G_iff_m2_Wz0', 'G_iff_m3_Wz0', 'f');
#+end_src

#+begin_src matlab :exports none :results none
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');

nexttile();
hold on;
plot(f, abs(G_int_m0_Wz0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
     'DisplayName', '$\Omega_z = 0$');
plot(f, abs(G_int_m0_Wz36(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
     'DisplayName', '$\Omega_z = 36$ deg/s');
plot(f, abs(G_int_m0_Wz180(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
     'DisplayName', '$\Omega_z = 180$ deg/s');
for i = 2:6
    plot(f, abs(G_int_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], ...
         'HandleVisibility', 'off')
    plot(f, abs(G_int_m0_Wz36(:,i, i)), 'color', [colors(2,:), 0.5], ...
         'HandleVisibility', 'off')
    plot(f, abs(G_int_m0_Wz180(:,i, i)), 'color', [colors(3,:), 0.5], ...
         'HandleVisibility', 'off')
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([10, 1e3]); ylim([1e-8, 2e-4])
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/test_id31_effect_rotation_direct.pdf', 'width', 'half', 'height', 'short');
#+end_src

#+begin_src matlab :exports none :results none
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');

nexttile();
hold on;
plot(f, abs(G_int_m0_Wz0(:, 1, 2)), 'color', [colors(1,:), 0.5], ...
     'DisplayName', '$\Omega_z = 0$');
plot(f, abs(G_int_m0_Wz36(:, 1, 2)), 'color', [colors(2,:), 0.5], ...
     'DisplayName', '$\Omega_z = 36$ deg/s');
plot(f, abs(G_int_m0_Wz180(:, 1, 2)), 'color', [colors(3,:), 0.5], ...
     'DisplayName', '$\Omega_z = 180$ deg/s');
for i = 1:5
    for j = i+1:6
        plot(f, abs(G_int_m0_Wz0(:, i, j)), 'color', [colors(1,:), 0.5], ...
             'HandleVisibility', 'off');
        plot(f, abs(G_int_m0_Wz36(:, i, j)), 'color', [colors(2,:), 0.5], ...
             'HandleVisibility', 'off');
        plot(f, abs(G_int_m0_Wz180(:, i, j)), 'color', [colors(3,:), 0.5], ...
             'HandleVisibility', 'off');
    end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([10, 1e3]); ylim([1e-8, 2e-4])
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/test_id31_effect_rotation_coupling.pdf', 'width', 'half', 'height', 'short');
#+end_src

#+name: fig:test_id31_effect_rotation
#+caption: Effect of the spindle rotation on the plant dynamics from $u$ to $e\mathcal{L}$. Three rotational velocities are tested ($0\,\text{deg}/s$, $36\,\text{deg}/s$ and $180\,\text{deg}/s$). Both direct terms (\subref{fig:test_id31_effect_rotation_direct}) and coupling terms (\subref{fig:test_id31_effect_rotation_coupling}) are displayed.
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_effect_rotation_direct}Direct terms}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_id31_effect_rotation_direct.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_effect_rotation_coupling}Coupling terms}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_id31_effect_rotation_coupling.png]]
#+end_subfigure
#+end_figure

** TODO Identification of Spurious modes

- [ ] These are made to identify the modes of the spheres
- [ ] Also discuss other observed modes

#+begin_src matlab
%% Load identification data
% mat_dir = "/home/thomas/mnt/data_id31/nass";
data = load(sprintf('%s/dynamics/2023-08-10_18-32_identify_spurious_modes.mat', mat_dir));
#+end_src

#+begin_src matlab :exports none
Ts = 1e-4;
% Hannning Windows
Nfft = floor(1.0/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);
#+end_src

#+begin_src matlab :exports none
% And we get the frequency vector
[G1, f] = tfestimate(data.id_plant, data.d1, win, Noverlap, Nfft, 1/Ts);
[G2, ~] = tfestimate(data.id_plant, data.d2, win, Noverlap, Nfft, 1/Ts);
[G3, ~] = tfestimate(data.id_plant, data.d3, win, Noverlap, Nfft, 1/Ts);
[G4, ~] = tfestimate(data.id_plant, data.d4, win, Noverlap, Nfft, 1/Ts);
[G5, ~] = tfestimate(data.id_plant, data.d5, win, Noverlap, Nfft, 1/Ts);
#+end_src

#+begin_src matlab :exports none :results none
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(f, abs(G1), 'DisplayName', '1 - top');
plot(f, abs(G2), 'DisplayName', '2 - bot');
plot(f, abs(G3), 'DisplayName', '3 - top');
plot(f, abs(G4), 'DisplayName', '4 - bot');
plot(f, abs(G5), 'DisplayName', '5 - vertical');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
xlim([500, 800])
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
#+end_src

** Conclusion
:PROPERTIES:
:UNNUMBERED: t
:END:

Thanks to the model, poor alignment between the nano-hexapod axes and the external metrology axes could be identified.
After alignment, the identified dynamics is well matching with the multi-body model.

Also, the observed effects of the payload mass and of the spindle rotation on the dynamics are well matching the model predictions.

* Decentralized Integral Force Feedback
:PROPERTIES:
:header-args:matlab+: :tangle matlab/test_id31_3_iff.m
:END:
<<sec:test_id31_iff>>
** Introduction                                                      :ignore:

Before implementing a position controller, an active damping controller was first implemented as shown in Figure ref:fig:test_id31_iff_block_diagram.
It consisted of a decentralized Integral Force Feedback controller $\bm{K}_{\text{IFF}}$, with all the diagonal terms being equal eqref:eq:test_id31_Kiff.

\begin{equation}\label{eq:test_id31_iff_diagonal}
\bm{K}_{\text{IFF}} = K_{\text{IFF}} \cdot \bm{I}_6 = \begin{bmatrix}
K_{\text{IFF}} & & 0 \\
& \ddots & \\
0 & & K_{\text{IFF}}
\end{bmatrix}
\end{equation}

#+begin_src latex :file test_id31_iff_schematic.pdf
\begin{tikzpicture}
  % Blocs
  \node[block={2.0cm}{2.0cm}] (P) {Plant};
  \coordinate[] (input)   at ($(P.south west)!0.5!(P.north west)$);
  \coordinate[] (outputH) at ($(P.south east)!0.2!(P.north east)$);
  \coordinate[] (outputL) at ($(P.south east)!0.8!(P.north east)$);

  \node[block, above=0.6 of P] (Klac) {$\bm{K}_\text{IFF}$};
  \node[addb, left= of input] (addF) {};

  % Connections and labels
  \draw[->] (outputL) -- ++(0.6, 0) coordinate(eastlac) |- (Klac.east);
  \node[above right] at (outputL){$\bm{V}_s$};
  \draw[->] (Klac.west) -| (addF.north);
  \draw[->] (addF.east) -- (input) node[above left]{$\bm{u}$};

  \draw[->] (outputH) -- ++(1.6, 0) node[above left]{$\bm{\epsilon}_{\mathcal{L}}$};
  \draw[<-] (addF.west) -- ++(-1.0, 0) node[above right]{$\bm{u}^{\prime}$};

  \begin{scope}[on background layer]
    \node[fit={(Klac.north-|eastlac) (addF.west|-P.south)}, fill=black!20!white, draw, dashed, inner sep=10pt] (Pi) {};
    \node[anchor={north west}] at (Pi.north west){$\text{Damped Plant}$};
  \end{scope}
\end{tikzpicture}
#+end_src

#+name: fig:test_id31_iff_block_diagram
#+caption: Block diagram of the implemented decentralized IFF controller. The controller $\bm{K}_{\text{IFF}}$ is a diagonal controller with the same elements on every diagonal term $K_{\text{IFF}}$.
#+RESULTS:
[[file:figs/test_id31_iff_schematic.png]]

** Matlab Init                                              :noexport:ignore:
#+begin_src matlab
%% test_id31_3_iff.m
#+end_src

#+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 :noweb yes
<<m-init-path>>
#+end_src

#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle>>
#+end_src

#+begin_src matlab :noweb yes
<<m-init-simscape>>
#+end_src

#+begin_src matlab :noweb yes
<<m-init-other>>
#+end_src

** IFF Plant
<<ssec:test_id31_iff_plant>>

As the multi-body model is going to be used to estimate the stability of the IFF controller and to optimize achievable damping, it is first checked is this model accurately represents the system dynamics.

In Figure ref:fig:test_id31_comp_simscape_iff_diag_masses, it was shown that the model well captures the dynamics from each actuator to its collocated force sensor, as that for all considered payloads.
The model is also accurate for the dynamics from an actuator to the force sensors in the other struts (i.e. the off-diagonal dynamics) as shown in Figure ref:fig:test_id31_comp_simscape_Vs.

#+begin_src matlab
% Load identified FRF for IFF Plant and Multi-Body Model
load('test_id31_identified_open_loop_plants.mat', 'G_iff_m0_Wz0', 'G_iff_m1_Wz0', 'G_iff_m2_Wz0', 'G_iff_m3_Wz0', 'f');
load('test_id31_simscape_open_loop_plants.mat', 'Gm_m0_Wz0', 'Gm_m1_Wz0', 'Gm_m2_Wz0', 'Gm_m3_Wz0');
#+end_src

#+begin_src matlab :exports none
figure;
tiledlayout(2, 3, 'TileSpacing', 'tight', 'Padding', 'tight');

ax1 = nexttile();
hold on;
plot(f, abs(G_iff_m0_Wz0(:, 1, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('Vs1', 'u1'), freqs, 'Hz'))));
text(12, 4e1, '$V_{s1}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]');
yticks([1e-2, 1e-1, 1e0, 1e1]);

ax2 = nexttile();
hold on;
plot(f, abs(G_iff_m0_Wz0(:, 2, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('Vs2', 'u1'), freqs, 'Hz'))));
text(12, 4e1, '$V_{s2}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);

ax3 = nexttile();
hold on;
plot(f, abs(G_iff_m0_Wz0(:, 3, 1)), ...
     'DisplayName', 'Measurements');
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('Vs3', 'u1'), freqs, 'Hz'))), ...
     'DisplayName', 'Model (2-DoF APA)');
text(12, 4e1, '$V_{s3}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax4 = nexttile();
hold on;
plot(f, abs(G_iff_m0_Wz0(:, 4, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('Vs4', 'u1'), freqs, 'Hz'))));
text(12, 4e1, '$V_{s4}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
xticks([10, 20, 50, 100, 200])
yticks([1e-2, 1e-1, 1e0, 1e1]);

ax5 = nexttile();
hold on;
plot(f, abs(G_iff_m0_Wz0(:, 5, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('Vs5', 'u1'), freqs, 'Hz'))));
text(12, 4e1, '$V_{s5}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xticks([10, 20, 50, 100, 200])

ax6 = nexttile();
hold on;
plot(f, abs(G_iff_m0_Wz0(:, 6, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('Vs6', 'u1'), freqs, 'Hz'))));
text(12, 4e1, '$V_{s6}/u_{1}$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
xticks([10, 20, 50, 100, 200])

linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'xy');
xlim([10, 5e2]); ylim([1e-2, 5e1]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/test_id31_comp_simscape_Vs.pdf', 'width', 'full', 'height', 700);
#+end_src

#+name: fig:test_id31_comp_simscape_Vs
#+caption: Comparison of the measured (in blue) and modeled (in red) frequency transfer functions from the first control signal $u_1$ to the six force sensor voltages $V_{s1}$ to $V_{s6}$
#+RESULTS:
[[file:figs/test_id31_comp_simscape_Vs.png]]

** IFF Controller
<<ssec:test_id31_iff_controller>>

A decentralized IFF controller is there designed such that it adds damping to the suspension modes of the nano-hexapod for all considered payloads.
The frequency of the suspension modes are ranging from $\approx 30\,\text{Hz}$ to $\approx 250\,\text{Hz}$ (Figure ref:fig:test_id31_comp_simscape_iff_diag_masses), and therefore the IFF controller should provide integral action in this frequency range.
A second order high pass filter (cut-off frequency of $10\,\text{Hz}$) is added to limit the low frequency gain.

The bode plot of the decentralized IFF controller is shown in Figure ref:fig:test_id31_Kiff_bode_plot and the "decentralized loop-gains" for all considered payload masses are shown in Figure ref:fig:test_id31_Kiff_loop_gain.
It can be seen that the loop-gain is larger than $1$ around suspension modes indicating that some damping should be added to the suspension modes.

\begin{equation}\label{eq:test_id31_Kiff}
K_{\text{IFF}} = g_0 \cdot \underbrace{\frac{1}{s}}_{\text{int}} \cdot \underbrace{\frac{s^2/\omega_z^2}{s^2/\omega_z^2 + 2\xi_z s /\omega_z + 1}}_{\text{2nd order LPF}},\quad \left(g_0 = -100,\ \omega_z = 2\pi10\,\text{rad/s},\ \xi_z = 0.7\right)
\end{equation}

#+begin_src matlab
%% IFF Controller Design
% Second order high pass filter
wz = 2*pi*10;
xiz = 0.7;
Ghpf = (s^2/wz^2)/(s^2/wz^2 + 2*xiz*s/wz + 1);

% IFF Controller
Kiff = -1e2 * ...              % Gain
       1/(0.01*2*pi + s) * ... % LPF: provides integral action
       Ghpf * ...              % 2nd order HPF (limit low frequency gain)
       eye(6);                 % Diagonal 6x6 controller (i.e. decentralized)

Kiff.InputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'};
Kiff.OutputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
#+end_src

#+begin_src matlab :exports none :tangle no
% The designed IFF controller is saved
save('./matlab/mat/test_id31_K_iff.mat', 'Kiff');
#+end_src

#+begin_src matlab :eval no
% The designed IFF controller is saved
save('./mat/test_id31_K_iff.mat', 'Kiff');
#+end_src

#+begin_src matlab :exports none :results none
%% Bode plot of the designed decentralized IFF controller
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(squeeze(freqresp(Kiff(1,1), f, 'Hz'))), 'color', colors(1,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
ylim([1e-2, 1e1]);

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(squeeze(freqresp(Kiff(1,1), f, 'Hz'))), 'color', colors(1,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-180, 180])

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

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/test_id31_Kiff_bode_plot.pdf', 'width', 'half', 'height', 600);
#+end_src

#+begin_src matlab :exports none :results none
%% Loop gain for the decentralized IFF controller
Kiff_frf = squeeze(freqresp(Kiff(1,1), f, 'Hz'));

figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_iff_m0_Wz0(:, 1, 1).*Kiff_frf), 'color', colors(1,:), ...
     'DisplayName', '$m = 0$ kg');
plot(f, abs(G_iff_m1_Wz0(:, 1, 1).*Kiff_frf), 'color', colors(2,:), ...
     'DisplayName', '$m = 13$ kg');
plot(f, abs(G_iff_m2_Wz0(:, 1, 1).*Kiff_frf), 'color', colors(3,:), ...
     'DisplayName', '$m = 26$ kg');
plot(f, abs(G_iff_m3_Wz0(:, 1, 1).*Kiff_frf), 'color', colors(4,:), ...
     'DisplayName', '$m = 39$ kg');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
ylim([1e-2, 1e1]);
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(-G_iff_m0_Wz0(:,1,1).*Kiff_frf), 'color', colors(1,:));
plot(f, 180/pi*angle(-G_iff_m1_Wz0(:,1,1).*Kiff_frf), 'color', colors(2,:));
plot(f, 180/pi*angle(-G_iff_m2_Wz0(:,1,1).*Kiff_frf), 'color', colors(3,:));
plot(f, 180/pi*angle(-G_iff_m3_Wz0(:,1,1).*Kiff_frf), 'color', colors(4,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-180, 180])

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

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/test_id31_Kiff_loop_gain.pdf', 'width', 'half', 'height', 600);
#+end_src

#+name: fig:test_id31_Kiff
#+caption: Bode plot of the decentralized IFF controller (\subref{fig:test_id31_Kiff_bode_plot}). The decentralized controller $K_{\text{IFF}}$ multiplied by the identified dynamics from $u_1$ to $V_{s1}$ for all payloads are shown in (\subref{fig:test_id31_Kiff_loop_gain})
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_Kiff_bode_plot}Bode plot of $K_{\text{IFF}}$}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_id31_Kiff_bode_plot.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_Kiff_loop_gain}Decentralized Loop gains}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_id31_Kiff_loop_gain.png]]
#+end_subfigure
#+end_figure

To estimate the added damping, a root-locus plot is computed using the multi-body model (Figure ref:fig:test_id31_iff_root_locus_m0).
It can be seen that for all considered payloads, the poles are bounded to the "left-half plane" indicating that the decentralized IFF is robust.
The closed-loop poles for the chosen value of the gain are displayed by black crosses.
It can be seen that while damping can be added for all payloads (as compared to the open-loop case), the optimal value of the gain could be tuned separately for each payload.
For instance, for low payload masses, a higher value of the IFF controller gain could lead to better damping.
However, in this study, it was chosen to implement a fix (i.e. non-adaptive) decentralized IFF controller.

#+begin_src matlab :exports none :results none
%% Root Locus for IFF
gains = logspace(-2, 2, 100);
Gm_iff_m0 = Gm_m0_Wz0({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'});
Gm_iff_m1 = Gm_m1_Wz0({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'});
Gm_iff_m2 = Gm_m2_Wz0({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'});
Gm_iff_m3 = Gm_m3_Wz0({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'});
#+end_src

#+begin_src matlab :exports none :results none
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;
plot(real(pole(Gm_iff_m0)),  imag(pole(Gm_iff_m0)),  'x', 'color', colors(1,:), ...
    'DisplayName', '$g = 0$');
plot(real(tzero(Gm_iff_m0)), imag(tzero(Gm_iff_m0)), 'o', 'color', colors(1,:), ...
    'HandleVisibility', 'off');

for g = gains
    clpoles = pole(feedback(Gm_iff_m0, g*Kiff, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
        'HandleVisibility', 'off');
end

% Optimal gain
clpoles = pole(feedback(Gm_iff_m0, Kiff, +1));
plot(real(clpoles), imag(clpoles), 'kx', ...
    'DisplayName', '$g_{opt}$');
hold off;
axis equal;
xlim([-600, 0]); ylim([0, 1500]);
xticks([-600:300:0]);
yticks([0:300:1500]);
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
xlabel('Real part'); ylabel('Imaginary part');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/test_id31_iff_root_locus_m0.pdf', 'width', 'third', 'height', 'normal');
#+end_src

#+begin_src matlab :exports none :results none
%% description
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;
plot(real(pole(Gm_iff_m1)),  imag(pole(Gm_iff_m1)),  'x', 'color', colors(2,:), ...
    'DisplayName', '$g = 0$');
plot(real(tzero(Gm_iff_m1)), imag(tzero(Gm_iff_m1)), 'o', 'color', colors(2,:), ...
    'HandleVisibility', 'off');

for g = gains
    clpoles = pole(feedback(Gm_iff_m1, g*Kiff, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ...
        'HandleVisibility', 'off');
end

% Optimal gain
clpoles = pole(feedback(Gm_iff_m1, Kiff, +1));
plot(real(clpoles), imag(clpoles), 'kx', ...
    'DisplayName', '$g_{opt}$');
hold off;
axis equal;
xlim([-200, 0]); ylim([0, 500]);
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
xlabel('Real part'); ylabel('Imaginary part');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/test_id31_iff_root_locus_m1.pdf', 'width', 'third', 'height', 'normal');
#+end_src

#+begin_src matlab :exports none :results none
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;
plot(real(pole(Gm_iff_m2)),  imag(pole(Gm_iff_m2)),  'x', 'color', colors(3,:), ...
    'DisplayName', '$g = 0$');
plot(real(tzero(Gm_iff_m2)), imag(tzero(Gm_iff_m2)), 'o', 'color', colors(3,:), ...
    'HandleVisibility', 'off');

for g = gains
    clpoles = pole(feedback(Gm_iff_m2, g*Kiff, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ...
        'HandleVisibility', 'off');
end

% Optimal gain
clpoles = pole(feedback(Gm_iff_m2, Kiff, +1));
plot(real(clpoles), imag(clpoles), 'kx', ...
    'DisplayName', '$g_{opt}$');
hold off;
axis equal;
xlim([-200, 0]); ylim([0, 500]);
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
xlabel('Real part'); ylabel('Imaginary part');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/test_id31_iff_root_locus_m2.pdf', 'width', 'third', 'height', 'normal');
#+end_src

#+begin_src matlab :exports none :results none
figure;
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
nexttile();
hold on;
plot(real(pole(Gm_iff_m3)),  imag(pole(Gm_iff_m3)),  'x', 'color', colors(4,:), ...
    'DisplayName', '$g = 0$');
plot(real(tzero(Gm_iff_m3)), imag(tzero(Gm_iff_m3)), 'o', 'color', colors(4,:), ...
    'HandleVisibility', 'off');

for g = gains
    clpoles = pole(feedback(Gm_iff_m3, g*Kiff, +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(4,:), ...
        'HandleVisibility', 'off');
end

% Optimal gain
clpoles = pole(feedback(Gm_iff_m3, Kiff, +1));
plot(real(clpoles), imag(clpoles), 'kx', ...
    'DisplayName', '$g_{opt}$');
hold off;
axis equal;
xlim([-200, 0]); ylim([0, 500]);
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
xlabel('Real part'); ylabel('Imaginary part');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/test_id31_iff_root_locus_m3.pdf', 'width', 'third', 'height', 'normal');
#+end_src

#+name: fig:test_id31_iff_root_locus_m0
#+caption: Root Locus plots for the designed decentralized IFF controller and using the multy-body model. Black crosses indicate the closed-loop poles for the choosen value of the gain.
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_iff_root_locus_m0}$m = 0\,\text{kg}$}
#+attr_latex: :options {0.24\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.9\linewidth
[[file:figs/test_id31_iff_root_locus_m0.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_iff_root_locus_m1}$m = 13\,\text{kg}$}
#+attr_latex: :options {0.24\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.9\linewidth
[[file:figs/test_id31_iff_root_locus_m1.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_iff_root_locus_m2}$m = 26\,\text{kg}$}
#+attr_latex: :options {0.24\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.9\linewidth
[[file:figs/test_id31_iff_root_locus_m2.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_iff_root_locus_m3}$m = 39\,\text{kg}$}
#+attr_latex: :options {0.24\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.9\linewidth
[[file:figs/test_id31_iff_root_locus_m3.png]]
#+end_subfigure
#+end_figure

** Estimated Damped Plant
<<ssec:test_id31_iff_perf>>

As the model is accurately modelling the system dynamics, it can be used to estimate the damped plant, i.e. the transfer functions from $\bm{u}^\prime$ to $\bm{\mathcal{L}}$.
The obtained damped plants are compared with the open-loop plants in Figure ref:fig:test_id31_comp_ol_iff_plant_model.
The peak amplitudes corresponding to the suspension modes are approximately reduced by a factor $10$ for all considered payloads, and with the same decentralized IFF controller.

#+begin_src matlab
%% Estimate damped plant from Multi-Body model
Gm_hac_m0_Wz0 = feedback(Gm_m0_Wz0, Kiff, 'name', +1);
Gm_hac_m1_Wz0 = feedback(Gm_m1_Wz0, Kiff, 'name', +1);
Gm_hac_m2_Wz0 = feedback(Gm_m2_Wz0, Kiff, 'name', +1);
Gm_hac_m3_Wz0 = feedback(Gm_m3_Wz0, Kiff, 'name', +1);

% Check Stability
isstable(Gm_hac_m0_Wz0)
isstable(Gm_hac_m1_Wz0)
isstable(Gm_hac_m2_Wz0)
isstable(Gm_hac_m3_Wz0)
#+end_src

#+begin_src matlab :exports none :tangle no
% The estimated damped plants from the multi-body model are saved
save('./matlab/mat/test_id31_simscape_damped_plants.mat', 'Gm_hac_m0_Wz0', 'Gm_hac_m1_Wz0', 'Gm_hac_m2_Wz0', 'Gm_hac_m3_Wz0');
#+end_src

#+begin_src matlab :eval no
% The estimated damped plants from the multi-body model are saved
save('./mat/test_id31_simscape_damped_plants.mat', 'Gm_hac_m0_Wz0', 'Gm_hac_m1_Wz0', 'Gm_hac_m2_Wz0', 'Gm_hac_m3_Wz0');
#+end_src

#+begin_src matlab :exports none :results none
%% Comparison of the open-loop plants and the estimated damped plant with IFF
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.3], ...
    'DisplayName', '$-e\mathcal{L}_{1}/u_1$ - 0 kg');
plot(freqs, abs(squeeze(freqresp(Gm_m1_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', [colors(2,:), 0.3], ...
    'DisplayName', '$-e\mathcal{L}_{1}/u_1$ - 13 kg');
plot(freqs, abs(squeeze(freqresp(Gm_m2_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', [colors(3,:), 0.3], ...
    'DisplayName', '$-e\mathcal{L}_{1}/u_1$ - 26 kg');
plot(freqs, abs(squeeze(freqresp(Gm_m3_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', [colors(4,:), 0.3], ...
    'DisplayName', '$-e\mathcal{L}_{1}/u_1$ - 39 kg');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', '$-e\mathcal{L}_{1}/u_1^\prime$ - 0 kg');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m1_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', '$-e\mathcal{L}_{1}/u_1^\prime$ - 13 kg');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m2_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', '$-e\mathcal{L}_{1}/u_1^\prime$ - 26 kg');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m3_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', colors(4,:), ...
    'DisplayName', '$-e\mathcal{L}_{1}/u_1^\prime$ - 39 kg');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-7, 4e-4]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0_Wz0('eL1','u1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.3]);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m1_Wz0('eL1','u1'), freqs, 'Hz'))), 'color', [colors(2,:), 0.3]);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m2_Wz0('eL1','u1'), freqs, 'Hz'))), 'color', [colors(3,:), 0.3]);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m3_Wz0('eL1','u1'), freqs, 'Hz'))), 'color', [colors(4,:), 0.3]);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_hac_m0_Wz0('eL1','u1'), freqs, 'Hz')))), 'color', colors(1,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_hac_m1_Wz0('eL1','u1'), freqs, 'Hz')))), 'color', colors(2,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_hac_m2_Wz0('eL1','u1'), freqs, 'Hz')))), 'color', colors(3,:));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_hac_m3_Wz0('eL1','u1'), freqs, 'Hz')))), 'color', colors(4,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-20, 200])

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

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/test_id31_comp_ol_iff_plant_model.pdf', 'width', 'wide', 'height', 600);
#+end_src

#+name: fig:test_id31_comp_ol_iff_plant_model
#+caption: Comparison of the open-loop plants and the estimated damped plant with Decentralized IFF.
#+RESULTS:
[[file:figs/test_id31_comp_ol_iff_plant_model.png]]

** Conclusion
:PROPERTIES:
:UNNUMBERED: t
:END:
* High Authority Control in the frame of the struts
:PROPERTIES:
:header-args:matlab+: :tangle matlab/test_id31_4_hac.m
:END:
<<sec:test_id31_iff_hac>>
** Introduction                                                      :ignore:

The position of the sample is actively stabilized by implementing a High-Authority-Controller as shown in Figure ref:fig:test_id31_iff_hac_schematic.

\begin{equation}\label{eq:eq:test_id31_hac_diagonal}
\bm{K}_{\text{HAC}} = K_{\text{HAC}} \cdot \bm{I}_6 = \begin{bmatrix}
K_{\text{HAC}} & & 0 \\
& \ddots & \\
0 & & K_{\text{HAC}}
\end{bmatrix}
\end{equation}

#+begin_src latex :file test_id31_iff_hac_schematic.pdf
\begin{tikzpicture}
  % Blocs
  \node[block={2.0cm}{2.0cm}] (P) {Plant};
  \coordinate[] (input)   at ($(P.south west)!0.5!(P.north west)$);
  \coordinate[] (outputH) at ($(P.south east)!0.2!(P.north east)$);
  \coordinate[] (outputL) at ($(P.south east)!0.8!(P.north east)$);

  \node[block, above=0.6 of P] (Klac) {$\bm{K}_\text{IFF}$};
  \node[addb, left= of input] (addF) {};

  \node[block, left= of addF] (Khac) {$\bm{K}_\text{HAC}$};

  % Connections and labels
  \draw[->] (outputL) -- ++(0.6, 0) coordinate(eastlac) |- (Klac.east);
  \node[above right] at (outputL){$\bm{V}_s$};
  \draw[->] (Klac.west) -| (addF.north);
  \draw[->] (addF.east) -- (input) node[above left]{$\bm{u}$};

  \draw[->] (outputH) -- ++(1.6, 0) node[above left]{$\bm{\epsilon\mathcal{L}}$};
  \draw[->] (Khac.east) node[above right]{$\bm{u}^{\prime}$} -- (addF.west);

  \draw[->] ($(outputH) + (1.2, 0)$)node[branch]{} |- ($(Khac.west)+(-0.6, -1.6)$) |- (Khac.west);

  \begin{scope}[on background layer]
    \node[fit={(Klac.north-|eastlac) (addF.west|-P.south)}, fill=black!20!white, draw, dashed, inner sep=10pt] (Pi) {};
    \node[anchor={north west}] at (Pi.north west){$\text{Damped Plant}$};
  \end{scope}
\end{tikzpicture}
#+end_src

#+name: fig:test_id31_iff_hac_schematic
#+caption: Block diagram of the implemented HAC-IFF controllers. The controller $\bm{K}_{\text{HAC}}$ is a diagonal controller with the same elements on every diagonal term $K_{\text{HAC}}$.
#+RESULTS:
[[file:figs/test_id31_iff_hac_schematic.png]]

** Matlab Init                                              :noexport:ignore:
#+begin_src matlab
%% test_id31_4_hac.m
#+end_src

#+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 :noweb yes
<<m-init-path>>
#+end_src

#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle>>
#+end_src

#+begin_src matlab :noweb yes
<<m-init-simscape>>
#+end_src

#+begin_src matlab :noweb yes
<<m-init-other>>
#+end_src

** Damped Plant
<<ssec:test_id31_iff_hac_plant>>

The damped plants (i.e. the transfer function from $\bm{u}^\prime$ to $\bm{\epsilon\mathcal{L}}$) were identified for all payload conditions.
To verify if the model accurately represents the damped plants, both direct terms and coupling terms corresponding to the first actuator are compared in Figure ref:fig:test_id31_comp_simscape_hac.

#+begin_src matlab
%% Identification of the damped Plant (transfer function from u' to dL)

% Load identification data
data_m0 = load('2023-08-17_17-53_damp_plant_m0_Wz0.mat');
data_m1 = load('2023-08-10_16-01_damp_plant_m1_Wz0.mat');
data_m2 = load('2023-08-10_17-28_damp_plant_m2_Wz0.mat');
data_m3 = load('2023-08-10_18-16_damp_plant_m3_Wz0.mat');

% Hannning Windows
Ts = 1e-4; % Sampling Time [s]
Nfft = floor(2.0/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

% And we get the frequency vector
[~, f] = tfestimate(data_m0.uL1.id_plant, data_m0.uL1.e_L1, win, Noverlap, Nfft, 1/Ts);

% Identification without any payload
G_hac_m0_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
    eL = [data_m0.(sprintf("uL%i", i_strut)).e_L1 ; data_m0.(sprintf("uL%i", i_strut)).e_L2 ; data_m0.(sprintf("uL%i", i_strut)).e_L3 ; data_m0.(sprintf("uL%i", i_strut)).e_L4 ; data_m0.(sprintf("uL%i", i_strut)).e_L5 ; data_m0.(sprintf("uL%i", i_strut)).e_L6]';

    G_hac_m0_Wz0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts);
end

% Identification with 1 "payload layer"
G_hac_m1_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
    eL = [data_m1.(sprintf("uL%i", i_strut)).e_L1 ; data_m1.(sprintf("uL%i", i_strut)).e_L2 ; data_m1.(sprintf("uL%i", i_strut)).e_L3 ; data_m1.(sprintf("uL%i", i_strut)).e_L4 ; data_m1.(sprintf("uL%i", i_strut)).e_L5 ; data_m1.(sprintf("uL%i", i_strut)).e_L6]';

    G_hac_m1_Wz0(:,:,i_strut) = tfestimate(data_m1.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts);
end

% Identification with 2 "payload layers"
G_hac_m2_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
    eL = [data_m2.(sprintf("uL%i", i_strut)).e_L1 ; data_m2.(sprintf("uL%i", i_strut)).e_L2 ; data_m2.(sprintf("uL%i", i_strut)).e_L3 ; data_m2.(sprintf("uL%i", i_strut)).e_L4 ; data_m2.(sprintf("uL%i", i_strut)).e_L5 ; data_m2.(sprintf("uL%i", i_strut)).e_L6]';

    G_hac_m2_Wz0(:,:,i_strut) = tfestimate(data_m2.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts);
end

% Identification with 3 "payload layers"
G_hac_m3_Wz0 = zeros(length(f), 6, 6);
for i_strut = 1:6
    eL = [data_m3.(sprintf("uL%i", i_strut)).e_L1 ; data_m3.(sprintf("uL%i", i_strut)).e_L2 ; data_m3.(sprintf("uL%i", i_strut)).e_L3 ; data_m3.(sprintf("uL%i", i_strut)).e_L4 ; data_m3.(sprintf("uL%i", i_strut)).e_L5 ; data_m3.(sprintf("uL%i", i_strut)).e_L6]';

    G_hac_m3_Wz0(:,:,i_strut) = tfestimate(data_m3.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts);
end
#+end_src

#+begin_src matlab :exports none :tangle no
% The identified dynamics are then saved for further use.
save('./matlab/mat/test_id31_identified_damped_plants.mat', 'G_hac_m0_Wz0', 'G_hac_m1_Wz0', 'G_hac_m2_Wz0', 'G_hac_m3_Wz0', 'f');
#+end_src

#+begin_src matlab :eval no
% The identified dynamics are then saved for further use.
save('./mat/test_id31_identified_damped_plants.mat', 'G_hac_m0_Wz0', 'G_hac_m1_Wz0', 'G_hac_m2_Wz0', 'G_hac_m3_Wz0', 'f');
#+end_src

#+begin_src matlab
% Load the estimated damped plant from the multi-body model
load('test_id31_simscape_damped_plants.mat', 'Gm_hac_m0_Wz0', 'Gm_hac_m1_Wz0', 'Gm_hac_m2_Wz0', 'Gm_hac_m3_Wz0');
% Load the undamped plant for comparison
load('test_id31_identified_open_loop_plants.mat', 'G_int_m0_Wz0', 'G_int_m1_Wz0', 'G_int_m2_Wz0', 'G_int_m3_Wz0', 'f');
#+end_src

#+begin_src matlab :exports none
figure;
tiledlayout(2, 3, 'TileSpacing', 'tight', 'Padding', 'tight');

ax1 = nexttile();
hold on;
plot(f, abs(G_hac_m0_Wz0(:, 1, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))));
text(12, 3e-5, '$\epsilon_{\mathcal{L}1}/u_1^\prime$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]');
yticks([1e-7, 1e-6, 1e-5]);

ax2 = nexttile();
hold on;
plot(f, abs(G_hac_m0_Wz0(:, 2, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL2', 'u1'), freqs, 'Hz'))));
text(12, 3e-5, '$\epsilon_{\mathcal{L}2}/u_1^\prime$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);

ax3 = nexttile();
hold on;
plot(f, abs(G_hac_m0_Wz0(:, 3, 1)))
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL3', 'u1'), freqs, 'Hz'))))
text(12, 3e-5, '$\epsilon_{\mathcal{L}3}/u_1^\prime$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);

ax4 = nexttile();
hold on;
plot(f, abs(G_hac_m0_Wz0(:, 4, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL4', 'u1'), freqs, 'Hz'))));
text(12, 3e-5, '$\epsilon_{\mathcal{L}4}/u_1^\prime$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
xticks([10, 20, 50, 100, 200])
yticks([1e-7, 1e-6, 1e-5]);

ax5 = nexttile();
hold on;
plot(f, abs(G_hac_m0_Wz0(:, 5, 1)));
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL5', 'u1'), freqs, 'Hz'))));
text(12, 3e-5, '$\epsilon_{\mathcal{L}5}/u_1^\prime$', 'Horiz','left', 'Vert','top')
hold off;
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xticks([10, 20, 50, 100, 200])

ax6 = nexttile();
hold on;
plot(f, abs(G_hac_m0_Wz0(:, 6, 1)), ...
     'DisplayName', 'Measurements');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL6', 'u1'), freqs, 'Hz'))), ...
     'DisplayName', 'Model (2-DoF APA)');
text(12, 3e-5, '$\epsilon_{\mathcal{L}6}/u_1^\prime$', 'Horiz','left', 'Vert','top')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
xticks([10, 20, 50, 100, 200])
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;

linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'xy');
xlim([10, 5e2]); ylim([1e-7, 4e-5]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/test_id31_comp_simscape_hac.pdf', 'width', 'full', 'height', 700);
#+end_src

#+name: fig:test_id31_comp_simscape_hac
#+caption: Comparison of the measured (in blue) and modeled (in red) frequency transfer functions from the first control signal ($u_1^\prime$) of the damped plant to the estimated errors ($\epsilon_{\mathcal{L}_i}$) in the frame of the six struts by the external metrology
#+RESULTS:
[[file:figs/test_id31_comp_simscape_hac.png]]

The six direct terms for all four payload conditions are compared with the model in Figure ref:fig:test_id31_hac_plant_effect_mass.
It is shown that the model accurately represents the dynamics for all payloads.

In Section ref:sec:test_id31_iff_hac, a High Authority Controller is tuned to be robust to the change of dynamics due to different payloads used.
Without decentralized IFF being applied, the controller would have had to be robust to all the undamped dynamics shown in Figure ref:fig:test_id31_comp_all_undamped_damped_plants, which is a very complex control problem.
With the applied decentralized IFF, the HAC instead had to be be robust to all the damped dynamics shown in Figure ref:fig:test_id31_comp_all_undamped_damped_plants, which is easier from a control perspective.
This is one of the key benefit of using the HAC-LAC strategy.

#+begin_src matlab :exports none :results none
%% Comparison of the identified HAC plant and the HAC plant extracted from the simscape model
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_hac_m0_Wz0(:, 1, 1)), 'color', [colors(1,:), 0.2], ...
     'DisplayName', '$m = 0$ kg');
for i = 2:6
    plot(f, abs(G_hac_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.2], ...
         'HandleVisibility', 'off')
end
plot(f, abs(G_hac_m1_Wz0(:, 1, 1)), 'color', [colors(2,:), 0.2], ...
     'DisplayName', '$m = 13$ kg');
for i = 2:6
    plot(f, abs(G_hac_m1_Wz0(:,i, i)), 'color', [colors(2,:), 0.2], ...
         'HandleVisibility', 'off')
end
plot(f, abs(G_hac_m2_Wz0(:, 1, 1)), 'color', [colors(3,:), 0.2], ...
     'DisplayName', '$m = 26$ kg');
for i = 2:6
    plot(f, abs(G_hac_m2_Wz0(:,i, i)), 'color', [colors(3,:), 0.2], ...
         'HandleVisibility', 'off')
end
plot(f, abs(G_hac_m3_Wz0(:, 1, 1)), 'color', [colors(4,:), 0.2], ...
     'DisplayName', '$m = 39$ kg');
for i = 2:6
    plot(f, abs(G_hac_m3_Wz0(:,i, i)), 'color', [colors(4,:), 0.2], ...
         'HandleVisibility', 'off')
end
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:), ...
     'DisplayName', '$m = 0$ kg (model)');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m1_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:), ...
     'DisplayName', '$m = 13$ kg (model)');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m2_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:), ...
     'DisplayName', '$m = 26$ kg (model)');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m3_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:), ...
     'DisplayName', '$m = 39$ kg (model)');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([2e-7, 3e-5]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
plot(f, 180/pi*unwrapphase(angle(G_hac_m0_Wz0(:,1,1)), f), 'color', [colors(1,:), 0.2]);
for i = 2:6
    plot(f, 180/pi*unwrapphase(angle(G_hac_m0_Wz0(:,i, i)), f), 'color', [colors(1,:), 0.2]);
end
plot(f, 180/pi*unwrapphase(angle(G_hac_m1_Wz0(:,1,1)), f), 'color', [colors(2,:), 0.2]);
for i = 2:6
    plot(f, 180/pi*unwrapphase(angle(G_hac_m1_Wz0(:,i, i)), f), 'color', [colors(2,:), 0.2]);
end
plot(f, 180/pi*unwrapphase(angle(G_hac_m2_Wz0(:,1,1)), f), 'color', [colors(3,:), 0.2]);
for i = 2:6
    plot(f, 180/pi*unwrapphase(angle(G_hac_m2_Wz0(:,i, i)), f), 'color', [colors(3,:), 0.2]);
end
plot(f, 180/pi*unwrapphase(angle(G_hac_m3_Wz0(:,1,1)), f), 'color', [colors(4,:), 0.2]);
for i = 2:6
    plot(f, 180/pi*unwrapphase(angle(G_hac_m3_Wz0(:,i, i)), f), 'color', [colors(4,:), 0.2]);
end
plot(freqs, 180/pi*unwrapphase(angle(squeeze(freqresp(-exp(-3e-4*s)*Gm_hac_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))), f), '--', 'color', colors(1,:));
plot(freqs, 180/pi*unwrapphase(angle(squeeze(freqresp(-exp(-3e-4*s)*Gm_hac_m1_Wz0('eL1', 'u1'), freqs, 'Hz'))), f), '--', 'color', colors(2,:));
plot(freqs, 180/pi*unwrapphase(angle(squeeze(freqresp(-exp(-3e-4*s)*Gm_hac_m2_Wz0('eL1', 'u1'), freqs, 'Hz'))), f), '--', 'color', colors(3,:));
plot(freqs, 180/pi*unwrapphase(angle(squeeze(freqresp(-exp(-3e-4*s)*Gm_hac_m3_Wz0('eL1', 'u1'), freqs, 'Hz'))), f), '--', 'color', colors(4,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-270, 20])

linkaxes([ax1,ax2],'x');
xlim([1, 5e2]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/test_id31_hac_plant_effect_mass.pdf', 'width', 'half', 'height', 600);
#+end_src

#+begin_src matlab :exports none :results none
%% Comparison of all the undamped FRF and all the damped FRF
figure;
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_int_m0_Wz0(:,1,1)), 'color', [colors(1,:), 0.5], 'DisplayName', 'Undamped - $\epsilon\mathcal{L}_i/u_i$');
plot(f, abs(G_hac_m0_Wz0(:,1,1)), 'color', [colors(2,:), 0.5], 'DisplayName', 'damped - $\epsilon\mathcal{L}_i/u_i^\prime$');
for i = 1:6
    plot(f, abs(G_int_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
    plot(f, abs(G_int_m1_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
    plot(f, abs(G_int_m2_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
    plot(f, abs(G_int_m3_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
end
for i = 1:6
    plot(f, abs(G_hac_m0_Wz0(:,i, i)), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
    plot(f, abs(G_hac_m1_Wz0(:,i, i)), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
    plot(f, abs(G_hac_m2_Wz0(:,i, i)), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
    plot(f, abs(G_hac_m3_Wz0(:,i, i)), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
ylim([2e-7, 4e-4]);

ax2 = nexttile;
hold on;
for i =1:6
    plot(f, 180/pi*unwrapphase(angle(-G_int_m0_Wz0(:,i, i)), f), 'color', [colors(1,:), 0.5]);
    plot(f, 180/pi*unwrapphase(angle(-G_int_m1_Wz0(:,i, i)), f), 'color', [colors(1,:), 0.5]);
    plot(f, 180/pi*unwrapphase(angle(-G_int_m2_Wz0(:,i, i)), f), 'color', [colors(1,:), 0.5]);
    plot(f, 180/pi*unwrapphase(angle(-G_int_m3_Wz0(:,i, i)), f), 'color', [colors(1,:), 0.5]);
end
for i = 1:6
    plot(f, 180/pi*unwrapphase(angle(G_hac_m0_Wz0(:,i, i)), f), 'color', [colors(2,:), 0.5]);
    plot(f, 180/pi*unwrapphase(angle(G_hac_m1_Wz0(:,i, i)), f), 'color', [colors(2,:), 0.5]);
    plot(f, 180/pi*unwrapphase(angle(G_hac_m2_Wz0(:,i, i)), f), 'color', [colors(2,:), 0.5]);
    plot(f, 180/pi*unwrapphase(angle(G_hac_m3_Wz0(:,i, i)), f), 'color', [colors(2,:), 0.5]);
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-270, 20])

linkaxes([ax1,ax2],'x');
xlim([1, 5e2]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/test_id31_comp_all_undamped_damped_plants.pdf', 'width', 'half', 'height', 600);
#+end_src

#+name: fig:test_id31_hac_plant_effect_mass_comp_model
#+caption: Comparison of the measured damped plants and modeled plants for all considered payloads, only "direct" terms ($\epsilon\mathcal{L}_i/u_i^\prime$) are displayed (\subref{fig:test_id31_hac_plant_effect_mass}). Comparison of all undamped $\epsilon\mathcal{L}_i/u_i$ and damped $\epsilon\mathcal{L}_i/u_i^\prime$ measured frequency response functions for all payloads is done in (\subref{fig:test_id31_comp_all_undamped_damped_plants}).
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_hac_plant_effect_mass}Effect of payload mass on $\epsilon\mathcal{L}_i/u_i^\prime$}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_id31_hac_plant_effect_mass.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_comp_all_undamped_damped_plants}Undamped and damped plants}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_id31_comp_all_undamped_damped_plants.png]]
#+end_subfigure
#+end_figure

** Robust Controller Design
<<ssec:test_id31_iff_hac_controller>>

A first diagonal controller was designed to be robust to change of payloads, which means that every damped plants shown in Figure ref:fig:test_id31_comp_all_undamped_damped_plants should be considered during the controller design.
For a first design, a crossover frequency of $5\,\text{Hz}$ for chosen.
One integrator is added to increase the low frequency gain, a lead is added around $5\,\text{Hz}$ to increase the stability margins and a first order low pass filter with a cut-off frequency of $30\,\text{Hz}$ is added to improve the robustness to dynamical uncertainty at high frequency.
The obtained "decentralized" loop-gains are shown in Figure ref:fig:test_id31_hac_loop_gain.

\begin{equation}\label{eq:test_id31_robust_hac}
K_{\text{HAC}} = g_0 \cdot \underbrace{\frac{\omega_c}{s}}_{\text{int}} \cdot \underbrace{\frac{1}{\sqrt{\alpha}}\frac{1 + \frac{s}{\omega_c/\sqrt{\alpha}}}{1 + \frac{s}{\omega_c\sqrt{\alpha}}}}_{\text{lead}} \cdot \underbrace{\frac{1}{1 + \frac{s}{\omega_0}}}_{\text{LPF}}, \quad \left( \omega_c = 2\pi5\,\text{rad/s},\ \alpha = 2,\ \omega_0 = 2\pi30\,\text{rad/s} \right)
\end{equation}

The closed-loop stability is verified by computing the characteristic Loci (Figure ref:fig:test_id31_hac_characteristic_loci).

#+begin_src matlab
%% HAC Design
% Wanted crossover
wc = 2*pi*5; % [rad/s]

% Integrator
H_int = wc/s;

% Lead to increase phase margin
a  = 2;  % Amount of phase lead / width of the phase lead / high frequency gain
H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));

% Low Pass filter to increase robustness
H_lpf = 1/(1 + s/2/pi/30);

% Gain to have unitary crossover at 5Hz
[~, i_f] = min(abs(f - wc/2/pi));
H_gain = 1./abs(G_hac_m0_Wz0(i_f, 1, 1));

% Decentralized HAC
Khac = H_gain  * ... % Gain
       H_int   * ... % Integrator
       H_lpf   * ... % Low Pass filter
       eye(6);       % 6x6 Diagonal
#+end_src

#+begin_src matlab :exports none :tangle no
% The designed HAC controller is saved
save('./matlab/mat/test_id31_K_hac_robust.mat', 'Khac');
#+end_src

#+begin_src matlab :eval no
% The designed HAC controller is saved
save('./mat/test_id31_K_hac_robust.mat', 'Khac');
#+end_src

#+begin_src matlab :exports none :results none
%% Decentralized Loop gain for the High Authority Controller
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_hac_m0_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(1,:), 'DisplayName', '$0$ kg');
plot(f, abs(G_hac_m1_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(2,:), 'DisplayName', '$13$ kg');
plot(f, abs(G_hac_m2_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(3,:), 'DisplayName', '$26$ kg');
plot(f, abs(G_hac_m3_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(4,:), 'DisplayName', '$39$ kg');
xline(5, '--', 'linewidth', 1, 'color', [0,0,0,0.2], 'HandleVisibility', 'off')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
ylim([1e-5, 1e2]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(G_hac_m0_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(1,:));
plot(f, 180/pi*angle(G_hac_m1_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(2,:));
plot(f, 180/pi*angle(G_hac_m2_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(3,:));
plot(f, 180/pi*angle(G_hac_m3_Wz0(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(4,:));
xline(5, '--', 'linewidth', 1, 'color', [0,0,0,0.2])
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-180, 180])

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

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/test_id31_hac_loop_gain.pdf', 'width', 'half', 'height', 600);
#+end_src

#+begin_src matlab :exports none
%% Compute the Eigenvalues of the loop gain
Ldet = zeros(4, 6, length(f));

% Loop gain
Lmimo = pagemtimes(permute(G_hac_m0, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
for i_f = 2:length(f)
    Ldet(1,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
end

Lmimo = pagemtimes(permute(G_hac_m1, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
for i_f = 2:length(f)
    Ldet(2,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
end

Lmimo = pagemtimes(permute(G_hac_m2, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
for i_f = 2:length(f)
    Ldet(3,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
end

Lmimo = pagemtimes(permute(G_hac_m3, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
for i_f = 2:length(f)
    Ldet(4,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
end
#+end_src

#+begin_src matlab :exports none
%% Plot of the eigenvalues of L in the complex plane
figure;
hold on;
for i_mass = 1:4
    plot(real(squeeze(Ldet(i_mass, 1,:))), imag(squeeze(Ldet(i_mass, 1,:))), ...
         '.', 'color', colors(i_mass, :), ...
         'DisplayName', sprintf('$m_%i$', i_mass));
    plot(real(squeeze(Ldet(i_mass, 1,:))), -imag(squeeze(Ldet(i_mass, 1,:))), ...
         '.', 'color', colors(i_mass, :), ...
         'HandleVisibility', 'off');
    for i = 1:6
        plot(real(squeeze(Ldet(i_mass, i,:))), imag(squeeze(Ldet(i_mass, i,:))), ...
             '.', 'color', colors(i_mass, :), ...
             'HandleVisibility', 'off');
        plot(real(squeeze(Ldet(i_mass, i,:))), -imag(squeeze(Ldet(i_mass, i,:))), ...
             '.', 'color', colors(i_mass, :), ...
             'HandleVisibility', 'off');
    end
end
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
hold off;
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
xlabel('Real'); ylabel('Imag');
legend('location', 'southeast');
axis square
xlim([-1.5, 0.5]); ylim([-1, 1]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/test_id31_hac_characteristic_loci.pdf', 'width', 'half', 'height', 600);
#+end_src

#+name: fig:test_id31_hac_loop_gain_loci
#+caption: Robust High Authority Controller. "Decentralized loop-gains" are shown in (\subref{fig:test_id31_hac_loop_gain}) and characteristic loci are shown in (\subref{fig:test_id31_hac_characteristic_loci})
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_hac_loop_gain}Loop Gains}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_id31_hac_loop_gain.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_hac_characteristic_loci}Characteristic Loci}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_id31_hac_characteristic_loci.png]]
#+end_subfigure
#+end_figure

** Estimation of performances
<<ssec:test_id31_iff_hac_perf>>

To estimate the performances that can be expected with this HAC-LAC architecture and the designed controllers, two simulations of tomography experiments were performed[fn:4].
The rotational velocity was set to 30rpm, and no payload was added on top of the nano-hexapod.
An open-loop simulation and a closed-loop simulation were performed and compared in Figure ref:fig:test_id31_tomo_m0_30rpm_robust_hac_iff_sim.

#+begin_src matlab
%% Tomography experiment
% Sample is not centered with the rotation axis
% This is done by offsetfing the micro-hexapod by 0.9um
P_micro_hexapod = [2.5e-6; 0; -0.3e-6]; % [m]

set_param(mdl, 'StopTime', '3'); % 6 turns at 30rpm

initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod('AP', P_micro_hexapod);
initializeNanoHexapod('flex_bot_type', '2dof', ...
                      'flex_top_type', '3dof', ...
                      'motion_sensor_type', 'plates', ...
                      'actuator_type', '2dof');
initializeSample('type', '0');

initializeSimscapeConfiguration('gravity', false);
initializeLoggingConfiguration('log', 'all', 'Ts', 1e-4);
initializeController('type', 'open-loop');

initializeDisturbances(...
    'Dw_x',  true, ... % Ground Motion - X direction
    'Dw_y',  true, ... % Ground Motion - Y direction
    'Dw_z',  true, ... % Ground Motion - Z direction
    'Fdy_x', false, ... % Translation Stage - X direction
    'Fdy_z', false, ... % Translation Stage - Z direction
    'Frz_x', true, ... % Spindle - X direction
    'Frz_y', true, ... % Spindle - Y direction
    'Frz_z', true);    % Spindle - Z direction

initializeReferences(...
    'Rz_type', 'rotating', ...
    'Rz_period', 360/180, ... % 180deg/s, 30rpm
    'Dh_pos', [P_micro_hexapod; 0; 0; 0]);

% Open-Loop Simulation
sim(mdl);
exp_tomo_ol_m0_Wz180 = simout;

% Closed-Loop Simulation
load('test_id31_K_iff.mat', 'Kiff');
load('test_id31_K_hac_robust.mat', 'Khac');
initializeController('type', 'hac-iff');
initializeSample('type', '0');
sim(mdl);
exp_tomo_cl_m0_Wz180 = simout;
#+end_src

#+begin_src matlab :exports none :tangle no
% Save the simulation results
save('./matlab/mat/test_id31_exp_tomo_ol_cl_30rpm_sim.mat', 'exp_tomo_ol_m0_Wz180', 'exp_tomo_cl_m0_Wz180');
#+end_src

#+begin_src matlab :eval no
% Save the simulation results
save('./mat/test_id31_exp_tomo_ol_cl_30rpm_sim.mat', 'exp_tomo_ol_m0_Wz180', 'exp_tomo_cl_m0_Wz180');
#+end_src

#+begin_src matlab :exports none :results none
%% Simulation of tomography experiment - no payload, 30rpm - XY errors
figure;
hold on;
plot(1e6*exp_tomo_ol_m0_Wz180.y.x.Data, 1e6*exp_tomo_ol_m0_Wz180.y.y.Data, 'DisplayName', 'OL')
plot(1e6*exp_tomo_cl_m0_Wz180.y.x.Data(1:2e3), 1e6*exp_tomo_cl_m0_Wz180.y.y.Data(1:2e3), 'color', colors(3,:), 'HandleVisibility', 'off')
plot(1e6*exp_tomo_cl_m0_Wz180.y.x.Data(2e3:end), 1e6*exp_tomo_cl_m0_Wz180.y.y.Data(2e3:end), 'color', colors(2,:), 'DisplayName', 'CL')
hold off;
xlabel('$D_x$ [$\mu$m]'); ylabel('$D_y$ [$\mu$m]');
axis equal
xlim([-3, 3]); ylim([-3, 3]);
xticks([-3:1:3]);
yticks([-3:1:3]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/test_id31_tomo_m0_30rpm_robust_hac_iff_sim_xy.pdf', 'width', 'half', 'height', 'normal');
#+end_src

#+begin_src matlab :exports none :results none
%% Simulation of tomography experiment - no payload, 30rpm - YZ errors
figure;
hold on;
plot(1e6*exp_tomo_ol_m0_Wz180.y.y.Data, 1e6*exp_tomo_ol_m0_Wz180.y.z.Data, 'DisplayName', 'OL')
plot(1e6*exp_tomo_cl_m0_Wz180.y.y.Data(1:2e3), 1e6*exp_tomo_cl_m0_Wz180.y.z.Data(1:2e3), 'color', colors(3,:), 'HandleVisibility', 'off')
plot(1e6*exp_tomo_cl_m0_Wz180.y.y.Data(2e3:end), 1e6*exp_tomo_cl_m0_Wz180.y.z.Data(2e3:end), 'color', colors(2,:), 'DisplayName', 'CL')
hold off;
xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]');
axis equal
xlim([-3, 3]); ylim([-1, 1]);
xticks([-3:1:3]);
yticks([-3:1:3]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/test_id31_tomo_m0_30rpm_robust_hac_iff_sim_yz.pdf', 'width', 'half', 'height', 'normal');
#+end_src

#+name: fig:test_id31_tomo_m0_30rpm_robust_hac_iff_sim
#+caption: Position error of the sample in the XY (\subref{fig:test_id31_tomo_m0_30rpm_robust_hac_iff_sim_xy}) and YZ (\subref{fig:test_id31_tomo_m0_30rpm_robust_hac_iff_sim_yz}) planes during a simulation of a tomography experiment at 30RPM. No payload is placed on top of the nano-hexapod.
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_tomo_m0_30rpm_robust_hac_iff_sim_xy}XY plane}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :scale 0.9
[[file:figs/test_id31_tomo_m0_30rpm_robust_hac_iff_sim_xy.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_tomo_m0_30rpm_robust_hac_iff_sim_yz}YZ plane}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :scale 0.9
[[file:figs/test_id31_tomo_m0_30rpm_robust_hac_iff_sim_yz.png]]
#+end_subfigure
#+end_figure

Then the same tomography experiment (i.e. constant spindle rotation at 30rpm, and no payload) was performed experimentally.
The measured position of the "point of interest" during the experiment are shown in Figure ref:fig:test_id31_tomo_m0_30rpm_robust_hac_iff_exp.

#+begin_src matlab
%% Experimental Results for Tomography at 30RPM, no payload
% Load measured noise
data_tomo_m0_Wz180 = load('2023-08-17_15-26_tomography_30rpm_m0_robust.mat');

[~, i_cl] = find(data_tomo_m0_Wz180.hac_status == 1);
#+end_src

#+begin_src matlab :exports none :results none
%% Measured radial errors of the Spindle
figure;
hold on;
plot(1e6*data_tomo_m0_Wz180.Dx_int(1:i_cl), 1e6*data_tomo_m0_Wz180.Dy_int(1:i_cl), 'DisplayName', 'OL')
plot(1e6*data_tomo_m0_Wz180.Dx_int(i_cl:i_cl+1e4), 1e6*data_tomo_m0_Wz180.Dy_int(i_cl:i_cl+1e4), 'color', colors(3,:), 'HandleVisibility', 'off')
plot(1e6*data_tomo_m0_Wz180.Dx_int(i_cl+1e4:end), 1e6*data_tomo_m0_Wz180.Dy_int(i_cl+1e4:end), 'color', colors(2,:), 'DisplayName', 'CL')
hold off;
xlabel('$D_x$ [$\mu$m]'); ylabel('$D_y$ [$\mu$m]');
axis equal
xlim([-3, 3]); ylim([-3, 3]);
xticks([-3:1:3]);
yticks([-3:1:3]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/test_id31_tomo_m0_30rpm_robust_hac_iff_exp_xy.pdf', 'width', 'half', 'height', 'normal');
#+end_src

#+begin_src matlab :exports none :results none
%% Measured radial errors of the Spindle
figure;
hold on;
plot(1e6*data_tomo_m0_Wz180.Dy_int(1:i_cl), 1e6*data_tomo_m0_Wz180.Dz_int(1:i_cl), 'DisplayName', 'OL')
plot(1e6*data_tomo_m0_Wz180.Dy_int(i_cl:i_cl+1e4), 1e6*data_tomo_m0_Wz180.Dz_int(i_cl:i_cl+1e4), 'color', colors(3,:), 'HandleVisibility', 'off')
plot(1e6*data_tomo_m0_Wz180.Dy_int(i_cl+1e4:end), 1e6*data_tomo_m0_Wz180.Dz_int(i_cl+1e4:end), 'color', colors(2,:), 'DisplayName', 'CL')
hold off;
xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]');
axis equal
xlim([-3, 3]); ylim([-1, 1]);
xticks([-3:1:3]);
yticks([-3:1:3]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/test_id31_tomo_m0_30rpm_robust_hac_iff_exp_yz.pdf', 'width', 'half', 'height', 'normal');
#+end_src

#+name: fig:test_id31_tomo_m0_30rpm_robust_hac_iff_exp
#+caption: Experimental results of a tomography experiment at 30RPM without payload. Position error of the sample is shown in the XY (\subref{fig:test_id31_tomo_m0_30rpm_robust_hac_iff_exp_xy}) and YZ (\subref{fig:test_id31_tomo_m0_30rpm_robust_hac_iff_exp_yz}) planes.
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_tomo_m0_30rpm_robust_hac_iff_exp_xy}XY plane}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :scale 0.9
[[file:figs/test_id31_tomo_m0_30rpm_robust_hac_iff_exp_xy.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_tomo_m0_30rpm_robust_hac_iff_exp_yz}YZ plane}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :scale 0.9
[[file:figs/test_id31_tomo_m0_30rpm_robust_hac_iff_exp_yz.png]]
#+end_subfigure
#+end_figure

Even though the simulation (Figure ref:fig:test_id31_tomo_m0_30rpm_robust_hac_iff_sim) and the experimental results (Figure ref:fig:test_id31_tomo_m0_30rpm_robust_hac_iff_exp) are looking similar, the most important metric to compare is the RMS values of the positioning errors in closed-loop.
These are computed for both the simulation and the experimental results and are compared in Table ref:tab:test_id31_tomo_m0_30rpm_robust_hac_iff_rms.
The lateral and vertical errors are similar, however the tilt ($R_y$) errors are underestimated by the model, which is reasonable as disturbances in $R_y$ were not modeled.

Results obtained with this conservative HAC are already close to the specifications.

#+begin_src matlab
%% Compute RMS of errors

% Simulation - OL
rms_Dy_m0_Wz180_ol_sim = rms(detrend(exp_tomo_ol_m0_Wz180.y.y.Data, 0));
rms_Dz_m0_Wz180_ol_sim = rms(detrend(exp_tomo_ol_m0_Wz180.y.z.Data, 0));
rms_Ry_m0_Wz180_ol_sim = rms(detrend(squeeze(atan2(exp_tomo_ol_m0_Wz180.y.R.Data(1,3,:), sqrt(exp_tomo_ol_m0_Wz180.y.R.Data(1,1,:).^2+exp_tomo_ol_m0_Wz180.y.R.Data(1,2,:).^2))), 0));
i_stab = 1000; % Remove the transient that is irrelevant here
% Simulation - CL
rms_Dy_m0_Wz180_cl_sim = rms(detrend(exp_tomo_cl_m0_Wz180.y.y.Data(i_stab:end), 0));
rms_Dz_m0_Wz180_cl_sim = rms(detrend(exp_tomo_cl_m0_Wz180.y.z.Data(i_stab:end), 0));
rms_Ry_m0_Wz180_cl_sim = rms(detrend(squeeze(atan2(exp_tomo_cl_m0_Wz180.y.R.Data(1,3,i_stab:end), sqrt(exp_tomo_cl_m0_Wz180.y.R.Data(1,1,i_stab:end).^2+exp_tomo_cl_m0_Wz180.y.R.Data(1,2,i_stab:end).^2))), 0));

% Experimental - OL
[~, i_cl] = find(data_tomo_m0_Wz180.hac_status == 1);
rms_Dy_m0_Wz180_ol_exp = rms(detrend(data_tomo_m0_Wz180.Dy_int(1:i_cl), 0));
rms_Dz_m0_Wz180_ol_exp = rms(detrend(data_tomo_m0_Wz180.Dz_int(1:i_cl), 0));
rms_Ry_m0_Wz180_ol_exp = rms(detrend(data_tomo_m0_Wz180.Ry_int(1:i_cl), 0));
% Experimental - CL
rms_Dy_m0_Wz180_cl_exp = rms(detrend(data_tomo_m0_Wz180.Dy_int(i_cl+10000:end), 0));
rms_Dz_m0_Wz180_cl_exp = rms(detrend(data_tomo_m0_Wz180.Dz_int(i_cl+10000:end), 0));
rms_Ry_m0_Wz180_cl_exp = rms(detrend(data_tomo_m0_Wz180.Ry_int(i_cl+10000:end), 0));
#+end_src

#+name: tab:test_id31_tomo_m0_30rpm_robust_hac_iff_rms
#+caption: RMS values of the errors for a tomography experiment at 30RPM and without payload. Experimental results and simulation are compared.
#+attr_latex: :environment tabularx :width 0.7\linewidth :align Xccc
#+attr_latex: :center t :booktabs t
|                     | $D_y$                 | $D_z$              | $R_y$                  |
|---------------------+-----------------------+--------------------+------------------------|
| Experiment (OL)     | $1.8\,\mu\text{mRMS}$ | $24\,\text{nmRMS}$ | $10\,\mu\text{radRMS}$ |
|---------------------+-----------------------+--------------------+------------------------|
| Simulation (CL)     | $30\,\text{nmRMS}$    | $8\,\text{nmRMS}$  | $73\,\text{nradRMS}$   |
| Experiment (CL)     | $39\,\text{nmRMS}$    | $11\,\text{nmRMS}$ | $130\,\text{nradRMS}$  |
|---------------------+-----------------------+--------------------+------------------------|
| Specifications (CL) | $30\,\text{nmRMS}$    | $15\,\text{nmRMS}$ | $250\,\text{nradRMS}$  |

** Robustness to change of payload
<<ssec:test_id31_iff_hac_robustness>>

To verify the robustness to the change of payload mass, four simulations of tomography experiments were performed with payloads as shown Figure ref:fig:test_id31_picture_masses (i.e. $0\,kg$, $13\,kg$, $26\,kg$ and $39\,kg$).
This time, the rotational velocity was set at 1rpm (i.e. 6deg/s), as it is the typical rotational velocity for heavy samples.
The closed-loop systems were stable for all payload conditions, indicating good control robustness.

#+begin_src matlab
%% Simulation of tomography experiments at 1RPM with all payloads

% Configuration
set_param(mdl, 'StopTime', '2'); % 30 degrees at 1rpm
initializeLoggingConfiguration('log', 'all', 'Ts', 1e-3);
initializeController('type', 'hac-iff');
initializeReferences(...
    'Rz_type', 'rotating', ...
    'Rz_period', 360/6, ... % 6deg/s, 1 rpm
    'Dh_pos', [P_micro_hexapod; 0; 0; 0]);

% Perform the simulations
initializeSample('type', '0');
sim(mdl);
exp_tomo_cl_m0_1rpm = simout;
initializeSample('type', '1');
sim(mdl);
exp_tomo_cl_m1_1rpm = simout;
initializeSample('type', '2');
sim(mdl);
exp_tomo_cl_m2_1rpm = simout;
initializeSample('type', '3');
sim(mdl);
exp_tomo_cl_m3_1rpm = simout;
#+end_src

#+begin_src matlab :exports none :tangle no
% Save the simulation results
save('./matlab/mat/test_id31_exp_tomo_cl_1rpm_sim.mat', 'exp_tomo_cl_m0_1rpm', 'exp_tomo_cl_m1_1rpm', 'exp_tomo_cl_m2_1rpm', 'exp_tomo_cl_m3_1rpm');
#+end_src

#+begin_src matlab :eval no
% Save the simulation results
save('./mat/test_id31_exp_tomo_cl_1rpm_sim.mat', 'exp_tomo_cl_m0_1rpm', 'exp_tomo_cl_m1_1rpm', 'exp_tomo_cl_m2_1rpm', 'exp_tomo_cl_m3_1rpm');
#+end_src

#+begin_src matlab
figure;
hold on;
plot(exp_tomo_cl_m3_1rpm.y.x.Data, exp_tomo_cl_m3_1rpm.y.y.Data)
plot(exp_tomo_cl_m2_1rpm.y.x.Data, exp_tomo_cl_m2_1rpm.y.y.Data)
plot(exp_tomo_cl_m1_1rpm.y.x.Data, exp_tomo_cl_m1_1rpm.y.y.Data)
plot(exp_tomo_cl_m0_1rpm.y.x.Data, exp_tomo_cl_m0_1rpm.y.y.Data)
#+end_src

#+begin_src matlab
% Estimate the RMS value of the errors
i_stab = 1000; % Remove the transient that is irrelevant here
rms_Dy_m0_1rpm = rms(detrend(exp_tomo_cl_m0_1rpm.y.y.Data(i_stab:end), 0));
rms_Dz_m0_1rpm = rms(detrend(exp_tomo_cl_m0_1rpm.y.z.Data(i_stab:end), 0));
rms_Ry_m0_1rpm = rms(detrend(squeeze(atan2(exp_tomo_cl_m0_1rpm.y.R.Data(1,3,:), sqrt(exp_tomo_cl_m0_1rpm.y.R.Data(1,1,:).^2+exp_tomo_cl_m0_1rpm.y.R.Data(1,2,:).^2))), 0));
rms_Dy_m1_1rpm = rms(detrend(exp_tomo_cl_m1_1rpm.y.y.Data(i_stab:end), 0));
rms_Dz_m1_1rpm = rms(detrend(exp_tomo_cl_m1_1rpm.y.z.Data(i_stab:end), 0));
rms_Ry_m1_1rpm = rms(detrend(squeeze(atan2(exp_tomo_cl_m1_1rpm.y.R.Data(1,3,:), sqrt(exp_tomo_cl_m1_1rpm.y.R.Data(1,1,:).^2+exp_tomo_cl_m1_1rpm.y.R.Data(1,2,:).^2))), 0));
rms_Dy_m2_1rpm = rms(detrend(exp_tomo_cl_m2_1rpm.y.y.Data(i_stab:end), 0));
rms_Dz_m2_1rpm = rms(detrend(exp_tomo_cl_m2_1rpm.y.z.Data(i_stab:end), 0));
rms_Ry_m2_1rpm = rms(detrend(squeeze(atan2(exp_tomo_cl_m2_1rpm.y.R.Data(1,3,:), sqrt(exp_tomo_cl_m2_1rpm.y.R.Data(1,1,:).^2+exp_tomo_cl_m2_1rpm.y.R.Data(1,2,:).^2))), 0));
rms_Dy_m3_1rpm = rms(detrend(exp_tomo_cl_m3_1rpm.y.y.Data(i_stab:end), 0));
rms_Dz_m3_1rpm = rms(detrend(exp_tomo_cl_m3_1rpm.y.z.Data(i_stab:end), 0));
rms_Ry_m3_1rpm = rms(detrend(squeeze(atan2(exp_tomo_cl_m3_1rpm.y.R.Data(1,3,:), sqrt(exp_tomo_cl_m3_1rpm.y.R.Data(1,1,:).^2+exp_tomo_cl_m3_1rpm.y.R.Data(1,2,:).^2))), 0));
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable(1e9*[rms_Dy_m0_1rpm, rms_Dz_m0_1rpm, rms_Ry_m0_1rpm; rms_Dy_m1_1rpm, rms_Dz_m1_1rpm, rms_Ry_m1_1rpm; rms_Dy_m2_1rpm, rms_Dz_m2_1rpm, rms_Ry_m2_1rpm; rms_Dy_m3_1rpm, rms_Dz_m3_1rpm, rms_Ry_m3_1rpm], {'0 kg', '13 kg', '26 kg', '39 kg'}, {'$D_y$', '$D_z$', '$R_y$'}, ' %.0f ');
#+end_src

The tomography experiments that were simulated were then experimentally conducted.
For each payload, a spindle rotating was first performed in open-loop, and then the loop was closed during another full spindle rotation.
An example with the $26\,\text{kg}$ payload is shown in Figure ref:fig:test_id31_tomo_m2_1rpm_robust_hac_iff_fit.
The eccentricity between the "point of interest" and the spindle rotation axis is quite large as the added payload mass statically deforms the micro-station stages.
To estimate the open-loop errors, it is supposed that the "point of interest" can be perfectly aligned with the spindle rotation axis.
Therefore, the eccentricity is first estimated by performing a circular fit (dashed black circle in Figure ref:fig:test_id31_tomo_m2_1rpm_robust_hac_iff_fit), and then subtracted from the data in Figure ref:fig:test_id31_tomo_m2_1rpm_robust_hac_iff_fit_removed.
This underestimate the real condition open-loop errors as it is difficult to obtain a perfect alignment in practice.

#+begin_src matlab
%% Load Tomography scans with robust controller
data_tomo_m0_Wz6 = load("2023-08-11_11-37_tomography_1rpm_m0.mat");
data_tomo_m0_Wz6.time = Ts*[0:length(data_tomo_m0_Wz6.Rz)-1];

data_tomo_m1_Wz6 = load("2023-08-11_11-15_tomography_1rpm_m1.mat");
data_tomo_m1_Wz6.time = Ts*[0:length(data_tomo_m1_Wz6.Rz)-1];

data_tomo_m2_Wz6 = load("2023-08-11_10-59_tomography_1rpm_m2.mat");
data_tomo_m2_Wz6.time = Ts*[0:length(data_tomo_m2_Wz6.Rz)-1];

data_tomo_m3_Wz6 = load("2023-08-11_10-24_tomography_1rpm_m3.mat");
data_tomo_m3_Wz6.time = Ts*[0:length(data_tomo_m3_Wz6.Rz)-1];
#+end_src

#+begin_src matlab :exports none :results none
%% Compute best circle fit for the displayed tomography experiment
[~, i_cl] = find(data_tomo_m0_Wz6.hac_status == 1);

% Find best circle
[x0, y0, R] = circlefit(data_tomo_m2_Wz6.Dx_int(1:i_m2), data_tomo_m2_Wz6.Dy_int(1:i_m2));
fun = @(theta)rms((data_tomo_m2_Wz6.Dx_int(1:i_m2) - (x0 + R*cos(data_tomo_m2_Wz6.Rz(1:i_m2)+theta(1)))).^2 + ...
                  (data_tomo_m2_Wz6.Dy_int(1:i_m2) - (y0 + R*sin(data_tomo_m2_Wz6.Rz(1:i_m2)+theta(1)))).^2);
delta_theta = fminsearch(fun, 0);
#+end_src

#+begin_src matlab :exports none :results none
%% Tomography experiment at 1rpm with 26kg payload
figure;
hold on;
plot(1e6*data_tomo_m2_Wz6.Dx_int(1:i_cl), 1e6*data_tomo_m2_Wz6.Dy_int(1:i_cl), 'DisplayName', 'OL')
plot(1e6*data_tomo_m2_Wz6.Dx_int(i_cl:i_cl+1e4), 1e6*data_tomo_m2_Wz6.Dy_int(i_cl:i_cl+1e4), 'color', colors(3,:), 'HandleVisibility', 'off')
plot(1e6*data_tomo_m2_Wz6.Dx_int(i_cl+1e4:end), 1e6*data_tomo_m2_Wz6.Dy_int(i_cl+1e4:end), 'color', colors(2,:), 'DisplayName', 'CL')
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
plot(1e6*(x0 + R*cos(theta)), 1e6*(y0 + R*sin(theta)), 'k--', 'DisplayName', 'Best Fit')
hold off;
xlabel('$D_x$ [$\mu$m]'); ylabel('$D_y$ [$\mu$m]');
axis equal
xlim([-20, 100]); ylim([-20, 100]);
xticks([-20:20:100]);
yticks([-20:20:100]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/test_id31_tomo_m2_1rpm_robust_hac_iff_fit.pdf', 'width', 'half', 'height', 'normal');
#+end_src

#+begin_src matlab :exports none :results none
%% Measured radial errors of the Spindle
figure;
hold on;
plot(1e6*(data_tomo_m2_Wz6.Dx_int(1:100:i_m2) - (x0 + R*cos(data_tomo_m2_Wz6.Rz(1:100:i_m2)+delta_theta))), ...
     1e6*(data_tomo_m2_Wz6.Dy_int(1:100:i_m2) - (y0 + R*sin(data_tomo_m2_Wz6.Rz(1:100:i_m2)+delta_theta))), 'color', colors(1,:), 'DisplayName', 'OL')
plot(1e6*detrend(data_tomo_m2_Wz6.Dx_int(i_cl+1e4:100:end), 0), 1e6*detrend(data_tomo_m2_Wz6.Dy_int(i_cl+1e4:100:end), 0), 'color', colors(2,:), 'DisplayName', 'CL')
hold off;
xlabel('$D_x$ [$\mu$m]'); ylabel('$D_y$ [$\mu$m]');
axis equal
xlim([-0.6, 0.4]); ylim([-0.4, 0.6]);
xticks([-0.6:0.2:0.4]);
yticks([-0.4:0.2:0.6]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/test_id31_tomo_m2_1rpm_robust_hac_iff_fit_removed.pdf', 'width', 'half', 'height', 'normal');
#+end_src

#+name: fig:test_id31_tomo_m2_1rpm_robust_hac_iff
#+caption: Tomography experiment with rotation velocity of 1rpm, and payload mass of 26kg. Errors in the $(x,y)$ plane are shown in (\subref{fig:test_id31_tomo_m2_1rpm_robust_hac_iff_fit}). The estimated eccentricity is displayed by the black dashed circle. Errors with subtracted eccentricity are shown in (\subref{fig:test_id31_tomo_m2_1rpm_robust_hac_iff_fit_removed}).
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_tomo_m2_1rpm_robust_hac_iff_fit}Errors in $(x,y)$ plane}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :scale 0.9
[[file:figs/test_id31_tomo_m2_1rpm_robust_hac_iff_fit.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_tomo_m2_1rpm_robust_hac_iff_fit_removed}Removed eccentricity}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :scale 0.9
[[file:figs/test_id31_tomo_m2_1rpm_robust_hac_iff_fit_removed.png]]
#+end_subfigure
#+end_figure

The RMS values of the open-loop and closed-loop errors for all masses are summarized in Table ref:tab:test_id31_tomo_1rpm_robust_ol_cl_errors.
The obtained closed-loop errors are fulfilling the requirements, except for the $39\,\text{kg}$ payload in the lateral ($D_y$) direction.

#+begin_src matlab
%% Estimate RMS of the errors while in closed-loop and open-loop
% No mass
[~, i_m0] = find(data_tomo_m0_Wz6.hac_status == 1);
data_tomo_m0_Wz6.Dy_rms_cl = rms(detrend(data_tomo_m0_Wz6.Dy_int(i_m0+50000:end), 0));
data_tomo_m0_Wz6.Dz_rms_cl = rms(detrend(data_tomo_m0_Wz6.Dz_int(i_m0+50000:end), 0));
data_tomo_m0_Wz6.Ry_rms_cl = rms(detrend(data_tomo_m0_Wz6.Ry_int(i_m0+50000:end), 0));

% Remove eccentricity for OL errors
[x0, y0, R] = circlefit(data_tomo_m0_Wz6.Dx_int(1:i_m0), data_tomo_m0_Wz6.Dy_int(1:i_m0));
fun = @(theta)rms((data_tomo_m0_Wz6.Dx_int(1:i_m0) - (x0 + R*cos(data_tomo_m0_Wz6.Rz(1:i_m0)+theta(1)))).^2 + ...
                  (data_tomo_m0_Wz6.Dy_int(1:i_m0) - (y0 + R*sin(data_tomo_m0_Wz6.Rz(1:i_m0)+theta(1)))).^2);
delta_theta = fminsearch(fun, 0);
data_tomo_m0_Wz6.Dy_rms_ol = rms(data_tomo_m0_Wz6.Dy_int(1:i_m0) - (y0 + R*sin(data_tomo_m0_Wz6.Rz(1:i_m0)+delta_theta)));
data_tomo_m0_Wz6.Dz_rms_ol = rms(detrend(data_tomo_m0_Wz6.Dz_int(1:i_m0), 0));
[x0, y0, R] = circlefit(data_tomo_m0_Wz6.Rx_int(1:i_m0), data_tomo_m0_Wz6.Ry_int(1:i_m0));
fun = @(theta)rms((data_tomo_m0_Wz6.Rx_int(1:i_m0) - (x0 + R*cos(data_tomo_m0_Wz6.Rz(1:i_m0)+theta(1)))).^2 + ...
                  (data_tomo_m0_Wz6.Ry_int(1:i_m0) - (y0 + R*sin(data_tomo_m0_Wz6.Rz(1:i_m0)+theta(1)))).^2);
delta_theta = fminsearch(fun, 0);
data_tomo_m0_Wz6.Ry_rms_ol = rms(data_tomo_m0_Wz6.Ry_int(1:i_m0) - (y0 + R*sin(data_tomo_m0_Wz6.Rz(1:i_m0)+delta_theta)));

% 1 "layer mass"
[~, i_m1] = find(data_tomo_m1_Wz6.hac_status == 1);
data_tomo_m1_Wz6.Dy_rms_cl = rms(detrend(data_tomo_m1_Wz6.Dy_int(i_m1+50000:end), 0));
data_tomo_m1_Wz6.Dz_rms_cl = rms(detrend(data_tomo_m1_Wz6.Dz_int(i_m1+50000:end), 0));
data_tomo_m1_Wz6.Ry_rms_cl = rms(detrend(data_tomo_m1_Wz6.Ry_int(i_m1+50000:end), 0));

% Remove eccentricity for OL errors
[x0, y0, R] = circlefit(data_tomo_m1_Wz6.Dx_int(1:i_m1), data_tomo_m1_Wz6.Dy_int(1:i_m1));
fun = @(theta)rms((data_tomo_m1_Wz6.Dx_int(1:i_m1) - (x0 + R*cos(data_tomo_m1_Wz6.Rz(1:i_m1)+theta(1)))).^2 + ...
                  (data_tomo_m1_Wz6.Dy_int(1:i_m1) - (y0 + R*sin(data_tomo_m1_Wz6.Rz(1:i_m1)+theta(1)))).^2);
delta_theta = fminsearch(fun, 0);
data_tomo_m1_Wz6.Dy_rms_ol = rms(data_tomo_m1_Wz6.Dy_int(1:i_m1) - (y0 + R*sin(data_tomo_m1_Wz6.Rz(1:i_m1)+delta_theta)));
data_tomo_m1_Wz6.Dz_rms_ol = rms(detrend(data_tomo_m1_Wz6.Dz_int(1:i_m1), 0));
[x0, y0, R] = circlefit(data_tomo_m1_Wz6.Rx_int(1:i_m1), data_tomo_m1_Wz6.Ry_int(1:i_m1));
fun = @(theta)rms((data_tomo_m1_Wz6.Rx_int(1:i_m1) - (x0 + R*cos(data_tomo_m1_Wz6.Rz(1:i_m1)+theta(1)))).^2 + ...
                  (data_tomo_m1_Wz6.Ry_int(1:i_m1) - (y0 + R*sin(data_tomo_m1_Wz6.Rz(1:i_m1)+theta(1)))).^2);
delta_theta = fminsearch(fun, 0);
data_tomo_m1_Wz6.Ry_rms_ol = rms(data_tomo_m1_Wz6.Ry_int(1:i_m1) - (y0 + R*sin(data_tomo_m1_Wz6.Rz(1:i_m1)+delta_theta)));

% 2 "layer masses"
[~, i_m2] = find(data_tomo_m2_Wz6.hac_status == 1);
data_tomo_m2_Wz6.Dy_rms_cl = rms(detrend(data_tomo_m2_Wz6.Dy_int(i_m2+50000:end), 0));
data_tomo_m2_Wz6.Dz_rms_cl = rms(detrend(data_tomo_m2_Wz6.Dz_int(i_m2+50000:end), 0));
data_tomo_m2_Wz6.Ry_rms_cl = rms(detrend(data_tomo_m2_Wz6.Ry_int(i_m2+50000:end), 0));

% Remove eccentricity for OL errors
[x0, y0, R] = circlefit(data_tomo_m2_Wz6.Dx_int(1:i_m2), data_tomo_m2_Wz6.Dy_int(1:i_m2));
fun = @(theta)rms((data_tomo_m2_Wz6.Dx_int(1:i_m2) - (x0 + R*cos(data_tomo_m2_Wz6.Rz(1:i_m2)+theta(1)))).^2 + ...
                  (data_tomo_m2_Wz6.Dy_int(1:i_m2) - (y0 + R*sin(data_tomo_m2_Wz6.Rz(1:i_m2)+theta(1)))).^2);
delta_theta = fminsearch(fun, 0);
data_tomo_m2_Wz6.Dy_rms_ol = rms(data_tomo_m2_Wz6.Dy_int(1:i_m2) - (y0 + R*sin(data_tomo_m2_Wz6.Rz(1:i_m2)+delta_theta)));
data_tomo_m2_Wz6.Dz_rms_ol = rms(detrend(data_tomo_m2_Wz6.Dz_int(1:i_m2), 0));
[x0, y0, R] = circlefit(data_tomo_m2_Wz6.Rx_int(1:i_m2), data_tomo_m2_Wz6.Ry_int(1:i_m2));
fun = @(theta)rms((data_tomo_m2_Wz6.Rx_int(1:i_m2) - (x0 + R*cos(data_tomo_m2_Wz6.Rz(1:i_m2)+theta(1)))).^2 + ...
                  (data_tomo_m2_Wz6.Ry_int(1:i_m2) - (y0 + R*sin(data_tomo_m2_Wz6.Rz(1:i_m2)+theta(1)))).^2);
delta_theta = fminsearch(fun, 0);
data_tomo_m2_Wz6.Ry_rms_ol = rms(data_tomo_m2_Wz6.Ry_int(1:i_m2) - (y0 + R*sin(data_tomo_m2_Wz6.Rz(1:i_m2)+delta_theta)));

% 3 "layer masses"
[~, i_m3] = find(data_tomo_m3_Wz6.hac_status == 1);
data_tomo_m3_Wz6.Dy_rms_cl = rms(detrend(data_tomo_m3_Wz6.Dy_int(i_m3+50000:end), 0));
data_tomo_m3_Wz6.Dz_rms_cl = rms(detrend(data_tomo_m3_Wz6.Dz_int(i_m3+50000:end), 0));
data_tomo_m3_Wz6.Ry_rms_cl = rms(detrend(data_tomo_m3_Wz6.Ry_int(i_m3+50000:end), 0));

% Remove eccentricity for OL errors
[x0, y0, R] = circlefit(data_tomo_m3_Wz6.Dx_int(1:i_m3), data_tomo_m3_Wz6.Dy_int(1:i_m3));
fun = @(theta)rms((data_tomo_m3_Wz6.Dx_int(1:i_m3) - (x0 + R*cos(data_tomo_m3_Wz6.Rz(1:i_m3)+theta(1)))).^2 + ...
                  (data_tomo_m3_Wz6.Dy_int(1:i_m3) - (y0 + R*sin(data_tomo_m3_Wz6.Rz(1:i_m3)+theta(1)))).^2);
delta_theta = fminsearch(fun, 0);
data_tomo_m3_Wz6.Dy_rms_ol = rms(data_tomo_m3_Wz6.Dy_int(1:i_m3) - (y0 + R*sin(data_tomo_m3_Wz6.Rz(1:i_m3)+delta_theta)));
data_tomo_m3_Wz6.Dz_rms_ol = rms(detrend(data_tomo_m3_Wz6.Dz_int(1:i_m3), 0));
[x0, y0, R] = circlefit(data_tomo_m3_Wz6.Rx_int(1:i_m3), data_tomo_m3_Wz6.Ry_int(1:i_m3));
fun = @(theta)rms((data_tomo_m3_Wz6.Rx_int(1:i_m3) - (x0 + R*cos(data_tomo_m3_Wz6.Rz(1:i_m3)+theta(1)))).^2 + ...
                  (data_tomo_m3_Wz6.Ry_int(1:i_m3) - (y0 + R*sin(data_tomo_m3_Wz6.Rz(1:i_m3)+theta(1)))).^2);
delta_theta = fminsearch(fun, 0);
data_tomo_m3_Wz6.Ry_rms_ol = rms(data_tomo_m3_Wz6.Ry_int(1:i_m3) - (y0 + R*sin(data_tomo_m3_Wz6.Rz(1:i_m3)+delta_theta)));
#+end_src

#+name: tab:test_id31_tomo_1rpm_robust_ol_cl_errors
#+caption: RMS values of the measured errors during open-loop and closed-loop tomography scans (1rpm) for all considered payloads. Measured closed-Loop errors are indicated by "bold" font.
#+attr_latex: :environment tabularx :width 0.9\linewidth :align Xccc
#+attr_latex: :center t :booktabs t
|                  | $D_y$                                        | $D_z$                                      | $R_y$                                            |
|------------------+----------------------------------------------+--------------------------------------------+--------------------------------------------------|
| $0\,kg$          | $142 \Longrightarrow \bm{15}\,\text{nm RMS}$ | $32 \Longrightarrow \bm{5}\,\text{nm RMS}$ | $460 \Longrightarrow \bm{55}\,\text{nrad RMS}$   |
| $13\,kg$         | $149 \Longrightarrow \bm{25}\,\text{nm RMS}$ | $26 \Longrightarrow \bm{6}\,\text{nm RMS}$ | $470 \Longrightarrow \bm{55}\,\text{nrad RMS}$   |
| $26\,kg$         | $202 \Longrightarrow \bm{25}\,\text{nm RMS}$ | $36 \Longrightarrow \bm{7}\,\text{nm RMS}$ | $1700 \Longrightarrow \bm{103}\,\text{nrad RMS}$ |
| $39\,kg$         | $297 \Longrightarrow \bm{53}\,\text{nm RMS}$ | $38 \Longrightarrow \bm{9}\,\text{nm RMS}$ | $1700 \Longrightarrow \bm{169}\,\text{nrad RMS}$ |
|------------------+----------------------------------------------+--------------------------------------------+--------------------------------------------------|
| *Specifications* | $30\,\text{nmRMS}$                           | $15\,\text{nmRMS}$                         | $250\,\text{nradRMS}$                            |

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([1e9*data_tomo_m0_Wz6.Dy_rms_ol, 1e9*data_tomo_m0_Wz6.Dz_rms_ol, 1e9*data_tomo_m0_Wz6.Ry_rms_ol; ...
               1e9*data_tomo_m1_Wz6.Dy_rms_ol, 1e9*data_tomo_m1_Wz6.Dz_rms_ol, 1e9*data_tomo_m1_Wz6.Ry_rms_ol; ...
               1e9*data_tomo_m2_Wz6.Dy_rms_ol, 1e9*data_tomo_m2_Wz6.Dz_rms_ol, 1e9*data_tomo_m2_Wz6.Ry_rms_ol; ...
               1e9*data_tomo_m3_Wz6.Dy_rms_ol, 1e9*data_tomo_m3_Wz6.Dz_rms_ol, 1e9*data_tomo_m3_Wz6.Ry_rms_ol], ...
              {'$m_0$', '$m_1$', '$m_2$', '$m_3$'}, {'$D_y$ [$\mu m$]', '$D_z$ [$nm$]', '$R_y$ [$\mu\text{rad}$]'}, ' %.0f ');
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([1e9*data_tomo_m0_Wz6.Dx_rms_cl, 1e9*data_tomo_m0_Wz6.Dy_rms_cl, 1e9*data_tomo_m0_Wz6.Dz_rms_cl, 1e9*data_tomo_m0_Wz6.Rx_rms_cl, 1e9*data_tomo_m0_Wz6.Ry_rms_cl; ...
               1e9*data_tomo_m1_Wz6.Dx_rms_cl, 1e9*data_tomo_m1_Wz6.Dy_rms_cl, 1e9*data_tomo_m1_Wz6.Dz_rms_cl, 1e9*data_tomo_m1_Wz6.Rx_rms_cl, 1e9*data_tomo_m1_Wz6.Ry_rms_cl; ...
               1e9*data_tomo_m2_Wz6.Dx_rms_cl, 1e9*data_tomo_m2_Wz6.Dy_rms_cl, 1e9*data_tomo_m2_Wz6.Dz_rms_cl, 1e9*data_tomo_m2_Wz6.Rx_rms_cl, 1e9*data_tomo_m2_Wz6.Ry_rms_cl; ...
               1e9*data_tomo_m3_Wz6.Dx_rms_cl, 1e9*data_tomo_m3_Wz6.Dy_rms_cl, 1e9*data_tomo_m3_Wz6.Dz_rms_cl, 1e9*data_tomo_m3_Wz6.Rx_rms_cl, 1e9*data_tomo_m3_Wz6.Ry_rms_cl], ...
              {'$m_0$', '$m_1$', '$m_2$', '$m_3$'}, {'$D_x$ [nm]', '$D_y$ [nm]', '$D_z$ [nm]', '$R_x$ [nrad]', '$R_y$ [nrad]'}, ' %.0f ');
#+end_src

** Conclusion
:PROPERTIES:
:UNNUMBERED: t
:END:

* Dynamic Error Budgeting                                           :noexport:
<<sec:test_id31_error_budget>>
** Introduction                                                      :ignore:

In this section, the noise budget is performed.
The vibrations of the sample is measured in different conditions using the external metrology.

** 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 :noweb yes
<<m-init-path>>
#+end_src

#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle>>
#+end_src

#+begin_src matlab :noweb yes
<<m-init-other>>
#+end_src

** Open-Loop Noise Budget

- Effect of rotation.
- Comparison with measurement noise: should be higher

#+begin_src matlab
%% Effect of rotation
data_ol_Wz0   = load('2023-08-11_16-51_m0_lac_off.mat');
data_ol_Wz36  = load('2023-08-11_17-18_m0_lac_off_1rpm.mat');
data_ol_Wz180 = load('2023-08-11_17-39_m0_lac_off_30rpm.mat');

% Coordinate transform
J_int_to_X = [ 0                 0                -0.787401574803149 -0.212598425196851  0;
               0.78740157480315  0.21259842519685  0                  0                  0;
               0                 0                 0                  0                 -1;
             -13.1233595800525  13.1233595800525   0                  0                  0;
               0                 0               -13.1233595800525   13.1233595800525    0];

a = J_int_to_X*[data_ol_Wz0.d1; data_ol_Wz0.d2; data_ol_Wz0.d3; data_ol_Wz0.d4; data_ol_Wz0.d5];
data_ol_Wz0.Dx_int = a(1,:);
data_ol_Wz0.Dy_int = a(2,:);
data_ol_Wz0.Dz_int = a(3,:);
data_ol_Wz0.Rx_int = a(4,:);
data_ol_Wz0.Ry_int = a(5,:);

a = J_int_to_X*[data_ol_Wz36.d1; data_ol_Wz36.d2; data_ol_Wz36.d3; data_ol_Wz36.d4; data_ol_Wz36.d5];
data_ol_Wz36.Dx_int = a(1,:);
data_ol_Wz36.Dy_int = a(2,:);
data_ol_Wz36.Dz_int = a(3,:);
data_ol_Wz36.Rx_int = a(4,:);
data_ol_Wz36.Ry_int = a(5,:);

a = J_int_to_X*[data_ol_Wz180.d1; data_ol_Wz180.d2; data_ol_Wz180.d3; data_ol_Wz180.d4; data_ol_Wz180.d5];
data_ol_Wz180.Dx_int = a(1,:);
data_ol_Wz180.Dy_int = a(2,:);
data_ol_Wz180.Dz_int = a(3,:);
data_ol_Wz180.Rx_int = a(4,:);
data_ol_Wz180.Ry_int = a(5,:);

% Hannning Windows
Nfft = floor(20.0/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

[data_ol_Wz0.pxx_Dx, data_ol_Wz0.f] = pwelch(detrend(data_ol_Wz0.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_ol_Wz0.pxx_Dy, ~         ] = pwelch(detrend(data_ol_Wz0.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_ol_Wz0.pxx_Dz, ~         ] = pwelch(detrend(data_ol_Wz0.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_ol_Wz0.pxx_Rx, ~         ] = pwelch(detrend(data_ol_Wz0.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_ol_Wz0.pxx_Ry, ~         ] = pwelch(detrend(data_ol_Wz0.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);

[data_ol_Wz36.pxx_Dx, data_ol_Wz36.f] = pwelch(detrend(data_ol_Wz36.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_ol_Wz36.pxx_Dy, ~          ] = pwelch(detrend(data_ol_Wz36.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_ol_Wz36.pxx_Dz, ~          ] = pwelch(detrend(data_ol_Wz36.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_ol_Wz36.pxx_Rx, ~          ] = pwelch(detrend(data_ol_Wz36.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_ol_Wz36.pxx_Ry, ~          ] = pwelch(detrend(data_ol_Wz36.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);

[data_ol_Wz180.pxx_Dx, data_ol_Wz180.f] = pwelch(detrend(data_ol_Wz180.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_ol_Wz180.pxx_Dy, ~          ] = pwelch(detrend(data_ol_Wz180.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_ol_Wz180.pxx_Dz, ~          ] = pwelch(detrend(data_ol_Wz180.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_ol_Wz180.pxx_Rx, ~          ] = pwelch(detrend(data_ol_Wz180.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_ol_Wz180.pxx_Ry, ~          ] = pwelch(detrend(data_ol_Wz180.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
#+end_src

#+begin_src matlab :exports none :results none
figure;
tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(data_ol_Wz0.f, sqrt(flip(-cumtrapz(flip(data_ol_Wz0.f), flip(data_ol_Wz0.pxx_Dy)))),    ...
     'DisplayName', sprintf('0rpm: $%.0f nm$', 1e9*rms(detrend(data_ol_Wz0.Dy_int, 0))));
plot(data_ol_Wz36.f, sqrt(flip(-cumtrapz(flip(data_ol_Wz36.f), flip(data_ol_Wz36.pxx_Dy)))), ...
     'DisplayName', sprintf('6rpm: $%.0f nm$', 1e9*rms(detrend(data_ol_Wz36.Dy_int, 0))));
plot(data_ol_Wz180.f, sqrt(flip(-cumtrapz(flip(data_ol_Wz180.f), flip(data_ol_Wz180.pxx_Dy)))), ...
     'DisplayName', sprintf('30rpm: $%.1f \\mu m$', 1e6*rms(detrend(data_ol_Wz180.Dy_int, 0))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('CAS [m rms, rad RMS]');
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
xticks([1e0, 1e1, 1e2]);
title('$D_y$')

ax2 = nexttile();
hold on;
plot(data_ol_Wz0.f, sqrt(flip(-cumtrapz(flip(data_ol_Wz0.f), flip(data_ol_Wz0.pxx_Dz)))),    ...
     'DisplayName', sprintf('0rpm: $%.0f nm$', 1e9*rms(detrend(data_ol_Wz0.Dz_int, 0))));
plot(data_ol_Wz36.f, sqrt(flip(-cumtrapz(flip(data_ol_Wz36.f), flip(data_ol_Wz36.pxx_Dz)))), ...
     'DisplayName', sprintf('6rpm: $%.0f nm$', 1e9*rms(detrend(data_ol_Wz36.Dz_int, 0))));
plot(data_ol_Wz180.f, sqrt(flip(-cumtrapz(flip(data_ol_Wz180.f), flip(data_ol_Wz180.pxx_Dz)))), ...
     'DisplayName', sprintf('30rpm: $%.0f nm$', 1e9*rms(detrend(data_ol_Wz180.Dz_int, 0))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
xticks([1e0, 1e1, 1e2]);
title('$D_z$')

ax3 = nexttile();
hold on;
plot(data_ol_Wz0.f, sqrt(flip(-cumtrapz(flip(data_ol_Wz0.f), flip(data_ol_Wz0.pxx_Ry)))),    ...
     'DisplayName', sprintf('0rpm: $%.1f \\mu$rad', 1e6*rms(detrend(data_ol_Wz0.Ry_int, 0))));
plot(data_ol_Wz36.f, sqrt(flip(-cumtrapz(flip(data_ol_Wz36.f), flip(data_ol_Wz36.pxx_Ry)))), ...
     'DisplayName', sprintf('6rpm: $%.1f \\mu$rad', 1e6*rms(detrend(data_ol_Wz36.Ry_int, 0))));
plot(data_ol_Wz180.f, sqrt(flip(-cumtrapz(flip(data_ol_Wz180.f), flip(data_ol_Wz180.pxx_Ry)))), ...
     'DisplayName', sprintf('30rpm: $%.0f \\mu$rad', 1e6*rms(detrend(data_ol_Wz180.Ry_int, 0))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
xticks([1e0, 1e1, 1e2]);
title('$R_y$')

linkaxes([ax1,ax2,ax3],'xy');
xlim([0.1, 5e2]); ylim([1e-10, 3e-5]);
#+end_src

** Effect of LAC

#+begin_src matlab
%% Effect of LAC

% Load measured noise
data_lac_Wz180 = load('2023-08-11_17-36_m0_lac_on_30rpm.mat');

a = J_int_to_X*[data_lac_Wz180.d1; data_lac_Wz180.d2; data_lac_Wz180.d3; data_lac_Wz180.d4; data_lac_Wz180.d5];
data_lac_Wz180.Dx_int = a(1,:);
data_lac_Wz180.Dy_int = a(2,:);
data_lac_Wz180.Dz_int = a(3,:);
data_lac_Wz180.Rx_int = a(4,:);
data_lac_Wz180.Ry_int = a(5,:);

[data_lac_Wz180.pxx_Dx, data_lac_Wz180.f] = pwelch(detrend(data_lac_Wz180.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_lac_Wz180.pxx_Dy, ~         ] = pwelch(detrend(data_lac_Wz180.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_lac_Wz180.pxx_Dz, ~         ] = pwelch(detrend(data_lac_Wz180.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_lac_Wz180.pxx_Rx, ~         ] = pwelch(detrend(data_lac_Wz180.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_lac_Wz180.pxx_Ry, ~         ] = pwelch(detrend(data_lac_Wz180.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
#+end_src

Effect of LAC:
- reduce amplitude around 80Hz
- Inject some noise between 200 and 700Hz?

#+begin_src matlab :exports none :results none
%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion
figure;
tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(data_ol_Wz180.f, sqrt(flip(-cumtrapz(flip(data_ol_Wz180.f), flip(data_ol_Wz180.pxx_Dy)))), 'DisplayName', 'OL');
plot(data_lac_Wz180.f, sqrt(flip(-cumtrapz(flip(data_lac_Wz180.f), flip(data_lac_Wz180.pxx_Dy)))), 'DisplayName', 'LAC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('CAS [$m$]');
title('$D_y$');
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile();
hold on;
plot(data_ol_Wz180.f, sqrt(flip(-cumtrapz(flip(data_ol_Wz180.f), flip(data_ol_Wz180.pxx_Dz)))), 'DisplayName', 'OL');
plot(data_lac_Wz180.f, sqrt(flip(-cumtrapz(flip(data_lac_Wz180.f), flip(data_lac_Wz180.pxx_Dz)))), 'DisplayName', 'LAC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
title('$D_z$');
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);

ax3 = nexttile();
hold on;
plot(data_ol_Wz180.f, sqrt(flip(-cumtrapz(flip(data_ol_Wz180.f), flip(data_ol_Wz180.pxx_Ry)))), 'DisplayName', 'OL');
plot(data_lac_Wz180.f, sqrt(flip(-cumtrapz(flip(data_lac_Wz180.f), flip(data_lac_Wz180.pxx_Ry)))), 'DisplayName', 'LAC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
title('$R_y$');
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);

linkaxes([ax1,ax2,ax3],'xy');
xlim([0.1, 5e2]); ylim([1e-10, 3e-5]);
#+end_src

** Effect of HAC

#+begin_src matlab
%% Effect of HAC
% Load measured noise
data_hac_Wz180 = load('2023-08-11_16-49_m0_hac_on.mat');

a = J_int_to_X*[data_hac_Wz180.d1; data_hac_Wz180.d2; data_hac_Wz180.d3; data_hac_Wz180.d4; data_hac_Wz180.d5];
data_hac_Wz180.Dx_int = a(1,:);
data_hac_Wz180.Dy_int = a(2,:);
data_hac_Wz180.Dz_int = a(3,:);
data_hac_Wz180.Rx_int = a(4,:);
data_hac_Wz180.Ry_int = a(5,:);

[data_hac_Wz180.pxx_Dx, data_hac_Wz180.f] = pwelch(detrend(data_hac_Wz180.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_hac_Wz180.pxx_Dy, ~         ] = pwelch(detrend(data_hac_Wz180.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_hac_Wz180.pxx_Dz, ~         ] = pwelch(detrend(data_hac_Wz180.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_hac_Wz180.pxx_Rx, ~         ] = pwelch(detrend(data_hac_Wz180.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
[data_hac_Wz180.pxx_Ry, ~         ] = pwelch(detrend(data_hac_Wz180.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
#+end_src

Bandwidth is approximately 10Hz.

#+begin_src matlab :exports none :results none
%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion
figure;
tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(data_ol_Wz180.f,  sqrt(data_ol_Wz180.pxx_Dy),  'DisplayName', 'OL');
plot(data_lac_Wz180.f, sqrt(data_lac_Wz180.pxx_Dy), 'DisplayName', 'LAC');
plot(data_hac_Wz180.f, sqrt(data_hac_Wz180.pxx_Dy), 'DisplayName', 'HAC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]');
title('$D_y$');
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile();
hold on;
plot(data_ol_Wz180.f,  sqrt(data_ol_Wz180.pxx_Dz),  'DisplayName', 'OL');
plot(data_lac_Wz180.f, sqrt(data_lac_Wz180.pxx_Dz), 'DisplayName', 'LAC');
plot(data_hac_Wz180.f, sqrt(data_hac_Wz180.pxx_Dz), 'DisplayName', 'HAC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
title('$D_z$');
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);

ax3 = nexttile();
hold on;
plot(data_ol_Wz180.f,  sqrt(data_ol_Wz180.pxx_Ry),  'DisplayName', 'OL');
plot(data_lac_Wz180.f, sqrt(data_lac_Wz180.pxx_Ry), 'DisplayName', 'LAC');
plot(data_hac_Wz180.f, sqrt(data_hac_Wz180.pxx_Ry), 'DisplayName', 'HAC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
title('$R_y$');
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);

linkaxes([ax1,ax2,ax3],'xy');
xlim([1, 1e3]); ylim([1e-12, 1e-7]);
#+end_src

#+begin_src matlab :exports none :results none
%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion
figure;
tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(data_ol_Wz180.f, sqrt(flip(-cumtrapz(flip(data_ol_Wz180.f), flip(data_ol_Wz180.pxx_Dy)))), 'DisplayName', sprintf('OL %.1f nm RMS' ));
plot(data_lac_Wz180.f, sqrt(flip(-cumtrapz(flip(data_lac_Wz180.f), flip(data_lac_Wz180.pxx_Dy)))), 'DisplayName', 'LAC');
plot(data_hac_Wz180.f, sqrt(flip(-cumtrapz(flip(data_hac_Wz180.f), flip(data_hac_Wz180.pxx_Dy)))), 'DisplayName', 'HAC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('CAS [$m$]');
title('$D_y$');
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile();
hold on;
plot(data_ol_Wz180.f, sqrt(flip(-cumtrapz(flip(data_ol_Wz180.f), flip(data_ol_Wz180.pxx_Dz)))), 'DisplayName', 'OL');
plot(data_lac_Wz180.f, sqrt(flip(-cumtrapz(flip(data_lac_Wz180.f), flip(data_lac_Wz180.pxx_Dz)))), 'DisplayName', 'LAC');
plot(data_hac_Wz180.f, sqrt(flip(-cumtrapz(flip(data_hac_Wz180.f), flip(data_hac_Wz180.pxx_Dz)))), 'DisplayName', 'HAC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
title('$D_z$');
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);

ax3 = nexttile();
hold on;
plot(data_ol_Wz180.f, sqrt(flip(-cumtrapz(flip(data_ol_Wz180.f), flip(data_ol_Wz180.pxx_Ry)))), 'DisplayName', 'OL');
plot(data_lac_Wz180.f, sqrt(flip(-cumtrapz(flip(data_lac_Wz180.f), flip(data_lac_Wz180.pxx_Ry)))), 'DisplayName', 'LAC');
plot(data_hac_Wz180.f, sqrt(flip(-cumtrapz(flip(data_hac_Wz180.f), flip(data_hac_Wz180.pxx_Ry)))), 'DisplayName', 'HAC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
title('$R_y$');
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);

linkaxes([ax1,ax2,ax3],'xy');
xlim([0.1, 5e2]); ylim([1e-10, 3e-5]);
#+end_src

* TODO Scans for scientific experiments                             :noexport:
:PROPERTIES:
:header-args:matlab+: :tangle matlab/test_id31_x_scans.m
:END:
<<sec:test_id31_scans>>
** Introduction                                                      :ignore:

- Section ref:sec:id31_scans_tomography
- Section ref:sec:id31_scans_dz
- Section ref:sec:id31_scans_reflectivity
- Section ref:sec:id31_scans_dy
- Section ref:sec:id31_scans_diffraction_tomo

** 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 :noweb yes
<<m-init-path>>
#+end_src

#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle>>
#+end_src

#+begin_src matlab :noweb yes
<<m-init-other>>
#+end_src

** $R_z$ scans: Tomography
<<ssec:test_id31_scans_tomography>>
*** Introduction                                                    :ignore:

- m0: 30rpm, 6rpm, 1rpm
- m1: 6rpm, 1rpm
- m2: 6rpm, 1rpm
- m3: 1rpm

*Issue with this control architecture (or controller?)*:
- The position is not converging to zero

*Compare*:
- 1rpm, 6rpm, 30rpm
- at 1rpm: m0, m1, m2, m3 (same robust controller!)

*** Robust Control - 1rpm
1RPM scans are performed for all the masses with the same robust controller.

#+begin_src matlab
%% Load Tomography scans with robust controller
data_tomo_1rpm_m0 = load("2023-08-11_11-37_tomography_1rpm_m0.mat");
data_tomo_1rpm_m0.time = Ts*[0:length(data_tomo_1rpm_m0.Rz)-1];

data_tomo_1rpm_m1 = load("2023-08-11_11-15_tomography_1rpm_m1.mat");
data_tomo_1rpm_m1.time = Ts*[0:length(data_tomo_1rpm_m1.Rz)-1];

data_tomo_1rpm_m2 = load("2023-08-11_10-59_tomography_1rpm_m2.mat");
data_tomo_1rpm_m2.time = Ts*[0:length(data_tomo_1rpm_m2.Rz)-1];

data_tomo_1rpm_m3 = load("2023-08-11_10-24_tomography_1rpm_m3.mat");
data_tomo_1rpm_m3.time = Ts*[0:length(data_tomo_1rpm_m3.Rz)-1];
#+end_src

The problem for these scans is that the position initialization was not make properly, so the open-loop errors are quite large (see Figure ref:fig:id31_tomo_1rpm_robust_m0).

#+begin_src matlab :exports none :results none
%% $D_x$, $D_y$ and $D_z$ motion during a slow (1RPM) tomography experiment. Open Loop data is shown in blue and closed-loop data in red
figure;
tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile;
hold on;
plot(1e6*data_tomo_1rpm_m0.Dx_int(data_tomo_1rpm_m0.hac_status == 0), 1e6*data_tomo_1rpm_m0.Dy_int(data_tomo_1rpm_m0.hac_status == 0), ...
    'DisplayName', 'OL')
plot(1e6*data_tomo_1rpm_m0.Dx_int(data_tomo_1rpm_m0.hac_status == 1), 1e6*data_tomo_1rpm_m0.Dy_int(data_tomo_1rpm_m0.hac_status == 1), ...
    'DisplayName', 'CL')
hold off;
axis equal
xlabel("X motion [$\mu m$]");
ylabel("Y motion [$\mu m$]");
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
% xlim([-3, 3])
% ylim([-3, 3])

ax2 = nexttile;
hold on;
plot(1e6*data_tomo_1rpm_m0.Dy_int(data_tomo_1rpm_m0.hac_status == 0), 1e6*data_tomo_1rpm_m0.Dz_int(data_tomo_1rpm_m0.hac_status == 0), ...
    'DisplayName', 'OL')
plot(1e6*data_tomo_1rpm_m0.Dy_int(data_tomo_1rpm_m0.hac_status == 1), 1e6*data_tomo_1rpm_m0.Dz_int(data_tomo_1rpm_m0.hac_status == 1), ...
    'DisplayName', 'CL')
hold off;
% axis equal
% xlim([-3, 3])
ylim([-0.2, 1.1])
xlabel("Y motion [$\mu m$]");
ylabel("Z motion [$\mu m$]");
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/id31_tomo_1rpm_robust_m0.pdf', 'width', 'full', 'height', 'normal');
#+end_src

#+name: fig:id31_tomo_1rpm_robust_m0
#+caption: $D_x$, $D_y$ and $D_z$ motion during a slow (1RPM) tomography experiment. Open Loop data is shown in blue and closed-loop data in red
#+RESULTS:
[[file:figs/id31_tomo_1rpm_robust_m0.png]]

The obtained open-loop and closed-loop errors are shown in tables ref:tab:id31_tomo_1rpm_robust_ol_errors and ref:tab:id31_tomo_1rpm_robust_cl_errors respectively.

#+begin_src matlab
%% Compute RMS values while in closed-loop and open-loop
[~, i_m0] = find(data_tomo_1rpm_m0.hac_status == 1);
data_tomo_1rpm_m0.Dx_rms_cl = rms(detrend(data_tomo_1rpm_m0.Dx_int(i_m0+50000:end), 0));
data_tomo_1rpm_m0.Dy_rms_cl = rms(detrend(data_tomo_1rpm_m0.Dy_int(i_m0+50000:end), 0));
data_tomo_1rpm_m0.Dz_rms_cl = rms(detrend(data_tomo_1rpm_m0.Dz_int(i_m0+50000:end), 0));
data_tomo_1rpm_m0.Rx_rms_cl = rms(detrend(data_tomo_1rpm_m0.Rx_int(i_m0+50000:end), 0));
data_tomo_1rpm_m0.Ry_rms_cl = rms(detrend(data_tomo_1rpm_m0.Ry_int(i_m0+50000:end), 0));

data_tomo_1rpm_m0.Dx_rms_ol = rms(detrend(data_tomo_1rpm_m0.Dx_int(1:i_m0), 0));
data_tomo_1rpm_m0.Dy_rms_ol = rms(detrend(data_tomo_1rpm_m0.Dy_int(1:i_m0), 0));
data_tomo_1rpm_m0.Dz_rms_ol = rms(detrend(data_tomo_1rpm_m0.Dz_int(1:i_m0), 0));
data_tomo_1rpm_m0.Rx_rms_ol = rms(detrend(data_tomo_1rpm_m0.Rx_int(1:i_m0), 0));
data_tomo_1rpm_m0.Ry_rms_ol = rms(detrend(data_tomo_1rpm_m0.Ry_int(1:i_m0), 0));

%% Compute RMS values while in closed-loop and open-loop
[~, i_m1] = find(data_tomo_1rpm_m1.hac_status == 1);
data_tomo_1rpm_m1.Dx_rms_cl = rms(detrend(data_tomo_1rpm_m1.Dx_int(i_m1+50000:end), 0));
data_tomo_1rpm_m1.Dy_rms_cl = rms(detrend(data_tomo_1rpm_m1.Dy_int(i_m1+50000:end), 0));
data_tomo_1rpm_m1.Dz_rms_cl = rms(detrend(data_tomo_1rpm_m1.Dz_int(i_m1+50000:end), 0));
data_tomo_1rpm_m1.Rx_rms_cl = rms(detrend(data_tomo_1rpm_m1.Rx_int(i_m1+50000:end), 0));
data_tomo_1rpm_m1.Ry_rms_cl = rms(detrend(data_tomo_1rpm_m1.Ry_int(i_m1+50000:end), 0));

data_tomo_1rpm_m1.Dx_rms_ol = rms(detrend(data_tomo_1rpm_m1.Dx_int(1:i_m1), 0));
data_tomo_1rpm_m1.Dy_rms_ol = rms(detrend(data_tomo_1rpm_m1.Dy_int(1:i_m1), 0));
data_tomo_1rpm_m1.Dz_rms_ol = rms(detrend(data_tomo_1rpm_m1.Dz_int(1:i_m1), 0));
data_tomo_1rpm_m1.Rx_rms_ol = rms(detrend(data_tomo_1rpm_m1.Rx_int(1:i_m1), 0));
data_tomo_1rpm_m1.Ry_rms_ol = rms(detrend(data_tomo_1rpm_m1.Ry_int(1:i_m1), 0));

%% Compute RMS values while in closed-loop and open-loop
[~, i_m2] = find(data_tomo_1rpm_m2.hac_status == 1);
data_tomo_1rpm_m2.Dx_rms_cl = rms(detrend(data_tomo_1rpm_m2.Dx_int(i_m2+50000:end), 0));
data_tomo_1rpm_m2.Dy_rms_cl = rms(detrend(data_tomo_1rpm_m2.Dy_int(i_m2+50000:end), 0));
data_tomo_1rpm_m2.Dz_rms_cl = rms(detrend(data_tomo_1rpm_m2.Dz_int(i_m2+50000:end), 0));
data_tomo_1rpm_m2.Rx_rms_cl = rms(detrend(data_tomo_1rpm_m2.Rx_int(i_m2+50000:end), 0));
data_tomo_1rpm_m2.Ry_rms_cl = rms(detrend(data_tomo_1rpm_m2.Ry_int(i_m2+50000:end), 0));

data_tomo_1rpm_m2.Dx_rms_ol = rms(detrend(data_tomo_1rpm_m2.Dx_int(1:i_m2), 0));
data_tomo_1rpm_m2.Dy_rms_ol = rms(detrend(data_tomo_1rpm_m2.Dy_int(1:i_m2), 0));
data_tomo_1rpm_m2.Dz_rms_ol = rms(detrend(data_tomo_1rpm_m2.Dz_int(1:i_m2), 0));
data_tomo_1rpm_m2.Rx_rms_ol = rms(detrend(data_tomo_1rpm_m2.Rx_int(1:i_m2), 0));
data_tomo_1rpm_m2.Ry_rms_ol = rms(detrend(data_tomo_1rpm_m2.Ry_int(1:i_m2), 0));

%% Compute RMS values while in closed-loop and open-loop
[~, i_m3] = find(data_tomo_1rpm_m3.hac_status == 1);
data_tomo_1rpm_m3.Dx_rms_cl = rms(detrend(data_tomo_1rpm_m3.Dx_int(i_m3+50000:end), 0));
data_tomo_1rpm_m3.Dy_rms_cl = rms(detrend(data_tomo_1rpm_m3.Dy_int(i_m3+50000:end), 0));
data_tomo_1rpm_m3.Dz_rms_cl = rms(detrend(data_tomo_1rpm_m3.Dz_int(i_m3+50000:end), 0));
data_tomo_1rpm_m3.Rx_rms_cl = rms(detrend(data_tomo_1rpm_m3.Rx_int(i_m3+50000:end), 0));
data_tomo_1rpm_m3.Ry_rms_cl = rms(detrend(data_tomo_1rpm_m3.Ry_int(i_m3+50000:end), 0));

data_tomo_1rpm_m3.Dx_rms_ol = rms(detrend(data_tomo_1rpm_m3.Dx_int(1:i_m3), 0));
data_tomo_1rpm_m3.Dy_rms_ol = rms(detrend(data_tomo_1rpm_m3.Dy_int(1:i_m3), 0));
data_tomo_1rpm_m3.Dz_rms_ol = rms(detrend(data_tomo_1rpm_m3.Dz_int(1:i_m3), 0));
data_tomo_1rpm_m3.Rx_rms_ol = rms(detrend(data_tomo_1rpm_m3.Rx_int(1:i_m3), 0));
data_tomo_1rpm_m3.Ry_rms_ol = rms(detrend(data_tomo_1rpm_m3.Ry_int(1:i_m3), 0));
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([1e6*data_tomo_1rpm_m0.Dx_rms_ol, 1e6*data_tomo_1rpm_m0.Dy_rms_ol, 1e9*data_tomo_1rpm_m0.Dz_rms_ol, 1e6*data_tomo_1rpm_m0.Rx_rms_ol, 1e6*data_tomo_1rpm_m0.Ry_rms_ol; ...
               1e6*data_tomo_1rpm_m1.Dx_rms_ol, 1e6*data_tomo_1rpm_m1.Dy_rms_ol, 1e9*data_tomo_1rpm_m1.Dz_rms_ol, 1e6*data_tomo_1rpm_m1.Rx_rms_ol, 1e6*data_tomo_1rpm_m1.Ry_rms_ol; ...
               1e6*data_tomo_1rpm_m2.Dx_rms_ol, 1e6*data_tomo_1rpm_m2.Dy_rms_ol, 1e9*data_tomo_1rpm_m2.Dz_rms_ol, 1e6*data_tomo_1rpm_m2.Rx_rms_ol, 1e6*data_tomo_1rpm_m2.Ry_rms_ol; ...
               1e6*data_tomo_1rpm_m3.Dx_rms_ol, 1e6*data_tomo_1rpm_m3.Dy_rms_ol, 1e9*data_tomo_1rpm_m3.Dz_rms_ol, 1e6*data_tomo_1rpm_m3.Rx_rms_ol, 1e6*data_tomo_1rpm_m3.Ry_rms_ol], ...
              {'$m_0$', '$m_1$', '$m_2$', '$m_3$'}, {'$D_x$ [$\mu m$]', '$D_y$ [$\mu m$]', '$D_z$ [$nm$]', '$R_x$ [$\mu\text{rad}$]', '$R_y$ [$\mu\text{rad}$]'}, ' %.0f ');
#+end_src

#+name: tab:id31_tomo_1rpm_robust_ol_errors
#+caption: Measured error during open-loop tomography scans (1rpm)
#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
#+attr_latex: :center t :booktabs t
#+RESULTS:
|       | $D_x$ [$\mu m$] | $D_y$ [$\mu m$] | $D_z$ [$nm$] | $R_x$ [$\mu\text{rad}$] | $R_y$ [$\mu\text{rad}$] |
|-------+-----------------+-----------------+--------------+-------------------------+-------------------------|
| $m_0$ |               6 |               6 |           32 |                      34 |                      34 |
| $m_1$ |               6 |               7 |           26 |                      51 |                      55 |
| $m_2$ |              36 |              38 |           36 |                     259 |                     253 |
| $m_3$ |              31 |              33 |           38 |                     214 |                     203 |

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([1e9*data_tomo_1rpm_m0.Dx_rms_cl, 1e9*data_tomo_1rpm_m0.Dy_rms_cl, 1e9*data_tomo_1rpm_m0.Dz_rms_cl, 1e9*data_tomo_1rpm_m0.Rx_rms_cl, 1e9*data_tomo_1rpm_m0.Ry_rms_cl; ...
               1e9*data_tomo_1rpm_m1.Dx_rms_cl, 1e9*data_tomo_1rpm_m1.Dy_rms_cl, 1e9*data_tomo_1rpm_m1.Dz_rms_cl, 1e9*data_tomo_1rpm_m1.Rx_rms_cl, 1e9*data_tomo_1rpm_m1.Ry_rms_cl; ...
               1e9*data_tomo_1rpm_m2.Dx_rms_cl, 1e9*data_tomo_1rpm_m2.Dy_rms_cl, 1e9*data_tomo_1rpm_m2.Dz_rms_cl, 1e9*data_tomo_1rpm_m2.Rx_rms_cl, 1e9*data_tomo_1rpm_m2.Ry_rms_cl; ...
               1e9*data_tomo_1rpm_m3.Dx_rms_cl, 1e9*data_tomo_1rpm_m3.Dy_rms_cl, 1e9*data_tomo_1rpm_m3.Dz_rms_cl, 1e9*data_tomo_1rpm_m3.Rx_rms_cl, 1e9*data_tomo_1rpm_m3.Ry_rms_cl], ...
              {'$m_0$', '$m_1$', '$m_2$', '$m_3$'}, {'$D_x$ [nm]', '$D_y$ [nm]', '$D_z$ [nm]', '$R_x$ [nrad]', '$R_y$ [nrad]'}, ' %.0f ');
#+end_src

#+name: tab:id31_tomo_1rpm_robust_cl_errors
#+caption: Measured error during closed-loop tomography scans (1rpm, robust controller)
#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
#+attr_latex: :center t :booktabs t
#+RESULTS:
|       | $D_x$ [nm] | $D_y$ [nm] | $D_z$ [nm] | $R_x$ [nrad] | $R_y$ [nrad] |
|-------+------------+------------+------------+--------------+--------------|
| $m_0$ |         13 |         15 |          5 |           57 |           55 |
| $m_1$ |         16 |         25 |          6 |          102 |           55 |
| $m_2$ |         25 |         25 |          7 |          120 |          103 |
| $m_3$ |         40 |         53 |          9 |          225 |          169 |

*** Robust Control - 6rpm
#+begin_src matlab
data_tomo_6rpm_m0 = load("2023-08-11_11-31_tomography_6rpm_m0.mat");
data_tomo_6rpm_m0.time = Ts*[0:length(data_tomo_6rpm_m0.Rz)-1];
#+end_src

#+begin_src matlab
data_tomo_6rpm_m1 = load("2023-08-11_11-23_tomography_6rpm_m1.mat");
data_tomo_6rpm_m1.time = Ts*[0:length(data_tomo_6rpm_m1.Rz)-1];
#+end_src

#+begin_src matlab :exports none :results none
%% $D_x$, $D_y$ and $D_z$ motion during a slow (6RPM) tomography experiment. Open Loop data is shown in blue and closed-loop data in red
figure;
tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile;
hold on;
plot(1e6*data_tomo_6rpm_m1.Dx_int(data_tomo_6rpm_m1.hac_status == 0), 1e6*data_tomo_6rpm_m1.Dy_int(data_tomo_6rpm_m1.hac_status == 0), ...
    'DisplayName', 'OL')
plot(1e6*data_tomo_6rpm_m1.Dx_int(data_tomo_6rpm_m1.hac_status == 1), 1e6*data_tomo_6rpm_m1.Dy_int(data_tomo_6rpm_m1.hac_status == 1), ...
    'DisplayName', 'CL')
hold off;
axis equal
xlabel("X motion [$\mu m$]");
ylabel("Y motion [$\mu m$]");
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
% xlim([-3, 3])
% ylim([-3, 3])

ax2 = nexttile;
hold on;
plot(1e6*data_tomo_6rpm_m1.Dy_int(data_tomo_6rpm_m1.hac_status == 0), 1e6*data_tomo_6rpm_m1.Dz_int(data_tomo_6rpm_m1.hac_status == 0), ...
    'DisplayName', 'OL')
plot(1e6*data_tomo_6rpm_m1.Dy_int(data_tomo_6rpm_m1.hac_status == 1), 1e6*data_tomo_6rpm_m1.Dz_int(data_tomo_6rpm_m1.hac_status == 1), ...
    'DisplayName', 'CL')
hold off;
% axis equal
% xlim([-3, 3])
ylim([-0.2, 1.1])
xlabel("Y motion [$\mu m$]");
ylabel("Z motion [$\mu m$]");
#+end_src

#+begin_src matlab
%% Compute RMS values while in closed-loop
[~, i_m0] = find(data_tomo_6rpm_m0.hac_status == 1);
data_tomo_6rpm_m0.Dx_rms_cl = rms(detrend(data_tomo_6rpm_m0.Dx_int(i_m0+50000:end), 0));
data_tomo_6rpm_m0.Dy_rms_cl = rms(detrend(data_tomo_6rpm_m0.Dy_int(i_m0+50000:end), 0));
data_tomo_6rpm_m0.Dz_rms_cl = rms(detrend(data_tomo_6rpm_m0.Dz_int(i_m0+50000:end), 0));
data_tomo_6rpm_m0.Rx_rms_cl = rms(detrend(data_tomo_6rpm_m0.Rx_int(i_m0+50000:end), 0));
data_tomo_6rpm_m0.Ry_rms_cl = rms(detrend(data_tomo_6rpm_m0.Ry_int(i_m0+50000:end), 0));

data_tomo_6rpm_m0.Dx_rms_ol = rms(detrend(data_tomo_6rpm_m0.Dx_int(1:i_m0), 0));
data_tomo_6rpm_m0.Dy_rms_ol = rms(detrend(data_tomo_6rpm_m0.Dy_int(1:i_m0), 0));
data_tomo_6rpm_m0.Dz_rms_ol = rms(detrend(data_tomo_6rpm_m0.Dz_int(1:i_m0), 0));
data_tomo_6rpm_m0.Rx_rms_ol = rms(detrend(data_tomo_6rpm_m0.Rx_int(1:i_m0), 0));
data_tomo_6rpm_m0.Ry_rms_ol = rms(detrend(data_tomo_6rpm_m0.Ry_int(1:i_m0), 0));

%% Compute RMS values while in closed-loop
[~, i_m1] = find(data_tomo_6rpm_m1.hac_status == 1);
data_tomo_6rpm_m1.Dx_rms_cl = rms(detrend(data_tomo_6rpm_m1.Dx_int(i_m1+50000:end), 0));
data_tomo_6rpm_m1.Dy_rms_cl = rms(detrend(data_tomo_6rpm_m1.Dy_int(i_m1+50000:end), 0));
data_tomo_6rpm_m1.Dz_rms_cl = rms(detrend(data_tomo_6rpm_m1.Dz_int(i_m1+50000:end), 0));
data_tomo_6rpm_m1.Rx_rms_cl = rms(detrend(data_tomo_6rpm_m1.Rx_int(i_m1+50000:end), 0));
data_tomo_6rpm_m1.Ry_rms_cl = rms(detrend(data_tomo_6rpm_m1.Ry_int(i_m1+50000:end), 0));

data_tomo_6rpm_m1.Dx_rms_ol = rms(detrend(data_tomo_6rpm_m1.Dx_int(1:i_m1), 0));
data_tomo_6rpm_m1.Dy_rms_ol = rms(detrend(data_tomo_6rpm_m1.Dy_int(1:i_m1), 0));
data_tomo_6rpm_m1.Dz_rms_ol = rms(detrend(data_tomo_6rpm_m1.Dz_int(1:i_m1), 0));
data_tomo_6rpm_m1.Rx_rms_ol = rms(detrend(data_tomo_6rpm_m1.Rx_int(1:i_m1), 0));
data_tomo_6rpm_m1.Ry_rms_ol = rms(detrend(data_tomo_6rpm_m1.Ry_int(1:i_m1), 0));
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([1e6*data_tomo_6rpm_m0.Dx_rms_ol, 1e6*data_tomo_6rpm_m0.Dy_rms_ol, 1e9*data_tomo_6rpm_m0.Dz_rms_ol, 1e6*data_tomo_6rpm_m0.Rx_rms_ol, 1e6*data_tomo_6rpm_m0.Ry_rms_ol; ...
               1e6*data_tomo_6rpm_m1.Dx_rms_ol, 1e6*data_tomo_6rpm_m1.Dy_rms_ol, 1e9*data_tomo_6rpm_m1.Dz_rms_ol, 1e6*data_tomo_6rpm_m1.Rx_rms_ol, 1e6*data_tomo_6rpm_m1.Ry_rms_ol], ...
              {'$m_0$', '$m_1$'}, {'$D_x$ [$\mu m$]', '$D_y$ [$\mu m$]', '$D_z$ [$nm$]', '$R_x$ [$\mu\text{rad}$]', '$R_y$ [$\mu\text{rad}$]'}, ' %.0f ');
#+end_src

#+name: tab:id31_tomo_6rpm_robust_ol_errors
#+caption: Measured error during open-loop tomography scans (6rpm)
#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
#+attr_latex: :center t :booktabs t
#+RESULTS:
|       | $D_x$ [$\mu m$] | $D_y$ [$\mu m$] | $D_z$ [$nm$] | $R_x$ [$\mu\text{rad}$] | $R_y$ [$\mu\text{rad}$] |
|-------+-----------------+-----------------+--------------+-------------------------+-------------------------|
| $m_0$ |               8 |               7 |           20 |                      41 |                      41 |
| $m_1$ |               4 |               4 |           21 |                      39 |                      39 |

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([1e9*data_tomo_6rpm_m0.Dx_rms_cl, 1e9*data_tomo_6rpm_m0.Dy_rms_cl, 1e9*data_tomo_6rpm_m0.Dz_rms_cl, 1e9*data_tomo_6rpm_m0.Rx_rms_cl, 1e9*data_tomo_6rpm_m0.Ry_rms_cl; ...
               1e9*data_tomo_6rpm_m1.Dx_rms_cl, 1e9*data_tomo_6rpm_m1.Dy_rms_cl, 1e9*data_tomo_6rpm_m1.Dz_rms_cl, 1e9*data_tomo_6rpm_m1.Rx_rms_cl, 1e9*data_tomo_6rpm_m1.Ry_rms_cl], ...
              {'$m_0$', '$m_1$'}, {'$D_x$ [nm]', '$D_y$ [nm]', '$D_z$ [nm]', '$R_x$ [nrad]', '$R_y$ [nrad]'}, ' %.0f ');
#+end_src

#+name: tab:id31_tomo_6rpm_robust_cl_errors
#+caption: Measured error during closed-loop tomography scans (6rpm, robust controller)
#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
#+attr_latex: :center t :booktabs t
#+RESULTS:
|       | $D_x$ [nm] | $D_y$ [nm] | $D_z$ [nm] | $R_x$ [nrad] | $R_y$ [nrad] |
|-------+------------+------------+------------+--------------+--------------|
| $m_0$ |         17 |         19 |          5 |           70 |           73 |
| $m_1$ |         20 |         26 |          7 |          110 |           77 |

*** Robust Control - 30rpm
#+begin_src matlab
%% Load Data
data_tomo_30rpm_m0 = load("2023-08-17_15-26_tomography_30rpm_m0_robust.mat");
data_tomo_30rpm_m0.time = Ts*[0:length(data_tomo_30rpm_m0.Rz)-1];
#+end_src

#+begin_src matlab
figure;
hold on;
plot3(1e6*data_tomo_30rpm_m0.Dx_int(data_tomo_30rpm_m0.hac_status == 0), ...
      1e6*data_tomo_30rpm_m0.Dy_int(data_tomo_30rpm_m0.hac_status == 0), ...
      1e6*data_tomo_30rpm_m0.Dz_int(data_tomo_30rpm_m0.hac_status == 0), ...
      'DisplayName', 'OL')
plot3(1e6*data_tomo_30rpm_m0.Dx_int(data_tomo_30rpm_m0.hac_status == 1), ...
      1e6*data_tomo_30rpm_m0.Dy_int(data_tomo_30rpm_m0.hac_status == 1), ...
      1e6*data_tomo_30rpm_m0.Dz_int(data_tomo_30rpm_m0.hac_status == 1), ...
      'DisplayName', 'CL')
hold off;
xlabel('$D_x$ [$\mu$m]');
ylabel('$D_y$ [$\mu$m]');
zlabel('$D_z$ [$\mu$m]');
axis equal
#+end_src

#+begin_src matlab :exports none :results none
%% Measured motion during tomography scan at 30RPM with a robust controller
figure;
tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile;
hold on;
plot(1e6*data_tomo_30rpm_m0.Dx_int(data_tomo_30rpm_m0.hac_status == 0), 1e6*data_tomo_30rpm_m0.Dy_int(data_tomo_30rpm_m0.hac_status == 0), ...
    'DisplayName', 'OL')
plot(1e6*data_tomo_30rpm_m0.Dx_int(data_tomo_30rpm_m0.hac_status == 1), 1e6*data_tomo_30rpm_m0.Dy_int(data_tomo_30rpm_m0.hac_status == 1), ...
    'DisplayName', 'CL')
hold off;
axis square
xlabel("X motion [$\mu m$]");
ylabel("Y motion [$\mu m$]");
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
xlim([-3, 3])
ylim([-3, 3])

ax2 = nexttile;
hold on;
plot(1e6*data_tomo_30rpm_m0.Dy_int(data_tomo_30rpm_m0.hac_status == 0), 1e6*data_tomo_30rpm_m0.Dz_int(data_tomo_30rpm_m0.hac_status == 0), ...
    'DisplayName', 'OL')
plot(1e6*data_tomo_30rpm_m0.Dy_int(data_tomo_30rpm_m0.hac_status == 1), 1e6*data_tomo_30rpm_m0.Dz_int(data_tomo_30rpm_m0.hac_status == 1), ...
    'DisplayName', 'CL')
hold off;
axis equal
xlim([-3, 3])
ylim([-3, 3])
xlabel("Y motion [$\mu m$]");
ylabel("Z motion [$\mu m$]");
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/id31_tomography_m0_30rpm_robust_xyz.pdf', 'width', 'full', 'height', 'tall');
#+end_src

#+name: fig:id31_tomography_m0_30rpm_robust_xyz
#+caption: Measured motion during tomography scan at 30RPM with a robust controller
#+RESULTS:
[[file:figs/id31_tomography_m0_30rpm_robust_xyz.png]]

#+begin_src matlab
%% Compute RMS values while in closed-loop
[~, i_m0] = find(data_tomo_30rpm_m0.hac_status == 1);
data_tomo_30rpm_m0.Dx_rms_cl = rms(detrend(data_tomo_30rpm_m0.Dx_int(i_m0+50000:end), 0));
data_tomo_30rpm_m0.Dy_rms_cl = rms(detrend(data_tomo_30rpm_m0.Dy_int(i_m0+50000:end), 0));
data_tomo_30rpm_m0.Dz_rms_cl = rms(detrend(data_tomo_30rpm_m0.Dz_int(i_m0+50000:end), 0));
data_tomo_30rpm_m0.Rx_rms_cl = rms(detrend(data_tomo_30rpm_m0.Rx_int(i_m0+50000:end), 0));
data_tomo_30rpm_m0.Ry_rms_cl = rms(detrend(data_tomo_30rpm_m0.Ry_int(i_m0+50000:end), 0));

data_tomo_30rpm_m0.Dx_rms_ol = rms(detrend(data_tomo_30rpm_m0.Dx_int(1:i_m0), 0));
data_tomo_30rpm_m0.Dy_rms_ol = rms(detrend(data_tomo_30rpm_m0.Dy_int(1:i_m0), 0));
data_tomo_30rpm_m0.Dz_rms_ol = rms(detrend(data_tomo_30rpm_m0.Dz_int(1:i_m0), 0));
data_tomo_30rpm_m0.Rx_rms_ol = rms(detrend(data_tomo_30rpm_m0.Rx_int(1:i_m0), 0));
data_tomo_30rpm_m0.Ry_rms_ol = rms(detrend(data_tomo_30rpm_m0.Ry_int(1:i_m0), 0));
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([1e6*data_tomo_30rpm_m0.Dx_rms_ol, 1e6*data_tomo_30rpm_m0.Dy_rms_ol, 1e9*data_tomo_30rpm_m0.Dz_rms_ol, 1e6*data_tomo_30rpm_m0.Rx_rms_ol, 1e6*data_tomo_30rpm_m0.Ry_rms_ol], ...
              {'$m_0$'}, {'$D_x$ [$\mu m$]', '$D_y$ [$\mu m$]', '$D_z$ [$nm$]', '$R_x$ [$\mu\text{rad}$]', '$R_y$ [$\mu\text{rad}$]'}, ' %.0f ');
#+end_src

#+name: tab:id31_tomo_30rpm_robust_ol_errors
#+caption: Measured error during open-loop tomography scans (30rpm)
#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
#+attr_latex: :center t :booktabs t
#+RESULTS:
|       | $D_x$ [$\mu m$] | $D_y$ [$\mu m$] | $D_z$ [$nm$] | $R_x$ [$\mu\text{rad}$] | $R_y$ [$\mu\text{rad}$] |
|-------+-----------------+-----------------+--------------+-------------------------+-------------------------|
| $m_0$ |               2 |               2 |           24 |                      10 |                      10 |

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([1e9*data_tomo_30rpm_m0.Dx_rms_cl, 1e9*data_tomo_30rpm_m0.Dy_rms_cl, 1e9*data_tomo_30rpm_m0.Dz_rms_cl, 1e9*data_tomo_30rpm_m0.Rx_rms_cl, 1e9*data_tomo_30rpm_m0.Ry_rms_cl], ...
              {'$m_0$'}, {'$D_x$ [nm]', '$D_y$ [nm]', '$D_z$ [nm]', '$R_x$ [nrad]', '$R_y$ [nrad]'}, ' %.0f ');
#+end_src

#+name: tab:id31_tomo_30rpm_robust_cl_errors
#+caption: Measured error during closed-loop tomography scans (30rpm, robust controller)
#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
#+attr_latex: :center t :booktabs t
#+RESULTS:
|       | $D_x$ [nm] | $D_y$ [nm] | $D_z$ [nm] | $R_x$ [nrad] | $R_y$ [nrad] |
|-------+------------+------------+------------+--------------+--------------|
| $m_0$ |         34 |         38 |         10 |          127 |          129 |

#+begin_src matlab :exports none
yztomography3dmovie('movies/id31_tomography_m0_30rpm_robust_xyz.avi', data_tomo_30rpm_m0, 'di', 300);
#+end_src

#+begin_src matlab :exports none
yztomographymovie('movies/tomography_30rpm_m0_robust', data_tomo_30rpm_m0, 'xlim_ax1', [-3, 3], 'ylim_ax1', [-3, 3], 'xlim_ax2', [-300, 300], 'ylim_ax2', [-300, 300])
#+end_src

*** TODO Slow Tomography Scans Comparison of control performances :noexport:
#+begin_src matlab
% Decentralized in the frame of the struts
data = load("2023-08-18_10-43_m0_1rpm.mat");
data.time = Ts*[0:length(data.Rz)-1];

% Rotating cartesian frame
data_cart = load("2023-08-18_18-33_m0_1rpm_K_cart.mat");
data_cart.time = Ts*[0:length(data_cart.Rz)-1];

% Fixed cartesian frame
data_cart_fixed = load("2023-08-18_19-08_m0_1rpm_K_cart_fixed.mat");
data_cart_fixed.time = Ts*[0:length(data_cart_fixed.Rz)-1];

% Fixed cartesian frame with Complementary Filters
data_cf = load("2023-08-21_14-28_m0_1rpm_K_cf.mat");
data_cf.time = Ts*[0:length(data_cf.Rz)-1];
#+end_src

#+begin_src matlab
1e9*rms(data.Dx_int(data.time<45))
1e9*rms(data_cart.Dx_int(data_cart.time<45))
1e9*rms(data_cart_fixed.Dx_int(data_cart_fixed.time<45))
1e9*rms(data_cf.Dx_int(data_cf.time<45))
#+end_src

#+begin_src matlab
1e9*rms(data.Dy_int(data.time<45))
1e9*rms(data_cart.Dy_int(data_cart.time<45))
1e9*rms(data_cart_fixed.Dy_int(data_cart_fixed.time<45))
1e9*rms(data_cf.Dy_int(data_cf.time<45))
#+end_src

#+begin_src matlab
1e9*rms(data.Dz_int(data.time<45))
1e9*rms(data_cart.Dz_int(data_cart.time<45))
1e9*rms(data_cart_fixed.Dz_int(data_cart_fixed.time<45))
1e9*rms(data_cf.Dz_int(data_cf.time<45))
#+end_src

#+begin_src matlab
1e9*rms(data.Rx_int(data.time<45))
1e9*rms(data_cart.Rx_int(data_cart.time<45))
1e9*rms(data_cart_fixed.Rx_int(data_cart_fixed.time<45))
1e9*rms(data_cf.Rx_int(data_cf.time<45))
#+end_src

#+begin_src matlab
1e9*rms(data.Ry_int(data.time<45))
1e9*rms(data_cart.Ry_int(data_cart.time<45))
1e9*rms(data_cart_fixed.Ry_int(data_cart_fixed.time<45))
1e9*rms(data_cf.Ry_int(data_cf.time<45))
#+end_src

#+begin_src matlab
figure;

hold on;
plot(data.time, data.Dy_int)
plot(data_cart.time, data_cart.Dy_int)
plot(data_cart_fixed.time, data_cart_fixed.Dy_int)
plot(data_cf.time, data_cf.Dy_int)
hold off;
#+end_src

#+begin_src matlab :exports none
% Hannning Windows
Nfft = floor(10.0/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

[data.pxx_Dx, data.f] = pwelch(detrend(data.Dx_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts);
[data.pxx_Dy, ~     ] = pwelch(detrend(data.Dy_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts);
[data.pxx_Dz, ~     ] = pwelch(detrend(data.Dz_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts);
[data.pxx_Rx, ~     ] = pwelch(detrend(data.Rx_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts);
[data.pxx_Ry, ~     ] = pwelch(detrend(data.Ry_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts);

[data_cart.pxx_Dx, data_cart.f] = pwelch(detrend(data_cart.Dx_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts);
[data_cart.pxx_Dy, ~          ] = pwelch(detrend(data_cart.Dy_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts);
[data_cart.pxx_Dz, ~          ] = pwelch(detrend(data_cart.Dz_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts);
[data_cart.pxx_Rx, ~          ] = pwelch(detrend(data_cart.Rx_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts);
[data_cart.pxx_Ry, ~          ] = pwelch(detrend(data_cart.Ry_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts);

[data_cart_fixed.pxx_Dx, data_cart_fixed.f] = pwelch(detrend(data_cart_fixed.Dx_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts);
[data_cart_fixed.pxx_Dy, ~                ] = pwelch(detrend(data_cart_fixed.Dy_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts);
[data_cart_fixed.pxx_Dz, ~                ] = pwelch(detrend(data_cart_fixed.Dz_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts);
[data_cart_fixed.pxx_Rx, ~                ] = pwelch(detrend(data_cart_fixed.Rx_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts);
[data_cart_fixed.pxx_Ry, ~                ] = pwelch(detrend(data_cart_fixed.Ry_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts);

[data_cf.pxx_Dx, data_cf.f] = pwelch(detrend(data_cf.Dx_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts);
[data_cf.pxx_Dy, ~        ] = pwelch(detrend(data_cf.Dy_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts);
[data_cf.pxx_Dz, ~        ] = pwelch(detrend(data_cf.Dz_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts);
[data_cf.pxx_Rx, ~        ] = pwelch(detrend(data_cf.Rx_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts);
[data_cf.pxx_Ry, ~        ] = pwelch(detrend(data_cf.Ry_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts);
#+end_src

#+begin_src matlab
figure;

hold on;
plot(data.f, data.pxx_Dy)
plot(data_cart.f, data_cart.pxx_Dy)
plot(data_cart_fixed.f, data_cart_fixed.pxx_Dy)
plot(data_cf.f, data_cf.pxx_Dy)
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
% legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
xlim([0.1, 5e2]);
#+end_src

#+begin_src matlab
figure;

hold on;
plot(data.f, sqrt(flip(-cumtrapz(flip(data.f), flip(data.pxx_Dy)))))
plot(data_cart.f, sqrt(flip(-cumtrapz(flip(data_cart.f), flip(data_cart.pxx_Dy)))))
plot(data_cart_fixed.f, sqrt(flip(-cumtrapz(flip(data_cart_fixed.f), flip(data_cart_fixed.pxx_Dy)))))
plot(data_cf.f, sqrt(flip(-cumtrapz(flip(data_cf.f), flip(data_cf.pxx_Dy)))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
% legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
xlim([0.1, 5e2]);
#+end_src

*** TODO Medium velocity tomography scans with CF control         :noexport:
#+begin_src matlab
data_m1_cf = load("2023-08-21_19-18_m1_6rpm_cf_control.mat");
data_m1_cf.time = Ts*[0:length(data_m1_cf.Rz)-1];
#+end_src

#+begin_src matlab
data_m2_cf = load("2023-08-21_18-07_m2_6rpm_cf_control.mat");
data_m2_cf.time = Ts*[0:length(data_m2_cf.Rz)-1];
#+end_src

And higher bandwidth:
#+begin_src matlab
data_m1_cf_high_fb = load("2023-08-21_19-24_m1_6rpm_cf_control_60Hz.mat");
data_m1_cf_high_fb.time = Ts*[0:length(data_m1_cf_high_fb.Rz)-1];
#+end_src

#+begin_src matlab
figure;
hold on;
plot(data_m1_cf.Dy_int, detrend(data_m1_cf.Dz_int, 0), 'DisplayName', 'm1')
plot(data_m2_cf.Dy_int, detrend(data_m2_cf.Dz_int, 0), 'DisplayName', 'm2')
plot(data_m1_cf_high_fb.Dy_int, detrend(data_m1_cf_high_fb.Dz_int, 0), 'DisplayName', 'm1, high BW')
axis equal
legend
#+end_src

#+begin_src matlab
1e9*rms(detrend(data_m1.Dz_int(i_m1+50000:end), 0))
1e9*rms(detrend(data_m1.Dy_int(i_m1+50000:end), 0))
1e9*rms(detrend(data_m1.Ry_int(i_m1+50000:end), 0))
#+end_src

#+begin_src matlab
1e9*rms(detrend(data_m1_cf.Dz_int, 0))
1e9*rms(detrend(data_m1_cf.Dy_int, 0))
1e9*rms(detrend(data_m1_cf.Ry_int, 0))
#+end_src

#+begin_src matlab
1e9*rms(detrend(data_m2.Dz_int, 0))
1e9*rms(detrend(data_m2.Dy_int, 0))
1e9*rms(detrend(data_m2.Ry_int, 0))
#+end_src

#+begin_src matlab
1e9*rms(detrend(data_m1_high_fb.Dz_int, 0))
1e9*rms(detrend(data_m1_high_fb.Dy_int, 0))
1e9*rms(detrend(data_m1_high_fb.Ry_int, 0))
#+end_src

*** TODO Fast Tomography Scan with Complementary Filter Controller :noexport:
#+begin_src matlab
data_cf = load("2023-08-21_14-33_m0_30rpm_cf_control.mat");
data_cf.time = Ts*[0:length(data_cf.Rz)-1];
#+end_src

#+begin_src matlab
[~, i0] = find(data.hac_status == 1);
1e9*rms(data.Dy_int(i0(1)+5000:end))
1e9*rms(data.Dz_int(i0(1)+5000:end))

1e9*rms(data_cf.Dy_int)
1e9*rms(data_cf.Dz_int)
#+end_src

Same performance than the robust controller in terms of RMS residual motion.

#+begin_src matlab
figure; plot3(data.Dx_int, data.Dy_int, data.Dz_int)
#+end_src

*** Tomography - Effect of the scanning velocity                  :noexport:
- [ ] What are the controller used here? Why worst results than with the robust controller?

#+begin_src matlab
data_1rpm = load("2023-08-18_10-43_m0_1rpm.mat");
data_1rpm.time = Ts*[0:length(data_1rpm.Rz)-1];
#+end_src

#+begin_src matlab
data_30rpm = load("2023-08-18_10-45_m0_30rpm.mat");
data_30rpm.time = Ts*[0:length(data_30rpm.Rz)-1];
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([1e9*rms(detrend(data_1rpm.Dy_int, 0)), 1e9*rms(detrend(data_30rpm.Dy_int, 0)); 1e9*rms(detrend(data_1rpm.Dz_int, 0)), 1e9*rms(detrend(data_30rpm.Dz_int, 0)); 1e9*rms(detrend(data_1rpm.Ry_int, 0)), 1e9*rms(detrend(data_30rpm.Ry_int, 0))]', {'1RPM', '30RPM'}, {'$D_y$', '$D_z$', '$R_y$'}, ' %.1f ');
#+end_src

#+name: tab:id31_tomography_effect_velocity_rms
#+caption: RMS values of the errors during tomography scan - Effect of $R_z$ velocity
#+attr_latex: :environment tabularx :width 0.5\linewidth :align lXXX
#+attr_latex: :center t :booktabs t
#+RESULTS:
|       | $D_y$ | $D_z$ | $R_y$ |
|-------+-------+-------+-------|
| 1RPM  |  30.9 |   5.9 |  92.4 |
| 30RPM |  71.7 |  12.5 | 301.3 |


#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([1e9*data_tomo_1rpm_m0.Dx_rms_cl, 1e9*data_tomo_1rpm_m0.Dy_rms_cl, 1e9*data_tomo_1rpm_m0.Dz_rms_cl, 1e9*data_tomo_1rpm_m0.Rx_rms_cl, 1e9*data_tomo_1rpm_m0.Ry_rms_cl; ...
               1e9*data_tomo_6rpm_m0.Dx_rms_cl, 1e9*data_tomo_6rpm_m0.Dy_rms_cl, 1e9*data_tomo_6rpm_m0.Dz_rms_cl, 1e9*data_tomo_6rpm_m0.Rx_rms_cl, 1e9*data_tomo_6rpm_m0.Ry_rms_cl; ...
               1e9*data_tomo_30rpm_m0.Dx_rms_cl, 1e9*data_tomo_30rpm_m0.Dy_rms_cl, 1e9*data_tomo_30rpm_m0.Dz_rms_cl, 1e9*data_tomo_30rpm_m0.Rx_rms_cl, 1e9*data_tomo_30rpm_m0.Ry_rms_cl], ...
              {'1rpm', '6rpm', '30rpm'}, {'$D_x$ [$\mu m$]', '$D_y$ [$\mu m$]', '$D_z$ [$nm$]', '$R_x$ [$\mu\text{rad}$]', '$R_y$ [$\mu\text{rad}$]'}, ' %.0f ');
#+end_src

#+RESULTS:
|       | $D_x$ [$\mu m$] | $D_y$ [$\mu m$] | $D_z$ [$nm$] | $R_x$ [$\mu\text{rad}$] | $R_y$ [$\mu\text{rad}$] |
|-------+-----------------+-----------------+--------------+-------------------------+-------------------------|
| 1rpm  |              13 |              15 |            5 |                      57 |                      55 |
| 6rpm  |              17 |              19 |            5 |                      70 |                      73 |
| 30rpm |              34 |              38 |           10 |                     127 |                     129 |

** $D_z$ scans: Dirty Layer Scans
<<ssec:test_id31_scans_dz>>
*** Step by Step $D_z$ motion
#+begin_src matlab
%% Load Dz MIM data
data_dz_steps_3nm = load("2023-08-18_14-57_dz_mim_3_nm.mat");
data_dz_steps_3nm.time = Ts*[0:length(data_dz_steps_3nm.Dz_int)-1];

data_dz_steps_10nm = load("2023-08-18_14-57_dz_mim_10_nm.mat");
data_dz_steps_10nm.time = Ts*[0:length(data_dz_steps_10nm.Dz_int)-1];

data_dz_steps_100nm = load("2023-08-18_14-57_dz_mim_100_nm.mat");
data_dz_steps_100nm.time = Ts*[0:length(data_dz_steps_100nm.Dz_int)-1];

data_dz_steps_1000nm = load("2023-08-18_14-57_dz_mim_1000_nm.mat");
data_dz_steps_1000nm.time = Ts*[0:length(data_dz_steps_1000nm.Dz_int)-1];
#+end_src

Three step sizes are tested:
- $10\,nm$ steps (Figure ref:fig:id31_dz_mim_10nm_steps)
- $100\,nm$ steps (Figure ref:fig:id31_dz_mim_100nm_steps)
- $1\,\mu m$ steps (Figure ref:fig:id31_dz_steps_response)

#+begin_src matlab :exports none :results none
%% Dz MIM test with 10nm steps
figure;
hold on;
plot(data_dz_steps_10nm.time, 1e9*(data_dz_steps_10nm.Dz_int - mean(data_dz_steps_10nm.Dz_int(1:1000))))
hold off;
xlabel('Time [s]');
ylabel('$D_z$ Motion [nm]');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/id31_dz_mim_10nm_steps.pdf', 'width', 'wide', 'height', 'normal');
#+end_src

#+name: fig:id31_dz_mim_10nm_steps
#+caption: Dz MIM test with 10nm steps (low pass filter with cut-off frequency of 10Hz is applied)
#+RESULTS:
[[file:figs/id31_dz_mim_10nm_steps.png]]

#+begin_src matlab :exports none :results none
%% Dz MIM test with 10nm steps
figure;
hold on;
plot(data_dz_steps_100nm.time, 1e9*(data_dz_steps_100nm.Dz_int - mean(data_dz_steps_100nm.Dz_int(1:1000))))
hold off;
xlabel('Time [s]');
ylabel('$D_z$ Motion [nm]');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/id31_dz_mim_100nm_steps.pdf', 'width', 'wide', 'height', 'normal');
#+end_src

#+name: fig:id31_dz_mim_100nm_steps
#+caption: Dz MIM test with 100nm steps
#+RESULTS:
[[file:figs/id31_dz_mim_100nm_steps.png]]

#+begin_src matlab :exports none :results none
%% Dz step response - Stabilization time is around 70ms
figure;
[~, i] = find(data_dz_steps_1000nm.m_hexa_dz>data_dz_steps_1000nm.m_hexa_dz(1));
i0 = i(1);

figure;
hold on;
plot(data_dz_steps_1000nm.time-data_dz_steps_1000nm.time(i0), 1e9*(data_dz_steps_1000nm.Dz_int - mean(data_dz_steps_1000nm.Dz_int(1:1000))))
plot([-1, 1], [1000-20, 1000-20], 'k--')
plot([-1, 1], [1000+20, 1000+20], 'k--')
xline(0, 'k--', 'LineWidth', 1.5)
xline(0.07, 'k--', 'LineWidth', 1.5)
hold off;
xlabel('Time [s]');
ylabel('$D_z$ Motion [nm]');
xlim([-0.1, 0.2]);
ylim([-100, 1600])
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/id31_dz_steps_response.pdf', 'width', 'wide', 'height', 'normal');
#+end_src

#+name: fig:id31_dz_steps_response
#+caption: $D_z$ step response - Stabilization time is around 70ms
#+RESULTS:
[[file:figs/id31_dz_steps_response.png]]


*** Continuous $D_z$ motion: Dirty Layer Scans
#+begin_src matlab
data_dz_10ums = load("2023-08-18_15-33_dirty_layer_m0_small.mat");
data_dz_10ums.time = Ts*[0:length(data_dz_10ums.Dz_int)-1];
#+end_src

#+begin_src matlab
data_dz_100ums = load("2023-08-18_15-32_dirty_layer_m0.mat");
data_dz_100ums.time = Ts*[0:length(data_dz_100ums.Dz_int)-1];
#+end_src

Two $D_z$ scans are performed:
- at $10\,\mu m/s$ in Figure ref:fig:id31_dirty_layer_scan_m0
- at $100\,\mu m/s$ in Figure ref:fig:id31_dirty_layer_scan_m0_large

#+begin_src matlab :exports none :results none
%% Dirty layer scan: Dz motion
figure;
hold on;
plot(data_dz_10ums.time, 1e6*data_dz_10ums.Dz_int, ...
    'DisplayName', sprintf('$\\epsilon D_z = %.0f$ nm RMS', 1e9*rms(data_dz_10ums.e_dz)))
plot(data_dz_10ums.time, 1e6*data_dz_10ums.e_dy, ...
    'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nm RMS', 1e9*rms(data_dz_10ums.e_dy)))
plot(data_dz_10ums.time, 1e6*data_dz_10ums.e_ry, ...
    'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$rad RMS', 1e6*rms(data_dz_10ums.e_ry)))
hold off;
xlabel('Time [s]');
ylabel('Motion [$\mu$m]');
xlim([0, 2.2])
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/id31_dirty_layer_scan_m0.pdf', 'width', 'wide', 'height', 'normal');
#+end_src

#+name: fig:id31_dirty_layer_scan_m0
#+caption: Dirty layer scan: $D_z$ motion at $10\,\mu m/s$
#+RESULTS:
[[file:figs/id31_dirty_layer_scan_m0.png]]

#+begin_src matlab :exports none :results none
%% Dirty layer scan: Dz motion
figure;
hold on;
plot(data_dz_100ums.time, 1e6*data_dz_100ums.Dz_int, ...
    'DisplayName', sprintf('$\\epsilon D_z = %.0f$ nm RMS', 1e9*rms(data_dz_100ums.e_dz)))
plot(data_dz_100ums.time, 1e6*data_dz_100ums.e_dy, ...
    'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nm RMS', 1e9*rms(data_dz_100ums.e_dy)))
plot(data_dz_100ums.time, 1e6*data_dz_100ums.e_ry, ...
    'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$rad RMS', 1e6*rms(data_dz_100ums.e_ry)))
hold off;
xlabel('Time [s]');
ylabel('Motion [$\mu$m]');
xlim([0, 2.2])
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/id31_dirty_layer_scan_m0_large.pdf', 'width', 'wide', 'height', 'normal');
#+end_src

#+name: fig:id31_dirty_layer_scan_m0_large
#+caption: Dirty layer scan: $D_z$ motion at $100\,\mu m/s$
#+RESULTS:
[[file:figs/id31_dirty_layer_scan_m0_large.png]]

#+begin_src matlab
%% Not so good results with the CF controller
data_cf = load(sprintf("%s/scans/2023-08-21_19-20_dirty_layer_m1_cf.mat", mat_dir));
data_cf.time = Ts*[0:length(data_cf.Dz_int)-1];
#+end_src

** $R_y$ scans: Reflectivity
<<ssec:test_id31_scans_reflectivity>>
#+begin_src matlab
%% Load data for the reflectivity scan
data_ry = load("2023-08-18_15-24_first_reflectivity_m0.mat");
data_ry.time = Ts*[0:length(data_ry.Ry_int)-1];
#+end_src

An $R_y$ scan is performed at $100\,\mu rad/s$ velocity (Figure ref:fig:id31_reflectivity_scan_Ry_100urads).
During the $R_y$ scan, the errors in $D_y$ are $D_z$ are kept small.

#+begin_src matlab :exports none :results none
%% Ry reflectivity scan
figure;
hold on;
plot(data_ry.time, 1e6*data_ry.Ry_int, 'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$rad RMS', 1e6*rms(data_ry.e_ry)))
plot(data_ry.time, 1e6*data_ry.e_dy, 'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nm RMS', 1e9*rms(data_ry.e_dy)))
plot(data_ry.time, 1e6*data_ry.e_dz, 'DisplayName', sprintf('$\\epsilon D_z = %.0f$ nm RMS', 1e9*rms(data_ry.e_dz)))
hold off;
xlabel('Time [s]');
ylabel('$R_y$ motion [$\mu$rad]')
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
xlim([0, 6.2]);
ylim([-310, 310])
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/id31_reflectivity_scan_Ry_100urads.pdf', 'width', 'wide', 'height', 'normal');
#+end_src

#+name: fig:id31_reflectivity_scan_Ry_100urads
#+caption: $R_y$ reflecitivity scan at $100\,\mu\text{rad}/s$ velocity
#+RESULTS:
[[file:figs/id31_reflectivity_scan_Ry_100urads.png]]


** $D_y$ Scans
<<ssec:test_id31_scans_dy>>
*** Introduction                                                    :ignore:
The steps generated by the IcePAP for the $T_y$ stage are send to the Speedgoat.
Then, we can know in real time what is the wanted position in $D_y$ during $T_y$ scans.

*** Open Loop
#+begin_src matlab
%% Slow Ty scan (10um/s)
data_ty_ol_slow = load("2023-08-21_20-05_ty_scan_m1_open_loop_slow.mat");
data_ty_ol_slow.time = Ts*[0:length(data_ty_ol_slow.Dy_int)-1];
#+end_src

We can clearly see micro-stepping errors of the stepper motor used for the $T_y$ stage.
The errors have a period of $10\,\mu m$ with an amplitude of $\pm 100\,nm$.

#+begin_src matlab :exports none :results none
%% Ty scan (at 10um/s) - Dy errors
figure;
plot(1e6*data_ty_ol_slow.Ty, 1e6*detrend(data_ty_ol_slow.e_dy, 0))
xlabel('Ty position [$\mu$m]');
ylabel('$D_y$ error [$\mu$m]');
xlim([-100, 100])
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/id31_ty_scan_10ums_ol_dy_errors.pdf', 'width', 'wide', 'height', 'normal');
#+end_src

#+name: fig:id31_ty_scan_10ums_ol_dy_errors
#+caption: $T_y$ scan (at $10\,\mu m/s$) - $D_y$ errors. The micro-stepping errors can clearly be seen with a period of $10\,\mu m$ and an amplitude of $\pm 100\,nm$
#+RESULTS:
[[file:figs/id31_ty_scan_10ums_ol_dy_errors.png]]

#+begin_src matlab :exports none :results none
%% Ty scan (at 10um/s) - Dz and Ry errors
figure;
tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile;
hold on;
plot(1e6*data_ty_ol_slow.Ty, 1e6*detrend(data_ty_ol_slow.e_dz, 0))
hold off;
xlabel('Ty position [$\mu$m]');
ylabel('$D_z$ error [$\mu$m]');
xlim([-100, 100])

ax2 = nexttile;
hold on;
plot(1e6*data_ty_ol_slow.Ty, 1e6*data_ty_ol_slow.e_ry)
hold off;
xlabel('Ty position [$\mu$m]');
ylabel('$R_y$ error [$\mu$rad]');
xlim([-100, 100])
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/id31_ty_scan_10ums_ol_dz_ry_errors.pdf', 'width', 'full', 'height', 'normal');
#+end_src

#+name: fig:id31_ty_scan_10ums_ol_dz_ry_errors
#+caption: $T_y$ scan (at $10\,\mu m/s$) - $D_z$ and $R_y$ errors. The $D_z$ error is most likely due to having the top interferometer pointing to a sphere. The large $R_y$ errors might also be due to the metrology system
#+RESULTS:
[[file:figs/id31_ty_scan_10ums_ol_dz_ry_errors.png]]

*** Closed Loop
#+begin_src matlab
%% Slow Ty scan (10um/s) - CL
data_ty_cl_slow = load("2023-08-21_20-07_ty_scan_m1_cf_closed_loop_slow.mat");
data_ty_cl_slow.time = Ts*[0:length(data_ty_cl_slow.Dy_int)-1];
#+end_src

#+begin_src matlab :exports none :results none
%% Ty scan (at 10um/s) - Dy errors
figure;
hold on;
plot(1e6*data_ty_ol_slow.Ty, 1e6*detrend(data_ty_ol_slow.e_dy, 0), ...
     'DisplayName', 'OL')
plot(1e6*data_ty_cl_slow.Ty, 1e6*detrend(data_ty_cl_slow.e_dy, 0), ...
     'DisplayName', sprintf('CL - $\\epsilon D_y = %.0f$ nmRMS', 1e9*rms(detrend(data_ty_cl_slow.e_dy, 0))))
hold off;
xlabel('Ty position [$\mu$m]');
ylabel('$D_y$ error [$\mu$m]');
xlim([-100, 100])
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/id31_ty_scan_10ums_cl_dy_errors.pdf', 'width', 'wide', 'height', 'normal');
#+end_src

#+name: fig:id31_ty_scan_10ums_cl_dy_errors
#+caption: $T_y$ scan (at $10\,\mu m/s$) - $D_y$ errors. Open-loop and Closed-loop scans
#+RESULTS:
[[file:figs/id31_ty_scan_10ums_cl_dy_errors.png]]

#+begin_src matlab :exports none :results none
%% Ty scan (at 10um/s) - Dz and Ry errors
figure;
tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile;
hold on;
plot(1e6*data_ty_ol_slow.Ty, 1e6*detrend(data_ty_ol_slow.e_dz, 0), ...
     'DisplayName', 'OL')
plot(1e6*data_ty_cl_slow.Ty, 1e6*detrend(data_ty_cl_slow.e_dz, 0), ...
     'DisplayName', sprintf('Cl - $\\epsilon D_z = %.0f$ nmRMS', 1e9*rms(detrend(data_ty_cl_slow.e_dz, 0))))
hold off;
xlabel('Ty position [$\mu$m]');
ylabel('$D_z$ error [$\mu$m]');
xlim([-100, 100])
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile;
hold on;
plot(1e6*data_ty_ol_slow.Ty, 1e6*data_ty_ol_slow.e_ry, ...
     'DisplayName', 'OL')
plot(1e6*data_ty_cl_slow.Ty, 1e6*data_ty_cl_slow.e_ry, ...
     'DisplayName', sprintf('Cl - $\\epsilon R_y = %.2f$ uradRMS', 1e6*rms(detrend(data_ty_cl_slow.e_ry, 0))))
hold off;
xlabel('Ty position [$\mu$m]');
ylabel('$R_y$ error [$\mu$rad]');
xlim([-100, 100])
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/id31_ty_scan_10ums_cl_dz_ry_errors.pdf', 'width', 'full', 'height', 'normal');
#+end_src

#+name: fig:id31_ty_scan_10ums_cl_dz_ry_errors
#+caption: $T_y$ scan (at $10\,\mu m/s$) - $D_z$ and $R_y$ errors. Open-loop and Closed-loop scans
#+RESULTS:
[[file:figs/id31_ty_scan_10ums_cl_dz_ry_errors.png]]

*** Faster Scan
#+begin_src matlab
%% Fast Ty scan (100um/s) - OL
data_ty_ol_fast = load("2023-08-21_20-05_ty_scan_m1_open_loop.mat");
data_ty_ol_fast.time = Ts*[0:length(data_ty_ol_fast.Dy_int)-1];
#+end_src

#+begin_src matlab
%% Fast Ty scan (10um/s) - CL
data_ty_cl_fast = load("2023-08-21_20-07_ty_scan_m1_cf_closed_loop.mat");
data_ty_cl_fast.time = Ts*[0:length(data_ty_cl_fast.Dy_int)-1];
#+end_src

Because of micro-stepping errors of the Ty stepper motor, when scanning at high velocity this induce high frequency vibration that are outside the bandwidth of the feedback controller.

At $100\,\mu m/s$, the micro-stepping errors with a period of $10\,\mu m$ (see Figure ref:fig:id31_ty_scan_10ums_ol_dy_errors) are at 10Hz.
These errors are them amplified by some resonances in the system.

This could be easily solved by changing the stepper motor for a torque motor for instance.

#+begin_src matlab :exports none :results none
%% Ty scan (at 100um/s) - Dy errors
figure;
hold on;
plot(1e6*data_ty_ol_fast.Ty, 1e6*detrend(data_ty_ol_fast.e_dy, 0), ...
     'DisplayName', 'OL')
plot(1e6*data_ty_cl_fast.Ty, 1e6*detrend(data_ty_cl_fast.e_dy, 0), ...
     'DisplayName', sprintf('CL - $\\epsilon D_y = %.0f$ nmRMS', 1e9*rms(detrend(data_ty_cl_fast.e_dy, 0))))
hold off;
xlabel('Ty position [$\mu$m]');
ylabel('$D_y$ error [$\mu$m]');
xlim([-100, 100])
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/id31_ty_scan_100ums_cl_dy_errors.pdf', 'width', 'wide', 'height', 'normal');
#+end_src

#+name: fig:id31_ty_scan_100ums_cl_dy_errors
#+caption: $T_y$ scan (at $100\,\mu m/s$) - $D_y$ errors. Open-loop and Closed-loop scans
#+RESULTS:
[[file:figs/id31_ty_scan_100ums_cl_dy_errors.png]]

#+begin_src matlab :exports none :results none
%% Ty scan (at 100um/s) - Dz and Ry errors
figure;
tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');

ax1 = nexttile;
hold on;
plot(1e6*data_ty_ol_fast.Ty, 1e6*detrend(data_ty_ol_fast.e_dz, 0), ...
     'DisplayName', 'OL')
plot(1e6*data_ty_cl_fast.Ty, 1e6*detrend(data_ty_cl_fast.e_dz, 0), ...
     'DisplayName', sprintf('Cl - $\\epsilon D_z = %.0f$ nmRMS', 1e9*rms(detrend(data_ty_cl_fast.e_dz, 0))))
hold off;
xlabel('Ty position [$\mu$m]');
ylabel('$D_z$ error [$\mu$m]');
xlim([-100, 100])
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile;
hold on;
plot(1e6*data_ty_ol_fast.Ty, 1e6*data_ty_ol_fast.e_ry, ...
     'DisplayName', 'OL')
plot(1e6*data_ty_cl_fast.Ty, 1e6*data_ty_cl_fast.e_ry, ...
     'DisplayName', sprintf('Cl - $\\epsilon R_y = %.2f$ uradRMS', 1e6*rms(detrend(data_ty_cl_fast.e_ry, 0))))
hold off;
xlabel('Ty position [$\mu$m]');
ylabel('$R_y$ error [$\mu$rad]');
xlim([-100, 100])
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/id31_ty_scan_100ums_cl_dz_ry_errors.pdf', 'width', 'full', 'height', 'normal');
#+end_src

#+name: fig:id31_ty_scan_100ums_cl_dz_ry_errors
#+caption: $T_y$ scan (at $100\,\mu m/s$) - $D_z$ and $R_y$ errors. Open-loop and Closed-loop scans
#+RESULTS:
[[file:figs/id31_ty_scan_100ums_cl_dz_ry_errors.png]]

** Combined $R_z$ and $D_y$: Diffraction Tomography
<<ssec:test_id31_scans_diffraction_tomo>>
Instead of doing a fast $R_z$ motion a slow $D_y$, the idea is to perform slow $R_z$ (here 1rpm) and fast $D_y$ scans with the nano-hexapod.

#+begin_src matlab
%% 100um/s - Robust controller
data_dt_100ums = load("2023-08-18_17-12_diffraction_tomo_m0.mat");
data_dt_100ums.time = Ts*[0:length(data_dt_100ums.Dy_int)-1];

%% 500um/s - Robust controller (Not used)
% data_dt_500ums = load(sprintf("%s/scans/2023-08-18_17-19_diffraction_tomo_m0_fast.mat", mat_dir));
% data_dt_500ums.time = Ts*[0:length(data_dt_500ums.Dy_int)-1];

%% 500um/s - Complementary filters
data_dt_500ums = load("2023-08-21_15-15_diffraction_tomo_m0_fast_cf.mat");
data_dt_500ums.time = Ts*[0:length(data_dt_500ums.Dy_int)-1];

%% 1mm/s - Complementary filters
data_dt_1000ums = load("2023-08-21_15-16_diffraction_tomo_m0_fast_cf.mat");
data_dt_1000ums.time = Ts*[0:length(data_dt_1000ums.Dy_int)-1];

%% 5mm/s - Complementary filters
% data_dt_5000ums = load(sprintf("%s/scans/2023-08-21_18-03_diffraction_tomo_m2_fast_cf.mat", mat_dir));
% data_dt_5000ums.time = Ts*[0:length(data_dt_5000ums.Dy_int)-1];

%% 10mm/s - Complementary filters
data_dt_10000ums = load("2023-08-21_15-17_diffraction_tomo_m0_fast_cf.mat");
data_dt_10000ums.time = Ts*[0:length(data_dt_10000ums.Dy_int)-1];
#+end_src

Here, the $D_y$ scans are performed only with the nano-hexapod (the Ty stage is not moving), so we are limited to $\pm 100\,\mu m$.

Several $D_y$ velocities are tested: $0.1\,mm/s$, $0.5\,mm/s$, $1\,mm/s$ and $10\,mm/s$ (see Figure ref:fig:id31_diffraction_tomo_velocities).

#+begin_src matlab :exports none :results none
%% Dy motion for several configured velocities
figure;
hold on;
plot(data_dt_10000ums.time, 1e6*data_dt_10000ums.Dy_int, ...
     'DisplayName', '$10 mm/s$')
plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.Dy_int, ...
     'DisplayName', '$1 mm/s$')
plot(data_dt_500ums.time, 1e6*data_dt_500ums.Dy_int, ...
     'DisplayName', '$0.5 mm/s$')
plot(data_dt_100ums.time, 1e6*data_dt_100ums.Dy_int, ...
     'DisplayName', '$0.1 mm/s$')
hold off;
xlim([0, 4]);
xlabel('Time [s]');
ylabel('$D_y$ position [$\mu$m]')
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/id31_diffraction_tomo_velocities.pdf', 'width', 'wide', 'height', 'normal');
#+end_src

#+name: fig:id31_diffraction_tomo_velocities
#+caption: Dy motion for several configured velocities
#+RESULTS:
[[file:figs/id31_diffraction_tomo_velocities.png]]

The corresponding "repetition rate" and $D_y$ scan per spindle turn are shown in Table ref:tab:diffraction_tomo_velocities.

The main issue here is the "waiting" time between two scans that is in the order of 50ms.
By removing this waiting time (fairly easily), we can double the repetition rate at 10mm/s.

#+name: tab:diffraction_tomo_velocities
#+caption: $D_y$ scaning repetition rate
#+attr_latex: :environment tabularx :width 0.6\linewidth :align lXX
#+attr_latex: :center t :booktabs t
| $D_y$ Velocity | Repetition rate | Scans per turn (at 1RPM) |
|----------------+-----------------+--------------------------|
| 0.1 mm/s       | 4 s             |                       15 |
| 0.5 mm/s       | 0.9 s           |                       65 |
| 1 mm/s         | 0.5 s           |                      120 |
| 10 mm/s        | 0.18 s          |                      330 |


The scan results for a velocity of 1mm/s is shown in Figure ref:fig:id31_diffraction_tomo_1mms.
The $D_z$ and $R_y$ errors are quite small during the scan.

The $D_y$ errors are quite large as the velocity is increased.
This type of scan can probably be massively improved by using feed-forward and optimizing the trajectory.
Also, if the detectors are triggered in position (the Speedgoat could generate an encoder signal for instance), we don't care about the $D_y$ errors.

#+begin_src matlab :exports none :results none
%% Diffraction tomography with Dy velocity of 1mm/s and Rz velocity of 1RPM
figure;
hold on;
plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.Dz_int, ...
     'DisplayName', sprintf('$\\epsilon D_z = %.0f$ nmRMS', 1e9*rms(data_dt_1000ums.Dz_int)))
plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.Ry_int, ...
     'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$radRMS', 1e6*rms(data_dt_1000ums.Ry_int)))
plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.Dy_int, ...
     'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nmRMS', 1e9*rms(data_dt_1000ums.Dy_int - data_dt_1000ums.m_hexa_dy)))
plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.m_hexa_dy, 'k--', 'HandleVisibility', 'off')
hold off;
xlim([0, 1])
xlabel('Time [s]');
ylabel('Measurement [nm,nrad]')
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
ylim([-110, 110])
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/id31_diffraction_tomo_1mms.pdf', 'width', 'full', 'height', 'normal');
#+end_src

#+name: fig:id31_diffraction_tomo_1mms
#+caption: Diffraction tomography with Dy velocity of 1mm/s and Rz velocity of 1RPM
#+RESULTS:
[[file:figs/id31_diffraction_tomo_1mms.png]]

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([1e9*rms(data_dt_100ums.Dy_int   - data_dt_100ums.m_hexa_dy), 1e9*rms(data_dt_500ums.Dy_int   - data_dt_500ums.m_hexa_dy), 1e9*rms(data_dt_1000ums.Dy_int  - data_dt_1000ums.m_hexa_dy), 1e9*rms(data_dt_10000ums.Dy_int - data_dt_10000ums.m_hexa_dy);
1e9*rms(data_dt_100ums.Dz_int), 1e9*rms(data_dt_500ums.Dz_int), 1e9*rms(data_dt_1000ums.Dz_int), 1e9*rms(data_dt_10000ums.Dz_int);
1e6*rms(data_dt_100ums.Ry_int), 1e6*rms(data_dt_500ums.Ry_int), 1e6*rms(data_dt_1000ums.Ry_int), 1e6*rms(data_dt_10000ums.Ry_int)]', {'0.1 mm/s' ,'0.5 mm/s', '1 mm/s', '10 mm/s'}, {'Velocity', '$D_y$ [nmRMS]', '$D_z$ [nmRMS]', '$R_y$ [$\mu\text{radRMS}$]'}, ' %.1f ');
#+end_src

#+name: tab:id31_diffraction_tomo_results
#+caption: Obtained errors for several $D_y$ velocities
#+attr_latex: :environment tabularx :width \linewidth :align lXX
#+attr_latex: :center t :booktabs t
#+RESULTS:
| Velocity | $D_y$ [nmRMS] | $D_z$ [nmRMS] | $R_y$ [$\mu\text{radRMS}$] |
|----------+---------------+---------------+----------------------------|
| 0.1 mm/s |          75.5 |           9.1 |                        0.1 |
| 0.5 mm/s |         190.5 |          10.0 |                        0.1 |
| 1 mm/s   |         428.0 |          11.2 |                        0.2 |
| 10 mm/s  |        4639.9 |          55.9 |                        1.4 |

** Conclusion
:PROPERTIES:
:UNNUMBERED: t
:END:
<<ssec:test_id31_scans_conclusion>>

For each conducted experiments, the $D_y$, $D_z$ and $R_y$ errors are computed and summarized in Table ref:tab:id31_experiments_results_summary.

#+begin_src matlab
%% Summary of results
data_results = [...
    1e9*data_tomo_1rpm_m0.Dy_rms_cl,                                      1e9*data_tomo_1rpm_m0.Dz_rms_cl,            1e9*data_tomo_1rpm_m0.Ry_rms_cl           ; ... % Tomo 1rpm
    1e9*data_tomo_6rpm_m0.Dy_rms_cl,                                      1e9*data_tomo_6rpm_m0.Dz_rms_cl,            1e9*data_tomo_6rpm_m0.Ry_rms_cl           ; ... % Tomo 6rpm
    1e9*data_tomo_30rpm_m0.Dy_rms_cl,                                     1e9*data_tomo_30rpm_m0.Dz_rms_cl,           1e9*data_tomo_30rpm_m0.Ry_rms_cl          ; ... % Tomo 30rpm
    1e9*rms(detrend(data_dz_10ums.e_dy, 0)),                              1e9*rms(detrend(data_dz_10ums.e_dz, 0)),    1e9*rms(detrend(data_dz_10ums.e_ry, 0))   ; ... % Dz 10um/s
    1e9*rms(detrend(data_dz_100ums.e_dy,0)),                              1e9*rms(detrend(data_dz_100ums.e_dz,0)),    1e9*rms(detrend(data_dz_100ums.e_ry,0))   ; ... % Dz 100um/s
    1e9*rms(detrend(data_ry.e_dy,0)),                                     1e9*rms(detrend(data_ry.e_dz,0)),           1e9*rms(detrend(data_ry.e_ry,0))          ; ... % Ry 100urad/s
    1e9*rms(detrend(data_ty_cl_slow.e_dy, 0)),                            1e9*rms(detrend(data_ty_cl_slow.e_dz, 0)),  1e9*rms(detrend(data_ty_cl_slow.e_rz, 0)) ; ... % Dy 10 um/s
    1e9*rms(detrend(data_dt_100ums.Dy_int-data_dt_100ums.m_hexa_dy,  0)), 1e9*rms(detrend(data_dt_100ums.Dz_int, 0)), 1e9*rms(detrend(data_dt_100ums.Ry_int, 0)); ... % Diffraction tomo 0.1mm/s
    1e9*rms(detrend(data_dt_1000ums.Dy_int-data_dt_1000ums.m_hexa_dy,0)), 1e9*rms(detrend(data_dt_1000ums.Dz_int,0)), 1e9*rms(detrend(data_dt_1000ums.Ry_int,0))  ... % Diffraction tomo 1mm/s
];
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable(data_results, {'Tomography ($R_z$ 1rpm)', 'Tomography ($R_z$ 6rpm)', 'Tomography ($R_z$ 30rpm)', 'Dirty Layer ($D_z$ $10\,\mu m/s$)', 'Dirty Layer ($D_z$ $100\,\mu m/s$)', 'Reflectivity ($R_y$ $100\,\mu\text{rad}/s$)', 'Lateral Scan ($D_y$ $10\,\mu m/s$)', 'Diffraction Tomography ($R_z$ 1rpm, $D_y$ 0.1mm/s)', 'Diffraction Tomography ($R_z$ 1rpm, $D_y$ 1mm/s)'}, {'$D_y$ [nmRMS]', '$D_z$ [nmRMS]', '$R_y$ [nradRMS]'}, ' %.0f ');
#+end_src

#+name: tab:id31_experiments_results_summary
#+caption: Table caption
#+attr_latex: :environment tabularx :width \linewidth :align Xccc
#+attr_latex: :center t :booktabs t
#+RESULTS:
|                                                    | $D_y$ [nmRMS] | $D_z$ [nmRMS] | $R_y$ [nradRMS] |
|----------------------------------------------------+---------------+---------------+-----------------|
| Tomography ($R_z$ 1rpm)                            |            15 |             5 |              55 |
| Tomography ($R_z$ 6rpm)                            |            19 |             5 |              73 |
| Tomography ($R_z$ 30rpm)                           |            38 |            10 |             129 |
| Dirty Layer ($D_z$ $10\,\mu m/s$)                  |            25 |             5 |             114 |
| Dirty Layer ($D_z$ $100\,\mu m/s$)                 |            34 |            15 |             130 |
| Reflectivity ($R_y$ $100\,\mu\text{rad}/s$)        |            28 |             6 |             118 |
| Lateral Scan ($D_y$ $10\,\mu m/s$)                 |            21 |            10 |              37 |
| Diffraction Tomography ($R_z$ 1rpm, $D_y$ 0.1mm/s) |            75 |             9 |             118 |
| Diffraction Tomography ($R_z$ 1rpm, $D_y$ 1mm/s)   |           428 |            11 |             169 |

* Bibliography                                                        :ignore:
#+latex: \printbibliography[heading=bibintoc,title={Bibliography}]

* Helping Functions                                                 :noexport:
** Initialize Path
#+NAME: m-init-path
#+BEGIN_SRC matlab
addpath('./matlab/');     % Path for scripts

%% Path for functions, data and scripts
addpath('./matlab/mat/'); % Path for Computed FRF
addpath('./matlab/src/'); % Path for functions
addpath('./matlab/STEPS/'); % Path for STEPS
addpath('./matlab/subsystems/'); % Path for Subsystems Simulink files

%% Data directory
data_dir = './matlab/mat/'
#+END_SRC

#+NAME: m-init-path-tangle
#+BEGIN_SRC matlab
%% Path for functions, data and scripts
addpath('./mat/'); % Path for Data
addpath('./src/'); % Path for functions
addpath('./STEPS/'); % Path for STEPS
addpath('./subsystems/'); % Path for Subsystems Simulink files

%% Data directory
data_dir = './mat/'
#+END_SRC

** Initialize Simscape Model
#+NAME: m-init-simscape
#+begin_src matlab
% Simulink Model name
mdl = 'nass_model_id31';
#+end_src

** Initialize other elements
#+NAME: m-init-other
#+BEGIN_SRC matlab
%% Colors for the figures
colors = colororder;

%% Frequency Vector
freqs = logspace(log10(1), log10(2e3), 1000);

%% Sampling Time
Ts = 1e-4;
#+END_SRC

* Matlab Functions                                                  :noexport:
** Utility Functions
*** =h5scan= - Easily load h5 files
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/h5scan.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
#+begin_src matlab
function [cntrs,tp] = h5scan(pth,smp,ds,sn,varargin)

i = cellfun(@(x) isa(x,'detector'),varargin);
if any(i), det = varargin{i}; varargin = varargin(~i); else, det = []; end;
if ~isstr(ds), ds = sprintf('%.4d',ds); end;

f = sprintf('%s/%s/%s_%s/%s_%s.h5',pth,smp,smp,ds,smp,ds);
h = h5info(f,sprintf('/%d.1/measurement',sn));
fid = H5F.open(f);
for i = 1:length(h.Links),
  nm = h.Links(i).Name;
  try,
    id = H5D.open(fid,h.Links(i).Value{1});
    cntrs.(nm) = H5D.read(id);
    H5D.close(id);
    if ~isempty(det) & strcmp(nm,det.name), cntrs.(nm) = integrate(det,double(cntrs.(nm))); end;
  catch,
    warning('solving problem with %s\n',nm);
    cntrs.(nm) = vrtlds(sprintf('%s/%s/%s_%s/scan%.4d/',pth,smp,smp,ds,sn),nm,det);
  end;
  [~,tp.(nm)] = fileparts(h.Links(i).Value{1});
end;
try,
  h = h5info(f,sprintf('/%d.2/measurement',sn));
catch,
  h = [];
end;
if ~isempty(h),
  for i = 1:length(h.Links),
    nm = h.Links(i).Name;
    try,
      id = H5D.open(fid,h.Links(i).Value{1});
      cntrs.part2.(nm) = H5D.read(id);
      H5D.close(id);
    catch,
      warning('solving problem with %s\n',nm);
      cntrs.part2.(nm) = vrtlds(sprintf('%s/%s/%s_%s/scan%.4d/',pth,smp,smp,ds,sn),nm,det);
    end;
    [~,tp.part2.(nm)] = fileparts(h.Links(i).Value{1});
  end;
end;
if length(varargin),
  fn = sprintf('/%d.1/instrument/positioners/',sn);
  h = h5info(f,fn);
  [~,k,m] = intersect({h.Datasets.Name},varargin,'stable');
  h.Datasets = h.Datasets(k);
  for i = 1:length(h.Datasets),
    id = H5D.open(fid,[fn h.Datasets(i).Name]);
    cntrs.(h.Datasets(i).Name) = H5D.read(id);
    H5D.close(id);
  end;
end;

H5F.close(fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%

function A = vrtlds(f,nm,det)
%try,
  n = 0; A = [];
  fn = sprintf('%s/%s_%.4d.h5',f,nm,n);
  while exist(fn) == 2,
    fid = H5F.open(fn); n = n+1;
    id = H5D.open(fid,sprintf('/entry_0000/ESRF-ID31/%s/data',nm));
    if 2 < nargin & strcmp(nm,'p3') & ~isempty(det),
      fprintf('integrating %s\n',fn);
      if isempty(A),
        A = integrate(det,double(H5D.read(id)),1);
      else,
        tmp = integrate(det,double(H5D.read(id)),1); A.y = cat(2,A.y,tmp.y); A.y0 = cat(2,A.y0,tmp.y0);
      end;
    else,
      fprintf('loading %s\n',fn);
      A = cat(3,A,H5D.read(id));
    end;
    H5D.close(id); H5F.close(fid);
    fn = sprintf('%s/%s_%.4d.h5',f,nm,n);
  end;
%catch,
%  A = [];
%end;

% fid = H5F.open...
% id = H5D.open...
% sid = H5D.get_space(id);
% [ndims,h5_dims]=H5S.get_simple_extent_dims(sid)

% Read a 2x3 hyperslab of data from a dataset, starting in the 4th row and 5th column of the example dataset.
% Create a property list identifier, then open the HDF5 file and the dataset /g1/g1.1/dset1.1.1.

% fid = H5F.open('example.h5');
% id = H5D.open(fid,'/g1/g1.1/dset1.1.1');

% dims = ([500 1679 1475];
% msid = H5S.create_simple(3,dims,[]);
% sid = H5D.get_space(id);
% offset = [n*500 0 0];
% block = dims; % d1: 500 or min(d1tot-n*500,500)
% H5S.select_hyperslab(sid,'H5S_SELECT_SET',offset,[],[],block);
% data = H5D.read(id,'H5ML_DEFAULT',msid,sid,'H5P_DEFAULT');
% H5D.close(id);
% H5F.close(fid);
#+end_src

*** =sphereFit= - Fit sphere from x,y,z points
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/sphereFit.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:

#+begin_src matlab
function [Center,Radius] = sphereFit(X)
% this fits a sphere to a collection of data using a closed form for the
% solution (opposed to using an array the size of the data set).
% Minimizes Sum((x-xc)^2+(y-yc)^2+(z-zc)^2-r^2)^2
% x,y,z are the data, xc,yc,zc are the sphere's center, and r is the radius
% Assumes that points are not in a singular configuration, real numbers, ...
% if you have coplanar data, use a circle fit with svd for determining the
% plane, recommended Circle Fit (Pratt method), by Nikolai Chernov
% http://www.mathworks.com/matlabcentral/fileexchange/22643
% Input:
% X: n x 3 matrix of cartesian data
% Outputs:
% Center: Center of sphere
% Radius: Radius of sphere
% Author:
% Alan Jennings, University of Dayton
A=[mean(X(:,1).*(X(:,1)-mean(X(:,1)))), ...
    2*mean(X(:,1).*(X(:,2)-mean(X(:,2)))), ...
    2*mean(X(:,1).*(X(:,3)-mean(X(:,3)))); ...
    0, ...
    mean(X(:,2).*(X(:,2)-mean(X(:,2)))), ...
    2*mean(X(:,2).*(X(:,3)-mean(X(:,3)))); ...
    0, ...
    0, ...
    mean(X(:,3).*(X(:,3)-mean(X(:,3))))];
A=A+A.';
B=[mean((X(:,1).^2+X(:,2).^2+X(:,3).^2).*(X(:,1)-mean(X(:,1))));...
    mean((X(:,1).^2+X(:,2).^2+X(:,3).^2).*(X(:,2)-mean(X(:,2))));...
    mean((X(:,1).^2+X(:,2).^2+X(:,3).^2).*(X(:,3)-mean(X(:,3))))];
Center=(A\B).';
Radius=sqrt(mean(sum([X(:,1)-Center(1),X(:,2)-Center(2),X(:,3)-Center(3)].^2,2)));
#+end_src

*** =unwrapphase= - Unwrap phase for FRF data
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/unwrapphase.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:

#+begin_src matlab
function [unwraped_phase] = unwrapphase(frf, f, args)
#+end_src

#+begin_src matlab
arguments
  frf
  f
  args.f0 (1,1) double {mustBeNumeric} = 1
end
#+end_src

#+begin_src matlab
unwraped_phase = unwrap(frf);
[~,i] = min(abs(f - args.f0));
unwraped_phase = unwraped_phase - 2*pi*round(unwraped_phase(i)./(2*pi));
#+end_src

*** =circlefit=
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/circlefit.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:

#+begin_src matlab
function   [xc,yc,R,a] = circlefit(x,y)
%
%   [xc yx R] = circfit(x,y)
%
%   fits a circle  in x,y plane in a more accurate
%   (less prone to ill condition )
%  procedure than circfit2 but using more memory
%  x,y are column vector where (x(i),y(i)) is a measured point
%
%  result is center point (yc,xc) and radius R
%  an optional output is the vector of coeficient a
% describing the circle's equation
%
%   x^2+y^2+a(1)*x+a(2)*y+a(3)=0
%
%  By:  Izhak bucher 25/oct /1991,
    x=x(:); y=y(:);
    a=[x y ones(size(x))]\[-(x.^2+y.^2)];
    xc = -.5*a(1);
    yc = -.5*a(2);
    R  =  sqrt((a(1)^2+a(2)^2)/4-a(3));
#+end_src

** Initialize Simscape Model
*** =initializeSimscapeConfiguration=: Simscape Configuration
#+begin_src matlab :tangle matlab/src/initializeSimscapeConfiguration.m :comments none :mkdirp yes :eval no
function [] = initializeSimscapeConfiguration(args)

    arguments
        args.gravity logical {mustBeNumericOrLogical} = true
    end

    conf_simscape = struct();

    if args.gravity
        conf_simscape.type = 1;
    else
        conf_simscape.type = 2;
    end

    if exist('./mat', 'dir')
        if exist('./mat/nass_model_conf_simscape.mat', 'file')
            save('mat/nass_model_conf_simscape.mat', 'conf_simscape', '-append');
        else
            save('mat/nass_model_conf_simscape.mat', 'conf_simscape');
        end
    elseif exist('./matlab', 'dir')
        if exist('./matlab/mat/nass_model_conf_simscape.mat', 'file')
            save('matlab/mat/nass_model_conf_simscape.mat', 'conf_simscape', '-append');
        else
            save('matlab/mat/nass_model_conf_simscape.mat', 'conf_simscape');
        end
    end

end
#+end_src

*** =initializeLoggingConfiguration=: Logging Configuration
#+begin_src matlab :tangle matlab/src/initializeLoggingConfiguration.m :comments none :mkdirp yes :eval no
function [] = initializeLoggingConfiguration(args)

    arguments
        args.log      char   {mustBeMember(args.log,{'none', 'all', 'forces'})} = 'none'
        args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
    end

    conf_log = struct();

    switch args.log
      case 'none'
        conf_log.type = 0;
      case 'all'
        conf_log.type = 1;
      case 'forces'
        conf_log.type = 2;
    end

    conf_log.Ts = args.Ts;

    if exist('./mat', 'dir')
        if exist('./mat/nass_model_conf_log.mat', 'file')
            save('mat/nass_model_conf_log.mat', 'conf_log', '-append');
        else
            save('mat/nass_model_conf_log.mat', 'conf_log');
        end
    elseif exist('./matlab', 'dir')
        if exist('./matlab/mat/nass_model_conf_log.mat', 'file')
            save('matlab/mat/nass_model_conf_log.mat', 'conf_log', '-append');
        else
            save('matlab/mat/nass_model_conf_log.mat', 'conf_log');
        end
    end

end
#+end_src

*** =initializeReferences=: Generate Reference Signals
#+begin_src matlab :tangle matlab/src/initializeReferences.m :comments none :mkdirp yes :eval no
function [ref] = initializeReferences(args)

    arguments
        % Sampling Frequency [s]
        args.Ts           (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
        % Maximum simulation time [s]
        args.Tmax         (1,1) double {mustBeNumeric, mustBePositive} = 100
        % Either "constant" / "triangular" / "sinusoidal"
        args.Dy_type      char {mustBeMember(args.Dy_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
        % Amplitude of the displacement [m]
        args.Dy_amplitude (1,1) double {mustBeNumeric} = 0
        % Period of the displacement [s]
        args.Dy_period    (1,1) double {mustBeNumeric, mustBePositive} = 1
        % Either "constant" / "triangular" / "sinusoidal"
        args.Ry_type      char {mustBeMember(args.Ry_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
        % Amplitude [rad]
        args.Ry_amplitude (1,1) double {mustBeNumeric} = 0
        % Period of the displacement [s]
        args.Ry_period    (1,1) double {mustBeNumeric, mustBePositive} = 1
        % Either "constant" / "rotating"
        args.Rz_type      char {mustBeMember(args.Rz_type,{'constant', 'rotating', 'rotating-not-filtered'})} = 'constant'
        % Initial angle [rad]
        args.Rz_amplitude (1,1) double {mustBeNumeric} = 0
        % Period of the rotating [s]
        args.Rz_period    (1,1) double {mustBeNumeric, mustBePositive} = 1
        % For now, only constant is implemented
        args.Dh_type      char {mustBeMember(args.Dh_type,{'constant'})} = 'constant'
        % Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
        args.Dh_pos       (6,1) double {mustBeNumeric} = zeros(6, 1), ...
        % For now, only constant is implemented
        args.Rm_type      char {mustBeMember(args.Rm_type,{'constant'})} = 'constant'
        % Initial position of the two masses
        args.Rm_pos       (2,1) double {mustBeNumeric} = [0; pi]
        % For now, only constant is implemented
        args.Dn_type      char {mustBeMember(args.Dn_type,{'constant'})} = 'constant'
        % Initial position [m,m,m,rad,rad,rad] of the top platform
        args.Dn_pos       (6,1) double {mustBeNumeric} = zeros(6,1)
    end

    %% Set Sampling Time
    Ts = args.Ts;
    Tmax = args.Tmax;

    %% Low Pass Filter to filter out the references
    s = zpk('s');
    w0 = 2*pi*10;
    xi = 1;
    H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2);

    %% Translation stage - Dy
    t = 0:Ts:Tmax; % Time Vector [s]
    Dy   = zeros(length(t), 1);
    Dyd  = zeros(length(t), 1);
    Dydd = zeros(length(t), 1);
    switch args.Dy_type
      case 'constant'
        Dy(:)   = args.Dy_amplitude;
        Dyd(:)  = 0;
        Dydd(:) = 0;
      case 'triangular'
        % This is done to unsure that we start with no displacement
        Dy_raw = args.Dy_amplitude*sawtooth(2*pi*t/args.Dy_period,1/2);
        i0 = find(t>=args.Dy_period/4,1);
        Dy(1:end-i0+1) = Dy_raw(i0:end);
        Dy(end-i0+2:end) = Dy_raw(end); % we fix the last value

        % The signal is filtered out
        Dy   = lsim(H_lpf,     Dy, t);
        Dyd  = lsim(H_lpf*s,   Dy, t);
        Dydd = lsim(H_lpf*s^2, Dy, t);
      case 'sinusoidal'
        Dy(:) = args.Dy_amplitude*sin(2*pi/args.Dy_period*t);
        Dyd   = args.Dy_amplitude*2*pi/args.Dy_period*cos(2*pi/args.Dy_period*t);
        Dydd  = -args.Dy_amplitude*(2*pi/args.Dy_period)^2*sin(2*pi/args.Dy_period*t);
      otherwise
        warning('Dy_type is not set correctly');
    end

    Dy = struct('time', t, 'signals', struct('values', Dy), 'deriv', Dyd, 'dderiv', Dydd);

    %% Tilt Stage - Ry
    t = 0:Ts:Tmax; % Time Vector [s]
    Ry   = zeros(length(t), 1);
    Ryd  = zeros(length(t), 1);
    Rydd = zeros(length(t), 1);

    switch args.Ry_type
      case 'constant'
        Ry(:) = args.Ry_amplitude;
        Ryd(:)   = 0;
        Rydd(:)  = 0;
      case 'triangular'
        Ry_raw = args.Ry_amplitude*sawtooth(2*pi*t/args.Ry_period,1/2);
        i0 = find(t>=args.Ry_period/4,1);
        Ry(1:end-i0+1) = Ry_raw(i0:end);
        Ry(end-i0+2:end) = Ry_raw(end); % we fix the last value

        % The signal is filtered out
        Ry   = lsim(H_lpf,     Ry, t);
        Ryd  = lsim(H_lpf*s,   Ry, t);
        Rydd = lsim(H_lpf*s^2, Ry, t);
      case 'sinusoidal'
        Ry(:) = args.Ry_amplitude*sin(2*pi/args.Ry_period*t);

        Ryd  = args.Ry_amplitude*2*pi/args.Ry_period*cos(2*pi/args.Ry_period*t);
        Rydd = -args.Ry_amplitude*(2*pi/args.Ry_period)^2*sin(2*pi/args.Ry_period*t);
      otherwise
        warning('Ry_type is not set correctly');
    end

    Ry = struct('time', t, 'signals', struct('values', Ry), 'deriv', Ryd, 'dderiv', Rydd);

    %% Spindle - Rz
    t = 0:Ts:Tmax; % Time Vector [s]
    Rz   = zeros(length(t), 1);
    Rzd  = zeros(length(t), 1);
    Rzdd = zeros(length(t), 1);

    switch args.Rz_type
      case 'constant'
        Rz(:)   = args.Rz_amplitude;
        Rzd(:)  = 0;
        Rzdd(:) = 0;
      case 'rotating-not-filtered'
        Rz(:) = 2*pi/args.Rz_period*t;

        % The signal is filtered out
        Rz(:)   = 2*pi/args.Rz_period*t;
        Rzd(:)  = 2*pi/args.Rz_period;
        Rzdd(:) = 0;

        % We add the angle offset
        Rz = Rz + args.Rz_amplitude;

      case 'rotating'
        Rz(:) = 2*pi/args.Rz_period*t;

        % The signal is filtered out
        Rz   = lsim(H_lpf,     Rz, t);
        Rzd  = lsim(H_lpf*s,   Rz, t);
        Rzdd = lsim(H_lpf*s^2, Rz, t);

        % We add the angle offset
        Rz = Rz + args.Rz_amplitude;
      otherwise
        warning('Rz_type is not set correctly');
    end

    Rz = struct('time', t, 'signals', struct('values', Rz), 'deriv', Rzd, 'dderiv', Rzdd);

    %% Micro-Hexapod
    t = [0, Ts];
    Dh = zeros(length(t), 6);
    Dhl = zeros(length(t), 6);

    switch args.Dh_type
      case 'constant'
        Dh = [args.Dh_pos, args.Dh_pos];

        load('nass_model_stages.mat', 'micro_hexapod');

        AP = [args.Dh_pos(1) ; args.Dh_pos(2) ; args.Dh_pos(3)];

        tx = args.Dh_pos(4);
        ty = args.Dh_pos(5);
        tz = args.Dh_pos(6);

        ARB = [cos(tz) -sin(tz) 0;
               sin(tz)  cos(tz) 0;
               0        0       1]*...
            [ cos(ty) 0 sin(ty);
              0       1 0;
              -sin(ty) 0 cos(ty)]*...
            [1 0        0;
             0 cos(tx) -sin(tx);
             0 sin(tx)  cos(tx)];

        [~, Dhl] = inverseKinematics(micro_hexapod, 'AP', AP, 'ARB', ARB);
        Dhl = [Dhl, Dhl];
      otherwise
        warning('Dh_type is not set correctly');
    end

    Dh = struct('time', t, 'signals', struct('values', Dh));
    Dhl = struct('time', t, 'signals', struct('values', Dhl));

    if exist('./mat', 'dir')
        if exist('./mat/nass_model_references.mat', 'file')
            save('mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts', '-append');
        else
            save('mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts');
        end
    elseif exist('./matlab', 'dir')
        if exist('./matlab/mat/nass_model_references.mat', 'file')
            save('matlab/mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts', '-append');
        else
            save('matlab/mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts');
        end
    end

end
#+end_src

*** =initializeDisturbances=: Initialize Disturbances
#+begin_src matlab :tangle matlab/src/initializeDisturbances.m :comments none :mkdirp yes :eval no
function [] = initializeDisturbances(args)
% initializeDisturbances - Initialize the disturbances
%
% Syntax: [] = initializeDisturbances(args)
%
% Inputs:
%    - args -


    arguments
        % Global parameter to enable or disable the disturbances
        args.enable logical {mustBeNumericOrLogical} = true
        % Ground Motion - X direction
        args.Dw_x logical {mustBeNumericOrLogical} = true
        % Ground Motion - Y direction
        args.Dw_y logical {mustBeNumericOrLogical} = true
        % Ground Motion - Z direction
        args.Dw_z logical {mustBeNumericOrLogical} = true
        % Translation Stage - X direction
        args.Fdy_x logical {mustBeNumericOrLogical} = true
        % Translation Stage - Z direction
        args.Fdy_z logical {mustBeNumericOrLogical} = true
        % Spindle - X direction
        args.Frz_x logical {mustBeNumericOrLogical} = true
        % Spindle - Y direction
        args.Frz_y logical {mustBeNumericOrLogical} = true
        % Spindle - Z direction
        args.Frz_z logical {mustBeNumericOrLogical} = true
    end

    % Initialization of random numbers
    rng("shuffle");

    %% Ground Motion
    if args.enable
        % Load the PSD of disturbance
        load('ustation_disturbance_psd.mat', 'gm_dist')

        % Frequency Data
        Dw.f = gm_dist.f;
        Dw.psd_x = gm_dist.pxx_x;
        Dw.psd_y = gm_dist.pxx_y;
        Dw.psd_z = gm_dist.pxx_z;

        % Time data
        Fs = 2*Dw.f(end);      % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
        N  = 2*length(Dw.f);   % Number of Samples match the one of the wanted PSD
        T0 = N/Fs;             % Signal Duration [s]
        Dw.t = linspace(0, T0, N+1)'; % Time Vector [s]

        % ASD representation of the ground motion
        C = zeros(N/2,1);
        for i = 1:N/2
            C(i) = sqrt(Dw.psd_x(i)/T0);
        end

        if args.Dw_x
            theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
            Cx = [0 ; C.*complex(cos(theta),sin(theta))];
            Cx = [Cx; flipud(conj(Cx(2:end)))];;
            Dw.x = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m]
        else
            Dw.x = zeros(length(Dw.t), 1);
        end

        if args.Dw_y
            theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
            Cx = [0 ; C.*complex(cos(theta),sin(theta))];
            Cx = [Cx; flipud(conj(Cx(2:end)))];;
            Dw.y = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m]
        else
            Dw.y = zeros(length(Dw.t), 1);
        end

        if args.Dw_y
            theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
            Cx = [0 ; C.*complex(cos(theta),sin(theta))];
            Cx = [Cx; flipud(conj(Cx(2:end)))];;
            Dw.z = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m]
        else
            Dw.z = zeros(length(Dw.t), 1);
        end

    else
        Dw.t = [0,1]; % Time Vector [s]
        Dw.x = [0,0]; % Ground Motion - X [m]
        Dw.y = [0,0]; % Ground Motion - Y [m]
        Dw.z = [0,0]; % Ground Motion - Z [m]
    end

    %% Translation stage
    if args.enable
        % Load the PSD of disturbance
        load('ustation_disturbance_psd.mat', 'dy_dist')

        % Frequency Data
        Dy.f = dy_dist.f;
        Dy.psd_x = dy_dist.pxx_fx;
        Dy.psd_z = dy_dist.pxx_fz;

        % Time data
        Fs = 2*Dy.f(end);      % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
        N  = 2*length(Dy.f);   % Number of Samples match the one of the wanted PSD
        T0 = N/Fs;             % Signal Duration [s]
        Dy.t = linspace(0, T0, N+1)'; % Time Vector [s]

        % ASD representation of the disturbance voice
        C = zeros(N/2,1);
        for i = 1:N/2
            C(i) = sqrt(Dy.psd_x(i)/T0);
        end

        if args.Fdy_x
            theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
            Cx = [0 ; C.*complex(cos(theta),sin(theta))];
            Cx = [Cx; flipud(conj(Cx(2:end)))];;
            Dy.x = N/sqrt(2)*ifft(Cx); % Translation stage disturbances - X direction [N]
        else
            Dy.x = zeros(length(Dy.t), 1);
        end

        if args.Fdy_z
            theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
            Cx = [0 ; C.*complex(cos(theta),sin(theta))];
            Cx = [Cx; flipud(conj(Cx(2:end)))];;
            Dy.z = N/sqrt(2)*ifft(Cx); % Translation stage disturbances - Z direction [N]
        else
            Dy.z = zeros(length(Dy.t), 1);
        end

    else
        Dy.t = [0,1]; % Time Vector [s]
        Dy.x = [0,0]; % Translation Stage disturbances - X [N]
        Dy.z = [0,0]; % Translation Stage disturbances - Z [N]
    end

    %% Spindle
    if args.enable
        % Load the PSD of disturbance
        load('ustation_disturbance_psd.mat', 'rz_dist')

        % Frequency Data
        Rz.f = rz_dist.f;
        Rz.psd_x = rz_dist.pxx_fx;
        Rz.psd_y = rz_dist.pxx_fy;
        Rz.psd_z = rz_dist.pxx_fz;

        % Time data
        Fs = 2*Rz.f(end);      % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
        N  = 2*length(Rz.f);   % Number of Samples match the one of the wanted PSD
        T0 = N/Fs;             % Signal Duration [s]
        Rz.t = linspace(0, T0, N+1)'; % Time Vector [s]

        % ASD representation of the disturbance voice
        C = zeros(N/2,1);
        for i = 1:N/2
            C(i) = sqrt(Rz.psd_x(i)/T0);
        end

        if args.Frz_x
            theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
            Cx = [0 ; C.*complex(cos(theta),sin(theta))];
            Cx = [Cx; flipud(conj(Cx(2:end)))];;
            Rz.x = N/sqrt(2)*ifft(Cx); % spindle disturbances - X direction [N]
        else
            Rz.x = zeros(length(Rz.t), 1);
        end

        if args.Frz_y
            theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
            Cx = [0 ; C.*complex(cos(theta),sin(theta))];
            Cx = [Cx; flipud(conj(Cx(2:end)))];;
            Rz.y = N/sqrt(2)*ifft(Cx); % spindle disturbances - Y direction [N]
        else
            Rz.y = zeros(length(Rz.t), 1);
        end

        if args.Frz_z
            theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
            Cx = [0 ; C.*complex(cos(theta),sin(theta))];
            Cx = [Cx; flipud(conj(Cx(2:end)))];;
            Rz.z = N/sqrt(2)*ifft(Cx); % spindle disturbances - Z direction [N]
        else
            Rz.z = zeros(length(Rz.t), 1);
        end

    else
        Rz.t = [0,1]; % Time Vector [s]
        Rz.x = [0,0]; % Spindle disturbances - X [N]
        Rz.y = [0,0]; % Spindle disturbances - X [N]
        Rz.z = [0,0]; % Spindle disturbances - Z [N]
    end

    u = zeros(100, 6);
    Fd = u;

    Dw.x   = Dw.x   - Dw.x(1);
    Dw.y   = Dw.y   - Dw.y(1);
    Dw.z   = Dw.z   - Dw.z(1);

    Dy.x   = Dy.x   - Dy.x(1);
    Dy.z   = Dy.z   - Dy.z(1);

    Rz.x   = Rz.x   - Rz.x(1);
    Rz.y   = Rz.y   - Rz.y(1);
    Rz.z   = Rz.z   - Rz.z(1);

    if exist('./mat', 'dir')
        save('mat/nass_model_disturbances.mat', 'Dw', 'Dy', 'Rz', 'Fd', 'args');
    elseif exist('./matlab', 'dir')
        save('matlab/mat/nass_model_disturbances.mat', 'Dw', 'Dy', 'Rz', 'Fd', 'args');
    end

end
#+end_src

*** =initializeController=: Initialize Controller
#+begin_src matlab :tangle matlab/src/initializeController.m :comments none :mkdirp yes :eval no
function [] = initializeController(args)

    arguments
        args.type char {mustBeMember(args.type,{'open-loop', 'iff', 'dvf', 'hac-dvf', 'ref-track-L', 'ref-track-iff-L', 'cascade-hac-lac', 'hac-iff', 'stabilizing'})} = 'open-loop'
    end

    controller = struct();

    switch args.type
      case 'open-loop'
        controller.type = 1;
        controller.name = 'Open-Loop';
      case 'dvf'
        controller.type = 2;
        controller.name = 'Decentralized Direct Velocity Feedback';
      case 'iff'
        controller.type = 3;
        controller.name = 'Decentralized Integral Force Feedback';
      case 'hac-dvf'
        controller.type = 4;
        controller.name = 'HAC-DVF';
      case 'ref-track-L'
        controller.type = 5;
        controller.name = 'Reference Tracking in the frame of the legs';
      case 'ref-track-iff-L'
        controller.type = 6;
        controller.name = 'Reference Tracking in the frame of the legs + IFF';
      case 'cascade-hac-lac'
        controller.type = 7;
        controller.name = 'Cascade Control + HAC-LAC';
      case 'hac-iff'
        controller.type = 8;
        controller.name = 'HAC-IFF';
      case 'stabilizing'
        controller.type = 9;
        controller.name = 'Stabilizing Controller';
    end

    if exist('./mat', 'dir')
        save('mat/nass_model_controller.mat', 'controller');
    elseif exist('./matlab', 'dir')
        save('matlab/mat/nass_model_controller.mat', 'controller');
    end

end
#+end_src

*** =computeReferencePose=
#+begin_src matlab :tangle matlab/src/computeReferencePose.m :comments none :mkdirp yes :eval no
  function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
  % computeReferencePose - Compute the homogeneous transformation matrix corresponding to the wanted pose of the sample
  %
  % Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
  %
  % Inputs:
  %    - Dy - Reference of the Translation Stage [m]
  %    - Ry - Reference of the Tilt Stage [rad]
  %    - Rz - Reference of the Spindle [rad]
  %    - Dh - Reference of the Micro Hexapod (Pitch, Roll, Yaw angles) [m, m, m, rad, rad, rad]
  %    - Dn - Reference of the Nano Hexapod [m, m, m, rad, rad, rad]
  %
  % Outputs:
  %    - WTr -

    %% Translation Stage
    Rty = [1 0 0 0;
           0 1 0 Dy;
           0 0 1 0;
           0 0 0 1];

    %% Tilt Stage - Pure rotating aligned with Ob
    Rry = [ cos(Ry) 0 sin(Ry) 0;
            0       1 0       0;
           -sin(Ry) 0 cos(Ry) 0;
            0       0 0       1];

    %% Spindle - Rotation along the Z axis
    Rrz = [cos(Rz) -sin(Rz) 0 0 ;
           sin(Rz)  cos(Rz) 0 0 ;
           0        0       1 0 ;
           0        0       0 1 ];


    %% Micro-Hexapod
    Rhx = [1 0           0;
           0 cos(Dh(4)) -sin(Dh(4));
           0 sin(Dh(4))  cos(Dh(4))];

    Rhy = [ cos(Dh(5)) 0 sin(Dh(5));
           0           1 0;
           -sin(Dh(5)) 0 cos(Dh(5))];

    Rhz = [cos(Dh(6)) -sin(Dh(6)) 0;
           sin(Dh(6))  cos(Dh(6)) 0;
           0           0          1];

    Rh = [1 0 0 Dh(1) ;
          0 1 0 Dh(2) ;
          0 0 1 Dh(3) ;
          0 0 0 1 ];

    Rh(1:3, 1:3) = Rhz*Rhy*Rhx;

    %% Nano-Hexapod
    Rnx = [1 0           0;
           0 cos(Dn(4)) -sin(Dn(4));
           0 sin(Dn(4))  cos(Dn(4))];

    Rny = [ cos(Dn(5)) 0 sin(Dn(5));
           0           1 0;
           -sin(Dn(5)) 0 cos(Dn(5))];

    Rnz = [cos(Dn(6)) -sin(Dn(6)) 0;
           sin(Dn(6))  cos(Dn(6)) 0;
           0           0          1];

    Rn = [1 0 0 Dn(1) ;
          0 1 0 Dn(2) ;
          0 0 1 Dn(3) ;
          0 0 0 1 ];

    Rn(1:3, 1:3) = Rnz*Rny*Rnx;

    %% Total Homogeneous transformation
    WTr = Rty*Rry*Rrz*Rh*Rn;
  end
#+end_src

*** =extractNodes=
#+begin_src matlab :tangle matlab/src/extractNodes.m :comments none :mkdirp yes :eval no
function [int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes(filename)
% extractNodes -
%
% Syntax: [n_xyz, nodes] = extractNodes(filename)
%
% Inputs:
%    - filename - relative or absolute path of the file that contains the Matrix
%
% Outputs:
%    - n_xyz -
%    - nodes - table containing the node numbers and corresponding dof of the interfaced DoFs

arguments
    filename
end

fid = fopen(filename,'rt');

if fid == -1
    error('Error opening the file');
end

n_xyz = []; % Contains nodes coordinates
n_i = []; % Contains nodes indices

n_num = []; % Contains node numbers
n_dof = {}; % Contains node directions

while 1
    % Read a line
    nextline = fgetl(fid);

    % End of the file
    if ~isstr(nextline), break, end

    % Line just before the list of nodes coordinates
    if contains(nextline, 'NODE') && ...
            contains(nextline, 'X') && ...
            contains(nextline, 'Y') && ...
            contains(nextline, 'Z')

        while 1
            nextline = fgetl(fid);

            if nextline < 0, break, end

            c = sscanf(nextline, ' %f');

            if isempty(c), break, end

            n_xyz = [n_xyz; c(2:4)'];
            n_i = [n_i; c(1)];
        end
    end

    if nextline < 0, break, end

    % Line just before the list of node DOF
    if contains(nextline, 'NODE  LABEL')

        while 1
            nextline = fgetl(fid);

            if nextline < 0, break, end

            c = sscanf(nextline, ' %d %s');

            if isempty(c), break, end

            n_num = [n_num; c(1)];

            n_dof{length(n_dof)+1} = char(c(2:end)');
        end

        nodes = table(n_num, string(n_dof'), 'VariableNames', {'node_i', 'node_dof'});
    end

    if nextline < 0, break, end
end

fclose(fid);

int_i = unique(nodes.('node_i')); % indices of interface nodes

% Extract XYZ coordinates of only the interface nodes
if length(n_xyz) > 0 && length(n_i) > 0
    int_xyz = n_xyz(logical(sum(n_i.*ones(1, length(int_i)) == int_i', 2)), :);
else
    int_xyz = n_xyz;
end
#+end_src

** Initialize Micro-Station Stages
*** =initializeGround=: Ground
#+begin_src matlab :tangle matlab/src/initializeGround.m :comments none :mkdirp yes :eval no
function [ground] = initializeGround(args)

    arguments
        args.type char {mustBeMember(args.type,{'none', 'rigid'})} = 'rigid'
        args.rot_point (3,1) double {mustBeNumeric} = zeros(3,1) % Rotation point for the ground motion [m]
    end

    ground = struct();

    switch args.type
      case 'none'
        ground.type = 0;
      case 'rigid'
        ground.type = 1;
    end

    ground.shape   = [2, 2, 0.5]; % [m]
    ground.density = 2800;        % [kg/m3]

    ground.rot_point = args.rot_point;

    if exist('./mat', 'dir')
        if exist('./mat/nass_model_stages.mat', 'file')
            save('mat/nass_model_stages.mat', 'ground', '-append');
        else
            save('mat/nass_model_stages.mat', 'ground');
        end
    elseif exist('./matlab', 'dir')
        if exist('./matlab/mat/nass_model_stages.mat', 'file')
            save('matlab/mat/nass_model_stages.mat', 'ground', '-append');
        else
            save('matlab/mat/nass_model_stages.mat', 'ground');
        end
    end
end
#+end_src

*** =initializeGranite=: Granite
#+begin_src matlab :tangle matlab/src/initializeGranite.m :comments none :mkdirp yes :eval no
function [granite] = initializeGranite(args)

    arguments
        args.type          char    {mustBeMember(args.type,{'rigid', 'flexible', 'none'})} = 'flexible'
        args.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 % Density [kg/m3]
        args.K  (6,1) double {mustBeNumeric, mustBeNonnegative} = [5e9;   5e9;   5e9;   2.5e7; 2.5e7; 1e7] % [N/m]
        args.C  (6,1) double {mustBeNumeric, mustBeNonnegative} = [4.0e5; 1.1e5; 9.0e5; 2e4;   2e4; 1e4] % [N/(m/s)]
        args.x0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the X direction [m]
        args.y0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Y direction [m]
        args.z0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Z direction [m]
        args.sample_pos (1,1) double {mustBeNumeric} = 0.775 % Height of the measurment point [m]
    end

    granite = struct();

    switch args.type
      case 'none'
        granite.type = 0;
      case 'rigid'
        granite.type = 1;
      case 'flexible'
        granite.type = 2;
    end

    granite.density = args.density; % [kg/m3]
    granite.STEP    = 'granite.STEP';

    % Z-offset for the initial position of the sample with respect to the granite top surface.
    granite.sample_pos = args.sample_pos; % [m]

    granite.K = args.K; % [N/m]
    granite.C = args.C; % [N/(m/s)]

    if exist('./mat', 'dir')
        if exist('./mat/nass_model_stages.mat', 'file')
            save('mat/nass_model_stages.mat', 'granite', '-append');
        else
            save('mat/nass_model_stages.mat', 'granite');
        end
    elseif exist('./matlab', 'dir')
        if exist('./matlab/mat/nass_model_stages.mat', 'file')
            save('matlab/mat/nass_model_stages.mat', 'granite', '-append');
        else
            save('matlab/mat/nass_model_stages.mat', 'granite');
        end
    end

end
#+end_src

*** =initializeTy=: Translation Stage
#+begin_src matlab :tangle matlab/src/initializeTy.m :comments none :mkdirp yes :eval no
function [ty] = initializeTy(args)

    arguments
        args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
    end

    ty = struct();

    switch args.type
      case 'none'
        ty.type = 0;
      case 'rigid'
        ty.type = 1;
      case 'flexible'
        ty.type = 2;
    end

    % Ty Granite frame
    ty.granite_frame.density = 7800; % [kg/m3] => 43kg
    ty.granite_frame.STEP    = 'Ty_Granite_Frame.STEP';

    % Guide Translation Ty
    ty.guide.density = 7800; % [kg/m3] => 76kg
    ty.guide.STEP            = 'Ty_Guide.STEP';

    % Ty - Guide_Translation12
    ty.guide12.density       = 7800; % [kg/m3]
    ty.guide12.STEP          = 'Ty_Guide_12.STEP';

    % Ty - Guide_Translation11
    ty.guide11.density       = 7800; % [kg/m3]
    ty.guide11.STEP          = 'Ty_Guide_11.STEP';

    % Ty - Guide_Translation22
    ty.guide22.density       = 7800; % [kg/m3]
    ty.guide22.STEP          = 'Ty_Guide_22.STEP';

    % Ty - Guide_Translation21
    ty.guide21.density       = 7800; % [kg/m3]
    ty.guide21.STEP          = 'Ty_Guide_21.STEP';

    % Ty - Plateau translation
    ty.frame.density         = 7800; % [kg/m3]
    ty.frame.STEP            = 'Ty_Stage.STEP';

    % Ty Stator Part
    ty.stator.density        = 5400; % [kg/m3]
    ty.stator.STEP           = 'Ty_Motor_Stator.STEP';

    % Ty Rotor Part
    ty.rotor.density         = 5400; % [kg/m3]
    ty.rotor.STEP            = 'Ty_Motor_Rotor.STEP';

    ty.K = [2e8; 1e8; 2e8; 6e7; 9e7; 6e7]; % [N/m, N*m/rad]
    ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 1e4]; % [N/(m/s), N*m/(rad/s)]

    if exist('./mat', 'dir')
        if exist('./mat/nass_model_stages.mat', 'file')
            save('mat/nass_model_stages.mat', 'ty', '-append');
        else
            save('mat/nass_model_stages.mat', 'ty');
        end
    elseif exist('./matlab', 'dir')
        if exist('./matlab/mat/nass_model_stages.mat', 'file')
            save('matlab/mat/nass_model_stages.mat', 'ty', '-append');
        else
            save('matlab/mat/nass_model_stages.mat', 'ty');
        end
    end

end
#+end_src

*** =initializeRy=: Tilt Stage
#+begin_src matlab :tangle matlab/src/initializeRy.m :comments none :mkdirp yes :eval no
function [ry] = initializeRy(args)

    arguments
        args.type          char    {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
        args.Ry_init (1,1) double  {mustBeNumeric} = 0
    end

    ry = struct();

    switch args.type
      case 'none'
        ry.type = 0;
      case 'rigid'
        ry.type = 1;
      case 'flexible'
        ry.type = 2;
    end

    % Ry - Guide for the tilt stage
    ry.guide.density = 7800; % [kg/m3]
    ry.guide.STEP    = 'Tilt_Guide.STEP';

    % Ry - Rotor of the motor
    ry.rotor.density = 2400; % [kg/m3]
    ry.rotor.STEP    = 'Tilt_Motor_Axis.STEP';

    % Ry - Motor
    ry.motor.density = 3200; % [kg/m3]
    ry.motor.STEP    = 'Tilt_Motor.STEP';

    % Ry - Plateau Tilt
    ry.stage.density = 7800; % [kg/m3]
    ry.stage.STEP    = 'Tilt_Stage.STEP';

    % Z-Offset so that the center of rotation matches the sample center;
    ry.z_offset = 0.58178; % [m]

    ry.Ry_init = args.Ry_init; % [rad]

    ry.K = [3.8e8; 4e8; 3.8e8; 1.2e8; 6e4; 1.2e8];
    ry.C = [1e5;   1e5; 1e5;   3e4;   1e3; 3e4];

    if exist('./mat', 'dir')
        if exist('./mat/nass_model_stages.mat', 'file')
            save('mat/nass_model_stages.mat', 'ry', '-append');
        else
            save('mat/nass_model_stages.mat', 'ry');
        end
    elseif exist('./matlab', 'dir')
        if exist('./matlab/mat/nass_model_stages.mat', 'file')
            save('matlab/mat/nass_model_stages.mat', 'ry', '-append');
        else
            save('matlab/mat/nass_model_stages.mat', 'ry');
        end
    end

end
#+end_src

*** =initializeRz=: Spindle
#+begin_src matlab :tangle matlab/src/initializeRz.m :comments none :mkdirp yes :eval no
function [rz] = initializeRz(args)

    arguments
        args.type    char    {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
    end

    rz = struct();

    switch args.type
      case 'none'
        rz.type = 0;
      case 'rigid'
        rz.type = 1;
      case 'flexible'
        rz.type = 2;
    end

    % Spindle - Slip Ring
    rz.slipring.density = 7800; % [kg/m3]
    rz.slipring.STEP    = 'Spindle_Slip_Ring.STEP';

    % Spindle - Rotor
    rz.rotor.density    = 7800; % [kg/m3]
    rz.rotor.STEP       = 'Spindle_Rotor.STEP';

    % Spindle - Stator
    rz.stator.density   = 7800; % [kg/m3]
    rz.stator.STEP      = 'Spindle_Stator.STEP';

    rz.K = [7e8; 7e8; 2e9; 1e7; 1e7; 1e7];
    rz.C = [4e4; 4e4; 7e4; 1e4; 1e4; 1e4];

    if exist('./mat', 'dir')
        if exist('./mat/nass_model_stages.mat', 'file')
            save('mat/nass_model_stages.mat', 'rz', '-append');
        else
            save('mat/nass_model_stages.mat', 'rz');
        end
    elseif exist('./matlab', 'dir')
        if exist('./matlab/mat/nass_model_stages.mat', 'file')
            save('matlab/mat/nass_model_stages.mat', 'rz', '-append');
        else
            save('matlab/mat/nass_model_stages.mat', 'rz');
        end
    end

end
#+end_src

*** =initializeMicroHexapod=: Micro Hexapod

#+begin_src matlab :tangle matlab/src/initializeMicroHexapod.m :comments none :mkdirp yes :eval no
function [micro_hexapod] = initializeMicroHexapod(args)

    arguments
        args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
        % initializeFramesPositions
        args.H    (1,1) double {mustBeNumeric, mustBePositive} = 350e-3
        args.MO_B (1,1) double {mustBeNumeric} = 270e-3
        % generateGeneralConfiguration
        args.FH  (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
        args.FR  (1,1) double {mustBeNumeric, mustBePositive} = 175.5e-3
        args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180)
        args.MH  (1,1) double {mustBeNumeric, mustBePositive} = 45e-3
        args.MR  (1,1) double {mustBeNumeric, mustBePositive} = 118e-3
        args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180)
        % initializeStrutDynamics
        args.Ki (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e7*ones(6,1)
        args.Ci (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.4e3*ones(6,1)
        % initializeCylindricalPlatforms
        args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 10
        args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
        args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 207.5e-3
        args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 10
        args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
        args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3
        % initializeCylindricalStruts
        args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1
        args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
        args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
        args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1
        args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
        args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
        % inverseKinematics
        args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
        args.ARB (3,3) double {mustBeNumeric} = eye(3)
    end

    stewart = initializeStewartPlatform();

    stewart = initializeFramesPositions(stewart, ...
                                        'H', args.H, ...
                                        'MO_B', args.MO_B);

    stewart = generateGeneralConfiguration(stewart, ...
                                           'FH', args.FH, ...
                                           'FR', args.FR, ...
                                           'FTh', args.FTh, ...
                                           'MH', args.MH, ...
                                           'MR', args.MR, ...
                                           'MTh', args.MTh);

    stewart = computeJointsPose(stewart);

    stewart = initializeStrutDynamics(stewart, ...
                                      'K', args.Ki, ...
                                      'C', args.Ci);

    stewart = initializeJointDynamics(stewart, ...
                                      'type_F', 'universal_p', ...
                                      'type_M', 'spherical_p');

    stewart = initializeCylindricalPlatforms(stewart, ...
                                             'Fpm', args.Fpm, ...
                                             'Fph', args.Fph, ...
                                             'Fpr', args.Fpr, ...
                                             'Mpm', args.Mpm, ...
                                             'Mph', args.Mph, ...
                                             'Mpr', args.Mpr);

    stewart = initializeCylindricalStruts(stewart, ...
                                          'Fsm', args.Fsm, ...
                                          'Fsh', args.Fsh, ...
                                          'Fsr', args.Fsr, ...
                                          'Msm', args.Msm, ...
                                          'Msh', args.Msh, ...
                                          'Msr', args.Msr);

    stewart = computeJacobian(stewart);

    stewart = initializeStewartPose(stewart, ...
                                    'AP', args.AP, ...
                                    'ARB', args.ARB);

    stewart = initializeInertialSensor(stewart, 'type', 'none');

    switch args.type
      case 'none'
        stewart.type = 0;
      case 'rigid'
        stewart.type = 1;
      case 'flexible'
        stewart.type = 2;
    end

    micro_hexapod = stewart;
    if exist('./mat', 'dir')
        if exist('./mat/nass_model_stages.mat', 'file')
            save('mat/nass_model_stages.mat', 'micro_hexapod', '-append');
        else
            save('mat/nass_model_stages.mat', 'micro_hexapod');
        end
    elseif exist('./matlab', 'dir')
        if exist('./matlab/mat/nass_model_stages.mat', 'file')
            save('matlab/mat/nass_model_stages.mat', 'micro_hexapod', '-append');
        else
            save('matlab/mat/nass_model_stages.mat', 'micro_hexapod');
        end
    end
end
#+end_src

*** =initializeNanoHexapod=: Nano-Hexapod
#+begin_src matlab :tangle matlab/src/initializeNanoHexapod.m :comments none :mkdirp yes :eval no
function [nano_hexapod] = initializeNanoHexapod(args)

    arguments
        %% Bottom Flexible Joints
        args.flex_bot_type      char {mustBeMember(args.flex_bot_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof'
        args.flex_bot_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad]
        args.flex_bot_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad]
        args.flex_bot_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad]
        args.flex_bot_kz  (6,1) double {mustBeNumeric} = ones(6,1)*7e7 % Axial Stiffness [N/m]
        args.flex_bot_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % X bending Damping [Nm/(rad/s)]
        args.flex_bot_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Y bending Damping [Nm/(rad/s)]
        args.flex_bot_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Torsionnal Damping [Nm/(rad/s)]
        args.flex_bot_cz  (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Axial Damping [N/(m/s)]

        %% Top Flexible Joints
        args.flex_top_type      char {mustBeMember(args.flex_top_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof'
        args.flex_top_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad]
        args.flex_top_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad]
        args.flex_top_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad]
        args.flex_top_kz  (6,1) double {mustBeNumeric} = ones(6,1)*7e7 % Axial Stiffness [N/m]
        args.flex_top_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % X bending Damping [Nm/(rad/s)]
        args.flex_top_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Y bending Damping [Nm/(rad/s)]
        args.flex_top_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Torsionnal Damping [Nm/(rad/s)]
        args.flex_top_cz  (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Axial Damping [N/(m/s)]

        %% Jacobian - Location of frame {A} and {B}
        args.MO_B (1,1) double {mustBeNumeric} = 150e-3 % Height of {B} w.r.t. {M} [m]

        %% Relative Motion Sensor
        args.motion_sensor_type char {mustBeMember(args.motion_sensor_type,{'struts', 'plates'})} = 'struts'

        %% Top Plate
        args.top_plate_type char {mustBeMember(args.top_plate_type,{'rigid', 'flexible'})} = 'rigid'
        args.top_plate_xi  (1,1) double {mustBeNumeric} = 0.01 % Damping Ratio

        %% Actuators
        args.actuator_type char {mustBeMember(args.actuator_type,{'2dof', 'flexible frame', 'flexible'})} = 'flexible'
        args.actuator_Ga  (6,1) double {mustBeNumeric} = zeros(6,1) % Actuator gain [N/V]
        args.actuator_Gs  (6,1) double {mustBeNumeric} = zeros(6,1) % Sensor gain [V/m]
                                                                    % For 2DoF
        args.actuator_k   (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*380000
        args.actuator_ke  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*4952605
        args.actuator_ka  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*2476302
        args.actuator_c   (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*5
        args.actuator_ce  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*100
        args.actuator_ca  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*50
        args.actuator_Leq (6,1) double {mustBeNumeric} = ones(6,1)*0.056 % [m]
                                                                         % For Flexible Frame
        args.actuator_ks (6,1) double {mustBeNumeric} = ones(6,1)*235e6 % Stiffness of one stack [N/m]
        args.actuator_cs (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % Stiffness of one stack [N/m]
                                                                      % Misalignment
        args.actuator_d_align (6,3) double {mustBeNumeric} = zeros(6,3) % [m]

        args.actuator_xi  (1,1) double {mustBeNumeric} = 0.01 % Damping Ratio

        %% Controller
        args.controller_type char {mustBeMember(args.controller_type,{'none', 'iff', 'dvf', 'hac-iff-struts'})} = 'none'
    end

    nano_hexapod = struct();

    nano_hexapod.flex_bot = struct();

    switch args.flex_bot_type
      case '2dof'
        nano_hexapod.flex_bot.type = 1;
      case '3dof'
        nano_hexapod.flex_bot.type = 2;
      case '4dof'
        nano_hexapod.flex_bot.type = 3;
      case 'flexible'
        nano_hexapod.flex_bot.type = 4;
    end

    nano_hexapod.flex_bot.kRx = args.flex_bot_kRx; % X bending stiffness [Nm/rad]
    nano_hexapod.flex_bot.kRy = args.flex_bot_kRy; % Y bending stiffness [Nm/rad]
    nano_hexapod.flex_bot.kRz = args.flex_bot_kRz; % Torsionnal stiffness [Nm/rad]
    nano_hexapod.flex_bot.kz  = args.flex_bot_kz;  % Axial stiffness [N/m]

    nano_hexapod.flex_bot.cRx = args.flex_bot_cRx; % [Nm/(rad/s)]
    nano_hexapod.flex_bot.cRy = args.flex_bot_cRy; % [Nm/(rad/s)]
    nano_hexapod.flex_bot.cRz = args.flex_bot_cRz; % [Nm/(rad/s)]
    nano_hexapod.flex_bot.cz  = args.flex_bot_cz;  %[N/(m/s)]

    nano_hexapod.flex_top = struct();

    switch args.flex_top_type
      case '2dof'
        nano_hexapod.flex_top.type = 1;
      case '3dof'
        nano_hexapod.flex_top.type = 2;
      case '4dof'
        nano_hexapod.flex_top.type = 3;
      case 'flexible'
        nano_hexapod.flex_top.type = 4;
    end

    nano_hexapod.flex_top.kRx = args.flex_top_kRx; % X bending stiffness [Nm/rad]
    nano_hexapod.flex_top.kRy = args.flex_top_kRy; % Y bending stiffness [Nm/rad]
    nano_hexapod.flex_top.kRz = args.flex_top_kRz; % Torsionnal stiffness [Nm/rad]
    nano_hexapod.flex_top.kz  = args.flex_top_kz;  % Axial stiffness [N/m]

    nano_hexapod.flex_top.cRx = args.flex_top_cRx; % [Nm/(rad/s)]
    nano_hexapod.flex_top.cRy = args.flex_top_cRy; % [Nm/(rad/s)]
    nano_hexapod.flex_top.cRz = args.flex_top_cRz; % [Nm/(rad/s)]
    nano_hexapod.flex_top.cz  = args.flex_top_cz;  %[N/(m/s)]

    nano_hexapod.motion_sensor = struct();

    switch args.motion_sensor_type
      case 'struts'
        nano_hexapod.motion_sensor.type = 1;
      case 'plates'
        nano_hexapod.motion_sensor.type = 2;
    end

    nano_hexapod.actuator = struct();

    switch args.actuator_type
      case '2dof'
        nano_hexapod.actuator.type = 1;
      case 'flexible frame'
        nano_hexapod.actuator.type = 2;
      case 'flexible'
        nano_hexapod.actuator.type = 3;
    end

    %% Actuator gain [N/V]
    if all(args.actuator_Ga == 0)
        switch args.actuator_type
          case '2dof'
            nano_hexapod.actuator.Ga = ones(6,1)*(-2.5796);
          case 'flexible frame'
            nano_hexapod.actuator.Ga = ones(6,1); % TODO
          case 'flexible'
            nano_hexapod.actuator.Ga = ones(6,1)*23.2;
        end
    else
        nano_hexapod.actuator.Ga = args.actuator_Ga; % Actuator gain [N/V]
    end

    %% Sensor gain [V/m]
    if all(args.actuator_Gs == 0)
        switch args.actuator_type
          case '2dof'
            nano_hexapod.actuator.Gs = ones(6,1)*466664;
          case 'flexible frame'
            nano_hexapod.actuator.Gs = ones(6,1); % TODO
          case 'flexible'
            nano_hexapod.actuator.Gs = ones(6,1)*(-4898341);
        end
    else
        nano_hexapod.actuator.Gs = args.actuator_Gs; % Sensor gain [V/m]
    end

    switch args.actuator_type
      case '2dof'
        nano_hexapod.actuator.k  = args.actuator_k;  % [N/m]
        nano_hexapod.actuator.ke = args.actuator_ke; % [N/m]
        nano_hexapod.actuator.ka = args.actuator_ka; % [N/m]

        nano_hexapod.actuator.c  = args.actuator_c;  % [N/(m/s)]
        nano_hexapod.actuator.ce = args.actuator_ce; % [N/(m/s)]
        nano_hexapod.actuator.ca = args.actuator_ca; % [N/(m/s)]

        nano_hexapod.actuator.Leq = args.actuator_Leq; % [m]

      case 'flexible frame'
        nano_hexapod.actuator.K = readmatrix('APA300ML_b_mat_K.CSV'); % Stiffness Matrix
        nano_hexapod.actuator.M = readmatrix('APA300ML_b_mat_M.CSV'); % Mass Matrix
        nano_hexapod.actuator.P = extractNodes('APA300ML_b_out_nodes_3D.txt'); % Node coordinates [m]

        nano_hexapod.actuator.ks = args.actuator_ks; % Stiffness of one stack [N/m]
        nano_hexapod.actuator.cs = args.actuator_cs; % Damping of one stack [N/m]
        nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio

      case 'flexible'
        nano_hexapod.actuator.K = readmatrix('full_APA300ML_K.CSV'); % Stiffness Matrix
        nano_hexapod.actuator.M = readmatrix('full_APA300ML_M.CSV'); % Mass Matrix
        nano_hexapod.actuator.P = extractNodes('full_APA300ML_out_nodes_3D.txt'); % Node coordiantes [m]

        nano_hexapod.actuator.d_align = args.actuator_d_align; % Misalignment
        nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio

    end

    nano_hexapod.geometry = struct();

    Fa = [[-86.05,  -74.78, 22.49],
          [ 86.05,  -74.78, 22.49],
          [ 107.79, -37.13, 22.49],
          [ 21.74,  111.91, 22.49],
          [-21.74,  111.91, 22.49],
          [-107.79, -37.13, 22.49]]'*1e-3; % Ai w.r.t. {F} [m]

    Mb = [[-28.47, -106.25, -22.50],
          [ 28.47, -106.25, -22.50],
          [ 106.25, 28.47,  -22.50],
          [ 77.78,  77.78,  -22.50],
          [-77.78,  77.78,  -22.50],
          [-106.25, 28.47,  -22.50]]'*1e-3; % Bi w.r.t. {M} [m]

    Fb = Mb + [0; 0; 95e-3]; % Bi w.r.t. {F} [m]

    si = Fb - Fa;
    si = si./vecnorm(si); % Normalize

    Fc = [[-29.362, -105.765, 52.605]
          [ 29.362, -105.765, 52.605]
          [ 106.276, 27.454,  52.605]
          [ 76.914,  78.31,   52.605]
          [-76.914,  78.31,   52.605]
          [-106.276, 27.454,  52.605]]'*1e-3; % Meas pos w.r.t. {F}
    Mc = Fc - [0; 0; 95e-3]; % Meas pos w.r.t. {M}

    nano_hexapod.geometry.Fa = Fa;
    nano_hexapod.geometry.Fb = Fb;
    nano_hexapod.geometry.Fc = Fc;
    nano_hexapod.geometry.Mb = Mb;
    nano_hexapod.geometry.Mc = Mc;
    nano_hexapod.geometry.si = si;
    nano_hexapod.geometry.MO_B = args.MO_B;

    Bb = Mb - [0; 0; args.MO_B];

    nano_hexapod.geometry.J = [nano_hexapod.geometry.si', cross(Bb, nano_hexapod.geometry.si)'];

    switch args.motion_sensor_type
      case 'struts'
        nano_hexapod.geometry.Js = nano_hexapod.geometry.J;
      case 'plates'
        Bc = Mc - [0; 0; args.MO_B];
        nano_hexapod.geometry.Js = [nano_hexapod.geometry.si', cross(Bc, nano_hexapod.geometry.si)'];
    end

    nano_hexapod.top_plate = struct();

    switch args.top_plate_type
      case 'rigid'
        nano_hexapod.top_plate.type = 1;
      case 'flexible'
        nano_hexapod.top_plate.type = 2;

        nano_hexapod.top_plate.R_flex = ...
            {[ 0.53191886726305  0.4795690716524  0.69790817745892
               -0.29070157897799  0.8775041341865 -0.38141720787774
               -0.79533320729697  0                0.60617249143351 ],
             [ 0.53191886726305 -0.4795690716524 -0.69790817745892
               0.29070157897799  0.8775041341865 -0.38141720787774
               0.79533320729697  0                0.60617249143351 ],
             [-0.01420448131633 -0.9997254079576 -0.01863709726680
              0.60600604129104 -0.0234330681729  0.79511481512719
              -0.79533320729697  0                0.60617249143351 ],
             [-0.51771438594672 -0.5201563363051  0.67927108019212
              0.31530446231304 -0.8540710660135 -0.41369760724945
              0.79533320729697  0                0.60617249143351 ],
             [-0.51771438594671  0.5201563363052 -0.67927108019211
              -0.31530446231304 -0.8540710660135 -0.41369760724945
              -0.79533320729697  0                0.60617249143351 ],
             [-0.01420448131632  0.9997254079576  0.01863709726679
              -0.60600604129104 -0.0234330681729  0.79511481512719
              0.79533320729697  0                0.60617249143351 ] };


        nano_hexapod.top_plate.R_enc = ...
            { [-0.877504134186525 -0.479569071652412 0
               0.479569071652412 -0.877504134186525 0
               0                  0 1 ],
              [ 0.877504134186525 -0.479569071652413 0
                0.479569071652413  0.877504134186525 0
                0                  0 1 ],
              [ 0.023433068172945  0.999725407957606 0
                -0.999725407957606  0.023433068172945 0
                0                  0 1 ],
              [-0.854071066013566 -0.520156336305202 0
               0.520156336305202 -0.854071066013566 0
               0                  0 1 ],
              [ 0.854071066013574 -0.520156336305191 0
                0.520156336305191  0.854071066013574 0
                0                  0 1 ],
              [-0.023433068172958  0.999725407957606 0
               -0.999725407957606 -0.023433068172958 0
               0                  0 1 ] };

        nano_hexapod.top_plate.K = readmatrix('top_plate_K_6.CSV'); % Stiffness Matrix
        nano_hexapod.top_plate.M = readmatrix('top_plate_M_6.CSV'); % Mass Matrix
        nano_hexapod.top_plate.P = extractNodes('top_plate_out_nodes_3D_qua.txt'); % Node coordiantes [m]
        nano_hexapod.top_plate.xi = args.top_plate_xi; % Damping ratio

    end

    if exist('./mat', 'dir')
        if exist('./mat/nass_model_stages.mat', 'file')
            save('mat/nass_model_stages.mat', 'nano_hexapod', '-append');
        else
            save('mat/nass_model_stages.mat', 'nano_hexapod');
        end
    elseif exist('./matlab', 'dir')
        if exist('./matlab/mat/nass_model_stages.mat', 'file')
            save('matlab/mat/nass_model_stages.mat', 'nano_hexapod', '-append');
        else
            save('matlab/mat/nass_model_stages.mat', 'nano_hexapod');
        end
    end
end
#+end_src

*** =initializeSample=: Sample

#+begin_src matlab :tangle matlab/src/initializeSample.m :comments none :mkdirp yes :eval no
function [sample] = initializeSample(args)

    arguments
        args.type         char    {mustBeMember(args.type,{'0', '1', '2', '3'})} = '0'
    end

    sample = struct();

    switch args.type
      case '0'
        sample.type = 0;
      case '1'
        sample.type = 1;
      case '2'
        sample.type = 2;
      case '3'
        sample.type = 3;
    end

    if exist('./mat', 'dir')
        if exist('./mat/nass_model_stages.mat', 'file')
            save('mat/nass_model_stages.mat', 'sample', '-append');
        else
            save('mat/nass_model_stages.mat', 'sample');
        end
    elseif exist('./matlab', 'dir')
        if exist('./matlab/mat/nass_model_stages.mat', 'file')
            save('matlab/mat/nass_model_stages.mat', 'sample', '-append');
        else
            save('matlab/mat/nass_model_stages.mat', 'sample');
        end
    end

end
#+end_src

** Stewart platform
*** =initializeStewartPlatform=: Initialize the Stewart Platform structure
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeStewartPlatform.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:

**** Documentation

#+name: fig:stewart-frames-position
#+caption: Definition of the position of the frames
[[file:figs/stewart-frames-position.png]]

**** Function description
#+begin_src matlab
  function [stewart] = initializeStewartPlatform()
  % initializeStewartPlatform - Initialize the stewart structure
  %
  % Syntax: [stewart] = initializeStewartPlatform(args)
  %
  % Outputs:
  %    - stewart - A structure with the following sub-structures:
  %      - platform_F -
  %      - platform_M -
  %      - joints_F   -
  %      - joints_M   -
  %      - struts_F   -
  %      - struts_M   -
  %      - actuators  -
  %      - geometry   -
  %      - properties -
#+end_src

**** Initialize the Stewart structure
#+begin_src matlab
  stewart = struct();
  stewart.platform_F = struct();
  stewart.platform_M = struct();
  stewart.joints_F   = struct();
  stewart.joints_M   = struct();
  stewart.struts_F   = struct();
  stewart.struts_M   = struct();
  stewart.actuators  = struct();
  stewart.sensors    = struct();
  stewart.sensors.inertial = struct();
  stewart.sensors.force    = struct();
  stewart.sensors.relative = struct();
  stewart.geometry   = struct();
  stewart.kinematics = struct();
#+end_src

*** =initializeFramesPositions=: Initialize the positions of frames {A}, {B}, {F} and {M}
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeFramesPositions.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:

**** Documentation

#+name: fig:stewart-frames-position
#+caption: Definition of the position of the frames
[[file:figs/stewart-frames-position.png]]

**** Function description
#+begin_src matlab
  function [stewart] = initializeFramesPositions(stewart, args)
  % initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
  %
  % Syntax: [stewart] = initializeFramesPositions(stewart, args)
  %
  % Inputs:
  %    - args - Can have the following fields:
  %        - H    [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m]
  %        - MO_B [1x1] - Height of the frame {B} with respect to {M} [m]
  %
  % Outputs:
  %    - stewart - A structure with the following fields:
  %        - geometry.H      [1x1] - Total Height of the Stewart Platform [m]
  %        - geometry.FO_M   [3x1] - Position of {M} with respect to {F} [m]
  %        - platform_M.MO_B [3x1] - Position of {B} with respect to {M} [m]
  %        - platform_F.FO_A [3x1] - Position of {A} with respect to {F} [m]
#+end_src

**** Optional Parameters
#+begin_src matlab
  arguments
      stewart
      args.H    (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
      args.MO_B (1,1) double {mustBeNumeric} = 50e-3
  end
#+end_src

**** Compute the position of each frame
#+begin_src matlab
  H = args.H; % Total Height of the Stewart Platform [m]

  FO_M = [0; 0; H]; % Position of {M} with respect to {F} [m]

  MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m]

  FO_A = MO_B + FO_M; % Position of {A} with respect to {F} [m]
#+end_src

**** Populate the =stewart= structure
#+begin_src matlab
  stewart.geometry.H      = H;
  stewart.geometry.FO_M   = FO_M;
  stewart.platform_M.MO_B = MO_B;
  stewart.platform_F.FO_A = FO_A;
#+end_src

*** =generateGeneralConfiguration=: Generate a Very General Configuration
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/generateGeneralConfiguration.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:

**** Documentation

#+begin_src latex :file stewart_bottom_plate.pdf :tangle no
  \begin{tikzpicture}
    % Internal and external limit
    \draw[fill=white!80!black] (0, 0) circle [radius=3];
    % Circle where the joints are located
    \draw[dashed] (0, 0) circle [radius=2.5];

    % Bullets for the positions of the joints
    \node[] (J1) at ( 80:2.5){$\bullet$};
    \node[] (J2) at (100:2.5){$\bullet$};
    \node[] (J3) at (200:2.5){$\bullet$};
    \node[] (J4) at (220:2.5){$\bullet$};
    \node[] (J5) at (320:2.5){$\bullet$};
    \node[] (J6) at (340:2.5){$\bullet$};

    % Name of the points
    \node[above right] at (J1) {$a_{1}$};
    \node[above left]  at (J2) {$a_{2}$};
    \node[above left]  at (J3) {$a_{3}$};
    \node[right     ]  at (J4) {$a_{4}$};
    \node[left      ]  at (J5) {$a_{5}$};
    \node[above right] at (J6) {$a_{6}$};

    % First 2 angles
    \draw[dashed, ->] (0:1)   arc [start angle=0, end angle=80, radius=1]    node[below right]{$\theta_{1}$};
    \draw[dashed, ->] (0:1.5) arc [start angle=0, end angle=100, radius=1.5] node[left       ]{$\theta_{2}$};

    % Division of 360 degrees by 3
    \draw[dashed] (0, 0) -- ( 80:3.2);
    \draw[dashed] (0, 0) -- (100:3.2);
    \draw[dashed] (0, 0) -- (200:3.2);
    \draw[dashed] (0, 0) -- (220:3.2);
    \draw[dashed] (0, 0) -- (320:3.2);
    \draw[dashed] (0, 0) -- (340:3.2);

    % Radius for the position of the joints
    \draw[<->] (0, 0) --node[near end, above]{$R$} (180:2.5);

    \draw[->] (0, 0) -- ++(3.4, 0) node[above]{$x$};
    \draw[->] (0, 0) -- ++(0, 3.4) node[left]{$y$};
  \end{tikzpicture}
#+end_src

**** Function description
#+begin_src matlab
  function [stewart] = generateGeneralConfiguration(stewart, args)
  % generateGeneralConfiguration - Generate a Very General Configuration
  %
  % Syntax: [stewart] = generateGeneralConfiguration(stewart, args)
  %
  % Inputs:
  %    - args - Can have the following fields:
  %        - FH  [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m]
  %        - FR  [1x1] - Radius of the position of the fixed joints in the X-Y [m]
  %        - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad]
  %        - MH  [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m]
  %        - FR  [1x1] - Radius of the position of the mobile joints in the X-Y [m]
  %        - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad]
  %
  % Outputs:
  %    - stewart - updated Stewart structure with the added fields:
  %        - platform_F.Fa  [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
  %        - platform_M.Mb  [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
#+end_src

**** Optional Parameters
#+begin_src matlab
  arguments
      stewart
      args.FH  (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
      args.FR  (1,1) double {mustBeNumeric, mustBePositive} = 115e-3;
      args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180);
      args.MH  (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
      args.MR  (1,1) double {mustBeNumeric, mustBePositive} = 90e-3;
      args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
  end
#+end_src

**** Compute the pose
#+begin_src matlab
  Fa = zeros(3,6);
  Mb = zeros(3,6);
#+end_src

#+begin_src matlab
  for i = 1:6
    Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i));  args.FH];
    Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH];
  end
#+end_src

**** Populate the =stewart= structure
#+begin_src matlab
  stewart.platform_F.Fa = Fa;
  stewart.platform_M.Mb = Mb;
#+end_src

*** =computeJointsPose=: Compute the Pose of the Joints
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/computeJointsPose.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:

**** Documentation

#+name: fig:stewart-struts
#+caption: Position and orientation of the struts
[[file:figs/stewart-struts.png]]

**** Function description
#+begin_src matlab
  function [stewart] = computeJointsPose(stewart)
  % computeJointsPose -
  %
  % Syntax: [stewart] = computeJointsPose(stewart)
  %
  % Inputs:
  %    - stewart - A structure with the following fields
  %        - platform_F.Fa   [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
  %        - platform_M.Mb   [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
  %        - platform_F.FO_A [3x1] - Position of {A} with respect to {F}
  %        - platform_M.MO_B [3x1] - Position of {B} with respect to {M}
  %        - geometry.FO_M   [3x1] - Position of {M} with respect to {F}
  %
  % Outputs:
  %    - stewart - A structure with the following added fields
  %        - geometry.Aa    [3x6]   - The i'th column is the position of ai with respect to {A}
  %        - geometry.Ab    [3x6]   - The i'th column is the position of bi with respect to {A}
  %        - geometry.Ba    [3x6]   - The i'th column is the position of ai with respect to {B}
  %        - geometry.Bb    [3x6]   - The i'th column is the position of bi with respect to {B}
  %        - geometry.l     [6x1]   - The i'th element is the initial length of strut i
  %        - geometry.As    [3x6]   - The i'th column is the unit vector of strut i expressed in {A}
  %        - geometry.Bs    [3x6]   - The i'th column is the unit vector of strut i expressed in {B}
  %        - struts_F.l     [6x1]   - Length of the Fixed part of the i'th strut
  %        - struts_M.l     [6x1]   - Length of the Mobile part of the i'th strut
  %        - platform_F.FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}
  %        - platform_M.MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
#+end_src

**** Check the =stewart= structure elements
#+begin_src matlab
  assert(isfield(stewart.platform_F, 'Fa'),   'stewart.platform_F should have attribute Fa')
  Fa = stewart.platform_F.Fa;

  assert(isfield(stewart.platform_M, 'Mb'),   'stewart.platform_M should have attribute Mb')
  Mb = stewart.platform_M.Mb;

  assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A')
  FO_A = stewart.platform_F.FO_A;

  assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B')
  MO_B = stewart.platform_M.MO_B;

  assert(isfield(stewart.geometry,   'FO_M'), 'stewart.geometry should have attribute FO_M')
  FO_M = stewart.geometry.FO_M;
#+end_src

**** Compute the position of the Joints
#+begin_src matlab
  Aa = Fa - repmat(FO_A, [1, 6]);
  Bb = Mb - repmat(MO_B, [1, 6]);

  Ab = Bb - repmat(-MO_B-FO_M+FO_A, [1, 6]);
  Ba = Aa - repmat( MO_B+FO_M-FO_A, [1, 6]);
#+end_src

**** Compute the strut length and orientation
#+begin_src matlab
  As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As

  l = vecnorm(Ab - Aa)';
#+end_src

#+begin_src matlab
  Bs = (Bb - Ba)./vecnorm(Bb - Ba);
#+end_src

**** Compute the orientation of the Joints
#+begin_src matlab
  FRa = zeros(3,3,6);
  MRb = zeros(3,3,6);

  for i = 1:6
    FRa(:,:,i) = [cross([0;1;0], As(:,i)) , cross(As(:,i), cross([0;1;0], As(:,i))) , As(:,i)];
    FRa(:,:,i) = FRa(:,:,i)./vecnorm(FRa(:,:,i));

    MRb(:,:,i) = [cross([0;1;0], Bs(:,i)) , cross(Bs(:,i), cross([0;1;0], Bs(:,i))) , Bs(:,i)];
    MRb(:,:,i) = MRb(:,:,i)./vecnorm(MRb(:,:,i));
  end
#+end_src

**** Populate the =stewart= structure
#+begin_src matlab
  stewart.geometry.Aa = Aa;
  stewart.geometry.Ab = Ab;
  stewart.geometry.Ba = Ba;
  stewart.geometry.Bb = Bb;
  stewart.geometry.As = As;
  stewart.geometry.Bs = Bs;
  stewart.geometry.l  = l;

  stewart.struts_F.l  = l/2;
  stewart.struts_M.l  = l/2;

  stewart.platform_F.FRa = FRa;
  stewart.platform_M.MRb = MRb;
#+end_src

*** =initializeCylindricalPlatforms=: Initialize the geometry of the Fixed and Mobile Platforms
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeCylindricalPlatforms.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:

**** Function description
#+begin_src matlab
  function [stewart] = initializeCylindricalPlatforms(stewart, args)
  % initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms
  %
  % Syntax: [stewart] = initializeCylindricalPlatforms(args)
  %
  % Inputs:
  %    - args - Structure with the following fields:
  %        - Fpm [1x1] - Fixed Platform Mass [kg]
  %        - Fph [1x1] - Fixed Platform Height [m]
  %        - Fpr [1x1] - Fixed Platform Radius [m]
  %        - Mpm [1x1] - Mobile Platform Mass [kg]
  %        - Mph [1x1] - Mobile Platform Height [m]
  %        - Mpr [1x1] - Mobile Platform Radius [m]
  %
  % Outputs:
  %    - stewart - updated Stewart structure with the added fields:
  %      - platform_F [struct] - structure with the following fields:
  %        - type = 1
  %        - M [1x1] - Fixed Platform Mass [kg]
  %        - I [3x3] - Fixed Platform Inertia matrix [kg*m^2]
  %        - H [1x1] - Fixed Platform Height [m]
  %        - R [1x1] - Fixed Platform Radius [m]
  %      - platform_M [struct] - structure with the following fields:
  %        - M [1x1] - Mobile Platform Mass [kg]
  %        - I [3x3] - Mobile Platform Inertia matrix [kg*m^2]
  %        - H [1x1] - Mobile Platform Height [m]
  %        - R [1x1] - Mobile Platform Radius [m]
#+end_src

**** Optional Parameters
#+begin_src matlab
  arguments
      stewart
      args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
      args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
      args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e-3
      args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
      args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
      args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
  end
#+end_src

**** Compute the Inertia matrices of platforms
#+begin_src matlab
  I_F = diag([1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
              1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
              1/2 *args.Fpm * args.Fpr^2]);
#+end_src

#+begin_src matlab
  I_M = diag([1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
              1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
              1/2 *args.Mpm * args.Mpr^2]);
#+end_src

**** Populate the =stewart= structure
#+begin_src matlab
  stewart.platform_F.type = 1;

  stewart.platform_F.I = I_F;
  stewart.platform_F.M = args.Fpm;
  stewart.platform_F.R = args.Fpr;
  stewart.platform_F.H = args.Fph;
#+end_src

#+begin_src matlab
  stewart.platform_M.type = 1;

  stewart.platform_M.I = I_M;
  stewart.platform_M.M = args.Mpm;
  stewart.platform_M.R = args.Mpr;
  stewart.platform_M.H = args.Mph;
#+end_src

*** =initializeCylindricalStruts=: Define the inertia of cylindrical struts
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeCylindricalStruts.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:

**** Function description
#+begin_src matlab
  function [stewart] = initializeCylindricalStruts(stewart, args)
  % initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts
  %
  % Syntax: [stewart] = initializeCylindricalStruts(args)
  %
  % Inputs:
  %    - args - Structure with the following fields:
  %        - Fsm [1x1] - Mass of the Fixed part of the struts [kg]
  %        - Fsh [1x1] - Height of cylinder for the Fixed part of the struts [m]
  %        - Fsr [1x1] - Radius of cylinder for the Fixed part of the struts [m]
  %        - Msm [1x1] - Mass of the Mobile part of the struts [kg]
  %        - Msh [1x1] - Height of cylinder for the Mobile part of the struts [m]
  %        - Msr [1x1] - Radius of cylinder for the Mobile part of the struts [m]
  %
  % Outputs:
  %    - stewart - updated Stewart structure with the added fields:
  %      - struts_F [struct] - structure with the following fields:
  %        - M [6x1]   - Mass of the Fixed part of the struts [kg]
  %        - I [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2]
  %        - H [6x1]   - Height of cylinder for the Fixed part of the struts [m]
  %        - R [6x1]   - Radius of cylinder for the Fixed part of the struts [m]
  %      - struts_M [struct] - structure with the following fields:
  %        - M [6x1]   - Mass of the Mobile part of the struts [kg]
  %        - I [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2]
  %        - H [6x1]   - Height of cylinder for the Mobile part of the struts [m]
  %        - R [6x1]   - Radius of cylinder for the Mobile part of the struts [m]
#+end_src

**** Optional Parameters
#+begin_src matlab
  arguments
      stewart
      args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
      args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
      args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
      args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
      args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
      args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
  end
#+end_src

**** Compute the properties of the cylindrical struts

#+begin_src matlab
  Fsm = ones(6,1).*args.Fsm;
  Fsh = ones(6,1).*args.Fsh;
  Fsr = ones(6,1).*args.Fsr;

  Msm = ones(6,1).*args.Msm;
  Msh = ones(6,1).*args.Msh;
  Msr = ones(6,1).*args.Msr;
#+end_src

#+begin_src matlab
  I_F = zeros(3, 3, 6); % Inertia of the "fixed" part of the strut
  I_M = zeros(3, 3, 6); % Inertia of the "mobile" part of the strut

  for i = 1:6
    I_F(:,:,i) = diag([1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
                       1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
                       1/2  * Fsm(i) * Fsr(i)^2]);

    I_M(:,:,i) = diag([1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
                       1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
                       1/2  * Msm(i) * Msr(i)^2]);
  end
#+end_src

**** Populate the =stewart= structure
#+begin_src matlab
  stewart.struts_M.type = 1;

  stewart.struts_M.I = I_M;
  stewart.struts_M.M = Msm;
  stewart.struts_M.R = Msr;
  stewart.struts_M.H = Msh;
#+end_src

#+begin_src matlab
  stewart.struts_F.type = 1;

  stewart.struts_F.I = I_F;
  stewart.struts_F.M = Fsm;
  stewart.struts_F.R = Fsr;
  stewart.struts_F.H = Fsh;
#+end_src

*** =initializeStrutDynamics=: Add Stiffness and Damping properties of each strut
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeStrutDynamics.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:

**** Documentation

#+name: fig:piezoelectric_stack
#+attr_html: :width 500px
#+caption: Example of a piezoelectric stach actuator (PI)
[[file:figs/piezoelectric_stack.jpg]]

A simplistic model of such amplified actuator is shown in Figure ref:fig:actuator_model_simple where:
- $K$ represent the vertical stiffness of the actuator
- $C$ represent the vertical damping of the actuator
- $F$ represents the force applied by the actuator
- $F_{m}$ represents the total measured force
- $v_{m}$ represents the absolute velocity of the top part of the actuator
- $d_{m}$ represents the total relative displacement of the actuator

#+begin_src latex :file actuator_model_simple.pdf :tangle no
  \begin{tikzpicture}
    \draw (-1, 0) -- (1, 0);

    % Spring, Damper, and Actuator
    \draw[spring]   (-1, 0) -- (-1, 1.5) node[midway, left=0.1]{$K$};
    \draw[damper]   ( 0, 0) -- ( 0, 1.5) node[midway, left=0.2]{$C$};
    \draw[actuator] ( 1, 0) -- ( 1, 1.5) node[midway, left=0.1](F){$F$};

    \node[forcesensor={2}{0.2}] (fsens) at (0, 1.5){};

    \node[left] at (fsens.west) {$F_{m}$};

    \draw[dashed] (1, 0) -- ++(0.4, 0);
    \draw[dashed] (1, 1.7) -- ++(0.4, 0);

    \draw[->] (0, 1.7)node[]{$\bullet$} -- ++(0, 0.5) node[right]{$v_{m}$};

    \draw[<->] (1.4, 0) -- ++(0, 1.7) node[midway, right]{$d_{m}$};
  \end{tikzpicture}
#+end_src

#+name: fig:actuator_model_simple
#+caption: Simple model of an Actuator
#+RESULTS:
[[file:figs/actuator_model_simple.png]]

**** Function description
#+begin_src matlab
  function [stewart] = initializeStrutDynamics(stewart, args)
  % initializeStrutDynamics - Add Stiffness and Damping properties of each strut
  %
  % Syntax: [stewart] = initializeStrutDynamics(args)
  %
  % Inputs:
  %    - args - Structure with the following fields:
  %        - K [6x1] - Stiffness of each strut [N/m]
  %        - C [6x1] - Damping of each strut [N/(m/s)]
  %
  % Outputs:
  %    - stewart - updated Stewart structure with the added fields:
  %      - actuators.type = 1
  %      - actuators.K [6x1] - Stiffness of each strut [N/m]
  %      - actuators.C [6x1] - Damping of each strut [N/(m/s)]
#+end_src

**** Optional Parameters
#+begin_src matlab
  arguments
      stewart
      args.type char {mustBeMember(args.type,{'classical', 'amplified'})} = 'classical'
      args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = 20e6*ones(6,1)
      args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e1*ones(6,1)
      args.k1 (6,1) double {mustBeNumeric} = 1e6*ones(6,1)
      args.ke (6,1) double {mustBeNumeric} = 5e6*ones(6,1)
      args.ka (6,1) double {mustBeNumeric} = 60e6*ones(6,1)
      args.c1 (6,1) double {mustBeNumeric} = 10*ones(6,1)
      args.F_gain (6,1) double {mustBeNumeric} = 1*ones(6,1)
      args.me (6,1) double {mustBeNumeric} = 0.01*ones(6,1)
      args.ma (6,1) double {mustBeNumeric} = 0.01*ones(6,1)
  end
#+end_src

**** Add Stiffness and Damping properties of each strut
#+begin_src matlab
  if strcmp(args.type, 'classical')
      stewart.actuators.type = 1;
  elseif strcmp(args.type, 'amplified')
      stewart.actuators.type = 2;
  end

  stewart.actuators.K = args.K;
  stewart.actuators.C = args.C;

  stewart.actuators.k1 = args.k1;
  stewart.actuators.c1 = args.c1;

  stewart.actuators.ka = args.ka;
  stewart.actuators.ke = args.ke;

  stewart.actuators.F_gain = args.F_gain;

  stewart.actuators.ma = args.ma;
  stewart.actuators.me = args.me;
#+end_src

*** =initializeJointDynamics=: Add Stiffness and Damping properties for spherical joints
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeJointDynamics.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:

**** Function description
#+begin_src matlab
  function [stewart] = initializeJointDynamics(stewart, args)
  % initializeJointDynamics - Add Stiffness and Damping properties for the spherical joints
  %
  % Syntax: [stewart] = initializeJointDynamics(args)
  %
  % Inputs:
  %    - args - Structure with the following fields:
  %        - type_F - 'universal', 'spherical', 'universal_p', 'spherical_p'
  %        - type_M - 'universal', 'spherical', 'universal_p', 'spherical_p'
  %        - Kf_M [6x1] - Bending (Rx, Ry) Stiffness for each top joints [(N.m)/rad]
  %        - Kt_M [6x1] - Torsion (Rz) Stiffness for each top joints [(N.m)/rad]
  %        - Cf_M [6x1] - Bending (Rx, Ry) Damping of each top joint [(N.m)/(rad/s)]
  %        - Ct_M [6x1] - Torsion (Rz) Damping of each top joint [(N.m)/(rad/s)]
  %        - Kf_F [6x1] - Bending (Rx, Ry) Stiffness for each bottom joints [(N.m)/rad]
  %        - Kt_F [6x1] - Torsion (Rz) Stiffness for each bottom joints [(N.m)/rad]
  %        - Cf_F [6x1] - Bending (Rx, Ry) Damping of each bottom joint [(N.m)/(rad/s)]
  %        - Cf_F [6x1] - Torsion (Rz) Damping of each bottom joint [(N.m)/(rad/s)]
  %
  % Outputs:
  %    - stewart - updated Stewart structure with the added fields:
  %      - stewart.joints_F and stewart.joints_M:
  %        - type - 1 (universal), 2 (spherical), 3 (universal perfect), 4 (spherical perfect)
  %        - Kx, Ky, Kz [6x1] - Translation (Tx, Ty, Tz) Stiffness [N/m]
  %        - Kf [6x1] - Flexion (Rx, Ry) Stiffness [(N.m)/rad]
  %        - Kt [6x1] - Torsion (Rz) Stiffness [(N.m)/rad]
  %        - Cx, Cy, Cz [6x1] - Translation (Rx, Ry) Damping [N/(m/s)]
  %        - Cf [6x1] - Flexion (Rx, Ry) Damping [(N.m)/(rad/s)]
  %        - Cb [6x1] - Torsion (Rz) Damping [(N.m)/(rad/s)]
#+end_src

**** Optional Parameters
#+begin_src matlab
  arguments
      stewart
      args.type_F     char   {mustBeMember(args.type_F,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'universal'
      args.type_M     char   {mustBeMember(args.type_M,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'spherical'
      args.Kf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1)
      args.Cf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
      args.Kt_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1)
      args.Ct_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
      args.Kf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1)
      args.Cf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
      args.Kt_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1)
      args.Ct_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
      args.Ka_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1)
      args.Ca_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
      args.Kr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1)
      args.Cr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
      args.Ka_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1)
      args.Ca_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
      args.Kr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1)
      args.Cr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
      args.K_M        double {mustBeNumeric} = zeros(6,6)
      args.M_M        double {mustBeNumeric} = zeros(6,6)
      args.n_xyz_M    double {mustBeNumeric} = zeros(2,3)
      args.xi_M       double {mustBeNumeric} = 0.1
      args.step_file_M char {} = ''
      args.K_F        double {mustBeNumeric} = zeros(6,6)
      args.M_F        double {mustBeNumeric} = zeros(6,6)
      args.n_xyz_F    double {mustBeNumeric} = zeros(2,3)
      args.xi_F       double {mustBeNumeric} = 0.1
      args.step_file_F char {} = ''
  end
#+end_src

**** Add Actuator Type
#+begin_src matlab
  switch args.type_F
    case 'universal'
      stewart.joints_F.type = 1;
    case 'spherical'
      stewart.joints_F.type = 2;
    case 'universal_p'
      stewart.joints_F.type = 3;
    case 'spherical_p'
      stewart.joints_F.type = 4;
    case 'flexible'
      stewart.joints_F.type = 5;
    case 'universal_3dof'
      stewart.joints_F.type = 6;
    case 'spherical_3dof'
      stewart.joints_F.type = 7;
  end

  switch args.type_M
    case 'universal'
      stewart.joints_M.type = 1;
    case 'spherical'
      stewart.joints_M.type = 2;
    case 'universal_p'
      stewart.joints_M.type = 3;
    case 'spherical_p'
      stewart.joints_M.type = 4;
    case 'flexible'
      stewart.joints_M.type = 5;
    case 'universal_3dof'
      stewart.joints_M.type = 6;
    case 'spherical_3dof'
      stewart.joints_M.type = 7;
  end
#+end_src

**** Add Stiffness and Damping in Translation of each strut
Axial and Radial (shear) Stiffness
#+begin_src matlab
  stewart.joints_M.Ka = args.Ka_M;
  stewart.joints_M.Kr = args.Kr_M;

  stewart.joints_F.Ka = args.Ka_F;
  stewart.joints_F.Kr = args.Kr_F;
#+end_src

Translation Damping
#+begin_src matlab
  stewart.joints_M.Ca = args.Ca_M;
  stewart.joints_M.Cr = args.Cr_M;

  stewart.joints_F.Ca = args.Ca_F;
  stewart.joints_F.Cr = args.Cr_F;
#+end_src

**** Add Stiffness and Damping in Rotation of each strut
Rotational Stiffness
#+begin_src matlab
  stewart.joints_M.Kf = args.Kf_M;
  stewart.joints_M.Kt = args.Kt_M;

  stewart.joints_F.Kf = args.Kf_F;
  stewart.joints_F.Kt = args.Kt_F;
#+end_src

Rotational Damping
#+begin_src matlab
  stewart.joints_M.Cf = args.Cf_M;
  stewart.joints_M.Ct = args.Ct_M;

  stewart.joints_F.Cf = args.Cf_F;
  stewart.joints_F.Ct = args.Ct_F;
#+end_src

**** Stiffness and Mass matrices for flexible joint

#+begin_src matlab
  stewart.joints_F.M = args.M_F;
  stewart.joints_F.K = args.K_F;
  stewart.joints_F.n_xyz = args.n_xyz_F;
  stewart.joints_F.xi = args.xi_F;
  stewart.joints_F.xi = args.xi_F;
  stewart.joints_F.step_file = args.step_file_F;

  stewart.joints_M.M = args.M_M;
  stewart.joints_M.K = args.K_M;
  stewart.joints_M.n_xyz = args.n_xyz_M;
  stewart.joints_M.xi = args.xi_M;
  stewart.joints_M.step_file = args.step_file_M;
#+end_src

*** =initializeStewartPose=: Determine the initial stroke in each leg to have the wanted pose
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeStewartPose.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:

**** Function description
#+begin_src matlab
  function [stewart] = initializeStewartPose(stewart, args)
  % initializeStewartPose - Determine the initial stroke in each leg to have the wanted pose
  %                         It uses the inverse kinematic
  %
  % Syntax: [stewart] = initializeStewartPose(stewart, args)
  %
  % Inputs:
  %    - stewart - A structure with the following fields
  %        - Aa   [3x6] - The positions ai expressed in {A}
  %        - Bb   [3x6] - The positions bi expressed in {B}
  %    - args - Can have the following fields:
  %        - AP   [3x1] - The wanted position of {B} with respect to {A}
  %        - ARB  [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
  %
  % Outputs:
  %    - stewart - updated Stewart structure with the added fields:
  %      - actuators.Leq [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
#+end_src

**** Optional Parameters
#+begin_src matlab
  arguments
      stewart
      args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
      args.ARB (3,3) double {mustBeNumeric} = eye(3)
  end
#+end_src

**** Use the Inverse Kinematic function
#+begin_src matlab
  [Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);
#+end_src

**** Populate the =stewart= structure
#+begin_src matlab
  stewart.actuators.Leq = dLi;
#+end_src

*** =computeJacobian=: Compute the Jacobian Matrix
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/computeJacobian.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:

**** Function description
#+begin_src matlab
  function [stewart] = computeJacobian(stewart)
  % computeJacobian -
  %
  % Syntax: [stewart] = computeJacobian(stewart)
  %
  % Inputs:
  %    - stewart - With at least the following fields:
  %      - geometry.As [3x6] - The 6 unit vectors for each strut expressed in {A}
  %      - geometry.Ab [3x6] - The 6 position of the joints bi expressed in {A}
  %      - actuators.K [6x1] - Total stiffness of the actuators
  %
  % Outputs:
  %    - stewart - With the 3 added field:
  %        - kinematics.J [6x6] - The Jacobian Matrix
  %        - kinematics.K [6x6] - The Stiffness Matrix
  %        - kinematics.C [6x6] - The Compliance Matrix
#+end_src

**** Check the =stewart= structure elements
#+begin_src matlab
  assert(isfield(stewart.geometry, 'As'),   'stewart.geometry should have attribute As')
  As = stewart.geometry.As;

  assert(isfield(stewart.geometry, 'Ab'),   'stewart.geometry should have attribute Ab')
  Ab = stewart.geometry.Ab;

  assert(isfield(stewart.actuators, 'K'),   'stewart.actuators should have attribute K')
  Ki = stewart.actuators.K;
#+end_src


**** Compute Jacobian Matrix
#+begin_src matlab
  J = [As' , cross(Ab, As)'];
#+end_src

**** Compute Stiffness Matrix
#+begin_src matlab
  K = J'*diag(Ki)*J;
#+end_src

**** Compute Compliance Matrix
#+begin_src matlab
  C = inv(K);
#+end_src

**** Populate the =stewart= structure
#+begin_src matlab
  stewart.kinematics.J = J;
  stewart.kinematics.K = K;
  stewart.kinematics.C = C;
#+end_src

*** =initializeInertialSensor=: Initialize the inertial sensor in each strut
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeInertialSensor.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:

**** Geophone - Working Principle
From the schematic of the Z-axis geophone shown in Figure ref:fig:z_axis_geophone, we can write the transfer function from the support velocity $\dot{w}$ to the relative velocity of the inertial mass $\dot{d}$:
\[ \frac{\dot{d}}{\dot{w}} = \frac{-\frac{s^2}{{\omega_0}^2}}{\frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1} \]
with:
- $\omega_0 = \sqrt{\frac{k}{m}}$
- $\xi = \frac{1}{2} \sqrt{\frac{m}{k}}$

#+name: fig:z_axis_geophone
#+caption: Schematic of a Z-Axis geophone
[[file:figs/inertial_sensor.png]]

We see that at frequencies above $\omega_0$:
\[ \frac{\dot{d}}{\dot{w}} \approx -1 \]

And thus, the measurement of the relative velocity of the mass with respect to its support gives the absolute velocity of the support.

We generally want to have the smallest resonant frequency $\omega_0$ to measure low frequency absolute velocity, however there is a trade-off between $\omega_0$ and the mass of the inertial mass.

**** Accelerometer - Working Principle
From the schematic of the Z-axis accelerometer shown in Figure ref:fig:z_axis_accelerometer, we can write the transfer function from the support acceleration $\ddot{w}$ to the relative position of the inertial mass $d$:
\[ \frac{d}{\ddot{w}} = \frac{-\frac{1}{{\omega_0}^2}}{\frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1} \]
with:
- $\omega_0 = \sqrt{\frac{k}{m}}$
- $\xi = \frac{1}{2} \sqrt{\frac{m}{k}}$

#+name: fig:z_axis_accelerometer
#+caption: Schematic of a Z-Axis geophone
[[file:figs/inertial_sensor.png]]

We see that at frequencies below $\omega_0$:
\[ \frac{d}{\ddot{w}} \approx -\frac{1}{{\omega_0}^2} \]

And thus, the measurement of the relative displacement of the mass with respect to its support gives the absolute acceleration of the support.

Note that there is trade-off between:
- the highest measurable acceleration $\omega_0$
- the sensitivity of the accelerometer which is equal to $-\frac{1}{{\omega_0}^2}$

**** Function description
#+begin_src matlab
  function [stewart] = initializeInertialSensor(stewart, args)
  % initializeInertialSensor - Initialize the inertial sensor in each strut
  %
  % Syntax: [stewart] = initializeInertialSensor(args)
  %
  % Inputs:
  %    - args - Structure with the following fields:
  %        - type       - 'geophone', 'accelerometer', 'none'
  %        - mass [1x1] - Weight of the inertial mass [kg]
  %        - freq [1x1] - Cutoff frequency [Hz]
  %
  % Outputs:
  %    - stewart - updated Stewart structure with the added fields:
  %      - stewart.sensors.inertial
  %        - type    - 1 (geophone), 2 (accelerometer), 3 (none)
  %        - K [1x1] - Stiffness [N/m]
  %        - C [1x1] - Damping [N/(m/s)]
  %        - M [1x1] - Inertial Mass [kg]
  %        - G [1x1] - Gain
#+end_src

**** Optional Parameters
#+begin_src matlab
  arguments
      stewart
      args.type       char   {mustBeMember(args.type,{'geophone', 'accelerometer', 'none'})} = 'none'
      args.mass (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e-2
      args.freq (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e3
  end
#+end_src

**** Compute the properties of the sensor
#+begin_src matlab
  sensor = struct();

  switch args.type
    case 'geophone'
      sensor.type = 1;

      sensor.M = args.mass;
      sensor.K = sensor.M * (2*pi*args.freq)^2;
      sensor.C = 2*sqrt(sensor.M * sensor.K);
    case 'accelerometer'
      sensor.type = 2;

      sensor.M = args.mass;
      sensor.K = sensor.M * (2*pi*args.freq)^2;
      sensor.C = 2*sqrt(sensor.M * sensor.K);
      sensor.G = -sensor.K/sensor.M;
    case 'none'
      sensor.type = 3;
  end
#+end_src

**** Populate the =stewart= structure
#+begin_src matlab
  stewart.sensors.inertial = sensor;
#+end_src



*** =inverseKinematics=: Compute Inverse Kinematics
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/inverseKinematics.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:

**** Theory
For inverse kinematic analysis, it is assumed that the position ${}^A\bm{P}$ and orientation of the moving platform ${}^A\bm{R}_B$ are given and the problem is to obtain the joint variables, namely, $\bm{L} = [l_1, l_2, \dots, l_6]^T$.

From the geometry of the manipulator, the loop closure for each limb, $i = 1, 2, \dots, 6$ can be written as
\begin{align*}
  l_i {}^A\hat{\bm{s}}_i &= {}^A\bm{A} + {}^A\bm{b}_i - {}^A\bm{a}_i \\
                         &= {}^A\bm{A} + {}^A\bm{R}_b {}^B\bm{b}_i - {}^A\bm{a}_i
\end{align*}

To obtain the length of each actuator and eliminate $\hat{\bm{s}}_i$, it is sufficient to dot multiply each side by itself:
\begin{equation}
  l_i^2 \left[ {}^A\hat{\bm{s}}_i^T {}^A\hat{\bm{s}}_i \right] = \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]^T \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]
\end{equation}

Hence, for $i = 1, 2, \dots, 6$, each limb length can be uniquely determined by:
\begin{equation}
  l_i = \sqrt{{}^A\bm{P}^T {}^A\bm{P} + {}^B\bm{b}_i^T {}^B\bm{b}_i + {}^A\bm{a}_i^T {}^A\bm{a}_i - 2 {}^A\bm{P}^T {}^A\bm{a}_i + 2 {}^A\bm{P}^T \left[{}^A\bm{R}_B {}^B\bm{b}_i\right] - 2 \left[{}^A\bm{R}_B {}^B\bm{b}_i\right]^T {}^A\bm{a}_i}
\end{equation}

If the position and orientation of the moving platform lie in the feasible workspace of the manipulator, one unique solution to the limb length is determined by the above equation.
Otherwise, when the limbs' lengths derived yield complex numbers, then the position or orientation of the moving platform is not reachable.

**** Function description
#+begin_src matlab
  function [Li, dLi] = inverseKinematics(stewart, args)
  % inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}
  %
  % Syntax: [stewart] = inverseKinematics(stewart)
  %
  % Inputs:
  %    - stewart - A structure with the following fields
  %        - geometry.Aa   [3x6] - The positions ai expressed in {A}
  %        - geometry.Bb   [3x6] - The positions bi expressed in {B}
  %        - geometry.l    [6x1] - Length of each strut
  %    - args - Can have the following fields:
  %        - AP   [3x1] - The wanted position of {B} with respect to {A}
  %        - ARB  [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
  %
  % Outputs:
  %    - Li   [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}
  %    - dLi  [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
#+end_src

**** Optional Parameters
#+begin_src matlab
  arguments
      stewart
      args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
      args.ARB (3,3) double {mustBeNumeric} = eye(3)
  end
#+end_src

**** Check the =stewart= structure elements
#+begin_src matlab
  assert(isfield(stewart.geometry, 'Aa'),   'stewart.geometry should have attribute Aa')
  Aa = stewart.geometry.Aa;

  assert(isfield(stewart.geometry, 'Bb'),   'stewart.geometry should have attribute Bb')
  Bb = stewart.geometry.Bb;

  assert(isfield(stewart.geometry, 'l'),   'stewart.geometry should have attribute l')
  l = stewart.geometry.l;
#+end_src


**** Compute
#+begin_src matlab
  Li = sqrt(args.AP'*args.AP + diag(Bb'*Bb) + diag(Aa'*Aa) - (2*args.AP'*Aa)' + (2*args.AP'*(args.ARB*Bb))' - diag(2*(args.ARB*Bb)'*Aa));
#+end_src

#+begin_src matlab
  dLi = Li-l;
#+end_src

* Footnotes
[fn:4]Note that the eccentricity of the "point of interest" with respect to the Spindle rotation axis has been tuned from the measurements.
[fn:3]The "PEPU" [[cite:&hino18_posit_encod_proces_unit]] was used for digital protocol conversion between the interferometers and the Speedgoat
[fn:2]M12/F40 model from Attocube
[fn:1]Depending on the measuring range, gap can range from $\approx 1\,\mu m$ to $\approx 100\,\mu m$