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

This commit is contained in:
Thomas Dehaeze 2019-09-17 15:55:59 +02:00
parent 69a382d52a
commit 9a276fe64d
37 changed files with 4177 additions and 2214 deletions

BIN
figs/4qd_linear_reg.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 77 KiB

BIN
figs/calib_4qd_h.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 98 KiB

BIN
figs/calib_4qd_v.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 93 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 7.5 KiB

After

Width:  |  Height:  |  Size: 9.6 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 35 KiB

After

Width:  |  Height:  |  Size: 45 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 20 KiB

After

Width:  |  Height:  |  Size: 30 KiB

BIN
figs/coh_cercalo.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 109 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 129 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 127 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 172 KiB

After

Width:  |  Height:  |  Size: 153 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 98 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 103 KiB

BIN
figs/frf_cercalo_gain.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 181 KiB

BIN
figs/frf_cercalo_phase.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 124 KiB

BIN
figs/frf_newport_gain.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 179 KiB

BIN
figs/frf_newport_phase.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 119 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 106 KiB

After

Width:  |  Height:  |  Size: 108 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 184 KiB

After

Width:  |  Height:  |  Size: 186 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 122 KiB

After

Width:  |  Height:  |  Size: 126 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 56 KiB

After

Width:  |  Height:  |  Size: 57 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 95 KiB

After

Width:  |  Height:  |  Size: 51 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 47 KiB

After

Width:  |  Height:  |  Size: 49 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 54 KiB

After

Width:  |  Height:  |  Size: 54 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 122 KiB

After

Width:  |  Height:  |  Size: 128 KiB

BIN
figs/weights_cercalo.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 63 KiB

1780
index.html

File diff suppressed because it is too large Load Diff

3013
index.org

File diff suppressed because it is too large Load Diff

Binary file not shown.

File diff suppressed because it is too large Load Diff

View File

@ -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');

141
matlab/frf_processing.m Normal file
View File

@ -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);

View File

@ -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');

BIN
matlab/test_bench.slx Normal file

Binary file not shown.