#+TITLE: Nano-Hexapod on top of a Spindle - Test Bench
: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-nass-spindle.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:

#+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_rot=

*Goals*:
- Short stroke metrology with Interferometers to have more stroke
- Validation of IFF
- Validation of control architecture with rotation (kinematics)
  - How to estimate Rz? (useful for kinematics): what happens if Rz is ignored (i.e. supposed to be zero)?
- Effect of rotation on the plant dynamics

*Notes*:
- Robustness will be checked on ID31
- Tests were performed in December 2022 and January/February 2023
- All that can be explained here before the ID31 tests is nice
- 20 pages maximum should be good enough
- Should I speak here about the Simscape model? (Maybe not useful)

- [ ] Explain that beam time is complex to have, so first tests are made in the lab.
- [ ] talk about the metrology system.
  Comparison with LION precision system.
- [ ] Find pictures with additional mass with the metrology? (I don't find them)

** Alignment

Optimal distance for top Attocube: 179.67 (distance between reference surface and bottom surface of LION sphere)

Nouvelle mesure après alignement

aligné à mieux que 0.1mm en XYZ:
- XY tête du haut / axe LION
- XZ tête face / axe lion
- YZ tête droite / axe lion

* Introduction                                                        :ignore:

As the different beamlines are running 24/7
It is difficult to have access to the micro station to perform tests.
The only slot available is 3 weeks during the summer.

Before the tests on ID31:
- Development of a 5DoF metrology system
- Make sure all the kinematic is working properly


#+name: fig:picture_setup
#+caption: Setup with the Spindle, nano-hexapod and metrology
#+attr_latex: :width \linewidth
[[file:figs/IMG_20221220_152429.jpg]]

* Test-Bench Description
** Introduction                                                      :ignore:
#+begin_note
Here are the documentation of the equipment used for this test bench:
- Voltage Amplifier: PiezoDrive [[file:doc/PD200-V7-R1.pdf][PD200]]
- Amplified Piezoelectric Actuator: Cedrat [[file:doc/APA300ML.pdf][APA300ML]]
- DAC/ADC: Speedgoat [[file:doc/IO131-OEM-Datasheet.pdf][IO131]]
- Encoder: Renishaw [[file:doc/L-9517-9678-05-A_Data_sheet_VIONiC_series_en.pdf][Vionic]] and used [[file:doc/L-9517-9862-01-C_Data_sheet_RKLC_EN.pdf][Ruler]]
- LION Precision [[file:doc/Catalog-CPL190290.pdf][CPL290]]
- Spindle: Lab Motion [[file:doc/RS250S.pdf][RT250S]] with [[file:doc/DB36_connections_english_V2.pdf][Drivebox 3.6]] controller
#+end_note

** 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

** Alignment

Procedure:
1. Align bottom sphere with the spindle rotation axis (~ 10um)
2. Align top sphere with the spindle rotation axis (~ 10um)

** Short Range metrology system

There are 5 interferometers pointing at 2 spheres as shown in Figure ref:fig:LION_metrology_interferometers.

#+name: fig:picture_metrology
#+caption: Metrology system with LION sphere (1 inch diameter) and 5 interferometers fixed to their individual tip-tilts
#+attr_latex: :width 0.5\linewidth
[[file:figs/IMG_20221216_181305.jpg]]

|                              | Value  |
|------------------------------+--------|
| Sphere Diameter              | 25.4mm |
| Distance between the spheres | 76.2mm |

#+name: fig:LION_metrology_interferometers
#+caption: Schematic of the measurement system
#+attr_latex: :width 0.5\linewidth
[[file:figs/LION_metrology_interferometers.png]]

*Assumptions*:
- Interferometers are perfectly positioned / oriented
- Sphere is perfect

*Compute the Jacobian matrix*:
- From pure X-Y-Z-Rx-Ry small motions, compute the effect on the 5 measured distances
- Compute the matrix
- Inverse the matrix
- Verify that it is working with simple example (for example using Solidworkds)

We have the following set of equations:
\begin{align}
d_1 &= -D_y + l_2 R_x \\
d_2 &= -D_y - l_1 R_x \\
d_3 &= -D_x - l_2 R_y \\
d_4 &= -D_x + l_1 R_y \\
d_5 &= -D_z
\end{align}

That can be written as a linear transformation:
\begin{equation}
\begin{bmatrix}
  d_1 \\ d_2 \\ d_3 \\ d_4 \\ d_5
\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} \cdot \begin{bmatrix}
  D_x \\ D_y \\ D_z \\ R_x \\ R_y
\end{bmatrix}
\end{equation}

By inverting the matrix, we obtain the Jacobian relation:
\begin{equation}
\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
%% Parameters
H = 150e-3;
l1 = (150-38-52)*1e-3;
l2 = (52+38+76.2-150)*1e-3;
#+end_src

#+begin_src matlab
%% 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

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable(pinv(Hm), {'*Dx*', '*Dy*', '*Dz*', '*Rx*', '*Ry*'}, {'*d1*', '*d2*', '*d3*', '*d4*', '*d5*'}, ' %.2f ');
#+end_src

#+name: tab:jacobian_metrology
#+caption: Jacobian matrix for the metrology system
#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
#+attr_latex: :center t :booktabs t
#+RESULTS:
|      |  *d1* |   *d2* |   *d3* |  *d4* | *d5* |
|------+-------+--------+--------+-------+------|
| *Dx* |   0.0 |    0.0 |  -0.79 | -0.21 |  0.0 |
| *Dy* | -0.79 |  -0.21 |   -0.0 |  -0.0 |  0.0 |
| *Dz* |   0.0 |    0.0 |    0.0 |   0.0 | -1.0 |
| *Rx* | 13.12 | -13.12 |    0.0 |  -0.0 |  0.0 |
| *Ry* |   0.0 |    0.0 | -13.12 | 13.12 |  0.0 |

** Spindle errors
*** Introduction                                                    :ignore:

The spindle is rotated at 60rpm during 10 turns.
The signal of all 5 interferometers are recorded.

#+begin_src matlab
data = load(sprintf('%s/spindle/mat/2022-12-20_15-47_sec_test.mat', data_dir));
#+end_src

*** Errors in $D_x$ and $D_y$

Because of the eccentricity of the reference surfaces (the spheres), we expect the motion in the X-Y plane to be a circle as a first approximation.
We can first see that in Figure ref:fig:dx_dy_motion_rotation that shows the measured $D_x$ and $D_y$ motion as a function of the $R_z$ angle.

#+begin_src matlab :exports none :results none
%% Dz motion during the rotation
figure;
hold on;
plot(1/2/pi*unwrap(data.Rz - data.Rz(1)), 1e6*detrend(data.Dx_int, 0), 'DisplayName', '$D_x$')
plot(1/2/pi*unwrap(data.Rz - data.Rz(1)), 1e6*detrend(data.Dy_int, 0), 'DisplayName', '$D_y$')
hold off;
xlim([0, 10])
xlabel('Rotation [turn]'); ylabel('$D_z$ motion [$\mu$m]');
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
#+end_src

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

#+name: fig:dx_dy_motion_rotation
#+caption: Dx and Dy motion during the rotation
#+RESULTS:
[[file:figs/dx_dy_motion_rotation.png]]

#+begin_src matlab
%% Circle Fit
[xc,yc,R,a] = circlefit(data.Dx_int, data.Dy_int);
#+end_src

A circle is fit, and the obtained radius of the circle (i.e. the excentricity) is estimated to be:

#+begin_src matlab :results value replace :exports results :tangle no
sprintf('Error linked to excentricity = %.0f um', 1e6*R);
#+end_src

#+RESULTS:
: Error linked to excentricity = 19 um

The motion in the X-Y plane as well as the circle fit and the residual motion (circle fit subtracted from the measured motion) are shown in Figure ref:fig:dx_dy_spindle_rotation.

#+begin_src matlab :exports none :results none
%% Dx and Dy motion during the spindle rotation
theta = linspace(0, 2*pi, 1000);
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(1e6*detrend(data.Dx_int, 0), 1e6*detrend(data.Dy_int, 0), 'DisplayName', 'Raw data')
plot(1e6*detrend(xc + R*cos(theta), 0), 1e6*detrend(yc + R*sin(theta), 0), '--', 'DisplayName', 'fit')
plot(1e6*(data.Dx_int - (xc + R*cos(data.Rz-0.07))), 1e6*(data.Dy_int - (yc + R*sin(data.Rz-0.07))), 'k.', 'DisplayName', 'Residual')
hold off;
xlabel('$D_x$ [$\mu$m]')
ylabel('$D_y$ [$\mu$m]')
axis equal
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile();
plot(1e6*(data.Dx_int - (xc + R*cos(data.Rz-0.07))), 1e6*(data.Dy_int - (yc + R*sin(data.Rz-0.07))), 'k.')
xlabel('$D_x$ [$\mu$m]')
ylabel('$D_y$ [$\mu$m]')
axis equal
#+end_src

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

#+name: fig:dx_dy_spindle_rotation
#+caption: Dx and Dy motion during the spindle rotation
#+RESULTS:
[[file:figs/dx_dy_spindle_rotation.png]]

Let's now analyse the frequency content in the signal.

#+begin_src matlab :exports none
%% Hanning window used for pwelch function
Ts = 1e-4;
win = hanning(5/Ts);

%% Compute PSD of initial motion error
[S_Dx, f] = pwelch(detrend(data.Dx_int, 0), win, 0, [], 1/Ts);
[S_Dy, ~] = pwelch(detrend(data.Dy_int, 0), win, 0, [], 1/Ts);
#+end_src

#+begin_src matlab :exports none :results none
%% Amplitude Spectral Density of the measured Dx and Dy motion
figure;
hold on;
plot(f, sqrt(S_Dx), 'DisplayName', '$D_x$');
plot(f, sqrt(S_Dy), 'DisplayName', '$D_y$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]');
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
xlim([0.1, 1e3]); ylim([1e-11, 1e-4]);
#+end_src

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

#+name: fig:dx_dy_spindle_rotation_asd
#+caption: Amplitude Spectral Density of the measured Dx and Dy motion
#+RESULTS:
[[file:figs/dx_dy_spindle_rotation_asd.png]]

#+begin_src matlab :exports none :results none
%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion
figure;
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(S_Dx)))), 'DisplayName', '$D_x$');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(S_Dy)))), 'DisplayName', '$D_y$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('CAS [$m$]');
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
xlim([0.1, 1e3]); ylim([1e-9, 2e-5]);
#+end_src

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

#+name: fig:dx_dy_spindle_rotation_cas
#+caption: Cumulative Amplitude Spectrum of the measured Dx and Dy motion
#+RESULTS:
[[file:figs/dx_dy_spindle_rotation_cas.png]]

*** Errors in vertical motion $D_z$

The top interferometer is measuring the vertical motion of the sphere.

However, if the top sphere is not perfectly aligned with the spindle axis, there will also measure some vertical motion due to this excentricity.

#+begin_src matlab :exports none :results none
%% Dz motion during the rotation
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
plot(1/2/pi*unwrap(data.Rz - data.Rz(1)), 1e6*detrend(data.Dz_int, 0), '-')
xlim([0, 10])
xlabel('Rotation [turn]'); ylabel('$D_z$ motion [$\mu$m]');

ax1 = nexttile();
plot(180/pi*unwrap(data.Rz - data.Rz(1)), 1e6*detrend(data.Dz_int, 0), '-')
xlim([0, 90])
xlabel('Rotation [deg]'); ylabel('$D_z$ motion [$\mu$m]');
#+end_src

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

#+name: fig:dz_motion_rotation
#+caption: Dz motion during the rotation
#+RESULTS:
[[file:figs/dz_motion_rotation.png]]

Let's fit a sinus with a period of one turn.

#+begin_src matlab
%% Fit Sinus with period of one turn
x1 = data.Rz(data.t<1);
y1 = data.Dz_int(data.t<1);

fit = @(b,x)  b(1).*sin(x + b(2)) + b(3);    % Function to fit
fcn = @(b) sum((fit(b,x1) - y1).^2);                             % Least-Squares cost function

s1 = fminsearch(fcn, [1e-6;  30;  1.5e-6])                      % Minimise Least-Squares
#+end_src

#+begin_src matlab :results value replace :exports results :tangle no
sprintf('Errors linked to excentricity = %.0f [nm]', 1e9*s1(1));
#+end_src

#+RESULTS:
: Errors linked to excentricity = 410 [nm]

#+begin_src matlab :exports none :results none
%% Effect of the excentricity and remaining Dz motion
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(180/pi*x1, 1e6*y1, '.', 'DisplayName', 'Raw')
plot(180/pi*x1, 1e6*fit(s1, x1), '.', 'DisplayName', 'Fit')
hold off;
xlabel('$R_z$ [deg]')
ylabel('$D_z$ [$\mu$m]')
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
xlim([0, 360]);

ax2 = nexttile();
plot(180/pi*x1, 1e9*(y1 - fit(s1, x1)), 'k.')
xlabel('$R_z$ [deg]')
ylabel('$D_z$ [$\mu$m]')
xlim([0, 360]);
#+end_src

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

#+name: fig:dz_motion_rotation_excentricity
#+caption: Effect of the excentricity and remaining Dz motion
#+RESULTS:
[[file:figs/dz_motion_rotation_excentricity.png]]

If we look at the remaining motion after removing the effect of the eccentricity (Figure ref:fig:dz_motion_rotation_excentricity, right), we can see a signal with 20 periods every turn.
Let's fit this.

#+begin_src matlab
%% Fit Sinus with period of 18 degrees (one electrical period)
x2 = data.Rz(data.t<1)*20;
y2 = y1 - fit(s1, x1);

fit = @(b,x)  b(1).*sin(x + b(2)) + b(3);    % Function to fit
fcn = @(b) sum((fit(b,x2) - y2).^2);                             % Least-Squares cost function

s2 = fminsearch(fcn, [50e-9;  30;  0])                      % Minimise Least-Squares
#+end_src

#+begin_src matlab :results value replace :exports results :tangle no
sprintf('Errors linked to spindle motor = %.0f [nm]', 1e9*s2(1));
#+end_src

#+RESULTS:
: Errors linked to spindle motor = 58 [nm]

#+begin_src matlab :exports none :results none
%% Effect of the poles and remaining Dz motion
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(180/pi*x1, 1e6*y2, '.', 'DisplayName', 'Raw')
plot(180/pi*x1, 1e6*fit(s2, x2), '.', 'DisplayName', 'Fit')
hold off;
xlabel('$R_z$ [deg]')
ylabel('$D_z$ [$\mu$m]')
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
xlim([0, 360]);

ax2 = nexttile();
plot(180/pi*x1, 1e9*(y2 - fit(s2, x2)), 'k.')
xlabel('$R_z$ [deg]')
ylabel('$D_z$ [$\mu$m]')
xlim([0, 360]);
#+end_src

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

#+name: fig:dz_motion_rotation_poles
#+caption: Effect of the magnetic pole pairs and remaining Dz motion
#+RESULTS:
[[file:figs/dz_motion_rotation_poles.png]]

Let's look at the signal in the frequency domain.

On top of the peak at 1Hz (excentricity) and at 20Hz (number of pole pairs), we can observe a frequency of 126Hz (i.e. 126 periods per turn, approx 2.85 deg).

#+begin_quote
Could this be related to the air bearing system?
#+end_quote

#+begin_src matlab :exports none
%% Hanning window used for pwelch function
Ts = 1e-4;
win = hanning(5/Ts);

%% Compute PSD of initial motion error
[S_Dz, f] = pwelch(detrend(data.Dz_int, 0), win, 0, [], 1/Ts);
#+end_src

#+begin_src matlab :exports none :results none
%% Amplitude Spectral Density of the measured Dz motion
figure;
hold on;
plot(f, sqrt(S_Dz));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD of $D_z$ [$m/\sqrt{Hz}$]');
xlim([0.1, 1e3]); ylim([1e-11, 1e-4]);
#+end_src

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

#+name: fig:dz_spindle_rotation_asd
#+caption: Amplitude Spectral Density of the measured Dz motion
#+RESULTS:
[[file:figs/dz_spindle_rotation_asd.png]]

#+begin_src matlab :exports none :results none
%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion
figure;
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(S_Dx)))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('CAS of $D_z$ [$m$]');
xlim([0.1, 1e3]); ylim([1e-9, 2e-5]);
#+end_src

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

#+name: fig:dz_spindle_rotation_cas
#+caption: Cumulative Amplitude Spectrum of the measured Dz motion
#+RESULTS:
[[file:figs/dz_spindle_rotation_cas.png]]

*** Angle errors in $R_x$ and $R_y$

#+begin_src matlab
[xc,yc,R,a] = circlefit(data.Rx_int, data.Ry_int);
#+end_src

#+begin_src matlab :results value replace :exports results :tangle no
sprintf('amplitude = %.0f urad', 1e6*R);
#+end_src

#+RESULTS:
: amplitude = 281 urad

#+begin_src matlab :exports none :results none
%% Rx and Ry motion during the spindle rotation
theta = linspace(0, 2*pi, 1000);
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(1e6*detrend(data.Rx_int, 0), 1e6*detrend(data.Ry_int, 0), 'DisplayName', 'Raw data')
plot(1e6*detrend(xc + R*cos(theta), 0), 1e6*detrend(yc + R*sin(theta), 0), '--', 'DisplayName', 'fit')
plot(1e6*(data.Rx_int - (xc + R*cos(data.Rz+2.97))), 1e6*(data.Ry_int - (yc + R*sin(data.Rz+2.97))), 'k.', 'DisplayName', 'Residual')
hold off;
xlabel('$R_x$ [$\mu$rad]')
ylabel('$R_y$ [$\mu$rad]')
axis equal
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile();
plot(1e6*(data.Rx_int - (xc + R*cos(data.Rz+2.97))), 1e6*(data.Ry_int - (yc + R*sin(data.Rz+2.97))), 'k.')
xlabel('$R_x$ [$\mu$rad]')
ylabel('$R_y$ [$\mu$rad]')
axis equal
#+end_src

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

#+name: fig:rx_ry_spindle_rotation
#+caption: Rx and Ry motion during the spindle rotation
#+RESULTS:
[[file:figs/rx_ry_spindle_rotation.png]]

Let's now analyse the frequency content in the signal.

#+begin_src matlab :exports none
%% Hanning window used for pwelch function
Ts = 1e-4;
win = hanning(5/Ts);

%% Compute PSD of initial motion error
[S_Rx, f] = pwelch(detrend(data.Rx_int, 0), win, 0, [], 1/Ts);
[S_Ry, ~] = pwelch(detrend(data.Ry_int, 0), win, 0, [], 1/Ts);
#+end_src

#+begin_src matlab :exports none :results none
%% Amplitude Spectral Density of the measured Rx and Ry motion
figure;
hold on;
plot(f, sqrt(S_Rx), 'DisplayName', '$D_x$');
plot(f, sqrt(S_Ry), 'DisplayName', '$D_y$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD [rad$/\sqrt{Hz}$]');
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
xlim([0.1, 1e3]); ylim([1e-10, 1e-3]);
#+end_src

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

#+name: fig:rx_ry_spindle_rotation_asd
#+caption: Amplitude Spectral Density of the measured Rx and Ry motion
#+RESULTS:
[[file:figs/rx_ry_spindle_rotation_asd.png]]

#+begin_src matlab :exports none :results none
%% Cumulative Amplitude Spectrum of the measured Rx and Ry motion
figure;
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(S_Rx)))), 'DisplayName', '$D_x$');
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(S_Ry)))), 'DisplayName', '$D_y$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('CAS [rad]');
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
xlim([0.1, 1e3]); ylim([5e-8, 1e-3]);
#+end_src

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

#+name: fig:rx_ry_spindle_rotation_cas
#+caption: Cumulative Amplitude Spectrum of the measured Rx and Ry motion
#+RESULTS:
[[file:figs/rx_ry_spindle_rotation_cas.png]]


* Simscape Model
** Introduction                                                      :ignore:

A 3D view of the Simscape model is shown in Figure ref:fig:simscape_model_spindle_bench.
The Spindle is represented by a /Bushing joint/.
Axial, radial and tilt stiffnesses are taken from the Spindle datasheet (see Table).

#+name: tab:spindle_stiffnesses
#+caption: Spindle stiffnesses
#+attr_latex: :environment tabularx :width \linewidth :align lXX
#+attr_latex: :center t :booktabs t
| *Stiffness* | *Value* | *Unit*    |
|-------------+---------+-----------|
| Axial       |     402 | $N/\mu m$ |
| Radial      |     226 | $N/\mu m$ |
| Tilt        |    2380 | $Nm/mrad$ |

The metrology system consists of 5 distance measurements (represented by the red lines in Figure ref:fig:simscape_model_spindle_bench).

#+name: fig:simscape_model_spindle_bench
#+caption: Screenshot of the 3D view of the Simscape model
#+attr_latex: :width 0.5\linewidth
[[file:figs/simscape_model_spindle_bench.jpg]]

** 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

#+begin_src matlab
%% Frequency vector for further use
freqs = logspace(0,3,1000);

%% Start angle of the Spindle
start_angle = 0;
#+end_src

#+begin_src matlab :tangle no
%% Add path for Nano-Hexapod Simscape model
addpath('./matlab/nass-simscape/STEPS/nano_hexapod')
addpath('./matlab/nass-simscape/matlab/nano_hexapod')
addpath('./matlab/nass-simscape/src')
addpath('./matlab/nass-simscape/mat')
#+end_src

#+begin_src matlab :eval no
%% Add path for Nano-Hexapod Simscape model
addpath('./nass-simscape/STEPS/nano_hexapod')
addpath('./nass-simscape/matlab/nano_hexapod')
addpath('./nass-simscape/src')
addpath('./nass-simscape/mat')
#+end_src

** Simscape model parameters
The nano-hexapod is initialized.

#+begin_src matlab
%% Initialize Nano-Hexapod
n_hexapod = initializeNanoHexapodFinal('MO_B', 150e-3, ...
                                       'actuator_type', '2dof', ...
                                       'motion_sensor_type', 'plates');
#+end_src

#+begin_src matlab
%% Initialize Spindle
spindle = struct();

%% Stiffnesses
spindle.kx = 226e6; % Radial stiffness in [N/m]
spindle.ky = 226e6; % Radial stiffness in [N/m]
spindle.kz = 402e6; % Axial stiffness in [N/m]
spindle.krx = 2.38e6; % Tilt stiffness in [Nm/rad]
spindle.kry = 2.38e6; % Tilt stiffness in [Nm/rad]
spindle.krz = 0; % Rotation stiffness in [Nm/rad]

%% Damping
spindle.cx = 1/(2*0.01)/(sqrt(spindle.kx*50)); % Radial damping in [N/(m/s)]
spindle.cy = 1/(2*0.01)/(sqrt(spindle.ky*50)); % Radial damping in [N/(m/s)]
spindle.cz = 1/(2*0.01)/(sqrt(spindle.kz*50)); % Axial damping in [N/(m/s)]
spindle.crx = 1/(2*0.01)/(sqrt(spindle.krx*50)); % Tilt damping in [Nm/(rad/s)]
spindle.cry = 1/(2*0.01)/(sqrt(spindle.kry*50)); % Tilt damping in [Nm/(rad/s)]
spindle.crz = 0; % Rotation damping in [Nm/(rad/s)]
#+end_src

The Jacobian matrix that computes the $[x, y, z, R_x, R_y]$ motion of the sample from the 5 interferometers is defined below.

#+begin_src matlab
% Sensor Jacobian (Interferometers to Cartesian motion)
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];
#+end_src

#+begin_src matlab :exports none
%% Open the Simscape model
mdl = 'spindle_test_bench'
open(mdl)
#+end_src

** Control Architecture

Let's note:
- $d\mathcal{L}_m = [d_{\mathcal{L}_1},\ d_{\mathcal{L}_2},\ d_{\mathcal{L}_3},\ d_{\mathcal{L}_4},\ d_{\mathcal{L}_5},\ d_{\mathcal{L}_6}]$ the measurement of the 6 encoders fixed to the nano-hexapod
- $\bm{\tau}_m = [\tau_{m_1},\ \tau_{m_2},\ \tau_{m_3},\ \tau_{m_4},\ \tau_{m_5},\ \tau_{m_6}]$ the voltages measured by the 6 force sensors
- $\bm{u} = [u_1,\ u_2,\ u_3,\ u_4,\ u_5,\ u_6]$ the voltages send to the voltage amplifiers for the 6 piezoelectric actuators
- $R_z$ the spindle measured angle (encoder)
- $\bm{d}_m = [d_1,\ d_2,\ d_3,\ d_4,\ d_5]$ the distances measured by the 5 interferometers (see Figure ref:fig:LION_metrology_interferometers_bis)

#+name: fig:LION_metrology_interferometers_bis
#+caption: Schematic of the measurement system
#+attr_latex: :width 0.5\linewidth
[[file:figs/LION_metrology_interferometers.png]]

** Computation of the strut errors from the external metrology

The following frames are defined:
- $\{ W \}$: the frame that represents the wanted pose of the sample
- $\{ M \}$: the frame that represents the measured pose of the sample (estimated from the 5 interferometers and the spindle encoder)
- $\{ G \}$: the frame fixed to the granite and positioned at the sample's center
- $\{ H \}$: the frame fixed to the the spindle rotor, and positioned at the sample's center

We can express several homogeneous transformation matrices.

Frame fixed to the spindle rotor (centered on the sample's position), expressed in the frame of the granite:
\begin{equation}
{}^{G}\bm{T}_H = \begin{bmatrix}
cos(R_z) & -sin(R_z) & 0 & 0 \\
sin(R_z) &  cos(R_z) & 0 & 0 \\
0        &  0        & 1 & 0 \\
0        &  0        & 0 & 1
\end{bmatrix}
\end{equation}
with $R_z$ the spindle encoder.


Wanted position expressed in the frame of the granite:
\begin{equation}
{}^{G}\bm{T}_W = \begin{bmatrix}
 &                                 & & r_{D_x} \\
 & \bm{R}_x(r_{R_x}) \bm{R}_y(r_{R_y}) \bm{R}_z(r_{R_z}) & & r_{D_y} \\
 &                                 & & r_{D_z} \\
0        &  0                      & 0 & 1
\end{bmatrix}
\end{equation}
with $\bm{R}(r_{R_x}, r_{R_y}, r_{R_z})$ representing the wanted orientation of the sample with respect to the granite.
Typically, $r_{R_x} = 0$, $r_{R_y} = 0$ and $r_{R_z}$ corresponds to the spindle encoder $R_z$.


Measured position of the sample with respect to the granite:
\begin{equation}
{}^{G}\bm{T}_M = \begin{bmatrix}
 &                                 & & y_{D_x} \\
 & \bm{R}_x(y_{R_x}) \bm{R}_y(y_{R_y}) \bm{R}_z(R_z) & & y_{D_y} \\
 &                                 & & y_{D_z} \\
0        &  0                      & 0 & 1
\end{bmatrix}
\end{equation}
with $R_z$ the spindle encoder, and $[y_{D_x},\ y_{D_y},\ y_{D_z},\ y_{R_x},\ y_{R_y}]$ are obtained from the 5 interferometers:
\begin{equation}
\begin{bmatrix}
  y_{D_x} \\ y_{D_y} \\ y_{D_z} \\ y_{R_x} \\ y_{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}


In order to have the *position error in the frame of the nano-hexapod*, we have to compute ${}^M\bm{T}_W$:
\begin{align}
{}^M\bm{T}_W &= {}^M\bm{T}_G \cdot {}^G\bm{T}_W \\
             &= {{}^G\bm{T}_M}^{-1} \cdot {}^G\bm{T}_W
\end{align}

The *inverse of the transformation matrix* can be obtained by
\begin{equation}
  {}^B\bm{T}_A = {}^A\bm{T}_B^{-1} =
  \left[ \begin{array}{ccc|c}
      &   &   & \\
      & {}^A\bm{R}_B^T  &   & -{}^A \bm{R}_B^T {}^A\bm{P}_{O_B} \\
      &   &   & \cr
    \hline
    0 & 0 & 0 & 1 \\
  \end{array} \right]
\end{equation}

The position errors $\bm{\epsilon}_{\mathcal{X}} = [\epsilon_{D_x},\ \epsilon_{D_y},\ \epsilon_{D_z},\ \epsilon_{R_x},\ \epsilon_{R_y},\ \epsilon_{R_z}]$ expressed in a frame fixed to the nano-hexapod can be extracted from ${}^W\bm{T}_M$:
- $\epsilon_{D_x} = {}^M\bm{T}_W(1,4)$
- $\epsilon_{D_y} = {}^M\bm{T}_W(2,4)$
- $\epsilon_{D_z} = {}^M\bm{T}_W(3,4)$
- $\epsilon_{R_y} = \text{atan2}({}^M\bm{T}_W(1,3), \sqrt{{}^M\bm{T}_W(1,1)^2 + {}^M\bm{T}_W(1,2)^2})$
- $\epsilon_{R_x} = \text{atan2}(\frac{-{}^M\bm{T}_W(2,3)}{\cos(\epsilon_{R_y})}, \frac{{}^M\bm{T}_W(3,3)}{\cos(\epsilon_{R_y})})$
- $\epsilon_{R_z} = \text{atan2}(\frac{-{}^M\bm{T}_W(1,2)}{\cos(\epsilon_{R_y})}, \frac{{}^M\bm{T}_W(1,1)}{\cos(\epsilon_{R_y})})$

Finally, the strut errors $\bm{\epsilon}_{\mathcal{L}} = [\epsilon_{\matcal{L}_1},\ \epsilon_{\matcal{L}_2},\ \epsilon_{\matcal{L}_3},\ \epsilon_{\matcal{L}_4},\ \epsilon_{\matcal{L}_5},\ \epsilon_{\matcal{L}_6}]$ can be computed from:
\begin{equation}
\bm{\epsilon}_\mathcal{L} = \bm{J} \cdot \bm{\epsilon}_\mathcal{X}
\end{equation}

** IFF Plant
#+begin_src matlab
start_angle = 0; % [deg]
#+end_src

#+begin_src matlab
options = linearizeOptions;
options.SampleTime = 0;

%% Identify the transfer function from u to taum
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'],  1, 'openinput');   io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1;  % Force Sensors

%% Perform the model extraction
G_iff = linearize(mdl, io, 0.0, options);
#+end_src

#+begin_src matlab :exports none :results none
%% IFF plant obtained on the Simscape model
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_iff(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i =1:6
    set(gca,'ColorOrderIndex',i)
    plot(freqs, abs(squeeze(freqresp(G_iff(i, i), freqs, 'Hz'))), ...
         'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i));
end
plot(freqs, abs(squeeze(freqresp(G_iff(1, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$\\tau_{m,i}/u_j$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
% ylim([1e-9, 1e-3]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);

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

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

** DVF Plant
#+begin_src matlab
start_angle = 0; % [deg]
#+end_src

#+begin_src matlab
options = linearizeOptions;
options.SampleTime = 0;

%% Identify the transfer function from u to taum
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'],  1, 'openinput');   io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1;  % Encoders

%% Perform the model extraction
G_dvf = linearize(mdl, io, 0.0, options);
#+end_src

#+begin_src matlab :exports none :results none
%% DVF plant obtained on the Simscape model
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G_dvf(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i =1:6
    set(gca,'ColorOrderIndex',i)
    plot(freqs, abs(squeeze(freqresp(G_dvf(i, i), freqs, 'Hz'))), ...
         'DisplayName', sprintf('$d\\mathcal{L}_{m,%i}/u_%i$', i, i));
end
plot(freqs, abs(squeeze(freqresp(G_dvf(1, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$d\mathcal{L}_{m,i}/u_j$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
% ylim([1e-9, 1e-3]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);

ax2 = nexttile;
hold on;
for i =1:6
    set(gca,'ColorOrderIndex',i)
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf(i, i), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);

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

** HAC Plant
The transfer functions from the 6 actuator inputs to the 6 estimated strut errors are extracted from the Simscape model.

#+begin_src matlab
start_angle = 0; % [deg]
#+end_src

#+begin_src matlab
options = linearizeOptions;
options.SampleTime = 0;

%% Identify the transfer function from u to taum
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'],  1, 'openinput');   io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/J_X_to_L'],  1, 'openoutput'); io_i = io_i + 1; % Estimated Strut Error

%% Perform the model extraction
G = linearize(mdl, io, 0.0, options);
#+end_src

The obtained transfer functions are shown in Figure ref:fig:simscape_model_hac_plant.

We can see that the system is well decoupled at low frequency (i.e. below the first resonance of the Nano-Hexapod).

#+begin_src matlab :exports none :results none
%% HAC plant obtained on the Simscape model
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:5
    for j = i+1:6
        plot(freqs, abs(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i =1:6
    set(gca,'ColorOrderIndex',i)
    plot(freqs, abs(squeeze(freqresp(G(i, i), freqs, 'Hz'))), ...
         'DisplayName', sprintf('$d\\mathcal{L}_%i/u_%i$', i, i));
end
plot(freqs, abs(squeeze(freqresp(G(1, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$d\mathcal{L}_i/u_j$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
% ylim([1e-9, 1e-3]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);

ax2 = nexttile;
hold on;
for i =1:6
    set(gca,'ColorOrderIndex',i)
    plot(freqs, 180/pi*angle(squeeze(freqresp(G(i, i), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);

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

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

#+name: fig:simscape_model_hac_plant
#+caption: HAC plant obtained on the Simscape model
#+RESULTS:
[[file:figs/simscape_model_hac_plant.png]]

** Save Plants                                                     :noexport:
#+begin_src matlab :tangle no
save('matlab/mat/simscape_plants.mat', 'G_iff', 'G_dvf', 'G');
#+end_src

* Control Experiment
** 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

#+begin_src matlab
%% Frequency vector for further use
freqs = logspace(0,3,1000);
#+end_src

#+begin_src matlab
data_path = '/home/thomas/mnt/data_mel/NASS';
#+end_src

** IFF Plant
#+begin_src matlab
%% Load identification data
data = load(sprintf('%s/dynamics/2023-02-01_15-21_identification_new_matrices_long_bis.mat', data_path));
#+end_src

#+begin_src matlab :exports none
%% Load extracted plants from Simscape
model = load('simscape_plants.mat', 'G_iff', 'G_dvf', 'G');
#+end_src

#+begin_src matlab :exports none
% Sampling Time [s]
Ts = 1e-4;

% Hannning Windows
win = hanning(ceil(1/Ts));
#+end_src

#+begin_src matlab :exports none
% And we get the frequency vector
[~, f] = tfestimate(data.uL1.id_plant, data.uL1.e_L1, win, [], [], 1/Ts);
#+end_src

#+begin_src matlab :exports none
%% IFF Plant (transfer function from u to taum)
G_iff = zeros(length(f), 6, 6);

for i_strut = 1:6
    eL = [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]';

    G_iff(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, eL, win, [], [], 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_iff(:, i, j)), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i = 1:6
    plot(f, abs(G_iff(:,i, i)), 'color', colors(i,:), ...
        'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i));
end
plot(f, abs(G_iff(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$\tau_{m,i}/u_j$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
% ylim([1e-8, 1e-3]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);

ax2 = nexttile;
hold on;
for i =1:6
    plot(f, 180/pi*angle(G_iff(:,i, i)));
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 replace
exportFig('figs/G_iif_exp_no_rotation.pdf', 'width', 'full', 'height', 'tall');
#+end_src

#+name: fig:G_iif_exp_no_rotation
#+caption: Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
#+RESULTS:
[[file:figs/G_iif_exp_no_rotation.png]]

#+begin_src matlab :exports none
%% Comparison with the model
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:6
    plot(f, abs(G_iff(:,i, i)), '-', 'color', colors(i,:), ...
        'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i));
end
for i = 1:6
    plot(f, abs(squeeze(freqresp(model.G_iff(i, i), f, 'Hz'))), '--', 'color', colors(i,:), ...
        'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
% ylim([1e-8, 1e-3]);
% legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);

ax2 = nexttile;
hold on;
for i =1:6
    plot(f, 180/pi*angle(G_iff(:,i, i)), '-', 'color', colors(i,:));
end
for i =1:6
    plot(f, 180/pi*angle(squeeze(freqresp(model.G_iff(i, i), f, 'Hz'))), '--', 'color', colors(i,:));
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 replace
exportFig('figs/G_iif_exp_comp_no_rotation.pdf', 'width', 'full', 'height', 'tall');
#+end_src

#+name: fig:G_iif_exp_comp_no_rotation
#+caption: Comparison with the model
#+RESULTS:
[[file:figs/G_iif_exp_comp_no_rotation.png]]

** IFF Controller
#+begin_src matlab
%% IFF Controller
Kiff_g1 = -(1/(s + 2*pi*40))*...    % LPF: provides integral action above 40Hz
           (s/(s + 2*pi*30))*...    % HPF: limit low frequency gain
           (1/(1 + s/2/pi/500))*... % LPF: more robust to high frequency resonances
           eye(6);                  % Diagonal 6x6 controller
#+end_src

#+begin_src matlab :exports none :results none
%% Root Locus for IFF
gains = logspace(1, 4, 100);

figure;

hold on;
plot(real(pole(model.G_iff)),  imag(pole(model.G_iff)),  'x', 'color', colors(1,:), ...
    'DisplayName', '$g = 0$');
plot(real(tzero(model.G_iff)), imag(tzero(model.G_iff)), 'o', 'color', colors(1,:), ...
    'HandleVisibility', 'off');

for g = gains
    clpoles = pole(feedback(model.G_iff, g*Kiff_g1*eye(6), +1));
    plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
        'HandleVisibility', 'off');
end

% Optimal gain
g = 4e2;
clpoles = pole(feedback(model.G_iff, g*Kiff_g1*eye(6), +1));
plot(real(clpoles), imag(clpoles), 'x', 'color', colors(2,:), ...
    'DisplayName', sprintf('$g=%.0f$', g));
hold off;
xlim([-1500, 0]); ylim([0, 1500]);
axis square;
xlabel('Real Part'); ylabel('Imaginary Part');
legend('location', 'northwest');
#+end_src

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

#+name: fig:root_locus_iff_no_payload
#+caption: Root Locus for IFF
#+RESULTS:
[[file:figs/root_locus_iff_no_payload.png]]


#+begin_src matlab :exports none
K = g*Kiff_g1(1,1);
K_order = order(K(1,1));

Kz = c2d(K(1,1)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
[num, den] = tfdata(Kz, 'v');

formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
fileID = fopen('matlab/mat/K_iff_first_test.dat', 'w');
fprintf(fileID, formatSpec, [num; den]');
fclose(fileID);
#+end_src

** Open Loop Plant
Here the $R_z$ motion of the Hexapod is estimated from the encoders.

#+begin_src matlab :exports none
%% IFF Plant (transfer function from u to taum)
G_hac = zeros(length(f), 6, 6);

for i_strut = 1:6
    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]';

    G_hac(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, eL, win, [], [], 1/Ts);
end
#+end_src

#+begin_src matlab :exports none
%% Bode plot for the transfer function from u to dLm
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_hac(:, i, j)), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i = 1:6
    plot(f, abs(G_hac(:,i, i)), 'color', colors(i,:), ...
        'DisplayName', sprintf('$\\epsilon_{m,%i}/u_%i$', i, i));
end
plot(f, abs(G_hac(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$\epsilon_{m,i}/u_j$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-8, 1e-3]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);

ax2 = nexttile;
hold on;
for i =1:6
    plot(f, 180/pi*angle(G_hac(:,i, i)));
end
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/G_damp_exp_no_rotation.pdf', 'width', 'full', 'height', 'tall');
#+end_src

#+name: fig:G_damp_exp_no_rotation
#+caption: Obtained transfer function from generated voltages to estimated strut motion
#+RESULTS:
[[file:figs/G_damp_exp_no_rotation.png]]

#+begin_src matlab :exports none :results none
%% Comparison of the open-loop plant measured experimentally and extracted from Simscape
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_hac(:, i, j)), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
plot(f, abs(G_hac(:,1, 1)), 'color', colors(1,:), ...
     'DisplayName', 'Model');
for i = 1:6
    plot(f, abs(G_hac(:,i, i)), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
end
for i = 1:5
    for j = i+1:6
        plot(f, abs(squeeze(freqresp(model.G(i, j), f, 'Hz'))), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
plot(f, abs(squeeze(freqresp(model.G(1, 1), f, 'Hz'))), 'color', colors(2,:), ...
     'DisplayName', 'Experiment');
for i = 1:6
    plot(f, abs(squeeze(freqresp(model.G(i, i), f, 'Hz'))), 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-8, 1e-3]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile;
hold on;
for i =1:6
    plot(f, 180/pi*angle(G_hac(:,i, i)), 'color', colors(1,:));
end
for i =1:6
    plot(f, 180/pi*angle(squeeze(freqresp(model.G(i, i), f, '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([-180, 180])

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

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

#+name: fig:comp_hac_plant_exp_simscape
#+caption: Comparison of the open-loop plant measured experimentally and extracted from Simscape
#+RESULTS:
[[file:figs/comp_hac_plant_exp_simscape.png]]


** Damped Plant
#+begin_src matlab
%% Load identification data for the damped plant
data = load(sprintf('%s/dynamics/2023-02-01_16-04_identification_damped_iff_long.mat', data_path));
#+end_src

#+begin_src matlab :exports none
%% IFF Plant (transfer function from u to taum)
G_damp = zeros(length(f), 6, 6);

for i_strut = 1:6
    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]';

    G_damp(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, eL, win, [], [], 1/Ts);
end
#+end_src

#+begin_src matlab :exports none
%% Bode plot for the transfer function from u to dLm
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_damp(:, i, j)), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i = 1:6
    plot(f, abs(G_damp(:,i, i)), 'color', colors(i,:), ...
        'DisplayName', sprintf('$\\epsilon_{m,%i}/u_%i$', i, i));
end
plot(f, abs(G_damp(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$\epsilon_{m,i}/u_j$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-8, 1e-3]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);

ax2 = nexttile;
hold on;
for i =1:6
    plot(f, 180/pi*angle(G_damp(:,i, i)));
end
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/G_damp_damped_exp_no_rotation.pdf', 'width', 'full', 'height', 'tall');
#+end_src

#+name: fig:G_damp_damped_exp_no_rotation
#+caption: Obtained transfer function from generated voltages to estimated strut motion
#+RESULTS:
[[file:figs/G_damp_damped_exp_no_rotation.png]]

#+begin_src matlab :exports none :results none
%% Comparison of the undamped and damped plant with IFF
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_hac(:, i, j)), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
plot(f, abs(G_hac(:, 1, 1)), 'color', colors(1,:), ...
     'DisplayName', 'Undamped');
for i = 2:6
    plot(f, abs(G_hac(:,i, i)), 'color', colors(1,:), ...
             'HandleVisibility', 'off');
end
for i = 1:5
    for j = i+1:6
        plot(f, abs(G_damp(:, i, j)), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
plot(f, abs(G_damp(:, 1, 1)), 'color', colors(2,:), ...
     'DisplayName', 'Damped');
for i = 2:6
    plot(f, abs(G_damp(:,i, i)), 'color', colors(2,:), ...
             'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-8, 1e-3]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile;
hold on;
for i =1:6
    plot(f, 180/pi*angle(G_hac(:,i, i)), 'color', colors(1,:));
end
for i =1:6
    plot(f, 180/pi*angle(G_damp(:,i, i)), '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([-180, 180])

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

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

#+name: fig:comp_damped_undamped_plant
#+caption: Comparison of the undamped and damped plant with IFF
#+RESULTS:
[[file:figs/comp_damped_undamped_plant.png]]

** HAC Controller
#+begin_src matlab
%% Lead to increase phase margin
a  = 4;  % Amount of phase lead / width of the phase lead / high frequency gain
wc = 2*pi*15; % Frequency with the maximum phase lead [rad/s]

H_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));

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

%% Notch at the top-plate resonance
gm = 0.02;
xi = 0.3;
wn = 2*pi*665;

H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);

%% Decentralized HAC
Khac_iff_struts = (8e3) * ... % Gain
                  H_notch * ...      % Notch
                  H_lpf * ...      % LPF
                  (2*pi*100/s) * ... % Integrator
                  eye(6);            % 6x6 Diagonal
#+end_src

#+begin_src matlab :exports none
%% Loop Gain
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:6
    plot(f, abs(G_damp(:,i, i).*squeeze(freqresp(Khac_iff_struts(i,i), f, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
% ylim([1e-8, 1e-3]);

ax2 = nexttile;
hold on;
for i =1:6
    plot(f, 180/pi*angle(G_damp(:,i, i).*squeeze(freqresp(Khac_iff_struts(i,i), f, 'Hz'))));
end
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/first_hac_K_exp_loop_gain.pdf', 'width', 'full', 'height', 'tall');
#+end_src

#+name: fig:first_hac_K_exp_loop_gain
#+caption: Loop gain for the HAC
#+RESULTS:
[[file:figs/first_hac_K_exp_loop_gain.png]]

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

Lmimo = pagemtimes(permute(G_damp, [2,3,1]),squeeze(freqresp(Khac_iff_struts, f, 'Hz')));
for i_f = 2:length(f)
    Ldet(:, 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 = 1:6
    plot(real(squeeze(Ldet(i,:))),  imag(squeeze(Ldet(i,:))), 'k.');
    plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.');
end
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
hold off;
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
xlabel('Real'); ylabel('Imag');
xlim([-3, 1]); ylim([-2, 2]);
#+end_src

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

#+name: fig:first_hac_K_exp_root_locus
#+caption: Obtained Root Locus
#+RESULTS:
[[file:figs/first_hac_K_exp_root_locus.png]]

#+begin_src matlab :exports none
%% Export to Bliss format
K = - Khac_iff_struts(1,1);
K_order = order(K(1,1));

Kz = c2d(K(1,1)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
[num, den] = tfdata(Kz, 'v');

formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
fileID = fopen('matlab/mat/K_hac_first_test.dat', 'w');
fprintf(fileID, formatSpec, [num; den]');
fclose(fileID);
#+end_src

** Compare dynamics seen by interferometers and by encoders
#+begin_src matlab :exports none
%% IFF Plant (transfer function from u to taum)
G_dl = zeros(length(f), 6, 6);

for i_strut = 1:6
    eL = -[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_dl(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, eL, win, [], [], 1/Ts);
end
#+end_src

#+begin_src matlab :exports none :results none
%% Comparison of the identified dynamic by the internal metrology (encoders) and by the external metrology (interferometers)
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_hac(:,1, 1)), '-', ...
     'DisplayName', 'External Metrology');
plot(f, abs(G_dl(:,1, 1)), '-', ...
     'DisplayName', 'Internal Metrology');
for i = 1:5
    for j = i+1:6
        plot(f, abs(G_hac(:, i, j)), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(f, abs(G_dl(:, i, j)), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-8, 1e-3]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(G_hac(:,1, 1)), '-');
plot(f, 180/pi*angle(G_dl(:,1, 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 replace
exportFig('figs/comp_dynamics_int_ext_metrology.pdf', 'width', 'full', 'height', 'tall');
#+end_src

#+name: fig:comp_dynamics_int_ext_metrology
#+caption: Comparison of the identified dynamic by the internal metrology (encoders) and by the external metrology (interferometers)
#+RESULTS:
[[file:figs/comp_dynamics_int_ext_metrology.png]]


** Compare dynamics obtained with different Rz estimations
#+begin_src matlab :exports none
%% Load identification data
data = load(sprintf('%s/dynamics/2023-02-01_15-18_identification_new_matrices_long_Va.mat', data_path));
#+end_src

#+begin_src matlab :exports none
%% IFF Plant (transfer function from u to taum)
G_hac_Va = zeros(length(f), 6, 6);

for i_strut = 1:6
    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]';

    G_hac_Va(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, eL, win, [], [], 1/Ts);
end
#+end_src

#+begin_src matlab :exports none :results none
%% Comparison of the obtained plant using the Encoders or using the output Voltages to estimate Rz
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_hac(:,1, 1)), '-', ...
     'DisplayName', 'Encoders');
plot(f, abs(G_hac_Va(:,1, 1)), '-', ...
     'DisplayName', 'Voltage');
for i = 1:5
    for j = i+1:6
        plot(f, abs(G_hac(:, i, j)), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(f, abs(G_hac_Va(:, i, j)), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-8, 1e-3]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(G_hac(:,1, 1)), '-');
plot(f, 180/pi*angle(G_hac_Va(:,1, 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 replace
exportFig('figs/comp_plant_encoders_Va.pdf', 'width', 'full', 'height', 'tall');
#+end_src

#+name: fig:comp_plant_encoders_Va
#+caption: Comparison of the obtained plant using the Encoders or using the output Voltages to estimate Rz
#+RESULTS:
[[file:figs/comp_plant_encoders_Va.png]]


* Closed-Loop Results
** 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

#+begin_src matlab
%% Frequency vector for further use
freqs = logspace(0,3,1000);
#+end_src

#+begin_src matlab
data_path = '/home/thomas/mnt/data_mel/NASS';
#+end_src

** Open and Closed loop results
#+begin_src matlab
data_ol = load(sprintf('%s/spindle/2022-12-20_15-43_sec_test.mat', data_path));
data_cl = load(sprintf('%s/spindle/2023-02-01_16-57_closed_loop.mat', data_path));
#+end_src

#+begin_src matlab :exports none :results none
%% Comparison of the Open-Loop and Closed-Loop spindle errors
figure;
hold on;
plot(detrend(1e6*data_ol.Dx_int, 0), detrend(1e6*data_ol.Dy_int, 0), 'DisplayName', sprintf('OL $\\epsilon_d = %.1f$ [$\\mu$m RMS]', 1e6*rms(sqrt(detrend(data_ol.Dx_int, 0).^2 + detrend(data_ol.Dy_int, 0).^2))))
hold off;
xlabel('$x$ motion [$\mu$m]');
ylabel('$y$ motion [$\mu$m]');
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
axis square
#+end_src

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

#+RESULTS:
[[file:figs/spindle_errors_1rpm_ol.png]]

#+begin_src matlab :exports none :results none
%% Comparison of the Open-Loop and Closed-Loop spindle errors
figure;
hold on;
plot(detrend(1e6*data_ol.Dx_int, 0), detrend(1e6*data_ol.Dy_int, 0), 'DisplayName', sprintf('OL $\\epsilon_d = %.1f$ [$\\mu$m RMS]', 1e6*rms(sqrt(detrend(data_ol.Dx_int, 0).^2 + detrend(data_ol.Dy_int, 0).^2))))
plot(1e6*data_cl.Dx_int, 1e6*data_cl.Dy_int, 'DisplayName', sprintf('CL $\\epsilon_d = %.1f$ [nm RMS]', 1e9*rms(sqrt(data_cl.Dx_int.^2 + data_cl.Dy_int.^2))))
hold off;
xlabel('$x$ motion [$\mu$m]');
ylabel('$y$ motion [$\mu$m]');
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
axis square
#+end_src

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

#+name: fig:spindle_errors_1rpm_op_cl
#+caption: Comparison of the Open-Loop and Closed-Loop spindle errors
#+RESULTS:
[[file:figs/spindle_errors_1rpm_op_cl.png]]

#+begin_src matlab :exports none :results none
%% Comparison of the Open-Loop and Closed-Loop spindle errors - Rotation
figure;
hold on;
plot(detrend(1e6*data_ol.Rx_int, 0), detrend(1e6*data_ol.Ry_int, 0), 'DisplayName', 'Open-Loop')
plot(1e6*data_cl.Rx_int, 1e6*data_cl.Ry_int, 'DisplayName', 'Closed-Loop')
hold off;
xlabel('$R_x$ motion [$\mu$rad]');
ylabel('$R_y$ motion [$\mu$rad]');
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
axis square
#+end_src

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

#+name: fig:spindle_errors_1rpm_op_cl_rot
#+caption: Comparison of the Open-Loop and Closed-Loop spindle errors - Rotation
#+RESULTS:
[[file:figs/spindle_errors_1rpm_op_cl_rot.png]]

* Nano-Hexapod fixed on the Spindle                                 :noexport:
<<sec:nano_hexapod_spindle>>
** Introduction                                                      :ignore:

The Spindle is now fixed on top of a Spindle: the RS250S from LAB Motion Systems ([[file:doc/RS250S.pdf][doc]]).
All electrical connections between the nano-hexapod and the control electronics are passing trough a Slip-Ring.
A picture of the nano-hexapod on top of the Spindle is shown in Figure ref:fig:hexapod_spindle_picture.

#+name: fig:hexapod_spindle_picture
#+caption: Nano-Hexapod fixed on top of the Spindle
#+attr_latex: :width \linewidth
[[file:figs/hexapod_spindle_picture.jpg]]

** Change of dynamics when fixed on the Spindle
*** 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

*** Measured Frequency Response Functions
The identification only performed without any payload.

The following data are loaded:
- =Va=: the excitation voltage (corresponding to $u_i$)
- =Vs=: the generated voltage by the 6 force sensors (corresponding to $\bm{\tau}_m$)
- =de=: the measured motion by the 6 encoders (corresponding to $d\bm{\mathcal{L}}_m$)
#+begin_src matlab
%% Load Identification Data
meas_added_mass = {};

for i_strut = 1:6
    meas_added_mass(i_strut) = {load(sprintf('frf_data_exc_strut_%i_spindle_0m.mat', i_strut), 't', 'Va', 'Vs', 'de')};
end
#+end_src

The window =win= and the frequency vector =f= are defined.
#+begin_src matlab
% Sampling Time [s]
Ts = (meas_added_mass{1}.t(end) - (meas_added_mass{1}.t(1)))/(length(meas_added_mass{1}.t)-1);

% Hannning Windows
win = hanning(ceil(1/Ts));

% And we get the frequency vector
[~, f] = tfestimate(meas_added_mass{1}.Va, meas_added_mass{1}.de, win, [], [], 1/Ts);
#+end_src

Finally the $6 \times 6$ transfer function matrices from $\bm{u}$ to $d\bm{\mathcal{L}}_m$ and from $\bm{u}$ to $\bm{\tau}_m$ are identified:
#+begin_src matlab
%% DVF Plant (transfer function from u to dLm)
G_dL = zeros(length(f), 6, 6);

for i_strut = 1:6
    G_dL(:,:,i_strut) = tfestimate(meas_added_mass{i_strut}.Va, meas_added_mass{i_strut}.de, win, [], [], 1/Ts);
end

%% IFF Plant (transfer function from u to taum)
G_tau = zeros(length(f), 6, 6);

for i_strut = 1:6
    G_tau(:,:,i_strut) = tfestimate(meas_added_mass{i_strut}.Va, meas_added_mass{i_strut}.Vs, win, [], [], 1/Ts);
end
#+end_src

The identified dynamics are then saved for further use.
#+begin_src matlab :exports none :tangle no
save('matlab/data_frf/frf_spindle_m.mat', 'f', 'Ts', 'G_tau', 'G_dL')
#+end_src

#+begin_src matlab :eval no
save('data_frf/frf_spindle_m.mat', 'f', 'Ts', 'G_tau', 'G_dL')
#+end_src

#+begin_src matlab :exports none
frf_ol = load('frf_spindle_m.mat', 'f', 'Ts', 'G_tau', 'G_dL');
frf_vib_tab = load('frf_vib_table_m.mat', 'f', 'Ts', 'G_tau', 'G_dL');
#+end_src

*** Transfer function from Actuator to Encoder
The transfer functions from $u_i$ to $d\mathcal{L}_{m,i}$ are shown in Figure ref:fig:frf_GdL_spindle_0m.

#+begin_src matlab :exports none
%% Bode plot for the transfer function from u to dLm
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:5
    for j = i+1:6
        plot(frf_ol.f, abs(frf_ol.G_dL(:, i, j)), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i = 1:6
    set(gca,'ColorOrderIndex',i)
    plot(frf_ol.f, abs(frf_ol.G_dL(:,i, i)), ...
     'DisplayName', sprintf('$d\\mathcal{L}_{m,%i}/u_%i$', i, i));
end
plot(frf_ol.f, abs(frf_ol.G_dL(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$d\mathcal{L}_{m,i}/u_j$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-8, 1e-3]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);

ax2 = nexttile;
hold on;
for i =1:6
    plot(frf_ol.f, 180/pi*angle(frf_ol.G_dL(:,i, i)));
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([10, 1e3]);
#+end_src

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

#+name: fig:frf_GdL_spindle_0m
#+caption: Measured Frequency Response Functions from $u_i$ to $d\mathcal{L}_{m,i}$ when the nano-hexapod is fixed to the Spindle
#+RESULTS:
[[file:figs/frf_GdL_spindle_0m.png]]

The dynamics of the nano-hexapod when fixed on the Spindle is compared with the dynamics when the nano-hexapod is fixed on the "vibration table" in Figure ref:fig:frf_GdL_comp_spindle_vib_table_0m.

#+begin_question
Why are the frequency of the suspension modes are *decreased* when the nano-hexapod is fixed to the Spindle?
#+end_question

#+begin_src matlab :exports none
%% Bode plot for the transfer function from u to dLm
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:5
    for j = i+1:6
        plot(frf_ol.f, abs(frf_ol.G_dL(:,i,j)), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(frf_vib_tab.f, abs(frf_vib_tab.G_dL{1}(:,i,j)), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
plot(frf_ol.f, abs(frf_ol.G_dL(:,1,1)), 'color', colors(1,:), ...
     'DisplayName', 'Spindle');
plot(frf_vib_tab.f, abs(frf_vib_tab.G_dL{1}(:,1,1)), 'color', colors(2,:), ...
     'DisplayName', 'Vib. Table');
for i = 2:6
    plot(frf_ol.f, abs(frf_ol.G_dL(:,i, i)), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
    plot(frf_vib_tab.f, abs(frf_vib_tab.G_dL{1}(:,i, i)), 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
plot(frf_ol.f, abs(frf_ol.G_dL(:,1,2)), 'color', [colors(1,:), 0.2], ...
     'DisplayName', 'Spindle - Coupling');
plot(frf_vib_tab.f, abs(frf_vib_tab.G_dL{1}(:,1,2)), 'color', [colors(2,:), 0.2], ...
     'DisplayName', 'Vib. Table - Coupling');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-8, 1e-3]);
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);

ax2 = nexttile;
hold on;
for i =1:6
    set(gca,'ColorOrderIndex',1)
    plot(frf_ol.f, 180/pi*angle(frf_ol.G_dL(:,i, i)));
    set(gca,'ColorOrderIndex',2)
    plot(frf_vib_tab.f, 180/pi*angle(frf_vib_tab.G_dL{1}(:,i, i)));
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([10, 1e3]);
#+end_src

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

#+name: fig:frf_GdL_comp_spindle_vib_table_0m
#+caption: Comparison of the dynamics from $u$ to $d\mathcal{L}$ when the nano-hexapod is fixed on top of the Spindle and when it is fixed on top of the "Vibration Table".
#+RESULTS:
[[file:figs/frf_GdL_comp_spindle_vib_table_0m.png]]

*** Transfer function from Actuator to Force Sensor
The transfer functions from $u_i$ to $\tau_m$ are shown in Figure ref:fig:frf_Gtau_spindle_0m.

#+begin_src matlab :exports none
%% Bode plot for the transfer function from u to dLm
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:5
    for j = i+1:6
        plot(frf_ol.f, abs(frf_ol.G_tau(:, i, j)), 'color', [0, 0, 0, 0.2], ...
             'HandleVisibility', 'off');
    end
end
for i = 1:6
    set(gca,'ColorOrderIndex',i)
    plot(frf_ol.f, abs(frf_ol.G_tau(:,i, i)), ...
         'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i));
end
plot(frf_ol.f, abs(frf_ol.G_tau(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
     'DisplayName', '$\\tau_{m,i}/u_j$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-2, 1e2]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);

ax2 = nexttile;
hold on;
for i =1:6
    plot(frf_ol.f, 180/pi*angle(frf_ol.G_tau(:,i, i)));
end
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([20, 2e3]);
#+end_src

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

#+name: fig:frf_Gtau_spindle_0m
#+caption: Measured Frequency Response Functions from $u_i$ to $\tau_{m,i}$ when the nano-hexapod is fixed to the Spindle
#+RESULTS:
[[file:figs/frf_Gtau_spindle_0m.png]]

The dynamics of the nano-hexapod when fixed on the Spindle is compared with the dynamics when the nano-hexapod is fixed on the "vibration table" in Figure ref:fig:frf_Gtau_comp_spindle_vib_table_0m.

#+begin_src matlab :exports none
%% Bode plot for the transfer function from u to taum
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:5
    for j = i+1:6
        plot(frf_ol.f, abs(frf_ol.G_tau(:,i,j)), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(frf_vib_tab.f, abs(frf_vib_tab.G_tau{1}(:,i,j)), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
plot(frf_ol.f, abs(frf_ol.G_tau(:,1,1)), 'color', colors(1,:), ...
     'DisplayName', 'Spindle');
plot(frf_vib_tab.f, abs(frf_vib_tab.G_tau{1}(:,1,1)), 'color', colors(2,:), ...
     'DisplayName', 'Vib. Table');
for i = 2:6
    plot(frf_ol.f, abs(frf_ol.G_tau(:,i, i)), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
    plot(frf_vib_tab.f, abs(frf_vib_tab.G_tau{1}(:,i, i)), 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
plot(frf_ol.f, abs(frf_ol.G_tau(:,1,2)), 'color', [colors(1,:), 0.2], ...
     'DisplayName', 'Spindle - Coupling');
plot(frf_vib_tab.f, abs(frf_vib_tab.G_tau{1}(:,1,2)), 'color', [colors(2,:), 0.2], ...
     'DisplayName', 'Vib. Table - Coupling');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [-]'); set(gca, 'XTickLabel',[]);
ylim([1e-3, 1e2]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);

ax2 = nexttile;
hold on;
for i =1:6
    set(gca,'ColorOrderIndex',1)
    plot(frf_ol.f, 180/pi*angle(frf_ol.G_tau(:,i, i)));
    set(gca,'ColorOrderIndex',2)
    plot(frf_vib_tab.f, 180/pi*angle(frf_vib_tab.G_tau{1}(:,i, i)));
end
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([10, 1e3]);
#+end_src

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

#+name: fig:frf_Gtau_comp_spindle_vib_table_0m
#+caption: Comparison of the dynamics from $u$ to $d\mathcal{L}$ when the nano-hexapod is fixed on top of the Spindle and when it is fixed on top of the "Vibration Table".
#+RESULTS:
[[file:figs/frf_Gtau_comp_spindle_vib_table_0m.png]]

*** Conclusion
#+begin_important
The dynamics of the nano-hexapod does not change a lot when it is fixed to the Spindle.
The "suspension" modes are just increased a little bit due to the added stiffness of the spindle as compared to the vibration table.
#+end_important

** Dynamics of the Damped plant
*** Introduction                                                    :ignore:
As the dynamics is not much changed when the nano-hexapod is fixed on top of the Spindle, the same IFF controller is used to damp the plant.

*** 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

*** Measured Frequency Response Functions
The identification is performed without added mass, and with one, two and three layers of added cylinders.
#+begin_src matlab
i_masses = 0:3;
#+end_src

The following data are loaded:
- =Va=: the excitation voltage (corresponding to $u_i$)
- =Vs=: the generated voltage by the 6 force sensors (corresponding to $\bm{\tau}_m$)
- =de=: the measured motion by the 6 encoders (corresponding to $d\bm{\mathcal{L}}_m$)
#+begin_src matlab
%% Load Identification Data
meas_added_mass = {};

for i_mass = i_masses
    for i_strut = 1:6
        meas_added_mass(i_strut, i_mass+1) = {load(sprintf('frf_data_exc_strut_%i_spindle_%im_iff.mat', i_strut, i_mass), 't', 'Va', 'Vs', 'de')};
    end
end
#+end_src

The window =win= and the frequency vector =f= are defined.
#+begin_src matlab
% Sampling Time [s]
Ts = (meas_added_mass{1,1}.t(end) - (meas_added_mass{1,1}.t(1)))/(length(meas_added_mass{1,1}.t)-1);

% Hannning Windows
win = hanning(ceil(1/Ts));

% And we get the frequency vector
[~, f] = tfestimate(meas_added_mass{1,1}.Va, meas_added_mass{1,1}.de, win, [], [], 1/Ts);
#+end_src

Finally the $6 \times 6$ transfer function matrices from $\bm{u}$ to $d\bm{\mathcal{L}}_m$ and from $\bm{u}$ to $\bm{\tau}_m$ are identified:
#+begin_src matlab
%% DVF Plant (transfer function from u to dLm)
G_dL = {};

for i_mass = i_masses
    G_dL(i_mass+1) = {zeros(length(f), 6, 6)};
    for i_strut = 1:6
        G_dL{i_mass+1}(:,:,i_strut) = tfestimate(meas_added_mass{i_strut, i_mass+1}.Va, meas_added_mass{i_strut, i_mass+1}.de, win, [], [], 1/Ts);
    end
end
#+end_src

The identified dynamics are then saved for further use.
#+begin_src matlab :exports none :tangle no
save('matlab/data_frf/frf_spindle_iff_m.mat', 'f', 'Ts', 'G_dL')
#+end_src

#+begin_src matlab :eval no
save('data_frf/frf_spindle_iff_m.mat', 'f', 'Ts', 'G_dL')
#+end_src

#+begin_src matlab :exports none
frf_ol  = load('frf_spindle_m.mat', 'f', 'Ts', 'G_tau', 'G_dL');
frf_iff = load('frf_spindle_iff_m.mat', 'f', 'Ts', 'G_dL');
#+end_src

*** Effect of Integral Force Feedback
#+begin_src matlab :exports none
%% Bode plot for the transfer function from u to dLm
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:5
    for j = i+1:6
        plot(frf_ol.f, abs(frf_ol.G_dL(:, i, j)), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(frf_ol.f, abs(frf_iff.G_dL{1}(:, i, j)), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
plot(frf_ol.f, abs(frf_ol.G_dL(:,1,1)), 'color', colors(1,:), ...
     'DisplayName', '$d\mathcal{L}_{m,i}/u_j$');
plot(frf_iff.f, abs(frf_iff.G_dL{1}(:,1,1)), 'color', colors(2,:), ...
     'DisplayName', '$d\mathcal{L}_{m,i}/u_j^\prime$');
for i = 2:6
    plot(frf_ol.f, abs(frf_ol.G_dL(:,i, i)), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
    plot(frf_iff.f, abs(frf_iff.G_dL{1}(:,i, i)), 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
plot(frf_ol.f, abs(frf_ol.G_dL(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
     'DisplayName', '$d\mathcal{L}_{m,i}/u_j$');
plot(frf_iff.f, abs(frf_iff.G_dL{1}(:, 1, 2)), 'color', [colors(2,:), 0.2], ...
     'DisplayName', '$d\mathcal{L}_{m,i}/u_j^\prime$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-8, 1e-3]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);

ax2 = nexttile;
hold on;
for i =1:6
    set(gca,'ColorOrderIndex',1)
    plot(frf_ol.f, 180/pi*angle(frf_ol.G_dL(:,i, i)));
    set(gca,'ColorOrderIndex',2)
    plot(frf_iff.f, 180/pi*angle(frf_iff.G_dL{1}(:,i, i)));
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([10, 1e3]);
#+end_src

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

#+name: fig:frf_spindle_comp_ol_iff
#+caption: Effect of Integral Force Feedback on the transfer function from $u_i$ to $d\mathcal{L}_i$
#+RESULTS:
[[file:figs/frf_spindle_comp_ol_iff.png]]

*** Effect of the payload
#+begin_important
From Figure ref:fig:frf_spindle_iff_effect_payload we can see that the coupling is quite large when payloads are added to the nano-hexapod.
This was not the case when the nano-hexapod was fixed to the vibration table.
#+end_important

#+begin_question
What is causing the resonances at 20Hz, 25Hz and 30Hz when there is some added payload?
Why the coupling is much larger than when the nano-hexapod was on top of the isolation table?
#+end_question

#+begin_src matlab :exports none
%% Bode plot for the transfer function from u to dLm
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i_mass = i_masses
    for i = 1:5
        for j = i+1:6
            plot(frf_iff.f, abs(frf_iff.G_dL{i_mass+1}(:, i, j)), 'color', [colors(i_mass+1,:), 0.2], ...
                 'HandleVisibility', 'off');
        end
    end
    plot(frf_iff.f, abs(frf_iff.G_dL{i_mass+1}(:,1, 1)), 'color', colors(i_mass+1,:), ...
         'DisplayName', sprintf('$d\\mathcal{L}_{m,i}/u_i$ - %i', i_mass));
    for i = 2:6
        plot(frf_iff.f, abs(frf_iff.G_dL{i_mass+1}(:,i, i)), 'color', colors(i_mass+1,:), ...
             'HandleVisibility', 'off');
    end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-8, 1e-3]);
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);

ax2 = nexttile;
hold on;
for i_mass = i_masses
    for i =1:6
        plot(frf_iff.f, 180/pi*angle(frf_iff.G_dL{i_mass+1}(:,i, i)), 'color', colors(i_mass+1,:));
    end
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([10, 1e3]);
#+end_src

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

#+name: fig:frf_spindle_iff_effect_payload
#+caption: Effect of the payload on the transfer functions from $u^\prime_i$ to $d\mathcal{L}_i$
#+RESULTS:
[[file:figs/frf_spindle_iff_effect_payload.png]]

*** Effect of rotation
#+begin_src matlab :exports none
%% Load Identification Data
meas_0rpm = {};
meas_60rpm = {};

for i_strut = 1:6
    meas_0rpm(i_strut) = {load(sprintf('frf_data_exc_strut_%i_spindle_3m_iff.mat', i_strut), 't', 'Va', 'Vs', 'de')};
    meas_60rpm(i_strut) = {load(sprintf('frf_data_exc_strut_%i_spindle_3m_iff_60rpm.mat', i_strut), 't', 'Va', 'Vs', 'de')};
end
#+end_src

#+begin_src matlab :exports none
% Sampling Time [s]
Ts = (meas_0rpm{1}.t(end) - (meas_0rpm{1}.t(1)))/(length(meas_0rpm{1}.t)-1);

% Hannning Windows
win = hanning(ceil(1/Ts));

% And we get the frequency vector
[~, f] = tfestimate(meas_0rpm{1}.Va, meas_0rpm{1}.de, win, [], [], 1/Ts);
#+end_src

#+begin_src matlab :exports none
%% DVF Plant (transfer function from u to dLm)
G_dL_0rpm = zeros(length(f), 6, 6);
G_dL_60rpm = zeros(length(f), 6, 6);

for i_strut = 1:6
    G_dL_0rpm(:,:,i_strut)  = tfestimate(meas_0rpm{i_strut}.Va,  meas_0rpm{i_strut}.de,  win, [], [], 1/Ts);
    G_dL_60rpm(:,:,i_strut) = tfestimate(meas_60rpm{i_strut}.Va, meas_60rpm{i_strut}.de, win, [], [], 1/Ts);
end
#+end_src

#+begin_important
The identified plants with and without spindle's rotation are compared in Figure ref:fig:frf_comp_spindle_0rpm_60rpm_3m.
It is shown that the rotational speed as little effect on the plant dynamics.
#+end_important

#+begin_src matlab :exports none
%% Bode plot for the transfer function from u to dLm
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:5
    for j = i+1:6
        plot(f, abs(G_dL_0rpm(:, i, j)), 'color', [colors(1,:), 0.2], ...
             'HandleVisibility', 'off');
        plot(f, abs(G_dL_60rpm(:, i, j)), 'color', [colors(2,:), 0.2], ...
             'HandleVisibility', 'off');
    end
end
plot(f, abs(G_dL_0rpm(:,1,1)), 'color', colors(1,:), ...
     'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - 0rpm');
plot(f, abs(G_dL_60rpm(:,1,1)), 'color', colors(2,:), ...
     'DisplayName', '$d\mathcal{L}_{m,i}/u_j^\prime$ - 60rpm');
for i = 2:6
    plot(f, abs(G_dL_0rpm(:,i, i)), 'color', colors(1,:), ...
         'HandleVisibility', 'off');
    plot(f, abs(G_dL_60rpm(:,i, i)), 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
plot(f, abs(G_dL_0rpm(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
     'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - 0rpm');
plot(f, abs(G_dL_60rpm(:, 1, 2)), 'color', [colors(2,:), 0.2], ...
     'DisplayName', '$d\mathcal{L}_{m,i}/u_j^\prime$ - 60rpm');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
ylim([1e-8, 1e-4]);
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 3);

ax2 = nexttile;
hold on;
for i =1:6
    set(gca,'ColorOrderIndex',1)
    plot(f, 180/pi*angle(G_dL_0rpm(:,i, i)));
    set(gca,'ColorOrderIndex',2)
    plot(f, 180/pi*angle(G_dL_60rpm(:,i, i)));
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([10, 1e3]);
#+end_src

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

#+name: fig:frf_comp_spindle_0rpm_60rpm_3m
#+caption: Comparison of the damped plant when the spindle is not rotating and when it is rotating at 60RPM
#+RESULTS:
[[file:figs/frf_comp_spindle_0rpm_60rpm_3m.png]]

* Helping Functions                                                 :noexport:
** Initialize Path
#+NAME: m-init-path
#+BEGIN_SRC matlab
%% Path for functions, data and scripts
addpath('./matlab/STEPS/'); % Path for STEPS files
addpath('./matlab/subsystems/'); % Path for Simulink subsystems
addpath('./matlab/mat/'); % Path for data
addpath('./matlab/src/'); % Path for functions
addpath('./matlab/');     % Path for scripts
#+END_SRC

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

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

Ts = 1e-4;
data_dir = "/home/thomas/mnt/data_easy/id00/inhouse/MEL/NASS"
#+END_SRC

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

#+begin_src matlab
function   [xc,yc,R,a] = circfit(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