identification and huddle test
This commit is contained in:
parent
9b5e782f8b
commit
a59c0b2007
@ -86,7 +86,7 @@ This takes into account the sensibility of the sensor and possible integration t
|
|||||||
#+begin_src matlab
|
#+begin_src matlab
|
||||||
acc_1 = lsim(inv(G_acc), acc_1, t);
|
acc_1 = lsim(inv(G_acc), acc_1, t);
|
||||||
acc_2 = lsim(inv(G_acc), acc_2, t);
|
acc_2 = lsim(inv(G_acc), acc_2, t);
|
||||||
geo_1 = 1e2*lsim(inv(G_geo), geo_1, t);
|
geo_1 = lsim(inv(G_geo), geo_1, t);
|
||||||
geo_2 = lsim(inv(G_geo), geo_2, t);
|
geo_2 = lsim(inv(G_geo), geo_2, t);
|
||||||
#+end_src
|
#+end_src
|
||||||
|
|
||||||
|
|||||||
Binary file not shown.
BIN
mat/identification_chirp_40_400.mat
Normal file
BIN
mat/identification_chirp_40_400.mat
Normal file
Binary file not shown.
BIN
mat/identification_chirp_40_400_iff.mat
Normal file
BIN
mat/identification_chirp_40_400_iff.mat
Normal file
Binary file not shown.
BIN
mat/identification_noise_iff.mat
Normal file
BIN
mat/identification_noise_iff.mat
Normal file
Binary file not shown.
152
runtest.m
152
runtest.m
@ -8,53 +8,85 @@ close(f);
|
|||||||
%% Convert the Data
|
%% Convert the Data
|
||||||
data = SimulinkRealTime.utils.getFileScopeData('data/apa95ml.dat').data;
|
data = SimulinkRealTime.utils.getFileScopeData('data/apa95ml.dat').data;
|
||||||
|
|
||||||
acc_1 = data(:, 1);
|
d = data(:, 1);
|
||||||
acc_2 = data(:, 2);
|
acc_1 = data(:, 2);
|
||||||
geo_1 = data(:, 3);
|
acc_2 = data(:, 3);
|
||||||
geo_2 = data(:, 4);
|
geo_1 = data(:, 4);
|
||||||
t = data(:, 5);
|
geo_2 = data(:, 5);
|
||||||
|
u = data(:, 6);
|
||||||
|
f_meas = data(:, 7);
|
||||||
|
t = data(:, 8);
|
||||||
|
|
||||||
save('./mat/huddle_test.mat', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 't');
|
save('./mat/identification_noise_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
|
||||||
|
|
||||||
%%
|
%%
|
||||||
figure;
|
d = detrend(d, 0);
|
||||||
subplot(1,2,1);
|
acc_1 = detrend(acc_1, 0);
|
||||||
plot(t, u)
|
acc_2 = detrend(acc_2, 0);
|
||||||
subplot(1,2,2);
|
geo_1 = detrend(geo_1, 0);
|
||||||
plot(t, y)
|
geo_2 = detrend(geo_2, 0);
|
||||||
|
u = detrend(u, 0);
|
||||||
%%
|
|
||||||
load('../mat/apa95ml_5kg_10V.mat', 'u', 't', 'y');
|
|
||||||
ht = load('../mat/huddle_test.mat', 'u', 't', 'y');
|
|
||||||
u = u - mean(u);
|
|
||||||
y = y - mean(y);
|
|
||||||
|
|
||||||
%%
|
|
||||||
[pxx, f] = pwelch(y, win, [], [], 1/Ts);
|
|
||||||
[pht, ~] = pwelch(ht.y, win, [], [], 1/Ts);
|
|
||||||
|
|
||||||
figure;
|
|
||||||
|
|
||||||
hold on;
|
|
||||||
plot(f, pxx);
|
|
||||||
plot(f, pht);
|
|
||||||
hold off;
|
|
||||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
|
||||||
ylabel('PSD'); xlabel('Frequency [Hz]');
|
|
||||||
|
|
||||||
%%
|
%%
|
||||||
run setup;
|
run setup;
|
||||||
|
|
||||||
win = hann(ceil(0.1/Ts));
|
win = hann(ceil(10/Ts));
|
||||||
|
|
||||||
[tf_est, f] = tfestimate(u, y, win, [], [], 1/Ts);
|
[p_d, f] = pwelch(d, win, [], [], 1/Ts);
|
||||||
[co_est, ~] = mscohere(u, y, win, [], [], 1/Ts);
|
[p_acc1, ~] = pwelch(acc_1, win, [], [], 1/Ts);
|
||||||
|
[p_acc2, ~] = pwelch(acc_2, win, [], [], 1/Ts);
|
||||||
|
[p_geo1, ~] = pwelch(geo_1, win, [], [], 1/Ts);
|
||||||
|
[p_geo2, ~] = pwelch(geo_2, win, [], [], 1/Ts);
|
||||||
|
|
||||||
|
%%
|
||||||
|
figure;
|
||||||
|
hold on;
|
||||||
|
plot(f, p_acc1);
|
||||||
|
plot(f, p_acc2);
|
||||||
|
hold off;
|
||||||
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||||
|
ylabel('PSD [$(m/s^2)^2/Hz$]'); xlabel('Frequency [Hz]');
|
||||||
|
|
||||||
|
figure;
|
||||||
|
hold on;
|
||||||
|
plot(f, p_geo1);
|
||||||
|
plot(f, p_geo2);
|
||||||
|
hold off;
|
||||||
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||||
|
ylabel('PSD [$(m/s)^2/Hz$]'); xlabel('Frequency [Hz]');
|
||||||
|
|
||||||
|
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]');
|
||||||
|
|
||||||
|
%%
|
||||||
|
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, acc_1, win, [], [], 1/Ts);
|
||||||
|
[co_acc1_est, ~] = mscohere(d, acc_1, win, [], [], 1/Ts);
|
||||||
|
|
||||||
|
[tf_acc2_est, ~] = tfestimate(d, acc_2, win, [], [], 1/Ts);
|
||||||
|
[co_acc2_est, ~] = mscohere(d, acc_2, win, [], [], 1/Ts);
|
||||||
|
|
||||||
%%
|
%%
|
||||||
figure;
|
figure;
|
||||||
|
|
||||||
hold on;
|
hold on;
|
||||||
plot(f, co_est, 'k-')
|
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');
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||||
ylabel('Coherence'); xlabel('Frequency [Hz]');
|
ylabel('Coherence'); xlabel('Frequency [Hz]');
|
||||||
hold off;
|
hold off;
|
||||||
@ -63,51 +95,61 @@ hold off;
|
|||||||
figure;
|
figure;
|
||||||
ax1 = subplot(2, 1, 1);
|
ax1 = subplot(2, 1, 1);
|
||||||
hold on;
|
hold on;
|
||||||
plot(f, abs(tf_est), 'k-', 'DisplayName', 'Identified')
|
plot(f, abs(tf_geo1_est), '-', 'DisplayName', 'Geo1')
|
||||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||||
ylabel('Amplitude'); xlabel('Frequency [Hz]');
|
ylabel('Amplitude'); xlabel('Frequency [Hz]');
|
||||||
hold off;
|
hold off;
|
||||||
|
|
||||||
ax2 = subplot(2, 1, 2);
|
ax2 = subplot(2, 1, 2);
|
||||||
hold on;
|
hold on;
|
||||||
plot(f, 180/pi*unwrap(angle(-tf_est)), 'k-')
|
plot(f, 180/pi*unwrap(angle(tf_geo1_est)), '-')
|
||||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||||
ylabel('Phase'); xlabel('Frequency [Hz]');
|
ylabel('Phase'); xlabel('Frequency [Hz]');
|
||||||
hold off;
|
hold off;
|
||||||
|
|
||||||
linkaxes([ax1,ax2], 'x');
|
linkaxes([ax1,ax2], 'x');
|
||||||
xlim([10, 5000]);
|
|
||||||
|
|
||||||
%%
|
|
||||||
win = hann(ceil(0.2/Ts));
|
|
||||||
|
|
||||||
[tf_est, f] = tfestimate(u, v, win, [], [], 1/Ts);
|
|
||||||
[co_est, ~] = mscohere(u, v, win, [], [], 1/Ts);
|
|
||||||
|
|
||||||
%%
|
|
||||||
figure;
|
|
||||||
|
|
||||||
hold on;
|
|
||||||
plot(f, co_est, 'k-')
|
|
||||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
|
||||||
ylabel('Coherence'); xlabel('Frequency [Hz]');
|
|
||||||
hold off;
|
|
||||||
|
|
||||||
%%
|
%%
|
||||||
figure;
|
figure;
|
||||||
ax1 = subplot(2, 1, 1);
|
ax1 = subplot(2, 1, 1);
|
||||||
hold on;
|
hold on;
|
||||||
plot(f, abs(tf_est), 'k-', 'DisplayName', 'Identified')
|
plot(f, abs(tf_acc1_est), '-', 'DisplayName', 'Acc1')
|
||||||
|
plot(f, abs(tf_acc2_est), '-', 'DisplayName', 'Acc2')
|
||||||
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||||
ylabel('Amplitude'); xlabel('Frequency [Hz]');
|
ylabel('Amplitude'); xlabel('Frequency [Hz]');
|
||||||
hold off;
|
hold off;
|
||||||
|
|
||||||
ax2 = subplot(2, 1, 2);
|
ax2 = subplot(2, 1, 2);
|
||||||
hold on;
|
hold on;
|
||||||
plot(f, 180/pi*unwrap(angle(-tf_est)), 'k-')
|
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');
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||||
ylabel('Phase'); xlabel('Frequency [Hz]');
|
ylabel('Phase'); xlabel('Frequency [Hz]');
|
||||||
hold off;
|
hold off;
|
||||||
|
|
||||||
linkaxes([ax1,ax2], 'x');
|
linkaxes([ax1,ax2], 'x');
|
||||||
xlim([10, 5000]);
|
|
||||||
|
%%
|
||||||
|
win = hann(ceil(10/Ts));
|
||||||
|
|
||||||
|
[p_acc_1, f] = pwelch(acc_1, win, [], [], 1/Ts);
|
||||||
|
[co_acc12, ~] = mscohere(acc_1, 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]');
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
421
sensor_fusion_analysis.m
Normal file
421
sensor_fusion_analysis.m
Normal file
@ -0,0 +1,421 @@
|
|||||||
|
|
||||||
|
%% 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, p_acc1./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
|
||||||
|
'DisplayName', 'Accelerometer');
|
||||||
|
set(gca, 'ColorOrderIndex', 1);
|
||||||
|
plot(f, p_acc2./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
|
||||||
|
'HandleVisibility', 'off');
|
||||||
|
set(gca, 'ColorOrderIndex', 2);
|
||||||
|
plot(f, p_geo1./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
|
||||||
|
'DisplayName', 'Geophone');
|
||||||
|
set(gca, 'ColorOrderIndex', 2);
|
||||||
|
plot(f, p_geo2./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
|
||||||
|
'HandleVisibility', 'off');
|
||||||
|
set(gca, 'ColorOrderIndex', 3);
|
||||||
|
plot(f, p_d, 'DisplayName', 'Interferometer');
|
||||||
|
hold off;
|
||||||
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||||||
|
ylabel('PSD [$m^2/Hz$]'); xlabel('Frequency [Hz]');
|
||||||
|
title('Huddle Test')
|
||||||
|
legend();
|
||||||
|
|
||||||
|
%% Compare Theoretical model with identified one
|
||||||
|
id_ol = load('./mat/identification_chirp_40_400.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*unwrap(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*237;
|
||||||
|
xi_p = 0.015;
|
||||||
|
|
||||||
|
Giff = -20*(s^2 + 2*xi_z*s*wz + wz^2)/(s^2 + 2*xi_p*s*wp + wp^2);
|
||||||
|
|
||||||
|
% 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]);
|
||||||
|
|
||||||
|
%% Estimation of the inertial sensor transfer functions
|
||||||
|
id = load('./mat/identification_noise_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');
|
||||||
|
|
||||||
|
% Compare PSD
|
||||||
|
run setup;
|
||||||
|
win = hann(ceil(1/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();
|
||||||
|
|
||||||
|
% Transfer Functions
|
||||||
|
figure;
|
||||||
|
ax1 = subplot(2, 1, 1);
|
||||||
|
hold on;
|
||||||
|
plot(f, abs(tf_acc1_est), '-')
|
||||||
|
plot(f, abs(tf_acc2_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_acc1_est), '-')
|
||||||
|
plot(f, 180/pi*angle(tf_acc2_est), '-')
|
||||||
|
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), '-')
|
||||||
|
plot(f, abs(tf_geo2_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_geo1_est), '-')
|
||||||
|
plot(f, 180/pi*angle(tf_geo2_est), '-')
|
||||||
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||||||
|
ylabel('Phase'); xlabel('Frequency [Hz]');
|
||||||
|
hold off;
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2], 'x');
|
||||||
|
xlim([2, 2e3]);
|
||||||
Binary file not shown.
18
setup.m
18
setup.m
@ -1,6 +1,18 @@
|
|||||||
|
%%
|
||||||
s = tf('s');
|
s = tf('s');
|
||||||
Ts = 1e-4;
|
Ts = 1e-4; % [s]
|
||||||
|
|
||||||
Glpf = 1/(1 + s/2/pi/500);
|
|
||||||
|
|
||||||
|
%% Pre-Filter
|
||||||
|
% Glpf = 1/(1 + s/2/pi/2e3);
|
||||||
|
Glpf = 1/(1 + s/2/pi/50); % Used to excite with constant velocity
|
||||||
Gz = c2d(Glpf, Ts, 'tustin');
|
Gz = c2d(Glpf, 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);
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user