Add simulation of HAC-DVF with opt-stiffness

org/experiments.org

@@ -202,7 +202,7 @@ And we save the obtained data.
 In this section, we also perform a tomography experiment with the sample's center of mass aligned with the rotation axis.
 However this time, we include perturbations such as ground motion and stage vibrations.
 
-** TODO Simulation Setup
+** Simulation Setup
 We now activate the disturbances.
 #+begin_src matlab
   initializeDisturbances(...

org/optimal_stiffness_control.org

@@ -678,6 +678,11 @@ exportFig('figs/opt_stiff_sensibility_dist_dvf.pdf', 'width', 'full', 'height',
 #+RESULTS:
 [[file:figs/opt_stiff_sensibility_dist_dvf.png]]
 
+** Conclusion
+#+begin_important
+
+#+end_important
+
 * Primary Control in the leg space
 <<sec:primary_control_L>>
 ** Introduction                                                      :ignore:
@@ -691,7 +696,7 @@ In this section we implement the control architecture shown in Figure [[fig:cont
 
 The controller for decentralized direct velocity feedback is the one designed in Section [[sec:lac_dvf]].
 
-** Plant in the task space
+** Plant in the leg space
 We now loop at the transfer function matrix from $\bm{\tau}^\prime$ to $\bm{\epsilon}_{\mathcal{X}_n}$ for the design of $\bm{K}_\mathcal{L}$.
 
 The diagonal elements of the transfer function matrix from $\bm{\tau}^\prime$ to $\bm{\epsilon}_{\mathcal{X}_n}$ for the three considered masses are shown in Figure [[fig:opt_stiff_primary_plant_L]].
@@ -742,6 +747,37 @@ exportFig('figs/opt_stiff_primary_plant_L.pdf', 'width', 'full', 'height', 'full
 #+RESULTS:
 [[file:figs/opt_stiff_primary_plant_L.png]]
 
+
+#+begin_src matlab :exports none
+  c1 = [     0    0.4470    0.7410 0.2]; % Blue
+  c2 = [0.8500    0.3250    0.0980 0.2]; % Orange
+  c3 = [0.9290    0.6940    0.1250 0.2]; % Yellow
+  c4 = [0.4940    0.1840    0.5560 0.2]; % Purple
+  c5 = [0.4660    0.6740    0.1880 0.2]; % Green
+  c6 = [0.3010    0.7450    0.9330 0.2]; % Light Blue
+  c7 = [0.6350    0.0780    0.1840 0.2]; % Red
+  colors = [c1; c2; c3; c4; c5; c6; c7];
+
+  freqs = logspace(0, 3, 1000);
+
+  figure;
+
+  hold on;
+  for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
+      plot(freqs, abs(squeeze(freqresp(Gm_l{i}(1, 1), freqs, 'Hz'))), '-');
+      for j = 1:5
+          for k = j+1:6
+              plot(freqs, abs(squeeze(freqresp(Gm_l{i}(j, k), freqs, 'Hz'))), '--', 'color', colors(i, :));
+          end
+      end
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+  ylim([1e-9, inf]);
+#+end_src
+
 ** Control in the leg space
 We design a diagonal controller with all the same diagonal elements.
 
@@ -759,19 +795,14 @@ The design controller is as follows:
 The loop gain is shown in Figure [[fig:opt_stiff_primary_loop_gain_L]].
 
 #+begin_src matlab
-  h = 2.5;
+  h = 2.0;
   Kl = 2e7 * eye(6) * ...
+       1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
        1/h*(s/(2*pi*200/h) + 1)/(s/(2*pi*200*h) + 1) * ...
        (s/2/pi/10 + 1)/(s/2/pi/10) * ...
        1/(1 + s/2/pi/300);
 #+end_src
 
-#+begin_src matlab :exports none
-  for i = 1:length(Ms)
-      isstable(feedback(Gm_l{i}(1,1)*Kl(1,1), 1, -1))
-  end
-#+end_src
-
 #+begin_src matlab :exports none
   freqs = logspace(0, 3, 1000);
 
@@ -817,7 +848,14 @@ exportFig('figs/opt_stiff_primary_loop_gain_L.pdf', 'width', 'full', 'height', '
 
 #+begin_src matlab
   load('mat/stages.mat', 'nano_hexapod');
-  K = Kl*nano_hexapod.J;
+  K = Kl*nano_hexapod.J*diag([1, 1, 1, 1, 1, 0]);
+#+end_src
+
+Check the MIMO stability
+#+begin_src matlab :exports none
+  for i = 1:length(Ms)
+      isstable(feedback(nano_hexapod.J\Gm_l{i}*K, eye(6), -1))
+  end
 #+end_src
 
 ** Sensibility to Disturbances and Noise Budget
@@ -1014,18 +1052,21 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
 [[file:figs/opt_stiff_primary_control_L_cas_tot.png]]
 
 ** Simulations
+Let's now simulate a tomography experiment.
+To do so, we include all disturbances except vibrations of the translation stage.
 #+begin_src matlab
   initializeDisturbances('Fty_x', false, 'Fty_z', false);
   initializeSimscapeConfiguration('gravity', false);
   initializeLoggingConfiguration('log', 'all');
 #+end_src
 
-#+begin_src matlab
+#+begin_src matlab :exports none
   load('mat/conf_simulink.mat');
   set_param(conf_simulink, 'StopTime', '2');
 #+end_src
 
-#+begin_src matlab
+And we run the simulation for all three payload Masses.
+#+begin_src matlab :exports none
   hac_dvf_L = {zeros(length(Ms)), 1};
 
   for i = 1:length(Ms)
@@ -1037,21 +1078,27 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
   end
 #+end_src
 
-#+begin_src matlab
+#+begin_src matlab :exports none
   save('./mat/tomo_exp_hac_dvf.mat', 'hac_dvf_L');
 #+end_src
 
 ** Results
-#+begin_src matlab
+#+begin_src matlab :exports none
   load('./mat/experiment_tomography.mat', 'tomo_align_dist');
+  load('./mat/tomo_exp_hac_dvf.mat', 'hac_dvf_L');
 #+end_src
 
-#+begin_src matlab
+Let's now see how this controller performs.
+
+First, we compute the Power Spectral Density of the sample's position error and we compare it with the open loop case in Figure [[fig:opt_stiff_hac_dvf_L_psd_disp_error]].
+
+Similarly, the Cumulative Amplitude Spectrum is shown in Figure [[fig:opt_stiff_hac_dvf_L_cas_disp_error]].
+
+Finally, the time domain position error signals are shown in Figure [[fig:opt_stiff_hac_dvf_L_pos_error]].
+#+begin_src matlab :exports none
   n_av = 4;
   han_win = hanning(ceil(length(simout.Em.En.Data(:,1))/n_av));
-#+end_src
 
-#+begin_src matlab
   t = simout.Em.En.Time;
   Ts = t(2)-t(1);
 
@@ -1068,8 +1115,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
   figure;
   ax1 = subplot(2, 3, 1);
   hold on;
-  plot(f, sqrt(pxx_ol(:, 1)))
+  plot(f, sqrt(pxx_ol(:, 1)), 'k-')
   for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
       plot(f, sqrt(pxx_dvf_L(:, 1, i)))
   end
   hold off;
@@ -1079,8 +1127,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
 
   ax2 = subplot(2, 3, 2);
   hold on;
-  plot(f, sqrt(pxx_ol(:, 2)))
+  plot(f, sqrt(pxx_ol(:, 2)), 'k-')
   for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
       plot(f, sqrt(pxx_dvf_L(:, 2, i)))
   end
   hold off;
@@ -1090,8 +1139,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
 
   ax3 = subplot(2, 3, 3);
   hold on;
-  plot(f, sqrt(pxx_ol(:, 3)))
+  plot(f, sqrt(pxx_ol(:, 3)), 'k-')
   for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
       plot(f, sqrt(pxx_dvf_L(:, 3, i)))
   end
   hold off;
@@ -1101,8 +1151,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
 
   ax4 = subplot(2, 3, 4);
   hold on;
-  plot(f, sqrt(pxx_ol(:, 4)))
+  plot(f, sqrt(pxx_ol(:, 4)), 'k-')
   for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
       plot(f, sqrt(pxx_dvf_L(:, 4, i)))
   end
   hold off;
@@ -1112,8 +1163,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
 
   ax5 = subplot(2, 3, 5);
   hold on;
-  plot(f, sqrt(pxx_ol(:, 5)))
+  plot(f, sqrt(pxx_ol(:, 5)), 'k-')
   for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
       plot(f, sqrt(pxx_dvf_L(:, 5, i)))
   end
   hold off;
@@ -1123,8 +1175,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
 
   ax6 = subplot(2, 3, 6);
   hold on;
-  plot(f, sqrt(pxx_ol(:, 6)), 'DisplayName', '$\mu$-Station')
+  plot(f, sqrt(pxx_ol(:, 6)), 'k-', 'DisplayName', '$\mu$-Station')
   for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
       plot(f, sqrt(pxx_dvf_L(:, 6, i)), ...
            'DisplayName', sprintf('HAC-DVF $m = %.0f kg$', Ms(i)))
   end
@@ -1138,12 +1191,22 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
   xlim([f(2), f(end)])
 #+end_src
 
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/opt_stiff_hac_dvf_L_psd_disp_error.pdf', 'width', 'full', 'height', 'full')
+#+end_src
+
+#+name: fig:opt_stiff_hac_dvf_L_psd_disp_error
+#+caption: Amplitude Spectral Density of the position error in Open Loop and with the HAC-LAC controller
+#+RESULTS:
+[[file:figs/opt_stiff_hac_dvf_L_psd_disp_error.png]]
+
 #+begin_src matlab :exports none
   figure;
   ax1 = subplot(2, 3, 1);
   hold on;
-  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 1))))))
+  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 1))))), 'k-')
   for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
       plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 1, i))))));
   end
   hold off;
@@ -1154,8 +1217,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
 
   ax2 = subplot(2, 3, 2);
   hold on;
-  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 2))))))
+  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 2))))), 'k-')
   for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
       plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 2, i))))));
   end
   hold off;
@@ -1166,8 +1230,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
 
   ax3 = subplot(2, 3, 3);
   hold on;
-  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 3))))))
+  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 3))))), 'k-')
   for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
       plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 3, i))))));
   end
   hold off;
@@ -1178,8 +1243,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
 
   ax4 = subplot(2, 3, 4);
   hold on;
-  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 4))))))
+  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 4))))), 'k-')
   for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
       plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 4, i))))));
   end
   hold off;
@@ -1190,8 +1256,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
 
   ax5 = subplot(2, 3, 5);
   hold on;
-  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 5))))))
+  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 5))))), 'k-')
   for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
       plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 5, i))))));
   end
   hold off;
@@ -1202,8 +1269,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
 
   ax6 = subplot(2, 3, 6);
   hold on;
-  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 6))))), 'DisplayName', '$\mu$-Station')
+  plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 6))))), 'k-', 'DisplayName', '$\mu$-Station')
   for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
       plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 6, i))))), ...
            'DisplayName', sprintf('HAC-DVF $m = %.0f kg$', Ms(i)));
   end
@@ -1218,12 +1286,22 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
   xlim([f(2), f(end)])
 #+end_src
 
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/opt_stiff_hac_dvf_L_cas_disp_error.pdf', 'width', 'full', 'height', 'full')
+#+end_src
+
+#+name: fig:opt_stiff_hac_dvf_L_cas_disp_error
+#+caption: Cumulative Amplitude Spectrum of the position error in Open Loop and with the HAC-LAC controller
+#+RESULTS:
+[[file:figs/opt_stiff_hac_dvf_L_cas_disp_error.png]]
+
 #+begin_src matlab :exports none
   figure;
   ax1 = subplot(2, 3, 1);
   hold on;
-  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 1))
+  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 1), 'k-')
   for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
       plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 1));
   end
   hold off;
@@ -1232,8 +1310,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
 
   ax2 = subplot(2, 3, 2);
   hold on;
-  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 2))
+  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 2), 'k-')
   for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
       plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 2));
   end
   hold off;
@@ -1242,8 +1321,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
 
   ax3 = subplot(2, 3, 3);
   hold on;
-  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 3))
+  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 3), 'k-')
   for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
       plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 3));
   end
   hold off;
@@ -1252,8 +1332,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
 
   ax4 = subplot(2, 3, 4);
   hold on;
-  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 4))
+  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 4), 'k-')
   for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
       plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 4));
   end
   hold off;
@@ -1262,8 +1343,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
 
   ax5 = subplot(2, 3, 5);
   hold on;
-  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 5))
+  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 5), 'k-')
   for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
       plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 5));
   end
   hold off;
@@ -1272,9 +1354,10 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
 
   ax6 = subplot(2, 3, 6);
   hold on;
-  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 6), ...
+  plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 6), 'k-', ...
-        'DisplayName', '$\mu$-Station')
+        'DisplayName', '$\mu$-Station');
   for i = 1:length(Ms)
+      set(gca,'ColorOrderIndex',i);
       plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 6), ...
            'DisplayName', sprintf('HAC-DVF $m = %.0f kg$', Ms(i)));
   end
@@ -1286,9 +1369,31 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
   linkaxes([ax1,ax2,ax3,ax4],'x');
   xlim([0.5, inf]);
 #+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+exportFig('figs/opt_stiff_hac_dvf_L_pos_error.pdf', 'width', 'full', 'height', 'full')
+#+end_src
+
+#+name: fig:opt_stiff_hac_dvf_L_pos_error
+#+caption: Position Error of the sample during a tomography experiment when no control is applied and with the HAC-DVF control architecture
+#+RESULTS:
+[[file:figs/opt_stiff_hac_dvf_L_pos_error.png]]
+
+** Conclusion
+#+begin_important
+
+#+end_important
+
 * Primary Control in the task space
 <<sec:primary_control_X>>
 ** Introduction                                                      :ignore:
+In this section, the control architecture shown in Figure [[fig:control_architecture_hac_dvf_pos_X]] is applied and consists of:
+- an inner Low Authority Control loop consisting of a decentralized direct velocity control controller
+- an outer loop with the primary controller $\bm{K}_\mathcal{X}$ designed in the task space
+
+#+name: fig:control_architecture_hac_dvf_pos_X
+#+caption: HAC-LAC architecture
+[[file:figs/control_architecture_hac_dvf_pos_X.png]]
 
 ** Plant in the task space
 Let's look $\bm{G}_\mathcal{X}(s)$.
@@ -1572,3 +1677,7 @@ Let's look $\bm{G}_\mathcal{X}(s)$.
 #+end_src
 
 ** Simulation
+** Conclusion
+#+begin_important
+
+#+end_important