test-bench-apa300ml/test-bench-apa300ml.org

#+TITLE: Nano-Hexapod Struts - 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 ""
#+BIND: org-latex-bib-compiler "biber"

#+LaTeX_CLASS: scrreprt
#+LaTeX_CLASS_OPTIONS: [a4paper, 10pt, DIV=12, parskip=full]
#+LaTeX_HEADER_EXTRA: \input{preamble.tex}
#+LATEX_HEADER_EXTRA: \addbibresource{ref.bib}

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

#+PROPERTY: header-args:latex  :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/tikz/org/}{config.tex}")
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
#+PROPERTY: header-args:latex+ :results file raw replace
#+PROPERTY: header-args:latex+ :buffer no
#+PROPERTY: header-args:latex+ :tangle no
#+PROPERTY: header-args:latex+ :eval no-export
#+PROPERTY: header-args:latex+ :exports results
#+PROPERTY: header-args:latex+ :mkdirp yes
#+PROPERTY: header-args:latex+ :output-dir figs
#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png")
:END:

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

#+latex: \clearpage

* Introduction                                                        :ignore:

In this document, a test-bench is used to characterize the struts of the nano-hexapod.

Each strut includes (Figure [[fig:picture_strut_top_view]]):
- 2 flexible joints at each ends.
  These flexible joints have been characterized in a [[file:../test-bench-nass-flexible-joints/test-bench-flexible-joints.org][separate test bench]].
- 1 Amplified Piezoelectric Actuator (APA300ML) (described in Section [[sec:model_apa]]).
  Two stacks are used as an actuator and one stack as a (force) sensor.
- 1 encoder (Renishaw Vionic) that has been characterized in a [[file:../test-bench-vionic/test-bench-vionic.org][separate test bench]].

#+name: fig:picture_strut_top_view
#+caption: One strut including two flexible joints, an amplified piezoelectric actuator and an encoder
#+attr_latex: :width 0.8\linewidth
[[file:figs/picture_strut_top_view.jpg]]

The first goal is to characterize the APA300ML in terms of:
- The, geometric features, electrical capacitance, stroke, hysteresis, spurious resonances.
  This is performed in Section [[sec:first_measurements]].
- The dynamics from the generated DAC voltage (going to the voltage amplifiers and then applied on the actuator stacks) to the induced displacement, and to the measured voltage by the force sensor stack.
  Also the "actuator constant" and "sensor constant" are identified.
  This is done in Section [[sec:dynamical_meas_apa]].
- Compare the measurements with the Simscape models (2DoF, Super-Element) in order to tuned/validate the models.
  This is explained in Section [[sec:simscape_bench_apa]].

Then the struts are mounted (procedure described [[file:../test-bench-strut-mounting/test-bench-strut-mounting.org][here]]), and are fixed to the same measurement bench.
Similarly, the goals are to:
- Section [[sec:dynamical_meas_struts]]: Identify the dynamics from the generated DAC voltage to:
  - the sensors stack generated voltage
  - the measured displacement by the encoder
  - the measured displacement by the interferometer (representing encoders that would be fixed to the nano-hexapod's plates instead of the struts)
- Section [[sec:simscape_bench_struts]]: Compare the measurements with the Simscape model of the struts and tune the models

The final goal of the work presented in this document is to have an accurate Simscape model of the struts that can then be included in the Simscape model of the nano-hexapod.

* Model of the Amplified Piezoelectric Actuator
<<sec:model_apa>>
** Introduction                                                      :ignore:
The Amplified Piezoelectric Actuator (APA) used is the APA300ML from Cedrat technologies (Figure [[fig:apa300ML]]).

#+name: fig:apa300ML
#+caption: Picture of the APA300ML
#+attr_latex: :width 0.8\linewidth
[[file:figs/apa300ML.png]]

Two simscape models of the APA300ML are developed:
- Section [[sec:apa_2dof_model]]: a simple 2 degrees of freedom (DoF) model
- Section [[sec:apa_flexible_model]]: a "flexible" model using a "super-element" extracted from a Finite Element Model of the APA

For both models, an "actuator constant" and a "sensor constant" are used.
These constants are used to link the electrical domain and the mechanical domain.
They are described in Section [[sec:apa_constants]].

** Two Degrees of Freedom Model
<<sec:apa_2dof_model>>

The presented model is based on cite:souleille18_concep_activ_mount_space_applic and represented in Figure [[fig:souleille18_model_piezo]].

#+name: fig:souleille18_model_piezo
#+caption: Picture of an APA100M from Cedrat Technologies. Simplified model of a one DoF payload mounted on such isolator
#+attr_latex: :width 0.6\linewidth
[[file:./figs/souleille18_model_piezo.png]]

The parameters are described in Table [[tab:souleille18_model_params]].

#+name: tab:souleille18_model_params
#+caption: Parameters used for the model of the APA 100M
#+attr_latex: :environment tabularx :width 0.6\linewidth :align lX
#+attr_latex: :center t :booktabs t :float t
|       | *Meaning*                                                      |
|-------+----------------------------------------------------------------|
| $k_e$ | Stiffness used to adjust the pole of the isolator              |
| $k_1$ | Stiffness of the metallic suspension when the stack is removed |
| $k_a$ | Stiffness of the actuator                                      |
| $c_1$ | Added viscous damping                                          |

The model is shown again in Figure [[fig:2dof_apa_model]].
As will be shown in the next section, such model can be quite accurate in modelling the axial behavior of the APA.
However, it does not model the flexibility of the APA in the other directions.

Therefore this model can be useful for quick simulations as it contains a very limited number of states, but when more complex dynamics of the APA is to be modelled, a flexible model will be used.

#+name: fig:2dof_apa_model
#+attr_latex: :width 0.2\linewidth
#+caption: Schematic of the 2DoF model for the Amplified Piezoelectric Actuator
[[file:figs/2dof_apa_model.png]]

** Flexible Model
<<sec:apa_flexible_model>>

In order to model with high accuracy the behavior of the APA, a flexible model can be used.

The idea is to do a Finite element model of the structure, and to defined "remote points" as shown in Figure [[fig:apa300ml_ansys]].
Then, on the finite element software, a "super-element" can be extracted which consists of a mass matrix, a stiffness matrix, and the coordinates of the remote points.

#+name: fig:apa300ml_ansys
#+caption: Remote points for the APA300ML (Ansys)
#+attr_latex: :width 0.3\linewidth
[[file:figs/mesh_APA.png]]

This "super-element" can then be included in the Simscape model as shown in Figure [[fig:figure_name]].
The remotes points are defined as "frames" in Simscape, and the "super-element" can be connected with other Simscape elements (mechanical joints, masses, force actuators, etc..).

#+name: fig:figure_name
#+caption: From a finite Element Model (Ansys, bottom left) is extract the mass and stiffness matrices that are then used on Simscape (right)
#+attr_latex: :width \linewidth
[[file:figs/super_element_simscape.png]]

** Actuator and Sensor constants
<<sec:apa_constants>>

On Simscape, we want to model both the actuator stacks and the sensors stack.
We therefore need to link the electrical domain (voltages, charges) with the mechanical domain (forces, strain).
To do so, we use the "actuator constant" and the "sensor constant".

Consider a schematic of the Amplified Piezoelectric Actuator in Figure [[fig:apa_model_schematic]].

#+name: fig:apa_model_schematic
#+caption: Amplified Piezoelectric Actuator Schematic
#+attr_latex: :width 0.5\linewidth
[[file:figs/apa_model_schematic.png]]

A voltage $V_a$ applied to the actuator stacks will induce an actuator force $F_a$:
\begin{equation}
  \boxed{F_a = g_a \cdot V_a}
\end{equation}

A change of length $dl$ of the sensor stack will induce a voltage $V_s$:
\begin{equation}
  \boxed{V_s = g_s \cdot dl}
\end{equation}

The block-diagram model of the piezoelectric actuator is then as shown in Figure [[fig:apa-model-simscape-schematic]].

#+begin_src latex :file apa-model-simscape-schematic.pdf
  \begin{tikzpicture}
    \node[block={2.0cm}{2.0cm}, align=center] (model) at (0,0){Simscape\\Model};
    \node[block, left=1.0 of model] (ga){$g_a(s)$};
    \node[block, right=1.0 of model] (gs){$g_s(s)$};

    \draw[<-] (ga.west) -- node[midway, above]{$V_a$} node[midway, below]{$[V]$} ++(-1.0, 0);
    \draw[->] (ga.east) --node[midway, above]{$F_a$} node[midway, below]{$[N]$} (model.west);
    \draw[->] (model.east) --node[midway, above]{$dl$} node[midway, below]{$[m]$} (gs.west);
    \draw[->] (gs.east) -- node[midway, above]{$V_s$} node[midway, below]{$[V]$} ++(1.0, 0);
  \end{tikzpicture}
#+end_src

#+name: fig:apa-model-simscape-schematic
#+caption: Model of the APA with Simscape/Simulink
#+RESULTS:
[[file:figs/apa-model-simscape-schematic.png]]

The constants $g_a$ and $g_s$ will be experimentally estimated.

* First Basic Measurements
<<sec:first_measurements>>
** Introduction                                                      :ignore:

Before using the measurement bench to characterize the APA300ML, first simple measurements are performed:
- Section [[sec:geometrical_measurements]]: the geometric tolerances of the interface planes are checked
- Section [[sec:electrical_measurements]]: the capacitance of the stacks are measured
- Section [[sec:stroke_measurements]]: the stroke of the APA are measured
- Section [[sec:spurious_resonances]]: the "spurious" resonances of the APA are investigated

** Geometrical Measurements
:PROPERTIES:
:header-args:matlab: :tangle matlab/basic_meas_geometrical.m
:header-args:matlab+: :comments no
:END:
<<sec:geometrical_measurements>>
*** Introduction                                                    :ignore:

The received APA are shown in Figure [[fig:received_apa]].

#+name: fig:received_apa
#+caption: Received APA
#+attr_latex: :width 0.6\linewidth
[[file:figs/received_apa.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
colors = colororder;
#+end_src

#+begin_src matlab :tangle no
addpath('./matlab/mat/');
addpath('./matlab/');
#+end_src

#+begin_src matlab :eval no
addpath('./mat/');
#+end_src

*** Measurement Setup

The flatness corresponding to the two interface planes are measured as shown in Figure [[fig:flatness_meas_setup]].

#+name: fig:flatness_meas_setup
#+caption: Measurement Setup
#+attr_latex: :width 0.6\linewidth
[[file:figs/flatness_meas_setup.jpg]]

*** Measurement Results

The height (Z) measurements at the 8 locations (4 points by plane) are defined below.
#+begin_src matlab
%% Measured height for all the APA at the 8 locations
apa1  = 1e-6*[0, -0.5 , 3.5  , 3.5  , 42   , 45.5, 52.5 , 46];
apa2  = 1e-6*[0, -2.5 , -3   , 0    , -1.5 , 1   , -2   , -4];
apa3  = 1e-6*[0, -1.5 , 15   , 17.5 , 6.5  , 6.5 , 21   , 23];
apa4  = 1e-6*[0, 6.5  , 14.5 , 9    , 16   , 22  , 29.5 , 21];
apa5  = 1e-6*[0, -12.5, 16.5 , 28.5 , -43  , -52 , -22.5, -13.5];
apa6  = 1e-6*[0, -8   , -2   , 5    , -57.5, -62 , -55.5, -52.5];
apa7  = 1e-6*[0, 19.5 , -8   , -29.5, 75   , 97.5, 70   , 48];
apa7b = 1e-6*[0, 9    , -18.5, -30  , 31   , 46.5, 16.5 , 7.5];

apa = {apa1, apa2, apa3, apa4, apa5, apa6, apa7b};
#+end_src

The X/Y Positions of the 8 measurement points are defined below.
#+begin_src matlab
%% X-Y positions of the measurements points
W = 20e-3;   % Width [m]
L = 61e-3;   % Length [m]
d = 1e-3;    % Distance from border [m]
l = 15.5e-3; % [m]

pos = [[-L/2 + d;      W/2 - d],
       [-L/2 + l - d;  W/2 - d],
       [-L/2 + l - d; -W/2 + d],
       [-L/2 + d;     -W/2 + d],
       [L/2 - l + d;   W/2 - d],
       [L/2 - d;       W/2 - d],
       [L/2 - d;      -W/2 + d],
       [L/2 - l + d;  -W/2 + d]];
#+end_src

Finally, the flatness is estimated by fitting a plane through the 8 points using the =fminsearch= command.
#+begin_src matlab
%% Using fminsearch to find the best fitting plane
apa_d = zeros(1, 7);
for i = 1:7
    fun = @(x)max(abs(([pos; apa{i}]-[0;0;x(1)])'*([x(2:3);1]/norm([x(2:3);1]))));
    x0 = [0;0;0];
    [x, min_d] = fminsearch(fun,x0);
    apa_d(i) = min_d;
end
#+end_src

The obtained flatness are shown in Table [[tab:flatness_meas]].

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable(1e6*apa_d', {'APA 1', 'APA 2', 'APA 3', 'APA 4', 'APA 5', 'APA 6', 'APA 7'}, {'*Flatness* $[\mu m]$'}, ' %.1f ');
#+end_src

#+name: tab:flatness_meas
#+caption: Estimated flatness
#+attr_latex: :environment tabularx :width 0.25\linewidth :align lc
#+attr_latex: :center t :booktabs t :float t
#+RESULTS:
|       | *Flatness* $[\mu m]$ |
|-------+----------------------|
| APA 1 |                  8.9 |
| APA 2 |                  3.1 |
| APA 3 |                  9.1 |
| APA 4 |                  3.0 |
| APA 5 |                  1.9 |
| APA 6 |                  7.1 |
| APA 7 |                 18.7 |

#+begin_important
The measured flatness of the APA300ML interface planes are within the specifications.
#+end_important

** Electrical Measurements
<<sec:electrical_measurements>>
*** Measurement Setup
#+begin_note
The capacitance of the stacks is measure with the [[https://www.gwinstek.com/en-global/products/detail/LCR-800][LCR-800 Meter]] ([[file:doc/DS_LCR-800_Series_V2_E.pdf][doc]]) shown in Figure [[fig:LCR_meter]].
The excitation frequency is set to be 1kHz.
#+end_note

#+name: fig:LCR_meter
#+caption: LCR Meter used for the measurements
#+attr_latex: :width 0.9\linewidth
[[file:figs/LCR_meter.jpg]]

*** Measured Capacitance
From the documentation of the APA300ML, the total capacitance of the three stacks should be between $18\mu F$ and $26\mu F$ with a nominal capacitance of $20\mu F$.
However, from the documentation of the stack themselves, it can be seen that the capacitance of a single stack should be $4.4\mu F$.
Clearly, the total capacitance of the APA300ML if more than just three times the capacitance of one stack.

#+begin_question
Could it be possible that the capacitance of the stacks increase that much when they are pre-stressed?
#+end_question

The measured capacitance of the stacks are summarized in Table [[tab:apa300ml_capacitance]].

#+name: tab:apa300ml_capacitance
#+caption: Capacitance measured with the LCR meter. The excitation signal is a sinus at 1kHz
#+attr_latex: :environment tabularx :width 0.5\linewidth :align lcc
#+attr_latex: :center t :booktabs t :float t
|       | *Sensor Stack* | *Actuator Stacks* |
|-------+----------------+-------------------|
| APA 1 |           5.10 |             10.03 |
| APA 2 |           4.99 |              9.85 |
| APA 3 |           1.72 |              5.18 |
| APA 4 |           4.94 |              9.82 |
| APA 5 |           4.90 |              9.66 |
| APA 6 |           4.99 |              9.91 |
| APA 7 |           4.85 |              9.85 |

#+begin_important
From the measurements (Table [[tab:apa300ml_capacitance]]), the capacitance of one stack is found to be $\approx 5 \mu F$.
#+end_important

#+begin_warning
There is clearly a problem with APA300ML number 3
The APA number 3 has ben sent back to Cedrat, and a new APA300ML has been shipped back.
#+end_warning

** Stroke measurement
:PROPERTIES:
:header-args:matlab: :tangle matlab/basic_meas_stroke.m
:header-args:matlab+: :comments no
:END:
<<sec:stroke_measurements>>
*** Introduction                                                    :ignore:
We here wish to estimate the stroke of the APA.

To do so, one side of the APA is fixed, and a displacement probe is located on the other side as shown in Figure [[fig:stroke_test_bench]].

Then, a voltage is applied on either one or two stacks using a DAC and a voltage amplifier.

#+begin_note
Here are the documentation of the equipment used for this test bench:
- *Voltage Amplifier*: [[file:doc/PD200-V7-R1.pdf][PD200]] with a gain of 20
- *16bits DAC*: [[file:doc/IO131-OEM-Datasheet.pdf][IO313 Speedgoat card]]
- *Displacement Probe*: [[file:doc/Millimar--3723046--BA--C1208-C1216-C1240--FR--2016-11-08.pdf][Millimar C1216 electronics]] and [[file:doc/tmp3m0cvmue_7888038c-cdc8-48d8-a837-35de02760685.pdf][Millimar 1318 probe]]
#+end_note

#+name: fig:stroke_test_bench
#+caption: Bench to measured the APA stroke
#+attr_latex: :width 0.9\linewidth
[[file:figs/stroke_test_bench.jpg]]

From the documentation, the nominal stroke of the APA300ML is $304\,\mu m$.

*** 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
colors = colororder;
#+end_src

#+begin_src matlab :tangle no
addpath('./matlab/mat/');
addpath('./matlab/');
#+end_src

#+begin_src matlab :eval no
addpath('./mat/');
#+end_src

*** Voltage applied on one stack

Let's first look at the relation between the voltage applied to *one* stack to the displacement of the APA as measured by the displacement probe.

#+begin_src matlab :exports none
%% Load the measurements
apa300ml_1s = {};
for i = 1:7
    apa300ml_1s(i) = {load(['mat/stroke_apa_1stacks_' num2str(i) '.mat'], 't', 'V', 'd')};
end
#+end_src

#+begin_src matlab :exports none
%% Only take the data between t=2 and t=10 and reset the measured displacement at t=2
for i = 1:7
    t = apa300ml_1s{i}.t;
    apa300ml_1s{i}.d = apa300ml_1s{i}.d - mean(apa300ml_1s{i}.d(t > 1.9 & t < 2.0));
    apa300ml_1s{i}.d = apa300ml_1s{i}.d(t > 2.0 & t < 10.0);
    apa300ml_1s{i}.V = apa300ml_1s{i}.V(t > 2.0 & t < 10.0);
    apa300ml_1s{i}.t = apa300ml_1s{i}.t(t > 2.0 & t < 10.0);
end
#+end_src

The applied voltage is shown in Figure [[fig:apa_stroke_voltage_time]].

#+begin_src matlab :exports none
%% Applied voltage as a function of time
figure;
plot(apa300ml_1s{1}.t, 20*apa300ml_1s{1}.V)
xlabel('Time [s]'); ylabel('Voltage [V]');
ylim([-20,160]); yticks([-20     0    20    40    60    80   100   120   140   160]);
#+end_src

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

#+name: fig:apa_stroke_voltage_time
#+caption: Applied voltage as a function of time
#+RESULTS:
[[file:figs/apa_stroke_voltage_time.png]]

The obtained displacements for all the APA are shown in Figure [[fig:apa_stroke_time_1s]].
The displacement is set to zero at initial time when the voltage applied is -20V.

#+begin_src matlab :exports none
%% Measured motion for all the APA300ML
figure;
hold on;
for i = 1:7
    plot(apa300ml_1s{i}.t, 1e6*apa300ml_1s{i}.d, 'DisplayName', sprintf('APA %i', i))
end
hold off;
xlabel('Time [s]'); ylabel('Displacement [$\mu m$]')
legend('location', 'southeast', 'FontSize', 8)
#+end_src

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

#+name: fig:apa_stroke_time_1s
#+caption: Displacement as a function of time for all the APA300ML (only one stack is used as an actuator)
#+RESULTS:
[[file:figs/apa_stroke_time_1s.png]]

Finally, the displacement is shown as a function of the applied voltage in Figure [[fig:apa_d_vs_V_1s]].
We can clearly see that there is a problem with the APA 3.
Also, there is a large hysteresis.

#+begin_src matlab :exports none
%% Displacement as a function of the applied voltage
figure;
hold on;
for i = 1:7
    plot(20*apa300ml_1s{i}.V, 1e6*apa300ml_1s{i}.d, 'DisplayName', sprintf('APA %i', i))
end
hold off;
xlabel('Voltage [V]'); ylabel('Displacement [$\mu m$]')
legend('location', 'southwest', 'FontSize', 8)
xlim([-20, 160]); ylim([-140, 0]);
#+end_src

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

#+name: fig:apa_d_vs_V_1s
#+caption: Displacement as a function of the applied voltage (on only one stack)
#+RESULTS:
[[file:figs/apa_d_vs_V_1s.png]]

#+begin_important
We can clearly confirm from Figure [[fig:apa_d_vs_V_1s]] that there is a problem with the APA number 3.
#+end_important

*** Voltage applied on two stacks

Now look at the relation between the voltage applied to the *two* other stacks to the displacement of the APA as measured by the displacement probe.

#+begin_src matlab :exports none
%% Load the measurements
apa300ml_2s = {};
for i = 1:7
    apa300ml_2s(i) = {load(['mat/stroke_apa_2stacks_' num2str(i) '.mat'], 't', 'V', 'd')};
end
#+end_src

#+begin_src matlab :exports none
%% Only take the data between t=2 and t=10 and reset the measured displacement at t=2
for i = 1:7
    t = apa300ml_2s{i}.t;
    apa300ml_2s{i}.d = apa300ml_2s{i}.d - mean(apa300ml_2s{i}.d(t > 1.9 & t < 2.0));
    apa300ml_2s{i}.d = apa300ml_2s{i}.d(t > 2.0 & t < 10.0);
    apa300ml_2s{i}.V = apa300ml_2s{i}.V(t > 2.0 & t < 10.0);
    apa300ml_2s{i}.t = apa300ml_2s{i}.t(t > 2.0 & t < 10.0);
end
#+end_src

The obtained displacement is shown in Figure [[fig:apa_stroke_time_2s]].
The displacement is set to zero at initial time when the voltage applied is -20V.
#+begin_src matlab :exports none
%% Measured motion for all the APA300ML
figure;
hold on;
for i = 1:7
    plot(apa300ml_2s{i}.t, 1e6*apa300ml_2s{i}.d, 'DisplayName', sprintf('APA %i', i))
end
hold off;
xlabel('Time [s]'); ylabel('Displacement [$\mu m$]')
legend('location', 'southeast', 'FontSize', 8)
ylim([-250, 0]);
#+end_src

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

#+name: fig:apa_stroke_time_2s
#+caption: Displacement as a function of time for all the APA300ML (two stacks are used as actuators)
#+RESULTS:
[[file:figs/apa_stroke_time_2s.png]]

Finally, the displacement is shown as a function of the applied voltage in Figure [[fig:apa_d_vs_V_2s]].
#+begin_src matlab :exports none
%% Displacement as a function of the applied voltage
figure;
hold on;
for i = 1:7
    plot(20*apa300ml_2s{i}.V, 1e6*apa300ml_2s{i}.d, 'DisplayName', sprintf('APA %i', i))
end
hold off;
xlabel('Voltage [V]'); ylabel('Displacement [$\mu m$]')
legend('location', 'southwest', 'FontSize', 8)
xlim([-20, 160]); ylim([-250, 0]);
#+end_src

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

#+name: fig:apa_d_vs_V_2s
#+caption: Displacement as a function of the applied voltage on two stacks
#+RESULTS:
[[file:figs/apa_d_vs_V_2s.png]]

*** Voltage applied on all three stacks

Finally, we can combine the two measurements to estimate the relation between the displacement and the voltage applied to the *three* stacks (Figure [[fig:apa_d_vs_V_3s]]).

#+begin_src matlab :exports none
%% Motion induced by applying a voltage to the three stack is the sum to the previous two measured displacements
apa300ml_3s = {};
for i = 1:7
    apa300ml_3s(i) = apa300ml_1s(i);
    apa300ml_3s{i}.d = apa300ml_1s{i}.d + apa300ml_2s{i}.d;
end
#+end_src

#+begin_src matlab :exports none
%% Displacement as a function of the applied voltage
figure;
hold on;
for i = 1:7
    plot(20*apa300ml_3s{i}.V, 1e6*apa300ml_3s{i}.d, 'DisplayName', sprintf('APA %i', i))
end
hold off;
xlabel('Voltage [V]'); ylabel('Displacement [$\mu m$]')
legend('location', 'southwest', 'FontSize', 8)
xlim([-20, 160]); ylim([-400, 0]);
#+end_src

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

#+name: fig:apa_d_vs_V_3s
#+caption: Displacement as a function of the applied voltage on all three stacks
#+RESULTS:
[[file:figs/apa_d_vs_V_3s.png]]

The obtained maximum stroke for all the APA are summarized in Table [[tab:apa_measured_stroke]].

#+begin_src matlab :exports none
%% Estimate the maximum stroke
apa300ml_stroke = zeros(1, 7);
for i = 1:7
    apa300ml_stroke(i) = max(apa300ml_3s{i}.d) - min(apa300ml_3s{i}.d);
end
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable(1e6*apa300ml_stroke', {'APA 1', 'APA 2', 'APA 3', 'APA 4', 'APA 5', 'APA 6', 'APA 7'}, {'*Stroke* $[\mu m]$'}, ' %.1f ');
#+end_src

#+name: tab:apa_measured_stroke
#+caption: Measured maximum stroke
#+attr_latex: :environment tabularx :width 0.25\linewidth :align lc
#+attr_latex: :center t :booktabs t :float t
#+RESULTS:
|       | *Stroke* $[\mu m]$ |
|-------+--------------------|
| APA 1 |              373.2 |
| APA 2 |              365.5 |
| APA 3 |              181.7 |
| APA 4 |              359.7 |
| APA 5 |              361.5 |
| APA 6 |              363.9 |
| APA 7 |              358.4 |

*** Conclusion
#+begin_important
The except from APA 3 that has a problem, all the APA are similar when it comes to stroke and hysteresis.
Also, the obtained stroke is more than specified in the documentation.
Therefore, only two stacks can be used as an actuator.
#+end_important

** Spurious resonances
:PROPERTIES:
:header-args:matlab: :tangle matlab/basic_meas_spurious_res.m
:header-args:matlab+: :comments no
:END:
<<sec:spurious_resonances>>
*** Introduction

From a Finite Element Model of the struts, it have been found that three main resonances are foreseen to be problematic for the control of the APA300ML (Figure [[fig:apa_mode_shapes]]):
- Mode in X-bending at 200Hz
- Mode in Y-bending at 285Hz
- Mode in Z-torsion at 400Hz

#+name: fig:apa_mode_shapes
#+caption: Spurious resonances. a) X-bending mode at 189Hz. b) Y-bending mode at 285Hz. c) Z-torsion mode at 400Hz
#+attr_latex: :width \linewidth
[[file:figs/apa_mode_shapes.gif]]

These modes are present when flexible joints are fixed to the ends of the APA300ML.

In this section, we try to find the resonance frequency of these modes when one end of the APA is fixed and the other is free.

*** 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
colors = colororder;
#+end_src

#+begin_src matlab :tangle no
addpath('matlab/');
addpath('matlab/mat/');
#+end_src

#+begin_src matlab :eval no
addpath('mat/');
#+end_src

*** Measurement Setup

The measurement setup is shown in Figure [[fig:measurement_setup_torsion]].
A Laser vibrometer is measuring the difference of motion between two points.
The APA is excited with an instrumented hammer and the transfer function from the hammer to the measured rotation is computed.

#+begin_note
The instrumentation used are:
- Laser Doppler Vibrometer Polytec OFV512
- Instrumented hammer
#+end_note

#+name: fig:measurement_setup_torsion
#+caption: Measurement setup with a Laser Doppler Vibrometer and one instrumental hammer
#+attr_latex: :width 0.7\linewidth
[[file:figs/measurement_setup_torsion.jpg]]

*** X-Bending Mode

The vibrometer is setup to measure the X-bending motion is shown in Figure [[fig:measurement_setup_X_bending]].
The APA is excited with an instrumented hammer having a solid metallic tip.
The impact point is on the back-side of the APA aligned with the top measurement point.

#+name: fig:measurement_setup_X_bending
#+caption: X-Bending measurement setup
#+attr_latex: :width 0.7\linewidth
[[file:figs/measurement_setup_X_bending.jpg]]

The data is loaded.
#+begin_src matlab
%% Load Data
bending_X = load('apa300ml_bending_X_top.mat');
#+end_src

The configuration (Sampling time and windows) for =tfestimate= is done:
#+begin_src matlab
%% Spectral Analysis setup
Ts = bending_X.Track1_X_Resolution; % Sampling Time [s]
win = hann(ceil(1/Ts));
#+end_src

The transfer function from the input force to the output "rotation" (difference between the two measured distances).
#+begin_src matlab
%% Compute the transfer function from applied force to measured rotation
[G_bending_X, f]  = tfestimate(bending_X.Track1, bending_X.Track2, win, [], [], 1/Ts);
#+end_src

The result is shown in Figure [[fig:apa300ml_meas_freq_bending_x]].

The can clearly observe a nice peak at 280Hz, and then peaks at the odd "harmonics" (third "harmonic" at 840Hz, and fifth "harmonic" at 1400Hz).
#+begin_src matlab :exports none
%% Plot the transfer function
figure;
hold on;
plot(f, abs(G_bending_X), 'k-');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude');
xlim([50, 2e3]); ylim([1e-5, 2e-1]);
text(280, 5.5e-2,{'280Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
text(840, 2.0e-3,{'840Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
text(1400, 7.0e-3,{'1400Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
#+end_src

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

#+name: fig:apa300ml_meas_freq_bending_x
#+caption: Obtained FRF for the X-bending
#+RESULTS:
[[file:figs/apa300ml_meas_freq_bending_x.png]]

Then the APA is in the "free-free" condition, this bending mode is foreseen to be at 200Hz (Figure [[fig:apa_mode_shapes]]).
We are here in the "fixed-free" condition.
If we consider that we therefore double the stiffness associated with this mode, we should obtain a resonance a factor $\sqrt{2}$ higher than 200Hz which is indeed 280Hz.
Not sure this reasoning is correct though.

*** Y-Bending Mode

The setup to measure the Y-bending is shown in Figure [[fig:measurement_setup_Y_bending]].

The impact point of the instrumented hammer is located on the back surface of the top interface (on the back of the 2 measurements points).

#+name: fig:measurement_setup_Y_bending
#+caption: Y-Bending measurement setup
#+attr_latex: :width 0.7\linewidth
[[file:figs/measurement_setup_Y_bending.jpg]]

The data is loaded, and the transfer function from the force to the measured rotation is computed.
#+begin_src matlab
%% Load Data
bending_Y = load('apa300ml_bending_Y_top.mat');

%% Compute the transfer function
[G_bending_Y, ~]  = tfestimate(bending_Y.Track1, bending_Y.Track2, win, [], [], 1/Ts);
#+end_src

The results are shown in Figure [[fig:apa300ml_meas_freq_bending_y]].
The main resonance is at 412Hz, and we also see the third "harmonic" at 1220Hz.

#+begin_src matlab :exports none
%% Plot the transfer function
figure;
hold on;
plot(f, abs(G_bending_Y), 'k-');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude');
xlim([50, 2e3]); ylim([1e-5, 3e-2])
text(412, 1.5e-2,{'412Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
text(1218, 1.5e-2,{'1220Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
#+end_src

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

#+name: fig:apa300ml_meas_freq_bending_y
#+caption: Obtained FRF for the Y-bending
#+RESULTS:
[[file:figs/apa300ml_meas_freq_bending_y.png]]

We can apply the same reasoning as in the previous section and estimate the mode to be a factor $\sqrt{2}$ higher than the mode estimated in the "free-free" condition.
We would obtain a mode at 403Hz which is very close to the one estimated here.

*** Z-Torsion Mode

Finally, we measure the Z-torsion resonance as shown in Figure [[fig:measurement_setup_torsion_bis]].

The excitation is shown on the other side of the APA, on the side to excite the torsion motion.

#+name: fig:measurement_setup_torsion_bis
#+caption: Z-Torsion measurement setup
#+attr_latex: :width 0.7\linewidth
[[file:figs/measurement_setup_torsion_bis.jpg]]

The data is loaded, and the transfer function computed.
#+begin_src matlab
%% Load Data
torsion = load('apa300ml_torsion_left.mat');

%% Compute transfer function
[G_torsion, ~]  = tfestimate(torsion.Track1, torsion.Track2, win, [], [], 1/Ts);
#+end_src

The results are shown in Figure [[fig:apa300ml_meas_freq_torsion_z]].
We observe a first peak at 267Hz, which corresponds to the X-bending mode that was measured at 280Hz.
And then a second peak at 415Hz, which corresponds to the X-bending mode that was measured at 412Hz.
A third mode at 800Hz could correspond to this torsion mode.

#+begin_src matlab :exports none
%% Plot the transfer function
figure;
hold on;
plot(f, abs(G_torsion), 'k-');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude');
xlim([50, 2e3]); ylim([1e-5, 2e-2])
text(415, 4.3e-3,{'415Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
text(267, 8e-4,{'267Hz'}, 'VerticalAlignment', 'bottom','HorizontalAlignment','center')
text(800, 6e-4,{'800Hz'}, 'VerticalAlignment', 'bottom','HorizontalAlignment','center')
#+end_src

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

#+name: fig:apa300ml_meas_freq_torsion_z
#+caption: Obtained FRF for the Z-torsion
#+RESULTS:
[[file:figs/apa300ml_meas_freq_torsion_z.png]]

In order to verify that, the APA is excited on the top part such that the torsion mode should not be excited.
#+begin_src matlab
%% Load data
torsion = load('apa300ml_torsion_top.mat');

%% Compute transfer function
[G_torsion_top, ~]  = tfestimate(torsion.Track1, torsion.Track2, win, [], [], 1/Ts);
#+end_src

The two FRF are compared in Figure [[fig:apa300ml_meas_freq_torsion_z_comp]].
It is clear that the first two modes does not correspond to the torsional mode.
Maybe the resonance at 800Hz, or even higher resonances. It is difficult to conclude here.
#+begin_src matlab :exports none
%% Plot the two transfer functions
figure;
hold on;
plot(f, abs(G_torsion), 'k-', 'DisplayName', 'Left excitation');
plot(f, abs(G_torsion_top), '-', 'DisplayName', 'Top excitation');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude');
xlim([50, 2e3]); ylim([1e-5, 2e-2])
text(415, 4.3e-3,{'415Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
text(267, 8e-4,{'267Hz'}, 'VerticalAlignment', 'bottom','HorizontalAlignment','center')
text(800, 2e-3,{'800Hz'}, 'VerticalAlignment', 'bottom','HorizontalAlignment','center')
legend('location', 'northwest');
#+end_src

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

#+name: fig:apa300ml_meas_freq_torsion_z_comp
#+caption: Obtained FRF for the Z-torsion
#+RESULTS:
[[file:figs/apa300ml_meas_freq_torsion_z_comp.png]]

*** Compare
The three measurements are shown in Figure [[fig:apa300ml_meas_freq_compare]].
#+begin_src matlab :exports none
figure;
hold on;
plot(f, abs(G_torsion),    'DisplayName', 'Torsion');
plot(f, abs(G_bending_X),  'DisplayName', 'Bending - X');
plot(f, abs(G_bending_Y),  'DisplayName', 'Bending - Y');
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude');
xlim([50, 2e3]); ylim([1e-5, 1e-1]);
legend('location', 'southeast');
#+end_src

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

#+name: fig:apa300ml_meas_freq_compare
#+caption: Obtained FRF - Comparison
#+RESULTS:
[[file:figs/apa300ml_meas_freq_compare.png]]

*** Conclusion

When two flexible joints are fixed at each ends of the APA, the APA is mostly in a free/free condition in terms of bending/torsion (the bending/torsional stiffness of the joints being very small).

In the current tests, the APA are in a fixed/free condition.
Therefore, it is quite obvious that we measured higher resonance frequencies than what is foreseen for the struts.
It is however quite interesting that there is a factor $\approx \sqrt{2}$ between the two (increased of the stiffness by a factor 2?).

#+name: tab:apa300ml_measured_modes_freq
#+caption: Measured frequency of the modes
#+attr_latex: :environment tabularx :width 0.7\linewidth :align ccc
#+attr_latex: :center t :booktabs t :float t
| *Mode*    | *FEM - Strut mode* | *Measured Frequency* |
|-----------+--------------------+----------------------|
| X-Bending | 189Hz              | 280Hz                |
| Y-Bending | 285Hz              | 410Hz                |
| Z-Torsion | 400Hz              | 800Hz?               |

* Dynamical measurements - APA
<<sec:dynamical_meas_apa>>
** Introduction                                                      :ignore:

In this section, a measurement test bench is used to extract all the important parameters of the Amplified Piezoelectric Actuator APA300ML.

This include:
- Stroke
- Stiffness
- Hysteresis
- "Actuator constant": Gain from the applied voltage $V_a$ to the generated Force $F_a$
- "Sensor constant": Gain from the sensor stack strain $\delta L$ to the generated voltage $V_s$
- Dynamical behavior from the actuator to the force sensor and to the motion of the APA

The bench is shown in Figure [[fig:picture_apa_bench]], and a zoom picture on the APA and encoder is shown in Figure [[fig:picture_apa_bench_encoder]].

#+name: fig:picture_apa_bench
#+caption: Picture of the test bench
#+attr_latex: :width 0.5\linewidth
[[file:figs/picture_apa_bench.jpg]]

#+name: fig:picture_apa_bench_encoder
#+caption: Zoom on the APA with the encoder
#+attr_latex: :width 0.5\linewidth
[[file:figs/picture_apa_bench_encoder.jpg]]

#+begin_note
Here are the documentation of the equipment used for this test bench:
- Voltage Amplifier: [[file:doc/PD200-V7-R1.pdf][PD200]]
- Amplified Piezoelectric Actuator: [[file:doc/APA300ML.pdf][APA300ML]]
- DAC/ADC: Speedgoat [[file:doc/IO131-OEM-Datasheet.pdf][IO313]]
- Encoder: [[file:doc/L-9517-9678-05-A_Data_sheet_VIONiC_series_en.pdf][Renishaw Vionic]] and used [[file:doc/L-9517-9862-01-C_Data_sheet_RKLC_EN.pdf][Ruler]]
- Interferometer: [[https://www.attocube.com/en/products/laser-displacement-sensor/displacement-measuring-interferometer][Attocube IDS3010]]
#+end_note

The bench is schematically shown in Figure [[fig:test_bench_apa_alone]] and the signal used are summarized in Table [[tab:test_bench_apa_variables]].

#+name: fig:test_bench_apa_alone
#+caption: Schematic of the Test Bench
#+attr_latex: :width 0.8\linewidth
[[file:figs/test_bench_apa_alone.png]]

#+name: tab:test_bench_apa_variables
#+caption: Variables used during the measurements
#+attr_latex: :environment tabularx :width 0.8\linewidth :align lXXX
#+attr_latex: :center t :booktabs t :float t
| Variable | Description                  | Unit | Hardware                      |
|----------+------------------------------+------+-------------------------------|
| =Va=     | Output DAC voltage           | [V]  | DAC - Ch. 1 => PD200 => APA   |
| =Vs=     | Measured stack voltage (ADC) | [V]  | APA => ADC - Ch. 1            |
| =de=     | Encoder Measurement          | [m]  | PEPU Ch. 1 - IO318(1) - Ch. 1 |
| =da=     | Attocube Measurement         | [m]  | PEPU Ch. 2 - IO318(1) - Ch. 2 |
| =t=      | Time                         | [s]  |                               |

This section is structured as follows:
- Section [[sec:meas_one_apa]]: the measurements are first performed on one APA.
- Section [[sec:meas_all_apa]]: the same measurements are performed on all the APA and are compared.

** Speedgoat Setup                                                 :noexport:
<<sec:meas_apa_speedgoat_setup>>
*** Introduction                                                    :ignore:
*** 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
colors = colororder;
#+end_src

#+begin_src matlab :tangle no
addpath('./matlab/src/');
addpath('./matlab/');
#+end_src

#+begin_src matlab :eval no
addpath('./src/');
#+end_src

*** =frf_setup.m= - Measurement Setup
:PROPERTIES:
:header-args:matlab: :tangle matlab/frf_setup.m
:header-args:matlab+: :comments no
:END:
#+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 :eval no :exports none
addpath('./src/');
#+end_src

First is defined the sampling frequency:
#+begin_src matlab
%% Simulation configuration
Fs   = 10e3; % Sampling Frequency [Hz]
Ts   = 1/Fs; % Sampling Time [s]
#+end_src

#+begin_src matlab
%% Data record configuration
Trec_start = 5;  % Start time for Recording [s]
Trec_dur   = 100; % Recording Duration [s]
#+end_src

#+begin_src matlab
Tsim = 2*Trec_start + Trec_dur;  % Simulation Time [s]
#+end_src

A white noise excitation signal can be very useful in order to obtain a first idea of the plant FRF.
The gain can be gradually increased until satisfactory output is obtained.
#+begin_src matlab
%% Shaped Noise
V_noise = generateShapedNoise('Ts', 1/Fs, ...
                              'V_mean', 3.25, ...
                              't_start', Trec_start, ...
                              'exc_duration', Trec_dur, ...
                              'smooth_ends', true, ...
                              'V_exc', 0.05/(1 + s/2/pi/10));
#+end_src

#+begin_src matlab :exports none :tangle no
figure;
tiledlayout(1, 2, 'TileSpacing', 'Normal', 'Padding', 'None');

ax1 = nexttile;
plot(V_noise(1,:), V_noise(2,:));
xlabel('Time [s]'); ylabel('Amplitude [V]');

ax2 = nexttile;
win = hanning(floor(length(V_noise)/8));
[pxx, f] = pwelch(V_noise(2,:), win, 0, [], Fs);
plot(f, pxx)
xlabel('Frequency [Hz]'); ylabel('Power Spectral Density [$V^2/Hz$]');
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlim([1, Fs/2]); ylim([1e-10, 1e0]);
#+end_src

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

#+name: fig:frf_meas_noise_excitation
#+caption: Example of Shaped noise excitation signal
#+RESULTS:
[[file:figs/frf_meas_noise_excitation.png]]

The maximum excitation voltage at resonance is 9Vrms, therefore corresponding to 0.6V of output DAC voltage.
#+begin_src matlab
%% Sweep Sine
gc = 0.1;
xi = 0.5;
wn = 2*pi*94.3;

% Notch filter at the resonance of the APA
G_sweep = 0.2*(s^2 + 2*gc*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);

V_sweep = generateSweepExc('Ts',      Ts, ...
                           'f_start', 10, ...
                           'f_end',   400, ...
                           'V_mean',  3.25, ...
                           't_start', Trec_start, ...
                           'exc_duration', Trec_dur, ...
                           'sweep_type',   'log', ...
                           'V_exc', G_sweep*1/(1 + s/2/pi/500));
#+end_src

#+begin_src matlab :exports none :tangle no
figure;
tiledlayout(1, 2, 'TileSpacing', 'Normal', 'Padding', 'None');

ax1 = nexttile;
plot(V_sweep(1,:), V_sweep(2,:));
xlabel('Time [s]'); ylabel('Amplitude [V]');

ax2 = nexttile;
win = hanning(floor(length(V_sweep(2,:))/80));
[pxx, f] = pwelch(V_sweep(2,:), win, 0, [], Fs);
plot(f, pxx)
xlabel('Frequency [Hz]'); ylabel('Power Spectral Density [$V^2/Hz$]');
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlim([1, Fs/2]); ylim([1e-10, 1e0]);
#+end_src

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

#+name: fig:frf_meas_sweep_excitation
#+caption: Example of Sweep Sin excitation signal
#+RESULTS:
[[file:figs/frf_meas_sweep_excitation.png]]

In order to better estimate the high frequency dynamics, a band-limited noise can be used (Figure [[fig:frf_meas_noise_hf_exc]]).
The frequency content of the noise can be precisely controlled.
#+begin_src matlab
%% High Frequency Shaped Noise
[b,a] = cheby1(10, 2, 2*pi*[300 2e3], 'bandpass', 's');
wL = 0.005*tf(b, a);

V_noise_hf = generateShapedNoise('Ts', 1/Fs, ...
                              'V_mean', 3.25, ...
                              't_start', Trec_start, ...
                              'exc_duration', Trec_dur, ...
                              'smooth_ends', true, ...
                              'V_exc', wL);
#+end_src

#+begin_src matlab :exports none :tangle no
figure;
tiledlayout(1, 2, 'TileSpacing', 'Normal', 'Padding', 'None');

ax1 = nexttile;
plot(V_noise_hf(1,:), V_noise_hf(2,:));
xlabel('Time [s]'); ylabel('Amplitude [V]');

ax2 = nexttile;
win = hanning(floor(length(V_noise_hf(2,:))/80));
[pxx, f] = pwelch(V_noise_hf(2,:), win, 0, [], Fs);
plot(f, pxx)
xlabel('Frequency [Hz]'); ylabel('Power Spectral Density [$V^2/Hz$]');
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlim([1, Fs/2]); ylim([1e-10, 1e0]);
#+end_src

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

#+name: fig:frf_meas_noise_hf_exc
#+caption: Example of band-limited noise excitation signal
#+RESULTS:
[[file:figs/frf_meas_noise_hf_exc.png]]


Then a sinus excitation can be used to estimate the hysteresis.
#+begin_src matlab
%% Sinus excitation with increasing amplitude
V_sin = generateSinIncreasingAmpl('Ts', 1/Fs, ...
                                  'V_mean', 3.25, ...
                                  'sin_ampls', [0.1, 0.2, 0.4, 1, 2, 4], ...
                                  'sin_period', 1, ...
                                  'sin_num', 5, ...
                                  't_start', Trec_start, ...
                                  'smooth_ends', true);
#+end_src

#+begin_src matlab :exports none :tangle no
figure;
plot(V_sin(1,:), V_sin(2,:));
xlabel('Time [s]'); ylabel('Amplitude [V]');
#+end_src

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

#+name: fig:frf_meas_sin_excitation
#+caption: Example of Shaped noise excitation signal
#+RESULTS:
[[file:figs/frf_meas_sin_excitation.png]]

Then, we select the wanted excitation signal.
#+begin_src matlab
%% Select the excitation signal
V_exc = timeseries(V_noise(2,:), V_noise(1,:));
#+end_src

#+begin_src matlab :exports none :eval no
%% Plot
figure;
tiledlayout(1, 2, 'TileSpacing', 'Normal', 'Padding', 'None');

ax1 = nexttile;
plot(V_exc(1,:), V_exc(2,:));
xlabel('Time [s]'); ylabel('Amplitude [V]');

ax2 = nexttile;
win = hanning(floor(length(V_exc)/8));
[pxx, f] = pwelch(V_exc(2,:), win, 0, [], Fs);
plot(f, pxx)
xlabel('Frequency [Hz]'); ylabel('Power Spectral Density [$V^2/Hz$]');
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlim([1, Fs/2]); ylim([1e-10, 1e0]);
#+end_src

#+begin_src matlab
%% Save data that will be loaded in the Simulink file
save('./frf_data.mat', 'Fs', 'Ts', 'Tsim', 'Trec_start', 'Trec_dur', 'V_exc');
#+end_src

*** =frf_save.m= - Save Data
:PROPERTIES:
:header-args: :tangle matlab/frf_save.m
:header-args:matlab+: :comments no
:END:

First, we get data from the Speedgoat:
#+begin_src matlab
tg = slrt;

f = SimulinkRealTime.openFTP(tg);
mget(f, 'data/data.dat');
close(f);
#+end_src

And we load the data on the Workspace:
#+begin_src matlab
data = SimulinkRealTime.utils.getFileScopeData('data/data.dat').data;

da = data(:, 1); % Excitation Voltage (input of PD200) [V]
de = data(:, 2); % Measured voltage (force sensor) [V]
Vs = data(:, 3); % Measurment displacement (encoder) [m]
Va = data(:, 4); % Measurement displacement (attocube) [m]
t  = data(:, end); % Time [s]
#+end_src

And we save this to a =mat= file:
#+begin_src matlab
apa_number = 1;

save(sprintf('mat/frf_data_%i_huddle.mat', apa_number), 't', 'Va', 'Vs', 'de', 'da');
#+end_src

** Measurements on APA 1
:PROPERTIES:
:header-args:matlab: :tangle matlab/apa_meas_analysis_1.m
:header-args:matlab+: :comments no
:END:
<<sec:meas_one_apa>>
*** Introduction                                                    :ignore:
Measurements are first performed on only *one* APA.
Once the measurement procedure is validated, it is performed on all the other APA.

*** 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
colors = colororder;
Fs = 1e4; % Sampling Frequency [Hz]
Ts = 1/Fs; % Sampling Time [s]
#+end_src

#+begin_src matlab :tangle no
addpath('./matlab/mat/');
addpath('./matlab/src/');
addpath('./matlab/');
#+end_src

#+begin_src matlab :eval no
addpath('./mat/');
addpath('./src/');
#+end_src

*** Excitation Signals
Different excitation signals are used to perform FRF estimations.

Typically, this is done in three steps:
1. A low pass filtered white noise is used with rather small amplitudes (Figure [[fig:exc_signal_1_noise]]).
   This first excitation is used to estimate the main resonance of the system.
2. A sweep-sine from 10Hz to 400Hz is used (Figure [[fig:exc_signal_2_sweep]]).
   The sweep-sine is is notched around the estimated resonance of the system.
3. A band-limited white noise from 300Hz to 2kHz is used to estimate the high frequency behavior (Figure [[fig:exc_signal_3_hf_noise]]).

For all the excitation signals, before the excitation starts, the mean voltage is slowly increased halfway between the minimum voltage (-20V) and the maximum (150V).

The first measurement is only used to have a first estimation of the dynamics and verify that everything is setup correctly.
The second excitation is done to estimate the dynamics from 10Hz to 350Hz and the third excitation from 350Hz to 2kHz.
The second and third measurements are therefore combined in the frequency domain to form one good estimation of the dynamics from 10Hz up to 2kHz.

#+begin_src matlab :exports none :tangle no
V_noise = generateShapedNoise('Ts', 1/Fs, ...
                              'V_mean', 3.25, ...
                              't_start', 5, ...
                              'exc_duration', 50, ...
                              'smooth_ends', true, ...
                              'V_exc', 0.05/(1 + s/2/pi/10));
#+end_src

#+begin_src matlab :exports none :tangle no
%% Plot of the excitation signal and associated PSD
figure;
tiledlayout(1, 2, 'TileSpacing', 'Normal', 'Padding', 'None');

ax1 = nexttile;
plot(V_noise(1,:), V_noise(2,:));
xlabel('Time [s]'); ylabel('Amplitude [V]');

ax2 = nexttile;
win = hanning(floor(length(V_noise(2,:))/8));
[pxx, f] = pwelch(V_noise(2,:), win, 0, [], Fs);
plot(f, pxx)
xlabel('Frequency [Hz]'); ylabel('Power Spectral Density [$V^2/Hz$]');
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlim([1, Fs/2]); ylim([1e-8, 1e-2]);
#+end_src

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

#+name: fig:exc_signal_1_noise
#+caption:
#+RESULTS:
[[file:figs/exc_signal_1_noise.png]]

#+begin_src matlab :exports none :tangle no
gc = 0.1;
xi = 0.5;
wn = 2*pi*94.3;

% Notch filter at the resonance of the APA
G_sweep = 0.2*(s^2 + 2*gc*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);

V_sweep = generateSweepExc('Ts',      1/Fs, ...
                           'f_start', 10, ...
                           'f_end',   400, ...
                           'V_mean',  3.25, ...
                           't_start', 5, ...
                           'exc_duration', 50, ...
                           'sweep_type',   'log', ...
                           'V_exc', G_sweep*1/(1 + s/2/pi/500));
#+end_src

#+begin_src matlab :exports none :tangle no
%% Plot of the Sweep excitation signal
figure;
plot(V_sweep(1,:), V_sweep(2,:));
xlabel('Time [s]'); ylabel('Amplitude [V]');
#+end_src

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

#+name: fig:exc_signal_2_sweep
#+caption:
#+RESULTS:
[[file:figs/exc_signal_2_sweep.png]]

#+begin_src matlab :exports none :tangle no
%% High Frequency Noise
[b,a] = cheby1(10, 2, 2*pi*[300 2e3], 'bandpass', 's');
wL = 0.005*tf(b, a);

V_noise_hf = generateShapedNoise('Ts', 1/Fs, ...
                                 'V_mean', 3.25, ...
                                 't_start', 5, ...
                                 'exc_duration', 40, ...
                                 'smooth_ends', true, ...
                                 'V_exc', wL);
#+end_src

#+begin_src matlab :exports none :tangle no
%% Plot of the excitation signal and associated PSD
figure;
tiledlayout(1, 2, 'TileSpacing', 'Normal', 'Padding', 'None');

ax1 = nexttile;
plot(V_noise_hf(1,:), V_noise_hf(2,:));
xlabel('Time [s]'); ylabel('Amplitude [V]');

ax2 = nexttile;
win = hanning(floor(length(V_noise_hf(2,:))/8));
[pxx, f] = pwelch(V_noise_hf(2,:), win, 0, [], Fs);
plot(f, pxx)
xlabel('Frequency [Hz]'); ylabel('Power Spectral Density [$V^2/Hz$]');
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlim([1, Fs/2]); ylim([1e-8, 1e-2]);
#+end_src

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

#+name: fig:exc_signal_3_hf_noise
#+caption:
#+RESULTS:
[[file:figs/exc_signal_3_hf_noise.png]]

*** First Measurement
For this first measurement for the first APA, a basic logarithmic sweep is used between 10Hz and 2kHz.

The data are loaded.
#+begin_src matlab
%% Load data
apa_sweep = load(sprintf('mat/frf_data_%i_sweep.mat', 1), 't', 'Va', 'Vs', 'da', 'de');
#+end_src

The initial time is set to zero.
#+begin_src matlab
%% Time vector
t = apa_sweep.t - apa_sweep.t(1) ; % Time vector [s]
#+end_src

The excitation signal is shown in Figure [[fig:apa_bench_exc_sweep]].
It is a sweep sine from 10Hz up to 2kHz filtered with a notch centered with the main resonance of the system and a low pass filter.
#+begin_src matlab :exports none
%% Plot the excitation signal
figure;
plot(t, apa_sweep.Va)
xlabel('Time [s]'); ylabel('Excitation Voltage $V_a$ [V]');
#+end_src

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

#+name: fig:apa_bench_exc_sweep
#+caption:  Excitation voltage
#+RESULTS:
[[file:figs/apa_bench_exc_sweep.png]]

*** FRF - Setup
Let's define the sampling time/frequency.
#+begin_src matlab
%% Sampling Frequency / Time
Ts = (t(end) - t(1))/(length(t)-1); % Sampling Time [s]
Fs = 1/Ts; % Sampling Frequency [Hz]
#+end_src

Then we defined a "Hanning" windows that will be used for the spectral analysis:
#+begin_src matlab
win = hanning(ceil(1*Fs)); % Hannning Windows
#+end_src

We get the frequency vector that will be the same for all the frequency domain analysis.
#+begin_src matlab
% Only used to have the frequency vector "f"
[~, f] = tfestimate(apa_sweep.Va, apa_sweep.de, win, [], [], 1/Ts);
#+end_src

*** FRF - Encoder and Interferometer
In this section, the transfer function from the excitation voltage $V_a$ to the encoder measured displacement $d_e$ and interferometer measurement $d_a$.

The coherence from $V_a$ to $d_e$ and from $V_a$ to $d_a$ are computed and shown in Figure [[fig:apa_1_coh_dvf]].
They are quite good from 10Hz up to 500Hz.
#+begin_src matlab
%% Compute the coherence
[enc_coh, ~] = mscohere(apa_sweep.Va, apa_sweep.de, win, [], [], 1/Ts);
[int_coh, ~] = mscohere(apa_sweep.Va, apa_sweep.da, win, [], [], 1/Ts);
#+end_src

#+begin_src matlab :exports none
%% Plot the coherence
figure;
hold on;
plot(f, enc_coh, 'DisplayName', '$d_e/V_a$');
plot(f, int_coh, 'DisplayName', '$d_a/V_a$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
xlim([5, 5e3]); ylim([0, 1]);
#+end_src

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

#+name: fig:apa_1_coh_dvf
#+caption: Coherence for the identification from $V_a$ to $d_e$
#+RESULTS:
[[file:figs/apa_1_coh_dvf.png]]

The transfer functions are then estimated and shown in Figure [[fig:apa_1_frf_dvf]].
#+begin_src matlab
%% TF - Encoder and interferometer
[frf_enc, ~] = tfestimate(apa_sweep.Va, apa_sweep.de, win, [], [], 1/Ts);
[frf_int, ~] = tfestimate(apa_sweep.Va, apa_sweep.da, win, [], [], 1/Ts);
#+end_src

It is shown than both the encoder and interferometers are measuring the same dynamics up to $\approx 700\,Hz$.
Above that, it is possible that there is some flexible elements apart from the APA that is adding resonances into one or the other FRF.

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

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(frf_enc), 'color', colors(1, :), ...
     'DisplayName', 'Encoder');
plot(f, abs(frf_int), 'color', colors(2, :), ...
     'DisplayName', 'Interferometer');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'northeast');
ylim([1e-9, 1e-3]);

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(frf_enc), 'color', colors(1, :));
plot(f, 180/pi*angle(frf_int), 'color', colors(2, :));
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([10, 2e3]);
#+end_src

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

#+name: fig:apa_1_frf_dvf
#+caption: Obtained transfer functions from $V_a$ to both $d_e$ and $d_a$
#+RESULTS:
[[file:figs/apa_1_frf_dvf.png]]

#+begin_important
The transfer functions obtained in Figure [[fig:apa_1_frf_dvf]] are very close to what was expected:
- constant gain at low frequency
- resonance at around 100Hz which corresponds to the APA axial mode
- no further resonance up until high frequency ($\approx 700\,Hz$) at which points several elements of the test bench can induces resonances in the measured FRF

However, it was not expected to observe a "double resonance" at around 95Hz (instead of only one resonance).
#+end_important

*** FRF - Force Sensor
Now the dynamics from excitation voltage $V_a$ to the force sensor stack voltage $V_s$ is identified.

The coherence is computed and shown in Figure [[fig:apa_1_coh_iff]] and found very good from 10Hz up to 2kHz.
#+begin_src matlab
%% Compute the coherence from Va to Vs
[iff_coh, ~] = mscohere(apa_sweep.Va, apa_sweep.Vs, win, [], [], 1/Ts);
#+end_src

#+begin_src matlab :exports none
%% Plot the coherence
figure;
hold on;
plot(f, iff_coh, 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
xlim([5, 5e3]); ylim([0, 1]);
#+end_src

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

#+name: fig:apa_1_coh_iff
#+caption: Coherence for the identification from $V_a$ to $V_s$
#+RESULTS:
[[file:figs/apa_1_coh_iff.png]]

The transfer function is estimated and shown in Figure [[fig:apa_1_frf_iff]].
#+begin_src matlab
%% Compute the TF from Va to Vs
[iff_sweep, ~] = tfestimate(apa_sweep.Va, apa_sweep.Vs, win, [], [], 1/Ts);
#+end_src

#+begin_src matlab :exports none
%% Plot the TF
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(iff_sweep), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(iff_sweep), 'k-');
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([10, 2e3]);
#+end_src

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

#+name: fig:apa_1_frf_iff
#+caption: Obtained transfer functions from $V_a$ to $V_s$
#+RESULTS:
[[file:figs/apa_1_frf_iff.png]]

#+begin_important
The obtained dynamics from the excitation voltage $V_a$ to the measured sensor stack voltage $V_s$ is corresponding to what was expected:
- constant gain at low frequency
- complex conjugate zero and then complex conjugate pole
- constant gain at high frequency
#+end_important

*** Hysteresis
We here wish to visually see the amount of hysteresis present in the APA.

To do so, a quasi static sinusoidal excitation $V_a$ at different voltages is used.

The offset is 65V (halfway between -20V and 150V), and the sin amplitude is ranging from 1V up to 80V (full range).

For each excitation amplitude, the vertical displacement $d$ of the mass is measured.

Then, $d$ is plotted as a function of $V_a$ for all the amplitudes.

We expect to obtained something like the hysteresis shown in Figure [[fig:expected_hysteresis]].

#+name: fig:expected_hysteresis
#+caption: Expected Hysteresis cite:poel10_explor_activ_hard_mount_vibrat
#+attr_latex: :width 0.8\linewidth
[[file:figs/expected_hysteresis.png]]

The data is loaded.
#+begin_src matlab
%% Load measured data - hysteresis
apa_hyst = load('frf_data_1_hysteresis.mat', 't', 'Va', 'de');
% Initial time set to zero
apa_hyst.t = apa_hyst.t - apa_hyst.t(1);
#+end_src

The excitation voltage amplitudes are:
#+begin_src matlab
ampls = [0.1, 0.2, 0.4, 1, 2, 4]; % Excitation voltage amplitudes
#+end_src

The excitation voltage and the measured displacement are shown in Figure [[fig:hyst_exc_signal_time]].
#+begin_src matlab :exports none
%% Plot the excitation voltages and measured displacements
figure;
tiledlayout(1, 2, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile;
plot(apa_hyst.t, apa_hyst.Va)
xlabel('Time [s]'); ylabel('Output Voltage [V]');

ax2 = nexttile;
plot(apa_hyst.t, apa_hyst.de)
xlabel('Time [s]'); ylabel('Measured Displacement [m]');
#+end_src

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

#+name: fig:hyst_exc_signal_time
#+caption: Excitation voltage and measured displacement
#+RESULTS:
[[file:figs/hyst_exc_signal_time.png]]

For each amplitude, we only take the last sinus in order to reduce possible transients.
Also, the motion is centered on zero.

The measured displacement at a function of the output voltage are shown in Figure [[fig:hyst_results_multi_ampl]].
#+begin_src matlab :exports none
%% Measured displacement as a function of the output voltage
figure;
tiledlayout(1, 3, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([1,2]);
hold on;
for i = flip(1:6)
    i_lim = apa_hyst.t > i*5-1 & apa_hyst.t < i*5;
    plot(apa_hyst.Va(i_lim) - mean(apa_hyst.Va(i_lim)), apa_hyst.de(i_lim) - mean(apa_hyst.de(i_lim)), ...
         'DisplayName', sprintf('$V_a = %.1f [V]$', ampls(i)))
end
hold off;
xlabel('Output Voltage [V]'); ylabel('Measured Displacement [m]');
legend('location', 'northeast');
xlim([-4, 4]); ylim([-1.2e-4, 1.2e-4]);

ax2 = nexttile;
hold on;
for i = flip(1:6)
    i_lim = apa_hyst.t > i*5-1 & apa_hyst.t < i*5;
    plot(apa_hyst.Va(i_lim) - mean(apa_hyst.Va(i_lim)), apa_hyst.de(i_lim) - mean(apa_hyst.de(i_lim)))
end
hold off;
xlim([-0.4, 0.4]); ylim([-0.8e-5, 0.8e-5]);
#+end_src

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

#+name: fig:hyst_results_multi_ampl
#+caption: Obtained hysteresis for multiple excitation amplitudes
#+RESULTS:
[[file:figs/hyst_results_multi_ampl.png]]

#+begin_important
From Figure [[fig:hyst_results_multi_ampl]], it is quite clear that hysteresis is increasing with the excitation amplitude.
For small excitation amplitudes ($V_a < 0.4\,V$) the hysteresis stays reasonably small.

Also, it is quite interesting to see that no hysteresis is found on the sensor stack voltage when using the same excitation signal.
#+end_important

*** Estimation of the APA axial stiffness
In order to estimate the stiffness of the APA, a weight with known mass $m_a$ is added on top of the suspended granite and the deflection $d_e$ is measured using the encoder.

The APA stiffness can then be estimated to be:
\begin{equation}
  k_{\text{apa}} = \frac{m_a g}{d}
\end{equation}

The data is loaded, and the measured displacement is shown in Figure [[fig:apa_1_meas_stiffness]].
#+begin_src matlab
%% Load data for stiffness measurement
apa_mass = load(sprintf('frf_data_%i_add_mass_closed_circuit.mat', 1), 't', 'de');
apa_mass.de = apa_mass.de - mean(apa_mass.de(apa_mass.t<11));
#+end_src

#+begin_src matlab :exports none
%% Plot the deflection at a function of time
figure;
plot(apa_mass.t, apa_mass.de, 'k-');
xlabel('Time [s]'); ylabel('Displacement $d_e$ [m]');
#+end_src

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

#+name: fig:apa_1_meas_stiffness
#+caption: Measured displacement when adding the mass and removing the mass
#+RESULTS:
[[file:figs/apa_1_meas_stiffness.png]]

From Figure [[fig:apa_1_meas_stiffness]], it can be seen that there are some drifts that are probably due to some creep.
This will induce some uncertainties in the measured stiffness.

Here, a mass of 6.4 kg was used:
#+begin_src matlab
added_mass = 6.4; % Added mass [kg]
#+end_src

The stiffness is then computed as follows:
#+begin_src matlab
k = 9.8 * added_mass / (mean(apa_mass.de(apa_mass.t > 12 & apa_mass.t < 12.5)) - mean(apa_mass.de(apa_mass.t > 20 & apa_mass.t < 20.5)));
#+end_src

And the stiffness obtained is very close to the one specified in the documentation ($k = 1.794\,[N/\mu m]$).
#+begin_src matlab :results value replace :exports results :tangle no
sprintf('k = %.2f [N/um]', 1e-6*k);
#+end_src

#+RESULTS:
: k = 1.68 [N/um]

The stiffness could also be estimated based on the main vertical resonance of the system at $\omega_z = 2\pi \cdot 94 \,[rad/s]$.
The suspended mass is $m_{\text{sus}} = 5\,kg$.
And therefore, the axial stiffness of the APA can be estimated to be:
\begin{equation}
k_{\text{APA}} = m_{\text{sus}} \omega_z^2
\end{equation}

#+begin_src matlab
wz = 2*pi*94; % [rad/s]
msus = 5.7; % [kg]
k = msus * wz^2;
#+end_src

#+begin_src matlab :results value replace :exports results :tangle no
sprintf('k = %.2f [N/um]', 1e-6*k);
#+end_src

#+RESULTS:
: k = 1.99 [N/um]

The two values are found relatively close to each other.
Anyway, the stiffness of the model will be tuned to match the measured FRF.

*** Stiffness change due to electrical connections
Changes in the electrical impedance connected to the piezoelectric actuator causes changes in the mechanical compliance (or stiffness) of the piezoelectric actuator.

In this section is measured the stiffness of the APA whether the piezoelectric actuator is connected to an open circuit or a short circuit (e.g. the output of a voltage amplifier).

Note here that the resistor in parallel to the sensor stack is present in both cases.

First, the data are loaded.
#+begin_src matlab
%% Load Data
add_mass_oc = load(sprintf('frf_data_%i_add_mass_open_circuit.mat',   1), 't', 'de');
add_mass_cc = load(sprintf('frf_data_%i_add_mass_closed_circuit.mat', 1), 't', 'de');
#+end_src

And the initial displacement is set to zero.
#+begin_src matlab
%% Zero displacement at initial time
add_mass_oc.de = add_mass_oc.de - mean(add_mass_oc.de(add_mass_oc.t<11));
add_mass_cc.de = add_mass_cc.de - mean(add_mass_cc.de(add_mass_cc.t<11));
#+end_src

The measured displacements are shown in Figure [[fig:apa_meas_k_time_oc_cc]].
#+begin_src matlab :exports none
figure;
hold on;
plot(add_mass_oc.t, add_mass_oc.de, 'DisplayName', 'Not connected');
plot(add_mass_cc.t, add_mass_cc.de, 'DisplayName', 'Connected');
hold off;
xlabel('Time [s]'); ylabel('Displacement $d_e$ [m]');
legend('location', 'northeast');
#+end_src

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

#+name: fig:apa_meas_k_time_oc_cc
#+caption: Measured displacement
#+RESULTS:
[[file:figs/apa_meas_k_time_oc_cc.png]]

And the stiffness is estimated in both case.
The results are shown in Table [[tab:APA_measured_k_oc_cc]].
#+begin_src matlab
apa_k_oc = 9.8 * added_mass / (mean(add_mass_oc.de(add_mass_oc.t > 12 & add_mass_oc.t < 12.5)) - mean(add_mass_oc.de(add_mass_oc.t > 20 & add_mass_oc.t < 20.5)));
apa_k_cc = 9.8 * added_mass / (mean(add_mass_cc.de(add_mass_cc.t > 12 & add_mass_cc.t < 12.5)) - mean(add_mass_cc.de(add_mass_cc.t > 20 & add_mass_cc.t < 20.5)));
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable(1e-6*[apa_k_oc; apa_k_cc], {'Not connected', 'Connected'}, {'$k [N/\mu m]$'}, ' %.1f ');
#+end_src

#+name: tab:APA_measured_k_oc_cc
#+caption: Measured stiffnesses on "open" and "closed" circuits
#+attr_latex: :environment tabularx :width 0.3\linewidth :align cc
#+attr_latex: :center t :booktabs t :float t
#+RESULTS:
|               | $k [N/\mu m]$ |
|---------------+---------------|
| Not connected |           2.3 |
| Connected     |           1.7 |

#+begin_important
Clearly, connecting the actuator stacks to the amplified (basically equivalent as to short circuiting them) lowers its stiffness.
#+end_important

*** Effect of the resistor on the IFF Plant
A resistor $R \approx 80.6\,k\Omega$ is added in parallel with the sensor stack.
This has the effect to form a high pass filter with the capacitance of the stack.

This is done for two reasons (explained in details [[file:../test-bench-force-sensor/test-bench-force-sensor.org][this document]]):
1. Limit the voltage offset due to the input bias current of the ADC
2. Limit the low frequency gain

The (low frequency) transfer function from $V_a$ to $V_s$ with and without this resistor have been measured.

#+begin_src matlab
%% Load the data
wi_k = load('frf_data_1_sweep_lf_with_R.mat', 't', 'Vs', 'Va'); % With the resistor
wo_k = load('frf_data_1_sweep_lf.mat',        't', 'Vs', 'Va'); % Without the resistor
#+end_src

We use a very long "Hanning" window for the spectral analysis in order to estimate the low frequency behavior.
#+begin_src matlab
win = hanning(ceil(50*Fs)); % Hannning Windows
#+end_src

And we estimate the transfer function from $V_a$ to $V_s$ in both cases:
#+begin_src matlab
%% Compute the transfer functions from Va to Vs
[frf_wo_k, f] = tfestimate(wo_k.Va, wo_k.Vs, win, [], [], 1/Ts);
[frf_wi_k, ~] = tfestimate(wi_k.Va, wi_k.Vs, win, [], [], 1/Ts);
#+end_src

With the following values of the resistor and capacitance, we obtain a first order high pass filter with a crossover frequency equal to:
#+begin_src matlab
%% Model for the high pass filter
C = 5.1e-6; % Sensor Stack capacitance [F]
R = 80.6e3; % Parallel Resistor [Ohm]

f0 = 1/(2*pi*R*C); % Crossover frequency of RC HPF [Hz]
#+end_src

#+begin_src matlab :results value replace :exports results :tangle no
sprintf('f0 = %.2f [Hz]', f0)
#+end_src

#+RESULTS:
: f0 = 0.39 [Hz]

The transfer function of the corresponding high pass filter is:
#+begin_src matlab
G_hpf = 0.6*(s/2*pi*f0)/(1 + s/2*pi*f0);
#+end_src

Let's compare the transfer function from actuator stack to sensor stack with and without the added resistor in Figure [[fig:frf_iff_effect_R]].
#+begin_src matlab :exports none
%% Compare the HPF model and the measured FRF
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(frf_wo_k), 'DisplayName', 'Without $k$');
plot(f, abs(frf_wi_k), 'DisplayName', 'With $k$');
plot(f, abs(squeeze(freqresp(G_hpf, f, 'Hz'))), 'k--', 'DisplayName', sprintf('HPF $f_o = %.2f [Hz]$', f0));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_{out}/V_{in}$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-1, 1e0]);
legend('location', 'southeast')

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

linkaxes([ax1,ax2],'x');
xlim([0.2, 8]);
#+end_src

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

#+name: fig:frf_iff_effect_R
#+caption: Transfer function from $V_a$ to $V_s$ with and without the resistor $k$
#+RESULTS:
[[file:figs/frf_iff_effect_R.png]]

#+begin_important
The added resistor has indeed the expected effect of forming an high pass filter.
#+end_important

** Comparison of all the APA
:PROPERTIES:
:header-args:matlab: :tangle matlab/apa_meas_analysis_all.m
:header-args:matlab+: :comments no
:END:
<<sec:meas_all_apa>>
*** Introduction                                                    :ignore:
The same measurements that was performed in Section [[sec:meas_one_apa]] are now performed on all the APA and then compared.

*** 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
colors = colororder;
#+end_src

#+begin_src matlab :tangle no
addpath('./matlab/mat/');
addpath('./matlab/src/');
addpath('./matlab/');
#+end_src

#+begin_src matlab :eval no
addpath('./mat/');
addpath('./src/');
#+end_src

*** Axial Stiffnesses - Comparison
Let's first compare the APA axial stiffnesses.

The added mass is:
#+begin_src matlab
added_mass = 6.4; % Added mass [kg]
#+end_src

Here are the numbers of the APA that have been measured:
#+begin_src matlab
apa_nums = [1 2 4 5 6 7 8];
#+end_src

The data are loaded.
#+begin_src matlab
%% Load Data
apa_mass = {};
for i = 1:length(apa_nums)
    apa_mass(i) = {load(sprintf('frf_data_%i_add_mass_closed_circuit.mat', apa_nums(i)), 't', 'de')};
    % The initial displacement is set to zero
    apa_mass{i}.de = apa_mass{i}.de - mean(apa_mass{i}.de(apa_mass{i}.t<11));
end
#+end_src

The raw measurements are shown in Figure [[fig:apa_meas_k_time]].
All the APA seems to have similar stiffness except the APA 7 which show strange behavior.

#+begin_question
It is however strange that the displacement $d_e$ when the mass is removed is higher for the APA 7 than for the other APA.
What could cause that?
It is probably due to the fact that the mechanical element holding the granite in place was not removed.
#+end_question

#+begin_src matlab :exports none
%% Plot the time domain measured deflection
figure;
hold on;
for i = 1:length(apa_nums)
  plot(apa_mass{i}.t, apa_mass{i}.de, 'DisplayName', sprintf('APA %i', apa_nums(i)));
end
hold off;
xlabel('Time [s]'); ylabel('Displacement $d_e$ [m]');
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
#+end_src

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

#+name: fig:apa_meas_k_time
#+caption: Raw measurements for all the APA. A mass of 6.4kg is added at arround 15s and removed at arround 22s
#+RESULTS:
[[file:figs/apa_meas_k_time.png]]

The stiffnesses are computed for all the APA and are summarized in Table [[tab:APA_measured_k]].

#+begin_src matlab :exports none
%% Compute the stiffness
apa_k = zeros(length(apa_nums), 1);
for i = 1:length(apa_nums)
    apa_k(i) = 9.8 * added_mass / (mean(apa_mass{i}.de(apa_mass{i}.t > 12 & apa_mass{i}.t < 12.5)) - mean(apa_mass{i}.de(apa_mass{i}.t > 20 & apa_mass{i}.t < 20.5)));
end
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
  data2orgtable(1e-6*apa_k, cellstr(num2str(apa_nums')), {'APA Num', '$k [N/\mu m]$'}, ' %.2f ');
#+end_src

#+name: tab:APA_measured_k
#+caption: Measured stiffnesses
#+attr_latex: :environment tabularx :width 0.3\linewidth :align cc
#+attr_latex: :center t :booktabs t :float t
#+RESULTS:
| APA Num | $k [N/\mu m]$ |
|---------+---------------|
|       1 |          1.68 |
|       2 |          1.69 |
|       4 |           1.7 |
|       5 |           1.7 |
|       6 |           1.7 |
|       7 |          1.93 |
|       8 |          1.73 |

#+begin_important
The APA300ML manual specifies the nominal stiffness to be $1.8\,[N/\mu m]$ which is very close to what have been measured.
Only the APA number 7 is a little bit off, maybe there was a problem with the experimental setup.
#+end_important

*** FRF - Setup
The identification is performed in three steps:
1. White noise excitation with small amplitude.
   This is used to determine the main resonance of the system.
2. Sweep sine excitation with the amplitude lowered around the resonance.
   The sweep sine is from 10Hz to 400Hz.
3. High frequency noise.
   The noise is band-passed between 300Hz and 2kHz.

Then, the result of the second identification is used between 10Hz and 350Hz and the result of the third identification if used between 350Hz and 2kHz.

The data are loaded for both the second and third identification:
#+begin_src matlab
%% Second identification
apa_sweep = {};
for i = 1:length(apa_nums)
    apa_sweep(i) = {load(sprintf('frf_data_%i_sweep.mat', apa_nums(i)), 't', 'Va', 'Vs', 'de', 'da')};
end

%% Third identification
apa_noise_hf = {};
for i = 1:length(apa_nums)
    apa_noise_hf(i) = {load(sprintf('frf_data_%i_noise_hf.mat', apa_nums(i)), 't', 'Va', 'Vs', 'de', 'da')};
end
#+end_src

The time is the same for all measurements.
#+begin_src matlab
%% Time vector
t = apa_sweep{1}.t - apa_sweep{1}.t(1) ; % Time vector [s]

%% Sampling
Ts = (t(end) - t(1))/(length(t)-1); % Sampling Time [s]
Fs = 1/Ts; % Sampling Frequency [Hz]
#+end_src

Then we defined a "Hanning" windows that will be used for the spectral analysis:
#+begin_src matlab
win = hanning(ceil(0.5*Fs)); % Hannning Windows
#+end_src

We get the frequency vector that will be the same for all the frequency domain analysis.
#+begin_src matlab
% Only used to have the frequency vector "f"
[~, f] = tfestimate(apa_sweep{1}.Va, apa_sweep{1}.de, win, [], [], 1/Ts);
i_lf = f <= 350;
i_hf = f >  350;
#+end_src

*** FRF - Encoder and Interferometer
In this section, the dynamics from excitation voltage $V_a$ to encoder measured displacement $d_e$ is identified.

We compute the coherence for 2nd and 3rd identification:
#+begin_src matlab
%% Coherence computation
coh_enc = zeros(length(f), length(apa_nums));
for i = 1:length(apa_nums)
    [coh_lf, ~] = mscohere(apa_sweep{i}.Va,    apa_sweep{i}.de,    win, [], [], 1/Ts);
    [coh_hf, ~] = mscohere(apa_noise_hf{i}.Va, apa_noise_hf{i}.de, win, [], [], 1/Ts);
    coh_enc(:, i) = [coh_lf(i_lf); coh_hf(i_hf)];
end
#+end_src

The coherence is shown in Figure [[fig:apa_frf_dvf_plant_coh]], and it is found that the coherence is good from low frequency up to 700Hz.

#+begin_src matlab :exports none
figure;
hold on;
for i = 1:length(apa_nums)
    plot(f, coh_enc(:, i));
end;
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
xlim([5, 5e3]); ylim([0, 1]);
#+end_src

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

#+name: fig:apa_frf_dvf_plant_coh
#+caption: Obtained coherence for the plant from $V_a$ to $d_e$
#+RESULTS:
[[file:figs/apa_frf_dvf_plant_coh.png]]


Then, the transfer function from the DAC output voltage $V_a$ to the measured displacement by the encoders is computed:
#+begin_src matlab
%% Transfer function estimation
enc_frf = zeros(length(f), length(apa_nums));
for i = 1:length(apa_nums)
    [frf_lf, ~] = tfestimate(apa_sweep{i}.Va,    apa_sweep{i}.de,    win, [], [], 1/Ts);
    [frf_hf, ~] = tfestimate(apa_noise_hf{i}.Va, apa_noise_hf{i}.de, win, [], [], 1/Ts);
    enc_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
end
#+end_src

The obtained transfer functions are shown in Figure [[fig:apa_frf_dvf_plant_tf]].
They are all superimposed except for the APA7.

#+begin_question
Why is the APA7 dynamical behavior is so different from the other?
We could think that the APA7 is stiffer, but also the mass line is off, and it should indeed be identical.
#+end_question

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

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
    plot(f, abs(enc_frf(:, i)), ...
         'DisplayName', sprintf('APA %i', apa_nums(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
ylim([1e-9, 1e-3]);

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

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

#+name: fig:apa_frf_dvf_plant_tf
#+caption: Estimated FRF for the DVF plant (transfer function from $V_a$ to the encoder $d_e$)
#+RESULTS:
[[file:figs/apa_frf_dvf_plant_tf.png]]

A zoom on the main resonance is shown in Figure [[fig:apa_frf_dvf_zoom_res_plant_tf]].
It is clear that expect for the APA 7, the response around the resonances are well matching for all the APA.

It is also clear that there is not a single resonance but two resonances, a first one at 95Hz and a second one at 105Hz.

#+begin_question
Why is there a double resonance at around 94Hz?
#+end_question

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

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
    plot(f, abs(enc_frf(:, i)), ...
         'DisplayName', sprintf('APA %i', apa_nums(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
ylim([2e-5, 4e-4]);

ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, 180/pi*angle(enc_frf(:, 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([-10, 180]);

linkaxes([ax1,ax2],'x');
xlim([80, 120]);
#+end_src

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

#+name: fig:apa_frf_dvf_zoom_res_plant_tf
#+caption: Estimated FRF for the DVF plant (transfer function from $V_a$ to the encoder $d_e$) - Zoom on the main resonance
#+RESULTS:
[[file:figs/apa_frf_dvf_zoom_res_plant_tf.png]]

*** FRF - Force Sensor
In this section, the dynamics from $V_a$ to $V_s$ is identified.

First the coherence is computed and shown in Figure [[fig:apa_frf_iff_plant_coh]].
The coherence is very nice from 10Hz to 2kHz.
It is only dropping near a zeros at 40Hz, and near the resonance at 95Hz (the excitation amplitude being lowered).

#+begin_src matlab
%% Compute the Coherence
coh_iff = zeros(length(f), length(apa_nums));
for i = 1:length(apa_nums)
    [coh_lf, ~] = mscohere(apa_sweep{i}.Va,    apa_sweep{i}.Vs,    win, [], [], 1/Ts);
    [coh_hf, ~] = mscohere(apa_noise_hf{i}.Va, apa_noise_hf{i}.Vs, win, [], [], 1/Ts);
    coh_iff(:, i) = [coh_lf(i_lf); coh_hf(i_hf)];
end
#+end_src

#+begin_src matlab :exports none
%% Plot the coherence
figure;
hold on;
for i = 1:length(apa_nums)
    plot(f, coh_iff(:, i));
end;
hold off;
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlim([5, 5e3]); ylim([0, 1]);
#+end_src

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

#+name: fig:apa_frf_iff_plant_coh
#+caption: Obtained coherence for the IFF plant
#+RESULTS:
[[file:figs/apa_frf_iff_plant_coh.png]]

Then the FRF are estimated and shown in Figure [[fig:apa_frf_iff_plant_tf]]
#+begin_src matlab
%% FRF estimation of the transfer function from Va to Vs
iff_frf = zeros(length(f), length(apa_nums));
for i = 1:length(apa_nums)
    [frf_lf, ~] = tfestimate(apa_sweep{i}.Va,    apa_sweep{i}.Vs,    win, [], [], 1/Ts);
    [frf_hf, ~] = tfestimate(apa_noise_hf{i}.Va, apa_noise_hf{i}.Vs, win, [], [], 1/Ts);
    iff_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
end
#+end_src

#+begin_src matlab :exports none
%% Plot the FRF from Va to Vs
figure;
tiledlayout(2, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, abs(iff_frf(:, i)), ...
         'DisplayName', sprintf('APA %i', apa_nums(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);

ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, 180/pi*angle(iff_frf(:, 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, 2e3]);
#+end_src

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

#+name: fig:apa_frf_iff_plant_tf
#+caption:Identified IFF Plant
#+RESULTS:
[[file:figs/apa_frf_iff_plant_tf.png]]

*** Conclusion

#+begin_important
Except the APA 7 which shows strange behavior, all the other APA are showing a very similar behavior.

So far, all the measured FRF are showing the dynamical behavior that was expected.
#+end_important

#+begin_src matlab
%% Remove the APA 7 (6 in the list) from measurements
apa_nums(6) = [];
enc_frf(:,6) = [];
iff_frf(:,6) = [];
#+end_src

#+begin_src matlab :tangle no :exports none
save('matlab/mat/meas_apa_frf.mat', 'f', 'Ts', 'enc_frf', 'iff_frf', 'apa_nums');
#+end_src

#+begin_src matlab :eval no
%% Save the measured FRF
save('mat/meas_apa_frf.mat', 'f', 'Ts', 'enc_frf', 'iff_frf', 'apa_nums');
#+end_src

* Test Bench APA300ML - Simscape Model
<<sec:simscape_bench_apa>>
** Introduction
In this section, a simscape model (Figure [[fig:model_bench_apa]]) of the measurement bench is used to compare the model of the APA with the measured FRF.

After the transfer functions are extracted from the model (Section [[sec:simscape_bench_apa_first_id]]), the comparison of the obtained dynamics with the measured FRF will permit to:
1. Estimate the "actuator constant" and "sensor constant" (Section [[sec:simscape_bench_apa_id_constants]])
2. Tune the model of the APA to match the measured dynamics (Section [[sec:simscape_bench_apa_tune_2dof_model]])

#+name: fig:model_bench_apa
#+caption: Screenshot of the Simscape model
#+attr_latex: :width 0.5\linewidth
[[file:figs/model_bench_apa.png]]

** Matlab Init                                              :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
#+end_src

#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src

#+begin_src matlab :tangle no
%% Add useful folders to the path
addpath('matlab/');
addpath('matlab/test_bench_apa300ml/');
addpath('matlab/mat/');
addpath('matlab/src/');
addpath('matlab/png/');
#+end_src

#+begin_src matlab :eval no
%% Add useful folders to the path
addpath('test_bench_apa300ml/');
addpath('png/');
addpath('mat/');
addpath('src/');
#+end_src

#+begin_src matlab
%% Frequency vector used for many plots
freqs = 2*logspace(0, 3, 1000);
#+end_src

#+begin_src matlab
%% Open Simscape Model
options = linearizeOptions;
options.SampleTime = 0;

% Name of the Simulink File
mdl = 'test_bench_apa300ml';

open(mdl)
#+end_src

** First Identification
<<sec:simscape_bench_apa_first_id>>

The APA is first initialized with default parameters:
#+begin_src matlab
%% Initialize the structure with default values
n_hexapod = struct();
n_hexapod.actuator = initializeAPA(...
    'type', '2dof', ...
    'Ga', 1, ... % Actuator constant [N/V]
    'Gs', 1);    % Sensor constant [V/m]
#+end_src

The transfer function from excitation voltage $V_a$ (before the amplification of $20$ due to the PD200 amplifier) to:
1. the sensor stack voltage $V_s$
2. the measured displacement by the encoder $d_e$
3. the measured displacement by the interferometer $d_a$
#+begin_src matlab
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Va'],  1, 'openinput');  io_i = io_i + 1; % DAC Voltage
io(io_i) = linio([mdl, '/Vs'],  1, 'openoutput'); io_i = io_i + 1; % Sensor Voltage
io(io_i) = linio([mdl, '/de'],  1, 'openoutput'); io_i = io_i + 1; % Encoder
io(io_i) = linio([mdl, '/da'],  1, 'openoutput'); io_i = io_i + 1; % Interferometer

%% Run the linearization
Ga = linearize(mdl, io, 0.0, options);
Ga.InputName  = {'Va'};
Ga.OutputName = {'Vs', 'de', 'da'};
#+end_src

The obtain dynamics are shown in Figure [[fig:apa_model_bench_bode_vs]] and [[fig:apa_model_bench_bode_dl_z]].
It can be seen that:
- the shape of these bode plots are very similar to the one measured in Section [[sec:dynamical_meas_apa]] expect from a change in gain and exact location of poles and zeros
- there is a sign error for the transfer function from $V_a$ to $V_s$.
  This will be corrected by taking a negative "sensor gain".
- the low frequency zero of the transfer function from $V_a$ to $V_s$ is minimum phase as expected.
  The measured FRF are showing non-minimum phase zero, but it is most likely due to measurements artifacts.

#+begin_src matlab :exports none
%% Bode plot of the transfer function from u to taum
freqs = logspace(1, 3, 1000);

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

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(Ga('Vs', 'Va'), freqs, 'Hz'))), 'k-')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Ga('Vs', 'Va'), freqs, 'Hz'))), 'k-')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360);
ylim([-180, 0])

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

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

#+name: fig:apa_model_bench_bode_vs
#+caption: Bode plot of the transfer function from $V_a$ to $V_s$
#+RESULTS:
[[file:figs/apa_model_bench_bode_vs.png]]

#+begin_src matlab :exports none
%% Bode plot of the transfer function from Va to de and da
freqs = logspace(1, 3, 1000);

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

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(Ga('de', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Encoder')
plot(freqs, abs(squeeze(freqresp(Ga('da', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Interferometer')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'southwest');

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Ga('de', 'Va'), freqs, 'Hz'))))
plot(freqs, 180/pi*angle(squeeze(freqresp(Ga('da', 'Va'), freqs, 'Hz'))))
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360);
ylim([-180, 0])

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

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

#+name: fig:apa_model_bench_bode_dl_z
#+caption: Bode plot of the transfer function from $V_a$ to $d_L$ and to $z$
#+RESULTS:
[[file:figs/apa_model_bench_bode_dl_z.png]]

** Identify Sensor/Actuator constants and compare with measured FRF
<<sec:simscape_bench_apa_id_constants>>
*** How to identify these constants?
**** Piezoelectric Actuator Constant

Using the measurement test-bench, it is rather easy the determine the static gain between the applied voltage $V_a$ to the induced displacement $d$.
\begin{equation}
  d = g_{d/V_a} \cdot V_a
\end{equation}

Using the Simscape model of the APA, it is possible to determine the static gain between the actuator force $F_a$ to the induced displacement $d$:
\begin{equation}
  d = g_{d/F_a} \cdot F_a
\end{equation}

From the two gains, it is then easy to determine $g_a$:
\begin{equation} \label{eq:actuator_constant_formula}
  \boxed{g_a = \frac{F_a}{V_a} = \frac{F_a}{d} \cdot \frac{d}{V_a} = \frac{g_{d/V_a}}{g_{d/F_a}}}
\end{equation}

**** Piezoelectric Sensor Constant

Similarly, it is easy to determine the gain from the excitation voltage $V_a$ to the voltage generated by the sensor stack $V_s$:
\begin{equation}
  V_s = g_{V_s/V_a} V_a
\end{equation}

Note here that there is an high pass filter formed by the piezoelectric capacitor and parallel resistor.

The gain can be computed from the dynamical identification and taking the gain at the wanted frequency (above the first resonance).

Using the simscape model, compute the gain at the same frequency from the actuator force $F_a$ to the strain of the sensor stack $dl$:
\begin{equation}
  dl = g_{dl/F_a} F_a
\end{equation}

Then, the "sensor" constant is:
\begin{equation} \label{eq:sensor_constant_formula}
  \boxed{g_s = \frac{V_s}{dl} = \frac{V_s}{V_a} \cdot \frac{V_a}{F_a} \cdot \frac{F_a}{dl} = \frac{g_{V_s/V_a}}{g_a \cdot g_{dl/F_a}}}
\end{equation}

*** Identification Data
Let's load the measured FRF from the DAC voltage to the measured encoder and to the sensor stack voltage.
#+begin_src matlab
%% Load Data
load('meas_apa_frf.mat', 'f', 'Ts', 'enc_frf', 'iff_frf', 'apa_nums');
#+end_src

*** 2DoF APA
**** 2DoF APA
Let's initialize the APA as a 2DoF model with unity sensor and actuator gains.
#+begin_src matlab
%% Initialize a 2DoF APA with Ga=Gs=1
n_hexapod.actuator = initializeAPA(...
    'type', '2dof', ...
    'ga', 1, ...
    'gs', 1);
#+end_src

**** Identification without actuator or sensor constants
The transfer function from $V_a$ to $V_s$, $d_e$ and $d_a$ is identified.
#+begin_src matlab
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Va'], 1, 'openinput');  io_i = io_i + 1; % Actuator Voltage
io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Sensor Voltage
io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoder
io(io_i) = linio([mdl, '/da'], 1, 'openoutput'); io_i = io_i + 1; % Attocube

%% Identification
Gs = linearize(mdl, io, 0.0, options);
Gs.InputName  = {'Va'};
Gs.OutputName = {'Vs', 'de', 'da'};
#+end_src

**** Actuator Constant
Then, the actuator constant can be computed as shown in Eq. eqref:eq:actuator_constant_formula by dividing the measured DC gain of the transfer function from $V_a$ to $d_e$ by the estimated DC gain of the transfer function from $V_a$ (in truth the actuator force called $F_a$) to $d_e$ using the Simscape model.
#+begin_src matlab
%% Estimated Actuator Constant
ga = -mean(abs(enc_frf(f>10 & f<20)))./dcgain(Gs('de', 'Va')); % [N/V]
#+end_src

#+begin_src matlab :results value replace :exports results :tangle no
sprintf('ga = %.1f [N/V]', ga);
#+end_src

#+RESULTS:
: ga = -32.2 [N/V]

**** Sensor Constant
Similarly, the sensor constant can be estimated using Eq. eqref:eq:sensor_constant_formula.

#+begin_src matlab
%% Estimated Sensor Constant
gs = -mean(abs(iff_frf(f>400 & f<500)))./(ga*abs(squeeze(freqresp(Gs('Vs', 'Va'), 1e3, 'Hz')))); % [V/m]
#+end_src

#+begin_src matlab :results value replace :exports results :tangle no
sprintf('gs = %.3f [V/m]', gs);
#+end_src

#+RESULTS:
: gs = 0.088 [V/m]

**** Comparison
Let's now initialize the APA with identified sensor and actuator constant:
#+begin_src matlab
%% Set the identified constants
n_hexapod.actuator = initializeAPA(...
    'type', '2dof', ...
    'ga', ga, ... % Actuator gain [N/V]
    'gs', gs);    % Sensor gain [V/m]
#+end_src

And identify the dynamics with included constants.
#+begin_src matlab
%% Identify again the dynamics with correct Ga,Gs
Gs = linearize(mdl, io, 0.0, options);
Gs = Gs*exp(-Ts*s);
Gs.InputName  = {'Va'};
Gs.OutputName = {'Vs', 'de', 'da'};
#+end_src

The transfer functions from $V_a$ to $d_e$ are compared in Figure [[fig:apa_act_constant_comp]] and the one from $V_a$ to $V_s$ are compared in Figure [[fig:apa_sens_constant_comp]].

#+begin_src matlab :exports none
%% Bode plot of the transfer function from u to de
freqs = logspace(1,4,1000);

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

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
    plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d\mathcal{L}_m/u$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);

ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, 180/pi*angle(enc_frf(:,1)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
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, 2e3]);
#+end_src

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

#+name: fig:apa_act_constant_comp
#+caption: Comparison of the experimental data and Simscape model ($V_a$ to $d_e$)
#+RESULTS:
[[file:figs/apa_act_constant_comp.png]]

#+begin_src matlab :exports none
%% Bode plot of the transfer function from Va to Vs (both Simscape and measured FRF)
freqs = logspace(1,4,1000);

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

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
    plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $\tau_m/u$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);

ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, 180/pi*angle(iff_frf(:,1)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
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, 2e3]);
#+end_src

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

#+name: fig:apa_sens_constant_comp
#+caption: Comparison of the experimental data and Simscape model ($V_a$ to $V_s$)
#+RESULTS:
[[file:figs/apa_sens_constant_comp.png]]

#+begin_important
The "actuator constant" and "sensor constant" can indeed be identified using this test bench.
After identifying these constants, the 2DoF model shows good agreement with the measured dynamics.
#+end_important

*** Flexible APA
**** Introduction                                                  :ignore:
In this section, the sensor and actuator "constants" are also estimated for the flexible model of the APA.

**** Flexible APA
The Simscape APA model is initialized as a flexible one with unity "constants".
#+begin_src matlab
%% Initialize the APA as a flexible body
n_hexapod.actuator = initializeAPA(...
    'type', 'flexible', ...
    'ga', 1, ...
    'gs', 1);
#+end_src

**** Identification without actuator or sensor constants
The dynamics from $V_a$ to $V_s$, $d_e$ and $d_a$ is identified.
#+begin_src matlab
%% Identify the dynamics
Gs = linearize(mdl, io, 0.0, options);
Gs.InputName  = {'Va'};
Gs.OutputName = {'Vs', 'de', 'da'};
#+end_src

**** Actuator Constant
Then, the actuator constant can be computed as shown in Eq. eqref:eq:actuator_constant_formula:
#+begin_src matlab
%% Actuator Constant
ga = -mean(abs(enc_frf(f>10 & f<20)))./dcgain(Gs('de', 'Va')); % [N/V]
#+end_src

#+begin_src matlab :results value replace :exports results :tangle no
sprintf('ga = %.1f [N/V]', ga);
#+end_src

#+RESULTS:
: ga = 23.5 [N/V]

**** Sensor Constant
#+begin_src matlab
%% Sensor Constant
gs = -mean(abs(iff_frf(f>400 & f<500)))./(ga*abs(squeeze(freqresp(Gs('Vs', 'Va'), 1e3, 'Hz')))); % [V/m]
#+end_src

#+begin_src matlab :results value replace :exports results :tangle no
sprintf('gs = %.3f [V/m]', gs);
#+end_src

#+RESULTS:
: gs = -4839841.756 [V/m]

**** Comparison
Let's now initialize the flexible APA with identified sensor and actuator constant:
#+begin_src matlab
%% Set the identified constants
n_hexapod.actuator = initializeAPA(...
    'type', 'flexible', ...
    'ga', ga, ... % Actuator gain [N/V]
    'gs', gs);    % Sensor gain [V/m]
#+end_src

And identify the dynamics with included constants.
#+begin_src matlab
%% Identify with updated constants
Gs = linearize(mdl, io, 0.0, options);
Gs = Gs*exp(-Ts*s);
Gs.InputName  = {'Va'};
Gs.OutputName = {'Vs', 'de', 'da'};
#+end_src

The obtained dynamics is compared with the measured one in Figures [[fig:apa_act_constant_comp_flex]] and [[fig:apa_sens_constant_comp_flex]].

#+begin_src matlab :exports none
%% Bode plot of the transfer function from V_a to d_e (both Simscape and measured FRF)
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
    plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d\mathcal{L}_m/u$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-9, 1e-3]);

ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, 180/pi*angle(enc_frf(:,1)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
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, 2e3]);
#+end_src

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

#+name: fig:apa_act_constant_comp_flex
#+caption: Comparison of the experimental data and Simscape model ($u$ to $d\mathcal{L}_m$)
#+RESULTS:
[[file:figs/apa_act_constant_comp_flex.png]]

#+begin_src matlab :exports none
%% Bode plot of the transfer function from Va to Vs (both Simscape and measured FRF)
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
    plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $\tau_m/u$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);

ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, 180/pi*angle(iff_frf(:,1)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
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, 2e3]);
#+end_src

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

#+name: fig:apa_sens_constant_comp_flex
#+caption: Comparison of the experimental data and Simscape model ($u$ to $\tau_m$)
#+RESULTS:
[[file:figs/apa_sens_constant_comp_flex.png]]

#+begin_important
The flexible model is a bit "soft" as compared with the experimental results.
#+end_important

** Optimize 2-DoF model to fit the experimental Data
<<sec:simscape_bench_apa_tune_2dof_model>>
The parameters of the 2DoF model presented in Section [[sec:apa_2dof_model]] are now optimize such that the model best matches the measured FRF.

After optimization, the following parameters are used:
#+begin_src matlab
%% Optimized parameters
n_hexapod.actuator = initializeAPA('type', '2dof', ...
                                   'Ga', -32.2, ...
                                   'Gs', 0.088, ...
                                   'k',  ones(6,1)*0.38e6, ...
                                   'ke', ones(6,1)*1.75e6, ...
                                   'ka', ones(6,1)*3e7, ...
                                   'c',  ones(6,1)*1.3e2, ...
                                   'ce', ones(6,1)*1e1, ...
                                   'ca', ones(6,1)*1e1 ...
                                   );
#+end_src

#+begin_src matlab :exports none
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Va'],  1, 'openinput');  io_i = io_i + 1; % Actuator Voltage
io(io_i) = linio([mdl, '/Vs'],  1, 'openoutput'); io_i = io_i + 1; % Sensor Voltage
io(io_i) = linio([mdl, '/de'],  1, 'openoutput'); io_i = io_i + 1; % Encoder

%% Identification with optimized parameters
Gs = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
Gs.InputName  = {'Va'};
Gs.OutputName = {'Vs', 'de'};
#+end_src

The dynamics is identified using the Simscape model and compared with the measured FRF in Figure [[fig:comp_apa_plant_after_opt]].

#+begin_src matlab :exports none
%% Comparison of the experimental data and Simscape Model
freqs = 5*logspace(0, 3, 1000);
figure;
tiledlayout(3, 2, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
    plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);

ax1b = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
    plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);

ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, 180/pi*angle(enc_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
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]);

ax2b = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, 180/pi*angle(iff_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))))
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,ax1b,ax2b],'x');
xlim([10, 2e3]);
#+end_src

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

#+name: fig:comp_apa_plant_after_opt
#+caption: Comparison of the measured FRF and the optimized model
#+RESULTS:
[[file:figs/comp_apa_plant_after_opt.png]]

#+begin_important
The tuned 2DoF is very well representing the (axial) dynamics of the APA.
#+end_important

* Dynamical measurements - Struts
<<sec:dynamical_meas_struts>>
** Introduction                                                      :ignore:

The same bench used in Section [[sec:dynamical_meas_apa]] is here used with the strut instead of only the APA.

The bench is shown in Figure [[fig:test_bench_leg_overview]].
Measurements are performed either when no encoder is fixed to the strut (Figure [[fig:test_bench_leg_front]]) or when one encoder is fixed to the strut (Figure [[fig:test_bench_leg_overview]]).

#+name: fig:test_bench_leg_overview
#+caption: Test Bench with Strut - Overview
#+attr_latex: :width 0.5\linewidth
[[file:figs/test_bench_leg_overview.jpg]]

#+name: fig:test_bench_leg_front
#+caption: Test Bench with Strut - Zoom on the strut
#+attr_latex: :width 0.5\linewidth
[[file:figs/test_bench_leg_front.jpg]]

#+name: fig:test_bench_leg_overview
#+caption: Test Bench with Strut - Zoom on the strut with the encoder
#+attr_latex: :width 0.5\linewidth
[[file:figs/test_bench_leg_coder.jpg]]

** Measurement on Strut 1
<<sec:meas_strut_1>>
*** Introduction                                                    :ignore:
Measurements are first performed on the strut 1 that contains:
- APA 1
- flex 1 and flex 2

*** 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
colors = colororder;
#+end_src

#+begin_src matlab :tangle no
addpath('./matlab/mat/');
addpath('./matlab/src/');
addpath('./matlab/');
#+end_src

#+begin_src matlab :eval no
addpath('./mat/');
addpath('./src/');
#+end_src

*** Without Encoder
<<sec:meas_strut_1_no_encoder>>
**** FRF Identification - Setup
The identification is performed in three steps:
1. White noise excitation with small amplitude.
   This is used to determine the main resonance of the system.
2. Sweep sine excitation with the amplitude lowered around the resonance.
   The sweep sine is from 10Hz to 400Hz.
3. High frequency noise.
   The noise is band-passed between 300Hz and 2kHz.

Then, the result of the second identification is used between 10Hz and 350Hz and the result of the third identification if used between 350Hz and 2kHz.

#+begin_src matlab
leg_sweep    = load(sprintf('frf_data_leg_%i_sweep.mat',    1), 't', 'Va', 'Vs', 'de', 'da');
leg_noise_hf = load(sprintf('frf_data_leg_%i_noise_hf.mat', 1), 't', 'Va', 'Vs', 'de', 'da');
#+end_src

The time is the same for all measurements.
#+begin_src matlab
%% Time vector
t = leg_sweep.t - leg_sweep.t(1) ; % Time vector [s]

%% Sampling
Ts = (t(end) - t(1))/(length(t)-1); % Sampling Time [s]
Fs = 1/Ts; % Sampling Frequency [Hz]
#+end_src

Then we defined a "Hanning" windows that will be used for the spectral analysis:
#+begin_src matlab
win = hanning(ceil(0.5*Fs)); % Hannning Windows
#+end_src

We get the frequency vector that will be the same for all the frequency domain analysis.
#+begin_src matlab
% Only used to have the frequency vector "f"
[~, f] = tfestimate(leg_sweep.Va, leg_sweep.de, win, [], [], 1/Ts);
#+end_src

**** FRF Identification - Displacement
In this section, the dynamics from the excitation voltage $V_a$ to the interferometer $d_a$ is identified.

We compute the coherence for 2nd and 3rd identification:
#+begin_src matlab
[coh_sweep, ~]    = mscohere(leg_sweep.Va,    leg_sweep.da,    win, [], [], 1/Ts);
[coh_noise_hf, ~] = mscohere(leg_noise_hf.Va, leg_noise_hf.da, win, [], [], 1/Ts);
#+end_src

#+begin_src matlab :exports none
figure;
hold on;
plot(f, coh_noise_hf(:, 1), 'color', colors(1, :), ...
     'DisplayName', 'HF Noise');
plot(f, coh_sweep(:, 1), 'color', colors(2, :), ...
     'DisplayName', 'Sweep');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
xlim([5, 5e3]); ylim([0, 1]);
legend('location', 'southeast');
#+end_src

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

#+name: fig:strut_1_frf_dvf_plant_coh
#+caption: Obtained coherence for the plant from $V_a$ to $d_a$
#+RESULTS:
[[file:figs/strut_1_frf_dvf_plant_coh.png]]

The transfer function from $V_a$ to the interferometer measured displacement $d_a$ is estimated and shown in Figure [[fig:strut_1_frf_dvf_plant_tf]].
#+begin_src matlab
[dvf_sweep, ~]    = tfestimate(leg_sweep.Va,    leg_sweep.da,    win, [], [], 1/Ts);
[dvf_noise_hf, ~] = tfestimate(leg_noise_hf.Va, leg_noise_hf.da, win, [], [], 1/Ts);
#+end_src

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

ax1 = nexttile([2,1]);
hold on;
plot(f(f> 350), abs(dvf_noise_hf(f> 350)), 'k-');
plot(f(f<=350), abs(dvf_sweep(   f<=350)), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-9, 1e-3]);

ax2 = nexttile;
hold on;
plot(f(f> 350), 180/pi*angle(dvf_noise_hf(f> 350)), 'k-');
plot(f(f<=350), 180/pi*angle(dvf_sweep(   f<=350)), 'k-');
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, 2e3]);
#+end_src

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

#+name: fig:strut_1_frf_dvf_plant_tf
#+caption: Estimated FRF for the DVF plant (transfer function from $V_a$ to the interferometer $d_a$)
#+RESULTS:
[[file:figs/strut_1_frf_dvf_plant_tf.png]]

**** FRF Identification - IFF
In this section, the dynamics from $V_a$ to $V_s$ is identified.

First the coherence is computed and shown in Figure [[fig:strut_1_frf_iff_plant_coh]].
The coherence is very nice from 10Hz to 2kHz.
It is only dropping near a zeros at 40Hz, and near the resonance at 95Hz (the excitation amplitude being lowered).

#+begin_src matlab
[coh_sweep, ~] = mscohere(leg_sweep.Va, leg_sweep.Vs, win, [], [], 1/Ts);
[coh_noise_hf, ~] = mscohere(leg_noise_hf.Va, leg_noise_hf.Vs, win, [], [], 1/Ts);
#+end_src

#+begin_src matlab :exports none
figure;
hold on;
plot(f, coh_noise_hf, 'DisplayName', 'HF Noise');
plot(f, coh_sweep,    'DisplayName', 'Sweep');
hold off;
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlim([5, 5e3]); ylim([0, 1]);
legend('location', 'southeast');
#+end_src

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

#+name: fig:strut_1_frf_iff_plant_coh
#+caption: Obtained coherence for the IFF plant
#+RESULTS:
[[file:figs/strut_1_frf_iff_plant_coh.png]]

Then the FRF are estimated and shown in Figure [[fig:strut_1_frf_iff_plant_tf]]
#+begin_src matlab
[iff_sweep, ~] = tfestimate(leg_sweep.Va, leg_sweep.Vs, win, [], [], 1/Ts);
[iff_noise_hf, ~] = tfestimate(leg_noise_hf.Va, leg_noise_hf.Vs, win, [], [], 1/Ts);
#+end_src

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

ax1 = nexttile([2,1]);
hold on;
plot(f(f> 350), abs(iff_noise_hf(f> 350)), 'k-');
plot(f(f<=350), abs(iff_sweep(   f<=350)), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);

ax2 = nexttile;
hold on;
plot(f(f> 350), 180/pi*angle(iff_noise_hf(f> 350)), 'k-');
plot(f(f<=350), 180/pi*angle(iff_sweep(   f<=350)), 'k-');
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, 2e3]);
#+end_src

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

#+name: fig:strut_1_frf_iff_plant_tf
#+caption:Identified IFF Plant for the Strut 1
#+RESULTS:
[[file:figs/strut_1_frf_iff_plant_tf.png]]

*** With Encoder
<<sec:meas_strut_1_encoder>>
**** Measurement Data
#+begin_src matlab
leg_enc_sweep    = load(sprintf('frf_data_leg_coder_badly_align_%i_noise.mat',    1), 't', 'Va', 'Vs', 'de', 'da');
leg_enc_noise_hf = load(sprintf('frf_data_leg_coder_badly_align_%i_noise_hf.mat', 1), 't', 'Va', 'Vs', 'de', 'da');
#+end_src

**** FRF Identification - DVF
In this section, the dynamics from $V_a$ to $d_e$ is identified.

We compute the coherence for 2nd and 3rd identification:
#+begin_src matlab
[coh_enc_sweep, ~]    = mscohere(leg_enc_sweep.Va,    leg_enc_sweep.de,    win, [], [], 1/Ts);
[coh_enc_noise_hf, ~] = mscohere(leg_enc_noise_hf.Va, leg_enc_noise_hf.de, win, [], [], 1/Ts);
#+end_src

#+begin_src matlab :exports none
figure;
hold on;
plot(f, coh_enc_noise_hf(:, 1), 'color', colors(1, :), ...
     'DisplayName', 'HF Noise');
plot(f, coh_enc_sweep(:, 1), 'color', colors(2, :), ...
     'DisplayName', 'Sweep');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
% xlim([5, 5e3]); ylim([0, 1]);
legend('location', 'southeast');
#+end_src

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

#+name: fig:strut_1_enc_frf_dvf_plant_coh
#+caption: Obtained coherence for the plant from $V_a$ to $d_e$
#+RESULTS:
[[file:figs/strut_1_enc_frf_dvf_plant_coh.png]]

#+begin_src matlab
[dvf_enc_sweep, ~]    = tfestimate(leg_enc_sweep.Va,    leg_enc_sweep.de,    win, [], [], 1/Ts);
[dvf_enc_noise_hf, ~] = tfestimate(leg_enc_noise_hf.Va, leg_enc_noise_hf.de, win, [], [], 1/Ts);
#+end_src

#+begin_src matlab
[dvf_int_sweep, ~]    = tfestimate(leg_enc_sweep.Va,    leg_enc_sweep.da,    win, [], [], 1/Ts);
[dvf_int_noise_hf, ~] = tfestimate(leg_enc_noise_hf.Va, leg_enc_noise_hf.da, win, [], [], 1/Ts);
#+end_src

The obtained transfer functions are shown in Figure [[fig:strut_1_enc_frf_dvf_plant_tf]].

They are all superimposed except for the APA7.

#+begin_question
Why is the APA7 off?
We could think that the APA7 is stiffer, but also the mass line is off.

It seems that there is a "gain" problem.
The encoder seems fine (it measured the same as the Interferometer).
Maybe it could be due to the amplifier?
#+end_question

#+begin_question
Why is there a double resonance at around 94Hz?
#+end_question

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

ax1 = nexttile([2,1]);
hold on;
plot(f(f> 350), abs(dvf_enc_noise_hf(f> 350)), 'k-');
plot(f(f<=350), abs(dvf_enc_sweep(   f<=350)), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-7, 1e-3]);

ax2 = nexttile;
hold on;
plot(f(f> 350), 180/pi*angle(dvf_enc_noise_hf(f> 350)), 'k-');
plot(f(f<=350), 180/pi*angle(dvf_enc_sweep(   f<=350)), 'k-');
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, 2e3]);
#+end_src

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

#+name: fig:strut_1_enc_frf_dvf_plant_tf
#+caption: Estimated FRF for the DVF plant (transfer function from $V_a$ to the encoder $d_e$)
#+RESULTS:
[[file:figs/strut_1_enc_frf_dvf_plant_tf.png]]

**** Comparison of the Encoder and Interferometer
The interferometer could here represent the case where the encoders are fixed to the plates and not the APA.

The dynamics from $V_a$ to $d_e$ and from $V_a$ to $d_a$ are compared in Figure [[fig:strut_1_comp_enc_int]].

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

ax1 = nexttile([2,1]);
hold on;
plot(f(f> 350), abs(dvf_enc_noise_hf(f> 350)), 'color', colors(1, :), ...
     'DisplayName', 'Encoder');
plot(f(f<=350), abs(dvf_enc_sweep(   f<=350)), 'color', colors(1, :), ...
     'HandleVisibility', 'off');
plot(f(f> 350), abs(dvf_int_noise_hf(f> 350)), 'color', colors(2, :), ...
     'DisplayName', 'Interferometer');
plot(f(f<=350), abs(dvf_int_sweep(   f<=350)), 'color', colors(2, :), ...
     'HandleVisibility', 'off');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
ylim([1e-8, 1e-3]);

ax2 = nexttile;
hold on;
plot(f(f> 350), 180/pi*angle(dvf_enc_noise_hf(f> 350)), 'color', colors(1, :));
plot(f(f<=350), 180/pi*angle(dvf_enc_sweep(   f<=350)), 'color', colors(1, :));
plot(f(f> 350), 180/pi*angle(dvf_int_noise_hf(f> 350)), 'color', colors(2, :));
plot(f(f<=350), 180/pi*angle(dvf_int_sweep(   f<=350)), 'color', colors(2, :));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);

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

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

#+name: fig:strut_1_comp_enc_int
#+caption: Comparison of the transfer functions from excitation voltage $V_a$ to either the encoder $d_e$ or the interferometer $d_a$
#+RESULTS:
[[file:figs/strut_1_comp_enc_int.png]]

#+begin_important
It will clearly be difficult to do something (except some low frequency positioning) with the encoders fixed to the APA.
#+end_important

**** APA Resonances Frequency
As shown in Figure [[fig:strut_1_spurious_resonances]], we can clearly see three spurious resonances at 197Hz, 290Hz and 376Hz.

#+begin_src matlab :exports none
figure;
hold on;
plot(f(f> 350), abs(dvf_enc_noise_hf(f> 350)), 'k-');
plot(f(f<=350), abs(dvf_enc_sweep(   f<=350)), 'k-');

text(93,  4e-4,  {'93Hz'}, 'VerticalAlignment','bottom','HorizontalAlignment','center')
text(197, 1.3e-4,{'197Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
text(290, 4e-6,  {'290Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
text(376, 1.4e-6,{'376Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); xlabel('Frequency [Hz]');
hold off;
ylim([1e-7, 1e-3]); xlim([10, 2e3]);
#+end_src

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

#+name: fig:strut_1_spurious_resonances
#+caption: Magnitude of the transfer function from excitation voltage $V_a$ to encoder measurement $d_e$. The frequency of the resonances are noted.
#+RESULTS:
[[file:figs/strut_1_spurious_resonances.png]]


These resonances correspond to parasitic resonances of the APA itself.
They are very close to what was estimated using the FEM (Figure [[fig:apa_mode_shapes_bis]]):
- Mode in X-bending at 189Hz
- Mode in Y-bending at 285Hz
- Mode in Z-torsion at 400Hz

#+name: fig:apa_mode_shapes_bis
#+caption: Spurious resonances. a) X-bending mode at 189Hz. b) Y-bending mode at 285Hz. c) Z-torsion mode at 400Hz
#+attr_latex: :width \linewidth
[[file:figs/apa_mode_shapes.gif]]

#+begin_important
The resonances are indeed due to limited stiffness of the APA.
#+end_important

**** TODO Estimated Flexible Joint axial stiffness
#+begin_src matlab
load(sprintf('frf_data_leg_coder_%i_add_mass_closed_circuit.mat',    1), 't', 'Va', 'Vs', 'de', 'da');
de = de - de(1);
da = da - da(1);
#+end_src

#+begin_src matlab
figure;
hold on;
plot(t, de, 'DisplayName', 'Encoder')
plot(t, da, 'DisplayName', 'Interferometer')
hold off;
xlabel('Time [s]'); ylabel('Displacement [m]');
legend('location', 'northeast');
#+end_src

#+begin_src matlab
de_steps = [mean(de(t > 13 & t < 14));
            mean(de(t > 19 & t < 20));
            mean(de(t > 24 & t < 25));
            mean(de(t > 28.5 & t < 29.5));
            mean(de(t > 49 & t < 50))] - mean(de(t > 13 & t < 14));

da_steps = [mean(da(t > 13 & t < 14));
            mean(da(t > 19 & t < 20));
            mean(da(t > 24 & t < 25));
            mean(da(t > 28.5 & t < 29.5));
            mean(da(t > 49 & t < 50))] - mean(da(t > 13 & t < 14));
#+end_src

#+begin_src matlab
added_mass = [0; 1; 2; 3; 4];
#+end_src

#+begin_src matlab
lin_fit = (added_mass\abs(de_steps-da_steps) - abs(de_steps(1)-da_steps(1)));
#+end_src

#+begin_src matlab
figure;
hold on;
plot(added_mass, abs(de_steps-da_steps) - abs(de_steps(1)-da_steps(1)), 'ok')
plot(added_mass, added_mass*lin_fit, '-r')
hold off;
#+end_src

#+begin_question
What is strange is that the encoder is measuring a larger displacement than the interferometer.
That should be the opposite.
Maybe is is caused by the fact that the APA is experiencing some bending and therefore a larger motion is measured on the encoder.
#+end_question

**** FRF Identification - IFF
In this section, the dynamics from $V_a$ to $V_s$ is identified.

First the coherence is computed and shown in Figure [[fig:strut_1_frf_iff_plant_coh]].
The coherence is very nice from 10Hz to 2kHz.
It is only dropping near a zeros at 40Hz, and near the resonance at 95Hz (the excitation amplitude being lowered).

#+begin_src matlab
[coh_enc_sweep,    ~] = mscohere(leg_enc_sweep.Va,    leg_enc_sweep.Vs,    win, [], [], 1/Ts);
[coh_enc_noise_hf, ~] = mscohere(leg_enc_noise_hf.Va, leg_enc_noise_hf.Vs, win, [], [], 1/Ts);
#+end_src

#+begin_src matlab :exports none
figure;
hold on;
plot(f, coh_enc_noise_hf, 'color', colors(1, :), 'DisplayName', 'HF Noise');
plot(f, coh_enc_sweep,    'color', colors(2, :), 'DisplayName', 'Sweep');
hold off;
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlim([5, 5e3]); ylim([0, 1]);
legend('location', 'southeast');
#+end_src

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

#+name: fig:strut_1_frf_iff_plant_coh
#+caption: Obtained coherence for the IFF plant
#+RESULTS:
[[file:figs/strut_1_frf_iff_plant_coh.png]]

Then the FRF are estimated and shown in Figure [[fig:strut_1_enc_frf_iff_plant_tf]]
#+begin_src matlab
[iff_enc_sweep,    ~] = tfestimate(leg_enc_sweep.Va,    leg_enc_sweep.Vs,    win, [], [], 1/Ts);
[iff_enc_noise_hf, ~] = tfestimate(leg_enc_noise_hf.Va, leg_enc_noise_hf.Vs, win, [], [], 1/Ts);
#+end_src

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

ax1 = nexttile([2,1]);
hold on;
plot(f(f> 350), abs(iff_enc_noise_hf(f> 350)), 'k-');
plot(f(f<=350), abs(iff_enc_sweep(   f<=350)), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);

ax2 = nexttile;
hold on;
plot(f(f> 350), 180/pi*angle(iff_enc_noise_hf(f> 350)), 'k');
plot(f(f<=350), 180/pi*angle(iff_enc_sweep(   f<=350)), 'k');
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, 2e3]);
#+end_src

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

#+name: fig:strut_1_enc_frf_iff_plant_tf
#+caption:Identified IFF Plant
#+RESULTS:
[[file:figs/strut_1_enc_frf_iff_plant_tf.png]]

Let's now compare the IFF plants whether the encoders are fixed to the APA or not (Figure [[fig:strut_1_frf_iff_comp_enc]]).
#+begin_src matlab :exports none
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f(f> 350), abs(iff_enc_noise_hf(f> 350)), 'color', colors(1, :), ...
     'DisplayName', 'Encoder');
plot(f(f<=350), abs(iff_enc_sweep(   f<=350)), 'color', colors(1, :), ...
     'HandleVisibility', 'off');
plot(f(f> 350), abs(iff_noise_hf(f> 350)), 'color', colors(2, :), ...
     'DisplayName', 'Without Encoder');
plot(f(f<=350), abs(iff_sweep(   f<=350)), 'color', colors(2, :), ...
     'HandleVisibility', 'off');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
ylim([1e-2, 1e2]);

ax2 = nexttile;
hold on;
plot(f(f> 350), 180/pi*angle(iff_enc_noise_hf(f> 350)), 'color', colors(1, :));
plot(f(f<=350), 180/pi*angle(iff_enc_sweep(   f<=350)), 'color', colors(1, :));
plot(f(f> 350), 180/pi*angle(iff_noise_hf(f> 350)), 'color', colors(2, :));
plot(f(f<=350), 180/pi*angle(iff_sweep(   f<=350)), 'color', colors(2, :));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);

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

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

#+name: fig:strut_1_frf_iff_comp_enc
#+caption: Effect of the encoder on the IFF plant
#+RESULTS:
[[file:figs/strut_1_frf_iff_effect_enc.png]]

#+begin_important
We can see that the IFF does not change whether of not the encoder are fixed to the struts.
#+end_important

** Comparison of all the Struts
<<sec:meas_all_struts>>
*** Introduction                                                    :ignore:
Now all struts are measured using the same procedure and test bench.

*** 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
colors = colororder;
#+end_src

#+begin_src matlab :tangle no
addpath('./matlab/mat/');
addpath('./matlab/src/');
addpath('./matlab/');
#+end_src

#+begin_src matlab :eval no
addpath('./mat/');
addpath('./src/');
#+end_src

*** FRF Identification - Setup
The identification is performed in two steps:
1. White noise excitation with small amplitude.
   This is used to estimate the low frequency dynamics.
2. High frequency noise.
   The noise is band-passed between 300Hz and 2kHz.

Then, the result of the first identification is used between 10Hz and 350Hz and the result of the second identification if used between 350Hz and 2kHz.

Here are the LEG numbers that have been measured.
#+begin_src matlab
leg_nums = [1 2 3 4 5];
#+end_src

The data are loaded for both the first and second identification:
#+begin_src matlab
%% Second identification
leg_noise = {};
for i = 1:length(leg_nums)
    leg_noise(i) = {load(sprintf('frf_data_leg_coder_%i_noise.mat', leg_nums(i)), 't', 'Va', 'Vs', 'de', 'da')};
end

%% Third identification
leg_noise_hf = {};
for i = 1:length(leg_nums)
    leg_noise_hf(i) = {load(sprintf('frf_data_leg_coder_%i_noise_hf.mat', leg_nums(i)), 't', 'Va', 'Vs', 'de', 'da')};
end
#+end_src

The time is the same for all measurements.
#+begin_src matlab
%% Time vector
t = leg_noise{1}.t - leg_noise{1}.t(1) ; % Time vector [s]

%% Sampling
Ts = (t(end) - t(1))/(length(t)-1); % Sampling Time [s]
Fs = 1/Ts; % Sampling Frequency [Hz]
#+end_src

Then we defined a "Hanning" windows that will be used for the spectral analysis:
#+begin_src matlab
win = hanning(ceil(0.5*Fs)); % Hannning Windows
#+end_src

We get the frequency vector that will be the same for all the frequency domain analysis.
#+begin_src matlab
% Only used to have the frequency vector "f"
[~, f] = tfestimate(leg_noise{1}.Va, leg_noise{1}.de, win, [], [], 1/Ts);
i_lf = f <= 350;
i_hf = f >  350;
#+end_src

*** FRF Identification - Encoder
In this section, the dynamics from $V_a$ to $d_e$ is identified.

We compute the coherence for 2nd and 3rd identification:
#+begin_src matlab
%% Coherence computation
coh_enc = zeros(length(f), length(leg_nums));
for i = 1:length(leg_nums)
    [coh_lf, ~] = mscohere(leg_noise{i}.Va,    leg_noise{i}.de,    win, [], [], 1/Ts);
    [coh_hf, ~] = mscohere(leg_noise_hf{i}.Va, leg_noise_hf{i}.de, win, [], [], 1/Ts);
    coh_enc(:, i) = [coh_lf(i_lf); coh_hf(i_hf)];
end
#+end_src

The coherence is shown in Figure [[fig:struts_frf_dvf_plant_coh]].
It is clear that the Noise sine gives good coherence up to 400Hz and that the high frequency noise excitation signal helps increasing a little bit the coherence at high frequency.

#+begin_src matlab :exports none
figure;
hold on;
for i = 1:length(leg_nums)
    plot(f, coh_enc(:, i));
end;
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
xlim([5, 5e3]); ylim([0, 1]);
#+end_src

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

#+name: fig:struts_frf_dvf_plant_coh
#+caption: Obtained coherence for the plant from $V_a$ to $d_e$
#+RESULTS:
[[file:figs/struts_frf_dvf_plant_coh.png]]


Then, the transfer function from the DAC output voltage $V_a$ to the measured displacement by the encoders is computed:
#+begin_src matlab
%% Transfer function estimation
enc_frf = zeros(length(f), length(leg_nums));

for i = 1:length(leg_nums)
    [frf_lf, ~] = tfestimate(leg_noise{i}.Va,    leg_noise{i}.de,    win, [], [], 1/Ts);
    [frf_hf, ~] = tfestimate(leg_noise_hf{i}.Va, leg_noise_hf{i}.de, win, [], [], 1/Ts);
    enc_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
end
#+end_src

The obtained transfer functions are shown in Figure [[fig:struts_frf_dvf_plant_tf]].
#+begin_src matlab :exports none
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(leg_nums)
    plot(f, abs(enc_frf(:, i)), ...
         'DisplayName', sprintf('Leg %i', leg_nums(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
ylim([1e-8, 1e-3]);

ax2 = nexttile;
hold on;
for i = 1:length(leg_nums)
    plot(f, 180/pi*angle(enc_frf(:, 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, 2e3]);
#+end_src

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

#+name: fig:struts_frf_dvf_plant_tf
#+caption: Estimated FRF for the DVF plant (transfer function from $V_a$ to the encoder $d_e$)
#+RESULTS:
[[file:figs/struts_frf_dvf_plant_tf.png]]

#+begin_important
Depending on how the APA are mounted with the flexible joints, the dynamics can change a lot as shown in Figure [[fig:struts_frf_dvf_plant_tf]].
In the future, a "pin" will be used to better align the APA with the flexible joints.
We can expect the amplitude of the spurious resonances to decrease.
We can also observe the presence or absence of complex conjugate zeros between the first two modes.
This might also be dependent on the alignment.
#+end_important

*** FRF Identification - Interferometer
In this section, the dynamics from $V_a$ to $d_a$ is identified.

We compute the coherence.
#+begin_src matlab
%% Coherence computation
coh_int = zeros(length(f), length(leg_nums));
for i = 1:length(leg_nums)
    [coh_lf, ~] = mscohere(leg_noise{i}.Va,    leg_noise{i}.da,    win, [], [], 1/Ts);
    [coh_hf, ~] = mscohere(leg_noise_hf{i}.Va, leg_noise_hf{i}.da, win, [], [], 1/Ts);
    coh_int(:, i) = [coh_lf(i_lf); coh_hf(i_hf)];
end
#+end_src

The coherence is shown in Figure [[fig:struts_frf_int_plant_coh]].
It is clear that the Noise sine gives good coherence up to 400Hz and that the high frequency noise excitation signal helps increasing a little bit the coherence at high frequency.
#+begin_src matlab :exports none
figure;
hold on;
for i = 1:length(leg_nums)
    plot(f, coh_int(:, i));
end;
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
xlim([5, 5e3]); ylim([0, 1]);
#+end_src

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

#+name: fig:struts_frf_int_plant_coh
#+caption: Obtained coherence for the plant from $V_a$ to $d_e$
#+RESULTS:
[[file:figs/struts_frf_int_plant_coh.png]]

Then, the transfer function from the DAC output voltage $V_a$ to the measured displacement by the Attocube is computed:
#+begin_src matlab
%% Transfer function estimation
int_frf = zeros(length(f), length(leg_nums));
for i = 1:length(leg_nums)
    [frf_lf, ~] = tfestimate(leg_noise{i}.Va,    leg_noise{i}.da,    win, [], [], 1/Ts);
    [frf_hf, ~] = tfestimate(leg_noise_hf{i}.Va, leg_noise_hf{i}.da, win, [], [], 1/Ts);
    int_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
end
#+end_src

The obtained transfer functions are shown in Figure [[fig:struts_frf_int_plant_tf]].

They are all superimposed except for the LEG7.
#+begin_src matlab :exports none
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(leg_nums)
    plot(f, abs(int_frf(:, i)), ...
         'DisplayName', sprintf('Leg %i', leg_nums(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
ylim([1e-9, 1e-3]);

ax2 = nexttile;
hold on;
for i = 1:length(leg_nums)
    plot(f, 180/pi*angle(int_frf(:, 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, 2e3]);
#+end_src

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

#+name: fig:struts_frf_int_plant_tf
#+caption: Estimated FRF for the DVF plant (transfer function from $V_a$ to the encoder $d_e$)
#+RESULTS:
[[file:figs/struts_frf_int_plant_tf.png]]

*** FRF Identification - Force Sensor
In this section, the dynamics from $V_a$ to $V_s$ is identified.

First the coherence is computed and shown in Figure [[fig:struts_frf_iff_plant_coh]].
The coherence is very nice from 10Hz to 2kHz.
It is only dropping near a zeros at 40Hz, and near the resonance at 95Hz (the excitation amplitude being lowered).

#+begin_src matlab
%% Coherence
coh_iff = zeros(length(f), length(leg_nums));
for i = 1:length(leg_nums)
    [coh_lf, ~] = mscohere(leg_noise{i}.Va,    leg_noise{i}.Vs,    win, [], [], 1/Ts);
    [coh_hf, ~] = mscohere(leg_noise_hf{i}.Va, leg_noise_hf{i}.Vs, win, [], [], 1/Ts);
    coh_iff(:, i) = [coh_lf(i_lf); coh_hf(i_hf)];
end
#+end_src

#+begin_src matlab :exports none
figure;
hold on;
for i = 1:length(leg_nums)
    plot(f, coh_iff(:, i));
end;
hold off;
xlabel('Frequency [Hz]'); ylabel('Coherence [-]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlim([5, 5e3]); ylim([0, 1]);
#+end_src

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

#+name: fig:struts_frf_iff_plant_coh
#+caption: Obtained coherence for the IFF plant
#+RESULTS:
[[file:figs/struts_frf_iff_plant_coh.png]]

Then the FRF are estimated and shown in Figure [[fig:struts_frf_iff_plant_tf]]
#+begin_src matlab
%% FRF estimation of the transfer function from Va to Vs
iff_frf = zeros(length(f), length(leg_nums));
for i = 1:length(leg_nums)
    [frf_lf, ~] = tfestimate(leg_noise{i}.Va,    leg_noise{i}.Vs,    win, [], [], 1/Ts);
    [frf_hf, ~] = tfestimate(leg_noise_hf{i}.Va, leg_noise_hf{i}.Vs, win, [], [], 1/Ts);
    iff_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
end
#+end_src

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

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(leg_nums)
    plot(f, abs(iff_frf(:, i)), ...
         'DisplayName', sprintf('Leg %i', leg_nums(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);

ax2 = nexttile;
hold on;
for i = 1:length(leg_nums)
    plot(f, 180/pi*angle(iff_frf(:, 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, 2e3]);
#+end_src

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

#+name: fig:struts_frf_iff_plant_tf
#+caption:Identified IFF Plant
#+RESULTS:
[[file:figs/struts_frf_iff_plant_tf.png]]

*** Save measured FRF
#+begin_src matlab :tangle no :exports none
save('matlab/mat/meas_struts_frf.mat', 'f', 'Ts', 'enc_frf', 'int_frf', 'iff_frf', 'leg_nums');
#+end_src

#+begin_src matlab :eval no
save('mat/meas_struts_frf.mat', 'f', 'Ts', 'enc_frf', 'int_frf', 'iff_frf', 'leg_nums');
#+end_src

* Test Bench Struts - Simscape Model
<<sec:simscape_bench_struts>>
** Introduction

- Section [[sec:struts_comp_2dof]]:
- Section [[sec:struts_effect_misalignment]]:
- Section [[sec:struts_effect_joint_stiffness]]:

** 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
%% Add useful folders to the path
addpath('matlab/');
addpath('matlab/test_bench_struts/');
addpath('matlab/mat/');
addpath('matlab/src/');
addpath('matlab/png/');
#+end_src

#+begin_src matlab :eval no
%% Add useful folders to the path
addpath('test_bench_struts/');
addpath('png/');
addpath('mat/');
addpath('src/');
#+end_src

#+begin_src matlab
%% Frequency vector used for many plots
freqs = 2*logspace(0, 3, 1000);
#+end_src

#+begin_src matlab
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;

%% Name of the Simulink File
mdl = 'test_bench_struts';

%% Open the Simulink File
open(mdl)
#+end_src

** Comparison with the 2-DoF Model
<<sec:struts_comp_2dof>>
*** First Identification
The object containing all the data is created.
#+begin_src matlab
%% Initialize structure containing data for the Simscape model
n_hexapod = struct();
n_hexapod.flex_bot = initializeBotFlexibleJoint('type', '2dof');
n_hexapod.flex_top = initializeTopFlexibleJoint('type', '2dof');
n_hexapod.actuator = initializeAPA('type', '2dof');
#+end_src

#+begin_src matlab
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Va'],  1, 'openinput');  io_i = io_i + 1; % Actuator Voltage
io(io_i) = linio([mdl, '/Vs'],  1, 'openoutput'); io_i = io_i + 1; % Sensor Voltage
io(io_i) = linio([mdl, '/dL'],  1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs
io(io_i) = linio([mdl, '/z'],   1, 'openoutput'); io_i = io_i + 1; % Vertical Motion
#+end_src

#+begin_src matlab
%% Run the linearization
Gs = linearize(mdl, io, 0.0, options);
Gs.InputName  = {'Va'};
Gs.OutputName = {'Vs', 'dL', 'z'};
#+end_src

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

ax1 = nexttile([2,1]);
plot(freqs, abs(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))), 'k-')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))), 'k-')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360);
ylim([0, 180])

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

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

#+name: fig:strut_bench_model_iff_bode
#+caption: Identified transfer function from $V_a$ to $V_s$
#+RESULTS:
[[file:figs/strut_bench_model_iff_bode.png]]

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

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(Gs('dL', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Encoder')
plot(freqs, abs(squeeze(freqresp(Gs('z', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Interferometer')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'southwest');

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('dL', 'Va'), freqs, 'Hz'))))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('z', 'Va'), freqs, 'Hz'))))
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360);
ylim([-180, 180])

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

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

#+name: fig:strut_bench_model_dvf_bode
#+caption: Identified transfer function from $V_a$ to $d_L$
#+RESULTS:
[[file:figs/strut_bench_model_dvf_bode.png]]

*** Effect of flexible joints
Perfect
#+begin_src matlab
%% Perfect Joints
n_hexapod.flex_bot = initializeBotFlexibleJoint('type', '2dof');
n_hexapod.flex_top = initializeTopFlexibleJoint('type', '3dof');

Gp = linearize(mdl, io, 0.0, options);
Gp.InputName  = {'Va'};
Gp.OutputName = {'Vs', 'dL', 'z'};
#+end_src

Top Flexible
#+begin_src matlab
%% Top joint with Axial stiffness
n_hexapod.flex_bot = initializeBotFlexibleJoint('type', '2dof');
n_hexapod.flex_top = initializeTopFlexibleJoint('type', '4dof');

Gt = linearize(mdl, io, 0.0, options);
Gt.InputName  = {'Va'};
Gt.OutputName = {'Vs', 'dL', 'z'};
#+end_src

Bottom Flexible
#+begin_src matlab
%% Bottom joint with Axial stiffness
n_hexapod.flex_bot = initializeBotFlexibleJoint('type', '4dof');
n_hexapod.flex_top = initializeTopFlexibleJoint('type', '3dof');

Gb = linearize(mdl, io, 0.0, options);
Gb.InputName  = {'Va'};
Gb.OutputName = {'Vs', 'dL', 'z'};
#+end_src

Both Flexible
#+begin_src matlab
%% Both joints with Axial stiffness
n_hexapod.flex_bot = initializeBotFlexibleJoint('type', '4dof');
n_hexapod.flex_top = initializeTopFlexibleJoint('type', '4dof');

Gf = linearize(mdl, io, 0.0, options);
Gf.InputName  = {'Va'};
Gf.OutputName = {'Vs', 'dL', 'z'};
#+end_src

Comparison in Figures [[fig:strut_effect_joint_comp_vs]], [[fig:strut_effect_joint_comp_dl]] and [[fig:strut_effect_joint_comp_z]].

#+begin_src matlab :exports none
%% Transfer function from u to taum - Effect of flexible joints
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(Gp('Vs', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Perfect')
plot(freqs, abs(squeeze(freqresp(Gt('Vs', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Top')
plot(freqs, abs(squeeze(freqresp(Gb('Vs', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Bot')
plot(freqs, abs(squeeze(freqresp(Gf('Vs', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Flex')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'southeast');

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Gp('Vs', 'Va'), freqs, 'Hz'))))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gt('Vs', 'Va'), freqs, 'Hz'))))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gb('Vs', 'Va'), freqs, 'Hz'))))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gf('Vs', 'Va'), freqs, 'Hz'))))
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360);
ylim([-180, 180])

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

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

#+name: fig:strut_effect_joint_comp_vs
#+caption: Effect of the joint's flexibility on the transfer function from $V_a$ to $V_s$
#+RESULTS:
[[file:figs/strut_effect_joint_comp_vs.png]]

#+begin_src matlab :exports none
%% Transfer function from u to dLm - Effect of flexible joints
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(Gp('dL', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Perfect')
plot(freqs, abs(squeeze(freqresp(Gt('dL', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Top')
plot(freqs, abs(squeeze(freqresp(Gb('dL', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Bot')
plot(freqs, abs(squeeze(freqresp(Gf('dL', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Flex')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_L/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'southwest');

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Gp('dL', 'Va'), freqs, 'Hz'))))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gt('dL', 'Va'), freqs, 'Hz'))))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gb('dL', 'Va'), freqs, 'Hz'))))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gf('dL', 'Va'), freqs, 'Hz'))))
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360);
ylim([-180, 180])

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

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

#+name: fig:strut_effect_joint_comp_dl
#+caption: Effect of the joint's flexibility on the transfer function from $V_a$ to $d_L$ (encoder)
#+RESULTS:
[[file:figs/strut_effect_joint_comp_dl.png]]

#+begin_src matlab :exports none
%% Transfer function from u to dZm - Effect of flexible joints
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(Gp('z', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Perfect')
plot(freqs, abs(squeeze(freqresp(Gt('z', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Top')
plot(freqs, abs(squeeze(freqresp(Gb('z', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Bot')
plot(freqs, abs(squeeze(freqresp(Gf('z', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Flex')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $z/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'southwest');

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Gp('z', 'Va'), freqs, 'Hz'))))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gt('z', 'Va'), freqs, 'Hz'))))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gb('z', 'Va'), freqs, 'Hz'))))
plot(freqs, 180/pi*angle(squeeze(freqresp(Gf('z', 'Va'), freqs, 'Hz'))))
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360);
ylim([-180, 180])

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

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

#+name: fig:strut_effect_joint_comp_z
#+caption: Effect of the joint's flexibility on the transfer function from $V_a$ to $z$ (interferometer)
#+RESULTS:
[[file:figs/strut_effect_joint_comp_z.png]]

#+begin_important
Imperfection of the flexible joints has the largest impact on the transfer function from $V_a$ to $d_L$.
However, it has relatively small impact on the transfer functions from $V_a$ to $V_s$ and to $z$.
#+end_important

*** TODO Integral Force Feedback                                  :noexport:
#+begin_src matlab
%% Initialize typical system
n_hexapod = struct();
n_hexapod.flex_bot = initializeBotFlexibleJoint('type', '2dof');
n_hexapod.flex_top = initializeTopFlexibleJoint('type', '3dof');
n_hexapod.actuator = initializeAPA('type', '2dof');
#+end_src

#+begin_src matlab
%% Identify the dynamics
G = linearize(mdl, io, 0.0, options);
G.InputName  = {'Va'};
G.OutputName = {'Vs', 'dL', 'z'};
#+end_src

#+begin_src matlab
%% IFF Plant (transfer function from u to taum)
Giff = G('Vs', 'Va');
#+end_src

\begin{equation}
K_{\text{IFF}} = \frac{g}{s + \omega_c}
\end{equation}

#+begin_src matlab
%% Cut-off frequency of the LPF
wc = 2*pi*20;
#+end_src

#+begin_src matlab :exports none
% Gains used for the Root Locus plot
gains = logspace(1, 5, 300);

%% Root Locus for the IFF
figure;

hold on;
% Pure Integrator
set(gca,'ColorOrderIndex',1);
plot(real(pole(Giff)),  imag(pole(Giff)), 'x', 'DisplayName', '$g = 0$');
set(gca,'ColorOrderIndex',1);
plot(real(tzero(Giff)),  imag(tzero(Giff)), 'o', 'HandleVisibility', 'off');
for g = gains
    clpoles = pole(feedback(Giff, (g/(s + wc))));
    set(gca,'ColorOrderIndex',1);
    plot(real(clpoles), imag(clpoles), '.', 'HandleVisibility', 'off');
end

g = 2e2;
clpoles = pole(feedback(Giff, (g/(s + wc))));
set(gca,'ColorOrderIndex',2);
plot(real(clpoles), imag(clpoles), 'x', 'DisplayName', sprintf('$g=%.0f$', g));
axis square;
xlim([-700, 0]); ylim([-0, 700]);
xlabel('Real Part'); ylabel('Imaginary Part');
legend('location', 'northwest');
#+end_src

*** Comparison with the experimental Data
#+begin_src matlab
%% Load measured FRF
load('meas_struts_frf.mat', 'f', 'Ts', 'enc_frf', 'int_frf', 'iff_frf', 'leg_nums');
#+end_src

#+begin_src matlab
%% Initialize Simscape data
n_hexapod.flex_bot = initializeBotFlexibleJoint('type', '4dof');
n_hexapod.flex_top = initializeTopFlexibleJoint('type', '4dof');
n_hexapod.actuator = initializeAPA('type', '2dof');
#+end_src

#+begin_src matlab
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Va'],  1, 'openinput');  io_i = io_i + 1; % Actuator Voltage
io(io_i) = linio([mdl, '/Vs'],  1, 'openoutput'); io_i = io_i + 1; % Sensor Voltage
io(io_i) = linio([mdl, '/z'],   1, 'openoutput'); io_i = io_i + 1; % Interferometer
io(io_i) = linio([mdl, '/dL'],   1, 'openoutput'); io_i = io_i + 1; % Encoder

%% Identification
Gs = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
Gs.InputName  = {'Va'};
Gs.OutputName = {'Vs', 'z', 'dL'};
#+end_src

#+begin_src matlab :exports none
%% Compare the FRF and identified dynamics from DAC voltage to force sensor voltage and interferometer
freqs = 5*logspace(0, 3, 1000);

figure;
tiledlayout(3, 2, 'TileSpacing', 'None', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(leg_nums)
    plot(f, abs(int_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',2);
plot(freqs, abs(squeeze(freqresp(Gs('z', 'Va'), freqs, 'Hz'))), '-')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);

ax1b = nexttile([2,1]);
hold on;
for i = 1:length(leg_nums)
    plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',2);
plot(freqs, abs(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))), '-')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);

ax2 = nexttile;
hold on;
for i = 1:length(leg_nums)
    plot(f, 180/pi*angle(int_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',2);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('z', 'Va'), freqs, 'Hz'))), '-')
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]);

ax2b = nexttile;
hold on;
for i = 1:length(leg_nums)
    plot(f, 180/pi*angle(iff_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',2);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))), '-')
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,ax1b,ax2b],'x');
xlim([10, 2e3]);
#+end_src

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

#+name: fig:comp_strut_plant_after_opt
#+caption: Comparison of the measured FRF and the optimized model
#+RESULTS:
[[file:figs/comp_strut_plant_after_opt.png]]

#+begin_src matlab :exports none
%% Compare the FRF and identified dynamics from DAC voltage to force sensor voltage and interferometer
freqs = 2*logspace(1, 3, 1000);

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

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(leg_nums)
    plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',2);
plot(freqs, abs(squeeze(freqresp(Gs('dL', 'Va'), freqs, 'Hz'))), '-')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);

ax2 = nexttile;
hold on;
for i = 1:length(leg_nums)
    plot(f, 180/pi*angle(enc_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',2);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('dL', 'Va'), freqs, 'Hz'))), '-')
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/comp_strut_plant_iff_after_opt.pdf', 'width', 'normal', 'height', 'tall');
#+end_src

#+name: fig:comp_strut_plant_iff_after_opt
#+caption: Comparison of the measured FRF and the optimized model
#+RESULTS:
[[file:figs/comp_strut_plant_iff_after_opt.png]]

#+begin_important
The 2-DoF model is quite effective in modelling the transfer function from actuator to force sensor and from actuator to interferometer (Figure [[fig:comp_strut_plant_after_opt]]).
But it is not effective in modeling the transfer function from actuator to encoder (Figure [[fig:comp_strut_plant_iff_after_opt]]).
This is due to the fact that resonances greatly affecting the encoder reading are not modelled.
In the next section, flexible model of the APA will be used to model such resonances.
#+end_important

** Effect of a misalignment of the APA and flexible joints on the transfer function from actuator to encoder
<<sec:struts_effect_misalignment>>
*** Introduction                                                    :ignore:

As shown in Figure [[fig:struts_frf_dvf_plant_tf]], the dynamics from actuator to encoder for all the struts is very different.

This could be explained by a large variability in the alignment of the flexible joints and the APA (at the time, the alignment pins were not used).

Depending on the alignment, the spurious resonances of the struts (Figure [[fig:apa_mode_shapes]]) can be excited differently.

#+name: fig:apa_mode_shapes
#+caption: Spurious resonances. a) X-bending mode at 189Hz. b) Y-bending mode at 285Hz. c) Z-torsion mode at 400Hz
#+attr_latex: :width \linewidth
[[file:figs/apa_mode_shapes.gif]]

For instance, consider Figure [[fig:strut_misalign_schematic]] where there is a misalignment in the $y$ direction.
In such case, the mode at 200Hz is foreseen to be more excited as the misalignment $d_y$ increases and therefore the dynamics from the actuator to the encoder should also change around 200Hz.

#+name: fig:strut_misalign_schematic
#+caption: Mis-alignement between the joints and the APA
[[file:figs/strut_misalign_schematic.png]]

If the misalignment is in the $x$ direction, the mode at 300Hz should be more affected whereas a misalignment in the $z$ direction should not affect these resonances.

Such statement is studied in this section.

*** Load Data
First, the measured FRF are loaded.
#+begin_src matlab
load('meas_struts_frf.mat', 'f', 'Ts', 'enc_frf', 'int_frf', 'iff_frf', 'leg_nums');
#+end_src

*** Perfectly aligned APA
Let's first consider that the strut is perfectly mounted such that the two flexible joints and the APA are aligned.
#+begin_src matlab
%% Initialize Simscape data
n_hexapod.flex_bot = initializeBotFlexibleJoint('type', '4dof');
n_hexapod.flex_top = initializeTopFlexibleJoint('type', '4dof');
n_hexapod.actuator = initializeAPA('type', 'flexible');
#+end_src

And define the inputs and outputs of the models:
- Input: voltage generated by the DAC
- Output: measured displacement by the encoder

#+begin_src matlab
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Va'],  1, 'openinput');  io_i = io_i + 1; % Actuator Voltage
io(io_i) = linio([mdl, '/dL'],  1, 'openoutput'); io_i = io_i + 1; % Encoder
#+end_src

The transfer function is identified and shown in Figure [[fig:comp_enc_frf_align_perfect]].
#+begin_src matlab
%% Identification
Gs = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
Gs.InputName  = {'Va'};
Gs.OutputName = {'dL'};
#+end_src

#+begin_important
From Figure [[fig:comp_enc_frf_align_perfect]], it is clear that:
1. The model with perfect alignment is not matching the measured FRF
2. The measured FRF have different shapes
#+end_important

#+begin_question
Why is the flexible mode of the strut at 200Hz is not seen in the model in Figure [[fig:comp_enc_frf_align_perfect]]?
Probably because the presence of this mode is not due because of the "unbalanced" mass of the encoder, but rather because of the misalignment of the APA with respect to the two flexible joints.
This will be verified in the next sections.
#+end_question

#+begin_src matlab :exports none
freqs = 5*logspace(0, 3, 1000);

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

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2], ...
     'DisplayName', 'Measured FRF');
for i = 2:length(leg_nums)
    plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2], ...
         'HandleVisibility', 'off');
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('dL', 'Va'), freqs, 'Hz'))), '-', ...
     'DisplayName', 'Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
legend('location', 'northeast');

ax2 = nexttile;
hold on;
for i = 1:length(leg_nums)
    plot(f, 180/pi*angle(enc_frf(:, i)), 'color', [0,0,0,0.2]);
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('dL', 'Va'), freqs, 'Hz'))), '-')
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, 2e3]);
#+end_src

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

#+name: fig:comp_enc_frf_align_perfect
#+caption: Comparison of the model with a perfectly aligned APA and flexible joints with the measured FRF from actuator to encoder
#+RESULTS:
[[file:figs/comp_enc_frf_align_perfect.png]]

*** Effect of a misalignment in y
Let's compute the transfer function from output DAC voltage to the measured displacement by the encoder for several misalignment in the $y$ direction:
#+begin_src matlab
%% Considered misalignments
dy_aligns = [-0.5, -0.1, 0, 0.1, 0.5]*1e-3; % [m]
#+end_src

#+begin_src matlab
%% Transfer functions from u to dL for all the misalignment in y direction
Gs_align = {zeros(length(dy_aligns), 1)};

for i = 1:length(dy_aligns)
    n_hexapod.actuator = initializeAPA('type', 'flexible', 'd_align', [0; dy_aligns(i); 0]);

    G = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
    G.InputName  = {'Va'};
    G.OutputName = {'dL'};

    Gs_align(i) = {G};
end
#+end_src

The obtained dynamics are shown in Figure [[fig:effect_misalignment_y]].

#+begin_src matlab :exports none
freqs = 2*logspace(1, 3, 1000);

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

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(dy_aligns)
    plot(freqs, abs(squeeze(freqresp(Gs_align{i}('dL', 'Va'), freqs, 'Hz'))), ...
         'DisplayName', sprintf('$d_y = %.1f$ [mm]', 1e3*dy_aligns(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
legend('location', 'northeast');

ax2 = nexttile;
hold on;
for i = 1:length(dy_aligns)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_align{i}('dL', 'Va'), 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); ylim([-180, 180]);

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

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

#+name: fig:effect_misalignment_y
#+caption: Effect of a misalignement in the $y$ direction
#+RESULTS:
[[file:figs/effect_misalignment_y.png]]


#+begin_important
The alignment of the APA with the flexible joints as a *huge* influence on the dynamics from actuator voltage to measured displacement by the encoder.
The misalignment in the $y$ direction mostly influences:
- the presence of the flexible mode at 200Hz
- the location of the complex conjugate zero between the first two resonances:
  - if $d_y < 0$: there is no zero between the two resonances and possibly not even between the second and third ones
  - if $d_y > 0$: there is a complex conjugate zero between the first two resonances
- the location of the high frequency complex conjugate zeros at 500Hz (secondary effect, as the axial stiffness of the joint also has large effect on the position of this zero)
#+end_important

*** Effect of a misalignment in x
Let's compute the transfer function from output DAC voltage to the measured displacement by the encoder for several misalignment in the $x$ direction:
#+begin_src matlab
%% Considered misalignments
dx_aligns = [-0.1, -0.05, 0, 0.05, 0.1]*1e-3; % [m]
#+end_src

#+begin_src matlab
%% Transfer functions from u to dL for all the misalignment in x direction
Gs_align = {zeros(length(dx_aligns), 1)};

for i = 1:length(dx_aligns)
    n_hexapod.actuator = initializeAPA('type', 'flexible', 'd_align', [dx_aligns(i); 0; 0]);

    G = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
    G.InputName  = {'Va'};
    G.OutputName = {'dL'};

    Gs_align(i) = {G};
end
#+end_src

The obtained dynamics are shown in Figure [[fig:effect_misalignment_x]].
#+begin_src matlab :exports none
freqs = 2*logspace(1, 3, 1000);

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

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(dx_aligns)
    plot(freqs, abs(squeeze(freqresp(Gs_align{i}('dL', 'Va'), freqs, 'Hz'))), ...
         'DisplayName', sprintf('$d_x = %.2f$ [mm]', 1e3*dx_aligns(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
legend('location', 'northeast');

ax2 = nexttile;
hold on;
for i = 1:length(dx_aligns)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_align{i}('dL', 'Va'), 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); ylim([-180, 180]);

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

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

#+name: fig:effect_misalignment_x
#+caption: Effect of a misalignement in the $x$ direction
#+RESULTS:
[[file:figs/effect_misalignment_x.png]]

#+begin_important
The misalignment in the $x$ direction mostly influences the presence of the flexible mode at 300Hz.
#+end_important

*** Find the misalignment of each strut
From the previous analysis on the effect of a $x$ and $y$ misalignment, it is possible to estimate the $x,y$ misalignment of the measured struts.

The misalignment that gives the best match for the FRF are defined below.
#+begin_src matlab
%% Tuned misalignment [m]
d_aligns = [[-0.05,  -0.3,  0];
            [ 0,      0.5,  0];
            [-0.1,   -0.3,  0];
            [ 0,      0.3,  0];
            [-0.05,   0.05, 0]]'*1e-3;
#+end_src

For each misalignment, the dynamics from the DAC voltage to the encoder measurement is identified.
#+begin_src matlab
%% Idenfity the transfer function from actuator to encoder for all cases
Gs_align = {zeros(size(d_aligns,2), 1)};

for i = 1:size(d_aligns,2)
    n_hexapod.actuator = initializeAPA('type', 'flexible', 'd_align', d_aligns(:,i));

    G = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
    G.InputName  = {'Va'};
    G.OutputName = {'dL'};

    Gs_align(i) = {G};
end
#+end_src

The results are shown in Figure [[fig:comp_all_struts_corrected_misalign]].
#+begin_src matlab :exports none
%% Comparison of the plants (encoder output) when tuning the misalignment
freqs = 2*logspace(1, 3, 1000);

figure;
tiledlayout(2, 3, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile();
hold on;
plot(f, abs(enc_frf(:, 1)));
plot(freqs, abs(squeeze(freqresp(Gs_align{1}('dL', 'Va'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude $d_e/V_a$ [m/V]');

ax2 = nexttile();
hold on;
plot(f, abs(enc_frf(:, 2)));
plot(freqs, abs(squeeze(freqresp(Gs_align{2}('dL', 'Va'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude $d_e/V_a$ [m/V]');

ax3 = nexttile();
hold on;
plot(f, abs(enc_frf(:, 3)));
plot(freqs, abs(squeeze(freqresp(Gs_align{3}('dL', 'Va'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude $d_e/V_a$ [m/V]');


ax4 = nexttile();
hold on;
plot(f, abs(enc_frf(:, 4)));
plot(freqs, abs(squeeze(freqresp(Gs_align{4}('dL', 'Va'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude $d_e/V_a$ [m/V]');

ax5 = nexttile();
hold on;
plot(f, abs(enc_frf(:, 5)), 'DisplayName', 'Meas.');
plot(freqs, abs(squeeze(freqresp(Gs_align{5}('dL', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Model');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude $d_e/V_a$ [m/V]');
legend('location', 'Northeast')

linkaxes([ax1,ax2,ax3,ax4,ax5],'xy');
ylim([1e-8, 1e-3]);
xlim([20, 2e3]);
#+end_src

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

#+name: fig:comp_all_struts_corrected_misalign
#+caption: Comparison (model and measurements) of the FRF from DAC voltage u to measured displacement by the encoders for all the struts
#+RESULTS:
[[file:figs/comp_all_struts_corrected_misalign.png]]

#+begin_important
By tuning the misalignment of the APA with respect to the flexible joints, it is relatively easy to obtain a good fit between the model and the measurements (Figure [[fig:comp_all_struts_corrected_misalign]]).

If encoders are to be used when fixed on the struts, it is therefore very important to properly align the APA and the flexible joints when mounting the struts.

This should be achieve by using alignment pins.
#+end_important

** Effect of flexible joint's stiffness
<<sec:struts_effect_joint_stiffness>>
*** Introduction                                                    :ignore:

*** Effect of bending stiffness of the flexible joints
Let's initialize an APA which is a little bit misaligned.
#+begin_src matlab
n_hexapod.actuator = initializeAPA('type', 'flexible', 'd_align', [0.1e-3; 0.5e-3; 0]);
#+end_src

The bending stiffnesses for which the dynamics is identified are defined below.
#+begin_src matlab
%% Tested bending stiffnesses [Nm/rad]
kRs = [3, 4, 5, 6, 7];
#+end_src

Then the identification is performed for all the values of the bending stiffnesses.
#+begin_src matlab
%% Idenfity the transfer function from actuator to encoder for all bending stiffnesses
Gs = {zeros(length(kRs), 1)};

for i = 1:length(kRs)
    n_hexapod.flex_bot = initializeBotFlexibleJoint(...
        'type', '4dof', ...
        'kRx', kRs(i), ...
        'kRy', kRs(i));
    n_hexapod.flex_top = initializeTopFlexibleJoint(...
        'type', '4dof', ...
        'kRx', kRs(i), ...
        'kRy', kRs(i));

    G = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
    G.InputName  = {'Va'};
    G.OutputName = {'dL'};

    Gs(i) = {G};
end
#+end_src

The obtained dynamics from DAC voltage to encoder measurements are compared in Figure [[fig:effect_enc_bending_stiff]].
It is clear that the bending stiffness has a negligible impact on the observed dynamics.

#+begin_src matlab :exports none
freqs = 2*logspace(1, 3, 1000);

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

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(kRs)
    plot(freqs, abs(squeeze(freqresp(Gs{i}('dL', 'Va'), freqs, 'Hz'))), ...
         'DisplayName', sprintf('$k_R = %.0f$ [Nm/rad]', kRs(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
legend('location', 'northeast');

ax2 = nexttile;
hold on;
for i = 1:length(kRs)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('dL', 'Va'), 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); ylim([-180, 180]);

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

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

#+name: fig:effect_enc_bending_stiff
#+caption: Dynamics from DAC output to encoder for several bending stiffnesses
#+RESULTS:
[[file:figs/effect_enc_bending_stiff.png]]

*** Effect of axial stiffness of the flexible joints
The axial stiffnesses for which the dynamics is identified are defined below.
#+begin_src matlab
%% Tested axial stiffnesses [N/m]
kzs = [5e7 7.5e7 1e8 2.5e8];
#+end_src

Then the identification is performed for all the values of the bending stiffnesses.
#+begin_src matlab
%% Idenfity the transfer function from actuator to encoder for all bending stiffnesses
Gs = {zeros(length(kzs), 1)};

for i = 1:length(kzs)
    n_hexapod.flex_bot = initializeBotFlexibleJoint(...
        'type', '4dof', ...
        'kz', kzs(i));
    n_hexapod.flex_top = initializeTopFlexibleJoint(...
        'type', '4dof', ...
        'kz', kzs(i));

    G = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
    G.InputName  = {'Va'};
    G.OutputName = {'dL'};

    Gs(i) = {G};
end
#+end_src

The obtained dynamics from DAC voltage to encoder measurements are compared in Figure [[fig:effect_enc_bending_stiff]].
#+begin_src matlab :exports none
freqs = 2*logspace(1, 3, 1000);

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

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(kzs)
    plot(freqs, abs(squeeze(freqresp(Gs{i}('dL', 'Va'), freqs, 'Hz'))), ...
         'DisplayName', sprintf('$k_z = %.1e$ [N/m]', kzs(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
legend('location', 'northeast');

ax2 = nexttile;
hold on;
for i = 1:length(kzs)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('dL', 'Va'), 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); ylim([-180, 180]);

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

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

#+name: fig:effect_enc_axial_stiff
#+caption: Dynamics from DAC output to encoder for several axial stiffnesses
#+RESULTS:
[[file:figs/effect_enc_axial_stiff.png]]

#+begin_important
The axial stiffness of the flexible joint has a large impact on the frequency of the complex conjugate zero.
Using the measured FRF on the test-bench, if is therefore possible to estimate the axial stiffness of the flexible joints from le location of the zero.
This method gives nice match between the measured FRF and the one extracted from the simscape model, however it could give not so accurate values of the joint's axial stiffness as other factors are also influencing the location of the zero.

Using this method, an axial stiffness of $70 N/\mu m$ is found to give good results (and is reasonable based on the finite element models).
#+end_important

*** TODO Effect of bending damping

* TODO Compare with the FEM/Simscape Model                          :noexport:
:PROPERTIES:
:header-args:matlab+: :tangle matlab/APA300ML.m
:END:

** Introduction                                                      :ignore:
In this section, the Amplified Piezoelectric Actuator APA300ML ([[file:doc/APA300ML.pdf][doc]]) is modeled using a Finite Element Software.
Then a /super element/ is exported and imported in Simscape where its dynamic is studied.

A 3D view of the Amplified Piezoelectric Actuator (APA300ML) is shown in Figure [[fig:apa300ml_ansys]].
The remote point used are also shown in this figure.

#+name: fig:apa300ml_ansys
#+caption: Ansys FEM of the APA300ML
[[file:figs/apa300ml_ansys.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
  addpath('matlab/');
  addpath('matlab/APA300ML/');
#+end_src

#+begin_src matlab :eval no
  addpath('APA300ML/');
#+end_src

#+begin_src matlab
  open('APA300ML.slx');
#+end_src

** Import Mass Matrix, Stiffness Matrix, and Interface Nodes Coordinates
We first extract the stiffness and mass matrices.
#+begin_src matlab
  K = readmatrix('APA300ML_mat_K.CSV');
  M = readmatrix('APA300ML_mat_M.CSV');
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no
  data2orgtable(K(1:10, 1:10), {}, {}, ' %.1g ');
#+end_src

#+caption: First 10x10 elements of the Stiffness matrix
#+RESULTS:
| 200000000.0 |    30000.0 |  -20000.0 |     -70.0 | 300000.0 |    40.0 |  10000000.0 |    10000.0 |   -6000.0 |     30.0 |
|     30000.0 | 30000000.0 |    2000.0 | -200000.0 |     60.0 |   -10.0 |      4000.0 |  2000000.0 |    -500.0 |   9000.0 |
|    -20000.0 |     2000.0 | 7000000.0 |     -10.0 |    -30.0 |    10.0 |      6000.0 |      900.0 | -500000.0 |        3 |
|       -70.0 |  -200000.0 |     -10.0 |    1000.0 |     -0.1 |    0.08 |       -20.0 |    -9000.0 |         3 |    -30.0 |
|    300000.0 |       60.0 |     -30.0 |      -0.1 |    900.0 |     0.1 |     30000.0 |       20.0 |     -10.0 |     0.06 |
|        40.0 |      -10.0 |      10.0 |      0.08 |      0.1 | 10000.0 |        20.0 |          9 |        -5 |     0.03 |
|  10000000.0 |     4000.0 |    6000.0 |     -20.0 |  30000.0 |    20.0 | 200000000.0 |    10000.0 |    9000.0 |     50.0 |
|     10000.0 |  2000000.0 |     900.0 |   -9000.0 |     20.0 |       9 |     10000.0 | 30000000.0 |    -500.0 | 200000.0 |
|     -6000.0 |     -500.0 | -500000.0 |         3 |    -10.0 |      -5 |      9000.0 |     -500.0 | 7000000.0 |       -2 |
|        30.0 |     9000.0 |         3 |     -30.0 |     0.06 |    0.03 |        50.0 |   200000.0 |        -2 |   1000.0 |


#+begin_src matlab :exports results :results value table replace :tangle no
  data2orgtable(M(1:10, 1:10), {}, {}, ' %.1g ');
#+end_src

#+caption: First 10x10 elements of the Mass matrix
#+RESULTS:
|    0.01 |  -2e-06 |  1e-06 |  6e-09 |  5e-05 | -5e-09 | -0.0005 |  -7e-07 |  6e-07 | -3e-09 |
|  -2e-06 |    0.01 |  8e-07 | -2e-05 | -8e-09 |  2e-09 |  -9e-07 | -0.0002 |  1e-08 | -9e-07 |
|   1e-06 |   8e-07 |  0.009 |  5e-10 |  1e-09 | -1e-09 |  -5e-07 |   3e-08 |  6e-05 |  1e-10 |
|   6e-09 |  -2e-05 |  5e-10 |  3e-07 |  2e-11 | -3e-12 |   3e-09 |   9e-07 | -4e-10 |  3e-09 |
|   5e-05 |  -8e-09 |  1e-09 |  2e-11 |  6e-07 | -4e-11 |  -1e-06 |  -2e-09 |  1e-09 | -8e-12 |
|  -5e-09 |   2e-09 | -1e-09 | -3e-12 | -4e-11 |  1e-07 |  -2e-09 |  -1e-09 | -4e-10 | -5e-12 |
| -0.0005 |  -9e-07 | -5e-07 |  3e-09 | -1e-06 | -2e-09 |    0.01 |   1e-07 | -3e-07 | -2e-08 |
|  -7e-07 | -0.0002 |  3e-08 |  9e-07 | -2e-09 | -1e-09 |   1e-07 |    0.01 | -4e-07 |  2e-05 |
|   6e-07 |   1e-08 |  6e-05 | -4e-10 |  1e-09 | -4e-10 |  -3e-07 |  -4e-07 |  0.009 | -2e-10 |
|  -3e-09 |  -9e-07 |  1e-10 |  3e-09 | -8e-12 | -5e-12 |  -2e-08 |   2e-05 | -2e-10 |  3e-07 |


Then, we extract the coordinates of the interface nodes.
#+begin_src matlab
  [int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('APA300ML_out_nodes_3D.txt');
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
  data2orgtable([[1:length(int_i)]', int_i, int_xyz], {}, {'Node i', 'Node Number', 'x [m]', 'y [m]', 'z [m]'}, ' %f ');
#+end_src

#+caption: Coordinates of the interface nodes
#+RESULTS:
| Node i | Node Number |   x [m] | y [m] |  z [m] |
|--------+-------------+---------+-------+--------|
|    1.0 |    697783.0 |     0.0 |   0.0 | -0.015 |
|    2.0 |    697784.0 |     0.0 |   0.0 |  0.015 |
|    3.0 |    697785.0 | -0.0325 |   0.0 |    0.0 |
|    4.0 |    697786.0 | -0.0125 |   0.0 |    0.0 |
|    5.0 |    697787.0 | -0.0075 |   0.0 |    0.0 |
|    6.0 |    697788.0 |  0.0125 |   0.0 |    0.0 |
|    7.0 |    697789.0 |  0.0325 |   0.0 |    0.0 |

#+begin_src matlab :exports results :results value table replace :tangle no
  data2orgtable([length(n_i); length(int_i); size(M,1) - 6*length(int_i); size(M,1)], {'Total number of Nodes', 'Number of interface Nodes', 'Number of Modes', 'Size of M and K matrices'}, {}, ' %.0f ');
#+end_src

#+caption: Some extracted parameters of the FEM
#+RESULTS:
| Total number of Nodes     |   7 |
| Number of interface Nodes |   7 |
| Number of Modes           | 120 |
| Size of M and K matrices  | 162 |

Using =K=, =M= and =int_xyz=, we can now use the =Reduced Order Flexible Solid= simscape block.

** Piezoelectric parameters
#+begin_src matlab
  Ga = 1; % [N/V]
  Gs = 1; % [V/m]
#+end_src

#+begin_src matlab
  m = 0.1; % [kg]
#+end_src

** Simscape Model
The flexible element is imported using the =Reduced Order Flexible Solid= simscape block.

Let's say we use two stacks as a force sensor and one stack as an actuator:
- A =Relative Motion Sensor= block is added between the nodes A and C
- An =Internal Force= block is added between the remote points E and B

The interface nodes are shown in Figure [[fig:apa300ml_ansys]].

One mass is fixed at one end of the piezo-electric stack actuator (remove point F), the other end is fixed to the world frame (remote point G).

** Identification of the APA Characteristics
*** Stiffness
#+begin_src matlab :exports none
  m = 0.0001;
#+end_src

The transfer function from vertical external force to the relative vertical displacement is identified.

#+begin_src matlab :exports none
  %% Name of the Simulink File
  mdl = 'APA300ML';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Fd'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/z'],  1, 'openoutput'); io_i = io_i + 1;

  G = linearize(mdl, io);
#+end_src

The inverse of its DC gain is the axial stiffness of the APA:
#+begin_src matlab :results replace value
  1e-6/dcgain(G) % [N/um]
#+end_src

#+RESULTS:
: 1.753

The specified stiffness in the datasheet is $k = 1.8\, [N/\mu m]$.

*** Resonance Frequency
The resonance frequency is specified to be between 650Hz and 840Hz.
This is also the case for the FEM model (Figure [[fig:apa300ml_resonance]]).

#+begin_src matlab :exports none
  freqs = logspace(2, 4, 5000);

  figure;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G, freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Amplitude');
  hold off;
#+end_src

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

#+name: fig:apa300ml_resonance
#+caption: First resonance is around 800Hz
#+RESULTS:
[[file:figs/apa300ml_resonance.png]]

*** Amplification factor
The amplification factor is the ratio of the vertical displacement to the stack displacement.

#+begin_src matlab :exports none
  %% Name of the Simulink File
  mdl = 'APA300ML';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/F'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/z'], 1, 'openoutput'); io_i = io_i + 1;
  io(io_i) = linio([mdl, '/d'], 1, 'openoutput'); io_i = io_i + 1;

  G = linearize(mdl, io);
#+end_src

The ratio of the two displacement is computed from the FEM model.
#+begin_src matlab :results replace value
  abs(dcgain(G(1,1))./dcgain(G(2,1)))
#+end_src

#+RESULTS:
: 5.0749

This is actually correct and approximately corresponds to the ratio of the piezo height and length:
#+begin_src matlab :results replace value
  75/15
#+end_src

#+RESULTS:
: 5

*** Stroke

Estimation of the actuator stroke:
\[ \Delta H = A n \Delta L \]
with:
- $\Delta H$ Axial Stroke of the APA
- $A$ Amplification factor (5 for the APA300ML)
- $n$ Number of stack used
- $\Delta L$ Stroke of the stack (0.1% of its length)

#+begin_src matlab :results replace value
  1e6 * 5 * 3 * 20e-3 * 0.1e-2
#+end_src

#+RESULTS:
: 300

This is exactly the specified stroke in the data-sheet.

*** TODO Stroke BIS
- [ ] Identified the stroke form the transfer function from V to z

#+begin_src matlab :exports none
  %% Name of the Simulink File
  mdl = 'APA300ML';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/V'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/d'], 1, 'openoutput'); io_i = io_i + 1;

  G = linearize(mdl, io);

  1e6*170*abs(dcgain(G))
#+end_src

** Identification of the Dynamics from actuator to replace displacement
We first set the mass to be approximately zero.
#+begin_src matlab :exports none
  m = 0.01;
#+end_src

The dynamics is identified from the applied force to the measured relative displacement.
#+begin_src matlab :exports none
  %% Name of the Simulink File
  mdl = 'APA300ML';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/F'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/z'], 1, 'openoutput'); io_i = io_i + 1;

  Gh = -linearize(mdl, io);
#+end_src

The same dynamics is identified for a payload mass of 10Kg.
#+begin_src matlab
  m = 10;
#+end_src

#+begin_src matlab :exports none
  %% Name of the Simulink File
  mdl = 'APA300ML';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/F'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/z'], 1, 'openoutput'); io_i = io_i + 1;

  Ghm = -linearize(mdl, io);
#+end_src

#+begin_src matlab :exports none
  freqs = logspace(0, 4, 5000);

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

  ax1 = nexttile([2,1]);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gh, freqs, 'Hz'))), '-');
  plot(freqs, abs(squeeze(freqresp(Ghm, freqs, 'Hz'))), '-');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
  hold off;

  ax2 = nexttile;
  hold on;
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gh, freqs, 'Hz')))), '-', ...
       'DisplayName', '$m = 0kg$');
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Ghm, freqs, 'Hz')))), '-', ...
       'DisplayName', '$m = 10kg$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  yticks(-360:90:360);
  ylim([-360 0]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
  legend('location', 'southwest');
#+end_src

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

#+name: fig:apa300ml_plant_dynamics
#+caption: Transfer function from forces applied by the stack to the axial displacement of the APA
#+RESULTS:
[[file:figs/apa300ml_plant_dynamics.png]]

The root locus corresponding to Direct Velocity Feedback with a mass of 10kg is shown in Figure [[fig:apa300ml_dvf_root_locus]].
#+begin_src matlab :exports none
  figure;

  gains = logspace(0, 5, 500);

  hold on;
  plot(real(pole(Ghm)),  imag(pole(G)), 'kx');
  plot(real(tzero(Ghm)),  imag(tzero(G)), 'ko');
  for k = 1:length(gains)
      cl_poles = pole(feedback(Ghm, gains(k)*s));
      plot(real(cl_poles), imag(cl_poles), 'k.');
  end
  hold off;
  axis square;
  xlim([-500, 10]); ylim([0, 510]);

  xlabel('Real Part'); ylabel('Imaginary Part');
#+end_src

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

#+name: fig:apa300ml_dvf_root_locus
#+caption: Root Locus for Direct Velocity Feedback
#+RESULTS:
[[file:figs/apa300ml_dvf_root_locus.png]]

** Identification of the Dynamics from actuator to force sensor
Let's use 2 stacks as a force sensor and 1 stack as force actuator.

The transfer function from actuator voltage to sensor voltage is identified and shown in Figure [[fig:apa300ml_iff_plant]].
#+begin_src matlab :exports none
  m = 10;
#+end_src

#+begin_src matlab :exports none
  %% Name of the Simulink File
  mdl = 'APA300ML';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Va'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1;

  Giff = -linearize(mdl, io);
#+end_src

#+begin_src matlab :exports none
  freqs = logspace(0, 4, 5000);

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

  ax1 = nexttile([2,1]);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Giff, freqs, 'Hz'))), '-');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
  hold off;

  ax2 = nexttile;
  hold on;
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Giff, freqs, 'Hz')))), '-');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  yticks(-360:90:360);
  ylim([-180 180]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
#+end_src

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

#+name: fig:apa300ml_iff_plant
#+caption: Transfer function from actuator to force sensor
#+RESULTS:
[[file:figs/apa300ml_iff_plant.png]]

For root locus corresponding to IFF is shown in Figure [[fig:apa300ml_iff_root_locus]].
#+begin_src matlab :exports none
  figure;

  gains = logspace(0, 5, 500);

  hold on;
  plot(real(pole(Giff)),  imag(pole(Giff)), 'kx');
  plot(real(tzero(Giff)),  imag(tzero(Giff)), 'ko');
  for k = 1:length(gains)
      cl_poles = pole(feedback(Giff, gains(k)/s));
      plot(real(cl_poles), imag(cl_poles), 'k.');
  end
  hold off;
  axis square;
  xlim([-500, 10]); ylim([0, 510]);

  xlabel('Real Part'); ylabel('Imaginary Part');
#+end_src

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

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

** Identification for a simpler model
The goal in this section is to identify the parameters of a simple APA model from the FEM.
This can be useful is a lower order model is to be used for simulations.

The presented model is based on cite:souleille18_concep_activ_mount_space_applic.

The model represents the Amplified Piezo Actuator (APA) from Cedrat-Technologies (Figure [[fig:souleille18_model_piezo]]).
The parameters are shown in the table below.

#+name: fig:souleille18_model_piezo
#+caption: Picture of an APA100M from Cedrat Technologies. Simplified model of a one DoF payload mounted on such isolator
[[file:./figs/souleille18_model_piezo.png]]

#+caption:Parameters used for the model of the APA 100M
|       | Meaning                                                        |
|-------+----------------------------------------------------------------|
| $k_e$ | Stiffness used to adjust the pole of the isolator              |
| $k_1$ | Stiffness of the metallic suspension when the stack is removed |
| $k_a$ | Stiffness of the actuator                                      |
| $c_1$ | Added viscous damping                                          |

The goal is to determine $k_e$, $k_a$ and $k_1$ so that the simplified model fits the FEM model.

\[ \alpha = \frac{x_1}{f}(\omega=0) = \frac{\frac{k_e}{k_e + k_a}}{k_1 + \frac{k_e k_a}{k_e + k_a}} \]
\[ \beta = \frac{x_1}{F}(\omega=0) = \frac{1}{k_1 + \frac{k_e k_a}{k_e + k_a}} \]

If we can fix $k_a$, we can determine $k_e$ and $k_1$ with:
\[ k_e = \frac{k_a}{\frac{\beta}{\alpha} - 1} \]
\[ k_1 = \frac{1}{\beta} - \frac{k_e k_a}{k_e + k_a} \]

#+begin_src matlab :exports none
  m = 10;
#+end_src

#+begin_src matlab :exports none
  %% Name of the Simulink File
  mdl = 'APA300ML';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Fd'], 1, 'openinput');  io_i = io_i + 1; % External Vertical Force [N]
  io(io_i) = linio([mdl, '/w'],  1, 'openinput');  io_i = io_i + 1; % Base Motion [m]
  io(io_i) = linio([mdl, '/Fa'], 1, 'openinput');  io_i = io_i + 1; % Actuator Force [N]
  io(io_i) = linio([mdl, '/z'],  1, 'openoutput'); io_i = io_i + 1; % Vertical Displacement [m]
  io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensor [V]
  io(io_i) = linio([mdl, '/d'],  1, 'openoutput'); io_i = io_i + 1; % Stack Displacement [m]

  G = linearize(mdl, io);

  G.InputName = {'Fd', 'w', 'Fa'};
  G.OutputName = {'y', 'Fs', 'd'};
#+end_src

From the identified dynamics, compute $\alpha$ and $\beta$
#+begin_src matlab
  alpha = abs(dcgain(G('y', 'Fa')));
  beta  = abs(dcgain(G('y', 'Fd')));
#+end_src

$k_a$ is estimated using the following formula:
#+begin_src matlab
  ka = 0.8/abs(dcgain(G('y', 'Fa')));
#+end_src
The factor can be adjusted to better match the curves.

Then $k_e$ and $k_1$ are computed.
#+begin_src matlab
  ke = ka/(beta/alpha - 1);
  k1 = 1/beta - ke*ka/(ke + ka);
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
  data2orgtable(1e-6*[ka; ke; k1], {'ka', 'ke', 'k1'}, {'Value [N/um]'}, ' %.1f ');
#+end_src

#+RESULTS:
|    | Value [N/um] |
|----+--------------|
| ka |         40.5 |
| ke |          1.5 |
| k1 |          0.4 |

The damping in the system is adjusted to match the FEM model if necessary.
#+begin_src matlab
  c1 = 1e2;
#+end_src

The analytical model of the simpler system is defined below:
#+begin_src matlab
  Ga = 1/(m*s^2 + k1 + c1*s + ke*ka/(ke + ka)) * ...
       [ 1 ,              k1 + c1*s + ke*ka/(ke + ka)  , ke/(ke + ka) ;
        -ke*ka/(ke + ka), ke*ka/(ke + ka)*m*s^2 ,       -ke/(ke + ka)*(m*s^2 + c1*s + k1)];

  Ga.InputName = {'Fd', 'w', 'Fa'};
  Ga.OutputName = {'y', 'Fs'};
#+end_src

And the DC gain is adjusted for the force sensor:
#+begin_src matlab
  F_gain = dcgain(G('Fs', 'Fd'))/dcgain(Ga('Fs', 'Fd'));
#+end_src

The dynamics of the FEM model and the simpler model are compared in Figure [[fig:apa300ml_comp_simpler_model]].

#+begin_src matlab :exports none
  freqs = logspace(0, 5, 1000);

  figure;
  tiledlayout(2, 3, 'TileSpacing', 'None', 'Padding', 'None');

  ax1 = nexttile;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G( 'y', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Ga('y', 'w'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('$x_1/w$ [m/m]');
  ylim([1e-6, 1e2]);

  ax2 = nexttile;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G( 'y', 'Fa'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Ga('y', 'Fa'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('$x_1/f$ [m/N]');
  ylim([1e-14, 1e-6]);

  ax3 = nexttile;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G( 'y', 'Fd'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Ga('y', 'Fd'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('$x_1/F$ [m/N]');
  ylim([1e-14, 1e-4]);

  ax4 = nexttile;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G( 'Fs', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(F_gain*Ga('Fs', 'w'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]');
  ylabel('$F_s/w$ [m/m]');
  ylim([1e2, 1e8]);

  ax5 = nexttile;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G( 'Fs', 'Fa'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(F_gain*Ga('Fs', 'Fa'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]');
  ylabel('$F_s/f$ [m/N]');
  ylim([1e-4, 1e1]);

  ax6 = nexttile;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G( 'Fs', 'Fd'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(F_gain*Ga('Fs', 'Fd'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]');
  ylabel('$F_s/F$ [m/N]');
  ylim([1e-7, 1e2]);

  linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
#+end_src

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

#+name: fig:apa300ml_comp_simpler_model
#+caption: Comparison of the Dynamics between the FEM model and the simplified one
#+RESULTS:
[[file:figs/apa300ml_comp_simpler_model.png]]

The simplified model has also been implemented in Simscape.

The dynamics of the Simscape simplified model is identified and compared with the FEM one in Figure [[fig:apa300ml_comp_simpler_simscape]].
#+begin_src matlab :exports none
  %% Name of the Simulink File
  mdl = 'APA300ML_simplified';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Fd'], 1, 'openinput');  io_i = io_i + 1; % External Vertical Force [N]
  io(io_i) = linio([mdl, '/w'],  1, 'openinput');  io_i = io_i + 1; % Base Motion [m]
  io(io_i) = linio([mdl, '/Fa'], 1, 'openinput');  io_i = io_i + 1; % Actuator Force [N]
  io(io_i) = linio([mdl, '/y'],  1, 'openoutput'); io_i = io_i + 1; % Vertical Displacement [m]
  io(io_i) = linio([mdl, '/Fs'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensor [V]

  Gs = linearize(mdl, io);

  Gs.InputName = {'Fd', 'w', 'Fa'};
  Gs.OutputName = {'y', 'Fs'};
#+end_src

#+begin_src matlab :exports none
  freqs = logspace(0, 5, 1000);

  figure;
  tiledlayout(2, 3, 'TileSpacing', 'None', 'Padding', 'None');

  ax1 = nexttile;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G( 'y', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gs('y', 'w'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('$x_1/w$ [m/m]');
  ylim([1e-6, 1e2]);

  ax2 = nexttile;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G( 'y', 'Fa'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gs('y', 'Fa'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('$x_1/f$ [m/N]');
  ylim([1e-14, 1e-6]);

  ax3 = nexttile;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G( 'y', 'Fd'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gs('y', 'Fd'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('$x_1/F$ [m/N]');
  ylim([1e-14, 1e-4]);

  ax4 = nexttile;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G( 'Fs', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(F_gain*Gs('Fs', 'w'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]');
  ylabel('$F_s/w$ [m/m]');
  ylim([1e2, 1e8]);

  ax5 = nexttile;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G( 'Fs', 'Fa'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(F_gain*Gs('Fs', 'Fa'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]');
  ylabel('$F_s/f$ [m/N]');
  ylim([1e-4, 1e1]);

  ax6 = nexttile;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G( 'Fs', 'Fd'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(F_gain*Gs('Fs', 'Fd'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]');
  ylabel('$F_s/F$ [m/N]');
  ylim([1e-7, 1e2]);

  linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
#+end_src

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

#+name: fig:apa300ml_comp_simpler_simscape
#+caption: Comparison of the Dynamics between the FEM model and the simplified simscape model
#+RESULTS:
[[file:figs/apa300ml_comp_simpler_simscape.png]]

** Integral Force Feedback
In this section, Integral Force Feedback control architecture is applied on the APA300ML.

First, the plant (dynamics from voltage actuator to voltage sensor is identified).
#+begin_src matlab :exports none
  Kiff = tf(0);
#+end_src

The payload mass is set to 10kg.
#+begin_src matlab
  m = 10;
#+end_src

#+begin_src matlab :exports none
  %% Name of the Simulink File
  mdl = 'APA300ML_IFF';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/w'],         1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/F'],         1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Fd'],        1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/z'],         1, 'openoutput'); io_i = io_i + 1;
  io(io_i) = linio([mdl, '/APA300ML'],  1, 'openoutput'); io_i = io_i + 1;

  G_ol = linearize(mdl, io);
  G_ol.InputName  = {'w', 'f', 'F'};
  G_ol.OutputName  = {'x1', 'Fs'};

  G = G_ol({'Fs'}, {'f'});
#+end_src

The obtained dynamics is shown in Figure [[fig:piezo_amplified_iff_plant]].

#+begin_src matlab :exports none
  freqs = logspace(1, 5, 1000);

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

  ax1 = nexttile([2,1]);
  hold on;
  plot(freqs, abs(squeeze(freqresp(G, freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
  hold off;

  ax2 = nexttile;
  hold on;
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G, freqs, 'Hz')))));
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  yticks(-360:90:360);
  ylim([-390 30]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
#+end_src

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

#+name: fig:piezo_amplified_iff_plant
#+caption: IFF Plant
#+RESULTS:
[[file:figs/piezo_amplified_iff_plant.png]]

The controller is defined below and the loop gain is shown in Figure [[fig:piezo_amplified_iff_loop_gain]].
#+begin_src matlab
  Kiff = -1e3/s;
#+end_src

#+begin_src matlab :exports none
  freqs = logspace(1, 5, 1000);

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

  ax1 = nexttile([2,1]);
  hold on;
  plot(freqs, abs(squeeze(freqresp(G*Kiff, freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
  hold off;

  ax2 = nexttile;
  hold on;
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G*Kiff, freqs, 'Hz')))));
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  yticks(-360:90:360);
  ylim([-180 180]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
#+end_src

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

#+name: fig:piezo_amplified_iff_loop_gain
#+caption: IFF Loop Gain
#+RESULTS:
[[file:figs/piezo_amplified_iff_loop_gain.png]]

Now the closed-loop system is identified again and compare with the open loop system in Figure [[fig:piezo_amplified_iff_comp]].

It is the expected behavior as shown in the Figure [[fig:souleille18_results]] (from cite:souleille18_concep_activ_mount_space_applic).

#+begin_src matlab :exports none
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/w'],         1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/F'],         1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Fd'],        1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/z'],         1, 'openoutput'); io_i = io_i + 1;
  io(io_i) = linio([mdl, '/APA300ML'],  1, 'output');     io_i = io_i + 1;

  Giff = linearize(mdl, io);
  Giff.InputName  = {'w', 'f', 'F'};
  Giff.OutputName  = {'x1', 'Fs'};
#+end_src

#+begin_src matlab :exports none
  freqs = logspace(0, 3, 1000);

  figure;
  tiledlayout(2, 3, 'TileSpacing', 'None', 'Padding', 'None');

  ax1 = nexttile;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G_ol('x1', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Giff('x1', 'w'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]); ylabel('$x_1/w$ [m/m]')

  ax2 = nexttile;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G_ol('x1', 'f'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Giff('x1', 'f'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]); ylabel('$x_1/f$ [m/N]');

  ax3 = nexttile;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G_ol('x1', 'F'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Giff('x1', 'F'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]); ylabel('$x_1/F$ [m/N]');

  ax4 = nexttile;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G_ol('Fs', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Giff('Fs', 'w'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('$F_s/w$ [N/m]');

  ax5 = nexttile;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G_ol('Fs', 'f'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Giff('Fs', 'f'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('$F_s/f$ [N/N]');

  ax6 = nexttile;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G_ol('Fs', 'F'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Giff('Fs', 'F'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('$F_s/F$ [N/N]');

  linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
#+end_src

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

#+name: fig:piezo_amplified_iff_comp
#+caption: OL and CL transfer functions
#+RESULTS:
[[file:figs/piezo_amplified_iff_comp.png]]

#+name: fig:souleille18_results
#+caption: Results obtained in cite:souleille18_concep_activ_mount_space_applic
[[file:figs/souleille18_results.png]]

* Function
<<sec:functions>>
** =initializeBotFlexibleJoint= - Initialize Flexible Joint
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeBotFlexibleJoint.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:initializeBotFlexibleJoint>>

*** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
function [flex_bot] = initializeBotFlexibleJoint(args)
% initializeBotFlexibleJoint -
%
% Syntax: [flex_bot] = initializeBotFlexibleJoint(args)
%
% Inputs:
%    - args -
%
% Outputs:
%    - flex_bot -
#+end_src

*** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
arguments
    args.type      char   {mustBeMember(args.type,{'2dof', '3dof', '4dof'})} = '2dof'

    args.kRx (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*5
    args.kRy (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*5
    args.kRz (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*260
    args.kz  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*7e7

    args.cRx (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*0.001
    args.cRy (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*0.001
    args.cRz (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*0.001
    args.cz  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*0.001
end
#+end_src

*** Initialize the structure
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
flex_bot = struct();
#+end_src

*** Set the Joint's type
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
switch args.type
  case '2dof'
    flex_bot.type = 1;
  case '3dof'
    flex_bot.type = 2;
  case '4dof'
    flex_bot.type = 3;
end
#+end_src

*** Set parameters
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
flex_bot.kRx = args.kRx;
flex_bot.kRy = args.kRy;
flex_bot.kRz = args.kRz;
flex_bot.kz  = args.kz;
#+end_src

#+begin_src matlab
flex_bot.cRx = args.cRx;
flex_bot.cRy = args.cRy;
flex_bot.cRz = args.cRz;
flex_bot.cz  = args.cz;
#+end_src

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

*** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
function [flex_top] = initializeTopFlexibleJoint(args)
% initializeTopFlexibleJoint -
%
% Syntax: [flex_top] = initializeTopFlexibleJoint(args)
%
% Inputs:
%    - args -
%
% Outputs:
%    - flex_top -
#+end_src

*** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
arguments
    args.type      char   {mustBeMember(args.type,{'2dof', '3dof', '4dof'})} = '2dof'

    args.kRx (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*5
    args.kRy (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*5
    args.kRz (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*260
    args.kz  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*7e7

    args.cRx (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*0.001
    args.cRy (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*0.001
    args.cRz (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*0.001
    args.cz  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*0.001
end
#+end_src

*** Initialize the structure
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
flex_top = struct();
#+end_src

*** Set the Joint's type
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
switch args.type
  case '2dof'
    flex_top.type = 1;
  case '3dof'
    flex_top.type = 2;
  case '4dof'
    flex_top.type = 3;
end
#+end_src

*** Set parameters
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
flex_top.kRx = args.kRx;
flex_top.kRy = args.kRy;
flex_top.kRz = args.kRz;
flex_top.kz  = args.kz;
#+end_src

#+begin_src matlab
flex_top.cRx = args.cRx;
flex_top.cRy = args.cRy;
flex_top.cRz = args.cRz;
flex_top.cz  = args.cz;
#+end_src

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

*** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
function [actuator] = initializeAPA(args)
% initializeAPA -
%
% Syntax: [actuator] = initializeAPA(args)
%
% Inputs:
%    - args -
%
% Outputs:
%    - actuator -
#+end_src

*** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
arguments
    args.type      char   {mustBeMember(args.type,{'2dof', 'flexible frame', 'flexible'})} = '2dof'

    % Actuator and Sensor constants
    args.Ga  (1,1) double {mustBeNumeric} = 0
    args.Gs  (1,1) double {mustBeNumeric} = 0

    % For 2DoF
    args.k   (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*0.38e6
    args.ke  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*1.75e6
    args.ka  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*3e7

    args.c   (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*3e1
    args.ce  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*2e1
    args.ca  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*2e1

    args.Leq (6,1) double {mustBeNumeric} = ones(6,1)*0.056

    % Force Flexible APA
    args.xi (1,1) double {mustBeNumeric, mustBePositive} = 0.01
    args.d_align (3,1) double {mustBeNumeric} = zeros(3,1) % [m]

    % For Flexible Frame
    args.ks (1,1) double {mustBeNumeric, mustBePositive} = 235e6
    args.cs (1,1) double {mustBeNumeric, mustBePositive} = 1e1
end
#+end_src

*** Initialize Structure
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
actuator = struct();
#+end_src

*** Type
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
switch args.type
  case '2dof'
    actuator.type = 1;
  case 'flexible frame'
    actuator.type = 2;
  case 'flexible'
    actuator.type = 3;
end
#+end_src

*** Actuator/Sensor Constants
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
if args.Ga == 0
    switch args.type
      case '2dof'
        actuator.Ga = -30.0;
      case 'flexible frame'
        actuator.Ga = 1; % TODO
      case 'flexible'
        actuator.Ga = 23.4;
    end
else
    actuator.Ga = args.Ga; % Actuator gain [N/V]
end
#+end_src

#+begin_src matlab
if args.Gs == 0
    switch args.type
      case '2dof'
        actuator.Gs = 0.098;
      case 'flexible frame'
        actuator.Gs = 1; % TODO
      case 'flexible'
        actuator.Gs = -4674824;
    end
else
    actuator.Gs = args.Gs; % Sensor gain [V/m]
end
#+end_src

*** 2DoF parameters
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
actuator.k  = args.k;  % [N/m]
actuator.ke = args.ke; % [N/m]
actuator.ka = args.ka; % [N/m]

actuator.c  = args.c;  % [N/(m/s)]
actuator.ce = args.ce; % [N/(m/s)]
actuator.ca = args.ca; % [N/(m/s)]

actuator.Leq = args.Leq; % [m]
#+end_src

*** Flexible frame and fully flexible
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
switch args.type
  case 'flexible frame'
    actuator.K = readmatrix('APA300ML_b_mat_K.CSV'); % Stiffness Matrix
    actuator.M = readmatrix('APA300ML_b_mat_M.CSV'); % Mass Matrix
    actuator.P = extractNodes('APA300ML_b_out_nodes_3D.txt'); % Node coordinates [m]
  case 'flexible'
    actuator.K = readmatrix('full_APA300ML_K.CSV'); % Stiffness Matrix
    actuator.M = readmatrix('full_APA300ML_M.CSV'); % Mass Matrix
    actuator.P = extractNodes('full_APA300ML_out_nodes_3D.txt'); % Node coordiantes [m]
    actuator.d_align = args.d_align;
end

actuator.xi = args.xi; % Damping ratio

actuator.ks = args.ks; % Stiffness of one stack [N/m]
actuator.cs = args.cs; % Damping of one stack [N/m]
#+end_src

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

*** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
function [U_exc] = generateSweepExc(args)
% generateSweepExc - Generate a Sweep Sine excitation signal
%
% Syntax: [U_exc] = generateSweepExc(args)
%
% Inputs:
%    - args - Optinal arguments:
%        - Ts              - Sampling Time                                              - [s]
%        - f_start         - Start frequency of the sweep                               - [Hz]
%        - f_end           - End frequency of the sweep                                 - [Hz]
%        - V_mean          - Mean value of the excitation voltage                       - [V]
%        - V_exc           - Excitation Amplitude for the Sweep, could be numeric or TF - [V]
%        - t_start         - Time at which the sweep begins                             - [s]
%        - exc_duration    - Duration of the sweep                                      - [s]
%        - sweep_type      - 'logarithmic' or 'linear'                                  - [-]
%        - smooth_ends     - 'true' or 'false': smooth transition between 0 and V_mean  - [-]
#+end_src

*** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
arguments
    args.Ts              (1,1) double  {mustBeNumeric, mustBePositive} = 1e-4
    args.f_start         (1,1) double  {mustBeNumeric, mustBePositive} = 1
    args.f_end           (1,1) double  {mustBeNumeric, mustBePositive} = 1e3
    args.V_mean          (1,1) double  {mustBeNumeric} = 0
    args.V_exc                                         = 1
    args.t_start         (1,1) double  {mustBeNumeric, mustBeNonnegative} = 5
    args.exc_duration    (1,1) double  {mustBeNumeric, mustBePositive} = 10
    args.sweep_type            char    {mustBeMember(args.sweep_type,{'log', 'lin'})} = 'lin'
    args.smooth_ends           logical {mustBeNumericOrLogical} = true
end
#+end_src

*** Sweep Sine part
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
t_sweep = 0:args.Ts:args.exc_duration;

if strcmp(args.sweep_type, 'log')
    V_exc = sin(2*pi*args.f_start * args.exc_duration/log(args.f_end/args.f_start) * (exp(log(args.f_end/args.f_start)*t_sweep/args.exc_duration) - 1));
elseif strcmp(args.sweep_type, 'lin')
    V_exc = sin(2*pi*(args.f_start + (args.f_end - args.f_start)/2/args.exc_duration*t_sweep).*t_sweep);
else
    error('sweep_type should either be equal to "log" or to "lin"');
end
#+end_src

#+begin_src matlab
if isnumeric(args.V_exc)
    V_sweep = args.V_mean + args.V_exc*V_exc;
elseif isct(args.V_exc)
    if strcmp(args.sweep_type, 'log')
        V_sweep = args.V_mean + abs(squeeze(freqresp(args.V_exc, args.f_start*(args.f_end/args.f_start).^(t_sweep/args.exc_duration), 'Hz')))'.*V_exc;
    elseif strcmp(args.sweep_type, 'lin')
        V_sweep = args.V_mean + abs(squeeze(freqresp(args.V_exc, args.f_start+(args.f_end-args.f_start)/args.exc_duration*t_sweep, 'Hz')))'.*V_exc;
    end
end
#+end_src

*** Smooth Ends
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
if args.t_start > 0
    t_smooth_start = args.Ts:args.Ts:args.t_start;

    V_smooth_start = zeros(size(t_smooth_start));
    V_smooth_end   = zeros(size(t_smooth_start));

    if args.smooth_ends
        Vd_max = args.V_mean/(0.7*args.t_start);

        V_d = zeros(size(t_smooth_start));
        V_d(t_smooth_start < 0.2*args.t_start) = t_smooth_start(t_smooth_start < 0.2*args.t_start)*Vd_max/(0.2*args.t_start);
        V_d(t_smooth_start > 0.2*args.t_start & t_smooth_start < 0.7*args.t_start) = Vd_max;
        V_d(t_smooth_start > 0.7*args.t_start & t_smooth_start < 0.9*args.t_start) = Vd_max - (t_smooth_start(t_smooth_start > 0.7*args.t_start & t_smooth_start < 0.9*args.t_start) - 0.7*args.t_start)*Vd_max/(0.2*args.t_start);

        V_smooth_start = cumtrapz(V_d)*args.Ts;

        V_smooth_end = args.V_mean - V_smooth_start;
    end
else
    V_smooth_start = [];
    V_smooth_end = [];
end
#+end_src

*** Combine Excitation signals
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
V_exc = [V_smooth_start, V_sweep, V_smooth_end];
t_exc = args.Ts*[0:1:length(V_exc)-1];
#+end_src

#+begin_src matlab
U_exc = [t_exc; V_exc];
#+end_src

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

*** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
function [U_exc] = generateShapedNoise(args)
% generateShapedNoise - Generate a Shaped Noise excitation signal
%
% Syntax: [U_exc] = generateShapedNoise(args)
%
% Inputs:
%    - args - Optinal arguments:
%        - Ts              - Sampling Time                                              - [s]
%        - V_mean          - Mean value of the excitation voltage                       - [V]
%        - V_exc           - Excitation Amplitude, could be numeric or TF               - [V rms]
%        - t_start         - Time at which the noise begins                             - [s]
%        - exc_duration    - Duration of the noise                                      - [s]
%        - smooth_ends     - 'true' or 'false': smooth transition between 0 and V_mean  - [-]
#+end_src

*** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
arguments
    args.Ts              (1,1) double  {mustBeNumeric, mustBePositive} = 1e-4
    args.V_mean          (1,1) double  {mustBeNumeric} = 0
    args.V_exc                                         = 1
    args.t_start         (1,1) double  {mustBeNumeric, mustBePositive} = 5
    args.exc_duration    (1,1) double  {mustBeNumeric, mustBePositive} = 10
    args.smooth_ends           logical {mustBeNumericOrLogical} = true
end
#+end_src

*** Shaped Noise
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
t_noise = 0:args.Ts:args.exc_duration;

#+end_src

#+begin_src matlab
if isnumeric(args.V_exc)
    V_noise = args.V_mean + args.V_exc*sqrt(1/args.Ts/2)*randn(length(t_noise), 1)';
elseif isct(args.V_exc)
    V_noise = args.V_mean + lsim(args.V_exc, sqrt(1/args.Ts/2)*randn(length(t_noise), 1), t_noise)';
end
#+end_src

*** Smooth Ends
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
t_smooth_start = args.Ts:args.Ts:args.t_start;

V_smooth_start = zeros(size(t_smooth_start));
V_smooth_end   = zeros(size(t_smooth_start));

if args.smooth_ends
    Vd_max = args.V_mean/(0.7*args.t_start);

    V_d = zeros(size(t_smooth_start));
    V_d(t_smooth_start < 0.2*args.t_start) = t_smooth_start(t_smooth_start < 0.2*args.t_start)*Vd_max/(0.2*args.t_start);
    V_d(t_smooth_start > 0.2*args.t_start & t_smooth_start < 0.7*args.t_start) = Vd_max;
    V_d(t_smooth_start > 0.7*args.t_start & t_smooth_start < 0.9*args.t_start) = Vd_max - (t_smooth_start(t_smooth_start > 0.7*args.t_start & t_smooth_start < 0.9*args.t_start) - 0.7*args.t_start)*Vd_max/(0.2*args.t_start);

    V_smooth_start = cumtrapz(V_d)*args.Ts;

    V_smooth_end = args.V_mean - V_smooth_start;
end
#+end_src

*** Combine Excitation signals
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
V_exc = [V_smooth_start, V_noise, V_smooth_end];
t_exc = args.Ts*[0:1:length(V_exc)-1];
#+end_src

#+begin_src matlab
U_exc = [t_exc; V_exc];
#+end_src

** =generateSinIncreasingAmpl=: Generate Sinus with increasing amplitude
:PROPERTIES:
:header-args:matlab+: :tangle ./matlab/src/generateSinIncreasingAmpl.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:generateSinIncreasingAmpl>>

*** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
function [U_exc] = generateSinIncreasingAmpl(args)
% generateSinIncreasingAmpl - Generate Sinus with increasing amplitude
%
% Syntax: [U_exc] = generateSinIncreasingAmpl(args)
%
% Inputs:
%    - args - Optinal arguments:
%        - Ts              - Sampling Time                                              - [s]
%        - V_mean          - Mean value of the excitation voltage                       - [V]
%        - sin_ampls       - Excitation Amplitudes                                      - [V]
%        - sin_freq        - Excitation Frequency                                       - [Hz]
%        - sin_num         - Number of period for each amplitude                        - [-]
%        - t_start         - Time at which the excitation begins                        - [s]
%        - smooth_ends     - 'true' or 'false': smooth transition between 0 and V_mean  - [-]
#+end_src

*** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
arguments
    args.Ts              (1,1) double  {mustBeNumeric, mustBePositive} = 1e-4
    args.V_mean          (1,1) double  {mustBeNumeric} = 0
    args.sin_ampls             double  {mustBeNumeric, mustBePositive} = [0.1, 0.2, 0.3]
    args.sin_period      (1,1) double  {mustBeNumeric, mustBePositive} = 1
    args.sin_num         (1,1) double  {mustBeNumeric, mustBePositive, mustBeInteger} = 3
    args.t_start         (1,1) double  {mustBeNumeric, mustBePositive} = 5
    args.smooth_ends           logical {mustBeNumericOrLogical} = true
end
#+end_src

*** Sinus excitation
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
t_noise = 0:args.Ts:args.sin_period*args.sin_num;
sin_exc = [];
#+end_src

#+begin_src matlab
for sin_ampl = args.sin_ampls
    sin_exc = [sin_exc, args.V_mean + sin_ampl*sin(2*pi/args.sin_period*t_noise)];
end
#+end_src

*** Smooth Ends
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
t_smooth_start = args.Ts:args.Ts:args.t_start;

V_smooth_start = zeros(size(t_smooth_start));
V_smooth_end   = zeros(size(t_smooth_start));

if args.smooth_ends
    Vd_max = args.V_mean/(0.7*args.t_start);

    V_d = zeros(size(t_smooth_start));
    V_d(t_smooth_start < 0.2*args.t_start) = t_smooth_start(t_smooth_start < 0.2*args.t_start)*Vd_max/(0.2*args.t_start);
    V_d(t_smooth_start > 0.2*args.t_start & t_smooth_start < 0.7*args.t_start) = Vd_max;
    V_d(t_smooth_start > 0.7*args.t_start & t_smooth_start < 0.9*args.t_start) = Vd_max - (t_smooth_start(t_smooth_start > 0.7*args.t_start & t_smooth_start < 0.9*args.t_start) - 0.7*args.t_start)*Vd_max/(0.2*args.t_start);

    V_smooth_start = cumtrapz(V_d)*args.Ts;

    V_smooth_end = args.V_mean - V_smooth_start;
end
#+end_src

*** Combine Excitation signals
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
V_exc = [V_smooth_start, sin_exc, V_smooth_end];
t_exc = args.Ts*[0:1:length(V_exc)-1];
#+end_src

#+begin_src matlab
U_exc = [t_exc; V_exc];
#+end_src

* Bibliography                                                        :ignore:
#+latex: \printbibliography