#+TITLE: Tomography Experiment
:DRAWER:
#+STARTUP: overview

#+LANGUAGE: en
#+EMAIL: dehaeze.thomas@gmail.com
#+AUTHOR: Dehaeze Thomas

#+HTML_LINK_HOME: ../index.html
#+HTML_LINK_UP: ../index.html

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

#+HTML_MATHJAX: align: center tagside: right font: TeX

#+PROPERTY: header-args:matlab  :session *MATLAB*
#+PROPERTY: header-args:matlab+ :comments org
#+PROPERTY: header-args:matlab+ :results none
#+PROPERTY: header-args:matlab+ :exports both
#+PROPERTY: header-args:matlab+ :eval no-export
#+PROPERTY: header-args:matlab+ :output-dir figs
#+PROPERTY: header-args:matlab+ :tangle matlab/modal_frf_coh.m
#+PROPERTY: header-args:matlab+ :mkdirp yes

#+PROPERTY: header-args:shell  :eval no-export

#+PROPERTY: header-args:latex  :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{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
:END:

* 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
  simulinkproject('../');
#+end_src

#+begin_src matlab
  open 'simscape/sim_nano_station_tomo.slx'
#+end_src

* Initialize Experiment
#+begin_src matlab
  initializeGround();
  initializeGranite();
  initializeTy();
  initializeRy();
  initializeRz();
  initializeMicroHexapod();
  initializeAxisc();
  initializeMirror();
  initializeNanoHexapod(struct('actuator', 'piezo'));
  initializeSample(struct('mass', 1));
#+end_src

#+begin_src matlab
  t = 0:Ts:Tsim;
#+end_src

Generate perturbations
#+begin_src matlab
  win = hanning(ceil(1/Ts));
  [pxx, f] = pwelch(sqrt(1/(2*Ts))*randn(length(t), 1), win, [], [], 1/Ts);
#+end_src

#+begin_src matlab
  figure;
  hold on;
  plot(f, sqrt(pxx));
  hold off;
  set(gca, 'xscale', 'log');
  set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('ASD of the measured velocity $\left[\frac{m/s}{\sqrt{Hz}}\right]$')
  ylim([0.01, 100]);
#+end_src

#+begin_src matlab
  load('./disturbances/mat/dist_psd.mat', 'dist_f');

  Dwx = lsim(dist_f.G_gm, sqrt(1/(2*Ts))*randn(length(t), 1), t);
  Dwy = lsim(dist_f.G_gm, sqrt(1/(2*Ts))*randn(length(t), 1), t);
  Dwz = lsim(dist_f.G_gm, sqrt(1/(2*Ts))*randn(length(t), 1), t);

  Dw = [Dwx, Dwy, Dwz];
#+end_src

#+begin_src matlab
  figure;
  hold on;
  plot(t, Dwx);
  plot(t, Dwy);
  plot(t, Dwz);
  hold off;
  xlabel('Time [s]'); ylabel('Displacement [m]');
#+end_src

#+begin_src matlab
  Fty_z = lsim(dist_f.G_ty, sqrt(1/(2*Ts))*randn(length(t), 1), t);
  Frz_z = lsim(dist_f.G_rz, sqrt(1/(2*Ts))*randn(length(t), 1), t);
#+end_src

#+begin_src matlab
  figure;
  hold on;
  plot(t, Fty_z);
  plot(t, Frz_z);
  hold off;
  xlabel('Time [s]'); ylabel('Force [N]');
#+end_src

#+begin_src matlab
  % Spindle
  Rz = 2*pi*t;

  % Axisc
  Rm = zeros(length(t), 2);
  Rm(:, 2) = pi*ones(length(t), 1);

  inputs = struct( ...
      'Ts', Ts,       ...
      'Dw', timeseries(Dw, t), ...
      'Dy', timeseries(zeros(length(t), 1), t), ...
      'Ry', timeseries(zeros(length(t), 1), t), ...
      'Rz', timeseries(Rz, t), ...
      'Dh', timeseries(zeros(length(t), 6), t), ...
      'Rm', timeseries(Rm, t), ...
      'Dn', timeseries(zeros(length(t), 6), t), ...
      'Ds', timeseries(zeros(length(t), 6), t), ...
      'Fg', timeseries(zeros(length(t), 3), t), ...
      'Fn', timeseries(zeros(length(t), 6), t), ...
      'Fnl', timeseries(zeros(length(t), 6), t), ...
      'Fty_x', timeseries(zeros(length(t), 1), t), ...
      'Fty_z', timeseries(Fty_z, t), ...
      'Frz_z', timeseries(Frz_z, t), ...
      'Fs', timeseries(zeros(length(t), 6), t)  ...
  );
#+end_src

* Test
#+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
  cd ..
#+end_src

We define some parameters:
- =T0= is the duration in seconds
- =N= is the number of samples

#+begin_src matlab
  T0 = 10; % Signal Duration [s]
  N = 10000; % Number of Samples (should be even)
#+end_src

Then, we have:
- $f_s = N/T_0$ is the sampling frequency in Hz
- $f_c = \frac{1}{2} N/T_0$ is the cut-off frequency in Hz
- $f_0 = 1/T_0$ is the frequency resolution of the DFT in Hz
#+begin_src matlab
  Fs = N/T0; % Sampling frequency [Hz]
  Fc = (1/2)*N/T0; % Sampling frequency [Hz]
  df = 1/T0; % Frequency resolution of the DFT [Hz]
#+end_src

We then specify the wanted PSD.
#+begin_src matlab
  phi = ones(N/2, 1);
  % phi = logspace(3, 0, N/2);
  phi(df:df:Fs/2>10) = 0;
#+end_src

We create $C_x(k)$ such that:
\[ C_x(k) = \sqrt{\Phi_{xx}(k\omega_0)\omega_0} \quad k = 1 \dots N/2 \]
where $\Phi_{xx}$ is the wanted PSD.
#+begin_src matlab
  C = zeros(N/2, 1);
  for i = 1:N/2
    C(i) = sqrt(phi(i))*2*Fs; % TODO - Why this normalization?
  end
#+end_src

We generate some random phase that will be added to =C=.
#+begin_src matlab
  theta = 2*pi*rand(N/2, 1); % Generate random phase [rad]
#+end_src

In order to have
\[ C_x(N/2+i) = C_x^*(N/2-i) \quad i = 1 \dots N/2 \]
We do the following
#+begin_src matlab
  Cx = [0 ; C.*complex(cos(theta),sin(theta))];
  Cx = [Cx; flipud(conj(Cx(2:end)))];;
#+end_src

The time domain data is generated by an inverse FFT.
#+begin_src matlab
  u = ifft(Cx);
#+end_src

#+begin_src matlab
  A = fft(u);
  figure; plot(2*A.*conj(A))
#+end_src

#+begin_src matlab
  t = linspace(0, T0, N+1); % Time Vector [s]
#+end_src

#+begin_src matlab
  figure;
  plot(t, u)
  xlabel('Time [s]');
  ylabel('Amplitude');
#+end_src

#+begin_src matlab
  nx = length(u);
  na = 16;
  win = hanning(floor(nx/na));

  [pxx, f] = pwelch(u, win, 0, [], Fs);
#+end_src

#+begin_src matlab
  figure;
  hold on;
  plot(f,pxx)
  plot(df:df:Fc,phi)
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('Power Spectral Density');
  set(gca, 'xscale', 'log');
  set(gca, 'yscale', 'log');
#+end_src

* Test
#+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
  cd ..
#+end_src

#+begin_src matlab
  load('./disturbances/mat/dist_psd.mat', 'dist_f');
#+end_src

We remove the first value with very high PSD.
#+begin_src matlab
  dist_f.f = dist_f.f(3:end);
  dist_f.psd_gm = dist_f.psd_gm(3:end);
#+end_src

We define some parameters.
#+begin_src matlab
  Fs = 2*dist_f.f(end);   % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
  N = 2*length(dist_f.f); % Number of Samples match the one of the wanted PSD
  T0 = N/Fs;              % Signal Duration [s]
  df = 1/T0;              % Frequency resolution of the DFT [Hz]
                          % Also equal to (dist_f.f(2)-dist_f.f(1))
#+end_src

We then specify the wanted PSD.
#+begin_src matlab
  phi = dist_f.psd_gm;
#+end_src

Create amplitudes corresponding to wanted PSD.
#+begin_src matlab
  C = zeros(N/2,1);
  for i = 1:N/2
    C(i) = sqrt(phi(i)*df);
  end
#+end_src

Add random phase to =C=.
#+begin_src matlab
  theta = 2*pi*rand(N/2,1); % Generate random phase [rad]

  Cx = [0 ; C.*complex(cos(theta),sin(theta))];
  Cx = [Cx; flipud(conj(Cx(2:end)))];;
#+end_src

The time domain data is generated by an inverse FFT.
We normalize the =ifft= Matlab command.
#+begin_src matlab
  u = 1/(sqrt(2)*df*1/Fs)*ifft(Cx); % Normalisation of the IFFT
  t = linspace(0, T0, N+1); % Time Vector [s]
#+end_src

#+begin_src matlab
  figure;
  plot(t, u)
  xlabel('Time [s]');
  ylabel('Amplitude');
#+end_src

#+begin_src matlab
  u_rep = [u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u];
#+end_src

#+begin_src matlab
  nx = length(u_rep);
  na = 16;
  win = hanning(floor(nx/na));

  [pxx, f] = pwelch(u_rep, win, 0, [], Fs);
#+end_src

#+begin_src matlab
  figure;
  hold on;
  plot(dist_f.f, dist_f.psd_gm, 'DisplayName', 'Original PSD')
  plot(f, pxx, 'DisplayName', 'Computed')
  % plot(f, pxx./dist_f.psd_gm, 'k-')
  hold off;
  xlabel('Frequency [Hz]');
  ylabel('Power Spectral Density');
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  legend('location', 'northeast');
#+end_src