#+TITLE: ESRF Double Crystal Monochromator - Lookup Tables
:DRAWER:
#+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="https://research.tdehaeze.xyz/css/style.css"/>
#+HTML_HEAD: <script type="text/javascript" src="https://research.tdehaeze.xyz/js/script.js"></script>

#+BIND: org-latex-image-default-option "scale=1"
#+BIND: org-latex-image-default-width ""

#+LaTeX_CLASS: scrreprt
#+LaTeX_CLASS_OPTIONS: [a4paper, 10pt, DIV=12, parskip=full]
#+LaTeX_HEADER_EXTRA: \input{preamble.tex}
#+LATEX_HEADER_EXTRA: \bibliography{ref}

#+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+ :tangle no
#+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

#+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 file raw replace
#+PROPERTY: header-args:latex+ :buffer no
#+PROPERTY: header-args:latex+ :tangle no
#+PROPERTY: header-args:latex+ :eval no-export
#+PROPERTY: header-args:latex+ :exports results
#+PROPERTY: header-args:latex+ :mkdirp yes
#+PROPERTY: header-args:latex+ :output-dir figs
#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png")
:END:

#+begin_export html
  <hr>
  <p>This report is also available as a <a href="./dcm_lookup_tables.pdf">pdf</a>.</p>
  <hr>
#+end_export

#+latex: \clearpage

* Introduction                                                        :ignore:

Several Lookup Tables (LUT) are used for the DCM in order to compensate for *repeatable* errors.
- Section [[sec:dcm_stepper_lut]]: the stepper motors are calibrated using interferometers.
- Section [[sec:dcm_attocube_lut]]: the Attocube periodic non-linearities are calibrated using piezoelectric actuators.

* Stepper Motors Calibration
:PROPERTIES:
:header-args:matlab+: :tangle matlab/dcm_stepper_lut.m
:END:
<<sec:dcm_stepper_lut>>
** Introduction                                                      :ignore:
Fast jack coarse displacement is performed with a Stepper motor and a ball screw mechanism.
Such positioning system has some repeatable motion errors than can be calibrated using a measurement system having less errors.

For the DCM, this can be done using the interferometers.

** 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 :noweb yes
<<m-init-path>>
#+end_src

#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle>>
#+end_src

#+begin_src matlab :noweb yes
<<m-init-other>>
#+end_src

** Schematic

In order to measure the errors induced by the fast jacks, we have to make some scans, and measure simultaneously:
- The wanted fast jack position: signal/step sent by the IcePAP
- The actual (measured) position

The experimental setup to perform this is shown in Figure [[fig:block_diagram_lut_stepper]].

The procedure is the following:
- A scan on the Bragg angle $\theta$ is generated from Bliss
- Reference paths $[r_{u_r},\ r_{u_h},\ r_{d}]$ are sent to the IcePAP
- Initially, the LUT inside the IcePAP is not changing the reference path
- The IcePAP generates some steps $[u_{u_r},\ u_{u_h},\ u_{d}]$ that are sent to the fast jacks
- The motion of the crystals $[d_z,\ r_y,\ r_x]$ is measured with the interferometers and computed in the Speedgoat
- Finally, the corresponding motion $[d_{u_r},\ r_{u_h},\ r_d]$ of the fast jack is computed afterwards in BLISS

The measured motion of the fast jacks $[d_{u_r},\ r_{u_h},\ r_d]$ can be compared with the IcePAP steps $[u_{u_r},\ u_{u_h},\ u_{d}]$ in order to create the LUT inside the IcePAP.

#+begin_src latex :file block_diagram_lut_stepper.pdf
\definecolor{myblue}{rgb}{0, 0.447, 0.741}
\definecolor{myred}{rgb}{0.8500, 0.325, 0.098}
\definecolor{mygreen}{rgb}{0.639, 0.745, 0.549}
\definecolor{myyellow}{rgb}{0.922, 0.796, 0.545}

\begin{tikzpicture}
  % Blocks
  \node[block={1.5cm}{2.9cm}] (traj) at (0,0){$\frac{d_{\text{off}}}{2 \cos \theta}$};

  \node[block={1.5cm}{0.8cm}, right=1.1   of traj] (lut_uh) {LUT $u_h$};
  \node[block={1.5cm}{0.8cm}, above=0.2 of lut_uh] (lut_ur) {LUT $u_r$};
  \node[block={1.5cm}{0.8cm}, below=0.2 of lut_uh] (lut_d)  {LUT $d$};

  \node[block={1.5cm}{0.8cm}, right=1.1 of lut_ur] (fj_ur) {FJ $u_h$};
  \node[block={1.5cm}{0.8cm}, right=1.1 of lut_uh] (fj_uh) {FJ $u_r$};
  \node[block={1.5cm}{0.8cm}, right=1.1 of lut_d]  (fj_d)  {FJ $d$};

  \node[block={1.5cm}{2.9cm}, right=0.2 of fj_uh] (int) {\rotatebox{90}{Interferometers}};

  \node[block={1.5cm}{2.9cm}, right=0.6 of int] (Js) {\rotatebox{90}{\parbox[c]{2.0cm}{\centering Forward Kinematics}}};

  \node[block={1.5cm}{2.9cm}, right=1.1 of Js] (Ja) {\rotatebox{90}{\parbox[c]{2.0cm}{\centering Inverse Kinematics}}};

  % Signals
  \draw[->] ($(traj.west) + (-0.7, 0)$)node[above right]{$\theta$} -- (traj.west);

  \draw[->] (lut_ur-|traj.east) --node[midway, above]{$r_{u_r}$} (lut_ur.west);
  \draw[->] (lut_uh-|traj.east) --node[midway, above]{$r_{u_h}$} (lut_uh.west);
  \draw[->] (lut_d -|traj.east) --node[midway, above]{$r_{d}$}   (lut_d.west);

  \draw[->] (lut_ur.east) --node[midway, above]{$u_{u_r}$} (fj_ur.west);
  \draw[->] (lut_uh.east) --node[midway, above]{$u_{u_h}$} (fj_uh.west);
  \draw[->] (lut_d.east)  --node[midway, above]{$u_{d}$}   (fj_d.west);

  \draw[->] (int.east) -- (Js.west);

  \draw[->] (Js.east|-fj_ur) --node[midway, above]{$d_z$} (Ja.west|-fj_ur);
  \draw[->] (Js.east|-fj_uh) --node[midway, above]{$r_y$} (Ja.west|-fj_uh);
  \draw[->] (Js.east|-fj_d)  --node[midway, above]{$r_x$} (Ja.west|-fj_d) ;

  \draw[->] (Ja.east|-fj_ur) -- ++(1.0, 0)node[above left]{$d_{u_r}$};
  \draw[->] (Ja.east|-fj_uh) -- ++(1.0, 0)node[above left]{$d_{u_h}$};
  \draw[->] (Ja.east|-fj_d)  -- ++(1.0, 0)node[above left]{$d_{d}$};

  \begin{scope}[on background layer]
    \node[fit={(fj_d.south west) (int.north east)}, fill=myblue!20!white, draw, inner sep=6pt] (dcm) {};
    \node[fit={(Js.south west) (Js.north east)}, fill=myyellow!20!white, draw, inner sep=6pt] (speedgoat) {};
    \node[fit={(lut_d.south west) (lut_ur.north east)}, fill=myred!20!white, draw, inner sep=6pt] (icepap) {};
    \node[fit={(traj.south west) (traj.north east)}, fill=mygreen!20!white, draw, inner sep=6pt] (bliss_1) {};
    \node[fit={(Ja.south west) (Ja.north east)}, fill=mygreen!20!white, draw, inner sep=6pt] (bliss_2) {};
    \node[above] at (dcm.north) {DCM};
    \node[above] at (speedgoat.north) {Speedgoat};
    \node[above] at (icepap.north) {IcePAP};
    \node[above] at (bliss_1.north) {BLISS};
    \node[above] at (bliss_2.north) {BLISS};
  \end{scope}
\end{tikzpicture}
#+end_src

#+name: fig:block_diagram_lut_stepper
#+caption: Block diagram of the experiment to create the Lookup Table
#+RESULTS:
[[file:figs/block_diagram_lut_stepper.png]]

** TODO Repeatability of the motion                                :noexport:

** Patterns in the Fast Jack motion errors
In order to understand what should be the "sampling distance" for the lookup table of the stepper motor, we have to analyze the displacement errors induced by the stepper motor.

Let's load the measurements of one bragg angle scan without any LUT.
#+begin_src matlab
%% Load Data of the new LUT method
ol_bragg = (pi/180)*1e-5*double(h5read('Qutools_test_0001.h5','/33.1/instrument/trajmot/data')); % Bragg angle [rad]
ol_dzw   = 10.5e-3./(2*cos(ol_bragg)); % Wanted distance between crystals [m]

ol_dz    = 1e-9*double(h5read('Qutools_test_0001.h5','/33.1/instrument/xtal_111_dz_filter/data'));  % Dz distance between crystals [m]
ol_dry   = 1e-9*double(h5read('Qutools_test_0001.h5','/33.1/instrument/xtal_111_dry_filter/data')); % Ry [rad]
ol_drx   = 1e-9*double(h5read('Qutools_test_0001.h5','/33.1/instrument/xtal_111_drx_filter/data')); % Rx [rad]

ol_t = 1e-6*double(h5read('Qutools_test_0001.h5','/33.1/instrument/time/data')); % Time [s]

ol_ddz   = ol_fj-ol_dz; % Distance Error between crystals [m]
#+end_src

#+begin_src matlab :exports none
%% Orientation and Distance error of the Crystal measured by the interferometers
figure;
hold on;
plot(180/pi*ol_bragg, 1e6*ol_drx, 'DisplayName', '$R_x$')
plot(180/pi*ol_bragg, 1e6*ol_dry, 'DisplayName', '$R_y$')
hold off;
legend('location', 'northwest');
xlabel('Bragg Angle [deg]'); ylabel('Angle Error [$\mu$rad]');
yyaxis right
plot(180/pi*ol_bragg, 1e6*ol_ddz, 'DisplayName', '$\epsilon D_z$')
ylabel('Distance Error [$\mu$m]');
#+end_src

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

#+name: fig:exp_without_lut_xtal_pos_errors
#+caption: Orientation and Distance error of the Crystal measured by the interferometers
#+RESULTS:
[[file:figs/exp_without_lut_xtal_pos_errors.png]]

Now let's convert the errors from the frame of the crystal to the frame of the fast jacks (inverse kinematics problem) using the Jacobian matrix.
#+begin_src matlab
%% Compute Fast Jack position errors
% Jacobian matrix for Fast Jacks and 111 crystal
J_a_111 = [1,  0.14, -0.1525
           1,  0.14,  0.0675
           1, -0.14,  0.0425];

ol_de_111 = [ol_ddz'; ol_dry'; ol_drx'];

% Fast Jack position errors
ol_de_fj = J_a_111*ol_de_111;

ol_fj_ur = ol_de_fj(1,:);
ol_fj_uh = ol_de_fj(2,:);
ol_fj_d  = ol_de_fj(3,:);
#+end_src

#+begin_src matlab :exports none
%% Fast Jack position errors
figure;
hold on;
plot(180/pi*ol_bragg, 1e6*ol_fj_ur, 'DisplayName', '$\epsilon_{u_r}$')
plot(180/pi*ol_bragg, 1e6*ol_fj_uh, 'DisplayName', '$\epsilon_{u_h}$')
plot(180/pi*ol_bragg, 1e6*ol_fj_d , 'DisplayName', '$\epsilon_{d}$')
hold off;
xlabel('Bragg Angle [deg]'); ylabel('Distance Error [$\mu$m]');
legend('location', 'northwest');
#+end_src

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

#+name: fig:exp_without_lut_fj_pos_errors
#+caption: Estimated motion errors of the fast jacks during the scan
#+RESULTS:
[[file:figs/exp_without_lut_fj_pos_errors.png]]

Let's now identify this pattern as a function of the fast-jack position.

As we want to done frequency Fourier transform, we need to have uniform sampling along the fast jack position.
To do so, the function =resample= is used.
#+begin_src matlab
Xs = 0.1e-6; % Sampling Distance [m]

%% Re-sampled data with uniform spacing [m]
ol_fj_ur_u = resample(ol_fj_ur, ol_dzw, 1/Xs);
ol_fj_uh_u = resample(ol_fj_uh, ol_dzw, 1/Xs);
ol_fj_d_u  = resample(ol_fj_d,  ol_dzw, 1/Xs);

ol_fj_u = Xs*[1:length(ol_fj_ur_u)]; % Sampled Jack Position
#+end_src

#+begin_src matlab :exports none
% Only take first 500um
ol_fj_ur_u = ol_fj_ur_u(ol_fj_u<0.5e-3);
ol_fj_uh_u = ol_fj_uh_u(ol_fj_u<0.5e-3);
ol_fj_d_u  = ol_fj_d_u (ol_fj_u<0.5e-3);
ol_fj_u    = ol_fj_u   (ol_fj_u<0.5e-3);
#+end_src

The result is shown in Figure [[fig:exp_without_lut_fj_pos_errors_distance]].

#+begin_src matlab :exports none
%% Fast Jack position errors
figure;
hold on;
plot(1e3*ol_fj_u, 1e6*ol_fj_ur_u, 'DisplayName', '$\epsilon_{u_r}$')
plot(1e3*ol_fj_u, 1e6*ol_fj_uh_u, 'DisplayName', '$\epsilon_{u_h}$')
plot(1e3*ol_fj_u, 1e6*ol_fj_d_u , 'DisplayName', '$\epsilon_{d}$')
hold off;
xlabel('Fast Jack Position [mm]'); ylabel('Distance Error [$\mu$m]');
legend('location', 'northwest');
#+end_src

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

#+name: fig:exp_without_lut_fj_pos_errors_distance
#+caption: Position error of fast jacks as a function of the fast jack motion
#+RESULTS:
[[file:figs/exp_without_lut_fj_pos_errors_distance.png]]

Let's now perform a Power Spectral Analysis of the measured displacement errors of the Fast Jack.
#+begin_src matlab
% Hanning Windows with 250um width
win = hanning(floor(400e-6/Xs));

% Power Spectral Density [m2/(1/m)]
[S_fj_ur, f] = pwelch(ol_fj_ur_u-mean(ol_fj_ur_u), win, 0, [], 1/Xs);
[S_fj_uh, ~] = pwelch(ol_fj_uh_u-mean(ol_fj_uh_u), win, 0, [], 1/Xs);
[S_fj_d,  ~] = pwelch(ol_fj_d_u -mean(ol_fj_d_u ),  win, 0, [], 1/Xs);
#+end_src

As shown in Figure [[fig:exp_without_lut_wavenumber_asd]], we can see a fundamental "reciprocal length" of $5 \cdot 10^4\,[1/m]$ and its harmonics.
This corresponds to a length of $\frac{1}{5\cdot 10^4} = 20\,[\mu m]$.

#+begin_src matlab :exports none
%%
figure;
hold on;
plot(f, sqrt(S_fj_ur), 'DisplayName', '$u_r$');
plot(f, sqrt(S_fj_uh), 'DisplayName', '$u_h$');
plot(f, sqrt(S_fj_d), 'DisplayName', '$d$');
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Reciprocal Length [1/m]');
ylabel('ASD [$\frac{m}{1/\sqrt{m}}$]')
legend('location', 'northeast');
xlim([1e4, 2e6]); ylim([1e-13, 2e-9]);
#+end_src

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

#+name: fig:exp_without_lut_wavenumber_asd
#+caption: Spectral content of the error as a function of the reciprocal length
#+RESULTS:
[[file:figs/exp_without_lut_wavenumber_asd.png]]



Instead of looking at that as a function of the reciprocal length, we can look at it as a function of the spectral distance (Figure [[fig:exp_without_lut_spectral_content_fj_error]]).

We see that the errors have a pattern with "spectral distances" equal to $5\,[\mu m]$, $10\,[\mu m]$, $20\,[\mu m]$ and smaller harmonics.
#+begin_src matlab :exports none
figure;
hold on;
plot(1e6./f, sqrt(S_fj_ur), 'DisplayName', '$u_r$');
plot(1e6./f, sqrt(S_fj_uh), 'DisplayName', '$u_h$');
plot(1e6./f, sqrt(S_fj_d), 'DisplayName', '$d$');
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Spectral Distance [$\mu$m]');
ylabel('Spectral Content [$\frac{m}{1/\sqrt{m}}$]')
legend('location', 'northwest');
xlim([0.5, 200]); ylim([1e-12, 1e-8]);
#+end_src

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

#+name: fig:exp_without_lut_spectral_content_fj_error
#+caption: Spectral content of the error as a function of the spectral distance
#+RESULTS:
[[file:figs/exp_without_lut_spectral_content_fj_error.png]]

Let's try to understand these results.
One turn of the stepper motor corresponds to a vertical motion of 1mm.
The stepper motor has 50 pairs of poles, therefore one pair of pole corresponds to a motion of $20\,[\mu m]$ which is the fundamental "spectral distance" we observe.

#+begin_src matlab
CPS_ur = flip(-cumtrapz(flip(f), flip(S_fj_ur)));
CPS_uh = flip(-cumtrapz(flip(f), flip(S_fj_uh)));
CPS_d  = flip(-cumtrapz(flip(f), flip(S_fj_d)));
#+end_src

From Figure [[fig:exp_without_lut_cas_pos_error]], we can see that if the motion errors with a period of $5\,[\mu m]$ and $10\,[\mu m]$ can be dealt with the lookup table, this will reduce a lot the positioning errors of the fast jack.

#+begin_src matlab :results none
%% Cumulative Spectrum
figure;
hold on;
plot(1e6./f, sqrt(CPS_ur), 'DisplayName', '$u_r$');
plot(1e6./f, sqrt(CPS_uh), 'DisplayName', '$u_j$');
plot(1e6./f, sqrt(CPS_d),  'DisplayName', '$d$');
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
xlabel('Spectral Distance [$\mu m$]'); ylabel('Cumulative Spectrum [$m$]')
xlim([1, 500]); ylim([1e-9, 1e-5]);
legend('location', 'northwest');
#+end_src

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

#+name: fig:exp_without_lut_cas_pos_error
#+caption: Cumulative spectrum from small spectral distances to large spectral distances
#+RESULTS:
[[file:figs/exp_without_lut_cas_pos_error.png]]

** Experimental Data - Current Method
The current used method is an iterative one.

#+begin_src matlab
%% Load Experimental Data
ol_bragg    = double(h5read('first_beam_0001.h5','/31.1/instrument/trajmot/data'));
ol_drx      = h5read('first_beam_0001.h5','/31.1/instrument/xtal_111_drx_filter/data');

lut_1_bragg = double(h5read('first_beam_0001.h5','/32.1/instrument/trajmot/data'));
lut_1_drx   = h5read('first_beam_0001.h5','/32.1/instrument/xtal_111_drx_filter/data');

lut_2_bragg = double(h5read('first_beam_0001.h5','/33.1/instrument/trajmot/data'));
lut_2_drx   = h5read('first_beam_0001.h5','/33.1/instrument/xtal_111_drx_filter/data');

lut_3_bragg = double(h5read('first_beam_0001.h5','/34.1/instrument/trajmot/data'));
lut_3_drx   = h5read('first_beam_0001.h5','/34.1/instrument/xtal_111_drx_filter/data');

lut_4_bragg = double(h5read('first_beam_0001.h5','/36.1/instrument/trajmot/data'));
lut_4_drx   = h5read('first_beam_0001.h5','/36.1/instrument/xtal_111_drx_filter/data');
#+end_src

The relative orientation of the two =111= mirrors in the $x$ directions are compared in Figure [[fig:lut_old_method_exp_data]] for several iterations.
We can see that after the first iteration, the orientation error has an opposite sign as for the case without LUT.

#+begin_src matlab :exports none
%% Plot Drx for all the LUT iterations
figure;
hold on;
plot(1e-5*ol_bragg,    1e-3*ol_drx   , ...
     'DisplayName', sprintf('$i=0$, $\\delta_{R_x} = %.0f$ [nrad rms]', rms(ol_drx-mean(ol_drx))))
plot(1e-5*lut_1_bragg, 1e-3*lut_1_drx, ...
     'DisplayName', sprintf('$i=1$, $\\delta_{R_x} = %.0f$ [nrad rms]', rms(lut_1_drx-mean(lut_1_drx))))
plot(1e-5*lut_2_bragg, 1e-3*lut_2_drx, ...
     'DisplayName', sprintf('$i=2$, $\\delta_{R_x} = %.0f$ [nrad rms]', rms(lut_2_drx-mean(lut_2_drx))))
plot(1e-5*lut_4_bragg, 1e-3*lut_4_drx, ...
     'DisplayName', sprintf('$i=4$, $\\delta_{R_x} = %.0f$ [nrad rms]', rms(lut_4_drx-mean(lut_4_drx))))
hold off;
xlabel('Bragg Angle [deg]'); ylabel('$R_x$ error [$\mu$rad]');
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
#+end_src

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

#+name: fig:lut_old_method_exp_data
#+caption: $R_x$ error with the current LUT method
#+RESULTS:
[[file:figs/lut_old_method_exp_data.png]]

** Simulation
In this section, we suppose that we are in the frame of one fast jack (all transformations are already done), and we wish to create a LUT for one fast jack.

Let's say with make a Bragg angle scan between 10deg and 60deg during 100s.
#+begin_src matlab
Fs = 10e3; % Sample Frequency [Hz]
t = 0:1/Fs:10; % Time vector [s]
theta = linspace(10, 40, length(t)); % Bragg Angle [deg]
#+end_src

The IcePAP steps are following the theoretical formula:
\begin{equation}
d_z = \frac{d_{\text{off}}}{2 \cos \theta}
\end{equation}
with $\theta$ the bragg angle and $d_{\text{off}} = 10\,mm$.

The motion to follow is then:
#+begin_src matlab
perfect_motion = 10e-3./(2*cos(theta*pi/180)); % Perfect motion [m]
#+end_src

And the IcePAP is generated those steps:
#+begin_src matlab
icepap_steps = perfect_motion; % IcePAP steps measured by Speedgoat [m]
#+end_src

#+begin_src matlab :exports none
%% Steps as a function of the bragg angle
figure;
plot(theta, icepap_steps);
xlabel('Bragg Angle [deg]'); ylabel('IcePAP Steps [m]');
#+end_src

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

#+name: fig:bragg_angle_icepap_steps_idealized
#+caption: IcePAP Steps as a function of the Bragg Angle
#+RESULTS:
[[file:figs/bragg_angle_icepap_steps_idealized.png]]

Then, we are measuring the motion of the Fast Jack using the Interferometer.
The motion error is larger than in reality to be angle to see it more easily.
#+begin_src matlab
motion_error = 100e-6*sin(2*pi*perfect_motion/1e-3); % Error motion [m]

measured_motion = perfect_motion + motion_error; % Measured motion of the Fast Jack [m]
#+end_src

#+begin_src matlab :exports none
%% Measured Motion and Idealized Motion
figure;
hold on;
plot(icepap_steps, measured_motion, ...
    'DisplayName', 'Measured Motion');
plot(icepap_steps, perfect_motion, 'k--', ...
    'DisplayName', 'Ideal Motion');
hold off;
xlabel('IcePAP Steps [m]'); ylabel('Measured Motion [m]');
legend('location', 'southeast');
#+end_src

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

#+name: fig:measured_and_ideal_motion_fast_jacks
#+caption: Measured motion as a function of the IcePAP Steps
#+RESULTS:
[[file:figs/measured_and_ideal_motion_fast_jacks.png]]

Let's now compute the lookup table.
For each micrometer of the IcePAP step, another step is associated that correspond to a position closer to the wanted position.

#+begin_src matlab
%% Get range for the LUT
% We correct only in the range of tested/measured motion
lut_range = round(1e6*min(icepap_steps)):round(1e6*max(icepap_steps)); % IcePAP steps [um]

%% Initialize the LUT
lut = zeros(size(lut_range));

%% For each um in this range
for i = 1:length(lut_range)
    % Get points indices where the measured motion is closed to the wanted one
    close_points = measured_motion > 1e-6*lut_range(i) - 500e-9 & measured_motion < 1e-6*lut_range(i) + 500e-9;
    % Get the corresponding closest IcePAP step
    lut(i) = round(1e6*mean(icepap_steps(close_points))); % [um]
end
#+end_src

#+begin_src matlab :exports none
%% Generated Lookup Table
figure;
plot(lut_range, lut);
xlabel('IcePAP input step [um]'); ylabel('Lookup Table output [um]');
#+end_src

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

#+name: fig:generated_lut_icepap
#+caption: Generated Lookup Table
#+RESULTS:
[[file:figs/generated_lut_icepap.png]]

The current LUT implementation is the following:
#+begin_src matlab
motion_error_lut = zeros(size(lut_range));
for i = 1:length(lut_range)
    % Get points indices where the icepap step is close to the wanted one
    close_points = icepap_steps > 1e-6*lut_range(i) - 500e-9 & icepap_steps < 1e-6*lut_range(i) + 500e-9;
    % Get the corresponding motion error
    motion_error_lut(i) = lut_range(i) + (lut_range(i) - round(1e6*mean(measured_motion(close_points)))); % [um]
end
#+end_src

Let's compare the two Lookup Table in Figure [[fig:lut_comparison_two_methods]].
#+begin_src matlab :exports none
%% Comparison of the two Generated Lookup Table
figure;
hold on;
plot(lut_range, lut, ...
    'DisplayName', 'New LUT');
plot(lut_range, motion_error_lut, ...
    'DisplayName', 'Old LUT');
hold off;
xlabel('IcePAP input step [um]'); ylabel('Lookup Table output [um]');
legend('location', 'southeast');
#+end_src

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

#+name: fig:lut_comparison_two_methods
#+caption: Comparison of the two lookup tables
#+RESULTS:
[[file:figs/lut_comparison_two_methods.png]]

If we plot the "corrected steps" for all steps for both methods, we clearly see the difference (Figure [[fig:lut_correct_and_motion_error]]).

#+begin_src matlab :exports none
%% Corrected motion and motion error at each step position
figure;
hold on;
plot(lut_range, lut-lut_range, ...
    'DisplayName', 'New LUT');
plot(lut_range, motion_error_lut-lut_range, ...
    'DisplayName', 'Old LUT');
hold off;
xlabel('IcePAP Steps [um]'); ylabel('Corrected motion [um]');
ylim([-110, 110])
legend('location', 'southeast');
#+end_src

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

#+name: fig:lut_correct_and_motion_error
#+caption: LUT correction and motion error as a function of the IcePAP steps
#+RESULTS:
[[file:figs/lut_correct_and_motion_error.png]]

Let's now implement both LUT to see which implementation is correct.
#+begin_src matlab :exports none
icepap_steps_output_new = lut(round(1e6*icepap_steps)-lut_range(1)+1);
i = round(1e6*icepap_steps)-motion_error_lut(1)+1;
i(i>length(motion_error_lut)) = length(motion_error_lut);
icepap_steps_output_old = motion_error_lut(i);
#+end_src

#+begin_src matlab
motion_new = zeros(size(icepap_steps_output_new));
motion_old = zeros(size(icepap_steps_output_old));

for i = 1:length(icepap_steps_output_new)
    [~, i_step] = min(abs(icepap_steps_output_new(i) - 1e6*icepap_steps));
    motion_new(i) = measured_motion(i_step);

    [~, i_step] = min(abs(icepap_steps_output_old(i) - 1e6*icepap_steps));
    motion_old(i) = measured_motion(i_step);
end
#+end_src

The output motion with both LUT are shown in Figure [[fig:compare_old_new_lut_motion]].
It is confirmed that the new LUT is the correct one.
Also, it is interesting to note that the old LUT gives an output motion that is above the ideal one, as was seen during the experiments.
#+begin_src matlab :exports none
%% Measured Motion and Idealized Motion
% Use only middle motion where the LUT is working
i = round(0.1*length(icepap_steps)):round(0.9*length(icepap_steps));
figure;
hold on;
plot(icepap_steps(i), motion_new(i), ...
    'DisplayName', 'Motion (new LUT)');
plot(icepap_steps(i), motion_old(i), ...
    'DisplayName', 'Motion (old LUT)');
plot(icepap_steps(i), perfect_motion(i), 'k--', ...
    'DisplayName', 'Ideal Motion');
hold off;
xlabel('IcePAP Steps [m]'); ylabel('Measured Motion [m]');
legend('location', 'southeast');
#+end_src

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

#+name: fig:compare_old_new_lut_motion
#+caption: Comparison of the obtained motion with new and old LUT
#+RESULTS:
[[file:figs/compare_old_new_lut_motion.png]]

** Experimental Data - Proposed method
The new proposed method has been implemented and tested.

The result is shown in Figure [[fig:lut_comp_old_new_experiment]].
After only one iteration, the result is close to the previous method.

#+begin_src matlab
%% Load Data of the new LUT method
ol_new_bragg = double(h5read('Qutools_test_0001.h5','/33.1/instrument/trajmot/data'));
ol_new_drx   = h5read('Qutools_test_0001.h5','/33.1/instrument/xtal_111_drx_filter/data');

lut_new_bragg = double(h5read('Qutools_test_0001.h5','/34.1/instrument/trajmot/data'));
lut_new_drx   = h5read('Qutools_test_0001.h5','/34.1/instrument/xtal_111_drx_filter/data');
#+end_src

#+begin_src matlab :exports none
%% Plot Drx for new and old method
figure;
hold on;
plot(1e-5*ol_new_bragg,    1e-3*ol_new_drx   , ...
     'DisplayName', sprintf('$i=0$, $\\delta_{R_x} = %.0f$ [nrad rms]', rms(ol_new_drx-mean(ol_new_drx))))
plot(1e-5*lut_new_bragg, 1e-3*lut_new_drx, ...
     'DisplayName', sprintf('New LUT $i=1$, $\\delta_{R_x} = %.0f$ [nrad rms]', rms(lut_new_drx-mean(lut_new_drx))))
plot(1e-5*lut_4_bragg, 1e-3*lut_4_drx, ...
     'DisplayName', sprintf('Old LUT $i=4$, $\\delta_{R_x} = %.0f$ [nrad rms]', rms(lut_4_drx-mean(lut_4_drx))))
hold off;
xlabel('Bragg Angle [deg]'); ylabel('$R_x$ error [$\mu$rad]');
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
#+end_src

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

#+name: fig:lut_comp_old_new_experiment
#+caption: Comparison of the $R_x$ error for the current LUT method and the proposed one
#+RESULTS:
[[file:figs/lut_comp_old_new_experiment.png]]

If we zoom on the 20deg to 25deg bragg angles, we can see that the new method has much less "periodic errors" as compared to the previous one which shows some patterns.

#+begin_src matlab :exports none
%% Plot Drx for new and old method
figure;
hold on;
set(gca,'ColorOrderIndex',2)
plot(1e-5*lut_new_bragg, lut_new_drx, ...
     'DisplayName', 'New LUT')
plot(1e-5*lut_4_bragg, lut_4_drx, ...
     'DisplayName', 'Old LUT')
hold off;
xlabel('Bragg Angle [deg]'); ylabel('$R_x$ error [nrad]');
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
xlim([20, 25]);
#+end_src

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

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


* Attocube Calibration
:PROPERTIES:
:header-args:matlab+: :tangle matlab/dcm_attocube_lut.m
:END:
<<sec:dcm_attocube_lut>>
** Introduction                                                      :ignore:

The idea is to calibrate the periodic non-linearity of the interferometers, a known displacement must be imposed and the interferometer output compared to this displacement.
This should be performed over several periods in order to characterize the error.

We here suppose that we are already in the frame of the Attocube (the fast-jack displacements are converted to Attocube displacement using the transformation matrices).
We also suppose that we are at a certain Bragg angle, and that the stepper motors are not moving: only the piezoelectric actuators are used.

The setup is schematically with the block diagram in Figure [[fig:block_diagram_lut_attocube]].
The signals are:
- $u$: Actuator Signal (position where we wish to go)
- $d$: Disturbances affecting the signal
- $y$: Displacement of the crystal
- $y_g$: Measurement of the crystal motion by the strain gauge with some noise $n_g$
- $y_a$: Measurement of the crystal motion by the interferometer with some noise $n_a$

#+begin_src latex :file block_diagram_lut_attocube.pdf
\definecolor{myblue}{rgb}{0, 0.447, 0.741}
\definecolor{myred}{rgb}{0.8500, 0.325, 0.098}

\begin{tikzpicture}
  \node[block] (G) at (0,0){$G(s)$};
  \node[addb, right=1 of G] (addd) {};
  \node[block, align=center, right=1 of addd] (non_linearity) {Periodic\\Non-linearity};
  \node[addb, right=1 of non_linearity] (addna) {};
  \node[addb, below=1.8 of addna] (addnsg) {};

  \draw[->] ($(G.west) + (-1.0, 0)$) node[above right]{$u$} -- (G.west);
  \draw[->] (G.east) -- (addd.west);
  \draw[->] (addd.east) -- (non_linearity.west);
  \draw[->] ($(addd.north) + (0, 1.0)$) node[below right]{$d$} -- (addd.north);
  \draw[->] (non_linearity.east) -- (addna.west);
  \draw[->] (addna.east) -- ++(1.2, 0) node[above left]{$y_a$};
  \draw[->] ($(addna.north) + (0, 1.0)$) node[below right](na){$n_a$} -- (addna.north);
  \draw[->] ($(addd.east) + (0.4, 0)$)node[branch]{} node[above]{$y$} |- (addnsg.west);
  \draw[->] (addnsg.east) -- ++(1.2, 0) node[above left]{$y_g$};
  \draw[->] ($(addnsg.north) + (0, 1.0)$) node[below right](nsg){$n_{g}$} -- (addnsg.north);

  \begin{scope}[on background layer]
    \node[fit={(non_linearity.south west) (na.north east)}, fill=myblue!20!white, draw, inner sep=6pt] (attocube) {};
    \node[fit={(non_linearity.west|-addnsg.south) (nsg.north east)}, fill=myred!20!white, draw, inner sep=6pt] (straingauge) {};
    \node[below right] at (attocube.north west) {Attocube};
    \node[below right] at (straingauge.north west) {Strain Gauge};
  \end{scope}
\end{tikzpicture}
#+end_src

#+name: fig:block_diagram_lut_attocube
#+caption: Block Diagram schematic of the setup used to measure the periodic non-linearity of the Attocube
#+RESULTS:
[[file:figs/block_diagram_lut_attocube.png]]

The problem is to estimate the periodic non-linearity of the Attocube from the imperfect measurments $y_a$ and $y_g$.

Then a Lookup Table (LUT) is build.

The wavelength of the Attocube is 1530nm, therefore the non-linearity has a period of 765nm.
The amplitude of the non-linearity can vary from one unit to the other (and maybe from one experimental condition to the other).
It is typically between 5nm peak to peak and 20nm peak to peak.

** 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 :noweb yes
<<m-init-path>>
#+end_src

#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle>>
#+end_src

#+begin_src matlab :noweb yes
<<m-init-other>>
#+end_src



** Simulations
We have some constrains on the way the motion is imposed and measured:
- We want the frequency content of the imposed motion to be at low frequency in order not to induce vibrations of the structure.
  We have to make sure the forces applied by the piezoelectric actuator only moves the crystal and not the fast jack below.
  Therefore, we have to move much slower than the first resonance frequency in the system.
- As both $y_a$ and $y_g$ should have rather small noise, we have to filter them with low pass filters.
  The cut-off frequency of the low pass filter should be high as compared to the motion (to not induce any distortion) but still reducing sufficiently the noise.
  Let's say we want the noise to be less than 1nm ($6 \sigma$).

Suppose we have the power spectral density (PSD) of both $n_a$ and $n_g$.

- [ ] Take the PSD of the Attocube
- [ ] Take the PSD of the strain gauge
- [ ] Using 2nd order low pass filter, estimate the required low pass filter cut-off frequency to have sufficiently low noise

* Helping Functions                                                 :noexport:
** Initialize Path
#+NAME: m-init-path
#+BEGIN_SRC matlab
%% Path for functions, data and scripts
addpath('./matlab/mat/'); % Path for data
addpath('./matlab/');     % Path for scripts
#+END_SRC

#+NAME: m-init-path-tangle
#+BEGIN_SRC matlab
%% Path for functions, data and scripts
addpath('./mat/'); % Path for data
#+END_SRC

** Initialize Simscape Model
#+NAME: m-init-simscape
#+begin_src matlab
#+end_src

** Initialize other elements
#+NAME: m-init-other
#+BEGIN_SRC matlab
%% Colors for the figures
colors = colororder;

%% Frequency Vector
freqs = logspace(1, 3, 1000);
#+END_SRC

* Bibliography                                                        :ignore:
#+latex: \printbibliography
