tdehaeze/sensor-fusion-test-bench
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

new identifications

Browse Source
This commit is contained in:
Thomas Dehaeze 2020-09-02 17:30:22 +02:00
parent a13d698fb0
commit 582f2575ed
7 changed files with 225 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
mat/identification_noise_bis.mat
View File

Binary file not shown.

BIN
mat/identification_noise_iff_bis.mat Normal file
View File

Binary file not shown.

BIN
mat/identification_noise_opt_iff.mat Normal file
View File

Binary file not shown.

14
runtest.m
View File

@ -8,16 +8,17 @@ close(f);
%% Convert the Data %% Convert the Data
data = SimulinkRealTime.utils.getFileScopeData('data/apa95ml.dat').data; data = SimulinkRealTime.utils.getFileScopeData('data/apa95ml.dat').data;
d = data(:, 1); d = data(:, 1); % Interferomter [m]
acc_1 = data(:, 2); acc_1 = data(:, 2);
acc_2 = data(:, 3); acc_2 = data(:, 3);
geo_1 = data(:, 4); geo_1 = data(:, 4);
geo_2 = data(:, 5); geo_2 = data(:, 5);
u = data(:, 6); u = data(:, 6); % Excitation Signal [V]
f_meas = data(:, 7); v = data(:, 7); % Input signal to the amplifier [V]
t = data(:, 8); f_meas = data(:, 8); % Voltage generated by the force sensor [V]
t = data(:, 9);
save('./mat/identification_noise_bis.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't'); save('./mat/identification_noise_opt_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'v', 'u', 't');
%% %%
d = detrend(d, 0); d = detrend(d, 0);
@ -150,6 +151,3 @@ hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log'); set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('PSD'); xlabel('Frequency [Hz]'); ylabel('PSD'); xlabel('Frequency [Hz]');

239
sensor_fusion_analysis.m
View File

@ -68,22 +68,22 @@ G_geo = 120*s^2/(s^2 + 2*0.7*2*pi*2*s + (2*pi*2)^2); % [[V/(m/s)]
figure; figure;
hold on; hold on;
set(gca, 'ColorOrderIndex', 1); set(gca, 'ColorOrderIndex', 1);
plot(f, p_acc1./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ... plot(f, sqrt(p_acc1)./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
'DisplayName', 'Accelerometer'); 'DisplayName', 'Accelerometer');
set(gca, 'ColorOrderIndex', 1); set(gca, 'ColorOrderIndex', 1);
plot(f, p_acc2./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ... plot(f, sqrt(p_acc2)./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
'HandleVisibility', 'off'); 'HandleVisibility', 'off');
set(gca, 'ColorOrderIndex', 2); set(gca, 'ColorOrderIndex', 2);
plot(f, p_geo1./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ... plot(f, sqrt(p_geo1)./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
'DisplayName', 'Geophone'); 'DisplayName', 'Geophone');
set(gca, 'ColorOrderIndex', 2); set(gca, 'ColorOrderIndex', 2);
plot(f, p_geo2./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ... plot(f, sqrt(p_geo2)./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
'HandleVisibility', 'off'); 'HandleVisibility', 'off');
set(gca, 'ColorOrderIndex', 3); set(gca, 'ColorOrderIndex', 3);
plot(f, p_d, 'DisplayName', 'Interferometer'); plot(f, sqrt(p_d), 'DisplayName', 'Interferometer');
hold off; hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log'); set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('PSD [$m^2/Hz$]'); xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]'); xlabel('Frequency [Hz]');
title('Huddle Test') title('Huddle Test')
legend(); legend();
@ -191,6 +191,8 @@ hold off;
linkaxes([ax1,ax2], 'x'); linkaxes([ax1,ax2], 'x');
xlim([40, 400]); xlim([40, 400]);
%% IFF development %% IFF development
[tf_fmeas_est, f] = tfestimate(id_ol.u, id_ol.f_meas, win, [], [], 1/Ts); % [V/m] [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); [co_fmeas_est, ~] = mscohere(id_ol.u, id_ol.f_meas, win, [], [], 1/Ts);
@ -201,7 +203,7 @@ xi_z = 0.01;
wp = 2*pi*238; wp = 2*pi*238;
xi_p = 0.015; xi_p = 0.015;
Giff = 2*(s^2 + 2*xi_z*s*wz + wz^2)/(s^2 + 2*xi_p*s*wp + wp^2); 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 % Comparison model and identification
figure; figure;
@ -242,7 +244,7 @@ ylabel('Imaginary Part')
axis square axis square
% Optimal Controller % Optimal Controller
Kiff_opt = 1100/(s + 2*pi*2); Kiff_opt = 110/(s + 2*pi*2);
%% New identification %% 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_ol = load('./mat/identification_chirp_40_400.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
@ -286,13 +288,150 @@ hold off;
linkaxes([ax1,ax2], 'x'); linkaxes([ax1,ax2], 'x');
xlim([40, 400]); xlim([40, 400]);
%% Estimation of the inertial sensor transfer functions
id = load('./mat/identification_noise.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't'); %% 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 = 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 % Compare PSD
run setup; run setup;
win = hann(ceil(1/Ts)); win = hann(ceil(10/Ts));
[p_id_d, f] = pwelch(id.d, win, [], [], 1/Ts); [p_id_d, f] = pwelch(id.d, win, [], [], 1/Ts);
[p_id_acc1, ~] = pwelch(id.acc_1, win, [], [], 1/Ts); [p_id_acc1, ~] = pwelch(id.acc_1, win, [], [], 1/Ts);
@ -378,20 +517,27 @@ hold off;
xlim([2, 2e3]); ylim([0, 1]) xlim([2, 2e3]); ylim([0, 1])
legend(); 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 % Transfer Functions
figure; figure;
ax1 = subplot(2, 1, 1); ax1 = subplot(2, 1, 1);
hold on; hold on;
plot(f, abs(tf_acc1_est), '-') plot(f, abs(tf_acc1_est./(1i*2*pi*f).^2), '-')
plot(f, abs(tf_acc2_est), '-') 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'); set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]'); ylabel('Amplitude [V/(m/s^2)]'); 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*angle(tf_acc1_est), '-') plot(f, 180/pi*angle(tf_acc1_est./(1i*2*pi*f).^2), '-')
plot(f, 180/pi*angle(tf_acc2_est), '-') 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'); set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]'); ylabel('Phase'); xlabel('Frequency [Hz]');
hold off; hold off;
@ -403,19 +549,68 @@ xlim([2, 2e3]);
figure; figure;
ax1 = subplot(2, 1, 1); ax1 = subplot(2, 1, 1);
hold on; hold on;
plot(f, abs(tf_geo1_est), '-') plot(f, abs(tf_geo1_est./(1i*2*pi*f)), '-')
plot(f, abs(tf_geo2_est), '-') 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'); set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]'); ylabel('Amplitude[V/(m/s)]'); 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*angle(tf_geo1_est), '-') plot(f, 180/pi*angle(tf_geo1_est./(1i*2*pi*f)), '-')
plot(f, 180/pi*angle(tf_geo2_est), '-') 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'); 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([2, 2e3]); 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]');

BIN
sensor_fusion_test_bench.slx
View File

Binary file not shown.

2
setup.m
View File

@ -9,6 +9,7 @@ G_pf = c2d(G_pf, Ts, 'tustin');
%% Force Sensor Filter (HPF) %% Force Sensor Filter (HPF)
Gf_hpf = s/(s + 2*pi*2); Gf_hpf = s/(s + 2*pi*2);
Gf_hpf = tf(1);
Gf_hpf = c2d(Gf_hpf, Ts, 'tustin'); Gf_hpf = c2d(Gf_hpf, Ts, 'tustin');
%% IFF Controller %% IFF Controller
@ -21,3 +22,4 @@ Tsim = 180; % Excitation time + Measurement time [s]
t = 0:Ts:Tsim; t = 0:Ts:Tsim;
% u_exc = timeseries(chirp(t, 0.1, Tsim, 1e3, 'logarithmic'), t); % u_exc = timeseries(chirp(t, 0.1, Tsim, 1e3, 'logarithmic'), t);
u_exc = timeseries(chirp(t, 40, Tsim, 400, 'logarithmic'), t); u_exc = timeseries(chirp(t, 40, Tsim, 400, 'logarithmic'), t);
u_exc = timeseries(y_v, t);