diff --git a/gravimeter/script.m b/gravimeter/script.m index 5b89318..c7d3b57 100644 --- a/gravimeter/script.m +++ b/gravimeter/script.m @@ -13,10 +13,10 @@ open('gravimeter.slx') % Parameters l = 1.0; % Length of the mass [m] -la = 0.5; % Position of Act. [m] +h = 1.7; % Height of the mass [m] -h = 3.4; % Height of the mass [m] -ha = 1.7; % Position of Act. [m] +la = l/2; % Position of Act. [m] +ha = h/2; % Position of Act. [m] m = 400; % Mass [kg] I = 115; % Inertia [kg m^2] @@ -47,20 +47,36 @@ G = linearize(mdl, io); G.InputName = {'F1', 'F2', 'F3'}; G.OutputName = {'Ax1', 'Az1', 'Ax2', 'Az2'}; + + +% #+name: fig:gravimeter_plant_schematic +% #+caption: Schematic of the gravimeter plant +% #+RESULTS: +% [[file:figs/gravimeter_plant_schematic.png]] + +% \begin{equation} +% \bm{a} = \begin{bmatrix} a_{1x} \\ a_{1z} \\ a_{2x} \\ a_{2z} \end{bmatrix} +% \end{equation} + +% \begin{equation} +% \bm{\tau} = \begin{bmatrix}\tau_1 \\ \tau_2 \\ \tau_2 \end{bmatrix} +% \end{equation} + +% We can check the poles of the plant: + + pole(G) % #+RESULTS: % #+begin_example -% pole(G) -% ans = -% -0.000473481142385795 + 21.7596190728632i -% -0.000473481142385795 - 21.7596190728632i -% -7.49842879459172e-05 + 8.6593576906982i -% -7.49842879459172e-05 - 8.6593576906982i -% -5.1538686792578e-06 + 2.27025295182756i -% -5.1538686792578e-06 - 2.27025295182756i +% -0.000183495485977108 + 13.546056874877i +% -0.000183495485977108 - 13.546056874877i +% -7.49842878906757e-05 + 8.65934902322567i +% -7.49842878906757e-05 - 8.65934902322567i +% -1.33171230256362e-05 + 3.64924169037897i +% -1.33171230256362e-05 - 3.64924169037897i % #+end_example % The plant as 6 states as expected (2 translations + 1 rotation) @@ -72,408 +88,463 @@ size(G) % #+RESULTS: % : State-space model with 4 outputs, 3 inputs, and 6 states. +% The bode plot of all elements of the plant are shown in Figure [[fig:open_loop_tf]]. -freqs = logspace(-2, 2, 1000); + +freqs = logspace(-1, 2, 1000); figure; -for in_i = 1:3 - for out_i = 1:4 - subplot(4, 3, 3*(out_i-1)+in_i); +tiledlayout(4, 3, 'TileSpacing', 'None', 'Padding', 'None'); + +for out_i = 1:4 + for in_i = 1:3 + nexttile; plot(freqs, abs(squeeze(freqresp(G(out_i,in_i), freqs, 'Hz'))), '-'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + xlim([1e-1, 2e1]); ylim([1e-4, 1e0]); + + if in_i == 1 + ylabel('Amplitude [m/N]') + else + set(gca, 'YTickLabel',[]); + end + + if out_i == 4 + xlabel('Frequency [Hz]') + else + set(gca, 'XTickLabel',[]); + end end end -% System Identification - With Gravity -g = 9.80665; % Gravity [m/s2] -Gg = linearize(mdl, io); -Gg.InputName = {'F1', 'F2', 'F3'}; -Gg.OutputName = {'Ax1', 'Az1', 'Ax2', 'Az2'}; +% #+name: fig:gravimeter_decouple_jacobian +% #+caption: Decoupled plant $\bm{G}_x$ using the Jacobian matrix $J$ +% #+RESULTS: +% [[file:figs/gravimeter_decouple_jacobian.png]] + +% The jacobian corresponding to the sensors and actuators are defined below. + +Ja = [1 0 h/2 + 0 1 -l/2 + 1 0 -h/2 + 0 1 0]; + +Jt = [1 0 ha + 0 1 -la + 0 1 la]; + +Gx = pinv(Ja)*G*pinv(Jt'); +Gx.InputName = {'Fx', 'Fz', 'My'}; +Gx.OutputName = {'Dx', 'Dz', 'Ry'}; -% We can now see that the system is unstable due to gravity. +% The diagonal and off-diagonal elements of $G_x$ are shown in Figure [[fig:gravimeter_jacobian_plant]]. -pole(Gg) + +freqs = logspace(-1, 2, 1000); + +figure; + +% Magnitude +hold on; +for i_in = 1:3 + for i_out = [1:i_in-1, i_in+1:3] + plot(freqs, abs(squeeze(freqresp(Gx(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(freqs, abs(squeeze(freqresp(Gx(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ... + 'DisplayName', '$G_x(i,j)\ i \neq j$'); +set(gca,'ColorOrderIndex',1) +for i_in_out = 1:3 + plot(freqs, abs(squeeze(freqresp(Gx(i_in_out, i_in_out), freqs, 'Hz'))), 'DisplayName', sprintf('$G_x(%d,%d)$', i_in_out, i_in_out)); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Magnitude'); +legend('location', 'southeast'); +ylim([1e-8, 1e0]); + +% Real Approximation of $G$ at the decoupling frequency +% <> + +% Let's compute a real approximation of the complex matrix $H_1$ which corresponds to the the transfer function $G_u(j\omega_c)$ from forces applied by the actuators to the measured acceleration of the top platform evaluated at the frequency $\omega_c$. + +wc = 2*pi*10; % Decoupling frequency [rad/s] + +H1 = evalfr(G, j*wc); + + + +% The real approximation is computed as follows: + +D = pinv(real(H1'*H1)); +H1 = inv(D*real(H1'*diag(exp(j*angle(diag(H1*D*H1.'))/2)))); + +% SVD Decoupling +% <> + +% First, the Singular Value Decomposition of $H_1$ is performed: +% \[ H_1 = U \Sigma V^H \] + + +[U,~,V] = svd(H1); + + + +% #+name: fig:gravimeter_decouple_svd +% #+caption: Decoupled plant $\bm{G}_{SVD}$ using the Singular Value Decomposition +% #+RESULTS: +% [[file:figs/gravimeter_decouple_svd.png]] + +% The decoupled plant is then: +% \[ G_{SVD}(s) = U^{-1} G_u(s) V^{-H} \] + + +Gsvd = inv(U)*G*inv(V'); + + + +% The diagonal and off-diagonal elements of the "SVD" plant are shown in Figure [[fig:gravimeter_svd_plant]]. + + +freqs = logspace(-1, 2, 1000); + +figure; + +% Magnitude +hold on; +for i_in = 1:3 + for i_out = [1:i_in-1, i_in+1:3] + plot(freqs, abs(squeeze(freqresp(Gsvd(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(freqs, abs(squeeze(freqresp(Gsvd(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ... + 'DisplayName', '$G_x(i,j)\ i \neq j$'); +set(gca,'ColorOrderIndex',1) +for i_in_out = 1:3 + plot(freqs, abs(squeeze(freqresp(Gsvd(i_in_out, i_in_out), freqs, 'Hz'))), 'DisplayName', sprintf('$G_x(%d,%d)$', i_in_out, i_in_out)); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Magnitude'); +legend('location', 'southeast', 'FontSize', 8); +ylim([1e-8, 1e0]); + +% TODO Verification of the decoupling using the "Gershgorin Radii" +% <> + +% The "Gershgorin Radii" is computed for the coupled plant $G(s)$, for the "Jacobian plant" $G_x(s)$ and the "SVD Decoupled Plant" $G_{SVD}(s)$: + +% The "Gershgorin Radii" of a matrix $S$ is defined by: +% \[ \zeta_i(j\omega) = \frac{\sum\limits_{j\neq i}|S_{ij}(j\omega)|}{|S_{ii}(j\omega)|} \] + +% This is computed over the following frequencies. + +freqs = logspace(-2, 2, 1000); % [Hz] + +% Gershgorin Radii for the coupled plant: +Gr_coupled = zeros(length(freqs), size(Gu,2)); +H = abs(squeeze(freqresp(Gu, freqs, 'Hz'))); +for out_i = 1:size(Gu,2) + Gr_coupled(:, out_i) = squeeze((sum(H(out_i,:,:)) - H(out_i,out_i,:))./H(out_i, out_i, :)); +end + +% Gershgorin Radii for the decoupled plant using SVD: +Gr_decoupled = zeros(length(freqs), size(Gsvd,2)); +H = abs(squeeze(freqresp(Gsvd, freqs, 'Hz'))); +for out_i = 1:size(Gsvd,2) + Gr_decoupled(:, out_i) = squeeze((sum(H(out_i,:,:)) - H(out_i,out_i,:))./H(out_i, out_i, :)); +end + +% Gershgorin Radii for the decoupled plant using the Jacobian: +Gr_jacobian = zeros(length(freqs), size(Gx,2)); +H = abs(squeeze(freqresp(Gx, freqs, 'Hz'))); +for out_i = 1:size(Gx,2) + Gr_jacobian(:, out_i) = squeeze((sum(H(out_i,:,:)) - H(out_i,out_i,:))./H(out_i, out_i, :)); +end + +figure; +hold on; +plot(freqs, Gr_coupled(:,1), 'DisplayName', 'Coupled'); +plot(freqs, Gr_decoupled(:,1), 'DisplayName', 'SVD'); +plot(freqs, Gr_jacobian(:,1), 'DisplayName', 'Jacobian'); +for in_i = 2:6 + set(gca,'ColorOrderIndex',1) + plot(freqs, Gr_coupled(:,in_i), 'HandleVisibility', 'off'); + set(gca,'ColorOrderIndex',2) + plot(freqs, Gr_decoupled(:,in_i), 'HandleVisibility', 'off'); + set(gca,'ColorOrderIndex',3) + plot(freqs, Gr_jacobian(:,in_i), 'HandleVisibility', 'off'); +end +plot(freqs, 0.5*ones(size(freqs)), 'k--', 'DisplayName', 'Limit') +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +hold off; +xlabel('Frequency (Hz)'); ylabel('Gershgorin Radii') +legend('location', 'northwest'); +ylim([1e-3, 1e3]); + +% TODO Obtained Decoupled Plants +% <> + +% The bode plot of the diagonal and off-diagonal elements of $G_{SVD}$ are shown in Figure [[fig:simscape_model_decoupled_plant_svd]]. + + +freqs = logspace(-1, 2, 1000); + +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +% Magnitude +ax1 = nexttile([2, 1]); +hold on; +for i_in = 1:6 + for i_out = [1:i_in-1, i_in+1:6] + plot(freqs, abs(squeeze(freqresp(Gsvd(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(freqs, abs(squeeze(freqresp(Gsvd(1, 2), freqs, 'Hz'))), 'color', [0,0,0,0.5], ... + 'DisplayName', '$G_{SVD}(i,j),\ i \neq j$'); +set(gca,'ColorOrderIndex',1) +for ch_i = 1:6 + plot(freqs, abs(squeeze(freqresp(Gsvd(ch_i, ch_i), freqs, 'Hz'))), ... + 'DisplayName', sprintf('$G_{SVD}(%i,%i)$', ch_i, ch_i)); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Magnitude'); set(gca, 'XTickLabel',[]); +legend('location', 'northwest'); +ylim([1e-1, 1e5]) + +% Phase +ax2 = nexttile; +hold on; +for ch_i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(Gsvd(ch_i, ch_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:360]); + +linkaxes([ax1,ax2],'x'); + + + +% #+name: fig:simscape_model_decoupled_plant_svd +% #+caption: Decoupled Plant using SVD +% #+RESULTS: +% [[file:figs/simscape_model_decoupled_plant_svd.png]] + +% Similarly, the bode plots of the diagonal elements and off-diagonal elements of the decoupled plant $G_x(s)$ using the Jacobian are shown in Figure [[fig:simscape_model_decoupled_plant_jacobian]]. + + +freqs = logspace(-1, 2, 1000); + +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +% Magnitude +ax1 = nexttile([2, 1]); +hold on; +for i_in = 1:6 + for i_out = [1:i_in-1, i_in+1:6] + plot(freqs, abs(squeeze(freqresp(Gx(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(freqs, abs(squeeze(freqresp(Gx(1, 2), freqs, 'Hz'))), 'color', [0,0,0,0.5], ... + 'DisplayName', '$G_x(i,j),\ i \neq j$'); +set(gca,'ColorOrderIndex',1) +plot(freqs, abs(squeeze(freqresp(Gx('Ax', 'Fx'), freqs, 'Hz'))), 'DisplayName', '$G_x(1,1) = A_x/F_x$'); +plot(freqs, abs(squeeze(freqresp(Gx('Ay', 'Fy'), freqs, 'Hz'))), 'DisplayName', '$G_x(2,2) = A_y/F_y$'); +plot(freqs, abs(squeeze(freqresp(Gx('Az', 'Fz'), freqs, 'Hz'))), 'DisplayName', '$G_x(3,3) = A_z/F_z$'); +plot(freqs, abs(squeeze(freqresp(Gx('Arx', 'Mx'), freqs, 'Hz'))), 'DisplayName', '$G_x(4,4) = A_{R_x}/M_x$'); +plot(freqs, abs(squeeze(freqresp(Gx('Ary', 'My'), freqs, 'Hz'))), 'DisplayName', '$G_x(5,5) = A_{R_y}/M_y$'); +plot(freqs, abs(squeeze(freqresp(Gx('Arz', 'Mz'), freqs, 'Hz'))), 'DisplayName', '$G_x(6,6) = A_{R_z}/M_z$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Magnitude'); set(gca, 'XTickLabel',[]); +legend('location', 'northwest'); +ylim([1e-2, 2e6]) + +% Phase +ax2 = nexttile; +hold on; +plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Ax', 'Fx'), freqs, 'Hz')))); +plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Ay', 'Fy'), freqs, 'Hz')))); +plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Az', 'Fz'), freqs, 'Hz')))); +plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Arx', 'Mx'), freqs, 'Hz')))); +plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Ary', 'My'), freqs, 'Hz')))); +plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Arz', 'Mz'), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([0, 180]); +yticks([0:45:360]); + +linkaxes([ax1,ax2],'x'); + + + +% #+name: fig:svd_control +% #+caption: Control Diagram for the SVD control +% #+RESULTS: +% [[file:figs/svd_control.png]] + + +% We choose the controller to be a low pass filter: +% \[ K_c(s) = \frac{G_0}{1 + \frac{s}{\omega_0}} \] + +% $G_0$ is tuned such that the crossover frequency corresponding to the diagonal terms of the loop gain is equal to $\omega_c$ + + +wc = 2*pi*80; % Crossover Frequency [rad/s] +w0 = 2*pi*0.1; % Controller Pole [rad/s] + +K_cen = diag(1./diag(abs(evalfr(Gx, j*wc))))*(1/abs(evalfr(1/(1 + s/w0), j*wc)))/(1 + s/w0); +L_cen = K_cen*Gx; +G_cen = feedback(G, pinv(J')*K_cen, [7:12], [1:6]); + +K_svd = diag(1./diag(abs(evalfr(Gsvd, j*wc))))*(1/abs(evalfr(1/(1 + s/w0), j*wc)))/(1 + s/w0); +L_svd = K_svd*Gsvd; +G_svd = feedback(G, inv(V')*K_svd*inv(U), [7:12], [1:6]); + + + +% The obtained diagonal elements of the loop gains are shown in Figure [[fig:gravimeter_comp_loop_gain_diagonal]]. + + +freqs = logspace(-1, 2, 1000); + +figure; +tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + +% Magnitude +ax1 = nexttile([2, 1]); +hold on; +plot(freqs, abs(squeeze(freqresp(L_svd(1, 1), freqs, 'Hz'))), 'DisplayName', '$L_{SVD}(i,i)$'); +for i_in_out = 2:6 + set(gca,'ColorOrderIndex',1) + plot(freqs, abs(squeeze(freqresp(L_svd(i_in_out, i_in_out), freqs, 'Hz'))), 'HandleVisibility', 'off'); +end + +set(gca,'ColorOrderIndex',2) +plot(freqs, abs(squeeze(freqresp(L_cen(1, 1), freqs, 'Hz'))), ... + 'DisplayName', '$L_{J}(i,i)$'); +for i_in_out = 2:6 + set(gca,'ColorOrderIndex',2) + plot(freqs, abs(squeeze(freqresp(L_cen(i_in_out, i_in_out), freqs, 'Hz'))), 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Magnitude'); set(gca, 'XTickLabel',[]); +legend('location', 'northwest'); +ylim([5e-2, 2e3]) + +% Phase +ax2 = nexttile; +hold on; +for i_in_out = 1:6 + set(gca,'ColorOrderIndex',1) + plot(freqs, 180/pi*angle(squeeze(freqresp(L_svd(i_in_out, i_in_out), freqs, 'Hz')))); +end +set(gca,'ColorOrderIndex',2) +for i_in_out = 1:6 + set(gca,'ColorOrderIndex',2) + plot(freqs, 180/pi*angle(squeeze(freqresp(L_cen(i_in_out, i_in_out), 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:360]); + +linkaxes([ax1,ax2],'x'); + +% TODO Closed-Loop system Performances +% <> + +% Let's first verify the stability of the closed-loop systems: + +isstable(G_cen) % #+RESULTS: -% #+begin_example -% pole(Gg) -% ans = -% -10.9848275341252 + 0i -% 10.9838836405201 + 0i -% -7.49855379478109e-05 + 8.65962885770051i -% -7.49855379478109e-05 - 8.65962885770051i -% -6.68819548733559e-06 + 0.832960422243848i -% -6.68819548733559e-06 - 0.832960422243848i -% #+end_example +% : ans = +% : logical +% : 1 + + +isstable(G_svd) + + + +% #+RESULTS: +% : ans = +% : logical +% : 1 + +% The obtained transmissibility in Open-loop, for the centralized control as well as for the SVD control are shown in Figure [[fig:gravimeter_platform_simscape_cl_transmissibility]]. freqs = logspace(-2, 2, 1000); figure; -for in_i = 1:3 - for out_i = 1:4 - subplot(4, 3, 3*(out_i-1)+in_i); - hold on; - plot(freqs, abs(squeeze(freqresp(G(out_i,in_i), freqs, 'Hz'))), '-'); - plot(freqs, abs(squeeze(freqresp(Gg(out_i,in_i), freqs, 'Hz'))), '-'); - hold off; - set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - end -end - -% Parameters -% Bode options. - -P = bodeoptions; -P.FreqUnits = 'Hz'; -P.MagUnits = 'abs'; -P.MagScale = 'log'; -P.Grid = 'on'; -P.PhaseWrapping = 'on'; -P.Title.FontSize = 14; -P.XLabel.FontSize = 14; -P.YLabel.FontSize = 14; -P.TickLabel.FontSize = 12; -P.Xlim = [1e-1,1e2]; -P.MagLowerLimMode = 'manual'; -P.MagLowerLim= 1e-3; - - - -% Frequency vector. - -w = 2*pi*logspace(-1,2,1000); % [rad/s] - -% Generation of the State Space Model -% Mass matrix - -M = [m 0 0 - 0 m 0 - 0 0 I]; - - - -% Jacobian of the bottom sensor - -Js1 = [1 0 h/2 - 0 1 -l/2]; - - - -% Jacobian of the top sensor - -Js2 = [1 0 -h/2 - 0 1 0]; - - - -% Jacobian of the actuators - -Ja = [1 0 ha % Left horizontal actuator - 0 1 -la % Left vertical actuator - 0 1 la]; % Right vertical actuator -Jta = Ja'; - - - -% Stiffness and Damping matrices - -K = k*Jta*Ja; -C = c*Jta*Ja; - - - -% State Space Matrices - -E = [1 0 0 - 0 1 0 - 0 0 1]; %projecting ground motion in the directions of the legs - -AA = [zeros(3) eye(3) - -M\K -M\C]; - -BB = [zeros(3,6) - M\Jta M\(k*Jta*E)]; - -CC = [[Js1;Js2] zeros(4,3); - zeros(2,6) - (Js1+Js2)./2 zeros(2,3) - (Js1-Js2)./2 zeros(2,3) - (Js1-Js2)./(2*h) zeros(2,3)]; - -DD = [zeros(4,6) - zeros(2,3) eye(2,3) - zeros(6,6)]; - - - -% State Space model: -% - Input = three actuators and three ground motions -% - Output = the bottom sensor; the top sensor; the ground motion; the half sum; the half difference; the rotation - - -system_dec = ss(AA,BB,CC,DD); - -size(system_dec) - -% Comparison with the Simscape Model - -freqs = logspace(-2, 2, 1000); - -figure; -for in_i = 1:3 - for out_i = 1:4 - subplot(4, 3, 3*(out_i-1)+in_i); - hold on; - plot(freqs, abs(squeeze(freqresp(G(out_i,in_i), freqs, 'Hz'))), '-'); - plot(freqs, abs(squeeze(freqresp(system_dec(out_i,in_i)*s^2, freqs, 'Hz'))), '-'); - hold off; - set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - end -end - -% Analysis - -% figure -% bode(system_dec,P); -% return - -%% svd decomposition -% system_dec_freq = freqresp(system_dec,w); -% S = zeros(3,length(w)); -% for m = 1:length(w) -% S(:,m) = svd(system_dec_freq(1:4,1:3,m)); -% end -% figure -% loglog(w./(2*pi), S);hold on; -% % loglog(w./(2*pi), abs(Val(1,:)),w./(2*pi), abs(Val(2,:)),w./(2*pi), abs(Val(3,:))); -% xlabel('Frequency [Hz]');ylabel('Singular Value [-]'); -% legend('\sigma_1','\sigma_2','\sigma_3');%,'\sigma_4','\sigma_5','\sigma_6'); -% ylim([1e-8 1e-2]); -% -% %condition number -% figure -% loglog(w./(2*pi), S(1,:)./S(3,:));hold on; -% % loglog(w./(2*pi), abs(Val(1,:)),w./(2*pi), abs(Val(2,:)),w./(2*pi), abs(Val(3,:))); -% xlabel('Frequency [Hz]');ylabel('Condition number [-]'); -% % legend('\sigma_1','\sigma_2','\sigma_3');%,'\sigma_4','\sigma_5','\sigma_6'); -% -% %performance indicator -% system_dec_svd = freqresp(system_dec(1:4,1:3),2*pi*10); -% [U,S,V] = svd(system_dec_svd); -% H_svd_OL = -eye(3,4);%-[zpk(-2*pi*10,-2*pi*40,40/10) 0 0 0; 0 10*zpk(-2*pi*40,-2*pi*200,40/200) 0 0; 0 0 zpk(-2*pi*2,-2*pi*10,10/2) 0];% - eye(3,4);% -% H_svd = pinv(V')*H_svd_OL*pinv(U); -% % system_dec_control_svd_ = feedback(system_dec,g*pinv(V')*H*pinv(U)); -% -% OL_dec = g_svd*H_svd*system_dec(1:4,1:3); -% OL_freq = freqresp(OL_dec,w); % OL = G*H -% CL_system = feedback(eye(3),-g_svd*H_svd*system_dec(1:4,1:3)); -% CL_freq = freqresp(CL_system,w); % CL = (1+G*H)^-1 -% % CL_system_2 = feedback(system_dec,H); -% % CL_freq_2 = freqresp(CL_system_2,w); % CL = G/(1+G*H) -% for i = 1:size(w,2) -% OL(:,i) = svd(OL_freq(:,:,i)); -% CL (:,i) = svd(CL_freq(:,:,i)); -% %CL2 (:,i) = svd(CL_freq_2(:,:,i)); -% end -% -% un = ones(1,length(w)); -% figure -% loglog(w./(2*pi),OL(3,:)+1,'k',w./(2*pi),OL(3,:)-1,'b',w./(2*pi),1./CL(1,:),'r--',w./(2*pi),un,'k:');hold on;% -% % loglog(w./(2*pi), 1./(CL(2,:)),w./(2*pi), 1./(CL(3,:))); -% % semilogx(w./(2*pi), 1./(CL2(1,:)),w./(2*pi), 1./(CL2(2,:)),w./(2*pi), 1./(CL2(3,:))); -% xlabel('Frequency [Hz]');ylabel('Singular Value [-]'); -% legend('GH \sigma_{inf} +1 ','GH \sigma_{inf} -1','S 1/\sigma_{sup}');%,'\lambda_1','\lambda_2','\lambda_3'); -% -% figure -% loglog(w./(2*pi),OL(1,:)+1,'k',w./(2*pi),OL(1,:)-1,'b',w./(2*pi),1./CL(3,:),'r--',w./(2*pi),un,'k:');hold on;% -% % loglog(w./(2*pi), 1./(CL(2,:)),w./(2*pi), 1./(CL(3,:))); -% % semilogx(w./(2*pi), 1./(CL2(1,:)),w./(2*pi), 1./(CL2(2,:)),w./(2*pi), 1./(CL2(3,:))); -% xlabel('Frequency [Hz]');ylabel('Singular Value [-]'); -% legend('GH \sigma_{sup} +1 ','GH \sigma_{sup} -1','S 1/\sigma_{inf}');%,'\lambda_1','\lambda_2','\lambda_3'); - -% Control Section - -system_dec_10Hz = freqresp(system_dec,2*pi*10); -system_dec_0Hz = freqresp(system_dec,0); - -system_decReal_10Hz = pinv(align(system_dec_10Hz)); -[Ureal,Sreal,Vreal] = svd(system_decReal_10Hz(1:4,1:3)); -normalizationMatrixReal = abs(pinv(Ureal)*system_dec_0Hz(1:4,1:3)*pinv(Vreal')); - -[U,S,V] = svd(system_dec_10Hz(1:4,1:3)); -normalizationMatrix = abs(pinv(U)*system_dec_0Hz(1:4,1:3)*pinv(V')); - -H_dec = ([zpk(-2*pi*5,-2*pi*30,30/5) 0 0 0 - 0 zpk(-2*pi*4,-2*pi*20,20/4) 0 0 - 0 0 0 zpk(-2*pi,-2*pi*10,10)]); -H_cen_OL = [zpk(-2*pi,-2*pi*10,10) 0 0; 0 zpk(-2*pi,-2*pi*10,10) 0; - 0 0 zpk(-2*pi*5,-2*pi*30,30/5)]; -H_cen = pinv(Jta)*H_cen_OL*pinv([Js1; Js2]); -% H_svd_OL = -[1/normalizationMatrix(1,1) 0 0 0 -% 0 1/normalizationMatrix(2,2) 0 0 -% 0 0 1/normalizationMatrix(3,3) 0]; -% H_svd_OL_real = -[1/normalizationMatrixReal(1,1) 0 0 0 -% 0 1/normalizationMatrixReal(2,2) 0 0 -% 0 0 1/normalizationMatrixReal(3,3) 0]; -H_svd_OL = -[1/normalizationMatrix(1,1)*zpk(-2*pi*10,-2*pi*60,60/10) 0 0 0 - 0 1/normalizationMatrix(2,2)*zpk(-2*pi*5,-2*pi*30,30/5) 0 0 - 0 0 1/normalizationMatrix(3,3)*zpk(-2*pi*2,-2*pi*10,10/2) 0]; -H_svd_OL_real = -[1/normalizationMatrixReal(1,1)*zpk(-2*pi*10,-2*pi*60,60/10) 0 0 0 - 0 1/normalizationMatrixReal(2,2)*zpk(-2*pi*5,-2*pi*30,30/5) 0 0 - 0 0 1/normalizationMatrixReal(3,3)*zpk(-2*pi*2,-2*pi*10,10/2) 0]; -% H_svd_OL_real = -[zpk(-2*pi*10,-2*pi*40,40/10) 0 0 0; 0 10*zpk(-2*pi*10,-2*pi*100,100/10) 0 0; 0 0 zpk(-2*pi*2,-2*pi*10,10/2) 0];%-eye(3,4); -% H_svd_OL = -[zpk(-2*pi*10,-2*pi*40,40/10) 0 0 0; 0 zpk(-2*pi*4,-2*pi*20,4/20) 0 0; 0 0 zpk(-2*pi*2,-2*pi*10,10/2) 0];% - eye(3,4);% -H_svd = pinv(V')*H_svd_OL*pinv(U); -H_svd_real = pinv(Vreal')*H_svd_OL_real*pinv(Ureal); - -OL_dec = g*H_dec*system_dec(1:4,1:3); -OL_cen = g*H_cen_OL*pinv([Js1; Js2])*system_dec(1:4,1:3)*pinv(Jta); -OL_svd = 100*H_svd_OL*pinv(U)*system_dec(1:4,1:3)*pinv(V'); -OL_svd_real = 100*H_svd_OL_real*pinv(Ureal)*system_dec(1:4,1:3)*pinv(Vreal'); - -% figure -% bode(OL_dec,w,P);title('OL Decentralized'); -% figure -% bode(OL_cen,w,P);title('OL Centralized'); - -figure -bode(g*system_dec(1:4,1:3),w,P); -title('gain * Plant'); - -figure -bode(OL_svd,OL_svd_real,w,P); -title('OL SVD'); -legend('SVD of Complex plant','SVD of real approximation of the complex plant') - -figure -bode(system_dec(1:4,1:3),pinv(U)*system_dec(1:4,1:3)*pinv(V'),P); - -CL_dec = feedback(system_dec,g*H_dec,[1 2 3],[1 2 3 4]); -CL_cen = feedback(system_dec,g*H_cen,[1 2 3],[1 2 3 4]); -CL_svd = feedback(system_dec,100*H_svd,[1 2 3],[1 2 3 4]); -CL_svd_real = feedback(system_dec,100*H_svd_real,[1 2 3],[1 2 3 4]); - -pzmap_testCL(system_dec,H_dec,g,[1 2 3],[1 2 3 4]) -title('Decentralized control'); - -pzmap_testCL(system_dec,H_cen,g,[1 2 3],[1 2 3 4]) -title('Centralized control'); - -pzmap_testCL(system_dec,H_svd,100,[1 2 3],[1 2 3 4]) -title('SVD control'); - -pzmap_testCL(system_dec,H_svd_real,100,[1 2 3],[1 2 3 4]) -title('Real approximation SVD control'); - -P.Ylim = [1e-8 1e-3]; -figure -bodemag(system_dec(1:4,1:3),CL_dec(1:4,1:3),CL_cen(1:4,1:3),CL_svd(1:4,1:3),CL_svd_real(1:4,1:3),P); -title('Motion/actuator') -legend('Control OFF','Decentralized control','Centralized control','SVD control','SVD control real appr.'); - -P.Ylim = [1e-5 1e1]; -figure -bodemag(system_dec(1:4,4:6),CL_dec(1:4,4:6),CL_cen(1:4,4:6),CL_svd(1:4,4:6),CL_svd_real(1:4,4:6),P); -title('Transmissibility'); -legend('Control OFF','Decentralized control','Centralized control','SVD control','SVD control real appr.'); - -figure -bodemag(system_dec([7 9],4:6),CL_dec([7 9],4:6),CL_cen([7 9],4:6),CL_svd([7 9],4:6),CL_svd_real([7 9],4:6),P); -title('Transmissibility from half sum and half difference in the X direction'); -legend('Control OFF','Decentralized control','Centralized control','SVD control','SVD control real appr.'); - -figure -bodemag(system_dec([8 10],4:6),CL_dec([8 10],4:6),CL_cen([8 10],4:6),CL_svd([8 10],4:6),CL_svd_real([8 10],4:6),P); -title('Transmissibility from half sum and half difference in the Z direction'); -legend('Control OFF','Decentralized control','Centralized control','SVD control','SVD control real appr.'); - -% Greshgorin radius - -system_dec_freq = freqresp(system_dec,w); -x1 = zeros(1,length(w)); -z1 = zeros(1,length(w)); -x2 = zeros(1,length(w)); -S1 = zeros(1,length(w)); -S2 = zeros(1,length(w)); -S3 = zeros(1,length(w)); - -for t = 1:length(w) - x1(t) = (abs(system_dec_freq(1,2,t))+abs(system_dec_freq(1,3,t)))/abs(system_dec_freq(1,1,t)); - z1(t) = (abs(system_dec_freq(2,1,t))+abs(system_dec_freq(2,3,t)))/abs(system_dec_freq(2,2,t)); - x2(t) = (abs(system_dec_freq(3,1,t))+abs(system_dec_freq(3,2,t)))/abs(system_dec_freq(3,3,t)); - system_svd = pinv(Ureal)*system_dec_freq(1:4,1:3,t)*pinv(Vreal'); - S1(t) = (abs(system_svd(1,2))+abs(system_svd(1,3)))/abs(system_svd(1,1)); - S2(t) = (abs(system_svd(2,1))+abs(system_svd(2,3)))/abs(system_svd(2,2)); - S2(t) = (abs(system_svd(3,1))+abs(system_svd(3,2)))/abs(system_svd(3,3)); -end - -limit = 0.5*ones(1,length(w)); - -figure -loglog(w./(2*pi),x1,w./(2*pi),z1,w./(2*pi),x2,w./(2*pi),limit,'--'); -legend('x_1','z_1','x_2','Limit'); -xlabel('Frequency [Hz]'); -ylabel('Greshgorin radius [-]'); - -figure -loglog(w./(2*pi),S1,w./(2*pi),S2,w./(2*pi),S3,w./(2*pi),limit,'--'); -legend('S1','S2','S3','Limit'); -xlabel('Frequency [Hz]'); -ylabel('Greshgorin radius [-]'); -% set(gcf,'color','w') - -% Injecting ground motion in the system to have the output - -Fr = logspace(-2,3,1e3); -w=2*pi*Fr*1i; -%fit of the ground motion data in m/s^2/rtHz -Fr_ground_x = [0.07 0.1 0.15 0.3 0.7 0.8 0.9 1.2 5 10]; -n_ground_x1 = [4e-7 4e-7 2e-6 1e-6 5e-7 5e-7 5e-7 1e-6 1e-5 3.5e-5]; -Fr_ground_v = [0.07 0.08 0.1 0.11 0.12 0.15 0.25 0.6 0.8 1 1.2 1.6 2 6 10]; -n_ground_v1 = [7e-7 7e-7 7e-7 1e-6 1.2e-6 1.5e-6 1e-6 9e-7 7e-7 7e-7 7e-7 1e-6 2e-6 1e-5 3e-5]; - -n_ground_x = interp1(Fr_ground_x,n_ground_x1,Fr,'linear'); -n_ground_v = interp1(Fr_ground_v,n_ground_v1,Fr,'linear'); -% figure -% loglog(Fr,abs(n_ground_v),Fr_ground_v,n_ground_v1,'*'); -% xlabel('Frequency [Hz]');ylabel('ASD [m/s^2 /rtHz]'); -% return - -%converting into PSD -n_ground_x = (n_ground_x).^2; -n_ground_v = (n_ground_v).^2; - -%Injecting ground motion in the system and getting the outputs -system_dec_f = (freqresp(system_dec,abs(w))); -PHI = zeros(size(Fr,2),12,12); -for p = 1:size(Fr,2) - Sw=zeros(6,6); - Iact = zeros(3,3); - Sw(4,4) = n_ground_x(p); - Sw(5,5) = n_ground_v(p); - Sw(6,6) = n_ground_v(p); - Sw(1:3,1:3) = Iact; - PHI(p,:,:) = (system_dec_f(:,:,p))*Sw(:,:)*(system_dec_f(:,:,p))'; -end -x1 = PHI(:,1,1); -z1 = PHI(:,2,2); -x2 = PHI(:,3,3); -z2 = PHI(:,4,4); -wx = PHI(:,5,5); -wz = PHI(:,6,6); -x12 = PHI(:,1,3); -z12 = PHI(:,2,4); -PHIwx = PHI(:,1,5); -PHIwz = PHI(:,2,6); -xsum = PHI(:,7,7); -zsum = PHI(:,8,8); -xdelta = PHI(:,9,9); -zdelta = PHI(:,10,10); -rot = PHI(:,11,11); +tiledlayout(2, 2, 'TileSpacing', 'None', 'Padding', 'None'); + +ax1 = nexttile; +hold on; +plot(freqs, abs(squeeze(freqresp(G( 'Ax', 'Dwx')/s^2, freqs, 'Hz'))), 'DisplayName', 'Open-Loop'); +plot(freqs, abs(squeeze(freqresp(G_cen('Ax', 'Dwx')/s^2, freqs, 'Hz'))), 'DisplayName', 'Centralized'); +plot(freqs, abs(squeeze(freqresp(G_svd('Ax', 'Dwx')/s^2, freqs, 'Hz'))), '--', 'DisplayName', 'SVD'); +set(gca,'ColorOrderIndex',1) +plot(freqs, abs(squeeze(freqresp(G( 'Ay', 'Dwy')/s^2, freqs, 'Hz'))), 'HandleVisibility', 'off'); +plot(freqs, abs(squeeze(freqresp(G_cen('Ay', 'Dwy')/s^2, freqs, 'Hz'))), 'HandleVisibility', 'off'); +plot(freqs, abs(squeeze(freqresp(G_svd('Ay', 'Dwy')/s^2, freqs, 'Hz'))), '--', 'HandleVisibility', 'off'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('$D_x/D_{w,x}$, $D_y/D_{w, y}$'); set(gca, 'XTickLabel',[]); +legend('location', 'southwest'); + +ax2 = nexttile; +hold on; +plot(freqs, abs(squeeze(freqresp(G( 'Az', 'Dwz')/s^2, freqs, 'Hz')))); +plot(freqs, abs(squeeze(freqresp(G_cen('Az', 'Dwz')/s^2, freqs, 'Hz')))); +plot(freqs, abs(squeeze(freqresp(G_svd('Az', 'Dwz')/s^2, freqs, 'Hz'))), '--'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('$D_z/D_{w,z}$'); set(gca, 'XTickLabel',[]); + +ax3 = nexttile; +hold on; +plot(freqs, abs(squeeze(freqresp(G( 'Arx', 'Rwx')/s^2, freqs, 'Hz')))); +plot(freqs, abs(squeeze(freqresp(G_cen('Arx', 'Rwx')/s^2, freqs, 'Hz')))); +plot(freqs, abs(squeeze(freqresp(G_svd('Arx', 'Rwx')/s^2, freqs, 'Hz'))), '--'); +set(gca,'ColorOrderIndex',1) +plot(freqs, abs(squeeze(freqresp(G( 'Ary', 'Rwy')/s^2, freqs, 'Hz')))); +plot(freqs, abs(squeeze(freqresp(G_cen('Ary', 'Rwy')/s^2, freqs, 'Hz')))); +plot(freqs, abs(squeeze(freqresp(G_svd('Ary', 'Rwy')/s^2, freqs, 'Hz'))), '--'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('$R_x/R_{w,x}$, $R_y/R_{w,y}$'); xlabel('Frequency [Hz]'); + +ax4 = nexttile; +hold on; +plot(freqs, abs(squeeze(freqresp(G( 'Arz', 'Rwz')/s^2, freqs, 'Hz')))); +plot(freqs, abs(squeeze(freqresp(G_cen('Arz', 'Rwz')/s^2, freqs, 'Hz')))); +plot(freqs, abs(squeeze(freqresp(G_svd('Arz', 'Rwz')/s^2, freqs, 'Hz'))), '--'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('$R_z/R_{w,z}$'); xlabel('Frequency [Hz]'); + +linkaxes([ax1,ax2,ax3,ax4],'xy'); +xlim([freqs(1), freqs(end)]); +ylim([1e-3, 1e2]);