initial commit
This commit is contained in:
Operator Cad
commit
d6e261a6bc
38
.gitignore
vendored
Normal file
38
.gitignore
vendored
Normal file
@ -0,0 +1,38 @@
|
||||
# Windows default autosave extension
|
||||
*.asv
|
||||
*rtw/
|
||||
test_enc**.m
|
||||
data/
|
||||
|
||||
# OSX / *nix default autosave extension
|
||||
*.m~
|
||||
|
||||
# Compiled MEX binaries (all platforms)
|
||||
*.mex*
|
||||
|
||||
# Packaged app and toolbox files
|
||||
*.mlappinstall
|
||||
*.slxc
|
||||
*.mldatx
|
||||
*.mltbx
|
||||
*.xml
|
||||
piezoapa_slrt_rtw/
|
||||
|
||||
# Generated helpsearch folders
|
||||
helpsearch*/
|
||||
|
||||
# Simulink code generation folders
|
||||
slprj/
|
||||
sccprj/
|
||||
|
||||
# Matlab code generation folders
|
||||
codegen/
|
||||
|
||||
# Simulink autosave extension
|
||||
*.autosave
|
||||
|
||||
# Simulink cache files
|
||||
*.slxc
|
||||
|
||||
# Octave session info
|
||||
octave-workspace
|
||||
BIN
mat/int_enc_comp.mat
Normal file
BIN
mat/int_enc_comp.mat
Normal file
Binary file not shown.
BIN
mat/int_enc_huddle_test.mat
Normal file
BIN
mat/int_enc_huddle_test.mat
Normal file
Binary file not shown.
616
matlab/sensor_fusion_analysis.m
Normal file
616
matlab/sensor_fusion_analysis.m
Normal file
@ -0,0 +1,616 @@
|
||||
|
||||
%% Huddle Test
|
||||
ht = load('./mat/huddle_test.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
|
||||
|
||||
% Detrend Data
|
||||
ht.d = detrend(ht.d, 0);
|
||||
ht.acc_1 = detrend(ht.acc_1, 0);
|
||||
ht.acc_2 = detrend(ht.acc_2, 0);
|
||||
ht.geo_1 = detrend(ht.geo_1, 0);
|
||||
ht.geo_2 = detrend(ht.geo_2, 0);
|
||||
ht.f_meas = detrend(ht.f_meas, 0);
|
||||
|
||||
% Compute PSD
|
||||
run setup;
|
||||
|
||||
win = hann(ceil(10/Ts));
|
||||
|
||||
[p_d, f] = pwelch(ht.d, win, [], [], 1/Ts);
|
||||
[p_acc1, ~] = pwelch(ht.acc_1, win, [], [], 1/Ts);
|
||||
[p_acc2, ~] = pwelch(ht.acc_2, win, [], [], 1/Ts);
|
||||
[p_geo1, ~] = pwelch(ht.geo_1, win, [], [], 1/Ts);
|
||||
[p_geo2, ~] = pwelch(ht.geo_2, win, [], [], 1/Ts);
|
||||
[p_fmeas, ~] = pwelch(ht.f_meas, win, [], [], 1/Ts);
|
||||
|
||||
% Plot PSD
|
||||
figure;
|
||||
hold on;
|
||||
plot(f, p_acc1);
|
||||
plot(f, p_acc2);
|
||||
hold off;
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('PSD [$V^2/Hz$]'); xlabel('Frequency [Hz]');
|
||||
title('Huddle Test - Accelerometers')
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(f, p_geo1);
|
||||
plot(f, p_geo2);
|
||||
hold off;
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('PSD [$V^2/Hz$]'); xlabel('Frequency [Hz]');
|
||||
title('Huddle Test - Geophones')
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(f, p_d);
|
||||
hold off;
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('PSD [$m^2/Hz$]'); xlabel('Frequency [Hz]');
|
||||
title('Huddle Test - Interferometers')
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(f, p_fmeas);
|
||||
hold off;
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('PSD [$V^2/Hz$]'); xlabel('Frequency [Hz]');
|
||||
title('Huddle Test - Force Sensor')
|
||||
|
||||
%% Accelerometer and Geophone Models
|
||||
% Accelerometer used: https://www.pcb.com/products?model=393B05
|
||||
% Geophone used: L22 https://www.sercel.com/products/Lists/ProductSpecification/Geophones_brochure_Sercel_EN.pdf
|
||||
|
||||
G_acc = 1/(1 + s/2/pi/2500); % [V/(m/s2)]
|
||||
G_geo = 120*s^2/(s^2 + 2*0.7*2*pi*2*s + (2*pi*2)^2); % [[V/(m/s)]
|
||||
|
||||
% PSD of intertial sensors in [m^2/Hz]
|
||||
figure;
|
||||
hold on;
|
||||
set(gca, 'ColorOrderIndex', 1);
|
||||
plot(f, sqrt(p_acc1)./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
|
||||
'DisplayName', 'Accelerometer');
|
||||
set(gca, 'ColorOrderIndex', 1);
|
||||
plot(f, sqrt(p_acc2)./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
|
||||
'HandleVisibility', 'off');
|
||||
set(gca, 'ColorOrderIndex', 2);
|
||||
plot(f, sqrt(p_geo1)./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
|
||||
'DisplayName', 'Geophone');
|
||||
set(gca, 'ColorOrderIndex', 2);
|
||||
plot(f, sqrt(p_geo2)./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
|
||||
'HandleVisibility', 'off');
|
||||
set(gca, 'ColorOrderIndex', 3);
|
||||
plot(f, sqrt(p_d), 'DisplayName', 'Interferometer');
|
||||
hold off;
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('ASD [$m/\sqrt{Hz}$]'); xlabel('Frequency [Hz]');
|
||||
title('Huddle Test')
|
||||
legend();
|
||||
|
||||
%% Compare Theoretical model with identified one
|
||||
id_ol = load('./mat/identification_noise_bis.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
|
||||
|
||||
% Detrend Data
|
||||
id_ol.d = detrend(id_ol.d, 0);
|
||||
id_ol.acc_1 = detrend(id_ol.acc_1, 0);
|
||||
id_ol.acc_2 = detrend(id_ol.acc_2, 0);
|
||||
id_ol.geo_1 = detrend(id_ol.geo_1, 0);
|
||||
id_ol.geo_2 = detrend(id_ol.geo_2, 0);
|
||||
id_ol.f_meas = detrend(id_ol.f_meas, 0);
|
||||
id_ol.u = detrend(id_ol.u, 0);
|
||||
|
||||
% Identification Parameters
|
||||
run setup;
|
||||
win = hann(ceil(10/Ts));
|
||||
|
||||
% IFF Plant
|
||||
[tf_fmeas_est, f] = tfestimate(id_ol.u, id_ol.f_meas, win, [], [], 1/Ts); % [V/m]
|
||||
[co_fmeas_est, ~] = mscohere(id_ol.u, id_ol.f_meas, win, [], [], 1/Ts);
|
||||
|
||||
figure;
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
plot(f, abs(tf_fmeas_est), '-')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('Amplitude'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
plot(f, 180/pi*angle(tf_fmeas_est), '-')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||
ylabel('Phase'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
|
||||
linkaxes([ax1,ax2], 'x');
|
||||
xlim([40, 400]);
|
||||
|
||||
% Geophones
|
||||
[tf_geo1_est, ~] = tfestimate(id_ol.d, id_ol.geo_1, win, [], [], 1/Ts); % [V/m]
|
||||
[co_geo1_est, ~] = mscohere(id_ol.d, id_ol.geo_1, win, [], [], 1/Ts);
|
||||
|
||||
[tf_geo2_est, ~] = tfestimate(id_ol.d, id_ol.geo_2, win, [], [], 1/Ts); % [V/m]
|
||||
[co_geo2_est, ~] = mscohere(id_ol.d, id_ol.geo_2, win, [], [], 1/Ts);
|
||||
|
||||
figure;
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
set(gca, 'ColorOrderIndex', 1);
|
||||
plot(f, abs(tf_geo1_est), '.')
|
||||
set(gca, 'ColorOrderIndex', 1);
|
||||
plot(f, abs(tf_geo2_est), '.')
|
||||
plot(f, abs(squeeze(freqresp(G_geo, f, 'Hz')).*(1i*2*pi*f)), 'k-')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('Amplitude'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
set(gca, 'ColorOrderIndex', 1);
|
||||
plot(f, 180/pi*angle(tf_geo1_est), '.')
|
||||
set(gca, 'ColorOrderIndex', 1);
|
||||
plot(f, 180/pi*angle(tf_geo2_est), '.')
|
||||
plot(f, 180/pi*angle(-squeeze(freqresp(G_geo, f, 'Hz')).*(1i*2*pi*f)), 'k-')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||
ylabel('Phase'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
|
||||
linkaxes([ax1,ax2], 'x');
|
||||
xlim([40, 400]);
|
||||
|
||||
% Accelerometers
|
||||
[tf_acc1_est, ~] = tfestimate(id_ol.d, id_ol.acc_1, win, [], [], 1/Ts); % [V/m]
|
||||
[co_acc1_est, ~] = mscohere(id_ol.d, id_ol.acc_1, win, [], [], 1/Ts);
|
||||
|
||||
[tf_acc2_est, ~] = tfestimate(id_ol.d, id_ol.acc_2, win, [], [], 1/Ts); % [V/m]
|
||||
[co_acc2_est, ~] = mscohere(id_ol.d, id_ol.acc_2, win, [], [], 1/Ts);
|
||||
|
||||
figure;
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
set(gca, 'ColorOrderIndex', 1);
|
||||
plot(f, abs(tf_acc1_est), '.')
|
||||
set(gca, 'ColorOrderIndex', 1);
|
||||
plot(f, abs(tf_acc2_est), '.')
|
||||
plot(f, abs(squeeze(freqresp(G_acc, f, 'Hz')).*(1i*2*pi*f).^2), 'k-')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('Amplitude'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
set(gca, 'ColorOrderIndex', 1);
|
||||
plot(f, 180/pi*angle(tf_acc1_est), '.')
|
||||
set(gca, 'ColorOrderIndex', 1);
|
||||
plot(f, 180/pi*angle(tf_acc2_est), '.')
|
||||
plot(f, 180/pi*angle(squeeze(freqresp(G_acc, f, 'Hz')).*(1i*2*pi*f).^2), 'k-')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||
ylabel('Phase'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
|
||||
linkaxes([ax1,ax2], 'x');
|
||||
xlim([40, 400]);
|
||||
|
||||
|
||||
|
||||
%% IFF development
|
||||
[tf_fmeas_est, f] = tfestimate(id_ol.u, id_ol.f_meas, win, [], [], 1/Ts); % [V/m]
|
||||
[co_fmeas_est, ~] = mscohere(id_ol.u, id_ol.f_meas, win, [], [], 1/Ts);
|
||||
|
||||
% Model
|
||||
wz = 2*pi*103;
|
||||
xi_z = 0.01;
|
||||
wp = 2*pi*238;
|
||||
xi_p = 0.015;
|
||||
|
||||
Giff = 20*(s^2 + 2*xi_z*s*wz + wz^2)/(s^2 + 2*xi_p*s*wp + wp^2)*(s/3/pi/(1 + s/3/pi));
|
||||
|
||||
% Comparison model and identification
|
||||
figure;
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
plot(f, abs(tf_fmeas_est), '.')
|
||||
plot(f, abs(squeeze(freqresp(Giff, f, 'Hz'))), 'k-')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('Amplitude'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
plot(f, 180/pi*angle(tf_fmeas_est), '.')
|
||||
plot(f, 180/pi*angle(squeeze(freqresp(Giff, f, 'Hz'))), 'k-')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||
ylabel('Phase'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
|
||||
linkaxes([ax1,ax2], 'x');
|
||||
xlim([40, 400]);
|
||||
|
||||
% Root Locus
|
||||
gains = logspace(0, 5, 1000);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(real(pole(Giff)), imag(pole(Giff)), 'kx');
|
||||
plot(real(tzero(Giff)), imag(tzero(Giff)), 'ko');
|
||||
for i = 1:length(gains)
|
||||
cl_poles = pole(feedback(Giff, gains(i)/(s + 2*pi*2)));
|
||||
plot(real(cl_poles), imag(cl_poles), 'k.');
|
||||
end
|
||||
ylim([0, 1800]);
|
||||
xlim([-1600,200]);
|
||||
xlabel('Real Part')
|
||||
ylabel('Imaginary Part')
|
||||
axis square
|
||||
|
||||
% Optimal Controller
|
||||
Kiff_opt = 110/(s + 2*pi*2);
|
||||
|
||||
%% New identification
|
||||
id_ol = load('./mat/identification_chirp_40_400.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
|
||||
id_cl = load('./mat/identification_chirp_40_400_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
|
||||
|
||||
% Used controller
|
||||
Kiff = -110/(s + 2*pi*2);
|
||||
|
||||
[tf_G_ol_est, f] = tfestimate(id_ol.u, id_ol.d, win, [], [], 1/Ts);
|
||||
[co_G_ol_est, ~] = mscohere(id_ol.u, id_ol.d, win, [], [], 1/Ts);
|
||||
[tf_G_cl_est, ~] = tfestimate(id_cl.u, id_cl.d, win, [], [], 1/Ts);
|
||||
[co_G_cl_est, ~] = mscohere(id_cl.u, id_cl.d, win, [], [], 1/Ts);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(f, co_G_ol_est, '-')
|
||||
plot(f, co_G_cl_est, '-')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||
ylabel('Coherence'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
xlim([40, 400]); ylim([0, 1])
|
||||
|
||||
% Comparison model and identification
|
||||
figure;
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
plot(f, abs(tf_G_ol_est), '-')
|
||||
plot(f, abs(tf_G_cl_est), '-')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('Amplitude'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
plot(f, 180/pi*angle(tf_G_ol_est), '-')
|
||||
plot(f, 180/pi*angle(tf_G_cl_est), '-')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||
ylabel('Phase'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
|
||||
linkaxes([ax1,ax2], 'x');
|
||||
xlim([40, 400]);
|
||||
|
||||
|
||||
%% Excitation Signal
|
||||
run setup;
|
||||
|
||||
% Get trasnfer function from input [V] to output displacement [m]
|
||||
id_cl = load('./mat/identification_noise_iff_bis.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
|
||||
|
||||
win = hann(ceil(10/Ts));
|
||||
|
||||
[tf_G_cl_est, f] = tfestimate(id_cl.u, id_cl.d, win, [], [], 1/Ts);
|
||||
[co_G_cl_est, ~] = mscohere(id_cl.u, id_cl.d, win, [], [], 1/Ts);
|
||||
|
||||
G_d_est = -5e-6*(2*pi*230)^2/(s^2 + 2*0.3*2*pi*240*s + (2*pi*240)^2);
|
||||
|
||||
figure;
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
plot(f, abs(tf_G_cl_est), '-')
|
||||
plot(f, abs(squeeze(freqresp(G_d_est, f, 'Hz'))), '--')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('Amplitude [m/V]'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
plot(f, 180/pi*angle(tf_G_cl_est), '-')
|
||||
plot(f, 180/pi*angle(squeeze(freqresp(G_d_est, f, 'Hz'))), '--')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||
ylabel('Phase'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
|
||||
linkaxes([ax1,ax2], 'x');
|
||||
xlim([10, 1000]);
|
||||
|
||||
%
|
||||
ht = load('./mat/huddle_test.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
|
||||
|
||||
ht.d = detrend(ht.d, 0);
|
||||
ht.acc_1 = detrend(ht.acc_1, 0);
|
||||
ht.acc_2 = detrend(ht.acc_2, 0);
|
||||
ht.geo_1 = detrend(ht.geo_1, 0);
|
||||
ht.geo_2 = detrend(ht.geo_2, 0);
|
||||
|
||||
win = hann(ceil(10/Ts));
|
||||
|
||||
[p_d, f] = pwelch(ht.d, win, [], [], 1/Ts);
|
||||
[p_acc1, ~] = pwelch(ht.acc_1, win, [], [], 1/Ts);
|
||||
[p_acc2, ~] = pwelch(ht.acc_2, win, [], [], 1/Ts);
|
||||
[p_geo1, ~] = pwelch(ht.geo_1, win, [], [], 1/Ts);
|
||||
[p_geo2, ~] = pwelch(ht.geo_2, win, [], [], 1/Ts);
|
||||
|
||||
% Generate Time domain signal with wanted PSD
|
||||
Fs = 1/Ts; % Sampling Frequency [Hz]
|
||||
|
||||
t = 0:Ts:180; % Time Vector [s]
|
||||
u = sqrt(Fs/2)*randn(length(t), 1); % Signal with an ASD equal to one
|
||||
|
||||
G_exc = 0.2e-6/(1 + s/2/pi/2)/(1 + s/2/pi/50);
|
||||
|
||||
y_d = lsim(G_exc, u, t);
|
||||
|
||||
[pxx, ~] = pwelch(y_d, win, 0, [], Fs);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
set(gca, 'ColorOrderIndex', 1);
|
||||
plot(f, sqrt(p_acc1)./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
|
||||
'DisplayName', 'Accelerometer');
|
||||
set(gca, 'ColorOrderIndex', 1);
|
||||
plot(f, sqrt(p_acc2)./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
|
||||
'HandleVisibility', 'off');
|
||||
set(gca, 'ColorOrderIndex', 2);
|
||||
plot(f, sqrt(p_geo1)./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
|
||||
'DisplayName', 'Geophone');
|
||||
set(gca, 'ColorOrderIndex', 2);
|
||||
plot(f, sqrt(p_geo2)./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
|
||||
'HandleVisibility', 'off');
|
||||
plot(f, sqrt(pxx), 'k-', ...
|
||||
'DisplayName', 'Excitation');
|
||||
set(gca, 'ColorOrderIndex', 3);
|
||||
plot(f, sqrt(p_d), 'DisplayName', 'Interferometer');
|
||||
hold off;
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('ASD [$m/\sqrt{Hz}$]'); xlabel('Frequency [Hz]');
|
||||
title('Huddle Test')
|
||||
legend();
|
||||
|
||||
% From displacement to Voltage
|
||||
y_v = lsim(G_exc*(1 + s/2/pi/50)/G_d_est/(1 + s/2/pi/5e3), u, t);
|
||||
figure; plot(t, y_v)
|
||||
figure; plot(t, lsim(G_pf, y_v, t))
|
||||
|
||||
%% Transfer function of inertial sensors
|
||||
load('./mat/identification_noise_opt_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
%% Estimation of the inertial sensor transfer functions
|
||||
id = load('./mat/identification_noise_opt_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
|
||||
ht = load('./mat/huddle_test.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
|
||||
|
||||
ht.d = detrend(ht.d, 0);
|
||||
ht.acc_1 = detrend(ht.acc_1, 0);
|
||||
ht.acc_2 = detrend(ht.acc_2, 0);
|
||||
ht.geo_1 = detrend(ht.geo_1, 0);
|
||||
ht.geo_2 = detrend(ht.geo_2, 0);
|
||||
ht.f_meas = detrend(ht.f_meas, 0);
|
||||
|
||||
id.d = detrend(id.d, 0);
|
||||
id.acc_1 = detrend(id.acc_1, 0);
|
||||
id.acc_2 = detrend(id.acc_2, 0);
|
||||
id.geo_1 = detrend(id.geo_1, 0);
|
||||
id.geo_2 = detrend(id.geo_2, 0);
|
||||
id.f_meas = detrend(id.f_meas, 0);
|
||||
|
||||
% Compare PSD
|
||||
run setup;
|
||||
win = hann(ceil(10/Ts));
|
||||
|
||||
[p_id_d, f] = pwelch(id.d, win, [], [], 1/Ts);
|
||||
[p_id_acc1, ~] = pwelch(id.acc_1, win, [], [], 1/Ts);
|
||||
[p_id_acc2, ~] = pwelch(id.acc_2, win, [], [], 1/Ts);
|
||||
[p_id_geo1, ~] = pwelch(id.geo_1, win, [], [], 1/Ts);
|
||||
[p_id_geo2, ~] = pwelch(id.geo_2, win, [], [], 1/Ts);
|
||||
[p_id_fmeas, ~] = pwelch(id.f_meas, win, [], [], 1/Ts);
|
||||
|
||||
[p_ht_d, ~] = pwelch(ht.d, win, [], [], 1/Ts);
|
||||
[p_ht_acc1, ~] = pwelch(ht.acc_1, win, [], [], 1/Ts);
|
||||
[p_ht_acc2, ~] = pwelch(ht.acc_2, win, [], [], 1/Ts);
|
||||
[p_ht_geo1, ~] = pwelch(ht.geo_1, win, [], [], 1/Ts);
|
||||
[p_ht_geo2, ~] = pwelch(ht.geo_2, win, [], [], 1/Ts);
|
||||
[p_ht_fmeas, ~] = pwelch(ht.f_meas, win, [], [], 1/Ts);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
set(gca, 'ColorOrderIndex', 1);
|
||||
plot(f, p_ht_acc1, 'DisplayName', 'Huddle Test');
|
||||
set(gca, 'ColorOrderIndex', 1);
|
||||
plot(f, p_ht_acc2, 'HandleVisibility', 'off');
|
||||
set(gca, 'ColorOrderIndex', 2);
|
||||
plot(f, p_id_acc1, 'DisplayName', 'Identification Test');
|
||||
set(gca, 'ColorOrderIndex', 2);
|
||||
plot(f, p_id_acc2, 'HandleVisibility', 'off');
|
||||
hold off;
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('PSD [$V^2/Hz$]'); xlabel('Frequency [Hz]');
|
||||
title('Huddle Test - Accelerometers')
|
||||
legend();
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
set(gca, 'ColorOrderIndex', 1);
|
||||
plot(f, p_ht_geo1, 'DisplayName', 'Huddle Test');
|
||||
set(gca, 'ColorOrderIndex', 1);
|
||||
plot(f, p_ht_geo2, 'HandleVisibility', 'off');
|
||||
set(gca, 'ColorOrderIndex', 2);
|
||||
plot(f, p_id_geo1, 'DisplayName', 'Identification Test');
|
||||
set(gca, 'ColorOrderIndex', 2);
|
||||
plot(f, p_id_geo2, 'HandleVisibility', 'off');
|
||||
hold off;
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('PSD [$V^2/Hz$]'); xlabel('Frequency [Hz]');
|
||||
title('Huddle Test - Geophones')
|
||||
legend();
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(f, p_ht_d, 'DisplayName', 'Huddle Test');
|
||||
plot(f, p_id_d, 'DisplayName', 'Identification Test');
|
||||
hold off;
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('PSD [$m^2/Hz$]'); xlabel('Frequency [Hz]');
|
||||
title('Huddle Test - Interferometers')
|
||||
legend();
|
||||
|
||||
% tf and coh computation
|
||||
[tf_acc1_est, f] = tfestimate(id.d, id.acc_1, win, [], [], 1/Ts);
|
||||
[co_acc1_est, ~] = mscohere(id.d, id.acc_1, win, [], [], 1/Ts);
|
||||
[tf_acc2_est, ~] = tfestimate(id.d, id.acc_2, win, [], [], 1/Ts);
|
||||
[co_acc2_est, ~] = mscohere(id.d, id.acc_2, win, [], [], 1/Ts);
|
||||
|
||||
[tf_geo1_est, ~] = tfestimate(id.d, id.geo_1, win, [], [], 1/Ts);
|
||||
[co_geo1_est, ~] = mscohere(id.d, id.geo_1, win, [], [], 1/Ts);
|
||||
[tf_geo2_est, ~] = tfestimate(id.d, id.geo_2, win, [], [], 1/Ts);
|
||||
[co_geo2_est, ~] = mscohere(id.d, id.geo_2, win, [], [], 1/Ts);
|
||||
|
||||
% Coherence
|
||||
figure;
|
||||
hold on;
|
||||
set(gca, 'ColorOrderIndex', 1);
|
||||
plot(f, co_acc1_est, '-', 'DisplayName', 'Accelerometer')
|
||||
set(gca, 'ColorOrderIndex', 1);
|
||||
plot(f, co_acc2_est, '-', 'HandleVisibility', 'off')
|
||||
set(gca, 'ColorOrderIndex', 2);
|
||||
plot(f, co_geo1_est, '-', 'DisplayName', 'Geophone')
|
||||
set(gca, 'ColorOrderIndex', 2);
|
||||
plot(f, co_geo2_est, '-', 'HandleVisibility', 'off')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||
ylabel('Coherence'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
xlim([2, 2e3]); ylim([0, 1])
|
||||
legend();
|
||||
|
||||
% Models
|
||||
G_acc = 1/(1 + s/2/pi/2500); % [V/(m/s2)]
|
||||
G_geo = -1200*s^2/(s^2 + 2*0.7*2*pi*2*s + (2*pi*2)^2); % [[V/(m/s)]
|
||||
|
||||
|
||||
% Transfer Functions
|
||||
figure;
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
plot(f, abs(tf_acc1_est./(1i*2*pi*f).^2), '-')
|
||||
plot(f, abs(tf_acc2_est./(1i*2*pi*f).^2), '-')
|
||||
plot(f, abs(squeeze(freqresp(G_acc, f, 'Hz'))), 'k-')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('Amplitude [V/(m/s^2)]'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
plot(f, 180/pi*angle(tf_acc1_est./(1i*2*pi*f).^2), '-')
|
||||
plot(f, 180/pi*angle(tf_acc2_est./(1i*2*pi*f).^2), '-')
|
||||
plot(f, 180/pi*angle(squeeze(freqresp(G_acc, f, 'Hz'))), 'k-')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||
ylabel('Phase'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
|
||||
linkaxes([ax1,ax2], 'x');
|
||||
xlim([2, 2e3]);
|
||||
|
||||
|
||||
figure;
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
plot(f, abs(tf_geo1_est./(1i*2*pi*f)), '-')
|
||||
plot(f, abs(tf_geo2_est./(1i*2*pi*f)), '-')
|
||||
plot(f, abs(squeeze(freqresp(G_geo, f, 'Hz'))), 'k-')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('Amplitude[V/(m/s)]'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
plot(f, 180/pi*angle(tf_geo1_est./(1i*2*pi*f)), '-')
|
||||
plot(f, 180/pi*angle(tf_geo2_est./(1i*2*pi*f)), '-')
|
||||
plot(f, 180/pi*angle(squeeze(freqresp(G_geo, f, 'Hz'))), 'k-')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||
ylabel('Phase'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
|
||||
linkaxes([ax1,ax2], 'x');
|
||||
xlim([0.5, 2e3]);
|
||||
|
||||
|
||||
%% Compare signal
|
||||
|
||||
id.acc_1 = detrend(id.acc_1, 0);
|
||||
id.acc_2 = detrend(id.acc_2, 0);
|
||||
id.geo_1 = detrend(id.geo_1, 0);
|
||||
id.geo_2 = detrend(id.geo_2, 0);
|
||||
id.d = detrend(id.d, 0);
|
||||
|
||||
G_acc = 1/(1 + s/2/pi/2500); % [V/(m/s2)]
|
||||
G_geo = -1200*s^2/(s^2 + 2*0.7*2*pi*2*s + (2*pi*2)^2); % [V/(m/s)]
|
||||
|
||||
G_hpf = (s/2/pi/2)/(1 + s/2/pi/2);
|
||||
|
||||
acc1_d = lsim(G_hpf*1/G_acc/(s + 2*pi)^2, id.acc_1, id.t);
|
||||
acc2_d = lsim(G_hpf*1/G_acc/(s + 2*pi)^2, id.acc_2, id.t);
|
||||
geo1_d = lsim(G_hpf*1/G_geo/(s + 2*pi), id.geo_1, id.t);
|
||||
geo2_d = lsim(G_hpf*1/G_geo/(s + 2*pi), id.geo_2, id.t);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(id.t, id.d);
|
||||
plot(id.t, acc1_d);
|
||||
plot(id.t, acc2_d);
|
||||
plot(id.t, geo1_d);
|
||||
plot(id.t, geo2_d);
|
||||
hold off;
|
||||
xlabel('Time [s]'); ylabel('Displacement [m]');
|
||||
|
||||
% Fusion
|
||||
wc = 2*pi*200;
|
||||
G_hpf = (s/wc)/(1 + s/wc);
|
||||
G_lpf = 1/(1 + s/wc);
|
||||
|
||||
ss_d = lsim(G_hpf, acc1_d, id.t) + lsim(G_lpf, geo1_d, id.t);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(id.t, id.d);
|
||||
plot(id.t, ss_d);
|
||||
hold off;
|
||||
xlabel('Time [s]'); ylabel('Displacement [m]');
|
||||
|
||||
|
||||
|
||||
|
||||
128
runtest.m
Normal file
128
runtest.m
Normal file
@ -0,0 +1,128 @@
|
||||
tg = slrt;
|
||||
|
||||
%%
|
||||
f = SimulinkRealTime.openFTP(tg);
|
||||
mget(f, 'int_enc.dat', 'data');
|
||||
close(f);
|
||||
|
||||
%% Convert the Data
|
||||
data = SimulinkRealTime.utils.getFileScopeData('data/int_enc.dat').data;
|
||||
|
||||
interferometer = data(:, 1);
|
||||
encoder = data(:, 2);
|
||||
u = data(:, 3);
|
||||
t = data(:, 4);
|
||||
|
||||
save('./mat/int_enc_comp.mat', 'interferometer', 'encoder', 'u' , 't');
|
||||
|
||||
%%
|
||||
interferometer = detrend(interferometer, 0);
|
||||
encoder = detrend(encoder, 0);
|
||||
|
||||
%%
|
||||
run setup;
|
||||
|
||||
win = hann(ceil(10/Ts));
|
||||
|
||||
[p_i, f] = pwelch(interferometer, win, [], [], 1/Ts);
|
||||
[p_e, ~] = pwelch(encoder, win, [], [], 1/Ts);
|
||||
|
||||
%%
|
||||
figure;
|
||||
hold on;
|
||||
plot(f, sqrt(p_i), 'DisplayName', 'Interferometer');
|
||||
plot(f, sqrt(p_e), 'DisplayName', 'Encoder');
|
||||
hold off;
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('ASD [$m/\sqrt{Hz}$]'); xlabel('Frequency [Hz]');
|
||||
grid;
|
||||
legend('location', 'northeast');
|
||||
|
||||
%%
|
||||
run setup;
|
||||
|
||||
win = hann(ceil(10/Ts));
|
||||
|
||||
[tf_geo1_est, f] = tfestimate(d, geo_1, win, [], [], 1/Ts);
|
||||
[co_geo1_est, ~] = mscohere(d, geo_1, win, [], [], 1/Ts);
|
||||
|
||||
[tf_geo2_est, ~] = tfestimate(d, geo_2, win, [], [], 1/Ts);
|
||||
[co_geo2_est, ~] = mscohere(d, geo_2, win, [], [], 1/Ts);
|
||||
|
||||
[tf_acc1_est, ~] = tfestimate(d, encoder, win, [], [], 1/Ts);
|
||||
[co_acc1_est, ~] = mscohere(d, encoder, win, [], [], 1/Ts);
|
||||
|
||||
[tf_acc2_est, ~] = tfestimate(d, acc_2, win, [], [], 1/Ts);
|
||||
[co_acc2_est, ~] = mscohere(d, acc_2, win, [], [], 1/Ts);
|
||||
|
||||
%%
|
||||
figure;
|
||||
|
||||
hold on;
|
||||
plot(f, co_geo1_est, '-')
|
||||
plot(f, co_geo2_est, '-')
|
||||
plot(f, co_acc1_est, '-')
|
||||
plot(f, co_acc2_est, '-')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||
ylabel('Coherence'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
|
||||
%%
|
||||
figure;
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
plot(f, abs(tf_geo1_est), '-', 'DisplayName', 'Geo1')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('Amplitude'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
plot(f, 180/pi*unwrap(angle(tf_geo1_est)), '-')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||
ylabel('Phase'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
|
||||
linkaxes([ax1,ax2], 'x');
|
||||
|
||||
%%
|
||||
figure;
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
plot(f, abs(tf_acc1_est), '-', 'DisplayName', 'Acc1')
|
||||
plot(f, abs(tf_acc2_est), '-', 'DisplayName', 'Acc2')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('Amplitude'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
plot(f, 180/pi*unwrap(angle(tf_acc1_est)), '-')
|
||||
plot(f, 180/pi*unwrap(angle(tf_acc2_est)), '-')
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||
ylabel('Phase'); xlabel('Frequency [Hz]');
|
||||
hold off;
|
||||
|
||||
linkaxes([ax1,ax2], 'x');
|
||||
|
||||
%%
|
||||
win = hann(ceil(10/Ts));
|
||||
|
||||
[p_acc_1, f] = pwelch(encoder, win, [], [], 1/Ts);
|
||||
[co_acc12, ~] = mscohere(encoder, acc_2, win, [], [], 1/Ts);
|
||||
|
||||
[p_geo_1, ~] = pwelch(geo_1, win, [], [], 1/Ts);
|
||||
[co_geo12, ~] = mscohere(geo_1, geo_2, win, [], [], 1/Ts);
|
||||
|
||||
p_acc_N = p_acc_1.*(1 - co_acc12);
|
||||
p_geo_N = p_geo_1.*(1 - co_geo12);
|
||||
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(f, sqrt(p_acc_N)./abs(tf_acc1_est));
|
||||
plot(f, sqrt(p_geo_N)./abs(tf_geo1_est));
|
||||
hold off;
|
||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||
ylabel('PSD'); xlabel('Frequency [Hz]');
|
||||
|
||||
24
setup.m
Normal file
24
setup.m
Normal file
@ -0,0 +1,24 @@
|
||||
%%
|
||||
s = tf('s');
|
||||
Ts = 1e-4; % [s]
|
||||
|
||||
%% Pre-Filter
|
||||
G_pf = 1/(1 + s/2/pi/10);
|
||||
G_pf = c2d(G_pf, Ts, 'tustin');
|
||||
|
||||
% %% Force Sensor Filter (HPF)
|
||||
% Gf_hpf = s/(s + 2*pi*2);
|
||||
% Gf_hpf = tf(1);
|
||||
% Gf_hpf = c2d(Gf_hpf, Ts, 'tustin');
|
||||
%
|
||||
% %% IFF Controller
|
||||
% Kiff = 1/(s + 2*pi*2);
|
||||
% Kiff = c2d(Kiff, Ts, 'tustin');
|
||||
%
|
||||
% %% Excitation Signal
|
||||
% Tsim = 180; % Excitation time + Measurement time [s]
|
||||
%
|
||||
% t = 0:Ts:Tsim;
|
||||
% % u_exc = timeseries(chirp(t, 0.1, Tsim, 1e3, 'logarithmic'), t);
|
||||
% u_exc = timeseries(chirp(t, 40, Tsim, 400, 'logarithmic'), t);
|
||||
% % u_exc = timeseries(y_v, t);
|
||||
BIN
speedgoat_IO318_100k_CI_01585.mat
Normal file
BIN
speedgoat_IO318_100k_CI_01585.mat
Normal file
Binary file not shown.
BIN
test_encoder.slx
Normal file
BIN
test_encoder.slx
Normal file
Binary file not shown.
Loading…
Reference in New Issue
Block a user