#+TITLE: Active Damping of Rotating Platforms using Integral Force Feedback - Matlab Computation
:DRAWER:
#+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-bib-compiler "biber"
#+BIND: org-latex-image-default-width ""

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

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

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

* Introduction                                                        :ignore:
This document gathers the Matlab code used to for the conference paper cite:dehaeze20_activ_dampin_rotat_platf_integ_force_feedb and the journal paper cite:dehaeze21_activ_dampin_rotat_platf_using.

It is structured in several sections:
- Section [[sec:system_description]]: presents a simple model of a rotating suspended platform that will be used throughout this study.
- Section [[sec:iff_pure_int]]: explains how the unconditional stability of IFF is lost due to Gyroscopic effects induced by the rotation.
- Section [[sec:iff_pseudo_int]]: suggests a simple modification of the control law such that damping can be added to the suspension modes in a robust way.
- Section [[sec:iff_parallel_stiffness]]: proposes to add springs in parallel with the force sensors to regain the unconditional stability of IFF.
- Section [[sec:comparison]]: compares both proposed modifications to the classical IFF in terms of damping authority and closed-loop system behavior.
- Section [[sec:notations]]: contains the notations used for both the Matlab code and the paper

The matlab code is accessible on [[https://zenodo.org/record/3894343][Zonodo]] and [[https://github.com/tdehaeze/dehaeze20_contr_stewa_platf][Github]] cite:dehaeze20_activ_dampin_rotat_posit_platf. It can also be download as a =.zip= file [[file:matlab.zip][here]].

To run the Matlab code, go in the =matlab= directory and run the following Matlab files corresponding to each section.

#+caption: Paper's sections and corresponding Matlab files
| Sections                           | Matlab File                |
|------------------------------------+----------------------------|
| Section [[sec:system_description]]     | =s1_system_description.m=  |
| Section [[sec:iff_pure_int]]           | =s2_iff_pure_int.m=        |
| Section [[sec:iff_pseudo_int]]         | =s3_iff_hpf.m=             |
| Section [[sec:iff_parallel_stiffness]] | =s4_iff_kp.m=              |
| Section [[sec:comparison]]             | =s5_act_damp_comparison.m= |

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

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

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

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

** System description
The system consists of one 2 degree of freedom translation stage on top of a spindle (figure [[fig:system]]).

#+name: fig:system
#+caption: Schematic of the studied system
[[file:figs-paper/system.png]]

The control inputs are the forces applied by the actuators of the translation stage ($F_u$ and $F_v$).
As the translation stage is rotating around the Z axis due to the spindle, the forces are applied along $\vec{i}_u$ and $\vec{i}_v$.

** Equations
Based on the Figure [[fig:system]], the equations of motions are:
#+begin_important
\begin{equation}
\begin{bmatrix} d_u \\ d_v \end{bmatrix} =
\bm{G}_d
\begin{bmatrix} F_u \\ F_v \end{bmatrix}
\end{equation}
Where $\bm{G}_d$ is a $2 \times 2$ transfer function matrix.

\begin{equation}
\bm{G}_d = \frac{1}{k} \frac{1}{G_{dp}}
\begin{bmatrix}
   G_{dz} & G_{dc} \\
  -G_{dc} & G_{dz}
\end{bmatrix}
\end{equation}
With:
\begin{align}
  G_{dp} &= \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 \\
  G_{dz} &= \frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1 - \frac{{\Omega}^2}{{\omega_0}^2} \\
  G_{dc} &= 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0}
\end{align}
#+end_important

** Numerical Values
Let's define initial values for the model.
#+begin_src matlab
  k = 1;    % Actuator Stiffness [N/m]
  c = 0.05; % Actuator Damping [N/(m/s)]
  m = 1;    % Payload mass [kg]
#+end_src

#+begin_src matlab
  xi = c/(2*sqrt(k*m));
  w0 = sqrt(k/m); % [rad/s]
#+end_src

** Campbell Diagram
The Campbell Diagram displays the evolution of the real and imaginary parts of the system as a function of the rotating speed.

It is shown in Figures [[fig:campbell_diagram_real]] and [[fig:campbell_diagram_imag]], and one can see that the system becomes unstable for $\Omega > \omega_0$ (the real part of one of the poles becomes positive).

#+begin_src matlab :exports none
  Ws = linspace(0, 2, 51); % Vector of considered rotation speed [rad/s]

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

  for W_i = 1:length(Ws)
      W = Ws(W_i);

      pole_G = pole(1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2));
      [~, i_sort] = sort(imag(pole_G));
      p_ws(:, W_i) = pole_G(i_sort);
  end

  clear pole_G;
#+end_src

#+begin_src matlab :exports none :tangle no
  figure;
  hold on;
  set(gca,'ColorOrderIndex', 1);
  plot(Ws, real(p_ws(1, :)), '-', 'DisplayName', '$p_{+}$')
  set(gca,'ColorOrderIndex', 1);
  plot(Ws, real(p_ws(4, :)), '-', 'HandleVisibility', 'off')
  set(gca,'ColorOrderIndex', 2);
  plot(Ws, real(p_ws(2, :)), '-', 'DisplayName', '$p_{-}$')
  set(gca,'ColorOrderIndex', 2);
  plot(Ws, real(p_ws(3, :)), '-', 'HandleVisibility', 'off')
  plot(Ws, zeros(size(Ws)), 'k--', 'HandleVisibility', 'off')
  hold off;
  xlabel('Rotational Speed $\Omega$'); ylabel('Real Part');
  xlim([0, 2*w0]);
  xticks([0,w0/2,w0,3/2*w0,2*w0])
  xticklabels({'$0$', '', '$\omega_0$', '', '$2 \omega_0$'})
  ylim([-3*xi, 3*xi]);
  yticks([-3*xi, -2*xi, -xi, 0, xi, 2*xi, 3*xi])
  yticklabels({'', '', '$-\xi\omega_0$', '$0$', ''})
  leg = legend('location', 'northwest', 'FontSize', 8);
  leg.ItemTokenSize(1) = 6;
#+end_src

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

#+name: fig:campbell_diagram_real
#+caption: Campbell Diagram - Real Part
#+RESULTS:
[[file:figs/campbell_diagram_real.png]]

#+begin_src matlab :exports none :tangle no
  figure;
  hold on;
  set(gca,'ColorOrderIndex', 1);
  plot(Ws, imag(p_ws(1, :)), '-')
  set(gca,'ColorOrderIndex', 1);
  plot(Ws, imag(p_ws(4, :)), '-')
  set(gca,'ColorOrderIndex', 2);
  plot(Ws, imag(p_ws(2, :)), '-')
  set(gca,'ColorOrderIndex', 2);
  plot(Ws, imag(p_ws(3, :)), '-')
  plot(Ws, zeros(size(Ws)), 'k--')
  hold off;
  xlabel('Rotational Speed $\Omega$'); ylabel('Imaginary Part');
  xlim([0, 2*w0]);
  xticks([0,w0/2,w0,3/2*w0,2*w0])
  xticklabels({'$0$', '', '$\omega_0$', '', '$2 \omega_0$'})
  ylim([-3*w0, 3*w0]);
  yticks([-3*w0, -2*w0, -w0, 0, w0, 2*w0, 3*w0])
  yticklabels({'', '', '$-\omega_0$', '$0$', '$\omega_0$', '', ''})
#+end_src

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

#+name: fig:campbell_diagram_imag
#+caption: Campbell Diagram - Imaginary Part
#+RESULTS:
[[file:figs/campbell_diagram_imag.png]]

** Simscape Model
In order to validate all the equations of motion, a Simscape model of the same system has been developed.
The dynamics of the system can be identified from the Simscape model and compare with the analytical model.

The rotating speed for the Simscape Model is defined.
#+begin_src matlab
  W = 0.1; % Rotation Speed [rad/s]
#+end_src

#+begin_src matlab :exports none
  Kiff = tf(zeros(2));

  kp = 0; % Parallel Stiffness [N/m]
  cp = 0; % Parallel Damping [N/(m/s)]
#+end_src

#+begin_src matlab
  open('rotating_frame.slx');
#+end_src

The transfer function from $[F_u, F_v]$ to $[d_u, d_v]$ is identified from the Simscape model.

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

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/K'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/G'], 2, 'openoutput'); io_i = io_i + 1;
#+end_src

#+begin_src matlab
  G = linearize(mdl, io, 0);

  %% Input/Output definition
  G.InputName  = {'Fu', 'Fv'};
  G.OutputName = {'du', 'dv'};
#+end_src

The same transfer function from $[F_u, F_v]$ to $[d_u, d_v]$ is written down from the analytical model.
#+begin_src matlab
  Gth = (1/k)/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
        [(s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2), 2*W*s/(w0^2) ; ...
         -2*W*s/(w0^2), (s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)];
#+end_src

Both transfer functions are compared in Figure [[fig:plant_simscape_analytical]] and are found to perfectly match.

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

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

  % Magnitude
  ax1 = nexttile([2, 1]);
  hold on;
  plot(freqs, abs(squeeze(freqresp(G(1,1), freqs))), '-')
  plot(freqs, abs(squeeze(freqresp(Gth(1,1), freqs))), '--')
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
  title('$d_u/F_u$, $d_v/F_v$');

  ax2 = nexttile([2, 1]);
  hold on;
  plot(freqs, abs(squeeze(freqresp(G(1,2), freqs))), '-')
  plot(freqs, abs(squeeze(freqresp(Gth(1,2), freqs))), '--')
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
  title('$d_u/F_v$, $d_v/F_u$');

  ax3 = nexttile;
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(G(1,1), freqs))), '-')
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gth(1,1), freqs))), '--')
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
  yticks(-180:90:180);
  ylim([-180 180]);
  hold off;

  ax4 = nexttile;
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(G(1,2), freqs))), '-', ...
       'DisplayName', 'Simscape')
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gth(1,2), freqs))), '--', ...
       'DisplayName', 'Analytical')
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
  yticks(-180:90:180);
  ylim([-180 180]);
  hold off;
  legend('location', 'southwest', 'FontSize', 8);

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

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

#+name: fig:plant_simscape_analytical
#+caption: Bode plot of the transfer function from $[F_u, F_v]$ to $[d_u, d_v]$ as identified from the Simscape model and from an analytical model
#+RESULTS:
[[file:figs/plant_simscape_analytical.png]]

** Effect of the rotation speed
The transfer functions from $[F_u, F_v]$ to $[d_u, d_v]$ are identified for the following rotating speeds.
#+begin_src matlab
  Ws = [0, 0.2, 0.7, 1.1]*w0; % Rotating Speeds [rad/s]
#+end_src

#+begin_src matlab
  Gs = {zeros(2, 2, length(Ws))};

  for W_i = 1:length(Ws)
      W = Ws(W_i);

      Gs(:, :, W_i) = {(1/k)/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
                       [(s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2), 2*W*s/(w0^2) ; ...
                        -2*W*s/(w0^2), (s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)]};
  end
#+end_src

They are compared in Figures [[fig:plant_compare_rotating_speed_direct]] and [[fig:plant_compare_rotating_speed_coupling]].

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

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

  % Magnitude
  ax1 = nexttile([2, 1]);
  hold on;
  for W_i = 1:length(Ws)
      plot(freqs, abs(squeeze(freqresp(Gs{W_i}(1,1), freqs))), ...
           'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Ws(W_i)/w0))
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
  leg = legend('location', 'southwest', 'FontSize', 8);
  leg.ItemTokenSize(1) = 6;
  ylim([1e-4, 1e2]);
  title('$d_u/F_u$, $d_v/F_v$');

  % Phase
  ax2 = nexttile;
  hold on;
  for W_i = 1:length(Ws)
      plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{W_i}(1,1), freqs))))
  end
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
  yticks(-180:90:180);
  ylim([-180 180]);
  hold off;

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

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

#+name: fig:plant_compare_rotating_speed_direct
#+caption: Comparison of the transfer functions from $[F_u, F_v]$ to $[d_u, d_v]$ for several rotating speed - Direct Terms
#+RESULTS:
[[file:figs/plant_compare_rotating_speed_direct.png]]

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

  % Magnitude
  ax1 = nexttile([2, 1]);
  hold on;
  for W_i = 1:length(Ws)
      plot(freqs, abs(squeeze(freqresp(Gs{W_i}(2,1), freqs))))
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
  ylim([1e-4, 1e2]);
  title('$d_u/F_v$, $d_v/F_u$');

  % Phase
  ax2 = nexttile;
  hold on;
  for W_i = 1:length(Ws)
      plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{W_i}(2,1), freqs))))
  end
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
  yticks(-180:90:180);
  ylim([-180 180]);
  hold off;

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

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

#+name: fig:plant_compare_rotating_speed_coupling
#+caption: Comparison of the transfer functions from $[F_u, F_v]$ to $[d_u, d_v]$ for several rotating speed - Coupling Terms
#+RESULTS:
[[file:figs/plant_compare_rotating_speed_coupling.png]]

* Problem with pure Integral Force Feedback
:PROPERTIES:
:header-args:matlab+: :tangle matlab/s2_iff_pure_int.m
:END:
<<sec:iff_pure_int>>

** Introduction                                                      :ignore:
Force sensors are added in series with the two actuators (Figure [[fig:system_iff]]).

Two identical controllers $K_F$ are used to feedback each of the sensed force to its associated actuator.

#+name: fig:system_iff
#+caption: System with added Force Sensor in series with the actuators
[[file:figs-paper/system_iff.png]]

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

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

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

** Plant Parameters
Let's define initial values for the model.
#+begin_src matlab
  k = 1;    % Actuator Stiffness [N/m]
  c = 0.05; % Actuator Damping [N/(m/s)]
  m = 1;    % Payload mass [kg]
#+end_src

#+begin_src matlab
  xi = c/(2*sqrt(k*m));
  w0 = sqrt(k/m); % [rad/s]
#+end_src

#+begin_src matlab :exports none
  kp = 0; % [N/m]
  cp = 0; % [N/(m/s)]
#+end_src

** Equations
The sensed forces are equal to:
\begin{equation}
\begin{bmatrix} f_{u} \\ f_{v} \end{bmatrix} =
\begin{bmatrix}
  1 & 0 \\
  0 & 1
\end{bmatrix}
\begin{bmatrix} F_u \\ F_v \end{bmatrix} - (c s + k)
\begin{bmatrix} d_u \\ d_v \end{bmatrix}
\end{equation}

Which then gives:
#+begin_important
\begin{equation}
\begin{bmatrix} f_{u} \\ f_{v} \end{bmatrix} =
\bm{G}_{f}
\begin{bmatrix} F_u \\ F_v \end{bmatrix}
\end{equation}

\begin{equation}
\begin{bmatrix} f_{u} \\ f_{v} \end{bmatrix} =
\frac{1}{G_{fp}}
\begin{bmatrix}
  G_{fz} & -G_{fc} \\
  G_{fc} &  G_{fz}
\end{bmatrix}
\begin{bmatrix} F_u \\ F_v \end{bmatrix}
\end{equation}

\begin{align}
  G_{fp} &= \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 \\
  G_{fz} &= \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 \\
  G_{fc} &= \left( 2 \xi \frac{s}{\omega_0} + 1 \right) \left( 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \right)
\end{align}
#+end_important

** Comparison of the Analytical Model and the Simscape Model
The rotation speed is set to $\Omega = 0.1 \omega_0$.
#+begin_src matlab
  W = 0.1*w0; % [rad/s]
#+end_src

#+begin_src matlab :exports none
  Kiff = tf(zeros(2));
#+end_src

#+begin_src matlab
  open('rotating_frame.slx');
#+end_src

And the transfer function from $[F_u, F_v]$ to $[f_u, f_v]$ is identified using the Simscape model.
#+begin_src matlab
  %% Name of the Simulink File
  mdl = 'rotating_frame';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/K'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/G'], 1, 'openoutput'); io_i = io_i + 1;
#+end_src

#+begin_src matlab
  Giff = linearize(mdl, io, 0);

  %% Input/Output definition
  Giff.InputName  = {'Fu', 'Fv'};
  Giff.OutputName = {'fu', 'fv'};
#+end_src

The same transfer function from $[F_u, F_v]$ to $[f_u, f_v]$ is written down from the analytical model.
#+begin_src matlab
  Giff_th = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
            [(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ...
             (2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2];
#+end_src

The two are compared in Figure [[fig:plant_iff_comp_simscape_analytical]] and found to perfectly match.
#+begin_src matlab :exports none
  freqs = logspace(-1, 1, 1000);

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

  % Magnitude
  ax1 = nexttile([2, 1]);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs))), '-')
  plot(freqs, abs(squeeze(freqresp(Giff_th(1,1), freqs))), '--')
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
  title('$f_u/F_u$, $f_v/F_v$');

  ax2 = nexttile([2, 1]);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Giff(1,2), freqs))), '-')
  plot(freqs, abs(squeeze(freqresp(Giff_th(1,2), freqs))), '--')
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
  title('$f_u/F_v$, $f_v/F_u$');

  ax3 = nexttile;
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1), freqs))), '-')
  plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_th(1,1), freqs))), '--')
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
  yticks(-180:90:180);
  ylim([-180 180]);
  hold off;

  ax4 = nexttile;
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,2), freqs))), '-', ...
       'DisplayName', 'Simscape')
  plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_th(1,2), freqs))), '--', ...
       'DisplayName', 'Analytical')
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
  yticks(-180:90:180);
  ylim([-180 180]);
  hold off;
  legend('location', 'northeast', 'FontSize', 8);

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

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

#+name: fig:plant_iff_comp_simscape_analytical
#+caption: Comparison of the transfer functions from $[F_u, F_v]$ to $[f_u, f_v]$ between the Simscape model and the analytical one
#+RESULTS:
[[file:figs/plant_iff_comp_simscape_analytical.png]]

** Effect of the rotation speed
The transfer functions from $[F_u, F_v]$ to $[f_u, f_v]$ are identified for the following rotating speeds.
#+begin_src matlab
  Ws = [0, 0.2, 0.7]*w0; % Rotating Speeds [rad/s]
#+end_src

#+begin_src matlab
  Gsiff = {zeros(2, 2, length(Ws))};

  for W_i = 1:length(Ws)
      W = Ws(W_i);

      Gsiff(:, :, W_i) = {1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
                        [(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ...
                         (2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2]};
  end
#+end_src

The obtained transfer functions are shown in Figure [[fig:plant_iff_compare_rotating_speed]].
#+begin_src matlab :exports none
  freqs = logspace(-2, 1, 1000);

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

  % Magnitude
  ax1 = nexttile([2, 1]);
  hold on;
  for W_i = 1:length(Ws)
      plot(freqs, abs(squeeze(freqresp(Gsiff{W_i}(1,1), freqs))), ...
           'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Ws(W_i)/w0))
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
  leg = legend('location', 'southeast', 'FontSize', 8);
  leg.ItemTokenSize(1) = 6;

  % Phase
  ax2 = nexttile;
  hold on;
  for W_i = 1:length(Ws)
      plot(freqs, 180/pi*angle(squeeze(freqresp(Gsiff{W_i}(1,1), freqs))))
  end
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
  yticks(-180:90:180);
  ylim([-180 180]);
  hold off;

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

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

#+name: fig:plant_iff_compare_rotating_speed
#+caption: Comparison of the transfer functions from $[F_u, F_v]$ to $[f_u, f_v]$ for several rotating speed
#+RESULTS:
[[file:figs/plant_iff_compare_rotating_speed.png]]

** Decentralized Integral Force Feedback
The decentralized IFF controller consists of pure integrators:
\begin{equation}
  \bm{K}_{\text{IFF}}(s) = \frac{g}{s} \begin{bmatrix}
    1 & 0 \\
    0 & 1
  \end{bmatrix}
\end{equation}

The Root Locus (evolution of the poles of the closed loop system in the complex plane as a function of $g$) is shown in Figure [[fig:root_locus_pure_iff]].
It is shown that for non-null rotating speed, one pole is bound to the right-half plane, and thus the closed loop system is unstable.

#+begin_src matlab :exports none
  figure;

  gains = logspace(-2, 4, 100);

  hold on;
  for W_i = 1:length(Ws)
      set(gca,'ColorOrderIndex',W_i);
      plot(real(pole(Gsiff{W_i})),  imag(pole(Gsiff{W_i})), 'x', ...
           'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Ws(W_i)/w0));
      set(gca,'ColorOrderIndex',W_i);
      plot(real(tzero(Gsiff{W_i})),  imag(tzero(Gsiff{W_i})), 'o', ...
           'HandleVisibility', 'off');
      for g = gains
          set(gca,'ColorOrderIndex',W_i);
          cl_poles = pole(feedback(Gsiff{W_i}, g/s*eye(2)));
          plot(real(cl_poles), imag(cl_poles), '.', ...
               'HandleVisibility', 'off');
      end
  end
  hold off;
  axis square;
  xlim([-2, 0.5]); ylim([0, 2.5]);

  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 replace
  exportFig('figs/root_locus_pure_iff.pdf', 'width', 'half', 'height', 'normal');
#+end_src

#+name: fig:root_locus_pure_iff
#+caption: Root Locus for the Decentralized Integral Force Feedback controller. Several rotating speed are shown.
#+RESULTS:
[[file:figs/root_locus_pure_iff.png]]

#+begin_src matlab :exports none :tangle no
  gains = logspace(-2, 4, 1000);

  figure;
  hold on;
  for W_i = 1:length(Ws)

      set(gca,'ColorOrderIndex',W_i);
      plot(real(pole(Gsiff{W_i})),  imag(pole(Gsiff{W_i})), 'x', ...
           'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Ws(W_i)/w0));
      set(gca,'ColorOrderIndex',W_i);
      plot(real(tzero(Gsiff{W_i})),  imag(tzero(Gsiff{W_i})), 'o', ...
           'HandleVisibility', 'off');
      poles = rootLocusPolesSorted(Gsiff{W_i}, 1/s*eye(2), gains, 'd_max', 1e-4);
      for p_i = 1:size(poles, 2)
          set(gca,'ColorOrderIndex',W_i);
          plot(real(poles(:, p_i)), imag(poles(:, p_i)), '-', ...
               'HandleVisibility', 'off');
      end
  end
  hold off;
  axis square;
  xlim([-2, 0.5]); ylim([0, 2.5]);

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

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

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

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

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

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

** Plant Parameters
Let's define initial values for the model.
#+begin_src matlab
  k = 1;    % Actuator Stiffness [N/m]
  c = 0.05; % Actuator Damping [N/(m/s)]
  m = 1;    % Payload mass [kg]
#+end_src

#+begin_src matlab
  xi = c/(2*sqrt(k*m));
  w0 = sqrt(k/m); % [rad/s]
#+end_src

#+begin_src matlab :exports none
  kp = 0; % [N/m]
  cp = 0; % [N/(m/s)]
#+end_src

** Modified Integral Force Feedback Controller
Let's modify the initial Integral Force Feedback Controller ; instead of using pure integrators, pseudo integrators (i.e. low pass filters) are used:
\begin{equation}
  K_{\text{IFF}}(s) = g\frac{1}{\omega_i + s} \begin{bmatrix}
  1 & 0 \\
  0 & 1
\end{bmatrix}
\end{equation}
where $\omega_i$ characterize down to which frequency the signal is integrated.

Let's arbitrary choose the following control parameters:
#+begin_src matlab
  g = 2;
  wi = 0.1*w0;
#+end_src

#+begin_src matlab :exports none
  Kiff = (g/(wi+s))*eye(2);
#+end_src

And the following rotating speed.
#+begin_src matlab :exports none
  W = 0.1*w0;
#+end_src

#+begin_src matlab
  Giff = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
          [(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ...
           (2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2];
#+end_src

The obtained Loop Gain is shown in Figure [[fig:loop_gain_modified_iff]].
#+begin_src matlab :exports none
  freqs = logspace(-2, 1, 1000);

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

  % Magnitude
  ax1 = nexttile([2, 1]);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Giff(1,1)*(g/s), freqs))))
  plot(freqs, abs(squeeze(freqresp(Giff(1,1)*Kiff(1,1), freqs))))
  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(Giff(1,1)*(g/s), freqs))), ...
       'DisplayName', 'IFF')
  plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1)*Kiff(1,1), freqs))), ...
       'DisplayName', 'IFF + HPF')
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
  yticks(-180:90:180);
  ylim([-180 180]);
  leg = legend('location', 'southwest', 'FontSize', 8);
  leg.ItemTokenSize(1) = 6;

  hold off;

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

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

#+name: fig:loop_gain_modified_iff
#+caption: Loop Gain for the modified IFF controller
#+RESULTS:
[[file:figs/loop_gain_modified_iff.png]]

** Root Locus
As shown in the Root Locus plot (Figure [[fig:root_locus_modified_iff]]), for some value of the gain, the system remains stable.

#+begin_src matlab :exports none
  figure;

  gains = logspace(-2, 4, 100);

  hold on;
  % Pure Integrator
  set(gca,'ColorOrderIndex',1);
  plot(real(pole(Giff)),  imag(pole(Giff)), 'x', 'DisplayName', 'IFF');
  set(gca,'ColorOrderIndex',1);
  plot(real(tzero(Giff)),  imag(tzero(Giff)), 'o', 'HandleVisibility', 'off');
  for g = gains
      clpoles = pole(feedback(Giff, (g/s)*eye(2)));
      set(gca,'ColorOrderIndex',1);
      plot(real(clpoles), imag(clpoles), '.', 'HandleVisibility', 'off');
  end
  % Modified IFF
  set(gca,'ColorOrderIndex',2);
  plot(real(pole(Giff)),  imag(pole(Giff)), 'x', 'DisplayName', 'IFF + HPF');
  set(gca,'ColorOrderIndex',2);
  plot(real(tzero(Giff)),  imag(tzero(Giff)), 'o', 'HandleVisibility', 'off');
  for g = gains
      clpoles = pole(feedback(Giff, (g/(wi+s))*eye(2)));
      set(gca,'ColorOrderIndex',2);
      plot(real(clpoles), imag(clpoles), '.', 'HandleVisibility', 'off');
  end
  hold off;
  axis square;
  xlim([-2, 0.5]); ylim([-1.25, 1.25]);
  leg = legend('location', 'northwest', 'FontSize', 8);
  leg.ItemTokenSize(1) = 8;
  xlabel('Real Part'); ylabel('Imaginary Part');
#+end_src

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

#+name: fig:root_locus_modified_iff
#+caption: Root Locus for the modified IFF controller
#+RESULTS:
[[file:figs/root_locus_modified_iff.png]]

#+begin_src matlab :exports none
  xlim([-0.2, 0.1]); ylim([-0.15, 0.15]);
  legend('hide');
#+end_src

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

#+name: fig:root_locus_modified_iff_zoom
#+caption: Root Locus for the modified IFF controller - Zoom
#+RESULTS:
[[file:figs/root_locus_modified_iff_zoom.png]]

#+begin_src matlab :exports none :tangle no
  gains = logspace(-2, 3, 200);

  poles_iff = rootLocusPolesSorted(Giff, 1/s*eye(2), gains, 'd_max', 1e-4);
  poles_iff_hpf = rootLocusPolesSorted(Giff, 1/(s + wi)*eye(2), gains, 'd_max', 1e-4);

  figure;

  hold on;
  % Pure Integrator
  set(gca,'ColorOrderIndex',1);
  plot(real(pole(Giff)),  imag(pole(Giff)), 'x', 'DisplayName', 'IFF');
  set(gca,'ColorOrderIndex',1);
  plot(real(tzero(Giff)),  imag(tzero(Giff)), 'o', 'HandleVisibility', 'off');
  for p_i = 1:size(poles_iff, 2)
      set(gca,'ColorOrderIndex',1);
      plot(real(poles_iff(:, p_i)), imag(poles_iff(:, p_i)), '-', ...
           'HandleVisibility', 'off');
  end
  % Modified IFF
  set(gca,'ColorOrderIndex',2);
  plot(real(pole(Giff)),  imag(pole(Giff)), 'x', 'DisplayName', 'IFF + HPF');
  set(gca,'ColorOrderIndex',2);
  plot(real(tzero(Giff)),  imag(tzero(Giff)), 'o', 'HandleVisibility', 'off');
  for p_i = 1:size(poles_iff_hpf, 2)
      set(gca,'ColorOrderIndex',2);
      plot(real(poles_iff_hpf(:, p_i)), imag(poles_iff_hpf(:, p_i)), '-', ...
           'HandleVisibility', 'off');
  end
  hold off;
  axis square;
  xlim([-2, 0.5]); ylim([-1.25, 1.25]);
  leg = legend('location', 'northwest', 'FontSize', 8);
  leg.ItemTokenSize(1) = 8;
  xlabel('Real Part'); ylabel('Imaginary Part');
#+end_src

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

#+begin_src matlab :exports none :tangle no
  xlim([-0.2, 0.1]); ylim([-0.15, 0.15]);
  legend('hide');
#+end_src

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

** What is the optimal $\omega_i$ and $g$?
In order to visualize the effect of $\omega_i$ on the attainable damping, the Root Locus is displayed in Figure [[fig:root_locus_wi_modified_iff]] for the following $\omega_i$:
#+begin_src matlab
  wis = [0.01, 0.1, 0.5, 1]*w0; % [rad/s]
#+end_src

#+begin_src matlab :exports none
  figure;

  gains = logspace(-2, 4, 100);

  hold on;
  for wi_i = 1:length(wis)
      set(gca,'ColorOrderIndex',wi_i);
      wi = wis(wi_i);
      L(wi_i) = plot(nan, nan, '.', 'DisplayName', sprintf('$\\omega_i = %.2f \\omega_0$', wi./w0));
      for g = gains
          clpoles = pole(feedback(Giff, (g/(wi+s))*eye(2)));
          set(gca,'ColorOrderIndex',wi_i);
          plot(real(clpoles), imag(clpoles), '.');
      end
  end
  plot(real(pole(Giff)),  imag(pole(Giff)), 'kx');
  plot(real(tzero(Giff)),  imag(tzero(Giff)), 'ko');
  hold off;
  axis square;
  xlim([-2.3, 0.1]); ylim([-1.2, 1.2]);
  xticks([-2:1:2]); yticks([-2:1:2]);
  leg = legend(L, 'location', 'northwest', 'FontSize', 8);
  leg.ItemTokenSize(1) = 8;
  xlabel('Real Part'); ylabel('Imaginary Part');

  clear L
#+end_src

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

#+name: fig:root_locus_wi_modified_iff
#+caption: Root Locus for the modified IFF controller (zoomed plot on the left)
#+RESULTS:
[[file:figs/root_locus_wi_modified_iff.png]]

#+begin_src matlab :exports none
  xlim([-0.2, 0.1]); ylim([-0.15, 0.15]);
  xticks([-0.2:0.1:0.1]); yticks([-0.2:0.1:0.2]);
  legend('hide');
#+end_src

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

#+name: fig:root_locus_wi_modified_iff_zoom
#+caption: Root Locus for the modified IFF controller (zoomed plot on the left)
#+RESULTS:
[[file:figs/root_locus_wi_modified_iff_zoom.png]]

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

  poles_iff_hpf = rootLocusPolesSorted(Giff, 1/(s + wi)*eye(2), gains, 'd_max', 1e-4);

  figure;

  hold on;
  for wi_i = 1:length(wis)
      wi = wis(wi_i);

      set(gca,'ColorOrderIndex',wi_i);
      L(wi_i) = plot(nan, nan, '.', 'DisplayName', sprintf('$\\omega_i = %.2f \\omega_0$', wi./w0));

      poles = rootLocusPolesSorted(Giff, 1/(s + wi)*eye(2), gains, 'd_max', 1e-4);
      for p_i = 1:size(poles, 2)
          set(gca,'ColorOrderIndex',wi_i);
          plot(real(poles(:, p_i)), imag(poles(:, p_i)), '-', ...
               'HandleVisibility', 'off');
      end
  end
  plot(real(pole(Giff)),  imag(pole(Giff)), 'kx');
  plot(real(tzero(Giff)),  imag(tzero(Giff)), 'ko');
  hold off;
  axis square;
  xlim([-2.3, 0.1]); ylim([-1.2, 1.2]);
  xticks([-2:1:2]); yticks([-2:1:2]);
  leg = legend(L, 'location', 'northwest', 'FontSize', 8);
  leg.ItemTokenSize(1) = 8;
  xlabel('Real Part'); ylabel('Imaginary Part');

  clear L
#+end_src

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

#+begin_src matlab :exports none :tangle no
  xlim([-0.2, 0.1]); ylim([-0.15, 0.15]);
  xticks([-0.2:0.1:0.1]); yticks([-0.2:0.1:0.2]);
  legend('hide');
#+end_src

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

For the controller
\begin{equation}
  K_{\text{IFF}}(s) = g\frac{1}{\omega_i + s} \begin{bmatrix}
  1 & 0 \\
  0 & 1
\end{bmatrix}
\end{equation}
The gain at which the system becomes unstable is
\begin{equation}
  g_\text{max} = \omega_i \left( \frac{{\omega_0}^2}{\Omega^2} - 1 \right) \label{eq:iff_gmax}
\end{equation}

While it seems that small $\omega_i$ do allow more damping to be added to the system (Figure [[fig:root_locus_wi_modified_iff]]), the control gains may be limited to small values due to eqref:eq:iff_gmax thus reducing the attainable damping.

There must be an optimum for $\omega_i$.
To find the optimum, the gain that maximize the simultaneous damping of the mode is identified for a wide range of $\omega_i$ (Figure [[fig:mod_iff_damping_wi]]).

#+begin_src matlab
  wis = logspace(-2, 1, 100)*w0; % [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, Giff, Kiff);

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

#+begin_src matlab :exports none
  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*((w0/W)^2 - 1), '--', 'DisplayName', '$g_{max}$');
  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 replace
  exportFig('figs/mod_iff_damping_wi.pdf', 'width', 'half', 'height', 'short');
#+end_src

#+name: fig:mod_iff_damping_wi
#+caption: Simultaneous attainable damping of the closed loop poles as a function of $\omega_i$
#+RESULTS:
[[file:figs/mod_iff_damping_wi.png]]

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

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

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

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

** Schematic
In this section additional springs in parallel with the force sensors are added to counteract the negative stiffness induced by the rotation.

#+name: fig:system_parallel_springs
#+caption: Studied system with additional springs in parallel with the actuators and force sensors
[[file:figs-paper/system_parallel_springs.png]]

In order to keep the overall stiffness $k = k_a + k_p$ constant, 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
\begin{equation}
  k_p = \alpha k, \quad k_a = (1 - \alpha) k
\end{equation}

** Equations
#+begin_important
\begin{equation}
\begin{bmatrix} f_u \\ f_v \end{bmatrix} =
\bm{G}_k
\begin{bmatrix} F_u \\ F_v \end{bmatrix}
\end{equation}

\begin{equation}
\begin{bmatrix} f_u \\ f_v \end{bmatrix} =
\frac{1}{G_{kp}}
\begin{bmatrix}
   G_{kz} & -G_{kc} \\
   G_{kc} &  G_{kz}
\end{bmatrix}
\begin{bmatrix} F_u \\ F_v \end{bmatrix}
\end{equation}
With:
\begin{align}
  G_{kp} &= \left( \frac{s^2}{{\omega_0}^2} + 2\xi \frac{s}{{\omega_0}^2} + 1 - \frac{\Omega^2}{{\omega_0}^2} \right)^2 + \left( 2 \frac{\Omega}{\omega_0}\frac{s}{\omega_0} \right)^2 \\
  G_{kz} &= \left( \frac{s^2}{{\omega_0}^2} - \frac{\Omega^2}{{\omega_0}^2} + \alpha \right) \left( \frac{s^2}{{\omega_0}^2} + 2\xi \frac{s}{{\omega_0}^2} + 1 - \frac{\Omega^2}{{\omega_0}^2} \right) + \left( 2 \frac{\Omega}{\omega_0}\frac{s}{\omega_0} \right)^2 \\
  G_{kc} &= \left( 2 \xi \frac{s}{\omega_0} + 1 - \alpha \right) \left( 2 \frac{\Omega}{\omega_0}\frac{s}{\omega_0} \right)
\end{align}
#+end_important

If we compare $G_{kz}$ and $G_{fz}$, we see that the spring in parallel adds a term $\alpha$.
In order to have two complex conjugate zeros (instead of real zeros):
\begin{equation}
  \alpha > \frac{\Omega^2}{{\omega_0}^2} \quad \Leftrightarrow \quad k_p > m \Omega^2
\end{equation}

** Plant Parameters
Let's define initial values for the model.
#+begin_src matlab
  k = 1;    % Actuator Stiffness [N/m]
  c = 0.05; % Actuator Damping [N/(m/s)]
  m = 1;    % Payload mass [kg]
#+end_src

#+begin_src matlab
  xi = c/(2*sqrt(k*m));
  w0 = sqrt(k/m); % [rad/s]
#+end_src

#+begin_src matlab :exports none
  kp = 0; % [N/m]
  cp = 0; % [N/(m/s)]
#+end_src

** Comparison of the Analytical Model and the Simscape Model
The same transfer function from $[F_u, F_v]$ to $[f_u, f_v]$ is written down from the analytical model.
#+begin_src matlab
  W = 0.1*w0; % [rad/s]

  kp = 1.5*m*W^2;
  cp = 0;
#+end_src

#+begin_src matlab :exports none
  Kiff = tf(zeros(2));
#+end_src

#+begin_src matlab
  open('rotating_frame.slx');
#+end_src

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

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/K'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/G'], 1, 'openoutput'); io_i = io_i + 1;

  Giff = linearize(mdl, io, 0);

  %% Input/Output definition
  Giff.InputName  = {'Fu', 'Fv'};
  Giff.OutputName = {'fu', 'fv'};
#+end_src

#+begin_src matlab
  w0p = sqrt((k + kp)/m);
  xip = c/(2*sqrt((k+kp)*m));

  Giff_th = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ...
                     (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p));
                     (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2 ];
  Giff_th.InputName  = {'Fu', 'Fv'};
  Giff_th.OutputName = {'fu', 'fv'};
#+end_src

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

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

  ax1 = nexttile([2, 1]);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs))), '-')
  plot(freqs, abs(squeeze(freqresp(Giff_th(1,1), freqs))), '--')
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
  title('$f_u/F_u$, $f_v/F_v$');

  ax2 = nexttile([2, 1]);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Giff(1,2), freqs))), '-')
  plot(freqs, abs(squeeze(freqresp(Giff_th(1,2), freqs))), '--')
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
  title('$f_u/F_v$, $f_v/F_u$');

  ax3 = nexttile;
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1), freqs))), '-', ...
       'DisplayName', 'Simscape')
  plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_th(1,1), freqs))), '--', ...
       'DisplayName', 'Analytical')
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
  yticks(-180:90:180);
  ylim([-180 180]);
  hold off;
  legend('location', 'southwest', 'FontSize', 8);

  ax4 = nexttile;
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,2), freqs))), '-')
  plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_th(1,2), freqs))), '--')
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
  yticks(-180:90:180);
  ylim([-180 180]);
  hold off;

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

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

#+name: fig:plant_iff_kp_comp_simscape_analytical
#+caption: Comparison of the transfer functions from $[F_u, F_v]$ to $[f_u, f_v]$ between the Simscape model and the analytical one
#+RESULTS:
[[file:figs/plant_iff_kp_comp_simscape_analytical.png]]

** Effect of the parallel stiffness on the IFF plant
The rotation speed is set to $\Omega = 0.1 \omega_0$.
#+begin_src matlab
  W = 0.1*w0; % [rad/s]
#+end_src

And the IFF plant (transfer function from $[F_u, F_v]$ to $[f_u, f_v]$) is identified in three different cases:
- without parallel stiffness
- with a small parallel stiffness $k_p < m \Omega^2$
- with a large parallel stiffness $k_p > m \Omega^2$

The results are shown in Figure [[fig:plant_iff_kp]].

One can see that for $k_p > m \Omega^2$, the systems shows alternating complex conjugate poles and zeros.

#+begin_src matlab
  kp = 0;

  w0p = sqrt((k + kp)/m);
  xip = c/(2*sqrt((k+kp)*m));

  Giff = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ...
      (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p));
      (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2];
#+end_src

#+begin_src matlab
  kp = 0.5*m*W^2;
  k = 1 - kp;

  w0p = sqrt((k + kp)/m);
  xip = c/(2*sqrt((k+kp)*m));

  Giff_s = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ...
      (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p));
      (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2];
#+end_src

#+begin_src matlab
  kp = 1.5*m*W^2;
  k = 1 - kp;

  w0p = sqrt((k + kp)/m);
  xip = c/(2*sqrt((k+kp)*m));

  Giff_l = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ...
      (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p));
      (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2];
#+end_src

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

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

  % Magnitude
  ax1 = nexttile([2, 1]);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Giff(1,1),   freqs))), 'k-', ...
       'DisplayName', '$k_p = 0$')
  plot(freqs, abs(squeeze(freqresp(Giff_s(1,1), freqs))), 'k--', ...
       'DisplayName', '$k_p < m\Omega^2$')
  plot(freqs, abs(squeeze(freqresp(Giff_l(1,1), freqs))), 'k:', ...
       '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, 2e1]);
  legend('location', 'southeast', 'FontSize', 8);

  % Phase
  ax2 = nexttile;
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1),   freqs))), 'k-')
  plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_s(1,1), freqs))), 'k--')
  plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_l(1,1), freqs))), 'k:')
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
  yticks(-180:90:180);
  ylim([-180 180]);
  hold off;

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

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

#+name: fig:plant_iff_kp
#+caption: Transfer function from $[F_u, F_v]$ to $[f_u, f_v]$ for $k_p = 0$, $k_p < m \Omega^2$ and $k_p > m \Omega^2$
#+RESULTS:
[[file:figs/plant_iff_kp.png]]

** IFF when adding a spring in parallel
In Figure [[fig:root_locus_iff_kp]] is displayed the Root Locus in the three considered cases with
\begin{equation}
  K_{\text{IFF}} = \frac{g}{s} \begin{bmatrix}
  1 & 0 \\
  0 & 1
\end{bmatrix}
\end{equation}

One can see that for $k_p > m \Omega^2$, the root locus stays in the left half of the complex plane and thus the control system is unconditionally stable.

Thus, decentralized IFF controller with pure integrators can be used if:
\begin{equation}
  k_{p} > m \Omega^2
\end{equation}

#+begin_src matlab :exports none
  gains = logspace(-2, 2, 100);

  figure;
  hold on;
  set(gca,'ColorOrderIndex',1);
  plot(real(pole(Giff)),  imag(pole(Giff)), 'x', ...
       'DisplayName', '$k_p = 0$');
  set(gca,'ColorOrderIndex',1);
  plot(real(tzero(Giff)),  imag(tzero(Giff)), 'o', ...
       'HandleVisibility', 'off');
  for g = gains
      cl_poles = pole(feedback(Giff, (g/s)*eye(2)));
      set(gca,'ColorOrderIndex',1);
      plot(real(cl_poles), imag(cl_poles), '.', ...
           'HandleVisibility', 'off');
  end

  set(gca,'ColorOrderIndex',2);
  plot(real(pole(Giff_s)),  imag(pole(Giff_s)), 'x', ...
       'DisplayName', '$k_p < m\Omega^2$');
  set(gca,'ColorOrderIndex',2);
  plot(real(tzero(Giff_s)),  imag(tzero(Giff_s)), 'o', ...
       'HandleVisibility', 'off');
  for g = gains
      cl_poles = pole(feedback(Giff_s, (g/s)*eye(2)));
      set(gca,'ColorOrderIndex',2);
      plot(real(cl_poles), imag(cl_poles), '.', ...
           'HandleVisibility', 'off');
  end

  set(gca,'ColorOrderIndex',3);
  plot(real(pole(Giff_l)),  imag(pole(Giff_l)), 'x', ...
       'DisplayName', '$k_p > m\Omega^2$');
  set(gca,'ColorOrderIndex',3);
  plot(real(tzero(Giff_l)),  imag(tzero(Giff_l)), 'o', ...
       'HandleVisibility', 'off');
  for g = gains
      set(gca,'ColorOrderIndex',3);
      cl_poles = pole(feedback(Giff_l, (g/s)*eye(2)));
      plot(real(cl_poles), imag(cl_poles), '.', ...
           'HandleVisibility', 'off');
  end
  hold off;
  axis square;
  xlim([-1, 0.2]); ylim([0, 1.2]);

  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 replace
  exportFig('figs/root_locus_iff_kp.pdf', 'width', 'half', 'height', 'normal');
#+end_src

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

#+begin_src matlab :exports none
  xlim([-0.04, 0.06]); ylim([0, 0.1]);
  legend('hide');
#+end_src

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

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

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

  poles_iff = rootLocusPolesSorted(Giff, 1/s*eye(2), gains, 'd_max', 1e-4);
  poles_iff_s = rootLocusPolesSorted(Giff_s, 1/s*eye(2), gains, 'd_max', 1e-4);
  poles_iff_l = rootLocusPolesSorted(Giff_l, 1/s*eye(2), gains, 'd_max', 1e-4);

  figure;
  hold on;
  set(gca,'ColorOrderIndex',1);
  plot(real(pole(Giff)),  imag(pole(Giff)), 'x', ...
       'DisplayName', '$k_p = 0$');
  set(gca,'ColorOrderIndex',1);
  plot(real(tzero(Giff)),  imag(tzero(Giff)), 'o', ...
       'HandleVisibility', 'off');
  for p_i = 1:size(poles_iff, 2)
      set(gca,'ColorOrderIndex',1);
      plot(real(poles_iff(:, p_i)), imag(poles_iff(:, p_i)), '-', ...
           'HandleVisibility', 'off');
  end

  set(gca,'ColorOrderIndex',2);
  plot(real(pole(Giff_s)),  imag(pole(Giff_s)), 'x', ...
       'DisplayName', '$k_p < m\Omega^2$');
  set(gca,'ColorOrderIndex',2);
  plot(real(tzero(Giff_s)),  imag(tzero(Giff_s)), 'o', ...
       'HandleVisibility', 'off');
  for p_i = 1:size(poles_iff_s, 2)
      set(gca,'ColorOrderIndex',2);
      plot(real(poles_iff_s(:, p_i)), imag(poles_iff_s(:, p_i)), '-', ...
           'HandleVisibility', 'off');
  end

  set(gca,'ColorOrderIndex',3);
  plot(real(pole(Giff_l)),  imag(pole(Giff_l)), 'x', ...
       'DisplayName', '$k_p > m\Omega^2$');
  set(gca,'ColorOrderIndex',3);
  plot(real(tzero(Giff_l)),  imag(tzero(Giff_l)), 'o', ...
       'HandleVisibility', 'off');
  for p_i = 1:size(poles_iff_l, 2)
      set(gca,'ColorOrderIndex',3);
      plot(real(poles_iff_l(:, p_i)), imag(poles_iff_l(:, p_i)), '-', ...
           'HandleVisibility', 'off');
  end
  hold off;
  axis square;
  xlim([-1, 0.2]); ylim([0, 1.2]);

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

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

#+begin_src matlab :exports none :tangle no
  xlim([-0.04, 0.06]); ylim([0, 0.1]);
  legend('hide');
#+end_src

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

** Effect of $k_p$ on the attainable damping
However, having large values of $k_p$ may decrease the attainable damping.

To study the second point, Root Locus plots for the following values of $k_p$ are shown in Figure [[fig:root_locus_iff_kps]].
#+begin_src matlab
  kps = [2, 20, 40]*m*W^2;
#+end_src

It is shown that large values of $k_p$ decreases the attainable damping.
#+begin_src matlab :exports none
  figure;

  gains = logspace(-2, 4, 500);

  hold on;
  for kp_i = 1:length(kps)
      kp = kps(kp_i);
      k = 1 - kp;

      w0p = sqrt((k + kp)/m);
      xip = c/(2*sqrt((k+kp)*m));

      Giff = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ...
          (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p));
          (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2 ];

      set(gca,'ColorOrderIndex',kp_i);
      plot(real(pole(Giff)),  imag(pole(Giff)), 'x', ...
           'DisplayName', sprintf('$k_p = %.1f m \\Omega^2$', kp/(m*W^2)));
      set(gca,'ColorOrderIndex',kp_i);
      plot(real(tzero(Giff)),  imag(tzero(Giff)), 'o', ...
           'HandleVisibility', 'off');
      for g = gains
          Kiffa = (g/s)*eye(2);
          cl_poles = pole(feedback(Giff, Kiffa));
          set(gca,'ColorOrderIndex',kp_i);
          plot(real(cl_poles), imag(cl_poles), '.', ...
               'HandleVisibility', 'off');
      end
  end
  hold off;
  axis square;
  xlim([-1.2, 0.2]); ylim([0, 1.4]);

  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 replace
  exportFig('figs/root_locus_iff_kps.pdf', 'width', 'half', 'height', 'normal');
#+end_src

#+name: fig:root_locus_iff_kps
#+caption: Root Locus plot
#+RESULTS:
[[file:figs/root_locus_iff_kps.png]]

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

  figure;

  hold on;
  for kp_i = 1:length(kps)
      kp = kps(kp_i);
      k = 1 - kp;

      w0p = sqrt((k + kp)/m);
      xip = c/(2*sqrt((k+kp)*m));

      Giff = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ...
          (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p));
          (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2 ];

      poles_iff = rootLocusPolesSorted(Giff, 1/s*eye(2), gains, 'd_max', 1e-4);

      set(gca,'ColorOrderIndex',kp_i);
      plot(real(pole(Giff)),  imag(pole(Giff)), 'x', ...
           'DisplayName', sprintf('$k_p = %.1f m \\Omega^2$', kp/(m*W^2)));
      set(gca,'ColorOrderIndex',kp_i);
      plot(real(tzero(Giff)),  imag(tzero(Giff)), 'o', ...
           'HandleVisibility', 'off');
      for p_i = 1:size(poles_iff, 2)
          set(gca,'ColorOrderIndex', kp_i);
          plot(real(poles_iff(:, p_i)), imag(poles_iff(:, p_i)), '-', ...
               'HandleVisibility', 'off');
      end
  end
  hold off;
  axis square;
  xlim([-1.2, 0.2]); ylim([0, 1.4]);

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

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

#+begin_src matlab
  alphas = logspace(-2, 0, 100);

  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);
      k = 1 - alphas(alpha_i);

      w0p = sqrt((k + kp)/m);
      xip = c/(2*sqrt((k+kp)*m));

      Giff = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ...
          (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p));
          (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2];

      fun = @(g)computeSimultaneousDamping(g, Giff, 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 :exports none
  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$');

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

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

#+name: fig:opt_damp_alpha
#+caption: Attainable damping ratio and corresponding controller gain for different parameter $\alpha$
#+RESULTS:
[[file:figs/opt_damp_alpha.png]]

* Comparison
:PROPERTIES:
:header-args:matlab+: :tangle matlab/s5_act_damp_comparison.m
:END:
<<sec:comparison>>

** Introduction                                                      :ignore:
Two modifications to adapt the IFF control strategy to rotating platforms have been proposed.
These two methods are now compared in terms of added damping, closed-loop compliance and transmissibility.

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

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

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

** Plant Parameters
Let's define initial values for the model.
#+begin_src matlab
  k = 1;    % Actuator Stiffness [N/m]
  c = 0.05; % Actuator Damping [N/(m/s)]
  m = 1;    % Payload mass [kg]
#+end_src

#+begin_src matlab
  xi = c/(2*sqrt(k*m));
  w0 = sqrt(k/m); % [rad/s]
#+end_src

#+begin_src matlab :exports none
  kp = 0; % [N/m]
  cp = 0; % [N/(m/s)]
#+end_src

The rotating speed is set to $\Omega = 0.1 \omega_0$.
#+begin_src matlab
  W = 0.1*w0;
#+end_src

** Root Locus
IFF with High Pass Filter
#+begin_src matlab
  wi = 0.1*w0; % [rad/s]

  Giff = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
          [(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ...
           (2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2];
#+end_src

IFF With parallel Stiffness
#+begin_src matlab
  kp = 5*m*W^2;
  k = k - kp;

  w0p = sqrt((k + kp)/m);
  xip = c/(2*sqrt((k+kp)*m));

  Giff_kp = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ...
                     (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p));
                     (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2 ];

  k = k + kp;
#+end_src

#+begin_src matlab :exports none
  figure;

  gains = logspace(-2, 2, 100);

  hold on;
  set(gca,'ColorOrderIndex',1);
  plot(real(pole(Giff)),  imag(pole(Giff)), 'x', ...
       'DisplayName', 'IFF + LFP');
  set(gca,'ColorOrderIndex',1);
  plot(real(tzero(Giff)),  imag(tzero(Giff)), 'o', ...
       'HandleVisibility', 'off');
  for g = gains
      Kiff = (g/(wi + s))*eye(2);
      cl_poles = pole(feedback(Giff, Kiff));
      set(gca,'ColorOrderIndex',1);
      plot(real(cl_poles), imag(cl_poles), '.', ...
           'HandleVisibility', 'off');
  end

  set(gca,'ColorOrderIndex',2);
  plot(real(pole(Giff_kp)),  imag(pole(Giff_kp)), 'x', ...
       'DisplayName', 'IFF + $k_p$');
  set(gca,'ColorOrderIndex',2);
  plot(real(tzero(Giff_kp)),  imag(tzero(Giff_kp)), 'o', ...
       'HandleVisibility', 'off');
  for g = gains
      Kiffa = (g/s)*eye(2);
      cl_poles = pole(feedback(Giff_kp, Kiffa));
      set(gca,'ColorOrderIndex',2);
      plot(real(cl_poles), imag(cl_poles), '.', ...
           'HandleVisibility', 'off');
  end
  hold off;
  axis square;
  xlim([-1.2, 0.05]); ylim([0, 1.25]);

  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 replace
  exportFig('figs/comp_root_locus.pdf', 'width', 'half', 'height', 'normal');
#+end_src

#+name: fig:comp_root_locus
#+caption: Root Locus plot - Comparison of IFF with additional high pass filter, IFF with additional parallel stiffness
#+RESULTS:
[[file:figs/comp_root_locus.png]]

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

  poles_iff_hpf = rootLocusPolesSorted(Giff, 1/(s + wi)*eye(2), gains, 'd_max', 1e-4);
  poles_iff_kp = rootLocusPolesSorted(Giff_kp, 1/s*eye(2), gains, 'd_max', 1e-4);

  figure;

  hold on;
  set(gca,'ColorOrderIndex',1);
  plot(real(pole(Giff)),  imag(pole(Giff)), 'x', ...
       'DisplayName', 'IFF + LFP');
  set(gca,'ColorOrderIndex',1);
  plot(real(tzero(Giff)),  imag(tzero(Giff)), 'o', ...
       'HandleVisibility', 'off');
  for p_i = 1:size(poles_iff_hpf, 2)
      set(gca,'ColorOrderIndex',1);
      plot(real(poles_iff_hpf(:, p_i)), imag(poles_iff_hpf(:, p_i)), '-', ...
           'HandleVisibility', 'off');
  end

  set(gca,'ColorOrderIndex',2);
  plot(real(pole(Giff_kp)),  imag(pole(Giff_kp)), 'x', ...
       'DisplayName', 'IFF + $k_p$');
  set(gca,'ColorOrderIndex',2);
  plot(real(tzero(Giff_kp)),  imag(tzero(Giff_kp)), 'o', ...
       'HandleVisibility', 'off');
  for p_i = 1:size(poles_iff_kp, 2)
      set(gca,'ColorOrderIndex',2);
      plot(real(poles_iff_kp(:, p_i)), imag(poles_iff_kp(:, p_i)), '-', ...
           'HandleVisibility', 'off');
  end
  hold off;
  axis square;
  xlim([-1.2, 0.05]); ylim([0, 1.25]);

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

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

** Controllers - Optimal Gains
In order to compare to three considered Active Damping techniques, gains that yield maximum damping of all the modes are computed for each case.

#+begin_src matlab :exports none
  fun = @(g)computeSimultaneousDamping(g, Giff, (1/(wi+s))*eye(2));

  [opt_gain_iff, opt_xi_iff] = fminsearch(fun, 0.5*(w0^2/W^2 - 1)*wi);
  opt_xi_iff = 1/opt_xi_iff;
#+end_src

#+begin_src matlab :exports none
  fun = @(g)computeSimultaneousDamping(g, Giff_kp, 1/s*eye(2));

  [opt_gain_kp, opt_xi_kp] = fminsearch(fun, 2);
  opt_xi_kp = 1/opt_xi_kp;
#+end_src

The obtained damping ratio and control are shown below.

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
  data2orgtable([opt_xi_iff, opt_xi_kp; opt_gain_iff, opt_gain_kp]', {'Modified IFF', 'IFF with $k_p$'}, {'Obtained $\xi$', 'Control Gain'}, ' %.2f ');
#+end_src

#+RESULTS:
|                | Obtained $\xi$ | Control Gain |
|----------------+----------------+--------------|
| Modified IFF   |           0.83 |         1.99 |
| IFF with $k_p$ |           0.83 |         2.02 |

** Passive Damping - Critical Damping
\begin{equation}
  \xi = \frac{c}{2 \sqrt{km}}
\end{equation}

Critical Damping corresponds to to $\xi = 1$, and thus:
\begin{equation}
  c_{\text{crit}} = 2 \sqrt{km}
\end{equation}

#+begin_src matlab
  c_opt = 2*sqrt(k*m);
#+end_src

** Transmissibility And Compliance
<<sec:comp_transmissibilty>>

#+begin_src matlab
  open('rotating_frame.slx');
#+end_src

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

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/dw'], 1, 'input');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/fd'], 1, 'input');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Meas'], 1, 'output');  io_i = io_i + 1;
#+end_src

*** Open Loop                                                       :ignore:
#+begin_src matlab :exports none
  Kiff = tf(zeros(2));

  kp = 0;
  cp = 0;
#+end_src

#+begin_src matlab
  G_ol = linearize(mdl, io, 0);

  %% Input/Output definition
  G_ol.InputName  = {'Dwx', 'Dwy', 'Fdx', 'Fdy'};
  G_ol.OutputName = {'Dx', 'Dy'};
#+end_src

*** Passive Damping
#+begin_src matlab
  kp = 0;
  cp = 0;
#+end_src

#+begin_src matlab
  c_old = c;
  c = c_opt;
#+end_src

#+begin_src matlab :exports none
  Kiff = tf(zeros(2));
#+end_src

#+begin_src matlab
  G_pas = linearize(mdl, io, 0);

  %% Input/Output definition
  G_pas.InputName  = {'Dwx', 'Dwy', 'Fdx', 'Fdy'};
  G_pas.OutputName = {'Dx', 'Dy'};
#+end_src

#+begin_src matlab
  c = c_old;
#+end_src

*** Pseudo Integrator IFF                                           :ignore:
#+begin_src matlab :exports none
  kp = 0;
  cp = 0;
#+end_src

#+begin_src matlab
  Kiff = opt_gain_iff/(wi + s)*tf(eye(2));
#+end_src

#+begin_src matlab
  G_iff = linearize(mdl, io, 0);

  %% Input/Output definition
  G_iff.InputName  = {'Dwx', 'Dwy', 'Fdx', 'Fdy'};
  G_iff.OutputName = {'Dx', 'Dy'};
#+end_src

*** IFF With parallel Stiffness                                     :ignore:
#+begin_src matlab
  kp = 5*m*W^2;
  cp = 0.01;
#+end_src

#+begin_src matlab
  Kiff = opt_gain_kp/s*tf(eye(2));
#+end_src

#+begin_src matlab
  G_kp = linearize(mdl, io, 0);

  %% Input/Output definition
  G_kp.InputName  = {'Dwx', 'Dwy', 'Fdx', 'Fdy'};
  G_kp.OutputName = {'Dx', 'Dy'};
#+end_src

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

  figure;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G_iff({'Dx'}, {'Dwx'}), freqs))), ...
       'DisplayName', 'IFF + HPF')
  plot(freqs, abs(squeeze(freqresp(G_kp( {'Dx'}, {'Dwx'}), freqs))), ...
       'DisplayName', 'IFF + $k_p$')
  plot(freqs, abs(squeeze(freqresp(G_pas({'Dx'}, {'Dwx'}), freqs))), ...
       'DisplayName', 'Passive')
  plot(freqs, abs(squeeze(freqresp(G_ol( {'Dx'}, {'Dwx'}), freqs))), 'k-', ...
       'DisplayName', 'Open-Loop')
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylim([1e-2, 3e1]);
  xlabel('Frequency [rad/s]'); ylabel('Transmissibility [m/m]');
  legend('location', 'southwest', 'FontSize', 8);
#+end_src

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

#+name: fig:comp_transmissibility
#+caption: Comparison of the transmissibility
#+RESULTS:
[[file:figs/comp_transmissibility.png]]

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

  figure;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G_iff({'Dx'}, {'Fdx'}), freqs))), ...
       'DisplayName', 'IFF + HPF')
  plot(freqs, abs(squeeze(freqresp(G_kp( {'Dx'}, {'Fdx'}), freqs))), ...
       'DisplayName', 'IFF + $k_p$')
  plot(freqs, abs(squeeze(freqresp(G_pas({'Dx'}, {'Fdx'}), freqs))), ...
       'DisplayName', 'Passive')
  plot(freqs, abs(squeeze(freqresp(G_ol( {'Dx'}, {'Fdx'}), freqs))), 'k-', ...
       'DisplayName', 'Open-Loop')
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylim([1e-2, 3e1]);
  xlabel('Frequency [rad/s]'); ylabel('Compliance [m/N]');
  legend('location', 'southwest', 'FontSize', 8);
#+end_src

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

#+name: fig:comp_compliance
#+caption: Comparison of the obtained Compliance
#+RESULTS:
[[file:figs/comp_compliance.png]]

* Notations
<<sec:notations>>

|                                   | Mathematical Notation        | Matlab        | Unit    |
|-----------------------------------+------------------------------+---------------+---------|
| Actuator Stiffness                | $k$                          | =k=           | N/m     |
| Actuator Damping                  | $c$                          | =c=           | N/(m/s) |
| Payload Mass                      | $m$                          | =m=           | kg      |
| Damping Ratio                     | $\xi = \frac{c}{2\sqrt{km}}$ | =xi=          |         |
| Actuator Force                    | $\bm{F}, F_u, F_v$           | =F= =Fu= =Fv= | N       |
| Force Sensor signal               | $\bm{f}, f_u, f_v$           | =f= =fu= =fv= | N       |
| Relative Displacement             | $\bm{d}, d_u, d_v$           | =d= =du= =dv= | m       |
| Resonance freq. when $\Omega = 0$ | $\omega_0$                   | =w0=          | rad/s   |
| Rotation Speed                    | $\Omega = \dot{\theta}$      | =W=           | rad/s   |
| Low Pass Filter corner frequency  | $\omega_i$                   | =wi=          | rad/s   |

|                  | Mathematical Notation | Matlab | Unit    |
|------------------+-----------------------+--------+---------|
| Laplace variable | $s$                   | =s=    |         |
| Complex number   | $j$                   | =j=    |         |
| Frequency        | $\omega$              | =w=    | [rad/s] |

* Bibliography                                                        :ignore:
#+latex: \printbibliography
bibliography:ref.bib

* Functions                                                         :noexport:
** Sort Poles for the Root Locus
:PROPERTIES:
:header-args:matlab+: :tangle src/rootLocusPolesSorted.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:rootLocusPolesSorted>>

This Matlab function is accessible [[file:src/rootLocusPolesSorted.m][here]].

*** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  function [poles] = rootLocusPolesSorted(G, K, gains, args)
  % rootLocusPolesSorted -
  %
  % Syntax: [poles] = rootLocusPolesSorted(G, K, gains, args)
  %
  % Inputs:
  %    - G, K, gains, args -
  %
  % Outputs:
  %    - poles -
#+end_src

*** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  arguments
      G
      K
      gains
      args.minreal double {mustBeNumericOrLogical} = false
      args.p_half  double {mustBeNumericOrLogical} = false
      args.d_max   double {mustBeNumeric} = -1
  end
#+end_src

*** Function
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
  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
#+end_src

#+begin_src matlab
  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
#+end_src

#+begin_src matlab
  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
#+end_src

*** Remove useless poles
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
  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
#+end_src

*** Sort poles
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
  [~, s_p] = sort(imag(poles(:,1)), 'descend');
  poles = poles(s_p, :);
#+end_src

*** Transpose for easy plotting
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
  poles = poles.';
#+end_src

** =computeSimultaneousDamping=
#+begin_src matlab :tangle src/computeSimultaneousDamping.m
  function [xi_min] = computeSimultaneousDamping(g, G, K)
      [w, xi] = damp(minreal(feedback(G, g*K)));
      xi_min = 1/min(xi);
     
      if xi_min < 0
        xi_min = 1e8;
      end
  end
#+end_src