Add study about spurious resonance

.gitignore

@@ -1,5 +1,6 @@
 auto/
 *.tex
+_minted*
 
 nohup.out
 
@@ -36,3 +37,14 @@ octave-workspace
 
 # Simulink Cache
 *.slxc
+
+## Build tool auxiliary files:
+*.fdb_latexmk
+*.synctex
+*.synctex(busy)
+*.synctex.gz
+*.synctex.gz(busy)
+*.pdfsync
+
+.auctex-auto/
+_minted-index/

docs/modal_control_gravimeter_numerical.m

@@ -0,0 +1,103 @@
+clc
+clear all
+close all
+
+%% System properties
+g = 100000;
+w0 = 2*pi*.5; % MinusK BM1 tablle
+l = 0.5; %[m]
+la = 1; %[m]
+h = 1.7; %[m]
+ha = 1.7;% %[m]
+m = 400; %[kg]
+k = 15e3;%[N/m]
+kv = k;
+kh = 15e3;
+I = 115;%[kg m^2]
+dampv = 0.03;
+damph = 0.03;
+s = tf('s');
+
+%% State-space model
+M = [m 0 0
+     0 m 0
+     0 0 I];
+
+la = l;
+ha = h;
+kv = k;
+kh = k;
+ 
+%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/2 %Left horizontal actuator
+    %1 0 h/2 %Right horizontal actuator
+    0 1 -la/2 %Left vertical actuator
+    0 1 la/2]; %Right vertical actuator
+Jah = [1 0 ha/2];
+Jav = [0 1 -la/2 %Left vertical actuator
+    0 1 la/2]; %Right vertical actuator
+Jta = Ja';
+Jtah = Jah';
+Jtav = Jav';
+K = kv*Jtav*Jav + kh*Jtah*Jah;    
+C = dampv*kv*Jtav*Jav+damph*kh*Jtah*Jah;  
+
+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,3)
+    M\Jta ];
+  
+% CC = [[Js1;Js2] zeros(4,3)];
+CC = [[Jah;Jav] zeros(3,3)];
+
+% DD = zeros(4,3);
+DD = zeros(3);
+
+G = ss(AA,BB,CC,DD);
+%% Modal coordinates
+[V,D] = eig(M\K);
+Mm = V'*M*V; % Modal mass matrix
+Dm = V'*C*V; % Modal damping matrix
+Km = V'*K*V; % Modal stiffness matrix
+
+Bm = inv(Mm)*V'*Jta;
+% Cm = [Js1;Js2]*V;
+Cm = [Jah;Jav]*V;
+
+
+omega = real(sqrt(inv(Mm)*Km));
+zeta = real(0.5*inv(Mm)*Dm*inv(omega));
+
+Gm = [1/(s^2+2*zeta(1,1)*omega(1,1)*s+omega(1,1)^2),0,0;
+    0,1/(s^2+2*zeta(2,2)*omega(2,2)*s+omega(2,2)^2),0;
+    0,0,1/(s^2+2*zeta(3,3)*omega(3,3)*s+omega(3,3)^2)];
+figure(1)
+bode(G,Cm*Gm*Bm)
+figure(2)
+bode(G,Gm)
+
+%% Controller
+s = tf('s');
+w0 = 2*pi*0.1;
+Kc = 100/(1+s/w0);
+Knet = inv(Bm)*Kc*inv(Cm);
+Gc = -lft(G,Knet); 
+isstable(Gc)
+
+
+
+
+

gravimeter/script.m

@@ -4,7 +4,7 @@ clear; close all; clc;
 %% Intialize Laplace variable
 s = zpk('s');
 
-freqs = logspace(-1, 2, 1000);
+freqs = logspace(-1, 3, 1000);
 
 % Gravimeter Model - Parameters
 % <<sec:gravimeter_model>>
@@ -107,7 +107,7 @@ for out_i = 1:4
         xlim([1e-1, 2e1]); ylim([1e-4, 1e0]);
 
         if in_i == 1
-            ylabel('Amplitude [m/N]')
+            ylabel('Amplitude [$\frac{m/s^2}{N}$]')
         else
             set(gca, 'YTickLabel',[]);
         end
@@ -156,6 +156,12 @@ size(Gx)
 
 % The diagonal and off-diagonal elements of $G_x$ are shown in Figure [[fig:gravimeter_jacobian_plant]].
 
+% It is shown at the system is:
+% - decoupled at high frequency thanks to a diagonal mass matrix (the Jacobian being evaluated at the center of mass of the payload)
+% - coupled at low frequency due to the non-diagonal terms in the stiffness matrix, especially the term corresponding to a coupling between a force in the x direction to a rotation around z (due to the torque applied by the stiffness 1).
+
+% The choice of the frame in this the Jacobian is evaluated is discussed in Section [[sec:choice_jacobian_reference]].
+
 
 figure;
 
@@ -171,7 +177,7 @@ plot(freqs, abs(squeeze(freqresp(Gx(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,
      '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));
+    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');
@@ -256,7 +262,7 @@ plot(freqs, abs(squeeze(freqresp(Gsvd(i_out, i_in), freqs, 'Hz'))), 'color', [0,
      '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));
+    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');
@@ -329,14 +335,14 @@ ylim([1e-4, 1e2]);
 RGA_svd = zeros(length(freqs), size(Gsvd,1), size(Gsvd,2));
 Gsvd_inv = inv(Gsvd);
 for f_i = 1:length(freqs)
-  RGA_svd(f_i, :, :) = abs(evalfr(Gsvd, j*2*pi*freqs(f_i)).*evalfr(Gsvd_inv, j*2*pi*freqs(f_i))');
+    RGA_svd(f_i, :, :) = abs(evalfr(Gsvd, j*2*pi*freqs(f_i)).*evalfr(Gsvd_inv, j*2*pi*freqs(f_i))');
 end
 
 % Relative Gain Array for the decoupled plant using the Jacobian:
 RGA_x = zeros(length(freqs), size(Gx,1), size(Gx,2));
 Gx_inv = inv(Gx);
 for f_i = 1:length(freqs)
-  RGA_x(f_i, :, :) = abs(evalfr(Gx, j*2*pi*freqs(f_i)).*evalfr(Gx_inv, j*2*pi*freqs(f_i))');
+    RGA_x(f_i, :, :) = abs(evalfr(Gx, j*2*pi*freqs(f_i)).*evalfr(Gx_inv, j*2*pi*freqs(f_i))');
 end
 
 figure;
@@ -356,8 +362,8 @@ plot(freqs, RGA_svd(:, 1, 2), '--', 'color', [0 0 0 0.2], ...
 plot(freqs, RGA_svd(:, 1, 1), 'k-', ...
      'DisplayName', '$RGA_{SVD}(i,i)$');
 for ch_i = 1:3
-  plot(freqs, RGA_svd(:, ch_i, ch_i), 'k-', ...
-       'HandleVisibility', 'off');
+    plot(freqs, RGA_svd(:, ch_i, ch_i), 'k-', ...
+         'HandleVisibility', 'off');
 end
 hold off;
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
@@ -378,8 +384,8 @@ plot(freqs, RGA_x(:, 1, 2), '--', 'color', [0 0 0 0.2], ...
 plot(freqs, RGA_x(:, 1, 1), 'k-', ...
      'DisplayName', '$RGA_{X}(i,i)$');
 for ch_i = 1:3
-  plot(freqs, RGA_x(:, ch_i, ch_i), 'k-', ...
-       'HandleVisibility', 'off');
+    plot(freqs, RGA_x(:, ch_i, ch_i), 'k-', ...
+         'HandleVisibility', 'off');
 end
 hold off;
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
@@ -406,14 +412,14 @@ ylim([1e-5, 1e1]);
 RGA_svd = zeros(size(Gsvd,1), size(Gsvd,2), length(freqs));
 Gsvd_inv = inv(Gsvd);
 for f_i = 1:length(freqs)
-  RGA_svd(:, :, f_i) = abs(evalfr(Gsvd, j*2*pi*freqs(f_i)).*evalfr(Gsvd_inv, j*2*pi*freqs(f_i))');
+    RGA_svd(:, :, f_i) = abs(evalfr(Gsvd, j*2*pi*freqs(f_i)).*evalfr(Gsvd_inv, j*2*pi*freqs(f_i))');
 end
 
 % Relative Gain Array for the decoupled plant using the Jacobian:
 RGA_x = zeros(size(Gx,1), size(Gx,2), length(freqs));
 Gx_inv = inv(Gx);
 for f_i = 1:length(freqs)
-  RGA_x(:, :, f_i) = abs(evalfr(Gx, j*2*pi*freqs(f_i)).*evalfr(Gx_inv, j*2*pi*freqs(f_i))');
+    RGA_x(:, :, f_i) = abs(evalfr(Gx, j*2*pi*freqs(f_i)).*evalfr(Gx_inv, j*2*pi*freqs(f_i))');
 end
 
 RGA_num_svd = squeeze(sum(sum(RGA_svd - eye(3))));
@@ -448,8 +454,8 @@ 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:3
-  plot(freqs, abs(squeeze(freqresp(Gsvd(ch_i, ch_i), freqs, 'Hz'))), ...
-       'DisplayName', sprintf('$G_{SVD}(%i,%i)$', ch_i, ch_i));
+    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');
@@ -461,7 +467,7 @@ ylim([1e-8, 1e0])
 ax2 = nexttile;
 hold on;
 for ch_i = 1:3
-  plot(freqs, 180/pi*angle(squeeze(freqresp(Gsvd(ch_i, ch_i), freqs, 'Hz'))));
+    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');
@@ -538,12 +544,10 @@ 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(Jt')*K_cen*pinv(Ja));
 
 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;
 U_inv = inv(U);
-G_svd = feedback(G, inv(V')*K_svd*U_inv(1:3, :));
 
 
 
@@ -558,16 +562,16 @@ 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:3
-  set(gca,'ColorOrderIndex',1)
-  plot(freqs, abs(squeeze(freqresp(L_svd(i_in_out, i_in_out), freqs, 'Hz'))), 'HandleVisibility', 'off');
+    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:3
-  set(gca,'ColorOrderIndex',2)
-  plot(freqs, abs(squeeze(freqresp(L_cen(i_in_out, i_in_out), freqs, 'Hz'))), 'HandleVisibility', 'off');
+    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');
@@ -579,13 +583,13 @@ ylim([5e-2, 2e3])
 ax2 = nexttile;
 hold on;
 for i_in_out = 1:3
-  set(gca,'ColorOrderIndex',1)
-  plot(freqs, 180/pi*angle(squeeze(freqresp(L_svd(i_in_out, i_in_out), freqs, 'Hz'))));
+    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:3
-  set(gca,'ColorOrderIndex',2)
-  plot(freqs, 180/pi*angle(squeeze(freqresp(L_cen(i_in_out, i_in_out), freqs, 'Hz'))));
+    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');
@@ -598,6 +602,43 @@ linkaxes([ax1,ax2],'x');
 % Closed-Loop system Performances
 % <<sec:gravimeter_closed_loop_results>>
 
+% Now the system is identified again with additional inputs and outputs:
+% - $x$, $y$ and $R_z$ ground motion
+% - $x$, $y$ and $R_z$ acceleration of the payload.
+
+
+%% Name of the Simulink File
+mdl = 'gravimeter';
+
+%% Input/Output definition
+clear io; io_i = 1;
+io(io_i) = linio([mdl, '/Dx'], 1, 'openinput');  io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Dy'], 1, 'openinput');  io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Rz'], 1, 'openinput');  io_i = io_i + 1;
+io(io_i) = linio([mdl, '/F1'], 1, 'openinput');  io_i = io_i + 1;
+io(io_i) = linio([mdl, '/F2'], 1, 'openinput');  io_i = io_i + 1;
+io(io_i) = linio([mdl, '/F3'], 1, 'openinput');  io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Abs_Motion'], 1, 'openoutput'); io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Abs_Motion'], 2, 'openoutput'); io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Abs_Motion'], 3, 'openoutput'); io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Acc_side'], 1, 'openoutput'); io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Acc_side'], 2, 'openoutput'); io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Acc_top'], 1, 'openoutput'); io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Acc_top'], 2, 'openoutput'); io_i = io_i + 1;
+
+G = linearize(mdl, io);
+G.InputName  = {'Dx', 'Dy', 'Rz', 'F1', 'F2', 'F3'};
+G.OutputName = {'Ax', 'Ay', 'Arz', 'Ax1', 'Ay1', 'Ax2', 'Ay2'};
+
+
+
+% The loop is closed using the developed controllers.
+
+G_cen = lft(G, -pinv(Jt')*K_cen*pinv(Ja));
+G_svd = lft(G, -inv(V')*K_svd*U_inv(1:3, :));
+
+
+
 % Let's first verify the stability of the closed-loop systems:
 
 isstable(G_cen)
@@ -629,9 +670,9 @@ tiledlayout(1, 3, 'TileSpacing', 'None', 'Padding', 'None');
 
 ax1 = nexttile;
 hold on;
-plot(freqs, abs(squeeze(freqresp(G(    1,1)/s^2, freqs, 'Hz'))), 'DisplayName', 'Open-Loop');
-plot(freqs, abs(squeeze(freqresp(G_cen(1,1)/s^2, freqs, 'Hz'))), 'DisplayName', 'Centralized');
-plot(freqs, abs(squeeze(freqresp(G_svd(1,1)/s^2, freqs, 'Hz'))), '--', 'DisplayName', 'SVD');
+plot(freqs, abs(squeeze(freqresp(G(    'Ax','Dx')/s^2, freqs, 'Hz'))), 'DisplayName', 'Open-Loop');
+plot(freqs, abs(squeeze(freqresp(G_cen('Ax','Dx')/s^2, freqs, 'Hz'))), 'DisplayName', 'Centralized');
+plot(freqs, abs(squeeze(freqresp(G_svd('Ax','Dx')/s^2, freqs, 'Hz'))), '--', 'DisplayName', 'SVD');
 hold off;
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
 ylabel('Transmissibility'); xlabel('Frequency [Hz]');
@@ -640,9 +681,9 @@ legend('location', 'southwest');
 
 ax2 = nexttile;
 hold on;
-plot(freqs, abs(squeeze(freqresp(G(    2,2)/s^2, freqs, 'Hz'))));
-plot(freqs, abs(squeeze(freqresp(G_cen(2,2)/s^2, freqs, 'Hz'))));
-plot(freqs, abs(squeeze(freqresp(G_svd(2,2)/s^2, freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G(    'Ay','Dy')/s^2, freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G_cen('Ay','Dy')/s^2, freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G_svd('Ay','Dy')/s^2, freqs, 'Hz'))), '--');
 hold off;
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
 set(gca, 'YTickLabel',[]); xlabel('Frequency [Hz]');
@@ -650,9 +691,9 @@ title('$D_y/D_{w,y}$');
 
 ax3 = nexttile;
 hold on;
-plot(freqs, abs(squeeze(freqresp(G(    3,3)/s^2, freqs, 'Hz'))));
-plot(freqs, abs(squeeze(freqresp(G_cen(3,3)/s^2, freqs, 'Hz'))));
-plot(freqs, abs(squeeze(freqresp(G_svd(3,3)/s^2, freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G(    'Arz','Rz')/s^2, freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G_cen('Arz','Rz')/s^2, freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G_svd('Arz','Rz')/s^2, freqs, 'Hz'))), '--');
 hold off;
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
 set(gca, 'YTickLabel',[]); xlabel('Frequency [Hz]');
@@ -660,7 +701,7 @@ title('$R_z/R_{w,z}$');
 
 linkaxes([ax1,ax2,ax3],'xy');
 xlim([freqs(1), freqs(end)]);
-xlim([1e-2, 5e1]); ylim([1e-7, 1e-2]);
+xlim([1e-2, 5e1]); ylim([1e-2, 1e1]);
 
 
 
@@ -686,8 +727,10 @@ for out_i = 1:3
 end
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
 ylabel('Transmissibility'); xlabel('Frequency [Hz]');
+ylim([1e-6, 1e3]);
 
 % Robustness to a change of actuator position
+% <<sec:robustness_actuator_position>>
 
 % Let say we change the position of the actuators:
 
@@ -699,26 +742,36 @@ mdl = 'gravimeter';
 
 %% Input/Output definition
 clear io; io_i = 1;
+io(io_i) = linio([mdl, '/Dx'], 1, 'openinput');  io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Dy'], 1, 'openinput');  io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Rz'], 1, 'openinput');  io_i = io_i + 1;
 io(io_i) = linio([mdl, '/F1'], 1, 'openinput');  io_i = io_i + 1;
 io(io_i) = linio([mdl, '/F2'], 1, 'openinput');  io_i = io_i + 1;
 io(io_i) = linio([mdl, '/F3'], 1, 'openinput');  io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Abs_Motion'], 1, 'openoutput'); io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Abs_Motion'], 2, 'openoutput'); io_i = io_i + 1;
+io(io_i) = linio([mdl, '/Abs_Motion'], 3, 'openoutput'); io_i = io_i + 1;
 io(io_i) = linio([mdl, '/Acc_side'], 1, 'openoutput'); io_i = io_i + 1;
 io(io_i) = linio([mdl, '/Acc_side'], 2, 'openoutput'); io_i = io_i + 1;
 io(io_i) = linio([mdl, '/Acc_top'], 1, 'openoutput'); io_i = io_i + 1;
 io(io_i) = linio([mdl, '/Acc_top'], 2, 'openoutput'); io_i = io_i + 1;
 
 G = linearize(mdl, io);
-G.InputName  = {'F1', 'F2', 'F3'};
-G.OutputName = {'Ax1', 'Ay1', 'Ax2', 'Ay2'};
-
-G_cen_b = feedback(G, pinv(Jt')*K_cen*pinv(Ja));
-G_svd_b = feedback(G, inv(V')*K_svd*U_inv(1:3, :));
+G.InputName  = {'Dx', 'Dy', 'Rz', 'F1', 'F2', 'F3'};
+G.OutputName = {'Ax', 'Ay', 'Arz', 'Ax1', 'Ay1', 'Ax2', 'Ay2'};
 
 
 
-% The new plant is computed, and the centralized and SVD control architectures are applied using the previsouly computed Jacobian matrices and $U$ and $V$ matrices.
+% The loop is closed using the developed controllers.
 
-% The closed-loop system are still stable, and their
+G_cen_b = lft(G, -pinv(Jt')*K_cen*pinv(Ja));
+G_svd_b = lft(G, -inv(V')*K_svd*U_inv(1:3, :));
+
+
+
+% The new plant is computed, and the centralized and SVD control architectures are applied using the previously computed Jacobian matrices and $U$ and $V$ matrices.
+
+% The closed-loop system are still stable in both cases, and the obtained transmissibility are equivalent as shown in Figure [[fig:gravimeter_transmissibility_offset_act]].
 
 
 freqs = logspace(-2, 2, 1000);
@@ -728,9 +781,9 @@ tiledlayout(1, 3, 'TileSpacing', 'None', 'Padding', 'None');
 
 ax1 = nexttile;
 hold on;
-plot(freqs, abs(squeeze(freqresp(G_cen(1,1)/s^2, freqs, 'Hz'))), 'DisplayName', 'Initial');
-plot(freqs, abs(squeeze(freqresp(G_cen_b(1,1)/s^2, freqs, 'Hz'))), 'DisplayName', 'Jacobian');
-plot(freqs, abs(squeeze(freqresp(G_svd_b(1,1)/s^2, freqs, 'Hz'))), '--', 'DisplayName', 'SVD');
+plot(freqs, abs(squeeze(freqresp(G_cen(      'Ax','Dx')/s^2, freqs, 'Hz'))), 'DisplayName', 'Open-Loop');
+plot(freqs, abs(squeeze(freqresp(G_cen_b('Ax','Dx')/s^2, freqs, 'Hz'))), 'DisplayName', 'Centralized');
+plot(freqs, abs(squeeze(freqresp(G_svd_b('Ax','Dx')/s^2, freqs, 'Hz'))), '--', 'DisplayName', 'SVD');
 hold off;
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
 ylabel('Transmissibility'); xlabel('Frequency [Hz]');
@@ -739,9 +792,9 @@ legend('location', 'southwest');
 
 ax2 = nexttile;
 hold on;
-plot(freqs, abs(squeeze(freqresp(G_cen(2,2)/s^2, freqs, 'Hz'))));
-plot(freqs, abs(squeeze(freqresp(G_cen_b(2,2)/s^2, freqs, 'Hz'))));
-plot(freqs, abs(squeeze(freqresp(G_svd_b(2,2)/s^2, freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_cen(      'Ay','Dy')/s^2, freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G_cen_b('Ay','Dy')/s^2, freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G_svd_b('Ay','Dy')/s^2, freqs, 'Hz'))), '--');
 hold off;
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
 set(gca, 'YTickLabel',[]); xlabel('Frequency [Hz]');
@@ -749,9 +802,9 @@ title('$D_y/D_{w,y}$');
 
 ax3 = nexttile;
 hold on;
-plot(freqs, abs(squeeze(freqresp(G_cen(3,3)/s^2, freqs, 'Hz'))));
-plot(freqs, abs(squeeze(freqresp(G_cen_b(3,3)/s^2, freqs, 'Hz'))));
-plot(freqs, abs(squeeze(freqresp(G_svd_b(3,3)/s^2, freqs, 'Hz'))), '--');
+plot(freqs, abs(squeeze(freqresp(G_cen(      'Arz','Rz')/s^2, freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G_cen_b('Arz','Rz')/s^2, freqs, 'Hz'))));
+plot(freqs, abs(squeeze(freqresp(G_svd_b('Arz','Rz')/s^2, freqs, 'Hz'))), '--');
 hold off;
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
 set(gca, 'YTickLabel',[]); xlabel('Frequency [Hz]');
@@ -759,7 +812,7 @@ title('$R_z/R_{w,z}$');
 
 linkaxes([ax1,ax2,ax3],'xy');
 xlim([freqs(1), freqs(end)]);
-xlim([1e-2, 5e1]); ylim([1e-7, 3e-4]);
+xlim([1e-2, 5e1]); ylim([1e-2, 1e1]);
 
 % Decoupling of the mass matrix
 
@@ -819,7 +872,7 @@ plot(freqs, abs(squeeze(freqresp(GM(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,
      'DisplayName', '$G_x(i,j)\ i \neq j$');
 set(gca,'ColorOrderIndex',1)
 for i_in_out = 1:3
-  plot(freqs, abs(squeeze(freqresp(GM(i_in_out, i_in_out), freqs, 'Hz'))), 'DisplayName', sprintf('$G_x(%d,%d)$', i_in_out, i_in_out));
+    plot(freqs, abs(squeeze(freqresp(GM(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');
@@ -866,7 +919,7 @@ plot(freqs, abs(squeeze(freqresp(GK(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,
      'DisplayName', '$G_x(i,j)\ i \neq j$');
 set(gca,'ColorOrderIndex',1)
 for i_in_out = 1:3
-  plot(freqs, abs(squeeze(freqresp(GK(i_in_out, i_in_out), freqs, 'Hz'))), 'DisplayName', sprintf('$G_x(%d,%d)$', i_in_out, i_in_out));
+    plot(freqs, abs(squeeze(freqresp(GK(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');
@@ -930,7 +983,7 @@ plot(freqs, abs(squeeze(freqresp(GKM(i_out, i_in), freqs, 'Hz'))), 'color', [0,0
      'DisplayName', '$G_x(i,j)\ i \neq j$');
 set(gca,'ColorOrderIndex',1)
 for i_in_out = 1:3
-  plot(freqs, abs(squeeze(freqresp(GKM(i_in_out, i_in_out), freqs, 'Hz'))), 'DisplayName', sprintf('$G_x(%d,%d)$', i_in_out, i_in_out));
+    plot(freqs, abs(squeeze(freqresp(GKM(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');
@@ -938,11 +991,13 @@ xlabel('Frequency [Hz]'); ylabel('Magnitude');
 legend('location', 'southeast');
 ylim([1e-8, 1e0]);
 
-% SVD decoupling performances                                     :noexport:
+% SVD decoupling performances
+% <<sec:decoupling_performances>>
+% As the SVD is applied on a *real approximation* of the plant dynamics at a frequency $\omega_0$, it is foreseen that the effectiveness of the decoupling depends on the validity of the real approximation.
 
+% Let's do the SVD decoupling on a plant that is mostly real (low damping) and one with a large imaginary part (larger damping).
 
-la = l/2; % Position of Act. [m]
-ha = 0; % Position of Act. [m]
+% Start with small damping, the obtained diagonal and off-diagonal terms are shown in Figure [[fig:gravimeter_svd_low_damping]].
 
 c = 2e1; % Actuator Damping [N/(m/s)]
 
@@ -970,6 +1025,37 @@ H1 = pinv(D*real(H1'*diag(exp(j*angle(diag(H1*D*H1.'))/2))));
 [U,S,V] = svd(H1);
 Gsvd = inv(U)*G*inv(V');
 
+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_{svd}(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_{svd}(%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', 'northwest');
+ylim([1e-8, 1e0]);
+
+
+
+% #+name: fig:gravimeter_svd_low_damping
+% #+caption: Diagonal and off-diagonal term when decoupling with SVD on the gravimeter with small damping
+% #+RESULTS:
+% [[file:figs/gravimeter_svd_low_damping.png]]
+
+% Now take a larger damping, the obtained diagonal and off-diagonal terms are shown in Figure [[fig:gravimeter_svd_high_damping]].
+
 c = 5e2; % Actuator Damping [N/(m/s)]
 
 %% Name of the Simulink File
@@ -996,63 +1082,6 @@ H1 = pinv(D*real(H1'*diag(exp(j*angle(diag(H1*D*H1.'))/2))));
 [U,S,V] = svd(H1);
 Gsvdd = inv(U)*G*inv(V');
 
-JMa = [1 0 -h/2
-       0 1  l/2
-       1 0  h/2
-       0 1  0];
-
-JMt = [1 0 -ha
-       0 1  la
-       0 1 -la];
-
-GM = pinv(JMa)*G*pinv(JMt');
-GM.InputName  = {'Fx', 'Fy', 'Mz'};
-GM.OutputName  = {'Dx', 'Dy', 'Rz'};
-
-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(GM(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
-             'HandleVisibility', 'off');
-    end
-end
-plot(freqs, abs(squeeze(freqresp(GM(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(GM(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]);
-
-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');
-ylim([1e-8, 1e0]);
-
 figure;
 
 % Magnitude
@@ -1064,13 +1093,13 @@ for i_in = 1:3
     end
 end
 plot(freqs, abs(squeeze(freqresp(Gsvdd(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
-     'DisplayName', '$G_x(i,j)\ i \neq j$');
+     'DisplayName', '$G_{svd}(i,j)\ i \neq j$');
 set(gca,'ColorOrderIndex',1)
 for i_in_out = 1:3
-  plot(freqs, abs(squeeze(freqresp(Gsvdd(i_in_out, i_in_out), freqs, 'Hz'))), 'DisplayName', sprintf('$G_x(%d,%d)$', i_in_out, i_in_out));
+    plot(freqs, abs(squeeze(freqresp(Gsvdd(i_in_out, i_in_out), freqs, 'Hz'))), 'DisplayName', sprintf('$G_{svd}(%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');
+legend('location', 'northwest');
 ylim([1e-8, 1e0]);

index.html

@@ -0,0 +1 @@
+svd-control.html

stewart_platform/script.m

@@ -108,7 +108,7 @@ plot(freqs, abs(squeeze(freqresp(Gu(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,
      'DisplayName', '$G_u(i,j)\ i \neq j$');
 set(gca,'ColorOrderIndex',1)
 for i_in_out = 1:6
-  plot(freqs, abs(squeeze(freqresp(Gu(i_in_out, i_in_out), freqs, 'Hz'))), 'DisplayName', sprintf('$G_u(%d,%d)$', i_in_out, i_in_out));
+    plot(freqs, abs(squeeze(freqresp(Gu(i_in_out, i_in_out), freqs, 'Hz'))), 'DisplayName', sprintf('$G_u(%d,%d)$', i_in_out, i_in_out));
 end
 hold off;
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
@@ -246,21 +246,21 @@ ylim([1e-3, 1e3]);
 RGA_coupled = zeros(length(freqs), size(Gu,1), size(Gu,2));
 Gu_inv = inv(Gu);
 for f_i = 1:length(freqs)
-  RGA_coupled(f_i, :, :) = abs(evalfr(Gu, j*2*pi*freqs(f_i)).*evalfr(Gu_inv, j*2*pi*freqs(f_i))');
+    RGA_coupled(f_i, :, :) = abs(evalfr(Gu, j*2*pi*freqs(f_i)).*evalfr(Gu_inv, j*2*pi*freqs(f_i))');
 end
 
 % Relative Gain Array for the decoupled plant using SVD:
 RGA_svd = zeros(length(freqs), size(Gsvd,1), size(Gsvd,2));
 Gsvd_inv = inv(Gsvd);
 for f_i = 1:length(freqs)
-  RGA_svd(f_i, :, :) = abs(evalfr(Gsvd, j*2*pi*freqs(f_i)).*evalfr(Gsvd_inv, j*2*pi*freqs(f_i))');
+    RGA_svd(f_i, :, :) = abs(evalfr(Gsvd, j*2*pi*freqs(f_i)).*evalfr(Gsvd_inv, j*2*pi*freqs(f_i))');
 end
 
 % Relative Gain Array for the decoupled plant using the Jacobian:
 RGA_x = zeros(length(freqs), size(Gx,1), size(Gx,2));
 Gx_inv = inv(Gx);
 for f_i = 1:length(freqs)
-  RGA_x(f_i, :, :) = abs(evalfr(Gx, j*2*pi*freqs(f_i)).*evalfr(Gx_inv, j*2*pi*freqs(f_i))');
+    RGA_x(f_i, :, :) = abs(evalfr(Gx, j*2*pi*freqs(f_i)).*evalfr(Gx_inv, j*2*pi*freqs(f_i))');
 end
 
 figure;
@@ -280,8 +280,8 @@ plot(freqs, RGA_svd(:, 1, 2), '--', 'color', [0 0 0 0.2], ...
 plot(freqs, RGA_svd(:, 1, 1), 'k-', ...
      'DisplayName', '$RGA_{SVD}(i,i)$');
 for ch_i = 1:6
-  plot(freqs, RGA_svd(:, ch_i, ch_i), 'k-', ...
-       'HandleVisibility', 'off');
+    plot(freqs, RGA_svd(:, ch_i, ch_i), 'k-', ...
+         'HandleVisibility', 'off');
 end
 hold off;
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
@@ -302,8 +302,8 @@ plot(freqs, RGA_x(:, 1, 2), '--', 'color', [0 0 0 0.2], ...
 plot(freqs, RGA_x(:, 1, 1), 'k-', ...
      'DisplayName', '$RGA_{X}(i,i)$');
 for ch_i = 1:6
-  plot(freqs, RGA_x(:, ch_i, ch_i), 'k-', ...
-       'HandleVisibility', 'off');
+    plot(freqs, RGA_x(:, ch_i, ch_i), 'k-', ...
+         'HandleVisibility', 'off');
 end
 hold off;
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
@@ -335,8 +335,8 @@ 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));
+    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');
@@ -348,7 +348,7 @@ ylim([1e-1, 1e5])
 ax2 = nexttile;
 hold on;
 for ch_i = 1:6
-  plot(freqs, 180/pi*angle(squeeze(freqresp(Gsvd(ch_i, ch_i), freqs, 'Hz'))));
+    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');
@@ -450,16 +450,16 @@ 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');
+    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');
+    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');
@@ -471,13 +471,13 @@ ylim([5e-2, 2e3])
 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'))));
+    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'))));
+    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');