#+TITLE: Amplified Piezoelectric Stack Actuator #+SETUPFILE: ./setup/org-setup-file.org * Introduction :ignore: The presented model is based on cite:souleille18_concep_activ_mount_space_applic. The model represents the amplified piezo APA100M from Cedrat-Technologies (Figure [[fig:souleille18_model_piezo]]). The parameters are shown in the table below. #+name: fig:souleille18_model_piezo #+caption: Picture of an APA100M from Cedrat Technologies. Simplified model of a one DoF payload mounted on such isolator [[file:./figs/souleille18_model_piezo.png]] #+caption: Parameters used for the model of the APA 100M | | Value | Meaning | |-------+-------------------+----------------------------------------------------------------| | $m$ | $1\,[kg]$ | Payload mass | | $k_e$ | $4.8\,[N/\mu m]$ | Stiffness used to adjust the pole of the isolator | | $k_1$ | $0.96\,[N/\mu m]$ | Stiffness of the metallic suspension when the stack is removed | | $k_a$ | $65\,[N/\mu m]$ | Stiffness of the actuator | | $c_1$ | $10\,[N/(m/s)]$ | Added viscous damping | * Simplified Model ** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) <> #+end_src #+begin_src matlab :exports none :results silent :noweb yes <> #+end_src #+BEGIN_SRC matlab simulinkproject('../'); #+END_SRC #+begin_src matlab open 'amplified_piezo_model.slx' #+end_src ** Parameters #+begin_src matlab m = 1; % [kg] ke = 4.8e6; % [N/m] ce = 5; % [N/(m/s)] me = 0.001; % [kg] k1 = 0.96e6; % [N/m] c1 = 10; % [N/(m/s)] ka = 65e6; % [N/m] ca = 5; % [N/(m/s)] ma = 0.001; % [kg] h = 0.2; % [m] #+end_src IFF Controller: #+begin_src matlab Kiff = -8000/s; #+end_src ** Identification Identification in open-loop. #+begin_src matlab %% Name of the Simulink File mdl = 'amplified_piezo_model'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/w'], 1, 'openinput'); io_i = io_i + 1; % Base Motion io(io_i) = linio([mdl, '/f'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % External Force io(io_i) = linio([mdl, '/Fs'], 3, 'openoutput'); io_i = io_i + 1; % Force Sensors io(io_i) = linio([mdl, '/x1'], 1, 'openoutput'); io_i = io_i + 1; % Mass displacement G = linearize(mdl, io, 0); G.InputName = {'w', 'f', 'F'}; G.OutputName = {'Fs', 'x1'}; #+end_src Identification in closed-loop. #+begin_src matlab %% Name of the Simulink File mdl = 'amplified_piezo_model'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/w'], 1, 'input'); io_i = io_i + 1; % Base Motion io(io_i) = linio([mdl, '/f'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs io(io_i) = linio([mdl, '/F'], 1, 'input'); io_i = io_i + 1; % External Force io(io_i) = linio([mdl, '/Fs'], 3, 'output'); io_i = io_i + 1; % Force Sensors io(io_i) = linio([mdl, '/x1'], 1, 'output'); io_i = io_i + 1; % Mass displacement Giff = linearize(mdl, io, 0); Giff.InputName = {'w', 'f', 'F'}; Giff.OutputName = {'Fs', 'x1'}; #+end_src #+begin_src matlab :exports none freqs = logspace(1, 3, 1000); figure; ax1 = subplot(2, 3, 1); title('$\displaystyle \frac{x_1}{w}$') hold on; plot(freqs, abs(squeeze(freqresp(G('x1', 'w'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Giff('x1', 'w'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/m]');xlabel('Frequency [Hz]'); ax2 = subplot(2, 3, 2); title('$\displaystyle \frac{x_1}{f}$') hold on; plot(freqs, abs(squeeze(freqresp(G('x1', 'f'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Giff('x1', 'f'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]'); ax3 = subplot(2, 3, 3); title('$\displaystyle \frac{x_1}{F}$') hold on; plot(freqs, abs(squeeze(freqresp(G('x1', 'F'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Giff('x1', 'F'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]'); ax4 = subplot(2, 3, 4); title('$\displaystyle \frac{F_s}{w}$') hold on; plot(freqs, abs(squeeze(freqresp(G('Fs', 'w'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Giff('Fs', 'w'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/m]');xlabel('Frequency [Hz]'); ax5 = subplot(2, 3, 5); title('$\displaystyle \frac{F_s}{f}$') hold on; plot(freqs, abs(squeeze(freqresp(G('Fs', 'f'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Giff('Fs', 'f'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]'); ax6 = subplot(2, 3, 6); title('$\displaystyle \frac{F_s}{F}$') hold on; plot(freqs, abs(squeeze(freqresp(G('Fs', 'F'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Giff('Fs', 'F'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]'); linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x'); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/amplified_piezo_tf_ol_and_cl.pdf', 'width', 'full', 'height', 'full'); #+end_src #+name: fig:amplified_piezo_tf_ol_and_cl #+caption: Matrix of transfer functions from input to output in open loop (blue) and closed loop (red) #+RESULTS: [[file:figs/amplified_piezo_tf_ol_and_cl.png]] ** Root Locus #+begin_src matlab :exports none :post figure; gains = logspace(1, 6, 500); hold on; plot(real(pole(G('Fs', 'f'))), imag(pole(G('Fs', 'f'))), 'kx'); plot(real(tzero(G('Fs', 'f'))), imag(tzero(G('Fs', 'f'))), 'ko'); for k = 1:length(gains) cl_poles = pole(feedback(G('Fs', 'f'), -gains(k)/s)); plot(real(cl_poles), imag(cl_poles), 'k.'); end hold off; axis square; xlim([-2500, 100]); ylim([0, 2600]); xlabel('Real Part'); ylabel('Imaginary Part'); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/amplified_piezo_root_locus.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:amplified_piezo_root_locus #+caption: Root Locus #+RESULTS: [[file:figs/amplified_piezo_root_locus.png]] ** Analytical Model If we apply the Newton's second law of motion on the top mass, we obtain: \[ ms^2 x_1 = F + k_1 (w - x_1) + k_e (x_e - x_1) \] Then, we can write that the measured force $F_s$ is equal to: \[ F_s = k_a(w - x_e) + f = -k_e (x_1 - x_e) \] which gives: \[ x_e = \frac{k_a}{k_e + k_a} w + \frac{1}{k_e + k_a} f + \frac{k_e}{k_e + k_a} x_1 \] Re-injecting that into the previous equations gives: \[ x_1 = F \frac{1}{ms^2 + k_1 + \frac{k_e k_a}{k_e + k_a}} + w \frac{k_1 + \frac{k_e k_a}{k_e + k_a}}{ms^2 + k_1 + \frac{k_e k_a}{k_e + k_a}} + f \frac{\frac{k_e}{k_e + k_a}}{ms^2 + k_1 + \frac{k_e k_a}{k_e + k_a}} \] \[ F_s = - F \frac{\frac{k_e k_a}{k_e + k_a}}{ms^2 + k_1 + \frac{k_e k_a}{k_e + k_a}} + w \frac{k_e k_a}{k_e + k_a} \Big( \frac{ms^2}{ms^2 + k_1 + \frac{k_e k_a}{k_e + k_a}} \Big) - f \frac{k_e}{k_e + k_a} \Big( \frac{ms^2 + k_1}{ms^2 + k_1 + \frac{k_e k_a}{k_e + k_a}} \Big) \] #+begin_src matlab Ga = 1/(m*s^2 + k1 + ke*ka/(ke + ka)) * ... [ 1 , k1 + ke*ka/(ke + ka) , ke/(ke + ka) ; -ke*ka/(ke + ka), ke*ka/(ke + ka)*m*s^2 , -ke/(ke+ka)*(m*s^2 + k1)]; Ga.InputName = {'F', 'w', 'f'}; Ga.OutputName = {'x1', 'Fs'}; #+end_src #+begin_src matlab :exports none freqs = logspace(1, 4, 1000); figure; ax1 = subplot(2, 3, 1); title('$\displaystyle \frac{x_1}{w}$') hold on; plot(freqs, abs(squeeze(freqresp(G('x1', 'w'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Ga('x1', 'w'), freqs, 'Hz'))), 'k--'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/m]');xlabel('Frequency [Hz]'); ax2 = subplot(2, 3, 2); title('$\displaystyle \frac{x_1}{f}$') hold on; plot(freqs, abs(squeeze(freqresp(G('x1', 'f'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Ga('x1', 'f'), freqs, 'Hz'))), 'k--'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]'); ax3 = subplot(2, 3, 3); title('$\displaystyle \frac{x_1}{F}$') hold on; plot(freqs, abs(squeeze(freqresp(G('x1', 'F'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Ga('x1', 'F'), freqs, 'Hz'))), 'k--'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]'); ax4 = subplot(2, 3, 4); title('$\displaystyle \frac{F_s}{w}$') hold on; plot(freqs, abs(squeeze(freqresp(G('Fs', 'w'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Ga('Fs', 'w'), freqs, 'Hz'))), 'k--'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/m]');xlabel('Frequency [Hz]'); ax5 = subplot(2, 3, 5); title('$\displaystyle \frac{F_s}{f}$') hold on; plot(freqs, abs(squeeze(freqresp(G('Fs', 'f'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Ga('Fs', 'f'), freqs, 'Hz'))), 'k--'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]'); ax6 = subplot(2, 3, 6); title('$\displaystyle \frac{F_s}{F}$') hold on; plot(freqs, abs(squeeze(freqresp(G('Fs', 'F'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Ga('Fs', 'F'), freqs, 'Hz'))), 'k--'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]'); linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x'); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/comp_simscape_analytical.pdf', 'width', 'full', 'height', 'full'); #+end_src #+name: fig:comp_simscape_analytical #+caption: Comparison of the Identified Simscape Dynamics (solid) and the Analytical Model (dashed) #+RESULTS: [[file:figs/comp_simscape_analytical.png]] ** Analytical Analysis For Integral Force Feedback Control, the plant is: \[ \frac{F_s}{f} = \frac{k_e}{k_e + k_a} \Big( \frac{ms^2 + k_1}{ms^2 + k_1 + \frac{k_e k_a}{k_e + k_a}} \Big) \] As high frequency, this converge to: \[ \frac{F_s}{f} \underset{\omega\to\infty}{\longrightarrow} \frac{k_e}{k_e + k_a} \] And at low frequency: \[ \frac{F_s}{f} \underset{\omega\to 0}{\longrightarrow} \frac{k_e}{k_e + k_a} \frac{k_1}{k_1 + \frac{k_e k_a}{k_e + k_a}} \] It has two complex conjugate zeros at: \[ z = \pm j \sqrt{\frac{k_1}{m}} \] And two complex conjugate poles at: \[ p = \pm j \sqrt{\frac{k_1 + \frac{k_e k_a}{k_e + k_a}}{m}} \] If maximal damping is to be attained with IFF, the distance between the zero and the pole is to be maximized. Thus, we wish to maximize $p/z$, which is equivalent as to minimize $k_1$ and have $k_e \approx k_a$ (supposing $k_e + k_a \approx \text{cst}$). #+begin_src matlab m = 1; k1 = 1e6; ka = 1e6; ke = 1e6; Giff.InputName = {'f'}; Giff.OutputName = {'Fs'}; #+end_src #+begin_src matlab :exports none m = 1; ka = 1e6; ke = 1e6; freqs = logspace(-1, 3, 1000); figure; hold on; for k1 = [1e5, 1e6, 1e7, 1e8] Giff = 1/(m*s^2 + k1 + ke*ka/(ke + ka)) * (-ke/(ke+ka))*(m*s^2 + k1); plot(freqs, abs(squeeze(freqresp(Giff, freqs, 'Hz'))), ... 'DisplayName', sprintf('$k_1 = %.1e$', k1)); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [N/N]'); xlabel('Frequency [Hz]'); legend('location', 'northeast'); #+end_src #+begin_src matlab :exports none m = 1; k1 = 1e6; ke = 1e6; freqs = logspace(-1, 3, 1000); figure; hold on; for ka = [1e5, 1e6, 1e7, 1e8] Giff = 1/(m*s^2 + k1 + ke*ka/(ke + ka)) * (-ke/(ke+ka))*(m*s^2 + k1); plot(freqs, abs(squeeze(freqresp(Giff, freqs, 'Hz'))), ... 'DisplayName', sprintf('$k_a = %.1e$', ka)); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [N/N]'); xlabel('Frequency [Hz]'); legend('location', 'northeast'); #+end_src #+begin_src matlab :exports none m = 1; k1 = 1e6; ka = 1e6; freqs = logspace(-1, 3, 1000); figure; hold on; for ke = [1e5, 1e6, 1e7, 1e8] Giff = 1/(m*s^2 + k1 + ke*ka/(ke + ka)) * (-ke/(ke+ka))*(m*s^2 + k1); plot(freqs, abs(squeeze(freqresp(Giff, freqs, 'Hz'))), ... 'DisplayName', sprintf('$k_e = %.1e$', ke)); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [N/N]'); xlabel('Frequency [Hz]'); legend('location', 'northeast'); #+end_src * Rotating X-Y platform ** Introduction :ignore: This analysis gave rise to a paper cite:dehaeze20_activ_dampin_rotat_platf_integ_force_feedb. ** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) <> #+end_src #+begin_src matlab :exports none :results silent :noweb yes <> #+end_src #+BEGIN_SRC matlab simulinkproject('../'); #+END_SRC #+begin_src matlab open 'amplified_piezo_xy_rotating_stage.slx' #+end_src ** Parameters #+begin_src matlab m = 1; % [kg] ke = 4.8e6; % [N/m] ce = 5; % [N/(m/s)] me = 0.001; % [kg] k1 = 0.96e6; % [N/m] c1 = 10; % [N/(m/s)] ka = 65e6; % [N/m] ca = 5; % [N/(m/s)] ma = 0.001; % [kg] h = 0.2; % [m] #+end_src #+begin_src matlab Kiff = tf(0); #+end_src ** Identification Rotating speed in rad/s: #+begin_src matlab Ws = 2*pi*[0, 1, 10, 100]; #+end_src #+begin_src matlab Gs = {zeros(length(Ws), 1)}; #+end_src Identification in open-loop. #+begin_src matlab %% Name of the Simulink File mdl = 'amplified_piezo_xy_rotating_stage'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/fx'], 1, 'openinput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/fy'], 1, 'openinput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/Fs'], 1, 'openoutput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/Fs'], 2, 'openoutput'); io_i = io_i + 1; for i = 1:length(Ws) ws = Ws(i); G = linearize(mdl, io, 0); G.InputName = {'fx', 'fy'}; G.OutputName = {'Fsx', 'Fsy'}; Gs(i) = {G}; end #+end_src #+begin_src matlab :exports none freqs = logspace(1, 3, 1000); figure; ax1 = subplot(2, 2, 1); title('$\displaystyle \frac{F_{s,x}}{f_x}$') hold on; for i = 1:length(Ws) plot(freqs, abs(squeeze(freqresp(Gs{i}('Fsx', 'fx'), freqs, 'Hz')))); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/m]');xlabel('Frequency [Hz]'); ax2 = subplot(2, 2, 2); title('$\displaystyle \frac{F_{s,y}}{f_x}$') hold on; for i = 1:length(Ws) plot(freqs, abs(squeeze(freqresp(Gs{i}('Fsy', 'fx'), freqs, 'Hz')))); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]'); ax3 = subplot(2, 2, 3); title('$\displaystyle \frac{F_{s,x}}{f_y}$') hold on; for i = 1:length(Ws) plot(freqs, abs(squeeze(freqresp(Gs{i}('Fsx', 'fy'), freqs, 'Hz')))); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]'); ax4 = subplot(2, 2, 4); title('$\displaystyle \frac{F_{s,y}}{f_y}$') hold on; for i = 1:length(Ws) plot(freqs, abs(squeeze(freqresp(Gs{i}('Fsy', 'fy'), freqs, 'Hz')))); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/m]');xlabel('Frequency [Hz]'); linkaxes([ax1,ax2,ax3,ax4],'x'); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/amplitifed_piezo_xy_rotation_plant_iff.pdf', 'width', 'full', 'height', 'full'); #+end_src #+name: fig:amplitifed_piezo_xy_rotation_plant_iff #+caption: Transfer function matrix from forces to force sensors for multiple rotation speed #+RESULTS: [[file:figs/amplitifed_piezo_xy_rotation_plant_iff.png]] ** Root Locus #+begin_src matlab :exports none :post figure; gains = logspace(1, 6, 500); hold on; for i = 1:length(Ws) set(gca,'ColorOrderIndex',i); plot(real(pole(Gs{i})), imag(pole(Gs{i})), 'x'); set(gca,'ColorOrderIndex',i); plot(real(tzero(Gs{i})), imag(tzero(Gs{i})), 'o'); for k = 1:length(gains) set(gca,'ColorOrderIndex',i); cl_poles = pole(feedback(Gs{i}, -gains(k)/s*eye(2))); plot(real(cl_poles), imag(cl_poles), '.'); end end hold off; axis square; xlim([-2900, 100]); ylim([0, 3000]); xlabel('Real Part'); ylabel('Imaginary Part'); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/amplified_piezo_xy_rotation_root_locus.pdf', 'width', 'tall', 'height', 'wide'); #+end_src #+name: fig:amplified_piezo_xy_rotation_root_locus #+caption: Root locus for 3 rotating speed #+RESULTS: [[file:figs/amplified_piezo_xy_rotation_root_locus.png]] ** Analysis The negative stiffness induced by the rotation is equal to $m \omega_0^2$. Thus, the maximum rotation speed where IFF can be applied is: \[ \omega_\text{max} = \sqrt{\frac{k_1}{m}} \approx 156\,[Hz] \] Let's verify that. #+begin_src matlab Ws = 2*pi*[140, 160]; #+end_src #+begin_src matlab :exports none Gs = {zeros(length(Ws), 1)}; #+end_src Identification #+begin_src matlab %% Name of the Simulink File mdl = 'amplified_piezo_xy_rotating_stage'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/fx'], 1, 'openinput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/fy'], 1, 'openinput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/Fs'], 1, 'openoutput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/Fs'], 2, 'openoutput'); io_i = io_i + 1; for i = 1:length(Ws) ws = Ws(i); G = linearize(mdl, io, 0); G.InputName = {'fx', 'fy'}; G.OutputName = {'Fsx', 'Fsy'}; Gs(i) = {G}; end #+end_src #+begin_src matlab :exports none figure; gains = logspace(1, 6, 500); hold on; for i = 1:length(Ws) set(gca,'ColorOrderIndex',i); plot(real(pole(Gs{i})), imag(pole(Gs{i})), 'x'); set(gca,'ColorOrderIndex',i); plot(real(tzero(Gs{i})), imag(tzero(Gs{i})), 'o'); for k = 1:length(gains) set(gca,'ColorOrderIndex',i); cl_poles = pole(feedback(Gs{i}, -gains(k)/s*eye(2))); plot(real(cl_poles), imag(cl_poles), '.'); end end hold off; axis square; xlim([-100, 50]); ylim([0, 150]); xlabel('Real Part'); ylabel('Imaginary Part'); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/amplified_piezo_xy_rotating_unstable_root_locus.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:amplified_piezo_xy_rotating_unstable_root_locus #+caption: Root Locus for the two considered rotation speed. For the red curve, the system is unstable. #+RESULTS: [[file:figs/amplified_piezo_xy_rotating_unstable_root_locus.png]] * Stewart Platform with Amplified Actuators ** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) <> #+end_src #+begin_src matlab :exports none :results silent :noweb yes <> #+end_src #+begin_src matlab :tangle no simulinkproject('../'); #+end_src #+begin_src matlab open('nass_model.slx') #+end_src ** Initialization #+begin_src matlab initializeGround(); initializeGranite(); initializeTy(); initializeRy(); initializeRz(); initializeMicroHexapod(); initializeAxisc(); initializeMirror(); initializeSimscapeConfiguration(); initializeDisturbances('enable', false); initializeLoggingConfiguration('log', 'none'); initializeController('type', 'hac-iff'); #+end_src We set the stiffness of the payload fixation: #+begin_src matlab Kp = 1e8; % [N/m] #+end_src ** APA-100 Amplified Actuator *** Identification #+begin_src matlab K = tf(zeros(6)); Kiff = tf(zeros(6)); #+end_src We identify the system for the following payload masses: #+begin_src matlab Ms = [1, 10, 50]; #+end_src #+begin_src matlab :exports none Gm_iff = {zeros(length(Ms), 1)}; #+end_src The nano-hexapod has the following leg's stiffness and damping. #+begin_src matlab initializeNanoHexapod('actuator', 'amplified'); #+end_src #+begin_src matlab :exports none %% Name of the Simulink File mdl = 'nass_model'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1; % Force Sensors #+end_src #+begin_src matlab :exports none for i = 1:length(Ms) initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1)); initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i)); %% Run the linearization G_iff = linearize(mdl, io); G_iff.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; G_iff.OutputName = {'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}; Gm_iff(i) = {G_iff}; end #+end_src *** Controller Design The loop gain for IFF is shown in Figure [[fig:amplified_piezo_iff_loop_gain]]. The corresponding root locus is shown in Figure [[fig:amplified_piezo_iff_root_locus]]. Finally, the damping as function of the gain is display in Figure [[fig:amplified_piezo_iff_damping_gain]]. #+begin_src matlab :exports none freqs = logspace(-1, 3, 1000); figure; ax1 = subplot(2, 1, 1); hold on; for i = 1:length(Ms) plot(freqs, abs(squeeze(freqresp(Gm_iff{i}(1, 1), freqs, 'Hz')))); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); ax2 = subplot(2, 1, 2); hold on; for i = 1:length(Ms) plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_iff{i}(1, 1), freqs, 'Hz')))), ... 'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i))); 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'); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/amplified_piezo_iff_loop_gain.pdf', 'width', 'full', 'height', 'full'); #+end_src #+name: fig:amplified_piezo_iff_loop_gain #+caption: Dynamics for the Integral Force Feedback for three payload masses #+RESULTS: [[file:figs/amplified_piezo_iff_loop_gain.png]] #+begin_src matlab :exports none figure; gains = logspace(2, 4, 200); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(real(pole(Gm_iff{i})), imag(pole(Gm_iff{i})), 'x', ... 'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i))); set(gca,'ColorOrderIndex',i); plot(real(tzero(Gm_iff{i})), imag(tzero(Gm_iff{i})), 'o', ... 'HandleVisibility', 'off'); for k = 1:length(gains) set(gca,'ColorOrderIndex',i); cl_poles = pole(feedback(Gm_iff{i}, -(gains(k)/s)*eye(6))); plot(real(cl_poles), imag(cl_poles), '.', ... 'HandleVisibility', 'off'); end end hold off; axis square; xlim([-400, 10]); ylim([0, 500]); xlabel('Real Part'); ylabel('Imaginary Part'); legend('location', 'northwest'); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/amplified_piezo_iff_root_locus.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:amplified_piezo_iff_root_locus #+caption: Root Locus for the IFF control for three payload masses #+RESULTS: [[file:figs/amplified_piezo_iff_root_locus.png]] #+begin_src matlab :exports none c1 = [ 0 0.4470 0.7410]; % Blue c2 = [0.8500 0.3250 0.0980]; % Orange c3 = [0.9290 0.6940 0.1250]; % Yellow c4 = [0.4940 0.1840 0.5560]; % Purple c5 = [0.4660 0.6740 0.1880]; % Green c6 = [0.3010 0.7450 0.9330]; % Light Blue c7 = [0.6350 0.0780 0.1840]; % Red colors = [c1; c2; c3; c4; c5; c6; c7]; figure; gains = logspace(2, 5, 100); hold on; for i = 1:length(Ms) for k = 1:length(gains) cl_poles = pole(feedback(Gm_iff{i}, -(gains(k)/s)*eye(6))); set(gca,'ColorOrderIndex',i); plot(gains(k), sin(-pi/2 + angle(cl_poles)), '.', 'color', colors(i, :)); end end hold off; xlabel('IFF Gain'); ylabel('Modal Damping'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylim([0, 1]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/amplified_piezo_iff_damping_gain.pdf', 'width', 'full', 'height', 'full'); #+end_src #+name: fig:amplified_piezo_iff_damping_gain #+caption: Damping ratio of the poles as a function of the IFF gain #+RESULTS: [[file:figs/amplified_piezo_iff_damping_gain.png]] The following controller for the Decentralized Integral Force Feedback is used: #+begin_src matlab Kiff = -1e4/s*eye(6); #+end_src *** Effect of the Low Authority Control on the Primary Plant **** Introduction :ignore: #+begin_src matlab :exports none %% Name of the Simulink File mdl = 'nass_model'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror #+end_src #+begin_src matlab :exports none load('mat/stages.mat', 'nano_hexapod'); #+end_src **** Identification of the undamped plant :ignore: #+begin_src matlab :exports none Kiff_backup = Kiff; Kiff = tf(zeros(6)); #+end_src #+begin_src matlab :exports none G_x = {zeros(length(Ms), 1)}; G_l = {zeros(length(Ms), 1)}; #+end_src #+begin_src matlab :exports none for i = 1:length(Ms) initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1)); initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i)); %% Run the linearization G = linearize(mdl, io); G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}; Gx = -G*inv(nano_hexapod.kinematics.J'); Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; G_x(i) = {Gx}; Gl = -nano_hexapod.kinematics.J*G; Gl.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'}; G_l(i) = {Gl}; end #+end_src #+begin_src matlab :exports none Kiff = Kiff_backup; #+end_src **** Identification of the damped plant :ignore: #+begin_src matlab :exports none Gm_x = {zeros(length(Ms), 1)}; Gm_l = {zeros(length(Ms), 1)}; #+end_src #+begin_src matlab :exports none for i = 1:length(Ms) initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1)); initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i)); %% Run the linearization G = linearize(mdl, io); G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}; Gx = -G*inv(nano_hexapod.kinematics.J'); Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; Gm_x(i) = {Gx}; Gl = -nano_hexapod.kinematics.J*G; Gl.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'}; Gm_l(i) = {Gl}; end #+end_src **** Effect of the Damping on the plant diagonal dynamics :ignore: #+begin_src matlab :exports none freqs = logspace(0, 3, 5000); figure; ax1 = subplot(2, 2, 1); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz')))); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(G_x{i}(2, 2), freqs, 'Hz')))); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gm_x{i}(1, 1), freqs, 'Hz'))), '--'); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gm_x{i}(2, 2), freqs, 'Hz'))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); title('$\mathcal{X}_x/\mathcal{F}_x$, $\mathcal{X}_y/\mathcal{F}_y$') ax2 = subplot(2, 2, 2); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz')))); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gm_x{i}(3, 3), freqs, 'Hz'))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); title('$\mathcal{X}_z/\mathcal{F}_z$') ax3 = subplot(2, 2, 3); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz'))))); set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(2, 2), freqs, 'Hz'))))); set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(1, 1), freqs, 'Hz')))), '--'); set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(2, 2), freqs, 'Hz')))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); ylim([-270, 90]); yticks([-360:90:360]); ax4 = subplot(2, 2, 4); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz')))), ... 'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i))); set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(3, 3), freqs, 'Hz')))), '--', ... 'HandleVisibility', 'off'); 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', 'southwest'); linkaxes([ax1,ax2,ax3,ax4],'x'); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/amplified_piezo_iff_plant_damped_X.pdf', 'width', 'full', 'height', 'full'); #+end_src #+name: fig:amplified_piezo_iff_plant_damped_X #+caption: Primary plant in the task space with (dashed) and without (solid) IFF #+RESULTS: [[file:figs/amplified_piezo_iff_plant_damped_X.png]] #+begin_src matlab :exports none freqs = logspace(0, 3, 5000); figure; ax1 = subplot(2, 1, 1); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(G_l{i}(1, 1), freqs, 'Hz')))); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gm_l{i}(1, 1), freqs, 'Hz'))), '--'); 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(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_l{i}(1, 1), freqs, 'Hz')))), ... 'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i))); set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_l{i}(1, 1), freqs, 'Hz')))), '--', ... 'HandleVisibility', 'off'); 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', 'southwest'); linkaxes([ax1,ax2],'x'); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/amplified_piezo_iff_damped_plant_L.pdf', 'width', 'full', 'height', 'full'); #+end_src #+name: fig:amplified_piezo_iff_damped_plant_L #+caption: Primary plant in the space of the legs with (dashed) and without (solid) IFF #+RESULTS: [[file:figs/amplified_piezo_iff_damped_plant_L.png]] **** Effect of the Damping on the coupling dynamics :ignore: #+begin_src matlab :exports none freqs = logspace(0, 3, 1000); figure; hold on; for i = 1:5 for j = i+1:6 plot(freqs, abs(squeeze(freqresp(G_x{1}(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); plot(freqs, abs(squeeze(freqresp(Gm_x{1}(i, j), freqs, 'Hz'))), '--', 'color', [0, 0, 0, 0.2]); end end set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(G_x{1}(1, 1), freqs, 'Hz')))); set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(Gm_x{1}(1, 1), freqs, 'Hz'))), '--'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); ylim([1e-12, inf]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/amplified_piezo_iff_damped_coupling_X.pdf', 'width', 'full', 'height', 'full'); #+end_src #+name: fig:amplified_piezo_iff_damped_coupling_X #+caption: Coupling in the primary plant in the task with (dashed) and without (solid) IFF #+RESULTS: [[file:figs/amplified_piezo_iff_damped_coupling_X.png]] #+begin_src matlab :exports none freqs = logspace(0, 3, 1000); figure; hold on; for i = 1:5 for j = i+1:6 plot(freqs, abs(squeeze(freqresp(G_l{1}(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); plot(freqs, abs(squeeze(freqresp(Gm_l{1}(i, j), freqs, 'Hz'))), '--', 'color', [0, 0, 0, 0.2]); end end set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(G_l{1}(1, 1), freqs, 'Hz')))); set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(Gm_l{1}(1, 1), freqs, 'Hz'))), '--'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); ylim([1e-9, inf]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/amplified_piezo_iff_damped_coupling_L.pdf', 'width', 'full', 'height', 'full'); #+end_src #+name: fig:amplified_piezo_iff_damped_coupling_L #+caption: Coupling in the primary plant in the space of the legs with (dashed) and without (solid) IFF #+RESULTS: [[file:figs/amplified_piezo_iff_damped_coupling_L.png]] *** Effect of the Low Authority Control on the Sensibility to Disturbances **** Introduction :ignore: **** Identification :ignore: #+begin_src matlab :exports none %% Name of the Simulink File mdl = 'nass_model'; %% Micro-Hexapod clear io; io_i = 1; io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwz'); io_i = io_i + 1; % Z Ground motion io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fty_z'); io_i = io_i + 1; % Parasitic force Ty - Z io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Frz_z'); io_i = io_i + 1; % Parasitic force Rz - Z io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fd'); io_i = io_i + 1; % Direct forces io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror #+end_src #+begin_src matlab :exports none Kiff_backup = Kiff; Kiff = tf(zeros(6)); #+end_src #+begin_src matlab :exports none Gd = {zeros(length(Ms), 1)}; for i = 1:length(Ms) initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1)); initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i)); %% Run the linearization G = linearize(mdl, io); G.InputName = {'Dwz', 'Fty_z', 'Frz_z', 'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'}; G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}; Gd(i) = {G}; end #+end_src #+begin_src matlab :exports none Kiff = Kiff_backup; #+end_src #+begin_src matlab :exports none Gd_iff = {zeros(length(Ms), 1)}; for i = 1:length(Ms) initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1)); initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i)); %% Run the linearization G = linearize(mdl, io); G.InputName = {'Dwz', 'Fty_z', 'Frz_z', 'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'}; G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}; Gd_iff(i) = {G}; end #+end_src **** Results :ignore: #+begin_src matlab :exports none freqs = logspace(0, 3, 5000); figure; subplot(2, 2, 1); title('$D_{w,z}$ to $E_z$'); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Dwz'), freqs, 'Hz'))), ... 'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i))); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd_iff{i}('Ez', 'Dwz'), freqs, 'Hz'))), '--', ... 'HandleVisibility', 'off'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/m]'); set(gca, 'XTickLabel',[]); legend('location', 'southeast'); subplot(2, 2, 2); title('$F_{dz}$ to $E_z$'); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Fdz'), freqs, 'Hz')))); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd_iff{i}('Ez', 'Fdz'), freqs, 'Hz'))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/N]'); subplot(2, 2, 3); title('$F_{T_y,z}$ to $E_z$'); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Fty_z'), freqs, 'Hz')))); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd_iff{i}('Ez', 'Fty_z'), freqs, 'Hz'))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]'); subplot(2, 2, 4); title('$F_{R_z,z}$ to $E_z$'); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Frz_z'), freqs, 'Hz')))); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd_iff{i}('Ez', 'Frz_z'), freqs, 'Hz'))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]'); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/amplified_piezo_iff_disturbances.pdf', 'width', 'full', 'height', 'full'); #+end_src #+name: fig:amplified_piezo_iff_disturbances #+caption: Norm of the transfer function from vertical disturbances to vertical position error with (dashed) and without (solid) Integral Force Feedback applied #+RESULTS: [[file:figs/amplified_piezo_iff_disturbances.png]] *** Conclusion :ignore: #+begin_important #+end_important ** Optimal Stiffnesses *** Introduction :ignore: Based on the analytical analysis, we can determine the parameters of the amplified piezoelectric actuator in order to be able to add a lots of damping using IFF: - $k_1$ should be minimized. - $k_e \approx k_a \approx 10^5 - 10^6\,[N/m]$ However, this might not be realizable. *** Low Authority Controller **** Identification The nano-hexapod is initialized with the following parameters: #+begin_src matlab initializeNanoHexapod('actuator', 'amplified', ... 'k1', 1e4, ... 'ke', 1e6, ... 'ka', 1e6); #+end_src #+begin_src matlab :exports none initializeController('type', 'hac-iff'); K = tf(zeros(6)); Kiff = tf(zeros(6)); Ms = [1, 10, 50]; Gm_iff = {zeros(length(Ms), 1)}; %% Name of the Simulink File mdl = 'nass_model'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1; % Force Sensors for i = 1:length(Ms) initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1)); initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i)); %% Run the linearization G_iff = linearize(mdl, io); G_iff.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; G_iff.OutputName = {'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}; Gm_iff(i) = {G_iff}; end #+end_src The obtain plan for the IFF control is shown in Figure [[fig:amplified_piezo_opt_stiff_iff_plant]]. The associated Root Locus is shown in Figure [[fig:amplified_piezo_opt_stiff_iff_root_locus]]. Based on that, the following IFF gain is chosen: #+begin_src matlab Kiff = -1e3/s*eye(6); #+end_src #+begin_src matlab :exports none freqs = logspace(-1, 3, 1000); figure; ax1 = subplot(2, 1, 1); hold on; for i = 1:length(Ms) plot(freqs, abs(squeeze(freqresp(Gm_iff{i}(1, 1), freqs, 'Hz')))); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); ax2 = subplot(2, 1, 2); hold on; for i = 1:length(Ms) plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_iff{i}(1, 1), freqs, 'Hz')))), ... 'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i))); 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'); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/amplified_piezo_opt_stiff_iff_plant.pdf', 'width', 'full', 'height', 'full'); #+end_src #+name: fig:amplified_piezo_opt_stiff_iff_plant #+caption: Plant dynamics for IFF with the amplified piezoelectric stack actuator #+RESULTS: [[file:figs/amplified_piezo_opt_stiff_iff_plant.png]] #+begin_src matlab :exports none figure; gains = logspace(1, 4, 400); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(real(pole(Gm_iff{i})), imag(pole(Gm_iff{i})), 'x', ... 'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i))); set(gca,'ColorOrderIndex',i); plot(real(tzero(Gm_iff{i})), imag(tzero(Gm_iff{i})), 'o', ... 'HandleVisibility', 'off'); for k = 1:length(gains) set(gca,'ColorOrderIndex',i); cl_poles = pole(feedback(Gm_iff{i}, -(gains(k)/s)*eye(6))); plot(real(cl_poles), imag(cl_poles), '.', ... 'HandleVisibility', 'off'); end end hold off; axis square; xlim([-400, 10]); ylim([0, 410]); xlabel('Real Part'); ylabel('Imaginary Part'); legend('location', 'northwest'); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/amplified_piezo_opt_stiff_iff_root_locus.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:amplified_piezo_opt_stiff_iff_root_locus #+caption: Root Locus for IFF with the amplified piezoelectric stack actuator #+RESULTS: [[file:figs/amplified_piezo_opt_stiff_iff_root_locus.png]] #+begin_src matlab :exports none c1 = [ 0 0.4470 0.7410]; % Blue c2 = [0.8500 0.3250 0.0980]; % Orange c3 = [0.9290 0.6940 0.1250]; % Yellow c4 = [0.4940 0.1840 0.5560]; % Purple c5 = [0.4660 0.6740 0.1880]; % Green c6 = [0.3010 0.7450 0.9330]; % Light Blue c7 = [0.6350 0.0780 0.1840]; % Red colors = [c1; c2; c3; c4; c5; c6; c7]; figure; gains = logspace(2, 5, 100); hold on; for i = 1:length(Ms) for k = 1:length(gains) cl_poles = pole(feedback(Gm_iff{i}, -(gains(k)/s)*eye(6))); set(gca,'ColorOrderIndex',i); plot(gains(k), sin(-pi/2 + angle(cl_poles)), '.', 'color', colors(i, :)); end end hold off; xlabel('IFF Gain'); ylabel('Modal Damping'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylim([0, 1]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/amplified_piezo_opt_stiff_gain_damping.pdf', 'width', 'full', 'height', 'full'); #+end_src #+name: fig:amplified_piezo_opt_stiff_gain_damping #+caption: Damping of the modes as a function of the IFF gain #+RESULTS: [[file:figs/amplified_piezo_opt_stiff_gain_damping.png]] **** Effect of the Low Authority Control on the Primary Plant ***** Introduction :ignore: #+begin_src matlab :exports none %% Name of the Simulink File mdl = 'nass_model'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror #+end_src #+begin_src matlab :exports none load('mat/stages.mat', 'nano_hexapod'); #+end_src ***** Identification of the undamped plant :ignore: #+begin_src matlab :exports none Kiff_backup = Kiff; Kiff = tf(zeros(6)); #+end_src #+begin_src matlab :exports none G_x = {zeros(length(Ms), 1)}; G_l = {zeros(length(Ms), 1)}; #+end_src #+begin_src matlab :exports none for i = 1:length(Ms) initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1)); initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i)); %% Run the linearization G = linearize(mdl, io); G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}; Gx = -G*inv(nano_hexapod.kinematics.J'); Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; G_x(i) = {Gx}; Gl = -nano_hexapod.kinematics.J*G; Gl.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'}; G_l(i) = {Gl}; end #+end_src #+begin_src matlab :exports none Kiff = Kiff_backup; #+end_src ***** Identification of the damped plant :ignore: #+begin_src matlab :exports none Gm_x = {zeros(length(Ms), 1)}; Gm_l = {zeros(length(Ms), 1)}; #+end_src #+begin_src matlab :exports none for i = 1:length(Ms) initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1)); initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i)); %% Run the linearization G = linearize(mdl, io); G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}; Gx = -G*inv(nano_hexapod.kinematics.J'); Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; Gm_x(i) = {Gx}; Gl = -nano_hexapod.kinematics.J*G; Gl.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'}; Gm_l(i) = {Gl}; end #+end_src ***** Effect of the Damping on the plant diagonal dynamics :ignore: #+begin_src matlab :exports none freqs = logspace(0, 3, 5000); figure; ax1 = subplot(2, 2, 1); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz')))); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(G_x{i}(2, 2), freqs, 'Hz')))); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gm_x{i}(1, 1), freqs, 'Hz'))), '--'); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gm_x{i}(2, 2), freqs, 'Hz'))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); title('$\mathcal{X}_x/\mathcal{F}_x$, $\mathcal{X}_y/\mathcal{F}_y$') ax2 = subplot(2, 2, 2); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz')))); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gm_x{i}(3, 3), freqs, 'Hz'))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); title('$\mathcal{X}_z/\mathcal{F}_z$') ax3 = subplot(2, 2, 3); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz'))))); set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(2, 2), freqs, 'Hz'))))); set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(1, 1), freqs, 'Hz')))), '--'); set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(2, 2), freqs, 'Hz')))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); ylim([-270, 90]); yticks([-360:90:360]); ax4 = subplot(2, 2, 4); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz')))), ... 'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i))); set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(3, 3), freqs, 'Hz')))), '--', ... 'HandleVisibility', 'off'); 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', 'southwest'); linkaxes([ax1,ax2,ax3,ax4],'x'); #+end_src #+begin_src matlab :exports none freqs = logspace(0, 3, 5000); figure; ax1 = subplot(2, 1, 1); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(G_l{i}(1, 1), freqs, 'Hz')))); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gm_l{i}(1, 1), freqs, 'Hz'))), '--'); 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(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_l{i}(1, 1), freqs, 'Hz')))), ... 'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i))); set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_l{i}(1, 1), freqs, 'Hz')))), '--', ... 'HandleVisibility', 'off'); 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', 'southwest'); linkaxes([ax1,ax2],'x'); #+end_src **** Effect of the Low Authority Control on the Sensibility to Disturbances ***** Introduction :ignore: ***** Identification :ignore: #+begin_src matlab :exports none %% Name of the Simulink File mdl = 'nass_model'; %% Micro-Hexapod clear io; io_i = 1; io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwz'); io_i = io_i + 1; % Z Ground motion io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fty_z'); io_i = io_i + 1; % Parasitic force Ty - Z io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Frz_z'); io_i = io_i + 1; % Parasitic force Rz - Z io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fd'); io_i = io_i + 1; % Direct forces io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror #+end_src #+begin_src matlab :exports none Kiff_backup = Kiff; Kiff = tf(zeros(6)); #+end_src #+begin_src matlab :exports none Gd = {zeros(length(Ms), 1)}; for i = 1:length(Ms) initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1)); initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i)); %% Run the linearization G = linearize(mdl, io); G.InputName = {'Dwz', 'Fty_z', 'Frz_z', 'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'}; G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}; Gd(i) = {G}; end #+end_src #+begin_src matlab :exports none Kiff = Kiff_backup; #+end_src #+begin_src matlab :exports none Gd_iff = {zeros(length(Ms), 1)}; for i = 1:length(Ms) initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1)); initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i)); %% Run the linearization G = linearize(mdl, io); G.InputName = {'Dwz', 'Fty_z', 'Frz_z', 'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'}; G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}; Gd_iff(i) = {G}; end #+end_src ***** Results :ignore: #+begin_src matlab :exports none freqs = logspace(0, 3, 5000); figure; subplot(2, 2, 1); title('$D_{w,z}$ to $E_z$'); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Dwz'), freqs, 'Hz'))), ... 'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i))); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd_iff{i}('Ez', 'Dwz'), freqs, 'Hz'))), '--', ... 'HandleVisibility', 'off'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/m]'); set(gca, 'XTickLabel',[]); legend('location', 'southeast'); subplot(2, 2, 2); title('$F_{dz}$ to $E_z$'); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Fdz'), freqs, 'Hz')))); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd_iff{i}('Ez', 'Fdz'), freqs, 'Hz'))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/N]'); subplot(2, 2, 3); title('$F_{T_y,z}$ to $E_z$'); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Fty_z'), freqs, 'Hz')))); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd_iff{i}('Ez', 'Fty_z'), freqs, 'Hz'))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]'); subplot(2, 2, 4); title('$F_{R_z,z}$ to $E_z$'); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Frz_z'), freqs, 'Hz')))); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd_iff{i}('Ez', 'Frz_z'), freqs, 'Hz'))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]'); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/amplified_piezo_opt_stiff_iff_dist.pdf', 'width', 'full', 'height', 'full'); #+end_src #+name: fig:amplified_piezo_opt_stiff_iff_dist #+caption: Effect of disturbance with and without IFF #+RESULTS: [[file:figs/amplified_piezo_opt_stiff_iff_dist.png]] ***** Conclusion :ignore: #+begin_important #+end_important *** High Authority Controller **** Introduction :ignore: **** Controller Design #+begin_src matlab h = 2.5; Kl = 5e6 * eye(6) * ... 1/h*(s/(2*pi*40/h) + 1)/(s/(2*pi*40*h) + 1) * ... 1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ... (s/2/pi/50 + 1)/(s/2/pi/50) * ... (s/2/pi/10 + 1)/(s/2/pi/10) * ... 1/(1 + s/2/pi/200); #+end_src #+begin_src matlab Kl = 3e10 * eye(6) * ... 1/s * ... (s+0.8)/s * ... (s+50)/(s+0.01) * ... (s+120)/(s+1000) * ... (s+150)/(s+1000); #+end_src #+begin_src matlab :exports none freqs = logspace(0, 3, 1000); figure; ax1 = subplot(2, 1, 1); hold on; for i = 1:length(Ms) for j = 1:6 set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gm_l{i}(j, j)*Kl(j,j), freqs, 'Hz')))); end end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); ax2 = subplot(2, 1, 2); hold on; for i = 1:length(Ms) for j = 1:6 set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_l{i}(j, j)*Kl(j,j), freqs, 'Hz')))); end 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]); linkaxes([ax1,ax2],'x'); #+end_src Finally, we include the Jacobian in the control and we ignore the measurement of the vertical rotation as for the real system. #+begin_src matlab load('mat/stages.mat', 'nano_hexapod'); K = Kl*nano_hexapod.kinematics.J*diag([1, 1, 1, 1, 1, 0]); #+end_src #+begin_src matlab :exports none for i = 1:length(Ms) isstable(feedback(nano_hexapod.kinematics.J\Gm_l{i}*K, eye(6), -1)) end #+end_src **** Sensibility to Disturbances and Noise Budget ***** Identification :ignore: We identify the transfer function from disturbances to the position error of the sample when the HAC-LAC control is applied. #+begin_src matlab :exports none %% Name of the Simulink File mdl = 'nass_model'; %% Micro-Hexapod clear io; io_i = 1; io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwz'); io_i = io_i + 1; % Z Ground motion io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fty_z'); io_i = io_i + 1; % Parasitic force Ty - Z io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Frz_z'); io_i = io_i + 1; % Parasitic force Rz - Z io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fd'); io_i = io_i + 1; % Direct forces io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror #+end_src #+begin_src matlab :exports none Gd_L = {zeros(length(Ms), 1)}; for i = 1:length(Ms) initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1)); initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i)); %% Run the linearization G = linearize(mdl, io); G.InputName = {'Dwz', 'Fty_z', 'Frz_z', 'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'}; G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}; Gd_L(i) = {G}; end #+end_src ***** Obtained Sensibility to Disturbances :ignore: #+begin_src matlab :exports none freqs = logspace(0, 3, 5000); figure; subplot(2, 2, 1); title('$D_{w,z}$ to $E_z$'); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Dwz'), freqs, 'Hz'))), ... 'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i))); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd_L{i}('Ez', 'Dwz'), freqs, 'Hz'))), '--', ... 'HandleVisibility', 'off'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/m]'); set(gca, 'XTickLabel',[]); legend('location', 'southeast'); subplot(2, 2, 2); title('$F_{dz}$ to $E_z$'); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Fdz'), freqs, 'Hz')))); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd_L{i}('Ez', 'Fdz'), freqs, 'Hz'))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/N]'); subplot(2, 2, 3); title('$F_{T_y,z}$ to $E_z$'); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Fty_z'), freqs, 'Hz')))); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd_L{i}('Ez', 'Fty_z'), freqs, 'Hz'))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]'); subplot(2, 2, 4); title('$F_{R_z,z}$ to $E_z$'); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Frz_z'), freqs, 'Hz')))); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd_L{i}('Ez', 'Frz_z'), freqs, 'Hz'))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]'); #+end_src **** Simulations of Tomography Experiment Let's now simulate a tomography experiment. To do so, we include all disturbances except vibrations of the translation stage. #+begin_src matlab initializeDisturbances(); initializeSimscapeConfiguration('gravity', false); initializeLoggingConfiguration('log', 'all'); #+end_src #+begin_src matlab :exports none load('mat/conf_simulink.mat'); set_param(conf_simulink, 'StopTime', '2'); #+end_src 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) initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1)); initializeReferences('Rz_type', 'rotating', 'Rz_period', Ms(i)); sim('nass_model'); hac_iff_L(i) = {simout}; end #+end_src #+begin_src matlab :exports none save('./mat/tomo_exp_hac_iff_opt_stiff.mat', 'hac_dvf_L', 'Ms'); #+end_src **** Results #+begin_src matlab :exports none load('./mat/experiment_tomography.mat', 'scans_rz_align_dist'); load('./mat/tomo_exp_hac_iff_opt_stiff.mat', 'hac_iff_L', 'Ms'); #+end_src #+begin_src matlab :exports none n_av = 4; han_win = hanning(ceil(length(hac_iff_L{1}.Em.En.Data(:,1))/n_av)); t = hac_iff_L{1}.Em.En.Time; Ts = t(2)-t(1); [pxx_ol, f] = pwelch(scans_rz_align_dist.Em.En.Data, han_win, [], [], 1/Ts); pxx_iff_L = zeros(length(f), 6, length(Ms)); for i = 1:length(Ms) [pxx, ~] = pwelch(hac_iff_L{i}.Em.En.Data(ceil(0.2/Ts):end,:), han_win, [], [], 1/Ts); pxx_iff_L(:, :, i) = pxx; end #+end_src #+begin_src matlab :exports none figure; ax1 = subplot(2, 3, 1); hold on; plot(f, sqrt(pxx_ol(:, 1)), 'k-') for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(f, sqrt(pxx_iff_L(:, 1, i))) end hold off; xlabel('Frequency [Hz]'); ylabel('$\Gamma_{D_x}$ [$m/\sqrt{Hz}$]'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ax2 = subplot(2, 3, 2); hold on; plot(f, sqrt(pxx_ol(:, 2)), 'k-') for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(f, sqrt(pxx_iff_L(:, 2, i))) end hold off; xlabel('Frequency [Hz]'); ylabel('$\Gamma_{D_y}$ [$m/\sqrt{Hz}$]'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ax3 = subplot(2, 3, 3); hold on; plot(f, sqrt(pxx_ol(:, 3)), 'k-') for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(f, sqrt(pxx_iff_L(:, 3, i))) end hold off; xlabel('Frequency [Hz]'); ylabel('$\Gamma_{D_z}$ [$m/\sqrt{Hz}$]'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ax4 = subplot(2, 3, 4); hold on; plot(f, sqrt(pxx_ol(:, 4)), 'k-') for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(f, sqrt(pxx_iff_L(:, 4, i))) end hold off; xlabel('Frequency [Hz]'); ylabel('$\Gamma_{R_x}$ [$rad/\sqrt{Hz}$]'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ax5 = subplot(2, 3, 5); hold on; plot(f, sqrt(pxx_ol(:, 5)), 'k-') for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(f, sqrt(pxx_iff_L(:, 5, i))) end hold off; xlabel('Frequency [Hz]'); ylabel('$\Gamma_{R_y}$ [$rad/\sqrt{Hz}$]'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ax6 = subplot(2, 3, 6); hold on; plot(f, sqrt(pxx_ol(:, 6)), 'k-', 'DisplayName', '$\mu$-Station') for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(f, sqrt(pxx_iff_L(:, 6, i)), ... 'DisplayName', sprintf('HAC-IFF $m = %.0f kg$', Ms(i))) end hold off; xlabel('Frequency [Hz]'); ylabel('$\Gamma_{R_z}$ [$rad/\sqrt{Hz}$]'); legend('location', 'southwest'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x'); xlim([f(2), f(end)]) #+end_src #+begin_src matlab :exports none figure; ax1 = subplot(2, 3, 1); hold on; 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_iff_L(:, 1, i)))))); end hold off; xlabel('Frequency [Hz]'); ylabel('CAS $D_x$ [$m$]'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylim([1e-11, 1e-5]); ax2 = subplot(2, 3, 2); hold on; 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_iff_L(:, 2, i)))))); end hold off; xlabel('Frequency [Hz]'); ylabel('CAS $D_y$ [$m$]'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylim([1e-11, 1e-5]); ax3 = subplot(2, 3, 3); hold on; 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_iff_L(:, 3, i)))))); end hold off; xlabel('Frequency [Hz]'); ylabel('CAS $D_z$ [$m$]'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylim([1e-11, 1e-5]); ax4 = subplot(2, 3, 4); hold on; 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_iff_L(:, 4, i)))))); end hold off; xlabel('Frequency [Hz]'); ylabel('CAS $R_x$ [$rad$]'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylim([1e-11, 1e-5]); ax5 = subplot(2, 3, 5); hold on; 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_iff_L(:, 5, i)))))); end hold off; xlabel('Frequency [Hz]'); ylabel('CAS $R_y$ [$rad$]'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylim([1e-11, 1e-5]); ax6 = subplot(2, 3, 6); hold on; 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_iff_L(:, 6, i))))), ... 'DisplayName', sprintf('HAC-IFF $m = %.0f kg$', Ms(i))); end hold off; xlabel('Frequency [Hz]'); ylabel('CAS $R_z$ [$rad$]'); legend('location', 'southwest'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylim([1e-11, 1e-5]); linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x'); xlim([f(2), f(end)]) #+end_src #+begin_src matlab :exports none figure; ax1 = subplot(2, 3, 1); hold on; plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 1), 'k-') for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(hac_iff_L{i}.Em.En.Time, hac_iff_L{i}.Em.En.Data(:, 1)); end hold off; xlabel('Time [s]'); ylabel('Dx [m]'); ax2 = subplot(2, 3, 2); hold on; plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 2), 'k-') for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(hac_iff_L{i}.Em.En.Time, hac_iff_L{i}.Em.En.Data(:, 2)); end hold off; xlabel('Time [s]'); ylabel('Dy [m]'); ax3 = subplot(2, 3, 3); hold on; plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 3), 'k-') for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(hac_iff_L{i}.Em.En.Time, hac_iff_L{i}.Em.En.Data(:, 3)); end hold off; xlabel('Time [s]'); ylabel('Dz [m]'); ax4 = subplot(2, 3, 4); hold on; plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 4), 'k-') for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(hac_iff_L{i}.Em.En.Time, hac_iff_L{i}.Em.En.Data(:, 4)); end hold off; xlabel('Time [s]'); ylabel('Rx [rad]'); ax5 = subplot(2, 3, 5); hold on; plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 5), 'k-') for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(hac_iff_L{i}.Em.En.Time, hac_iff_L{i}.Em.En.Data(:, 5)); end hold off; xlabel('Time [s]'); ylabel('Ry [rad]'); ax6 = subplot(2, 3, 6); hold on; plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 6), 'k-', ... 'DisplayName', '$\mu$-Station'); for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(hac_iff_L{i}.Em.En.Time, hac_iff_L{i}.Em.En.Data(:, 6), ... 'DisplayName', sprintf('HAC-IFF $m = %.0f kg$', Ms(i))); end hold off; xlabel('Time [s]'); ylabel('Rz [rad]'); legend(); linkaxes([ax1,ax2,ax3,ax4],'x'); xlim([0.5, inf]); #+end_src ** Direct Velocity Feedback with Amplified Actuators Lack of collocation. #+begin_src matlab initializeController('type', 'hac-dvf'); K = tf(zeros(6)); Kdvf = tf(zeros(6)); #+end_src We identify the system for the following payload masses: #+begin_src matlab Ms = [1, 10, 50]; #+end_src #+begin_src matlab :exports none Gm_dvf = {zeros(length(Ms), 1)}; #+end_src #+begin_src matlab initializeNanoHexapod('actuator', 'amplified', ... 'k1', 1e4, ... 'ke', 1e6, ... 'ka', 1e6); #+end_src #+begin_src matlab :exports none %% Name of the Simulink File mdl = 'nass_model'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1; % Force Sensors #+end_src #+begin_src matlab :exports none for i = 1:length(Ms) initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1)); initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i)); %% Run the linearization G_dvf = linearize(mdl, io); G_dvf.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; G_dvf.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}; Gm_dvf(i) = {G_dvf}; end #+end_src #+begin_src matlab :exports none freqs = logspace(-1, 4, 1000); figure; ax1 = subplot(2, 1, 1); hold on; for i = 1:length(Ms) plot(freqs, abs(squeeze(freqresp(Gm_dvf{i}(1, 1), freqs, 'Hz')))); 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(Ms) plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_dvf{i}(1, 1), freqs, 'Hz')))), ... 'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i))); 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'); #+end_src #+begin_src matlab :exports none figure; gains = logspace(2, 4, 100); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(real(pole(Gm_dvf{i})), imag(pole(Gm_dvf{i})), 'x', ... 'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i))); set(gca,'ColorOrderIndex',i); plot(real(tzero(Gm_dvf{i})), imag(tzero(Gm_dvf{i})), 'o', ... 'HandleVisibility', 'off'); for k = 1:length(gains) set(gca,'ColorOrderIndex',i); cl_poles = pole(feedback(Gm_dvf{i}, (gains(k)*s)*eye(6))); plot(real(cl_poles), imag(cl_poles), '.', ... 'HandleVisibility', 'off'); end end hold off; axis square; xlim([-400, 10]); ylim([0, 500]); xlabel('Real Part'); ylabel('Imaginary Part'); legend('location', 'northwest'); #+end_src * APA300ML ** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) <> #+end_src #+begin_src matlab :exports none :results silent :noweb yes <> #+end_src #+begin_src matlab :tangle no simulinkproject('../'); #+end_src #+begin_src matlab open('nass_model.slx') #+end_src ** Initialization #+begin_src matlab initializeGround(); initializeGranite(); initializeTy(); initializeRy(); initializeRz(); initializeMicroHexapod(); initializeAxisc(); initializeMirror(); initializeSimscapeConfiguration(); initializeDisturbances('enable', false); initializeLoggingConfiguration('log', 'none'); initializeController('type', 'hac-dvf'); #+end_src We set the stiffness of the payload fixation: #+begin_src matlab Kp = 1e8; % [N/m] #+end_src ** Identification #+begin_src matlab K = tf(zeros(6)); Kdvf = tf(zeros(6)); #+end_src We identify the system for the following payload masses: #+begin_src matlab Ms = [1, 10, 50]; #+end_src #+begin_src matlab :exports none Gm_dvf = {zeros(length(Ms), 1)}; #+end_src The nano-hexapod has the following leg's stiffness and damping. #+begin_src matlab initializeNanoHexapod('actuator', 'amplified', 'k1', 0.4e6, 'ka', 43e6, 'ke', 1.5e6); #+end_src #+begin_src matlab :exports none %% Name of the Simulink File mdl = 'nass_model'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1; % Force Sensors #+end_src #+begin_src matlab :exports none for i = 1:length(Ms) initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1)); initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i)); %% Run the linearization G_dvf = linearize(mdl, io); G_dvf.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; G_dvf.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}; Gm_dvf(i) = {G_dvf}; end #+end_src ** Controller Design #+begin_src matlab :exports none freqs = logspace(-1, 3, 1000); figure; ax1 = subplot(2, 1, 1); hold on; for i = 1:length(Ms) plot(freqs, abs(squeeze(freqresp(Gm_dvf{i}(1, 1), freqs, 'Hz')))); 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(Ms) plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_dvf{i}(1, 1), freqs, 'Hz')))), ... 'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i))); 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'); #+end_src #+begin_src matlab :exports none figure; gains = logspace(2, 6, 100); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(real(pole(Gm_dvf{i})), imag(pole(Gm_dvf{i})), 'x', ... 'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i))); set(gca,'ColorOrderIndex',i); plot(real(tzero(Gm_dvf{i})), imag(tzero(Gm_dvf{i})), 'o', ... 'HandleVisibility', 'off'); for k = 1:length(gains) set(gca,'ColorOrderIndex',i); cl_poles = pole(feedback(Gm_dvf{i}, (gains(k)*s)*eye(6))); plot(real(cl_poles), imag(cl_poles), '.', ... 'HandleVisibility', 'off'); end end hold off; axis square; xlim([-1e3, 10]); ylim([0, 1e3]); xlabel('Real Part'); ylabel('Imaginary Part'); legend('location', 'northwest'); #+end_src Damping as function of the gain #+begin_src matlab :exports none c1 = [ 0 0.4470 0.7410]; % Blue c2 = [0.8500 0.3250 0.0980]; % Orange c3 = [0.9290 0.6940 0.1250]; % Yellow c4 = [0.4940 0.1840 0.5560]; % Purple c5 = [0.4660 0.6740 0.1880]; % Green c6 = [0.3010 0.7450 0.9330]; % Light Blue c7 = [0.6350 0.0780 0.1840]; % Red colors = [c1; c2; c3; c4; c5; c6; c7]; figure; gains = logspace(4, 7, 100); hold on; for i = 1:length(Ms) for k = 1:length(gains) cl_poles = pole(feedback(Gm_dvf{i}, (gains(k)*s)*eye(6))); set(gca,'ColorOrderIndex',i); plot(gains(k), sin(-pi/2 + angle(cl_poles)), '.', 'color', colors(i, :)); end end hold off; xlabel('DVF Gain'); ylabel('Modal Damping'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylim([0, 1]); #+end_src Finally, we use the following controller for the Decentralized Direct Velocity Feedback: #+begin_src matlab Kdvf = 5e5*s/(1+s/2/pi/1e3)*eye(6); #+end_src ** Effect of the Low Authority Control on the Primary Plant *** Introduction :ignore: #+begin_src matlab :exports none %% Name of the Simulink File mdl = 'nass_model'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror #+end_src #+begin_src matlab :exports none load('mat/stages.mat', 'nano_hexapod'); #+end_src *** Identification of the undamped plant :ignore: #+begin_src matlab :exports none Kdvf_backup = Kdvf; Kdvf = tf(zeros(6)); #+end_src #+begin_src matlab :exports none G_x = {zeros(length(Ms), 1)}; G_l = {zeros(length(Ms), 1)}; #+end_src #+begin_src matlab :exports none for i = 1:length(Ms) initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1)); initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i)); %% Run the linearization G = linearize(mdl, io); G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}; Gx = -G*inv(nano_hexapod.kinematics.J'); Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; G_x(i) = {Gx}; Gl = -nano_hexapod.kinematics.J*G; Gl.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'}; G_l(i) = {Gl}; end #+end_src #+begin_src matlab :exports none Kdvf = Kdvf_backup; #+end_src *** Identification of the damped plant :ignore: #+begin_src matlab :exports none Gm_x = {zeros(length(Ms), 1)}; Gm_l = {zeros(length(Ms), 1)}; #+end_src #+begin_src matlab :exports none for i = 1:length(Ms) initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1)); initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i)); %% Run the linearization G = linearize(mdl, io); G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}; Gx = -G*inv(nano_hexapod.kinematics.J'); Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; Gm_x(i) = {Gx}; Gl = -nano_hexapod.kinematics.J*G; Gl.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'}; Gm_l(i) = {Gl}; end #+end_src *** Effect of the Damping on the plant diagonal dynamics :ignore: #+begin_src matlab :exports none freqs = logspace(0, 3, 5000); figure; ax1 = subplot(2, 2, 1); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz')))); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(G_x{i}(2, 2), freqs, 'Hz')))); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gm_x{i}(1, 1), freqs, 'Hz'))), '--'); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gm_x{i}(2, 2), freqs, 'Hz'))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); title('$\mathcal{X}_x/\mathcal{F}_x$, $\mathcal{X}_y/\mathcal{F}_y$') ax2 = subplot(2, 2, 2); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz')))); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gm_x{i}(3, 3), freqs, 'Hz'))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); title('$\mathcal{X}_z/\mathcal{F}_z$') ax3 = subplot(2, 2, 3); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz'))))); set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(2, 2), freqs, 'Hz'))))); set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(1, 1), freqs, 'Hz')))), '--'); set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(2, 2), freqs, 'Hz')))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); ylim([-270, 90]); yticks([-360:90:360]); ax4 = subplot(2, 2, 4); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz')))), ... 'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i))); set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(3, 3), freqs, 'Hz')))), '--', ... 'HandleVisibility', 'off'); 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', 'southwest'); linkaxes([ax1,ax2,ax3,ax4],'x'); #+end_src #+begin_src matlab :exports none freqs = logspace(0, 3, 5000); figure; ax1 = subplot(2, 1, 1); hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(G_l{i}(1, 1), freqs, 'Hz')))); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gm_l{i}(1, 1), freqs, 'Hz'))), '--'); 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(Ms) set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_l{i}(1, 1), freqs, 'Hz')))), ... 'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i))); set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_l{i}(1, 1), freqs, 'Hz')))), '--', ... 'HandleVisibility', 'off'); 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', 'southwest'); linkaxes([ax1,ax2],'x'); #+end_src ** Control in the leg space We design a diagonal controller with all the same diagonal elements. The requirements for the controller are: - Crossover frequency of around 100Hz - Stable for all the considered payload masses - Sufficient phase and gain margin - Integral action at low frequency The design controller is as follows: - Lead centered around the crossover - An integrator below 10Hz - A low pass filter at 250Hz #+begin_src matlab h = 2.0; Kl = 1e9 * 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 freqs = logspace(0, 3, 1000); figure; ax1 = subplot(2, 1, 1); hold on; for i = 1:length(Ms) for j = 1:6 set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gm_l{i}(j, j)*Kl(j,j), freqs, 'Hz')))); end end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); ax2 = subplot(2, 1, 2); hold on; for i = 1:length(Ms) for j = 1:6 set(gca,'ColorOrderIndex',i); plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_l{i}(j, j)*Kl(j,j), freqs, 'Hz')))); end 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]); linkaxes([ax1,ax2],'x'); #+end_src #+begin_src matlab load('mat/stages.mat', 'nano_hexapod'); K = Kl*nano_hexapod.kinematics.J*diag([1, 1, 1, 1, 1, 0]); #+end_src #+begin_src matlab :exports none for i = 1:length(Ms) isstable(feedback(nano_hexapod.kinematics.J\Gm_l{i}*K, eye(6), -1)) end #+end_src ** Sensibility to Disturbances and Noise Budget *** Identification :ignore: We identify the transfer function from disturbances to the position error of the sample when the HAC-LAC control is applied. #+begin_src matlab :exports none %% Name of the Simulink File mdl = 'nass_model'; %% Micro-Hexapod clear io; io_i = 1; io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwz'); io_i = io_i + 1; % Z Ground motion io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fty_z'); io_i = io_i + 1; % Parasitic force Ty - Z io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Frz_z'); io_i = io_i + 1; % Parasitic force Rz - Z io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fd'); io_i = io_i + 1; % Direct forces io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror #+end_src #+begin_src matlab :exports none Gd_L = {zeros(length(Ms), 1)}; for i = 1:length(Ms) initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1)); initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i)); %% Run the linearization G = linearize(mdl, io); G.InputName = {'Dwz', 'Fty_z', 'Frz_z', 'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'}; G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}; Gd_L(i) = {G}; end #+end_src *** Obtained Sensibility to Disturbances :ignore: #+begin_src matlab :exports none freqs = logspace(0, 3, 5000); figure; subplot(2, 2, 1); title('$D_{w,z}$ to $E_z$'); hold on; for i = 1:length(Ms) % set(gca,'ColorOrderIndex',i); % plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Dwz'), freqs, 'Hz'))), ... % 'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i))); set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd_L{i}('Ez', 'Dwz'), freqs, 'Hz'))), '--', ... 'HandleVisibility', 'off'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/m]'); set(gca, 'XTickLabel',[]); legend('location', 'southeast'); subplot(2, 2, 2); title('$F_{dz}$ to $E_z$'); hold on; for i = 1:length(Ms) % set(gca,'ColorOrderIndex',i); % plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Fdz'), freqs, 'Hz')))); % set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd_L{i}('Ez', 'Fdz'), freqs, 'Hz'))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/N]'); subplot(2, 2, 3); title('$F_{T_y,z}$ to $E_z$'); hold on; for i = 1:length(Ms) % set(gca,'ColorOrderIndex',i); % plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Fty_z'), freqs, 'Hz')))); % set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd_L{i}('Ez', 'Fty_z'), freqs, 'Hz'))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]'); subplot(2, 2, 4); title('$F_{R_z,z}$ to $E_z$'); hold on; for i = 1:length(Ms) % set(gca,'ColorOrderIndex',i); % plot(freqs, abs(squeeze(freqresp(Gd{i}('Ez', 'Frz_z'), freqs, 'Hz')))); % set(gca,'ColorOrderIndex',i); plot(freqs, abs(squeeze(freqresp(Gd_L{i}('Ez', 'Frz_z'), freqs, 'Hz'))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]'); #+end_src *** Noise Budgeting :ignore: #+begin_src matlab :exports none load('./mat/dist_psd.mat', 'dist_f'); #+end_src #+begin_src matlab :exports none figure; hold on; plot(dist_f.f, sqrt(dist_f.psd_gm).*abs(squeeze(freqresp(Gd_L{1}('Ez', 'Dwz' ), dist_f.f, 'Hz'))), 'DisplayName', '$D_w$') plot(dist_f.f, sqrt(dist_f.psd_rz).*abs(squeeze(freqresp(Gd_L{1}('Ez', 'Frz_z'), dist_f.f, 'Hz'))), 'DisplayName', '$F_{R_z}$') hold off; xlabel('Frequency [Hz]'); ylabel('Amplitude Spectral Density [$m/\sqrt{Hz}$]'); set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log'); legend('location', 'southwest'); xlim([1, 500]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/opt_stiff_primary_control_L_psd_dist.pdf', 'width', 'full', 'height', 'tall') #+end_src #+name: fig:opt_stiff_primary_control_L_psd_dist #+caption: Amplitude Spectral Density of the vertical position error of the sample when the HAC-DVF control is applied due to both the ground motion and spindle vibrations #+RESULTS: [[file:figs/opt_stiff_primary_control_L_psd_dist.png]] #+begin_src matlab :exports none figure; hold on; plot(dist_f.f, sqrt(dist_f.psd_gm.*abs(squeeze(freqresp(Gd{1}('Ez', 'Dwz' ), dist_f.f, 'Hz'))).^2 + ... dist_f.psd_rz.*abs(squeeze(freqresp(Gd{1}('Ez', 'Frz_z'), dist_f.f, 'Hz'))).^2), 'DisplayName', 'Open-Loop') plot(dist_f.f, sqrt(dist_f.psd_gm.*abs(squeeze(freqresp(Gd_L{1}('Ez', 'Dwz' ), dist_f.f, 'Hz'))).^2 + ... dist_f.psd_rz.*abs(squeeze(freqresp(Gd_L{1}('Ez', 'Frz_z'), dist_f.f, 'Hz'))).^2), 'DisplayName', 'HAC-DVF') hold off; xlabel('Frequency [Hz]'); ylabel('Amplitude Spectral Density [$m/\sqrt{Hz}$]'); set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log'); legend('location', 'northeast'); xlim([1, 500]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/opt_stiff_primary_control_L_psd_tot.pdf', 'width', 'full', 'height', 'tall') #+end_src #+name: fig:opt_stiff_primary_control_L_psd_tot #+caption: Amplitude Spectral Density of the vertical position error of the sample in Open-Loop and when the HAC-DVF control is applied #+RESULTS: [[file:figs/opt_stiff_primary_control_L_psd_tot.png]] #+begin_src matlab :exports none figure; hold on; for i = 1:length(Ms) set(gca,'ColorOrderIndex',i); plot(dist_f.f, sqrt(flip(-cumtrapz(flip(dist_f.f), flip(dist_f.psd_gm.*abs(squeeze(freqresp(Gd{i}('Ez', 'Dwz' ), dist_f.f, 'Hz'))).^2 + ... dist_f.psd_rz.*abs(squeeze(freqresp(Gd{i}('Ez', 'Frz_z'), dist_f.f, 'Hz'))).^2)))), ... 'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i))); set(gca,'ColorOrderIndex',i); plot(dist_f.f, sqrt(flip(-cumtrapz(flip(dist_f.f), flip(dist_f.psd_gm.*abs(squeeze(freqresp(Gd_L{i}('Ez', 'Dwz' ), dist_f.f, 'Hz'))).^2 + ... dist_f.psd_rz.*abs(squeeze(freqresp(Gd_L{i}('Ez', 'Frz_z'), dist_f.f, 'Hz'))).^2)))), '--', ... 'HandleVisibility', 'off') end hold off; xlabel('Frequency [Hz]'); ylabel('Amplitude Spectral Density [$m/\sqrt{Hz}$]'); set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log'); legend('location', 'southwest'); xlim([0.1, 500]); ylim([1e-12, 1e-6]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'height', 'tall') #+end_src #+name: fig:opt_stiff_primary_control_L_cas_tot #+caption: Cumulative Amplitude Spectrum of the vertical position error of the sample in Open-Loop and when the HAC-DVF control is applied #+RESULTS: [[file:figs/opt_stiff_primary_control_L_cas_tot.png]] ** Simulations of Tomography Experiment Let's now simulate a tomography experiment. To do so, we include all disturbances except vibrations of the translation stage. #+begin_src matlab initializeDisturbances(); initializeSimscapeConfiguration('gravity', false); initializeLoggingConfiguration('log', 'all'); #+end_src #+begin_src matlab :exports none load('mat/conf_simulink.mat'); set_param(conf_simulink, 'StopTime', '2'); #+end_src 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) initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1)); initializeReferences('Rz_type', 'rotating', 'Rz_period', Ms(i)); sim('nass_model'); hac_dvf_L(i) = {simout}; end #+end_src #+begin_src matlab :exports none save('./mat/tomo_exp_hac_dvf_ampl.mat', 'hac_dvf_L', 'Ms'); #+end_src ** Results #+begin_src matlab :exports none load('./mat/experiment_tomography.mat', 'scans_rz_align_dist'); load('./mat/tomo_exp_hac_dvf_ampl.mat', 'hac_dvf_L', 'Ms'); #+end_src #+begin_src matlab :exports none n_av = 4; han_win = hanning(ceil(length(hac_dvf_L{1}.Em.En.Data(:,1))/n_av)); t = hac_dvf_L{1}.Em.En.Time; Ts = t(2)-t(1); [pxx_ol, f] = pwelch(scans_rz_align_dist.Em.En.Data, han_win, [], [], 1/Ts); pxx_dvf_L = zeros(length(f), 6, length(Ms)); for i = 1:length(Ms) [pxx, ~] = pwelch(hac_dvf_L{i}.Em.En.Data(ceil(0.2/Ts):end,:), han_win, [], [], 1/Ts); pxx_dvf_L(:, :, i) = pxx; end #+end_src #+begin_src matlab :exports none figure; ax1 = subplot(2, 3, 1); hold on; 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; xlabel('Frequency [Hz]'); ylabel('$\Gamma_{D_x}$ [$m/\sqrt{Hz}$]'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ax2 = subplot(2, 3, 2); hold on; 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; xlabel('Frequency [Hz]'); ylabel('$\Gamma_{D_y}$ [$m/\sqrt{Hz}$]'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ax3 = subplot(2, 3, 3); hold on; 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; xlabel('Frequency [Hz]'); ylabel('$\Gamma_{D_z}$ [$m/\sqrt{Hz}$]'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ax4 = subplot(2, 3, 4); hold on; 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; xlabel('Frequency [Hz]'); ylabel('$\Gamma_{R_x}$ [$rad/\sqrt{Hz}$]'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ax5 = subplot(2, 3, 5); hold on; 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; xlabel('Frequency [Hz]'); ylabel('$\Gamma_{R_y}$ [$rad/\sqrt{Hz}$]'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ax6 = subplot(2, 3, 6); hold on; 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 hold off; xlabel('Frequency [Hz]'); ylabel('$\Gamma_{R_z}$ [$rad/\sqrt{Hz}$]'); legend('location', 'southwest'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x'); xlim([f(2), f(end)]) #+end_src #+begin_src matlab :exports none figure; ax1 = subplot(2, 3, 1); hold on; 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; xlabel('Frequency [Hz]'); ylabel('CAS $D_x$ [$m$]'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylim([1e-11, 1e-5]); ax2 = subplot(2, 3, 2); hold on; 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; xlabel('Frequency [Hz]'); ylabel('CAS $D_y$ [$m$]'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylim([1e-11, 1e-5]); ax3 = subplot(2, 3, 3); hold on; 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; xlabel('Frequency [Hz]'); ylabel('CAS $D_z$ [$m$]'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylim([1e-11, 1e-5]); ax4 = subplot(2, 3, 4); hold on; 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; xlabel('Frequency [Hz]'); ylabel('CAS $R_x$ [$rad$]'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylim([1e-11, 1e-5]); ax5 = subplot(2, 3, 5); hold on; 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; xlabel('Frequency [Hz]'); ylabel('CAS $R_y$ [$rad$]'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylim([1e-11, 1e-5]); ax6 = subplot(2, 3, 6); hold on; 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 hold off; xlabel('Frequency [Hz]'); ylabel('CAS $R_z$ [$rad$]'); legend('location', 'southwest'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylim([1e-11, 1e-5]); linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x'); xlim([f(2), f(end)]) #+end_src #+begin_src matlab :exports none figure; ax1 = subplot(2, 3, 1); hold on; plot(scans_rz_align_dist.Em.En.Time, scans_rz_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; xlabel('Time [s]'); ylabel('Dx [m]'); ax2 = subplot(2, 3, 2); hold on; plot(scans_rz_align_dist.Em.En.Time, scans_rz_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; xlabel('Time [s]'); ylabel('Dy [m]'); ax3 = subplot(2, 3, 3); hold on; plot(scans_rz_align_dist.Em.En.Time, scans_rz_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; xlabel('Time [s]'); ylabel('Dz [m]'); ax4 = subplot(2, 3, 4); hold on; plot(scans_rz_align_dist.Em.En.Time, scans_rz_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; xlabel('Time [s]'); ylabel('Rx [rad]'); ax5 = subplot(2, 3, 5); hold on; plot(scans_rz_align_dist.Em.En.Time, scans_rz_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; xlabel('Time [s]'); ylabel('Ry [rad]'); ax6 = subplot(2, 3, 6); hold on; plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 6), 'k-', ... '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 hold off; xlabel('Time [s]'); ylabel('Rz [rad]'); legend(); linkaxes([ax1,ax2,ax3,ax4],'x'); xlim([0.5, inf]); #+end_src