#+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/zenburn.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/jquery.stickytableheaders.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+ :tangle filters.m
#+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
:END:

* 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.

The inertial sensors used are shown in Table

#+name: tab:inertial_sensors
#+caption: Inertial Sensors used
| *Type*        | *Model*                      |
|---------------+------------------------------|
| Accelerometer | PCB 393B05 - Vertical ([[https://www.pcb.com/products?m=393B05][link]]) |
| Geophone      | Mark Product L-22 - Vertical |


#+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 ([[https://www.femto.de/en/products/voltage-amplifiers/variable-gain-100-khz-dlpva.html][link]]).
The force sensor signal is amplified using a Low Noise Voltage Preamplifier from Ametek ([[https://www.ameteksi.com/support-center/legacy-products/signal-recovery-legacy/5113-low-noise-preamplifier][link]]).

The excitation signal is amplified by a linear amplified from Cedrat (LA75B) with a gain equals to 20.

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

* Identification of the 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
  addpath('./matlab');
#+end_src

** Load Data
#+begin_src matlab
  id_ol = load('./mat/identification_noise_bis.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
  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

** Identified Plant
#+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_d_est,     ~] = tfestimate(id_ol.u, id_ol.d, win, [], [], 1/Ts); % [m/V]
#+end_src

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2, 1, 1);
  hold on;
  plot(f, abs(tf_fmeas_est), '-')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude'); xlabel('Frequency [Hz]');
  hold off;

  ax2 = subplot(2, 1, 2);
  hold on;
  plot(f, 180/pi*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 :exports none
  figure;
  ax1 = subplot(2, 1, 1);
  hold on;
  plot(f, abs(tf_d_est), '-')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
  hold off;

  ax2 = subplot(2, 1, 2);
  hold on;
  plot(f, 180/pi*angle(tf_d_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

** Simscape Model
#+begin_src matlab
  load('./mat/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;
  ax1 = subplot(2, 2, 1);
  hold on;
  set(gca, 'ColorOrderIndex', 1);
  plot(f, abs(tf_fmeas_est), '-')
  set(gca, 'ColorOrderIndex', 1);
  plot(f, abs(squeeze(freqresp(G('Vs', 'Va'), f, 'Hz'))), '--')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
  hold off;

  ax2 = subplot(2, 2, 2);
  hold on;
  set(gca, 'ColorOrderIndex', 1);
  plot(f, abs(tf_d_est), '-')
  set(gca, 'ColorOrderIndex', 1);
  plot(f, abs(squeeze(freqresp(G('y', 'Va'), f, 'Hz'))), '--')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
  hold off;

  ax3 = subplot(2, 2, 3);
  hold on;
  set(gca, 'ColorOrderIndex', 1);
  plot(f, 180/pi*angle(tf_fmeas_est), '-')
  set(gca, 'ColorOrderIndex', 1);
  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]);

  ax4 = subplot(2, 2, 4);
  hold on;
  set(gca, 'ColorOrderIndex', 1);
  plot(f, 180/pi*angle(tf_d_est), '-')
  set(gca, 'ColorOrderIndex', 1);
  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,ax3,ax4], 'x');
  xlim([1, 5e3]);
#+end_src

** IFF
#+begin_src matlab
  Kiff = 102/(s + 2*pi*2);
#+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, '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

#+begin_src matlab
  id_cl = load('./mat/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

#+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

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2, 1, 1);
  hold on;
  set(gca, 'ColorOrderIndex', 1);
  plot(f, abs(tf_G_ol_est), '-')
  set(gca, 'ColorOrderIndex', 1);
  plot(f, abs(squeeze(freqresp(G('y', 'Va'), f, 'Hz'))), '--')
  set(gca, 'ColorOrderIndex', 2);
  plot(f, abs(tf_G_cl_est), '-')
  set(gca, 'ColorOrderIndex', 2);
  plot(f, abs(squeeze(freqresp(G_cl('y', 'Va'), f, 'Hz'))), '--')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
  hold off;

  ax2 = subplot(2, 1, 2);
  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

** Inertial Sensors
#+begin_src matlab
  id = load('./mat/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

#+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

#+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, 'openoutput'); io_i = io_i + 1; % Accelerometer [V]
  io(io_i) = linio([mdl, '/Voltage_Ampl_geo'], 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

Model of the inertial sensors:
#+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

#+begin_src matlab :exports none
  freqs = logspace(-1, 4, 1000)';

  figure;
  ax1 = subplot(2, 1, 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 = subplot(2, 1, 2);
  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 :exports none
  freqs = logspace(-1, 4, 1000)';

  figure;
  ax1 = subplot(2, 1, 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 = subplot(2, 1, 2);
  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

* IFF Development
** 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

** Load Data
#+begin_src matlab
  id_ol = load('./mat/identification_noise_bis.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
  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
#+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]]

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2, 1, 1);
  hold on;
  plot(f, abs(tf_fmeas_est), '-')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
  hold off;

  ax2 = subplot(2, 1, 2);
  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', 'full', 'height', 'full');
#+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
#+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

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2, 1, 1);
  hold on;
  plot(f, abs(tf_fmeas_est), '.')
  plot(f, abs(squeeze(freqresp(Giff, f, 'Hz'))), 'k-')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
  hold off;
  ylim([1e-2, 1e3])

  ax2 = subplot(2, 1, 2);
  hold on;
  plot(f, 180/pi*angle(tf_fmeas_est), '.')
  plot(f, 180/pi*angle(squeeze(freqresp(Giff, f, 'Hz'))), 'k-')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
  ylabel('Phase'); xlabel('Frequency [Hz]');
  hold off;
  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', 'large', 'height', 'full');
#+end_src

#+name: fig:iff_plant_model
#+caption: IFF Plant + Model
#+RESULTS:
[[file:figs/iff_plant_model.png]]

** Root Locus and optimal Controller

#+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 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 resonance damped.
It helps to not induce too much motion at the resonance and damage the actuator.
#+begin_src matlab
  id_cl = load('./mat/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

#+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

#+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;
  ax1 = subplot(2, 1, 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 = subplot(2, 1, 2);
  hold on;
  plot(f, 180/pi*angle(tf_G_ol_est), '-')
  plot(f, 180/pi*angle(tf_G_cl_est), '-')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
  ylabel('Phase'); xlabel('Frequency [Hz]');
  hold off;
  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', 'full', 'height', 'full');
#+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
** 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

** Requirements
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)

** 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('./mat/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;
  ax1 = subplot(2, 1, 1);
  hold on;
  plot(f, abs(tf_G_cl_est), '-')
  plot(f, abs(squeeze(freqresp(G_d_est, f, 'Hz'))), '--')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
  hold off;

  ax2 = subplot(2, 1, 2);
  hold on;
  plot(f, 180/pi*angle(tf_G_cl_est), '-')
  plot(f, 180/pi*angle(squeeze(freqresp(G_d_est, f, 'Hz'))), '--')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
  ylabel('Phase'); xlabel('Frequency [Hz]');
  hold off;
  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', 'full', 'height', 'full');
#+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('./mat/huddle_test.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
  ht.d = detrend(ht.d, 0);
  ht.acc_1 = detrend(ht.acc_1, 0);
  ht.acc_2 = detrend(ht.acc_2, 0);
  ht.geo_1 = detrend(ht.geo_1, 0);
  ht.geo_2 = detrend(ht.geo_2, 0);
#+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(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/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
** 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

** Load Data
#+begin_src matlab
  id = load('./mat/identification_noise_opt_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
  ht = load('./mat/huddle_test.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
#+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
#+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);
  [p_id_fmeas, ~] = pwelch(id.f_meas, 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;
  ax1 = subplot(1, 2, 1);
  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();

  ax1 = subplot(1, 2, 2);
  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();
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/comp_psd_huddle_test_identification.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:comp_psd_huddle_test_identification
#+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.png]]

** Compute transfer functions
#+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

#+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]]

Model of the inertial sensors:
#+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

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2, 1, 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 = subplot(2, 1, 2);
  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', 'full', 'height', 'full');
#+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;
  ax1 = subplot(2, 1, 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 = subplot(2, 1, 2);
  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', 'full', 'height', 'full');
#+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 H2 Synthesis
** 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

** Load Data
#+begin_src matlab
  id = load('./mat/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
#+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);
  [p_id_fmeas, ~] = pwelch(id.f_meas, 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

#+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();
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/measure_displacement_all_sensors.pdf', 'width', 'full', 'height', 'full');
#+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
- [ ] Add formula to estimate the noise from data

#+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

#+begin_src matlab
  pN_acc = sqrt(p_id_acc1.*(1 - coh_acc)) .* ... % [V/sqrt(Hz)]
           1./abs(squeeze(freqresp(G_acc*s^2, f, 'Hz'))); % [m/V]
  pN_geo = sqrt(p_id_geo1.*(1 - coh_geo)) .* ... % [V/sqrt(Hz)]
           1./abs(squeeze(freqresp(G_geo*s, f, 'Hz'))); % [m/V]
#+end_src

#+begin_src matlab :results 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, pN_acc, '-', 'DisplayName', 'Accelerometers - Noise');
  plot(f, 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-13, 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', 'full', 'height', 'full');
#+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
#+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 :results 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');
  set(gca, 'ColorOrderIndex', 1);
  plot(freqs, abs(squeeze(freqresp(N_acc, freqs, 'Hz'))), '--', 'DisplayName', 'Geophones - 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]]

** H2 Synthesis
The generalize plant is created.
#+begin_src matlab
  P = [0      N_acc  1;
       N_geo -N_acc  0];
#+end_src

And we do the $\mathcal{H}_2$ synthesis using the =h2syn= command.
#+begin_src matlab
  [H_geo, ~, gamma] = h2syn(P, 1, 1);
  H_acc = 1 - H_geo;
#+end_src

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2, 1, 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 = subplot(2, 1, 2);
  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', 'full', 'height', 'full');
#+end_src

#+name: fig:complementary_filters_velocity_H2
#+caption: Obtained Complementary Filters
#+RESULTS:
[[file:figs/complementary_filters_velocity_H2.png]]

** Results
#+begin_src matlab :results 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]]


Integrate only down to 1Hz.
#+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
** 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

** Load Data
#+begin_src matlab
  id = load('./mat/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
#+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

Model of the inertial sensors:
#+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

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2, 1, 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 = subplot(2, 1, 2);
  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

** Dynamics uncertainty estimation
- [ ] Create a weight for both sensors and a nominal model
- [ ] Make sure the dynamics measured are inside the uncertain model
- [ ] Compare the uncertainties

#+begin_src matlab
  omegac = 2*2*pi; G0 = 5; Ginf = 0.1;
  w_acc = (Ginf*s/omegac + G0)/(s/omegac + 1);
  omegac = 2e3*2*pi; G0 = 1; Ginf = 50;
  w_acc = w_acc*(Ginf*s/omegac + G0)/(s/omegac + 1);

  n = 2; w0 = 2*pi*10; G0 = 5; G1 = 1/10; Gc = 1;
  w_acc = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n;
  n = 2; w0 = 2*pi*1000; G0 = 1/10; G1 = 5; Gc = 1;
  w_acc = 10*w_acc*(((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n;

  n = 4; w0 = 2*pi*500; G0 = 1/10; G1 = 10; Gc = 1;
  w_geo = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n;
#+end_src

The sensor uncertain models are defined below.
#+begin_src matlab
  G_acc = 1 + w_acc*ultidyn('Delta',[1 1]);
  G_geo = 1 + w_geo*ultidyn('Delta',[1 1]);


  G_acc_s = usample(G_acc, 20);
  G_geo_s = usample(G_geo, 20);
#+end_src

** Dynamical Uncertainty
#+begin_src matlab :results none
  [T_acc, ~] = tfestimate(id.acc_1, id.acc_2, win, [], [], 1/Ts);
  [T_geo, ~] = tfestimate(id.geo_1, id.geo_2, win, [], [], 1/Ts);
#+end_src

#+begin_src matlab :results none :exports none
  Dphi_acc = 180/pi*asin(abs(squeeze(freqresp(w_acc, freqs, 'Hz'))));
  Dphi_acc(abs(squeeze(freqresp(w_acc, freqs, 'Hz'))) > 1) = 190;

  freqs = logspace(0, 4, 1000);

  figure;

  ax1 = subplot(2,1,1);
  hold on;
  plot(f, abs(T_acc), 'DisplayName', 'Accelerometers');
  plot(freqs, max(1 - abs(squeeze(freqresp(w_acc, freqs, 'Hz'))), 0), 'k--', 'HandleVisibility', 'off');
  plot(freqs, 1 + abs(squeeze(freqresp(w_acc, freqs, 'Hz'))), 'k--', 'DisplayName', 'Bounds - S2');
  for i = 1:length(G_acc_s)
    plot(freqs, abs(squeeze(freqresp(G_acc_s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'HandleVisibility', 'off');
  end
  hold off;
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  ylabel('Magnitude'); xlabel('Frequency [Hz]');
  legend('location', 'southeast');

  ax2 = subplot(2,1,2);
  hold on;
  plot(f, 180/pi*angle(T_acc));
  plot(freqs,  Dphi_acc, 'k--');
  plot(freqs, -Dphi_acc, 'k--');
  for i = 1:length(G_acc_s)
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_acc_s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2]);
  end
  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, 5000]);
#+end_src

#+begin_src matlab :results none :exports none
  Dphi_geo = 180/pi*asin(abs(squeeze(freqresp(w_geo, freqs, 'Hz'))));
  Dphi_geo(abs(squeeze(freqresp(w_geo, freqs, 'Hz'))) > 1) = 190;

  freqs = logspace(0, 4, 1000);

  figure;

  ax1 = subplot(2,1,1);
  hold on;
  plot(f, abs(T_geo), 'DisplayName', 'Geophone');
  plot(freqs, max(1 - abs(squeeze(freqresp(w_geo, freqs, 'Hz'))), 0), 'k--', 'HandleVisibility', 'off');
  plot(freqs, 1 + abs(squeeze(freqresp(w_geo, freqs, 'Hz'))), 'k--', 'DisplayName', 'Bounds - S2');
  for i = 1:length(G_geo_s)
    plot(freqs, abs(squeeze(freqresp(G_geo_s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'HandleVisibility', 'off');
  end
  hold off;
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  ylabel('Magnitude'); xlabel('Frequency [Hz]');
  legend('location', 'southeast');

  ax2 = subplot(2,1,2);
  hold on;
  plot(f, 180/pi*angle(T_geo));
  plot(freqs,  Dphi_geo, 'k--');
  plot(freqs, -Dphi_geo, 'k--');
  for i = 1:length(G_geo_s)
    plot(freqs, 180/pi*angle(squeeze(freqresp(G_geo_s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2]);
  end
  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, 5000]);
#+end_src

#+begin_src matlab :results none :exports none
  freqs = logspace(0, 4, 1000);

  figure;
  hold on;
  set(gca, 'ColorOrderIndex', 1);
  plot(freqs, max(1 - abs(squeeze(freqresp(w_acc, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
  set(gca, 'ColorOrderIndex', 1);
  plot(freqs, 1 + abs(squeeze(freqresp(w_acc, freqs, 'Hz'))), '--', 'DisplayName', 'Accelerometer');
  set(gca, 'ColorOrderIndex', 2);
  plot(freqs, max(1 - abs(squeeze(freqresp(w_geo, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
  set(gca, 'ColorOrderIndex', 2);
  plot(freqs, 1 + abs(squeeze(freqresp(w_geo, freqs, 'Hz'))), '--', 'DisplayName', 'Geophone');
  hold off;
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  ylabel('Magnitude'); xlabel('Frequency [Hz]');
  legend('location', 'southeast');
  xlim([1, 5000]);
#+end_src

#+begin_src matlab
  P = [w_acc -w_acc;
       0      w_geo;
       1      0];
#+end_src

And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command.
#+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
[H_geo, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');

  Test bounds:  4.472 <=  gamma  <=  5

    gamma        X>=0        Y>=0       rho(XY)<1    p/f
  4.729e+00     0.0e+00     0.0e+00     3.000e-17     p
  4.599e+00     1.2e-16     0.0e+00     1.396e-17     p
  4.535e+00     1.5e-16     0.0e+00     8.854e-18     p
  4.503e+00     1.7e-16     0.0e+00     4.759e-18     p
  4.488e+00     3.5e-17     0.0e+00     1.565e-18     p
  4.480e+00     1.3e-16     0.0e+00     8.303e-17     p
  4.476e+00     0.0e+00     0.0e+00     1.173e-16     p

  Best performance (actual): 4.472
'org_babel_eoe'
ans =
    'org_babel_eoe'
#+end_example

#+begin_src matlab
  H_acc = 1 - H_geo;
#+end_src

#+begin_src matlab :exports none
  figure;

  ax1 = subplot(2,1,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([5e-4, 20]);
  legend('location', 'northeast');

  ax2 = subplot(2,1,2);
  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([freqs(1), freqs(end)]);
  xticks([0.1, 1, 10, 100, 1000]);
#+end_src

#+begin_src matlab
  Gss = G_acc*H_acc + G_geo*H_geo;
#+end_src

#+begin_src matlab :exports none
  Gsss = usample(Gss, 20);
#+end_src

#+begin_src matlab :exports none
  % We here compute the maximum and minimum phase of the super sensor
  Dphiss = 180/pi*asin(abs(squeeze(freqresp(w_acc*H_acc, freqs, 'Hz')))+abs(squeeze(freqresp(w_geo*H_geo, freqs, 'Hz'))));
  Dphiss(abs(squeeze(freqresp(w_acc*H_acc, freqs, 'Hz')))+abs(squeeze(freqresp(w_geo*H_geo, freqs, 'Hz'))) > 1) = 190;
#+end_src

#+begin_src matlab :exports none
  figure;
  % Magnitude
  ax1 = subplot(2,1,1);
  hold on;
  set(gca,'ColorOrderIndex',1);
  plot(freqs, 1 + abs(squeeze(freqresp(w_geo, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S1');
  set(gca,'ColorOrderIndex',1);
  plot(freqs, max(1 - abs(squeeze(freqresp(w_geo, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
  set(gca,'ColorOrderIndex',2);
  plot(freqs, 1 + abs(squeeze(freqresp(w_acc, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S2');
  set(gca,'ColorOrderIndex',2);
  plot(freqs, max(1 - abs(squeeze(freqresp(w_acc, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
  plot(freqs, 1 + abs(squeeze(freqresp(w_geo*H_geo, freqs, 'Hz'))) + abs(squeeze(freqresp(w_acc*H_acc, freqs, 'Hz'))), 'k--', 'DisplayName', 'Bounds - SS');
  plot(freqs, max(1 - abs(squeeze(freqresp(w_geo*H_geo, freqs, 'Hz'))) - abs(squeeze(freqresp(w_acc*H_acc, freqs, 'Hz'))), 0), 'k--', 'HandleVisibility', 'off');
  plot(freqs, abs(squeeze(freqresp(Gsss(1, 1, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'DisplayName', 'SS Dynamics');
  for i = 2:length(Gsss)
    plot(freqs, abs(squeeze(freqresp(Gsss(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'HandleVisibility', 'off');
  end
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  legend('location', 'southwest');
  ylabel('Magnitude');
  ylim([5e-2, 10]);
  hold off;

  % Phase
  ax2 = subplot(2,1,2);
  hold on;
  set(gca,'ColorOrderIndex',1);
  plot(freqs,  Dphi_geo, '--');
  set(gca,'ColorOrderIndex',1);
  plot(freqs, -Dphi_geo, '--');
  set(gca,'ColorOrderIndex',2);
  plot(freqs,  Dphi_acc, '--');
  set(gca,'ColorOrderIndex',2);
  plot(freqs, -Dphi_acc, '--');
  plot(freqs,  Dphiss, 'k--');
  plot(freqs, -Dphiss, 'k--');
  for i = 1:length(Gsss)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gsss(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2]);
  end
  set(gca,'xscale','log');
  yticks(-180:90:180);
  ylim([-180 180]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
#+end_src

* TODO To Order
** TODO Huddle Test
*** Introduction                                                    :ignore:
The goal here is to measure the noise of the inertial sensors.
Is also permits to measure the motion level when nothing is actuated.

*** 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

*** Load Data
#+begin_src matlab
  ht = load('./mat/huddle_test.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
#+end_src

*** Detrend Data
#+begin_src matlab
  ht.d = detrend(ht.d, 0); % [m]
  ht.acc_1 = detrend(ht.acc_1, 0); % [V]
  ht.acc_2 = detrend(ht.acc_2, 0); % [V]
  ht.geo_1 = detrend(ht.geo_1, 0); % [V]
  ht.geo_2 = detrend(ht.geo_2, 0); % [V]
  ht.f_meas = detrend(ht.f_meas, 0); % [V]
#+end_src

*** Compute PSD
We first define the parameters for the frequency domain analysis.
#+begin_src matlab
  Ts = ht.t(2) - ht.t(1); % [s]
  Fs = 1/Ts; % [Hz]

  win = hanning(ceil(1*Fs));
#+end_src

Then we compute the Power Spectral Density using =pwelch= function.
#+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);
  [p_fmeas, ~] = pwelch(ht.f_meas, win, [], [], 1/Ts);
#+end_src

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(1, 2, 1);
  hold on;
  plot(f, sqrt(p_acc1));
  plot(f, sqrt(p_acc2));
  hold off;
  set(gca, 'xscale', 'log');
  set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('ASD Accelerometers $[V/\sqrt{Hz}]$')
  xlim([1, 5000]);

  ax2 = subplot(1, 2, 2);
  hold on;
  plot(f, sqrt(p_geo1));
  plot(f, sqrt(p_geo2));
  hold off;
  set(gca, 'xscale', 'log');
  set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('ASD Geophones $[V/\sqrt{Hz}]$')
  xlim([1, 5000]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/huddle_test_measured_voltage_inertial_sensors.pdf', 'width', 'full', 'height', 'tall');
#+end_src

#+name: fig:huddle_test_measured_voltage_inertial_sensors
#+caption: ASD of the measured voltage from the inertial sensors during the Huddle test
#+RESULTS:
[[file:figs/huddle_test_measured_voltage_inertial_sensors.png]]

*** Sensor Noise in Volts
#+begin_src matlab
  [coh_acc, ~] = mscohere(ht.acc_1, ht.acc_2, win, [], [], Fs);
  [coh_geo, ~] = mscohere(ht.geo_1, ht.geo_2, win, [], [], Fs);
#+end_src

#+begin_src matlab
  pN_acc = p_acc1.*(1 - coh_acc);
  pN_geo = p_geo1.*(1 - coh_geo);
#+end_src

PSD of the ADC quantization noise.
#+begin_src matlab
  Sq = (20/2^16)^2/(12*Fs);
#+end_src

#+begin_src matlab :results none
  figure;
  hold on;
  plot(f, sqrt(pN_acc), '-', 'DisplayName', 'Accelerometers');
  plot(f, sqrt(pN_geo), '-', 'DisplayName', 'Geophones');
  plot(f, ones(size(f))*sqrt(Sq), '-', 'DisplayName', 'ADC');
  hold off;
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('ASD of the Measurement Noise $[V/\sqrt{Hz}]$');
  xlim([1, 5000]);
  legend('location', 'northeast');
#+end_src

** TODO Sensor Dynamics
*** Introduction                                                    :ignore:
Thanks to the interferometer, it is possible to compute the transfer function from the mass displacement to the voltage generated by the inertial sensors.
This permits to estimate the sensor dynamics and to calibrate the sensors.

*** 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

*** Time Domain Signals
Excitation signal: noise.

#+begin_src matlab :exports none
  figure;
  plot(id_ol.t, id_ol.u);
  xlim([0 0.1]);
  xlabel('Time [s]'); ylabel('Excitation Signal [V]');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/first_exc_signal_time.pdf', 'width', 'wide', 'height', 'normal');
#+end_src

#+name: fig:first_exc_signal_time
#+caption: Excitation signal used for the first identification
#+RESULTS:
[[file:figs/first_exc_signal_time.png]]

** H-2 Synthesis - Position
*** Noise Model
#+begin_src matlab
  N_acc = 1e-4*(s/(2*pi*2000) + 1)^2/(s + 0.1*2*pi)^2; % [m/sqrt(Hz)]
  N_geo = 7e-8*(s/(2*pi*200) + 1)/(s + 0.1*2*pi); % [m/sqrt(Hz)]
#+end_src

#+begin_src matlab :results none
  freqs = logspace(0, 4, 1000);

  figure;
  hold on;
  plot(f, pN_acc, '-', 'DisplayName', 'Accelerometers - Noise');
  plot(f, pN_geo, '-', 'DisplayName', 'Geophones - Noise');
  set(gca, 'ColorOrderIndex', 1);
  plot(freqs, abs(squeeze(freqresp(N_acc, freqs, 'Hz'))), '--', 'DisplayName', 'Geophones - 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}{\sqrt{Hz}}\right]$');
  xlim([1, 5000]); ylim([1e-13, 1e-5]);
  legend('location', 'northeast');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/inertial_sensor_noise_model.pdf', 'width', 'full', 'height', 'tall');
#+end_src

#+name: fig:inertial_sensor_noise_model
#+caption: Measured ASD of the inertial sensor noise and generated noise weighting filters
#+RESULTS:
[[file:figs/inertial_sensor_noise_model.png]]

*** H2 Synthesis
#+begin_src matlab
  P = [0      N_acc  1;
       N_geo -N_acc  0];
#+end_src

And we do the $\mathcal{H}_2$ synthesis using the =h2syn= command.
#+begin_src matlab
  [H_geo, ~, gamma] = h2syn(P, 1, 1);
  H_acc = 1 - H_geo;
#+end_src

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2, 1, 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 = subplot(2, 1, 2);
  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/opt_complementary_filters.pdf', 'width', 'wide', 'height', 'tall');
#+end_src

#+name: fig:opt_complementary_filters
#+caption:
#+RESULTS:
[[file:figs/opt_complementary_filters.png]]

*** Results
#+begin_src matlab :results none
  freqs = logspace(0, 4, 1000);

  figure;
  hold on;
  plot(f, pN_acc, '-', 'DisplayName', 'Accelerometers - Noise');
  plot(f, pN_geo, '-', 'DisplayName', 'Geophones - Noise');
  plot(f, sqrt(pN_acc.^2.*abs(squeeze(freqresp(H_acc, f, 'Hz'))).^2 + pN_geo.^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}{\sqrt{Hz}}\right]$');
  xlim([1, 5000]); ylim([1e-13, 1e-5]);
  legend('location', 'northeast');
#+end_src

Integrate only down to 1Hz.
#+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)));
  CPS_geo = 1/pi*flip(-cumtrapz(2*pi*flip(f), flip(pN_geo.^2)));
  CPS_SS  = 1/pi*flip(-cumtrapz(2*pi*flip(f), flip(pN_acc.^2.*abs(squeeze(freqresp(H_acc, f, 'Hz'))).^2 + pN_geo.^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\\,nm (rms)$', 1e9*sqrt(CPS_acc(i_1Hz))));
  plot(f, CPS_geo, '-',  'DisplayName', sprintf('$\\sigma_{\\hat{x}_{geo}} = %.0f\\,nm (rms)$', 1e9*sqrt(CPS_geo(i_1Hz))));
  plot(f, CPS_SS,  'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}} = %.1f\\,nm (rms)$',       1e9*sqrt(CPS_SS(i_1Hz))));
  set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum');
  hold off;
  xlim([1, 3e3]);
  legend('location', 'northeast');
#+end_src

** Compare Time domain Estimation of the displacement
Let's compare the measured accelerations instead of displacement (no integration).

#+begin_src matlab
  G_lpf = 1/(1 + s/2/pi/5e3);

  acc1_a = lsim(1/G_acc*G_lpf, id.acc_1, id.t);
  acc2_a = lsim(1/G_acc*G_lpf, id.acc_2, id.t);
#+end_src

#+begin_src matlab
  geo1_a = lsim(1/G_geo*s*G_lpf, id.geo_1, id.t);
  geo2_a = lsim(1/G_geo*s*G_lpf, id.geo_2, id.t);
#+end_src

#+begin_src matlab
  int_a = lsim(s^2*G_lpf*G_lpf, id.d, id.t);
#+end_src

#+begin_src matlab
  figure;
  hold on;
  plot(id.t, int_a);
  plot(id.t, acc1_a);
  plot(id.t, acc2_a);
  plot(id.t, geo1_a);
  plot(id.t, geo2_a);
  hold off;
  xlabel('Time [s]'); ylabel('Acceleration [m]');
#+end_src