Add option to initialize spindle rotation speed

org/optimal_stiffness.org

@@ -100,6 +100,7 @@ We don't include gravity nor disturbances in this model as it adds complexity to
   initializeSimscapeConfiguration('gravity', true);
   initializeDisturbances('enable', false);
   initializeLoggingConfiguration('log', 'none');
+  initializeController();
 #+end_src
 
 #+begin_src matlab :exports none
@@ -114,142 +115,132 @@ We don't include gravity nor disturbances in this model as it adds complexity to
   io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'En');   io_i = io_i + 1;
 #+end_src
 
-** Identification when not rotating
+** Identification when rotating at maximum speed
-We set the range of stiffness that we want to use.
+#+begin_src matlab
+  Rz_rpm = linspace(0, 60, 6);
+#+end_src
+
 #+begin_src matlab
   Ks = logspace(3,9,7); % [N/m]
 #+end_src
 
-We don't move any stage and no controller is used.
-#+begin_src matlab
-  initializeReferences();
-  initializeController();
-#+end_src
-
 #+begin_src matlab :exports none
-  Gk_iff = {zeros(length(Ks))};
+  Gk_wz_iff = {zeros(length(Ks), length(Rz_rpm))};
-  Gk_dvf = {zeros(length(Ks))};
+  Gk_wz_dvf = {zeros(length(Ks), length(Rz_rpm))};
-  Gk_err = {zeros(length(Ks))};
+  Gk_wz_err = {zeros(length(Ks), length(Rz_rpm))};
 #+end_src
 
 #+begin_src matlab :exports none
   for i = 1:length(Ks)
-    initializeNanoHexapod('k', Ks(i));
+    for j = 1:length(Rz_rpm)
+      initializeReferences('Rz_type', 'rotating-not-filtered', ...
+                          'Rz_period', 60/Rz_rpm(j));
+      initializeNanoHexapod('k', Ks(i));
 
-    %% Run the linearization
+      %% Run the linearization
-    G = linearize(mdl, io);
+      G = linearize(mdl, io);
-    G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
+      G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
-    G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
+      G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
-                    'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
+                      'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
-                    'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
+                      'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
 
-    Gk_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
+      Gk_wz_iff(i,j) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
-    Gk_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
+      Gk_wz_dvf(i,j) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
 
-    Jinvt = tf(inv(nano_hexapod.J)');
+      Jinvt = tf(inv(nano_hexapod.J)');
-    Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
+      Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
-    Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
+      Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
-    Gk_err(i) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'},                {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt};
+      Gk_wz_err(i,j) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'},                {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt};
-  end
+    end
-#+end_src
-
-** Identification when rotating at maximum speed
-We now set the reference path such that the Spindle is rotating at 60rpm and such that it is at the zero position at the time of the identification.
-#+begin_src matlab
-  Rz_rpm = 60;
-
-  initializeReferences('Rz_type', 'rotating', ...
-                       'Rz_period', 60/Rz_rpm, ... % Rotation period [s]
-                       'Rz_amplitude', -0.2*(2*pi*Rz_rpm/60)); % Angle offset [rad]
-
-  load('mat/nass_references.mat', 'Rz'); % We load the reference for the Spindle
-  [~, i_end] = min(abs(Rz.signals.values)); % Obtain the indice where the spindle angle is zero
-  t_sim = Rz.time(i_end); % Simulation time before identification [s]
-#+end_src
-
-We here use a decentralized controller that is used to stabilize the nano-hexapod until the identification is made.
-This controller virtually adds stiffness in each of the nano-hexapod leg.
-#+begin_src matlab
-  k_sta = -1e8;
-  initializeController('type', 'stabilizing');
-#+end_src
-
-#+begin_src matlab :exports none
-  Gk_wz_iff = {zeros(length(Ks))};
-  Gk_wz_dvf = {zeros(length(Ks))};
-  Gk_wz_err = {zeros(length(Ks))};
-#+end_src
-
-#+begin_src matlab :exports none
-  for i = 1:length(Ks)
-    initializeNanoHexapod('k', Ks(i));
-
-    %% Run the linearization
-    G = linearize(mdl, io, t_sim);
-    G.InputName  = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
-    G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ...
-                    'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ...
-                    'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
-
-    Gk_wz_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
-    Gk_wz_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
-
-    Jinvt = tf(inv(nano_hexapod.J)');
-    Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
-    Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
-    Gk_wz_err(i) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'},                {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt};
   end
 #+end_src
 
 #+begin_src matlab :exports none
-  save('mat/optimal_stiffness_Gk_wz.mat', 'Ks', ...
+  save('mat/optimal_stiffness_Gk_wz.mat', 'Ks', 'Rz_rpm', ...
-       'Gk_iff',    'Gk_dvf',    'Gk_err', ...
        'Gk_wz_iff', 'Gk_wz_dvf', 'Gk_wz_err');
 #+end_src
 
-** TODO Change of dynamics
+** Change of dynamics
-- [ ] problem of dynamics at low frequency
-  Check if gravity is a problem
-  Think of a before way to identify the dynamics
-
 #+begin_src matlab :exports none
   load('mat/optimal_stiffness_Gk_wz.mat');
 #+end_src
 
-Change of dynamics for decentralized IFF control.
+Change of dynamics for IFF
 #+begin_src matlab :exports none
-  freqs = logspace(-1, 3, 1000);
+  freqs = logspace(-2, 3, 1000);
 
   figure;
 
   ax1 = subplot(2, 1, 1);
   hold on;
-  for i = 1:length(Gk_iff)
+  for i = 1:length(Ks)
-    set(gca,'ColorOrderIndex',i);
+      for j = 1:length(Rz_rpm)
-    plot(freqs, abs(squeeze(freqresp(Gk_iff{i}(   'Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-');
+          set(gca,'ColorOrderIndex',i);
-    set(gca,'ColorOrderIndex',i);
+          plot(freqs, abs(squeeze(freqresp(Gk_wz_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-');
-    plot(freqs, abs(squeeze(freqresp(Gk_wz_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '--');
+      end
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
   ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
-  title('Soft Nano-Hexapod');
 
   ax2 = subplot(2, 1, 2);
   hold on;
-  for i = 1:length(Gk_iff)
+  for i = 1:length(Ks)
-    set(gca,'ColorOrderIndex',i);
+      set(gca,'ColorOrderIndex',i);
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-', ...
+      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_iff{i,1}('Fnlm1', 'Fnl1'), freqs, 'Hz')))), '-', ...
-         'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
+           'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
-    set(gca,'ColorOrderIndex',i);
+      for j = 2:length(Rz_rpm)
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_wz_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '--', ...
+          set(gca,'ColorOrderIndex',i);
-         'HandleVisibility', 'off');
+          plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz')))), '-', ...
+               'HandleVisibility', 'off');
+      end
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
   ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
+  ylim([-270, 90]);
-  yticks([-180, -90, 0, 90, 180]);
+  yticks([-360:90:360]);
+  legend('location', 'northeast');
+
+  linkaxes([ax1,ax2],'x');
+  xlim([freqs(1), freqs(end)]);
+#+end_src
+
+Change of dynamics for DVF
+#+begin_src matlab :exports none
+  freqs = logspace(-2, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  for i = 1:length(Ks)
+      for j = 1:length(Rz_rpm)
+          set(gca,'ColorOrderIndex',i);
+          plot(freqs, abs(squeeze(freqresp(Gk_wz_dvf{i,j}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), '-');
+      end
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  for i = 1:length(Ks)
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_dvf{i,1}('Dnlm1', 'Fnl1'), freqs, 'Hz')))), '-', ...
+           'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
+      for j = 2:length(Rz_rpm)
+          set(gca,'ColorOrderIndex',i);
+          plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_dvf{i,j}('Dnlm1', 'Fnl1'), freqs, 'Hz')))), '-', ...
+               'HandleVisibility', 'off');
+      end
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-270, 90]);
+  yticks([-360:90:360]);
   legend('location', 'northeast');
 
   linkaxes([ax1,ax2],'x');
@@ -264,11 +255,11 @@ Change of dynamics from $F_x$ to $D_x$.
 
   ax1 = subplot(2, 1, 1);
   hold on;
-  for i = 1:length(Gk_err)
+  for i = 1:length(Ks)
-    set(gca,'ColorOrderIndex',i);
+      for j = 1:length(Rz_rpm)
-    plot(freqs, abs(squeeze(freqresp(Gk_err{i}(   'Ex', 'Fx'), freqs, 'Hz'))), '-');
+          set(gca,'ColorOrderIndex',i);
-    set(gca,'ColorOrderIndex',i);
+          plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-');
-    plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '--');
+      end
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
@@ -276,106 +267,43 @@ Change of dynamics from $F_x$ to $D_x$.
 
   ax2 = subplot(2, 1, 2);
   hold on;
-  for i = 1:length(Gk_err)
+  for i = 1:length(Ks)
-    set(gca,'ColorOrderIndex',i);
+      set(gca,'ColorOrderIndex',i);
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
+      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_err{i,1}('Ex', 'Fx'), freqs, 'Hz')))), '-', ...
-         'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
+           'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
-    set(gca,'ColorOrderIndex',i);
+      for j = 2:length(Rz_rpm)
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_wz_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '--', ...
+          set(gca,'ColorOrderIndex',i);
-         'HandleVisibility', 'off');
+          plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), '-', ...
+               'HandleVisibility', 'off');
+      end
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
   ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
+  ylim([-270, 90]);
-  yticks([-180, -90, 0, 90, 180]);
+  yticks([-360:90:360]);
   legend('location', 'northeast');
 
   linkaxes([ax1,ax2],'x');
   xlim([freqs(1), freqs(end)]);
 #+end_src
 
-Change of dynamics from $F_z$ to $D_z$.
+Change of coupling from $F_x$ to $D_y$ with the rotating speed.
-#+begin_src matlab :exports none
-  freqs = logspace(-1, 3, 1000);
-
-  figure;
-
-  ax1 = subplot(2, 1, 1);
-  hold on;
-  for i = 1:length(Gk_err)
-    set(gca,'ColorOrderIndex',i);
-    plot(freqs, abs(squeeze(freqresp(Gk_err{i}(   'Ez', 'Fz'), freqs, 'Hz'))), '-');
-    set(gca,'ColorOrderIndex',i);
-    plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '--');
-  end
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
-  title('Soft Nano-Hexapod');
-
-  ax2 = subplot(2, 1, 2);
-  hold on;
-  for i = 1:length(Gk_err)
-    set(gca,'ColorOrderIndex',i);
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '-', ...
-         'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
-    set(gca,'ColorOrderIndex',i);
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_wz_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '--', ...
-         'HandleVisibility', 'off');
-  end
-  hold off;
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
-  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
-  yticks([-180, -90, 0, 90, 180]);
-  legend('location', 'northeast');
-
-  linkaxes([ax1,ax2],'x');
-  xlim([freqs(1), freqs(end)]);
-#+end_src
-
-** Change of coupling
-#+begin_src matlab :exports none
-  load('mat/optimal_stiffness_Gk_wz.mat');
-#+end_src
-
-Change of coupling from $F_x$ to $D_y$ when not rotating and when rotating at 60rpm.
 #+begin_src matlab :exports none
   freqs = logspace(-1, 3, 1000);
 
   figure;
 
   hold on;
-  for i = 1:length(Gk_err)
+  for i = 1:length(Ks)
-    set(gca,'ColorOrderIndex',i);
+      set(gca,'ColorOrderIndex',i);
-    plot(freqs, abs(squeeze(freqresp(Gk_err{i}(   'Ey', 'Fx'), freqs, 'Hz'))), '-', ...
+      plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,1}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
-         'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
+           'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
-    set(gca,'ColorOrderIndex',i);
+      for j = 1:length(Rz_rpm)
-    plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i}('Ey', 'Fx'), freqs, 'Hz'))), '--', ...
+          set(gca,'ColorOrderIndex',i);
-         'HandleVisibility', 'off');
+          plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,j}('Ey', 'Fx'), freqs, 'Hz'))), '--', ...
-  end
+           'HandleVisibility', 'off');
-  hold off;
+      end
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
-  xlim([freqs(1), freqs(end)]);
-  legend('location', 'northeast');
-#+end_src
-
-Comparison of the coupling from $F_x$ to $D_y$ when rotating at 60rpm to the direct term $F_x$ to $D_x$.
-#+begin_src matlab :exports none
-  freqs = logspace(-1, 3, 1000);
-
-  figure;
-
-  hold on;
-  for i = 1:length(Gk_err)
-    set(gca,'ColorOrderIndex',i);
-    plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
-         'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
-    set(gca,'ColorOrderIndex',i);
-    plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i}('Ey', 'Fx'), freqs, 'Hz'))), '--', ...
-         'HandleVisibility', 'off');
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
@@ -481,8 +409,16 @@ And we identify the dynamics from forces/torques applied on the micro-hexapod to
   legend('location', 'northwest');
 #+end_src
 
+#+header: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/opt_stiff_micro_station_compliance.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+<<plt-matlab>>
+#+end_src
+
+#+name: fig:opt_stiff_micro_station_compliance
+#+caption: Identified Compliance of the Micro-Station ([[./figs/opt_stiff_micro_station_compliance.png][png]], [[./figs/opt_stiff_micro_station_compliance.pdf][pdf]])
+[[file:figs/opt_stiff_micro_station_compliance.png]]
+
 ** Identification of the dynamics with a rigid micro-station
-*** Initialization
 #+begin_src matlab :exports none
   initializeReferences();
   initializeDisturbances();
@@ -509,10 +445,9 @@ And we identify the dynamics from forces/torques applied on the micro-hexapod to
 #+end_src
 
 #+begin_src matlab
-  initializeSample('type', 'rigid', 'mass', 20);
+  initializeSample('type', 'rigid', 'mass', 10);
 #+end_src
 
-*** Rigid micro-station
 #+begin_src matlab
   initializeGround('type', 'rigid');
   initializeGranite('type', 'rigid');
@@ -552,7 +487,6 @@ And we identify the dynamics from forces/torques applied on the micro-hexapod to
 #+end_src
 
 ** Identification of the dynamics with a flexible micro-station
-*** Flexible micro-station
 #+begin_src matlab
   initializeGround();
   initializeGranite();
@@ -602,7 +536,7 @@ And we identify the dynamics from forces/torques applied on the micro-hexapod to
   load('mat/optimal_stiffness_micro_station_compliance.mat');
 #+end_src
 
-IFF plant
+The IFF plant only changes when the stiffness is $10^7 [N/m]$ or higher.
 #+begin_src matlab :exports none
   freqs = logspace(-1, 3, 1000);
 
@@ -624,15 +558,18 @@ IFF plant
   hold on;
   for i = 1:length(Ks)
     set(gca,'ColorOrderIndex',i);
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gmr_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-');
+    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmr_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz')))), '-', ...
+         'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
     set(gca,'ColorOrderIndex',i);
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gmf_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '--');
+    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmf_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz')))), '--', ...
+         'HandleVisibility', 'off');
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
   ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
+  ylim([-270, 90]);
-  yticks([-180, -90, 0, 90, 180]);
+  yticks([-360:90:360]);
+  legend('location', 'northwest');
 
   linkaxes([ax1,ax2],'x');
   xlim([freqs(1), freqs(end)]);
@@ -660,15 +597,15 @@ DVF plant
   hold on;
   for i = 1:length(Ks)
     set(gca,'ColorOrderIndex',i);
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gmr_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), '-');
+    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmr_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz')))), '-');
     set(gca,'ColorOrderIndex',i);
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gmf_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), '--');
+    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmf_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz')))), '--');
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
   ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
+  ylim([-270, 90]);
-  yticks([-180, -90, 0, 90, 180]);
+  yticks([-360:90:360]);
 
   linkaxes([ax1,ax2],'x');
   xlim([freqs(1), freqs(end)]);
@@ -696,15 +633,15 @@ X direction
   hold on;
   for i = 1:length(Ks)
     set(gca,'ColorOrderIndex',i);
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-');
+    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz')))), '-');
     set(gca,'ColorOrderIndex',i);
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '--');
+    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz')))), '--');
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
   ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
+  ylim([-270, 90]);
-  yticks([-180, -90, 0, 90, 180]);
+  yticks([-360:90:360]);
 
   linkaxes([ax1,ax2],'x');
   xlim([freqs(1), freqs(end)]);
@@ -732,15 +669,15 @@ Z direction
   hold on;
   for i = 1:length(Ks)
     set(gca,'ColorOrderIndex',i);
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gmr_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '-');
+    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmr_err{i}('Ez', 'Fz'), freqs, 'Hz')))), '-');
     set(gca,'ColorOrderIndex',i);
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gmf_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '--');
+    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmf_err{i}('Ez', 'Fz'), freqs, 'Hz')))), '--');
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
   ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
+  ylim([-270, 90]);
-  yticks([-180, -90, 0, 90, 180]);
+  yticks([-360:90:360]);
 
   linkaxes([ax1,ax2],'x');
   xlim([freqs(1), freqs(end)]);
@@ -888,7 +825,7 @@ We then identify the dynamics for the following payload resonance frequencies =F
        'Gf_iff',    'Gf_dvf',    'Gf_err');
 #+end_src
 
-** Change of optimal gain for decentralized control
+** TODO Change of optimal gain for decentralized control
 For each payload, compute the optimal gain for the IFF control.
 The optimal value corresponds to critical damping to *all* the 6 modes of the nano-hexapod.
 
@@ -920,10 +857,10 @@ Change of Mass
     for j = 1:length(Ms)
       set(gca,'ColorOrderIndex',i);
       if j == 1
-        plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-', ...
+        plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz')))), '-', ...
             'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i)));
       else
-        plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-', ...
+        plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz')))), '-', ...
              'HandleVisibility', 'off');
       end
     end
@@ -931,15 +868,118 @@ Change of Mass
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
   ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
+  ylim([-270, 90]);
-  yticks([-180, -90, 0, 90, 180]);
+  yticks([-360:90:360]);
   legend('location', 'northeast');
 
   linkaxes([ax1,ax2],'x');
   xlim([freqs(1), freqs(end)]);
 #+end_src
 
+Optimal gains:
+#+begin_src matlab
+  opt_gains = [20 60 200 600 2000 6000 20000];
+#+end_src
+
+Change of poles with mass
+#+begin_src matlab :exports none
+  i = 7;
+
+  figure;
+  hold on;
+  for j = 1:length(Ms)
+    set(gca,'ColorOrderIndex',j);
+    cl_poles = pole(feedback(Gm_iff{i,j}, (-gains(k)/s)*eye(6)));
+    plot(real(cl_poles), imag(cl_poles), '.');
+  end
+#+end_src
+
+#+begin_src matlab :exports none
+  i = 4;
+
+  gains = logspace(1, 3, 500);
+
+  figure;
+  hold on;
+  for j = 1:length(Ms)
+    for k = 1:length(gains)
+        set(gca,'ColorOrderIndex',j);
+        cl_poles = pole(feedback(Gm_iff{i,j}, (-gains(k)/s)*eye(6)));
+        poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
+        plot(gains(k)*ones(size(poles_damp)), poles_damp, '.');
+    end
+  end
+  xlabel('Control Gain');
+  ylabel('Damping of the Poles');
+  set(gca, 'XScale', 'log');
+  ylim([0,pi/2]);
+#+end_src
+
+#+begin_src matlab :exports none
+  i = 7;
+
+  gains = logspace(3, 5, 500);
+
+  figure;
+  hold on;
+  for j = 1:length(Ms)
+    for k = 1:length(gains)
+        set(gca,'ColorOrderIndex',j);
+        cl_poles = pole(feedback(Gm_iff{i,j}, (-gains(k)/s)*eye(6)));
+        poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
+        plot(gains(k)*ones(size(poles_damp)), poles_damp, '.');
+    end
+  end
+  xlabel('Control Gain');
+  ylabel('Damping of the Poles');
+  set(gca, 'XScale', 'log');
+  ylim([0,pi/2]);
+#+end_src
+
 Change of payload resonance frequency
+#+begin_src matlab :exports none
+  i = 1;
+
+  gains = logspace(0, 2, 100);
+
+  figure;
+  hold on;
+  for j = 1:length(Fs)
+    for k = 1:length(gains)
+        set(gca,'ColorOrderIndex',j);
+        cl_poles = pole(feedback(Gf_iff{i,j}, (-gains(k)/s)*eye(6)));
+        poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
+        plot(gains(k)*ones(size(poles_damp)), poles_damp, '.');
+    end
+  end
+  xlabel('Control Gain');
+  ylabel('Damping of the Poles');
+  set(gca, 'XScale', 'log');
+  ylim([0,pi/2]);
+#+end_src
+
+#+begin_src matlab :exports none
+  i = 7;
+
+  gains = logspace(3, 5, 100);
+
+  figure;
+  hold on;
+  for j = 1:length(Fs)
+    for k = 1:length(gains)
+        set(gca,'ColorOrderIndex',j);
+        cl_poles = pole(feedback(Gf_iff{i,j}, (-gains(k)/s)*eye(6)));
+        poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
+        plot(gains(k)*ones(size(poles_damp)), poles_damp, '.');
+    end
+  end
+  xlabel('Control Gain');
+  ylabel('Damping of the Poles');
+  set(gca, 'XScale', 'log');
+  ylim([0,pi/2]);
+#+end_src
+
+
 #+begin_src matlab :exports none
   freqs = logspace(-1, 3, 1000);
 
@@ -963,10 +1003,10 @@ Change of payload resonance frequency
     for j = 1:length(Fs)
       set(gca,'ColorOrderIndex',i);
       if j == 1
-        plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-', ...
+        plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz')))), '-', ...
             'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i)));
       else
-        plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-', ...
+        plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz')))), '-', ...
              'HandleVisibility', 'off');
       end
     end
@@ -974,9 +1014,9 @@ Change of payload resonance frequency
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
   ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
+  ylim([-270, 90]);
-  yticks([-180, -90, 0, 90, 180]);
+  yticks([-360:90:360]);
-  legend('location', 'northeast');
+  legend('location', 'southwest');
 
   linkaxes([ax1,ax2],'x');
   xlim([freqs(1), freqs(end)]);
@@ -1341,16 +1381,16 @@ For a soft nano-hexapod
   ax2 = subplot(2, 1, 2);
   hold on;
   for j = 1:length(Fs)
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
+    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
   for j = 1:length(Ms)
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
+    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
   end
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
   ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
+  ylim([-270, 90]);
-  yticks([-180, -90, 0, 90, 180]);
+  yticks([-360:90:360]);
 
   linkaxes([ax1,ax2],'x');
   xlim([freqs(1), freqs(end)]);
@@ -1379,16 +1419,16 @@ For a stiff nano-hexapod
   ax2 = subplot(2, 1, 2);
   hold on;
   for j = 1:length(Fs)
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
+    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
   for j = 1:length(Ms)
-    plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
+    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
   end
   end
   hold off;
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
   ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
-  ylim([-180, 180]);
+  ylim([-270, 90]);
-  yticks([-180, -90, 0, 90, 180]);
+  yticks([-360:90:360]);
 
   linkaxes([ax1,ax2],'x');
   xlim([freqs(1), freqs(end)]);
@@ -1396,3 +1436,179 @@ For a stiff nano-hexapod
 
 ** Conclusion                                                        :ignore:
 * Total Change of dynamics
+#+begin_src matlab :exports none
+  load('mat/optimal_stiffness_Gm_Gf.mat');
+  load('mat/optimal_stiffness_micro_station_compliance.mat');
+  load('mat/optimal_stiffness_Gk_wz.mat');
+#+end_src
+
+- =Gk_wz_err= - Change of spindle rotation speed
+- =Gf_err= - Change of payload resonance
+- =Gm_err= - Change of payload mass
+- =Gmr_err= - Rigid Micro-Station
+- =Gmf_err= - Flexible Micro-Station
+
+Soft nano-hexapod
+#+begin_src matlab :exports none
+  i = 1;
+
+  freqs = logspace(-1, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  % =Gf_err= - Change of payload resonance
+  plot(freqs, abs(squeeze(freqresp(Gf_err{i,1}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
+       'DisplayName', 'Payload Freq');
+  for j = 2:length(Fs)
+    set(gca,'ColorOrderIndex',1);
+    plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
+         'HandleVisibility', 'off');
+  end
+  % =Gm_err= - Change of payload mass
+  set(gca,'ColorOrderIndex',2);
+  plot(freqs, abs(squeeze(freqresp(Gm_err{i,1}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
+       'DisplayName', 'Payload Mass');
+  for j = 2:length(Ms)
+    set(gca,'ColorOrderIndex',2);
+    plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
+         'HandleVisibility', 'off');
+  end
+  % =Gm_err= - Change of payload mass
+  set(gca,'ColorOrderIndex',3);
+  plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,1}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
+       'DisplayName', 'Rotationg Speed');
+  for j = 2:length(Rz_rpm)
+    set(gca,'ColorOrderIndex',3);
+    plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
+         'HandleVisibility', 'off');
+  end
+  set(gca,'ColorOrderIndex',4);
+  % =Gmr_err= - Rigid Micro-Station
+  plot(freqs, abs(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
+       'DisplayName', 'Rigid $\mu$-station');
+  % =Gmf_err= - Flexible Micro-Station
+  plot(freqs, abs(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
+       'DisplayName', 'Flexible $\mu$-station');
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+  legend('location', 'southwest');
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  for j = 1:length(Rz_rpm)
+    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
+  end
+  for j = 1:length(Fs)
+    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
+  end
+  % =Gm_err= - Change of payload mass
+  for j = 1:length(Ms)
+    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
+  end
+  % =Gmr_err= - Rigid Micro-Station
+  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
+  % =Gmf_err= - Flexible Micro-Station
+  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-270, 90]);
+  yticks([-360:90:360]);
+
+  linkaxes([ax1,ax2],'x');
+  xlim([freqs(1), freqs(end)]);
+#+end_src
+
+#+begin_src matlab :exports none
+  i = 1;
+
+  freqs = logspace(-1, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  % =Gf_err= - Change of payload resonance
+  for j = 1:length(Fs)
+    plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
+  end
+  % =Gm_err= - Change of payload mass
+  for j = 1:length(Ms)
+    plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
+  end
+  % Spindle Rotation Speed
+  for j = 1:length(Rz_rpm)
+    plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
+  end
+  % =Gmr_err= - Rigid Micro-Station
+  plot(freqs, abs(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
+  % =Gmf_err= - Flexible Micro-Station
+  plot(freqs, abs(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  % =Gf_err= - Change of payload resonance
+  for j = 1:length(Fs)
+    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
+  end
+  % =Gm_err= - Change of payload mass
+  for j = 1:length(Ms)
+    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
+  end
+  % Spindle Rotation Speed
+  for j = 1:length(Rz_rpm)
+    plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
+  end
+  % =Gmr_err= - Rigid Micro-Station
+  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
+  % =Gmf_err= - Flexible Micro-Station
+  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-270, 90]);
+  yticks([-360:90:360]);
+
+  linkaxes([ax1,ax2],'x');
+  xlim([freqs(1), freqs(end)]);
+#+end_src
+
+Comparison with initial TF
+#+begin_src matlab :exports none
+  i = 1;
+
+  G0 = abs(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz')));
+
+  freqs = logspace(-1, 3, 1000);
+
+  figure;
+
+  hold on;
+  % =Gf_err= - Change of payload resonance
+  for j = 1:length(Fs)
+    plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))./G0, 'k-');
+  end
+  % =Gm_err= - Change of payload mass
+  for j = 1:length(Ms)
+    plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))./G0, 'k-');
+  end
+  % Spindle Rotation Speed
+  for j = 1:length(Rz_rpm)
+    plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))./G0, 'k-');
+  end
+  % =Gmf_err= - Flexible Micro-Station
+  plot(freqs, abs(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz')))./G0, 'k-');
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+  xlim([freqs(1), freqs(end)]);
+  ylim([1e-2, 1e2])
+#+end_src
+
+- [ ] Make a gif with the stiffness varying

org/simscape_subsystems.org

@@ -1593,7 +1593,7 @@ The =controller= structure is saved.
       % Period of the displacement [s]
       args.Ry_period    (1,1) double {mustBeNumeric, mustBePositive} = 1
       % Either "constant" / "rotating"
-      args.Rz_type      char {mustBeMember(args.Rz_type,{'constant', 'rotating'})} = 'constant'
+      args.Rz_type      char {mustBeMember(args.Rz_type,{'constant', 'rotating', 'rotating-not-filtered'})} = 'constant'
       % Initial angle [rad]
       args.Rz_amplitude (1,1) double {mustBeNumeric} = 0
       % Period of the rotating [s]
@@ -1721,6 +1721,17 @@ The =controller= structure is saved.
       Rz(:)   = args.Rz_amplitude;
       Rzd(:)  = 0;
       Rzdd(:) = 0;
+    case 'rotating-not-filtered'
+      Rz(:) = 2*pi/args.Rz_period*t;
+
+      % The signal is filtered out
+      Rz(:)   = 2*pi/args.Rz_period*t;
+      Rzd(:)  = 2*pi/args.Rz_period;
+      Rzdd(:) = 0;
+
+      % We add the angle offset
+      Rz = Rz + args.Rz_amplitude;
+
     case 'rotating'
       Rz(:) = 2*pi/args.Rz_period*t;
 

src/initializeReferences.m

@@ -18,7 +18,7 @@ arguments
     % Period of the displacement [s]
     args.Ry_period    (1,1) double {mustBeNumeric, mustBePositive} = 1
     % Either "constant" / "rotating"
-    args.Rz_type      char {mustBeMember(args.Rz_type,{'constant', 'rotating'})} = 'constant'
+    args.Rz_type      char {mustBeMember(args.Rz_type,{'constant', 'rotating', 'rotating-not-filtered'})} = 'constant'
     % Initial angle [rad]
     args.Rz_amplitude (1,1) double {mustBeNumeric} = 0
     % Period of the rotating [s]
@@ -121,6 +121,17 @@ switch args.Rz_type
     Rz(:)   = args.Rz_amplitude;
     Rzd(:)  = 0;
     Rzdd(:) = 0;
+  case 'rotating-not-filtered'
+    Rz(:) = 2*pi/args.Rz_period*t;
+
+    % The signal is filtered out
+    Rz(:)   = 2*pi/args.Rz_period*t;
+    Rzd(:)  = 2*pi/args.Rz_period;
+    Rzdd(:) = 0;
+
+    % We add the angle offset
+    Rz = Rz + args.Rz_amplitude;
+
   case 'rotating'
     Rz(:) = 2*pi/args.Rz_period*t;