Add IFF and DVF study

index.org

@@ -23,6 +23,7 @@
 #+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.
@@ -437,7 +438,7 @@ Then we compare the result between voice coil and piezoelectric actuators.
   freqs = logspace(-2, 1, 1000);
 
   figure;
-  ax1 = subaxis(2,1,1);
+  ax1 = subplot(2,1,1);
   hold on;
   for i = 1:length(ws)
     plot(freqs, abs(squeeze(freqresp(Gs{i}, freqs, 'Hz'))));
@@ -448,7 +449,7 @@ Then we compare the result between voice coil and piezoelectric actuators.
   set(gca, 'XTickLabel',[]);
   ylabel('Magnitude [m/N]');
 
-  ax2 = subaxis(2,1,2);
+  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));
@@ -477,7 +478,7 @@ Then we compare the result between voice coil and piezoelectric actuators.
   freqs = logspace(-2, 1, 1000);
 
   figure;
-  ax1 = subaxis(2,1,1);
+  ax1 = subplot(2,1,1);
   hold on;
   for i = 1:length(ws)
     plot(freqs, abs(squeeze(freqresp(Gcs{i}, freqs, 'Hz'))));
@@ -488,7 +489,7 @@ Then we compare the result between voice coil and piezoelectric actuators.
   set(gca, 'XTickLabel',[]);
   ylabel('Magnitude [m/N]');
 
-  ax2 = subaxis(2,1,2);
+  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));
@@ -533,7 +534,7 @@ Then we compare the result between voice coil and piezoelectric actuators.
   freqs = logspace(2, 3, 1000);
 
   figure;
-  ax1 = subaxis(2,1,1);
+  ax1 = subplot(2,1,1);
   hold on;
   for i = 1:length(ws)
     plot(freqs, abs(squeeze(freqresp(Gs{i}, freqs, 'Hz'))));
@@ -544,7 +545,7 @@ Then we compare the result between voice coil and piezoelectric actuators.
   set(gca, 'XTickLabel',[]);
   ylabel('Magnitude [m/N]');
 
-  ax2 = subaxis(2,1,2);
+  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));
@@ -570,7 +571,7 @@ Then we compare the result between voice coil and piezoelectric actuators.
 
 #+begin_src matlab :exports none
   figure;
-  ax1 = subaxis(2,1,1);
+  ax1 = subplot(2,1,1);
   hold on;
   for i = 1:length(ws)
     plot(freqs, abs(squeeze(freqresp(Gcs{i}, freqs, 'Hz'))));
@@ -581,7 +582,7 @@ Then we compare the result between voice coil and piezoelectric actuators.
   set(gca, 'XTickLabel',[]);
   ylabel('Magnitude [m/N]');
 
-  ax2 = subaxis(2,1,2);
+  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));
@@ -728,6 +729,512 @@ For the voice coil (figure [[fig:poles_w_vc]]), the system is unstable when the
 #+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
@@ -913,6 +1420,62 @@ We obtain the same result than the analytical case (figures [[fig:coupling_light
 #+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.
@@ -963,7 +1526,7 @@ The bode plot of the system not rotating and rotating at 60rpm is shown figure [
 
 #+begin_src matlab :exports none
   figure;
-  ax1 = subaxis(2,2,1);
+  ax1 = subplot(2,2,1);
   title('From $F_u$ to $D_u$');
   hold on;
   plot(freqs, abs(squeeze(freqresp(Gvc(1, 1),  freqs, 'Hz'))));
@@ -974,7 +1537,7 @@ The bode plot of the system not rotating and rotating at 60rpm is shown figure [
   set(gca, 'XTickLabel',[]);
   ylabel('Magnitude [m/N]');
 
-  ax2 = subaxis(2,2,3);
+  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');
@@ -987,7 +1550,7 @@ The bode plot of the system not rotating and rotating at 60rpm is shown figure [
   legend('Location', 'northeast');
   linkaxes([ax1,ax2],'x');
 
-  ax1 = subaxis(2,2,2);
+  ax1 = subplot(2,2,2);
   title('From $F_u$ to $D_v$');
   hold on;
   plot(freqs, abs(squeeze(freqresp(Gvc(1, 2),  freqs, 'Hz'))));
@@ -997,7 +1560,7 @@ The bode plot of the system not rotating and rotating at 60rpm is shown figure [
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
   set(gca, 'XTickLabel',[]);
 
-  ax2 = subaxis(2,2,4);
+  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');
@@ -1052,7 +1615,7 @@ To stabilize the unstable pole, we need a control bandwidth of at least twice of
   freqs = logspace(-2, 2, 1000);
 
   figure;
-  ax1 = subaxis(2,2,1);
+  ax1 = subplot(2,2,1);
   title('$D_u/F_u$');
   hold on;
   for i = 1:length(ws)
@@ -1065,7 +1628,7 @@ To stabilize the unstable pole, we need a control bandwidth of at least twice of
   set(gca, 'XTickLabel',[]);
   ylabel('Magnitude [m/N]');
 
-  ax2 = subaxis(2,2,3);
+  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'))));
@@ -1078,7 +1641,7 @@ To stabilize the unstable pole, we need a control bandwidth of at least twice of
   xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
   linkaxes([ax1,ax2],'x');
 
-  ax1 = subaxis(2,2,2);
+  ax1 = subplot(2,2,2);
   title('$D_v/F_u$');
   hold on;
   for i = 1:length(ws)
@@ -1091,7 +1654,7 @@ To stabilize the unstable pole, we need a control bandwidth of at least twice of
   set(gca, 'XTickLabel',[]);
   set(gca, 'YTickLabel',[]);
 
-  ax2 = subaxis(2,2,4);
+  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));
@@ -1139,7 +1702,7 @@ Then, we can look at the same plots for the piezoelectric actuator (figure [[fig
   freqs = logspace(2, 3, 1000);
 
   figure;
-  ax1 = subaxis(2,1,1);
+  ax1 = subplot(2,1,1);
   hold on;
   for i = 1:length(ws)
     plot(freqs, abs(squeeze(freqresp(Gs_pz{i}(1, 1), freqs, 'Hz'))));
@@ -1150,7 +1713,7 @@ Then, we can look at the same plots for the piezoelectric actuator (figure [[fig
   set(gca, 'XTickLabel',[]);
   ylabel('Magnitude [m/N]');
 
-  ax2 = subaxis(2,1,2);
+  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));
@@ -1194,7 +1757,7 @@ The loop gain is displayed figure [[fig:Gvc_loop_gain]].
 
   figure;
   % Amplitude
-  ax1 = subaxis(2,1,1);
+  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');
@@ -1202,7 +1765,7 @@ The loop gain is displayed figure [[fig:Gvc_loop_gain]].
   set(gca, 'XTickLabel',[]);
   hold off;
   % Phase
-  ax2 = subaxis(2,1,2);
+  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');
@@ -1233,7 +1796,7 @@ Let's first plot the SISO loop gain.
 
   figure;
   % Amplitude
-  ax1 = subaxis(2,1,1);
+  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'))), '-');
@@ -1244,7 +1807,7 @@ Let's first plot the SISO loop gain.
   hold off;
 
   % Phase
-  ax2 = subaxis(2,1,2);
+  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));
@@ -1283,7 +1846,7 @@ The system then goes unstable at some point (here for $\omega=60rpm$).
   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)), 'o', 'DisplayName', sprintf('$\\omega = %.0f rpm$', ws(i)/(2*pi)*60));
+    plot(real(pole(sys)), imag(pole(sys)), 'x', 'DisplayName', sprintf('$\\omega = %.0f rpm$', ws(i)/(2*pi)*60));
   end
   hold off;
   xlim([-80, 10]);
@@ -1306,7 +1869,7 @@ If we look at the poles of the closed loop-system for multiple rotating speed (f
   figure;
   hold on;
   for i = 1:length(ws)
-    plot(real(pole(Gvc_cl{i})), imag(pole(Gvc_cl{i})), 'o', 'DisplayName', sprintf('$\\omega = %.0f rpm$', ws(i)/(2*pi)*60));
+    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]);
@@ -1323,6 +1886,12 @@ If we look at the poles of the closed loop-system for multiple rotating speed (f
 #+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]]).
 
@@ -1369,6 +1938,59 @@ First, we create the closed loop systems. Then, we plot the transfer function fr
 #+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.
@@ -1418,4 +2040,5 @@ We first look at the evolution of the singular values as a function of frequency
   <<sec:control>>
 
 * Bibliography                                                      :ignore:
-# #+BIBLIOGRAPHY: /home/tdehaeze/MEGA/These/Ressources/references.bib plain option:-a option:-noabstract option:-nokeywords option:-noheader option:-nofooter option:-nobibsource limit:t
+# bibliographystyle:unsrt
+# bibliography:refs.bib