phd-test-bench-apa/test-bench-apa.org

#+TITLE: Test Bench - Amplified Piezoelectric Actuator
:DRAWER:
#+LANGUAGE: en
#+EMAIL: dehaeze.thomas@gmail.com
#+AUTHOR: Dehaeze Thomas

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

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

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

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

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

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

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

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

#+latex: \clearpage

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


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

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

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

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

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

* Notes                                                             :noexport:

Prefix for figures/section/tables =test_apa=

** Add the following reports
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/test-bench-apa95ml/test-bench-apa95ml.org][test-bench-apa95ml]]
  Maybe not useful
  - [X] See if the IFF root locus has been measured with the APA300ML
 *Yes*
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/test-bench-apa300ml/test-bench-apa300ml.org][test-bench-apa300ml]]
  - Model (Section 1)
  - Basic measurements (dimensions, electrical, stroke, etc...) (Section 2)
  - Dynamical measurements (Section 3)
  - Simscape Model (Section 4)

** TODO [#B] Check things about resistor in parallel with the force sensor

Verify that everything interesting to say about that is either done before in the thesis or in this report.

* Introduction                                                        :ignore:


#+name: fig:test_apa_received
#+attr_latex: :width 0.7\linewidth
#+caption: Picture of 5 out of the 7 received APA300ML
[[file:figs/test_apa_received.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 ref:sec:test_apa_basic_meas.
- 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 ref:sec:test_apa_dynamics.
- Compare the measurements with the Simscape models (2DoF, Super-Element) in order to tuned/validate the models.
  This is explained in Section ref:sec:test_apa_simscape.

#+name: tab:test_apa_section_matlab_code
#+caption: Report sections and corresponding Matlab files
#+attr_latex: :environment tabularx :width 0.6\linewidth :align lX
#+attr_latex: :center t :booktabs t
| *Sections*                          | *Matlab File*             |
|-------------------------------------+---------------------------|
| Section ref:sec:test_apa_basic_meas | =test_apa_1_basic_meas.m= |
| Section ref:sec:test_apa_dynamics   | =test_apa_2_.m=           |
| Section ref:sec:test_apa_simscape   | =test_apa_3_.m=           |

* First Basic Measurements
:PROPERTIES:
:header-args:matlab+: :tangle matlab/test_apa_1_basic_meas.m
:END:
<<sec:test_apa_basic_meas>>
** Introduction                                                      :ignore:

Before using the measurement bench to characterize the APA300ML, first simple measurements are performed:
- Section ref:sec:test_apa_geometrical_measurements: the geometric tolerances of the interface planes are checked
- Section ref:sec:test_apa_electrical_measurements: the capacitance of the piezoelectric stacks is measured
- Section ref:sec:test_apa_stroke_measurements: the stroke of each APA is measured
- Section ref:sec:test_apa_spurious_resonances: the "spurious" resonances of the APA are investigated

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

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

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

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

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

** Geometrical Measurements
<<sec:test_apa_geometrical_measurements>>

To measure the flatness of the two mechanical interfaces of the APA300ML, a small measurement bench is installed on top of a metrology granite with very good flatness.

As shown in Figure ref:fig:test_apa_flatness_setup, the APA is fixed to a clamp while a measuring probe[fn:3] is used to measure the height of 4 points on each of the APA300ML interfaces.

From the X-Y-Z coordinates of the measured 8 points, the flatness is estimated by best fitting[fn:4] a plane through all the points.

#+name: fig:test_apa_flatness_setup
#+attr_latex: :width 0.4\linewidth
#+caption: Measurement setup for flatness estimation of the two mechanical interfaces
[[file:figs/test_apa_flatness_setup.png]]

#+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, 9    , -18.5, -30  , 31   , 46.5, 16.5 , 7.5];

apa = {apa1, apa2, apa3, apa4, apa5, apa6, apa7};

%% 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]]';

%% Using fminsearch to find the best fitting plane
apa_d = zeros(1, 7); % Measured flatness of the APA
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 measured flatness, summarized in Table ref:tab:test_apa_flatness_meas, are within the specifications.

#+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:test_apa_flatness_meas
#+caption: Estimated flatness of the APA300ML interfaces
#+attr_latex: :environment tabularx :width 0.3\linewidth :align Xc
#+attr_latex: :center t :booktabs 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 |

** Electrical Measurements
<<sec:test_apa_electrical_measurements>>

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$.

The capacitance of the piezoelectric stacks found in the APA300ML have been measured with the LCR meter[fn:1] shown in Figure ref:fig:test_apa_lcr_meter.
The two stacks used as an actuator and the stack used as a force sensor are measured separately.

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

The measured capacitance are summarized in Table ref:tab:test_apa_capacitance and the average capacitance of one stack is $\approx 5 \mu F$.
However, the measured capacitance of the stacks of "APA 3" is only half of the expected capacitance.
This may indicate a manufacturing defect.

The measured capacitance is found to be lower than the specified one.
This may be due to the fact that the manufacturer measures the capacitance with large signals ($-20\,V$ to $150\,V$) while it was here measured with small signals.

#+name: tab:test_apa_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
|       | *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 |

** Stroke Measurement
<<sec:test_apa_stroke_measurements>>

The goal is here to verify that the stroke of the APA300ML is as specified in the datasheet.
To do so, one side of the APA is fixed to the granite, and a displacement probe[fn:2] is located on the other side as shown in Figure ref:fig:test_apa_stroke_bench.

Then, the voltage across the two actuator stacks is varied from $-20\,V$ to $150\,V$ using a DAC and a voltage amplifier.
Note that the voltage is here slowly varied as the displacement probe has a very low measurement bandwidth (see Figure ref:fig:test_apa_stroke_bench, left).

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

The measured APA displacement is shown as a function of the applied voltage in Figure ref:fig:test_apa_stroke_result, right.

Typical hysteresis curves for piezoelectric stack actuators can be observed.
The measured stroke is approximately $250\,\mu m$ when using only two of the three stacks, which is enough for the current application.
This is even above what is specified as the nominal stroke in the data-sheet ($304\,\mu m$, therefore $\approx 200\,\mu m$ if only two stacks are used).

It is clear from Figure ref:fig:test_apa_stroke_result that "APA 3" has an issue compared to the other units.
This confirms the abnormal electrical measurements made in Section ref:sec:test_apa_electrical_measurements.
This unit was send sent back to Cedrat and a new one was shipped back.
From now on, only the six APA that behave as expected will be used.

#+begin_src matlab
%% Load the measured strokes
load('meas_apa_stroke.mat', 'apa300ml_2s')
#+end_src

#+begin_src matlab :exports none :results none
%% Results of the measured APA stroke
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

% Generated voltage across the two piezoelectric stack actuators to estimate the stroke of the APA300ML
ax1 = nexttile();
plot(apa300ml_2s{1}.t - apa300ml_2s{1}.t(1), 20*apa300ml_2s{1}.V, 'k-')
xlabel('Time [s]'); ylabel('Voltage [V]')
ylim([-20, 160])

% Measured displacement as a function of the applied voltage
ax2 = nexttile();
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, 150]); ylim([-250, 0]);
#+end_src

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

#+name: fig:test_apa_stroke_result
#+caption: Generated voltage across the two piezoelectric stack actuators to estimate the stroke of the APA300ML (left). Measured displacement as a function of the applied voltage (right)
#+RESULTS:
[[file:figs/test_apa_stroke_result.png]]

** TODO Spurious resonances - APA                                  :@philipp:
SCHEDULED: <2024-03-27 Wed>
<<sec:test_apa_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 ref:fig:test_apa_mode_shapes):
- Mode in X-bending at 189Hz
- Mode in Y-bending at 285Hz
- Mode in Z-torsion at 400Hz

#+name: fig:test_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/test_apa_mode_shapes.png]]

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.

In the section ref:sec:spurious_resonances_struts, a similar measurement will be performed directly on the struts.

*** Measurement Setup

The measurement setup is shown in Figure ref:fig:test_apa_meas_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:test_apa_meas_setup_torsion
#+caption: Measurement setup with a Laser Doppler Vibrometer and one instrumental hammer
#+attr_latex: :width 0.7\linewidth
[[file:figs/test_apa_meas_setup_torsion.jpg]]

*** X-Bending Mode

The vibrometer is setup to measure the X-bending motion is shown in Figure ref:fig:test_apa_meas_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:test_apa_meas_setup_X_bending
#+caption: X-Bending measurement setup
#+attr_latex: :width 0.7\linewidth
[[file:figs/test_apa_meas_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]
Nfft = floor(1/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);
#+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, Noverlap, Nfft, 1/Ts);
#+end_src

The result is shown in Figure ref:fig:test_apa_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/test_apa_meas_freq_bending_x.pdf', 'width', 'wide', 'height', 'normal');
#+end_src

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

Then the APA is in the "free-free" condition, this bending mode is foreseen to be at 200Hz (Figure ref:fig:test_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 ref:fig:test_apa_meas_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:test_apa_meas_setup_Y_bending
#+caption: Y-Bending measurement setup
#+attr_latex: :width 0.7\linewidth
[[file:figs/test_apa_meas_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, Noverlap, Nfft, 1/Ts);
#+end_src

The results are shown in Figure ref:fig:test_apa_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/test_apa_meas_freq_bending_y.pdf', 'width', 'wide', 'height', 'normal');
#+end_src

#+name: fig:test_apa_meas_freq_bending_y
#+caption: Obtained FRF for the Y-bending
#+RESULTS:
[[file:figs/test_apa_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 ref:fig:test_apa_meas_setup_torsion_bis.

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

#+name: fig:test_apa_meas_setup_torsion_bis
#+caption: Z-Torsion measurement setup
#+attr_latex: :width 0.7\linewidth
[[file:figs/test_apa_meas_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, Noverlap, Nfft, 1/Ts);
#+end_src

The results are shown in Figure ref:fig:test_apa_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/test_apa_meas_freq_torsion_z.pdf', 'width', 'wide', 'height', 'normal');
#+end_src

#+name: fig:test_apa_meas_freq_torsion_z
#+caption: Obtained FRF for the Z-torsion
#+RESULTS:
[[file:figs/test_apa_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, Noverlap, Nfft, 1/Ts);
#+end_src

The two FRF are compared in Figure ref:fig:test_apa_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/test_apa_meas_freq_torsion_z_comp.pdf', 'width', 'wide', 'height', 'normal');
#+end_src

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

*** Compare
The three measurements are shown in Figure ref:fig:test_apa_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/test_apa_meas_freq_compare.pdf', 'width', 'full', 'height', 'tall');
#+end_src

#+name: fig:test_apa_meas_freq_compare
#+caption: Obtained FRF - Comparison
#+RESULTS:
[[file:figs/test_apa_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 Xcc
#+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?               |

** Conclusion                                                        :ignore:

* Dynamical measurements
:PROPERTIES:
:header-args:matlab+: :tangle matlab/test_apa_2_dynamics.m
:END:
<<sec:test_apa_dynamics>>

** Introduction                                                      :ignore:
After the basic measurements on the APA were performed in Section ref:sec:test_apa_basic_meas, a new test bench is used to better characterize the APA.

This test bench is shown in Figure ref:fig:test_bench_apa and consists of the APA300ML fixed on one end to the fixed granite, and on the other end to the 5kg granite vertically guided with an air bearing.
An encoder is used to measure the relative motion between the two granites (i.e. the displacement of the APA).

#+name: fig:test_bench_apa
#+caption: Test bench used to characterize the APA300ML
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_apa_bench_picture}Picture of the test bench}
#+attr_latex: :options {0.3\textwidth}
#+begin_subfigure
#+attr_latex: :height 8cm
[[file:figs/test_apa_bench_picture.jpg]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_apa_bench_picture_encoder}Zoom on the APA with the encoder}
#+attr_latex: :options {0.69\textwidth}
#+begin_subfigure
#+attr_latex: :height 8cm
[[file:figs/test_apa_bench_picture_encoder.jpg]]
#+end_subfigure
#+end_figure

The bench is schematically shown in Figure ref:fig:test_apa_schematic and the signal used are summarized in Table ref:tab:test_apa_variables.

#+name: fig:test_apa_schematic
#+caption: Schematic of the Test Bench
#+attr_latex: :scale 1
[[file:figs/test_apa_schematic.png]]

#+name: tab:test_apa_variables
#+caption: Variables used during the measurements
#+attr_latex: :environment tabularx :width 0.6\linewidth :align cXc
#+attr_latex: :center t :booktabs t
| *Variable* | *Description*                | *Unit* |
|------------+------------------------------+--------|
| $u$        | Output DAC Voltage           | $V$    |
| $V_a$      | Output Amplifier Voltage     | $V$    |
| $V_s$      | Measured Stack Voltage (ADC) | $V$    |
| $d_e$      | Encoder Measurement          | $m$    |

This bench will be used to:
- measure the dynamics of the APA (from $V_a$ to $d_e$ and $d_a$ in Section ref:ssec:test_apa_meas_frf_disp, and from $V_a$ to $V_s$ in section ref:ssec:test_apa_meas_frf_force)
- estimate the added damping using Integral Force Feedback (Section ref:ssec:test_apa_iff_locus)

These measurements will also be used to tune the model of the APA in Section ref:sec:test_apa_simscape.

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

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

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

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

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

** Hysteresis
<<ssec:test_apa_hysteresis>>

As the payload is vertically guided without friction, the hysteresis of the APA can be estimated from the motion of the payload.

A quasi static sinusoidal excitation $V_a$ with an offset of $65\,V$ (halfway between $-20\,V$ and $150\,V$), and an amplitude varying from $4\,V$ up to $80\,V$.

For each excitation amplitude, the vertical displacement $d_e$ of the mass is measured and displayed as a function of the applied voltage..

#+begin_src matlab
%% Load measured data - hysteresis
apa_hyst = load('frf_data_1_hysteresis.mat', 't', 'u', 'de');

% Initial time set to zero
apa_hyst.t = apa_hyst.t - apa_hyst.t(1);

ampls = [0.1, 0.2, 0.4, 1, 2, 4]; % Excitation voltage amplitudes
#+end_src

The measured displacements as a function of the output voltages are shown in Figure ref:fig:test_apa_meas_hysteresis.
It is interesting to see that the hysteresis is increasing with the excitation amplitude.

#+begin_src matlab :exports none
%% Measured displacement as a function of the output voltage
figure;

hold on;
for i = [6,5,4,2]
    i_lim = apa_hyst.t > i*5-1 & apa_hyst.t < i*5;
    plot(20*apa_hyst.u(i_lim), 1e6*detrend(apa_hyst.de(i_lim), 0), ...
         'DisplayName', sprintf('$V_a = 65 + %.0f \\sin (\\omega t) \\ [V]$', 20*ampls(i)))
end
hold off;
xlabel('Stack Voltage $V_a$ [V]'); ylabel('Displacement $d_e$ [$\mu$m]');
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
xlim([-20, 150]);
ylim([-120, 120]);
#+end_src

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

#+name: fig:test_apa_meas_hysteresis
#+caption: Obtained hysteresis curves (displacement as a function of applied voltage) for multiple excitation amplitudes
#+RESULTS:
[[file:figs/test_apa_meas_hysteresis.png]]

** Axial stiffness
<<ssec:test_apa_stiffness>>

In order to estimate the stiffness of the APA, a weight with known mass $m_a = 6.4\,\text{kg}$ 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 from equation eqref:eq:test_apa_stiffness.

\begin{equation} \label{eq:test_apa_stiffness}
  k_{\text{apa}} = \frac{m_a g}{\Delta d_e}
\end{equation}

#+begin_src matlab
%% Load data for stiffness measurement
apa_nums = [1 2 4 5 6 8];
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

added_mass = 6.4; % Added mass [kg]
#+end_src

The measured displacement $d_e$ as a function of time is shown in Figure ref:fig:test_apa_meas_stiffness_time.
It can be seen that there are some drifts in the measured displacement (probably due to piezoelectric creep) and the that displacement does not come back to the initial position after the mass is removed (probably due to piezoelectric hysteresis).
These two effects induce some uncertainties in the measured stiffness.

#+begin_src matlab :exports none
%% Plot the deflection at a function of time
figure;
hold on;
plot(apa_mass{2}.t(1:100:end)-apa_mass{2}.t(1), 1e6*apa_mass{2}.de(1:100:end), 'k-');
plot([0,20], [-0.4, -0.4], 'k--', 'LineWidth', 0.5)
plot([0,20], [-4.5, -4.5], 'k--', 'LineWidth', 0.5)
plot([0,20], [-37.4, -37.4], 'k--', 'LineWidth', 0.5)
% first stroke for stiffness measurements
anArrow = annotation('doublearrow', 'LineWidth', 0.5);
anArrow.Parent = gca;
anArrow.Position = [2, -0.4, 0, -37];
text(2.5, -20, sprintf('$d_1$'), 'horizontalalignment', 'left');

% second stroke for stiffness measurements
anArrow = annotation('doublearrow', 'LineWidth', 0.5);
anArrow.Parent = gca;
anArrow.Position = [18, -37.4, 0, 32.9];
text(18.5, -20, sprintf('$d_2$'), 'horizontalalignment', 'left');

% annotation('textarrow',[],y,'String',' Growth ','FontSize',13,'Linewidth',2)
hold off;
xlabel('Time [s]'); ylabel('Displacement $d_e$ [$\mu$m]');
#+end_src

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

#+name: fig:test_apa_meas_stiffness_time
#+caption: Measured displacement when adding the mass (at $t \approx 3\,s$) and removing the mass(at $t \approx 13\,s$)
#+RESULTS:
[[file:figs/test_apa_meas_stiffness_time.png]]

The stiffnesses are computed for all the APA from the two displacements $d_1$ and $d_2$ (see Figure ref:fig:test_apa_meas_stiffness_time) leading to two stiffness estimations $k_1$ and $k_2$.
These estimated stiffnesses are summarized in Table ref:tab:test_apa_measured_stiffnesses and are found to be close to the nominal stiffness $k = 1.8\,N/\mu m$ found in the APA300ML manual.

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
  data2orgtable(1e-6*apa_k, cellstr(num2str(apa_nums')), {'APA', '$k_1$', '$k_2$'}, ' %.2f ');
#+end_src

#+name: tab:test_apa_measured_stiffnesses
#+caption: Measured stiffnesses (in $N/\mu m$)
#+attr_latex: :environment tabularx :width 0.2\linewidth :align ccc
#+attr_latex: :center t :booktabs t :float t
#+RESULTS:
| APA | $k_1$ | $k_2$ |
|-----+-------+-------|
|   1 |  1.68 |   1.9 |
|   2 |  1.69 |   1.9 |
|   4 |   1.7 |  1.91 |
|   5 |   1.7 |  1.93 |
|   6 |   1.7 |  1.92 |
|   8 |  1.73 |  1.98 |

The stiffness can also be computed using equation eqref:eq:test_apa_res_freq by knowing the main vertical resonance frequency $\omega_z \approx 94\,\text{Hz}$ (estimated by the dynamical measurements shown in section ref:ssec:test_apa_meas_frf_disp) and the suspended mass $m_{\text{sus}} = 5.7\,\text{kg}$.

\begin{equation} \label{eq:test_apa_res_freq}
\omega_z = \sqrt{\frac{k}{m_{\text{sus}}}}
\end{equation}

The obtain stiffness is $k \approx 2\,N/\mu m$ which is close to the values found in the documentation and by the "static deflection" method.


However, changes in the electrical impedance connected to the piezoelectric stacks impacts the mechanical compliance (or stiffness) of the piezoelectric stack [[cite:&reza06_piezoel_trans_vibrat_contr_dampin chap. 2]].

To estimate this effect, the stiffness of the APA if measured using the "static deflection" method in two cases:
- $k_{\text{os}}$: piezoelectric stacks left unconnected (or connect to the high impedance ADC)
- $k_{\text{sc}}$: piezoelectric stacks short circuited (or connected to the voltage amplifier with small output impedance)

The open-circuit stiffness is estimated at $k_{\text{oc}} \approx 2.3\,N/\mu m$ and the closed-circuit stiffness $k_{\text{sc}} \approx 1.7\,N/\mu m$.

#+begin_src matlab
%% Load Data
add_mass_oc = load('frf_data_1_add_mass_open_circuit.mat',   't', 'de');
add_mass_cc = load('frf_data_1_add_mass_closed_circuit.mat', 't', 'de');

%% 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));

%% Estimation of the stiffness in Open Circuit and Closed-Circuit
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_sc = 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)));

%% Estimated coupling factor
sqrt(1 - apa_k_sc/apa_k_oc)
#+end_src

** Dynamics
<<ssec:test_apa_meas_dynamics>>

In this section, the dynamics of the system from the excitation voltage $u$ to encoder measured displacement $d_e$  and to the force sensor voltage $V_s$ is identified.

#+begin_src matlab
%% Identification using sweep sine (low frequency)
load('frf_data_sweep.mat');
load('frf_data_noise_hf.mat');

%% Sampling Frequency
Ts = 1e-4; % Sampling Time [s]
Fs = 1/Ts; % Sampling Frequency [Hz]

%% "Hanning" windows used for the spectral analysis:
Nfft = floor(2/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

%% Separation of frequencies: low freqs using sweep sine, and high freq using noise
% Only used to have the frequency vector "f"
[~, f] = tfestimate(apa_sweep{1}.u, apa_sweep{1}.de, win, Noverlap, Nfft, 1/Ts);
i_lf = f <= 350;
i_hf = f >  350;

%% FRF estimation of the transfer function from u to de
enc_frf = zeros(length(f), length(apa_nums));
for i = 1:length(apa_nums)
    [frf_lf, ~] = tfestimate(apa_sweep{i}.u,    apa_sweep{i}.de,    win, Noverlap, Nfft, 1/Ts);
    [frf_hf, ~] = tfestimate(apa_noise_hf{i}.u, apa_noise_hf{i}.de, win, Noverlap, Nfft, 1/Ts);
    enc_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
end

%% FRF estimation of the transfer function from u to Vs
iff_frf = zeros(length(f), length(apa_nums));
for i = 1:length(apa_nums)
    [frf_lf, ~] = tfestimate(apa_sweep{i}.u,    apa_sweep{i}.Vs,    win, Noverlap, Nfft, 1/Ts);
    [frf_hf, ~] = tfestimate(apa_noise_hf{i}.u, apa_noise_hf{i}.Vs, win, Noverlap, Nfft, 1/Ts);
    iff_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
end
#+end_src

#+begin_src matlab :tangle no :exports none
%% Save the identified dynamics for further analysis
save('matlab/mat/meas_apa_frf.mat', 'f', 'Ts', 'enc_frf', 'iff_frf', 'apa_nums');
#+end_src

#+begin_src matlab :eval no
%% Save the identified dynamics for further analysis
save('mat/meas_apa_frf.mat', 'f', 'Ts', 'enc_frf', 'iff_frf', 'apa_nums');
#+end_src

The obtained transfer functions for the 6 APA between the excitation voltage $u$ and the encoder displacement $d_e$ are shown in Figure ref:fig:test_apa_frf_encoder.
The obtained transfer functions are close to a mass-spring-damper system.
The following can be observed:
- A "stiffness line" indicating a static gain equal to $\approx -17\,\mu m/V$.
  The minus sign comes from the fact that an increase in voltage stretches the piezoelectric stack that then reduces the height of the APA
- A lightly damped resonance at $95\,\text{Hz}$
- A "mass line" up to $\approx 800\,\text{Hz}$, above which some resonances appear

#+begin_src matlab :exports none
%% Plot the FRF from u to de
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', '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/u$ [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(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/test_apa_frf_encoder.pdf', 'width', 'wide', 'height', 'tall');
#+end_src

#+name: fig:test_apa_frf_encoder
#+caption: Estimated Frequency Response Function from generated voltage $u$ to the encoder displacement $d_e$ for the 6 APA300ML
#+RESULTS:
[[file:figs/test_apa_frf_encoder.png]]

The dynamics from $u$ to the measured voltage across the sensor stack $V_s$ is also identified and shown in Figure ref:fig:test_apa_frf_force.

A lightly damped resonance is observed at $95\,\text{Hz}$ and a lightly damped anti-resonance at $41\,\text{Hz}$.
No additional resonances is present up to at least $2\,\text{kHz}$ indicating at Integral Force Feedback can be applied without stability issues from high frequency flexible modes.

As illustrated by the Root Locus, the poles of the closed-loop system converges to the zeros of the open-loop plant.
Suppose that a controller with a very high gain is implemented such that the voltage $V_s$ across the sensor stack is zero.
In that case, because of the very high controller gain, no stress and strain is present on the sensor stack (and on the actuator stacks are well, as they are both in series).
Such closed-loop system would therefore virtually corresponds to a system for which the piezoelectric stacks have been removed and just the mechanical shell is kept.
From this analysis, the axial stiffness of the shell can be estimated to be $k_{\text{shell}} = 5.7 \cdot (2\pi \cdot 41)^2 = 0.38\,N/\mu m$.
# TODO - Compare with FEM result

Such reasoning can lead to very interesting insight into the system just from an open-loop identification.

#+begin_src matlab :exports none
%% Plot the FRF from u to Vs
figure;
tiledlayout(2, 1, 'TileSpacing', 'Compact', '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/u$ [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/test_apa_frf_force.pdf', 'width', 'wide', 'height', 'tall');
#+end_src

#+name: fig:test_apa_frf_force
#+caption: Estimated Frequency Response Function from generated voltage $u$ to the sensor stack voltage $V_s$ for the 6 APA300ML
#+RESULTS:
[[file:figs/test_apa_frf_force.png]]

All the identified dynamics of the six APA300ML (both when looking at the encoder in Figure ref:fig:test_apa_frf_encoder and at the force sensor in Figure ref:fig:test_apa_frf_force) are almost identical, indicating good manufacturing repeatability for the piezoelectric stacks and the mechanical lever.

** Effect of the resistor on the IFF Plant
<<ssec:test_apa_resistance_sensor_stack>>

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

As explain before, this is done for two reasons:
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 $u$ to $V_s$ with and without this resistor have been measured and are compared in Figure ref:fig:test_apa_effect_resistance.
It is confirmed that the added resistor as the effect of adding an high pass filter with a cut-off frequency of $\approx 0.35\,\text{Hz}$.

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

%% Large Hanning window for good low frequency estimate
Nfft = floor(50/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

%% Compute the transfer functions from Va to Vs
[frf_wo_k, f] = tfestimate(wo_k.Va, wo_k.Vs, win, Noverlap, Nfft, 1/Ts);
[frf_wi_k, ~] = tfestimate(wi_k.Va, wi_k.Vs, win, Noverlap, Nfft, 1/Ts);

%% 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]

G_hpf = 0.6*(s/2*pi*f0)/(1 + s/2*pi*f0);
#+end_src

#+begin_src matlab :exports none
%% Compare the HPF model and the measured FRF
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(frf_wo_k), 'DisplayName', 'Without $R$');
plot(f, abs(frf_wi_k), 'DisplayName', 'With $R$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/u$ [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));
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/test_apa_effect_resistance.pdf', 'width', 'wide', 'height', 'tall');
#+end_src

#+name: fig:test_apa_effect_resistance
#+caption: Transfer function from u to $V_s$ with and without the resistor $R$ in parallel with the piezoelectric stack used as the force sensor
#+RESULTS:
[[file:figs/test_apa_effect_resistance.png]]

** Integral Force Feedback
<<ssec:test_apa_iff_locus>>

This test bench can also be used to estimate the damping added by the implementation of an Integral Force Feedback strategy.

#+begin_src matlab
%% Load identification Data
data = load("2023-03-17_11-28_iff_plant.mat");

%% Spectral Analysis setup
Ts = 1e-4; % Sampling Time [s]
Nfft = floor(5/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

%% Compute the transfer function from applied force to measured rotation
[G_iff, f]  = tfestimate(data.id_plant, data.Vs, win, Noverlap, Nfft, 1/Ts);
#+end_src

First, the transfer function eqref:eq:test_apa_iff_manual_fit is manually tuned to match the identified dynamics from generated voltage $u$ to the measured sensor stack voltage $V_s$ in Section ref:ssec:test_apa_meas_dynamics.

The obtained parameter values are $\omega_{\textsc{hpf}} = 0.4\, \text{Hz}$, $\omega_{z} = 42.7\, \text{Hz}$, $\xi_{z} = 0.4\,\%$, $\omega_{p} = 95.2\, \text{Hz}$, $\xi_{p} = 2\,\%$ and $g_0 = 0.64$.

\begin{equation} \label{eq:test_apa_iff_manual_fit}
G_{\textsc{iff},m}(s) = g_0 \cdot \frac{1 + 2 \xi_z \frac{s}{\omega_z} + \frac{s^2}{\omega_z^2}}{1 + 2 \xi_p \frac{s}{\omega_p} + \frac{s^2}{\omega_p^2}} \cdot \frac{s}{\omega_{\textsc{hpf}} + s}
\end{equation}

The comparison between the identified plant and the manually tuned transfer function is done in Figure ref:fig:test_apa_iff_plant_comp_manual_fit.

#+begin_src matlab
%% Basic manually tuned model
w0z = 2*pi*42.7; % Zero frequency
xiz = 0.004; % Zero damping

w0p = 2*pi*95.2; % Pole frequency
xip = 0.02; % Pole damping

G_iff_model = exp(-2*s*Ts)*0.64*(1 + 2*xiz/w0z*s + s^2/w0z^2)/(1 + 2*xip/w0p*s + s^2/w0p^2)*(s/(s+2*pi*0.4));
#+end_src

#+begin_src matlab :exports none :results none
%% Identified IFF plant and manually tuned model of the plant
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_iff), 'color', colors(2,:), 'DisplayName', 'Identified plant')
plot(f, abs(squeeze(freqresp(G_iff_model, f, 'Hz'))), 'k--', 'DisplayName', 'Manual fit')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/u$ [V/V]'); set(gca, 'XTickLabel',[]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(G_iff), 'color', colors(2,:));
plot(f, 180/pi*angle(squeeze(freqresp(G_iff_model, 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([-90, 180])

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

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

#+name: fig:test_apa_iff_plant_comp_manual_fit
#+caption: Identified IFF plant and manually tuned model of the plant (a time delay of $200\,\mu s$ is added to the model of the plant to better match the identified phase)
#+RESULTS:
[[file:figs/test_apa_iff_plant_comp_manual_fit.png]]

The implemented Integral Force Feedback Controller transfer function is shown in equation eqref:eq:test_apa_Kiff_formula.
It contains an high pass filter (cut-off frequency of $2\,\text{Hz}$) to limit the low frequency gain, a low pass filter to add integral action above $20\,\text{Hz}$, a second low pass filter to add robustness to high frequency resonances and a tunable gain $g$.

\begin{equation} \label{eq:test_apa_Kiff_formula}
K_{\textsc{iff}}(s) = -10 \cdot g \cdot \frac{s}{s + 2\pi \cdot 2} \cdot \frac{1}{1 + 2\pi \cdot 20} \cdot \frac{1}{s + 2\pi\cdot 2000}
\end{equation}

#+begin_src matlab
%% Integral Force Feedback Controller
K_iff = -10*(1/(s + 2*pi*20)) * ... % LPF: provides integral action above 20Hz
            (s/(s + 2*pi*2))  * ... % HPF: limit low frequency gain
            (1/(1 + s/2/pi/2e3));   % LPF: more robust to high frequency resonances
#+end_src

To estimate how the dynamics of the APA changes when the Integral Force Feedback controller is implemented, the test bench shown in Figure ref:fig:test_apa_iff_schematic is used.
The transfer function from the "damped" plant input $u\prime$ to the encoder displacement $d_e$ is identified for several IFF controller gains $g$.

#+name: fig:test_apa_iff_schematic
#+caption: Figure caption
[[file:figs/test_apa_iff_schematic.png]]

#+begin_src matlab
%% Load Data
data = load("2023-03-17_14-10_damped_plants_new.mat");

%% Spectral Analysis setup
Ts = 1e-4; % Sampling Time [s]
Nfft = floor(1/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

%% Get the frequency vector
[~, f]  = tfestimate(data.data(1).id_plant(1:end), data.data(1).dL(1:end), win, Noverlap, Nfft, 1/Ts);

%% Gains used for analysis are between 1 and 50
i_kept = [5:10]

%% Identify the damped plant from u' to de for different IFF gains
G_dL_frf = {zeros(1,length(i_kept))};

for i = 1:length(i_kept)
    [G_dL, ~]  = tfestimate(data.data(i_kept(i)).id_plant(1:end), data.data(i_kept(i)).dL(1:end), win, Noverlap, Nfft, 1/Ts);
    G_dL_frf(i) = {G_dL};
end
#+end_src

The identified dynamics are then fitted by second order transfer functions.
The comparison between the identified damped dynamics and the fitted second order transfer functions is done in Figure ref:fig:test_apa_identified_damped_plants for different gains $g$.
It is clear that large amount of damping is added when the gain is increased and that the frequency of the pole is shifted to lower frequencies.

#+begin_src matlab
%% Fit the data with 2nd order transfer function using vectfit3
opts = struct();

opts.stable = 1;    % Enforce stable poles
opts.asymp = 1;     % Force D matrix to be null
opts.relax = 1;     % Use vector fitting with relaxed non-triviality constraint
opts.skip_pole = 0; % Do NOT skip pole identification
opts.skip_res = 0;  % Do NOT skip identification of residues (C,D,E)
opts.cmplx_ss = 0;  % Create real state space model with block diagonal A

opts.spy1 = 0;      % No plotting for first stage of vector fitting
opts.spy2 = 0;      % Create magnitude plot for fitting of f(s)


Niter = 100; % Number of iteration.
N = 2; % Order of approximation
poles = [-25 - 1i*60, -25 + 1i*60]; % First get for the pole location

G_dL_id  = {zeros(1,length(i_kept))};

% Identification just between two frequencies
f_keep = (f>20 & f<200);

for i = 1:length(i_kept)
    %% Estimate resonance frequency and damping
    for iter = 1:Niter
        [G_est, poles, ~, frf_est] = vectfit3(G_dL_frf{i}(f_keep).', 1i*2*pi*f(f_keep)', poles, ones(size(f(f_keep)))', opts);
    end
    G_dL_id(i) = {ss(G_est.A, G_est.B, G_est.C, G_est.D)};
end
#+end_src

#+begin_src matlab :exports none :results none
%% Identified dynamics from u' to de for different IFF gains
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
for i = 1:length(i_kept)
    plot(f, abs(G_dL_frf{i}), 'color', [colors(i,:), 1], 'DisplayName', sprintf('g = %.0f', data.gains(i_kept(i))))
    plot(f, abs(squeeze(freqresp(G_dL_id{i}, f, 'Hz'))), '--', 'color', [colors(i,:), 1], 'HandleVisibility', 'off')
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude $d_L/V_a$ [m/V]');
xlim([10, 1e3]);
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
#+end_src

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

#+name: fig:test_apa_identified_damped_plants
#+caption: Identified dynamics (solid lines) and fitted transfer functions (dashed lines) from $u\prime$ to $d_e$ for different IFF gains
#+RESULTS:
[[file:figs/test_apa_identified_damped_plants.png]]

The evolution of the pole in the complex plane as a function of the controller gain $g$ (i.e. the "root locus") is computed:
- using the IFF plant model eqref:eq:test_apa_iff_manual_fit and the implemented controller eqref:eq:test_apa_Kiff_formula
- from the fitted transfer functions of the damped plants experimentally identified for several controller gains

The two obtained root loci are compared in Figure ref:fig:test_apa_iff_root_locus and are in good agreement considering that the damped plants were only fitted using a second order transfer function.

#+begin_src matlab :exports none :results none
%% Root Locus of the APA300ML with Integral Force Feedback
% Comparison between the computed root locus from the plant model and the root locus estimated from the damped plant pole identification
figure;
gains = logspace(-1, 3, 1000);

figure;
hold on;
G_iff_poles = pole(G_iff_model);
i = imag(G_iff_poles) > 100; % Only keep relevant poles
plot(real(G_iff_poles(i)),  imag(G_iff_poles(i)),  'kx', ...
    'DisplayName', '$g = 0$');
G_iff_zeros = tzero(G_iff_model);
i = imag(G_iff_zeros) > 100; % Only keep relevant zeros
plot(real(G_iff_zeros(i)), imag(G_iff_zeros(i)), 'ko', ...
    'HandleVisibility', 'off');

for g = gains
    clpoles = pole(feedback(G_iff_model, g*K_iff, 1));
    i = imag(clpoles) > 100; % Only keep relevant poles
    plot(real(clpoles(i)), imag(clpoles(i)), 'k.', ...
        'HandleVisibility', 'off');
end

for i = 1:length(i_kept)
    plot(real(pole(G_dL_id{i})), imag(pole(G_dL_id{i})), 'x', 'color', [colors(i,:), 1], 'DisplayName', sprintf('g = %1.f', data.gains(i_kept(i))));
end
ylim([0, 700]);
xlim([-600,100]);
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/test_apa_iff_root_locus.pdf', 'width', 'wide', 'height', 'tall');
#+end_src

#+name: fig:test_apa_iff_root_locus
#+caption: Root Locus of the APA300ML with Integral Force Feedback - Comparison between the computed root locus from the plant model (black line) and the root locus estimated from the damped plant pole identification (colorful crosses)
#+RESULTS:
[[file:figs/test_apa_iff_root_locus.png]]
** Conclusion

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


* Simscape Model
:PROPERTIES:
:header-args:matlab+: :tangle matlab/test_apa_3_simscape.m
:END:
<<sec:test_apa_simscape>>
** Introduction                                                      :ignore:
In this section, a simscape model (Figure ref:fig:test_apa_bench_model) 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 ref: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 ref:sec:simscape_bench_apa_id_constants)
- "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$
2. Tune the model of the APA to match the measured dynamics (Section ref:sec:simscape_bench_apa_tune_2dof_model)

#+name: fig:test_apa_bench_model
#+caption: Screenshot of the Simscape model
#+attr_latex: :width 0.5\linewidth
[[file:figs/test_apa_bench_model.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 :noweb yes
<<m-init-path>>
#+end_src

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

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

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

#+begin_src matlab :noweb yes
<<m-init-other>>
#+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$

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

%% Linearization options
opts = linearizeOptions;
opts.SampleTime = 0;

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

The obtain dynamics are shown in Figure ref:fig:apa_model_bench_bode_vs and ref: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 ref: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', 'Compact', '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', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(Ga('de', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Encoder')
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'))))
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 ref:fig:apa_act_constant_comp and the one from $V_a$ to $V_s$ are compared in Figure ref: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', 'Compact', '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', 'Compact', '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_src matlab :exports none
%% Compare the FRF and identified dynamics from Va to Vs and da
colors = colororder;

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

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(enc_frf(:, 1)), 'color', [0,0,0,0.2], ...
     'DisplayName', 'FRF');
for i = 2:length(apa_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('da', 'Va'), freqs, 'Hz'))), '--', ...
     'DisplayName', 'Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_a/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
legend('location', 'southwest');

ax1b = nexttile([2,1]);
hold on;
plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2], ...
     'DisplayName', 'FRF');
for i = 1:length(apa_nums)
    plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2], ...
         'HandleVisibility', 'off');
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))), '--', ...
     'DisplayName', 'Model')
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');

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('da', '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/apa_comp_model_frf.pdf', 'width', 1500, 'height', 'tall');
#+end_src

#+name: fig:apa_comp_model_frf
#+caption:
#+RESULTS:
[[file:figs/apa_comp_model_frf.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 ref:fig:apa_act_constant_comp_flex and ref: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', 'Compact', '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', 'Compact', '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 ref: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 ref: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', 'Compact', '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

* TODO Compare with the FEM/Simscape Model                          :noexport:
** 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 ref:fig:test_apa_ansys.
The remote point used are also shown in this figure.

#+name: fig:test_apa_ansys
#+caption: Ansys FEM of the APA300ML
[[file:figs/test_apa_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 ref:fig:test_apa_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 ref: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', 'Compact', '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 ref: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 ref: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', 'Compact', '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 ref: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 ref: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 ref:fig:apa300ml_comp_simpler_model.

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

  figure;
  tiledlayout(2, 3, 'TileSpacing', 'Compact', '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 ref: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', 'Compact', '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 ref:fig:piezo_amplified_iff_plant.

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

  figure;
  tiledlayout(3, 1, 'TileSpacing', 'Compact', '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 ref: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', 'Compact', '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 ref:fig:piezo_amplified_iff_comp.

It is the expected behavior as shown in the Figure ref: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', 'Compact', '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]]

* Conclusion
<<sec:test_apa_conclusion>>

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

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

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

** Initialize Simscape
#+NAME: m-init-path-simscape
#+BEGIN_SRC matlab
addpath('./matlab/STEPS/'); % Path for Simscape Model

%% Open Simscape Model
mdl = 'test_apa_simscape'; % Name of the Simulink File
open(mdl); % Open Simscape Model
#+END_SRC

#+NAME: m-init-path-simscape-tangle
#+BEGIN_SRC matlab
addpath('./STEPS/'); % Path for Simscape Model

%% Open Simscape Model
mdl = 'test_apa_simscape'; % Name of the Simulink File
open(mdl); % Open Simscape Model
#+END_SRC

** Initialize other elements
#+NAME: m-init-other
#+BEGIN_SRC matlab
%% Colors for the figures
colors = colororder;
#+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]
    args.d_align_bot (3,1) double {mustBeNumeric} = zeros(3,1) % [m]
    args.d_align_top (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;
    actuator.d_align_bot = args.d_align_bot;
    actuator.d_align_top = args.d_align_top;
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

* Footnotes
[fn:4]The Matlab =fminsearch= command is used to fit the plane
[fn:3]Heidenhain MT25, specified accuracy of $0.5\,\mu m$
[fn:2]Millimar 1318 probe, specified linearity better than $1\,\mu m$
[fn:1]LCR-819 from Gwinstek, specified accuracy of $0.05\%$, measured frequency is set at $1\,\text{kHz}$