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

#+TITLE: Control in a rotating frame
:DRAWER:
#+STARTUP: overview

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

#+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 type="text/javascript" src="./js/jquery.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="./js/bootstrap.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="./js/jquery.stickytableheaders.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="./js/readtheorg.js"></script>

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

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

* Introduction                                                       :ignore:
The objective of this note it to highlight some control problems that arises when controlling the position of an object using actuators that are rotating with respect to a fixed reference frame.

In section [[sec:system]], a simple system composed of a spindle and a translation stage is defined and the equations of motion are written.
The rotation induces some coupling between the actuators and their displacement, and modifies the dynamics of the system.
This is studied using the equations, and some numerical computations are used to compare the use of voice coil and piezoelectric actuators.

Then, in section [[sec:control_strategies]], two different control approach are compared where:
- the measurement is made in the fixed frame
- the measurement is made in the rotating frame

In section [[sec:simscape]], the analytical study will be validated using a multi body model of the studied system.

Finally, in section [[sec:control]], the control strategies are implemented using Simulink and Simscape and compared.

* System Description and Analysis
:PROPERTIES:
:HEADER-ARGS:matlab+: :tangle matlab/system_numerical_analysis.m
:END:
<<sec:system>>
** Matlab Init                                              :noexport:ignore:
#+begin_src matlab :tangle no :exports none :noweb yes :var current_dir=(file-name-directory buffer-file-name)
  <<matlab-dir>>
#+end_src

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

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

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

#+name: fig:rotating_frame_2dof
#+caption: Schematic of the mecanical system
[[./figs/rotating_frame_2dof.png]]

In the following block diagram:
- $G$ is the transfer function from the forces applied in the actuators to the measurement
- $K$ is the controller to design
- $J$ is a Jacobian matrix usually used to change the reference frame

Indices $x$ and $y$ corresponds to signals in the fixed reference frame (along $\vec{i}_x$ and $\vec{i}_y$):
- $D_x$ is the measured position of the sample
- $r_x$ is the reference signal which corresponds to the wanted $D_x$
- $\epsilon_x$ is the position error

Indices $u$ and $v$ corresponds to signals in the rotating reference frame ($\vec{i}_u$ and $\vec{i}_v$):
- $F_u$ and $F_v$ are forces applied by the actuators
- $\epsilon_u$ and $\epsilon_v$ are position error of the sample along $\vec{i}_u$ and $\vec{i}_v$

** Equations
  <<sec:equations>>
Based on the figure [[fig:rotating_frame_2dof]], we can write the equations of motion of the system.

Let's express the kinetic energy $T$ and the potential energy $V$ of the mass $m$:
#+name: eq:energy_inertial_frame
\begin{align}
T & = \frac{1}{2} m \left( \dot{x}^2 + \dot{y}^2 \right) \\
V & = \frac{1}{2} k \left( x^2 + y^2 \right)
\end{align}

The Lagrangian is the kinetic energy minus the potential energy.
#+name: eq:lagrangian_inertial_frame
\begin{equation}
L = T-V = \frac{1}{2} m \left( \dot{x}^2 + \dot{y}^2 \right) - \frac{1}{2} k \left( x^2 + y^2 \right)
\end{equation}


The partial derivatives of the Lagrangian with respect to the variables $(x, y)$ are:
#+name: eq:inertial_frame_deriv
\begin{align*}
\frac{\partial L}{\partial x} & = -kx \\
\frac{\partial L}{\partial y} & = -ky \\
\frac{d}{dt}\frac{\partial L}{\partial \dot{x}} & = m\ddot{x} \\
\frac{d}{dt}\frac{\partial L}{\partial \dot{y}} & = m\ddot{y}
\end{align*}

The external forces applied to the mass are:
\begin{align*}
F_{\text{ext}, x} &= F_u \cos{\theta} - F_v \sin{\theta}\\
F_{\text{ext}, y} &= F_u \sin{\theta} + F_v \cos{\theta}
\end{align*}

By appling the Lagrangian equations, we obtain:
\begin{align}
m\ddot{x} + kx = F_u \cos{\theta} - F_v \sin{\theta}\\
m\ddot{y} + ky = F_u \sin{\theta} + F_v \cos{\theta}
\end{align}

We then change coordinates from $(x, y)$ to $(d_x, d_y, \theta)$.
\begin{align*}
x & = d_u \cos{\theta} - d_v \sin{\theta}\\
y & = d_u \sin{\theta} + d_v \cos{\theta}
\end{align*}

We obtain:
\begin{align*}
\ddot{x} & = \ddot{d_u} \cos{\theta} - 2\dot{d_u}\dot{\theta}\sin{\theta} - d_u\ddot{\theta}\sin{\theta} - d_u\dot{\theta}^2 \cos{\theta}
           - \ddot{d_v} \sin{\theta} - 2\dot{d_v}\dot{\theta}\cos{\theta} - d_v\ddot{\theta}\cos{\theta} + d_v\dot{\theta}^2 \sin{\theta} \\
\ddot{y} & = \ddot{d_u} \sin{\theta} + 2\dot{d_u}\dot{\theta}\cos{\theta} + d_u\ddot{\theta}\cos{\theta} - d_u\dot{\theta}^2 \sin{\theta}
           + \ddot{d_v} \cos{\theta} - 2\dot{d_v}\dot{\theta}\sin{\theta} - d_v\ddot{\theta}\sin{\theta} - d_v\dot{\theta}^2 \cos{\theta} \\
\end{align*}

By injecting the previous result into the Lagrangian equation, we obtain:
\begin{align*}
 m \ddot{d_u} \cos{\theta} - 2m\dot{d_u}\dot{\theta}\sin{\theta} - m d_u\ddot{\theta}\sin{\theta} - m d_u\dot{\theta}^2 \cos{\theta}
-m \ddot{d_v} \sin{\theta} - 2m\dot{d_v}\dot{\theta}\cos{\theta} - m d_v\ddot{\theta}\cos{\theta} + m d_v\dot{\theta}^2 \sin{\theta}
+ k d_u \cos{\theta} - k d_v \sin{\theta} = F_u \cos{\theta} - F_v \sin{\theta} \\
  m \ddot{d_u} \sin{\theta} + 2m\dot{d_u}\dot{\theta}\cos{\theta} + m d_u\ddot{\theta}\cos{\theta} - m d_u\dot{\theta}^2 \sin{\theta}
+ m \ddot{d_v} \cos{\theta} - 2m\dot{d_v}\dot{\theta}\sin{\theta} - m d_v\ddot{\theta}\sin{\theta} - m d_v\dot{\theta}^2 \cos{\theta}
+ k d_u \sin{\theta} + k d_v \cos{\theta} = F_u \sin{\theta} + F_v \cos{\theta}
\end{align*}

Which is equivalent to:
\begin{align*}
 m \ddot{d_u} - 2m\dot{d_u}\dot{\theta}\frac{\sin{\theta}}{\cos{\theta}} - m d_u\ddot{\theta}\frac{\sin{\theta}}{\cos{\theta}} - m d_u\dot{\theta}^2
-m \ddot{d_v} \frac{\sin{\theta}}{\cos{\theta}} - 2m\dot{d_v}\dot{\theta} - m d_v\ddot{\theta} + m d_v\dot{\theta}^2 \frac{\sin{\theta}}{\cos{\theta}}
+ k d_u - k d_v \frac{\sin{\theta}}{\cos{\theta}} = F_u - F_v \frac{\sin{\theta}}{\cos{\theta}} \\
  m \ddot{d_u} + 2m\dot{d_u}\dot{\theta}\frac{\cos{\theta}}{\sin{\theta}} + m d_u\ddot{\theta}\frac{\cos{\theta}}{\sin{\theta}} - m d_u\dot{\theta}^2
+ m \ddot{d_v} \frac{\cos{\theta}}{\sin{\theta}} - 2m\dot{d_v}\dot{\theta} - m d_v\ddot{\theta} - m d_v\dot{\theta}^2 \frac{\cos{\theta}}{\sin{\theta}}
+ k d_u + k d_v \frac{\cos{\theta}}{\sin{\theta}} = F_u + F_v \frac{\cos{\theta}}{\sin{\theta}}
\end{align*}

We can then subtract and add the previous equations to obtain the following equations:
#+begin_important
#+NAME: eq:du_coupled
\begin{equation}
 m \ddot{d_u} + (k - m\dot{\theta}^2) d_u = F_u + 2 m\dot{d_v}\dot{\theta} + m d_v\ddot{\theta}
\end{equation}
#+NAME: eq:dv_coupled
\begin{equation}
 m \ddot{d_v} + (k - m\dot{\theta}^2) d_v = F_v - 2 m\dot{d_u}\dot{\theta} - m d_u\ddot{\theta}
\end{equation}
#+end_important

We obtain two differential equations that are coupled through:
- *Euler forces*: $m d_v \ddot{\theta}$
- *Coriolis forces*: $2 m \dot{d_v} \dot{\theta}$

Without the coupling terms, each equation is the equation of a one degree of freedom mass-spring system with mass $m$ and stiffness $k- m\dot{\theta}^2$.
Thus, the term $- m\dot{\theta}^2$ acts like a negative stiffness (due to *centrifugal forces*).

The forces induced by the rotating reference frame are independent of the stiffness of the actuator.
The resulting effect of those forces should then be higher when using softer actuators.

** Numerical Values for the NASS
Let's define the parameters for the NASS.
#+begin_src matlab :exports none
  mlight = 35; % Mass for light sample [kg]
  mheavy = 85; % Mass for heavy sample [kg]

  wlight = 2*pi; % Max rot. speed for light sample [rad/s]
  wheavy = 2*pi/60; % Max rot. speed for heavy sample [rad/s]

  kvc = 1e3; % Voice Coil Stiffness [N/m]
  kpz = 1e8; % Piezo Stiffness [N/m]

  wdot = 1; % Maximum rotation acceleration [rad/s2]

  d = 0.01; % Maximum excentricity from rotational axis [m]
  ddot = 0.2; % Maximum Horizontal speed [m/s]

  save('./mat/parameters.mat');
#+end_src

#+begin_src matlab :results table :exports results
  labels = {'Light sample mass [kg]', ...
            'Heavy sample mass [kg]', ...
            'Max rot. speed - light [rpm]', ...
            'Max rot. speed - heavy [rpm]', ...
            'Voice Coil Stiffness [N/m]', ...
            'Piezo Stiffness [N/m]', ...
            'Max rot. acceleration [rad/s2]', ...
            'Max mass excentricity [m]', ...
            'Max Horizontal speed [m/s]'};
  data = [mlight, mheavy, 60*wlight/2/pi, 60*wheavy/2/pi, kvc, kpz, wdot, d, ddot];
  data2orgtable(data', labels, {}, ' %.1e ')
#+end_src

#+RESULTS:
| Light sample mass [kg]         | 3.5e+01 |
| Heavy sample mass [kg]         | 8.5e+01 |
| Max rot. speed - light [rpm]   | 6.0e+01 |
| Max rot. speed - heavy [rpm]   | 1.0e+00 |
| Voice Coil Stiffness [N/m]     | 1.0e+03 |
| Piezo Stiffness [N/m]          | 1.0e+08 |
| Max rot. acceleration [rad/s2] | 1.0e+00 |
| Max mass excentricity [m]      | 1.0e-02 |
| Max Horizontal speed [m/s]     | 2.0e-01 |

** Euler and Coriolis forces - Numerical Result
First we will determine the value for Euler and Coriolis forces during regular experiment.
- *Euler forces*: $m d_v \ddot{\theta}$
- *Coriolis forces*: $2 m \dot{d_v} \dot{\theta}$

#+begin_src matlab :exports none
  Felight = mlight*d*wdot;
  Feheavy = mheavy*d*wdot;

  Fclight = 2*mlight*ddot*wlight;
  Fcheavy = 2*mheavy*ddot*wheavy;
#+end_src

The obtained values are displayed in table [[tab:euler_coriolis]].

#+begin_src matlab :results value table :exports results :post addhdr(*this*)
  data = [Fclight, Fcheavy ;
          Felight, Feheavy];
  data2orgtable(data, {'Coriolis', 'Euler'}, {'Light', 'Heavy'}, ' %.1fN ')
#+end_src

#+NAME: tab:euler_coriolis
#+CAPTION: Euler and Coriolis forces for the NASS
#+RESULTS:
|          | Light | Heavy |
|----------+-------+-------|
| Coriolis | 88.0N | 3.6N  |
| Euler    | 0.4N  | 0.8N  |

** Negative Spring Effect - Numerical Result
The negative stiffness due to the rotation is equal to $-m{\omega_0}^2$.

#+begin_src matlab :exports none
  Klight = mlight*wlight^2;
  Kheavy = mheavy*wheavy^2;
#+end_src

The values for the negative spring effect are displayed in table [[tab:negative_spring]].

This is definitely negligible when using piezoelectric actuators. It may not be the case when using voice coil actuators.

#+begin_src matlab :results value table :exports results :post addhdr(*this*)
  data = [Klight, Kheavy];
  data2orgtable(data, {'Neg. Spring'}, {'Light', 'Heavy'}, ' %.1f[N/m] ')
#+end_src

#+NAME: tab:negative_spring
#+CAPTION: Negative Spring effect
#+RESULTS:
|             | Light       | Heavy    |
|-------------+-------------+----------|
| Neg. Spring | 1381.7[N/m] | 0.9[N/m] |

** Limitations due to coupling
To simplify, we consider a constant rotating speed $\dot{\theta} = \omega_0$ and thus $\ddot{\theta} = 0$.

From equations [[eq:du_coupled]] and [[eq:dv_coupled]], we obtain:
\begin{align*}
  (m s^2 + (k - m{\omega_0}^2)) d_u &= F_u + 2 m {\omega_0} s d_v \\
  (m s^2 + (k - m{\omega_0}^2)) d_v &= F_v - 2 m {\omega_0} s d_u \\
\end{align*}

From second equation:
\[ d_v = \frac{1}{m s^2 + (k - m{\omega_0}^2)} F_v - \frac{2 m {\omega_0} s}{m s^2 + (k - m{\omega_0}^2)} d_u \]

And we re-inject $d_v$ into the first equation:
\begin{equation*}
(m s^2 + (k - m{\omega_0}^2)) d_u = F_u + \frac{2 m {\omega_0} s}{m s^2 + (k - m{\omega_0}^2)} F_v - \frac{(2 m {\omega_0} s)^2}{m s^2 + (k - m{\omega_0}^2)} d_u
\end{equation*}

\begin{equation*}
\frac{(m s^2 + (k - m{\omega_0}^2))^2 + (2 m {\omega_0} s)^2}{m s^2 + (k - m{\omega_0}^2)} d_u = F_u + \frac{2 m {\omega_0} s}{m s^2 + (k - m{\omega_0}^2)} F_v
\end{equation*}

Finally we obtain $d_u$ function of $F_u$ and $F_v$.
\[ d_u = \frac{m s^2 + (k - m{\omega_0}^2)}{(m s^2 + (k - m{\omega_0}^2))^2 + (2 m {\omega_0} s)^2} F_u + \frac{2 m {\omega_0} s}{(m s^2 + (k - m{\omega_0}^2))^2 + (2 m {\omega_0} s)^2} F_v \]

Similarly we can obtain $d_v$ function of $F_u$ and $F_v$:
\[ d_v = \frac{m s^2 + (k - m{\omega_0}^2)}{(m s^2 + (k - m{\omega_0}^2))^2 + (2 m {\omega_0} s)^2} F_v - \frac{2 m {\omega_0} s}{(m s^2 + (k - m{\omega_0}^2))^2 + (2 m {\omega_0} s)^2} F_u \]

The two previous equations can be written in a matrix form:
#+begin_important
#+NAME: eq:coupledplant
\begin{equation}
\begin{bmatrix} d_u \\ d_v \end{bmatrix} =
\frac{1}{(m s^2 + (k - m{\omega_0}^2))^2 + (2 m {\omega_0} s)^2}
\begin{bmatrix}
  ms^2 + (k-m{\omega_0}^2) & 2 m \omega_0 s \\
  -2 m \omega_0 s          & ms^2 + (k-m{\omega_0}^2) \\
\end{bmatrix}
\begin{bmatrix} F_u \\ F_v \end{bmatrix}
\end{equation}
#+end_important

Then, coupling is negligible if $|-m \omega^2 + (k - m{\omega_0}^2)| \gg |2 m {\omega_0} \omega|$.

*** Numerical Analysis
We plot on the same graph $\frac{|-m \omega^2 + (k - m {\omega_0}^2)|}{|2 m \omega_0 \omega|}$ for the voice coil and the piezo:
- with the light sample (figure [[fig:coupling_light]]).
- with the heavy sample (figure [[fig:coupling_heavy]]).

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

  figure;
  hold on;
  plot(f, abs(-mlight*(2*pi*f).^2 + kvc - mlight * wlight^2)./abs(2*mlight*wlight*2*pi*f), 'DisplayName', 'Voice Coil')
  plot(f, abs(-mlight*(2*pi*f).^2 + kpz - mlight * wlight^2)./abs(2*mlight*wlight*2*pi*f), 'DisplayName', 'Piezo')
  plot(f, ones(1, length(f)), 'k--', 'HandleVisibility', 'off')
  hold off;
  xlim([f(1), f(end)]);
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]');
  legend('Location', 'northeast');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/coupling_light.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+LABEL: fig:coupling_light
#+CAPTION: Relative Coupling for light mass and high rotation speed
[[file:./figs/coupling_light.png]]

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(f, abs(-mheavy*(2*pi*f).^2 + kvc - mheavy * wheavy^2)./abs(2*mheavy*wheavy*2*pi*f), 'DisplayName', 'Voice Coil')
  plot(f, abs(-mheavy*(2*pi*f).^2 + kpz - mheavy * wheavy^2)./abs(2*mheavy*wheavy*2*pi*f), 'DisplayName', 'Piezo')
  plot(f, ones(1, length(f)), 'k--', 'HandleVisibility', 'off')
  hold off;
  xlim([f(1), f(end)]);
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]');
  legend('Location', 'northeast');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/coupling_heavy.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+LABEL: fig:coupling_heavy
#+CAPTION: Relative Coupling for heavy mass and low rotation speed
[[file:figs/coupling_heavy.png]]

#+begin_important
Coupling is higher for actuators with small stiffness.
#+end_important

** Limitations due to negative stiffness effect
If $\max{\dot{\theta}} \ll \sqrt{\frac{k}{m}}$, then the negative spring effect is negligible and $k - m\dot{\theta}^2 \approx k$.

Let's estimate what is the maximum rotation speed for which the negative stiffness effect is still negligible ($\omega_\text{max} = 0.1 \sqrt{\frac{k}{m}}$). Results are shown table [[tab:negative_stiffness]].
#+begin_src matlab :results table :exports results :post addhdr(*this*)
  data = 0.1*60*(1/2/pi)*[sqrt(kvc/mlight), sqrt(kpz/mlight);
                       sqrt(kvc/mheavy), sqrt(kpz/mheavy)];
  data2orgtable(data, {'Light', 'Heavy'}, {'Voice Coil', 'Piezo'}, ' %.0f[rpm] ')
#+end_src

#+NAME: tab:negative_stiffness
#+CAPTION: Maximum rotation speed at which negative stiffness is negligible ($0.1\sqrt{\frac{k}{m}}$)
#+RESULTS:
|       | Voice Coil | Piezo     |
|-------+------------+-----------|
| Light | 5[rpm]     | 1614[rpm] |
| Heavy | 3[rpm]     | 1036[rpm] |

The negative spring effect is proportional to the rotational speed $\omega$.
The system dynamics will be much more affected when using soft actuator.

#+begin_important
  Negative stiffness effect has very important effect when using soft actuators.
#+end_important

The system can even goes unstable when $m \omega^2 > k$, that is when the centrifugal forces are higher than the forces due to stiffness.

From this analysis, we can determine the lowest practical stiffness that is possible to use: $k_\text{min} = 10 m \omega^2$ (table sec:tab:min_k)

#+begin_src matlab :results table :exports results :post addhdr(*this*)
  data = 10*[mlight*2*pi, mheavy*2*pi/60]
  data2orgtable(data, {'k min [N/m]'}, {'Light', 'Heavy'}, ' %.0f ')
#+end_src

#+NAME: tab:min_k
#+CAPTION: Minimum possible stiffness
#+RESULTS:
|             | Light | Heavy |
|-------------+-------+-------|
| k min [N/m] |  2199 |    89 |

** Effect of rotation speed on the plant
As shown in equation [[eq:coupledplant]], the plant changes with the rotation speed $\omega_0$.

Then, we compute the bode plot of the direct term and coupling term for multiple rotating speed.

Then we compare the result between voice coil and piezoelectric actuators.

*** Voice coil actuator
#+begin_src matlab :exports none
  m = mlight;
  k = kvc;

  ws = linspace(0, 2*pi, 5); % Rotation speed vector [rad/s]

  Gs = {zeros(1, length(ws))};
  Gcs = {zeros(1, length(ws))};

  for i = 1:length(ws)
    w = ws(i);
    Gs(i) = {(m*s^2 + (k-m*w^2))/((m*s^2 + (k - m*w^2))^2 + (2*m*w*s)^2)};
    Gcs(i) = {(2*m*w*s)/((m*s^2 + (k - m*w^2))^2 + (2*m*w*s)^2)};
  end
#+end_src

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

  figure;
  ax1 = subplot(2,1,1);
  hold on;
  for i = 1:length(ws)
    plot(freqs, abs(squeeze(freqresp(Gs{i}, freqs, 'Hz'))));
  end
  hold off;
  xlim([freqs(1), freqs(end)]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude [m/N]');

  ax2 = subplot(2,1,2);
  hold on;
  for i = 1:length(ws)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}, freqs, 'Hz'))), 'DisplayName', sprintf('w = %.0f [rpm]', ws(i)*60/2/pi));
  end
  hold off;
  yticks(-180:90:180);
  ylim([-180 180]);
  xlim([freqs(1), freqs(end)]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  legend('Location', 'southwest');
  linkaxes([ax1,ax2],'x');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/G_ws_vc.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
  #+end_src

#+LABEL: fig:G_ws_vc
#+CAPTION: Bode plot of the direct transfer function term (from $F_u$ to $D_u$) for multiple rotation speed - Voice coil
[[file:figs/G_ws_vc.png]]


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

  figure;
  ax1 = subplot(2,1,1);
  hold on;
  for i = 1:length(ws)
    plot(freqs, abs(squeeze(freqresp(Gcs{i}, freqs, 'Hz'))));
  end
  hold off;
  xlim([freqs(1), freqs(end)]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude [m/N]');

  ax2 = subplot(2,1,2);
  hold on;
  for i = 1:length(ws)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gcs{i}, freqs, 'Hz'))), 'DisplayName', sprintf('w = %.0f [rpm]', ws(i)*60/2/pi));
  end
  hold off;
  yticks(-180:90:180);
  ylim([-180 180]);
  xlim([freqs(1), freqs(end)]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  legend('Location', 'southwest');
  linkaxes([ax1,ax2],'x');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/Gc_ws_vc.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+LABEL: fig:Gc_ws_vc
#+CAPTION: caption
[[file:figs/Gc_ws_vc.png]]

*** Piezoelectric actuator
#+begin_src matlab :exports none
  m = mlight;
  k = kpz;

  ws = linspace(0, 2*pi, 5); % Rotation speed vector [rad/s]

  Gs = {zeros(1, length(ws))};
  Gcs = {zeros(1, length(ws))};

  for i = 1:length(ws)
    w = ws(i);
    Gs(i) = {(m*s^2 + (k-m*w^2))/((m*s^2 + (k - m*w^2))^2 + (2*m*w*s)^2)};
    Gcs(i) = {(2*m*w*s)/((m*s^2 + (k - m*w^2))^2 + (2*m*w*s)^2)};
  end
#+end_src

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

  figure;
  ax1 = subplot(2,1,1);
  hold on;
  for i = 1:length(ws)
    plot(freqs, abs(squeeze(freqresp(Gs{i}, freqs, 'Hz'))));
  end
  hold off;
  xlim([freqs(1), freqs(end)]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude [m/N]');

  ax2 = subplot(2,1,2);
  hold on;
  for i = 1:length(ws)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}, freqs, 'Hz'))), 'DisplayName', sprintf('w = %.0f [rpm]', ws(i)*60/2/pi));
  end
  hold off;
  yticks(-180:90:180);
  ylim([-180 180]);
  xlim([freqs(1), freqs(end)]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  legend('Location', 'southwest');
  linkaxes([ax1,ax2],'x');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/G_ws_pz.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+LABEL: fig:G_ws_pz
#+CAPTION: Bode plot of the direct transfer function term (from $F_u$ to $D_u$) for multiple rotation speed - Piezoelectric actuator
[[file:figs/G_ws_pz.png]]

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2,1,1);
  hold on;
  for i = 1:length(ws)
    plot(freqs, abs(squeeze(freqresp(Gcs{i}, freqs, 'Hz'))));
  end
  hold off;
  xlim([freqs(1), freqs(end)]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude [m/N]');

  ax2 = subplot(2,1,2);
  hold on;
  for i = 1:length(ws)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gcs{i}, freqs, 'Hz'))), 'DisplayName', sprintf('w = %.0f [rpm]', ws(i)*60/2/pi));
  end
  hold off;
  yticks(-180:90:180);
  ylim([-180 180]);
  xlim([freqs(1), freqs(end)]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  legend('Location', 'southwest');
  linkaxes([ax1,ax2],'x');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/Gc_ws_pz.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+LABEL: fig:Gc_ws_pz
#+CAPTION: Bode plot of the coupling transfer function term (from $F_u$ to $D_v$) for multiple rotation speed - Piezoelectric actuator
[[file:figs/Gc_ws_pz.png]]

*** Analysis
When the rotation speed is null, the coupling terms are equal to zero and the diagonal terms corresponds to one degree of freedom mass spring system.

When the rotation speed in not null, the resonance frequency is duplicated into two pairs of complex conjugate poles.

As the rotation speed increases, one of the two resonant frequency goes to lower frequencies as the other one goes to higher frequencies.

The poles of the coupling terms are the same as the poles of the diagonal terms. The magnitude of the coupling terms are increasing with the rotation speed.

#+begin_important
As shown in the previous figures, the system with voice coil is much more sensitive to rotation speed.
#+end_important

*** Campbell diagram
The poles of the system are computed for multiple values of the rotation frequency. To simplify the computation of the poles, we add some damping to the system.

#+begin_src matlab :exports code
  m = mlight;
  k = kvc;
  c = 0.1*sqrt(k*m);

  wsvc = linspace(0, 10, 100); % [rad/s]

  polesvc = zeros(2, length(wsvc));

  for i = 1:length(wsvc)
    polei = pole(1/((m*s^2 + c*s + (k - m*wsvc(i)^2))^2 + (2*m*wsvc(i)*s)^2));
    polesvc(:, i) = sort(polei(imag(polei) > 0));
  end
#+end_src

#+begin_src matlab :exports code
  m = mlight;
  k = kpz;
  c = 0.1*sqrt(k*m);

  wspz = linspace(0, 1000, 100); % [rad/s]

  polespz = zeros(2, length(wspz));

  for i = 1:length(wspz)
    polei = pole(1/((m*s^2 + c*s + (k - m*wspz(i)^2))^2 + (2*m*wspz(i)*s)^2));
    polespz(:, i) = sort(polei(imag(polei) > 0));
  end
#+end_src

We then plot the real and imaginary part of the poles as a function of the rotation frequency (figures [[fig:poles_w_vc]] and [[fig:poles_w_pz]]).

When the real part of one pole becomes positive, the system goes unstable.

For the voice coil (figure [[fig:poles_w_vc]]), the system is unstable when the rotation speed is above 5 rad/s. The real and imaginary part of the poles of the system with piezoelectric actuators are changing much less (figure [[fig:poles_w_pz]]).

#+begin_src matlab :exports none
  figure;
  % Amplitude
  ax1 = subplot(1,2,1);
  hold on;
  plot(wsvc, real(polesvc(1, :)))
  set(gca,'ColorOrderIndex',1)
  plot(wsvc, real(polesvc(2, :)))
  plot(wsvc, zeros(size(wsvc)), 'k--')
  hold off;
  xlabel('Rotation Frequency [rad/s]');
  ylabel('Pole Real Part');
  ax2 = subplot(1,2,2);
  hold on;
  plot(wsvc, imag(polesvc(1, :)))
  set(gca,'ColorOrderIndex',1)
  plot(wsvc, -imag(polesvc(1, :)))
  set(gca,'ColorOrderIndex',1)
  plot(wsvc, imag(polesvc(2, :)))
  set(gca,'ColorOrderIndex',1)
  plot(wsvc, -imag(polesvc(2, :)))
  hold off;
  xlabel('Rotation Frequency [rad/s]');
  ylabel('Pole Imaginary Part');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes :mkdirp yes
#+begin_src matlab :var filepath="figs/poles_w_vc.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+LABEL: fig:poles_w_vc
#+CAPTION: Real and Imaginary part of the poles of the system as a function of the rotation speed - Voice Coil and light sample
[[file:figs/poles_w_vc.png]]


#+begin_src matlab :exports none
  figure;
  % Amplitude
  ax1 = subplot(1,2,1);
  hold on;
  plot(wspz, real(polespz(1, :)))
  set(gca,'ColorOrderIndex',1)
  plot(wspz, real(polespz(2, :)))
  plot(wspz, zeros(size(wspz)), 'k--')
  hold off;
  xlabel('Rotation Frequency [rad/s]');
  ylabel('Pole Real Part');
  ax2 = subplot(1,2,2);
  hold on;
  plot(wspz, imag(polespz(1, :)))
  set(gca,'ColorOrderIndex',1)
  plot(wspz, -imag(polespz(1, :)))
  set(gca,'ColorOrderIndex',1)
  plot(wspz, imag(polespz(2, :)))
  set(gca,'ColorOrderIndex',1)
  plot(wspz, -imag(polespz(2, :)))
  hold off;
  xlabel('Rotation Frequency [rad/s]');
  ylabel('Pole Imaginary Part');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/poles_w_pz.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+LABEL: fig:poles_w_pz
#+CAPTION: Real and Imaginary part of the poles of the system as a function of the rotation speed - Piezoelectric actuator and light sample
[[file:figs/poles_w_pz.png]]

* Integral Force Feedback
<<sec:iff>>
** Introduction                                                      :ignore:
In this section, we study the use of Decentralized Integral Force Feedback (IFF) to damp the resonance of the positioning system.

We thus suppose there is a force sensor in series with the actuator in the $u$ and $v$ directions.

** Analytical Derivation of the transfer function for IFF control
The sensed forces are equal to:
\begin{align}
  F_{um} &= F_u - k d_u \\
  F_{vm} &= F_v - k d_v
\end{align}

If we consider a constant rotating speed $\dot{\theta} = \omega$, we have:
\begin{equation}
\begin{bmatrix} d_u \\ d_v \end{bmatrix} =
\frac{1}{(m s^2 + (k - m{\omega}^2))^2 + (2 m {\omega} s)^2}
\begin{bmatrix}
  ms^2 + (k-m{\omega}^2) & 2 m \omega s \\
  -2 m \omega s          & ms^2 + (k-m{\omega}^2) \\
\end{bmatrix}
\begin{bmatrix} F_u \\ F_v \end{bmatrix}
\end{equation}

Which then give:
\begin{align}
  F_{um} &= \frac{(m s^2 + (k - m\omega^2))^2 + (2 m \omega s)^2 - k(m s^2 + (k - m\omega^2))}{(m s^2 + (k - m\omega^2))^2 + (2 m \omega s)^2} F_u - \frac{2 m \omega s}{(m s^2 + (k - m\omega^2))^2 + (2 m \omega s)^2} F_v\\
  F_{vm} &= \frac{2 m \omega s}{(m s^2 + (k - m\omega^2))^2 + (2 m \omega s)^2} F_u + \frac{(m s^2 + (k - m\omega^2))^2 + (2 m \omega s)^2 - k(m s^2 + (k - m\omega^2))}{(m s^2 + (k - m\omega^2))^2 + (2 m \omega s)^2} F_v
\end{align}

If we note $\omega_0 = \sqrt{\frac{k}{m}}$, which corresponds to the resonance of the positioning system when not rotating, we obtain:
#+begin_important
\begin{align}
  F_{um} &= \frac{s^4 + (\omega_0^2 + 2\omega^2) s^2 + \omega^2(\omega^2 - \omega_0^2)}{s^4 + 2 (\omega_0^2 + \omega^2) s^2 + (\omega_0^2 - \omega^2)^2} F_u - \frac{2 \omega_0^2 \omega s}{s^4 + 2 (\omega_0^2 + \omega^2) s^2 + (\omega_0^2 - \omega^2)^2} F_v\\
  F_{vm} &= \frac{2 \omega_0^2 \omega s}{s^4 + 2 (\omega_0^2 + \omega^2) s^2 + (\omega_0^2 - \omega^2)^2} F_u + \frac{s^4 + (\omega_0^2 + 2\omega^2) s^2 + \omega^2(\omega^2 - \omega_0^2)}{s^4 + 2 (\omega_0^2 + \omega^2) s^2 + (\omega_0^2 - \omega^2)^2} F_v
\end{align}
#+end_important

** Low and High frequency Behavior
At high frequency, the force sensors give:
\begin{align}
  F_{um} &= F_u \\
  F_{vm} &= F_v
\end{align}
which is usual for IFF control.


At low frequency:
\begin{align}
  F_{um} &=  \frac{\omega^2}{\omega_0^2 - \omega^2} F_u \\
  F_{vm} &=  \frac{\omega^2}{\omega_0^2 - \omega^2} F_v
\end{align}

The gain at low frequency is no more zero as it is usually the case.
Thus, a constant force will be sensed by the force sensor.


This could be explained by the fact that a constant force will induce some displacement of the mass.
The mass being more off-center with the rotation axis, it is experiencing a larger centrifugal force and this (constant) centrifugal force is sensed by the force sensor.
It is like a *negative stiffness is in parallel with both the stiffness and the force sensor*.

** Poles and Zeros
Let's use Matlab Symbolic toolbox to find the analytical formula for the poles and zeros of the transfer function from the force actuators to the force sensors.

Let's define the parameters and suppose that we have $\omega > \omega_0$ (stable poles).
#+begin_src matlab
  syms w w0 positive
  assume(w > w0)
  syms x
#+end_src

The characteristic equations to find the poles and zeros are:
#+begin_src matlab
  z = x^4 + (w0^2 + 2*w^2)*x^2 + w^2*(w^2 - w0^2) == 0
  p = x^4 + 2*(w0^2 + w^2)*x^2 +(w^2 - w0^2)^2 == 0
#+end_src

#+begin_src matlab
  solve(p, x)
#+end_src

The obtained poles are:
\begin{align}
  p_1 &= \pm j (\omega_0 - \omega) \\
  p_2 &= \pm j (\omega_0 + \omega)
\end{align}

We obtain two pairs of complex conjugate poles:
- $p_1$ is going to lower frequencies as the rotating speed $\omega$ is increased
- $p_2$ is going to higher frequencies as the rotating speed $\omega$ is increased

#+begin_src matlab
  solve(z, x)
#+end_src

The obtained zeros are:
\begin{align}
  z_1 &= \pm j \sqrt{ \frac{1}{2} \omega_0 \sqrt{(8 \omega^2 + \omega_0^2)} + \omega^2 + \frac{1}{2} \omega_0^2 } \\
  z_2 &= \pm \sqrt{ \frac{1}{2} \omega_0 \sqrt{(8 \omega^2 + \omega_0^2)} - \omega^2 - \frac{1}{2} \omega_0^2 }
\end{align}

Thus, there are *two complex conjugate zeros* and *two real zeros*.

We can easily show that the frequency of the two complex conjugate zeros is between the frequency of $p_1$ and $p_2$.

The effect of the two real zeros is an increase of +2 of the slope without change of phase (non-minimum phase behavior).

#+begin_important
Also, as one zero has a positive real part, the *IFF control is no more unconditionally stable*.

This is due to the fact that the zeros of the plant are the poles of the closed loop system with an infinite gain.

Thus, for some finite IFF gain, one pole will have a positive real part and the system will be unstable.
#+end_important

** IFF Plant and Root Locus
#+begin_src matlab :exports none
  w0 = 2*pi; % [rad/s]
  m = 1; % [kg]
  k = m*w0^2; % [N/m]

  ws = linspace(0, 0.9*w0, 4);

  G_iff = {zeros(2, 2, length(ws))};

  for i = 1:length(ws)
    w = ws(i);
    G_iff(:, :, i) = {[((m*s^2 + k-m*w^2)^2 + (2*m*w*s)^2 - k*(m*s^2 + k - m*w^2))/((m*s^2 + (k - m*w^2))^2 + (2*m*w*s)^2), -(2*k*m*w*s)/((m*s^2 + (k - m*w^2))^2 + (2*m*w*s)^2);
                        (2*k*m*w*s)/((m*s^2 + (k - m*w^2))^2 + (2*m*w*s)^2), ((m*s^2 + k-m*w^2)^2 + (2*m*w*s)^2 - k*(m*s^2 + k - m*w^2))/((m*s^2 + (k - m*w^2))^2 + (2*m*w*s)^2)]};
  end
#+end_src

The bode plot for the diagonal and off-diagonal elements are shown in Figure [[fig:iff_plant_and_coupling]].

The change of dynamics from $F_u$ to $F_{u,m}$ due to the change in rotation speed is shown in Figure [[fig:iff_variability_plant_ws]].
It is shown that the rotation speed does change the low frequency gain (explained in previous section).

The root locus for Decentralized Direct Velocity Feedback is shown in Figure [[fig:iff_root_locus_ws]].

It is shown that:
- the high frequency pair of complex conjugate poles cannot be critically damped.
  There is an optimal gain that gives maximum damping of this resonance.
- the low frequency resonance can be critically damped with sufficient gain
- a closed loop unstable pole is present whatever the gain of the IFF (if the rotation speed in not null)

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

  i = 2;

  figure;
  ax1 = subplot(2, 2, 1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(G_iff{i}(1,1), freqs, 'Hz'))))
  plot(freqs, ones(size(freqs))*(ws(i)^2/(k/m - ws(i)^2)), 'k--')
  plot(sqrt(0.5*sqrt(k/m)*sqrt(8*ws(i)^2 + k/m) + ws(i) + 0.5*k/m)/2/pi, 1, 'ko')
  plot(sqrt(0.5*sqrt(k/m)*sqrt(8*ws(i)^2 + k/m) - ws(i) - 0.5*k/m)/2/pi, 1, 'ko')
  plot((sqrt(k/m) + ws(i))/2/pi, 1, 'kx')
  plot((sqrt(k/m) - ws(i))/2/pi, 1, 'kx')
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [N/N]');
  title(['$\frac{F_{u,m}}{F_u}$' sprintf(', $\\omega = %.2f \\omega_0 $', ws(i)/w0)]);

  ax3 = subplot(2, 2, 3);
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff{i}(1, 1),  freqs, 'Hz'))));
  hold off;
  yticks(-180:90:180);
  ylim([-180 180]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');

  ax2 = subplot(2, 2, 2);
  hold on;
  plot(freqs, abs(squeeze(freqresp(G_iff{i}(1,2), freqs, 'Hz'))))
  plot((sqrt(k/m) + ws(i))/2/pi, 1, 'kx')
  plot((sqrt(k/m) - ws(i))/2/pi, 1, 'kx')
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  title(['$\frac{F_{v,m}}{F_u}$' sprintf(', $\\omega = %.2f \\omega_0 $', ws(i)/w0)]);

  ax4 = subplot(2, 2, 4);
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff{i}(1, 2),  freqs, 'Hz'))));
  hold off;
  yticks(-180:90:180);
  ylim([-180 180]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  xlabel('Frequency [Hz]');

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

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

#+name: fig:iff_plant_and_coupling
#+caption: Transfer Function from $F_u$ to $F_{u,m}$ and from $F_u$ to $F_{v,m}$ for $\omega = 0.3 \omega_0$
#+RESULTS:
[[file:figs/iff_plant_and_coupling.png]]

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

  figure;
  hold on;
  for i = 1:length(ws)
    plot(freqs, abs(squeeze(freqresp(G_iff{i}(1,1), freqs, 'Hz'))), ...
         'DisplayName', sprintf('$\\omega = %.2f \\omega_0 $', ws(i)/w0))
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Amplitude [N/N]');
  legend('location', 'northwest');
  ylim([0, 1e3]);
  title('$\frac{F_{u,m}}{F_u}$');
#+end_src

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

#+name: fig:iff_variability_plant_ws
#+caption: Transfer Function from $F_u$ to $F_{u,m}$ for multiple rotation speed
#+RESULTS:
[[file:figs/iff_variability_plant_ws.png]]

Let's see how the root locus is changing with the rotation speed.
#+begin_src matlab :exports none
  figure;

  gains = logspace(0, 3, 100);

  hold on;
  for i = 1:length(ws)
      set(gca,'ColorOrderIndex',i);
      plot(real(pole(G_iff{i})),  imag(pole(G_iff{i})), 'x', ...
           'DisplayName', sprintf('$\\omega = %.2f \\omega_0 $', ws(i)/w0));
      set(gca,'ColorOrderIndex',i);
      plot(real(tzero(G_iff{i})),  imag(tzero(G_iff{i})), 'o', ...
           'HandleVisibility', 'off');
      for k = 1:length(gains)
          set(gca,'ColorOrderIndex',i);
          cl_poles = pole(feedback(G_iff{i}, gains(k)/s*eye(2)));
          plot(real(cl_poles), imag(cl_poles), '.', ...
               'HandleVisibility', 'off');
      end
  end
  hold off;
  axis square;
  xlim([-13, 2]); ylim([0, 15]);

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

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

#+name: fig:iff_root_locus_ws
#+caption: Change of the Root Locus plot for different rotating speed
#+RESULTS:
[[file:figs/iff_root_locus_ws.png]]

** Conclusion
#+begin_important
The centrifugal forces can be modeled as a negative stiffness in parallel with the actuators and force sensors.
This changes the low frequency gain of the IFF plant.
It also add zeros with positive real part which removes the typical unconditional stability property of the IFF control.

From the Root Locus plot in Figure [[fig:iff_root_locus_ws]], it is shown that when rotating, unstable poles will be introduced due to IFF control.
Moreover, only one of the two pairs of complex conjugate poles can be critically damped.


Stiffness can be added in parallel to the force sensor to counteract the negative stiffness due to centrifugal forces.
If the added stiffness is higher than the maximum negative stiffness, then the poles of the IFF damped system will stay in the (stable) right half-plane.


Thus, IFF should not be used for rotating positioning platforms unless some stiffness is added in parallel with the force sensor.
If no parallel stiffness is added, an unstable pole is added in the system.
The frequency of this pole can be made small if the frequency of the position system resonance is much larger than the frequency of the rotation.
#+end_important

* Direct Velocity Feedback
<<sec:dvf>>
** Introduction                                                      :ignore:
In this section, we study the use of Direct Velocity Feedback to damp the resonances of the positioning station.
We thus suppose that we are able to measure to relative displacement (or velocity) $d_u$ and $d_v$.

** Analytical Study
The equation of motion are:
\begin{equation}
\begin{bmatrix} d_u \\ d_v \end{bmatrix} =
\frac{1}{(m s^2 + (k - m{\omega}^2))^2 + (2 m {\omega} s)^2}
\begin{bmatrix}
  ms^2 + (k-m{\omega}^2) & 2 m \omega s \\
  -2 m \omega s          & ms^2 + (k-m{\omega}^2) \\
\end{bmatrix}
\begin{bmatrix} F_u \\ F_v \end{bmatrix}
\end{equation}

If we note $\omega_0 = \sqrt{\frac{k}{m}}$, we obtain
\begin{align}
  d_u &= \frac{1}{m}\frac{s^2 + (\omega_0^2 - \omega^2)}{s^4 + 2 (\omega_0^2 + \omega^2) s^2 + (\omega_0^2 - \omega^2)^2} F_u + \frac{1}{m} \frac{2 \omega s}{s^4 + 2 (\omega_0^2 + \omega^2) s^2 + (\omega_0^2 - \omega^2)^2} F_v\\
  d_v &= - \frac{1}{m} \frac{2 \omega s}{s^4 + 2 (\omega_0^2 + \omega^2) s^2 + (\omega_0^2 - \omega^2)^2} F_u + \frac{1}{m}\frac{s^2 + (\omega_0^2 - \omega^2)}{s^4 + 2 (\omega_0^2 + \omega^2) s^2 + (\omega_0^2 - \omega^2)^2} F_v
\end{align}

** High and Low frequency behavior
At low frequency:
\begin{align}
  d_u &= \frac{1}{m}\frac{1}{\omega_0^2 - \omega^2} F_u \\
  d_v &= \frac{1}{m}\frac{1}{\omega_0^2 - \omega^2} F_v
\end{align}

Which can be re-written:
\begin{align}
  d_u &= \frac{1}{k} \frac{1}{1 - \frac{\omega^2}{\omega_0^2}} F_u \\
  d_v &= \frac{1}{k} \frac{1}{1 - \frac{\omega^2}{\omega_0^2}} F_v
\end{align}

The usual low frequency gain of $\frac{1}{k}$ is modified by a factor $\frac{1}{1 - \frac{\omega^2}{\omega_0^2}}$.

At high frequency:
\begin{align}
  d_u &=  \frac{1}{m s^2} F_u + \frac{2 \omega}{m s^3} F_v\\
  d_v &= -\frac{2 \omega}{m s^3} F_u + \frac{1}{m s^2} F_v
\end{align}

Thus the rotating speed does not change the high frequency behavior of the diagonal terms but change the coupling.

** Poles and Zeros
The direct terms (diagonal terms of the 2x2 transfer function matrix) have two complex conjugate zeros at:
\begin{equation}
  z = \pm j \sqrt{\omega_0^2 - \omega^2}
\end{equation}

Which are between the two pairs of complex conjugate poles at:
\begin{align}
  p_1 &= \pm j (\omega_0 - \omega) \\
  p_2 &= \pm j (\omega_0 + \omega)
\end{align}

** DVF Plant and Root Locus
#+begin_src matlab :exports none
  w0 = 2*pi; % [rad/s]
  m = 1; % [kg]
  k = m*w0^2; % [N/m]

  ws = linspace(0, 0.9*w0, 4);

  G_dvf = {zeros(2, 2, length(ws))};

  for i = 1:length(ws)
    w = ws(i);
    G_dvf(:, :, i) = {[ (m*s^2 + k-m*w^2)/((m*s^2 + (k - m*w^2))^2 + (2*m*w*s)^2), (2*m*w*s)/((m*s^2 + (k - m*w^2))^2 + (2*m*w*s)^2);
                       -(2*m*w*s)/((m*s^2 + (k - m*w^2))^2 + (2*m*w*s)^2), (m*s^2 + k-m*w^2)/((m*s^2 + (k - m*w^2))^2 + (2*m*w*s)^2) ]};
  end
#+end_src

The bode plot for the diagonal and off-diagonal elements are shown in Figure [[fig:dvf_plant_and_coupling]].

The change of dynamics from $F_u$ to $d_u$ due to the change in rotation speed is shown in Figure [[fig:dvf_variability_plant_ws]].
It is shown that the rotation speed does not change much the low frequency and high frequency gains.
It only modifies the location of the two pair of complex conjugate pairs.

The root locus for Decentralized Direct Velocity Feedback is shown in Figure [[fig:dvf_root_locus_ws]].
It is shown that the closed-loop poles are in the left-half of the complex plane, whatever the gain.
Moreover, has much damping can be added to both pairs of complex conjugate poles.

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

  i = 2;

  figure;
  ax1 = subplot(2, 2, 1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(G_dvf{i}(1,1), freqs, 'Hz'))))
  plot(freqs, ones(size(freqs))*1/(k * (1 - ws(i)^2/(k/m))), 'k--')
  plot(freqs, abs(squeeze(freqresp(1/m/s^2, freqs, 'Hz'))), 'k--')
  plot(sqrt(k/m - ws(i))/2/pi, 1, 'ko')
  plot((sqrt(k/m) + ws(i))/2/pi, 1, 'kx')
  plot((sqrt(k/m) - ws(i))/2/pi, 1, 'kx')
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]');
  title(['$\frac{d_u}{F_u}$' sprintf(', $\\omega = %.2f \\omega_0 $', ws(i)/w0)]);

  ax3 = subplot(2, 2, 3);
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf{i}(1, 1),  freqs, 'Hz'))));
  hold off;
  yticks(-180:90:180);
  ylim([-180 180]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');

  ax2 = subplot(2, 2, 2);
  hold on;
  plot(freqs, abs(squeeze(freqresp(G_dvf{i}(1,2), freqs, 'Hz'))))
  plot((sqrt(k/m) + ws(i))/2/pi, 1, 'kx')
  plot((sqrt(k/m) - ws(i))/2/pi, 1, 'kx')
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  title(['$\frac{d_v}{F_u}$' sprintf(', $\\omega = %.2f \\omega_0 $', ws(i)/w0)]);

  ax4 = subplot(2, 2, 4);
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf{i}(1, 2),  freqs, 'Hz'))));
  hold off;
  yticks(-180:90:180);
  ylim([-180 180]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  xlabel('Frequency [Hz]');

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

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

#+name: fig:dvf_plant_and_coupling
#+caption: Transfer Function from $F_u$ to $d_u$ and from $F_u$ to $d_v$ for $\omega = 0.3 \omega_0$
#+RESULTS:
[[file:figs/dvf_plant_and_coupling.png]]

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

  figure;
  hold on;
  for i = 1:length(ws)
    plot(freqs, abs(squeeze(freqresp(G_dvf{i}(1,1), freqs, 'Hz'))), ...
         'DisplayName', sprintf('$\\omega = %.2f \\omega_0 $', ws(i)/w0))
  end
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
  legend('location', 'northwest');
  title('$\frac{d_u}{F_u}$');
  ylim([0, 1e1]);
#+end_src

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

#+name: fig:dvf_variability_plant_ws
#+caption: Transfer Function from $F_u$ to $d_u$ for multiple rotation speed
#+RESULTS:
[[file:figs/dvf_variability_plant_ws.png]]

#+begin_src matlab :exports none
  figure;

  gains = logspace(-1, 3, 200);

  hold on;
  for i = 1:length(ws)
      set(gca,'ColorOrderIndex',i);
      plot(real(pole(G_dvf{i})),  imag(pole(G_dvf{i})), 'x', ...
           'DisplayName', sprintf('$\\omega = %.2f \\omega_0 $', ws(i)/w0));
      set(gca,'ColorOrderIndex',i);
      plot(real(tzero(G_dvf{i})),  imag(tzero(G_dvf{i})), 'o', ...
           'HandleVisibility', 'off');
      for k = 1:length(gains)
          set(gca,'ColorOrderIndex',i);
          cl_poles = pole(feedback(G_dvf{i}, s*gains(k)*eye(2)));
          plot(real(cl_poles), imag(cl_poles), '.', ...
               'HandleVisibility', 'off');
      end
  end
  hold off;
  axis square;
  xlim([-14, 1]); ylim([0, 15]);

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

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

#+name: fig:dvf_root_locus_ws
#+caption: Change of the Root Locus plot for multiple rotating speed
#+RESULTS:
[[file:figs/dvf_root_locus_ws.png]]

** Conclusion
#+begin_important
(Relative) Direct Velocity Feedback can be used to damp the resonance of a rotating positioning stage.

As shown by the Root Locus in Figure [[fig:dvf_root_locus_ws]]:
- DVF permits to critically damp the vibrations of the positioning stage
- DVF is unconditionally stage
#+end_important


* Control Strategies
  <<sec:control_strategies>>
** Measurement in the fixed reference frame
First, let's consider a measurement in the fixed referenced frame.

The transfer function from actuator $[F_u, F_v]$ to sensor $[D_x, D_y]$ is then $G(\theta)$.

Then the measurement is subtracted to the reference signal $[r_x, r_y]$ to obtain the position error in the fixed reference frame $[\epsilon_x, \epsilon_y]$.

The position error $[\epsilon_x, \epsilon_y]$ is then express in the rotating frame corresponding to the actuators $[\epsilon_u, \epsilon_v]$.

Finally, the control low $K$ links the position errors $[\epsilon_u, \epsilon_v]$ to the actuator forces $[F_u, F_v]$.

The block diagram is shown on figure [[fig:control_measure_fixed_2dof]].

#+name: fig:control_measure_fixed_2dof
#+caption: Control with a measure from fixed frame
[[./figs/control_measure_fixed_2dof.png]]

The loop gain is then $L = G(\theta) K J(\theta)$.

One question we wish to answer is: is $G(\theta) J(\theta) = G(\theta_0) J(\theta_0)$?

** Measurement in the rotating frame
Let's consider that the measurement is made in the rotating reference frame.

The corresponding block diagram is shown figure [[fig:control_measure_rotating_2dof]].

#+name: fig:control_measure_rotating_2dof
#+caption: Control with a measure from rotating frame
[[./figs/control_measure_rotating_2dof.png]]

The loop gain is $L = G K$.

* Multi Body Model - Simscape
  :PROPERTIES:
  :HEADER-ARGS:matlab+: :tangle matlab/simscape_analysis.m
  :END:
  <<sec:simscape>>

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

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

** Initialization
Let's load the previously defined parameters for the model.

#+begin_src matlab :exports none :noweb yes
  load('./mat/parameters.mat');
#+end_src

#+begin_src matlab :results none
  bode_opts = bodeoptions;
  bode_opts.FreqUnits = 'Hz';
  bode_opts.MagUnits  = 'abs';
  bode_opts.MagScale  = 'log';
  bode_opts.Grid      = 'on';
  bode_opts.PhaseVisible = 'off';
  bode_opts.Title.FontSize     = 10;
  bode_opts.XLabel.FontSize    = 10;
  bode_opts.YLabel.FontSize    = 10;
  bode_opts.TickLabel.FontSize = 10;
#+end_src

#+begin_src matlab :exports none
  open rotating_frame.slx
#+end_src

First we define the parameters that must be defined in order to run the Simscape simulation.
#+begin_src matlab :exports code
  w = 2*pi; % Rotation speed [rad/s]

  theta_e = 0; % Static measurement error on the angle theta [rad]

  m = 5; % mass of the sample [kg]

  mTuv = 30;% Mass of the moving part of the Tuv stage [kg]
  kTuv = 1e8; % Stiffness of the Tuv stage [N/m]
  cTuv = 0; % Damping of the Tuv stage [N/(m/s)]
#+end_src

Then, we defined parameters that will be used in the following analysis.
#+begin_src matlab :exports code
  mlight = 5; % Mass for light sample [kg]
  mheavy = 55; % Mass for heavy sample [kg]

  wlight = 2*pi; % Max rot. speed for light sample [rad/s]
  wheavy = 2*pi/60; % Max rot. speed for heavy sample [rad/s]

  kvc = 1e3; % Voice Coil Stiffness [N/m]
  kpz = 1e8; % Piezo Stiffness [N/m]

  d = 0.01; % Maximum excentricity from rotational axis [m]

  freqs = logspace(-2, 3, 1000); % Frequency vector for analysis [Hz]
#+end_src

** Identification in the rotating referenced frame
We initialize the inputs and outputs of the system to identify:
- Inputs: $f_u$ and $f_v$
- Outputs: $d_u$ and $d_v$

#+begin_src matlab :exports code
  %% Options for Linearized
  options = linearizeOptions;
  options.SampleTime = 0;

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

  %% Input/Output definition
  io(1) = linio([mdl, '/fu'], 1, 'input');
  io(2) = linio([mdl, '/fv'], 1, 'input');

  io(3) = linio([mdl, '/du'], 1, 'output');
  io(4) = linio([mdl, '/dv'], 1, 'output');
#+end_src

We start we identify the transfer functions at high speed with the light sample.
#+begin_src matlab :exports code
  w = wlight; % Rotation speed [rad/s]
  m = mlight; % mass of the sample [kg]

  kTuv = kpz;
  Gpz_light = linearize(mdl, io, 0.1);
  Gpz_light.InputName  = {'Fu', 'Fv'};
  Gpz_light.OutputName = {'Du', 'Dv'};

  kTuv = kvc;
  Gvc_light = linearize(mdl, io, 0.1);
  Gvc_light.InputName  = {'Fu', 'Fv'};
  Gvc_light.OutputName = {'Du', 'Dv'};
#+end_src

Then we identify the system with an heavy mass and low speed.
#+begin_src matlab :exports code
  w = wheavy; % Rotation speed [rad/s]
  m = mheavy; % mass of the sample [kg]

  kTuv = kpz;
  Gpz_heavy = linearize(mdl, io, 0.1);
  Gpz_heavy.InputName  = {'Fu', 'Fv'};
  Gpz_heavy.OutputName = {'Du', 'Dv'};

  kTuv = kvc;
  Gvc_heavy = linearize(mdl, io, 0.1);
  Gvc_heavy.InputName  = {'Fu', 'Fv'};
  Gvc_heavy.OutputName = {'Du', 'Dv'};
#+end_src

** Coupling ratio between $f_{uv}$ and $d_{uv}$
In order to validate the equations written, we can compute the coupling ratio using the simscape model and compare with the equations.

From the previous identification, we plot the coupling ratio in both case (figure [[fig:coupling_ratio_light_heavy]]).

We obtain the same result than the analytical case (figures [[fig:coupling_light]] and [[fig:coupling_heavy]]).
#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gvc_light('Du', 'Fu'), freqs, 'Hz')))./abs(squeeze(freqresp(Gvc_light('Dv', 'Fu'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_light('Du', 'Fu'), freqs, 'Hz')))./abs(squeeze(freqresp(Gpz_light('Dv', 'Fu'), freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',1);
  plot(freqs, abs(squeeze(freqresp(Gvc_heavy('Du', 'Fu'), freqs, 'Hz')))./abs(squeeze(freqresp(Gvc_heavy('Dv', 'Fu'), freqs, 'Hz'))), '--');
  plot(freqs, abs(squeeze(freqresp(Gpz_heavy('Du', 'Fu'), freqs, 'Hz')))./abs(squeeze(freqresp(Gpz_heavy('Dv', 'Fu'), freqs, 'Hz'))), '--');
  hold off;
  xlim([freqs(1), freqs(end)]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Coupling ratio');
  legend({'light - VC', 'light - PZ', 'heavy - VC',  'heavy - PZ'}, 'Location', 'northeast')
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes  :mkdirp yes
#+begin_src matlab :var filepath="figs/coupling_ratio_light_heavy.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+LABEL: fig:coupling_ratio_light_heavy
#+CAPTION: Coupling ratio obtained with the Simscape model
[[file:figs/coupling_ratio_light_heavy.png]]

** Transfer function from force to force sensor (IFF plant)
#+begin_src matlab :exports code
  %% Name of the Simulink File
  mdl = 'rotating_frame';

  %% Input/Output definition
  io(1) = linio([mdl, '/fu'], 1, 'input');
  io(2) = linio([mdl, '/fv'], 1, 'input');

  io(3) = linio([mdl, '/fum'], 1, 'output');
  io(4) = linio([mdl, '/fvm'], 1, 'output');
#+end_src

We start we identify the transfer functions at high speed with the light sample.
#+begin_src matlab :exports code
  w = wlight; % Rotation speed [rad/s]
  m = mlight; % mass of the sample [kg]

  kTuv = kpz;
  Gpz_light = linearize(mdl, io, 0.1);
  Gpz_light.InputName  = {'Fu', 'Fv'};
  Gpz_light.OutputName = {'Fum', 'Fvm'};

  kTuv = kvc;
  Gvc_light = linearize(mdl, io, 0.1);
  Gvc_light.InputName  = {'Fu', 'Fv'};
  Gvc_light.OutputName = {'Fum', 'Fvm'};
#+end_src

Then we identify the system with an heavy mass and low speed.
#+begin_src matlab :exports code
  w = wheavy; % Rotation speed [rad/s]
  m = mheavy; % mass of the sample [kg]

  kTuv = kpz;
  Gpz_heavy = linearize(mdl, io, 0.1);
  Gpz_heavy.InputName  = {'Fu', 'Fv'};
  Gpz_heavy.OutputName = {'Fum', 'Fvm'};

  kTuv = kvc;
  Gvc_heavy = linearize(mdl, io, 0.1);
  Gvc_heavy.InputName  = {'Fu', 'Fv'};
  Gvc_heavy.OutputName = {'Fum', 'Fvm'};
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gvc_heavy('Fum', 'Fu'), freqs, 'Hz'))));
  hold off;
  xlim([freqs(1), freqs(end)]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Coupling ratio');
  legend({'light - VC', 'light - PZ', 'heavy - VC',  'heavy - PZ'}, 'Location', 'northeast')
#+end_src

** Plant Control - SISO approach
*** Plant identification
The goal is to study the control problems due to the coupling that appears because of the rotation.

#+begin_src matlab :exports none
  %% Options for Linearized
  options = linearizeOptions;
  options.SampleTime = 0;

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

  %% Input/Output definition
  io(1) = linio([mdl, '/fu'], 1, 'input');
  io(2) = linio([mdl, '/fv'], 1, 'input');

  io(3) = linio([mdl, '/du'], 1, 'output');
  io(4) = linio([mdl, '/dv'], 1, 'output');
#+end_src

First, we identify the system when the rotation speed is null and then when the rotation speed is equal to 60rpm.

The actuators are voice coil with some damping added.

The bode plot of the system not rotating and rotating at 60rpm is shown figure [[fig:Gvc_speed]].

#+begin_src matlab :exports none
  w = 0; % Rotation speed [rad/s]
  m = mlight; % mass of the sample [kg]
  kTuv = kvc;
  cTuv = 0.1*sqrt(kTuv*m);

  Gvc = linearize(mdl, io, 0.1);
  Gvc.InputName  = {'Fu', 'Fv'};
  Gvc.OutputName = {'Du', 'Dv'};
#+end_src

#+begin_src matlab :exports none
  w = wlight; % Rotation speed [rad/s]
  m = mlight; % mass of the sample [kg]
  kTuv = kvc;
  cTuv = 0.1*sqrt(kTuv*m);

  Gtvc = linearize(mdl, io, 0.1);
  Gtvc.InputName  = {'Fu', 'Fv'};
  Gtvc.OutputName = {'Du', 'Dv'};
#+end_src

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2,2,1);
  title('From $F_u$ to $D_u$');
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gvc(1, 1),  freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gtvc(1, 1), freqs, 'Hz'))));
  hold off;
  xlim([freqs(1), freqs(end)]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude [m/N]');

  ax2 = subplot(2,2,3);
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gvc(1, 1),  freqs, 'Hz'))), 'DisplayName', 'Gvc - $\omega = 0$');
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gtvc(1, 1), freqs, 'Hz'))), 'DisplayName', 'Gvc - $\omega = 60$rpm');
  hold off;
  yticks(-180:90:180);
  ylim([-180 180]);
  xlim([freqs(1), freqs(end)]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  legend('Location', 'northeast');
  linkaxes([ax1,ax2],'x');

  ax1 = subplot(2,2,2);
  title('From $F_u$ to $D_v$');
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gvc(1, 2),  freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gtvc(1, 2), freqs, 'Hz'))));
  hold off;
  xlim([freqs(1), freqs(end)]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);

  ax2 = subplot(2,2,4);
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gvc(1, 2),  freqs, 'Hz'))), 'DisplayName', 'Gvc - $\omega = 0$');
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gtvc(1, 2), freqs, 'Hz'))), 'DisplayName', 'Gvc - $\omega = 60$rpm');
  hold off;
  yticks(-180:90:180);
  ylim([-180 180]);
  xlim([freqs(1), freqs(end)]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  xlabel('Frequency [Hz]');
  legend('Location', 'northeast');
  linkaxes([ax1,ax2],'x');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/Gvc_speed.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+LABEL: fig:Gvc_speed
#+CAPTION: Change of transfer functions due to rotating speed
[[file:figs/Gvc_speed.png]]

*** Effect of rotation speed
We first identify the system (voice coil and light mass) for multiple rotation speed.
Then we compute the bode plot of the system (figure [[fig:Guu_uv_ws]]).

As the rotation frequency increases:
- one pole goes to lower frequencies while the other goes to higher frequencies
- one zero appears between the two poles
- the zero disappears when $\omega > \sqrt{\frac{k}{m}}$ and the low frequency pole becomes unstable (positive real part)

To stabilize the unstable pole, we need a control bandwidth of at least twice of frequency of the unstable pole.

#+begin_src matlab :exports none
  ws = linspace(0, 2*pi, 5); % Rotation speed vector [rad/s]
  m = mlight; % mass of the sample [kg]

  kTuv = kvc;
  cTuv = 0.1*sqrt(kTuv*m);

  Gs_vc = {zeros(1, length(ws))};

  for i = 1:length(ws)
    w = ws(i);
    Gs_vc{i} = linearize(mdl, io, 0.1);
    Gs_vc{i}.InputName  = {'Fu', 'Fv'};
    Gs_vc{i}.OutputName = {'Du', 'Dv'};
  end
#+end_src

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

  figure;
  ax1 = subplot(2,2,1);
  title('$D_u/F_u$');
  hold on;
  for i = 1:length(ws)
    plot(freqs, abs(squeeze(freqresp(Gs_vc{i}(1, 1), freqs, 'Hz'))));
  end
  hold off;
  xlim([freqs(1), freqs(end)]);
  ylim([1e-8, 1]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude [m/N]');

  ax2 = subplot(2,2,3);
  hold on;
  for i = 1:length(ws)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_vc{i}(1, 1), freqs, 'Hz'))));
  end
  hold off;
  yticks(-180:90:180);
  ylim([-180 180]);
  xlim([freqs(1), freqs(end)]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  linkaxes([ax1,ax2],'x');

  ax1 = subplot(2,2,2);
  title('$D_v/F_u$');
  hold on;
  for i = 1:length(ws)
    plot(freqs, abs(squeeze(freqresp(Gs_vc{i}(1, 2), freqs, 'Hz'))));
  end
  hold off;
  xlim([freqs(1), freqs(end)]);
  ylim([1e-8, 1]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  set(gca, 'YTickLabel',[]);

  ax2 = subplot(2,2,4);
  hold on;
  for i = 1:length(ws)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_vc{i}(1, 2), freqs, 'Hz'))), 'DisplayName', sprintf('w = %.0f [rpm]', ws(i)*60/2/pi));
  end
  hold off;
  yticks(-180:90:180);
  ylim([-180 180]);
  xlim([freqs(1), freqs(end)]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  set(gca, 'YTickLabel',[]);
  xlabel('Frequency [Hz]');
  legend('Location', 'northeast');
  linkaxes([ax1,ax2],'x');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/Guu_uv_ws.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+LABEL: fig:Guu_uv_ws
#+CAPTION: Diagonal term as a function of the rotation frequency
[[file:figs/Guu_uv_ws.png]]

Then, we can look at the same plots for the piezoelectric actuator (figure [[fig:Guu_ws_pz]]). The effect of the rotation frequency has very little effect on the dynamics of the system to control.

#+begin_src matlab :exports none
  ws = linspace(0, 2*pi, 5); % Rotation speed vector [rad/s]
  m = mlight; % mass of the sample [kg]

  kTuv = kpz;
  cTuv = 0.1*sqrt(kTuv*m);

  Gs_pz = {zeros(1, length(ws))};

  for i = 1:length(ws)
    w = ws(i);
    Gs_pz{i} = linearize(mdl, io, 0.1);
    Gs_pz{i}.InputName  = {'Fu', 'Fv'};
    Gs_pz{i}.OutputName = {'Du', 'Dv'};
  end
#+end_src

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

  figure;
  ax1 = subplot(2,1,1);
  hold on;
  for i = 1:length(ws)
    plot(freqs, abs(squeeze(freqresp(Gs_pz{i}(1, 1), freqs, 'Hz'))));
  end
  hold off;
  xlim([freqs(1), freqs(end)]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude [m/N]');

  ax2 = subplot(2,1,2);
  hold on;
  for i = 1:length(ws)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_pz{i}(1, 1), freqs, 'Hz'))), 'DisplayName', sprintf('w = %.0f [rpm]', ws(i)*60/2/pi));
  end
  hold off;
  yticks(-180:90:180);
  ylim([-180 180]);
  xlim([freqs(1), freqs(end)]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  legend('Location', 'northeast');
  linkaxes([ax1,ax2],'x');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/Guu_ws_pz.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+LABEL: fig:Guu_ws_pz
#+CAPTION: Diagonal term as a function of the rotation frequency
[[file:figs/Guu_ws_pz.png]]

*** Controller design
We design a controller based on the identification when the system is not rotating.

The obtained controller is a lead-lag controller with the following transfer function.
#+begin_src matlab :results none :exports code
  Kll = 2.0698e09*(s+40.45)*(s+1.181)/((s+0.01)*(s+198.4)*(s+2790));
  K = [Kll 0;
       0   Kll];

  K.InputName  = {'Du', 'Dv'};
  K.OutputName = {'Fu', 'Fv'};
#+end_src

The loop gain is displayed figure [[fig:Gvc_loop_gain]].

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

  figure;
  % Amplitude
  ax1 = subplot(2,1,1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gvc('Du', 'fu')*Kll, freqs, 'Hz'))), '-');
  set(gca,'xscale','log'); set(gca,'yscale','log');
  ylabel('Amplitude [m/N]');
  set(gca, 'XTickLabel',[]);
  hold off;
  % Phase
  ax2 = subplot(2,1,2);
  hold on;
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gvc('Du', 'fu')*Kll, freqs, 'Hz')))), '-');
  set(gca,'xscale','log');
  yticks(-180:180:180);
  ylim([-180 180]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/Gvc_loop_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+LABEL: fig:Gvc_loop_gain
#+CAPTION: Loop gain obtained for a lead-lag controller on the system with a voice coil
[[file:figs/Gvc_loop_gain.png]]

*** Controlling the rotating system
We here want to see if the system is robust with respect to the rotation speed.
We use the controller that was designed based on the dynamics of the non-rotating system.

Let's first plot the SISO loop gain.

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

  figure;
  % Amplitude
  ax1 = subplot(2,1,1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gs_vc{1}(1, 1)*Kll, freqs, 'Hz'))), '-');
  plot(freqs, abs(squeeze(freqresp(Gs_vc{2}(1, 1)*Kll, freqs, 'Hz'))), '-');
  plot(freqs, abs(squeeze(freqresp(Gs_vc{5}(1, 1)*Kll, freqs, 'Hz'))), '-');
  set(gca,'xscale','log'); set(gca,'yscale','log');
  ylabel('Amplitude [m/N]');
  set(gca, 'XTickLabel',[]);
  hold off;

  % Phase
  ax2 = subplot(2,1,2);
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_vc{1}(1, 1)*Kll, freqs, 'Hz'))), 'DisplayName', sprintf('%.0f rpm', ws(1)./(2*pi).*60));
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_vc{2}(1, 1)*Kll, freqs, 'Hz'))), 'DisplayName', sprintf('%.0f rpm', ws(2)./(2*pi).*60));
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_vc{5}(1, 1)*Kll, freqs, 'Hz'))), 'DisplayName', sprintf('%.0f rpm', ws(5)./(2*pi).*60));
  set(gca,'xscale','log');
  yticks(-180:180:180);
  ylim([-180 180]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  legend('Location', 'northeast');
  linkaxes([ax1,ax2],'x');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/loop_gain_turning.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:loop_gain_turning
#+CAPTION: Loop Gain $G_u * K$
[[file:figs/loop_gain_turning.png]]

We can now compute the close-loop systems.
#+begin_src matlab
  Gvc_cl = {zeros(1, length(ws))};
  for i = 1:length(ws)
    Gvc_cl{i} = feedback(Gs_vc{i}, K, 'name');
  end
#+end_src

Let's now look on figure [[fig:evolution_poles_u]] at the evolution of the poles of the system when closing only one loop (controlling only one direction). We see that two complex conjugate poles are approaching the real axis and then they separate: one goes to positive real part and the other goes to negative real part.
The system then goes unstable at some point (here for $\omega=60rpm$).

#+begin_src matlab
  figure;
  hold on;
  for i = 1:length(ws)
    sys = feedback(Gs_vc{i}(1, 1), K(1, 1), 'name');
    plot(real(pole(sys)), imag(pole(sys)), 'x', 'DisplayName', sprintf('$\\omega = %.0f rpm$', ws(i)/(2*pi)*60));
  end
  hold off;
  xlim([-80, 10]);
  xlabel('Real Axis'); ylabel('Imaginary Axis');
  legend('Location', 'northeast');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/evolution_poles_u.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:evolution_poles_u
#+CAPTION: Evolution of the poles of the closed-loop system when closing just one loop
[[file:figs/evolution_poles_u.png]]

If we look at the poles of the closed loop-system for multiple rotating speed (figure [[fig:poles_cl_system]]) when closing the two loops (MIMO system), we see that they all have a negative real part (stable system), and their evolution on the complex plane is rather small compare to the open loop evolution.

#+begin_src matlab
  figure;
  hold on;
  for i = 1:length(ws)
    plot(real(pole(Gvc_cl{i})), imag(pole(Gvc_cl{i})), 'x', 'DisplayName', sprintf('$\\omega = %.0f rpm$', ws(i)/(2*pi)*60));
  end
  hold off;
  xlim([-80, 0]);
  xlabel('Real Axis'); ylabel('Imaginary Axis');
  legend('Location', 'northeast');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/poles_cl_system.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:poles_cl_system
#+CAPTION: Evolution of the poles of the closed-loop system
[[file:figs/poles_cl_system.png]]

*** TODO Neglected coupling
Take a diagonal system and apply the diagonal controller.

I expect that the system will be unstable
=> in some way the coupling makes the system stable

*** Close loop performance
First, we create the closed loop systems. Then, we plot the transfer function from the reference signals $[r_u, r_v]$ to the output $[d_u, d_v]$ (figure [[fig:perfcomp]]).

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

  figure;
  ax1 = subplot(1,2,1);
  title('$G_{r_u \to d_u}$')
  hold on;
  for i = 1:length(ws)
    sys = Gvc_cl{i}*K;
    plot(freqs, abs(squeeze(freqresp(sys(1, 1), freqs, 'Hz'))));
  end
  hold off;
  xlim([freqs(1), freqs(end)]);
  ylim([1e-4, 10]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');

  ax2 = subplot(1,2,2);
  title('$G_{r_u \to d_v}$')
  hold on;
  for i = 1:length(ws)
    sys = Gvc_cl{i}*K;
    plot(freqs, abs(squeeze(freqresp(sys(1, 2), freqs, 'Hz'))), 'DisplayName', sprintf('$\\omega = %.0f rpm$', ws(i)/(2*pi)*60));
  end
  hold off;
  xlim([freqs(1), freqs(end)]);
  ylim([1e-4, 10]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]');
  legend('Location', 'northeast')

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

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/perfcomp.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+LABEL: fig:perfcomp
#+CAPTION: Close loop performance for $\omega = 0$ and $\omega = 60 rpm$
[[file:figs/perfcomp.png]]

*** Campbell Diagram for the close loop system

#+begin_src matlab :exports code
  m = mlight;
  k = kvc;
  c = 0.1*sqrt(k*m);

  wsvc = linspace(0, 10, 100); % [rad/s]

  polesvc = zeros(8, length(wsvc));

  for i = 1:length(wsvc)
    Gs = (m*s^2 + (k-m*w^2))/((m*s^2 + (k - m*w^2))^2 + (2*m*w*s)^2);
    Gcs = (2*m*w*s)/((m*s^2 + (k - m*w^2))^2 + (2*m*w*s)^2);

    G = [Gs, Gcs; Gcs, Gs];
    G.InputName  = {'Fu', 'Fv'};
    G.OutputName = {'Du', 'Dv'};

    polei = pole(feedback(G, K, 'name'));
    polesvc(:, i) = sort(polei(imag(polei) > 0));

    polei = pole(feedback(G, 10*K, 'name'));
    polesvcb(:, i) = sort(polei(imag(polei) > 0));
  end
#+end_src

#+begin_src matlab :exports none
  figure;
  % Amplitude
  ax1 = subplot(1,2,1);
  hold on;
  for i = 1:8
    plot(wsvc, real(polesvc(i, :)), 'b')
    plot(wsvc, real(polesvcb(i, :)), 'r')
  end
  plot(wsvc, zeros(size(wsvc)), 'k--')
  hold off;
  xlabel('Rotation Frequency [rad/s]');
  ylabel('Pole Real Part');

  ax2 = subplot(1,2,2);
  hold on;
  for i = 1:8
    plot(wsvc, imag(polesvc(i, :)), 'b')
    plot(wsvc, imag(polesvcb(i, :)), 'r')
  end
  hold off;
  xlabel('Rotation Frequency [rad/s]');
  ylabel('Pole Imaginary Part');
#+end_src


*** Conclusion
#+begin_important
  Even though considering one input and output at a time would results in an unstable system (figure [[fig:evolution_poles_u]]), when using the diagonal MIMO controller, the system stays stable (figure [[fig:poles_cl_system]]). This could be understood by saying that when controlling both directions at the same time, the coupling effect should be much lower than when controlling only one direction.

  The close-loop performance does not vary a lot with the rotating speed (figure [[fig:perfcomp]]) even tough the open loop system is varying quite a lot (figure [[fig:Guu_uv_ws]]).
#+end_important

** BKMK Plant Control - MIMO approach
*** Analysis - SVD
The singular value decomposition of a MIMO system $G$ is defined as follow:
\[ G = U \Sigma V^H \]

With:
- $\Sigma$ is an $2 \times 2$ matrix with 2 non-negative *singular values* $\sigma_i$, arranged in descending order along its main diagonal
- $U$ is an $2 \times 2$ unitary matrix. The columns vectors of $U$, denoted $u_i$, represent the *output directions* of the plant. They are orthonomal
- $V$ is an $2 \times 2$ unitary matrix. The columns vectors of $V$, denoted $v_i$, represent the *input directions* of the plant. They are orthonomal

We first look at the evolution of the singular values as a function of frequency (figure [[fig:G_sigma]]).

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

  figure;
  hold on;
  for i = 1:length(ws)
    sv = sigma(Gs_vc{i}, 2*pi*freqs);
    set(gca,'ColorOrderIndex',i)
    plot(freqs, sv(1, :), 'DisplayName', sprintf('w = %.0f rpm', ws(i)*60/2/pi));
    set(gca,'ColorOrderIndex',i)
    plot(freqs, sv(2, :), '--', 'HandleVisibility', 'off');
  end
  hold off;
  set(gca,'xscale','log'); set(gca,'yscale','log');
  legend('location', 'southwest');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/G_sigma.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+LABEL: fig:G_sigma
#+CAPTION: Evolution of the singular values with frequency
[[file:figs/G_sigma.png]]

* Control Implementation
  <<sec:control>>

* Bibliography                                                      :ignore:
# bibliographystyle:unsrt
# bibliography:refs.bib