225 lines
8.6 KiB
Mathematica
225 lines
8.6 KiB
Mathematica
|
%% Clear Workspace and Close figures
|
||
|
|
clear; close all; clc;
|
||
|
|
|
||
|
|
%% Intialize Laplace variable
|
||
|
|
s = zpk('s');
|
||
|
|
|
||
|
|
addpath('./mat/');
|
||
|
|
|
||
|
|
% Load Data
|
||
|
|
% As before, the identification data is loaded and any offset if removed.
|
||
|
|
|
||
|
|
id = load('identification_noise_opt_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
|
||
|
|
|
||
|
|
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);
|
||
|
|
|
||
|
|
% ASD of the Measured displacement
|
||
|
|
% The Power Spectral Density of the displacement as measured by the interferometer and the inertial sensors is computed.
|
||
|
|
|
||
|
|
Ts = id.t(2) - id.t(1);
|
||
|
|
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);
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
% Let's use a model of the accelerometer and geophone to compute the motion from the measured voltage.
|
||
|
|
|
||
|
|
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)]
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
% The obtained ASD in $m/\sqrt{Hz}$ is shown in Figure [[fig:measure_displacement_all_sensors]].
|
||
|
|
|
||
|
|
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
set(gca, 'ColorOrderIndex', 1);
|
||
|
|
plot(f, sqrt(p_id_acc1)./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
|
||
|
|
'DisplayName', 'Accelerometer');
|
||
|
|
set(gca, 'ColorOrderIndex', 1);
|
||
|
|
plot(f, sqrt(p_id_acc2)./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
set(gca, 'ColorOrderIndex', 2);
|
||
|
|
plot(f, sqrt(p_id_geo1)./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
|
||
|
|
'DisplayName', 'Geophone');
|
||
|
|
set(gca, 'ColorOrderIndex', 2);
|
||
|
|
plot(f, sqrt(p_id_geo2)./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
set(gca, 'ColorOrderIndex', 3);
|
||
|
|
plot(f, sqrt(p_id_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();
|
||
|
|
xlim([1e-1, 5e3]); ylim([1e-12, 1e-4]);
|
||
|
|
|
||
|
|
% ASD of the Sensor Noise
|
||
|
|
% The noise of a sensor can be estimated using two identical sensors by computing:
|
||
|
|
% - the Power Spectral Density of the measured motion by the two sensors
|
||
|
|
% - the Cross Spectral Density between the two sensors (coherence)
|
||
|
|
|
||
|
|
% This technique to estimate the sensor noise is described in cite:barzilai98_techn_measur_noise_sensor_presen.
|
||
|
|
|
||
|
|
% The Power Spectral Density of the sensor noise can be estimated using the following equation:
|
||
|
|
% \begin{equation}
|
||
|
|
% |S_n(\omega)| = |S_{x_1}(\omega)| \Big( 1 - \gamma_{x_1 x_2}(\omega) \Big)
|
||
|
|
% \end{equation}
|
||
|
|
% with $S_{x_1}$ the PSD of one of the sensor and $\gamma_{x_1 x_2}$ the coherence between the two sensors.
|
||
|
|
|
||
|
|
% The coherence between the two accelerometers and between the two geophones is computed.
|
||
|
|
|
||
|
|
[coh_acc, ~] = mscohere(id.acc_1, id.acc_2, win, [], [], 1/Ts);
|
||
|
|
[coh_geo, ~] = mscohere(id.geo_1, id.geo_2, win, [], [], 1/Ts);
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
% Finally, the Power Spectral Density of the sensors is computed and converted in $[m^2/Hz]$.
|
||
|
|
|
||
|
|
pN_acc = p_id_acc1.*(1 - coh_acc) .* ... % [V^2/Hz]
|
||
|
|
1./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))).^2; % [(m/V)^2]
|
||
|
|
pN_geo = p_id_geo1.*(1 - coh_geo) .* ... % [V^2/Hz]
|
||
|
|
1./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))).^2; % [(m/V)^2]
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
% The ASD of obtained noises are compared with the ASD of the measured signals in Figure [[fig:noise_inertial_sensors_comparison]].
|
||
|
|
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(f, sqrt(p_id_acc1)./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))), ...
|
||
|
|
'DisplayName', 'Accelerometer');
|
||
|
|
plot(f, sqrt(p_id_geo1)./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))), ...
|
||
|
|
'DisplayName', 'Geophone');
|
||
|
|
plot(f, sqrt(pN_acc), '-', 'DisplayName', 'Accelerometers - Noise');
|
||
|
|
plot(f, sqrt(pN_geo), '-', 'DisplayName', 'Geophones - Noise');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{m}{\sqrt{Hz}}\right]$');
|
||
|
|
xlim([1, 5000]); ylim([1e-12, 1e-5]);
|
||
|
|
legend('location', 'northeast');
|
||
|
|
|
||
|
|
% Noise Model
|
||
|
|
% Transfer functions are adjusted in order to fit the ASD of the sensor noises (expressed in $[m/s/\sqrt{Hz}]$ for more easy fitting).
|
||
|
|
|
||
|
|
% These transfer functions are defined below and compared with the measured ASD in Figure [[fig:noise_models_velocity]].
|
||
|
|
|
||
|
|
N_acc = 1*(s/(2*pi*2000) + 1)^2/(s + 0.1*2*pi)/(s + 1e3*2*pi); % [m/sqrt(Hz)]
|
||
|
|
N_geo = 4e-4*(s/(2*pi*200) + 1)/(s + 1e3*2*pi); % [m/sqrt(Hz)]
|
||
|
|
|
||
|
|
freqs = logspace(0, 4, 1000);
|
||
|
|
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(f, sqrt(pN_acc).*(2*pi*f), '-', 'DisplayName', 'Accelerometers - Noise');
|
||
|
|
plot(f, sqrt(pN_geo).*(2*pi*f), '-', 'DisplayName', 'Geophones - Noise');
|
||
|
|
set(gca, 'ColorOrderIndex', 1);
|
||
|
|
plot(freqs, abs(squeeze(freqresp(N_acc, freqs, 'Hz'))), '--', 'DisplayName', 'Accelerometer - Noise Model');
|
||
|
|
plot(freqs, abs(squeeze(freqresp(N_geo, freqs, 'Hz'))), '--', 'DisplayName', 'Geophones - Noise Model');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{m/s}{\sqrt{Hz}}\right]$');
|
||
|
|
xlim([1, 5000]);
|
||
|
|
legend('location', 'northeast');
|
||
|
|
|
||
|
|
% $\mathcal{H}_2$ Synthesis of the Complementary Filters
|
||
|
|
% We now wish to synthesize two complementary filters to merge the geophone and the accelerometer signal in such a way that the fused signal has the lowest possible RMS noise.
|
||
|
|
|
||
|
|
% To do so, we use the $\mathcal{H}_2$ synthesis where the transfer functions representing the noise density of both sensors are used as weights.
|
||
|
|
|
||
|
|
% The generalized plant used for the synthesis is defined below.
|
||
|
|
|
||
|
|
P = [0 N_acc 1;
|
||
|
|
N_geo -N_acc 0];
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
% And the $\mathcal{H}_2$ synthesis is done using the =h2syn= command.
|
||
|
|
|
||
|
|
[H_geo, ~, gamma] = h2syn(P, 1, 1);
|
||
|
|
H_acc = 1 - H_geo;
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
% The obtained complementary filters are shown in Figure [[fig:complementary_filters_velocity_H2]].
|
||
|
|
|
||
|
|
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(freqs, abs(squeeze(freqresp(H_acc, freqs, 'Hz'))), '-', 'DisplayName', '$H_{acc}$');
|
||
|
|
plot(freqs, abs(squeeze(freqresp(H_geo, freqs, 'Hz'))), '-', 'DisplayName', '$H_{geo}$');
|
||
|
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
|
||
|
|
ylabel('Magnitude'); set(gca, 'XTickLabel',[]);
|
||
|
|
hold off;
|
||
|
|
legend('location', 'northeast');
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(H_acc, freqs, 'Hz'))), '-');
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(H_geo, freqs, 'Hz'))), '-');
|
||
|
|
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
|
||
|
|
ylabel('Phase'); xlabel('Frequency [Hz]');
|
||
|
|
hold off;
|
||
|
|
ylim([-180, 180]);
|
||
|
|
yticks([-180, -90, 0, 90, 180]);
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2], 'x');
|
||
|
|
xlim([freqs(1), freqs(end)]);
|
||
|
|
|
||
|
|
% Results
|
||
|
|
% Finally, the signals of both sensors are merged using the complementary filters and the super sensor noise is estimated and compared with the individual sensor noises in Figure [[fig:super_sensor_noise_asd_velocity]].
|
||
|
|
|
||
|
|
|
||
|
|
freqs = logspace(0, 4, 1000);
|
||
|
|
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(f, pN_acc.*(2*pi*f), '-', 'DisplayName', 'Accelerometers - Noise');
|
||
|
|
plot(f, pN_geo.*(2*pi*f), '-', 'DisplayName', 'Geophones - Noise');
|
||
|
|
plot(f, sqrt((pN_acc.*(2*pi*f)).^2.*abs(squeeze(freqresp(H_acc, f, 'Hz'))).^2 + (pN_geo.*(2*pi*f)).^2.*abs(squeeze(freqresp(H_geo, f, 'Hz'))).^2), 'k-', 'DisplayName', 'Super Sensor - Noise');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{m/s}{\sqrt{Hz}}\right]$');
|
||
|
|
xlim([1, 5000]);
|
||
|
|
legend('location', 'northeast');
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
% #+name: fig:super_sensor_noise_asd_velocity
|
||
|
|
% #+caption: ASD of the super sensor noise (velocity)
|
||
|
|
% #+RESULTS:
|
||
|
|
% [[file:figs/super_sensor_noise_asd_velocity.png]]
|
||
|
|
|
||
|
|
% Finally, the Cumulative Power Spectrum is computed and compared in Figure [[fig:super_sensor_noise_cas_velocity]].
|
||
|
|
|
||
|
|
[~, i_1Hz] = min(abs(f - 1));
|
||
|
|
|
||
|
|
CPS_acc = 1/pi*flip(-cumtrapz(2*pi*flip(f), flip((pN_acc.*(2*pi*f)).^2)));
|
||
|
|
CPS_geo = 1/pi*flip(-cumtrapz(2*pi*flip(f), flip((pN_geo.*(2*pi*f)).^2)));
|
||
|
|
CPS_SS = 1/pi*flip(-cumtrapz(2*pi*flip(f), flip((pN_acc.*(2*pi*f)).^2.*abs(squeeze(freqresp(H_acc, f, 'Hz'))).^2 + (pN_geo.*(2*pi*f)).^2.*abs(squeeze(freqresp(H_geo, f, 'Hz'))).^2)));
|
||
|
|
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(f, CPS_acc, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{acc}} = %.0f\\,\\mu m/s (rms)$', 1e6*sqrt(CPS_acc(i_1Hz))));
|
||
|
|
plot(f, CPS_geo, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{geo}} = %.0f\\,\\mu m/s (rms)$', 1e6*sqrt(CPS_geo(i_1Hz))));
|
||
|
|
plot(f, CPS_SS, 'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}} = %.0f\\,\\mu m/s (rms)$', 1e6*sqrt(CPS_SS(i_1Hz))));
|
||
|
|
set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum');
|
||
|
|
hold off;
|
||
|
|
xlim([1, 4e3]);
|
||
|
|
legend('location', 'northeast');
|