2541 lines
92 KiB
Org Mode
2541 lines
92 KiB
Org Mode
#+TITLE: Sensor Fusion - Test Bench
|
|
:DRAWER:
|
|
#+LANGUAGE: en
|
|
#+EMAIL: dehaeze.thomas@gmail.com
|
|
#+AUTHOR: Dehaeze Thomas
|
|
|
|
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
|
|
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
|
|
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/custom.css"/>
|
|
#+HTML_HEAD: <script type="text/javascript" src="./js/jquery.min.js"></script>
|
|
#+HTML_HEAD: <script type="text/javascript" src="./js/bootstrap.min.js"></script>
|
|
#+HTML_HEAD: <script type="text/javascript" src="./js/readtheorg.js"></script>
|
|
|
|
#+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 ""
|
|
#+OPTIONS: toc:2
|
|
|
|
#+LATEX_CLASS: cleanreport
|
|
#+LATEX_CLASS_OPTIONS: [tocnp, secbreak, minted]
|
|
|
|
#+LATEX_HEADER_EXTRA: \usepackage[cache=false]{minted}
|
|
#+LATEX_HEADER_EXTRA: \usemintedstyle{autumn}
|
|
#+LATEX_HEADER_EXTRA: \setminted[matlab]{linenos=true, breaklines=true, tabsize=4, fontsize=\scriptsize, autogobble=true}
|
|
|
|
#+LATEX_HEADER: \newcommand{\authorFirstName}{Thomas}
|
|
#+LATEX_HEADER: \newcommand{\authorLastName}{Dehaeze}
|
|
#+LATEX_HEADER: \newcommand{\authorEmail}{dehaeze.thomas@gmail.com}
|
|
|
|
#+LATEX_HEADER_EXTRA: \makeatletter
|
|
#+LATEX_HEADER_EXTRA: \preto\Gin@extensions{png,}
|
|
#+LATEX_HEADER_EXTRA: \DeclareGraphicsRule{.png}{pdf}{.pdf}{\noexpand\Gin@base.pdf}
|
|
#+LATEX_HEADER_EXTRA: \makeatother
|
|
|
|
#+LATEX_HEADER_EXTRA: \addbibresource{ref.bib}
|
|
:END:
|
|
|
|
#+begin_export html
|
|
<hr>
|
|
<p>This report is also available as a <a href="./index.pdf">pdf</a>.</p>
|
|
<hr>
|
|
#+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
|
|
<<sec: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:
|
|
<<sec:first_identification>>
|
|
** 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)
|
|
<<matlab-dir>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results silent :noweb yes
|
|
<<matlab-init>>
|
|
#+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 = 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, '/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:
|
|
<<sec:integral_force_feedback>>
|
|
** 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)
|
|
<<matlab-dir>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results silent :noweb yes
|
|
<<matlab-init>>
|
|
#+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:
|
|
<<sec:optimal_excitation>>
|
|
** 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)
|
|
<<matlab-dir>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results silent :noweb yes
|
|
<<matlab-init>>
|
|
#+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:
|
|
<<sec:inertial_sensor_dynamics>>
|
|
** 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)
|
|
<<matlab-dir>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results silent :noweb yes
|
|
<<matlab-init>>
|
|
#+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:
|
|
<<sec:inertial_sensor_noise>>
|
|
** 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)
|
|
<<matlab-dir>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results silent :noweb yes
|
|
<<matlab-init>>
|
|
#+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:
|
|
<<sec:inertial_sensor_uncertainty>>
|
|
** 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)
|
|
<<matlab-dir>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results silent :noweb yes
|
|
<<matlab-init>>
|
|
#+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:
|
|
<<sec:optimal_sensor_fusion>>
|
|
** 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)
|
|
<<matlab-dir>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results silent :noweb yes
|
|
<<matlab-init>>
|
|
#+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
|
|
<<sec:matlab_functions>>
|
|
** =createWeight=
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/createWeight.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:createWeight>>
|
|
|
|
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:
|
|
<<sec:plotMagUncertainty>>
|
|
|
|
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:
|
|
<<sec:plotPhaseUncertainty>>
|
|
|
|
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:
|
|
bibliography:ref.bib
|
|
|
|
#+latex: \printbibliography
|