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

|               |                              |
|---------------+------------------------------|
| Accelerometer | PCB 393B05 - Vertical ([[https://www.pcb.com/products?m=393B05][link]]) |
| Geophone      | Mark Product L4C - Vertical  |

* Huddle Test
** 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
  load('./mat/huddle_test.mat', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 't');
  dt = t(2) - t(1);
#+end_src

** Data
#+begin_src matlab
  acc_1 = acc_1 - mean(acc_1);
  acc_2 = acc_2 - mean(acc_2);
  geo_1 = geo_1 - mean(geo_1);
  geo_2 = geo_2 - mean(geo_2);
#+end_src

** Scale Data
From raw data to estimated velocity.
This takes into account the sensibility of the sensor and possible integration to go from acceleration to velocity.

#+begin_src matlab
  G0 = 1.02; % [V/(m/s2)]

  G_acc = tf(G0);
#+end_src

#+begin_src matlab
  T = 276;
  xi = 0.5;
  w = 2*pi;

  G_geo = -T*s^2/(s^2 + 2*xi*w*s + w^2);
#+end_src

#+begin_src matlab
  acc_1 = lsim(inv(G_acc), acc_1, t);
  acc_2 = lsim(inv(G_acc), acc_2, t);
  geo_1 = lsim(inv(G_geo), geo_1, t);
  geo_2 = lsim(inv(G_geo), geo_2, t);
#+end_src

** Compare Time Domain Signals
#+begin_src matlab
  figure;
  hold on;
  plot(t, acc_1);
  plot(t, acc_2);
  plot(t, geo_1);
  plot(t, geo_2);
  hold off;
#+end_src

** Compute PSD
We first define the parameters for the frequency domain analysis.
#+begin_src matlab :results none
  Fs = 1/dt; % [Hz]

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

Then we compute the Power Spectral Density using =pwelch= function.
#+begin_src matlab :results none
  [p_acc_1, f] = pwelch(acc_1, win, [], [], Fs);
  [p_acc_2, ~] = pwelch(acc_2, win, [], [], Fs);
  [p_geo_1, ~] = pwelch(geo_1, win, [], [], Fs);
  [p_geo_2, ~] = pwelch(geo_2, win, [], [], Fs);
#+end_src

#+begin_src matlab :results none
  figure;
  hold on;
  plot(f, sqrt(p_acc_1));
  plot(f, sqrt(p_acc_2));
  hold off;
  set(gca, 'xscale', 'log');
  set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('ASD Accelerometers $\left[\frac{m/s}{\sqrt{Hz}}\right]$')
  xlim([1, 5000]);
#+end_src

#+begin_src matlab :results none
  figure;
  hold on;
  plot(f, sqrt(p_geo_1));
  plot(f, sqrt(p_geo_2));
  hold off;
  set(gca, 'xscale', 'log');
  set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('ASD Geophones $\left[\frac{m/s}{\sqrt{Hz}}\right]$')
  xlim([1, 5000]);
#+end_src

** Dynamical Uncertainty

#+begin_src matlab :results none
  [T_acc, ~] = tfestimate(acc_1, acc_2, win, [], [], Fs);
  [T_geo, ~] = tfestimate(geo_1, geo_2, win, [], [], Fs);
#+end_src

#+begin_src matlab :results none :exports none
  figure;
  ax1 = subplot(2, 1, 1);
  hold on;
  plot(f, abs(T_acc), 'DisplayName', 'Accelerometers');
  plot(f, abs(T_geo), 'DisplayName', 'Geophones');
  hold off;
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude');
  legend('location', 'southeast');

  ax2 = subplot(2, 1, 2);
  hold on;
  plot(f, 180/pi*phase(T_acc));
  plot(f, 180/pi*phase(T_geo));
  hold off;
  set(gca, 'xscale', 'log');
  ylim([-180, 180]);
  yticks([-180, -90, 0, 90, 180]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');

  linkaxes([ax1,ax2],'x');
  xlim([1, 5000]);
#+end_src

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

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

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