dcm-stepper-calibration/dcm_lookup_tables.org

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

A Fast Jack is composed of one stepper motor and a piezoelectric stack in series.

The stepper motor is directly driving (i.e. without a reducer) a ball screw with a pitch of 1mm (i.e. 1 stepper motor turn makes a 1mm linear motion).

The stepper motor is doing the coarse displacement while the piezoelectric stack is only there to compensate all the motion errors of the stepper motor.

A Lookup Tables (LUT) is used to compensate for *repeatable* errors of the stepper motors.
This has several goals:
- Reduce errors down to the stroke of the piezoelectric stack actuator
- Reduce errors above the bandwidth of the feedback controller

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

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

** 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_dzw-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 (BLISS first implementation)
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: Comparison of the residual motion after old LUT and new LUT
#+RESULTS:
[[file:figs/lut_comp_old_new_experiment_zoom.png]]

** Comparison of the errors in the reciprocal length space
#+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'));
ol_dz    = 1e-9*double(h5read('Qutools_test_0001.h5','/33.1/instrument/xtal_111_dz_filter/data'));
ol_dry   = 1e-9*double(h5read('Qutools_test_0001.h5','/33.1/instrument/xtal_111_dry_filter/data'));
ol_drx   = 1e-9*double(h5read('Qutools_test_0001.h5','/33.1/instrument/xtal_111_drx_filter/data'));
ol_dzw   = 10.5e-3./(2*cos(ol_bragg)); % Wanted distance between crystals [m]
ol_t     = 1e-6*double(h5read('Qutools_test_0001.h5','/33.1/instrument/time/data')); % Time [s]
ol_ddz   = ol_dzw-ol_dz; % Distance Error between crystals [m]

lut_bragg = (pi/180)*1e-5*double(h5read('Qutools_test_0001.h5','/34.1/instrument/trajmot/data'));
lut_dz    = 1e-9*double(h5read('Qutools_test_0001.h5','/34.1/instrument/xtal_111_dz_filter/data'));
lut_dry   = 1e-9*double(h5read('Qutools_test_0001.h5','/34.1/instrument/xtal_111_dry_filter/data'));
lut_drx   = 1e-9*double(h5read('Qutools_test_0001.h5','/34.1/instrument/xtal_111_drx_filter/data'));
lut_dzw   = 10.5e-3./(2*cos(lut_bragg)); % Wanted distance between crystals [m]
lut_t     = 1e-6*double(h5read('Qutools_test_0001.h5','/34.1/instrument/time/data')); % Time [s]
lut_ddz   = lut_dzw-lut_dz; % Distance Error between crystals [m]
#+end_src

#+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,:);

lut_de_111 = [lut_ddz'; lut_dry'; lut_drx'];

% Fast Jack position errors
lut_de_fj = J_a_111*lut_de_111;

lut_fj_ur = lut_de_fj(1,:);
lut_fj_uh = lut_de_fj(2,:);
lut_fj_d  = lut_de_fj(3,:);
#+end_src

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

% 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

#+begin_src matlab
%% Re-sampled data with uniform spacing [m]
lut_fj_ur_u = resample(lut_fj_ur, lut_dzw, 1/Xs);
lut_fj_uh_u = resample(lut_fj_uh, lut_dzw, 1/Xs);
lut_fj_d_u  = resample(lut_fj_d,  lut_dzw, 1/Xs);

lut_fj_u = Xs*[1:length(lut_fj_ur_u)]; % Sampled Jack Position

% Only take first 500um
lut_fj_ur_u = lut_fj_ur_u(lut_fj_u<0.5e-3);
lut_fj_uh_u = lut_fj_uh_u(lut_fj_u<0.5e-3);
lut_fj_d_u  = lut_fj_d_u (lut_fj_u<0.5e-3);
lut_fj_u    = lut_fj_u   (lut_fj_u<0.5e-3);
#+end_src

#+begin_src matlab
% Hanning Windows with 250um width
win = hanning(floor(400e-6/Xs));

% Power Spectral Density [m2/(1/m)]
[S_ol_ur, f] = pwelch(ol_fj_ur_u-mean(ol_fj_ur_u), win, 0, [], 1/Xs);
[S_ol_uh, ~] = pwelch(ol_fj_uh_u-mean(ol_fj_uh_u), win, 0, [], 1/Xs);
[S_ol_d,  ~] = pwelch(ol_fj_d_u -mean(ol_fj_d_u ), win, 0, [], 1/Xs);

[S_lut_ur, ~] = pwelch(lut_fj_ur_u-mean(lut_fj_ur_u), win, 0, [], 1/Xs);
[S_lut_uh, ~] = pwelch(lut_fj_uh_u-mean(lut_fj_uh_u), win, 0, [], 1/Xs);
[S_lut_d,  ~] = pwelch(lut_fj_d_u -mean(lut_fj_d_u ), win, 0, [], 1/Xs);
#+end_src

As seen in Figure [[fig:effect_lut_on_psd_error_spatial]], the LUT as an effect only on spatial errors with a period of at least few $\mu m$.
This is very logical considering the $1\,\mu m$ sampling of the LUT in the IcePAP.

#+begin_src matlab :exports none
figure;
hold on;
plot(1e6./f, sqrt(S_ol_d) , 'DisplayName', '$u_r$ - OL');
plot(1e6./f, sqrt(S_lut_d), 'DisplayName', '$u_r$ - LUT');
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-13, 1e-8]);
#+end_src

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

#+name: fig:effect_lut_on_psd_error_spatial
#+caption: Effect of the LUT on the spectral content of the positioning errors
#+RESULTS:
[[file:figs/effect_lut_on_psd_error_spatial.png]]

Let's now look at it in a cumulative way.

#+begin_src matlab
CPS_ol_ur = flip(-cumtrapz(flip(f), flip(S_ol_ur)));
CPS_ol_uh = flip(-cumtrapz(flip(f), flip(S_ol_uh)));
CPS_ol_d  = flip(-cumtrapz(flip(f), flip(S_ol_d)));

CPS_lut_ur = flip(-cumtrapz(flip(f), flip(S_lut_ur)));
CPS_lut_uh = flip(-cumtrapz(flip(f), flip(S_lut_uh)));
CPS_lut_d  = flip(-cumtrapz(flip(f), flip(S_lut_d)));
#+end_src

#+begin_src matlab :results none
%% Cumulative Spectrum
figure;
hold on;
plot(1e6./f, sqrt(CPS_ol_ur) , 'DisplayName', '$u_r$ - OL');
plot(1e6./f, sqrt(CPS_lut_ur), 'DisplayName', '$u_r$ - LUT');
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/effect_lut_on_cps_error_spatial.pdf', 'width', 'wide', 'height', 'normal');
#+end_src

#+name: fig:effect_lut_on_cps_error_spatial
#+caption: Cumulative Spectrum with and without the LUT
#+RESULTS:
[[file:figs/effect_lut_on_cps_error_spatial.png]]

* LUT creation from experimental data
** Introduction                                                      :ignore:
In this section, the full process from measurement, filtering of data to generation of the LUT is detailed.

The computation is performed with Matlab.

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

** Load Data
A Bragg scan is performed using =thtraj= and data are acquired using the =fast_DAQ=.
#+begin_src matlab
%% Load Raw Data
load("scan_10_70_lut_1.mat")
#+end_src

Measured data are:
- =bragg=: Bragg angle in deg
- =dz=: distance between crystals in nm
- =dry=, =drx=: orientation errors between crystals in nrad
- =fjur=, =fjuh=, =fjd=: generated steps by the IcePAP in tens of nm

All are sampled at 10kHz with no filtering.

First, convert all the data to SI units (rad, and m).
#+begin_src matlab
%% Convert Data to Standard Units
% Bragg Angle [rad]
bragg = pi/180*bragg;
% Rx rotation of 1st crystal w.r.t. 2nd crystal [rad]
drx   = 1e-9*drx;
% Ry rotation of 1st crystal w.r.t. 2nd crystal [rad]
dry   = 1e-9*dry;
% Z distance between crystals [m]
dz    = 1e-9*dz;
% Z error between second crystal and first crystal [m]
ddz   = 10.5e-3./(2*cos(bragg)) - dz;
% Steps for Ur motor [m]
fjur  = 1e-8*fjur;
% Steps for Uh motor [m]
fjuh  = 1e-8*fjuh;
% Steps for D motor [m]
fjd   = 1e-8*fjd;
#+end_src

** IcePAP generated Steps
Here is how the steps of the IcePAP (=fjsur=, =fjsuh= and =fjsd=) are computed in mode A:
\begin{equation}
\begin{bmatrix}
\text{fjsur} \\
\text{fjsuh} \\
\text{fjsd}
\end{bmatrix} (\theta) = \text{fjs}_0 +
\bm{J}_{a,111} \cdot \begin{bmatrix}
0 \\
\text{fjsry} \\
\text{fjsrx}
\end{bmatrix} - \frac{10.5 \cdot 10^{-3}}{2 \cos (\theta)}
\end{equation}

There is a first offset $\text{fjs}_0$ that is initialized once, and a second offset which is a function of =fjsry= and =fjsrx=.

Let's compute the offset which is a function of =fjsry= and =fjsrx=:
#+begin_src matlab
fjsry = 0.53171e-3; % [rad]
fjsrx = 0.144e-3;   % [rad]

J_a_111 = [1,  0.14, -0.0675
           1,  0.14,  0.1525
           1, -0.14,  0.0425];

fjs_offset = J_a_111*[0; fjsry; fjsrx]; % ur,uh,d offsets [m]
#+end_src

#+begin_src matlab :results value replace :exports results :tangle no
ans = fjs_offset
#+end_src

#+RESULTS:
|  6.4719e-05 |
|  9.6399e-05 |
| -6.8319e-05 |

Let's now compute $\text{fjs}_0$ using first second of data where there is no movement and bragg axis is fixed at $\theta_0$:
\begin{equation}
\text{fjs}_0 = \begin{bmatrix}
\text{fjsur} \\
\text{fjsuh} \\
\text{fjsd}
\end{bmatrix} (\theta_0) + \frac{10.5 \cdot 10^{-3}}{2 \cos (\theta_0)} -
\bm{J}_{a,111} \cdot \begin{bmatrix}
0 \\
\text{fjsry} \\
\text{fjsrx}
\end{bmatrix}
\end{equation}

#+begin_src matlab
FJ0 = ...
    mean([fjur(time < 1), fjuh(time < 1), fjd(time < 1)])' ...
    + ones(3,1)*10.5e-3./(2*cos(mean(bragg(time < 1)))) ...
    - fjs_offset; % [m]
#+end_src

#+begin_src matlab :results value replace :exports results :tangle no
ans = FJ0
#+end_src

#+RESULTS:
| 0.030427 |
| 0.030427 |
| 0.030427 |

Values are very close for all three axis.
Therefore we take the mean of the three values for $\text{fjs}_0$.
#+begin_src matlab
FJ0 = mean(FJ0);
#+end_src

This approximately corresponds to the distance between the crystals for a Bragg angle of 80 degrees:
#+begin_src matlab :results value replace :exports both :tangle no
10.5e-3/(2*cos(80*pi/180))
#+end_src

#+RESULTS:
: 0.030234

The measured IcePAP steps are compared with the theoretical formulas in Figure [[fig:step_lut_estimation_wanted_fj_pos]].

If we were to zoom a lot, we would see a small delay between the estimation and the steps sent by the IcePAP.
This is due to the fact that the estimation is performed based on the measured Bragg angle while the IcePAP steps are based on the "requested" Bragg angle.
As will be shown in the next section, there is a small delay between the requested and obtained bragg angle which explains this delay.

#+begin_src matlab :exports none
%%
figure;
hold on;
plot(time, 1e3*fjur, 'DisplayName', '$u_r$')
plot(time, 1e3*fjuh, 'DisplayName', '$u_h$')
plot(time, 1e3*fjd , 'DisplayName', '$d$')
set(gca,'ColorOrderIndex',1)
plot(time, 1e3*(FJ0 + fjs_offset(1) - 10.5e-3./(2*cos(bragg))), 'k--', ...
    'DisplayName', 'Estimation')
plot(time, 1e3*(FJ0 + fjs_offset(2) - 10.5e-3./(2*cos(bragg))), 'k--', ...
    'HandleVisibility', 'off')
plot(time, 1e3*(FJ0 + fjs_offset(3) - 10.5e-3./(2*cos(bragg))), 'k--', ...
    'HandleVisibility', 'off')
xlabel('Bragg Angle [deg]'); ylabel('Fast Jack Pos [mm]');
xlim([64.2, 65.2]);
legend('location', 'northeast');
#+end_src

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

#+name: fig:step_lut_estimation_wanted_fj_pos
#+caption: Measured IcePAP Steps and estimation from theoretical formula
#+RESULTS:
[[file:figs/step_lut_estimation_wanted_fj_pos.png]]

** Bragg and Fast Jack Velocities
In order to estimate velocities from measured positions, a filter is used which approximate a pure derivative filter.

#+begin_src matlab
%% Filter to compute velocities
G_diff = (s/2/pi/10)/(1 + s/2/pi/10);
% Make sure the gain w = 2pi is equal to 2pi
G_diff = 2*pi*G_diff/(abs(evalfr(G_diff, 1j*2*pi)));
#+end_src

Only the high frequency amplitude is reduced to not amplified the measurement noise (Figure [[fig:step_lut_deriv_filter]]).

#+begin_src matlab :exports none
%% Bode Plot of the Gdiff filter and comparison with pure derivative filter
freqs = logspace(-1,3,1000);
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_diff, freqs, 'Hz'))), ...
     'DisplayName', '$G_d$');
plot(freqs, abs(squeeze(freqresp(s, freqs, 'Hz'))), ...
     'DisplayName', '$s$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
xlim([-1, 1e3]);
#+end_src

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

#+name: fig:step_lut_deriv_filter
#+caption: Magnitude of filter used to approximate the derivative
#+RESULTS:
[[file:figs/step_lut_deriv_filter.png]]


Using the filter, the Bragg velocity is estimated (Figure [[fig:step_lut_bragg_vel]]).
#+begin_src matlab
%% Bragg Velocity
bragg_vel = lsim(G_diff, bragg, time);
#+end_src

#+begin_src matlab :exports none
%% Plot of Bragg Velocity
figure;
hold on;
plot(time(time > 1), 180/pi*bragg_vel(time > 1))
hold off;
xlabel('Time [s]'); ylabel('Bragg Velocity [deg/s]');
xlim([2, 4]);
#+end_src

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

#+name: fig:step_lut_bragg_vel
#+caption: Estimated Bragg Velocity curing acceleration phase
#+RESULTS:
[[file:figs/step_lut_bragg_vel.png]]


Now, the Fast Jack velocity is estimated (Figure [[fig:step_lut_fast_jack_vel]]).
#+begin_src matlab
%% Fast Jack Velocity
fjur_vel = lsim(G_diff, fjur, time);
fjuh_vel = lsim(G_diff, fjuh, time);
fjd_vel  = lsim(G_diff, fjd , time);
#+end_src

#+begin_src matlab :exports none
%% Plot of Fast Jack Velocity
figure;
hold on;
plot(time(time > 1), 1e3*fjur_vel(time > 1), ...
    'DisplayName', '$u_r$')
plot(time(time > 1), 1e3*fjuh_vel(time > 1), ...
    'DisplayName', '$u_h$')
plot(time(time > 1), 1e3*fjd_vel( time > 1), ...
    'DisplayName', '$d$')
hold off;
xlabel('Time [s]'); ylabel('Fast Jack Velocity [mm/s]');
legend('location', 'southwest');
#+end_src

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

#+name: fig:step_lut_fast_jack_vel
#+caption: Estimated velocity of fast jacks
#+RESULTS:
[[file:figs/step_lut_fast_jack_vel.png]]

#+begin_src matlab :exports none
%% Plot of Fast Jack Velocity
figure;
hold on;
plot(1e3*fjur(time > 1), 1e3*fjur_vel(time > 1), ...
    'DisplayName', '$u_r$')
plot(1e3*fjuh(time > 1), 1e3*fjuh_vel(time > 1), ...
    'DisplayName', '$u_h$')
plot(1e3*fjd(time > 1), 1e3*fjd_vel( time > 1), ...
    'DisplayName', '$d$')
hold off;
xlabel('Fast Jack Position [mm]'); ylabel('Fast Jack Velocity [mm/s]');
legend('location', 'southeast');
#+end_src

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

#+name: fig:step_lut_fast_jack_vel_fct_pos
#+caption: Fast Jack Velocity as a function of its position
#+RESULTS:
[[file:figs/step_lut_fast_jack_vel_fct_pos.png]]

** Bragg Angle Errors / Delays

From the measured =fjur= steps generated by the IcePAP, we can estimate the steps generated corresponding to the Bragg angle.
#+begin_src matlab
%% Estimated Bragg angle requested by IcePAP
bragg_icepap = acos(10.5e-3./(2*(FJ0 + fjs_offset(1) - fjur)));
#+end_src

The generated steps by the IcePAP and the measured angle are compared in Figure [[fig:lut_step_bragg_angle_error_aerotech]].
There is clearly a lag of the Bragg angle compared to the generated IcePAP steps.
#+begin_src matlab :exports none
%% Error Between generated Bragg steps and measured angle
figure;
hold on;
plot(time, 180/pi*bragg_icepap, ...
    'DisplayName', 'IcePAP Steps')
plot(time, 180/pi*bragg, ...
    'DisplayName', 'Encoder Measurement')
hold off;
xlabel('Time [s]'); ylabel('Bragg Angle [deg]');
xlim([3.18, 3.19]);
legend('location', 'southeast');
#+end_src

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

#+name: fig:lut_step_bragg_angle_error_aerotech
#+caption: Estimated generated steps by the IcePAP and measured Bragg angle
#+RESULTS:
[[file:figs/lut_step_bragg_angle_error_aerotech.png]]


#+begin_src matlab :exprts none
% Take only deceleration portion
scan_i = time > 60 & time < 65.1;
#+end_src

If we plot the error between the measured and the requested bragg angle as a function of the bragg velocity (Figure [[fig:lut_step_bragg_error_fct_velocity]]), we can see an almost linear relationship.

This corresponds to a "time lag" of approximately:
#+begin_src matlab :results value replace :exports results :tangle no
sprintf('%.1f ms', 1e3*(bragg_vel(scan_i)\(bragg_icepap(scan_i) - bragg(scan_i))))
#+end_src

#+RESULTS:
: 2.4 ms

#+begin_src matlab :exports none
%% Bragg Error as a function fo the Bragg Velocity
figure;
plot(180/pi*bragg_vel(scan_i), 180/pi*bragg_icepap(scan_i) - 180/pi*bragg(scan_i), 'k.')
xlabel('Bragg Velocity [deg/s]'); ylabel('Bragg Error [deg]')
#+end_src

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

#+name: fig:lut_step_bragg_error_fct_velocity
#+caption: Bragg Error as a function fo the Bragg Velocity
#+RESULTS:
[[file:figs/lut_step_bragg_error_fct_velocity.png]]

#+begin_important
There is a "lag" between the Bragg steps sent by the IcePAP and the measured angle by the encoders.
This is probably due to the single integrator in the "Aerotech" controller.
Indeed, two integrators are required to have no tracking error during ramp reference signals.
#+end_important

** Errors in the Frame of the Crystals

The =dz=, =dry= and =drx= measured relative motion of the crystals are defined as follows:
- An increase of =dz= means the crystals are moving away from each other
- An positive =dry= means the second crystals has positive rotation around =y=
- An positive =drx= means the second crystals has positive rotation around =x=

The error in crystals' distance =ddz= is defined as:
\begin{equation}
ddz(\theta) = \frac{10.5 \cdot 10^{-3}}{2 \cos(\theta)} - dz(\theta)
\end{equation}

Therefore, a positive =ddz= means that the second crystal is too high (fast jacks have to move down).

The errors measured in the frame of the crystals are shown in Figure [[fig:lut_step_measured_errors]].

#+begin_src matlab :exports none
%% Open Loop Errors of the Fast Jacks
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(1e3*fjur, 1e6*dry, ...
    'DisplayName', '$dry$');
plot(1e3*fjur, 1e6*drx, ...
    'DisplayName', '$drx$');
hold off;
ylabel('Orientation Errors [$\mu$rad]');
xlabel('Fast Jack Position [mm]');
legend('location', 'northeast');

ax2 = nexttile();
plot(1e3*fjur, 1e6*ddz, ...
    'DisplayName', '$dz$');
ylabel('Distance Errors [$\mu$m]');
xlabel('Fast Jack Position [mm]');
legend('location', 'northeast');
#+end_src

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

#+name: fig:lut_step_measured_errors
#+caption: Measured errors in the frame of the crystals as a function of the fast jack position
#+RESULTS:
[[file:figs/lut_step_measured_errors.png]]

** Errors in the Frame of the Fast Jacks

From =ddz,dry,drx=, the motion errors of the jast-jacks (=fjur_e=, =fjuh_e= and =jfd_e=) as measured by the interferometers are computed.
#+begin_src matlab
%% Actuator Jacobian
J_a_111 = [1,  0.14, -0.0675
           1,  0.14,  0.1525
           1, -0.14,  0.0425];

%% Computation of the position of the FJ as measured by the interferometers
error = J_a_111 * [ddz, dry, drx]';

fjur_e = error(1,:)'; % [m]
fjuh_e = error(2,:)'; % [m]
fjd_e  = error(3,:)'; % [m]
#+end_src

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

#+begin_src matlab :exports none
%% Plot of the errors in the Frame of the Fast Jacks
figure;
hold on;
plot(1e3*fjur, 1e6*fjur_e, ...
    'DisplayName', '$u_r$');
plot(1e3*fjuh, 1e6*fjuh_e, ...
    'DisplayName', '$u_h$');
plot(1e3*fjd,  1e6*fjd_e, ...
    'DisplayName', '$d$');
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Position Error [$\mu$m]');
legend('location', 'northeast', 'FontSize', 8);
#+end_src

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

#+name: fig:lut_step_measured_error_fj
#+caption: Position error of the Fast jacks
#+RESULTS:
[[file:figs/lut_step_measured_error_fj.png]]

#+begin_src matlab :exports none
%% Plot of the measured position of the FJ as a function of their wanted positions
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(1e3*fjur, 1e6*fjur_e, ...
    'DisplayName', '$u_r$');
plot(1e3*fjuh, 1e6*fjuh_e, ...
    'DisplayName', '$u_h$');
plot(1e3*fjd,  1e6*fjd_e, ...
    'DisplayName', '$d$');
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Position Error [$\mu$m]');
legend('location', 'southeast', 'FontSize', 8);
axis square;
xlim([14.99, 15.01]);

ax2 = nexttile();
hold on;
plot(1e3*fjur, 1e6*fjur_e, ...
    'DisplayName', '$u_r$');
plot(1e3*fjuh, 1e6*fjuh_e, ...
    'DisplayName', '$u_h$');
plot(1e3*fjd,  1e6*fjd_e, ...
    'DisplayName', '$d$');
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Measured Position [$\mu$m]');
legend('location', 'southeast', 'FontSize', 8);
axis square;
xlim([19.99, 20.01]);
#+end_src

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

#+name: fig:lut_step_measured_error_fj_zoom
#+caption: Position error of the Fast jacks - Zoom near two positions
#+RESULTS:
[[file:figs/lut_step_measured_error_fj_zoom.png]]

** Analysis of the obtained error
The measured position of the fast jacks are displayed as a function of the IcePAP steps (Figure [[fig:lut_step_meas_pos_fct_wanted_pos]]).

#+begin_important
From Figure [[fig:lut_step_meas_pos_fct_wanted_pos]], it seems the position as a function of the IcePAP steps is not a bijection function.
Therefore, a measured position can corresponds to several IcePAP Steps.
This is very problematic for building a LUT that will be used to compensated the measured errors.
#+end_important

Also, it seems that the (spatial) period of the error depends on the actual position of the Fast Jack (and therefore of its velocity).
If we compute the equivalent temporal period, we find a frequency of around 370 Hz.

#+begin_src matlab :exports none
%% Plot of the measured position of the FJ as a function of their wanted positions
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(1e3*fjur, 1e3*(fjur + fjur_e), ...
    'DisplayName', '$u_r$');
plot(1e3*fjuh, 1e3*(fjuh + fjuh_e), ...
    'DisplayName', '$u_h$');
plot(1e3*fjd,  1e3*(fjd + fjd_e), ...
    'DisplayName', '$d$');
plot(1e3*fjd,  1e3*fjd, 'k--', ...
    'DisplayName', 'Ref');
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Measured Position [mm]');
legend('location', 'southeast', 'FontSize', 8);
axis square;
xlim([14.99, 15.01]); ylim([14.99, 15.01]);
xticks([14.99 14.995 15 15.005 15.01]);
yticks([14.99 14.995 15 15.005 15.01]);
xtickangle(45);
ytickangle(90);

ax2 = nexttile();
hold on;
plot(1e3*fjur, 1e3*(fjur + fjur_e), ...
    'DisplayName', '$u_r$');
plot(1e3*fjuh, 1e3*(fjuh + fjuh_e), ...
    'DisplayName', '$u_h$');
plot(1e3*fjd,  1e3*(fjd + fjd_e), ...
    'DisplayName', '$d$');
plot(1e3*fjd,  1e3*fjd, 'k--', ...
    'DisplayName', 'Ref');
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Measured Position [mm]');
legend('location', 'southeast', 'FontSize', 8);
axis square;
xlim([19.99, 20.01]); ylim([19.99, 20.01]);
xticks([19.99 19.995 20 20.005 20.01]);
yticks([19.99 19.995 20 20.005 20.01]);
xtickangle(45);
ytickangle(90);
#+end_src

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

#+name: fig:lut_step_meas_pos_fct_wanted_pos
#+caption: Measured Fast Jack position as a function of the IcePAP steps
#+RESULTS:
[[file:figs/lut_step_meas_pos_fct_wanted_pos.png]]

In order to better investigate what is going on, a spectrogram is computed (Figure [[fig:lut_step_meas_pos_error_spectrogram]]).

We clearly observe:
- Some rather constant vibrations with a frequency at around 363Hz and 374Hz.
  This corresponds to the clear periods in Figure [[fig:lut_step_meas_pos_fct_wanted_pos]].
  These are due to the =mcoil= stepper motor (magnetic period).
- Several frequencies which are increasing with time.
  These corresponds to (spatial) periodic errors of the stepper motor.
  The frequency of these errors are increasing because the velocity of the fast jack is also increasing with time (see Figure [[fig:step_lut_fast_jack_vel]]).
  The black dashed line in Figure [[fig:lut_step_meas_pos_error_spectrogram]] shows the frequency of errors with a period of $5\,\mu m$.
  We can also see lower frequencies corresponding to periods of $10\,\mu m$ and $20\,\mu m$ and lots of higher frequencies with are also exciting resonances of the system (second crystal) at around 200Hz

#+begin_src matlab :exports none
%% Spectrogram
figure;
hold on;
pspectrum(fjuh_e, 1e4, 'spectrogram', ...
          'FrequencyResolution', 1e0, ...
          'OverlapPercent',  99, ...
          'FrequencyLimits', [1, 400]);
plot((1/60)*time(time > 1), -(1/(5e-6))*fjur_vel(time > 1), 'k--')
hold off;
xlim([0.03, 1.14]); ylim([1, 400]);
caxis([-160, -130])
title('');
#+end_src

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

#+name: fig:lut_step_meas_pos_error_spectrogram
#+caption: Spectrogram of the $u_h$ errors. The black dashed line corresponds to an error with a period of $5\,\mu m$
#+RESULTS:
[[file:figs/lut_step_meas_pos_error_spectrogram.png]]

#+begin_important
As we would like to only measure the repeatable mechanical errors of the fast jacks and not the vibrations of natural frequencies of the system, we have to filter the data.
#+end_important

** Filtering of Data
As seen in Figure [[fig:lut_step_meas_pos_error_spectrogram]], the errors we wish to calibrate are below 160Hz while the vibrations we wish to ignore are above 200Hz.
We have to use a low pass filter that does not affects frequencies below 160Hz while having good rejection above 200Hz.

The filter used for current LUT is a moving average filter with a length of 100:
#+begin_src matlab
%% Moving Average Filter
B_mov_avg = 1/101*ones(101,1); % FIR Filter coeficients
#+end_src

We may also try a second order low pass filter:
#+begin_src matlab
%% 2nd order Low Pass Filter
G_lpf = 1/(1 + 2*s/(2*pi*80) + s^2/(2*pi*80)^2);
#+end_src

And a FIR filter with linear phase:
#+begin_src matlab
%% FIR with Linear Phase
Fs = 1e4; % Sampling Frequency [Hz]
B_fir = firls(1000, ... % Filter's order
              [0 140/(Fs/2) 180/(Fs/2) 1], ... % Frequencies [Hz]
              [1 1          0          0]); % Wanted Magnitudes
#+end_src

Filters' responses are computed and compared in the Bode plot of Figure [[fig:step_lut_filters_bode_plot]].
#+begin_src matlab
%% Computation of filters' responses
[h_mov_avg, f] = freqz(B_mov_avg, 1, 10000, Fs);
[h_fir,     ~] = freqz(B_fir,     1, 10000, Fs);
h_lpf          = squeeze(freqresp(G_lpf,  f, 'Hz'));
#+end_src

#+begin_src matlab :exports none
%% Bode plot of different filters that could be used
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(h_mov_avg), 'DisplayName', 'Moving Average');
plot(f, abs(h_fir),     'DisplayName', 'FIR');
plot(f, abs(h_lpf),     'DisplayName', '2nd order LPF');
hold off;
set(gca, 'YScale', 'log');
ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
ylim([2e-5, 2e0]);
legend('location', 'northeast');

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(h_mov_avg));
plot(f, 180/pi*angle(h_fir));
plot(f, 180/pi*angle(h_lpf));
hold off;
set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);

linkaxes([ax1,ax2],'x');
set(gca, 'XScale', 'lin');
xlim([0, 5e2]);
#+end_src

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

#+name: fig:step_lut_filters_bode_plot
#+caption: Bode plot of filters that could be used before making the LUT
#+RESULTS:
[[file:figs/step_lut_filters_bode_plot.png]]

Clearly, the currently used moving average filter is filtering too much below 160Hz and too little above 200Hz.
The FIR filter seems more suited for this case.

Let's now compare the filtered data.
#+begin_src matlab
fjur_e_cur = filter(B_mov_avg, 1, fjur_e);
fjur_e_fir = filter(B_fir,     1, fjur_e);
fjur_e_lpf = lsim(G_lpf, fjur_e, time);
#+end_src

As the FIR filter introduce some delays, we can identify this relay and shift the filtered data:
#+begin_src matlab
%% Compensate the FIR delay
delay = mean(grpdelay(B_fir));
#+end_src

#+begin_src matlab :results value replace :exports results :tangle no
ans = delay
#+end_src

#+RESULTS:
: 500

#+begin_src matlab
fjur_e_fir(1:end-delay) = fjur_e_fir(delay+1:end);
#+end_src

The same is done for the moving average filter
#+begin_src matlab
%% Compensate the Moving average delay
delay = mean(grpdelay(B_mov_avg));
fjur_e_cur(1:end-delay) = fjur_e_cur(delay+1:end);
#+end_src

The raw and filtered motion errors are displayed in Figure [[fig:step_lut_filtered_errors_comp]].

#+begin_important
It is shown that while the moving average average filter is working relatively well for low speeds (at around 20mm) it is not for high speeds (near 15mm).
This is because the frequency of the error is above 100Hz and the moving average is flipping the sign of the filtered data.

The IIR low pass filter has some phase issues.

Finally the FIR filter is perfectly in phase while showing good attenuation of the disturbances.
#+end_important

#+begin_src matlab :exports none
%% Plot of the position error of the FJ as a function of their wanted positions
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(1e3*fjur, 1e6*fjur_e, 'k-', ...
    'DisplayName', 'Raw Data');
set(gca,'ColorOrderIndex',1)
plot(1e3*fjur, 1e6*fjur_e_cur, '-', ...
    'DisplayName', 'Mov Avg');
plot(1e3*fjur, 1e6*fjur_e_fir, '-', ...
    'DisplayName', 'FIR');
plot(1e3*fjur, 1e6*fjur_e_lpf, '-', ...
    'DisplayName', 'LPF');
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Measured Error [$\mu$m]');
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
axis square;
xlim([14.99, 15.01]);

ax2 = nexttile();
hold on;
plot(1e3*fjur, 1e6*fjur_e, 'k-', ...
    'DisplayName', 'Raw Data');
set(gca,'ColorOrderIndex',1)
plot(1e3*fjur, 1e6*fjur_e_cur, '-', ...
    'DisplayName', 'Mov Avg');
plot(1e3*fjur, 1e6*fjur_e_fir, '-', ...
    'DisplayName', 'FIR');
plot(1e3*fjur, 1e6*fjur_e_lpf, '-', ...
    'DisplayName', 'LPF');
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Measured Error [$\mu$m]');
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
axis square;
xlim([19.99, 20.01]);
#+end_src

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

#+name: fig:step_lut_filtered_errors_comp
#+caption: Raw measured error and filtered data
#+RESULTS:
[[file:figs/step_lut_filtered_errors_comp.png]]

If we now look at the measured position as a function of the IcePAP steps (Figure [[fig:step_lut_filtered_motion_comp]]), we can see that we obtain a monotonous function for the FIR filtered data which is great to make the LUT.

#+begin_src matlab :exports none
%% Plot of the measured position of the FJ as a function of their wanted positions
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(1e3*fjur, 1e3*(fjur + fjur_e), '-', ...
    'DisplayName', 'Raw Data');
plot(1e3*fjur, 1e3*(fjur + fjur_e_fir), '-', ...
    'DisplayName', 'FIR');
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Measured Position [mm]');
legend('location', 'southeast', 'FontSize', 8);
axis square;
xlim([14.99, 15.01]); ylim([14.99, 15.01]);
xticks([14.99 14.995 15 15.005 15.01]);
yticks([14.99 14.995 15 15.005 15.01]);
xtickangle(45);
ytickangle(90);

ax2 = nexttile();
hold on;
plot(1e3*fjur, 1e3*(fjur + fjur_e), '-', ...
    'DisplayName', 'Raw Data');
plot(1e3*fjur, 1e3*(fjur + fjur_e_fir), '-', ...
    'DisplayName', 'FIR');
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Measured Position [mm]');
legend('location', 'southeast', 'FontSize', 8);
axis square;
xlim([19.99, 20.01]); ylim([19.99, 20.01]);
xticks([19.99 19.995 20 20.005 20.01]);
yticks([19.99 19.995 20 20.005 20.01]);
xtickangle(45);
ytickangle(90);
#+end_src

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

#+name: fig:step_lut_filtered_motion_comp
#+caption: Raw measured motion and filtered motion as a function of the IcePAP Steps
#+RESULTS:
[[file:figs/step_lut_filtered_motion_comp.png]]

If we subtract the raw data with the FIR filtered data, we obtain the remaining motion shown in Figure [[fig:step_lut_remain_motion_remove_filtered]] that only contains the high frequency motion not filtered.

#+begin_src matlab :exports none
%% Remaining motion after removing the filtered data
figure;
hold on;
plot(1e3*fjur, 1e6*(fjur_e - fjur_e_fir), 'k-');
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Measured Error [$\mu$m]');
#+end_src

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

#+name: fig:step_lut_remain_motion_remove_filtered
#+caption: Remaining motion error after removing the filtered part
#+RESULTS:
[[file:figs/step_lut_remain_motion_remove_filtered.png]]

** LUT creation
The procedure used to make the Lookup Table is schematically represented in Figure [[fig:step_lut_schematic_principle]].

For each IcePAP step separated by a constant value (typically $1\,\mu m$) a point of the LUT is computed:
- Points where the measured position is close to the wanted ideal position (i.e. the current IcePAP step) are found
- The corresponding IcePAP step at which the Fast Jack is at the wanted position is stored in the LUT

Therefore the LUT gives the IcePAP step for which the fast jack is at the wanted position as measured by the metrology, which is what we want.

#+begin_src matlab :exports none
%% Schematic of the LUT creation principle
figure;
hold on;
plot(1e3*fjur, 1e3*(fjur + fjur_e_fir), '-');
plot(1e3*fjur, 1e3*fjur, 'k--');
plot(20, 20, 'ko')
xline(20,'-',{'LUT Indice'});
yline(20,'-',{'Measured Position'});
[~, i] = min(abs(1e3*(fjur + fjur_e_fir) - 20));
plot(1e3*(fjur(i)), 20, 'ko')
xline(1e3*(fjur(i)),'-',{'Step stored in the LUT'});
q = quiver([20, 20],[19.992, 19.992],[-0.002, 0.002],[0,0], 'k-', 'LineWidth', 1);
q.MaxHeadSize = 0.5;
q.Marker = 'x';
hold off;
xlabel('IcePAP Steps'); ylabel('Measured Position');
set(gca,'Xticklabel',[]);  set(gca,'Yticklabel',[]);
axis square;
xlim([19.99, 20.01]); ylim([19.99, 20.01]);
#+end_src

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

#+name: fig:step_lut_schematic_principle
#+caption: Schematic of the principle used to make the Lookup Table
#+RESULTS:
[[file:figs/step_lut_schematic_principle.png]]

Let's first initialize the LUT which is table with 4 columns and 26001 rows.
The columns are:
1. IcePAP Step indices from 0 to 26mm with a step of $1\,\mu m$ (thus the 26001 rows)
2. IcePAP step for =fjur= at which point the fast jack is at the wanted position
3. Same for =fjuh=
4. Same for =fjd=

All the units of the LUT are in mm.
We will work in meters and convert to mm at the end.

Let's initialize the Lookup table:
#+begin_src matlab
%% Initialization of the LUT
lut = [0:1e-6:26e-3]'*ones(1,4);
#+end_src

And verify that it has the wanted size:
#+begin_src matlab :results output replace :exports results :tangle no
size(lut)
#+end_src

#+RESULTS:
: size(lut)
: ans =
:        26001           4

The measured Fast Jack position are filtered using the FIR filter:
#+begin_src matlab
%% FIR Filter
Fs = 1e4; % Sampling Frequency [Hz]
fir_order = 1000; % Filter's order
delay = fir_order/2; % Delay induced by the filter
B_fir = firls(fir_order, ... % Filter's order
              [0 140/(Fs/2) 180/(Fs/2) 1], ... % Frequencies [Hz]
              [1 1          0          0]); % Wanted Magnitudes

%% Filtering all measured Fast Jack Position using the FIR filter
fjur_e_filt = filter(B_fir, 1, fjur_e);
fjuh_e_filt = filter(B_fir, 1, fjuh_e);
fjd_e_filt  = filter(B_fir, 1, fjd_e);

%% Compensation of the delay introduced by the FIR filter
fjur_e_filt(1:end-delay) = fjur_e_filt(delay+1:end);
fjuh_e_filt(1:end-delay) = fjuh_e_filt(delay+1:end);
fjd_e_filt( 1:end-delay) = fjd_e_filt( delay+1:end);
#+end_src

The indices where the LUT will be populated are initialized.
#+begin_src matlab
%% Vector of Fast Jack positions [unit of lut_inc]
fjur_pos = floor(min(1e6*fjur)):floor(max(1e6*fjur));
fjuh_pos = floor(min(1e6*fjuh)):floor(max(1e6*fjuh));
fjd_pos  = floor(min(1e6*fjd )):floor(max(1e6*fjd ));
#+end_src

And the LUT is computed and shown in Figure [[fig:step_lut_obtained_lut]].
#+begin_src matlab
%% Build the LUT
for i = fjur_pos
    % Find indices where measured motion is close to the wanted one
    indices = fjur + fjur_e_filt > lut(i,1) - 500e-9 & ...
              fjur + fjur_e_filt < lut(i,1) + 500e-9;
    % Poputate the LUT with the mean of the IcePAP steps
    lut(i,2) = mean(fjur(indices));
end

for i = fjuh_pos
    % Find indices where measuhed motion is close to the wanted one
    indices = fjuh + fjuh_e_filt > lut(i,1) - 500e-9 & ...
              fjuh + fjuh_e_filt < lut(i,1) + 500e-9;
    % Poputate the LUT with the mean of the IcePAP steps
    lut(i,3) = mean(fjuh(indices));
end

for i = fjd_pos
    % Poputate the LUT with the mean of the IcePAP steps
    indices = fjd + fjd_e_filt > lut(i,1) - 500e-9 & ...
              fjd + fjd_e_filt < lut(i,1) + 500e-9;
    % Poputate the LUT
    lut(i,4) = mean(fjd(indices));
end
#+end_src

#+begin_src matlab :exports none
%% Plot the LUT
figure;
hold on;
plot(1e3*lut(:,1), 1e3*lut(:,2), '-o', ...
    'DisplayName', '$u_r$');
plot(1e3*lut(:,1), 1e3*lut(:,3), '-o', ...
    'DisplayName', '$u_h$');
plot(1e3*lut(:,1), 1e3*lut(:,4), '-o', ...
    'DisplayName', '$d$');
plot(1e3*lut(:,1), 1e3*lut(:,1), 'k--', ...
    'HandleVisibility', 'off');
hold off;
xlabel('Input IcePAP Step'); ylabel('Output IcePAP Step');
axis square;
legend('location', 'northwest');
set(gca,'Xticklabel',[]);  set(gca,'Yticklabel',[]);
xlim([19.99, 20.01]); ylim([19.99, 20.01]);
#+end_src

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

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

** Cubic Interpolation of the LUT
Once the LUT is built and loaded to the IcePAP, generated steps are taking the step values in the LUT and cubic spline interpolation is performed.

#+begin_src matlab
%% Estimation of the IcePAP output steps after interpolation
fjur_out_steps = spline(lut(:,1), lut(:,2), fjur);
#+end_src

The LUT data points as well as the spline interpolation values and the ideal values are compared in Figure [[fig:step_lut_spline_interpolation_lut]].
It is shown that the spline interpolation seems to be quite accurate.

#+begin_src matlab :exports none
%% Plot the LUT
figure;
hold on;
plot(1e3*lut(:,1), 1e3*lut(:,2), 'o', ...
    'DisplayName', 'LUT Data Points');
plot(1e3*fjur, 1e3*fjur_out_steps, '-', ...
    'DisplayName', 'Spline Interpolation');
plot(1e3*(fjur + fjur_e_fir), 1e3*fjur, '-', ...
    'DisplayName', 'Ideal Value');
plot(1e3*lut(:,1), 1e3*lut(:,1), 'k--', ...
    'HandleVisibility', 'off');
hold off;
xlabel('Input IcePAP Step'); ylabel('Output IcePAP Step');
axis square;
legend('location', 'northwest');
xlim([14.99, 15.01]); ylim([14.99, 15.01]);
#+end_src

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

#+name: fig:step_lut_spline_interpolation_lut
#+caption: Output IcePAP Steps avec spline interpolation compared with the ideal steps
#+RESULTS:
[[file:figs/step_lut_spline_interpolation_lut.png]]

The difference between the perfect step generation and the step generated after spline interpolation is shown in Figure [[fig:step_lut_error_after_interpolation]].
The remaining position error is in the order of 100nm peak to peak which is acceptable here.

#+begin_src matlab :exports none
%% Estimation of the Error due to limited number of points / interpolation
figure;
hold on;
plot(1e3*(fjur + fjur_e_fir), 1e9*(fjur - spline(lut(:,1), lut(:,2), fjur + fjur_e_fir)), 'k-');
hold off;
xlabel('Input IcePAP Step [mm]'); ylabel('Output Step Error [nm]');
xlim([14.99, 15.01]);
#+end_src

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

#+name: fig:step_lut_error_after_interpolation
#+caption: Errors on the computed IcePAP output steps after LUT generation and spline interpolation
#+RESULTS:
[[file:figs/step_lut_error_after_interpolation.png]]


* Test Matlab LUT
** LUT Creation
A =thtraj= scan from 10 to 70 deg is performed.

#+begin_src matlab
%% Extract measurement Data make from BLISS
data_ol = extractDatData("/home/thomas/mnt/data_id21/21Nov/blc13420/id21/LUT_Matlab/lut_matlab_22122021_1610.dat", ...
                     {"bragg", "dz", "dry", "drx", "fjur", "fjuh", "fjd"}, ...
                     ones(7,1));
data_ol.time = 1e-4*[1:1:length(data_ol.bragg)];
save("/tmp/data_lut.mat", "-struct", "data_ol");
#+end_src

A LUT is generated from this Data.
#+begin_src matlab
%% Generate LUT
createLUT("/tmp/data_lut.mat", "lut_matlab_22122021_1610_10_70_table.dat");
#+end_src

#+begin_src matlab
%% Load the generated LUT
data_lut = importdata("lut_matlab_22122021_1610_10_70_table.dat");
#+end_src

The generated LUT is shown in Figure [[fig:generated_matlab_lut_10_70]].

#+begin_src matlab :exports none
%% Plot LUT Data
figure;
hold on;
plot(data_lut(:,1), 1e3*(data_lut(:,1)-data_lut(:,2)), ...
    'DisplayName', '$u_r$');
plot(data_lut(:,1), 1e3*(data_lut(:,1)-data_lut(:,3)), ...
    'DisplayName', '$u_h$');
plot(data_lut(:,1), 1e3*(data_lut(:,1)-data_lut(:,4)), ...
    'DisplayName', '$d$');
hold off;
xlabel('IcePAP Step [mm]'); ylabel('Step Offset [$\mu$m]')
xlim([15, 25]);
legend('location', 'northeast');
#+end_src

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

#+name: fig:generated_matlab_lut_10_70
#+caption: Generated LUT
#+RESULTS:
[[file:figs/generated_matlab_lut_10_70.png]]

** Compare Mode A and Mode B
The LUT is loaded into BLISS and a new scan in mode B is performed.
#+begin_src matlab
%% Load mode B scan data
data_B = extractDatData("/home/thomas/mnt/data_id21/21Nov/blc13420/id21/LUT_Matlab/lut_matlab_result_22122021_1616.dat", ...
                     {"bragg", "dz", "dry", "drx", "fjur", "fjuh", "fjd"}, ...
                     ones(7,1));
data_B.time = 1e-4*[1:1:length(data_B.bragg)];
save("/tmp/data_lut.mat", "-struct", "data_ol");
#+end_src

#+begin_src matlab :exports none
%% Take only data during scan
data_ol_filt = data_ol.bragg > 11 & data_ol.bragg < 69;
data_B_filt  = data_B.bragg  > 11 & data_B.bragg  < 69;
#+end_src

#+begin_src matlab :exports none
%% Actuator Jacobian
J_a_111 = [1,  0.14, -0.0675
           1,  0.14,  0.1525
           1, -0.14,  0.0425];

%% Convert data in frame of the fast jacks
data_ol_ddz = 10.5e-3./(2*cos(pi/180*data_ol.bragg)) - 1e-9*data_ol.dz;
error = J_a_111 * [data_ol_ddz, 1e-9*data_ol.dry, 1e-9*data_ol.drx]';

data_ol_fjur_e = error(1,:)'; % [m]
data_ol_fjuh_e = error(2,:)'; % [m]
data_ol_fjd_e  = error(3,:)'; % [m]

data_B_ddz = 10.5e-3./(2*cos(pi/180*data_B.bragg)) - 1e-9*data_B.dz;
error = J_a_111 * [data_B_ddz, 1e-9*data_B.dry, 1e-9*data_B.drx]';

data_B_fjur_e = error(1,:)'; % [m]
data_B_fjuh_e = error(2,:)'; % [m]
data_B_fjd_e  = error(3,:)'; % [m]
#+end_src

#+begin_src matlab :exports none
%% Compare RAW data - Mode A and Mode B
figure;
tiledlayout(1, 3, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(data_ol.bragg(data_ol_filt), 1e6*data_ol_fjur_e(data_ol_filt), ...
    'DisplayName', 'Mode A - $u_r$')
plot(data_B.bragg(data_B_filt),   1e6*data_B_fjur_e( data_B_filt), ...
    'DisplayName', 'Mode B - $u_r$')
hold off;
xlabel('Bragg Angle [deg]'); ylabel('Fast Jack Error [um]')
legend('location', 'northwest')

ax2 = nexttile();
hold on;
plot(data_ol.bragg(data_ol_filt), 1e6*data_ol_fjuh_e(data_ol_filt), ...
    'DisplayName', 'Mode A - $u_h$')
plot(data_B.bragg(data_B_filt),   1e6*data_B_fjuh_e( data_B_filt), ...
    'DisplayName', 'Mode B - $u_h$')
hold off;
xlabel('Bragg Angle [deg]'); set(gca, 'YTickLabel',[]);
legend('location', 'northwest')

ax3 = nexttile();
hold on;
plot(data_ol.bragg(data_ol_filt), 1e6*data_ol_fjd_e(data_ol_filt), ...
    'DisplayName', 'Mode A - $d$')
plot(data_B.bragg(data_B_filt),   1e6*data_B_fjd_e( data_B_filt), ...
    'DisplayName', 'Mode B - $d$')
hold off;
xlabel('Bragg Angle [deg]'); set(gca, 'YTickLabel',[]);
legend('location', 'northwest')

linkaxes([ax1,ax2,ax3],'xy');
xlim([11, 69]);
#+end_src

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

#+name: fig:matlab_lut_comp_fj_raw
#+caption: Comparison of the Raw measurement of fast jack motion errors for mode A and mode B
#+RESULTS:
[[file:figs/matlab_lut_comp_fj_raw.png]]

Now look at the low frequency motion by using a low pass filter
#+begin_src matlab
%% FIR Filter
Fs = 1e4; % Sampling Frequency [Hz]
fir_order = 1000; % Filter's order
delay = fir_order/2; % Delay induced by the filter
B_fir = firls(fir_order, ... % Filter's order
              [0 5/(Fs/2)   10/(Fs/2) 1], ... % Frequencies [Hz]
              [1 1          0          0]); % Wanted Magnitudes

%% Filtering all measured Fast Jack Position using the FIR filter
data_B_fjur_f = filter(B_fir, 1, data_B_fjur_e);
data_B_fjuh_f = filter(B_fir, 1, data_B_fjuh_e);
data_B_fjd_f  = filter(B_fir, 1, data_B_fjd_e);

data_ol_fjur_f = filter(B_fir, 1, data_ol_fjur_e);
data_ol_fjuh_f = filter(B_fir, 1, data_ol_fjuh_e);
data_ol_fjd_f  = filter(B_fir, 1, data_ol_fjd_e);

%% Compensation of the delay introduced by the FIR filter
data_B_fjur_f(1:end-delay) = data_B_fjur_f(delay+1:end);
data_B_fjuh_f(1:end-delay) = data_B_fjuh_f(delay+1:end);
data_B_fjd_f( 1:end-delay) = data_B_fjd_f( delay+1:end);

data_ol_fjur_f(1:end-delay) = data_ol_fjur_f(delay+1:end);
data_ol_fjuh_f(1:end-delay) = data_ol_fjuh_f(delay+1:end);
data_ol_fjd_f( 1:end-delay) = data_ol_fjd_f( delay+1:end);
#+end_src

#+begin_src matlab :exports none
%% Compare RAW data - Mode A and Mode B
figure;
tiledlayout(1, 3, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(data_ol.bragg(data_ol_filt), 1e6*data_ol_fjur_f(data_ol_filt), ...
    'DisplayName', 'Mode A - $u_r$')
plot(data_B.bragg(data_B_filt),   1e6*data_B_fjur_f( data_B_filt), ...
    'DisplayName', 'Mode B - $u_r$')
hold off;
xlabel('Bragg Angle [deg]'); ylabel('Error for fjur [um]')
legend('location', 'northwest')

ax2 = nexttile();
hold on;
plot(data_ol.bragg(data_ol_filt), 1e6*data_ol_fjuh_f(data_ol_filt), ...
    'DisplayName', 'Mode A - $u_h$')
plot(data_B.bragg(data_B_filt),   1e6*data_B_fjuh_f( data_B_filt), ...
    'DisplayName', 'Mode B - $u_h$')
hold off;
xlabel('Bragg Angle [deg]'); set(gca, 'YTickLabel',[]);
legend('location', 'northwest')

ax3 = nexttile();
hold on;
plot(data_ol.bragg(data_ol_filt), 1e6*data_ol_fjd_f(data_ol_filt), ...
    'DisplayName', 'Mode A - $d$')
plot(data_B.bragg(data_B_filt),   1e6*data_B_fjd_f( data_B_filt), ...
    'DisplayName', 'Mode B - $d$')
hold off;
xlabel('Bragg Angle [deg]'); set(gca, 'YTickLabel',[]);
legend('location', 'northwest')

linkaxes([ax1,ax2,ax3],'xy');
xlim([11, 69]);
#+end_src

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

#+name: fig:matlab_lut_comp_fj_filt
#+caption: Comparison of the Raw measurement of fast jack motion errors for mode A and mode B
#+RESULTS:
[[file:figs/matlab_lut_comp_fj_filt.png]]

** Check IcePAP Steps with LUT
Compare the theoretical steps and the measured steps when the LUT is on.

Estimate wanted =fjur= steps.
#+begin_src matlab
FJ0 = 0.030427;

fjsry = 0.53171e-3; % [rad]
fjsrx = 0.144e-3;   % [rad]

J_a_111 = [1,  0.14, -0.0675
           1,  0.14,  0.1525
           1, -0.14,  0.0425];

fjs_offset = J_a_111*[0; fjsry; fjsrx]; % ur,uh,d offsets [m]

fjur_th = 1e3*(FJ0 + fjs_offset(1) - 10.5e-3./(2*cos(pi/180*data_B.bragg)));
#+end_src

#+begin_src matlab
%% Estimation of the IcePAP output steps after interpolation
fjur_out_steps = spline(data_lut(:,1), data_lut(:,2), fjur_th);
#+end_src

Difference between theoretical step using LUT and sent step using IcePAP
#+begin_src matlab
figure;
plot(data_B.bragg, 1e-2*data_B.fjur - 1e3*fjur_out_steps)
xlabel('Bragg [deg]'); ylabel('Difference on fjpur [$\mu$m]');
xlim([10, 70]);
#+end_src

This difference can be explained by the fact that we are basing the theoretical step after LUT on the measured Bragg angle and not on the requested one (there is a delay).

* LUT Without =mcoil=
** 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

** Load
#+begin_src matlab
%% Extract measurement Data make from BLISS
data_ol = extractDatData("/home/thomas/mnt/data_id21/21Nov/blc13420/id21/LUT_without_mcoil/lut_without_mcoil_06012022_1644.dat", ...
                     {"bragg", "dz", "dry", "drx", "fjur", "fjuh", "fjd"}, ...
                     ones(7,1));
data_ol.time = 1e-4*[1:1:length(data_ol.bragg)];
% save("/tmp/data_lut.mat", "-struct", "data_ol");
#+end_src

#+begin_src matlab
%% Extract measurement Data make from BLISS
data_bis = extractDatData("/home/thomas/mnt/data_id21/21Nov/blc13420/id21/LUT_Matlab/lut_matlab_06012022_1651.dat", ...
                     {"bragg", "dz", "dry", "drx", "fjur", "fjuh", "fjd"}, ...
                     ones(7,1));
data_bis.time = 1e-4*[1:1:length(data_bis.bragg)];
% save("/tmp/data_lut.mat", "-struct", "data_ol");
#+end_src

#+begin_src matlab :exports none
%% Spectrogram
figure;
hold on;
pspectrum(data_ol.drx, 1e4, 'spectrogram', ...
          'FrequencyResolution', 1e0, ...
          'OverlapPercent',  99, ...
          'FrequencyLimits', [1, 400]);
% plot((1/60)*time(time > 1), -(1/(5e-6))*fjur_vel(time > 1), 'k--')
hold off;
% xlim([0.03, 1.14]); ylim([1, 400]);
% caxis([-160, -130])
title('');
#+end_src

#+begin_src matlab :exports none
%% Spectrogram
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
pspectrum(data_bis.drx, 1e4, 'spectrogram', ...
          'FrequencyResolution', 1e0, ...
          'OverlapPercent',  99, ...
          'FrequencyLimits', [1, 400]);
xlim([0,10])
caxis([-70, 70])
title('With Mcoil');

ax2 = nexttile();
pspectrum(data_ol.drx, 1e4, 'spectrogram', ...
          'FrequencyResolution', 1e0, ...
          'OverlapPercent',  99, ...
          'FrequencyLimits', [1, 400]);
xlim([0,10])
caxis([-70, 70])
title('Without Mcoil');
#+end_src

#+begin_src matlab :exports none
figure;
hold on;
plot(data_bis.time, 1e-3*data_bis.bragg)
plot(data_ol.time,  1e-3*data_ol.bragg)
hold off;
xlabel('Bragg Angle [deg]'); ylabel('Drx [$\mu$rad]');
#+end_src

#+begin_src matlab :exports none
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(data_bis.bragg, 1e-3*data_bis.drx)
plot(data_ol.bragg,  1e-3*data_ol.drx)
hold off;
xlabel('Bragg Angle [deg]'); ylabel('Drx [$\mu$rad]');

ax2 = nexttile();
hold on;
plot(data_bis.bragg, 1e-3*data_bis.dry)
plot(data_ol.bragg,  1e-3*data_ol.dry)
hold off;
xlabel('Bragg Angle [deg]'); ylabel('Dry [$\mu$rad]');
#+end_src

#+begin_src matlab :exports none
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(data_bis.dz, 1e-3*data_bis.drx)
plot(data_ol.dz,  1e-3*data_ol.drx)
hold off;
xlabel('Bragg Angle [deg]'); ylabel('Drx [$\mu$rad]');

ax2 = nexttile();
hold on;
plot(data_bis.dz, 1e-3*data_bis.dry)
plot(data_ol.dz,  1e-3*data_ol.dry)
hold off;
xlabel('Bragg Angle [deg]'); ylabel('Dry [$\mu$rad]');
#+end_src

#+begin_src matlab
%% FIR Filter
Fs = 1e4; % Sampling Frequency [Hz]
fir_order = 1000; % Filter's order
delay = fir_order/2; % Delay induced by the filter
B_fir = firls(fir_order, ... % Filter's order
              [0 5/(Fs/2)   10/(Fs/2) 1], ... % Frequencies [Hz]
              [1 1          0          0]); % Wanted Magnitudes
#+end_src

#+begin_src matlab :exports none
figure;
hold on;
plot(data_bis.bragg, 1e-3*filter(B_fir, 1, data_bis.drx))
plot(data_ol.bragg,  1e-3*filter(B_fir, 1, data_ol.drx))
hold off;
xlabel('Bragg Angle [deg]'); ylabel('Dry [$\mu$rad]');
#+end_src

* Optimal Trajectory to make the LUT
<<sec:optimal_traj_dist>>
** Introduction                                                      :ignore:

In this section, the problem of generating an adequate trajectory to make the LUT is studied.

The problematic is the following:
1. the positioning errors of the fast jack should be measured
2. all external disturbances and measurement noise should be filtered out.

The main difficulty is that the frequency of both the positioning errors errors and the disturbances are a function of the scanning velocity.

First, the frequency of the disturbances as well as the errors to be measured are described and a filter is designed to optimally separate disturbances from positioning errors (Section [[sec:optimal_traj_dist]]).
The relation between the Bragg angular velocity and fast jack velocity is studied in Section [[sec:bragg_and_fj_velocities]].
Next, a trajectory with constant fast jack velocity (Section [[sec:optimal_traj_const_fj_velocity]]) and with constant Bragg angular velocity (Section [[sec:optimal_traj_const_bragg_velocity]]) are simulated to understand their limitations.
Finally, it is proposed to perform a scan in two parts (one part with constant fast jack velocity and the other part with constant bragg angle velocity) in Section [[sec:optimal_traj_const_bragg_velocity]].

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

** Filtering Disturbances and Noise
<<sec:disturbances_noise_filtering>>
**** Introduction                                                  :ignore:
Based on measurements made in mode A (without LUT or feedback control), several disturbances could be identified:
- vibrations coming from from the =mcoil= motor
- vibrations with constant frequencies at 29Hz (pump), 34Hz (air conditioning) and 45Hz (un-identified)

These disturbances as well as the noise of the interferometers should be filtered out, and only the fast jack motion errors should be left untouched.

Therefore, the goal is to make a scan such that during all the scan, the frequencies of the errors induced by the fast jack have are smaller than the frequencies of all other disturbances.
Then, it is easy to use a filter to separate the disturbances and noise from the positioning errors of the fast jack.

**** Errors induced by the Fast Jack
The Fast Jack is composed of one stepper motor, and a planetary roller screw with a pitch of 1mm/turn.
The stepper motor as 50 pairs of magnetic poles, and therefore positioning errors are to be expected every 1/50th of turn (and its harmonics: 1/100th of turn, 1/200th of turn, etc.).

One pair of magnetic pole corresponds to an axial motion of $20\,\mu m$.
Therefore, errors are to be expected with a period of $20\,\mu m$ and harmonics at $10\,\mu m$, $5\,\mu m$, $2.5\,\mu m$, etc.

As the LUT has one point every $1\,\mu m$, we wish to only measure errors with a period of $20\,\mu m$, $10\,\mu m$ and $5\,\mu m$.
Indeed, errors with smaller periods are small in amplitude (i.e. not worth to compensate) and are difficult to model with the limited number of points in the LUT.

The frequency corresponding to errors with a period of $5\,\mu m$ at 1mm/s is:
#+begin_src matlab :results value replace :exports results :tangle no
sprintf('Frequency or errors with period of 5um/s at 1mm/s is: %.1f [Hz]', 1e-3/5e-6);
#+end_src

#+RESULTS:
: Frequency or errors with period of 5um/s at 1mm/s is: 200.0 [Hz]

We wish that the frequency of the error corresponding to a period of $5\,\mu m$ to be smaller than the smallest disturbance to be filtered.

As the main disturbances are at 34Hz and 45Hz, we constrain the the maximum axial velocity of the Fast Jack such that the positioning error has a frequency bellow 25Hz:
#+begin_src matlab
max_fj_vel = 25*1e-3/(1e-3/5e-6); % [m/s]
#+end_src

#+begin_src matlab :results value replace :exports results :tangle no
sprintf('Maximum Fast Jack velocity: %.3f [mm/s]', 1e3*max_fj_vel);
#+end_src

#+RESULTS:
: Maximum Fast Jack velocity: 0.125 [mm/s]

#+begin_important
Therefore, the Fast Jack scans should be scanned at rather low velocity for the positioning errors to be at sufficiently low frequency.
#+end_important

**** Vibrations induced by =mcoil=
The =mcoil= system is composed of one stepper motor and a reducer such that one stepper motor turns makes the =mcoil= axis to rotate 0.2768 degrees.
When scanning the =mcoil= motor, periodic vibrations can be measured by the interferometers.

It has been identified that the period of these vibrations are corresponding to the period of the magnetic poles (50 per turn as for the Fast Jack stepper motors).

Therefore, the frequency of these periodic errors are a function of the angular velocity.
With an angular velocity of 1deg/s, the frequency of the vibrations are expected to be at:
#+begin_src matlab :results value replace :exports results :tangle no
sprintf('Fundamental frequency at 1deg/s: %.1f [Hz]', 50/0.2768);
#+end_src

#+RESULTS:
: Fundamental frequency at 1deg/s: 180.6 [Hz]

We wish the frequency of these errors to be at minimum 34Hz (smallest frequency of other disturbances).
The corresponding minimum =mcoil= velocity is:
#+begin_src matlab
min_bragg_vel = 34/(50/0.2768); % [deg/s]
#+end_src

#+begin_src matlab :results value replace :exports results :tangle no
sprintf('Min mcoil velocity is %.2f [deg/s]', min_bragg_vel);
#+end_src

#+RESULTS:
: Min mcoil velocity is 0.19 [deg/s]

#+begin_important
Regarding the =mcoil= motor, the problematic is to not scan too slowly.
It should however be checked whether the amplitude of the induced vibrations is significant of not.
#+end_important

Note that the maximum bragg angular velocity is:
#+begin_src matlab
max_bragg_vel = 1; % [deg/s]
#+end_src

**** Measurement noise of the interferometers

The motion of the fast jacks are measured by interferometers which have some measurement noise.
It is wanted to filter this noise to acceptable values to have a clean measured position.

As the interferometer noise has a rather flat spectral density, it is easy to estimate its RMS value as a function of the cut-off frequency of the filter.

#+begin_src matlab :exports none
%% Interferometer ASD
freqs = logspace(0,3,1000); % [Hz]
asd_int = 2e-11*ones(size(freqs)); % Estimation of Interferometer ASD [m/sqrt(Hz)]
#+end_src

The RMS value of the filtered interferometer signal as a function of the cutoff frequency of the low pass filter is computed and shown in Figure [[fig:interferometer_noise_cutoff_freq]].
#+begin_src matlab :exports none
%% Interferometer noise as a function of Filter Frequency
figure;
plot(freqs, 1e9*sqrt(cumtrapz(freqs, asd_int.^2)))
xlabel('Filter Cutoff Frequency [Hz]'); ylabel('Filtered Noise [nm, RMS]');
set(gca, 'XScale', 'log');
#+end_src

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

#+name: fig:interferometer_noise_cutoff_freq
#+caption: Filtered noise RMS value as a function of the low pass filter cut-off frequency
#+RESULTS:
[[file:figs/interferometer_noise_cutoff_freq.png]]

#+begin_important
As the filter will have a cut-off frequency between 25Hz (maximum frequency of the positioning errors) and 34Hz (minimum frequency of disturbances), a filtered measurement noise of 0.1nm RMS is to be expected.
#+end_important

#+begin_note
Figure [[fig:interferometer_noise_cutoff_freq]] is a rough estimate.
Precise estimation can be done by measuring the spectral density of the interferometer noise experimentally.
#+end_note

**** Interferometer - Periodic non-linearity
Interferometers can also show periodic non-linearity with a (fundamental) period equal to half the wavelength of its light (i.e. 765nm for Attocube) and with unacceptable amplitudes (up to tens of nanometers).

The minimum frequency associated with these errors is therefore a function of the fast jack velocity.
With a velocity of 1mm/s, the frequency is:
#+begin_src matlab :results value replace :exports results :tangle no
sprintf('Fundamental frequency at 1mm/s: %.1f [Hz]', 1e-3/765e-9);
#+end_src

#+RESULTS:
: Fundamental frequency at 1mm/s: 1307.2 [Hz]

We wish these errors to be at minimum 34Hz (smallest frequency of other disturbances).
The corresponding minimum velocity of the Fast Jack is:
#+begin_src matlab
min_fj_vel = 34*1e-3/(1e-3/765e-9); % [m/s]
#+end_src

#+begin_src matlab :results value replace :exports results :tangle no
sprintf('Minimum Fast Jack velocity is %.3f [mm/s]', 1e3*min_fj_vel);
#+end_src

#+RESULTS:
: Minimum Fast Jack velocity is 0.026 [mm/s]

#+begin_important
The Fast Jack Velocity should not be too low or the frequency of the periodic non-linearity of the interferometer would be too small to be filtered out (i.e. in the pass-band of the filter).
#+end_important

**** Implemented Filter
Let's now verify that it is possible to implement a filter that keep everything untouched below 25Hz and filters everything above 34Hz.

To do so, a FIR linear phase filter is designed:
#+begin_src matlab
%% FIR with Linear Phase
Fs = 1e4; % Sampling Frequency [Hz]
B_fir = firls(5000, ... % Filter's order
              [0 25/(Fs/2) 34/(Fs/2) 1], ... % Frequencies [Hz]
              [1 1          0          0]); % Wanted Magnitudes
#+end_src

Its amplitude response is shown in Figure [[fig:fir_filter_response_freq_ranges]].
It is confirmed that the errors to be measured (below 25Hz) are left untouched while the disturbances above 34Hz are reduced by at least a factor $10^4$.

#+begin_src matlab :exports none
%% Computation of filters' responses
[h_fir, f] = freqz(B_fir, 1, 10000, Fs);
#+end_src

#+begin_src matlab :exports none
%% Bode plot of FIR Filter with pass band and stop band
figure;
hold on;
plot(f, abs(h_fir));
xline(25, 'k--', 'Max. Error', ...
      'LineWidth', 2, 'LabelVerticalAlignment', 'bottom');
xline(34, 'k--', 'Min. Dist.', ...
      'LineWidth', 2, 'LabelVerticalAlignment', 'top');
hold off;
xlabel('Frequency [Hz]'); ylabel('Amplitude');
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'log');
xlim([0, 50]); ylim([2e-5, 2e0]);
#+end_src

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

#+name: fig:fir_filter_response_freq_ranges
#+caption: FIR filter's response
#+RESULTS:
[[file:figs/fir_filter_response_freq_ranges.png]]

To have such a steep change in gain, the order of the filter is rather large.
This has the negative effect of inducing large time delays:
#+begin_src matlab :results value replace :exports results :tangle no
sprintf('Induced time delay is %.2f [s]', (length(B_fir)-1)/2/Fs)
#+end_src

#+RESULTS:
: Induced time delay is 0.25 [s]

This time delay is only requiring us to start the acquisition 0.25 seconds before the important part of the scan is performed (i.e. the first 0.25 seconds of data cannot be filtered).

** First Estimation of the optimal trajectory
<<sec:bragg_and_fj_velocities>>
Based on previous analysis (Section [[sec:disturbances_noise_filtering]]), minimum and maximum fast jack velocities and bragg angular velocities could be determined.
These values are summarized in Table [[tab:max_min_vel]].
Therefore, if during the scan the velocities are within the defined bounds, it will be very easy to filter the data and extract only the relevant information (positioning error of the fast jack).

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([min_bragg_vel, max_bragg_vel; 1e3*min_fj_vel, 1e3*max_fj_vel], {'Bragg Angular Velocity [deg/s]', 'Fast Jack Velocity [mm/s]'}, {'Min', 'Max'}, ' %.3f ');
#+end_src

#+name: tab:max_min_vel
#+caption: Minimum and Maximum estimated velocities
#+attr_latex: :environment tabularx :width 0.6\linewidth :align lXX
#+attr_latex: :center t :booktabs t
#+RESULTS:
|                                |   Min |   Max |
|--------------------------------+-------+-------|
| Bragg Angular Velocity [deg/s] | 0.188 |   1.0 |
| Fast Jack Velocity [mm/s]      | 0.026 | 0.125 |


We now wish to see if it is possible to perform a scan from 5deg to 75deg of bragg angle while keeping the velocities within the bounds in Table [[tab:max_min_vel]].

To study that, we can compute the relation between the Bragg angular velocity and the Fast Jack velocity as a function of the Bragg angle.

To do so, we first look at the relation between the Bragg angle $\theta_b$ and the Fast Jack position $d_{\text{FJ}}$:
\begin{equation}
d_{FJ}(t) = d_0 - \frac{10.5 \cdot 10^{-3}}{2 \cos \theta_b(t)}
\end{equation}
with $d_0 \approx 0.030427\,m$.

Then, by taking the time derivative, we obtain the relation between the Fast Jack velocity $\dot{d}_{\text{FJ}}$ and the Bragg angular velocity $\dot{\theta}_b$ as a function of the bragg angle $\theta_b$:
\begin{equation} \label{eq:bragg_angle_formula}
\boxed{\dot{d}_{FJ}(t) = - \dot{\theta_b}(t) \cdot \frac{10.5 \cdot 10^{-3}}{2} \cdot \frac{\tan \theta_b(t)}{\cos \theta_b(t)}}
\end{equation}

The relation between the Bragg angular velocity and the Fast Jack velocity is computed for several angles starting from 5degrees up to 75 degrees and this is shown in Figure [[fig:bragg_vel_fct_fj_vel]].

#+begin_src matlab :exports none
%% Compute Fast Jack velocities as a function of Bragg Angle and Bragg angular velocity
bragg_p = 5:5:75; % Bragg Angles [deg]
bragg_v = pi/180*[0:1e-3:1.5]; % Tested Bragg Angular Velocities [rad/s]

[bragg_positions,bragg_velocities] = meshgrid(bragg_p,bragg_v);
fj_vel = -(bragg_velocities) .* (10.5e-3/2) .* tan(pi/180*bragg_positions)./cos(pi/180*bragg_positions); % FJ Velocity [m/s]
#+end_src

#+begin_src matlab :exports none
%% Plot the Bragg angular velocity as a function of the FJ velocity
figure;
hold on;
contour(1e3*abs(fj_vel), 180/pi*bragg_velocities, bragg_positions, bragg_p, ...
        'ShowText', 'on', 'LineWidth', 2)
xline(1e3*min_fj_vel, 'k--', 'LineWidth', 2);
xline(1e3*max_fj_vel, 'k--', 'LineWidth', 2);
yline(min_bragg_vel,  'k--', 'LineWidth', 2);
yline(max_bragg_vel,  'k--', 'LineWidth', 2);
plot(1e3*[min_fj_vel, max_fj_vel, max_fj_vel], [max_bragg_vel, max_bragg_vel, min_bragg_vel], 'r-');
text(1e3*(min_fj_vel+max_fj_vel)/2, max_bragg_vel, ...
     '$\textcircled{1}$','Color','red','FontSize',14, 'Interpreter', 'latex', 'VerticalAlignment', 'bottom')
text(1e3*max_fj_vel, (max_bragg_vel+min_bragg_vel)/2, ...
     '$\textcircled{2}$','Color','red','FontSize',14, 'Interpreter', 'latex', 'HorizontalAlignment', 'left')
hold off;
xlabel('Fast Jack Velocity [mm/s]');
ylabel('Bragg Angular Velocity [deg/s]');
xlim([0, 1.1*1e3*max_fj_vel]);
ylim([0, 1.1*max_bragg_vel]);
grid;
#+end_src

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

#+name: fig:bragg_vel_fct_fj_vel
#+caption: Bragg angular velocity as a function of the fast jack velocity for several bragg angles (indicated by the colorful lines in degrees). Black dashed lines indicated minimum/maximum bragg angular velocities as well as minimum/maximum fast jack velocities
#+RESULTS:
[[file:figs/bragg_vel_fct_fj_vel.png]]

#+begin_important
From Figure [[fig:bragg_vel_fct_fj_vel]], it is clear that only Bragg angles from apprimately 15 to 70 degrees can be scanned by staying in the "perfect" zone (defined by the dashed black lines).

To scan smaller bragg angles, either the maximum bragg angular velocity should be increased or the minimum fast jack velocity decreased (accepting some periodic non-linearity to be measured).

To scan higher bragg angle, either the maximum fast jack velocity should be increased or the minimum bragg angular velocity decreased (taking the risk to have some disturbances from the =mcoil= motion in the signal).
#+end_important

For Bragg angles between 15 degrees and 70 degrees, several strategies can be chosen:
- Constant Fast Jack velocity (Figure [[fig:bragg_vel_fct_fj_vel_example_traj]] - Left):
  1. Go from 15 degrees to 44 degrees at minimum fast jack velocity
  2. Go from 44 degrees to 70 degrees at maximum fast jack velocity
- Constant Bragg angular velocity (Figure [[fig:bragg_vel_fct_fj_vel_example_traj]] - Right):
  1. Go from 15 degrees to 44 degrees at maximum angular velocity
  2. Go from 44 to 70 degrees at minimum angular velocity
- A mixed of constant bragg angular velocity and constant fast jack velocity (Figure [[fig:bragg_vel_fct_fj_vel]] - Red line)
  1. from 15 to 44 degrees with maximum Bragg angular velocity
  2. from 44 to 70 degrees with maximum Bragg angular velocity

The third option is studied in Section [[sec:optimal_traj_const_bragg_velocity]]

#+begin_src matlab :exports none
%% Plot the Bragg angular velocity as a function of the FJ velocity
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
contour(1e3*abs(fj_vel), 180/pi*bragg_velocities, bragg_positions, bragg_p, 'LineWidth', 2)
xline(1e3*min_fj_vel, 'k--', 'LineWidth', 2);
xline(1e3*max_fj_vel, 'k--', 'LineWidth', 2);
yline(min_bragg_vel,  'k--', 'LineWidth', 2);
yline(max_bragg_vel,  'k--', 'LineWidth', 2);
theta_mid = pi/180*44.32;
plot(1e3*[min_fj_vel, min_fj_vel], [max_bragg_vel, 180/pi*(min_fj_vel) ./( (10.5e-3/2) .* tan(theta_mid)./cos(theta_mid))], 'r-');
plot(1e3*[min_fj_vel, max_fj_vel], [180/pi*(min_fj_vel) ./( (10.5e-3/2) .* tan(theta_mid)./cos(theta_mid)), 180/pi*(max_fj_vel) ./( (10.5e-3/2) .* tan(pi/180*44.32)./cos(pi/180*44.32))], 'r-.');
plot(1e3*[max_fj_vel, max_fj_vel], [180/pi*(max_fj_vel) ./( (10.5e-3/2) .* tan(pi/180*44.32)./cos(pi/180*44.32)), min_bragg_vel], 'r-');
text(1e3*min_fj_vel, (max_bragg_vel+min_bragg_vel)/2, ...
     '$\textcircled{1}$','Color','red','FontSize',14, 'Interpreter', 'latex', 'HorizontalAlignment', 'right')
text(1e3*max_fj_vel, (max_bragg_vel+min_bragg_vel)/2, ...
     '$\textcircled{2}$','Color','red','FontSize',14, 'Interpreter', 'latex', 'HorizontalAlignment', 'left')
hold off;
xlabel('Fast Jack Velocity [mm/s]');
ylabel('Angular Velocity [deg/s]');
xlim([0, 1.1*1e3*max_fj_vel]);
ylim([0, 1.1*max_bragg_vel]);
title('Constant Fast Jack Velocity')
grid;

ax2 = nexttile();
hold on;
contour(1e3*abs(fj_vel), 180/pi*bragg_velocities, bragg_positions, bragg_p, 'LineWidth', 2)
xline(1e3*min_fj_vel, 'k--', 'LineWidth', 2);
xline(1e3*max_fj_vel, 'k--', 'LineWidth', 2);
yline(min_bragg_vel,  'k--', 'LineWidth', 2);
yline(max_bragg_vel,  'k--', 'LineWidth', 2);
plot(1e3*[min_fj_vel, (max_bragg_vel*pi/180) .* (10.5e-3/2) .* tan(theta_mid)./cos(theta_mid)], [max_bragg_vel, max_bragg_vel], 'r-');
plot(1e3*[(max_bragg_vel*pi/180) .* (10.5e-3/2) .* tan(theta_mid)./cos(theta_mid), (min_bragg_vel*pi/180) .* (10.5e-3/2) .* tan(theta_mid)./cos(theta_mid)], [max_bragg_vel, min_bragg_vel], 'r-.');
plot(1e3*[(min_bragg_vel*pi/180) .* (10.5e-3/2) .* tan(theta_mid)./cos(theta_mid), max_fj_vel], [min_bragg_vel, min_bragg_vel], 'r-');
text(1e3*(min_fj_vel+max_fj_vel)/2, max_bragg_vel, ...
     '$\textcircled{1}$','Color','red','FontSize',14, 'Interpreter', 'latex', 'VerticalAlignment', 'top')
text(1e3*(min_fj_vel+max_fj_vel)/2, min_bragg_vel, ...
     '$\textcircled{2}$','Color','red','FontSize',14, 'Interpreter', 'latex', 'VerticalAlignment', 'top')
hold off;
xlabel('Fast Jack Velocity [mm/s]');
ylabel('Angular Velocity [deg/s]');
xlim([0, 1.1*1e3*max_fj_vel]);
ylim([0, 1.1*max_bragg_vel]);
title('Constant Bragg Angular Velocity')
grid;
#+end_src

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

#+name: fig:bragg_vel_fct_fj_vel_example_traj
#+caption: Angular velocity and fast jack velocity during two scans from 5 to 75 degrees. On the left for a scan with constant fast jack velocity. On the right for a scan with constant Bragg angular velocity.
#+RESULTS:
[[file:figs/bragg_vel_fct_fj_vel_example_traj.png]]

** Constant Fast Jack Velocity
<<sec:optimal_traj_const_fj_velocity>>

In this section, a scan with constant fast jack velocity is studied.

It was shown in Section [[sec:optimal_traj_dist]] that the maximum Fast Jack velocity should be 0.125mm/s in order for the frequency corresponding to the period of $5\,\mu m$ to be smaller than 25Hz.

Let's generate a trajectory between 5deg and 75deg Bragg angle with constant Fast Jack velocity at 0.125mm/s.
#+begin_src matlab
%% Compute extreme fast jack position
fj_max = 0.030427 - 10.5e-3/(2*cos(pi/180*5)); % Smallest FJ position [m]
fj_min = 0.030427 - 10.5e-3/(2*cos(pi/180*75)); % Largest FJ position [m]

%% Compute Fast Jack Trajectory
t = 0:0.1:(fj_max - fj_min)/max_fj_vel; % Time vector [s]
fj_pos = fj_max - t*max_fj_vel; % Fast Jack Position [m]

%% Compute corresponding Bragg trajectory
bragg_pos = acos(10.5e-3./(2*(0.030427 - fj_pos))); % [rad]
#+end_src

The Fast Jack position as well as the Bragg angle are shown as a function of time in Figure [[fig:trajectory_constant_fj_velocity]].
#+begin_src matlab :exports none
%% Trajectory with constant Fast Jack Velocity
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
plot(t, 1e3*fj_pos);
xlabel('Time [s]');
ylabel('Fast Jack Position [mm]')
xlim([t(1), t(end)])

ax2 = nexttile();
plot(t, 180/pi*bragg_pos);
xlabel('Time [s]');
ylabel('Bragg Angle [deg]');
xlim([t(1), t(end)])
#+end_src

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

#+name: fig:trajectory_constant_fj_velocity
#+caption: Trajectory with constant Fast Jack Velocity
#+RESULTS:
[[file:figs/trajectory_constant_fj_velocity.png]]

Let's now compute the Bragg angular velocity for this scan (Figure [[fig:trajectory_constant_fj_velocity_bragg_velocity]]).
It is shown that for large Fast Jack positions / small bragg angles, the bragg angular velocity is too large.
#+begin_src matlab :exports none
%% Bragg Angular Velocity
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(180/pi*bragg_pos(1:end-1), 180/pi*(bragg_pos(2:end)-bragg_pos(1:end-1))/0.1);
plot(180/pi*[bragg_pos(1), bragg_pos(end)], [max_bragg_vel, max_bragg_vel], 'r--');
plot(180/pi*[bragg_pos(1), bragg_pos(end)], [min_bragg_vel, min_bragg_vel], 'r--');
hold off;
xlabel('Bragg Angle [deg]');
ylabel('Bragg Angular Velocity [deg/s]');
ylim([0, 5]);

ax2 = nexttile();
hold on;
plot(1e3*fj_pos(1:end-1), 180/pi*(bragg_pos(2:end)-bragg_pos(1:end-1))/0.1);
plot(1e3*[fj_pos(1), fj_pos(end)], [max_bragg_vel, max_bragg_vel], 'r--');
plot(1e3*[fj_pos(1), fj_pos(end)], [min_bragg_vel, min_bragg_vel], 'r--');
hold off;
xlabel('Fast Jack Position [mm]');
ylabel('Bragg Angular Velocity [deg/s]');
ylim([0, 5]);
#+end_src

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

#+name: fig:trajectory_constant_fj_velocity_bragg_velocity
#+caption: Bragg Velocity as a function of the bragg angle or fast jack position
#+RESULTS:
[[file:figs/trajectory_constant_fj_velocity_bragg_velocity.png]]

#+begin_important
Between 45 and 70 degrees, the scan can be performed with *constant Fast Jack velocity* equal to 0.125 mm/s.
#+end_important

** Constant Bragg Angular Velocity
<<sec:optimal_traj_const_bragg_velocity>>

Let's now study a scan with a constant Bragg angular velocity of 1deg/s.

#+begin_src matlab
%% Time vector for the Scan with constant angular velocity
t = 0:0.1:(75 - 5)/max_bragg_vel; % Time vector [s]

%% Bragg angle during the scan
bragg_pos = 5 + t*max_bragg_vel; % Bragg Angle [deg]

%% Computation of the Fast Jack Position
fj_pos = 0.030427 - 10.5e-3./(2*cos(pi/180*bragg_pos)); % FJ position [m]
#+end_src

#+begin_src matlab :exports none
%% Trajectory with constant Fast Jack Velocity
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
plot(t, 1e3*fj_pos);
xlabel('Time [s]');
ylabel('Fast Jack Position [mm]')
xlim([t(1), t(end)])

ax2 = nexttile();
plot(t, bragg_pos);
xlabel('Time [s]');
ylabel('Bragg Angle [deg]');
xlim([t(1), t(end)])
#+end_src

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

#+name: fig:trajectory_constant_bragg_velocity
#+caption: Trajectory with constant Bragg angular velocity
#+RESULTS:
[[file:figs/trajectory_constant_bragg_velocity.png]]

#+begin_src matlab :exports none
%% Fast Jack Velocity
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(t(1:end-1), 1e3*abs(fj_pos(2:end)-fj_pos(1:end-1))/0.1);
plot([t(1), t(end)], 1e3*[max_fj_vel, max_fj_vel], 'r--');
plot([t(1), t(end)], 1e3*[min_fj_vel, min_fj_vel], 'r--');
hold off;
xlabel('Time [s]'); ylabel('Fast Jack Velocity [mm/s]');
xlim([t(1), t(end)]); ylim([0, 1]);

ax2 = nexttile();
hold on;
plot(bragg_pos(1:end-1), 1e3*abs(fj_pos(2:end)-fj_pos(1:end-1))/0.1);
plot([bragg_pos(1), bragg_pos(end)], 1e3*[max_fj_vel, max_fj_vel], 'r--');
plot([bragg_pos(1), bragg_pos(end)], 1e3*[min_fj_vel, min_fj_vel], 'r--');
hold off;
xlabel('Bragg Position [deg]'); ylabel('Fast Jack Velocity [mm/s]');
xlim([bragg_pos(1), bragg_pos(end)]); ylim([0, 1]);
#+end_src

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

#+name: fig:trajectory_constant_bragg_velocity_fj_velocity
#+caption: Fast Jack Velocity with a constant bragg angular velocity
#+RESULTS:
[[file:figs/trajectory_constant_bragg_velocity_fj_velocity.png]]

#+begin_important
Between 15 and 45 degrees, the scan can be performed with a *constant Bragg angular velocity* equal to 1 deg/s.
#+end_important

** Mixed Trajectory
<<sec:optimal_traj_const_bragg_velocity>>

Let's combine a scan with constant Bragg angular velocity for small bragg angles (< 44.3 deg) with a scan with constant Fast Jack velocity for large Bragg angle (> 44.3 deg).
The scan is performed from 5 degrees to 75 degrees.

Parameters for the scan are defined below:
#+begin_src matlab
%% Bragg Positions
bragg_start = 5; % Start Bragg angle [deg]
bragg_mid   = 44.3; % Transition between constant FJ vel and constant Bragg vel [deg]
bragg_end   = 75; % End Bragg angle [deg]

%% Fast Jack Positions
fj_start = 0.030427 - 10.5e-3/(2*cos(pi/180*bragg_start)); % Start FJ position [m]
fj_mid   = 0.030427 - 10.5e-3/(2*cos(pi/180*bragg_mid)); % Mid FJ position [m]
fj_end   = 0.030427 - 10.5e-3/(2*cos(pi/180*bragg_end)); % End FJ position [m]

%% Time vectors
Ts = 0.1; % Sampling Time [s]
t_c_bragg = 0:Ts:(bragg_mid-bragg_start)/max_bragg_vel; % Time Vector for constant bragg velocity [s]
t_c_fj = Ts+[0:Ts:(fj_mid-fj_end)/max_fj_vel]; % Time Vector for constant Fast Jack velocity [s]
#+end_src

Positions for the first part of the scan at constant Bragg angular velocity are computed:
#+begin_src matlab
%% Constant Bragg Angular Velocity
bragg_c_bragg = bragg_start + t_c_bragg*max_bragg_vel; % [deg]
fj_c_bragg = 0.030427 - 10.5e-3./(2*cos(pi/180*bragg_c_bragg)); % FJ position [m]
#+end_src

And positions for the part of the scan with constant Fast Jack Velocity are computed:
#+begin_src matlab
%% Constant Bragg Angular Velocity
fj_c_fj = fj_mid - t_c_fj*max_fj_vel; % FJ position [m]
bragg_c_fj = 180/pi*acos(10.5e-3./(2*(0.030427 - fj_c_fj))); % [deg]
#+end_src

#+begin_src matlab :exports none
%% Combine both segments
t = [t_c_bragg, t_c_fj+t_c_bragg(end)]; % Time [s]
bragg_pos = [bragg_c_bragg, bragg_c_fj]; % Bragg angle [deg]
fj_pos    = [fj_c_bragg,    fj_c_fj]; % FJ position [m]
#+end_src

Fast Jack position as well as Bragg angle are displayed as a function of time in Figure [[fig:combined_scan_trajectories]].

#+begin_src matlab :exports none
%% Fasj Jack position and Bragg angle as a function of time
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(t, 1e3*fj_pos)
xline(t_c_bragg(end), '-', {'Transition'}, ...
      'LineWidth', 2, 'LabelVerticalAlignment', 'bottom');
hold off;
xlabel('Time [s]'); ylabel('Fast Jack Position [mm]');

ax2 = nexttile();
hold on;
plot(t, bragg_pos)
xline(t_c_bragg(end), '-', {'Transition'}, ...
      'LineWidth', 2, 'LabelVerticalAlignment', 'bottom');
hold off;
xlabel('Time [s]'); ylabel('Bragg Angle [deg]');

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

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

#+name: fig:combined_scan_trajectories
#+caption: Fast jack trajectories and Bragg angular velocity during the scan
#+RESULTS:
[[file:figs/combined_scan_trajectories.png]]

The Fast Jack velocity as well as the Bragg angular velocity are shown as a function of the Bragg angle in Figure [[fig:combined_scan_velocities]].
#+begin_src matlab :exports none
%% Fasj Jack velocity and Bragg angular velocity as a function of time
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(bragg_pos(1:end-1), 1e3*abs(fj_pos(2:end)-fj_pos(1:end-1))/Ts)
plot([bragg_pos(1), bragg_pos(end)], 1e3*[max_fj_vel, max_fj_vel], 'r--');
plot([bragg_pos(1), bragg_pos(end)], 1e3*[min_fj_vel, min_fj_vel], 'r--');
hold off;
xlabel('Bragg Angle [deg]'); ylabel('Fast Jack Velocity [mm/s]');
xticks(5:5:75)

ax2 = nexttile();
hold on;
plot(bragg_pos(1:end-1), abs(bragg_pos(2:end)-bragg_pos(1:end-1))/Ts)
plot([bragg_pos(1), bragg_pos(end)], [max_bragg_vel, max_bragg_vel], 'r--');
plot([bragg_pos(1), bragg_pos(end)], [min_bragg_vel, min_bragg_vel], 'r--');
hold off;
xlabel('Bragg Angle [deg]'); ylabel('Bragg Velocity [deg/s]');
xticks(5:5:75)

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

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

#+name: fig:combined_scan_velocities
#+caption: Fast jack velocity and Bragg angular velocity during the scan
#+RESULTS:
[[file:figs/combined_scan_velocities.png]]

#+begin_important
From Figure [[fig:combined_scan_velocities]], it is shown that the fast jack velocity as well as the bragg angular velocity are within the bounds except:
- Below 15 degrees where the fast jack velocity is too small.
  The frequency of the non-linear periodic errors of the interferometers would be at too low frequency (in the pass-band of the filter, see Figure [[fig:optimal_lut_trajectory_frequencies]]).
  One easy option is to use an interferometer without periodic non-linearity.
  Another option is to increase the maximum Bragg angular velocity to 3 deg/s.
- Above 70 degrees where the Bragg angular velocity is too small.
  This may introduce low frequency disturbances induced by the =mcoil= motor that would be in the pass-band of the filter (see Figure [[fig:optimal_lut_trajectory_frequencies]]).
  It should be verified if this is indeed problematic of not.
  An other way would be to scan without the =mcoil= motor at very high bragg angle.
#+end_important

In order to better visualize the filtering problem, the frequency of all the signals are shown as a function of the Bragg angle during the scan in Figure [[fig:optimal_lut_trajectory_frequencies]].

#+begin_src matlab :exports none
%% Disturbances coming from mcoil
mcoil_freq = 50/0.2768*abs(bragg_pos(2:end)-bragg_pos(1:end-1))/Ts;

%% Errors Induced by the Fast Jack
fj_5u_freq  = (abs(fj_pos(2:end)-fj_pos(1:end-1))/Ts)/5e-6;
fj_10u_freq = (abs(fj_pos(2:end)-fj_pos(1:end-1))/Ts)/10e-6;
fj_20u_freq = (abs(fj_pos(2:end)-fj_pos(1:end-1))/Ts)/20e-6;

%% Periodic Non-Linearity of the interferometers
int_freq = (abs(fj_pos(2:end)-fj_pos(1:end-1))/Ts)/765e-9;

%% Constant Disturbances
dist_freq = 34*ones(size(int_freq));

%% Filtering Frequency
filt_freq = 30*ones(size(int_freq));
#+end_src

#+begin_src matlab :exports none
%% Frequencies of signals during trajectory
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(bragg_pos(1:end-1), fj_5u_freq, '-', 'color', colors(1,:), ...
    'DisplayName', 'FJ - $5\mu m$')
plot(bragg_pos(1:end-1), fj_10u_freq, '-.', 'color', colors(1,:), ...
    'DisplayName', 'FJ - $10\mu m$')
plot(bragg_pos(1:end-1), fj_20u_freq, '--', 'color', colors(1,:), ...
    'DisplayName', 'FJ - $20\mu m$')
plot(bragg_pos(1:end-1), mcoil_freq, '-', 'color', colors(2,:), ...
    'DisplayName', 'mcoil')
plot(bragg_pos(1:end-1), int_freq, '-.', 'color', colors(2,:), ...
    'DisplayName', 'Int.')
plot(bragg_pos(1:end-1), dist_freq, '--', 'color', colors(2,:), ...
    'DisplayName', 'Dist')
plot(bragg_pos(1:end-1), filt_freq, 'k-', ...
    'DisplayName', 'Filter')
hold off;
xlabel('Bragg Angle [deg]'); ylabel('Frequency [Hz]');
xlim([bragg_pos(1), bragg_pos(end)]);

ax2 = nexttile();
hold on;
plot(1e3*fj_pos(1:end-1), fj_5u_freq, '-', 'color', colors(1,:), ...
    'DisplayName', 'FJ - $5\mu m$')
plot(1e3*fj_pos(1:end-1), fj_10u_freq, '-.', 'color', colors(1,:), ...
    'DisplayName', 'FJ - $10\mu m$')
plot(1e3*fj_pos(1:end-1), fj_20u_freq, '--', 'color', colors(1,:), ...
    'DisplayName', 'FJ - $20\mu m$')
plot(1e3*fj_pos(1:end-1), mcoil_freq, '-', 'color', colors(2,:), ...
    'DisplayName', 'mcoil')
plot(1e3*fj_pos(1:end-1), int_freq, '-.', 'color', colors(2,:), ...
    'DisplayName', 'Int. Non-Lin')
plot(1e3*fj_pos(1:end-1), dist_freq, '--', 'color', colors(2,:), ...
    'DisplayName', 'Ext. Dist.')
plot(1e3*fj_pos(1:end-1), filt_freq, 'k-', ...
    'DisplayName', 'Filter Cut-off')
hold off;
xlabel('Fast Jack Position [mm]'); ylabel('Frequency [Hz]');
legend('location', 'northwest', 'FontSize', 8);
#+end_src

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

#+name: fig:optimal_lut_trajectory_frequencies
#+caption: Frequency of signals as a function of the Bragg angle and Fast Jack position
#+RESULTS:
[[file:figs/optimal_lut_trajectory_frequencies.png]]

* Piezoelectric LUT
** Introduction                                                      :ignore:

On main issue with implementing the LUT inside

The idea is to:
- Correct the low frequency errors inside the IcePAP (1mm period motor error).
- This should reduce the errors to an acceptable range (within the piezoelectric stack stroke).
- Correct the high frequency errors (periods of $5\,\mu m$, $10\,\mu m$ and $20\,\mu m$) using the piezoelectric stack.

* Generate =.dat= file                                              :noexport:
#+begin_src matlab
lut = 1e-3*[0:1:26000]'*ones(1, 4);
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no
data2orgtable(lut(1:10, :), {}, {}, ' %.1e ');
#+end_src

#+RESULTS:
|   0.0 |   0.0 |   0.0 |   0.0 |
| 0.001 | 0.001 | 0.001 | 0.001 |
| 0.002 | 0.002 | 0.002 | 0.002 |
| 0.003 | 0.003 | 0.003 | 0.003 |
| 0.004 | 0.004 | 0.004 | 0.004 |
| 0.005 | 0.005 | 0.005 | 0.005 |
| 0.006 | 0.006 | 0.006 | 0.006 |
| 0.007 | 0.007 | 0.007 | 0.007 |
| 0.008 | 0.008 | 0.008 | 0.008 |
| 0.009 | 0.009 | 0.009 | 0.009 |

#+begin_src matlab :results output replace
sprintf('%.18e %.18e %.18e %.18e\n', lut(1:10, :)')
#+end_src

#+RESULTS:
#+begin_example
ans =
    '0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00
     1.000000000000000021e-03 1.000000000000000021e-03 1.000000000000000021e-03 1.000000000000000021e-03
     2.000000000000000042e-03 2.000000000000000042e-03 2.000000000000000042e-03 2.000000000000000042e-03
     3.000000000000000062e-03 3.000000000000000062e-03 3.000000000000000062e-03 3.000000000000000062e-03
     4.000000000000000083e-03 4.000000000000000083e-03 4.000000000000000083e-03 4.000000000000000083e-03
     5.000000000000000104e-03 5.000000000000000104e-03 5.000000000000000104e-03 5.000000000000000104e-03
     6.000000000000000125e-03 6.000000000000000125e-03 6.000000000000000125e-03 6.000000000000000125e-03
     7.000000000000000146e-03 7.000000000000000146e-03 7.000000000000000146e-03 7.000000000000000146e-03
     8.000000000000000167e-03 8.000000000000000167e-03 8.000000000000000167e-03 8.000000000000000167e-03
     9.000000000000001055e-03 9.000000000000001055e-03 9.000000000000001055e-03 9.000000000000001055e-03
     '
#+end_example

#+begin_src matlab
%% Save lut as a .dat file
formatSpec = '%.18e %.18e %.18e %.18e\n';

fileID = fopen('test_lut.dat','w');
fprintf(fileID, formatSpec, lut');
fclose(fileID);
#+end_src

* TODO Repeatability of the motion                                  :noexport:
#+begin_src matlab
%% Load Data of the new LUT method
Ts = 0.1;

ol_drx_1 = 1e-9*double(h5read('xanes_0004.h5','/15.1/measurement/xtal_111_drx_filter')); % Rx [rad]
ol_drx_2 = 1e-9*double(h5read('xanes_0004.h5','/16.1/measurement/xtal_111_drx_filter')); % Rx [rad]
ol_drx_3 = 1e-9*double(h5read('xanes_0004.h5','/17.1/measurement/xtal_111_drx_filter')); % Rx [rad]
ol_drx_4 = 1e-9*double(h5read('xanes_0004.h5','/18.1/measurement/xtal_111_drx_filter')); % Rx [rad]

ol_dry_1 = 1e-9*double(h5read('xanes_0004.h5','/15.1/measurement/xtal_111_dry_filter')); % Ry [rad]
ol_dry_2 = 1e-9*double(h5read('xanes_0004.h5','/16.1/measurement/xtal_111_dry_filter')); % Ry [rad]
ol_dry_3 = 1e-9*double(h5read('xanes_0004.h5','/17.1/measurement/xtal_111_dry_filter')); % Ry [rad]
ol_dry_4 = 1e-9*double(h5read('xanes_0004.h5','/18.1/measurement/xtal_111_dry_filter')); % Ry [rad]

t = linspace(Ts, Ts*length(ol_drx_1), length(ol_drx_1));
#+end_src

#+begin_src matlab
figure;
hold on;
plot(t, ol_drx_1)
plot(t, ol_drx_2)
plot(t, ol_drx_3)
#+end_src

#+begin_src matlab
figure;
hold on;
plot(t, ol_dry_1)
plot(t, ol_dry_2)
plot(t, ol_dry_3)
#+end_src

#+begin_src matlab
%% Repeatable motion
ol_drx_mean = mean([ol_drx_1, ol_drx_2, ol_drx_4 ol_drx_3]')';
ol_dry_mean = mean([ol_dry_2, ol_dry_3, ol_dry_4]')';
#+end_src

#+begin_src matlab :results value replace
rms([ol_drx_1 - ol_drx_mean;
     ol_drx_2 - ol_drx_mean;
     ol_drx_3 - ol_drx_mean;
     ol_drx_4 - ol_drx_mean;])
#+end_src

#+RESULTS:
: 1.0335e-07

#+begin_src matlab :results value replace
rms([ol_dry_2 - ol_dry_mean;
     ol_dry_3 - ol_dry_mean;
     ol_dry_4 - ol_dry_mean;])
#+end_src

#+RESULTS:
: 9.7691e-08

#+begin_src matlab
figure;
hold on;
plot(t, ol_drx_1 - ol_drx_mean)
plot(t, ol_drx_2 - ol_drx_mean)
plot(t, ol_drx_3 - ol_drx_mean)
plot(t, ol_drx_4 - ol_drx_mean)
#+end_src

#+begin_src matlab
figure;
hold on;
plot(t, ol_dry_2 - ol_dry_mean)
plot(t, ol_dry_3 - ol_dry_mean)
plot(t, ol_dry_4 - ol_dry_mean)
#+end_src

* 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/src/'); % Path for functions
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
addpath('./src/'); % Path for functions
#+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

** =loadRepeatabiltyData=
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/loadRepeatabiltyData.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:loadRepeatabiltyData>>

*** Function description                                            :ignore:

Function description:

#+begin_src matlab -n
function [data] = loadRepeatabiltyData(filename, path)
% loadRepeatabiltyData -
%
% Syntax: [ref] = loadRepeatabiltyData(args)
%
% Inputs:
%    - args
%
% Outputs:
%    - ref - Reference Signal
#+end_src

*** Load Data                                                       :ignore:

#+begin_src matlab +n
data = struct();

data.bragg = (pi/180)*1e-6*double(h5read(filename, ['/', path, '/instrument/trajmot/data'])); % Bragg angle [rad]
data.dzw   = 10.5e-3./(2*cos(data.bragg)); % Wanted distance between crystals [m]

data.dz    = 1e-9*double(h5read(filename, ['/', path, '/instrument/xtal_111_dz/data' ])); % Dz distance between crystals [m]
data.dry   = 1e-9*double(h5read(filename, ['/', path, '/instrument/xtal_111_dry/data'])); % Ry [rad]
data.drx   = 1e-9*double(h5read(filename, ['/', path, '/instrument/xtal_111_drx/data'])); % Rx [rad]

data.t = 1e-6*double(h5read(filename, ['/', path, '/instrument/time/data'])); % Time [s]

data.ddz   = data.dzw-data.dz; % Distance Error between crystals [m]
#+end_src

** =computeErrorFJ=
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/computeErrorFJ.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:computeErrorFJ>>

*** Function description                                            :ignore:

Function description:

#+begin_src matlab -n
function [data] = computeErrorFJ(data)
% computeErrorFJ -
%
% Syntax: [data] = computeErrorFJ(data)
%
% Inputs:
%    - args
%
% Outputs:
%    - ref - Reference Signal
#+end_src

*** Load Data                                                       :ignore:

#+begin_src matlab +n
J_a_111 = [1,  0.14, -0.0675
           1,  0.14,  0.1525
           1, -0.14,  0.0425];

error = [-data.dzm, data.rym, data.rxm] * J_a_111;

data.fjur_e = error(:,1); % [m]
data.fjuh_e = error(:,2); % [m]
data.fjd_e  = error(:,3); % [m]
#+end_src

** =createLUT=
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/createLUT.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:createLUT>>

*** Function Description
#+begin_src matlab
function [] = createLUT(data_file, lut_file, args)
% createLUT -
%
% Syntax: createLUT(data_file, lut_file, args)
%
% Inputs:
%    - data_file - Where to load the .mat file
%    - lut_file - Where to save the .dat file
%    - args:
%       - plot   - Should plot data for diagnostic (default: false)
%       - update - Should update LUT file instead of creating from scratch (default: true)
#+end_src

*** Optional Parameters
#+begin_src matlab
arguments
    data_file
    lut_file
    args.plot   (1,1) logical {mustBeNumericOrLogical} = false
    args.update (1,1) logical {mustBeNumericOrLogical} = false
    args.fjsry  (1,1) double {mustBeNumeric} = 0.53171e-3 % [Rad]
    args.fjsrx  (1,1) double {mustBeNumeric} = 0.144e-3 % [Rad]
    args.FJ0    (1,1) double {mustBeNumeric} = 0.0304274021077198 % [m]
end
#+end_src

*** Load Data
#+begin_src matlab
%% Load Data
load(data_file)
#+end_src

#+begin_src matlab
%% Convert Data to Standard Units
% Bragg Angle [rad]
bragg = pi/180*bragg;
% Rx rotation of 1st crystal w.r.t. 2nd crystal [rad]
drx   = 1e-9*drx;
% Ry rotation of 1st crystal w.r.t. 2nd crystal [rad]
dry   = 1e-9*dry;
% Z distance between crystals [m]
dz    = 1e-9*dz;
% Z error between second crystal and first crystal [m]
ddz   = 10.5e-3./(2*cos(bragg)) - dz;
% Steps for Ur motor [m]
fjur  = 1e-8*fjur;
% Steps for Uh motor [m]
fjuh  = 1e-8*fjuh;
% Steps for D motor [m]
fjd   = 1e-8*fjd;
#+end_src

*** Compute the Fast Jack errors
#+begin_src matlab
%% Actuator Jacobian
J_a_111 = [1,  0.14, -0.0675
           1,  0.14,  0.1525
           1, -0.14,  0.0425];

%% Computation of the position of the FJ as measured by the interferometers
error = J_a_111 * [ddz, dry, drx]';

fjur_e = error(1,:)'; % [m]
fjuh_e = error(2,:)'; % [m]
fjd_e  = error(3,:)'; % [m]
#+end_src

*** Filtering of Fast Jack Errors
#+begin_src matlab
%% FIR Filter
Fs = round(1/((time(end)-time(1))/(length(time) - 1))); % Sampling Frequency [Hz]
fir_order = 1000; % Filter's order
delay = fir_order/2; % Delay induced by the filter
B_fir = firls(fir_order, ... % Filter's order
              [0 140/(Fs/2) 180/(Fs/2) 1], ... % Frequencies [Hz]
              [1 1          0          0]); % Wanted Magnitudes

%% Filtering all measured Fast Jack Position using the FIR filter
fjur_e_filt = filter(B_fir, 1, fjur_e);
fjuh_e_filt = filter(B_fir, 1, fjuh_e);
fjd_e_filt  = filter(B_fir, 1, fjd_e);

%% Compensation of the delay introduced by the FIR filter
fjur_e_filt(1:end-delay) = fjur_e_filt(delay+1:end);
fjuh_e_filt(1:end-delay) = fjuh_e_filt(delay+1:end);
fjd_e_filt( 1:end-delay) = fjd_e_filt( delay+1:end);
#+end_src

*** LUT Creation
#+begin_src matlab
%% Lut Initialization
lut = [0:1e-6:26e-3]'*ones(1,4);
#+end_src

#+begin_src matlab
%% Vector of Fast Jack positions [unit of lut_inc]
fjur_pos = floor(min(1e6*fjur)):floor(max(1e6*fjur));
fjuh_pos = floor(min(1e6*fjuh)):floor(max(1e6*fjuh));
fjd_pos  = floor(min(1e6*fjd )):floor(max(1e6*fjd ));
#+end_src

#+begin_src matlab
%% Build the LUT
for i = fjur_pos
    % Find indices where measured motion is close to the wanted one
    indices = fjur + fjur_e_filt > lut(i,1) - 500e-9 & ...
              fjur + fjur_e_filt < lut(i,1) + 500e-9;
    % Poputate the LUT with the mean of the IcePAP steps
    if sum(indices) > 0
        lut(i,2) = mean(fjur(indices));
    end
end

for i = fjuh_pos
    % Find indices where measuhed motion is close to the wanted one
    indices = fjuh + fjuh_e_filt > lut(i,1) - 500e-9 & ...
              fjuh + fjuh_e_filt < lut(i,1) + 500e-9;
    % Poputate the LUT with the mean of the IcePAP steps
    if sum(indices) > 0
        lut(i,3) = mean(fjuh(indices));
    end
end

for i = fjd_pos
    % Poputate the LUT with the mean of the IcePAP steps
    indices = fjd + fjd_e_filt > lut(i,1) - 500e-9 & ...
              fjd + fjd_e_filt < lut(i,1) + 500e-9;
    % Poputate the LUT
    if sum(indices) > 0
        lut(i,4) = mean(fjd(indices));
    end
end
#+end_src

#+begin_src matlab
%% Convert from [m] to [mm]
lut = 1e3*lut;
#+end_src

*** Save the LUT
#+begin_src matlab
%% Save lut as a .dat file
formatSpec = '%.18e %.18e %.18e %.18e\n';

fileID = fopen(lut_file, 'w');
fprintf(fileID, formatSpec, lut');
fclose(fileID);
#+end_src

** =extractDatData=
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/extractDatData.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:extractDatData>>

*** Function Description
#+begin_src matlab
function [data_struct] = extractDatData(dat_file, names, scale)
% extractDatData -
%
% Syntax: extractDatData(data_file, lut_file, args)
%
% Inputs:
%    - data_file - Where to load the .mat file
%    - lut_file - Where to save the .dat file
#+end_src

*** Load Data
#+begin_src matlab
%% Load Data
data_array = importdata(dat_file);
#+end_src

#+begin_src matlab
%% Initialize Struct
data_struct = struct();
#+end_src

#+begin_src matlab
%% Populate Struct
for i = 1:length(names)
      data_struct.(names{i}) = scale(i)*data_array(:,i);
end
#+end_src

* Bibliography                                                        :ignore:
#+latex: \printbibliography