#+TITLE: Modal Analysis - Processing of FRF
:DRAWER:
#+STARTUP: overview

#+LANGUAGE: en
#+EMAIL: dehaeze.thomas@gmail.com
#+AUTHOR: Dehaeze Thomas

#+HTML_LINK_HOME: ../index.html
#+HTML_LINK_UP: ./index.html

#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/zenburn.css"/>
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="../js/bootstrap.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.stickytableheaders.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="../js/readtheorg.js"></script>

#+HTML_MATHJAX: align: center tagside: right font: TeX

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

#+PROPERTY: header-args:shell  :eval no-export

#+PROPERTY: header-args:latex  :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/MEGA/These/LaTeX/}{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 raw replace :buffer no
#+PROPERTY: header-args:latex+ :eval no-export
#+PROPERTY: header-args:latex+ :exports both
#+PROPERTY: header-args:latex+ :mkdirp yes
#+PROPERTY: header-args:latex+ :output-dir figs
:END:

* Introduction                                                       :ignore:
The measurements have been conducted and we have computed the $n \times p \times q$ Frequency Response Functions Matrix with:
- $n$: the number of measurements: $23 \times 3 = 69$ (23 accelerometers measuring 3 directions each)
- $p$: the number of excitation inputs: $3$
- $q$: the number of frequency points $\omega_i$

However, in our model, we only consider 6 solid bodies, namely:
- Bottom Granite
- Top Granite
- Translation Stage
- Tilt Stage
- Spindle
- Hexapod

Thus, we are only interested in $6 \times 6 = 36$ degrees of freedom.

We here process the FRF matrix to go from the 69 measured DOFs to the wanted 36 DOFs.

* ZIP file containing the data and matlab files                      :ignore:
#+begin_src bash :exports none :results none
  if [ matlab/frf_processing.m -nt data/frf_processing.zip ]; then
    cp matlab/frf_processing.m frf_processing.m;
    zip data/frf_processing \
        mat/frf_coh_matrices.mat \
        mat/geometry.mat \
        frf_processing.m
    rm frf_processing.m;
  fi
#+end_src

#+begin_note
  All the files (data and Matlab scripts) are accessible [[file:data/frf_processing.zip][here]].
#+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

* Importation of measured FRF curves
We load the measured FRF and Coherence matrices.
We also load the geometric parameters of the station: solid bodies considered and the position of the accelerometers.

#+begin_src matlab
  load('mat/frf_coh_matrices.mat', 'FRFs', 'COHs', 'freqs');
  load('mat/geometry.mat', 'solids', 'solid_names', 'acc_pos');
#+end_src

* From accelerometer DOFs to solid body DOFs - Mathematics
Let's consider the schematic shown on figure [[fig:local_to_global_coordinates]] where we are measuring the motion of a (supposed) solid body at 4 distinct points in x-y-z.

The goal here is to link these $4 \times 3 = 12$ measurements to the 6 DOFs of the solid body expressed in the frame $\{O\}$.

#+begin_src latex :file local_to_global_coordinates.pdf :post pdf2svg(file=*this*, ext="png") :exports results
  \newcommand\irregularcircle[2]{% radius, irregularity
    \pgfextra {\pgfmathsetmacro\len{(#1)+rand*(#2)}}
    +(0:\len pt)
    \foreach \a in {10,20,...,350}{
      \pgfextra {\pgfmathsetmacro\len{(#1)+rand*(#2)}}
      -- +(\a:\len pt)
    } -- cycle
  }
  \begin{tikzpicture}
    \draw[rounded corners=1mm, fill=blue!30!white] (0, 0) \irregularcircle{3cm}{1mm};

    \node[] (origin) at (4, -1) {$\bullet$};
    \begin{scope}[shift={(origin)}]
      \def\axissize{0.8cm}
      \draw[->] (0, 0) -- ++(\axissize, 0) node[above left]{$x$};
      \draw[->] (0, 0) -- ++(0, \axissize) node[below right]{$y$};
      \draw[fill, color=black] (0, 0) circle (0.05*\axissize);
      \node[draw, circle, inner sep=0pt, minimum size=0.4*\axissize, label=left:$z$] (yaxis) at (0, 0){};
      \node[below right] at (0, 0){$\{O\}$};
    \end{scope}

    \coordinate[] (p1) at (-1.5, -1.5);
    \coordinate[] (p2) at (-1.5,  1.5);
    \coordinate[] (p3) at ( 1.5,  1.5);
    \coordinate[] (p4) at ( 1.5, -1.5);

    \draw[->] (p1)node[]{$\bullet$}node[above]{$p_1$} -- ++( 1  , 0.5)node[right]{$\delta p_1$};
    \draw[->] (p2)node[]{$\bullet$}node[above]{$p_2$} -- ++(-0.5, 1  )node[right]{$\delta p_2$};
    \draw[->] (p3)node[]{$\bullet$}node[above]{$p_3$} -- ++( 1  , 0.5)node[right]{$\delta p_3$};
    \draw[->] (p4)node[]{$\bullet$}node[above]{$p_4$} -- ++( 0.5, 1  )node[right]{$\delta p_4$};
  \end{tikzpicture}
#+end_src

#+name: fig:local_to_global_coordinates
#+caption: Schematic of the measured motions of a solid body
#+RESULTS:
[[file:figs/local_to_global_coordinates.png]]

We consider the motion of the rigid body defined by its displacement $\delta p$ and rotation $\vec{\Omega}$ with respect to the reference frame $\{O\}$.

From the figure [[fig:local_to_global_coordinates]], we can write:
\begin{align*}
  \delta p_1 &= \delta p + \delta\Omega p_1\\
  \delta p_2 &= \delta p + \delta\Omega p_2\\
  \delta p_3 &= \delta p + \delta\Omega p_3\\
  \delta p_4 &= \delta p + \delta\Omega p_4
\end{align*}
With
\begin{equation}
  \delta\Omega = \begin{bmatrix}
   0              & -\delta\Omega_z &  \delta\Omega_y \\
   \delta\Omega_z &  0              & -\delta\Omega_x \\
  -\delta\Omega_y &  \delta\Omega_x &  0
\end{bmatrix}
\end{equation}

We can rearrange the equations in a matrix form:

\begin{equation}
\left[\begin{array}{ccc|ccc}
  1 & 0 & 0 & 0 & p_{1z} & -p_{1y} \\
  0 & 1 & 0 & -p_{1z} & 0 & p_{1x} \\
  0 & 0 & 1 & p_{1y} & -p_{1x} & 0 \\ \hline
    & \vdots &   &    & \vdots &   \\ \hline
  1 & 0 & 0 & 0 & p_{4z} & -p_{4y} \\
  0 & 1 & 0 & -p_{4z} & 0 & p_{4x} \\
  0 & 0 & 1 & p_{4y} & -p_{4x} & 0
\end{array}\right] \begin{bmatrix}
  \delta p_x \\ \delta p_y \\ \delta p_z \\ \hline \delta\Omega_x \\ \delta\Omega_y \\ \delta\Omega_z
\end{bmatrix} = \begin{bmatrix}
  \delta p_{1x} \\ \delta p_{1y} \\ \delta p_{1z} \\\hline \vdots \\\hline \delta p_{4x} \\ \delta p_{4y} \\ \delta p_{4z}
\end{bmatrix}
\end{equation}

and then we obtain the velocity and rotation of the solid in the wanted frame $\{O\}$:
\begin{equation}
\begin{bmatrix}
  \delta p_x \\ \delta p_y \\ \delta p_z \\ \hline \delta\Omega_x \\ \delta\Omega_y \\ \delta\Omega_z
\end{bmatrix} =
\left[\begin{array}{ccc|ccc}
  1 & 0 & 0 & 0 & p_{1z} & -p_{1y} \\
  0 & 1 & 0 & -p_{1z} & 0 & p_{1x} \\
  0 & 0 & 1 & p_{1y} & -p_{1x} & 0 \\ \hline
    & \vdots &   &    & \vdots &   \\ \hline
  1 & 0 & 0 & 0 & p_{4z} & -p_{4y} \\
  0 & 1 & 0 & -p_{4z} & 0 & p_{4x} \\
  0 & 0 & 1 & p_{4y} & -p_{4x} & 0
\end{array}\right]^{-1} \begin{bmatrix}
  \delta p_{1x} \\ \delta p_{1y} \\ \delta p_{1z} \\\hline \vdots \\\hline \delta p_{4x} \\ \delta p_{4y} \\ \delta p_{4z}
\end{bmatrix} \label{eq:determine_global_disp}
\end{equation}

#+begin_important
  Using equation eqref:eq:determine_global_disp, we can determine the motion of the solid body expressed in a chosen frame $\{O\}$ using the accelerometers attached to it.
  The inversion is equivalent to resolving a mean square problem.
#+end_important

* What reference frame to choose?
The question we wish here to answer is how to choose the reference frame $\{O\}$ in which the DOFs of the solid bodies are defined.

The possibles choices are:
- *One frame for each solid body* which is located at its center of mass
- *One common frame*, for instance located at the point of interest ($270mm$ above the Hexapod)
- *Base located at the joint position*: this is where we want to see the motion and estimate stiffness

#+name: tab:frame_comparison
#+caption: Advantages and disadvantages for the choice of reference frame
| Chosen Frame             | Advantages                                          | Disadvantages                                        |
|--------------------------+-----------------------------------------------------+------------------------------------------------------|
| Frames at CoM            | Physically, it makes more sense                     | How to compare the motion of the solid bodies?       |
| Common Frame             | We can compare the motion of each solid body        | Small $\theta_{x, y}$ may result in large $T_{x, y}$ |
| Frames at joint position | Directly gives which joint direction can be blocked | How to choose the joint position?                    |

The choice of the frame depends of what we want to do with the data.

One of the goals is to compare the motion of each solid body to see which relative DOFs between solid bodies can be neglected, that is to say, which joint between solid bodies can be regarded as perfect (and this in all the frequency range of interest).
Ideally, we would like to have the same number of degrees of freedom than the number of identified modes.

In the next sections, we will express the FRF matrix in the different frames.

* FRF expressed in a common frame
** From accelerometer DOFs to solid body DOFs - Matlab Implementation
First, we initialize a new FRF matrix =FRFs_O= which is an $n \times p \times q$ with:
- $n$ is the number of DOFs of the considered 6 solid-bodies: $6 \times 6 = 36$
- $p$ is the number of excitation inputs: $3$
- $q$ is the number of frequency points $\omega_i$

#+begin_important
For each frequency point $\omega_i$, the FRF matrix =FRFs_O= is a $n\times p$ matrix:
\begin{equation}
  \text{FRF}_\text{O}(\omega_i) = \begin{bmatrix}
  \frac{D_{1,T_x}}{F_x}(\omega_i) & \frac{D_{1,T_x}}{F_y}(\omega_i) & \frac{D_{1,T_x}}{F_z}(\omega_i) \\
  \frac{D_{1,T_y}}{F_x}(\omega_i) & \frac{D_{1,T_y}}{F_y}(\omega_i) & \frac{D_{1,T_y}}{F_z}(\omega_i) \\
  \frac{D_{1,T_z}}{F_x}(\omega_i) & \frac{D_{1,T_z}}{F_y}(\omega_i) & \frac{D_{1,T_z}}{F_z}(\omega_i) \\
  \frac{D_{1,R_x}}{F_x}(\omega_i) & \frac{D_{1,R_x}}{F_y}(\omega_i) & \frac{D_{1,R_x}}{F_z}(\omega_i) \\
  \frac{D_{1,R_y}}{F_x}(\omega_i) & \frac{D_{1,R_y}}{F_y}(\omega_i) & \frac{D_{1,R_y}}{F_z}(\omega_i) \\
  \frac{D_{1,R_z}}{F_x}(\omega_i) & \frac{D_{1,R_z}}{F_y}(\omega_i) & \frac{D_{1,R_z}}{F_z}(\omega_i) \\
  \frac{D_{2,T_x}}{F_x}(\omega_i) & \frac{D_{2,T_x}}{F_y}(\omega_i) & \frac{D_{2,T_x}}{F_z}(\omega_i) \\
  \vdots              & \vdots              & \vdots              \\
  \frac{D_{6,R_z}}{F_x}(\omega_i) & \frac{D_{6,R_z}}{F_y}(\omega_i) & \frac{D_{6,R_z}}{F_z}(\omega_i)
\end{bmatrix}
\end{equation}
where $D_i$ corresponds to the solid body number i.
#+end_important

Then, as we know the positions of the accelerometers on each solid body, and we have the response of those accelerometers, we can use the equations derived in the previous section to determine the response of each solid body expressed in the frame $\{O\}$.

#+begin_src matlab
  FRFs_O = zeros(length(solid_names)*6, 3, 801);

  for solid_i = 1:length(solid_names)
    solids_i = solids.(solid_names{solid_i});

    A = zeros(3*length(solids_i), 6);
    for i = 1:length(solids_i)
      acc_i = solids_i(i);

      A(3*(i-1)+1:3*i, 1:3) = eye(3);
      A(3*(i-1)+1:3*i, 4:6) = [ 0                  acc_pos(acc_i, 3) -acc_pos(acc_i, 2) ;
                               -acc_pos(acc_i, 3)  0                  acc_pos(acc_i, 1) ;
                                acc_pos(acc_i, 2) -acc_pos(acc_i, 1)  0];
    end

    for exc_dir = 1:3
      FRFs_O((solid_i-1)*6+1:solid_i*6, exc_dir, :) = A\squeeze(FRFs((solids_i(1)-1)*3+1:solids_i(end)*3, exc_dir, :));
    end
  end
#+end_src

** Analysis of some FRF in the global coordinates
First, we can compare the motions of the 6 solid bodies in one direction (figure [[fig:frf_all_bodies_one_direction]])

We can also compare all the DOFs of one solid body (figure [[fig:frf_one_body_all_directions]]).

#+begin_src matlab :exports none
  exc_names = {'$F_x$', '$F_y$', '$F_z$'};
  DOFs = {'$T_x$', '$T_y$', '$T_z$', '$\theta_x$', '$\theta_y$', '$\theta_z$'};
  solids_i = 1:6;
  dir_i = 1;
  exc_dir = 1;

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for solid_i = solids_i
    plot(freqs, abs(squeeze(FRFs_O((solid_i-1)*6+dir_i, exc_dir, :))), 'DisplayName', solid_names{solid_i});
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Amplitude [$\frac{m/s^2}{N}$]');
  legend('Location', 'northwest');
  title(sprintf('FRF between %s and %s', exc_names{exc_dir}, DOFs{dir_i}));

  ax2 = subplot(2, 1, 2);
  hold on;
  for solid_i = solids_i
    plot(freqs, mod(180+180/pi*phase(squeeze(FRFs_O((solid_i-1)*6+dir_i, exc_dir, :))), 360)-180);
  end
  hold off;
  ylim([-180, 180]); yticks(-180:90:180);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  set(gca, 'xscale', 'log');

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

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/frf_all_bodies_one_direction.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:frf_all_bodies_one_direction
#+CAPTION: FRFs of all the 6 solid bodies in one direction
[[file:figs/frf_all_bodies_one_direction.png]]

#+begin_src matlab :exports none
  DOFs = {'$T_x$', '$T_y$', '$T_z$', '$\theta_x$', '$\theta_y$', '$\theta_z$'};
  solid_i = 3;
  dirs_i = 1:6;
  exc_dir = 1;

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for dir_i = dirs_i
    plot(freqs, abs(squeeze(FRFs_O((solid_i-1)*6+dir_i, exc_dir, :))), 'DisplayName', DOFs{dir_i});
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Amplitude [$\frac{m/s^2}{N}$]');
  legend('Location', 'northwest');
  title(sprintf('Motion of %s due to %s', solid_names{solid_i}, exc_names{exc_dir}));

  ax2 = subplot(2, 1, 2);
  hold on;
  for dir_i = dirs_i
    plot(freqs, mod(180+180/pi*phase(squeeze(FRFs_O((solid_i-1)*6+dir_i, exc_dir, :))), 360)-180);
  end
  hold off;
  ylim([-180, 180]); yticks(-180:90:180);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  set(gca, 'xscale', 'log');

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

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/frf_one_body_all_directions.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:frf_one_body_all_directions
#+CAPTION: FRFs of one solid body in all its DOFs
[[file:figs/frf_one_body_all_directions.png]]

** Comparison of the relative motion of solid bodies
Now that the motion of all the solid bodies are expressed in the same frame, we should be able to *compare them*.
This can be used to determine what joints direction between two solid bodies is stiff enough that we can fix this DoF.
This could help reduce the order of the model and simplify the extraction of the model parameters from the measurements.

We decide to plot the "normalized relative motion" between solid bodies $i$ and $j$:
\[ 0 < \Delta_{ij, x} = \frac{\left| |D_{i,x}| - |D_{j,x}| \right|}{|D_{i,x}| + |D_{j,x}|} < 1 \]

Then, if $\Delta_{ij,x} \ll 1$ in the frequency band of interest, we have that $D_{ix} \approx D_{jx}$ and we can neglect that DOF between the two solid bodies $i$ and $j$.

This normalized relative motion is shown on figure [[fig:relative_motion_comparison]] for all the directions and for all the adjacent pair of solid bodies.

#+begin_src matlab :exports none
  DOFs = {'$T_x$', '$T_y$', '$T_z$', '$\theta_x$', '$\theta_y$', '$\theta_z$'};

  dirs_i = 1:6;
  exc_dir = 1;

  figure;

  for i = 2:6
    subplot(3, 2, i);
    hold on;
    for dir_i = dirs_i
      H = (abs(squeeze(FRFs_O((i-1)*6+dir_i, exc_dir, :)))-abs(squeeze(FRFs_O((i-2)*6+dir_i, exc_dir, :))))./(abs(squeeze(FRFs_O((i-1)*6+dir_i, exc_dir, :)))+abs(squeeze(FRFs_O((i-2)*6+dir_i, exc_dir, :))));
      plot(freqs, abs(H));
    end
    hold off;
    set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
    xlim([1, 200]); ylim([0, 1]);
    % xlabel('Frequency [Hz]'); ylabel('Relative Motion');
    title(sprintf('Normalized motion %s - %s', solid_names{i-1}, solid_names{i}));
    if i > 4
      xlabel('Frequency [Hz]');
    else
      set(gca, 'XTickLabel',[]);
    end
  end

  for i = 1:length(dirs_i)
    legend_names{i} = DOFs{dirs_i(i)};
  end
  lgd = legend(legend_names);

  hL = subplot(3, 2, 1);
  poshL = get(hL,'position');

  set(lgd,'position', poshL);
  axis(hL, 'off');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/relative_motion_comparison.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:relative_motion_comparison
#+CAPTION: Relative motion between each stage
[[file:figs/relative_motion_comparison.png]]

#+begin_warning
  Can we really compare the motion of two solid bodies from Frequency Response Functions that clearly depends on the excitation point and direction?
  The relative motion of two solid bodies may be negligible when exciting the structure at on point and but at another point.
#+end_warning

** Verify that we find the original FRF from the FRF in the global coordinates
We have computed the Frequency Response Functions Matrix =FRFs_O= representing the response of the 6 solid bodies in their 6 DOFs with respect to the frame $\{O\}$.

From the response of one body in its 6 DOFs, we should be able to compute the FRF of each of its accelerometer fixed to it during the measurement, supposing that the stage is a solid body.

We can then compare the result with the original measurements.
This will help us to determine if:
- the previous inversion used is correct
- the solid body assumption is correct in the frequency band of interest

From the translation $\delta p$ and rotation $\delta \Omega$ of a solid body and the positions $p_i$ of the accelerometers attached to it, we can compute the response that would have been measured by the accelerometers using the following formula:
\begin{align*}
  \delta p_1 &= \delta p + \delta\Omega p_1\\
  \delta p_2 &= \delta p + \delta\Omega p_2\\
  \delta p_3 &= \delta p + \delta\Omega p_3\\
  \delta p_4 &= \delta p + \delta\Omega p_4
\end{align*}

Thus, we can obtain the FRF matrix =FRFs_A= that gives the responses of the accelerometers to the forces applied by the hammer.

It is implemented in matlab as follow:
#+begin_src matlab
  FRFs_A = zeros(size(FRFs));

  % For each excitation direction
  for exc_dir = 1:3
    % For each solid
    for solid_i = 1:length(solid_names)
      v0 = squeeze(FRFs_O((solid_i-1)*6+1:(solid_i-1)*6+3, exc_dir, :));
      W0 = squeeze(FRFs_O((solid_i-1)*6+4:(solid_i-1)*6+6, exc_dir, :));

      % For each accelerometer attached to the current solid
      for acc_i = solids.(solid_names{solid_i})
        % We get the position of the accelerometer expressed in frame O
        pos = acc_pos(acc_i, :).';
        posX = [0 pos(3) -pos(2); -pos(3) 0 pos(1) ; pos(2) -pos(1) 0];

        FRFs_A(3*(acc_i-1)+1:3*(acc_i-1)+3, exc_dir, :) = v0 + posX*W0;
      end
    end
  end
#+end_src

We then compare the original FRF measured for each accelerometer =FRFs= with the "recovered" FRF =FRFs_A= from the global FRF matrix in the common frame.

The FRF for the 4 accelerometers on the Hexapod are compared on figure [[fig:recovered_frf_comparison_hexa]].
All the FRF are matching very well in all the frequency range displayed.

The FRF for accelerometers located on the translation stage are compared on figure [[fig:recovered_frf_comparison_ty]].
The FRF are matching well until 100Hz.

#+begin_src matlab :exports none
  exc_names = {'$F_x$', '$F_y$', '$F_z$'};
  DOFs = {'$T_x$', '$T_y$', '$T_z$', '$\theta_x$', '$\theta_y$', '$\theta_z$'};

  solid_i = 6;
  exc_dir = 1;

  accs_i = solids.(solid_names{solid_i});

  figure;

  for i = 1:length(accs_i)
    acc_i = accs_i(i);

    subplot(2, 2, i);

    hold on;
    for dir_i = 1:3
      plot(freqs, abs(squeeze(FRFs(3*(acc_i-1)+dir_i, exc_dir, :))), '-', 'DisplayName', DOFs{dir_i});
    end
    set(gca,'ColorOrderIndex',1)
    for dir_i = 1:3
      plot(freqs, abs(squeeze(FRFs_A(3*(acc_i-1)+dir_i, exc_dir, :))), '--', 'HandleVisibility', 'off');
    end
    hold off;

    set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
    if i > 2
      xlabel('Frequency [Hz]');
    else
      set(gca, 'XTickLabel',[]);
    end

    if rem(i, 2) == 1
      ylabel('Amplitude [$\frac{m/s^2}{N}$]');
    end

    xlim([1, 200]); ylim([1e-6, 1e-1]);
    title(sprintf('Accelerometer %i', accs_i(i)));
    legend('location', 'northwest');
  end
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/recovered_frf_comparison_hexa.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:recovered_frf_comparison_hexa
#+CAPTION: Comparison of the original FRF with the recovered ones - Hexapod
[[file:figs/recovered_frf_comparison_hexa.png]]

#+begin_src matlab :exports none
  exc_names = {'$F_x$', '$F_y$', '$F_z$'};
  DOFs = {'$T_x$', '$T_y$', '$T_z$', '$\theta_x$', '$\theta_y$', '$\theta_z$'};

  solid_i = 3;
  exc_dir = 1;

  accs_i = solids.(solid_names{solid_i});

  figure;

  for i = 1:length(accs_i)
    acc_i = accs_i(i);

    subplot(2, 2, i);

    hold on;
    for dir_i = 1:3
      plot(freqs, abs(squeeze(FRFs(3*(acc_i-1)+dir_i, exc_dir, :))), '-', 'DisplayName', DOFs{dir_i});
    end
    set(gca,'ColorOrderIndex',1)
    for dir_i = 1:3
      plot(freqs, abs(squeeze(FRFs_A(3*(acc_i-1)+dir_i, exc_dir, :))), '--', 'HandleVisibility', 'off');
    end
    hold off;

    set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
    if i > 2
      xlabel('Frequency [Hz]');
    else
      set(gca, 'XTickLabel',[]);
    end

    if rem(i, 2) == 1
      ylabel('Amplitude [$\frac{m/s^2}{N}$]');
    end

    xlim([1, 200]); ylim([1e-6, 1e-1]);
    title(sprintf('Accelerometer %i', accs_i(i)));
    legend('location', 'northwest');
  end
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/recovered_frf_comparison_ty.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:recovered_frf_comparison_ty
#+CAPTION: Comparison of the original FRF with the recovered ones - Ty
[[file:figs/recovered_frf_comparison_ty.png]]


#+begin_important
  The reduction of the number of degrees of freedom from 69 (23 accelerometers with each 3DOF) to 36 (6 solid bodies with 6 DOF) seems to work well.

  This confirms the fact that the stages are indeed behaving as a solid body in the frequency band of interest.
  This valid the fact that a multi-body model can be used to represent the dynamics of the micro-station.
#+end_important

** Saving of the FRF expressed in the global coordinates
#+begin_src matlab
  save('mat/frf_o.mat', 'FRFs_O');
#+end_src

* FRF expressed in a frame centered at the CoM of each solid body
** Center of Mass of each solid body
From solidworks, we can export the position of the center of mass of each solid body considered.

For that, we have to suppose density of the materials.
We then process the exported file.

#+begin_src bash :results none
  sed -e '1,/^Center of mass/ d' mat/1_granite_bot.txt | sed 3q | sed 's/^\s*\t*[XYZ] = \([-0-9.]*\)\r/\1/' >  mat/model_solidworks_com.txt
  sed -e '1,/^Center of mass/ d' mat/2_granite_top.txt | sed 3q | sed 's/^\s*\t*[XYZ] = \([-0-9.]*\)\r/\1/' >> mat/model_solidworks_com.txt
  sed -e '1,/^Center of mass/ d' mat/3_ty.txt          | sed 3q | sed 's/^\s*\t*[XYZ] = \([-0-9.]*\)\r/\1/' >> mat/model_solidworks_com.txt
  sed -e '1,/^Center of mass/ d' mat/4_ry.txt          | sed 3q | sed 's/^\s*\t*[XYZ] = \([-0-9.]*\)\r/\1/' >> mat/model_solidworks_com.txt
  sed -e '1,/^Center of mass/ d' mat/5_rz.txt          | sed 3q | sed 's/^\s*\t*[XYZ] = \([-0-9.]*\)\r/\1/' >> mat/model_solidworks_com.txt
  sed -e '1,/^Center of mass/ d' mat/6_hexa.txt        | sed 3q | sed 's/^\s*\t*[XYZ] = \([-0-9.]*\)\r/\1/' >> mat/model_solidworks_com.txt
#+end_src

And we import the Center of mass on Matlab.
#+begin_src matlab
  model_com = table2array(readtable('mat/model_solidworks_com.txt', 'ReadVariableNames', false));
#+end_src

#+begin_src matlab
  model_com = reshape(model_com , [3, 6]);
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
  data2orgtable(1000*model_com, {'X [mm]', 'Y [mm]', 'Z [mm]'}, {'granite bot', 'granite top', 'ty', 'ry', 'rz', 'hexa'}, ' %.0f ');
#+end_src

#+RESULTS:
|        | granite bot | granite top |   ty |   ry |   rz | hexa |
|--------+-------------+-------------+------+------+------+------|
| X [mm] |          45 |          52 |    0 |    0 |    0 |   -4 |
| Y [mm] |         144 |         258 |   14 |   -5 |    0 |    6 |
| Z [mm] |       -1251 |        -778 | -600 | -628 | -580 | -319 |

** From accelerometer DOFs to solid body DOFs - Expressed at the CoM
First, we initialize a new FRF matrix =FRFs_CoM= which is an $n \times p \times q$ with:
- $n$ is the number of DOFs of the considered 6 solid-bodies: $6 \times 6 = 36$
- $p$ is the number of excitation inputs: $3$
- $q$ is the number of frequency points $\omega_i$

#+begin_important
For each frequency point $\omega_i$, the FRF matrix =FRFs_CoM= is a $n\times p$ matrix:
\begin{equation}
  \text{FRF}_\text{CoM}(\omega_i) = \begin{bmatrix}
  \frac{D_{1,T_x}}{F_x}(\omega_i) & \frac{D_{1,T_x}}{F_y}(\omega_i) & \frac{D_{1,T_x}}{F_z}(\omega_i) \\
  \frac{D_{1,T_y}}{F_x}(\omega_i) & \frac{D_{1,T_y}}{F_y}(\omega_i) & \frac{D_{1,T_y}}{F_z}(\omega_i) \\
  \frac{D_{1,T_z}}{F_x}(\omega_i) & \frac{D_{1,T_z}}{F_y}(\omega_i) & \frac{D_{1,T_z}}{F_z}(\omega_i) \\
  \frac{D_{1,R_x}}{F_x}(\omega_i) & \frac{D_{1,R_x}}{F_y}(\omega_i) & \frac{D_{1,R_x}}{F_z}(\omega_i) \\
  \frac{D_{1,R_y}}{F_x}(\omega_i) & \frac{D_{1,R_y}}{F_y}(\omega_i) & \frac{D_{1,R_y}}{F_z}(\omega_i) \\
  \frac{D_{1,R_z}}{F_x}(\omega_i) & \frac{D_{1,R_z}}{F_y}(\omega_i) & \frac{D_{1,R_z}}{F_z}(\omega_i) \\
  \frac{D_{2,T_x}}{F_x}(\omega_i) & \frac{D_{2,T_x}}{F_y}(\omega_i) & \frac{D_{2,T_x}}{F_z}(\omega_i) \\
  \vdots              & \vdots              & \vdots              \\
  \frac{D_{6,R_z}}{F_x}(\omega_i) & \frac{D_{6,R_z}}{F_y}(\omega_i) & \frac{D_{6,R_z}}{F_z}(\omega_i)
\end{bmatrix}
\end{equation}
where 1, 2, ..., 6 corresponds to the 6 solid bodies.
#+end_important

#+begin_src matlab
  FRFs_CoM = zeros(length(solid_names)*6, 3, 801);
#+end_src

Then, as we know the positions of the accelerometers on each solid body, and we have the response of those accelerometers, we can use the equations derived in the previous section to determine the response of each solid body expressed in the frame $\{O\}$.
#+begin_src matlab
  for solid_i = 1:length(solid_names)
    solids_i = solids.(solid_names{solid_i});

    A = zeros(3*length(solids_i), 6);
    for i = 1:length(solids_i)
      acc_i = solids_i(i);

      acc_pos_com = acc_pos(acc_i, :).' - model_com(:, solid_i);

      A(3*(i-1)+1:3*i, 1:3) = eye(3);
      A(3*(i-1)+1:3*i, 4:6) = [ 0               acc_pos_com(3) -acc_pos_com(2) ;
                               -acc_pos_com(3)  0               acc_pos_com(1) ;
                                acc_pos_com(2) -acc_pos_com(1)  0];
    end

    for exc_dir = 1:3
      FRFs_CoM((solid_i-1)*6+1:solid_i*6, exc_dir, :) = A\squeeze(FRFs((solids_i(1)-1)*3+1:solids_i(end)*3, exc_dir, :));
    end
  end
#+end_src

** Analysis of some FRF
#+begin_src matlab :exports none
  exc_names = {'$F_x$', '$F_y$', '$F_z$'};
  DOFs = {'$T_x$', '$T_y$', '$T_z$', '$\theta_x$', '$\theta_y$', '$\theta_z$'};
  solids_i = 1:6;
  dir_i = 1;
  exc_dir = 1;

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for solid_i = solids_i
    plot(freqs, abs(squeeze(FRFs_CoM((solid_i-1)*6+dir_i, exc_dir, :))), 'DisplayName', solid_names{solid_i});
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Amplitude [$\frac{m/s^2}{N}$]');
  legend('Location', 'northwest');
  title(sprintf('FRF between %s and %s', exc_names{exc_dir}, DOFs{dir_i}));

  ax2 = subplot(2, 1, 2);
  hold on;
  for solid_i = solids_i
    plot(freqs, mod(180+180/pi*phase(squeeze(FRFs_CoM((solid_i-1)*6+dir_i, exc_dir, :))), 360)-180);
  end
  hold off;
  ylim([-180, 180]); yticks(-180:90:180);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  set(gca, 'xscale', 'log');

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

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/frf_com_all_bodies_one_direction.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:frf_com_all_bodies_one_direction
#+CAPTION: FRFs of all the 6 solid bodies in one direction
[[file:figs/frf_com_all_bodies_one_direction.png]]

#+begin_src matlab :exports none
  DOFs = {'$T_x$', '$T_y$', '$T_z$', '$\theta_x$', '$\theta_y$', '$\theta_z$'};
  solid_i = 6;
  dirs_i = 1:6;
  exc_dir = 3;

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for dir_i = dirs_i
    plot(freqs, abs(squeeze(FRFs_CoM((solid_i-1)*6+dir_i, exc_dir, :))), 'DisplayName', DOFs{dir_i});
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Amplitude [$\frac{m/s^2}{N}$]');
  legend('Location', 'northwest');
  title(sprintf('Motion of %s due to %s', solid_names{solid_i}, exc_names{exc_dir}));

  ax2 = subplot(2, 1, 2);
  hold on;
  for dir_i = dirs_i
    plot(freqs, mod(180+180/pi*phase(squeeze(FRFs_CoM((solid_i-1)*6+dir_i, exc_dir, :))), 360)-180);
  end
  hold off;
  ylim([-180, 180]); yticks(-180:90:180);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  set(gca, 'xscale', 'log');

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

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/frf_com_one_body_all_directions.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:frf_com_one_body_all_directions
#+CAPTION: FRFs of one solid body in all its DOFs (expressed with a frame centered with its center of mass)
[[file:figs/frf_com_one_body_all_directions.png]]

** Save the FRF
#+begin_src matlab
  save('mat/frf_com.mat', 'FRFs_CoM');
#+end_src