#+TITLE: Sensor Fusion - Test Bench :DRAWER: #+LANGUAGE: en #+EMAIL: dehaeze.thomas@gmail.com #+AUTHOR: Dehaeze Thomas #+HTML_HEAD: #+HTML_HEAD: #+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/tikz/org/}{config.tex}") #+PROPERTY: header-args:latex+ :imagemagick t :fit yes #+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150 #+PROPERTY: header-args:latex+ :imoutoptions -quality 100 #+PROPERTY: header-args:latex+ :results raw replace :buffer no #+PROPERTY: header-args:latex+ :eval no-export #+PROPERTY: header-args:latex+ :exports both #+PROPERTY: header-args:latex+ :mkdirp yes #+PROPERTY: header-args:latex+ :output-dir figs #+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png") #+PROPERTY: header-args:matlab :session *MATLAB* #+PROPERTY: header-args:matlab+ :comments org #+PROPERTY: header-args:matlab+ :exports both #+PROPERTY: header-args:matlab+ :results none #+PROPERTY: header-args:matlab+ :eval no-export #+PROPERTY: header-args:matlab+ :noweb yes #+PROPERTY: header-args:matlab+ :mkdirp yes #+PROPERTY: header-args:matlab+ :output-dir figs #+BIND: org-latex-image-default-option "scale=1" #+BIND: org-latex-image-default-width "" #+BIND: org-latex-bib-compiler "biber" #+OPTIONS: toc:2 #+LaTeX_CLASS: scrreprt #+LaTeX_CLASS_OPTIONS: [a4paper, 10pt, DIV=12, parskip=full] #+LaTeX_HEADER_EXTRA: \input{preamble.tex} #+LATEX_HEADER_EXTRA: \addbibresource{ref.bib} :END: #+begin_export html

This report is also available as a pdf.

#+end_export * Introduction :ignore: In this document, we wish the experimentally validate sensor fusion of inertial sensors. This document is divided into the following sections: - Section [[sec:experimental_setup]]: the experimental setup is described - Section [[sec:first_identification]]: a first identification of the system dynamics is performed - Section [[sec:integral_force_feedback]]: the integral force feedback active damping technique is applied on the system - Section [[sec:optimal_excitation]]: the optimal excitation signal is determine in order to have the best possible system dynamics estimation - Section [[sec:inertial_sensor_dynamics]]: the inertial sensor dynamics are experimentally estimated - Section [[sec:inertial_sensor_noise]]: the inertial sensor noises are estimated and the $\mathcal{H}_2$ synthesis of complementary filters is performed in order to yield a super sensor with minimal noise - Section [[sec:inertial_sensor_uncertainty]]: the dynamical uncertainty of the inertial sensors is estimated. Then the $\mathcal{H}_\infty$ synthesis of complementary filters is performed in order to minimize the super sensor dynamical uncertainty - Section [[sec:optimal_sensor_fusion]]: Optimal sensor fusion is performed using the $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis * Experimental Setup <> The goal of this experimental setup is to experimentally merge inertial sensors. To merge the sensors, optimal and robust complementary filters are designed. A schematic of the test-bench used is shown in Figure [[fig:exp_setup_sensor_fusion]] and a picture of it is shown in Figure [[fig:test-bench-sensor-fusion-picture]]. #+name: fig:exp_setup_sensor_fusion #+caption: Schematic of the test-bench [[file:figs/exp_setup_sensor_fusion.png]] #+name: fig:test-bench-sensor-fusion-picture #+caption: Picture of the test-bench [[file:figs/test-bench-sensor-fusion-picture.png]] Two inertial sensors are used: - An vertical accelerometer /PCB 393B05/ ([[file:doc/TM-VIB-Seismic_Lowres.pdf][doc]]) - A vertical geophone /Mark Product L-22/ Basic characteristics of both sensors are shown in Tables [[tab:393B05_spec]] and [[tab:L22_spec]]. #+name: tab:393B05_spec #+caption: Accelerometer (393B05) Specifications | *Specification* | *Value* | |-------------------------+--------------------| | Sensitivity | 1.02 [V/(m/s2)] | | Resonant Frequency | > 2.5 [kHz] | | Resolution (1 to 10kHz) | 0.00004 [m/s2 rms] | #+name: tab:L22_spec #+caption: Geophone (L22) Specifications | *Specification* | *Value* | |-------------------------+--------------------------| | Sensitivity | To be measured [V/(m/s)] | | Resonant Frequency | 2 [Hz] | The ADC used are the IO131 Speedgoat module ([[https://www.speedgoat.com/products/io-connectivity-analog-io131][link]]) with a 16bit resolution over +/- 10V. The geophone signals are amplified using a DLPVA-100-B-D voltage amplified from Femto ([[file:doc/de-dlpva-100-b.pdf][doc]]). The force sensor signal is amplified using a Low Noise Voltage Preamplifier from Ametek ([[file:doc/model_5113.pdf][doc]]). The excitation signal is amplified by a linear amplified from Cedrat (LA75B) with a gain equals to 20 ([[file:doc/LA75B.pdf][doc]]). Geophone electronics: - gain: 10 (20dB) - low pass filter: 1.5Hz - hifh pass filter: 100kHz (2nd order) Force Sensor electronics: - gain: 10 (20dB) - low pass filter: 1st order at 3Hz - high pass filter: 1st order at 30kHz * First identification of the system :PROPERTIES: :header-args:matlab+: :tangle matlab/first_identification.m :END: <> ** Introduction :ignore: In this section, a first identification of each elements of the system is performed. This include the dynamics from the actuator to the force sensor, interferometer and inertial sensors. Each of the dynamics is compared with the dynamics identified form a Simscape model. ** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) <> #+end_src #+begin_src matlab :exports none :results silent :noweb yes <> #+end_src #+begin_src matlab :tangle no addpath('./matlab/'); addpath('./matlab/mat/'); #+end_src #+begin_src matlab :eval no addpath('./mat/'); #+end_src ** Load Data The data is loaded in the Matlab workspace. #+begin_src matlab id_ol = load('identification_noise_bis.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't'); #+end_src Then, any offset is removed. #+begin_src matlab 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); #+end_src ** Excitation Signal The generated voltage used to excite the system is a white noise and can be seen in Figure [[fig:excitation_signal_first_identification]]. #+begin_src matlab :exports none figure; plot(id_ol.t, id_ol.u) xlabel('Time [s]'); ylabel('Voltage [V]'); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/excitation_signal_first_identification.pdf', 'width', 'wide', 'height', 'normal'); #+end_src #+name: fig:excitation_signal_first_identification #+caption: Voltage excitation signal #+RESULTS: [[file:figs/excitation_signal_first_identification.png]] ** Identified Plant The transfer function from the excitation voltage to the mass displacement and to the force sensor stack voltage are identified using the =tfestimate= command. #+begin_src matlab Ts = id_ol.t(2) - id_ol.t(1); win = hann(ceil(10/Ts)); #+end_src #+begin_src matlab [tf_fmeas_est, f] = tfestimate(id_ol.u, id_ol.f_meas, win, [], [], 1/Ts); % [V/V] [tf_G_ol_est, ~] = tfestimate(id_ol.u, id_ol.d, win, [], [], 1/Ts); % [m/V] #+end_src The bode plots of the obtained dynamics are shown in Figures [[fig:force_sensor_bode_plot]] and [[fig:displacement_sensor_bode_plot]]. #+begin_src matlab :exports none figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; plot(f, abs(tf_fmeas_est), '-') hold off; set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log'); ylabel('Amplitude'); set(gca, 'XTickLabel',[]); ax2 = nexttile; 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; ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2], 'x'); xlim([1, 1e3]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/force_sensor_bode_plot.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:force_sensor_bode_plot #+caption: Bode plot of the dynamics from excitation voltage to measured force sensor stack voltage #+RESULTS: [[file:figs/force_sensor_bode_plot.png]] #+begin_src matlab :exports none figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; plot(f, abs(tf_G_ol_est), '-') hold off; set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log'); ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]); ax2 = nexttile; hold on; plot(f, 180/pi*angle(tf_G_ol_est), '-') 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([1, 1e3]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/displacement_sensor_bode_plot.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:displacement_sensor_bode_plot #+caption: Bode plot of the dynamics from excitation voltage to displacement of the mass as measured by the interferometer #+RESULTS: [[file:figs/displacement_sensor_bode_plot.png]] ** Simscape Model - Comparison A simscape model representing the test-bench has been developed. The same transfer functions as the one identified using the test-bench can be obtained thanks to the simscape model. They are compared in Figure [[fig:simscape_comp_iff_plant]] and [[fig:simscape_comp_disp_plant]]. It is shown that there is a good agreement between the model and the experiment. #+begin_src matlab load('piezo_amplified_3d.mat', 'int_xyz', 'int_i', 'n_xyz', 'n_i', 'nodes', 'M', 'K'); #+end_src #+begin_src matlab :exports none m = 7.5; Kiff = tf(0); #+end_src #+begin_src matlab :exports none %% Name of the Simulink File mdl = 'sensor_fusion_test_bench_simscape'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/Fd'], 1, 'openinput'); io_i = io_i + 1; % External Vertical Force [N] io(io_i) = linio([mdl, '/w'], 1, 'openinput'); io_i = io_i + 1; % Base Motion [m] io(io_i) = linio([mdl, '/Va'], 1, 'openinput'); io_i = io_i + 1; % Actuator Voltage [V] io(io_i) = linio([mdl, '/Interferometer'], 1, 'openoutput'); io_i = io_i + 1; % Vertical Displacement [m] io(io_i) = linio([mdl, '/Voltage_Conditioner'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensor [V] options = linearizeOptions('SampleTime', 1e-4); G = linearize(mdl, io, options); G.InputName = {'Fd', 'w', 'Va'}; G.OutputName = {'y', 'Vs'}; #+end_src #+begin_src matlab :exports none figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; plot(f, abs(tf_fmeas_est), 'DisplayName', 'Identification') plot(f, abs(squeeze(freqresp(G('Vs', 'Va'), f, 'Hz'))), 'DisplayName', 'Simscape Model') set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log'); ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]); hold off; ylim([1e-1, 1e3]); legend('location', 'northwest'); ax2 = nexttile; hold on; plot(f, 180/pi*angle(tf_fmeas_est)) plot(f, 180/pi*angle(squeeze(freqresp(G('Vs', 'Va'), f, '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([1, 5e3]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/simscape_comp_iff_plant.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:simscape_comp_iff_plant #+caption: Comparison of the dynamics from excitation voltage to measured force sensor stack voltage - Identified dynamics and Simscape Model #+RESULTS: [[file:figs/simscape_comp_iff_plant.png]] #+begin_src matlab :exports none figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; plot(f, abs(tf_G_ol_est), 'DisplayName', 'Identification') plot(f, abs(squeeze(freqresp(G('y', 'Va'), f, 'Hz'))), 'DisplayName', 'Simscape Model') set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log'); ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]); hold off; ylim([1e-8, 1e-3]); ax2 = nexttile; hold on; plot(f, 180/pi*angle(tf_G_ol_est)) plot(f, 180/pi*angle(squeeze(freqresp(G('y', 'Va'), f, '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([1, 5e3]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/simscape_comp_disp_plant.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:simscape_comp_disp_plant #+caption: Comparison of the dynamics from excitation voltage to measured mass displacement - Identified dynamics and Simscape Model #+RESULTS: [[file:figs/simscape_comp_disp_plant.png]] ** Integral Force Feedback The force sensor stack can be used to damp the system. This makes the system easier to excite properly without too much amplification near resonances. This is done thanks to the integral force feedback control architecture. The force sensor stack signal is integrated (or rather low pass filtered) and fed back to the force sensor stacks. The low pass filter used as the controller is defined below: #+begin_src matlab Kiff = 102/(s + 2*pi*2); #+end_src The integral force feedback control strategy is applied to the simscape model as well as to the real test bench. #+begin_src matlab :exports none %% Name of the Simulink File mdl = 'sensor_fusion_test_bench_simscape'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/Fd'], 1, 'openinput'); io_i = io_i + 1; % External Vertical Force [N] io(io_i) = linio([mdl, '/w'], 1, 'openinput'); io_i = io_i + 1; % Base Motion [m] io(io_i) = linio([mdl, '/Va'], 1, 'openinput'); io_i = io_i + 1; % Actuator Voltage [V] io(io_i) = linio([mdl, '/Interferometer'], 1, 'openoutput'); io_i = io_i + 1; % Vertical Displacement [m] io(io_i) = linio([mdl, '/Voltage_Conditioner'], 1, 'output'); io_i = io_i + 1; % Force Sensor [V] options = linearizeOptions('SampleTime', 1e-4); G_cl = linearize(mdl, io, options); G_cl.InputName = {'Fd', 'w', 'Va'}; G_cl.OutputName = {'y', 'Vs'}; #+end_src The damped system is then identified again using a noise excitation. The data is loaded into Matlab and any offset is removed. #+begin_src matlab id_cl = load('identification_noise_iff_bis.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't'); id_cl.d = detrend(id_cl.d, 0); id_cl.acc_1 = detrend(id_cl.acc_1, 0); id_cl.acc_2 = detrend(id_cl.acc_2, 0); id_cl.geo_1 = detrend(id_cl.geo_1, 0); id_cl.geo_2 = detrend(id_cl.geo_2, 0); id_cl.f_meas = detrend(id_cl.f_meas, 0); id_cl.u = detrend(id_cl.u, 0); #+end_src The transfer functions are estimated using =tfestimate=. #+begin_src matlab [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); #+end_src The dynamics from driving voltage to the measured displacement are compared both in the open-loop and IFF case, and for the test-bench experimental identification and for the Simscape model in Figure [[fig:iff_ol_cl_identified_simscape_comp]]. This shows that the Integral Force Feedback architecture effectively damps the first resonance of the system. #+begin_src matlab :exports none figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; set(gca, 'ColorOrderIndex', 1); plot(f, abs(tf_G_ol_est), '-', 'DisplayName', 'OL - Ident.') set(gca, 'ColorOrderIndex', 1); plot(f, abs(squeeze(freqresp(G('y', 'Va'), f, 'Hz'))), '--', 'DisplayName', 'OL - Simscape') set(gca, 'ColorOrderIndex', 2); plot(f, abs(tf_G_cl_est), '-', 'DisplayName', 'CL - Ident.') set(gca, 'ColorOrderIndex', 2); plot(f, abs(squeeze(freqresp(G_cl('y', 'Va'), f, 'Hz'))), '--', 'DisplayName', 'CL - Simscape') set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log'); ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]); legend('location', 'northeast'); hold off; ylim([1e-7, 1e-3]); ax2 = nexttile; hold on; set(gca, 'ColorOrderIndex', 1); plot(f, 180/pi*angle(tf_G_ol_est), '-') set(gca, 'ColorOrderIndex', 1); plot(f, 180/pi*angle(squeeze(freqresp(G('y', 'Va'), f, 'Hz'))), '--') set(gca, 'ColorOrderIndex', 2); plot(f, 180/pi*angle(tf_G_cl_est), '-') set(gca, 'ColorOrderIndex', 2); plot(f, 180/pi*angle(squeeze(freqresp(G_cl('y', 'Va'), f, '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([1, 5e3]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/iff_ol_cl_identified_simscape_comp.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:iff_ol_cl_identified_simscape_comp #+caption: Comparison of the open-loop and closed-loop (IFF) dynamics for both the real identification and the Simscape one #+RESULTS: [[file:figs/iff_ol_cl_identified_simscape_comp.png]] ** Inertial Sensors In order to estimate the dynamics of the inertial sensor (the transfer function from the "absolute" displacement to the measured voltage), the following experiment can be performed: - The mass is excited such that is relative displacement as measured by the interferometer is much larger that the ground "absolute" motion. - The transfer function from the measured displacement by the interferometer to the measured voltage generated by the inertial sensors can be estimated. The first point is quite important in order to have a good coherence between the interferometer measurement and the inertial sensor measurement. Here, a first identification is performed were the excitation signal is a white noise. As usual, the data is loaded and any offset is removed. #+begin_src matlab 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); #+end_src Then the transfer functions from the measured displacement by the interferometer to the generated voltage of the inertial sensors are computed.. #+begin_src matlab Ts = id.t(2) - id.t(1); win = hann(ceil(10/Ts)); #+end_src #+begin_src matlab [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); #+end_src The same transfer functions are estimated using the Simscape model. #+begin_src matlab :exports none m = 10; Kiff = tf(0); #+end_src #+begin_src matlab :exports none %% Name of the Simulink File mdl = 'sensor_fusion_test_bench_simscape'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/Va'], 1, 'openinput'); io_i = io_i + 1; % Actuator Voltage [V] io(io_i) = linio([mdl, '/Interferometer'], 1, 'openoutput'); io_i = io_i + 1; % Vertical Displacement [m] io(io_i) = linio([mdl, '/Vertical_Accelerometer_1'], 1, 'openoutput'); io_i = io_i + 1; % Accelerometer [V] io(io_i) = linio([mdl, '/Voltage_Ampl_geo_1'], 1, 'openoutput'); io_i = io_i + 1; % Geophone [V] options = linearizeOptions('SampleTime', 1e-4); G = linearize(mdl, io, options); G.InputName = {'Va'}; G.OutputName = {'y', 'a', 'v'}; G_acc = G('a', 'Va')*inv(G('y', 'Va')); % [V/m] G_geo = G('v', 'Va')*inv(G('y', 'Va')); % [V/m] #+end_src The obtained dynamics of the accelerometer are compared in Figure [[fig:comp_dynamics_accelerometer]] while the one of the geophones are compared in Figure [[fig:comp_dynamics_geophone]]. #+begin_src matlab :exports none freqs = logspace(-1, 4, 1000)'; figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,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(freqs, abs(squeeze(freqresp(G_acc, freqs, 'Hz'))./(1i*2*pi*freqs).^2), 'k-') set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log'); ylabel('Amplitude $\left[\frac{V}{m/s^2}\right]$'); set(gca, 'XTickLabel',[]); hold off; ax2 = nexttile; 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(freqs, 180/pi*angle(squeeze(freqresp(G_acc, freqs, 'Hz'))./(1i*2*pi*freqs).^2), 'k-') 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([2, 2e3]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/comp_dynamics_accelerometer.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:comp_dynamics_accelerometer #+caption: Comparison of the measured accelerometer dynamics #+RESULTS: [[file:figs/comp_dynamics_accelerometer.png]] #+begin_src matlab :exports none freqs = logspace(-1, 4, 1000)'; figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; plot(f, abs(tf_geo1_est./(1i*2*pi*f)), '.') plot(f, abs(tf_geo2_est./(1i*2*pi*f)), '.') plot(freqs, abs(squeeze(freqresp(G_geo, freqs, 'Hz'))./(1i*2*pi*freqs)), 'k-') set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log'); ylabel('Amplitude $\left[\frac{V}{m/s}\right]$'); set(gca, 'XTickLabel',[]); hold off; ax2 = nexttile; 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(freqs, 180/pi*angle(squeeze(freqresp(G_geo, freqs, 'Hz'))./(1i*2*pi*freqs)), 'k-') 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([0.5, 2e3]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/comp_dynamics_geophone.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:comp_dynamics_geophone #+caption: Comparison of the measured geophone dynamics #+RESULTS: [[file:figs/comp_dynamics_geophone.png]] * Optimal IFF Development :PROPERTIES: :header-args:matlab+: :tangle matlab/integral_force_feedback.m :END: <> ** Introduction :ignore: In this section, a proper identification of the transfer function from the force actuator to the force sensor is performed. Then, an optimal IFF controller is developed and applied experimentally. The damped system is identified to verified the effectiveness of the added method. ** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) <> #+end_src #+begin_src matlab :exports none :results silent :noweb yes <> #+end_src #+begin_src matlab :tangle no addpath('./matlab/mat/'); #+end_src #+begin_src matlab :eval no addpath('./mat/'); #+end_src ** Load Data The experimental data is loaded and any offset is removed. #+begin_src matlab id_ol = load('identification_noise_bis.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't'); #+end_src #+begin_src matlab 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); #+end_src ** Experimental Data The transfer function from force actuator to force sensors is estimated. The coherence shown in Figure [[fig:iff_identification_coh]] shows that the excitation signal is good enough. #+begin_src matlab Ts = id_ol.t(2) - id_ol.t(1); win = hann(ceil(10/Ts)); #+end_src #+begin_src matlab [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); #+end_src #+begin_src matlab :exports none figure; hold on; plot(f, co_fmeas_est, '-') set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin'); ylabel('Coherence'); xlabel('Frequency [Hz]'); hold off; xlim([1, 1e3]); ylim([0, 1]) #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/iff_identification_coh.pdf', 'width', 'wide', 'height', 'normal'); #+end_src #+name: fig:iff_identification_coh #+caption: Coherence for the identification of the IFF plant #+RESULTS: [[file:figs/iff_identification_coh.png]] The obtained dynamics is shown in Figure [[fig:iff_identification_bode_plot]]. #+begin_src matlab :exports none figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; plot(f, abs(tf_fmeas_est), '-') set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log'); ylabel('Amplitude'); set(gca, 'XTickLabel',[]); hold off; ylim([1e-1, 1e3]); ax2 = nexttile; 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; ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2], 'x'); xlim([1, 1e3]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/iff_identification_bode_plot.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:iff_identification_bode_plot #+caption: Bode plot of the identified IFF plant #+RESULTS: [[file:figs/iff_identification_bode_plot.png]] ** Model of the IFF Plant In order to plot the root locus for the IFF control strategy, a model of the identified plant is developed. It consists of several poles and zeros are shown below. #+begin_src matlab wz = 2*pi*102; xi_z = 0.01; wp = 2*pi*239.4; xi_p = 0.015; Giff = 2.2*(s^2 + 2*xi_z*s*wz + wz^2)/(s^2 + 2*xi_p*s*wp + wp^2) * ... % Dynamics 10*(s/3/pi/(1 + s/3/pi)) * ... % Low pass filter and gain of the voltage amplifier exp(-Ts*s); % Time delay induced by ADC/DAC #+end_src The comparison of the identified dynamics and the developed model is done in Figure [[fig:iff_plant_model]]. #+begin_src matlab :exports none figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,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 [V/V]'); set(gca, 'XTickLabel',[]); hold off; ylim([1e-2, 1e3]) ax2 = nexttile; 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; ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2], 'x'); xlim([0.5, 5e3]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/iff_plant_model.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:iff_plant_model #+caption: IFF Plant + Model #+RESULTS: [[file:figs/iff_plant_model.png]] ** Root Locus and optimal Controller Now, the root locus for the Integral Force Feedback strategy is computed and shown in Figure [[fig:iff_root_locus]]. Note that the controller used is not a pure integrator but rather a first order low pass filter with a cut-off frequency set at 2Hz. #+begin_src matlab :exports none 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 cl_poles = pole(feedback(Giff, 102/(s + 2*pi*2))); plot(real(cl_poles), imag(cl_poles), 'rx'); ylim([0, 1800]); xlim([-1600,200]); xlabel('Real Part') ylabel('Imaginary Part') axis square #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/iff_root_locus.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:iff_root_locus #+caption: Root Locus for the IFF control #+RESULTS: [[file:figs/iff_root_locus.png]] The controller that yield maximum damping (shown by the red cross in Figure [[fig:iff_root_locus]]) is: #+begin_src matlab Kiff_opt = 102/(s + 2*pi*2); #+end_src ** Verification of the achievable damping A new identification is performed with the IFF control strategy applied to the system. Data is loaded and offset removed. #+begin_src matlab id_cl = load('identification_noise_iff_bis.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't'); #+end_src #+begin_src matlab id_cl.d = detrend(id_cl.d, 0); id_cl.acc_1 = detrend(id_cl.acc_1, 0); id_cl.acc_2 = detrend(id_cl.acc_2, 0); id_cl.geo_1 = detrend(id_cl.geo_1, 0); id_cl.geo_2 = detrend(id_cl.geo_2, 0); id_cl.f_meas = detrend(id_cl.f_meas, 0); id_cl.u = detrend(id_cl.u, 0); #+end_src The open-loop and closed-loop dynamics are estimated. #+begin_src matlab [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); #+end_src The obtained coherence is shown in Figure [[fig:Gd_identification_iff_coherence]] and the dynamics in Figure [[fig:Gd_identification_iff_bode_plot]]. #+begin_src matlab :exports none figure; hold on; plot(f, co_G_ol_est, '-', 'DisplayName', 'OL') plot(f, co_G_cl_est, '-', 'DisplayName', 'IFF') set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin'); ylabel('Coherence'); xlabel('Frequency [Hz]'); hold off; xlim([1, 1e3]); ylim([0, 1]) legend('location', 'southwest'); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/Gd_identification_iff_coherence.pdf', 'width', 'wide', 'height', 'normal'); #+end_src #+name: fig:Gd_identification_iff_coherence #+caption: Coherence for the transfer function from F to d, with and without IFF #+RESULTS: [[file:figs/Gd_identification_iff_coherence.png]] #+begin_src matlab :exports none figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; plot(f, abs(tf_G_ol_est), '-', 'DisplayName', 'OL') plot(f, abs(tf_G_cl_est), '-', 'DisplayName', 'IFF') set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log'); ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]); hold off; legend('location', 'northeast'); ylim([2e-7, 2e-4]); ax2 = nexttile; 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; ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2], 'x'); xlim([1, 1e3]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/Gd_identification_iff_bode_plot.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:Gd_identification_iff_bode_plot #+caption: Coherence for the transfer function from F to d, with and without IFF #+RESULTS: [[file:figs/Gd_identification_iff_bode_plot.png]] * Generate the excitation signal :PROPERTIES: :header-args:matlab+: :tangle matlab/optimal_excitation.m :END: <> ** Introduction :ignore: In order to properly estimate the dynamics of the inertial sensor, the excitation signal must be properly chosen. The requirements on the excitation signal is: - General much larger motion that the measured motion during the huddle test - Don't damage the actuator To determine the perfect voltage signal to be generated, we need two things: - the transfer function from voltage to mass displacement - the PSD of the measured motion by the inertial sensors - not saturate the sensor signals - provide enough signal/noise ratio (good coherence) in the frequency band of interest (~0.5Hz to 3kHz) ** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) <> #+end_src #+begin_src matlab :exports none :results silent :noweb yes <> #+end_src #+begin_src matlab :tangle no addpath('./matlab/mat/'); #+end_src #+begin_src matlab :eval no addpath('./mat/'); #+end_src ** Transfer function from excitation signal to displacement Let's first estimate the transfer function from the excitation signal in [V] to the generated displacement in [m] as measured by the inteferometer. #+begin_src matlab id_cl = load('identification_noise_iff_bis.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't'); #+end_src #+begin_src matlab :exports none id_cl.d = detrend(id_cl.d, 0); id_cl.acc_1 = detrend(id_cl.acc_1, 0); id_cl.acc_2 = detrend(id_cl.acc_2, 0); id_cl.geo_1 = detrend(id_cl.geo_1, 0); id_cl.geo_2 = detrend(id_cl.geo_2, 0); id_cl.f_meas = detrend(id_cl.f_meas, 0); id_cl.u = detrend(id_cl.u, 0); #+end_src #+begin_src matlab Ts = id_cl.t(2) - id_cl.t(1); win = hann(ceil(10/Ts)); #+end_src #+begin_src matlab [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); #+end_src Approximate transfer function from voltage output to generated displacement when IFF is used, in [m/V]. #+begin_src matlab G_d_est = -5e-6*(2*pi*230)^2/(s^2 + 2*0.3*2*pi*240*s + (2*pi*240)^2); #+end_src #+begin_src matlab :exports none figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,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]'); set(gca, 'XTickLabel',[]); hold off; ax2 = nexttile; 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; ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2], 'x'); xlim([10, 1000]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/Gd_plant_estimation.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:Gd_plant_estimation #+caption: Estimation of the transfer function from the excitation signal to the generated displacement #+RESULTS: [[file:figs/Gd_plant_estimation.png]] ** Motion measured during Huddle test We now compute the PSD of the measured motion by the inertial sensors during the huddle test. #+begin_src matlab ht = load('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); #+end_src #+begin_src matlab [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); #+end_src Using an estimated model of the sensor dynamics from the documentation of the sensors, we can compute the ASD of the motion in $m/\sqrt{Hz}$ measured by the sensors. #+begin_src matlab 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)] #+end_src #+begin_src matlab :exports none 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(); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/huddle_test_psd_motion.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:huddle_test_psd_motion #+caption: ASD of the motion measured by the sensors #+RESULTS: [[file:figs/huddle_test_psd_motion.png]] From the ASD of the motion measured by the sensors, we can create an excitation signal that will generate much motion motion that the motion under no excitation. We create =G_exc= that corresponds to the wanted generated motion. #+begin_src matlab G_exc = 0.2e-6/(1 + s/2/pi/2)/(1 + s/2/pi/50); #+end_src And we create a time domain signal =y_d= that have the spectral density described by =G_exc=. #+begin_src matlab Fs = 1/Ts; t = 0:Ts:180; % Time Vector [s] u = sqrt(Fs/2)*randn(length(t), 1); % Signal with an ASD equal to one y_d = lsim(G_exc, u, t); #+end_src #+begin_src matlab [pxx, ~] = pwelch(y_d, win, 0, [], Fs); #+end_src #+begin_src matlab :exports none 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(pxx), 'k-', ... 'DisplayName', 'Excitation'); 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(); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/comp_huddle_test_excited_motion_psd.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:comp_huddle_test_excited_motion_psd #+caption: Comparison of the ASD of the motion during Huddle and the wanted generated motion #+RESULTS: [[file:figs/comp_huddle_test_excited_motion_psd.png]] We can now generate the voltage signal that will generate the wanted motion. #+begin_src matlab y_v = lsim(G_exc * ... % from unit PSD to shaped PSD (1 + s/2/pi/50) * ... % Inverse of pre-filter included in the Simulink file 1/G_d_est * ... % Wanted displacement => required voltage 1/(1 + s/2/pi/5e3), ... % Add some high frequency filtering u, t); #+end_src #+begin_src matlab :exports none figure; plot(t, y_v) xlabel('Time [s]'); ylabel('Voltage [V]'); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/optimal_exc_signal_time.pdf', 'width', 'wide', 'height', 'normal'); #+end_src #+name: fig:optimal_exc_signal_time #+caption: Generated excitation signal #+RESULTS: [[file:figs/optimal_exc_signal_time.png]] * Identification of the Inertial Sensors Dynamics :PROPERTIES: :header-args:matlab+: :tangle matlab/inertial_sensor_dynamics.m :END: <> ** Introduction :ignore: Using the excitation signal generated in Section [[sec:optimal_excitation]], the dynamics of the inertial sensors are identified. ** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) <> #+end_src #+begin_src matlab :exports none :results silent :noweb yes <> #+end_src #+begin_src matlab :tangle no addpath('./matlab/mat/'); #+end_src #+begin_src matlab :eval no addpath('./mat/'); #+end_src ** Load Data Both the measurement data during the identification test and during an "huddle test" are loaded. #+begin_src matlab id = load('identification_noise_opt_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't'); ht = load('huddle_test.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't'); #+end_src #+begin_src matlab 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); #+end_src #+begin_src matlab 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); #+end_src ** Compare PSD during Huddle and and during identification The Power Spectral Density of the measured motion during the huddle test and during the identification test are compared in Figures [[fig:comp_psd_huddle_test_identification_acc]] and [[fig:comp_psd_huddle_test_identification_geo]]. #+begin_src matlab Ts = ht.t(2) - ht.t(1); win = hann(ceil(10/Ts)); #+end_src #+begin_src matlab [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); #+end_src #+begin_src matlab [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); #+end_src #+begin_src matlab :exports none 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('location', 'northwest'); xlim([5e-1, 5e3]); ylim([1e-10, 1e-2]) #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/comp_psd_huddle_test_identification_acc.pdf', 'width', 'wide', 'height', 'normal'); #+end_src #+name: fig:comp_psd_huddle_test_identification_acc #+caption: Comparison of the PSD of the measured motion during the Huddle test and during the identification #+RESULTS: [[file:figs/comp_psd_huddle_test_identification_acc.png]] #+begin_src matlab :exports none 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('location', 'northeast'); xlim([1e-1, 5e3]); ylim([1e-11, 1e-4]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/comp_psd_huddle_test_identification_geo.pdf', 'width', 'wide', 'height', 'normal'); #+end_src #+name: fig:comp_psd_huddle_test_identification_geo #+caption: Comparison of the PSD of the measured motion during the Huddle test and during the identification #+RESULTS: [[file:figs/comp_psd_huddle_test_identification_geo.png]] ** Compute transfer functions The transfer functions from the motion as measured by the interferometer (and that should represent the absolute motion of the mass) to the inertial sensors are estimated: #+begin_src matlab [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); #+end_src The obtained coherence are shown in Figure [[fig:id_sensor_dynamics_coherence]]. #+begin_src matlab :exports none 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(); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/id_sensor_dynamics_coherence.pdf', 'width', 'wide', 'height', 'normal'); #+end_src #+name: fig:id_sensor_dynamics_coherence #+caption: Coherence for the estimation of the sensor dynamics #+RESULTS: [[file:figs/id_sensor_dynamics_coherence.png]] We also make a simplified model of the inertial sensors to be compared with the identified dynamics. #+begin_src matlab 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)] #+end_src The model and identified dynamics show good agreement (Figures [[fig:id_sensor_dynamics_accelerometers]] and [[fig:id_sensor_dynamics_geophones]].) #+begin_src matlab :exports none figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,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 $\left[\frac{V}{m/s^2}\right]$'); set(gca, 'XTickLabel',[]); hold off; ax2 = nexttile; 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; ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2], 'x'); xlim([2, 2e3]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/id_sensor_dynamics_accelerometers.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:id_sensor_dynamics_accelerometers #+caption: Identified dynamics of the accelerometers #+RESULTS: [[file:figs/id_sensor_dynamics_accelerometers.png]] #+begin_src matlab :exports none figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,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 $\left[\frac{V}{m/s}\right]$'); set(gca, 'XTickLabel',[]); hold off; ax2 = nexttile; 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; ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2], 'x'); xlim([0.5, 2e3]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/id_sensor_dynamics_geophones.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:id_sensor_dynamics_geophones #+caption: Identified dynamics of the geophones #+RESULTS: [[file:figs/id_sensor_dynamics_geophones.png]] * Inertial Sensor Noise and the $\mathcal{H}_2$ Synthesis of complementary filters :PROPERTIES: :header-args:matlab+: :tangle matlab/inertial_sensor_noise.m :END: <> ** Introduction :ignore: In this section, the noise of the inertial sensors (geophones and accelerometers) is estimated. ** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) <> #+end_src #+begin_src matlab :exports none :results silent :noweb yes <> #+end_src #+begin_src matlab :tangle no addpath('./matlab/mat/'); #+end_src #+begin_src matlab :eval no addpath('./mat/'); #+end_src ** Load Data As before, the identification data is loaded and any offset if removed. #+begin_src matlab id = load('identification_noise_opt_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't'); #+end_src #+begin_src matlab 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); #+end_src ** ASD of the Measured displacement The Power Spectral Density of the displacement as measured by the interferometer and the inertial sensors is computed. #+begin_src matlab Ts = id.t(2) - id.t(1); win = hann(ceil(10/Ts)); #+end_src #+begin_src matlab [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); #+end_src Let's use a model of the accelerometer and geophone to compute the motion from the measured voltage. #+begin_src matlab 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)] #+end_src The obtained ASD in $m/\sqrt{Hz}$ is shown in Figure [[fig:measure_displacement_all_sensors]]. #+begin_src matlab :exports none 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]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/measure_displacement_all_sensors.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:measure_displacement_all_sensors #+caption: ASD of the measured displacement as measured by all the sensors #+RESULTS: [[file:figs/measure_displacement_all_sensors.png]] ** 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. #+begin_src matlab [coh_acc, ~] = mscohere(id.acc_1, id.acc_2, win, [], [], 1/Ts); [coh_geo, ~] = mscohere(id.geo_1, id.geo_2, win, [], [], 1/Ts); #+end_src Finally, the Power Spectral Density of the sensors is computed and converted in $[m^2/Hz]$. #+begin_src matlab 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] #+end_src The ASD of obtained noises are compared with the ASD of the measured signals in Figure [[fig:noise_inertial_sensors_comparison]]. #+begin_src matlab :exports none 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'); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/noise_inertial_sensors_comparison.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:noise_inertial_sensors_comparison #+caption: Comparison of the computed ASD of the noise of the two inertial sensors #+RESULTS: [[file:figs/noise_inertial_sensors_comparison.png]] ** 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]]. #+begin_src matlab 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)] #+end_src #+begin_src matlab :exports none :tangle no save('./matlab/mat/sensor_noises.mat', 'pN_acc', 'pN_geo', 'N_acc', 'N_geo', 'f') #+end_src #+begin_src matlab :exports none 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'); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/noise_models_velocity.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:noise_models_velocity #+caption: ASD of the velocity noise measured by the sensors and the noise models #+RESULTS: [[file:figs/noise_models_velocity.png]] ** $\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. #+begin_src matlab P = [0 N_acc 1; N_geo -N_acc 0]; #+end_src And the $\mathcal{H}_2$ synthesis is done using the =h2syn= command. #+begin_src matlab [H_geo, ~, gamma] = h2syn(P, 1, 1); H_acc = 1 - H_geo; #+end_src The obtained complementary filters are shown in Figure [[fig:complementary_filters_velocity_H2]]. #+begin_src matlab :exports none 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)]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/complementary_filters_velocity_H2.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:complementary_filters_velocity_H2 #+caption: Obtained Complementary Filters #+RESULTS: [[file:figs/complementary_filters_velocity_H2.png]] ** 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]]. #+begin_src matlab :exports none 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'); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/super_sensor_noise_asd_velocity.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+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]]. #+begin_src matlab [~, i_1Hz] = min(abs(f - 1)); #+end_src #+begin_src matlab 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))); #+end_src #+begin_src matlab :exports none 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'); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/super_sensor_noise_cas_velocity.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:super_sensor_noise_cas_velocity #+caption: Cumulative Power Spectrum of the Sensor Noise (velocity) #+RESULTS: [[file:figs/super_sensor_noise_cas_velocity.png]] * Inertial Sensor Dynamics Uncertainty and the $\mathcal{H}_\infty$ Synthesis of complementary filters :PROPERTIES: :header-args:matlab+: :tangle matlab/inertial_sensor_uncertainty.m :END: <> ** Introduction :ignore: When merging two sensors, it is important to be sure that we correctly know the sensor dynamics near the merging frequency. Thus, identifying the uncertainty on the sensor dynamics is quite important to perform a robust merging. ** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) <> #+end_src #+begin_src matlab :exports none :results silent :noweb yes <> #+end_src #+begin_src matlab :tangle no addpath('./matlab/mat/'); addpath('./matlab/src/'); #+end_src #+begin_src matlab :eval no addpath('./mat/'); addpath('./src/'); #+end_src ** Load Data Data is loaded and offset is removed. #+begin_src matlab id = load('identification_noise_opt_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't'); #+end_src #+begin_src matlab 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); #+end_src ** Compute the dynamics of both sensors The dynamics of inertial sensors are estimated (in $[V/m]$). #+begin_src matlab Ts = id.t(2) - id.t(1); win = hann(ceil(10/Ts)); #+end_src #+begin_src matlab [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); #+end_src The (nominal) models of the inertial sensors from the absolute displacement to the generated voltage are defined below: #+begin_src matlab G_acc = 1/(1 + s/2/pi/2000) G_geo = -1200*s^2/(s^2 + 2*0.7*2*pi*2*s + (2*pi*2)^2); #+end_src These models are very simplistic models, and we then take into account the un-modelled dynamics with dynamical uncertainty. ** Dynamics uncertainty estimation Weights representing the dynamical uncertainty of the sensors are defined below. #+begin_src matlab w_acc = createWeight('n', 2, 'G0', 10, 'G1', 0.2, 'Gc', 1, 'w0', 6*2*pi) * ... createWeight('n', 2, 'G0', 1, 'G1', 5/0.2, 'Gc', 1/0.2, 'w0', 1300*2*pi); w_geo = createWeight('n', 2, 'G0', 0.6, 'G1', 0.2, 'Gc', 0.3, 'w0', 3*2*pi) * ... createWeight('n', 2, 'G0', 1, 'G1', 10/0.2, 'Gc', 1/0.2, 'w0', 800*2*pi); #+end_src #+begin_src matlab :exports none :tangle no save('./matlab/mat/sensor_dynamics.mat', 'tf_acc1_est', 'tf_acc2_est', 'tf_geo1_est', 'tf_geo2_est', 'f', 'G_acc', 'G_geo', 'w_acc', 'w_geo'); #+end_src The measured dynamics are compared with the modelled one as well as the modelled uncertainty in Figure [[fig:dyn_uncertainty_acc]] for the accelerometers and in Figure [[fig:dyn_uncertainty_geo]] for the geophones. #+begin_src matlab :exports none figure; tiledlayout(2, 1, 'TileSpacing', 'None', 'Padding', 'None'); % Magnitude ax1 = nexttile; hold on; plotMagUncertainty(w_acc, freqs, 'G', G_acc, 'color_i', 1, 'DisplayName', '$G_{acc}$'); set(gca,'ColorOrderIndex',1) plot(f, abs(tf_acc1_est./(1i*2*pi*f).^2), '.', 'DisplayName', 'Meaurement') set(gca,'ColorOrderIndex',1) plot(f, abs(tf_acc2_est./(1i*2*pi*f).^2), '.', 'HandleVisibility', 'off') set(gca,'ColorOrderIndex',1) plot(freqs, abs(squeeze(freqresp(G_acc, freqs, 'Hz'))), 'DisplayName', '$\hat{G}_{acc}$'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XTickLabel',[]); ylabel('Magnitude $[\frac{V}{m}]$'); legend('location', 'southwest', 'FontSize', 8); hold off; ylim([1e-3, 1e1]) % Phase ax2 = nexttile; hold on; plotPhaseUncertainty(w_acc, freqs, 'G', G_acc, 'color_i', 1); set(gca,'ColorOrderIndex',1) plot(f, 180/pi*angle(tf_acc1_est./(1i*2*pi*f).^2), '.'); set(gca,'ColorOrderIndex',1) plot(f, 180/pi*angle(tf_acc2_est./(1i*2*pi*f).^2), '.'); set(gca,'ColorOrderIndex',1) plot(freqs, 180/pi*angle(squeeze(freqresp(G_acc, 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([1, 5e3]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/dyn_uncertainty_acc.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:dyn_uncertainty_acc #+caption: Modeled dynamical uncertainty and meaured dynamics of the accelerometers #+RESULTS: [[file:figs/dyn_uncertainty_acc.png]] #+begin_src matlab :exports none figure; tiledlayout(2, 1, 'TileSpacing', 'None', 'Padding', 'None'); % Magnitude ax1 = nexttile; hold on; plotMagUncertainty(w_geo, freqs, 'G', G_geo, 'color_i', 2, 'DisplayName', '$G_{geo}$'); set(gca,'ColorOrderIndex',2) plot(f, abs(tf_geo1_est./(1i*2*pi*f)), '.', 'DisplayName', 'Measurement') set(gca,'ColorOrderIndex',2) plot(f, abs(tf_geo2_est./(1i*2*pi*f)), '.', 'HandleVisibility', 'off') set(gca,'ColorOrderIndex',2) plot(freqs, abs(squeeze(freqresp(G_geo, freqs, 'Hz'))), 'DisplayName', '$\hat{G}_{geo}$'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XTickLabel',[]); ylabel('Magnitude $[\frac{V}{m}]$'); legend('location', 'northwest', 'FontSize', 8); hold off; ylim([1, 1e4]) % Phase ax2 = nexttile; hold on; plotPhaseUncertainty(w_geo, freqs, 'G', G_geo, 'color_i', 2); set(gca,'ColorOrderIndex',2) plot(f, 180/pi*unwrap(angle(tf_geo1_est./(1i*2*pi*f)))+360, '.'); set(gca,'ColorOrderIndex',2) plot(f, 180/pi*unwrap(angle(tf_geo2_est./(1i*2*pi*f))), '.'); set(gca,'ColorOrderIndex',2) plot(freqs, 180/pi*angle(squeeze(freqresp(G_geo, freqs, 'Hz')))); set(gca,'xscale','log'); yticks(-270:90:180); ylim([-270 90]); xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); hold off; linkaxes([ax1,ax2],'x'); xlim([1, 5e3]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/dyn_uncertainty_geo.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:dyn_uncertainty_geo #+caption: Modeled dynamical uncertainty and meaured dynamics of the geophones #+RESULTS: [[file:figs/dyn_uncertainty_geo.png]] ** $\mathcal{H}_\infty$ Synthesis of Complementary Filters A last weight is now defined that represents the maximum dynamical uncertainty that is allowed for the super sensor. #+begin_src matlab wu = inv(createWeight('n', 2, 'G0', 0.7, 'G1', 0.3, 'Gc', 0.4, 'w0', 3*2*pi) * ... createWeight('n', 2, 'G0', 1, 'G1', 6/0.3, 'Gc', 1/0.3, 'w0', 1200*2*pi)); #+end_src This dynamical uncertainty is compared with the two sensor uncertainties in Figure [[fig:uncertainty_weight_and_sensor_uncertainties]]. #+begin_src matlab :exports none Dphi_Wu = 180/pi*asin(abs(squeeze(freqresp(inv(wu), freqs, 'Hz')))); Dphi_Wu(abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))) > 1) = 360; figure; tiledlayout(2, 1, 'TileSpacing', 'None', 'Padding', 'None'); % Magnitude ax1 = nexttile; hold on; plotMagUncertainty(w_acc, freqs, 'color_i', 1, 'DisplayName', '$1 + W_{acc} \Delta$'); plotMagUncertainty(w_geo, freqs, 'color_i', 2, 'DisplayName', '$1 + W_{geo} \Delta$'); plot(freqs, 1 + abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))), 'k--', ... 'DisplayName', '$1 + W_u^{-1} \Delta$') plot(freqs, 1 - abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))), 'k--', ... 'HandleVisibility', 'off') set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XTickLabel',[]); ylabel('Magnitude'); ylim([1e-2, 1e1]); legend('location', 'southeast', 'FontSize', 8); hold off; % Phase ax2 = nexttile; hold on; plotPhaseUncertainty(w_acc, freqs, 'color_i', 1); plotPhaseUncertainty(w_geo, freqs, 'color_i', 2); plot(freqs, Dphi_Wu, 'k--'); plot(freqs, -Dphi_Wu, 'k--'); 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)]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/uncertainty_weight_and_sensor_uncertainties.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:uncertainty_weight_and_sensor_uncertainties #+caption: Individual sensor uncertainty (normalized by their dynamics) and the wanted maximum super sensor noise uncertainty #+RESULTS: [[file:figs/uncertainty_weight_and_sensor_uncertainties.png]] The generalized plant used for the synthesis is defined: #+begin_src matlab P = [wu*w_acc -wu*w_acc; 0 wu*w_geo; 1 0]; #+end_src And the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command is performed. #+begin_src matlab :results output replace :exports both [H_geo, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on'); #+end_src #+RESULTS: #+begin_example Test bounds: 0.8556 <= gamma <= 1.34 gamma X>=0 Y>=0 rho(XY)<1 p/f 1.071e+00 0.0e+00 0.0e+00 0.000e+00 p 9.571e-01 0.0e+00 0.0e+00 9.436e-16 p 9.049e-01 0.0e+00 0.0e+00 1.084e-15 p 8.799e-01 0.0e+00 0.0e+00 1.191e-16 p 8.677e-01 0.0e+00 0.0e+00 6.905e-15 p 8.616e-01 0.0e+00 0.0e+00 0.000e+00 p 8.586e-01 1.1e-17 0.0e+00 6.917e-16 p 8.571e-01 0.0e+00 0.0e+00 6.991e-17 p 8.564e-01 0.0e+00 0.0e+00 1.492e-16 p Best performance (actual): 0.8563 #+end_example The complementary filter is defined as follows: #+begin_src matlab H_acc = 1 - H_geo; #+end_src The bode plot of the obtained complementary filters is shown in Figure #+begin_src matlab :exports none figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); % Magnitude ax1 = nexttile([2,1]); hold on; set(gca,'ColorOrderIndex',1) plot(freqs, 1./abs(squeeze(freqresp(w_geo, freqs, 'Hz'))), '--', 'DisplayName', '$w_{geo}$'); set(gca,'ColorOrderIndex',2) plot(freqs, 1./abs(squeeze(freqresp(w_acc, freqs, 'Hz'))), '--', 'DisplayName', '$w_{acc}$'); set(gca,'ColorOrderIndex',1) plot(freqs, abs(squeeze(freqresp(H_geo, freqs, 'Hz'))), '-', 'DisplayName', '$H_{geo}$'); set(gca,'ColorOrderIndex',2) plot(freqs, abs(squeeze(freqresp(H_acc, freqs, 'Hz'))), '-', 'DisplayName', '$H_{acc}$'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Magnitude'); set(gca, 'XTickLabel',[]); ylim([1e-2, 1e1]); legend('location', 'southeast'); ax2 = nexttile; hold on; set(gca,'ColorOrderIndex',1) plot(freqs, 180/pi*phase(squeeze(freqresp(H_geo, freqs, 'Hz'))), '-'); set(gca,'ColorOrderIndex',2) plot(freqs, 180/pi*phase(squeeze(freqresp(H_acc, freqs, 'Hz'))), '-'); hold off; xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); set(gca, 'XScale', 'log'); yticks([-360:90:360]); linkaxes([ax1,ax2],'x'); xlim([1, 1e3]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/h_infinity_obtained_complementary_filters.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:h_infinity_obtained_complementary_filters #+caption: Bode plot of the obtained complementary filters using the $\mathcal{H}_\infty$ synthesis #+RESULTS: [[file:figs/h_infinity_obtained_complementary_filters.png]] ** Obtained Super Sensor Dynamical Uncertainty The obtained super sensor dynamical uncertainty is shown in Figure [[fig:super_sensor_uncertainty_h_infinity]]. #+begin_src matlab :exports none Dphi_Wu = 180/pi*asin(abs(squeeze(freqresp(inv(wu), freqs, 'Hz')))); Dphi_Wu(abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))) > 1) = 360; Dphi_ss = 180/pi*asin(abs(squeeze(freqresp(w_geo*H_geo, freqs, 'Hz'))) + abs(squeeze(freqresp(w_acc*H_acc, freqs, 'Hz')))); Dphi_ss(abs(squeeze(freqresp(w_geo*H_geo, freqs, 'Hz'))) + abs(squeeze(freqresp(w_acc*H_acc, freqs, 'Hz'))) > 1) = 360; figure; tiledlayout(2, 1, 'TileSpacing', 'None', 'Padding', 'None'); % Magnitude ax1 = nexttile; hold on; plotMagUncertainty(w_acc, freqs, 'color_i', 1, 'DisplayName', '$1 + W_1 \Delta_1$'); plotMagUncertainty(w_geo, freqs, 'color_i', 2, 'DisplayName', '$1 + W_2 \Delta_2$'); plot(freqs, 1 + abs(squeeze(freqresp(w_geo*H_geo, freqs, 'Hz')))+abs(squeeze(freqresp(w_acc*H_acc, freqs, 'Hz'))), 'k-', ... 'DisplayName', '$1 + W_1 \Delta_1 + W_2 \Delta_2$') plot(freqs, max(1 - abs(squeeze(freqresp(w_geo*H_geo, freqs, 'Hz')))-abs(squeeze(freqresp(w_acc*H_acc, freqs, 'Hz'))), 0.001), 'k-', ... 'HandleVisibility', 'off'); plot(freqs, 1 + abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))), 'k--', ... 'DisplayName', '$1 + W_u^{-1}\Delta$') plot(freqs, 1 - abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))), 'k--', ... 'HandleVisibility', 'off') set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XTickLabel',[]); ylabel('Magnitude'); ylim([1e-2, 1e1]); legend('location', 'southeast', 'FontSize', 8); hold off; % Phase ax2 = nexttile; hold on; plotPhaseUncertainty(w_acc, freqs, 'color_i', 1); plotPhaseUncertainty(w_geo, freqs, 'color_i', 2); plot(freqs, Dphi_ss, 'k-'); plot(freqs, -Dphi_ss, 'k-'); plot(freqs, Dphi_Wu, 'k--'); plot(freqs, -Dphi_Wu, 'k--'); 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)]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/super_sensor_uncertainty_h_infinity.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:super_sensor_uncertainty_h_infinity #+caption: Obtained Super sensor dynamics uncertainty #+RESULTS: [[file:figs/super_sensor_uncertainty_h_infinity.png]] * Optimal and Robust sensor fusion using the $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis :PROPERTIES: :header-args:matlab+: :tangle matlab/optimal_sensor_fusion.m :END: <> ** Introduction :ignore: ** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) <> #+end_src #+begin_src matlab :exports none :results silent :noweb yes <> #+end_src #+begin_src matlab :tangle no addpath('./matlab/mat/'); addpath('./matlab/src/'); #+end_src #+begin_src matlab :eval no addpath('./mat/'); addpath('./src/'); #+end_src ** Noise and Dynamical uncertainty weights #+begin_src matlab N_acc = (s/(2*pi*2000) + 1)^2/(s + 0.1*2*pi)/(s + 1e3*2*pi)/(1 + s/2/pi/1e3); % [m/sqrt(Hz)] N_geo = 4e-4*((s + 2*pi)/(2*pi*200) + 1)/(s + 1e3*2*pi)/(1 + s/2/pi/1e3); % [m/sqrt(Hz)] #+end_src #+begin_src matlab w_acc = createWeight('n', 2, 'G0', 10, 'G1', 0.2, 'Gc', 1, 'w0', 6*2*pi) * ... createWeight('n', 2, 'G0', 1, 'G1', 5/0.2, 'Gc', 1/0.2, 'w0', 1300*2*pi); w_geo = createWeight('n', 2, 'G0', 0.6, 'G1', 0.2, 'Gc', 0.3, 'w0', 3*2*pi) * ... createWeight('n', 2, 'G0', 1, 'G1', 10/0.2, 'Gc', 1/0.2, 'w0', 800*2*pi); #+end_src #+begin_src matlab wu = inv(createWeight('n', 2, 'G0', 0.7, 'G1', 0.3, 'Gc', 0.4, 'w0', 3*2*pi) * ... createWeight('n', 2, 'G0', 1, 'G1', 6/0.3, 'Gc', 1/0.3, 'w0', 1200*2*pi)); #+end_src #+begin_src matlab P = [wu*w_acc -wu*w_acc; 0 wu*w_geo; N_acc -N_acc; 0 N_geo; 1 0]; #+end_src And the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis is performed. #+begin_src matlab [H_geo, ~] = h2hinfsyn(ss(P), 1, 1, 2, [0, 1], 'HINFMAX', 1, 'H2MAX', Inf, 'DKMAX', 100, 'TOL', 1e-3, 'DISPLAY', 'on'); #+end_src #+begin_src matlab H_acc = 1 - H_geo; #+end_src ** Obtained Super Sensor Noise #+begin_src matlab freqs = logspace(0, 4, 1000); PSD_Sgeo = abs(squeeze(freqresp(N_geo, freqs, 'Hz'))).^2; PSD_Sacc = abs(squeeze(freqresp(N_acc, freqs, 'Hz'))).^2; PSD_Hss = abs(squeeze(freqresp(N_acc*H_acc, freqs, 'Hz'))).^2 + ... abs(squeeze(freqresp(N_geo*H_geo, freqs, 'Hz'))).^2; #+end_src #+begin_src matlab :exports none figure; hold on; plot(freqs, sqrt(PSD_Sacc), '-', 'DisplayName', '$\Phi_{n_{acc}}$'); plot(freqs, sqrt(PSD_Sgeo), '-', 'DisplayName', '$\Phi_{n_{geo}}$'); plot(freqs, sqrt(PSD_Hss), 'k-.', 'DisplayName', '$\Phi_{n_{\mathcal{H}_2/\mathcal{H}_\infty}}$'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); xlabel('Frequency [Hz]'); ylabel('ASD $\left[ \frac{m/s}{\sqrt{Hz}} \right]$'); hold off; xlim([freqs(1), freqs(end)]); legend('location', 'northeast', 'FontSize', 8); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/psd_sensors_htwo_hinf_synthesis.pdf', 'width', 'half', 'height', 'normal'); #+end_src #+name: fig:psd_sensors_htwo_hinf_synthesis #+caption: Power Spectral Density of the Super Sensor obtained with the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis #+RESULTS: [[file:figs/psd_sensors_htwo_hinf_synthesis.png]] ** Obtained Super Sensor Dynamical Uncertainty #+begin_src matlab :exports none Dphi_wu = 180/pi*asin(abs(squeeze(freqresp(inv(wu), freqs, 'Hz')))); Dphi_wu(abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))) > 1) = 360; Dphi_ss = 180/pi*asin(abs(squeeze(freqresp(w_geo*H_geo, freqs, 'Hz'))) + abs(squeeze(freqresp(w_acc*H_acc, freqs, 'Hz')))); Dphi_ss(abs(squeeze(freqresp(w_geo*H_geo, freqs, 'Hz'))) + abs(squeeze(freqresp(w_acc*H_acc, freqs, 'Hz'))) > 1) = 360; figure; tiledlayout(2, 1, 'TileSpacing', 'None', 'Padding', 'None'); % Magnitude ax1 = nexttile; hold on; plotMagUncertainty(w_acc, freqs, 'color_i', 1, 'DisplayName', '$1 + W_1 \Delta_1$'); plotMagUncertainty(w_geo, freqs, 'color_i', 2, 'DisplayName', '$1 + W_2 \Delta_2$'); plot(freqs, 1 + abs(squeeze(freqresp(w_geo*H_geo, freqs, 'Hz')))+abs(squeeze(freqresp(w_acc*H_acc, freqs, 'Hz'))), 'k-', ... 'DisplayName', '$1 + W_1 \Delta_1 + W_2 \Delta_2$') plot(freqs, max(1 - abs(squeeze(freqresp(w_geo*H_geo, freqs, 'Hz')))-abs(squeeze(freqresp(w_acc*H_acc, freqs, 'Hz'))), 0.001), 'k-', ... 'HandleVisibility', 'off'); plot(freqs, 1 + abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))), 'k--', ... 'DisplayName', '$1 + W_u^{-1}\Delta$') plot(freqs, 1 - abs(squeeze(freqresp(inv(wu), freqs, 'Hz'))), 'k--', ... 'HandleVisibility', 'off') set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XTickLabel',[]); ylabel('Magnitude'); ylim([1e-2, 1e1]); legend('location', 'southeast', 'FontSize', 8); hold off; % Phase ax2 = nexttile; hold on; plotPhaseUncertainty(w_acc, freqs, 'color_i', 1); plotPhaseUncertainty(w_geo, freqs, 'color_i', 2); plot(freqs, Dphi_ss, 'k-'); plot(freqs, -Dphi_ss, 'k-'); plot(freqs, Dphi_wu, 'k--'); plot(freqs, -Dphi_wu, 'k--'); set(gca,'xscale','log'); ylim([-180 180]); yticks(-180:90:180); xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); hold off; linkaxes([ax1,ax2],'x'); xlim([freqs(1), freqs(end)]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/super_sensor_dynamical_uncertainty_Htwo_Hinf.pdf', 'width', 'half', 'height', 'tall'); #+end_src #+name: fig:super_sensor_dynamical_uncertainty_Htwo_Hinf #+caption: Super sensor dynamical uncertainty (solid curve) when using the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ Synthesis #+RESULTS: [[file:figs/super_sensor_dynamical_uncertainty_Htwo_Hinf.png]] ** Experimental Super Sensor Dynamical Uncertainty #+begin_src matlab :exports none load('./matlab/mat/sensor_dynamics.mat', 'tf_acc1_est', 'tf_acc2_est', 'tf_geo1_est', 'tf_geo2_est', 'f'); G_acc = s^2/(1 + s/2/pi/2000) % [V/m] G_geo = -1200*s^3/(s^2 + 2*0.7*2*pi*2*s + (2*pi*2)^2); % [V/m] #+end_src The super sensor dynamics is shown in Figure [[fig:super_sensor_optimal_uncertainty]]. #+begin_src matlab :exports none figure; tiledlayout(2, 1, 'TileSpacing', 'None', 'Padding', 'None'); % Magnitude ax1 = nexttile; hold on; plotMagUncertainty(w_acc, freqs, 'color_i', 1, 'DisplayName', '$G_{acc}$'); set(gca,'ColorOrderIndex',1) plot(f, abs(tf_acc1_est./squeeze(freqresp(G_acc, f, 'Hz'))), '.', 'DisplayName', 'Meaurement') set(gca,'ColorOrderIndex',1) plot(f, abs(tf_acc2_est./squeeze(freqresp(G_acc, f, 'Hz'))), '.', 'HandleVisibility', 'off') plotMagUncertainty(w_geo, freqs, 'color_i', 2, 'DisplayName', '$G_{geo}$'); set(gca,'ColorOrderIndex',2) plot(f, abs(tf_geo1_est./squeeze(freqresp(G_geo, f, 'Hz'))), '.', 'DisplayName', 'Meaurement') set(gca,'ColorOrderIndex',2) plot(f, abs(tf_geo2_est./squeeze(freqresp(G_geo, f, 'Hz'))), '.', 'HandleVisibility', 'off') plot(f, abs(tf_acc1_est.*squeeze(freqresp(inv(G_acc)*H_acc, f, 'Hz')) + ... tf_geo1_est.*squeeze(freqresp(inv(G_geo)*H_geo, f, 'Hz'))), 'k.', 'DisplayName', 'ss') plot(f, abs(tf_acc2_est.*squeeze(freqresp(inv(G_acc)*H_acc, f, 'Hz')) + ... tf_geo2_est.*squeeze(freqresp(inv(G_geo)*H_geo, f, 'Hz'))), 'k.', 'HandleVisibility', 'off') set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XTickLabel',[]); ylabel('Magnitude $[\frac{V}{m}]$'); legend('location', 'southwest', 'FontSize', 8); hold off; ylim([1e-3, 1e1]) % Phase ax2 = nexttile; hold on; plotPhaseUncertainty(w_acc, freqs, 'color_i', 1); set(gca,'ColorOrderIndex',1) plot(f, 180/pi*angle(tf_acc1_est./squeeze(freqresp(G_acc, f, 'Hz'))), '.'); set(gca,'ColorOrderIndex',1) plot(f, 180/pi*angle(tf_acc2_est./squeeze(freqresp(G_acc, f, 'Hz'))), '.'); plotPhaseUncertainty(w_geo, freqs, 'color_i', 2); set(gca,'ColorOrderIndex',2) plot(f, 180/pi*angle(tf_geo1_est./squeeze(freqresp(G_geo, f, 'Hz'))), '.'); set(gca,'ColorOrderIndex',2) plot(f, 180/pi*angle(tf_geo2_est./squeeze(freqresp(G_geo, f, 'Hz'))), '.'); plot(f, 180/pi*angle(tf_acc1_est.*squeeze(freqresp(inv(G_acc)*H_acc, f, 'Hz')) + ... tf_geo1_est.*squeeze(freqresp(inv(G_geo)*H_geo, f, 'Hz'))), 'k.') plot(f, 180/pi*angle(tf_acc2_est.*squeeze(freqresp(inv(G_acc)*H_acc, f, 'Hz')) + ... tf_geo2_est.*squeeze(freqresp(inv(G_geo)*H_geo, f, 'Hz'))), 'k.') set(gca,'xscale','log'); yticks(-180:90:180); ylim([-180 180]); xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); hold off; linkaxes([ax1,ax2],'x'); xlim([1, 5e3]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/super_sensor_optimal_uncertainty.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:super_sensor_optimal_uncertainty #+caption: Inertial Sensor dynamics as well as the super sensor dynamics #+RESULTS: [[file:figs/super_sensor_optimal_uncertainty.png]] ** Experimental Super Sensor Noise #+begin_src matlab :exports none load('./matlab/mat/sensor_noises.mat', 'pN_acc', 'pN_geo', 'N_acc', 'N_geo', 'f') #+end_src The obtained super sensor noise is shown in Figure [[fig:super_sensor_optimal_noise]]. #+begin_src matlab :exports none 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'); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/super_sensor_optimal_noise.pdf', 'width', 'wide', 'height', 'normal'); #+end_src #+name: fig:super_sensor_optimal_noise #+caption: ASD of the super sensor obtained using the $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis #+RESULTS: [[file:figs/super_sensor_optimal_noise.png]] * Matlab Functions <> ** =createWeight= :PROPERTIES: :header-args:matlab+: :tangle matlab/src/createWeight.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> This Matlab function is accessible [[file:src/createWeight.m][here]]. #+begin_src matlab function [W] = createWeight(args) % createWeight - % % Syntax: [in_data] = createWeight(in_data) % % Inputs: % - n - Weight Order % - G0 - Low frequency Gain % - G1 - High frequency Gain % - Gc - Gain of W at frequency w0 % - w0 - Frequency at which |W(j w0)| = Gc % % Outputs: % - W - Generated Weight arguments args.n (1,1) double {mustBeInteger, mustBePositive} = 1 args.G0 (1,1) double {mustBeNumeric, mustBePositive} = 0.1 args.G1 (1,1) double {mustBeNumeric, mustBePositive} = 10 args.Gc (1,1) double {mustBeNumeric, mustBePositive} = 1 args.w0 (1,1) double {mustBeNumeric, mustBePositive} = 1 end mustBeBetween(args.G0, args.Gc, args.G1); s = tf('s'); W = (((1/args.w0)*sqrt((1-(args.G0/args.Gc)^(2/args.n))/(1-(args.Gc/args.G1)^(2/args.n)))*s + (args.G0/args.Gc)^(1/args.n))/((1/args.G1)^(1/args.n)*(1/args.w0)*sqrt((1-(args.G0/args.Gc)^(2/args.n))/(1-(args.Gc/args.G1)^(2/args.n)))*s + (1/args.Gc)^(1/args.n)))^args.n; end % Custom validation function function mustBeBetween(a,b,c) if ~((a > b && b > c) || (c > b && b > a)) eid = 'createWeight:inputError'; msg = 'Gc should be between G0 and G1.'; throwAsCaller(MException(eid,msg)) end end #+end_src ** =plotMagUncertainty= :PROPERTIES: :header-args:matlab+: :tangle matlab/src/plotMagUncertainty.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> This Matlab function is accessible [[file:src/plotMagUncertainty.m][here]]. #+begin_src matlab function [p] = plotMagUncertainty(W, freqs, args) % plotMagUncertainty - % % Syntax: [p] = plotMagUncertainty(W, freqs, args) % % Inputs: % - W - Multiplicative Uncertainty Weight % - freqs - Frequency Vector [Hz] % - args - Optional Arguments: % - G % - color_i % - opacity % % Outputs: % - p - Plot Handle arguments W freqs double {mustBeNumeric, mustBeNonnegative} args.G = tf(1) args.color_i (1,1) double {mustBeInteger, mustBePositive} = 1 args.opacity (1,1) double {mustBeNumeric, mustBeNonnegative} = 0.3 args.DisplayName char = '' end % Get defaults colors colors = get(groot, 'defaultAxesColorOrder'); p = patch([freqs flip(freqs)], ... [abs(squeeze(freqresp(args.G, freqs, 'Hz'))).*(1 + abs(squeeze(freqresp(W, freqs, 'Hz')))); ... flip(abs(squeeze(freqresp(args.G, freqs, 'Hz'))).*max(1 - abs(squeeze(freqresp(W, freqs, 'Hz'))), 1e-6))], 'w', ... 'DisplayName', args.DisplayName); p.FaceColor = colors(args.color_i, :); p.EdgeColor = 'none'; p.FaceAlpha = args.opacity; end #+end_src ** =plotPhaseUncertainty= :PROPERTIES: :header-args:matlab+: :tangle matlab/src/plotPhaseUncertainty.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> This Matlab function is accessible [[file:src/plotPhaseUncertainty.m][here]]. #+begin_src matlab function [p] = plotPhaseUncertainty(W, freqs, args) % plotPhaseUncertainty - % % Syntax: [p] = plotPhaseUncertainty(W, freqs, args) % % Inputs: % - W - Multiplicative Uncertainty Weight % - freqs - Frequency Vector [Hz] % - args - Optional Arguments: % - G % - color_i % - opacity % % Outputs: % - p - Plot Handle arguments W freqs double {mustBeNumeric, mustBeNonnegative} args.G = tf(1) args.color_i (1,1) double {mustBeInteger, mustBePositive} = 1 args.opacity (1,1) double {mustBeNumeric, mustBePositive} = 0.3 args.DisplayName char = '' end % Get defaults colors colors = get(groot, 'defaultAxesColorOrder'); % Compute Phase Uncertainty Dphi = 180/pi*asin(abs(squeeze(freqresp(W, freqs, 'Hz')))); Dphi(abs(squeeze(freqresp(W, freqs, 'Hz'))) > 1) = 360; % Compute Plant Phase G_ang = 180/pi*angle(squeeze(freqresp(args.G, freqs, 'Hz'))); p = patch([freqs flip(freqs)], [G_ang+Dphi; flip(G_ang-Dphi)], 'w', ... 'DisplayName', args.DisplayName); p.FaceColor = colors(args.color_i, :); p.EdgeColor = 'none'; p.FaceAlpha = args.opacity; end #+end_src * Bibliography :ignore: #+latex: \printbibliography