#+TITLE: Test Bench - Amplified Piezoelectric Actuator
:DRAWER:
#+STARTUP: overview

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

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

#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="https://research.tdehaeze.xyz/css/style.css"/>
#+HTML_HEAD: <script type="text/javascript" src="https://research.tdehaeze.xyz/js/script.js"></script>

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

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

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

* Introduction                                                        :ignore:

- Section [[sec:experimental_setup]]:
- Section [[sec:simscape_model]]:
- Section [[]]:
- Section [[]]:
- Section [[]]:

* Experimental Setup
<<sec:experimental_setup>>

A schematic of the test-bench is shown in Figure [[fig:test_bench_apa_schematic]].

A mass can be vertically moved using the amplified piezoelectric actuator (APA95ML).
The displacement of the mass (relative to the mechanical frame) is measured by the interferometer.

The APA95ML has three stacks that can be used as actuator or as sensors.

Pictures of the test bench are shown in Figure [[fig:setup_picture]] and [[fig:setup_zoom]].

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

#+name: fig:setup_picture
#+caption: Picture of the Setup
[[file:figs/setup_picture.png]]

#+name: fig:setup_zoom
#+caption: Zoom on the APA
[[file:figs/setup_zoom.png]]

#+begin_note
Here are the equipment used in the test bench:
- Attocube interferometer ([[file:doc/IDS3010.pdf][doc]])
- Cedrat Amplified Piezoelectric Actuator APA95ML ([[file:doc/APA95ML.pdf][doc]])
- Voltage Amplifier LA75B ([[file:doc/LA75B.pdf][doc]])
- Speedgoat IO131 with 16bits ADC and DAC ([[file:doc/IO130 IO131 OEM Datasheet.pdf][doc]])
- Low Noise Voltage Preamplifier from Ametek ([[file:doc/model_5113.pdf][doc]])
#+end_note

* Estimation of electrical/mechanical relationships
** Introduction                                                      :ignore:
In order to correctly model the piezoelectric actuator with Simscape, we need to determine:
1. $g_a$: the ratio of the generated force $F_a$ to the supply voltage $V_a$ across the piezoelectric stack
2. $g_s$: the ratio of the generated voltage $V_s$ across the piezoelectric stack when subject to a strain $\Delta h$

We estimate $g_a$ and $g_s$ using different approaches:
1. Section [[sec:estimation_datasheet]]: $g_a$ is estimated from the datasheet of the piezoelectric stack
2. Section [[sec:estimation_piezo_params]]: $g_a$ and $g_s$ are estimated using the piezoelectric constants
3. Section [[sec:estimation_identification]]: $g_a$ and $g_s$ are estimated experimentally

** 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/mat/');
  addpath('./matlab/');
#+end_src

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

** Estimation from Data-sheet
<<sec:estimation_datasheet>>

The stack parameters taken from the data-sheet are shown in Table [[tab:stack_parameters]].

#+name: tab:stack_parameters
#+caption: Stack Parameters
| Parameter      | Unit      |    Value |
|----------------+-----------+----------|
| Nominal Stroke | $\mu m$   |       20 |
| Blocked force  | $N$       |     4700 |
| Stiffness      | $N/\mu m$ |      235 |
| Voltage Range  | $V$       | -20..150 |
| Capacitance    | $\mu F$   |      4.4 |
| Length         | $mm$      |       20 |
| Stack Area     | $mm^2$    |    10x10 |

Let's compute the generated force

The stroke is $L_{\max} = 20\mu m$ for a voltage range of $V_{\max} = 170 V$.
Furthermore, the stiffness is $k_a = 235 \cdot 10^6 N/m$.

The relation between the applied voltage and the generated force can be estimated as follows:
\begin{equation}
  g_a = k_a \frac{L_{\max}}{V_{\max}}
\end{equation}

#+begin_src matlab
  ka = 235e6; % [N/m]
  Lmax = 20e-6; % [m]
  Vmax = 170; % [V]
#+end_src

#+begin_src matlab :results value replace
  ka*Lmax/Vmax % [N/V]
#+end_src

#+RESULTS:
: 27.647

From the parameters of the stack, it seems not possible to estimate the relation between the strain and the generated voltage.

** Estimation from Piezoelectric parameters
<<sec:estimation_piezo_params>>

In order to make the conversion from applied voltage to generated force or from the strain to the generated voltage, we need to defined some parameters corresponding to the piezoelectric material:
#+begin_src matlab
  d33 = 300e-12; % Strain constant [m/V]
  n   = 80;      % Number of layers per stack
  ka  = 235e6;   % Stack stiffness [N/m]
#+end_src

The ratio of the developed force to applied voltage is:
#+name: eq:piezo_voltage_to_force
\begin{equation}
  F_a = g_a V_a, \quad g_a = d_{33} n k_a
\end{equation}
where:
- $F_a$: developed force in [N]
- $n$: number of layers of the actuator stack
- $d_{33}$: strain constant in [m/V]
- $k_a$: actuator stack stiffness in [N/m]
- $V_a$: applied voltage in [V]

If we take the numerical values, we obtain:
#+begin_src matlab
  ga = d33*n*ka; % [N/V]
#+end_src

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

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


From cite:fleming14_desig_model_contr_nanop_system (page 123), the relation between relative displacement of the sensor stack and generated voltage is:
#+name: eq:piezo_strain_to_voltage
\begin{equation}
  V_s = \frac{d_{33}}{\epsilon^T s^D n} \Delta h
\end{equation}
where:
- $V_s$: measured voltage in [V]
- $d_{33}$: strain constant in [m/V]
- $\epsilon^T$: permittivity under constant stress in [F/m]
- $s^D$: elastic compliance under constant electric displacement in [m^2/N]
- $n$: number of layers of the sensor stack
- $\Delta h$: relative displacement in [m]

If we take the numerical values, we obtain:
#+begin_src matlab
  d33 = 300e-12; % Strain constant [m/V]
  n   = 80;      % Number of layers per stack
  eT  = 5.3e-9;  % Permittivity under constant stress [F/m]
  sD  = 2e-11;   % Compliance under constant electric displacement [m2/N]
#+end_src

#+begin_src matlab
  gs = d33/(eT*sD*n); % [V/m]
#+end_src

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

#+RESULTS:
: gs = 35.4 [V/um]

** Estimation from Experiment
<<sec:estimation_identification>>
*** Introduction                                                    :ignore:
The idea here is to obtain the parameters $g_a$ and $g_s$ from the comparison of an experimental identification and the identification using Simscape.

Using the experimental identification, we can easily obtain the gain from the applied voltage to the generated displacement, but not to the generated force.
However, from the Simscape model, we can easily have the link from the generated force to the displacement, them we can computed $g_a$.

Similarly, it is fairly easy to experimentally obtain the gain from the stack displacement to the generated voltage across the stack.
To link that to the strain of the sensor stack, the simscape model is used.

*** From actuator voltage $V_a$ to actuator force $F_a$
The data from the identification test is loaded.
#+begin_src matlab
  load('apa95ml_5kg_Amp_E505.mat', 't', 'um', 'y');

  % Any offset value is removed:
  um = detrend(um, 0); % Amplifier Input Voltage [V]
  y  = detrend(y , 0); % Mass displacement [m]
#+end_src

Now we add a factor 10 to take into account the gain of the voltage amplifier and thus obtain the voltage across the piezoelectric stack.
#+begin_src matlab
  um = 10*um; % Stack Actuator Input Voltage [V]
#+end_src

Then, the transfer function from the stack voltage to the vertical displacement is computed.
#+begin_src matlab
  Ts = t(end)/(length(t)-1);
  Fs = 1/Ts;

  win = hanning(ceil(1*Fs));

  [tf_est, f] = tfestimate(um, y, win, [], [], 1/Ts);
#+end_src

The gain from input voltage of the stack to the vertical displacement is determined:
#+begin_src matlab
  g_d_Va = 4e-7; % [m/V]
#+end_src

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

  figure;
  hold on;
  plot(f, abs(tf_est), 'DisplayName', 'Identification')
  plot([f(2) f(end)], [g_d_Va g_d_Va], '--', 'DisplayName', sprintf('$d/V_a \\approx %.0g\\ m/V$', g_d_Va))
  hold off;
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Amplitude $d/V_a$ [m/V]');
  xlim([10, 5e3]); ylim([1e-8, 1e-5]);
  legend('location', 'southwest');
#+end_src

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

#+name: fig:gain_Va_to_d
#+caption: Transfer function from actuator stack voltage $V_a$ to vertical displacement of the mass $d$
#+RESULTS:
[[file:figs/gain_Va_to_d.png]]

Then, the transfer function from forces applied by the stack actuator to the vertical displacement of the mass is identified from the Simscape model.
#+begin_src matlab :exports none
  % Load Parameters
  K = readmatrix('APA95ML_K.CSV');
  M = readmatrix('APA95ML_M.CSV');
  [int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('APA95ML_out_nodes_3D.txt');

  % Define parameters just for the simulation
  ga = 1; % [N/V]
  gs = 1; % [V/m]
#+end_src

#+begin_src matlab
  m = 5.5;

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

  %% Input/Output definition
  clear io; io_i = 1;
  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]

  Gd = linearize(mdl, io);
#+end_src

The DC gain the the identified dynamics
#+begin_src matlab
  g_d_Fa = abs(dcgain(Gd)); % [m/N]
#+end_src

#+begin_src matlab :results value replace :exports results :tangle no
  sprintf('G_d_Fa = %.2g [m/N]', g_d_Fa)
#+end_src

#+RESULTS:
: G_d_Fa = 1.2e-08 [m/N]


And finally, the gain $g_a$ from the the actuator voltage $V_a$ to the generated force $F_a$ can be computed:
\begin{equation}
  g_a = \frac{F_a}{V_a} = \frac{F_a}{d} \frac{d}{V_a}
\end{equation}

#+begin_src matlab
  ga = g_d_Va/g_d_Fa;
#+end_src

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

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

The obtained comparison between the Simscape model and the identified dynamics is shown in Figure [[fig:compare_Gd_id_simscape]].

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

  figure;
  hold on;
  plot(f, abs(tf_est), 'DisplayName', 'Identification')
  plot(f, ga*abs(squeeze(freqresp(Gd, f, 'Hz'))), 'DisplayName', 'Simscape Model')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude $d/V_a$ [m/V]'); xlabel('Frequency [Hz]');
  hold off;
  xlim([1, 5e3]); ylim([1e-9, 1e-5]);
  legend('location', 'southwest');
#+end_src

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

#+name: fig:compare_Gd_id_simscape
#+caption: Comparison of the identified transfer function between actuator voltage $V_a$ and vertical mass displacement $d$
#+RESULTS:
[[file:figs/compare_Gd_id_simscape.png]]

*** From stack strain $\Delta h$ to generated voltage $V_s$
Now, the gain from the stack strain $\Delta h$ to the generated voltage $V_s$ is estimated.

We can determine the gain from actuator voltage $V_a$ to sensor voltage $V_s$ thanks to the identification.
Using the simscape model, we can have the transfer function from the actuator voltage $V_a$ (using the previously estimated gain $g_a$) to the sensor stack strain $\Delta h$.

Finally, using these two values, we can compute the gain $g_s$ from the stack strain $\Delta h$ to the generated Voltage $V_s$.

Identification data is loaded.
#+begin_src matlab
  load('apa95ml_5kg_2a_1s.mat', 't', 'u', 'v');

  u = detrend(u, 0); % Input Voltage of the Amplifier [V]
  v = detrend(v, 0); % Voltage accross the stack sensor [V]
#+end_src

Here, an amplifier with a gain of 20 is used.
#+begin_src matlab
  u = 20*u; % Input Voltage of the Amplifier [V]
#+end_src

Then, the transfer function from $V_a$ to $V_s$ is identified and its DC gain is estimated (Figure [[fig:gain_Va_to_Vs]]).
#+begin_src matlab
  Ts = t(end)/(length(t)-1);
  Fs = 1/Ts;

  win = hann(ceil(10/Ts));

  [tf_est, f] = tfestimate(u, v, win, [], [], 1/Ts);
#+end_src

#+begin_src matlab
  g_Vs_Va = 0.022; % [V/V]
#+end_src

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

  figure;
  hold on;
  plot(f, abs(tf_est), 'DisplayName', 'Identification')
  plot([f(2); f(end)], [g_Vs_Va; g_Vs_Va], '--', 'DisplayName', sprintf('$V_s/V_a \\approx %.0g\\ m/V$', g_Vs_Va))
  hold off;
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude $V_s/V_a$ [V/V]'); xlabel('Frequency [Hz]');
  xlim([5e-1 5e3]); ylim([1e-3, 1e1]);
  legend('location', 'northwest');
#+end_src

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

#+name: fig:gain_Va_to_Vs
#+caption: Transfer function from actuator stack voltage $V_a$ to sensor stack voltage $V_s$
#+RESULTS:
[[file:figs/gain_Va_to_Vs.png]]


Now the transfer function from the actuator stack voltage to the sensor stack strain is estimated using the Simscape model.
#+begin_src matlab
  m = 5.5;

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

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Va'], 1, 'openinput');  io_i = io_i + 1; % Actuator Voltage [V]
  io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Sensor Stack displacement [m]

  Gf = linearize(mdl, io);
#+end_src

The gain from the actuator stack voltage to the sensor stack strain is estimated below.
#+begin_src matlab
  G_dh_Va = abs(dcgain(Gf));
#+end_src

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

#+RESULTS:
: G_dh_Va = 6.2e-09 [m/V]

And finally, the gain $g_s$ from the sensor stack strain to the generated voltage can be estimated:
\begin{equation}
  g_s = \frac{V_s}{\Delta h} = \frac{V_s}{V_a} \frac{V_a}{\Delta h}
\end{equation}

#+begin_src matlab
  gs = g_Vs_Va/G_dh_Va; % [V/m]
#+end_src

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

#+RESULTS:
: gs = 3.5 [V/um]

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

  figure;
  hold on;
  plot(f, abs(tf_est), 'DisplayName', 'Identification')
  plot(f, gs*abs(squeeze(freqresp(Gf, f, 'Hz'))), 'DisplayName', 'Simscape Model')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude $V_s/V_a$ [V/V]'); xlabel('Frequency [Hz]');
  hold off;
  xlim([1, 5e3]); ylim([1e-3, 1e1]);
  legend('location', 'southwest');
#+end_src

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

#+name: fig:compare_Gf_id_simscape
#+caption: Comparison of the identified transfer function between actuator voltage $V_a$ and sensor stack voltage
#+RESULTS:
[[file:figs/compare_Gf_id_simscape.png]]

** Conclusion
The obtained parameters $g_a$ and $g_s$ are not consistent between the different methods.

The one using the experimental data are saved and further used.
#+begin_src matlab :tangle no
  save('./matlab/mat/apa95ml_params.mat', 'ga', 'gs');
#+end_src

#+begin_src matlab :exports none :eval no
  save('./mat/apa95ml_params.mat', 'ga', 'gs');
#+end_src

* Simscape model of the test-bench
:PROPERTIES:
:header-args:matlab+: :tangle matlab/simscape_model.m
:header-args:matlab+: :comments org :mkdirp yes
:END:
<<sec:simscape_model>>

** Introduction                                                      :ignore:
The idea here is to model the test-bench using Simscape.

Whereas the suspended mass and metrology frame can be considered as rigid bodies in the frequency range of interest, the Amplified Piezoelectric Actuator (APA) is flexible.

To model the APA, a Finite Element Model (FEM) is used (Figure [[fig:APA95ML_FEM]]) and imported into Simscape.

#+name: fig:APA95ML_FEM
#+caption: Finite Element Model of the APA95ML
[[file:figs/APA95ML_FEM.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
  addpath('./matlab/mat/');
  addpath('./matlab/');
#+end_src

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

** Import Mass Matrix, Stiffness Matrix, and Interface Nodes Coordinates
We first extract the stiffness and mass matrices.
#+begin_src matlab
  K = readmatrix('APA95ML_K.CSV');
  M = readmatrix('APA95ML_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:
| 300000000.0 |    -1000.0 |    -30000.0 |    -40.0 | 70000.0 |   300.0 |  20000000.0 |      -30.0 |     -5000.0 |         5 |
|     -1000.0 | 50000000.0 |     -7000.0 | 800000.0 |   -20.0 |   300.0 |      3000.0 |  5000000.0 |       400.0 |  -40000.0 |
|    -30000.0 |    -7000.0 | 100000000.0 |   -200.0 |   -60.0 |    70.0 |      3000.0 |     3000.0 |  -8000000.0 |     -30.0 |
|       -40.0 |   800000.0 |      -200.0 |  20000.0 |    -0.4 |       4 |        30.0 |    40000.0 |           7 |    -300.0 |
|     70000.0 |      -20.0 |       -60.0 |     -0.4 |  3000.0 |       1 |     -6000.0 |       10.0 |           8 |      -0.1 |
|       300.0 |      300.0 |        70.0 |        4 |       1 | 40000.0 |       -10.0 |      -10.0 |        30.0 |       0.1 |
|  20000000.0 |     3000.0 |      3000.0 |     30.0 | -6000.0 |   -10.0 | 300000000.0 |     2000.0 |      9000.0 |    -100.0 |
|       -30.0 |  5000000.0 |      3000.0 |  40000.0 |    10.0 |   -10.0 |      2000.0 | 50000000.0 |     -3000.0 | -800000.0 |
|     -5000.0 |      400.0 |  -8000000.0 |        7 |       8 |    30.0 |      9000.0 |    -3000.0 | 100000000.0 |     100.0 |
|           5 |   -40000.0 |       -30.0 |   -300.0 |    -0.1 |     0.1 |      -100.0 |  -800000.0 |       100.0 |   20000.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.03 |   7e-08 |  2e-06 | -3e-09 | -0.0002 | -6e-08 | -0.001 |   8e-07 |  6e-07 | -8e-09 |
|   7e-08 |    0.02 | -1e-06 |  9e-05 |  -3e-09 | -4e-09 | -1e-06 | -0.0006 | -4e-08 |  5e-06 |
|   2e-06 |  -1e-06 |   0.02 | -3e-08 |  -4e-08 |  1e-08 |  1e-07 |  -2e-07 | 0.0003 |  1e-09 |
|  -3e-09 |   9e-05 | -3e-08 |  1e-06 |  -3e-11 | -3e-13 | -7e-09 |  -5e-06 | -3e-10 |  3e-08 |
| -0.0002 |  -3e-09 | -4e-08 | -3e-11 |   2e-06 |  6e-10 |  2e-06 |  -7e-09 | -2e-09 |  7e-11 |
|  -6e-08 |  -4e-09 |  1e-08 | -3e-13 |   6e-10 |  1e-06 |  1e-08 |   3e-09 | -2e-09 |  2e-13 |
|  -0.001 |  -1e-06 |  1e-07 | -7e-09 |   2e-06 |  1e-08 |   0.03 |   4e-08 | -2e-06 |  8e-09 |
|   8e-07 | -0.0006 | -2e-07 | -5e-06 |  -7e-09 |  3e-09 |  4e-08 |    0.02 | -9e-07 | -9e-05 |
|   6e-07 |  -4e-08 | 0.0003 | -3e-10 |  -2e-09 | -2e-09 | -2e-06 |  -9e-07 |   0.02 |  2e-08 |
|  -8e-09 |   5e-06 |  1e-09 |  3e-08 |   7e-11 |  2e-13 |  8e-09 |  -9e-05 |  2e-08 |  1e-06 |


Then, we extract the coordinates of the interface nodes.
#+begin_src matlab
  [int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('APA95ML_out_nodes_3D.txt');
#+end_src

The interface nodes are shown in Figure [[fig:APA95ML_nodes_1]] and their coordinates are listed in Table [[tab:apa95ml_nodes_coordinates]].

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

#+RESULTS:
| Total number of Nodes     |  7 |
| Number of interface Nodes |  7 |
| Number of Modes           |  6 |
| Size of M and K matrices  | 48 |

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

#+name: tab:apa95ml_nodes_coordinates
#+caption: Coordinates of the interface nodes
#+RESULTS:
| Node i | Node Number |  x [m] | y [m] |     z [m] |
|--------+-------------+--------+-------+-----------|
|    1.0 |     40467.0 |    0.0 |   0.0 |  0.029997 |
|    2.0 |     40469.0 |    0.0 |   0.0 | -0.029997 |
|    3.0 |     40470.0 | -0.035 |   0.0 |       0.0 |
|    4.0 |     40475.0 | -0.015 |   0.0 |       0.0 |
|    5.0 |     40476.0 | -0.005 |   0.0 |       0.0 |
|    6.0 |     40477.0 |  0.015 |   0.0 |       0.0 |
|    7.0 |     40478.0 |  0.035 |   0.0 |       0.0 |

#+name: fig:APA95ML_nodes_1
#+caption: Interface Nodes chosen for the APA95ML
[[file:figs/APA95ML_nodes_1.png]]

Using =K=, =M= and =int_xyz=, we can use the =Reduced Order Flexible Solid= simscape block.

** Simscape Model
The flexible element is imported using the =Reduced Order Flexible Solid= Simscape block.

To model the actuator, an =Internal Force= block is added between the nodes 3 and 12.
A =Relative Motion Sensor= block is added between the nodes 1 and 2 to measure the displacement and the amplified piezo.

One mass is fixed at one end of the piezo-electric stack actuator, the other end is fixed to the world frame.
#+begin_src matlab
  m = 5;
#+end_src

** Dynamics from Actuator Voltage to Vertical Mass Displacement
The identified dynamics is shown in Figure [[fig:dynamics_act_disp_comp_mass]].

#+begin_src matlab
  %% Name of the Simulink File
  mdl = 'piezo_amplified_3d';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Va'], 1, 'openinput');  io_i = io_i + 1; % Actuator Voltage [V]
  io(io_i) = linio([mdl, '/y'],  1, 'openoutput'); io_i = io_i + 1; % Vertical Displacement [m]

  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(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(Ghm, freqs, 'Hz')))));
  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)]);
#+end_src

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

#+name: fig:dynamics_act_disp_comp_mass
#+caption: Dynamics from $F$ to $d$ without a payload and with a 5kg payload
#+RESULTS:
[[file:figs/dynamics_act_disp_comp_mass.png]]

** Dynamics from Actuator Voltage to Force Sensor Voltage
The obtained dynamics is shown in Figure [[fig:dynamics_force_force_sensor_comp_mass]].

#+begin_src matlab
  %% Name of the Simulink File
  mdl = 'piezo_amplified_3d';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Va'], 1, 'openinput');  io_i = io_i + 1; % Voltage Actuator [V]
  io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Sensor Voltage [V]

  Gfm = linearize(mdl, io);
#+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(Gfm, 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(Gfm, 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/dynamics_force_force_sensor_comp_mass.pdf', 'width', 'wide', 'height', 'tall');
#+end_src

#+name: fig:dynamics_force_force_sensor_comp_mass
#+caption: Dynamics from $F$ to $F_m$ for $m=0$ and $m = 10kg$
#+RESULTS:
[[file:figs/dynamics_force_force_sensor_comp_mass.png]]

** Save Data for further use
#+begin_src matlab :tangle no
  save('matlab/mat/fem_simscape_models.mat', 'Ghm', 'Gfm')
#+end_src

#+begin_src matlab :eval no
  save('mat/fem_simscape_models.mat', 'Ghm', 'Gfm')
#+end_src

* Huddle Test
:PROPERTIES:
:header-args:matlab+: :tangle matlab/huddle_test.m
:header-args:matlab+: :comments org :mkdirp yes
:END:
<<sec:huddle_test>>
** 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 :tangle no
  addpath('./matlab/mat/');
#+end_src

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

** Load Data                                                       :noexport:
#+begin_src matlab
  load('huddle_test.mat', 't', 'y');
#+end_src

#+begin_src matlab
  y = y - mean(y(1000:end));
#+end_src

** Time Domain Data
#+begin_src matlab :exports none
  figure;
  plot(t(1000:end), y(1000:end))
  ylabel('Output Displacement [m]'); xlabel('Time [s]');
#+end_src

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

#+name: fig:huddle_test_time_domain
#+caption: Measurement of the Mass displacement during Huddle Test
#+RESULTS:
[[file:figs/huddle_test_time_domain.png]]

** PSD of Measurement Noise
#+begin_src matlab
  Ts = t(end)/(length(t)-1);
  Fs = 1/Ts;
 
  win = hanning(ceil(1*Fs));
#+end_src

#+begin_src matlab
  [pxx, f] = pwelch(y(1000:end), win, [], [], Fs);
#+end_src

#+begin_src matlab :exports none
  figure;
  plot(f, sqrt(pxx));
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]');
  xlim([1, Fs/2]);
#+end_src

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

#+name: fig:huddle_test_pdf
#+caption: Amplitude Spectral Density of the Displacement during Huddle Test
#+RESULTS:
[[file:figs/huddle_test_pdf.png]]

* TODO Identification of the dynamics from actuator to displacement
:PROPERTIES:
:header-args:matlab+: :tangle matlab/motion_identification.m
:header-args:matlab+: :comments org :mkdirp yes
:END:
<<sec:motion_identification>>
** Introduction                                                      :ignore:

- [ ] List of equipment
- [ ] Schematic
- [ ] Problem of matching between the models? (there is a factor 10)

E505 with gain of 10.

** 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/mat/');
#+end_src

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

** Load Data
The data from the "noise test" and the identification test are loaded.
#+begin_src matlab
  ht = load('huddle_test.mat', 't', 'u', 'y');
  load('apa95ml_5kg_Amp_E505.mat', 't', 'um', 'y');
#+end_src

Any offset value is removed:
#+begin_src matlab
  um = detrend(um, 0); % Input Voltage [V]
  y  = detrend(y , 0); % Mass displacement [m]

  ht.u  = detrend(ht.u, 0);
  ht.y  = detrend(ht.y, 0);
#+end_src

Now we add a factor 10 to take into account the gain of the voltage amplifier.
#+begin_src matlab
  um   = 10*um;
  ht.u = 10*ht.u;
#+end_src

** Comparison of the PSD with Huddle Test
#+begin_src matlab
  Ts = t(end)/(length(t)-1);
  Fs = 1/Ts;

  win = hanning(ceil(1*Fs));
#+end_src

#+begin_src matlab
  [pxx, f] = pwelch(y, win, [], [], Fs);
  [pht, ~] = pwelch(ht.y, win, [], [], Fs);
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(f, sqrt(pxx), 'DisplayName', '5kg');
  plot(f, sqrt(pht), 'DisplayName', 'Huddle Test');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]');
  legend('location', 'northeast');
  xlim([1, Fs/2]);
#+end_src

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

#+name: fig:apa95ml_5kg_PI_pdf_comp_huddle
#+caption: Comparison of the ASD for the identification test and the huddle test
#+RESULTS:
[[file:figs/apa95ml_5kg_PI_pdf_comp_huddle.png]]

** Compute TF estimate and Coherence
#+begin_src matlab
  [tf_est, f] = tfestimate(um, -y, win, [], [], 1/Ts);
  [co_est, ~] = mscohere(  um, -y, win, [], [], 1/Ts);
#+end_src

#+begin_src matlab :exports none
  figure;

  hold on;
  plot(f, co_est, 'k-')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
  ylabel('Coherence'); xlabel('Frequency [Hz]');
  hold off;
  xlim([10, 5e3]);
#+end_src

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

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

Comparison with the FEM model
#+begin_src matlab
  load('mat/fem_simscape_models.mat', 'Ghm');
#+end_src

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

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

  ax1 = nexttile([2,1]);
  hold on;
  plot(f, abs(tf_est), 'DisplayName', 'Identification')
  plot(freqs, abs(squeeze(freqresp(Ghm, freqs, 'Hz'))), 'DisplayName', 'FEM')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel', []);
  legend('location', 'northeast')
  hold off;

  ax2 = nexttile;
  hold on;
  plot(f,     180/pi*angle(tf_est))
  plot(freqs, 180/pi*angle(squeeze(freqresp(Ghm, freqs, 'Hz'))))
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
  ylabel('Phase'); xlabel('Frequency [Hz]');
  hold off;
  ylim([-180, 180]);
  yticks(-180:90:180);

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

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

#+name: fig:apa95ml_5kg_pi_comp_fem
#+caption: Comparison of the identified transfer function and the one estimated from the FE model
#+RESULTS:
[[file:figs/apa95ml_5kg_pi_comp_fem.png]]

* Identification of the dynamics from actuator to force sensor
:PROPERTIES:
:header-args:matlab+: :tangle matlab/force_sensor_identification.m
:header-args:matlab+: :comments org :mkdirp yes
:END:
<<sec:force_sensor_identification>>
** Introduction                                                      :ignore:
Two measurements are performed:
- Speedgoat DAC => Voltage Amplifier (x20) => 1 Piezo Stack => ... => 2 Stacks as Force Sensor (parallel) => Speedgoat ADC
- Speedgoat DAC => Voltage Amplifier (x20) => 2 Piezo Stacks (parallel) => ... => 1 Stack as Force Sensor => Speedgoat ADC

The obtained dynamics from force actuator to force sensor are compare with the FEM model.

** 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/mat/');
#+end_src

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

** Load Data                                                         :ignore:
The data are loaded:
#+begin_src matlab
  load('apa95ml_5kg_2a_1s.mat', 't', 'u', 'v');
#+end_src

#+begin_src matlab
  u = detrend(u, 0);
  v = detrend(v, 0);
#+end_src

#+begin_src matlab
  u = 20*u;
#+end_src

** Adjust gain                                                       :ignore:
Let's use the amplifier gain to obtain the true voltage applied to the actuator stack(s)

The parameters of the piezoelectric stacks are defined below:
#+begin_src matlab
  d33 = 3e-10; % Strain constant [m/V]
  n = 80; % Number of layers per stack
  eT = 1.6e-8; % Permittivity under constant stress [F/m]
  sD = 2e-11; % Elastic compliance under constant electric displacement [m2/N]
  ka = 235e6; % Stack stiffness [N/m]
#+end_src

From the FEM, we construct the transfer function from DAC voltage to ADC voltage.
#+begin_src matlab
  Gfem_aa_s = exp(-s/1e4)*20*(2*d33*n*ka)*(G(3,1)+G(3,2))*d33/(eT*sD*n);
  Gfem_a_ss = exp(-s/1e4)*20*(  d33*n*ka)*(G(3,1)+G(2,1))*d33/(eT*sD*n);
#+end_src

#+begin_src matlab
  Gfem_aa_s = exp(-s/1e4)*20*(2*d33*n*ka)*Gfm*d33/(eT*sD*n);
  Gfem_a_ss = exp(-s/1e4)*20*(  d33*n*ka)*Gfm*d33/(eT*sD*n);
#+end_src

** Compute TF estimate and Coherence                                 :ignore:
The transfer function from input voltage to output voltage are computed and shown in Figure [[fig:bode_plot_force_sensor_voltage_comp_fem]].

#+begin_src matlab
  Ts = t(end)/(length(t)-1);
  Fs = 1/Ts;

  win = hann(ceil(10/Ts));

  [tf_est,  f] = tfestimate(u, v, win, [], [], 1/Ts);
  [coh,     ~] = mscohere(  u, v, win, [], [], 1/Ts);
#+end_src

#+begin_src matlab
  load('mat/fem_simscape_models.mat', 'Gfm');
#+end_src

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

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

  ax1 = nexttile([2,1]);
  hold on;
  plot(f, abs(tf_est))
  plot(freqs, abs(squeeze(freqresp(Gfm, freqs, 'Hz'))))
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
  hold off;
  ylim([1e-3, 1e1]);

  ax2 = nexttile;
  hold on;
  plot(f, 180/pi*angle(tf_est), ...
       'DisplayName', 'Identification')
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gfm, freqs, 'Hz'))), ...
       'DisplayName', 'FEM')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
  ylabel('Phase'); xlabel('Frequency [Hz]');
  hold off;
  ylim([-180, 180]);
  yticks(-180:90:180);
  legend('location', 'northeast')

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

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

#+name: fig:bode_plot_force_sensor_voltage_comp_fem
#+caption: Comparison of the identified dynamics from voltage output to voltage input and the FEM
#+RESULTS:
[[file:figs/bode_plot_force_sensor_voltage_comp_fem.png]]

** System Identification
#+begin_src matlab
  w_z = 2*pi*111; % Zeros frequency [rad/s]
  w_p = 2*pi*255; % Pole frequency [rad/s]
  xi_z = 0.05;
  xi_p = 0.015;
  G_inf = 0.1;

  Gi = G_inf*(s^2 - 2*xi_z*w_z*s + w_z^2)/(s^2 + 2*xi_p*w_p*s + w_p^2);
#+end_src

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

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

  ax1 = nexttile([2,1]);
  hold on;
  plot(f, abs(tf_est), '-')
  plot(freqs, abs(squeeze(freqresp(Gi, freqs, 'Hz'))), '--')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel', []);
  hold off;
  ylim([1e-3, 1e1]);

  ax2 = nexttile;
  hold on;
  plot(f, 180/pi*angle(tf_est), '-', 'DisplayName', '2 Act - 1 Sen')
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gi, freqs, 'Hz'))), '--', 'DisplayName', '2 Act - 1 Sen, - FEM')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
  ylabel('Phase'); xlabel('Frequency [Hz]');
  hold off;
  ylim([-180, 180]);
  yticks(-180:90:180);
  legend('location', 'northeast')

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

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

#+name: fig:iff_plant_identification_apa95ml
#+caption: Identification of the IFF plant
#+RESULTS:
[[file:figs/iff_plant_identification_apa95ml.png]]


** Integral Force Feedback
#+begin_src matlab :exports none
  gains = logspace(0, 5, 1000);

  figure;
  hold on;
  plot(real(pole(Gi)),  imag(pole(Gi)),  'kx');
  plot(real(tzero(Gi)),  imag(tzero(Gi)),  'ko');
  for i = 1:length(gains)
    cl_poles = pole(feedback(Gi, (gains(i)/(s + 2*2*pi)*s/(s + 0.5*2*pi))));
    plot(real(cl_poles), imag(cl_poles), 'k.');
  end
  ylim([0, 1800]);
  xlim([-1600,200]);
  xlabel('Real Part')
  ylabel('Imaginary Part')
  axis square
#+end_src

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

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

* Integral Force Feedback
:PROPERTIES:
:header-args:matlab+: :tangle matlab/integral_force_feedback.m
:header-args:matlab+: :comments org :mkdirp yes
:END:
<<sec:integral_force_feedback>>

** Introduction                                                      :ignore:

#+name: fig:test_bench_apa_schematic_iff
#+caption: Schematic of the test bench using IFF
[[file:figs/test_bench_apa_schematic_iff.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
  addpath('./matlab/mat/');
#+end_src

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

** First tests with few gains
#+begin_src matlab
  iff_g10  = load('apa95ml_iff_g10_res.mat',  'u', 't', 'y', 'v');
  iff_g100 = load('apa95ml_iff_g100_res.mat', 'u', 't', 'y', 'v');
  iff_of   = load('apa95ml_iff_off_res.mat',  'u', 't', 'y', 'v');
#+end_src

#+begin_src matlab
  Ts = 1e-4;
  win = hann(ceil(10/Ts));

  [tf_iff_g10, f] = tfestimate(iff_g10.u, iff_g10.y, win, [], [], 1/Ts);
  [co_iff_g10, ~] = mscohere(iff_g10.u, iff_g10.y, win, [], [], 1/Ts);

  [tf_iff_g100, ~] = tfestimate(iff_g100.u, iff_g100.y, win, [], [], 1/Ts);
  [co_iff_g100, ~] = mscohere(iff_g100.u, iff_g100.y, win, [], [], 1/Ts);

  [tf_iff_of, ~] = tfestimate(iff_of.u, iff_of.y, win, [], [], 1/Ts);
  [co_iff_of, ~] = mscohere(iff_of.u, iff_of.y, win, [], [], 1/Ts);
#+end_src

#+begin_src matlab :exports none
  figure;

  hold on;
  plot(f, co_iff_of, '-', 'DisplayName', 'g=0')
  plot(f, co_iff_g10, '-', 'DisplayName', 'g=10')
  plot(f, co_iff_g100, '-', 'DisplayName', 'g=100')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
  ylabel('Coherence'); xlabel('Frequency [Hz]');
  hold off;
  legend();
  xlim([60, 600])
#+end_src

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

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


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

  ax1 = nexttile([2, 1]);
  hold on;
  plot(f, abs(tf_iff_of), '-', 'DisplayName', 'g=0')
  plot(f, abs(tf_iff_g10), '-', 'DisplayName', 'g=10')
  plot(f, abs(tf_iff_g100), '-', 'DisplayName', 'g=100')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
  hold off;
  legend();

  ax2 = nexttile;
  hold on;
  plot(f, 180/pi*angle(-tf_iff_of), '-')
  plot(f, 180/pi*angle(-tf_iff_g10), '-')
  plot(f, 180/pi*angle(-tf_iff_g100), '-')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
  ylabel('Phase'); xlabel('Frequency [Hz]');
  hold off;
  yticks(-360:90:360);

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

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

#+name: fig:iff_first_test_bode_plot
#+caption: Bode plot for different values of IFF gain
#+RESULTS:
[[file:figs/iff_first_test_bode_plot.png]]

** Second test with many Gains
#+begin_src matlab
  load('apa95ml_iff_test.mat', 'results');
#+end_src

#+begin_src matlab
  Ts = 1e-4;
  win = hann(ceil(10/Ts));
#+end_src

#+begin_src matlab
  tf_iff = {zeros(1, length(results))};
  co_iff = {zeros(1, length(results))};
  g_iff = [0, 1, 5, 10, 50, 100];
 
  for i=1:length(results)
      [tf_est, f] = tfestimate(results{i}.u, results{i}.y, win, [], [], 1/Ts);
      [co_est, ~] = mscohere(results{i}.u, results{i}.y, win, [], [], 1/Ts);

      tf_iff(i) = {tf_est};
      co_iff(i) = {co_est};
  end
#+end_src

#+begin_src matlab :exports none
  figure;

  hold on;
  for i = 1:length(results)
    plot(f, co_iff{i}, '-', 'DisplayName', sprintf('g = %0.f', g_iff(i)))
  end
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
  ylabel('Coherence'); xlabel('Frequency [Hz]');
  hold off;
  legend();
  xlim([60, 600])
#+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(results)
    plot(f, abs(tf_iff{i}), '-', 'DisplayName', sprintf('g = %0.f', g_iff(i)))
  end
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
  hold off;
  legend();

  ax2 = nexttile;
  hold on;
  for i = 1:length(results)
    plot(f, 180/pi*angle(-tf_iff{i}), '-')
  end
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
  ylabel('Phase'); xlabel('Frequency [Hz]');
  yticks(-360:90:360);
  axis padded 'auto x'

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

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

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

#+begin_src matlab
  G_id = {zeros(1,length(results))};

  f_start = 70; % [Hz]
  f_end = 500; % [Hz]

  for i = 1:length(results)
      tf_id = tf_iff{i}(sum(f<f_start):length(f)-sum(f>f_end));
      f_id = f(sum(f<f_start):length(f)-sum(f>f_end));
     
      gfr = idfrd(tf_id, 2*pi*f_id, Ts);
      G_id(i) = {procest(gfr,'P2UDZ')};
  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(results)
    set(gca,'ColorOrderIndex',i)
    plot(f, abs(tf_iff{i}), '-', 'DisplayName', sprintf('g = %0.f', g_iff(i)))
    set(gca,'ColorOrderIndex',i)
    plot(f, abs(squeeze(freqresp(G_id{i}, f, 'Hz'))), '--', 'HandleVisibility', 'off')
  end
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
  hold off;
  legend();

  ax2 = nexttile;
  hold on;
  for i = 1:length(results)
    set(gca,'ColorOrderIndex',i)
    plot(f, 180/pi*angle(tf_iff{i}), '-')
    set(gca,'ColorOrderIndex',i)
    plot(f, 180/pi*angle(squeeze(freqresp(G_id{i}, f, 'Hz'))), '--')
  end
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
  ylabel('Phase'); xlabel('Frequency [Hz]');
  hold off;
  yticks(-360:90:360);
  axis padded 'auto x'

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

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

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

#+begin_src matlab :exports none
  w_z = 2*pi*111; % Zeros frequency [rad/s]
  w_p = 2*pi*255; % Pole frequency [rad/s]
  xi_z = 0.05;
  xi_p = 0.015;
  G_inf = 2;

  Gi = G_inf*(s^2 - 2*xi_z*w_z*s + w_z^2)/(s^2 + 2*xi_p*w_p*s + w_p^2);


  gains = logspace(0, 5, 1000);

  figure;
  hold on;
  plot(real(pole(Gi)),  imag(pole(Gi)),  'kx', 'HandleVisibility', 'off');
  plot(real(tzero(Gi)),  imag(tzero(Gi)),  'ko', 'HandleVisibility', 'off');
  for i = 1:length(results)
    set(gca,'ColorOrderIndex',i)
    plot(real(pole(G_id{i})), imag(pole(G_id{i})), 'o', 'DisplayName', sprintf('g = %0.f', g_iff(i)));
  end
  for i = 1:length(gains)
    cl_poles = pole(feedback(Gi, (gains(i)/(s + 2*pi*2))));
    plot(real(cl_poles), imag(cl_poles), 'k.', 'HandleVisibility', 'off');
  end
  ylim([0, 1800]);
  xlim([-1600,200]);
  xlabel('Real Part')
  ylabel('Imaginary Part')
  axis square
  legend('location', 'northwest');
#+end_src

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

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