Identification of all the elements in the system. Decentralized Controller.

matlab/decentralized_control.m

@@ -0,0 +1,77 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+freqs = logspace(0, 3, 1000);
+
+% Load Plant
+
+load('mat/plant.mat', 'sys', 'Gi', 'Zc', 'Ga', 'Gc', 'Gn', 'Gd');
+
+% Diagonal Controller
+% Using =SISOTOOL=, a diagonal controller is designed.
+% The two SISO loop gains are shown in Fig. [[fig:diag_contr_loop_gain]].
+
+Kh = -0.25598*(s+112)*(s^2 + 15.93*s + 6.686e06)/((s^2*(s+352.5)*(1+s/2/pi/2000)));
+Kv = 10207*(s+55.15)*(s^2 + 17.45*s + 2.491e06)/(s^2*(s+491.2)*(s+7695));
+
+K = blkdiag(Kh, Kv);
+K.InputName  = {'Rh', 'Rv'};
+K.OutputName = {'Uch', 'Ucv'};
+
+figure;
+% Magnitude
+ax1 = subaxis(2,1,1);
+hold on;
+plot(freqs, abs(squeeze(freqresp(Kh*sys('Rh', 'Uch'), freqs, 'Hz'))), 'DisplayName', '$L_h = K_h G_{d,h}^{-1} G_{\frac{V_{p,h}}{\tilde{U}_{c,h}}} G_{i,h} $');
+plot(freqs, abs(squeeze(freqresp(Kv*sys('Rv', 'Ucv'), freqs, 'Hz'))), 'DisplayName', '$L_v = K_v G_{d,v}^{-1} G_{\frac{V_{p,v}}{\tilde{U}_{c,v}}} G_{i,v} $');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+set(gca, 'XTickLabel',[]);
+ylabel('Magnitude [dB]');
+hold off;
+legend('location', 'northeast');
+
+% Phase
+ax2 = subaxis(2,1,2);
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(Kh*sys('Rh', 'Uch'), freqs, 'Hz'))));
+plot(freqs, 180/pi*angle(squeeze(freqresp(Kv*sys('Rv', 'Ucv'), freqs, 'Hz'))));
+set(gca,'xscale','log');
+yticks(-180:90:180);
+ylim([-180 180]);
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);
+
+
+
+% #+NAME: fig:diag_contr_loop_gain
+% #+CAPTION: Loop Gain using the Decentralized Diagonal Controller ([[./figs/diag_contr_loop_gain.png][png]], [[./figs/diag_contr_loop_gain.pdf][pdf]])
+% [[file:figs/diag_contr_loop_gain.png]]
+
+% We then close the loop and we look at the transfer function from the Newport rotation signal to the beam angle (Fig. [[fig:diag_contr_effect_newport]]).
+
+inputs  = {'Uch', 'Ucv', 'Unh', 'Unv'};
+outputs = {'Vch', 'Vcv', 'Ich', 'Icv', 'Rh', 'Rv', 'Vph', 'Vpv'};
+
+sys_cl = connect(sys, -K, inputs, outputs);
+
+figure;
+hold on;
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(sys('Rh', 'Unh'), freqs, 'Hz'))), '-', 'DisplayName', 'OL - $R_h/U_{n,h}$');
+set(gca,'ColorOrderIndex',1);
+plot(freqs, abs(squeeze(freqresp(sys_cl('Rh', 'Unh'), freqs, 'Hz'))), '--', 'DisplayName', 'CL - $R_h/U_{n,h}$');
+set(gca,'ColorOrderIndex',2);
+plot(freqs, abs(squeeze(freqresp(sys('Rv', 'Unv'), freqs, 'Hz'))), '-', 'DisplayName', 'OL - $R_v/U_{n,v}$');
+set(gca,'ColorOrderIndex',2);
+plot(freqs, abs(squeeze(freqresp(sys_cl('Rv', 'Unv'), freqs, 'Hz'))), '--', 'DisplayName', 'CL - $R_v/U_{n,v}$');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Magnitude [dB]');
+hold off;
+xlim([freqs(1), freqs(end)]);
+legend('location', 'southeast');

matlab/frf_processing.m

@@ -0,0 +1,141 @@
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+% Load Plant
+
+load('mat/plant.mat', 'Gi', 'Zc', 'Ga', 'Gc', 'Gn', 'Gd');
+
+% Test
+
+bodeFig({Ga*Zc*Gi}, struct('phase', true));
+
+% TODO Huddle Test
+% We load the data taken during the Huddle Test.
+
+load('mat/data_huddle_test.mat', ...
+     't', 'Uch', 'Ucv', ...
+     'Unh', 'Unv', ...
+     'Vph', 'Vpv', ...
+     'Vch', 'Vcv', ...
+     'Vnh', 'Vnv', ...
+     'Va');
+
+
+
+% We remove the first second of data where everything is settling down.
+
+t0 = 1;
+
+Uch(t<t0) = [];
+Ucv(t<t0) = [];
+Unh(t<t0) = [];
+Unv(t<t0) = [];
+Vph(t<t0) = [];
+Vpv(t<t0) = [];
+Vch(t<t0) = [];
+Vcv(t<t0) = [];
+Vnh(t<t0) = [];
+Vnv(t<t0) = [];
+Va(t<t0)  = [];
+t(t<t0)   = [];
+
+t = t - t(1); % We start at t=0
+
+figure;
+hold on;
+plot(t, Vph, 'DisplayName', '$Vp_h$');
+plot(t, Vpv, 'DisplayName', '$Vp_v$');
+hold off;
+xlabel('Time [s]');
+ylabel('Amplitude [V]');
+xlim([t(1), t(end)]);
+legend();
+
+
+
+% We compute the Power Spectral Density of the horizontal and vertical positions of the beam as measured by the 4 quadrant diode.
+
+[psd_Vph, f] = pwelch(Vph, hanning(ceil(1*fs)), [], [], fs);
+[psd_Vpv, ~] = pwelch(Vpv, hanning(ceil(1*fs)), [], [], fs);
+
+figure;
+hold on;
+plot(f, sqrt(psd_Vph), 'DisplayName', '$\Gamma_{Vp_h}$');
+plot(f, sqrt(psd_Vpv), 'DisplayName', '$\Gamma_{Vp_v}$');
+hold off;
+set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
+xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{V}{\sqrt{Hz}}\right]$')
+legend('Location', 'southwest');
+xlim([1, 1000]);
+
+figure;
+hold on;
+plot(t, Vch, 'DisplayName', '$Vc_h$');
+plot(t, Vcv, 'DisplayName', '$Vc_v$');
+hold off;
+xlabel('Time [s]');
+ylabel('Amplitude [V]');
+xlim([t(1), t(end)]);
+legend();
+
+
+
+% We compute the Power Spectral Density of the voltage across the inductance used for horizontal and vertical positioning of the Cercalo.
+
+[psd_Vch, f] = pwelch(Vch, hanning(ceil(1*fs)), [], [], fs);
+[psd_Vcv, ~] = pwelch(Vcv, hanning(ceil(1*fs)), [], [], fs);
+
+figure;
+hold on;
+plot(f, sqrt(psd_Vch), 'DisplayName', '$\Gamma_{Vc_h}$');
+plot(f, sqrt(psd_Vcv), 'DisplayName', '$\Gamma_{Vc_v}$');
+hold off;
+set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
+xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{V}{\sqrt{Hz}}\right]$')
+legend('Location', 'southwest');
+xlim([1, 1000]);
+
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+% Load Plant
+
+load('mat/plant.mat', 'G');
+
+% RGA-Number
+
+freqs = logspace(2, 4, 1000);
+G_resp = freqresp(G, freqs, 'Hz');
+A = zeros(size(G_resp));
+RGAnum = zeros(1, length(freqs));
+
+for i = 1:length(freqs)
+  A(:, :, i) = G_resp(:, :, i).*inv(G_resp(:, :, i))';
+  RGAnum(i) = sum(sum(abs(A(:, :, i)-eye(2))));
+end
+% RGA = G0.*inv(G0)';
+
+figure;
+plot(freqs, RGAnum);
+set(gca, 'xscale', 'log');
+
+U = zeros(2, 2, length(freqs));
+S = zeros(2, 2, length(freqs))
+V = zeros(2, 2, length(freqs));
+
+for i = 1:length(freqs)
+  [Ui, Si, Vi] = svd(G_resp(:, :, i));
+  U(:, :, i) = Ui;
+  S(:, :, i) = Si;
+  V(:, :, i) = Vi;
+end
+
+% Rotation Matrix
+
+G0 = freqresp(G, 0);

matlab/plant_identification.m

@@ -1,341 +0,0 @@
-%% Clear Workspace and Close figures
-clear; close all; clc;
-
-%% Intialize Laplace variable
-s = zpk('s');
-
-% Excitation Data
-
-fs = 1e4;
-Ts = 1/fs;
-
-
-
-% We generate white noise with the "random number" simulink block, and we filter that noise.
-
-
-Gi = (1)/(1+s/2/pi/100);
-
-c2d(Gi, Ts, 'tustin')
-
-% Input / Output data
-% The identification data is loaded
-
-ux = load('mat/data_ux.mat', 't', 'ux', 'yx', 'yy');
-uy = load('mat/data_uy.mat', 't', 'uy', 'yx', 'yy');
-
-
-
-% We remove the first seconds where the Cercalo is turned on.
-
-i0x = 20*fs;
-
-i0y = 10*fs;
-
-ux.t  = ux.t( i0x:end) - ux.t(i0x);
-ux.ux = ux.ux(i0x:end);
-ux.yx = ux.yx(i0x:end);
-ux.yy = ux.yy(i0x:end);
-
-uy.t  = uy.t( i0y:end) - uy.t(i0x);
-uy.uy = uy.uy(i0y:end);
-uy.yx = uy.yx(i0y:end);
-uy.yy = uy.yy(i0y:end);
-
-ux.ux = ux.ux-mean(ux.ux);
-ux.yx = ux.yx-mean(ux.yx);
-ux.yy = ux.yy-mean(ux.yy);
-
-uy.ux = uy.ux-mean(uy.ux);
-uy.yx = uy.yx-mean(uy.yx);
-uy.yy = uy.yy-mean(uy.yy);
-
-figure;
-ax1 = subplot(1, 2, 1);
-plot(ux.t, ux.ux);
-xlabel('Time [s]');
-ylabel('Amplitude [V]');
-legend({'$u_x$'});
-
-ax2 = subplot(1, 2, 2);
-hold on;
-plot(ux.t, ux.yx, 'DisplayName', '$y_x$');
-plot(ux.t, ux.yy, 'DisplayName', '$y_y$');
-hold off;
-xlabel('Time [s]');
-ylabel('Amplitude [V]');
-legend()
-
-linkaxes([ax1,ax2],'x');
-xlim([ux.t(1), ux.t(end)])
-
-
-
-% #+NAME: fig:identification_ux
-% #+CAPTION: Identification signals when exciting the $x$ axis ([[./figs/identification_ux.png][png]], [[./figs/identification_ux.pdf][pdf]])
-% [[file:figs/identification_ux.png]]
-
-
-
-figure;
-ax1 = subplot(1, 2, 1);
-plot(uy.t, uy.uy);
-xlabel('Time [s]');
-ylabel('Amplitude [V]');
-legend({'$u_y$'});
-
-ax2 = subplot(1, 2, 2);
-hold on;
-plot(uy.t, uy.yy, 'DisplayName', '$y_y$');
-plot(uy.t, uy.yx, 'DisplayName', '$y_x$');
-hold off;
-xlabel('Time [s]');
-ylabel('Amplitude [V]');
-legend()
-
-linkaxes([ax1,ax2],'x');
-xlim([uy.t(1), uy.t(end)])
-
-% Estimation of the Frequency Response Function Matrix
-% We compute an estimate of the transfer functions.
-
-[tf_ux_yx, f] = tfestimate(ux.ux, ux.yx, hanning(ceil(1*fs)), [], [], fs);
-[tf_ux_yy, ~] = tfestimate(ux.ux, ux.yy, hanning(ceil(1*fs)), [], [], fs);
-[tf_uy_yx, ~] = tfestimate(uy.uy, uy.yx, hanning(ceil(1*fs)), [], [], fs);
-[tf_uy_yy, ~] = tfestimate(uy.uy, uy.yy, hanning(ceil(1*fs)), [], [], fs);
-
-figure;
-ax11 = subplot(2, 2, 1);
-hold on;
-plot(f, abs(tf_ux_yx))
-title('Frequency Response Function $\frac{y_x}{u_x}$')
-set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
-ylabel('Amplitude')
-hold off;
-
-ax12 = subplot(2, 2, 2);
-hold on;
-plot(f, abs(tf_uy_yx))
-title('Frequency Response Function $\frac{y_x}{u_y}$')
-set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
-hold off;
-
-ax21 = subplot(2, 2, 3);
-hold on;
-plot(f, abs(tf_ux_yy))
-title('Frequency Response Function $\frac{y_y}{u_x}$')
-set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
-ylabel('Amplitude')
-xlabel('Frequency [Hz]')
-hold off;
-
-ax22 = subplot(2, 2, 4);
-hold on;
-plot(f, abs(tf_uy_yy))
-title('Frequency Response Function $\frac{y_y}{u_y}$')
-set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
-xlabel('Frequency [Hz]')
-hold off;
-
-linkaxes([ax11,ax12,ax21,ax22],'x');
-xlim([10, 1000]);
-linkaxes([ax11,ax12,ax21,ax22],'y');
-ylim([1e-2, 1e3])
-
-% Coherence
-
-[coh_ux_yx, f] = mscohere(ux.ux, ux.yx, hanning(ceil(1*fs)), [], [], fs);
-[coh_ux_yy, ~] = mscohere(ux.ux, ux.yy, hanning(ceil(1*fs)), [], [], fs);
-[coh_uy_yx, ~] = mscohere(uy.uy, uy.yx, hanning(ceil(1*fs)), [], [], fs);
-[coh_uy_yy, ~] = mscohere(uy.uy, uy.yy, hanning(ceil(1*fs)), [], [], fs);
-
-figure;
-ax11 = subplot(2, 2, 1);
-hold on;
-plot(f, coh_ux_yx)
-set(gca, 'Xscale', 'log');
-title('Coherence $\frac{y_x}{u_x}$')
-ylabel('Coherence')
-hold off;
-
-ax12 = subplot(2, 2, 2);
-hold on;
-plot(f, coh_uy_yx)
-set(gca, 'Xscale', 'log');
-title('Coherence $\frac{y_x}{u_y}$')
-hold off;
-
-ax21 = subplot(2, 2, 3);
-hold on;
-plot(f, coh_ux_yy)
-set(gca, 'Xscale', 'log');
-title('Coherence $\frac{y_y}{u_x}$')
-ylabel('Coherence')
-xlabel('Frequency [Hz]')
-hold off;
-
-ax22 = subplot(2, 2, 4);
-hold on;
-plot(f, coh_uy_yy)
-set(gca, 'Xscale', 'log');
-title('Coherence $\frac{y_y}{u_y}$')
-xlabel('Frequency [Hz]')
-hold off;
-
-linkaxes([ax11,ax12,ax21,ax22],'x');
-xlim([10, 1000]);
-linkaxes([ax11,ax12,ax21,ax22],'y');
-ylim([0, 1])
-
-% Extraction of a transfer function matrix
-% First we define the initial guess for the resonance frequencies and the weights associated.
-
-freqs_res = [410, 250]; % [Hz]
-freqs_res_weights = [10, 10]; % [Hz]
-
-
-
-% From the number of resonance frequency we want to fit, we define the order =N= of the system we want to obtain.
-
-N = 2*length(freqs_res);
-
-
-
-% We then make an initial guess on the complex values of the poles.
-
-xi = 0.001; % Approximate modal damping
-poles = [2*pi*freqs_res*(xi + 1i), 2*pi*freqs_res*(xi - 1i)];
-
-
-
-% We then define the weight that will be used for the fitting.
-% Basically, we want more weight around the resonance and at low frequency (below the first resonance).
-% Also, we want more importance where we have a better coherence.
-
-weight = ones(1, length(f));
-% weight = G_coh';
-
-% alpha = 0.1;
-
-% for freq_i = 1:length(freqs_res)
-%   weight(f>(1-alpha)*freqs_res(freq_i) & omega<(1 + alpha)*2*pi*freqs_res(freq_i)) = freqs_res_weights(freq_i);
-% end
-
-
-
-% Ignore data above some frequency.
-
-weight(f>1000) = 0;
-
-figure;
-hold on;
-plot(f, weight);
-plot(freqs_res, ones(size(freqs_res)), 'rx');
-hold off;
-xlabel('Frequency [Hz]');
-xlabel('Weight Amplitude');
-set(gca, 'xscale', 'log');
-xlim([f(1), f(end)]);
-
-
-
-% #+NAME: fig:weights
-% #+CAPTION: Weights amplitude ([[./figs/weights.png][png]], [[./figs/weights.pdf][pdf]])
-% [[file:figs/weights.png]]
-
-% When we set some options for =vfit3=.
-
-opts = struct();
-
-opts.stable = 1;    % Enforce stable poles
-opts.asymp = 1;     % Force D matrix to be null
-opts.relax = 1;     % Use vector fitting with relaxed non-triviality constraint
-opts.skip_pole = 0; % Do NOT skip pole identification
-opts.skip_res = 0;  % Do NOT skip identification of residues (C,D,E)
-opts.cmplx_ss = 0;  % Create real state space model with block diagonal A
-
-opts.spy1 = 0;      % No plotting for first stage of vector fitting
-opts.spy2 = 0;      % Create magnitude plot for fitting of f(s)
-
-
-
-% We define the number of iteration.
-
-Niter = 5;
-
-
-
-% An we run the =vectfit3= algorithm.
-
-for iter = 1:Niter
-  [SER_ux_yx, poles, ~, fit_ux_yx] = vectfit3(tf_ux_yx.', 1i*2*pi*f, poles, weight, opts);
-end
-for iter = 1:Niter
-  [SER_uy_yx, poles, ~, fit_uy_yx] = vectfit3(tf_uy_yx.', 1i*2*pi*f, poles, weight, opts);
-end
-for iter = 1:Niter
-  [SER_ux_yy, poles, ~, fit_ux_yy] = vectfit3(tf_ux_yy.', 1i*2*pi*f, poles, weight, opts);
-end
-for iter = 1:Niter
-  [SER_uy_yy, poles, ~, fit_uy_yy] = vectfit3(tf_uy_yy.', 1i*2*pi*f, poles, weight, opts);
-end
-
-figure;
-ax11 = subplot(2, 2, 1);
-hold on;
-plot(f, abs(tf_ux_yx))
-plot(f, abs(fit_ux_yx))
-title('Frequency Response Function $\frac{y_x}{u_x}$')
-set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
-ylabel('Amplitude')
-hold off;
-
-ax12 = subplot(2, 2, 2);
-hold on;
-plot(f, abs(tf_uy_yx))
-plot(f, abs(fit_uy_yx))
-title('Frequency Response Function $\frac{y_x}{u_y}$')
-set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
-hold off;
-
-ax21 = subplot(2, 2, 3);
-hold on;
-plot(f, abs(tf_ux_yy))
-plot(f, abs(fit_ux_yy))
-title('Frequency Response Function $\frac{y_y}{u_x}$')
-set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
-ylabel('Amplitude')
-xlabel('Frequency [Hz]')
-hold off;
-
-ax22 = subplot(2, 2, 4);
-hold on;
-plot(f, abs(tf_uy_yy))
-plot(f, abs(fit_uy_yy))
-title('Frequency Response Function $\frac{y_y}{u_y}$')
-set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
-xlabel('Frequency [Hz]')
-hold off;
-
-linkaxes([ax11,ax12,ax21,ax22],'x');
-xlim([10, 1000]);
-linkaxes([ax11,ax12,ax21,ax22],'y');
-ylim([1e-2, 1e3])
-
-
-
-% #+NAME: fig:identification_matrix_fit
-% #+CAPTION: Transfer Function Extraction of the FRF matrix ([[./figs/identification_matrix_fit.png][png]], [[./figs/identification_matrix_fit.pdf][pdf]])
-% [[file:figs/identification_matrix_fit.png]]
-
-% And finally, we create the identified state space model:
-
-G_ux_yx = minreal(ss(full(SER_ux_yx.A),SER_ux_yx.B,SER_ux_yx.C,SER_ux_yx.D));
-G_uy_yx = minreal(ss(full(SER_uy_yx.A),SER_uy_yx.B,SER_uy_yx.C,SER_uy_yx.D));
-G_ux_yy = minreal(ss(full(SER_ux_yy.A),SER_ux_yy.B,SER_ux_yy.C,SER_ux_yy.D));
-G_uy_yy = minreal(ss(full(SER_uy_yy.A),SER_uy_yy.B,SER_uy_yy.C,SER_uy_yy.D));
-
-G = [G_ux_yx, G_uy_yx;
-     G_ux_yy, G_uy_yy];
-
-save('mat/plant.mat', 'G');