phd-nass-rotating-3dof-model/nass-rotating-3dof-model.org

#+TITLE: Nano Active Stabilization System - Effect of rotation
:DRAWER:
#+LANGUAGE: en
#+EMAIL: dehaeze.thomas@gmail.com
#+AUTHOR: Dehaeze Thomas

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

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

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

#+LATEX_CLASS: scrreprt
#+LATEX_CLASS_OPTIONS: [a4paper, 10pt, DIV=12, parskip=full, bibliography=totoc]
#+LATEX_HEADER: \input{preamble.tex}
#+LATEX_HEADER_EXTRA: \input{preamble_extra.tex}
#+LATEX_HEADER_EXTRA: \bibliography{nass-rotating-3dof-model.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="./nass-rotating-3dof-model.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)

;; 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 is =rotating=

* Glossary and Acronyms - Tables                                      :ignore:

#+name: glossary
| label | name                    | description                                 |
|-------+-------------------------+---------------------------------------------|
| psdx  | \ensuremath{\Phi_{x}}   | Power spectral density of signal $x$        |
| asdx  | \ensuremath{\Gamma_{x}} | Amplitude spectral density of signal $x$    |
| cpsx  | \ensuremath{\Phi_{x}}   | Cumulative Power Spectrum of signal $x$     |
| casx  | \ensuremath{\Gamma_{x}} | Cumulative Amplitude Spectrum of signal $x$ |

#+name: acronyms
| key    | abbreviation | full form                                      |
|--------+--------------+------------------------------------------------|
| haclac | HAC-LAC      | High Authority Control - Low Authority Control |
| hac    | HAC          | High Authority Control                         |
| lac    | LAC          | Low Authority Control                          |
| nass   | NASS         | Nano Active Stabilization System               |
| asd    | ASD          | Amplitude Spectral Density                     |
| psd    | PSD          | Power Spectral Density                         |
| cps    | CPS          | Cumulative Power Spectrum                      |
| cas    | CAS          | Cumulative Amplitude Spectrum                  |
| frf    | FRF          | Frequency Response Function                    |
| iff    | IFF          | Integral Force Feedback                        |
| rdc    | RDC          | Relative Damping Control                       |
| drga   | DRGA         | Dynamical Relative Gain Array                  |

* Introduction                                                        :ignore:

An important aspect of the acrfull:nass is that the nano-hexapod is continuously rotating around a vertical axis while the external metrology is not.
Such rotation induces gyroscopic effects that may impact the system dynamics and obtained performance.
To study these effects, a model of a rotating suspended platform is first presented (Section ref:sec:rotating_system_description)
This model is simple enough to be able to derive its dynamics analytically and to well understand its behavior, while still allowing to capture the important physical effects in play.

acrfull:iff is then applied to the rotating platform, and it is shown that the unconditional stability of acrshort:iff is lost due to gyroscopic effects induced by the rotation (Section ref:sec:rotating_iff_pure_int).
Two modifications of the Integral Force Feedback are then proposed.
The first one consists of adding an high pass filter to the acrshort:iff controller (Section ref:sec:rotating_iff_pseudo_int).
It is shown that the acrshort:iff controller is stable for some values of the gain, and that damping can be added to the suspension modes.
Optimal high pass filter cut-off frequency is computed.
The second modification consists of adding a stiffness in parallel to the force sensors (Section ref:sec:rotating_iff_parallel_stiffness).
Under a certain condition, the unconditional stability of the the IFF controller is regained.
Optimal parallel stiffness is then computed.
This study of adapting acrshort:iff for the damping of rotating platforms was the subject of two published papers [[cite:&dehaeze20_activ_dampin_rotat_platf_integ_force_feedb;&dehaeze21_activ_dampin_rotat_platf_using]].

It is then shown that acrfull:rdc is less affected by gyroscopic effects (Section ref:sec:rotating_relative_damp_control).
Once the optimal control parameters for the three tested active damping techniques are obtained, they are compared in terms of achievable damping, obtained damped plant and closed-loop compliance and transmissibility (Section ref:sec:rotating_comp_act_damp).

The previous analysis is applied on three considered nano-hexapod stiffnesses ($k_n = 0.01\,N/\mu m$, $k_n = 1\,N/\mu m$ and $k_n = 100\,N/\mu m$) and optimal active damping controller are obtained in each case (Section ref:sec:rotating_nano_hexapod).
Up until this section, the study was performed on a very simplistic model that just captures the rotation aspect and the model parameters were not tuned to corresponds to the NASS.
In the last section (Section ref:sec:rotating_nass), a model of the micro-station is added below the suspended platform (i.e. the nano-hexapod) with a rotating spindle and parameters tuned to match the NASS dynamics.
The goal is to determine if the rotation imposes performance limitation for the NASS.

#+name: fig:rotating_overview
#+caption: Overview of this report on rotating effects
#+attr_latex: :width \linewidth
[[file:figs/rotating_overview.png]]

* System Description and Analysis
:PROPERTIES:
:header-args:matlab+: :tangle matlab/rotating_1_system_description.m
:END:
<<sec:rotating_system_description>>

** Introduction                                                      :ignore:
The studied system consists of a 2 degree of freedom translation stage on top of a rotating stage (Figure ref:fig:rotating_3dof_model_schematic).
The rotating stage is supposed to be ideal, meaning it induces a perfect rotation $\theta(t) = \Omega t$ where $\Omega$ is the rotational speed in $\si{\radian\per\s}$.
The suspended platform consists of two orthogonal actuators each represented by three elements in parallel: a spring with a stiffness $k$ in $\si{\newton\per\meter}$, a dashpot with a damping coefficient $c$ in $\si{\newton\per(\meter\per\second)}$ and an ideal force source $F_u, F_v$.
A payload with a mass $m$ in $\si{\kilo\gram}$, is mounted on the (rotating) suspended platform.
Two reference frames are used: an /inertial/ frame $(\vec{i}_x, \vec{i}_y, \vec{i}_z)$ and a /uniform rotating/ frame $(\vec{i}_u, \vec{i}_v, \vec{i}_w)$ rigidly fixed on top of the rotating stage with $\vec{i}_w$ aligned with the rotation axis.
The position of the payload is represented by $(d_u, d_v, 0)$ expressed in the rotating frame.
After the dynamics of this system is studied, the objective will be to damp the two suspension modes of the payload while the rotating stage performs a constant rotation.

#+begin_src latex :file rotating_3dof_model_schematic.pdf
\begin{tikzpicture}
  % Angle
  \def\thetau{25}

  % Rotational Stage
  \draw[fill=black!60!white] (0, 0) circle (4.3);
  \draw[fill=black!40!white] (0, 0) circle (3.8);

  % Label
  \node[anchor=north west, rotate=\thetau] at (-2.5, 2.5) {\small Rotating Stage};

  % Rotating Scope
  \begin{scope}[rotate=\thetau]
    % Rotating Frame
    \draw[fill=black!20!white] (-2.6, -2.6) rectangle (2.6, 2.6);
    % Label
    \node[anchor=north west, rotate=\thetau] at (-2.6, 2.6) {\small Suspended Platform};

    % Mass
    \draw[fill=white] (-1, -1) rectangle (1, 1);
    % Label
    \node[anchor=south west, rotate=\thetau] at (-1, -1) {\small Payload};

    % Attached Points
    \node[] at (-1, 0){$\bullet$};
    \draw[] (-1, 0) -- ++(-0.2, 0) coordinate(cu);
    \draw[] ($(cu) + (0, -0.8)$) coordinate(actu) -- ($(cu) + (0, 0.8)$) coordinate(ku);
    \node[] at (0, -1){$\bullet$};
    \draw[] (0, -1) -- ++(0, -0.2) coordinate(cv);
    \draw[] ($(cv) + (-0.8, 0)$)coordinate(kv) -- ($(cv) + (0.8, 0)$) coordinate(actv);

    % Spring and Actuator for U
    \draw[actuator={0.6}{0.2}{black}] (actu) -- node[above=0.1, rotate=\thetau]{$F_u$} (actu-|-2.6,0);
    \draw[spring=0.2] (ku) -- node[above=0.1, rotate=\thetau]{$k$} (ku-|-2.6,0);
    \draw[damper={black}{8}{8}] (cu) -- node[above left=0.2 and -0.1, rotate=\thetau]{$c$} (cu-|-2.6,0);

    \draw[actuator={0.6}{0.2}{black}] (actv) -- node[left, rotate=\thetau]{$F_v$} (actv|-0,-2.6);
    \draw[spring=0.2] (kv) -- node[left, rotate=\thetau]{$k$} (kv|-0,-2.6);
    \draw[damper={black}{8}{8}] (cv) -- node[left=0.1, rotate=\thetau]{$c$} (cv|-0,-2.6);
  \end{scope}

  % Inertial Frame
  \draw[->] (-4, -4) -- ++(2, 0) node[below]{$\vec{i}_x$};
  \draw[->] (-4, -4) -- ++(0, 2) node[left]{$\vec{i}_y$};
  \draw[fill, color=black] (-4, -4) circle (0.06);
  \node[draw, circle, inner sep=0pt, minimum size=0.3cm, label=left:$\vec{i}_z$] at (-4, -4){};

  \draw[->] (0, 0) node[above left, rotate=\thetau]{$\vec{i}_w$} -- ++(\thetau:2) node[above, rotate=\thetau]{$\vec{i}_u$};
  \draw[->] (0, 0) -- ++(\thetau+90:2) node[left, rotate=\thetau]{$\vec{i}_v$};
  \draw[fill, color=black] (0,0) circle (0.06);
  \node[draw, circle, inner sep=0pt, minimum size=0.3cm] at (0, 0){};
  \draw[dashed] (0, 0) -- ++(2, 0);
  \draw[] (1.5, 0) arc (0:\thetau:1.5) node[midway, right]{$\theta$};

  \draw[->] (3.5, 0) arc (0:40:3.5) node[midway, left]{$\Omega$};
\end{tikzpicture}
#+end_src

#+name: fig:rotating_3dof_model_schematic
#+caption: Schematic of the studied system
#+attr_latex: :scale 0.8
#+RESULTS:
[[file:figs/rotating_3dof_model_schematic.png]]

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

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

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

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

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

#+begin_src matlab
%% Simscape model name
mdl = 'rotating_model';
#+end_src

** Equations of motion
To obtain the equations of motion for the system represented in Figure ref:fig:rotating_3dof_model_schematic, the Lagrangian equation eqref:eq:rotating_lagrangian_equations is used.
$L = T - V$ is the Lagrangian, $T$ the kinetic coenergy, $V$ the potential energy, $D$ the dissipation function, and $Q_i$ the generalized force associated with the generalized variable $\begin{bmatrix}q_1 & q_2\end{bmatrix} = \begin{bmatrix}d_u & d_v\end{bmatrix}$.
These terms are derived in eqref:eq:rotating_energy_functions_lagrange.
Note that the equation of motion corresponding to the constant rotation along $\vec{i}_w$ is disregarded as this motion is considered to be imposed by the rotation stage.

\begin{equation}\label{eq:rotating_lagrangian_equations}
  \frac{d}{dt} \left( \frac{\partial L}{\partial \dot{q}_i} \right) + \frac{\partial D}{\partial \dot{q}_i} - \frac{\partial L}{\partial q_i} = Q_i
\end{equation}

\begin{equation} \label{eq:rotating_energy_functions_lagrange}
  \begin{aligned}
    T &= \frac{1}{2} m \left( ( \dot{d}_u - \Omega d_v )^2 + ( \dot{d}_v + \Omega d_u )^2 \right), \quad Q_1 = F_u, \quad Q_2 = F_v, \\
    V &= \frac{1}{2} k \big( {d_u}^2 + {d_v}^2 \big), \quad D = \frac{1}{2} c \big( \dot{d}_u{}^2 + \dot{d}_v{}^2 \big)
  \end{aligned}
\end{equation}

Substituting equations eqref:eq:rotating_energy_functions_lagrange into equation eqref:eq:rotating_lagrangian_equations for both generalized coordinates gives two coupled differential equations eqref:eq:rotating_eom_coupled_1 and eqref:eq:rotating_eom_coupled_2.

\begin{subequations} \label{eq:rotating_eom_coupled}
  \begin{align}
    m \ddot{d}_u + c \dot{d}_u + ( k - m \Omega^2 ) d_u &= F_u + 2 m \Omega \dot{d}_v \label{eq:rotating_eom_coupled_1} \\
    m \ddot{d}_v + c \dot{d}_v + ( k \underbrace{-\,m \Omega^2}_{\text{Centrif.}} ) d_v &= F_v \underbrace{-\,2 m \Omega \dot{d}_u}_{\text{Coriolis}} \label{eq:rotating_eom_coupled_2}
  \end{align}
\end{subequations}

The uniform rotation of the system induces two /gyroscopic effects/ as shown in equation eqref:eq:rotating_eom_coupled:
- /Centrifugal forces/: that can been seen as an added /negative stiffness/ $- m \Omega^2$ along $\vec{i}_u$ and $\vec{i}_v$
- /Coriolis forces/: that adds /coupling/ between the two orthogonal directions.
One can verify that without rotation ($\Omega = 0$) the system becomes equivalent to two /uncoupled/ one degree of freedom mass-spring-damper systems.

** Transfer Functions in the Laplace domain
To study the dynamics of the system, the two differential equations of motions eqref:eq:rotating_eom_coupled are converted into the Laplace domain and the $2 \times 2$ transfer function matrix $\mathbf{G}_d$ from $\begin{bmatrix}F_u & F_v\end{bmatrix}$ to $\begin{bmatrix}d_u & d_v\end{bmatrix}$ in equation eqref:eq:rotating_Gd_mimo_tf is obtained.
The four transfer functions in $\mathbf{G}_d$ are shown in equation eqref:eq:rotating_Gd_indiv_el.

\begin{equation}\label{eq:rotating_Gd_mimo_tf}
  \begin{bmatrix} d_u \\ d_v \end{bmatrix} = \mathbf{G}_d \begin{bmatrix} F_u \\ F_v \end{bmatrix}
\end{equation}

\begin{subequations}\label{eq:rotating_Gd_indiv_el}
\begin{align}
  \mathbf{G}_{d}(1,1) &=  \mathbf{G}_{d}(2,2) = \frac{ms^2 + cs + k - m \Omega^2}{\left( m s^2 + cs + k - m \Omega^2 \right)^2 + \left( 2 m \Omega s \right)^2} \\
  \mathbf{G}_{d}(1,2) &= -\mathbf{G}_{d}(2,1) = \frac{2 m \Omega s}{\left( m s^2 + cs + k - m \Omega^2 \right)^2 + \left( 2 m \Omega s \right)^2}
\end{align}
\end{subequations}

To simplify the analysis, the undamped natural frequency $\omega_0$ and the damping ratio $\xi$ defined in eqref:eq:rotating_xi_and_omega are used instead.
The elements of transfer function matrix $\mathbf{G}_d$ are now described by equation eqref:eq:rotating_Gd_w0_xi_k.
\begin{equation} \label{eq:rotating_xi_and_omega}
  \omega_0 = \sqrt{\frac{k}{m}} \text{ in } \si{\radian\per\second}, \quad \xi = \frac{c}{2 \sqrt{k m}}
\end{equation}

\begin{subequations} \label{eq:rotating_Gd_w0_xi_k}
  \begin{align}
    \mathbf{G}_{d}(1,1) &= \frac{\frac{1}{k} \left( \frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1 - \frac{{\Omega}^2}{{\omega_0}^2} \right)}{\left( \frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1 - \frac{{\Omega}^2}{{\omega_0}^2} \right)^2 + \left( 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \right)^2}  \\
    \mathbf{G}_{d}(1,2) &= \frac{\frac{1}{k} \left( 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \right)}{\left( \frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1 - \frac{{\Omega}^2}{{\omega_0}^2} \right)^2 + \left( 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \right)^2}
  \end{align}
\end{subequations}

** System Poles: Campbell Diagram
The poles of $\mathbf{G}_d$ are the complex solutions $p$ of equation eqref:eq:rotating_poles (i.e. the roots of its denominator).

\begin{equation}\label{eq:rotating_poles}
  \left( \frac{p^2}{{\omega_0}^2} + 2 \xi \frac{p}{\omega_0} + 1 - \frac{{\Omega}^2}{{\omega_0}^2} \right)^2 + \left( 2 \frac{\Omega}{\omega_0} \frac{p}{\omega_0} \right)^2 = 0
\end{equation}

Supposing small damping ($\xi \ll 1$), two pairs of complex conjugate poles $[p_{+}, p_{-}]$ are obtained as shown in equation eqref:eq:rotating_pole_values.

\begin{subequations} \label{eq:rotating_pole_values}
  \begin{align}
    p_{+} &= - \xi \omega_0 \left( 1 + \frac{\Omega}{\omega_0} \right) \pm j \omega_0 \left( 1 + \frac{\Omega}{\omega_0} \right) \\
    p_{-} &= - \xi \omega_0 \left( 1 - \frac{\Omega}{\omega_0} \right) \pm j \omega_0 \left( 1 - \frac{\Omega}{\omega_0} \right)
  \end{align}
\end{subequations}

#+begin_src matlab
%% Model parameters for first analysis
kn = 1;    % Actuator Stiffness [N/m]
mn = 1;    % Payload Mass [kg]
cn = 0.05; % Actuator Damping [N/(m/s)]

xin = cn/(2*sqrt(kn*mn)); % Modal Damping [-]
w0n = sqrt(kn/mn);       % Natural Frequency [rad/s]

%% Computation of the poles as a function of the rotating velocity
Wzs = linspace(0, 2, 51); % Vector of rotation speeds [rad/s]

p_ws = zeros(4, length(Wzs));

for i = 1:length(Wzs)
    Wz = Wzs(i);

    pole_G = pole(1/(((s^2)/(w0n^2) + 2*xin*s/w0n + 1 - (Wz^2)/(w0n^2))^2 + (2*Wz*s/(w0n^2))^2));
    [~, i_sort] = sort(imag(pole_G));
    p_ws(:, i) = pole_G(i_sort);
end

clear pole_G;
#+end_src

The real and complex parts of these two pairs of complex conjugate poles are represented in Figure ref:fig:rotating_campbell_diagram as a function of the rotational speed $\Omega$.
As the rotational speed increases, $p_{+}$ goes to higher frequencies and $p_{-}$ goes to lower frequencies (Figure ref:fig:rotating_campbell_diagram_imag).
The system becomes unstable for $\Omega > \omega_0$ as the real part of $p_{-}$ is positive (Figure ref:fig:rotating_campbell_diagram_real).
Physically, the negative stiffness term $-m\Omega^2$ induced by centrifugal forces exceeds the spring stiffness $k$.

#+begin_src matlab :results none
%% Campbell diagram - Real and Imaginary parts of the poles as a function of the rotating velocity
figure;
hold on;
plot(Wzs, real(p_ws(1, :)), '-', 'color', colors(1,:), 'DisplayName', '$p_{+}$')
plot(Wzs, real(p_ws(4, :)), '-', 'color', colors(1,:), 'HandleVisibility', 'off')
plot(Wzs, real(p_ws(2, :)), '-', 'color', colors(2,:), 'DisplayName', '$p_{-}$')
plot(Wzs, real(p_ws(3, :)), '-', 'color', colors(2,:), 'HandleVisibility', 'off')
plot(Wzs, zeros(size(Wzs)), 'k--', 'HandleVisibility', 'off')
hold off;
xlabel('Rotational Speed $\Omega$'); ylabel('Real Part');
leg = legend('location', 'northwest', 'FontSize', 8);
leg.ItemTokenSize(1) = 8;
xlim([0, 2*w0n]);
xticks([0,w0n/2,w0n,3/2*w0n,2*w0n])
xticklabels({'$0$', '', '$\omega_0$', '', '$2 \omega_0$'})
ylim([-3*xin, 3*xin]);
yticks([-3*xin, -2*xin, -xin, 0, xin, 2*xin, 3*xin])
yticklabels({'', '', '$-\xi\omega_0$', '$0$', ''})
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/rotating_campbell_diagram_real.pdf', 'width', 450, 'height', 400);
#+end_src

#+begin_src matlab :results none
figure
hold on;
plot(Wzs, imag(p_ws(1, :)), '-', 'color', colors(1,:))
plot(Wzs, imag(p_ws(4, :)), '-', 'color', colors(1,:))
plot(Wzs, imag(p_ws(2, :)), '-', 'color', colors(2,:))
plot(Wzs, imag(p_ws(3, :)), '-', 'color', colors(2,:))
plot(Wzs, zeros(size(Wzs)), 'k--')
hold off;
xlabel('Rotational Speed $\Omega$'); ylabel('Imaginary Part');
xlim([0, 2*w0n]);
xticks([0,w0n/2,w0n,3/2*w0n,2*w0n])
xticklabels({'$0$', '', '$\omega_0$', '', '$2 \omega_0$'})
ylim([-3*w0n, 3*w0n]);
yticks([-3*w0n, -2*w0n, -w0n, 0, w0n, 2*w0n, 3*w0n])
yticklabels({'', '', '$-\omega_0$', '$0$', '$\omega_0$', '', ''})
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/rotating_campbell_diagram_imag.pdf', 'width', 450, 'height', 400);
#+end_src

#+name: fig:rotating_campbell_diagram
#+caption: Campbell diagram - Real (\subref{fig:rotating_campbell_diagram_real}) and Imaginary (\subref{fig:rotating_campbell_diagram_imag}) parts of the poles as a function of the rotating velocity $\Omega$.
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:rotating_campbell_diagram_real}Real part}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :scale 1
[[file:figs/rotating_campbell_diagram_real.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_campbell_diagram_imag}Imaginary part}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :scale 1
[[file:figs/rotating_campbell_diagram_imag.png]]
#+end_subfigure
#+end_figure

** Identify Generic Dynamics                                       :noexport:
#+begin_src matlab
%% Identify the dynamics for several rotating velocities
% Sample
ms = 0.5; % Sample mass [kg]

% Tuv Stage
kn = 1; % Stiffness [N/m]
mn = 0.5; % Tuv mass [kg]
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]

% General Configuration
model_config = struct();
model_config.controller = "open_loop"; % Default: Open-Loop
model_config.Tuv_type   = "normal";    % Default: 2DoF stage

% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/controller'],        1, 'openinput');  io_i = io_i + 1; % [Fu, Fv]
io(io_i) = linio([mdl, '/fd'],                1, 'openinput');  io_i = io_i + 1; % [Fdu, Fdv]
io(io_i) = linio([mdl, '/xf'],                1, 'openinput');  io_i = io_i + 1; % [Dfx, Dfy]
io(io_i) = linio([mdl, '/translation_stage'], 1, 'openoutput'); io_i = io_i + 1; % [Fmu, Fmv]
io(io_i) = linio([mdl, '/translation_stage'], 2, 'openoutput'); io_i = io_i + 1; % [Du, Dv]
io(io_i) = linio([mdl, '/ext_metrology'],     1, 'openoutput'); io_i = io_i + 1; % [Dx, Dy]

% Tested rotating velocities [rad/s]
Wzs = [0, 0.1, 0.2, 0.7, 1.2]; % Vector of rotation speeds [rad/s]

Gs  = {zeros(2, 2, length(Wzs))}; % Direct terms

for i = 1:length(Wzs)
    Wz = Wzs(i);

    %% Linearize the model
    G = linearize(mdl, io, 0);

    %% Input/Output definition
    G.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
    G.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

    Gs{:,:,i} = G;
end
#+end_src

#+begin_src matlab :tangle no
% Save All Identified Plants
save('./matlab/mat/rotating_generic_plants.mat', 'Gs', 'Wzs');
#+end_src

#+begin_src matlab :eval no
% Save All Identified Plants
save('./mat/rotating_generic_plants.mat', 'Gs', 'Wzs');
#+end_src

** System Dynamics: Effect of rotation
The system dynamics from actuator forces $[F_u, F_v]$ to the relative motion $[d_u, d_v]$ is identified for several rotating velocities.
Looking at the transfer function matrix $\mathbf{G}_d$ in equation eqref:eq:rotating_Gd_w0_xi_k, one can see that the two diagonal (direct) terms are equal and that the two off-diagonal (coupling) terms are opposite.
The bode plot of these two terms are shown in Figure ref:fig:rotating_bode_plot for several rotational speeds $\Omega$.
These plots confirm the expected behavior: the frequency of the two pairs of complex conjugate poles are further separated as $\Omega$ increases.
For $\Omega > \omega_0$, the low frequency pair of complex conjugate poles $p_{-}$ becomes unstable (shown be the 180 degrees phase lead instead of phase lag).

#+begin_src matlab :results none
%% Bode plot of the direct and coupling terms for several rotating velocities
freqs = logspace(-1, 1, 1000);

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

% Magnitude
ax1 = nexttile([2, 1]);
hold on;
for i = 1:length(Wzs)
    plot(freqs, abs(squeeze(freqresp(Gs{i}('du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:))
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
ylim([1e-2, 1e2]);
yticks([1e-2,1e-1,1,1e1,1e2])
yticklabels({'$0.01/k$', '$0.1/k$', '$1/k$', '$10/k$', '$100/k$'})

ax2 = nexttile;
hold on;
for i = 1:length(Wzs)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:))
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([-180 180]);
xticks([1e-1,1,1e1])
xticklabels({'$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})

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

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

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

ax1 = nexttile([2, 1]);
hold on;
for i = 1:length(Wzs)
    plot(freqs, abs(squeeze(freqresp(Gs{i}('dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:), ...
         'DisplayName', sprintf('$\\Omega = %.1f \\omega_0$', Wzs(i)))
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
ldg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
ldg.ItemTokenSize = [10, 1];
ylim([1e-2, 1e2]);
yticks([1e-2,1e-1,1,1e1,1e2])
yticklabels({'$0.01/k$', '$0.1/k$', '$1/k$', '$10/k$', '$100/k$'})


ax2 = nexttile;
hold on;
for i = 1:length(Wzs)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([-180 180]);
xticks([1e-1,1,1e1])
xticklabels({'$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})

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

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

#+name: fig:rotating_bode_plot
#+caption: Bode plot of the direct (\subref{fig:rotating_bode_plot_direct}) and coupling (\subref{fig:rotating_bode_plot_direct}) terms for several rotating velocities
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:rotating_bode_plot_direct}Direct terms: $d_u/F_u$, $d_v/F_v$}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_bode_plot_direct.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_bode_plot_coupling}Coupling terms: $d_u/F_v$, $d_v/F_u$}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_bode_plot_coupling.png]]
#+end_subfigure
#+end_figure

* Integral Force Feedback
:PROPERTIES:
:header-args:matlab+: :tangle matlab/rotating_2_iff_pure_int.m
:END:
<<sec:rotating_iff_pure_int>>

** Introduction                                                      :ignore:
The goal is now to damp the two suspension modes of the payload using an active damping strategy while the rotating stage performs a constant rotation.
As was explained with the uniaxial model, such active damping strategy is key to both reducing the magnification of the response in the vicinity of the resonances cite:collette11_review_activ_vibrat_isolat_strat and to make the plant easier to control for the high authority controller.

Many active damping techniques have been developed over the years such as Positive Position Feedback (PPF) cite:lin06_distur_atten_precis_hexap_point,fanson90_posit_posit_feedb_contr_large_space_struc, Integral Force Feedback (IFF) cite:preumont91_activ and Direct Velocity Feedback (DVF) cite:karnopp74_vibrat_contr_using_semi_activ_force_gener,serrand00_multic_feedb_contr_isolat_base_excit_vibrat,preumont02_force_feedb_versus_accel_feedb.
In cite:preumont92_activ_dampin_by_local_force, the IFF control scheme has been proposed, where a force sensor, a force actuator and an integral controller are used to increase the damping of a mechanical system.
When the force sensor is collocated with the actuator, the open-loop transfer function has alternating poles and zeros which facilitates to guarantee the stability of the closed loop system cite:preumont02_force_feedb_versus_accel_feedb.
It was latter shown that this property holds for multiple collated actuator/sensor pairs cite:preumont08_trans_zeros_struc_contr_with.

The main advantages of IFF over other active damping techniques are the guaranteed stability even in presence of flexible dynamics, good performance and robustness properties cite:preumont02_force_feedb_versus_accel_feedb.

Several improvements of the classical IFF have been proposed, such as adding a feed-through term to increase the achievable damping cite:teo15_optim_integ_force_feedb_activ_vibrat_contr or adding an high pass filter to recover the loss of compliance at low frequency cite:chesne16_enhan_dampin_flexib_struc_using_force_feedb.
Recently, an $\mathcal{H}_\infty$ optimization criterion has been used to derive optimal gains for the IFF controller cite:zhao19_optim_integ_force_feedb_contr. \par

However, none of these study have been applied to a rotating system.
In this section, Integral Force Feedback strategy is applied on the rotating suspended platform, and it is shown that gyroscopic effects alters the system dynamics and that IFF cannot be applied as is.

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

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

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

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

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

#+begin_src matlab
%% Simscape model name
mdl = 'rotating_model';
#+end_src

#+begin_src matlab
%% Load "Generic" system dynamics
load('rotating_generic_plants.mat', 'Gs', 'Wzs');
#+end_src

** System and Equations of motion
In order to apply Integral Force Feedback, two force sensors are added in series with the actuators (Figure ref:fig:rotating_3dof_model_schematic_iff).
Two identical controllers $K_F$ described by eqref:eq:rotating_iff_controller are then used to feedback each of the sensed force to its associated actuator.

\begin{equation}\label{eq:rotating_iff_controller}
  K_{F}(s) = g \cdot \frac{1}{s}
\end{equation}

#+begin_src latex :file rotating_3dof_model_schematic_iff.pdf
\begin{tikzpicture}
  % Angle
  \def\thetau{25}

  % Rotational Stage
  \draw[fill=black!60!white] (0, 0) circle (4.3);
  \draw[fill=black!40!white] (0, 0) circle (3.8);

  % Label
  \node[anchor=north west, rotate=\thetau] at (-2.5, 2.5) {\small Rotating Stage};

  % Rotating Scope
  \begin{scope}[rotate=\thetau]
    % Rotating Frame
    \draw[fill=black!20!white] (-2.6, -2.6) rectangle (2.6, 2.6);
    % Label
    \node[anchor=north west, rotate=\thetau] at (-2.6, 2.6) {\small Suspended Platform};

    % Mass
    \draw[fill=white] (-1, -1) rectangle (1, 1);
    % Label
    \node[anchor=south west, rotate=\thetau] at (-1, -1) {\small Payload};

    % Attached Points
    \node[] at (-1, 0){$\bullet$};
    \draw[] (-1, 0) -- ++(-0.2, 0) coordinate(au);
    \node[] at (0, -1){$\bullet$};
    \draw[] (0, -1) -- ++(0, -0.2) coordinate(av);

    % Force Sensors
    \draw[draw=colorblue, fill=colorblue!10!white] ($(au) + (-0.2, -0.5)$) rectangle ($(au) + (0, 0.5)$);
    \draw[draw=colorblue] ($(au) + (-0.2, -0.5)$)coordinate(actu) -- ($(au) + (0,  0.5)$);
    \draw[draw=colorblue] ($(au) + (-0.2,  0.5)$)coordinate(ku)   -- ($(au) + (0, -0.5)$);

    \draw[draw=colorblue, fill=colorblue!10!white] ($(av) + (-0.5, -0.2)$) rectangle ($(av) + (0.5, 0)$);
    \draw[draw=colorblue] ($(av) + ( 0.5, -0.2)$)coordinate(actv) -- ($(av) + (-0.5,  0)$);
    \draw[draw=colorblue] ($(av) + (-0.5, -0.2)$)coordinate(kv)   -- ($(av) + ( 0.5,  0)$);

    % Spring and Actuator for U
    \draw[actuator={0.6}{0.2}{black}] (actu) -- coordinate[midway](actumid) (actu-|-2.6,0);
    \draw[spring=0.2] (ku) -- node[above=0.1, rotate=\thetau]{$k$} (ku-|-2.6,0);

    % \draw[actuator={0.6}{0.2}] (actv) -- node[right, rotate=\thetau]{$F_v$} (actv|-0,-2.6);
    \draw[actuator={0.6}{0.2}{black}] (actv) -- coordinate[midway](actvmid) (actv|-0,-2.6);
    \draw[spring=0.2] (kv) -- node[left, rotate=\thetau]{$k$} (kv|-0,-2.6);

    \node[color=colorblue, block={0.8cm}{0.6cm}, fill=colorblue!10!white, rotate=\thetau] (Ku) at ($(actumid) + (0, -1.2)$) {$K_{F}$};
    \draw[->, draw=colorblue] ($(au) + (-0.1, -0.5)$) |- (Ku.east) node[below right, rotate=\thetau]{$f_{u}$};
    \draw[->, draw=colorblue] (Ku.north) -- ($(actumid) + (0, -0.1)$) node[below left, rotate=\thetau]{$F_u$};

    \node[color=colorblue, block={0.8cm}{0.6cm}, fill=colorblue!10!white, rotate=\thetau] (Kv) at ($(actvmid) + (1.2, 0)$) {$K_{F}$};
    \draw[->, draw=colorblue] ($(av) + (0.5, -0.1)$) -| (Kv.north) node[above right, rotate=\thetau]{$f_{v}$};
    \draw[->, draw=colorblue] (Kv.west) -- ($(actvmid) + (0.1, 0)$) node[below right, rotate=\thetau]{$F_v$};
  \end{scope}

  % Inertial Frame
  \draw[->] (-4, -4) -- ++(2, 0) node[below]{$\vec{i}_x$};
  \draw[->] (-4, -4) -- ++(0, 2) node[left]{$\vec{i}_y$};
  \draw[fill, color=black] (-4, -4) circle (0.06);
  \node[draw, circle, inner sep=0pt, minimum size=0.3cm, label=left:$\vec{i}_z$] at (-4, -4){};

  \node[draw, circle, inner sep=0pt, minimum size=0.3cm] at (0, 0){};
  \draw[->] (0, 0) node[above left, rotate=\thetau]{$\vec{i}_w$} -- ++(\thetau:2) node[above, rotate=\thetau]{$\vec{i}_u$};
  \draw[->] (0, 0) -- ++(\thetau+90:2) node[left, rotate=\thetau]{$\vec{i}_v$};
  \draw[dashed] (0, 0) -- ++(2, 0);
  \draw[] (1.5, 0) arc (0:\thetau:1.5) node[midway, right]{$\theta$};
  \node[] at (0,0) {$\bullet$};

  \draw[->] (3.5, 0) arc (0:40:3.5) node[midway, left]{$\Omega$};
\end{tikzpicture}
#+end_src

#+begin_src latex :file rotating_iff_diagram.pdf
\tikzset{block/.default={0.8cm}{0.8cm}}
\tikzset{addb/.append style={scale=0.7}}
\tikzset{node distance=0.6}

\begin{tikzpicture}
  \node[block={1.8cm}{2.2cm}] (G) {$\bm{G}_f$};

  % Inputs of the controllers
  \coordinate[] (output1) at ($(G.south east)!0.75!(G.north east)$);
  \coordinate[] (output2) at ($(G.south east)!0.25!(G.north east)$);
  \coordinate[] (input1)  at ($(G.south west)!0.75!(G.north west)$);
  \coordinate[] (input2)  at ($(G.south west)!0.25!(G.north west)$);

  \node[block, left=1.8 of input1] (K1) {$g/s$};
  \node[block] (K2) at ($(K1.east|-input2)+(0.6, 0)$) {$g/s$};

  % Connections and labels
  \draw[->] (K1.east) -- (input1)node[above left]{$F_u$};
  \draw[->] (K2.east) -- (input2)node[above left]{$F_v$};

  \draw[->] (output1) -- ++(0.8, 0) node[above left]{$f_u$};
  \draw[->] (output2) -- ++(0.8, 0) node[above left]{$f_v$};

  \draw[->] ($(output1)+(0.2, 0)$)node[branch]{} -- ++(0,  1.2) -| ($(K1.west) + (-0.8, 0)$)coordinate(start) -- (K1.west);
  \draw[->] ($(output2)+(0.2, 0)$)node[branch]{} -- ++(0, -1.2) -| (start|-K2) -- (K2.west);

  \begin{scope}[on background layer]
    \node[fit={(K1.north west) (K2.south east)}, inner sep=6pt, draw, dashed, fill=black!20!white] (K) {};
    \node[below left] at (K.north east) {$\bm{K}_F$};
  \end{scope}
\end{tikzpicture}
#+end_src

#+name: fig:rotating_iff_pure_int
#+caption: Integral Force Feedback applied to the suspended rotating platform. The damper $c$ in (\subref{fig:rotating_3dof_model_schematic_iff}) is omitted for readability.
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:rotating_3dof_model_schematic_iff}System with added Force Sensor in series with the actuators}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_3dof_model_schematic_iff.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_iff_diagram}Control diagram}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :scale 1
[[file:figs/rotating_iff_diagram.png]]
#+end_subfigure
#+end_figure

The forces $\begin{bmatrix}f_u & f_v\end{bmatrix}$ measured by the two force sensors represented in Figure ref:fig:rotating_3dof_model_schematic_iff are described by equation eqref:eq:rotating_measured_force.

\begin{equation}\label{eq:rotating_measured_force}
  \begin{bmatrix} f_{u} \\ f_{v} \end{bmatrix} =
  \begin{bmatrix} F_u \\ F_v \end{bmatrix} - (c s + k)
  \begin{bmatrix} d_u \\ d_v \end{bmatrix}
\end{equation}

The transfer function matrix $\mathbf{G}_{f}$ from actuator forces to measured forces in equation eqref:eq:rotating_Gf_mimo_tf can be obtained by inserting equation eqref:eq:rotating_Gd_w0_xi_k into equation eqref:eq:rotating_measured_force.
Its elements are shown in equation eqref:eq:rotating_Gf.

\begin{equation}\label{eq:rotating_Gf_mimo_tf}
  \begin{bmatrix} f_{u} \\ f_{v} \end{bmatrix} = \mathbf{G}_{f} \begin{bmatrix} F_u \\ F_v \end{bmatrix}
\end{equation}

\begin{subequations}\label{eq:rotating_Gf}
  \begin{align}
    \mathbf{G}_{f}(1,1) &= \mathbf{G}_{f}(2,2) = \frac{\left( \frac{s^2}{{\omega_0}^2} - \frac{\Omega^2}{{\omega_0}^2} \right) \left( \frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1 - \frac{{\Omega}^2}{{\omega_0}^2} \right) + \left( 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \right)^2}{\left( \frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1 - \frac{{\Omega}^2}{{\omega_0}^2} \right)^2 + \left( 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \right)^2}  \label{eq:rotating_Gf_diag_tf} \\
    \mathbf{G}_{f}(1,2) &= -\mathbf{G}_{f}(2,1) = \frac{- \left( 2 \xi \frac{s}{\omega_0} + 1 \right) \left( 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \right)}{\left( \frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1 - \frac{{\Omega}^2}{{\omega_0}^2} \right)^2 + \left( 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \right)^2}  \label{eq:rotating_Gf_off_diag_tf}
  \end{align}
\end{subequations}

The zeros of the diagonal terms of $\mathbf{G}_f$ in equation eqref:eq:rotating_Gf_diag_tf are computed, and neglecting the damping for simplicity, two complex conjugated zeros $z_{c}$ eqref:eq:rotating_iff_zero_cc, and two real zeros $z_{r}$ eqref:eq:rotating_iff_zero_real are obtained.

\begin{subequations}
  \begin{align}
    z_c &= \pm j \omega_0 \sqrt{\frac{1}{2} \sqrt{8 \frac{\Omega^2}{{\omega_0}^2} + 1} + \frac{\Omega^2}{{\omega_0}^2} + \frac{1}{2} } \label{eq:rotating_iff_zero_cc} \\
    z_r &= \pm   \omega_0 \sqrt{\frac{1}{2} \sqrt{8 \frac{\Omega^2}{{\omega_0}^2} + 1} - \frac{\Omega^2}{{\omega_0}^2} - \frac{1}{2} } \label{eq:rotating_iff_zero_real}
  \end{align}
\end{subequations}

It is interesting to see that the frequency of the pair of complex conjugate zeros $z_c$ in equation eqref:eq:rotating_iff_zero_cc always lies between the frequency of the two pairs of complex conjugate poles $p_{-}$ and $p_{+}$ in equation eqref:eq:rotating_pole_values.
This is what usually gives the unconditional stability of IFF when collocated force sensors are used.

However, for non-null rotational speeds, the two real zeros $z_r$ in equation eqref:eq:rotating_iff_zero_real are inducing a /non-minimum phase behavior/.
This can be seen in the Bode plot of the diagonal terms (Figure ref:fig:rotating_iff_bode_plot_effect_rot) where the low frequency gain is no longer zero while the phase stays at $\SI{180}{\degree}$.

The low frequency gain of $\mathbf{G}_f$ increases with the rotational speed $\Omega$ as shown in equation eqref:eq:rotating_low_freq_gain_iff_plan.
This can be explained as follows: a constant actuator force $F_u$ induces a small displacement of the mass $d_u = \frac{F_u}{k - m\Omega^2}$ (Hooke's law taking into account the negative stiffness induced by the rotation).
This small displacement then increases the centrifugal force $m\Omega^2d_u = \frac{\Omega^2}{{\omega_0}^2 - \Omega^2} F_u$ which is then measured by the force sensors.

\begin{equation}\label{eq:rotating_low_freq_gain_iff_plan}
  \lim_{\omega \to 0} \left| \mathbf{G}_f (j\omega) \right| = \begin{bmatrix}
  \frac{\Omega^2}{{\omega_0}^2 - \Omega^2} & 0 \\
  0  & \frac{\Omega^2}{{\omega_0}^2 - \Omega^2}
\end{bmatrix}
\end{equation}

** Effect of the rotation speed on the IFF plant dynamics
The transfer functions from actuator forces $[F_u,\ F_v]$ to the measured force sensors $[f_u,\ f_v]$ are identified for several rotating velocities and are shown in Figure ref:fig:rotating_iff_bode_plot_effect_rot.
As was expected from the derived equations of motion:
- when $\Omega < \omega_0$: the low frequency gain is no longer zero and two (non-minimum phase) real zero appears at low frequency.
  The low frequency gain increases with $\Omega$.
  A pair of (minimum phase) complex conjugate zeros appears between the two complex conjugate poles that are split further apart as $\Omega$ increases.
- when $\omega_0 < \Omega$: the low frequency pole becomes unstable.

#+begin_src matlab :results none
%% Bode plot of the direct and coupling term for Integral Force Feedback - Effect of rotation
freqs = logspace(-2, 1, 1000);

Wz_i = [1,3,4];

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

% Magnitude
ax1 = nexttile([2, 1]);
hold on;
for i = 1:length(Wz_i)
    plot(freqs, abs(squeeze(freqresp(Gs{Wz_i(i)}('fu', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:), ...
         'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Wzs(Wz_i(i))),'MarkerSize',8);
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
ylim([1e-3, 1e2]);
leg = legend('location', 'northwest', 'FontSize', 8);

ax2 = nexttile;
hold on;
for i = 1:length(Wz_i)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{Wz_i(i)}('fu', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(i,:))
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([0 180]);
xticks([1e-2,1e-1,1,1e1])
xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})

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

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

#+name: fig:rotating_iff_bode_plot_effect_rot
#+caption: Effect of the rotation velocity on the bode plot of the direct terms (\subref{fig:rotating_iff_bode_plot_effect_rot_direct}) and on the IFF root locus (\subref{fig:rotating_root_locus_iff_pure_int})
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:rotating_iff_bode_plot_effect_rot_direct}Direct terms: $d_u/F_u$, $d_v/F_v$}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_iff_bode_plot_effect_rot_direct.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_root_locus_iff_pure_int}Root Locus}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :scale 1
[[file:figs/rotating_root_locus_iff_pure_int.png]]
#+end_subfigure
#+end_figure

** Decentralized Integral Force Feedback
The control diagram for decentralized Integral Force Feedback is shown in Figure ref:fig:rotating_iff_diagram.

The decentralized IFF controller $\bm{K}_F$ corresponds to a diagonal controller with integrators:
\begin{equation} \label{eq:rotating_Kf_pure_int}
\begin{aligned}
  \mathbf{K}_{F}(s) &= \begin{bmatrix} K_{F}(s) & 0 \\ 0 & K_{F}(s) \end{bmatrix} \\
  K_{F}(s) &= g \cdot \frac{1}{s}
\end{aligned}
\end{equation}

In order to see how the IFF controller affects the poles of the closed loop system, a Root Locus plot (Figure ref:fig:rotating_root_locus_iff_pure_int) is constructed as follows: the poles of the closed-loop system are drawn in the complex plane as the controller gain $g$ varies from $0$ to $\infty$ for the two controllers $K_{F}$ simultaneously.
As explained in cite:preumont08_trans_zeros_struc_contr_with,skogestad07_multiv_feedb_contr, the closed-loop poles start at the open-loop poles (shown by $\tikz[baseline=-0.6ex] \node[cross out, draw=black, minimum size=1ex, line width=2pt, inner sep=0pt, outer sep=0pt] at (0, 0){};$) for $g = 0$ and coincide with the transmission zeros (shown by $\tikz[baseline=-0.6ex] \draw[line width=2pt, inner sep=0pt, outer sep=0pt] (0,0) circle[radius=3pt];$) as $g \to \infty$.

Whereas collocated IFF is usually associated with unconditional stability cite:preumont91_activ, this property is lost due to gyroscopic effects as soon as the rotation velocity in non-null.
This can be seen in the Root Locus plot (Figure ref:fig:rotating_root_locus_iff_pure_int) where poles corresponding to the controller are bound to the right half plane implying closed-loop system instability.
Physically, this can be explained like so: at low frequency, the loop gain is very large due to the pure integrator in $K_{F}$ and the finite gain of the plant (Figure ref:fig:rotating_iff_bode_plot_effect_rot).
The control system is thus canceling the spring forces which makes the suspended platform not capable to hold the payload against centrifugal forces, hence the instability.

#+begin_src matlab
%% Root Locus for the Decentralized Integral Force Feedback controller
Kiff = 1/s*eye(2);

gains = logspace(-2, 4, 300);
Wz_i = [1,3,4];

figure;
hold on;
for i = 1:length(Wz_i)
    plot(real(pole(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)),  imag(pole(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), 'x', 'color', colors(i,:), ...
         'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Wzs(Wz_i(i))),'MarkerSize',8);
    plot(real(tzero(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)),  imag(tzero(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), 'o', 'color', colors(i,:), ...
         'HandleVisibility', 'off','MarkerSize',8);
    for g = gains
        cl_poles = pole(feedback(Gs{Wz_i(i)}({'fu', 'fv'}, {'Fu', 'Fv'}), g*Kiff, -1));
        plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(i,:), ...
             'HandleVisibility', 'off','MarkerSize',4);
    end
end
hold off;
axis square;
xlim([-1.8, 0.2]); ylim([0, 2]);
xticks([-1, 0])
xticklabels({'-$\omega_0$', '$0$'})
yticks([0, 1, 2])
yticklabels({'$0$', '$\omega_0$', '$2 \omega_0$'})

xlabel('Real Part'); ylabel('Imaginary Part');
leg = legend('location', 'northwest', 'FontSize', 8);
leg.ItemTokenSize(1) = 8;
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/rotating_root_locus_iff_pure_int.pdf', 'width', 600, 'height', 600);
#+end_src

* Integral Force Feedback with an High Pass Filter
:PROPERTIES:
:header-args:matlab+: :tangle matlab/rotating_3_iff_hpf.m
:END:
<<sec:rotating_iff_pseudo_int>>

** Introduction                                                      :ignore:
As was explained in the previous section, the instability of the IFF controller applied on the rotating system is due to the high gain of the integrator at low frequency.
In order to limit the low frequency controller gain, an High Pass Filter (HPF) can be added to the controller as shown in equation eqref:eq:rotating_iff_lhf.
This is equivalent to slightly shifting the controller pole to the left along the real axis.
This modification of the IFF controller is typically done to avoid saturation associated with the pure integrator cite:preumont91_activ,marneffe07_activ_passiv_vibrat_isolat_dampin_shunt_trans.
This is however not the reason why this high pass filter is added here.

\begin{equation}\label{eq:rotating_iff_lhf}
  \boxed{K_{F}(s) = g \cdot \frac{1}{s} \cdot \underbrace{\frac{s/\omega_i}{1 + s/\omega_i}}_{\text{HPF}} = g \cdot \frac{1}{s + \omega_i}}
\end{equation}

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

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

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

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

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

#+begin_src matlab
%% Simscape model name
mdl = 'rotating_model';
#+end_src

#+begin_src matlab
%% Load "Generic" system dynamics
load('rotating_generic_plants.mat', 'Gs', 'Wzs');
#+end_src

** Modified Integral Force Feedback Controller
The Integral Force Feedback Controller is modified such that instead of using pure integrators, pseudo integrators (i.e. low pass filters) are used eqref:eq:rotating_iff_lhf where $\omega_i$ characterize the frequency down to which the signal is integrated.
The loop gains ($K_F(s)$ times the direct dynamics $f_u/F_u$) with and without the added HPF are shown in Figure ref:fig:rotating_iff_modified_loop_gain.
The effect of the added HPF limits the low frequency gain to finite values as expected.

The Root Locus plots for the decentralized IFF with and without the HPF are displayed in Figure ref:fig:rotating_iff_root_locus_hpf_large.
With the added HPF, the poles of the closed loop system are shown to be stable up to some value of the gain $g_\text{max}$ given by equation eqref:eq:rotating_gmax_iff_hpf.
It is interesting to note that $g_{\text{max}}$ also corresponds to the controller gain at which the low frequency loop gain reaches one (for instance the gain $g$ can be increased by a factor $5$ in Figure ref:fig:rotating_iff_modified_loop_gain before the system becomes unstable).

\begin{equation}\label{eq:rotating_gmax_iff_hpf}
  \boxed{g_{\text{max}} = \omega_i \left( \frac{{\omega_0}^2}{\Omega^2} - 1 \right)}
\end{equation}

#+begin_src matlab
%% Modified IFF - parameters
g = 2; % Controller gain
wi = 0.1; % HPF Cut-Off frequency [rad/s]

Kiff     = (g/s)*eye(2); % Pure IFF
Kiff_hpf = (g/(wi+s))*eye(2); % IFF with added HPF
#+end_src

#+begin_src matlab :results none
%% Loop gain for the IFF with pure integrator and modified IFF with added high pass filter
freqs = logspace(-2, 1, 1000);
Wz_i = 2;

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

% Magnitude
ax1 = nexttile([2, 1]);
hold on;
plot(freqs, abs(squeeze(freqresp(Gs{Wz_i}('fu', 'Fu')*Kiff(1,1), freqs, 'rad/s'))))
plot(freqs, abs(squeeze(freqresp(Gs{Wz_i}('fu', 'Fu')*Kiff_hpf(1,1), freqs, 'rad/s'))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Loop Gain');

% Phase
ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{Wz_i}('fu', 'Fu')*Kiff(1,1), freqs, 'rad/s'))), 'DisplayName', 'IFF')
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{Wz_i}('fu', 'Fu')*Kiff_hpf(1,1), freqs, 'rad/s'))), 'DisplayName', 'IFF,HPF')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([-90 180]);
xticks([1e-2,1e-1,1,1e1])
xticklabels({'', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
leg = legend('location', 'southwest', 'FontSize', 8);
leg.ItemTokenSize(1) = 15;

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

#+name: fig:rotating_iff_modified_loop_gain_root_locus
#+caption: Comparison of the IFF with pure integrator and modified IFF with added high pass filter ($\Omega = 0.1\omega_0$). Loop gain is shown in (\subref{fig:rotating_iff_modified_loop_gain}) with $\omega_i = 0.1 \omega_0$ and $g = 2$. Root Locus is shown in (\subref{fig:rotating_iff_root_locus_hpf_large})
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:rotating_iff_modified_loop_gain}Loop gain}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :scale 0.8
[[file:figs/rotating_iff_modified_loop_gain.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_iff_root_locus_hpf_large}Root Locus}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :scale 0.8
[[file:figs/rotating_iff_root_locus_hpf_large.png]]
#+end_subfigure
#+end_figure

** Optimal IFF with HPF parameters $\omega_i$ and $g$
Two parameters can be tuned for the modified controller in equation eqref:eq:rotating_iff_lhf: the gain $g$ and the pole's location $\omega_i$.
The optimal values of $\omega_i$ and $g$ are here considered as the values for which the damping of all the closed-loop poles are simultaneously maximized.

In order to visualize how $\omega_i$ does affect the attainable damping, the Root Locus plots for several $\omega_i$ are displayed in Figure ref:fig:rotating_root_locus_iff_modified_effect_wi.
It is shown that even though small $\omega_i$ seem to allow more damping to be added to the suspension modes (see Root locus in Figure ref:fig:rotating_root_locus_iff_modified_effect_wi_large), the control gain $g$ may be limited to small values due to equation eqref:eq:rotating_gmax_iff_hpf.
In order to study this trade off, the attainable closed-loop damping ratio $\xi_{\text{cl}}$ is computed as a function of $\omega_i/\omega_0$.
The gain $g_{\text{opt}}$ at which this maximum damping is obtained is also displayed and compared with the gain $g_{\text{max}}$ at which the system becomes unstable (Figure ref:fig:rotating_iff_hpf_optimal_gain).

# TODO - Maybe comment on these "regions"

Three regions can be observed:
- $\omega_i/\omega_0 < 0.02$: the added damping is limited by the maximum allowed control gain $g_{\text{max}}$
- $0.02 < \omega_i/\omega_0 < 0.2$: the attainable damping ratio is maximized and is reached for $g \approx 2$
- $0.2 < \omega_i/\omega_0$: the added damping decreases as $\omega_i/\omega_0$ increases.

#+begin_src matlab
%% High Pass Filter Cut-Off Frequency
wis = [0.01, 0.05, 0.1]; % [rad/s]
#+end_src

#+begin_src matlab :results none
%% Root Locus for the initial IFF and the modified IFF
gains = logspace(-2, 4, 200);

figure;
hold on;
for wi_i = 1:length(wis)
    wi = wis(wi_i);
    Kiff = (1/(wi+s))*eye(2);
    L(wi_i) = plot(nan, nan, '.', 'color', colors(wi_i,:), 'DisplayName', sprintf('$\\omega_i = %.2f \\omega_0$', wi));
    for g = gains
        clpoles = pole(feedback(Gs{2}({'fu', 'fv'}, {'Fu', 'Fv'}), g*Kiff));
        plot(real(clpoles), imag(clpoles), '.', 'color', colors(wi_i,:),'MarkerSize',4);
    end
    plot(real(pole(Gs{2}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), ...
         imag(pole(Gs{2}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), ...
         'x', 'color', colors(wi_i,:),'MarkerSize',8);
    plot(real(tzero(Gs{2}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), ...
         imag(tzero(Gs{2}({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), ...
         'o', 'color', colors(wi_i,:),'MarkerSize',8);
end
hold off;
axis equal;
xlim([-2.3, 0.1]); ylim([-1.2, 1.2]);
xticks([-2:1:2]); yticks([-2:1:2]);
leg = legend(L, 'location', 'southwest', 'FontSize', 8);
leg.ItemTokenSize(1) = 8;
xlabel('Real Part'); ylabel('Imaginary Part');
clear L

xlim([-1.25, 0.25]); ylim([-1.25, 1.25]);
xticks([-1, 0])
xticklabels({'$-\omega_0$', '$0$'})
yticks([-1, 0, 1])
yticklabels({'$-\omega_0$', '$0$', '$\omega_0$'})
ytickangle(90)
#+end_src

#+begin_src matlab
%% Compute the optimal control gain
wis = logspace(-2, 1, 100); % [rad/s]

opt_xi = zeros(1, length(wis)); % Optimal simultaneous damping
opt_gain = zeros(1, length(wis)); % Corresponding optimal gain

for wi_i = 1:length(wis)
    wi = wis(wi_i);
    Kiff = 1/(s + wi)*eye(2);

    fun = @(g)computeSimultaneousDamping(g, Gs{2}({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff);

    [g_opt, xi_opt] = fminsearch(fun, 0.5*wi*((1/Wzs(2))^2 - 1));
    opt_xi(wi_i) = 1/xi_opt;
    opt_gain(wi_i) = g_opt;
end
#+end_src

#+begin_src matlab :results none
%% Attainable damping ratio as a function of wi/w0. Corresponding control gain g_opt and g_max are also shown
figure;
yyaxis left
plot(wis, opt_xi, '-', 'DisplayName', '$\xi_{cl}$');
set(gca, 'YScale', 'lin');
ylim([0,1]);
ylabel('Damping Ratio $\xi$');

yyaxis right
hold on;
plot(wis, opt_gain, '-', 'DisplayName', '$g_{opt}$');
plot(wis, wis*((1/Wzs(2))^2 - 1), '--', 'DisplayName', '$g_{max}$');
hold off;
set(gca, 'YScale', 'lin');
ylim([0,10]);
ylabel('Controller gain $g$');

xlabel('$\omega_i/\omega_0$');
set(gca, 'XScale', 'log');
legend('location', 'northeast', 'FontSize', 8);
#+end_src

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

#+name: fig:rotating_iff_modified_effect_wi
#+caption: Root Locus for several high pass filter cut-off frequency (\subref{fig:rotating_root_locus_iff_modified_effect_wi_large}).
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:rotating_root_locus_iff_modified_effect_wi}Root Locus}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_root_locus_iff_modified_effect_wi.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_iff_hpf_optimal_gain}Attainable damping ratio $\xi_\text{cl}$ as a function of $\omega_i/\omega_0$. Corresponding control gain $g_\text{opt}$ and $g_\text{max}$ are also shown}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_iff_hpf_optimal_gain.png]]
#+end_subfigure
#+end_figure

** Obtained Damped Plant
In order to study how the parameter $\omega_i$ affects the damped plant, the obtained damped plants for several $\omega_i$ are compared in Figure ref:fig:rotating_iff_hpf_damped_plant_effect_wi_plant.
It can be seen that the low frequency coupling increases as $\omega_i$ increases.
There is therefore a trade-off between achievable damping and added coupling when tuning $\omega_i$.
The same trade-off can be seen between achievable damping and loss of compliance at low frequency (see Figure ref:fig:rotating_iff_hpf_effect_wi_compliance).

#+begin_src matlab
%% Compute damped plant
wis = [0.03, 0.1, 0.5]; % [rad/s]
g = 2; % Gain

Gs_iff_hpf  = {};

for i = 1:length(wis)
    Kiff_hpf = (g/(wis(i)+s))*eye(2); % IFF with added HPF
    Kiff_hpf.InputName = {'fu', 'fv'};
    Kiff_hpf.OutputName = {'Fu', 'Fv'};

    Gs_iff_hpf(i) = {feedback(Gs{2}, Kiff_hpf, 'name')};
end
#+end_src

#+begin_src matlab :exports none :results none
%% Effect of $\omega_i$ on the damped plant coupling
freqs = logspace(-2, 1, 1000);

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

% Magnitude
ax1 = nexttile([2, 1]);
hold on;
for i = 1:length(wis)
    plot(freqs, abs(squeeze(freqresp(Gs_iff_hpf{i}('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(i,:)], ...
         'DisplayName', sprintf('$d_u/F_u$, $\\omega_i = %.2f \\omega_0$', wis(i)))
    plot(freqs, abs(squeeze(freqresp(Gs_iff_hpf{i}('Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(i,:), 0.5], ...
         'DisplayName', sprintf('$d_v/F_u$, $\\omega_i = %.2f \\omega_0$', wis(i)))
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 20;

ax2 = nexttile;
hold on;
for i = 1:length(wis)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_iff_hpf{i}('Du', 'Fu'), freqs, 'rad/s'))), '-')
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([-180 0]);
xticks([1e-2,1e-1,1,1e1])
xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})

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

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

#+begin_src matlab :exports none :results none
%% Effect of $\omega_i$ on the obtained compliance
freqs = logspace(-2, 1, 1000);

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

% Magnitude
ax1 = nexttile();
hold on;
for i = 1:length(wis)
    plot(freqs, abs(squeeze(freqresp(Gs_iff_hpf{i}('Du', 'Fdx'), freqs, 'rad/s'))), '-', 'color', [colors(i,:)], ...
         'DisplayName', sprintf('$d_{x}/F_{dx}$, $\\omega_i = %.2f \\omega_0$', wis(i)))
end
plot(freqs, abs(squeeze(freqresp(Gs{2}('Du', 'Fdx'), freqs, 'rad/s'))), 'k--', ...
     'DisplayName', '$d_{x}/F_{dx}$, OL')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [rad/s]'); ylabel('Compliance [m/N]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 20;
xticks([1e-2,1e-1,1,1e1])
xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})
#+end_src

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

#+name: fig:rotating_iff_hpf_damped_plant_effect_wi
#+caption: Effect of $\omega_i$ on the damped plant coupling
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:rotating_iff_hpf_damped_plant_effect_wi_plant}Obtained plants}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_iff_hpf_damped_plant_effect_wi_plant.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_iff_hpf_effect_wi_compliance}Effect of $\omega_i$ on the compliance}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_iff_hpf_effect_wi_compliance.png]]
#+end_subfigure
#+end_figure

* IFF with a stiffness in parallel with the force sensor
:PROPERTIES:
:header-args:matlab+: :tangle matlab/rotating_4_iff_kp.m
:END:
<<sec:rotating_iff_parallel_stiffness>>

** Introduction                                                      :ignore:
In this section it is proposed to add springs in parallel with the force sensors to counteract the negative stiffness induced by the gyroscopic effects.
Such springs are schematically shown in Figure ref:fig:rotating_3dof_model_schematic_iff_parallel_springs where $k_a$ is the stiffness of the actuator and $k_p$ the added stiffness in parallel with the actuator and force sensor.

#+begin_src latex :file rotating_3dof_model_schematic_iff_parallel_springs.pdf
\begin{tikzpicture}
  % Angle
  \def\thetau{25}

  % Rotational Stage
  \draw[fill=black!60!white] (0, 0) circle (4.3);
  \draw[fill=black!40!white] (0, 0) circle (3.8);

  % Label
  \node[anchor=north west, rotate=\thetau] at (-2.5, 2.5) {\small Rotating Stage};

  % Rotating Scope
  \begin{scope}[rotate=\thetau]
    % Rotating Frame
    \draw[fill=black!20!white] (-2.6, -2.6) rectangle (2.6, 2.6);
    % Label
    \node[anchor=north west, rotate=\thetau] at (-2.6, 2.6) {\small Suspended Platform};

    % Mass
    \draw[fill=white] (-1, -1) rectangle (1, 1);
    % Label
    \node[anchor=south west, rotate=\thetau] at (-1, -1) {\small Payload};

    % Attached Points
    \draw[] (-1, 0) -- ++(-0.2, 0) coordinate(au);
    \draw[] (0, -1) -- ++(0, -0.2) coordinate(av);

    % Force Sensors
    \draw[fill=white] ($(au) + (-0.2, -0.5)$) rectangle ($(au) + (0, 0.5)$);
    \draw[] ($(au) + (-0.2, -0.5)$)coordinate(actu) -- ($(au) + (0,  0.5)$);
    \draw[] ($(au) + (-0.2,  0.5)$)coordinate(ku)   -- ($(au) + (0, -0.5)$);
    \node[below=0.1, rotate=\thetau] at ($(au) + (-0.1, -0.5)$) {$f_{u}$};

    \draw[fill=white] ($(av) + (-0.5, -0.2)$) rectangle ($(av) + (0.5, 0)$);
    \draw[] ($(av) + ( 0.5, -0.2)$)coordinate(actv) -- ($(av) + (-0.5,  0)$);
    \draw[] ($(av) + (-0.5, -0.2)$)coordinate(kv)   -- ($(av) + ( 0.5,  0)$);
    \node[right=0.1, rotate=\thetau] at ($(av) + (0.5, -0.1)$) {$f_{v}$};

    % Spring and Actuator for U
    \draw[actuator={0.6}{0.2}{black}] (actu) -- node[below=0.1, rotate=\thetau]{$F_u$} (actu-|-2.6,0);
    \draw[spring=0.2] (ku) -- node[below=0.1, rotate=\thetau]{$k_a$} (ku-|-2.6,0);
    \draw[spring=0.2,draw=colorred] (-1, 0.8) -- node[above=0.1, rotate=\thetau, color=colorred]{$k_p$} (-1, 0.8-|-2.6,0);

    \draw[actuator={0.6}{0.2}{black}] (actv) -- node[right=0.1, rotate=\thetau]{$F_v$} (actv|-0,-2.6);
    \draw[spring=0.2] (kv) -- node[right=0.1, rotate=\thetau]{$k_a$} (kv|-0,-2.6);
    \draw[spring=0.2, draw=colorred] (-0.8, -1) -- node[left=0.1, rotate=\thetau, color=colorred]{$k_p$} (-0.8, -1|-0,-2.6);
  \end{scope}

  % Inertial Frame
  \draw[->] (-4, -4) -- ++(2, 0) node[below]{$\vec{i}_x$};
  \draw[->] (-4, -4) -- ++(0, 2) node[left]{$\vec{i}_y$};
  \draw[fill, color=black] (-4, -4) circle (0.06);
  \node[draw, circle, inner sep=0pt, minimum size=0.3cm, label=left:$\vec{i}_z$] at (-4, -4){};

  \node[draw, circle, inner sep=0pt, minimum size=0.3cm] at (0, 0){};
  \draw[->] (0, 0) node[above left, rotate=\thetau]{$\vec{i}_w$} -- ++(\thetau:2) node[above, rotate=\thetau]{$\vec{i}_u$};
  \draw[->] (0, 0) -- ++(\thetau+90:2) node[left, rotate=\thetau]{$\vec{i}_v$};
  \draw[dashed] (0, 0) -- ++(2, 0);
  \draw[] (1.5, 0) arc (0:\thetau:1.5) node[midway, right]{$\theta$};
  \node[] at (0,0) {$\bullet$};

  \draw[->] (3.5, 0) arc (0:40:3.5) node[midway, left]{$\Omega$};
\end{tikzpicture}
#+end_src

#+name: fig:rotating_3dof_model_schematic_iff_parallel_springs
#+caption: Studied system with additional springs in parallel with the actuators and force sensors (shown in red)
#+attr_latex: :scale 0.8
#+RESULTS:
[[file:figs/rotating_3dof_model_schematic_iff_parallel_springs.png]]

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

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

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

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

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

#+begin_src matlab
%% Simscape model name
mdl = 'rotating_model';
#+end_src

#+begin_src matlab
%% Load "Generic" system dynamics
load('rotating_generic_plants.mat', 'Gs', 'Wzs');
#+end_src

** Equations
The forces measured by the two force sensors represented in Figure ref:fig:rotating_3dof_model_schematic_iff_parallel_springs are described by eqref:eq:rotating_measured_force_kp.

\begin{equation}\label{eq:rotating_measured_force_kp}
  \begin{bmatrix} f_{u} \\ f_{v} \end{bmatrix} =
  \begin{bmatrix} F_u \\ F_v \end{bmatrix} - (c s + k_a)
  \begin{bmatrix} d_u \\ d_v \end{bmatrix}
\end{equation}

In order to keep the overall stiffness $k = k_a + k_p$ constant, thus not modifying the open-loop poles as $k_p$ is changed, a scalar parameter $\alpha$ ($0 \le \alpha < 1$) is defined to describe the fraction of the total stiffness in parallel with the actuator and force sensor as in eqref:eq:rotating_kp_alpha.

\begin{equation}\label{eq:rotating_kp_alpha}
  k_p = \alpha k, \quad k_a = (1 - \alpha) k
\end{equation}

After the equations of motion derived and transformed in the Laplace domain, the transfer function matrix $\mathbf{G}_k$ in Eq. eqref:eq:rotating_Gk_mimo_tf is computed.
Its elements are shown in Eq. eqref:eq:rotating_Gk_diag and eqref:eq:rotating_Gk_off_diag.

\begin{equation}\label{eq:rotating_Gk_mimo_tf}
  \begin{bmatrix} f_u \\ f_v \end{bmatrix} =
  \mathbf{G}_k
  \begin{bmatrix} F_u \\ F_v \end{bmatrix}
\end{equation}

\begin{subequations}\label{eq:rotating_Gk}
\begin{align}
\mathbf{G}_{k}(1,1) &= \mathbf{G}_{k}(2,2) = \frac{\big( \frac{s^2}{{\omega_0}^2} - \frac{\Omega^2}{{\omega_0}^2} + \alpha \big) \big( \frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1 - \frac{{\Omega}^2}{{\omega_0}^2} \big) + \big( 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \big)^2}{\big( \frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1 - \frac{{\Omega}^2}{{\omega_0}^2} \big)^2 + \big( 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \big)^2}  \label{eq:rotating_Gk_diag} \\
\mathbf{G}_{k}(1,2) &= -\mathbf{G}_{k}(2,1) = \frac{- \left( 2 \xi \frac{s}{\omega_0} + 1 - \alpha \right) \left( 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \right)}{\left( \frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1 - \frac{{\Omega}^2}{{\omega_0}^2} \right)^2 + \left( 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \right)^2}  \label{eq:rotating_Gk_off_diag}
\end{align}
\end{subequations}

Comparing $\mathbf{G}_k$ in eqref:eq:rotating_Gk with $\mathbf{G}_f$ in eqref:eq:rotating_Gf shows that while the poles of the system are kept the same, the zeros of the diagonal terms have changed.
The two real zeros $z_r$ in eqref:eq:rotating_iff_zero_real that were inducing a non-minimum phase behavior are transformed into two complex conjugate zeros if the condition in eqref:eq:rotating_kp_cond_cc_zeros holds.
Thus, if the added /parallel stiffness/ $k_p$ is higher than the /negative stiffness/ induced by centrifugal forces $m \Omega^2$, the dynamics from actuator to its collocated force sensor will show /minimum phase behavior/.

\begin{equation}\label{eq:rotating_kp_cond_cc_zeros}
  \boxed{\alpha > \frac{\Omega^2}{{\omega_0}^2} \quad \Leftrightarrow \quad k_p > m \Omega^2}
\end{equation}

** Identify plant with parallel stiffnesses                        :noexport:
#+begin_src matlab
%% Tuv Stage
mn = 0.5; % Tuv mass [kg]

%% Sample
ms = 0.5; % Sample mass [kg]

%% General Configuration
model_config = struct();
model_config.controller = "open_loop"; % Default: Open-Loop
model_config.Tuv_type   = "parallel_k";    % Default: 2DoF stage
#+end_src

#+begin_src matlab
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/controller'],        1, 'openinput');  io_i = io_i + 1; % [Fu, Fv]
io(io_i) = linio([mdl, '/fd'],                1, 'openinput');  io_i = io_i + 1; % [Fdu, Fdv]
io(io_i) = linio([mdl, '/translation_stage'], 1, 'openoutput'); io_i = io_i + 1; % [Fmu, Fmv]
io(io_i) = linio([mdl, '/translation_stage'], 2, 'openoutput'); io_i = io_i + 1; % [Du, Dv]
io(io_i) = linio([mdl, '/ext_metrology'],     1, 'openoutput'); io_i = io_i + 1; % [Dx, Dy]
#+end_src

** Effect of the parallel stiffness on the IFF plant
The IFF plant (transfer function from $[F_u, F_v]$ to $[f_u, f_v]$) is identified without parallel stiffness $k_p = 0$, with a small parallel stiffness $k_p < m \Omega^2$ and with a large parallel stiffness $k_p > m \Omega^2$.
The Bode plots of the obtained dynamics are shown in Figure ref:fig:rotating_iff_effect_kp.
One can see that the the two real zeros for $k_p < m \Omega^2$ are transformed into two complex conjugate zeros for $k_p > m \Omega^2$.
In that case, the systems shows alternating complex conjugate poles and zeros as what is the case in the non-rotating case.

Figure ref:fig:rotating_iff_kp_root_locus shows the Root Locus plots for $k_p = 0$, $k_p < m \Omega^2$ and $k_p > m \Omega^2$ when $K_F$ is a pure integrator as in Eq. eqref:eq:rotating_Kf_pure_int.
It is shown that if the added stiffness is higher than the maximum negative stiffness, the poles of the closed-loop system are bounded on the (stable) left half-plane, and hence the unconditional stability of IFF is recovered.

#+begin_src matlab
Wz = 0.1; % The rotation speed [rad/s]

%% No parallel Stiffness
kp = 0; % Parallel Stiffness [N/m]
cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
kn = 1 - kp; % Stiffness [N/m]
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]

G_no_kp = linearize(mdl, io, 0);
G_no_kp.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
G_no_kp.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

%% Small parallel Stiffness
kp = 0.5*(mn+ms)*Wz^2; % Parallel Stiffness [N/m]
cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
kn = 1 - kp; % Stiffness [N/m]
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]

G_low_kp = linearize(mdl, io, 0);
G_low_kp.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
G_low_kp.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

%% Large parallel Stiffness
kp = 1.5*(mn+ms)*Wz^2; % Parallel Stiffness [N/m]
cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
kn = 1 - kp; % Stiffness [N/m]
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]

G_high_kp = linearize(mdl, io, 0);
G_high_kp.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
G_high_kp.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
#+end_src

#+begin_src matlab :results none
%% Effect of the parallel stiffness on the IFF plant
freqs = logspace(-2, 1, 1000);

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

% Magnitude
ax1 = nexttile([2, 1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_no_kp(  'fu', 'Fu'), freqs, 'rad/s'))), '-', ...
     'DisplayName', '$k_p = 0$')
plot(freqs, abs(squeeze(freqresp(G_low_kp( 'fu', 'Fu'), freqs, 'rad/s'))), '-', ...
     'DisplayName', '$k_p < m\Omega^2$')
plot(freqs, abs(squeeze(freqresp(G_high_kp('fu', 'Fu'), freqs, 'rad/s'))), '-', ...
     'DisplayName', '$k_p > m\Omega^2$')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
ylim([1e-4, 5e1]);
legend('location', 'southeast', 'FontSize', 8);

% Phase
ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_no_kp(  'fu', 'Fu'), freqs, 'rad/s'))), '-')
plot(freqs, 180/pi*angle(squeeze(freqresp(G_low_kp( 'fu', 'Fu'), freqs, 'rad/s'))), '-')
plot(freqs, 180/pi*angle(squeeze(freqresp(G_high_kp('fu', 'Fu'), freqs, 'rad/s'))), '-')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([0 180]);
hold off;
xticks([1e-2,1e-1,1,1e1])
xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})

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

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

#+begin_src matlab :results none
%% Root Locus for IFF without parallel spring, with small parallel spring and with large parallel spring
gains = logspace(-2, 2, 200);

figure;
hold on;
plot(real(pole(G_no_kp({'fu','fv'},{'Fu','Fv'})*(1/s))),  imag(pole(G_no_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'x', 'color', colors(1,:), ...
     'DisplayName', '$k_p = 0$','MarkerSize',8);
plot(real(tzero(G_no_kp({'fu','fv'},{'Fu','Fv'})*(1/s))),  imag(tzero(G_no_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'o', 'color', colors(1,:), ...
     'HandleVisibility', 'off','MarkerSize',8);
for g = gains
    cl_poles = pole(feedback(G_no_kp({'fu','fv'},{'Fu','Fv'}), (g/s)*eye(2)));
    plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(1,:), ...
         'HandleVisibility', 'off','MarkerSize',4);
end

plot(real(pole(G_low_kp({'fu','fv'},{'Fu','Fv'})*(1/s))),  imag(pole(G_low_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'x', 'color', colors(2,:), ...
     'DisplayName', '$k_p < m\Omega^2$','MarkerSize',8);
plot(real(tzero(G_low_kp({'fu','fv'},{'Fu','Fv'})*(1/s))),  imag(tzero(G_low_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'o', 'color', colors(2,:), ...
     'HandleVisibility', 'off','MarkerSize',8);
for g = gains
    cl_poles = pole(feedback(G_low_kp({'fu','fv'},{'Fu','Fv'}), (g/s)*eye(2)));
    plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(2,:), ...
         'HandleVisibility', 'off','MarkerSize',4);
end

plot(real(pole(G_high_kp({'fu','fv'},{'Fu','Fv'})*(1/s))),  imag(pole(G_high_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'x', 'color', colors(3,:), ...
     'DisplayName', '$k_p > m\Omega^2$','MarkerSize',8);
plot(real(tzero(G_high_kp({'fu','fv'},{'Fu','Fv'})*(1/s))),  imag(tzero(G_high_kp({'fu','fv'},{'Fu','Fv'})*(1/s))), 'o', 'color', colors(3,:), ...
     'HandleVisibility', 'off','MarkerSize',8);
for g = gains
    cl_poles = pole(feedback(G_high_kp({'fu','fv'},{'Fu','Fv'}), (g/s)*eye(2)));
    plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(3,:), ...
         'HandleVisibility', 'off','MarkerSize',4);
end
hold off;
axis square;
xlim([-2.25, 0.25]); ylim([-1.25, 1.25]);
xticks([-2, -1, 0])
xticklabels({'$-2\omega_0$', '$-\omega_0$', '$0$'})
yticks([-1, 0, 1])
yticklabels({'$-\omega_0$', '$0$', '$\omega_0$'})

xlabel('Real Part'); ylabel('Imaginary Part');
leg = legend('location', 'northwest', 'FontSize', 8);
leg.ItemTokenSize(1) = 8;
#+end_src

#+name: fig:rotating_iff_plant_effect_kp
#+caption: Effect of the parallel stiffness on the IFF plant
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:rotating_iff_effect_kp}Bode plot of $G_{k}(1,1) = f_u/F_u$ without parallel spring, with parallel spring stiffness $k_p < m \Omega^2$ and $k_p > m \Omega^2$, $\Omega = 0.1 \omega_0$}
#+attr_latex: :options {0.55\linewidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_iff_effect_kp.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_iff_kp_root_locus}Root Locus for IFF without parallel spring, with small parallel spring and with large parallel spring}
#+attr_latex: :options {0.44\linewidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_iff_kp_root_locus.png]]
#+end_subfigure
#+end_figure

** Effect of $k_p$ on the attainable damping
Even though the parallel stiffness $k_p$ has no impact on the open-loop poles (as the overall stiffness $k$ is kept constant), it has a large impact on the transmission zeros.
Moreover, as the attainable damping is generally proportional to the distance between poles and zeros cite:preumont18_vibrat_contr_activ_struc_fourt_edition, the parallel stiffness $k_p$ is foreseen to have some impact on the attainable damping.
To study this effect, Root Locus plots for several parallel stiffnesses $k_p > m \Omega^2$ are shown in Figure ref:fig:rotating_iff_kp_root_locus_effect_kp.
The frequencies of the transmission zeros of the system are increasing with an increase of the parallel stiffness $k_p$ (thus getting closer to the poles) and the associated attainable damping is reduced.
Therefore, even though the parallel stiffness $k_p$ should be larger than $m \Omega^2$ for stability reasons, it should not be taken too large as this would limit the attainable damping.
This is confirmed by the Figure ref:fig:rotating_iff_kp_optimal_gain where the attainable closed-loop damping ratio $\xi_{\text{cl}}$ and the associated optimal control gain $g_\text{opt}$ are computed as a function of the parallel stiffness.

#+begin_src matlab
%% Tested parallel stiffnesses
kps = [2, 20, 40]*(mn + ms)*Wz^2;
#+end_src

#+begin_src matlab :results none
%% Root Locus: Effect of the parallel stiffness on the attainable damping
gains = logspace(-2, 4, 500);

figure;
hold on;
for kp_i = 1:length(kps)
    kp = kps(kp_i); % Parallel Stiffness [N/m]
    cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
    kn = 1 - kp; % Stiffness [N/m]
    cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]

    % Identify dynamics
    G = linearize(mdl, io, 0);
    G.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
    G.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

    plot(real(pole(G({'fu', 'fv'}, {'Fu', 'Fv'})*(1/s*eye(2)))),  imag(pole(G({'fu', 'fv'}, {'Fu', 'Fv'})*(1/s*eye(2)))), 'x', 'color', colors(kp_i,:), ...
         'DisplayName', sprintf('$k_p = %.1f m \\Omega^2$', kp/((mn+ms)*Wz^2)),'MarkerSize',8);
    plot(real(tzero(G({'fu', 'fv'}, {'Fu', 'Fv'})*(1/s*eye(2)))),  imag(tzero(G({'fu', 'fv'}, {'Fu', 'Fv'})*(1/s*eye(2)))), 'o', 'color', colors(kp_i,:), ...
         'HandleVisibility', 'off','MarkerSize',8);
    for g = gains
        cl_poles = pole(feedback(G({'fu', 'fv'}, {'Fu', 'Fv'}), (g/s)*eye(2)));
        plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(kp_i,:),'MarkerSize',4, ...
             'HandleVisibility', 'off');
    end
end
hold off;
axis square;
% xlim([-1.15, 0.05]); ylim([0, 1.2]);
xlim([-2.25, 0.25]); ylim([-1.25, 1.25]);
xticks([-2, -1, 0])
xticklabels({'$-2\omega_0$', '$-\omega_0$', '$0$'})
yticks([-1, 0, 1])
yticklabels({'$-\omega_0$', '$0$', '$\omega_0$'})

xlabel('Real Part'); ylabel('Imaginary Part');
leg = legend('location', 'northwest', 'FontSize', 8);
leg.ItemTokenSize(1) = 12;
#+end_src

#+begin_src matlab
%% Computes the optimal parameters and attainable simultaneous damping
alphas = logspace(-2, 0, 100);
alphas(end) = []; % Remove last point

opt_xi = zeros(1, length(alphas)); % Optimal simultaneous damping
opt_gain = zeros(1, length(alphas)); % Corresponding optimal gain

Kiff = 1/s*eye(2);

for alpha_i = 1:length(alphas)
    kp = alphas(alpha_i);
    cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
    kn = 1 - kp; % Stiffness [N/m]
    cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]

    % Identify dynamics
    G = linearize(mdl, io, 0);
    G.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
    G.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

    fun = @(g)computeSimultaneousDamping(g, G({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff);

    [g_opt, xi_opt] = fminsearch(fun, 2);
    opt_xi(alpha_i) = 1/xi_opt;
    opt_gain(alpha_i) = g_opt;
end
#+end_src

#+begin_src matlab
%% Attainable damping as a function of the stiffness ratio
figure;
yyaxis left
plot(alphas, opt_xi, '-', 'DisplayName', '$\xi_{cl}$');
set(gca, 'YScale', 'lin');
ylim([0,1]);
ylabel('Damping Ratio $\xi$');

yyaxis right
hold on;
plot(alphas, opt_gain, '-', 'DisplayName', '$g_{opt}$');
set(gca, 'YScale', 'lin');
ylim([0,2.5]);
ylabel('Controller gain $g$');

set(gca, 'XScale', 'log');
legend('location', 'northeast', 'FontSize', 8);

xlabel('$k_p$');
xlim([0.01, 1]);
xticks([0.01, 0.1, 1])
xticklabels({'$m\Omega^2$', '$10m\Omega^2$', '$100m\Omega^2$'})
#+end_src

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

#+name: fig:rotating_iff_optimal_kp
#+caption: Effect of the parallel stiffness on the IFF plant
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:rotating_iff_kp_root_locus_effect_kp}Root Locus: Effect of the parallel stiffness on the attainable damping, $\Omega = 0.1 \omega_0$}
#+attr_latex: :options {0.49\linewidth}
#+begin_subfigure
#+attr_latex: :scale 1
[[file:figs/rotating_iff_kp_root_locus_effect_kp.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_iff_kp_optimal_gain}Attainable damping ratio $\xi_\text{cl}$ as a function of the parallel stiffness $k_p$. Corresponding control gain $g_\text{opt}$ is also shown. Values for $k_p < m\Omega^2$ are not shown as the system is unstable.}
#+attr_latex: :options {0.49\linewidth}
#+begin_subfigure
#+attr_latex: :scale 0.9
[[file:figs/rotating_iff_kp_optimal_gain.png]]
#+end_subfigure
#+end_figure

** Damped plant
Let's choose a parallel stiffness equal to $k_p = 2 m \Omega^2$ and compute the damped plant.
The damped and undamped transfer functions from $F_u$ to $d_u$ are compared in Figure ref:fig:rotating_iff_kp_damped_plant.
Even though the two resonances are well damped, the IFF changes the low frequency behavior of the plant which is usually not wanted.
This is due to the fact that "pure" integrators are used, and that the low frequency loop gains becomes large below some frequency.

In order to lower the low frequency gain, a high pass filter is added to the IFF controller (which is equivalent as shifting the controller pole to the left in the complex plane):
\begin{equation}
  K_{\text{IFF}}(s) = g\frac{1}{\omega_i + s} \begin{bmatrix}
  1 & 0 \\
  0 & 1
\end{bmatrix}
\end{equation}

In order to see how the high pass filter impacts the attainable damping, the controller gain $g$ is kept constant while $\omega_i$ is changed, and the minimum damping ratio of the damped plant is computed.
The obtained damping ratio as a function of $\omega_i/\omega_0$ (where $\omega_0$ is the resonance of the system without rotation) is shown in Figure ref:fig:rotating_iff_kp_added_hpf_effect_damping.
It is shown that the attainable damping ratio reduces as $\omega_i$ is increased (same conclusion than in Section ref:sec:rotating_iff_pseudo_int).
Let's choose $\omega_i = 0.1 \cdot \omega_0$ and compare the obtained damped plant again with the undamped and with the "pure" IFF in Figure ref:fig:rotating_iff_kp_added_hpf_damped_plant.
The added high pass filter gives almost the same damping properties to the suspension while giving good low frequency behavior.

#+begin_src matlab
%% Identify dynamics with parallel stiffness = 2mW^2
Wz = 0.1; % [rad/s]
kp = 2*(mn + ms)*Wz^2; % Parallel Stiffness [N/m]
cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]
kn = 1 - kp; % Stiffness [N/m]
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]

% Identify dynamics
G = linearize(mdl, io, 0);
G.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy'};
G.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

%% IFF controller with pure integrator
Kiff_kp = (2.2/s)*eye(2);
Kiff_kp.InputName = {'fu', 'fv'};
Kiff_kp.OutputName = {'Fu', 'Fv'};

%% Compute the damped plant
G_cl_iff_kp = feedback(G, Kiff_kp, 'name');
#+end_src

#+begin_src matlab
w0 = sqrt((kn+kp)/(mn+ms)); % Resonance frequency [rad/s]
wis = w0*logspace(-2, 0, 100); % LPF cut-off [rad/s]
#+end_src

#+begin_src matlab
%% Computes the obtained damping as a function of the HPF cut-off frequency
opt_xi = zeros(1, length(wis)); % Optimal simultaneous damping

for wi_i = 1:length(wis)
    Kiff_kp_hpf = (2.2/(s + wis(wi_i)))*eye(2);
    Kiff_kp_hpf.InputName = {'fu', 'fv'};
    Kiff_kp_hpf.OutputName = {'Fu', 'Fv'};

    [~, xi] = damp(feedback(G, Kiff_kp_hpf, 'name'));
    opt_xi(wi_i) = min(xi);
end
#+end_src

#+begin_src matlab :results none
%% Effect of the high pass filter cut-off frequency on the obtained damping
figure;
plot(wis, opt_xi, '-');
set(gca, 'XScale', 'log');
set(gca, 'YScale', 'lin');
ylim([0,1]);
ylabel('Damping Ratio $\xi$');
xlabel('$\omega_i/\omega_0$');
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/rotating_iff_kp_added_hpf_effect_damping.pdf', 'width', 'third', 'height', 600);
#+end_src

#+begin_src matlab
%% Compute the damped plant with added High Pass Filter
Kiff_kp_hpf = (2.2/(s + 0.1*w0))*eye(2);
Kiff_kp_hpf.InputName = {'fu', 'fv'};
Kiff_kp_hpf.OutputName = {'Fu', 'Fv'};

G_cl_iff_hpf_kp = feedback(G, Kiff_kp_hpf, 'name');
#+end_src

#+begin_src matlab :results none
%% Bode plot of the direct and coupling terms for several rotating velocities
freqs = logspace(-3, 1, 1000);

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

% Magnitude
ax1 = nexttile([2, 1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G(              'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros( 1,3), ...
    'DisplayName', '$d_u/F_u$ - OL')
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp(    'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:), ...
    'DisplayName', '$d_u/F_u$ - IFF + $k_p$')
plot(freqs, abs(squeeze(freqresp(G_cl_iff_hpf_kp('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(2,:), ...
    'DisplayName', '$d_u/F_u$ - IFF + $k_p$ + HPF')
plot(freqs, abs(squeeze(freqresp(G(              'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [zeros( 1,3), 0.5], ...
    'HandleVisibility', 'off')
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp(    'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(1,:), 0.5], ...
    'HandleVisibility', 'off')
plot(freqs, abs(squeeze(freqresp(G_cl_iff_hpf_kp('Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(2,:), 0.5], ...
    'HandleVisibility', 'off')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
ldg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
ldg.ItemTokenSize(1) = 10;

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G(              'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros( 1,3))
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_iff_kp(    'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:))
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_iff_hpf_kp('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(2,:))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([-180 90]);
xticks([1e-3,1e-2,1e-1,1,1e1])
xticklabels({'$0.001 \omega_0$', '$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$'})

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

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/rotating_iff_kp_added_hpf_damped_plant.pdf', 'width', 700, 'height', 600);
#+end_src

#+name: fig:rotating_iff_optimal_kp
#+caption:Effect of the high pass filter cut-off frequency on the obtained damping
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:rotating_iff_kp_added_hpf_effect_damping}Reduced damping ratio with increased cut-off frequency $\omega_i$}
#+attr_latex: :options {0.34\linewidth}
#+begin_subfigure
#+attr_latex: :scale 0.95
[[file:figs/rotating_iff_kp_added_hpf_effect_damping.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_iff_kp_added_hpf_damped_plant}Damped plant with the parallel stiffness, effect of the added HPF}
#+attr_latex: :options {0.65\linewidth}
#+begin_subfigure
#+attr_latex: :scale 0.95
[[file:figs/rotating_iff_kp_added_hpf_damped_plant.png]]
#+end_subfigure
#+end_figure

* Relative Damping Control
:PROPERTIES:
:header-args:matlab+: :tangle matlab/rotating_5_rdc.m
:END:
<<sec:rotating_relative_damp_control>>

** Introduction                                                      :ignore:
In order to apply a "Relative Damping Control" strategy, relative motion sensors are added in parallel with the actuators as shown in Figure ref:fig:rotating_3dof_model_schematic_rdc.
Two controllers $K_d$ are used to fed back the relative motion to the actuator.
These controllers are in principle pure derivators ($K_d = s$), but to be implemented in practice they are usually replaced by a high pass filter eqref:eq:rotating_rdc_controller.

\begin{equation}\label{eq:rotating_rdc_controller}
K_d(s) = g \cdot \frac{s}{s + \omega_d}
\end{equation}

#+begin_src latex :file rotating_3dof_model_schematic_rdc.pdf
\begin{tikzpicture}
  % Angle
  \def\thetau{25}

  % Rotational Stage
  \draw[fill=black!60!white] (0, 0) circle (4.3);
  \draw[fill=black!40!white] (0, 0) circle (3.8);

  % Label
  \node[anchor=north west, rotate=\thetau] at (-2.5, 2.5) {\small Rotating Stage};

  % Rotating Scope
  \begin{scope}[rotate=\thetau]
    % Rotating Frame
    \draw[fill=black!20!white] (-2.6, -2.6) rectangle (2.6, 2.6);
    % Label
    \node[anchor=north west, rotate=\thetau] at (-2.6, 2.6) {\small Suspended Platform};

    % Mass
    \draw[fill=white] (-1, -1) rectangle (1, 1);
    % Label
    \node[anchor=south west, rotate=\thetau] at (-1, -1) {\small Payload};

    % Attached Points
    \node[] at (-1, 0){$\bullet$};
    \draw[] (-1, 0) -- ++(-0.2, 0) coordinate(cu);
    \draw[] ($(cu) + (0, -0.8)$) coordinate(actu) -- ($(cu) + (0, 0.8)$) coordinate(ku);
    \node[] at (0, -1){$\bullet$};
    \draw[] (0, -1) -- ++(0, -0.2) coordinate(cv);
    \draw[] ($(cv) + (-0.8, 0)$)coordinate(kv) -- ($(cv) + (0.8, 0)$) coordinate(actv);

    % Spring and Actuator for U
    \draw[actuator={0.6}{0.2}{black}] (cu) -- coordinate[midway, below=0.1](actumid) node[above=0.1, rotate=\thetau]{$F_u$} (cu-|-2.6,0);
    \draw[spring=0.2] (ku) -- node[above=0.1, rotate=\thetau]{$k$} (ku-|-2.6,0);
    \draw[<->, dashed, draw=colorgreen] (actu) node[below=0.1, rotate=\thetau, color=colorgreen](du){$d_u$} -- (actu-|-2.6,0);

    \node[color=colorgreen, block={0.6cm}{0.6cm}, fill=colorgreen!10!white, rotate=\thetau] (Ku) at ($(actumid) + (0, -1.4)$) {$K_{d}$};
    \draw[->, draw=colorgreen] (du.south) -- ++(0, -0.8) -| (Ku.south);
    \draw[->, draw=colorgreen] (Ku.north) -- (actumid);

    \draw[actuator={0.6}{0.2}{black}] (cv) -- coordinate[midway, right=0.1](actvmid) node[left, rotate=\thetau]{$F_v$} (cv|-0,-2.6);
    \draw[spring=0.2] (kv) -- node[left, rotate=\thetau]{$k$} (kv|-0,-2.6);
    \draw[<->, dashed, draw=colorgreen] (actv)node[right=0.1, rotate=\thetau, color=colorgreen](dv){$d_v$} -- (actv|-0,-2.6);

    \node[color=colorgreen, block={0.6cm}{0.6cm}, fill=colorgreen!10!white, rotate=\thetau] (Kv) at ($(actvmid) + (1.4, 0)$) {$K_{d}$};
    \draw[->, draw=colorgreen] (dv.east) -- ++(0.8, 0) |- (Kv.east);
    \draw[->, draw=colorgreen] (Kv.west) -- (actvmid);
  \end{scope}

  % Inertial Frame
  \draw[->] (-4, -4) -- ++(2, 0) node[below]{$\vec{i}_x$};
  \draw[->] (-4, -4) -- ++(0, 2) node[left]{$\vec{i}_y$};
  \draw[fill, color=black] (-4, -4) circle (0.06);
  \node[draw, circle, inner sep=0pt, minimum size=0.3cm, label=left:$\vec{i}_z$] at (-4, -4){};

  \draw[->] (0, 0) node[above left, rotate=\thetau]{$\vec{i}_w$} -- ++(\thetau:2) node[above, rotate=\thetau]{$\vec{i}_u$};
  \draw[->] (0, 0) -- ++(\thetau+90:2) node[left, rotate=\thetau]{$\vec{i}_v$};
  \draw[fill, color=black] (0,0) circle (0.06);
  \node[draw, circle, inner sep=0pt, minimum size=0.3cm] at (0, 0){};
  \draw[dashed] (0, 0) -- ++(2, 0);
  \draw[] (1.5, 0) arc (0:\thetau:1.5) node[midway, right]{$\theta$};

  \draw[->] (3.5, 0) arc (0:40:3.5) node[midway, left]{$\Omega$};
\end{tikzpicture}
#+end_src

#+name: fig:rotating_3dof_model_schematic_rdc
#+caption: System with relative motion sensor and decentralized "relative damping control" applied.
#+attr_latex: :scale 0.8
#+RESULTS:
[[file:figs/rotating_3dof_model_schematic_rdc.png]]

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

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

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

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

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

#+begin_src matlab
%% Simscape model name
mdl = 'rotating_model';
#+end_src

#+begin_src matlab
%% Load "Generic" system dynamics
load('rotating_generic_plants.mat', 'Gs', 'Wzs');
#+end_src

** Equations of motion
Let's note $\bm{G}_d$ the transfer function between actuator forces and measured relative motion in parallel with the actuators eqref:eq:rotating_rdc_plant_matrix.
The elements of $\bm{G}_d$ were derived in Section ref:sec:rotating_system_description are shown in eqref:eq:rotating_rdc_plant_elements.

\begin{equation}\label{eq:rotating_rdc_plant_matrix}
  \begin{bmatrix} d_u \\ d_v \end{bmatrix} = \mathbf{G}_d \begin{bmatrix} F_u \\ F_v \end{bmatrix}
\end{equation}

\begin{subequations}\label{eq:rotating_rdc_plant_elements}
  \begin{align}
    \mathbf{G}_{d}(1,1) &= \mathbf{G}_{d}(2,2) = \frac{\frac{1}{k} \left( \frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1 - \frac{{\Omega}^2}{{\omega_0}^2} \right)}{\left( \frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1 - \frac{{\Omega}^2}{{\omega_0}^2} \right)^2 + \left( 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \right)^2}  \\
    \mathbf{G}_{d}(1,2) &= -\mathbf{G}_{d}(2,1) = \frac{\frac{1}{k} \left( 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \right)}{\left( \frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1 - \frac{{\Omega}^2}{{\omega_0}^2} \right)^2 + \left( 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \right)^2}
  \end{align}
\end{subequations}

Neglecting the damping for simplicity ($\xi \ll 1$), the direct terms have two complex conjugate zeros which are between the two pairs of complex conjugate poles eqref:eq:rotating_rdc_zeros_poles.
Therefore, for $\Omega < \sqrt{k/m}$ (i.e. stable system), the transfer functions for Relative Damping Control have alternating complex conjugate poles and zeros.

\begin{equation}\label{eq:rotating_rdc_zeros_poles}
  z = \pm j \sqrt{\omega_0^2 - \omega^2}, \quad p_1 = \pm j (\omega_0 - \omega), \quad p_2 = \pm j (\omega_0 + \omega)
\end{equation}

** Decentralized Relative Damping Control
The transfer functions from $[F_u,\ F_v]$ to $[d_u,\ d_v]$ were identified for several rotating velocities in Section ref:sec:rotating_system_description and are shown in Figure ref:fig:rotating_bode_plot (page pageref:fig:rotating_bode_plot).

In order to see if large damping can be added with Relative Damping Control, the root locus is computed (Figure ref:fig:rotating_rdc_root_locus).
The closed-loop system is unconditionally stable as expected and the poles can be damped as much as wanted.

Let's select a reasonable "Relative Damping Control" gain, and compute the closed-loop damped system.
The open-loop and damped plants are compared in Figure ref:fig:rotating_rdc_damped_plant.
The rotating aspect does not add any complexity for the use of Relative Damping Control.
It does not increase the low frequency coupling as compared to Integral Force Feedback.

#+begin_src matlab :results none
%% Root Locus for Relative Damping Control
Krdc = s*eye(2); % Relative damping controller

gains = logspace(-2, 2, 300); % Tested gains
Wz_i = [1,3,4];

figure;
hold on;
for i = 1:length(Wz_i)
    plot(real(pole(Gs{Wz_i(i)}({'du', 'dv'}, {'Fu', 'Fv'})*Krdc)),  imag(pole(Gs{Wz_i(i)}({'du', 'dv'}, {'Fu', 'Fv'})*Krdc)), 'x', 'color', colors(i,:), ...
         'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Wzs(Wz_i(i))),'MarkerSize',8);
    plot(real(tzero(Gs{Wz_i(i)}({'du', 'dv'}, {'Fu', 'Fv'})*Krdc)),  imag(tzero(Gs{Wz_i(i)}({'du', 'dv'}, {'Fu', 'Fv'})*Krdc)), 'o', 'color', colors(i,:), ...
         'HandleVisibility', 'off','MarkerSize',8);
    for g = gains
        cl_poles = pole(feedback(Gs{Wz_i(i)}({'du', 'dv'}, {'Fu', 'Fv'}), g*Krdc, -1));
        plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(i,:), ...
             'HandleVisibility', 'off','MarkerSize',4);
    end
end
hold off;
axis square;
xlim([-1.8, 0.2]); ylim([0, 2]);
xticks([-1, 0])
xticklabels({'-$\omega_0$', '$0$'})
yticks([0, 1, 2])
yticklabels({'$0$', '$\omega_0$', '$2 \omega_0$'})

xlabel('Real Part'); ylabel('Imaginary Part');
leg = legend('location', 'northwest', 'FontSize', 8);
leg.ItemTokenSize(1) = 8;
#+end_src

#+begin_src matlab :tangle no :results none
%% Root Locus for Relative Damping Control
Krdc = s*eye(2); % Relative damping controller

gains = logspace(-2, 2, 300); % Tested gains
Wz_i = [1,3,4];

figure;
hold on;
for i = 1:length(Wz_i)
    plot(real(pole(Gs{Wz_i(i)}({'du', 'dv'}, {'Fu', 'Fv'})*Krdc)),  imag(pole(Gs{Wz_i(i)}({'du', 'dv'}, {'Fu', 'Fv'})*Krdc)), 'x', 'color', colors(i,:), ...
         'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Wzs(Wz_i(i))),'MarkerSize',8);
    plot(real(tzero(Gs{Wz_i(i)}({'du', 'dv'}, {'Fu', 'Fv'})*Krdc)),  imag(tzero(Gs{Wz_i(i)}({'du', 'dv'}, {'Fu', 'Fv'})*Krdc)), 'o', 'color', colors(i,:), ...
         'HandleVisibility', 'off','MarkerSize',8);

    poles_rdc = rootLocusPolesSorted(Gs{Wz_i(i)}({'du', 'dv'}, {'Fu', 'Fv'}), Krdc, gains, 'd_max', 1e-4);
    for p_i = 1:size(poles_rdc, 2)
        plot(real(poles_rdc(:, p_i)), imag(poles_rdc(:, p_i)), '-', 'color', colors(i,:), ...
            'HandleVisibility', 'off');
    end
end
hold off;
axis equal;
xlim([-1.8, 0.2]); ylim([0, 2]);
xticks([-1, 0])
xticklabels({'-$\omega_0$', '$0$'})
yticks([0, 1, 2])
yticklabels({'$0$', '$\omega_0$', '$2 \omega_0$'})

xlabel('Real Part'); ylabel('Imaginary Part');
leg = legend('location', 'northwest', 'FontSize', 8);
leg.ItemTokenSize(1) = 8;
#+end_src

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/rotating_rdc_root_locus_inkscape.pdf', 'width', 'half', 'height', 500, 'png', false, 'pdf', false, 'svg', true);
#+end_src

#+begin_src matlab
i = 2;

%% Relative Damping Controller
Krdc = 2*s*eye(2);
Krdc.InputName = {'Du', 'Dv'};
Krdc.OutputName = {'Fu', 'Fv'};

%% Compute the damped plant
G_cl_rdc = feedback(Gs{i}, Krdc, 'name');
#+end_src

#+begin_src matlab :results none
%% Damped plant using Relative Damping Control
freqs = logspace(-3, 2, 1000);

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

% Magnitude
ax1 = nexttile([2, 1]);
hold on;
plot(freqs, abs(squeeze(freqresp(Gs{i}(   'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros(1,3), ...
    'DisplayName', 'OL - $G_d(1,1)$')
plot(freqs, abs(squeeze(freqresp(G_cl_rdc('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:), ...
    'DisplayName', 'RDC - $G_d(1,1)$')
plot(freqs, abs(squeeze(freqresp(Gs{i}(   'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [zeros(1,3), 0.5], ...
    'DisplayName', 'OL - $G_d(2,1)$')
plot(freqs, abs(squeeze(freqresp(G_cl_rdc('Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(1,:), 0.5], ...
    'DisplayName', 'RDC - $G_d(2,1)$')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
ldg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
ldg.ItemTokenSize(1) = 20;
ylim([1e-4, 1e2]);

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros(1,3))
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_rdc('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([-180 0]);

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

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

#+name: fig:rotating_rdc_result
#+caption: Relative Damping Control. Root Locus (\subref{fig:rotating_rdc_root_locus}) and obtained damped plant (\subref{rotating_rdc_damped_plant})
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:rotating_rdc_root_locus}Root Locus for Relative Damping Control}
#+attr_latex: :options {0.49\linewidth}
#+begin_subfigure
#+attr_latex: :scale 1
[[file:figs/rotating_rdc_root_locus.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_rdc_damped_plant}Damped plant using Relative Damping Control}
#+attr_latex: :options {0.49\linewidth}
#+begin_subfigure
#+attr_latex: :scale 0.8
[[file:figs/rotating_rdc_damped_plant.png]]
#+end_subfigure
#+end_figure

* Comparison of Active Damping Techniques
:PROPERTIES:
:header-args:matlab+: :tangle matlab/rotating_6_act_damp_comparison.m
:END:
<<sec:rotating_comp_act_damp>>

** Introduction                                                      :ignore:
These two proposed IFF modifications as well as relative damping control are now compared in terms of added damping and closed-loop behavior.
For the following comparisons, the cut-off frequency for the added HPF is set to $\omega_i = 0.1 \omega_0$ and the stiffness of the parallel springs is set to $k_p = 5 m \Omega^2$ (corresponding to $\alpha = 0.05$).
These values are chosen based on previous discussion about optimal parameters.

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

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

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

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

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

#+begin_src matlab
%% Simscape model name
mdl = 'rotating_model';
#+end_src

** Identify plants                                                 :noexport:
#+begin_src matlab
%% The rotating speed is set to $\Omega = 0.1 \omega_0$.
Wz = 0.1; % [rad/s]

%% Masses
ms = 0.5; % Sample mass [kg]
mn = 0.5; % Tuv mass [kg]
#+end_src

#+begin_src matlab
%% General Configuration
model_config = struct();
model_config.controller = "open_loop"; % Default: Open-Loop

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/controller'],        1, 'openinput');  io_i = io_i + 1; % [Fu, Fv]
io(io_i) = linio([mdl, '/fd'],                1, 'openinput');  io_i = io_i + 1; % [Fdu, Fdv]
io(io_i) = linio([mdl, '/xf'],                1, 'openinput');  io_i = io_i + 1; % [Dfx, Dfy]
io(io_i) = linio([mdl, '/translation_stage'], 1, 'openoutput'); io_i = io_i + 1; % [Fmu, Fmv]
io(io_i) = linio([mdl, '/translation_stage'], 2, 'openoutput'); io_i = io_i + 1; % [Du, Dv]
io(io_i) = linio([mdl, '/ext_metrology'],     1, 'openoutput'); io_i = io_i + 1; % [Dx, Dy]
#+end_src

#+begin_src matlab
%% Identifying plant with parallel stiffness
model_config.Tuv_type   = "parallel_k";

% Parallel stiffness
kp = 2*(mn+ms)*Wz^2; % Parallel Stiffness [N/m]
cp = 0.001*2*sqrt(kp*(mn+ms)); % Small parallel damping [N/(m/s)]

% Tuv Stage
kn = 1 - kp; % Stiffness [N/m]
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]

% Linearize
G_kp = linearize(mdl, io, 0);
G_kp.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
G_kp.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
#+end_src

#+begin_src matlab
%% Identifying plant with no parallel stiffness
model_config.Tuv_type   = "normal";

% Tuv Stage
kn = 1; % Stiffness [N/m]
cn = 0.01*2*sqrt(kn*(mn+ms)); % Damping [N/(m/s)]

% Linearize
G = linearize(mdl, io, 0);
G.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
G.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
#+end_src

#+begin_src matlab
%% IFF Controller
Kiff = (2.2/(s + 0.1))*eye(2);
Kiff.InputName = {'fu', 'fv'};
Kiff.OutputName = {'Fu', 'Fv'};
#+end_src

#+begin_src matlab
%% IFF Controller with added stiffness
Kiff_kp = (2.2/(s + 0.1))*eye(2);
Kiff_kp.InputName = {'fu', 'fv'};
Kiff_kp.OutputName = {'Fu', 'Fv'};
#+end_src

#+begin_src matlab
%% Relative Damping Controller
Krdc = 2*s*eye(2);
Krdc.InputName = {'Du', 'Dv'};
Krdc.OutputName = {'Fu', 'Fv'};
#+end_src

** Root Locus
Figure ref:fig:rotating_comp_techniques_root_locus shows the Root Locus plots for the two proposed IFF modifications as well as for relative damping control.
While the two pairs of complex conjugate open-loop poles are identical for both IFF modifications, the transmission zeros are not.
This means that the closed-loop behavior of both systems will differ when large control gains are used.

One can observe that the closed loop poles corresponding to the system with added springs (in red) are bounded to the left half plane implying unconditional stability.
This is not the case for the system where the controller is augmented with an HPF (in blue).
It is interesting to note that the maximum added damping is very similar for both modified IFF techniques.

#+begin_src matlab :exports none :results none
%% Comparison of active damping techniques for rotating platform - Root Locus
gains = logspace(-2, 2, 500);

figure;
hold on;
% IFF
plot(real(pole(G({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)),  imag(pole(G({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), 'x', 'color', colors(1,:), ...
     'DisplayName', 'IFF with HPF', 'MarkerSize', 8);
plot(real(tzero(G({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)),  imag(tzero(G({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), 'o', 'color', colors(1,:), ...
     'HandleVisibility', 'off', 'MarkerSize', 8);
for g = gains
    cl_poles = pole(feedback(G({'fu', 'fv'}, {'Fu', 'Fv'}), g*Kiff));
    plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(1,:),'MarkerSize',4, ...
         'HandleVisibility', 'off');
end
% IFF with parallel stiffness
plot(real(pole(G_kp({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff_kp)),  imag(pole(G_kp({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff_kp)), 'x', 'color', colors(2,:), ...
     'DisplayName', 'IFF with $k_p$', 'MarkerSize', 8);
plot(real(tzero(G_kp({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff_kp)),  imag(tzero(G_kp({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff_kp)), 'o', 'color', colors(2,:), ...
     'HandleVisibility', 'off', 'MarkerSize', 8);
for g = gains
    cl_poles = pole(feedback(G_kp({'fu', 'fv'}, {'Fu', 'Fv'}), g*Kiff_kp));
    plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(2,:),'MarkerSize',4, ...
         'HandleVisibility', 'off');
end
% RDC
plot(real(pole(G({'Du', 'Dv'}, {'Fu', 'Fv'})*Krdc)),  imag(pole(G({'Du', 'Dv'}, {'Fu', 'Fv'})*Krdc)), 'x', 'color', colors(3,:), ...
     'DisplayName', 'RDC', 'MarkerSize', 8);
plot(real(tzero(G({'Du', 'Dv'}, {'Fu', 'Fv'})*Krdc)),  imag(tzero(G({'Du', 'Dv'}, {'Fu', 'Fv'})*Krdc)), 'o', 'color', colors(3,:), ...
     'HandleVisibility', 'off', 'MarkerSize', 8);
for g = gains
    cl_poles = pole(feedback(G({'Du', 'Dv'}, {'Fu', 'Fv'}), g*Krdc));
    plot(real(cl_poles), imag(cl_poles), '.', 'color', colors(3,:),'MarkerSize',4, ...
         'HandleVisibility', 'off');
end
hold off;
axis square;
xlim([-1.15, 0.05]); ylim([0, 1.2]);

xlabel('Real Part'); ylabel('Imaginary Part');
leg = legend('location', 'northwest', 'FontSize', 8);
leg.ItemTokenSize(1) = 12;
#+end_src

#+begin_src matlab :tangle no
gains = logspace(-2, 2, 500);

poles_iff_hpf = rootLocusPolesSorted(G({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff, gains, 'd_max', 1e-4);
poles_iff_kp = rootLocusPolesSorted(G_kp({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff_kp, gains, 'd_max', 1e-4);
poles_rdc = rootLocusPolesSorted(G({'Du', 'Dv'}, {'Fu', 'Fv'}), Krdc, gains, 'd_max', 1e-4);

figure;
hold on;
% IFF
plot(real(pole(G({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)),  imag(pole(G({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), 'x', 'color', colors(1,:), ...
     'DisplayName', 'IFF with HPF', 'MarkerSize', 8);
plot(real(tzero(G({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)),  imag(tzero(G({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff)), 'o', 'color', colors(1,:), ...
     'HandleVisibility', 'off', 'MarkerSize', 8);
for p_i = 1:size(poles_iff_hpf, 2)
    plot(real(poles_iff_hpf(:, p_i)), imag(poles_iff_hpf(:, p_i)), '-', 'color', colors(1,:), ...
         'HandleVisibility', 'off');
end

% IFF with parallel stiffness
plot(real(pole(G_kp({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff_kp)),  imag(pole(G_kp({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff_kp)), 'x', 'color', colors(2,:), ...
     'DisplayName', 'IFF with $k_p$', 'MarkerSize', 8);
plot(real(tzero(G_kp({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff_kp)),  imag(tzero(G_kp({'fu', 'fv'}, {'Fu', 'Fv'})*Kiff_kp)), 'o', 'color', colors(2,:), ...
     'HandleVisibility', 'off', 'MarkerSize', 8);
for p_i = 1:size(poles_iff_kp, 2)
    plot(real(poles_iff_kp(:, p_i)), imag(poles_iff_kp(:, p_i)), '-', 'color', colors(2,:), ...
         'HandleVisibility', 'off');
end
% RDC
plot(real(pole(G({'Du', 'Dv'}, {'Fu', 'Fv'})*Krdc)),  imag(pole(G({'Du', 'Dv'}, {'Fu', 'Fv'})*Krdc)), 'x', 'color', colors(3,:), ...
     'DisplayName', 'RDC', 'MarkerSize', 8);
plot(real(tzero(G({'Du', 'Dv'}, {'Fu', 'Fv'})*Krdc)),  imag(tzero(G({'Du', 'Dv'}, {'Fu', 'Fv'})*Krdc)), 'o', 'color', colors(3,:), ...
     'HandleVisibility', 'off', 'MarkerSize', 8);
for p_i = 1:size(poles_rdc, 2)
    plot(real(poles_rdc(:, p_i)), imag(poles_rdc(:, p_i)), '-', 'color', colors(3,:), ...
         'HandleVisibility', 'off');
end
hold off;
axis equal;
xlim([-1.15, 0.05]); ylim([0, 1.2]);
xticks([-1, -0.8, -0.6, -0.4, -0.2 , 0]);
xticklabels({'-$\omega_0$', '', '', '', '', '$0$'});
yticks([0, 0.2, 0.4, 0.6, 0.8, 1]);
yticklabels({'$0$', '', '', '', '', '$\omega_0$'});

xlabel('Real Part'); ylabel('Imaginary Part');
leg = legend('location', 'northwest', 'FontSize', 8);
leg.ItemTokenSize(1) = 12;

exportFig('figs/rotating_comp_techniques_root_locus_inkscape.pdf', 'width', 600, 'height', 600, 'png', false, 'pdf', false, 'svg', true);

xlim([-0.14, 0.02]); ylim([0, 0.2]);

exportFig('figs/rotating_comp_techniques_root_locus_zoom.pdf', 'width', 600, 'height', 600, 'png', false, 'pdf', false, 'svg', true);
#+end_src

#+name: fig:rotating_comp_techniques
#+caption: Comparison of active damping techniques for rotating platform
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:rotating_comp_techniques_root_locus}Root Locus}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_comp_techniques_root_locus.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_comp_techniques_dampled_plants}Damped plants}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_comp_techniques_dampled_plants.png]]
#+end_subfigure
#+end_figure

** Obtained Damped Plant
The actively damped plants are computed for the three techniques and compared in Figure ref:fig:rotating_comp_techniques_dampled_plants.
It is shown that while the diagonal (direct) terms of the damped plants are similar for the three active damping techniques, the off-diagonal (coupling) terms are not.
Integral Force Feedback strategy is adding some coupling at low frequency which may negatively impact the positioning performance.

#+begin_src matlab
%% Compute Damped plants
G_cl_iff    = feedback(G,    Kiff,    'name');
G_cl_iff_kp = feedback(G_kp, Kiff_kp, 'name');
G_cl_rdc    = feedback(G,    Krdc,    'name');
#+end_src

#+begin_src matlab :exports none :results none
%% Comparison of the damped plants obtained with the three active damping techniques
freqs = logspace(-2, 2, 1000);

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

% Magnitude
ax1 = nexttile([2, 1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G(          'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros(1,3), ...
    'DisplayName', 'OL')
plot(freqs, abs(squeeze(freqresp(G_cl_iff(   'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:), ...
    'DisplayName', 'IFF + HPF')
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(2,:), ...
    'DisplayName', 'IFF + $k_p$')
plot(freqs, abs(squeeze(freqresp(G_cl_rdc(   'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(3,:), ...
    'DisplayName', 'RDC')
plot(freqs, abs(squeeze(freqresp(G(          'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [zeros(1,3), 0.5], ...
    'DisplayName', 'Coupling')
plot(freqs, abs(squeeze(freqresp(G_cl_iff(   'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(1,:), 0.5], ...
    'DisplayName', 'Coupling')
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp('Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(2,:), 0.5], ...
    'DisplayName', 'Coupling')
plot(freqs, abs(squeeze(freqresp(G_cl_rdc(   'Dv', 'Fu'), freqs, 'rad/s'))), '-', 'color', [colors(3,:), 0.5], ...
    'DisplayName', 'Coupling')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
ldg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
ldg.ItemTokenSize = [10, 1];
ylim([1e-6, 1e2])

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G(          'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', zeros(1,3))
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_iff(   'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(1,:))
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_iff_kp('Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(2,:))
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cl_rdc(   'Du', 'Fu'), freqs, 'rad/s'))), '-', 'color', colors(3,:))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([-180 15]);

linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
xticks([1e-2,1e-1,1,1e1,1e2])
xticklabels({'$0.01 \omega_0$', '$0.1 \omega_0$', '$\omega_0$', '$10 \omega_0$', '$100 \omega_0$'})
#+end_src

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

** Transmissibility And Compliance
The proposed active damping techniques are now compared in terms of closed-loop transmissibility and compliance.
The transmissibility is here defined as the transfer function from a displacement of the rotating stage along $\vec{i}_x$ to the displacement of the payload along the same direction.
It is used to characterize how much vibration is transmitted through the suspended platform to the payload.
The compliance describes the displacement response of the payload to external forces applied to it.
This is a useful metric when disturbances are directly applied to the payload.
It is here defined as the transfer function from external forces applied on the payload along $\vec{i}_x$ to the displacement of the payload along the same direction.

Very similar results are obtained for the two proposed IFF modifications in terms of transmissibility and compliance (Figure ref:fig:rotating_comp_techniques_trans_compliance).
Using IFF degrades the compliance at low frequency while using relative damping control degrades the transmissibility at high frequency.
This is very well known characteristics of these common active damping techniques that holds when applied to rotating platforms.

#+begin_src matlab :exports none :results none
%% Comparison of the obtained transmissibilty and compliance for the three tested active damping techniques
freqs = logspace(-2, 2, 1000);

% transmissibility
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G(          'Dx', 'Dfx'), freqs, 'rad/s'))), '-', 'color', zeros(1,3), ...
    'DisplayName', 'OL')
plot(freqs, abs(squeeze(freqresp(G_cl_iff(   'Dx', 'Dfx'), freqs, 'rad/s'))), '-', 'color', colors(1,:), ...
    'DisplayName', 'IFF + HPF')
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp('Dx', 'Dfx'), freqs, 'rad/s'))), '-', 'color', colors(2,:), ...
    'DisplayName', 'IFF + $k_p$')
plot(freqs, abs(squeeze(freqresp(G_cl_rdc(   'Dx', 'Dfx'), freqs, 'rad/s'))), '-', 'color', colors(3,:), ...
    'DisplayName', 'RDC')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [rad/s]'); ylabel('Transmissibility [m/m]');
xlim([freqs(1), freqs(end)]);
#+end_src

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

#+begin_src matlab :exports none :results none
% Compliance
figure;
ax1 = nexttile();
hold on;
plot(freqs, abs(squeeze(freqresp(G(          'Dx', 'Fdx'), freqs, 'rad/s'))), '-', 'color', zeros(1,3), ...
    'DisplayName', 'OL')
plot(freqs, abs(squeeze(freqresp(G_cl_iff(   'Dx', 'Fdx'), freqs, 'rad/s'))), '-', 'color', colors(1,:), ...
    'DisplayName', 'IFF + HPF')
plot(freqs, abs(squeeze(freqresp(G_cl_iff_kp('Dx', 'Fdx'), freqs, 'rad/s'))), '-', 'color', colors(2,:), ...
    'DisplayName', 'IFF + $k_p$')
plot(freqs, abs(squeeze(freqresp(G_cl_rdc(   'Dx', 'Fdx'), freqs, 'rad/s'))), '-', 'color', colors(3,:), ...
    'DisplayName', 'RDC')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [rad/s]'); ylabel('Compliance [m/N]');
ldg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
ldg.ItemTokenSize = [10, 1];
xlim([freqs(1), freqs(end)]);
#+end_src

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

#+name: fig:rotating_comp_techniques_trans_compliance
#+caption: Comparison of the obtained transmissibilty (\subref{fig:rotating_comp_techniques_transmissibility}) and compliance (\subref{fig:rotating_comp_techniques_compliance}) for the three tested active damping techniques
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:rotating_comp_techniques_transmissibility}Transmissibility}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_comp_techniques_transmissibility.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_comp_techniques_compliance}Compliance}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_comp_techniques_compliance.png]]
#+end_subfigure
#+end_figure

* Rotating Nano-Hexapod
:PROPERTIES:
:header-args:matlab+: :tangle matlab/rotating_7_nano_hexapod.m
:END:
<<sec:rotating_nano_hexapod>>
** Introduction                                                      :ignore:
The previous analysis is now applied on a model representing the rotating nano-hexapod.
Three nano-hexapod stiffnesses are tested as for the uniaxial model: $k_n = \SI{0.01}{\N\per\mu\m}$, $k_n = \SI{1}{\N\per\mu\m}$ and $k_n = \SI{100}{\N\per\mu\m}$.
Only the maximum rotating velocity is here considered ($\Omega = \SI{60}{rpm}$) with the light sample ($m_s = \SI{1}{kg}$) as this is the worst identified case scenario in terms of gyroscopic effects.

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

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

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

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

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

#+begin_src matlab
%% Simscape model name
mdl = 'rotating_model';
#+end_src

** Identify NASS dynamics                                          :noexport:
#+begin_src matlab
%% Nano-Hexapod
mn = 15; % Nano-Hexapod mass [kg]

%% Light Sample
ms = 1; % Sample Mass [kg]

%% General Configuration
model_config = struct();
model_config.controller = "open_loop"; % Default: Open-Loop
model_config.Tuv_type   = "normal";    % Default: 2DoF stage
#+end_src

#+begin_src matlab
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/controller'],        1, 'openinput');  io_i = io_i + 1; % [Fu, Fv]
io(io_i) = linio([mdl, '/fd'],                1, 'openinput');  io_i = io_i + 1; % [Fdx, Fdy]
io(io_i) = linio([mdl, '/xf'],                1, 'openinput');  io_i = io_i + 1; % [Dfx, Dfy]
io(io_i) = linio([mdl, '/translation_stage'], 1, 'openoutput'); io_i = io_i + 1; % [Fmu, Fmv]
io(io_i) = linio([mdl, '/translation_stage'], 2, 'openoutput'); io_i = io_i + 1; % [Du, Dv]
io(io_i) = linio([mdl, '/ext_metrology'],     1, 'openoutput'); io_i = io_i + 1; % [Dx, Dy]
#+end_src

#+begin_src matlab
%% Voice Coil (i.e. soft) Nano-Hexapod
kn = 1e4; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

Wz = 0; % Rotating Velocity [rad/s]
G_vc_norot = linearize(mdl, io, 0.0);
G_vc_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
G_vc_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

Wz = 2*pi; % Rotating Velocity [rad/s]
G_vc_fast = linearize(mdl, io, 0.0);
G_vc_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
G_vc_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

%% APA (i.e. relatively stiff) Nano-Hexapod
kn = 1e6; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

Wz = 0; % Rotating Velocity [rad/s]
G_md_norot = linearize(mdl, io, 0.0);
G_md_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
G_md_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

Wz = 2*pi; % Rotating Velocity [rad/s]
G_md_fast = linearize(mdl, io, 0.0);
G_md_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
G_md_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

%% Piezoelectric (i.e. stiff) Nano-Hexapod
kn = 1e8; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

Wz = 0; % Rotating Velocity [rad/s]
G_pz_norot = linearize(mdl, io, 0.0);
G_pz_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
G_pz_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

Wz = 2*pi; % Rotating Velocity [rad/s]
G_pz_fast = linearize(mdl, io, 0.0);
G_pz_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
G_pz_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
#+end_src

** Nano-Active-Stabilization-System - Plant Dynamics
For the NASS, the maximum rotating velocity is $\Omega = \SI[parse-numbers=false]{2\pi}{\radian\per\s}$ for a suspended mass on top of the nano-hexapod's actuators equal to $m_n + m_s = \SI{16}{\kilo\gram}$.
The parallel stiffness corresponding to the centrifugal forces is $m \Omega^2 \approx \SI{0.6}{\newton\per\mm}$.

#+begin_src matlab
%% Compute negative spring in [N/m]
Kneg_light = (15+1)*(2*pi)^2;
#+end_src

The transfer functions from nano-hexapod actuator force $F_u$ to the displacement of the nano-hexapod in the same direction $d_u$ as well as in the orthogonal direction $d_v$ (coupling) are shown in Figure ref:fig:rotating_nano_hexapod_dynamics for all three considered nano-hexapod stiffnesses.
The soft nano-hexapod is the most affected by the rotation.
This can be seen by the large shift of the resonance frequencies, and by the induced coupling which is larger than for the stiffer nano-hexapods.
The coupling (or interaction) in a MIMO $2 \times 2$ system can be visually estimated as the ratio between the diagonal term and the off-diagonal terms (see corresponding Appendix).

#+begin_src matlab :results none
%% Effect of rotation on the nano-hexapod dynamics
freqs = logspace(0, 1, 1000);

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

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_vc_norot('Du', 'Fu'), freqs, 'Hz'))), '--',  'color', colors(1,:), ...
    'DisplayName', '$\Omega = 0\,$rpm, $D_u/F_u$');
plot(freqs, abs(squeeze(freqresp(G_vc_fast( 'Du', 'Fu'), freqs, 'Hz'))), '-' ,  'color', colors(1,:), ...
    'DisplayName', '$\Omega = 60\,$rpm, $D_u/F_u$');
plot(freqs, abs(squeeze(freqresp(G_vc_fast( 'Dv', 'Fu'), freqs, 'Hz'))), '-' , 'color', [colors(1,:), 0.5], ...
    'DisplayName', '$\Omega = 60\,$rpm, $D_v/F_u$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-6, 1e-2])

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_vc_norot('Du', 'Fu'), freqs, 'Hz'))), '--', 'color', colors(1,:));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_vc_fast( 'Du', 'Fu'), freqs, 'Hz'))), '-' , 'color', colors(1,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-180, 0]);

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

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/rotating_nano_hexapod_dynamics_vc.pdf', 'width', 'third', 'height', 600);
#+end_src

#+begin_src matlab :results none
%% Effect of rotation on the nano-hexapod dynamics
freqs = logspace(1, 2, 1000);

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

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_md_norot('Du', 'Fu'), freqs, 'Hz'))), '--',  'color', colors(2,:), ...
    'DisplayName', '$\Omega = 0\,$rpm, $D_u/F_u$');
plot(freqs, abs(squeeze(freqresp(G_md_fast( 'Du', 'Fu'), freqs, 'Hz'))), '-' ,  'color', colors(2,:), ...
    'DisplayName', '$\Omega = 60\,$rpm, $D_u/F_u$');
plot(freqs, abs(squeeze(freqresp(G_md_fast( 'Dv', 'Fu'), freqs, 'Hz'))), '-' , 'color', [colors(2,:), 0.5], ...
    'DisplayName', '$\Omega = 60\,$rpm, $D_v/F_u$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-8, 1e-4])

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_md_norot('Du', 'Fu'), freqs, 'Hz'))), '--', 'color', colors(2,:));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_md_fast( 'Du', 'Fu'), freqs, 'Hz'))), '-' , 'color', colors(2,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-180, 0]);

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

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/rotating_nano_hexapod_dynamics_md.pdf', 'width', 'third', 'height', 600);
#+end_src

#+begin_src matlab :results none
%% Effect of rotation on the nano-hexapod dynamics
freqs = logspace(2, 3, 1000);

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

ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(G_pz_norot('Du', 'Fu'), freqs, 'Hz'))), '--',  'color', colors(3,:), ...
    'DisplayName', '$\Omega = 0\,$rpm, $D_u/F_u$');
plot(freqs, abs(squeeze(freqresp(G_pz_fast( 'Du', 'Fu'), freqs, 'Hz'))), '-' ,  'color', colors(3,:), ...
    'DisplayName', '$\Omega = 60\,$rpm, $D_u/F_u$');
plot(freqs, abs(squeeze(freqresp(G_pz_fast( 'Dv', 'Fu'), freqs, 'Hz'))), '-' , 'color', [colors(3,:), 0.5], ...
    'DisplayName', '$\Omega = 60\,$rpm, $D_v/F_u$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-10, 1e-6])

ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz_norot('Du', 'Fu'), freqs, 'Hz'))), '--', 'color', colors(3,:));
plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz_fast( 'Du', 'Fu'), freqs, 'Hz'))), '-' , 'color', colors(3,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-180, 0]);

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

#+begin_src matlab :tangle no :exports results :results file none
exportFig('figs/rotating_nano_hexapod_dynamics_pz.pdf', 'width', 'third', 'height', 600);
#+end_src

#+name: fig:rotating_nano_hexapod_dynamics
#+caption: Effect of rotation on the nano-hexapod dynamics. Dashed lines are the plants without rotation, solid lines are plants at maximum rotating velocity ($\Omega = 60\,\text{rpm}$), and shaded lines are coupling terms at maximum rotating velocity
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:uniaxial_damped_plant_three_active_damping_techniques_vc}$k_n = 0.01\,N/\mu m$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_nano_hexapod_dynamics_vc.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_nano_hexapod_dynamics_md}$k_n = 1\,N/\mu m$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_nano_hexapod_dynamics_md.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_nano_hexapod_dynamics_pz}$k_n = 100\,N/\mu m$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_nano_hexapod_dynamics_pz.png]]
#+end_subfigure
#+end_figure

** Optimal IFF with High Pass Filter
Integral Force Feedback with an added High Pass Filter is applied to the three nano-hexapods.
First, the parameters ($\omega_i$ and $g$) of the IFF controller that yield best simultaneous damping are determined from Figure ref:fig:rotating_iff_hpf_nass_optimal_gain.
The IFF parameters are chosen as follow:
- for $k_n = \SI{0.01}{\N\per\mu\m}$ (Figure ref:fig:rotating_iff_hpf_nass_optimal_gain): $\omega_i$ is chosen such that the maximum damping is achieved while the gain is less than half of the maximum gain at which the system is unstable.
  This is done to have some control robustness.
- for $k_n = \SI{1}{\N\per\mu\m}$ and $k_n = \SI{100}{\N\per\mu\m}$ (Figure ref:fig:rotating_iff_hpf_nass_optimal_gain_md and ref:fig:rotating_iff_hpf_nass_optimal_gain_pz): the largest $\omega_i$ is chosen such that obtained damping is $\SI{95}{\percent}$ of the maximum achievable damping.
  Large $\omega_i$ is chosen here to limit the loss of compliance and the increase of coupling at low frequency as was shown in Section ref:sec:rotating_iff_pseudo_int.
The obtained IFF parameters and the achievable damping are visually shown by large dots in Figure ref:fig:rotating_iff_hpf_nass_optimal_gain and are summarized in Table ref:tab:rotating_iff_hpf_opt_iff_hpf_params_nass.

#+begin_src matlab
%% Compute the optimal control gain
wis = logspace(-2, 3, 200); % [rad/s]

opt_iff_hpf_xi_vc = zeros(1, length(wis)); % Optimal simultaneous damping
opt_iff_hpf_gain_vc = zeros(1, length(wis)); % Corresponding optimal gain

opt_iff_hpf_xi_md = zeros(1, length(wis)); % Optimal simultaneous damping
opt_iff_hpf_gain_md = zeros(1, length(wis)); % Corresponding optimal gain

opt_iff_hpf_xi_pz = zeros(1, length(wis)); % Optimal simultaneous damping
opt_iff_hpf_gain_pz = zeros(1, length(wis)); % Corresponding optimal gain

for wi_i = 1:length(wis)
    wi = wis(wi_i);
    Kiff = 1/(s + wi)*eye(2);

    fun = @(g)computeSimultaneousDamping(g, G_vc_fast({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff);

    [g_opt, xi_opt] = fminsearch(fun, 0.1);
    opt_iff_hpf_xi_vc(wi_i) = 1/xi_opt;
    opt_iff_hpf_gain_vc(wi_i) = g_opt;

    fun = @(g)computeSimultaneousDamping(g, G_md_fast({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff);

    [g_opt, xi_opt] = fminsearch(fun, 0.1);
    opt_iff_hpf_xi_md(wi_i) = 1/xi_opt;
    opt_iff_hpf_gain_md(wi_i) = g_opt;

    fun = @(g)computeSimultaneousDamping(g, G_pz_fast({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff);

    [g_opt, xi_opt] = fminsearch(fun, 0.1);
    opt_iff_hpf_xi_pz(wi_i) = 1/xi_opt;
    opt_iff_hpf_gain_pz(wi_i) = g_opt;
end
#+end_src

#+begin_src matlab
%% Find optimal parameters with at least a gain margin of 2
i_iff_hpf_vc = find(opt_iff_hpf_gain_vc < 0.5*(wis*((sqrt(1e4/16)/(2*pi))^2 - 1)));
i_iff_hpf_vc = i_iff_hpf_vc(1);

i_iff_hpf_md = find(opt_iff_hpf_xi_md > 0.95*max(opt_iff_hpf_xi_md));
i_iff_hpf_md = i_iff_hpf_md(end)+1;

i_iff_hpf_pz = find(opt_iff_hpf_xi_pz > 0.95*max(opt_iff_hpf_xi_pz));
i_iff_hpf_pz = i_iff_hpf_pz(end)+1;
#+end_src

#+begin_src matlab :results none
%% Optimal modified IFF parameters that yields maximum simultaneous damping
figure;
yyaxis left
hold on;
plot(wis, opt_iff_hpf_xi_vc, '-', 'DisplayName', '$\xi_{cl}$');
plot(wis(i_iff_hpf_vc), opt_iff_hpf_xi_vc(i_iff_hpf_vc), '.', 'MarkerSize', 15, 'HandleVisibility', 'off');
hold off;
set(gca, 'YScale', 'lin');
ylim([0,1]);
ylabel('Damping Ratio $\xi$');

yyaxis right
hold on;
plot(wis, opt_iff_hpf_gain_vc, '-', 'DisplayName', '$g_{opt}$');
plot(wis, wis*((sqrt(1e4/16)/(2*pi))^2 - 1), '--', 'DisplayName', '$g_{max}$');
plot(wis(i_iff_hpf_vc), opt_iff_hpf_gain_vc(i_iff_hpf_vc), '.', 'MarkerSize', 15, 'HandleVisibility', 'off');
set(gca, 'YScale', 'lin');
ylim([0,200]);
xlabel('$\omega_i$ [rad/s]');
set(gca, 'YTickLabel',[]);
ylabel('Controller gain $g$');
set(gca, 'XScale', 'log');
xticks([1e-2,1,1e2])
legend('location', 'northwest', 'FontSize', 8);
#+end_src

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

#+begin_src matlab :results none
figure;
yyaxis left
hold on;
plot(wis, opt_iff_hpf_xi_md, '-');
plot(wis(i_iff_hpf_md), opt_iff_hpf_xi_md(i_iff_hpf_md), '.', 'MarkerSize', 15, 'HandleVisibility', 'off');
hold off;
set(gca, 'YScale', 'lin');
ylim([0,1]);
ylabel('Damping Ratio $\xi$');

yyaxis right
hold on;
plot(wis, opt_iff_hpf_gain_md, '-');
plot(wis(i_iff_hpf_md), opt_iff_hpf_gain_md(i_iff_hpf_md), '.', 'MarkerSize', 15, 'HandleVisibility', 'off');
plot(wis, wis*((sqrt(1e6/16)/(2*pi))^2 - 1), '--');
set(gca, 'YScale', 'lin');
ylim([0,1000]);
xlabel('$\omega_i$ [rad/s]');
ylabel('Controller gain $g$');
set(gca, 'YTickLabel',[]);
set(gca, 'XScale', 'log');
xticks([1e-2,1,1e2])
#+end_src

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

#+begin_src matlab :results none
figure;
yyaxis left
hold on;
plot(wis, opt_iff_hpf_xi_pz, '-');
plot(wis(i_iff_hpf_pz), opt_iff_hpf_xi_pz(i_iff_hpf_pz), '.', 'MarkerSize', 15, 'HandleVisibility', 'off');
hold off;
set(gca, 'YScale', 'lin');
ylim([0,1]);
ylabel('Damping Ratio $\xi$');

yyaxis right
hold on;
plot(wis, opt_iff_hpf_gain_pz, '-');
plot(wis(i_iff_hpf_pz), opt_iff_hpf_gain_pz(i_iff_hpf_pz), '.', 'MarkerSize', 15, 'HandleVisibility', 'off');
plot(wis, wis*((sqrt(1e8/16)/(2*pi))^2 - 1), '--');
set(gca, 'YScale', 'lin');
ylim([0,10000]);
xlabel('$\omega_i$ [rad/s]');
set(gca, 'YTickLabel',[]);
ylabel('Controller gain $g$');
set(gca, 'XScale', 'log');
xticks([1e-2,1,1e2])
#+end_src

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

#+name: fig:rotating_iff_hpf_nass_optimal_gain
#+caption: For each value of $\omega_i$, the maximum damping ratio $\xi$ is computed (blue) and the corresponding controller gain is shown (in red). The choosen controller parameters used for further analysis are shown by the large dots.
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:rotating_iff_hpf_nass_optimal_gain_vc}$k_n = 0.01\,N/\mu m$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_iff_hpf_nass_optimal_gain_vc.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_iff_hpf_nass_optimal_gain_md}$k_n = 1\,N/\mu m$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_iff_hpf_nass_optimal_gain_md.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_iff_hpf_nass_optimal_gain_pz}$k_n = 100\,N/\mu m$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_iff_hpf_nass_optimal_gain_pz.png]]
#+end_subfigure
#+end_figure

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([wis(i_iff_hpf_vc), opt_iff_hpf_gain_vc(i_iff_hpf_vc), opt_iff_hpf_xi_vc(i_iff_hpf_vc); wis(i_iff_hpf_md), opt_iff_hpf_gain_md(i_iff_hpf_md), opt_iff_hpf_xi_md(i_iff_hpf_md); wis(i_iff_hpf_pz), opt_iff_hpf_gain_pz(i_iff_hpf_pz), opt_iff_hpf_xi_pz(i_iff_hpf_pz)], {'$k_n = 0.01\,N/\mu m$', '$k_n = 1\,N/\mu m$', '$k_n = 100\,N/\mu m$'}, {'$\omega_i$', '$g$', '$\xi$'}, ' %.2f ');
#+end_src

#+name: tab:rotating_iff_hpf_opt_iff_hpf_params_nass
#+caption: Obtained optimal parameters ($\omega_i$ and $g$) for the modified IFF controller including a high pass filter. The corresponding achievable simultaneous damping of the two modes $\xi$ is also shown.
#+attr_latex: :environment tabularx :width 0.4\linewidth :align Xccc
#+attr_latex: :center t :booktabs t
#+RESULTS:
| $k_n$           | $\omega_i$ |  $g$ | $\xi_\text{opt}$ |
|-----------------+------------+------+------------------|
| $0.01\,N/\mu m$ |        7.3 |   51 |             0.45 |
| $1\,N/\mu m$    |         39 |  427 |             0.93 |
| $100\,N/\mu m$  |        500 | 3775 |             0.94 |

** Optimal IFF with Parallel Stiffness
For each considered nano-hexapod stiffness, the parallel stiffness $k_p$ is varied from $k_{p,\text{min}} = m\Omega^2$ (the minimum stiffness that yields unconditional stability) to $k_{p,\text{max}} = k_n$ (the total nano-hexapod stiffness).
In order to keep the overall stiffness constant, the actuator stiffness $k_a$ is decreased when $k_p$ is increased ($k_a = k_n - k_p$, with $k_n$ the total nano-hexapod stiffness).
A high pass filter is also added to limit the low frequency gain with a cut-off frequency $\omega_i$ equal to one tenth of the system resonance ($\omega_i = \omega_0/10$).

The achievable maximum simultaneous damping of all the modes is computed as a function of the parallel stiffnesses (Figure ref:fig:rotating_iff_kp_nass_optimal_gain).
It is shown that the soft nano-hexapod cannot yield good damping as the parallel stiffness cannot be made large enough compared to the negative stiffness induced by the rotation.
For the two stiff options, the achievable damping decreases when the parallel stiffness is chosen too high as explained in Section ref:sec:rotating_iff_parallel_stiffness.
Such behavior can be explain by the fact that the achievable damping can be approximated by the distance between the open-loop pole and the open-loop zero [[cite:&preumont18_vibrat_contr_activ_struc_fourt_edition chapt 7.2]].
This distance is larger for stiff nano-hexapod as the open-loop pole will be at higher frequencies while the open-loop zero, which depends on the value of the parallel stiffness, can only be made large for stiff nano-hexapods.

Let's choose $k_p = 1\,N/mm$, $k_p = 0.01\,N/\mu m$ and $k_p = 1\,N/\mu m$ for the three considered nano-hexapods.
The corresponding optimal controller gains and achievable damping are summarized in Table ref:tab:rotating_iff_kp_opt_iff_kp_params_nass.

#+begin_src matlab
%% Maximum rotating velocity
Wz = 2*pi; % [rad/s]

%% Minimum parallel stiffness
kp_min = (mn + ms) * Wz^2; % [N/m]

%% Parameters for simulation
mn = 15; % Nano-Hexapod mass [kg]
ms = 1; % Sample Mass [kg]

%% IFF Controller
Kiff_vc = 1/(s + 0.1*sqrt(1e4/(mn+ms)))*eye(2); % IFF
Kiff_md = 1/(s + 0.1*sqrt(1e6/(mn+ms)))*eye(2); % IFF
Kiff_pz = 1/(s + 0.1*sqrt(1e8/(mn+ms)))*eye(2); % IFF

%% General Configuration
model_config = struct();
model_config.controller = "open_loop"; % Default: Open-Loop
model_config.Tuv_type   = "parallel_k";    % Default: 2DoF stage

%% Computes the optimal parameters and attainable simultaneous damping - Voice Coil nano-hexapod
kps_vc = logspace(log10(kp_min), log10(1e4), 100); % Tested parallel stiffnesses [N/m]
kps_vc(end) = [];

opt_iff_kp_xi_vc   = zeros(1, length(kps_vc)); % Optimal simultaneous damping
opt_iff_kp_gain_vc = zeros(1, length(kps_vc)); % Corresponding optimal gain

for kp_i = 1:length(kps_vc)
    % Voice Coil Nano-Hexapod
    kp = kps_vc(kp_i);
    cp = 2*0.001*sqrt((ms + mn)*kp);
    kn = 1e4 - kp; % Nano-Hexapod Stiffness [N/m]
    cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

    % Identify dynamics
    Giff_vc = linearize(mdl, io, 0);
    Giff_vc.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
    Giff_vc.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

    fun = @(g)computeSimultaneousDamping(g, Giff_vc({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff_vc);

    [g_opt, xi_opt] = fminsearch(fun, 0.1);
    opt_iff_kp_xi_vc(kp_i) = 1/xi_opt;
    opt_iff_kp_gain_vc(kp_i) = g_opt;
end

%% Computes the optimal parameters and attainable simultaneous damping - APA nano-hexapod
kps_md = logspace(log10(kp_min), log10(1e6), 100); % Tested parallel stiffnesses [N/m]
kps_md(end) = [];

opt_iff_kp_xi_md   = zeros(1, length(kps_md)); % Optimal simultaneous damping
opt_iff_kp_gain_md = zeros(1, length(kps_md)); % Corresponding optimal gain


for kp_i = 1:length(kps_md)
    % Voice Coil Nano-Hexapod
    kp = kps_md(kp_i);
    cp = 2*0.001*sqrt((ms + mn)*kp);
    kn = 1e6 - kp; % Nano-Hexapod Stiffness [N/m]
    cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

    % Identify dynamics
    Giff_md = linearize(mdl, io, 0);
    Giff_md.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
    Giff_md.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

    fun = @(g)computeSimultaneousDamping(g, Giff_md({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff_md);

    [g_opt, xi_opt] = fminsearch(fun, 0.1);
    opt_iff_kp_xi_md(kp_i) = 1/xi_opt;
    opt_iff_kp_gain_md(kp_i) = g_opt;
end

%% Computes the optimal parameters and attainable simultaneous damping - Piezo nano-hexapod
kps_pz = logspace(log10(kp_min), log10(1e8), 100); % Tested parallel stiffnesses [N/m]
kps_pz(end) = [];

opt_iff_kp_xi_pz   = zeros(1, length(kps_pz)); % Optimal simultaneous damping
opt_iff_kp_gain_pz = zeros(1, length(kps_pz)); % Corresponding optimal gain


for kp_i = 1:length(kps_pz)
    % Voice Coil Nano-Hexapod
    kp = kps_pz(kp_i);
    cp = 2*0.001*sqrt((ms + mn)*kp);
    kn = 1e8 - kp; % Nano-Hexapod Stiffness [N/m]
    cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

    % Identify dynamics
    Giff_pz = linearize(mdl, io, 0);
    Giff_pz.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
    Giff_pz.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

    fun = @(g)computeSimultaneousDamping(g, Giff_pz({'fu', 'fv'}, {'Fu', 'Fv'}), Kiff_pz);

    [g_opt, xi_opt] = fminsearch(fun, 0.1);
    opt_iff_kp_xi_pz(kp_i) = 1/xi_opt;
    opt_iff_kp_gain_pz(kp_i) = g_opt;
end
#+end_src

#+begin_src matlab
%% Find result with wanted parallel stiffness
[~, i_kp_vc] = min(abs(kps_vc - 1e3));
[~, i_kp_md] = min(abs(kps_md - 1e4));
[~, i_kp_pz] = min(abs(kps_pz - 1e6));

%% Identify plants with choosen Parallel stiffnesses
model_config.Tuv_type   = "parallel_k";    % Default: 2DoF stage

% Voice Coil
kp = 1e3;
cp = 2*0.001*sqrt((ms + mn)*kp);
kn = 1e4-kp; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

% Identify dynamics
Wz = 2*pi; % [rad/s]
G_vc_kp_fast = linearize(mdl, io, 0);
G_vc_kp_fast.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
G_vc_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

Wz = 0; % [rad/s]
G_vc_kp_norot = linearize(mdl, io, 0);
G_vc_kp_norot.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
G_vc_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

% APA
kp = 1e4;
cp = 2*0.001*sqrt((ms + mn)*kp);
kn = 1e6 - kp; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

% Identify dynamics
Wz = 2*pi; % [rad/s]
G_md_kp_fast = linearize(mdl, io, 0);
G_md_kp_fast.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
G_md_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

Wz = 0; % [rad/s]
G_md_kp_norot = linearize(mdl, io, 0);
G_md_kp_norot.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
G_md_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

% Piezo
kp = 1e6;
cp = 2*0.001*sqrt((ms + mn)*kp);
kn = 1e8 - kp; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

% Identify dynamics
Wz = 2*pi; % [rad/s]
G_pz_kp_fast = linearize(mdl, io, 0);
G_pz_kp_fast.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
G_pz_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

Wz = 0; % [rad/s]
G_pz_kp_norot = linearize(mdl, io, 0);
G_pz_kp_norot.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy'};
G_pz_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};
#+end_src

#+begin_src matlab :results none
%% Optimal IFF gain and associated simultaneous damping as a function of the parallel stiffness
figure;
hold on;
plot(kps_vc, opt_iff_kp_xi_vc, '-', ...
     'color', colors(1,:), 'DisplayName', '$k_n = 0.01\,N/\mu m$');
plot(kps_vc(i_kp_vc), opt_iff_kp_xi_vc(i_kp_vc), '.', ...
     'color', colors(1,:), 'MarkerSize', 15, 'HandleVisibility', 'off');
plot(kps_md, opt_iff_kp_xi_md, '-', ...
     'color', colors(2,:), 'DisplayName', '$k_n = 1\,N/\mu m$');
plot(kps_md(i_kp_md), opt_iff_kp_xi_md(i_kp_md), '.', ...
     'color', colors(2,:), 'MarkerSize', 15, 'HandleVisibility', 'off');
plot(kps_pz, opt_iff_kp_xi_pz, '-', ...
     'color', colors(3,:), 'DisplayName', '$k_n = 100\,N/\mu m$');
plot(kps_pz(i_kp_pz), opt_iff_kp_xi_pz(i_kp_pz), '.', ...
     'color', colors(3,:), 'MarkerSize', 15, 'HandleVisibility', 'off');
hold off;
xlabel('$k_p [N/m]$');
ylabel('Damping Ratio $\xi$');
set(gca, 'XScale', 'log');
set(gca, 'YScale', 'lin');
ylim([0,1]);
yticks([0:0.2:1])
legend('location', 'southeast', 'FontSize', 8);
xlim([kps_pz(1), kps_pz(end)])
#+end_src

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

#+attr_latex: :options [t]{0.49\linewidth}
#+begin_minipage
#+name: fig:rotating_iff_kp_nass_optimal_gain
#+attr_latex: :width \linewidth :float nil
#+caption: Maximum damping $\xi$ as a function of the parallel stiffness $k_p$
[[file:figs/rotating_iff_kp_nass_optimal_gain.png]]
#+end_minipage
\hfill
#+attr_latex: :options [b]{0.45\linewidth}
#+begin_minipage
#+caption: Obtained optimal parameters for the IFF controller when using parallel stiffnesses
#+name: tab:rotating_iff_kp_opt_iff_kp_params_nass
#+attr_latex: :environment tabularx :width \linewidth :placement [b] :align Xccc
#+attr_latex: :booktabs t :float nil
#+RESULTS:
| $k_n$           | $k_p$           |     $g$ | $\xi_{\text{opt}}$ |
|-----------------+-----------------+---------+--------------------|
| $0.01\,N/\mu m$ | $1\,N/mm$       |    47.9 |               0.44 |
| $1\,N/\mu m$    | $0.01\,N/\mu m$ |  465.57 |               0.97 |
| $100\,N/\mu m$  | $1\,N/\mu m$    | 4624.25 |                0.99 |
#+end_minipage

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([opt_iff_kp_gain_vc(i_kp_vc), opt_iff_kp_xi_vc(i_kp_vc); opt_iff_kp_gain_md(i_kp_md), opt_iff_kp_xi_md(i_kp_md); opt_iff_kp_gain_pz(i_kp_pz), opt_iff_kp_xi_pz(i_kp_pz)], {'$k_n = 0.01\,N/\mu m$', '$k_n = 1\,N/\mu m$', '$k_n = 100\,N/\mu m$'}, {'$g$', '$\xi_{\text{opt}}$'}, ' %.2f ');
#+end_src

** Optimal Relative Motion Control
For each considered nano-hexapod stiffness, relative damping control is applied and the achievable damping ratio as a function of the controller gain is computed (Figure ref:fig:rotating_rdc_optimal_gain).
The gain is chosen is chosen such that 99% of modal damping is obtained (obtained gains are summarized in Table ref:tab:rotating_rdc_opt_params_nass).

#+begin_src matlab
%% Computes the optimal parameters and attainable simultaneous damping - Piezo nano-hexapod
rdc_gains = 2*logspace(1, 5, 200);
% Obtained simultaneous damping
rdc_xi_vc = zeros(1, length(rdc_gains));
rdc_xi_md = zeros(1, length(rdc_gains));
rdc_xi_pz = zeros(1, length(rdc_gains));

Krdc = s*eye(2);
Krdc.InputName  = {'Du', 'Dv'};
Krdc.OutputName = {'Fu', 'Fv'};

for g_i = 1:length(rdc_gains)
    [~, xi] = damp(feedback(G_vc_fast({'Du', 'Dv'}, {'Fu', 'Fv'}), rdc_gains(g_i)*Krdc));
    rdc_xi_vc(g_i) = min(xi);

    [~, xi] = damp(feedback(G_md_fast({'Du', 'Dv'}, {'Fu', 'Fv'}), rdc_gains(g_i)*Krdc));
    rdc_xi_md(g_i) = min(xi);

    [~, xi] = damp(feedback(G_pz_fast({'Du', 'Dv'}, {'Fu', 'Fv'}), rdc_gains(g_i)*Krdc));
    rdc_xi_pz(g_i) = min(xi);
end
#+end_src

#+begin_src matlab
%% Optimal RDC
[~, i_rdc_vc] = min(abs(rdc_xi_vc - 0.99));
[~, i_rdc_md] = min(abs(rdc_xi_md - 0.99));
[~, i_rdc_pz] = min(abs(rdc_xi_pz - 0.99));

Krdc_vc = rdc_gains(i_rdc_vc)*Krdc;
Krdc_md = rdc_gains(i_rdc_md)*Krdc;
Krdc_pz = rdc_gains(i_rdc_pz)*Krdc;
#+end_src

#+begin_src matlab :results none
%% Optimal IFF gain and associated simultaneous damping as a function of the parallel stiffness
figure;
hold on;
plot(rdc_gains, rdc_xi_vc, '-', ...
     'color', colors(1,:), 'DisplayName', '$k_n = 0.01\,N/\mu m$');
plot(rdc_gains(i_rdc_vc), rdc_xi_vc(i_rdc_vc), '.', ...
     'color', colors(1,:), 'MarkerSize', 15, 'HandleVisibility', 'off');
plot(rdc_gains, rdc_xi_md, '-', ...
     'color', colors(2,:), 'DisplayName', '$k_n = 1\,N/\mu m$');
plot(rdc_gains(i_rdc_md), rdc_xi_md(i_rdc_md), '.', ...
     'color', colors(2,:), 'MarkerSize', 15, 'HandleVisibility', 'off');
plot(rdc_gains, rdc_xi_pz, '-', ...
     'color', colors(3,:), 'DisplayName', '$k_n = 100\,N/\mu m$');
plot(rdc_gains(i_rdc_pz), rdc_xi_pz(i_rdc_pz), '.', ...
     'color', colors(3,:), 'MarkerSize', 15, 'HandleVisibility', 'off');
hold off;
xlabel('Relative Damping Controller gain $g$');
ylabel('Damping Ratio $\xi$');
set(gca, 'XScale', 'log');
set(gca, 'YScale', 'lin');
ylim([0,1]);
yticks([0:0.2:1])
xlim([rdc_gains(1), rdc_gains(end)])
legend('location', 'southeast', 'FontSize', 8);
#+end_src

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

#+attr_latex: :options [t]{0.49\linewidth}
#+begin_minipage
#+name: fig:rotating_rdc_optimal_gain
#+attr_latex: :width \linewidth :float nil
#+caption: Maximum damping $\xi$ as a function of the RDC gain $g$
[[file:figs/rotating_rdc_optimal_gain.png]]
#+end_minipage
\hfill
#+attr_latex: :options [b]{0.45\linewidth}
#+begin_minipage
#+caption: Obtained optimal parameters for the RDC
#+name: tab:rotating_rdc_opt_params_nass
#+attr_latex: :environment tabularx :width 0.8\linewidth :placement [b] :align Xcc
#+attr_latex: :booktabs t :float nil
#+RESULTS:
| $k_n$           |   $g$ | $\xi_{\text{opt}}$ |
|-----------------+-------+--------------------|
| $0.01\,N/\mu m$ |  1600 |               0.99 |
| $1\,N/\mu m$    |  8200 |               0.99 |
| $100\,N/\mu m$  | 80000 |               0.99 |
#+end_minipage

** Comparison of the obtained damped plants
Now that optimal parameters for the three considered active damping techniques have been determined, the obtained damped plants are computed and compared in Figure ref:fig:rotating_nass_damped_plant_comp.

Similarly to what was concluded in previous analysis:
- acrshort:iff adds coupling below the resonance frequency as compared to the open-loop and acrshort:rdc cases
- All three methods are yielding good damping, except for acrshort:iff applied on the soft nano-hexapod
- Coupling is smaller for stiff nano-hexapods

#+begin_src matlab :tangle no
%% Saved controllers
save('./matlab/mat/nass_controllers.mat', ...
     'Kiff_hpf_vc', 'Kiff_hpf_md', 'Kiff_hpf_pz', ...
     'Kiff_kp_vc', 'Kiff_kp_md', 'Kiff_kp_pz', ...
     'Krdc_vc', 'Krdc_md', 'Krdc_pz');
#+end_src

#+begin_src matlab :tangle no
%% Load controllers
load('nass_controllers.mat');
#+end_src

#+begin_src matlab
%% Closed Loop Plants - IFF with HPF
G_vc_norot_iff_hpf = feedback(G_vc_norot, Kiff_hpf_vc, 'name');
G_vc_fast_iff_hpf  = feedback(G_vc_fast,  Kiff_hpf_vc, 'name');

G_md_norot_iff_hpf = feedback(G_md_norot, Kiff_hpf_md, 'name');
G_md_fast_iff_hpf  = feedback(G_md_fast,  Kiff_hpf_md, 'name');

G_pz_norot_iff_hpf = feedback(G_pz_norot, Kiff_hpf_pz, 'name');
G_pz_fast_iff_hpf  = feedback(G_pz_fast,  Kiff_hpf_pz, 'name');

%% Closed Loop Plants - IFF with Parallel Stiffness
G_vc_norot_iff_kp = feedback(G_vc_kp_norot, Kiff_kp_vc, 'name');
G_vc_fast_iff_kp  = feedback(G_vc_kp_fast,  Kiff_kp_vc, 'name');

G_md_norot_iff_kp = feedback(G_md_kp_norot, Kiff_kp_md, 'name');
G_md_fast_iff_kp  = feedback(G_md_kp_fast,  Kiff_kp_md, 'name');

G_pz_norot_iff_kp = feedback(G_pz_kp_norot, Kiff_kp_pz, 'name');
G_pz_fast_iff_kp  = feedback(G_pz_kp_fast,  Kiff_kp_pz, 'name');

%% Closed Loop Plants - RDC
G_vc_norot_rdc = feedback(G_vc_norot, Krdc_vc, 'name');
G_vc_fast_rdc  = feedback(G_vc_fast,  Krdc_vc, 'name');

G_md_norot_rdc = feedback(G_md_norot, Krdc_md, 'name');
G_md_fast_rdc  = feedback(G_md_fast,  Krdc_md, 'name');

G_pz_norot_rdc = feedback(G_pz_norot, Krdc_pz, 'name');
G_pz_fast_rdc  = feedback(G_pz_fast,  Krdc_pz, 'name');
#+end_src

#+begin_src matlab :exports none :results none
%% Comparison of the damped plants (direct and coupling terms) for the three proposed active damping techniques (IFF with HPF, IFF with $k_p$ and RDC) applied on the three nano-hexapod stiffnesses
freqs_vc = logspace(-1, 2, 1000);

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

ax1 = nexttile([2,1]);
hold on;
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [zeros(1,3)]);
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast( 'Dv', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [zeros(1,3), 0.5]);
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_hpf( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(1,:)]);
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_hpf( 'Dv', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(1,:), 0.5]);
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_kp( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(2,:)]);
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_kp( 'Dv', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(2,:), 0.5]);
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_rdc( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(3,:)]);
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_rdc( 'Dv', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(3,:), 0.5]);
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-8, 1e-2])

ax2 = nexttile;
hold on;
plot(freqs_vc, 180/pi*angle(squeeze(freqresp(G_vc_fast( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [zeros(1,3)]);
plot(freqs_vc, 180/pi*angle(squeeze(freqresp(G_vc_fast_iff_hpf( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(1,:)]);
plot(freqs_vc, 180/pi*angle(squeeze(freqresp(G_vc_fast_iff_kp( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(2,:)]);
plot(freqs_vc, 180/pi*angle(squeeze(freqresp(G_vc_fast_rdc( 'Du', 'Fu'), freqs_vc, 'Hz'))), '-' , 'color', [colors(3,:)]);
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-180, 0]);

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

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

#+begin_src matlab :exports none :results none
freqs_md = logspace(0, 3, 1000);

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

ax1 = nexttile([2,1]);
hold on;
plot(freqs_md, abs(squeeze(freqresp(G_md_fast( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [zeros(1,3)]);
plot(freqs_md, abs(squeeze(freqresp(G_md_fast( 'Dv', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [zeros(1,3), 0.5]);
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_hpf( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(1,:)]);
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_hpf( 'Dv', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(1,:), 0.5]);
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_kp( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(2,:)]);
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_kp( 'Dv', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(2,:), 0.5]);
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_rdc( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(3,:)]);
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_rdc( 'Dv', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(3,:), 0.5]);
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-10, 1e-4])

ax2 = nexttile;
hold on;
plot(freqs_md, 180/pi*angle(squeeze(freqresp(G_md_fast( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [zeros(1,3)]);
plot(freqs_md, 180/pi*angle(squeeze(freqresp(G_md_fast_iff_hpf( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(1,:)]);
plot(freqs_md, 180/pi*angle(squeeze(freqresp(G_md_fast_iff_kp( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(2,:)]);
plot(freqs_md, 180/pi*angle(squeeze(freqresp(G_md_fast_rdc( 'Du', 'Fu'), freqs_md, 'Hz'))), '-' , 'color', [colors(3,:)]);
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-180, 0]);

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

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

#+begin_src matlab :exports none :results none
freqs_pz = logspace(0, 3, 1000);

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

ax1 = nexttile([2,1]);
hold on;
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [zeros(1,3)], ...
    'DisplayName', 'OL');
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast( 'Dv', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [zeros(1,3), 0.5], ...
    'DisplayName', 'Coupling');
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_hpf( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(1,:)], ...
    'DisplayName', 'IFF + HPF');
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_hpf( 'Dv', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(1,:), 0.5], ...
    'HandleVisibility', 'off');
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_kp( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(2,:)], ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_kp( 'Dv', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(2,:), 0.5], ...
    'HandleVisibility', 'off');
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_rdc( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(3,:)], ...
    'DisplayName', 'RDC');
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_rdc( 'Dv', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(3,:), 0.5], ...
    'HandleVisibility', 'off');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-12, 1e-6])
ldg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
ldg.ItemTokenSize = [20, 1];

ax2 = nexttile;
hold on;
plot(freqs_pz, 180/pi*angle(squeeze(freqresp(G_pz_fast( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [zeros(1,3)]);
plot(freqs_pz, 180/pi*angle(squeeze(freqresp(G_pz_fast_iff_hpf( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(1,:)]);
plot(freqs_pz, 180/pi*angle(squeeze(freqresp(G_pz_fast_iff_kp( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(2,:)]);
plot(freqs_pz, 180/pi*angle(squeeze(freqresp(G_pz_fast_rdc( 'Du', 'Fu'), freqs_pz, 'Hz'))), '-' , 'color', [colors(3,:)]);
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-180, 0]);

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

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

#+name: fig:rotating_nass_damped_plant_comp
#+caption: Comparison of the damped plants for the three proposed active damping techniques (IFF with HPF in blue, IFF with $k_p$ in red and RDC in yellow). The direct terms are shown by the solid lines and coupling terms are shown by the shaded lines. Three nano-hexapod stiffnesses are considered. For this analysis the rotating velocity is $\Omega = 60\,\text{rpm}$ and the suspended mass is $m_n + m_s = \SI{16}{\kg}$.
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:rotating_nass_damped_plant_comp_vc}$k_n = 0.01\,N/\mu m$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_nass_damped_plant_comp_vc.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_nass_damped_plant_comp_md}$k_n = 1\,N/\mu m$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_nass_damped_plant_comp_md.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_nass_damped_plant_comp_pz}$k_n = 100\,N/\mu m$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_nass_damped_plant_comp_pz.png]]
#+end_subfigure
#+end_figure

* Nano-Active-Stabilization-System with rotation
:PROPERTIES:
:header-args:matlab+: :tangle matlab/rotating_8_nass.m
:END:
<<sec:rotating_nass>>
** Introduction                                                      :ignore:
Up until now, the model used to study gyroscopic effects consisted of an infinitely stiff rotating stage with a X-Y suspended stage on top.
While quite simplistic, this allowed to study the effects of rotation and the associated limitations when active damping is to be applied.
In this section, the limited compliance of the micro-station is taken into account as well as the rotation of the spindle.

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

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

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

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

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

#+begin_src matlab
%% Nano-Hexapod on top of Micro-Station model
mdl = 'nass_rotating_model';

%% Load micro-station parameters
load('uniaxial_micro_station_parameters.mat')

%% Load controllers
load('nass_controllers.mat');
#+end_src

** Nano Active Stabilization System model
In order to have a more realistic dynamics model of the NASS, the 2-DoF nano-hexapod (modelled as shown in Figure ref:fig:rotating_3dof_model_schematic) is now located on top of a model of the micro-station including (see Figure ref:fig:rotating_nass_model for a 3D view):
- the floor whose motion is imposed
- a 2-DoF granite ($k_{g,x} = k_{g,y} = \SI{950}{\N\per\mu\m}$, $m_g = \SI{2500}{\kg}$)
- a 2-DoF $T_y$ stage ($k_{t,x} = k_{t,y} = \SI{520}{\N\per\mu\m}$, $m_g = \SI{600}{\kg}$)
- a spindle (vertical rotation) stage whose rotation is imposed ($m_s = \SI{600}{\kg}$)
- a 2-DoF micro-hexapod ($k_{h,x} = k_{h,y} = \SI{61}{\N\per\mu\m}$, $m_g = \SI{15}{\kg}$)

A payload is rigidly fixed to the nano-hexapod and the $x,y$ motion of the payload is measured with respect to the granite.

#+name: fig:rotating_nass_model
#+caption: 3D view of the Nano-Active-Stabilization-System model.
#+attr_latex: :scale 0.7
[[file:figs/rotating_nass_model.png]]

** System dynamics

The dynamics of the un-damped and damped plants are identified using the optimal parameters found in Section ref:sec:rotating_nano_hexapod.
The obtained dynamics are compared in Figure ref:fig:rotating_nass_plant_comp_stiffness where the direct terms are shown by the solid curves while the coupling terms are shown by the shaded ones.
It can be observed that:
- Coupling (quantified by the ratio between the off-diagonal and direct terms) is higher for the soft nano-hexapod
- Damping added by the three proposed techniques is quite high and the obtained plant is rather easy to control
- There is some coupling between nano-hexapod and micro-station dynamics for the stiff nano-hexapod (mode at 200Hz)
- The two proposed IFF modification yields similar results

#+begin_src matlab
%% System parameters
mn = 15; % Nano-Hexapod mass [kg]
ms = 1; % Sample Mass [kg]

% General Configuration
model_config = struct();
model_config.controller = "open_loop"; % Default: Open-Loop
model_config.Tuv_type   = "normal";    % Default: 2DoF stage

% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/controller'],   1, 'openinput');  io_i = io_i + 1; % Actuator Forces [Fu, Fv]
io(io_i) = linio([mdl, '/fd'],           1, 'openinput');  io_i = io_i + 1; % Direct Forces on Sample [Fdx, Fdy]
io(io_i) = linio([mdl, '/xf'],           1, 'openinput');  io_i = io_i + 1; % Floor Motion [Dfx, Dfy]
io(io_i) = linio([mdl, '/ft'],           1, 'openinput');  io_i = io_i + 1; % Micro-Station Disturbances [Ftx, Fty]
io(io_i) = linio([mdl, '/nano_hexapod'], 1, 'openoutput'); io_i = io_i + 1; % [Fmu, Fmv]
io(io_i) = linio([mdl, '/nano_hexapod'], 2, 'openoutput'); io_i = io_i + 1; % [Du, Dv]
io(io_i) = linio([mdl, '/ext_metrology'],1, 'openoutput'); io_i = io_i + 1; % [Dx, Dy]

%% Identify plant without parallel stiffness
% Voice Coil (i.e. soft) Nano-Hexapod
kn = 1e4; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

Wz = 0; % Rotating Velocity [rad/s]
G_vc_norot = linearize(mdl, io, 0.0);
G_vc_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
G_vc_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

Wz = 2*pi; % Rotating Velocity [rad/s]
G_vc_fast = linearize(mdl, io, 0.0);
G_vc_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
G_vc_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

% APA (i.e. relatively stiff) Nano-Hexapod
kn = 1e6; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

Wz = 0; % Rotating Velocity [rad/s]
G_md_norot = linearize(mdl, io, 0.0);
G_md_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
G_md_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

Wz = 2*pi; % Rotating Velocity [rad/s]
G_md_fast = linearize(mdl, io, 0.0);
G_md_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
G_md_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

% Piezoelectric (i.e. stiff) Nano-Hexapod
kn = 1e8; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.005*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

Wz = 0; % Rotating Velocity [rad/s]
G_pz_norot = linearize(mdl, io, 0.0);
G_pz_norot.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
G_pz_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

Wz = 2*pi; % Rotating Velocity [rad/s]
G_pz_fast = linearize(mdl, io, 0.0);
G_pz_fast.InputName = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
G_pz_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

%% Identify plants with Parallel stiffnesses
model_config.Tuv_type   = "parallel_k";    % Default: 2DoF stage

% Voice Coil
kp = 1e3;
cp = 2*0.001*sqrt((ms + mn)*kp);
kn = 1e4-kp; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

% Identify dynamics
Wz = 2*pi; % [rad/s]
G_vc_kp_fast = linearize(mdl, io, 0);
G_vc_kp_fast.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
G_vc_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

Wz = 0; % [rad/s]
G_vc_kp_norot = linearize(mdl, io, 0);
G_vc_kp_norot.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
G_vc_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

% APA
kp = 1e4;
cp = 2*0.001*sqrt((ms + mn)*kp);
kn = 1e6 - kp; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

% Identify dynamics
Wz = 2*pi; % [rad/s]
G_md_kp_fast = linearize(mdl, io, 0);
G_md_kp_fast.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
G_md_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

Wz = 0; % [rad/s]
G_md_kp_norot = linearize(mdl, io, 0);
G_md_kp_norot.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
G_md_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

% Piezo
kp = 1e5;
cp = 2*0.001*sqrt((ms + mn)*kp);
kn = 1e8 - kp; % Nano-Hexapod Stiffness [N/m]
cn = 2*0.01*sqrt((ms + mn)*kn); % Nano-Hexapod Damping [N/(m/s)]

% Identify dynamics
Wz = 2*pi; % [rad/s]
G_pz_kp_fast = linearize(mdl, io, 0);
G_pz_kp_fast.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
G_pz_kp_fast.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

Wz = 0; % [rad/s]
G_pz_kp_norot = linearize(mdl, io, 0);
G_pz_kp_norot.InputName  = {'Fu', 'Fv', 'Fdx', 'Fdy', 'Dfx', 'Dfy', 'Ftx', 'Fty'};
G_pz_kp_norot.OutputName = {'fu', 'fv', 'Du', 'Dv', 'Dx', 'Dy'};

%% Compute dampepd plants
% Closed Loop Plants - IFF with HPF
G_vc_norot_iff_hpf = feedback(G_vc_norot, Kiff_hpf_vc, 'name');
G_vc_fast_iff_hpf  = feedback(G_vc_fast,  Kiff_hpf_vc, 'name');

G_md_norot_iff_hpf = feedback(G_md_norot, Kiff_hpf_md, 'name');
G_md_fast_iff_hpf  = feedback(G_md_fast,  Kiff_hpf_md, 'name');

G_pz_norot_iff_hpf = feedback(G_pz_norot, Kiff_hpf_pz, 'name');
G_pz_fast_iff_hpf  = feedback(G_pz_fast,  Kiff_hpf_pz, 'name');

% Closed Loop Plants - IFF with Parallel Stiffness
G_vc_norot_iff_kp = feedback(G_vc_kp_norot, Kiff_kp_vc, 'name');
G_vc_fast_iff_kp  = feedback(G_vc_kp_fast,  Kiff_kp_vc, 'name');

G_md_norot_iff_kp = feedback(G_md_kp_norot, Kiff_kp_md, 'name');
G_md_fast_iff_kp  = feedback(G_md_kp_fast,  Kiff_kp_md, 'name');

G_pz_norot_iff_kp = feedback(G_pz_kp_norot, Kiff_kp_pz, 'name');
G_pz_fast_iff_kp  = feedback(G_pz_kp_fast,  Kiff_kp_pz, 'name');

% Closed Loop Plants - RDC
G_vc_norot_rdc = feedback(G_vc_norot, Krdc_vc, 'name');
G_vc_fast_rdc  = feedback(G_vc_fast,  Krdc_vc, 'name');

G_md_norot_rdc = feedback(G_md_norot, Krdc_md, 'name');
G_md_fast_rdc  = feedback(G_md_fast,  Krdc_md, 'name');

G_pz_norot_rdc = feedback(G_pz_norot, Krdc_pz, 'name');
G_pz_fast_rdc  = feedback(G_pz_fast,  Krdc_pz, 'name');
#+end_src

#+begin_src matlab :exports none :results none
%% Bode plot of the transfer function from nano-hexapod actuator to measured motion by the external metrology
freqs_vc = logspace(-1, 2, 1000);

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

ax1 = nexttile([2,1]);
hold on;
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast('Dx', 'Fu'), freqs_vc, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast('Dy', 'Fu'), freqs_vc, 'Hz'))), 'color', [zeros(1,3), 0.5], ...
    'DisplayName', 'Coupling');
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dx', 'Fu'), freqs_vc, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dy', 'Fu'), freqs_vc, 'Hz'))), 'color', [colors(1,:), 0.5], ...
    'HandleVisibility', 'off');
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dx', 'Fu'), freqs_vc, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dy', 'Fu'), freqs_vc, 'Hz'))), 'color', [colors(2,:), 0.5], ...
    'HandleVisibility', 'off');
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_rdc('Dx', 'Fu'), freqs_vc, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_rdc('Dy', 'Fu'), freqs_vc, 'Hz'))), 'color', [colors(3,:), 0.5], ...
    'HandleVisibility', 'off');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-12, 1e-2])

ax2 = nexttile;
hold on;
plot(freqs_vc, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_fast('Dx', 'Fu'), freqs_vc, 'Hz')))), 'color', zeros(1,3));
plot(freqs_vc, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_fast_iff_hpf('Dx', 'Fu'), freqs_vc, 'Hz')))), 'color', colors(1,:));
plot(freqs_vc, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_fast_iff_kp('Dx', 'Fu'), freqs_vc, 'Hz')))), 'color', colors(2,:));
plot(freqs_vc, 180/pi*unwrap(angle(squeeze(freqresp(G_vc_fast_rdc('Dx', 'Fu'), freqs_vc, 'Hz')))), 'color', colors(3,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-270, 90]);

linkaxes([ax,ax2],'x');
xlim([freqs_vc(1), freqs_vc(end)]);
xticks([1e-1, 1e0, 1e1]);
#+end_src

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

#+begin_src matlab :exports none :results none
freqs_md = logspace(0, 3, 1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs_md, abs(squeeze(freqresp(G_md_fast('Dx', 'Fu'), freqs_md, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs_md, abs(squeeze(freqresp(G_md_fast('Dy', 'Fu'), freqs_md, 'Hz'))), 'color', [zeros(1,3), 0.5], ...
    'DisplayName', 'Coupling');
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dx', 'Fu'), freqs_md, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dy', 'Fu'), freqs_md, 'Hz'))), 'color', [colors(1,:), 0.5], ...
    'HandleVisibility', 'off');
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_kp('Dx', 'Fu'), freqs_md, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_kp('Dy', 'Fu'), freqs_md, 'Hz'))), 'color', [colors(2,:), 0.5], ...
    'HandleVisibility', 'off');
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_rdc('Dx', 'Fu'), freqs_md, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_rdc('Dy', 'Fu'), freqs_md, 'Hz'))), 'color', [colors(3,:), 0.5], ...
    'HandleVisibility', 'off');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-12, 1e-2])

ax2 = nexttile;
hold on;
plot(freqs_md, 180/pi*unwrap(angle(squeeze(freqresp(G_md_fast('Dx', 'Fu'), freqs_md, 'Hz')))), 'color', zeros(1,3));
plot(freqs_md, 180/pi*unwrap(angle(squeeze(freqresp(G_md_fast_iff_hpf('Dx', 'Fu'), freqs_md, 'Hz')))), 'color', colors(1,:));
plot(freqs_md, 180/pi*unwrap(angle(squeeze(freqresp(G_md_fast_iff_kp('Dx', 'Fu'), freqs_md, 'Hz')))), 'color', colors(2,:));
plot(freqs_md, 180/pi*unwrap(angle(squeeze(freqresp(G_md_fast_rdc('Dx', 'Fu'), freqs_md, 'Hz')))), 'color', colors(3,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-270, 90]);

linkaxes([ax1,ax2],'x');
xlim([freqs_md(1), freqs_md(end)]);
xticks([1e0, 1e1, 1e2]);
#+end_src

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

#+begin_src matlab :exports none :results none
freqs_pz = logspace(0, 3, 1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast('Dx', 'Fu'), freqs_pz, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast('Dy', 'Fu'), freqs_pz, 'Hz'))), 'color', [zeros(1,3), 0.5], ...
    'DisplayName', 'Coupling');
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dx', 'Fu'), freqs_pz, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dy', 'Fu'), freqs_pz, 'Hz'))), 'color', [colors(1,:), 0.5], ...
    'HandleVisibility', 'off');
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dx', 'Fu'), freqs_pz, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dy', 'Fu'), freqs_pz, 'Hz'))), 'color', [colors(2,:), 0.5], ...
    'HandleVisibility', 'off');
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_rdc('Dx', 'Fu'), freqs_pz, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_rdc('Dy', 'Fu'), freqs_pz, 'Hz'))), 'color', [colors(3,:), 0.5], ...
    'HandleVisibility', 'off');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-12, 1e-2])
ldg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
ldg.ItemTokenSize = [20, 1];

ax2 = nexttile;
hold on;
plot(freqs_pz, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_fast('Dx', 'Fu'), freqs_pz, 'Hz')))), 'color', zeros(1,3));
plot(freqs_pz, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_fast_iff_hpf('Dx', 'Fu'), freqs_pz, 'Hz')))), 'color', colors(1,:));
plot(freqs_pz, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_fast_iff_kp('Dx', 'Fu'), freqs_pz, 'Hz')))), 'color', colors(2,:));
plot(freqs_pz, 180/pi*unwrap(angle(squeeze(freqresp(G_pz_fast_rdc('Dx', 'Fu'), freqs_pz, 'Hz')))), 'color', colors(3,:));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);
ylim([-270, 90]);

linkaxes([ax1,ax2],'x');
xlim([freqs_pz(1), freqs_pz(end)]);
xticks([1e0, 1e1, 1e2]);
#+end_src

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

#+name: fig:rotating_nass_plant_comp_stiffness
#+caption: Bode plot of the transfer function from nano-hexapod actuator to measured motion by the external metrology
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:rotating_nass_plant_comp_stiffness_vc}$k_n = 0.01\,N/\mu m$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_nass_plant_comp_stiffness_vc.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_nass_plant_comp_stiffness_md}$k_n = 1\,N/\mu m$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_nass_plant_comp_stiffness_md.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_nass_plant_comp_stiffness_pz}$k_n = 100\,N/\mu m$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_nass_plant_comp_stiffness_pz.png]]
#+end_subfigure
#+end_figure

To confirm that the coupling is smaller when the stiffness of the nano-hexapod is increase, the /coupling ratio/ for the three nano-hexapod stiffnesses are shown in Figure ref:fig:rotating_nass_plant_coupling_comp.

#+begin_src matlab :exports none :results none
%% Coupling ratio for the proposed active damping techniques evaluated for the three nano-hexapod stiffnesses
freqs_vc = logspace(-1, 2, 1000);
figure;
hold on;
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast('Dy', 'Fu')/G_vc_fast('Dx', 'Fu'), freqs_vc, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dy', 'Fu')/G_vc_fast_iff_hpf('Dx', 'Fu'), freqs_vc, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dy', 'Fu')/G_vc_fast_iff_kp('Dx', 'Fu'), freqs_vc, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs_vc, abs(squeeze(freqresp(G_vc_fast_rdc('Dy', 'Fu')/G_vc_fast_rdc('Dx', 'Fu'), freqs_vc, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Coupling Ratio');
ylim([1e-4, 1e2]);
xticks([1e-1, 1e0, 1e1]);
#+end_src

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

#+begin_src matlab :exports none :results none
freqs_md = logspace(0, 3, 1000);
figure;
hold on;
plot(freqs_md, abs(squeeze(freqresp(G_md_fast('Dy', 'Fu')/G_md_fast('Dx', 'Fu'), freqs_md, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dy', 'Fu')/G_md_fast_iff_hpf('Dx', 'Fu'), freqs_md, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_iff_kp('Dy', 'Fu')/G_md_fast_iff_kp('Dx', 'Fu'), freqs_md, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs_md, abs(squeeze(freqresp(G_md_fast_rdc('Dy', 'Fu')/G_md_fast_rdc('Dx', 'Fu'), freqs_md, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Coupling Ratio');
ylim([1e-4, 1e2]);
xticks([1e0, 1e1, 1e2]);
#+end_src

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

#+begin_src matlab :exports none :results none
freqs_pz = logspace(0, 3, 1000);
figure;
hold on;
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast('Dy', 'Fu')/G_pz_fast('Dx', 'Fu'), freqs_pz, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dy', 'Fu')/G_pz_fast_iff_hpf('Dx', 'Fu'), freqs_pz, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dy', 'Fu')/G_pz_fast_iff_kp('Dx', 'Fu'), freqs_pz, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs_pz, abs(squeeze(freqresp(G_pz_fast_rdc('Dy', 'Fu')/G_pz_fast_rdc('Dx', 'Fu'), freqs_pz, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Coupling Ratio');
ldg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
ldg.ItemTokenSize = [20, 1];
ylim([1e-4, 1e2]);
xticks([1e0, 1e1, 1e2]);
#+end_src

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

#+name: fig:rotating_nass_plant_coupling_comp
#+caption: Coupling ratio for the proposed active damping techniques evaluated for the three nano-hexapod stiffnesses
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:rotating_nass_plant_coupling_comp_vc}$k_n = 0.01\,N/\mu m$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_nass_plant_coupling_comp_vc.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_nass_plant_coupling_comp_md}$k_n = 1\,N/\mu m$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_nass_plant_coupling_comp_md.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_nass_plant_coupling_comp_pz}$k_n = 100\,N/\mu m$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_nass_plant_coupling_comp_pz.png]]
#+end_subfigure
#+end_figure

** Effect of disturbances

The effect of three disturbances are considered:
- Floor motion (Figure ref:fig:rotating_nass_effect_floor_motion)
- Micro-Station vibrations (Figure ref:fig:rotating_nass_effect_stage_vibration)
- Direct force applied on the payload (Figure ref:fig:rotating_nass_effect_direct_forces)

#+begin_important
Conclusions are similar than with the uniaxial (non-rotating) model:
- Regarding the effect of floor motion and forces applied on the payload:
  - The stiffer, the better (magnitudes are lower for the right curves, Figures ref:fig:rotating_nass_effect_floor_motion and ref:fig:rotating_nass_effect_direct_forces)
  - Integral Force Feedback degrades the performance at low frequency compared to relative damping control
- Regarding the effect of micro-station vibrations:
  - Having a soft nano-hexapod allows to filter these vibrations between the suspensions modes of the nano-hexapod and some flexible modes of the micro-station. Using relative damping control reduce this filtering (Figure ref:fig:rotating_nass_effect_stage_vibration, left).
#+end_important

#+begin_src matlab :exports none :results none
%% Effect of Floor motion on the position error - Comparison of active damping techniques for the three nano-hexapod stiffnesses
freqs = logspace(-1, 3, 1000);

figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_vc_fast('Dx', 'Dfx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs, abs(squeeze(freqresp(G_vc_fast_rdc('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/D_{f,x}$ [m/N]');
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
xtickangle(0)
ylim([1e-4, 1e2]);
#+end_src

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

#+begin_src matlab :exports none :results none
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_md_fast('Dx', 'Dfx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_kp('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs, abs(squeeze(freqresp(G_md_fast_rdc('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/D_{f,x}$ [m/N]');
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
xtickangle(0)
ylim([1e-4, 1e2]);
#+end_src

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

#+begin_src matlab :exports none :results none
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_pz_fast('Dx', 'Dfx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs, abs(squeeze(freqresp(G_pz_fast_rdc('Dx', 'Dfx'), freqs, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/D_{f,x}$ [m/N]');
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
xtickangle(0)
ldg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
ldg.ItemTokenSize = [15, 1];
ylim([1e-4, 1e2]);
#+end_src

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

#+name: fig:rotating_nass_effect_floor_motion
#+caption: Effect of Floor motion on the position error - Comparison of active damping techniques for the three nano-hexapod stiffnesses
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:rotating_nass_effect_floor_motion_vc}$k_n = 0.01\,N/\mu m$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_nass_effect_floor_motion_vc.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_nass_effect_floor_motion_md}$k_n = 1\,N/\mu m$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_nass_effect_floor_motion_md.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_nass_effect_floor_motion_pz}$k_n = 100\,N/\mu m$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_nass_effect_floor_motion_pz.png]]
#+end_subfigure
#+end_figure

#+begin_src matlab :exports none :results none
%% Effect of micro-station vibrations on the position error - Comparison of active damping techniques for the three nano-hexapod stiffnesses
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_vc_fast('Dx', 'Ftx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs, abs(squeeze(freqresp(G_vc_fast_rdc('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{t,x}$ [m/N]');
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
xtickangle(0)
ylim([1e-12, 2e-7]);
#+end_src

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

#+begin_src matlab :exports none :results none
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_md_fast('Dx', 'Ftx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_kp('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs, abs(squeeze(freqresp(G_md_fast_rdc('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{t,x}$ [m/N]');
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
xtickangle(0)
ylim([1e-12, 2e-7]);
#+end_src

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

#+begin_src matlab :exports none :results none
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_pz_fast('Dx', 'Ftx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs, abs(squeeze(freqresp(G_pz_fast_rdc('Dx', 'Ftx'), freqs, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{t,x}$ [m/N]');
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
xtickangle(0)
ldg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
ldg.ItemTokenSize = [20, 1];
ylim([1e-12, 2e-7]);
#+end_src

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

#+name: fig:rotating_nass_effect_stage_vibration
#+caption: Effect of micro-station vibrations on the position error - Comparison of active damping techniques for the three nano-hexapod stiffnesses
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:rotating_nass_effect_stage_vibration_vc}$k_n = 0.01\,N/\mu m$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_nass_effect_stage_vibration_vc.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_nass_effect_stage_vibration_md}$k_n = 1\,N/\mu m$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_nass_effect_stage_vibration_md.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_nass_effect_stage_vibration_pz}$k_n = 100\,N/\mu m$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_nass_effect_stage_vibration_pz.png]]
#+end_subfigure
#+end_figure


#+begin_src matlab :exports none :results none
%% Effect of sample forces on the position error - Comparison of active damping techniques for the three nano-hexapod stiffnesses
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_vc_fast('Dx', 'Fdx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_hpf('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs, abs(squeeze(freqresp(G_vc_fast_iff_kp('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs, abs(squeeze(freqresp(G_vc_fast_rdc('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{s,x}$ [m/N]');
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
xtickangle(0)
ylim([1e-9, 1e-2])
#+end_src

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

#+begin_src matlab :exports none :results none
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_md_fast('Dx', 'Fdx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_hpf('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs, abs(squeeze(freqresp(G_md_fast_iff_kp('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs, abs(squeeze(freqresp(G_md_fast_rdc('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{s,x}$ [m/N]');
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
xtickangle(0)
ylim([1e-9, 1e-2])
#+end_src

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

#+begin_src matlab :exports none :results none
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_pz_fast('Dx', 'Fdx'), freqs, 'Hz'))), 'color', zeros(1,3), ...
    'DisplayName', 'OL');
plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_hpf('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(1,:), ...
    'DisplayName', 'IFF + $k_p$');
plot(freqs, abs(squeeze(freqresp(G_pz_fast_iff_kp('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(2,:), ...
    'DisplayName', 'IFF + HPF');
plot(freqs, abs(squeeze(freqresp(G_pz_fast_rdc('Dx', 'Fdx'), freqs, 'Hz'))), 'color', colors(3,:), ...
    'DisplayName', 'RDC');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude $d_x/f_{s,x}$ [m/N]');
xticks([1e-1, 1e0, 1e1, 1e2, 1e3]);
xtickangle(0)
ldg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
ldg.ItemTokenSize = [20, 1];

linkaxes([ax1,ax2,ax3], 'y')
ylim([1e-9, 1e-2])
#+end_src

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

#+name: fig:rotating_nass_effect_direct_forces
#+caption: Effect of sample forces on the position error - Comparison of active damping techniques for the three nano-hexapod stiffnesses
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:rotating_nass_effect_direct_forces_vc}$k_n = 0.01\,N/\mu m$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_nass_effect_direct_forces_vc.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_nass_effect_direct_forces_md}$k_n = 1\,N/\mu m$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_nass_effect_direct_forces_md.png]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:rotating_nass_effect_direct_forces_pz}$k_n = 100\,N/\mu m$}
#+attr_latex: :options {0.33\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/rotating_nass_effect_direct_forces_pz.png]]
#+end_subfigure
#+end_figure

* Conclusion
:PROPERTIES:
:UNNUMBERED: t
:END:

# - problem with voice coil actuator
# - Two solutions: add parallel stiffness, or change controller
# - Conclusion: minimum stiffness is required
# - APA is a nice architecture for parallel stiffness + integrated force sensor (have to speak about IFF before that)

In this study, the gyroscopic effects induced by the spindle's rotation have been studied using a spindle model (Section ref:sec:rotating_system_description).
Decentralized IFF with pure integrators was shown to be unstable when applied to rotating platforms (Section ref:sec:rotating_iff_pure_int).
Two modifications of the classical IFF control have been proposed to overcome this issue.

The first modification concerns the controller and consists of adding a high pass filter to the pure integrators.
This is equivalent to moving the controller pole to the left along the real axis.
This allows the closed loop system to be stable up to some value of the controller gain (Section ref:sec:rotating_iff_pseudo_int).

The second proposed modification concerns the mechanical system.
Additional springs are added in parallel with the actuators and force sensors.
It was shown that if the stiffness $k_p$ of the additional springs is larger than the negative stiffness $m \Omega^2$ induced by centrifugal forces, the classical decentralized IFF regains its unconditional stability property (Section ref:sec:rotating_iff_parallel_stiffness).

These two modifications were compared with relative damping control in Section ref:sec:rotating_comp_act_damp.
While having very different implementations, both proposed modifications were found to be very similar when it comes to the attainable damping and the obtained closed loop system behavior.

Then, this study has been applied to a rotating system that corresponds to the nano-hexapod parameters (Section ref:sec:rotating_nano_hexapod).
To be closer to the real system dynamics, the limited compliance of the micro-station has been taken into account.
Results show that the two proposed IFF modifications can be applied for the NASS even in the presence of spindle rotation.

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

* Glossary                                                            :ignore:
[[printglossaries:]]

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

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

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

** =rootLocusPolesSorted=
#+begin_src matlab :tangle matlab/src/rootLocusPolesSorted.m
  function [poles] = rootLocusPolesSorted(G, K, gains, args)
  % rootLocusPolesSorted -
  %
  % Syntax: [poles] = rootLocusPolesSorted(G, K, gains, args)
  %
  % Inputs:
  %    - G, K, gains, args -
  %
  % Outputs:
  %    - poles -

  arguments
      G
      K
      gains
      args.minreal double {mustBeNumericOrLogical} = false
      args.p_half  double {mustBeNumericOrLogical} = false
      args.d_max   double {mustBeNumeric} = -1
  end

  if args.minreal
      p1 = pole(minreal(feedback(G, gains(1)*K)));
      [~, i_uniq] = uniquetol([real(p1), imag(p1)], 1e-10, 'ByRows', true);
      p1 = p1(i_uniq);

      poles = zeros(length(p1), length(gains));
      poles(:, 1) = p1;
  else
      p1 = pole(feedback(G, gains(1)*K));
      [~, i_uniq] = uniquetol([real(p1), imag(p1)], 1e-10, 'ByRows', true);
      p1 = p1(i_uniq);

      poles = zeros(length(p1), length(gains));
      poles(:, 1) = p1;
  end

  if args.minreal
      p2 = pole(minreal(feedback(G, gains(2)*K)));
      [~, i_uniq] = uniquetol([real(p2), imag(p2)], 1e-10, 'ByRows', true);
      p2 = p2(i_uniq);
      poles(:, 2) = p2;
  else
      p2 = pole(feedback(G, gains(2)*K));
      [~, i_uniq] = uniquetol([real(p2), imag(p2)], 1e-10, 'ByRows', true);
      p2 = p2(i_uniq);
      poles(:, 2) = p2;
  end

  for g_i = 3:length(gains)
      % Estimated value of the poles
      poles_est = poles(:, g_i-1) + (poles(:, g_i-1) - poles(:, g_i-2))*(gains(g_i) - gains(g_i-1))/(gains(g_i-1) - gains(g_i - 2));

      % New values for the poles
      poles_gi = pole(feedback(G, gains(g_i)*K));
      [~, i_uniq] = uniquetol([real(poles_gi), imag(poles_gi)], 1e-10, 'ByRows', true);
      poles_gi = poles_gi(i_uniq);

      % Array of distances between all the poles
      poles_dist = sqrt((poles_est-poles_gi.').*conj(poles_est-poles_gi.'));

      % Get indices corresponding to distances from lowest to highest
      [~, c] = sort(min(poles_dist));

      as = 1:length(poles_gi);

      % for each column of poles_dist corresponding to the i'th pole
      % with closest previous poles
      for p_i = c
          % Get the indice a_i of the previous pole that is the closest
          % to pole c(p_i)
          [~, a_i] = min(poles_dist(:, p_i));

          poles(as(a_i), g_i) = poles_gi(p_i);

          % Remove old poles that are already matched
          % poles_gi(as(a_i), :) = [];
          poles_dist(a_i, :) = [];
          as(a_i) = [];
      end
  end


  if args.d_max > 0
      poles = poles(max(abs(poles(:, 2:end) - poles(:, 1:end-1))') > args.d_max, :);
  end

  if args.p_half
      poles = poles(1:round(end/2), :);
  end

  [~, s_p] = sort(imag(poles(:,1)), 'descend');
  poles = poles(s_p, :);

  poles = poles.';
#+end_src

** =computeSimultaneousDamping=
#+begin_src matlab :tangle matlab/src/computeSimultaneousDamping.m
function [xi_min] = computeSimultaneousDamping(g, G, K)
    [~, xi] = damp(minreal(feedback(G, g*K), [], false));
    xi_min = 1/min(xi);

    if xi_min < 0
        xi_min = 1e8;
    end
end
#+end_src