Change all the organization of the files

to-order/active_damping_uniaxial/matlab/dvf.m

@@ -0,0 +1,241 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+open 'simscape/sim_nano_station_id.slx'
+
+% Control Design
+% Let's load the undamped plant:
+
+load('./active_damping/mat/plants.mat', 'G');
+
+
+
+% Let's look at the transfer function from actuator forces in the nano-hexapod to the measured velocity of the nano-hexapod platform in the direction of the corresponding actuator for all 6 pairs of actuator/sensor (figure [[fig:dvf_plant]]).
+
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+ax1 = subplot(2, 1, 1);
+hold on;
+for i=1:6
+  plot(freqs, abs(squeeze(freqresp(G.G_geoph(['Vm', num2str(i)], ['F', num2str(i)]), 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:6
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_geoph(['Vm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
+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');
+
+
+
+% #+NAME: fig:dvf_plant
+% #+CAPTION: Transfer function from forces applied in the legs to leg velocity sensor ([[./figs/dvf_plant.png][png]], [[./figs/dvf_plant.pdf][pdf]])
+% [[file:figs/dvf_plant.png]]
+
+% The controller is defined below and the obtained loop gain is shown in figure [[fig:dvf_open_loop_gain]].
+
+
+K_dvf = tf(3e4);
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+ax1 = subplot(2, 1, 1);
+hold on;
+for i=1:6
+  plot(freqs, abs(squeeze(freqresp(K_dvf*G.G_geoph(['Vm', num2str(i)], ['F', num2str(i)]), 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:6
+  plot(freqs, 180/pi*angle(squeeze(freqresp(K_dvf*G.G_geoph(['Vm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
+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');
+
+% Identification of the damped plant
+% Let's initialize the system prior to identification.
+
+initializeGround();
+initializeGranite();
+initializeTy();
+initializeRy();
+initializeRz();
+initializeMicroHexapod();
+initializeAxisc();
+initializeMirror();
+initializeNanoHexapod(struct('actuator', 'piezo'));
+initializeSample(struct('mass', 50));
+
+
+
+% And initialize the controllers.
+
+K = tf(zeros(6));
+save('./mat/controllers.mat', 'K', '-append');
+K_iff = tf(zeros(6));
+save('./mat/controllers.mat', 'K_iff', '-append');
+K_rmc = tf(zeros(6));
+save('./mat/controllers.mat', 'K_rmc', '-append');
+K_dvf = -K_dvf*eye(6);
+save('./mat/controllers.mat', 'K_dvf', '-append');
+
+
+
+% We identify the system dynamics now that the RMC controller is ON.
+
+G_dvf = identifyPlant();
+
+
+
+% And we save the damped plant for further analysis.
+
+save('./active_damping/mat/plants.mat', 'G_dvf', '-append');
+
+% Sensitivity to disturbances
+
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+subplot(2, 1, 1);
+title('$D_g$ to $D$');
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / D_{g,x}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / D_{g,y}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / D_{g,z}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
+legend('location', 'northeast');
+
+subplot(2, 1, 2);
+title('$F_s$ to $D$');
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{s,x}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{s,y}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{s,z}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+legend('location', 'northeast');
+
+
+
+% #+NAME: fig:sensitivity_dist_dvf
+% #+CAPTION: Sensitivity to disturbance once the DVF controller is applied to the system ([[./figs/sensitivity_dist_dvf.png][png]], [[./figs/sensitivity_dist_dvf.pdf][pdf]])
+% [[file:figs/sensitivity_dist_dvf.png]]
+
+
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{rz, z}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{ty, z}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{ty, x}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+legend('location', 'northeast');
+
+% Damped Plant
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+ax1 = subplot(2, 2, 1);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+
+ax2 = subplot(2, 2, 2);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_dvf.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [rad/(Nm)]'); xlabel('Frequency [Hz]');
+
+ax3 = subplot(2, 2, 3);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{n,x}\right|$');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{n,y}\right|$');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{n,z}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+legend('location', 'northwest');
+
+ax4 = subplot(2, 2, 4);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$\left|R_x / M_{n,x}\right|$');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$\left|R_y / M_{n,y}\right|$');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$\left|R_z / M_{n,z}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+legend('location', 'northwest');
+
+linkaxes([ax1,ax2,ax3,ax4],'x');

to-order/active_damping_uniaxial/matlab/iff.m

@@ -0,0 +1,281 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+open 'simscape/sim_nano_station_id.slx'
+
+% Control Design
+% Let's load the undamped plant:
+
+load('./active_damping/mat/plants.mat', 'G');
+
+
+
+% Let's look at the transfer function from actuator forces in the nano-hexapod to the force sensor in the nano-hexapod legs for all 6 pairs of actuator/sensor (figure [[fig:iff_plant]]).
+
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+ax1 = subplot(2, 1, 1);
+hold on;
+for i=1:6
+  plot(freqs, abs(squeeze(freqresp(G.G_iff(['Fm', num2str(i)], ['F', num2str(i)]), 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:6
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_iff(['Fm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
+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');
+
+
+
+% #+NAME: fig:iff_plant
+% #+CAPTION: Transfer function from forces applied in the legs to force sensor ([[./figs/iff_plant.png][png]], [[./figs/iff_plant.pdf][pdf]])
+% [[file:figs/iff_plant.png]]
+
+% The controller for each pair of actuator/sensor is:
+
+K_iff = -1000/s;
+
+
+
+% The corresponding loop gains are shown in figure [[fig:iff_open_loop]].
+
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+ax1 = subplot(2, 1, 1);
+hold on;
+for i=1:6
+  plot(freqs, abs(squeeze(freqresp(K_iff*G.G_iff(['Fm', num2str(i)], ['F', num2str(i)]), 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:6
+  plot(freqs, 180/pi*angle(squeeze(freqresp(K_iff*G.G_iff(['Fm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
+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');
+
+% Identification of the damped plant
+% Let's initialize the system prior to identification.
+
+initializeGround();
+initializeGranite();
+initializeTy();
+initializeRy();
+initializeRz();
+initializeMicroHexapod();
+initializeAxisc();
+initializeMirror();
+initializeNanoHexapod(struct('actuator', 'piezo'));
+initializeSample(struct('mass', 50));
+
+
+
+% All the controllers are set to 0.
+
+K = tf(zeros(6));
+save('./mat/controllers.mat', 'K', '-append');
+K_iff = -K_iff*eye(6);
+save('./mat/controllers.mat', 'K_iff', '-append');
+K_rmc = tf(zeros(6));
+save('./mat/controllers.mat', 'K_rmc', '-append');
+K_dvf = tf(zeros(6));
+save('./mat/controllers.mat', 'K_dvf', '-append');
+
+
+
+% We identify the system dynamics now that the IFF controller is ON.
+
+G_iff = identifyPlant();
+
+
+
+% And we save the damped plant for further analysis
+
+save('./active_damping/mat/plants.mat', 'G_iff', '-append');
+
+% Sensitivity to disturbances
+% As shown on figure [[fig:sensitivity_dist_iff]]:
+% - The top platform of the nano-hexapod how behaves as a "free-mass".
+% - The transfer function from direct forces $F_s$ to the relative displacement $D$ is equivalent to the one of an isolated mass.
+% - The transfer function from ground motion $D_g$ to the relative displacement $D$ tends to the transfer function from $D_g$ to the displacement of the granite (the sample is being isolated thanks to IFF).
+%   However, as the goal is to make the relative displacement $D$ as small as possible (e.g. to make the sample motion follows the granite motion), this is not a good thing.
+
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+subplot(2, 1, 1);
+title('$D_g$ to $D$');
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / D_{g,x}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / D_{g,y}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / D_{g,z}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_iff.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_iff.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_iff.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
+legend('location', 'northeast');
+
+subplot(2, 1, 2);
+title('$F_s$ to $D$');
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{s,x}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{s,y}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{s,z}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_iff.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_iff.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_iff.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+legend('location', 'northeast');
+
+
+
+% #+NAME: fig:sensitivity_dist_iff
+% #+CAPTION: Sensitivity to disturbance once the IFF controller is applied to the system ([[./figs/sensitivity_dist_iff.png][png]], [[./figs/sensitivity_dist_iff.pdf][pdf]])
+% [[file:figs/sensitivity_dist_iff.png]]
+
+% #+begin_warning
+%   The order of the models are very high and thus the plots may be wrong.
+%   For instance, the plots are not the same when using =minreal=.
+% #+end_warning
+
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{rz, z}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{ty, z}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{ty, x}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(minreal(prescale(G_iff.G_dist('Dz', 'Frzz'), {2*pi, 2*pi*1e3})), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(minreal(G_iff.G_dist('Dz', 'Ftyz')), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(minreal(G_iff.G_dist('Dx', 'Ftyx')), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+legend('location', 'northeast');
+
+% Damped Plant
+% Now, look at the new damped plant to control.
+
+% It damps the plant (resonance of the nano hexapod as well as other resonances) as shown in figure [[fig:plant_iff_damped]].
+
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+ax1 = subplot(2, 2, 1);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+
+ax2 = subplot(2, 2, 2);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_iff.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [rad/(Nm)]'); xlabel('Frequency [Hz]');
+
+ax3 = subplot(2, 2, 3);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{n,x}\right|$');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{n,y}\right|$');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{n,z}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+legend('location', 'northwest');
+
+ax4 = subplot(2, 2, 4);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$\left|R_x / M_{n,x}\right|$');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$\left|R_y / M_{n,y}\right|$');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$\left|R_z / M_{n,z}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+legend('location', 'northwest');
+
+linkaxes([ax1,ax2,ax3,ax4],'x');
+
+
+
+% #+NAME: fig:plant_iff_damped
+% #+CAPTION: Damped Plant after IFF is applied ([[./figs/plant_iff_damped.png][png]], [[./figs/plant_iff_damped.pdf][pdf]])
+% [[file:figs/plant_iff_damped.png]]
+
+% However, it increases coupling at low frequency (figure [[fig:plant_iff_coupling]]).
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+for ix = 1:6
+  for iy = 1:6
+    subplot(6, 6, (ix-1)*6 + iy);
+    hold on;
+    plot(freqs, abs(squeeze(freqresp(G.G_cart(ix, iy), freqs, 'Hz'))), 'k-');
+    plot(freqs, abs(squeeze(freqresp(G_iff.G_cart(ix, iy), freqs, 'Hz'))), 'k--');
+    set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+    ylim([1e-12, 1e-5]);
+  end
+end

to-order/active_damping_uniaxial/matlab/rmc.m

@@ -0,0 +1,246 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+open 'simscape/sim_nano_station_id.slx'
+
+% Control Design
+% Let's load the undamped plant:
+
+load('./active_damping/mat/plants.mat', 'G');
+
+
+
+% Let's look at the transfer function from actuator forces in the nano-hexapod to the measured displacement of the actuator for all 6 pairs of actuator/sensor (figure [[fig:rmc_plant]]).
+
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+ax1 = subplot(2, 1, 1);
+hold on;
+for i=1:6
+  plot(freqs, abs(squeeze(freqresp(G.G_dleg(['Dm', num2str(i)], ['F', num2str(i)]), 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:6
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_dleg(['Dm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
+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');
+
+
+
+% #+NAME: fig:rmc_plant
+% #+CAPTION: Transfer function from forces applied in the legs to leg displacement sensor ([[./figs/rmc_plant.png][png]], [[./figs/rmc_plant.pdf][pdf]])
+% [[file:figs/rmc_plant.png]]
+
+% The Relative Motion Controller is defined below.
+% A Low pass Filter is added to make the controller transfer function proper.
+
+K_rmc = s*50000/(1 + s/2/pi/10000);
+
+
+
+% The obtained loop gains are shown in figure [[fig:rmc_open_loop]].
+
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+ax1 = subplot(2, 1, 1);
+hold on;
+for i=1:6
+  plot(freqs, abs(squeeze(freqresp(K_rmc*G.G_dleg(['Dm', num2str(i)], ['F', num2str(i)]), 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:6
+  plot(freqs, 180/pi*angle(squeeze(freqresp(K_rmc*G.G_dleg(['Dm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))));
+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');
+
+% Identification of the damped plant
+% Let's initialize the system prior to identification.
+
+initializeGround();
+initializeGranite();
+initializeTy();
+initializeRy();
+initializeRz();
+initializeMicroHexapod();
+initializeAxisc();
+initializeMirror();
+initializeNanoHexapod(struct('actuator', 'piezo'));
+initializeSample(struct('mass', 50));
+
+
+
+% And initialize the controllers.
+
+K = tf(zeros(6));
+save('./mat/controllers.mat', 'K', '-append');
+K_iff = tf(zeros(6));
+save('./mat/controllers.mat', 'K_iff', '-append');
+K_rmc = -K_rmc*eye(6);
+save('./mat/controllers.mat', 'K_rmc', '-append');
+K_dvf = tf(zeros(6));
+save('./mat/controllers.mat', 'K_dvf', '-append');
+
+
+
+% We identify the system dynamics now that the RMC controller is ON.
+
+G_rmc = identifyPlant();
+
+
+
+% And we save the damped plant for further analysis.
+
+save('./active_damping/mat/plants.mat', 'G_rmc', '-append');
+
+% Sensitivity to disturbances
+% As shown in figure [[fig:sensitivity_dist_rmc]], RMC control succeed in lowering the sensitivity to disturbances near resonance of the system.
+
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+subplot(2, 1, 1);
+title('$D_g$ to $D$');
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / D_{g,x}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / D_{g,y}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / D_{g,z}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_rmc.G_gm('Dx', 'Dgx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_rmc.G_gm('Dy', 'Dgy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_rmc.G_gm('Dz', 'Dgz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
+legend('location', 'northeast');
+
+subplot(2, 1, 2);
+title('$F_s$ to $D$');
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{s,x}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{s,y}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{s,z}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_rmc.G_fs('Dx', 'Fsx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_rmc.G_fs('Dy', 'Fsy'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_rmc.G_fs('Dz', 'Fsz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+legend('location', 'northeast');
+
+
+
+% #+NAME: fig:sensitivity_dist_rmc
+% #+CAPTION: Sensitivity to disturbance once the RMC controller is applied to the system ([[./figs/sensitivity_dist_rmc.png][png]], [[./figs/sensitivity_dist_rmc.pdf][pdf]])
+% [[file:figs/sensitivity_dist_rmc.png]]
+
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{rz, z}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{ty, z}\right|$');
+plot(freqs, abs(squeeze(freqresp(G.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{ty, x}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_rmc.G_dist('Dz', 'Frzz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_rmc.G_dist('Dz', 'Ftyz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, abs(squeeze(freqresp(G_rmc.G_dist('Dx', 'Ftyx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+legend('location', 'northeast');
+
+% Damped Plant
+
+freqs = logspace(0, 3, 1000);
+
+figure;
+
+ax1 = subplot(2, 2, 1);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_rmc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_rmc.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_rmc.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
+
+ax2 = subplot(2, 2, 2);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(G_rmc.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_rmc.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_rmc.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [rad/(Nm)]'); xlabel('Frequency [Hz]');
+
+ax3 = subplot(2, 2, 3);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), 'DisplayName', '$\left|D_x / F_{n,x}\right|$');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dy', 'Fny'), freqs, 'Hz'))), 'DisplayName', '$\left|D_y / F_{n,y}\right|$');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), 'DisplayName', '$\left|D_z / F_{n,z}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_rmc.G_cart('Dx', 'Fnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_rmc.G_cart('Dy', 'Fny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_rmc.G_cart('Dz', 'Fnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+legend('location', 'northwest');
+
+ax4 = subplot(2, 2, 4);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), 'DisplayName', '$\left|R_x / M_{n,x}\right|$');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Ry', 'Mny'), freqs, 'Hz'))), 'DisplayName', '$\left|R_y / M_{n,y}\right|$');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), 'DisplayName', '$\left|R_z / M_{n,z}\right|$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_rmc.G_cart('Rx', 'Mnx'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_rmc.G_cart('Ry', 'Mny'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_rmc.G_cart('Rz', 'Mnz'), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+ylim([-180, 180]);
+yticks([-180, -90, 0, 90, 180]);
+legend('location', 'northwest');
+
+linkaxes([ax1,ax2,ax3,ax4],'x');