#+TITLE: Matlab Computation
:DRAWER:
#+HTML_LINK_HOME: ../index.html
#+HTML_LINK_UP: ../index.html

#+LATEX_CLASS: cleanreport
#+LATEX_CLASS_OPTIONS: [tocnp, secbreak, minted]

#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
#+HTML_HEAD: <script src="../js/jquery.min.js"></script>
#+HTML_HEAD: <script src="../js/bootstrap.min.js"></script>
#+HTML_HEAD: <script src="../js/jquery.stickytableheaders.min.js"></script>
#+HTML_HEAD: <script src="../js/readtheorg.js"></script>

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

* Introduction                                                        :ignore:

- Section [[sec:system_description]]
- Section [[sec:iff_pure_int]]
- Section [[sec:iff_pseudo_int]]
- Section [[sec:iff_parallel_stiffness]]
- Section [[sec:dvf]]
- Section [[sec:comparison]]
- Section [[sec:notations]]

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

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

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

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

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

#+name: fig:rotating_xy_platform
#+caption: Schematic of the studied system
[[file:figs-tikz/rotating_xy_platform.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$.

The measurement is either the $x-y$ displacement of the object located on top of the translation stage or the $u-v$ displacement of the sample with respect to a fixed reference frame.

** Equations
Based on the Figure [[fig:rotating_xy_platform]], 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

Explain Coriolis and Centrifugal Forces (negative Stiffness)
=> First write the equations in terms of $k$, $m$ and $c$ and explain the terms.

** 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 Figure [[fig:campbell_diagram]], 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
  figure;

  ax1 = subplot(1,2,1);
  hold on;
  for p_i = 1:size(p_ws, 1)
      plot(Ws, real(p_ws(p_i, :)), 'k-')
  end
  plot(Ws, zeros(size(Ws)), 'k--')
  hold off;
  xlabel('Rotation Frequency [rad/s]'); ylabel('Real Part');

  ax2 = subplot(1,2,2);
  hold on;
  for p_i = 1:size(p_ws, 1)
      plot(Ws,  imag(p_ws(p_i, :)), 'k-')
      plot(Ws, -imag(p_ws(p_i, :)), 'k-')
  end
  hold off;
  xlabel('Rotation Frequency [rad/s]'); ylabel('Imaginary Part');
#+end_src

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

#+name: fig:campbell_diagram
#+caption: Campbell Diagram
#+RESULTS:
[[file:figs/campbell_diagram.png]]

#+begin_src matlab :exports none :tangle no
  figure;

  ax1 = subplot(1,2,1);
  hold on;
  for p_i = 1:size(p_ws, 1)
      plot(Ws, real(p_ws(p_i, :)), 'k-')
  end
  plot(Ws, zeros(size(Ws)), 'k--')
  hold off;
  xlabel('Rotation Frequency $\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, xi]);
  yticks([-3*xi, -2*xi, -xi, 0, xi])
  yticklabels({'', '', '$-\xi$', '$0$', ''})

  ax2 = subplot(1,2,2);
  hold on;
  for p_i = 1:size(p_ws, 1)
      plot(Ws,  imag(p_ws(p_i, :)), 'k-')
      plot(Ws, -imag(p_ws(p_i, :)), 'k-')
  end
  plot(Ws, zeros(size(Ws)), 'k--')
  hold off;
  xlabel('Rotation Frequency $\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 none :results none
  exportFig('figs-inkscape/campbell_diagram.pdf', 'width', 'full', 'height', 'normal', 'png', false, 'pdf', false, 'svg', true);
#+end_src

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

#+begin_src matlab :exports none
  Kiff = tf(zeros(2));
  Kdvf = 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'], 3, '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;
  ax1 = subplot(2, 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$');

  ax3 = subplot(2, 2, 3);
  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;

  ax2 = subplot(2, 2, 2);
  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$');

  ax4 = subplot(2, 2, 4);
  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');

  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', 'full', 'height', 'full');
#+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 Figure [[fig:plant_compare_rotating_speed]].

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

  figure;
  ax1 = subplot(2, 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]');
  legend('location', 'southwest');
  title('$d_u/F_u$, $d_v/F_v$');

  ax3 = subplot(2, 2, 3);
  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;

  ax2 = subplot(2, 2, 2);
  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]');
  title('$d_u/F_v$, $d_v/F_u$');

  ax4 = subplot(2, 2, 4);
  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,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_compare_rotating_speed.pdf', 'width', 'full', 'height', 'full');
#+end_src

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

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

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

** Introduction                                                      :ignore:
- Diagram with the controller
- Basic idea of IFF

** 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

** 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));
  Kdvf = 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'], 2, '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

** 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
  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;
  ax1 = subplot(2, 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$');

  ax3 = subplot(2, 2, 3);
  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;

  ax2 = subplot(2, 2, 2);
  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$');

  ax4 = subplot(2, 2, 4);
  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');

  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', 'full', 'height', 'full');
#+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, 1.1]*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;

  ax1 = subplot(2, 1, 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]');
  legend('location', 'southeast');

  ax2 = subplot(2, 1, 2);
  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', 'full', 'height', 'full');
#+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]]

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

** 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');
  legend('location', 'northwest');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/root_locus_pure_iff.pdf', 'width', 'wide', 'height', 'tall');
#+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');
  legend('location', 'northwest');
#+end_src

#+begin_src matlab :tangle no :exports none :results none
  exportFig('figs-inkscape/root_locus_pure_iff.pdf', 'width', 'wide', 'height', 'tall', '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>>

** Introduction                                                      :ignore:
- Classical modification of the IFF

** 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;

  ax1 = subplot(2, 1, 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');

  ax2 = subplot(2, 1, 2);
  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]);
  legend('location', 'southwest');
  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', 'full', 'height', 'full');
#+end_src

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

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

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

  ax1 = subplot(1, 2, 1);
  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]);
  legend('location', 'northwest');
  xlabel('Real Part'); ylabel('Imaginary Part');

  ax2 = subplot(1, 2, 2);
  hold on;
  % Pure Integrator
  set(gca,'ColorOrderIndex',1);
  plot(real(pole(Giff)),  imag(pole(Giff)), 'x');
  set(gca,'ColorOrderIndex',1);
  plot(real(tzero(Giff)),  imag(tzero(Giff)), 'o');
  for g = gains
      clpoles = pole(feedback(Giff, (g/s)*eye(2)));
      set(gca,'ColorOrderIndex',1);
      plot(real(clpoles), imag(clpoles), '.');
  end
  % Modified IFF
  set(gca,'ColorOrderIndex',2);
  plot(real(pole(Giff)),  imag(pole(Giff)), 'x');
  set(gca,'ColorOrderIndex',2);
  plot(real(tzero(Giff)),  imag(tzero(Giff)), 'o');
  for g = gains
      clpoles = pole(feedback(Giff, (g/(wi+s))*eye(2)));
      set(gca,'ColorOrderIndex',2);
      plot(real(clpoles), imag(clpoles), '.');
  end
  hold off;
  axis square;
  xlim([-0.2, 0.1]); ylim([-0.15, 0.15]);
  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', 'full', 'height', 'tall');
#+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 :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;

  ax1 = subplot(1, 2, 1);
  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]);
  legend('location', 'northwest');
  xlabel('Real Part'); ylabel('Imaginary Part');

  ax2 = subplot(1, 2, 2);
  hold on;
  % Pure Integrator
  set(gca,'ColorOrderIndex',1);
  plot(real(pole(Giff)),  imag(pole(Giff)), 'x');
  set(gca,'ColorOrderIndex',1);
  plot(real(tzero(Giff)),  imag(tzero(Giff)), 'o');
  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');
  set(gca,'ColorOrderIndex',2);
  plot(real(tzero(Giff)),  imag(tzero(Giff)), 'o');
  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([-0.2, 0.1]); ylim([-0.15, 0.15]);
  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', 'full', 'height', 'tall', '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);

  ax1 = subplot(1, 2, 1);
  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]);
  legend(L, 'location', 'northwest');
  xlabel('Real Part'); ylabel('Imaginary Part');

  clear L

  ax2 = subplot(1, 2, 2);
  hold on;
  for wi_i = 1:length(wis)
      set(gca,'ColorOrderIndex', wi_i);
      wi = wis(wi_i);
      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([-0.2, 0.1]); ylim([-0.15, 0.15]);
  xticks([-0.2:0.1:0.1]); yticks([-0.2:0.1:0.2]);
  xlabel('Real Part'); ylabel('Imaginary Part');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/root_locus_wi_modified_iff.pdf', 'width', 'full', 'height', 'tall');
#+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 :tangle no
  gains = logspace(-2, 4, 500);

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

  figure;

  ax1 = subplot(1, 2, 1);
  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]);
  legend(L, 'location', 'northwest');
  xlabel('Real Part'); ylabel('Imaginary Part');

  clear L

  ax2 = subplot(1, 2, 2);
  hold on;
  for wi_i = 1:length(wis)
      wi = wis(wi_i);

      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([-0.2, 0.1]); ylim([-0.15, 0.15]);
  xticks([-0.2:0.1:0.1]); yticks([-0.2:0.1:0.2]);
  xlabel('Real Part'); ylabel('Imaginary Part');
#+end_src

#+begin_src matlab :tangle no :exports none :results none
  exportFig('figs-inkscape/root_locus_wi_modified_iff.pdf', 'width', 'full', 'height', 'tall', '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, 31)*w0; % [rad/s]

  opt_zeta = 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);
      gains = linspace(0, (w0^2/W^2 - 1)*wi, 100);

      for g = gains
          Kiff = (g/(wi+s))*eye(2);

          [w, zeta] = damp(minreal(feedback(Giff, Kiff)));

          if min(zeta) > opt_zeta(wi_i) && all(zeta > 0)
              opt_zeta(wi_i) = min(zeta);
              opt_gain(wi_i) = g;
          end
      end
  end
#+end_src

#+begin_src matlab :exports none
  figure;
  yyaxis left
  plot(wis, opt_zeta, '-o', 'DisplayName', '$\xi_{cl}$');
  set(gca, 'YScale', 'lin');
  ylim([0,1]);
  ylabel('Attainable Damping Ratio $\xi$');

  yyaxis right
  hold on;
  plot(wis, opt_gain, '-x', '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');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/mod_iff_damping_wi.pdf', 'width', 'wide', 'height', 'normal');
#+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]]

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

* 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>>

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

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

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

** Schematic

#+name: fig:rotating_xy_platform_springs
#+caption: Figure caption
[[file:figs-tikz/rotating_xy_platform_springs.png]]

** 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^\prime}^2} + 2\xi^\prime \frac{s}{{\omega_0^\prime}^2} + 1 - \frac{\Omega^2}{{\omega_0^\prime}^2} \right)^2 + \left( 2 \frac{\Omega}{\omega_0^\prime}\frac{s}{\omega_0^\prime} \right)^2 \\
  G_{kz} &= \left( \frac{s^2}{{\omega_0^\prime}^2} + \frac{k_p}{k + k_p} - \frac{\Omega^2}{{\omega_0^\prime}^2} \right) \left( \frac{s^2}{{\omega_0^\prime}^2} + 2\xi^\prime \frac{s}{{\omega_0^\prime}^2} + 1 - \frac{\Omega^2}{{\omega_0^\prime}^2} \right) + \left( 2 \frac{\Omega}{\omega_0^\prime}\frac{s}{\omega_0^\prime} \right)^2 \\
  G_{kc} &= \left( 2 \xi^\prime \frac{s}{\omega_0^\prime} + \frac{k}{k + k_p} \right) \left( 2 \frac{\Omega}{\omega_0^\prime}\frac{s}{\omega_0^\prime} \right)
\end{align}
where:
- $\omega_0^\prime = \frac{k + k_p}{m}$
- $\xi^\prime = \frac{c}{2 \sqrt{(k + k_p) m}}$
#+end_important

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

** Physical Explanation
- Negative stiffness induced by gyroscopic effects
- Zeros of the open-loop <=> Poles of the subsystem with the force sensors removes
- As the zeros are the poles of the closed loop system for high gains, we want them to be in the left-half plane
- Thus we want the zeros to be in the left half plant and thus the system with the force sensors stable
- This can be done by adding springs in parallel with the force sensors with a stiffness larger than the virtual negative stiffness added by the gyroscopic effects

The negative stiffness induced by the rotation is:
\begin{equation}
  k_{n} = - m \Omega^2
\end{equation}

And thus, the stiffness in parallel should be such that:
\begin{equation}
  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));
  Kdvf = 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'], 2, '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;
  ax1 = subplot(2, 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$');

  ax3 = subplot(2, 2, 3);
  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;

  ax2 = subplot(2, 2, 2);
  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$');

  ax4 = subplot(2, 2, 4);
  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');

  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', 'full', 'height', 'full');
#+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;
  cp = 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;
  cp = 0;

  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;
  cp = 0;

  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;

  ax1 = subplot(2, 1, 1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Giff(1,1),   freqs))), 'k-')
  plot(freqs, abs(squeeze(freqresp(Giff_s(1,1), freqs))), 'k--')
  plot(freqs, abs(squeeze(freqresp(Giff_l(1,1), freqs))), 'k:')
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');

  ax2 = subplot(2, 1, 2);
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1),   freqs))), 'k-', ...
       'DisplayName', '$k_p = 0$')
  plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_s(1,1), freqs))), 'k--', ...
       'DisplayName', '$k_p < m\Omega^2$')
  plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_l(1,1), freqs))), 'k:', ...
       'DisplayName', '$k_p > m\Omega^2$')
  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');

  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', 'full', 'height', 'full');
#+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]]

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

** 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
  figure;

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

  subplot(1,2,1);
  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');
  legend('location', 'northwest');

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

  set(gca,'ColorOrderIndex',2);
  plot(real(pole(Giff_s)),  imag(pole(Giff_s)), 'x');
  set(gca,'ColorOrderIndex',2);
  plot(real(tzero(Giff_s)),  imag(tzero(Giff_s)), 'o');
  for g = gains
      cl_poles = pole(feedback(Giff_s, (g/s)*eye(2)));
      set(gca,'ColorOrderIndex',2);
      plot(real(cl_poles), imag(cl_poles), '.');
  end

  set(gca,'ColorOrderIndex',3);
  plot(real(pole(Giff_l)),  imag(pole(Giff_l)), 'x');
  set(gca,'ColorOrderIndex',3);
  plot(real(tzero(Giff_l)),  imag(tzero(Giff_l)), 'o');
  for g = gains
      set(gca,'ColorOrderIndex',3);
      cl_poles = pole(feedback(Giff_l, (g/s)*eye(2)));
      plot(real(cl_poles), imag(cl_poles), '.');
  end
  hold off;
  axis square;
  xlim([-0.04, 0.06]); ylim([0, 0.1]);

  xlabel('Real Part'); ylabel('Imaginary Part');
#+end_src

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

#+name: fig:root_locus_iff_kp
#+caption: Root Locus
#+RESULTS:
[[file:figs/root_locus_iff_kp.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;

  subplot(1,2,1);
  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');
  legend('location', 'northwest');

  subplot(1,2,2);
  hold on;
  set(gca,'ColorOrderIndex',1);
  plot(real(pole(Giff)),  imag(pole(Giff)), 'x');
  set(gca,'ColorOrderIndex',1);
  plot(real(tzero(Giff)),  imag(tzero(Giff)), 'o');
  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');
  set(gca,'ColorOrderIndex',2);
  plot(real(tzero(Giff_s)),  imag(tzero(Giff_s)), 'o');
  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');
  set(gca,'ColorOrderIndex',3);
  plot(real(tzero(Giff_l)),  imag(tzero(Giff_l)), 'o');
  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([-0.04, 0.06]); ylim([0, 0.1]);
  xlabel('Real Part'); ylabel('Imaginary Part');
#+end_src

#+begin_src matlab :tangle no :exports none :results none
  exportFig('figs-inkscape/root_locus_iff_kp.pdf', 'width', 'full', 'height', 'tall', '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 actuator force authority
- 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);

      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');
  legend('location', 'northwest');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/root_locus_iff_kps.pdf', 'width', 'wide', 'height', 'tall');
#+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');
  legend('location', 'northwest');
#+end_src

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

** Optimal Gain
Let's take $k_p = 5 m \Omega^2$ and find the optimal IFF control gain $g$ such that maximum damping are added to the poles of the closed loop system.

#+begin_src matlab
  kp = 5*m*W^2;
  cp = 0.01;

  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
  opt_zeta = 0;
  opt_gain = 0;

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

  for g = gains
      Kiff = (g/s)*eye(2);

      [w, zeta] = damp(minreal(feedback(Giff, Kiff)));

      if min(zeta) > opt_zeta && all(zeta > 0)
          opt_zeta = min(zeta);
          opt_gain = min(g);
      end
  end
#+end_src

#+begin_src matlab :exports none
  figure;

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

  hold on;
  plot(real(pole(Giff)),  imag(pole(Giff)), 'kx');
  plot(real(tzero(Giff)),  imag(tzero(Giff)), 'ko');
  for g = gains
      clpoles = pole(minreal(feedback(Giff, (g/s)*eye(2))));
      plot(real(clpoles), imag(clpoles), 'k.');
  end
  % Optimal Gain
  clpoles = pole(minreal(feedback(Giff, (opt_gain/s)*eye(2))));
  set(gca,'ColorOrderIndex',1);
  plot(real(clpoles), imag(clpoles), 'x');
  for clpole = clpoles'
    set(gca,'ColorOrderIndex',1);
    plot([0, real(clpole)], [0, imag(clpole)], '-', 'LineWidth', 1);
  end
  hold off;
  axis square;
  xlim([-1.2, 0.2]); ylim([0, 1.4]);
  xlabel('Real Part'); ylabel('Imaginary Part');
#+end_src

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

#+name: fig:root_locus_opt_gain_iff_kp
#+caption: Root Locus for $k_p = 5 m \Omega^2$ and the poles corresponding to the identified optimal gain
#+RESULTS:
[[file:figs/root_locus_opt_gain_iff_kp.png]]

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

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

  figure;

  hold on;
  plot(real(pole(Giff)),  imag(pole(Giff)), 'kx');
  plot(real(tzero(Giff)),  imag(tzero(Giff)), 'ko');
  for p_i = 1:size(poles_iff, 2)
      plot(real(poles_iff(:, p_i)), imag(poles_iff(:, p_i)), 'k-', ...
           'HandleVisibility', 'off');
  end
  % Optimal Gain
  clpoles = pole(minreal(feedback(Giff, (opt_gain/s)*eye(2))));
  set(gca,'ColorOrderIndex',1);
  plot(real(clpoles), imag(clpoles), 'x');
  for clpole = clpoles'
    set(gca,'ColorOrderIndex',1);
    plot([0, real(clpole)], [0, imag(clpole)], '-', 'LineWidth', 1);
  end
  hold off;
  axis square;
  xlim([-1.2, 0.2]); ylim([0, 1.4]);

  xlabel('Real Part'); ylabel('Imaginary Part');
#+end_src

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

* Direct Velocity Feedback
:PROPERTIES:
:header-args:matlab+: :tangle matlab/s5_dvf.m
:header-args:matlab+: :comments org :mkdirp yes
:END:
<<sec:dvf>>

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

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

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

** Equations
The sensed relative velocity are equal to:
#+begin_important
\begin{equation}
\begin{bmatrix} \dot{d}_u \\ \dot{d}_v \end{bmatrix} =
\bm{G}_v
\begin{bmatrix} F_u \\ F_v \end{bmatrix}
\end{equation}

\begin{equation}
\begin{bmatrix} \dot{d}_u \\ \dot{d}_v \end{bmatrix} =
\frac{s}{k} \frac{1}{G_{vp}}
\begin{bmatrix}
   G_{vz} & G_{vc} \\
  -G_{vc} & G_{vz}
\end{bmatrix}
\begin{bmatrix} F_u \\ F_v \end{bmatrix}
\end{equation}
With:
\begin{align}
  G_{vp} &= \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_{vz} &= \frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1 - \frac{{\Omega}^2}{{\omega_0}^2} \\
  G_{vc} &= 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0}
\end{align}
#+end_important

** 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 rotating speed is set to $\Omega = 0.1 \omega_0$.
#+begin_src matlab
  W = 0.1*w0;
#+end_src

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

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

And the transfer function from $[F_u, F_v]$ to $[v_u, v_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
  Gdvf = linearize(mdl, io, 0);

  %% Input/Output definition
  Gdvf.InputName  = {'Fu', 'Fv'};
  Gdvf.OutputName = {'Vu', 'Vv'};
#+end_src

The same transfer function from $[F_u, F_v]$ to $[v_u, v_v]$ is written down from the analytical model.
#+begin_src matlab
  Gdvf_th = (s/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)];

  Gdvf_th.InputName  = {'Fu', 'Fv'};
  Gdvf_th.OutputName = {'vu', 'vv'};
#+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;
  ax1 = subplot(2, 2, 1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gdvf(1,1), freqs))), '-')
  plot(freqs, abs(squeeze(freqresp(Gdvf_th(1,1), freqs))), '--')
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]); ylabel('Magnitude [$\frac{m/s}{N}$]');
  title('$v_u/F_u$, $v_v/F_v$');

  ax3 = subplot(2, 2, 3);
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf(1,1), freqs))), '-')
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf_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;

  ax2 = subplot(2, 2, 2);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gdvf(1,2), freqs))), '-')
  plot(freqs, abs(squeeze(freqresp(Gdvf_th(1,2), freqs))), '--')
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]); ylabel('Magnitude [$\frac{m/s}{N}$]');
  title('$v_u/F_v$, $v_v/F_u$');

  ax4 = subplot(2, 2, 4);
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf(1,2), freqs))), '-')
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf_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_dvf_comp_simscape_analytical.pdf', 'width', 'full', 'height', 'full');
#+end_src

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

** Root Locus
The Decentralized Direct Velocity Feedback controller consist of a pure gain on the diagonal:
\begin{equation}
  K_{\text{DVF}}(s) = g \begin{bmatrix}
  1 & 0 \\
  0 & 1
\end{bmatrix}
\end{equation}

The corresponding Root Locus plots for the following rotating speeds are shown in Figure [[fig:root_locus_dvf]].
#+begin_src matlab
  Ws = [0, 0.2, 0.7, 1.1]*w0; % Rotating Speeds [rad/s]
#+end_src

It is shown that for rotating speed $\Omega < \omega_0$, the closed loop system is unconditionally stable and arbitrary damping can be added to the poles.
#+begin_src matlab :exports none
  gains = logspace(-2, 1, 100);

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

      Gdvf = (s/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)];

      set(gca,'ColorOrderIndex',W_i);
      plot(real(pole(Gdvf)),  imag(pole(Gdvf)), 'x', ...
           'DisplayName', sprintf('$\\Omega = %.2f \\omega_0 $', W/w0));

      set(gca,'ColorOrderIndex',W_i);
      plot(real(tzero(Gdvf)),  imag(tzero(Gdvf)), 'o', ...
           'HandleVisibility', 'off');
   
      for g = gains
          set(gca,'ColorOrderIndex',W_i);
          cl_poles = pole(feedback(Gdvf, g*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');
  legend('location', 'northwest');
#+end_src

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

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

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

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

      Gdvf = (s/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)];

      set(gca,'ColorOrderIndex',W_i);
      plot(real(pole(Gdvf)),  imag(pole(Gdvf)), 'x', ...
           'DisplayName', sprintf('$\\Omega = %.2f \\omega_0 $', W/w0));

      set(gca,'ColorOrderIndex',W_i);
      plot(real(tzero(Gdvf)),  imag(tzero(Gdvf)), 'o', ...
           'HandleVisibility', 'off');

      poles_dvf = rootLocusPolesSorted(Gdvf, eye(2), gains, 'd_max', 1e-4);
      for p_i = 1:size(poles_dvf, 2)
          set(gca,'ColorOrderIndex', W_i);
          plot(real(poles_dvf(:, p_i)), imag(poles_dvf(:, p_i)), '-', ...
               'HandleVisibility', 'off');
      end
  end
  hold off;
  axis square;
  xlim([-2, 0.5]); ylim([0, 2.5]);

  xlabel('Real Part'); ylabel('Imaginary Part');
  legend('location', 'northwest');
#+end_src

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

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

** Introduction                                                      :ignore:

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

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

#+begin_src matlab :tangle no
  addpath('./matlab/');
  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

DVF
#+begin_src matlab
  Gdvf = (s/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

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

  set(gca,'ColorOrderIndex',3);
  plot(real(pole(Gdvf)),  imag(pole(Gdvf)), 'x', ...
       'DisplayName', 'DVF');
  set(gca,'ColorOrderIndex',3);
  plot(real(tzero(Gdvf)),  imag(tzero(Gdvf)), 'o', ...
       'HandleVisibility', 'off');
  for g = gains
      Kdvf = g*eye(2);
      cl_poles = pole(feedback(Gdvf, Kdvf));
      set(gca,'ColorOrderIndex',3);
      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');
  legend('location', 'northwest');
#+end_src

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

#+name: fig:comp_root_locus
#+caption: Root Locus plot - Comparison of IFF with additional high pass filter, IFF with additional parallel stiffness and DVF
#+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);
  poles_dvf = rootLocusPolesSorted(Gdvf, 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

  set(gca,'ColorOrderIndex',3);
  plot(real(pole(Gdvf)),  imag(pole(Gdvf)), 'x', ...
       'DisplayName', 'DVF');
  set(gca,'ColorOrderIndex',3);
  plot(real(tzero(Gdvf)),  imag(tzero(Gdvf)), 'o', ...
       'HandleVisibility', 'off');
  for p_i = 1:size(poles_dvf, 2)
      set(gca,'ColorOrderIndex',3);
      plot(real(poles_dvf(:, p_i)), imag(poles_dvf(:, p_i)), '-', ...
           'HandleVisibility', 'off');
  end
  hold off;
  axis square;
  xlim([-1.2, 0.05]); ylim([0, 1.25]);

  xlabel('Real Part'); ylabel('Imaginary Part');
  legend('location', 'northwest');
#+end_src

#+begin_src matlab :tangle no :exports none :results none
  exportFig('figs-inkscape/comp_root_locus.pdf', 'width', 'wide', 'height', 'tall', '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
  %% IFF with pseudo integrators
  gains = linspace(0, (w0^2/W^2 - 1)*wi, 100);
  opt_zeta_iff = 0;
  opt_gain_iff = 0;

  for g = gains
      Kiff = (g/(wi+s))*eye(2);

      [w, zeta] = damp(minreal(feedback(Giff, Kiff)));

      if min(zeta) > opt_zeta_iff && all(zeta > 0)
        opt_zeta_iff = min(zeta);
        opt_gain_iff = g;
      end
  end
#+end_src

#+begin_src matlab :exports none
  %% IFF with Parallel Stiffness
  gains = logspace(-2, 4, 100);
  opt_zeta_kp = 0;
  opt_gain_kp = 0;

  for g = gains
      Kiff = g/s*eye(2);

      [w, zeta] = damp(minreal(feedback(Giff_kp, Kiff)));

      if min(zeta) > opt_zeta_kp && all(zeta > 0)
        opt_zeta_kp = min(zeta);
        opt_gain_kp = g;
      end
  end
#+end_src

#+begin_src matlab :exports none
  %% Direct Velocity Feedback
  gains = logspace(0, 2, 100);
  opt_zeta_dvf = 0;
  opt_gain_dvf = 0;

  for g = gains
      Kdvf = g*eye(2);

      [w, zeta] = damp(minreal(feedback(Gdvf, Kdvf)));

      if min(zeta) > opt_zeta_dvf && all(zeta > 0) && min(zeta) < 0.85
        opt_zeta_dvf = min(zeta);
        opt_gain_dvf = g;
      end
  end
#+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_zeta_iff, opt_zeta_kp, opt_zeta_dvf; opt_gain_iff, opt_gain_kp, opt_gain_dvf]', {'Modified IFF', 'IFF with $k_p$', 'DVF'}, {'Obtained $\xi$', 'Control Gain'}, ' %.2f ');
#+end_src

#+RESULTS:
|                | Obtained $\xi$ | Control Gain |
|----------------+----------------+--------------|
| Modified IFF   |           0.83 |          2.0 |
| IFF with $k_p$ |           0.83 |         2.01 |
| DVF            |           0.85 |         1.67 |

** Transmissibility
<<sec:comp_transmissibilty>>

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

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

  kp = 0;
  cp = 0;
#+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, '/Meas'], 1, 'output');  io_i = io_i + 1;
#+end_src

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

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

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

  Kdvf = tf(zeros(2));
#+end_src

#+begin_src matlab
  Kiff = opt_gain_iff/(wi + s)*tf(eye(2));
#+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, '/Meas'], 1, 'output');  io_i = io_i + 1;
#+end_src

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

  %% Input/Output definition
  Tiff.InputName  = {'Dwx', 'Dwy'};
  Tiff.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 :exports none
  Kdvf = tf(zeros(2));
#+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, '/Meas'], 1, 'output');  io_i = io_i + 1;
#+end_src

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

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

*** DVF                                                             :ignore:
#+begin_src matlab :exports none
  kp = 0;
  cp = 0;

  Kiff = tf(zeros(2));
#+end_src

#+begin_src matlab
  Kdvf = opt_gain_kp*tf(eye(2));
#+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, '/Meas'], 1, 'output');  io_i = io_i + 1;
#+end_src

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

  %% Input/Output definition
  Tdvf.InputName  = {'Dwx', 'Dwy'};
  Tdvf.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(Tiff(1,1), freqs))), ...
       'DisplayName', 'IFF + HPF')
  plot(freqs, abs(squeeze(freqresp(Tiff_kp(1,1), freqs))), ...
       'DisplayName', 'IFF + $k_p$')
  plot(freqs, abs(squeeze(freqresp(Tdvf(1,1), freqs))), ...
       'DisplayName', 'DVF')
  plot(freqs, abs(squeeze(freqresp(Tol(1,1), freqs))), 'k-', ...
       'DisplayName', 'Open-Loop')
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [rad/s]'); ylabel('Transmissibility [m/m]');
  legend('location', 'southwest');
#+end_src

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

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

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

** Compliance
<<sec:comp_compliance>>
*** Open Loop                                                       :ignore:
#+begin_src matlab :exports none
  Kdvf = tf(zeros(2));
  Kiff = tf(zeros(2));

  kp = 0;
  cp = 0;
#+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, '/fd'], 1, 'input');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Meas'], 1, 'output');  io_i = io_i + 1;
#+end_src

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

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

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

  Kdvf = tf(zeros(2));
#+end_src

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

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

  %% Input/Output definition
  Ciff.InputName  = {'Fdx', 'Fdy'};
  Ciff.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 :exports none
  Kdvf = tf(zeros(2));
#+end_src

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

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

*** DVF                                                             :ignore:
#+begin_src matlab :exports none
  kp = 0;
  cp = 0;

  Kiff = tf(zeros(2));
#+end_src

#+begin_src matlab
  Kdvf = opt_gain_kp*tf(eye(2));
#+end_src

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

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

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

  figure;
  hold on;
  plot(freqs, abs(squeeze(freqresp(Ciff(1,1), freqs))), ...
       'DisplayName', 'IFF + HPF')
  plot(freqs, abs(squeeze(freqresp(Ciff_kp(1,1), freqs))), ...
       'DisplayName', 'IFF + $k_p$')
  plot(freqs, abs(squeeze(freqresp(Cdvf(1,1), freqs))), ...
       'DisplayName', 'DVF')
  plot(freqs, abs(squeeze(freqresp(Col(1,1), freqs))), 'k-', ...
       'DisplayName', 'Open-Loop')
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [rad/s]'); ylabel('Compliance [m/N]');
  legend('location', 'southwest');
#+end_src

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

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

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

* 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       |
| Relative Velocity                 | $\bm{v}, v_u, v_v$           | =v= =vu= =vv= | m/s     |
| 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] |

|                | Mathematical Notation                          | Matlab | Unit    |
|----------------+------------------------------------------------+--------+---------|
| IFF Plant      | $\bm{G}_\text{IFF}(s) = \frac{\bm{f}}{\bm{F}}$ | =Giff= | N/N     |
| DVF Plant      | $\bm{G}_\text{DVF}(s) = \frac{\bm{v}}{\bm{F}}$ | =Gdvf= | (m/s)/N |
| IFF Controller | $\bm{K}_\text{IFF}(s)$                         | =Kiff= |         |
| DVF Controller | $\bm{K}_\text{DVF}(s)$                         | =Kdvf= |         |

* Function
** 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
