dcm-stepper-calibration/dcm-stepper-calibration.org

#+TITLE: ESRF Double Crystal Monochromator - Compensating Repeatable Positioning Errors of Fast Jacks
: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:jupyter-python  :session *PYTHON*
#+PROPERTY: header-args:jupyter-python+ :kernel python3
#+PROPERTY: header-args:jupyter-python+ :async yes
#+PROPERTY: header-args:jupyter-python+ :comments org
#+PROPERTY: header-args:jupyter-python+ :exports both
#+PROPERTY: header-args:jupyter-python+ :results none
#+PROPERTY: header-args:jupyter-python+ :tangle no
#+PROPERTY: header-args:jupyter-python+ :eval no-export
#+PROPERTY: header-args:jupyter-python+ :noweb yes
#+PROPERTY: header-args:jupyter-python+ :mkdirp yes
#+PROPERTY: header-args:jupyter-python+ :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:
This document summarizes the studies done on the compensation of repeatable errors of the Fast Jacks.

Each Fast Jack is composed of one stepper motor directly driving (i.e. without any reducer) a ball screw with a pitch of 1mm (i.e. 1 stepper motor turn makes a 1mm linear motion).

When scanning using the fast jack without any sort of control (i.e. in =mode A=), rather large positioning errors can be measured by the interferometers.
Some of these errors are repeatable while other are not repeatable (see Section [[sec:fj_repeat]]).

It is here studied how to measure these repeatable positioning errors and how to compensate them using a Lookup Table (LUT).
This functioning mode is called =mode B=.

Then there is a piezoelectric stack in series with the fast-jack which is working in closed-loop with the interferometer signals and that is used to compensate the remaining (mostly non-repeatable) errors induced by the stepper motor and other disturbances.
This is called =mode C=.

The compensation of repeatable errors using the Lookup Tables has several goals:
- Reducing the positioning errors below the stroke of the piezoelectric stack actuator.
  Otherwise the stroke of the piezoelectric stack in mode C (feedback control) could be too small and errors cannot be further controlled.
- Reducing the errors above the bandwidth of the feedback controller.
  The bandwidth of the feedback controller is limited by the mechanical behavior of the DCM, and therefore vibrations outside this bandwidth can only be compensated using calibration / lookup tables.

The general procedure to compute and use the LUT is shown in Figure [[fig:lut_process_steps]].
Note that there is some exchange of information between each step indicated by the arrow and some =.dat= file containing the data.
It can separated into four main steps:
1. Perform a scan in mode A in order to properly measure the Fast Jack motion errors.
   The scan should be done in such a way that the motion errors of the Fast Jack can be separated from the other disturbances and non-repeatable errors by the use of filtering.
   This is the subject of Sections [[sec:optimal_traj_dist]] and [[sec:constant_fj_vel]]
2. Compute the LUT from the measured errors.
   For each Fast Jack, the LUT associates the wanted position with the corresponding IcePAP step at which the Fast Jack is effectively at the correct position (as measured during the previous scan).
   This is discussed in Section [[sec:proposed_lut_computation]] and the software implementation is described in Section [[sec:software_implementation]].
   The LUT data is stored in a =lut.dat= file and can be further loaded in the next step.
3. Generate a trajectory.
   The trajectory links several motors for synchronization (mainly bragg with fast jacks).
   The LUT data is included in this trajectory such that the measured repeatable errors that are included in the LUT are compensated.
   This is discussed in Section [[sec:optimal_trajectory_mode_B]].
4. Make a scan in mode B.
   The IcePAP is moving all motors in a synchronized way and tries to follow the trajectory data with included compensation of repeatable errors.

The Hardware and Software setup used for the measurement and tests of the lookup table is described in Section [[sec:lut_hardware_software]].

For each of these steps, several problems can lead to inaccuracies in the computed LUT and trajectory which will result in non optimal compensation of repeatable errors during a scan in mode B.
In order to have the best possible mode B positioning accuracy, each of these problems are studied in this document.

A comparison between the way the LUT was built before December 2021 and after is performed in Section [[sec:proposed_lut_computation]].
Complete process from measurement of Fast-Jack errors to the tests in mode B is described in Section [[sec:lut_experimental_data]].

As the DCM will be used for X-ray absorption techniques such as XANES, recommended scan parameters are given in Section [[sec:lut_xanes]].

#+begin_src latex :file lut_process_steps.pdf
\footnotesize
\newtcolorbox{mybox}[1][]{enhanced, size=fbox, colframe=colorblue, colback=lightblue, fonttitle=\bfseries, on line, equal height group=C,#1}
\setlist[itemize]{leftmargin=4mm}

\begin{mybox}[title=Measure of FJ errors,nobeforeafter,width=\linewidth/5,remember as=one]
  \textbf{Goal}:
  \begin{itemize}
    \item Make scan that will allow the separation of the FJ errors from disturbances and noise
  \end{itemize}
  \tcblower
  \textbf{Possible issues}:
  \begin{itemize}
    \item Measurement of non-repeatable errors
    \item Disturbances at the same frequency as FJ errors $\rightarrow$ no separation possible
  \end{itemize}
\end{mybox}
\hfill
\begin{mybox}[title=LUT Computation,nobeforeafter,width=\linewidth/5,remember as=two]
  \textbf{Goal}:
  \begin{itemize}
    \item Extract FJ repeatable errors (filtering)
    \item Associate wanted FJ position to IcePAP step
  \end{itemize}
  \tcblower
  \textbf{Possible issues}:
  \begin{itemize}
    \item Inclusion of other data than repeatable errors
    \item Non smooth lookup table
  \end{itemize}
\end{mybox}
\hfill
\begin{mybox}[title=Trajectory Generation,nobeforeafter,width=\linewidth/5,remember as=three]
  \textbf{Goal}:
  \begin{itemize}
    \item Associate FJ steps with other motors (for instance Bragg)
    \item Integrate LUT data in the FJ steps
  \end{itemize}
  \tcblower
  \textbf{Possible issues}:
  \begin{itemize}
    \item Interpolation errors due to limited number of points in the LUT
  \end{itemize}
\end{mybox}
\hfill
\begin{mybox}[title=IcePAP FJ Control,nobeforeafter,width=\linewidth/5,remember as=four]
  \textbf{Goal}:
  \begin{itemize}
    \item Control the position of the FJ motors based on the trajectory data
  \end{itemize}
  \tcblower
  \textbf{Possible issues}:
  \begin{itemize}
    \item Interpolation errors due to limited number of points in the trajectory
  \end{itemize}
\end{mybox}

\begin{tikzpicture}[overlay,remember picture,line width=1mm]
  \draw[->] (one.east) -- node[midway, above] {\texttt{err.dat}} (two.west);
  \draw[->] (two.east) -- node[midway, above] {\texttt{lut.dat}} (three.west);
  \draw[->] (three.east) -- node[midway, above] {\texttt{traj.dat}} (four.west);
\end{tikzpicture}
#+end_src

#+name: fig:lut_process_steps
#+attr_latex: :width \linewidth
#+caption: Overview of the process to make the LUT and associated possible issues.
#+RESULTS:
[[file:figs/lut_process_steps.png]]

* Hardware and Software Implementation
<<sec:lut_hardware_software>>
** Introduction                                                      :ignore:
In this section, a brief description of the experimental setup required to computed the Lookup Tables is given (Section [[sec:lut_implementation]]).

It is important also to see how the trajectories and Lookup Tables are computed and implemented in terms of software in order to understand the possible limitations.
This is described in Section [[sec:meas_setup]].

** Measurement setup
<<sec:meas_setup>>

In order to measure the errors induced by the fast jacks, scans have to be made, and the following signals have to be measured simultaneously:
- The wanted fast jack position: step sent by the IcePAP
- The actual (measured) position

The experimental setup is schematically shown in Figure [[fig:block_diagram_lut_stepper]].

The procedure is the following:
- A Bragg angle trajectory $\theta$ is generated from Bliss and loaded in the IcePAP as a kind of lookup table.
  This lookup table is only used to synchronize all the motors, and no compensation of errors are implemented.
- 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.
  The transformation from interferometers values to position and orientation errors of crystals is performed inside the Speedgoat.
- Finally, the corresponding motion $[d_{u_r},\ r_{u_h},\ r_d]$ of the each fast jack is computed afterwards in BLISS.

In order to create the LUT, the measured motion of the fast jacks $[d_{u_r},\ r_{u_h},\ r_d]$ and the IcePAP steps $[u_{u_r},\ u_{u_h},\ u_{d}]$ have to be measured simultaneously.

#+begin_src latex :file block_diagram_lut_stepper.pdf :exports results
\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}{2.9cm}, right=1.1 of traj] (lut) {\rotatebox{90}{\parbox[c]{2.0cm}{\centering Lookup Table}}};

  \node[block={1.5cm}{0.8cm}, right=1.1 of lut] (fj_uh) {FJ $u_r$};
  \node[block={1.5cm}{0.8cm}, above=0.2 of fj_uh]  (fj_ur) {FJ $u_h$};
  \node[block={1.5cm}{0.8cm}, below=0.2 of fj_uh]  (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[->] (traj.east) --node[midway, above]{\rotatebox{90}{Trajectory}} (lut.west);

  \draw[->] (lut.east|-fj_ur) --node[midway, above]{$u_{u_r}$} (fj_ur.west);
  \draw[->] (lut.east|-fj_uh) --node[midway, above]{$u_{u_h}$} (fj_uh.west);
  \draw[->] (lut.east|-fj_d)  --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.south west) (lut.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]]

** LUT Implementation
<<sec:lut_implementation>>

The computation of the LUT consists of generating an array with 4 columns.
The first column corresponds to the position (in mm) where it is wanted to position the Fast Jack.
The remaining three columns are corresponding (for each motor: =fjpur=, =fjpuh= and =fjpd=) to the position (i.e. step) where the IcePAP should position the motors such that the real position is corresponding to the first column.
This array =lut.dat= can have as many lines as wanted.

In BLISS, it is specified where the LUT is stored using the following command:
#+begin_src python
dcm.lut.load(data_file="lut.dat", data_dir="directory_where_lut_are_stored")
#+end_src

Then, to use the LUT, a trajectory has to be loaded with the =use_lut=True= parameter:
#+begin_src python
dcm.trajectory.load_bragg(12, 18, 1000, use_lut=True)
#+end_src

To perform the trajectory (synchronization of several motors), a "trajectory motor" is used in the IcePAP.
This motor is virtual and is used to synchronize the following motors: =mbrag=, =msafe=, =mcoil=, =fjsur=, =fjsuh= and =fjsd=.
To specify how to do the trajectory, an array with 7 columns is used.
The first column corresponds to the "trajectory motor" (i.e. Bragg, FJS, Energy, ...).
The remaining 6 columns are the 6 real motors that have to be synchronized.
Values are computed based on theoretical positions.

The lines of this array are separated with an constant =fjs= increment which is specified by the parameter =1000= when loading the trajectory.
The parameter =1000= indicates that the trajectory should contains 1000 points every millimeter of the Fast Jack motion.
In that case, the trajectory will be specific for every micrometer of fast jack motion.
Note that the loaded points of the trajectory are always with constant Fast Jack motion increment even though the trajectory is made over Bragg angle or energy.

Then, if =use_lut=True= is used, the LUT data will be integrated in the motor trajectory by modifying the columns corresponding to the =fjsur=, =fjsuh= and =fjsd= motors.
For every point in the trajectory:
- the data in the LUT corresponding to the wanted position of the fast-jack is found
- a linear interpolation between the two adjacent points is performed, and the result is loaded in the array

Then, when performing a trajectory, the IcePAP will use the loaded data (including the LUT information) to control the position of each motor.
Spline interpolation is performed between the specified points in the LUT.


#+begin_important
Therefore, several errors can be introduced even though the LUT is computed from perfect data:
- Linear interpolation of the LUT when computing the trajectory points can result in large errors if not enough points are used in the LUT
- Spline interpolation in the IcePAP can introduce errors
#+end_important

* Initial and proposed LUT computations
<<sec:proposed_lut_computation>>
** Introduction                                                      :ignore:
** 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]]

** Period of errors
**** Introduction                                                  :ignore:
The positioning errors of the fast jacks have different origins with different spatial periods:
- $1mm$ due to non-perfect planetary roller screw system
- $20\mu m$, $10\mu m$ and $5\mu m$ periods due non-perfect magnetic poles of the stepper motor.

In this section, we wish to see which of these errors are repeatable from one scan to the other.
This could help to determine which errors should be included in the LUT.

**** Load Test Data
A scan in mode A is performed at constant Fast Jack velocity.
#+begin_src matlab :exports none
%% Load and process measured data
data = extractDatData(sprintf("%s/21Nov/blc13420/id21/LUT_constant_fj_vel/%s", data_directory, "lut_const_fj_vel_14012022_1517.dat"), ...
                      {"bragg", "dz", "dry", "drx", "fjur", "fjuh", "fjd"}, ...
                      [pi/180, 1e-9, 1e-9, 1e-9, 1e-8, 1e-8, 1e-8]);
data = processMeasData(data);
#+end_src

#+begin_src matlab :exports none
%% Set the initial error to be zero (for ease of filtering)
data.fjur_e = data.fjur_e - data.fjur_e(1);
data.fjuh_e = data.fjuh_e - data.fjuh_e(1);
data.fjd_e = data.fjd_e - data.fjd_e(1);
#+end_src

#+begin_src matlab
fj_vel = 0.125e-3; % Fast Jack Velocity [m/s]
#+end_src

**** FIR Filters

#+begin_src matlab :exports none
%% Filter parameter
Fs = 1e4; % Sampling Frequency [Hz]
fir_order = 5000; % Filter's order
delay = fir_order/2; % Delay induced by the filter

%% 1mm FIR filter
B_fir_1mm = firls(fir_order, ... % Filter's order
                  [0 1/(Fs/2) 5/(Fs/2) 1], ... % Frequencies [Hz]
                  [1 1        0        0]); % Wanted Magnitudes
[h_fir_1mm,  f] = freqz(B_fir_1mm,  1, 10000, Fs);

%% 20um FIR Filter
B_fir_20um = firls(fir_order, ... % Filter's order
                  [0 (fj_vel)/(20e-6)/(Fs/2) 1.5*(fj_vel)/(20e-6)/(Fs/2) 1], ... % Frequencies [Hz]
                  [1 1        0        0]); % Wanted Magnitudes
[h_fir_20um,  ~] = freqz(B_fir_20um,  1, 10000, Fs);

%% 10um FIR Filter
B_fir_10um = firls(fir_order, ... % Filter's order
                  [0 (fj_vel)/(10e-6)/(Fs/2) 1.5*(fj_vel)/(10e-6)/(Fs/2) 1], ... % Frequencies [Hz]
                  [1 1        0        0]); % Wanted Magnitudes
[h_fir_10um,  ~] = freqz(B_fir_10um,  1, 10000, Fs);

%% 5um FIR filter
B_fir_5um = firls(fir_order, ... % Filter's order
                  [0 (fj_vel)/(5e-6)/(Fs/2) 1.5*(fj_vel)/(5e-6)/(Fs/2) 1], ... % Frequencies [Hz]
                  [1 1        0        0]); % Wanted Magnitudes
[h_fir_5um,  ~] = freqz(B_fir_5um,  1, 10000, Fs);
#+end_src

#+begin_src matlab :exports none
%% Bode plot of FIR Filter
figure;
hold on;
plot(f, abs(h_fir_1mm), ...
    'DisplayName', '1mm FIR');
plot(f, abs(h_fir_20um), ...
    'DisplayName', '$20\mu$m FIR');
plot(f, abs(h_fir_10um), ...
    'DisplayName', '$10\mu$m FIR');
plot(f, abs(h_fir_5um), ...
    'DisplayName', '$5\mu$m FIR');
xline(fj_vel/1e-3, 'k--', '1mm error', ...
      'LineWidth', 2, 'LabelVerticalAlignment', 'bottom', 'HandleVisibility', 'off');
xline(fj_vel/20e-6, 'k--', '$20um$ Error', ...
      'LineWidth', 2, 'LabelVerticalAlignment', 'bottom', 'HandleVisibility', 'off');
xline(fj_vel/10e-6, 'k--', '$10um$ Error', ...
      'LineWidth', 2, 'LabelVerticalAlignment', 'bottom', 'HandleVisibility', 'off');
xline(fj_vel/5e-6, 'k--', '$5um$ Error', ...
      'LineWidth', 2, 'LabelVerticalAlignment', 'bottom', 'HandleVisibility', 'off');
hold off;
xlabel('Frequency [Hz]'); ylabel('Amplitude');
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'log');
xlim([0, 50]); ylim([2e-5, 2e0]);
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
#+end_src

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

#+name: fig:fir_response_1mm_to_5um
#+caption: Amplitude response of FIR filters to only keep certain errors
#+RESULTS:
[[file:figs/fir_response_1mm_to_5um.png]]

**** Filtered Data
Now the same data is filtered with each filter.

The filtered data are shown in Figure [[fig:remain_motion_5um]].

#+begin_src matlab :exports none
%% Filter data - 1mm
data.fjur_e_1mm = filter(B_fir_1mm, 1, data.fjur_e);
data.fjuh_e_1mm = filter(B_fir_1mm, 1, data.fjuh_e);
data.fjd_e_1mm  = filter(B_fir_1mm, 1, data.fjd_e);

data.fjur_e_1mm(1:end-delay) = data.fjur_e_1mm(delay+1:end);
data.fjuh_e_1mm(1:end-delay) = data.fjuh_e_1mm(delay+1:end);
data.fjd_e_1mm( 1:end-delay) = data.fjd_e_1mm( delay+1:end);

%% Filter data - 20um
data.fjur_e_20um = filter(B_fir_20um, 1, data.fjur_e);
data.fjuh_e_20um = filter(B_fir_20um, 1, data.fjuh_e);
data.fjd_e_20um  = filter(B_fir_20um, 1, data.fjd_e);

data.fjur_e_20um(1:end-delay) = data.fjur_e_20um(delay+1:end);
data.fjuh_e_20um(1:end-delay) = data.fjuh_e_20um(delay+1:end);
data.fjd_e_20um( 1:end-delay) = data.fjd_e_20um( delay+1:end);

%% Filter data - 10um
data.fjur_e_10um = filter(B_fir_10um, 1, data.fjur_e);
data.fjuh_e_10um = filter(B_fir_10um, 1, data.fjuh_e);
data.fjd_e_10um  = filter(B_fir_10um, 1, data.fjd_e);

data.fjur_e_10um(1:end-delay) = data.fjur_e_10um(delay+1:end);
data.fjuh_e_10um(1:end-delay) = data.fjuh_e_10um(delay+1:end);
data.fjd_e_10um( 1:end-delay) = data.fjd_e_10um( delay+1:end);

%% Filter data - 5um
data.fjur_e_5um = filter(B_fir_5um, 1, data.fjur_e);
data.fjuh_e_5um = filter(B_fir_5um, 1, data.fjuh_e);
data.fjd_e_5um  = filter(B_fir_5um, 1, data.fjd_e);

data.fjur_e_5um(1:end-delay) = data.fjur_e_5um(delay+1:end);
data.fjuh_e_5um(1:end-delay) = data.fjuh_e_5um(delay+1:end);
data.fjd_e_5um( 1:end-delay) = data.fjd_e_5um( delay+1:end);
#+end_src

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

ax1 = nexttile();
hold on;
plot(1e3*data.dz, 1e6*data.fjur_e, ...
    'DisplayName', 'Raw Data');
plot(1e3*data.dz, 1e6*data.fjur_e_5um, ...
    'DisplayName', '$5\mu$m errors');
plot(1e3*data.dz, 1e6*data.fjur_e_10um, ...
    'DisplayName', '$10\mu$m errors');
plot(1e3*data.dz, 1e6*data.fjur_e_20um, ...
    'DisplayName', '$20\mu$m errors');
plot(1e3*data.dz, 1e6*data.fjur_e_1mm, ...
    'DisplayName', '1mm errors');
hold off;
xlabel('Fast Jack Motion [mm]'); ylabel('Measured Error [$\mu$m]')
xlim([9, 15]);
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile();
hold on;
plot(1e3*data.dz, 1e6*data.fjur_e);
plot(1e3*data.dz, 1e6*data.fjur_e_5um);
plot(1e3*data.dz, 1e6*data.fjur_e_10um);
plot(1e3*data.dz, 1e6*data.fjur_e_20um);
plot(1e3*data.dz, 1e6*data.fjur_e_1mm);
hold off;
xlabel('Fast Jack Motion [mm]'); ylabel('Measured Error [$\mu$m]')
xlim([11.98, 12.02]);
#+end_src

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

#+name: fig:remain_motion_5um
#+caption: Fast Jack measured error and filtered data
#+RESULTS:
[[file:figs/remain_motion_5um.png]]

**** Discussion

* LUT creation from experimental data
<<sec:lut_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

** Plots for Presentation                                          :noexport:
#+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

#+begin_src matlab :exports none :results none
%% Bragg Angle
figure;
plot(time, 180/pi*bragg)
xlabel('Time [s]'); ylabel('Bragg Angle [deg]');

yyaxis right
bragg_vel = lsim(G_diff, 180/pi*bragg, time);
plot(time(5000:end), abs(bragg_vel(5000:end)));
ylabel('Angular Velocity [deg]/s]');

xlim([0,70])
#+end_src

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

#+name: fig:first_scan_bragg_angle
#+caption: Bragg Angle
#+RESULTS:
[[file:figs/first_scan_bragg_angle.png]]

#+begin_src matlab :exports none :results none
%% Fast Jack
figure;
plot(time, 1e3*fjur)
xlabel('Time [s]'); ylabel('Fast Jack Displacement [mm]');

yyaxis right
fjur_vel = lsim(G_diff, 1e3*fjur, time);
plot(time(5000:end), abs(fjur_vel(5000:end)));
ylabel('Velocity [mm/s]');

xlim([0,70]);
#+end_src

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

#+name: fig:first_scan_fast_jack
#+caption: Fast Jack
#+RESULTS:
[[file:figs/first_scan_fast_jack.png]]

#+begin_src matlab :exports none
%% Hanning window used for pwelch function
Ts = 1e-4;
win = hanning(0.5/Ts);

%% Compute PSD of initial motion error
[Sxx, f] = pwelch(detrend(fjur_e(time<2.2), 0), win, 0, [], 1/Ts);
#+end_src

#+begin_src matlab
figure;
plot(f, sqrt(Sxx));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
#+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('Time [s]'); ylabel('Bragg Angle [deg]');
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 :exports none
%% Error Between generated Bragg steps and measured angle
figure;
hold on;
plot(time, 180/pi*(bragg_icepap - bragg), '.', ...
    'DisplayName', 'IcePAP Steps')
hold off;
xlabel('Time [s]'); ylabel('Bragg Angle [deg]');
% xlim([3.18, 3.19]);
legend('location', 'southeast');
#+end_src

#+begin_src matlab :exports 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(2, 1, '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
<<sec:icepap_lut_interpolation_errors>>
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]]

#+begin_important
In order to limit the errors due to spline interpolation, more points in the LUT should be included (ideally one point every 100nm).
This only makes the computation of the LUT a little bit longer.
#+end_important

* Position Repeatability
<<sec:fj_repeat>>
** Introduction                                                      :ignore:
In this section, the repeatability of the Fast Jacks over time is studied.

The goal is to determine:
1. How good the positioning accuracy can be when using the Lookup Table to correct the non-repeatability of the fast jack motion (i.e. mode B)?
2. During how long the lookup table are remaining valid?
3. Which errors are repeatable and which are not?

The trajectories to test the repeatability is the following:
#+begin_src python :eval no
tdh.lut_constant_fj_vel(15, 22, pts_per_mm=1000, use_lut=False)
#+end_src

The Fast Jack are scanned at constant velocity from 22mm to 15mm in mode A (no LUT).
The velocity is set to 0.125mm/s.

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

** Repeatability over several minutes
<<sec:fj_repeat_minutes>>

10 scans are done one after the other in mode A.
#+begin_src matlab
%% Filenames for the measurements
data_files_min = {
    "lut_const_fj_vel_14012022_1517.dat",
    "lut_const_fj_vel_14012022_1519.dat",
    "lut_const_fj_vel_14012022_1521.dat",
    "lut_const_fj_vel_14012022_1523.dat",
    "lut_const_fj_vel_14012022_1525.dat",
    "lut_const_fj_vel_14012022_1527.dat",
    "lut_const_fj_vel_14012022_1528.dat",
    "lut_const_fj_vel_14012022_1530.dat",
    "lut_const_fj_vel_14012022_1532.dat",
    "lut_const_fj_vel_14012022_1534.dat"
};
#+end_src

The data are filtered such that most of the disturbances and noise are filtered out.
There is only the motion errors induced by the fast jack left in the data.
#+begin_src matlab :exports none
%% Load and process measured data
data_min = {};

for i = 1:length(data_files_min)
    data_min{i} = extractDatData(sprintf("%s/21Nov/blc13420/id21/LUT_constant_fj_vel/%s", data_directory, data_files_min{i}), ...
                         {"bragg", "dz", "dry", "drx", "fjur", "fjuh", "fjd"}, ...
                         [pi/180, 1e-9, 1e-9, 1e-9, 1e-8, 1e-8, 1e-8]);
    data_min{i} = processMeasData(data_min{i});
    file_name = data_files_min{i}.char;
    data_min{i}.date = datetime(file_name(end-16:end-4),'Format','ddMMyyyy_HHmm');;
end
#+end_src

The measured position errors of =fjur= are shown in Figure [[fig:repeat_error_over_min]] for the 10 scans.
#+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*data_min{2}.fjur, 1e6*(data_min{2}.fjur_e_filt), 'color', [colors(1,:),0.5], ...
    'DisplayName', 'Scans');
for i = 3:length(data_files_min)
    plot(1e3*data_min{i}.fjur, 1e6*(data_min{i}.fjur_e_filt), 'color', [colors(1,:),0.5], ...
        'HandleVisibility', 'off');
end
plot(1e3*data_min{1}.fjur, 1e6*(data_min{1}.fjur_e_filt), 'k-', ...
    'DisplayName', 'First Scan');
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Position Error [$\mu$m]');
legend('location', 'southeast', 'FontSize', 8);
xlim([15.5, 21.5]);
ylim([2, 6]);

ax2 = nexttile();
hold on;
plot(1e3*data_min{2}.fjur, 1e6*(data_min{2}.fjur_e_filt), 'color', [colors(1,:),0.5], ...
    'DisplayName', 'Scans');
for i = 3:length(data_files_min)
    plot(1e3*data_min{i}.fjur, 1e6*(data_min{i}.fjur_e_filt), 'color', [colors(1,:),0.5], ...
        'HandleVisibility', 'off');
end
plot(1e3*data_min{1}.fjur, 1e6*(data_min{1}.fjur_e_filt), 'k-', ...
    'DisplayName', 'First Scan');
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Position Error [$\mu$m]');
legend('location', 'southeast', 'FontSize', 8);
xlim([15.98, 16.02]);
ylim([2, 3])
#+end_src

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

#+name: fig:repeat_error_over_min
#+caption: Repeatability of =fjur= over several minutes
#+RESULTS:
[[file:figs/repeat_error_over_min.png]]

The non-repeatable part (measured motion of scan i minus the measured motion of the first scan) is shown in Figure [[fig:repeat_error_over_min_non_repeat_part]].
Visually, we see that we cannot expect the positioning errors to be less than several hundreds of nanometers in mode B.
#+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;
for i = 2:length(data_files_min)
    plot(1e3*data_min{1}.fjur_resampl,  1e6*(data_min{i}.fjur_e_resampl - data_min{1}.fjur_e_resampl), 'color', [colors(1,:), 0.5]);
end
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Non-Repeatable Part [$\mu$m]');
xlim([15.5, 21.5]);

ax2 = nexttile();
hold on;
for i = 2:length(data_files_min)
    plot(1e3*data_min{1}.fjur_resampl,  1e6*(data_min{i}.fjur_e_resampl - data_min{1}.fjur_e_resampl), 'color', [colors(1,:), 0.5]);
end
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Non-Repeatable Part [$\mu$m]');
xlim([15.98, 16.02]);

linkaxes([ax1, ax2], 'y');
ylim([-0.4, 0.4]);
#+end_src

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

#+name: fig:repeat_error_over_min_non_repeat_part
#+caption: Non Repeatable part over several minutes
#+RESULTS:
[[file:figs/repeat_error_over_min_non_repeat_part.png]]

The RMS value of the non repeatable part is computed for each scan and summarized in Table [[tab:repeat_error_min]].
It is visually shown in Figure [[fig:repeat_error_fct_time_min]].

Clearly, the error is getting worse as more scan are performed and/or as elapse time is longer.

#+begin_src matlab :exports none
%% Compute the RMS of the remaining motion
elapse_min   = cellfun(@(x) etime(datevec(x.date),datevec(data_min{1}.date))/60, data_min);
fjur_min_rms = cellfun(@(x) rms(detrend(x.fjur_e_resampl - data_min{1}.fjur_e_resampl, 0)), data_min);
fjuh_min_rms = cellfun(@(x) rms(detrend(x.fjuh_e_resampl - data_min{1}.fjuh_e_resampl, 0)), data_min);
fjd_min_rms  = cellfun(@(x) rms(detrend(x.fjd_e_resampl  - data_min{1}.fjd_e_resampl,  0)), data_min);
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([elapse_min(2:end); 1e9*fjur_min_rms(2:end); 1e9*fjuh_min_rms(2:end); 1e9*fjd_min_rms(2:end)]', {}, {'Elapse Time [m]', '=fjur= [nm]', '=fjuh= [nm]', '=fjd= [nm]'}, ' %.1f ');
#+end_src

#+name: tab:repeat_error_min
#+caption: RMS value of the Non-repeatable part when during several identical scans within few minutes
#+attr_latex: :environment tabularx :width \linewidth :align lXXX
#+attr_latex: :center t :booktabs t
#+RESULTS:
| Elapse Time [m] | =fjur= [nm] | =fjuh= [nm] | =fjd= [nm] |
|-----------------+-------------+-------------+------------|
|             2.0 |        47.9 |        32.4 |       38.1 |
|             4.0 |        62.5 |        42.0 |       54.1 |
|             6.0 |        74.7 |        50.4 |       66.0 |
|             8.0 |        85.8 |        53.7 |       81.3 |
|            10.0 |        95.0 |        59.9 |       90.4 |
|            11.0 |       104.6 |        65.3 |       98.0 |
|            13.0 |       115.7 |        68.6 |      105.6 |
|            15.0 |       126.8 |        68.4 |      112.6 |
|            17.0 |       138.4 |        70.8 |      123.2 |

#+begin_src matlab :exports none
%% RMS value of the non-repeatability for all the scans
figure;
hold on;
plot(elapse_min(2:end), 1e9*fjur_min_rms(2:end), 'o', ...
    'DisplayName', 'fjpur')
plot(elapse_min(2:end), 1e9*fjuh_min_rms(2:end), 'o', ...
    'DisplayName', 'fjpuh')
plot(elapse_min(2:end), 1e9*fjd_min_rms(2:end), 'o', ...
    'DisplayName', 'fjpd')
hold off;
xlabel('Elapse Time [min]'); ylabel('Non-repeat. part [nm RMS]');
legend('location', 'southeast', 'FontSize', 8);
ylim([0, 140]);
#+end_src

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

#+name: fig:repeat_error_fct_time_min
#+caption: Remaining motion as a function of the elapse time / number of iteration
#+RESULTS:
[[file:figs/repeat_error_fct_time_min.png]]

** Repeatability over several days
<<sec:fj_repeat_minutes_days>>

The same scan is done with approximately 8 hours of time interval over three days.
#+begin_src matlab
%% Filenames for the measurements
data_files_day = {
    "lut_const_fj_vel_14012022_1824.dat",
    "lut_const_fj_vel_15012022_0234.dat",
    "lut_const_fj_vel_15012022_1043.dat",
    "lut_const_fj_vel_15012022_1852.dat",
    "lut_const_fj_vel_16012022_0302.dat",
    "lut_const_fj_vel_16012022_1111.dat",
    "lut_const_fj_vel_16012022_1920.dat"
};
#+end_src

#+begin_src matlab :exports none
%% Load and process measured data
data_day = {};

for i = 1:length(data_files_day)
    data_day{i} = extractDatData(sprintf("%s/21Nov/blc13420/id21/LUT_constant_fj_vel/%s", data_directory, data_files_day{i}), ...
                         {"bragg", "dz", "dry", "drx", "fjur", "fjuh", "fjd"}, ...
                         [pi/180, 1e-9, 1e-9, 1e-9, 1e-8, 1e-8, 1e-8]);
    data_day{i} = processMeasData(data_day{i});
    file_name = data_files_day{i}.char;
    data_day{i}.date = datetime(file_name(end-16:end-4),'Format','ddMMyyyy_HHmm');;
end
#+end_src

The measured position errors of =fjur= during all the scans are shown in Figure [[fig:repeat_error_over_hours]].
Clearly, the repeatability is worse than when the scans where only spaced by few minutes (Figure [[fig:repeat_error_over_min]]).
#+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*data_day{2}.fjur, 1e6*(data_day{2}.fjur_e_filt), 'color', [colors(1,:),0.5], ...
    'DisplayName', 'Scans');
for i = 3:length(data_files_day)
    plot(1e3*data_day{i}.fjur, 1e6*(data_day{i}.fjur_e_filt), 'color', [colors(1,:),0.5], ...
        'HandleVisibility', 'off');
end
plot(1e3*data_day{1}.fjur, 1e6*(data_day{1}.fjur_e_filt), 'k-', ...
    'DisplayName', 'First Scan');
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Position Error [$\mu$m]');
legend('location', 'southeast', 'FontSize', 8);
xlim([15.5, 21.5]);
ylim([2, 6]);

ax2 = nexttile();
hold on;
plot(1e3*data_day{2}.fjur, 1e6*(data_day{2}.fjur_e_filt), 'color', [colors(1,:),0.5], ...
    'DisplayName', 'Scans');
for i = 3:length(data_files_day)
    plot(1e3*data_day{i}.fjur, 1e6*(data_day{i}.fjur_e_filt), 'color', [colors(1,:),0.5], ...
        'HandleVisibility', 'off');
end
plot(1e3*data_day{1}.fjur, 1e6*(data_day{1}.fjur_e_filt), 'k-', ...
    'DisplayName', 'First Scan');
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Position Error [$\mu$m]');
legend('location', 'southeast', 'FontSize', 8);
xlim([15.98, 16.02]);
#+end_src

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

#+name: fig:repeat_error_over_hours
#+caption: Repeatability of =fjur= over several hours
#+RESULTS:
[[file:figs/repeat_error_over_hours.png]]

The non-repeatable part is computed, summarized in Table [[tab:repeat_error_day]] and visually shown in Figure [[fig:repeat_error_fct_time_hours]].
#+begin_src matlab :exports none
%% Compute RMS value of non-repeatability
elapse_day   = cellfun(@(x) etime(datevec(x.date),datevec(data_day{1}.date))/3600/24, data_day);
fjur_day_rms = cellfun(@(x) rms(detrend(x.fjur_e_resampl - data_day{1}.fjur_e_resampl, 0)), data_day);
fjuh_day_rms = cellfun(@(x) rms(detrend(x.fjuh_e_resampl - data_day{1}.fjuh_e_resampl, 0)), data_day);
fjd_day_rms  = cellfun(@(x) rms(detrend(x.fjd_e_resampl  - data_day{1}.fjd_e_resampl,  0)), data_day);
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([elapse_day(2:end); 1e9*fjur_day_rms(2:end); 1e9*fjuh_day_rms(2:end); 1e9*fjd_day_rms(2:end)]', {}, {'Elapse Time [days]', '=fjur= [nm]', '=fjuh= [nm]', '=fjd= [nm]'}, ' %.1f ');
#+end_src

#+name: tab:repeat_error_day
#+caption: RMS value of the Non-repeatable part when during several identical scans within few days
#+attr_latex: :environment tabularx :width \linewidth :align lXXX
#+attr_latex: :center t :booktabs t
#+RESULTS:
| Elapse Time [days] | =fjur= [nm] | =fjuh= [nm] | =fjd= [nm] |
|--------------------+-------------+-------------+------------|
|                0.3 |       102.9 |        65.4 |       76.0 |
|                0.7 |       147.3 |        75.1 |       91.5 |
|                1.0 |       188.8 |        86.9 |      106.2 |
|                1.4 |       220.8 |        97.2 |      107.2 |
|                1.7 |       244.2 |       106.6 |      104.3 |
|                2.0 |       252.7 |       117.6 |      106.1 |

#+begin_src matlab :exports none
figure;
hold on;
plot(24*elapse_day(2:end), 1e9*fjur_day_rms(2:end), 'o')
plot(24*elapse_day(2:end), 1e9*fjuh_day_rms(2:end), 'o')
plot(24*elapse_day(2:end), 1e9*fjd_day_rms( 2:end), 'o')
hold off;
xlabel('Elapse Time [hours]'); ylabel('Non-repeat. part [nm RMS]');
ylim([0, 260]);
#+end_src

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

#+name: fig:repeat_error_fct_time_hours
#+caption: RMS value of the non-repeatable part for scans spaced by 8 hours
#+RESULTS:
[[file:figs/repeat_error_fct_time_hours.png]]

** Which error is repeatable and which is not?
In the previous section, it was shown that the non-repeatable part of the fast jack motion is in the order of several hundreds of nano-meters.

In this section, we wish to see if this non-repeatable error is due to:
- Thermal drifts?
- non-repeatability of the ball-screw mechanism (1mm error period)
- non-repeatability of the $20\mu m/10\mu m/5\mu m$ period errors

To do so, a spectral analysis of the non-repeatable part is performed.

#+begin_src matlab :exports none
%% Hanning window used for pwelch function
win = hanning(floor(5e-3/100e-9));

%% Compute the PSD of non-repeatable part in the spatial domain
for i = 2:length(data_files_min)
    [Sxx, ~] = pwelch(detrend(data_min{i}.fjur_e_resampl - data_min{1}.fjur_e_resampl, 0), win, 0, [], 1/100e-9);
    data_min{i}.S_fjur = Sxx;
end
% for i = 2:length(data_files_day)
%     [Sxx, ~] = pwelch(detrend(data_day{i}.fjur_e_resampl - data_day{1}.fjur_e_resampl, 0), win, 0, [], 1/100e-9);
%     data_day{i}.S_fjur = Sxx;
% end

%% Compute PSD of initial motion error
[Sxx_min, f] = pwelch(detrend(data_min{1}.fjur_e_resampl, 0), win, 0, [], 1/100e-9);
[Sxx_day, f] = pwelch(detrend(data_day{1}.fjur_e_resampl, 0), win, 0, [], 1/100e-9);
#+end_src

First, we look at the errors with small spatial periods in Figure [[fig:non_repeatable_part_small_periods_min_hour]].
It is clear that:
- errors with periods of $5\mu m$ and $20\mu m$ are very repeatable over several days
- errors with periods of $10\mu m$ are well repeatable over several minutes but less over not hours/days (Figure [[fig:non_repeatable_part_small_periods_min_hour]], right)

#+begin_src matlab :exports none
%% ASD of measured errors with small spatial periods
figure;
tiledlayout(2, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(1e6./f, 1e9*sqrt(Sxx_min), 'k-', ...
    'DisplayName', 'Mode A Errors')
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'lin');
xlabel('Spatial Period [$\mu$m]'); ylabel('ASD [nm/$1/\sqrt{m}$]');
xlim([4.5, 25]);

ax1 = nexttile();
hold on;
plot(1e3./f, 1e9*sqrt(Sxx_min), 'k-', ...
    'DisplayName', 'Mode A Errors')
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'lin');
xlabel('Spatial Period [mm]'); ylabel('ASD [nm/$1/\sqrt{m}$]');
xlim([0.1, 2]);
#+end_src

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

#+begin_src matlab :exports none
%% ASD of measured errors with small spatial periods
figure;
tiledlayout(1, 3, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([1,2]);
hold on;
plot(1e6./f, 1e9*sqrt(Sxx_day), 'k-', ...
    'DisplayName', 'Mode A Errors')
plot(1e6./f, 1e9*sqrt(data_day{2}.S_fjur), 'color', [colors(2,:), 0.5], ...
    'DisplayName', 'Non-Rep. Part - hours')
plot(1e6./f, 1e9*sqrt(data_min{2}.S_fjur), 'color', [colors(1,:), 0.5], ...
    'DisplayName', 'Non-Rep. Part - min')
for i = 3:length(data_files_day)
    plot(1e6./f, 1e9*sqrt(data_day{i}.S_fjur), 'color', [colors(2,:), 0.5], ...
        'HandleVisibility', 'off')
end
for i = 3:length(data_files_min)
    plot(1e6./f, 1e9*sqrt(data_min{i}.S_fjur), 'color', [colors(1,:), 0.5], ...
        'HandleVisibility', 'off')
end
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'lin');
xlabel('Spatial Period [$\mu$m]'); ylabel('ASD [nm/$1/\sqrt{m}$]');
xlim([4.5, 25]);
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile();
hold on;
plot(1e6./f, 1e9*sqrt(Sxx_day), 'k-', ...
    'DisplayName', 'Mode A Errors')
plot(1e6./f, 1e9*sqrt(data_day{2}.S_fjur), 'color', [colors(2,:), 0.5], ...
    'DisplayName', 'Non-Rep. Part - hours')
plot(1e6./f, 1e9*sqrt(data_min{2}.S_fjur), 'color', [colors(1,:), 0.5], ...
    'DisplayName', 'Non-Rep. Part - min')
for i = 3:length(data_files_day)
    plot(1e6./f, 1e9*sqrt(data_day{i}.S_fjur), 'color', [colors(2,:), 0.5], ...
        'HandleVisibility', 'off')
end
for i = 3:length(data_files_min)
    plot(1e6./f, 1e9*sqrt(data_min{i}.S_fjur), 'color', [colors(1,:), 0.5], ...
        'HandleVisibility', 'off')
end
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'lin');
xlabel('Spatial Period [$\mu$m]'); ylabel('ASD [nm/$1/\sqrt{m}$]');
xlim([9.8, 10.2]);
#+end_src

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

#+name: fig:non_repeatable_part_small_periods_min_hour
#+caption: ASD of the non-repeatable motion part for scans spaced by several minutes and by several hours
#+RESULTS:
[[file:figs/non_repeatable_part_small_periods_min_hour.png]]

The errors related to large spatial periods are shown in Figure [[fig:non_repeatable_part_large_periods_min_hour]].
Two errors can be observed:
- 1mm error period with good repeatability. This repeatability degrades with time / number of scans.
  Still well after several days.
- 0.37mm error period.
  Well repeatable for the first scan after 2 minutes.
  Degrades quickly.

#+begin_src matlab :exports none
%% ASD of the low frequency errors (large spatial period)
figure;
hold on;
plot(1e3./f, 1e9*sqrt(Sxx_day), 'k-', ...
    'DisplayName', 'Initial Scan')
for i = 2:length(data_files_min)
    plot(1e3./f, 1e9*sqrt(data_min{i}.S_fjur), 'color', [colors(1,:), i/10], ...
        'DisplayName', sprintf('%.0f [min]', elapse_min(i)))
end
for i = 2:length(data_files_day)
    plot(1e3./f, 1e9*sqrt(data_day{i}.S_fjur), 'color', [colors(2,:), i/10], ...
        'DisplayName', sprintf('%.0f [h]', 24*elapse_day(i)))
end
hold off;
set(gca, 'xscale', '%log'); set(gca, 'yscale', 'lin');
xlabel('Spatial Period [mm]'); ylabel('ASD [nm/$1/\sqrt{m}$]');
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 2);
xlim([0.1, 2]); ylim([0, 25]);
#+end_src

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

#+name: fig:non_repeatable_part_large_periods_min_hour
#+caption: (Spatial) Spectral Density of the non-repeatability with large spatial periods
#+RESULTS:
[[file:figs/non_repeatable_part_large_periods_min_hour.png]]

#+begin_important
The repeatability is very good for the $5\,\mu m$ and $20\,\mu m$ period errors (Figure [[fig:non_repeatable_part_small_periods_min_hour]]), a little bit less for the $10\,\mu m$ error period (Figure [[fig:non_repeatable_part_small_periods_min_hour]], right).
What can be the physical cause of that?

The non-repeatability with large spatial periods are degrading over time (Figure [[fig:non_repeatable_part_large_periods_min_hour]]).
They are however more easily compensated with the feedback control (mode C).

The cause of the error with a period of 0.37mm is still unknown.
#+end_important

** Estimation of the errors in mode B
<<sec:mode_B_errors_estimation>>

In this section, the expected errors in mode B are estimated.

In order to do so the LUT for the initial scan is computed and then applied to the following scans.

#+begin_src matlab
%% Filenames for the measurements
data_files_min = {
    "lut_const_fj_vel_14012022_1517.dat",
    "lut_const_fj_vel_14012022_1519.dat",
    "lut_const_fj_vel_14012022_1521.dat",
    "lut_const_fj_vel_14012022_1523.dat",
    "lut_const_fj_vel_14012022_1525.dat",
    "lut_const_fj_vel_14012022_1527.dat",
    "lut_const_fj_vel_14012022_1528.dat",
    "lut_const_fj_vel_14012022_1530.dat",
    "lut_const_fj_vel_14012022_1532.dat",
    "lut_const_fj_vel_14012022_1534.dat"
};
#+end_src

#+begin_src matlab :exports none
%% Load and process measured data
data_min = {};

for i = 1:length(data_files_min)
    data_min{i} = extractDatData(sprintf("%s/21Nov/blc13420/id21/LUT_constant_fj_vel/%s", data_directory, data_files_min{i}), ...
                         {"bragg", "dz", "dry", "drx", "fjur", "fjuh", "fjd"}, ...
                         [pi/180, 1e-9, 1e-9, 1e-9, 1e-8, 1e-8, 1e-8]);
    data_min{i} = processMeasData(data_min{i});
    file_name = data_files_min{i}.char;
    data_min{i}.date = datetime(file_name(end-16:end-4),'Format','ddMMyyyy_HHmm');;
end
#+end_src

Let's first do this analysis for the first two scans and for the =fjur= fast jack.

The measured motion errors are shown in Figure [[fig:repeat_measured_fjur_motion_error]] (left) and the difference between the measured motion in Figure [[fig:repeat_measured_fjur_motion_error]] (right).
#+begin_src matlab :exports none
%% Error in fjur during the two scans and difference in measured error
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(1e3*data_min{1}.fjur, 1e6*data_min{1}.fjur_e_filt, '-');
plot(1e3*data_min{2}.fjur, 1e6*data_min{2}.fjur_e_filt, '-');
hold off;
xlabel('Input IcePAP Step [mm]'); ylabel('Measured Motion Error [$\mu$m]');

ax2 = nexttile();
plot(1e3*data_min{1}.fjur_resampl, 1e9*(data_min{1}.fjur_e_resampl - data_min{2}.fjur_e_resampl), 'k-');
xlabel('Input IcePAP Step [mm]'); ylabel('Motion Difference [nm]');

linkaxes([ax1,ax2],'x');
xlim([19.9, 20.1]);
#+end_src

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

#+name: fig:repeat_measured_fjur_motion_error
#+caption: Measured motion error for =fjur= during both scans (left) and difference in the measured motion (right)
#+RESULTS:
[[file:figs/repeat_measured_fjur_motion_error.png]]


Let's now compare the LUT that are computed from both scans (Figure [[fig:repeat_comp_lut_correction_fjur]]).
#+begin_src matlab :exports none
%% Compute the LUT
data_min{1}.lut = createLUT(data_min{1}, "/tmp/lut.dat", 'lut_inc', 100e-9);
data_min{2}.lut = createLUT(data_min{2}, "/tmp/lut.dat", 'lut_inc', 100e-9);
#+end_src

The two LUT corrections are differing by about 50 nm RMS.
#+begin_src matlab :exports none
%% Plot the LUT
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(data_min{1}.lut(:,1), 1e3*(data_min{1}.lut(:,2) - data_min{1}.lut(:,1)), '-');
plot(data_min{2}.lut(:,1), 1e3*(data_min{2}.lut(:,2) - data_min{2}.lut(:,1)), '-');
hold off;
xlabel('Input IcePAP Step [mm]'); ylabel('LUT correction [$\mu m$]');

ax2 = nexttile();
plot(data_min{1}.lut(:,1), 1e6*(data_min{1}.lut(:,2) - data_min{2}.lut(:,2)), 'k-');
xlabel('Input IcePAP Step [mm]'); ylabel('Difference [nm]');

linkaxes([ax1,ax2],'x');
xlim([19.9, 20.1]);
#+end_src

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

#+name: fig:repeat_comp_lut_correction_fjur
#+caption: Generated =fjur= LUT for both scans (left) and differences between the LUT (right)
#+RESULTS:
[[file:figs/repeat_comp_lut_correction_fjur.png]]

Let's now estimate the motion error in mode B is the LUT computed with the first scan were used.

#+begin_src matlab :exports none
%% Computation of the output motion in mode B
used_lut = 1e-3*data_min{1}.lut;

for i = 1:length(data_files_min)
    % Get data only inside LUT
    lut_i = data_min{i}.fjur > used_lut(1,1) & data_min{i}.fjur < used_lut(end,1);
    % Compute output IcePAP step using LUT data
    icepap_out_steps = interp1(used_lut(:,1), used_lut(:,2), data_min{i}.fjur(lut_i), 'linear');

    % Estimate output motion corresponding to this step
    data_min{i}.fjur_modeB     = data_min{i}.fjur(lut_i); % IcePAP reference step
    data_min{i}.fjur_out_modeB = interp1(data_min{i}.fjur_resampl, data_min{i}.fjur_resampl+data_min{i}.fjur_e_resampl, icepap_out_steps, 'linear'); % IcePAP output step
end

for i = 1:length(data_files_day)
    % Get data only inside LUT
    lut_i = data_day{i}.fjur > used_lut(1,1) & data_day{i}.fjur < used_lut(end,1);
    % Compute output IcePAP step using LUT data
    icepap_out_steps = interp1(used_lut(:,1), used_lut(:,2), data_day{i}.fjur(lut_i), 'linear');

    % Estimate output motion corresponding to this step
    data_day{i}.fjur_modeB     = data_day{i}.fjur(lut_i); % IcePAP reference step
    data_day{i}.fjur_out_modeB = interp1(data_day{i}.fjur_resampl, data_day{i}.fjur_resampl+data_day{i}.fjur_e_resampl, icepap_out_steps, 'linear'); % IcePAP output step
end
#+end_src

#+begin_src matlab :exports none
%% Estimated error on =fjur= using LUT made based on the first scan
figure;
tiledlayout(2, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(1e3*data_min{1}.fjur_modeB, 1e9*(data_min{1}.fjur_modeB - data_min{1}.fjur_out_modeB), 'k-', ...
     'DisplayName', 'Initial')
for i = 2:length(data_files_min)
    plot(1e3*data_min{i}.fjur_modeB, 1e9*(data_min{i}.fjur_modeB - data_min{i}.fjur_out_modeB), 'color', [colors(1,:), i/10], ...
         'DisplayName', sprintf('%.0f [m]', elapse_min(i)))
end
for i = 1:length(data_files_day)
    plot(1e3*data_day{i}.fjur_modeB, 1e9*(data_day{i}.fjur_modeB - data_day{i}.fjur_out_modeB), 'color', [colors(2,:), i/10], ...
         'DisplayName', sprintf('%.0f [h]', 24*elapse_day(i)))
end
hold off;
set(gca, 'xscale', 'lin'); set(gca, 'yscale', 'lin');
xlabel('IcePAP Step [mm]'); ylabel('Estimated Error [nm]');
legend('location', 'south', 'FontSize', 8, 'NumColumns', 3);
xlim([15.5, 21.5]);
ylim([-2000, 1000]);

ax2 = nexttile();
hold on;
plot(1e3*data_min{1}.fjur_modeB, 1e9*(data_min{1}.fjur_modeB - data_min{1}.fjur_out_modeB), 'k-', ...
     'DisplayName', 'Initial')
for i = 2:length(data_files_min)
    plot(1e3*data_min{i}.fjur_modeB, 1e9*(data_min{i}.fjur_modeB - data_min{i}.fjur_out_modeB), 'color', [colors(1,:), i/10], ...
         'DisplayName', sprintf('%.0f [m]', elapse_min(i)))
end
for i = 1:length(data_files_day)
    plot(1e3*data_day{i}.fjur_modeB, 1e9*(data_day{i}.fjur_modeB - data_day{i}.fjur_out_modeB), 'color', [colors(2,:), i/10], ...
         'DisplayName', sprintf('%.0f [h]', 24*elapse_day(i)))
end
hold off;
set(gca, 'xscale', 'lin'); set(gca, 'yscale', 'lin');
xlabel('IcePAP Step [mm]'); ylabel('Estimated Error [nm]');
xlim([17.98, 18.02]);
#+end_src

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

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

#+name: fig:repeat_comp_lut_fjur_error
#+caption: Estimated error on =fjur= using LUT made based on the first scan
#+RESULTS:
[[file:figs/repeat_comp_lut_fjur_error.png]]

The RMS value of the remaining error in mode B for =fjur= are computed and summarized in Table [[tab:errors_mode_B_fjur_duration]].
The error is increasing over first half hour and seems to stabilize after several hours.
#+begin_src matlab :exports none
%% Computation of the positioning errors for each scan
elapse_min     = cellfun(@(x) etime(datevec(x.date),datevec(data_min{1}.date))/60, data_min);
mode_B_rms_min = cellfun(@(x) rms((detrend(x.fjur_modeB - x.fjur_out_modeB, 0))), data_min);
elapse_day     = cellfun(@(x) etime(datevec(x.date),datevec(data_day{1}.date))/3600/24, data_day);
mode_B_rms_day = cellfun(@(x) rms((detrend(x.fjur_modeB - x.fjur_out_modeB, 0))), data_day);
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([[elapse_min(2:end)./60, 24*elapse_day(2:end)]; 1e9*[mode_B_rms_min(2:end), mode_B_rms_day(2:end)]]', {}, {'Elapse Time [h]', '=fjur= [nm RMs]'}, ' %.1f ');
#+end_src

#+name: tab:errors_mode_B_fjur_duration
#+caption: RMS value of the estimated errors in mode B for =fjur= as a function of the time between the creation of the LUT and the scan
#+attr_latex: :environment tabularx :width 0.4\linewidth :align lX
#+attr_latex: :center t :booktabs t
#+RESULTS:
| Elapse Time [h] | =fjur= [nm RMS] |
|-----------------+-----------------|
|             0.0 |            48.1 |
|             0.1 |            62.9 |
|             0.1 |            75.1 |
|             0.1 |            86.1 |
|             0.2 |            95.4 |
|             0.2 |           105.0 |
|             0.2 |           115.7 |
|             0.2 |           126.8 |
|             0.3 |           138.3 |
|             8.2 |           247.4 |
|            16.3 |           240.9 |
|            24.5 |           234.2 |
|            32.6 |           232.9 |
|            40.8 |           249.0 |
|            48.9 |           270.7 |

The (spatial) spectral density of the estimated errors in mode B are computed and shown in Figure [[fig:asd_estimated_errors_fjur_mode_B]] for short spatial periods and in Figure [[fig:asd_estimated_errors_fjur_mode_B_large_spatial_errors]] for large spatial errors.

#+begin_src matlab :exports none
%% Compute errors in mode B with constant spacing (used to compute spectral density)
for i = 1:length(data_files_min)
    [y, x] = resample(data_min{i}.fjur_out_modeB, data_min{i}.fjur_modeB, 1/100e-9);
    data_min{i}.fjur_e_resampl_B = y(20:end-20); % Output IcePAP steps
    data_min{i}.fjur_resampl_B = x(20:end-20); % Input IcePAP steps
end

for i = 1:length(data_files_day)
    [y, x] = resample(data_day{i}.fjur_out_modeB, data_day{i}.fjur_modeB, 1/100e-9);
    data_day{i}.fjur_e_resampl_B = y(20:end-20); % Output IcePAP steps
    data_day{i}.fjur_resampl_B = x(20:end-20); % Input IcePAP steps
end
#+end_src

#+begin_src matlab :exports none
%% Hanning window used for pwelch function
win = hanning(floor(5e-3/100e-9));

%% Compute the PSD of estimated fjur errors in mode B
for i = 2:length(data_files_min)
    [Sxx, ~] = pwelch(detrend(data_min{i}.fjur_e_resampl_B - data_min{i}.fjur_resampl_B, 0), win, 0, [], 1/100e-9);
    data_min{i}.S_fjur_B = Sxx;
end
for i = 2:length(data_files_day)
    [Sxx, ~] = pwelch(detrend(data_day{i}.fjur_e_resampl_B - data_day{i}.fjur_resampl_B, 0), win, 0, [], 1/100e-9);
    data_day{i}.S_fjur_B = Sxx;
end

%% Compute PSD of initial motion error
[Sxx_min, f] = pwelch(detrend(data_min{1}.fjur_e_resampl, 0), win, 0, [], 1/100e-9);
#+end_src

#+begin_src matlab :exports none
%% ASD of measured errors with small spatial periods
figure;
tiledlayout(1, 3, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([1,2]);
hold on;
plot(1e6./f, 1e9*sqrt(Sxx_min), 'k-', ...
    'DisplayName', 'Mode A Errors')
plot(1e6./f, 1e9*sqrt(data_day{2}.S_fjur_B), 'color', [colors(2,:), 0.5], ...
    'DisplayName', 'Mode B Erros - days')
plot(1e6./f, 1e9*sqrt(data_min{2}.S_fjur_B), 'color', [colors(1,:), 0.5], ...
    'DisplayName', 'Mode B Erros - min')
for i = 3:length(data_files_day)
    plot(1e6./f, 1e9*sqrt(data_day{i}.S_fjur_B), 'color', [colors(2,:), 0.5], ...
        'HandleVisibility', 'off')
end
for i = 3:length(data_files_min)
    plot(1e6./f, 1e9*sqrt(data_min{i}.S_fjur_B), 'color', [colors(1,:), 0.5], ...
        'HandleVisibility', 'off')
end
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'lin');
xlabel('Spatial Period [$\mu$m]'); ylabel('ASD [nm/$1/\sqrt{m}$]');
xlim([4.5, 25]);
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile();
hold on;
plot(1e3./f, 1e9*sqrt(Sxx_min), 'k-', ...
    'DisplayName', 'Mode A Errors')
for i = 2:length(data_files_min)
    plot(1e3./f, 1e9*sqrt(data_min{i}.S_fjur_B), 'color', [colors(1,:), i/10], ...
        'DisplayName', sprintf('%.0f [min]', elapse_min(i)))
end
for i = 2:length(data_files_day)
    plot(1e3./f, 1e9*sqrt(data_day{i}.S_fjur_B), 'color', [colors(2,:), i/10], ...
        'DisplayName', sprintf('%.0f [h]', 24*elapse_day(i)))
end
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'lin');
xlabel('Spatial Period [$\mu$m]'); ylabel('ASD [nm/$1/\sqrt{m}$]');
xlim([0.5, 2]); ylim([0, 25]);
#+end_src

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

#+begin_src matlab :exports none
%% ASD of measured errors with small spatial periods
figure;
tiledlayout(1, 3, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([1,2]);
hold on;
plot(1e6./f, 1e9*sqrt(Sxx_min), 'k-', ...
    'DisplayName', 'Mode A Errors')
plot(1e6./f, 1e9*sqrt(data_day{2}.S_fjur_B), 'color', [colors(2,:), 0.5], ...
    'DisplayName', 'Mode B Erros - days')
plot(1e6./f, 1e9*sqrt(data_min{2}.S_fjur_B), 'color', [colors(1,:), 0.5], ...
    'DisplayName', 'Mode B Erros - min')
for i = 3:length(data_files_day)
    plot(1e6./f, 1e9*sqrt(data_day{i}.S_fjur_B), 'color', [colors(2,:), 0.5], ...
        'HandleVisibility', 'off')
end
for i = 3:length(data_files_min)
    plot(1e6./f, 1e9*sqrt(data_min{i}.S_fjur_B), 'color', [colors(1,:), 0.5], ...
        'HandleVisibility', 'off')
end
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'lin');
xlabel('Spatial Period [$\mu$m]'); ylabel('ASD [nm/$1/\sqrt{m}$]');
xlim([4.5, 25]);
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile();
hold on;
plot(1e6./f, 1e9*sqrt(Sxx_min), 'k-', ...
    'DisplayName', 'Mode A Errors')
plot(1e6./f, 1e9*sqrt(data_day{2}.S_fjur_B), 'color', [colors(2,:), 0.5], ...
    'DisplayName', 'Mode B Erros - days')
plot(1e6./f, 1e9*sqrt(data_min{2}.S_fjur_B), 'color', [colors(1,:), 0.5], ...
    'DisplayName', 'Mode B Erros - min')
for i = 3:length(data_files_day)
    plot(1e6./f, 1e9*sqrt(data_day{i}.S_fjur_B), 'color', [colors(2,:), 0.5], ...
        'HandleVisibility', 'off')
end
for i = 3:length(data_files_min)
    plot(1e6./f, 1e9*sqrt(data_min{i}.S_fjur_B), 'color', [colors(1,:), 0.5], ...
        'HandleVisibility', 'off')
end
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'lin');
xlabel('Spatial Period [$\mu$m]'); ylabel('ASD [nm/$1/\sqrt{m}$]');
xlim([9.8, 10.2]);
#+end_src

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

#+name: fig:asd_estimated_errors_fjur_mode_B
#+caption: Estimated spectral density of the =fjur= errors in mode B for several scans. Focus on short spatial periods.
#+RESULTS:
[[file:figs/asd_estimated_errors_fjur_mode_B.png]]

#+begin_src matlab :exports none
%% ASD of measured errors with large spatial periods
figure;
hold on;
plot(1e3./f, 1e9*sqrt(Sxx_min), 'k-', ...
    'DisplayName', 'Mode A Errors')
for i = 2:length(data_files_min)
    plot(1e3./f, 1e9*sqrt(data_min{i}.S_fjur_B), 'color', [colors(1,:), i/10], ...
        'DisplayName', sprintf('%.0f [min]', elapse_min(i)))
end
for i = 2:length(data_files_day)
    plot(1e3./f, 1e9*sqrt(data_day{i}.S_fjur_B), 'color', [colors(2,:), i/10], ...
        'DisplayName', sprintf('%.0f [h]', 24*elapse_day(i)))
end
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'lin');
xlabel('Spatial Period [$\mu$m]'); ylabel('ASD [nm/$1/\sqrt{m}$]');
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 3);
xlim([0.1, 2]); ylim([0, 25]);
#+end_src

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

#+name: fig:asd_estimated_errors_fjur_mode_B_large_spatial_errors
#+caption: Estimated spectral density of the =fjur= errors in mode B for several scans. Focus on large spatial periods.
#+RESULTS:
[[file:figs/asd_estimated_errors_fjur_mode_B_large_spatial_errors.png]]

#+begin_important
Using the LUT, most of the errors can be compensated.
This includes the errors of the stepper motor (with periods of $5\,\mu m$, $10\,\mu m$ and $20\,\mu m$) and the errors of the ball-screw mechanism (periods of $1\,mm$).

The "quality" of the LUT is degrading over time, especially for periods of $10\,\mu m$ and $1\,mm$.
While the errors with a period of $1\,mm$ are not an issue as they will be easily compensated using feedback control, errors with a period of $10\,\mu m$ could be more problematic.
#+end_important

** Conclusion
#+begin_important
Repeatability of the Fast Jack motion has been studied.

Even though the repeatability degrades over time, the main errors with a period of $5\,\mu m$ are well repeatable over many scans and time spans of several days.
The degradation of the repeatability is mostly problematic of the errors with a period of $10\,\mu m$.

It was shown that the use of a Lookup Table can eliminate most of the repeatable errors.
The remaining motion error on each fast jack is expected to be in the order of $100\,nm \text{RMS}$ (see Figure [[fig:repeat_comp_lut_fjur_error]]).
#+end_important

* LUT Software Implementation
<<sec:software_implementation>>
** Introduction                                                      :ignore:
** Matlab implementation
<<sec:matlab_lut_first_test>>
*** Introduction                                                    :ignore:
In this section, the computation of the LUT is implemented using Matlab and tested experimentally.

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

*** LUT Creation
A scan in mode A is performed using the =thtraj= motor.
The scan is performed from 10 to 70 degrees.

#+begin_src matlab
%% Extract measurement Data make from BLISS
data_A = extractDatData(sprintf("%s/21Nov/blc13420/id21/LUT_Matlab/lut_matlab_22122021_1610.dat", data_directory), ...
                     {"bragg", "dz", "dry", "drx", "fjur", "fjuh", "fjd"}, ...
                     [pi/180, 1e-9, 1e-9, 1e-9, 1e-8, 1e-8, 1e-8]);
#+end_src

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

#+begin_src matlab :exports none
%% Import LUT data
data_lut_1 = importdata("/home/thomas/mnt/data_id21/21Nov/blc13420/id21/LUT_constant_fj_vel/lut_const_fj_vel_17012022_1724.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 the IcePAP and a new scan in mode B is performed over the same stroke.

#+begin_src matlab
%% Load mode B scan data
data_B = extractDatData(sprintf("%s/21Nov/blc13420/id21/LUT_Matlab/lut_matlab_result_22122021_1616.dat", data_directory), ...
                     {"bragg", "dz", "dry", "drx", "fjur", "fjuh", "fjd"}, ...
                     [pi/180, 1e-9, 1e-9, 1e-9, 1e-8, 1e-8, 1e-8]);
#+end_src

#+begin_src matlab :exports none
%% Get filtered data and error in the frame of the fast jacks
data_A = processMeasData(data_A, 'fir_order', 1000);
data_B  = processMeasData(data_B, 'fir_order', 1000);
#+end_src

The raw (unfiltered, 10kHz) measured motion for =fjur=, =fjuh= and =fjd= are displayed in Figure [[fig:matlab_lut_comp_fj_raw]].
#+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(180/pi*data_A.bragg, 1e6*data_A.fjur_e, ...
    'DisplayName', 'Mode A - $u_r$')
plot(180/pi*data_B.bragg,  1e6*data_B.fjur_e, ...
    'DisplayName', 'Mode B - $u_r$')
hold off;
xlabel('Bragg Angle [deg]'); ylabel('Fast Jack Error [um]')
legend('location', 'northwest')

ax2 = nexttile();
hold on;
plot(180/pi*data_A.bragg, 1e6*data_A.fjuh_e, ...
    'DisplayName', 'Mode A - $u_h$')
plot(180/pi*data_B.bragg,  1e6*data_B.fjuh_e, ...
    'DisplayName', 'Mode B - $u_h$')
hold off;
xlabel('Bragg Angle [deg]'); set(gca, 'YTickLabel',[]);
legend('location', 'northwest')

ax3 = nexttile();
hold on;
plot(180/pi*data_A.bragg, 1e6*data_A.fjd_e, ...
    'DisplayName', 'Mode A - $d$')
plot(180/pi*data_B.bragg,  1e6*data_B.fjd_e, ...
    '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]]

As the raw measured data is quite noisy and affected by disturbances, the data is filtered to obtain the motion errors of the fast jack.
The filtered measured errors are shown in Figure [[fig:matlab_lut_comp_fj_filt]].

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

ax1 = nexttile();
hold on;
plot(180/pi*data_A.bragg, 1e6*data_A.fjur_e_filt, ...
    'DisplayName', 'Mode A - $u_r$')
plot(180/pi*data_B.bragg,  1e6*data_B.fjur_e_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(180/pi*data_A.bragg, 1e6*data_A.fjuh_e_filt, ...
    'DisplayName', 'Mode A - $u_h$')
plot(180/pi*data_B.bragg,  1e6*data_B.fjuh_e_filt, ...
    'DisplayName', 'Mode B - $u_h$')
hold off;
xlabel('Bragg Angle [deg]'); set(gca, 'YTickLabel',[]);
legend('location', 'northwest')

ax3 = nexttile();
hold on;
plot(180/pi*data_A.bragg, 1e6*data_A.fjd_e_filt, ...
    'DisplayName', 'Mode A - $d$')
plot(180/pi*data_B.bragg,  1e6*data_B.fjd_e_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]]

*** Analysis of the remaining errors
Let's now analyze the remaining errors.

#+begin_src matlab :exports none
%% Hanning window used for pwelch function
win = hanning(floor(10e-3/100e-9));

%% Compute the PSD of non-repeatable part in the spatial domain
[Sxx_A, f] = pwelch(detrend(data_A.fjur_e_resampl, 0), win, 0, [], 1/100e-9);
[Sxx_B, ~] = pwelch(detrend(data_B.fjur_e_resampl, 0), win, 0, [], 1/100e-9);
#+end_src

The spectral content of the errors are shown in Figure [[fig:matlab_lut_mode_B_errors_spectral]].
The following can be observed:
- errors with periods of $5\,\mu m$, $10\,\mu m$ and $20\,\mu m$ are reduced
- errors with period of 0.37mm and 1mm are almost totally reduced
- additional motion are added in mode B with periods from $15\,\mu m$ to $25\,\mu m$

#+begin_src matlab :exports none
%% ASD of measured errors with small spatial periods
figure;
tiledlayout(1, 3, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([1,2]);
hold on;
plot(1e6./f, 1e9*sqrt(Sxx_A), '-', ...
    'DisplayName', 'Mode A Errors')
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'lin');
xlabel('Spatial Period [$\mu$m]'); ylabel('ASD [nm/$1/\sqrt{m}$]');
xlim([4.5, 25]);

ax2 = nexttile();
hold on;
plot(1e3./f, 1e9*sqrt(Sxx_A), '-', ...
    'DisplayName', 'Mode A Errors')
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'lin');
xlabel('Spatial Period [mm]'); ylabel('ASD [nm/$1/\sqrt{m}$]');
xlim([0.1, 2]);
#+end_src

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


#+begin_src matlab :exports none
%% ASD of measured errors with small spatial periods
figure;
tiledlayout(1, 3, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([1,2]);
hold on;
plot(1e6./f, 1e9*sqrt(Sxx_A), '-', ...
    'DisplayName', 'Mode A Errors')
plot(1e6./f, 1e9*sqrt(Sxx_B), '-', ...
    'DisplayName', 'Mode B Errors')
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'lin');
xlabel('Spatial Period [$\mu$m]'); ylabel('ASD [nm/$1/\sqrt{m}$]');
xlim([4.5, 25]);
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile();
hold on;
plot(1e3./f, 1e9*sqrt(Sxx_A), '-', ...
    'DisplayName', 'Mode A Errors')
plot(1e3./f, 1e9*sqrt(Sxx_B), '-', ...
    'DisplayName', 'Mode B Errors')
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'lin');
xlabel('Spatial Period [mm]'); ylabel('ASD [nm/$1/\sqrt{m}$]');
xlim([0.1, 2]);
#+end_src

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

#+name: fig:matlab_lut_mode_B_errors_spectral
#+caption: Spectral density of the =fjur= measured errors in mode A and mode B
#+RESULTS:
[[file:figs/matlab_lut_mode_B_errors_spectral.png]]

#+begin_important
Even though the errors in mode B are well reduced as compared to mode A, the LUT is not working as well as expected from Section [[sec:mode_B_errors_estimation]].

This can be due to several factors:
- limited number of points taken in the LUT (original 1 point every $\mu m$) which leads to errors when interpolating the LUT
- limited number of points taken in the mode B trajectory leading to interpolation errors

Further tests will be performed with in more ideal conditions:
- better trajectory used to build the LUT
- more points in the LUT as well as in the trajectory
#+end_important

** Python implementation
<<sec:python_lut_first_test>>
:PROPERTIES:
:header-args:jupyter-python+: :tangle python/src/computeLUT.py
:END:
*** Introduction                                                    :ignore:
In this section, the LUT is computed using Python.

*** Python Init                                            :noexport:ignore:
#+begin_src jupyter-python :exports both
<<p-init>>
#+end_src

#+begin_src jupyter-python :exports none
<<p-config>>
#+end_src

*** Load Data
A scan in mode A is performed and loaded.

#+begin_src jupyter-python
data = np.loadtxt("/home/thomas/mnt/data_id21/21Nov/blc13420/id21/LUT_constant_fj_vel/lut_const_fj_vel_17012022_1749.dat")
#+end_src

Useful data are extracted and converted to SI units.
#+begin_src jupyter-python
bragg = np.pi/180*data[:,0] # Bragg Angle [rad]
dz = 1e-9*data[:,1] # Distance between crystals [m]
dry = 1e-9*data[:,2] # dry [rad]
drx = 1e-9*data[:,3] # drx [rad]
fjur = 1e-8*data[:,4] # ur Fast Jack Step in [m]
fjuh = 1e-8*data[:,5] # uh Fast Jack Step in [m]
fjd = 1e-8*data[:,6] # d Fast Jack Step in [m]
time = 1e-4*np.arange(0, np.size(bragg), 1) # Time vector [s]
ddz = 10.5e-3/(2*np.cos(bragg)) - dz; # Z error between the two crystals [m]
#+end_src

The Bragg angle as a function of time is shown in Figure [[fig:python_bragg]] and the fast jack displacements are shown in Figure [[fig:python_fj_motion]].
#+BEGIN_SRC jupyter-python :file python_bragg.png :exports none
plt.figure(dpi=150)
plt.clf
plt.plot(time, 180/np.pi*bragg, '-', label='bragg')
plt.xlabel('Time [s]')
plt.ylabel('Bragg Angle [deg]')
plt.legend(frameon=True)
plt.savefig('figs/python_bragg.pdf', transparent=True, bbox_inches='tight', pad_inches=0)
#+END_SRC

#+name: fig:python_bragg
#+caption: Bragg angle during the mode A scan
[[file:figs/python_bragg.png]]

#+BEGIN_SRC jupyter-python :file python_fj_motion.png :exports none
plt.figure(dpi=150)
plt.clf
plt.plot(time, 1e3*fjur, '-', label='fjur')
plt.plot(time, 1e3*fjuh, '-', label='fjuh')
plt.plot(time, 1e3*fjd, '-', label='fjd')
plt.xlabel('Time [s]')
plt.ylabel('Fast Jack Motion [mm]')
plt.legend(frameon=True)
plt.savefig('figs/python_fj_motion.pdf', transparent=True, bbox_inches='tight', pad_inches=0)
#+END_SRC

#+name: fig:python_fj_motion
#+caption: Fast Jack motion during the mode A scan
[[file:figs/python_fj_motion.png]]

*** Convert Data in the frame of the fast jack
The measured motion of the crystals using the interferometers are converted to the motion of the three jacks using the Jacobian matrix.

#+begin_src jupyter-python
# Actuator Jacobian
J_a_111 = np.array([[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[0,:] # [m]
fjuh_e = error[1,:] # [m]
fjd_e  = error[2,:] # [m]
#+end_src

The obtained motion error of the fast jack as a function of time are shown in Figure [[fig:python_fj_errors]].
#+BEGIN_SRC jupyter-python :file python_fj_errors.png :exports none
plt.figure(dpi=150)
plt.clf
plt.plot(time, 1e9*fjur_e, '-', label='fjur')
plt.plot(time, 1e9*fjuh_e, '-', label='fjuh')
plt.plot(time, 1e9*fjd_e, '-', label='fjd')
plt.xlabel('Time [s]')
plt.ylabel('Fast Jack Errors [nm]')
plt.legend(frameon=True)
plt.savefig('figs/python_fj_errors.pdf', transparent=True, bbox_inches='tight', pad_inches=0)
#+END_SRC

#+name: fig:python_fj_errors
#+caption: Measured fast jack motion errors as a function of time
[[file:figs/python_fj_errors.png]]

*** Filter Data

In order to get rid of external disturbances and noise, the measured fast jack displacement errors are low pass filtered.

The filter parameters are defined below.
#+BEGIN_SRC jupyter-python
# Generate Low pass FIR Filter
sample_rate = 10000.0 # Sample Rate [Hz]
nyq_rate = sample_rate / 2.0 # Nyquist Rate [Hz]

cutoff_hz = 27 # The cutoff frequency of the filter [Hz]

# Window with specific ripple [dB] and width [Nyquist Fraction]
N, beta = kaiserord(60, 4/nyq_rate)

# Delay expressed in number of sample
N_delay = int((N-1)/2)

# Delay expressed in seconds
delay = N_delay / sample_rate
#+END_SRC

The filter is generated using the following command:
#+BEGIN_SRC jupyter-python
# Fitler generation
taps = firwin(N, cutoff_hz/nyq_rate, window=('kaiser', beta))
#+END_SRC

This filter will introduce a constant delay that is a function of its length:
#+BEGIN_SRC jupyter-python :results value replace
print("Length of the filter is %i\nDelay is %i samples (i.e. %.3f seconds)" % (N, N_delay, delay))
#+END_SRC

#+RESULTS:
: Length of the filter is 9065
: Delay is 4532 samples (i.e. 0.453 seconds)

The measured data is then filtered using the =lfilter= command.
The obtained raw and filtered data are displayed in Figure [[fig:python_fj_errors_filt]].
#+BEGIN_SRC jupyter-python
# Filtering of data, compensation of the delay introduced by the FIR filter
fjur_e_filt = lfilter(taps, 1.0, fjur_e)[N:]
fjuh_e_filt = lfilter(taps, 1.0, fjuh_e)[N:]
fjd_e_filt = lfilter(taps, 1.0, fjd_e)[N:]
time_filt = time[N_delay+1:-N_delay]
#+END_SRC

#+BEGIN_SRC jupyter-python :file python_fj_errors_filt.png :exports none
plt.figure(dpi=150)
plt.clf
plt.plot(time, 1e9*fjur_e, '-', alpha=.5, label='fjur - raw')
plt.plot(time, 1e9*fjuh_e, '-', alpha=.5, label='fjuh - raw')
plt.plot(time, 1e9*fjd_e,  '-', alpha=.5, label='fjd - raw')
plt.plot(time_filt, 1e9*fjur_e_filt, '-', label='fjur - filtered')
plt.plot(time_filt, 1e9*fjuh_e_filt, '-', label='fjuh - filtered')
plt.plot(time_filt, 1e9*fjd_e_filt, '-', label='fjd - filtered')
plt.xlabel('Time [s]')
plt.ylabel('Fast Jack Errors [nm]')
plt.legend(frameon=True)
plt.savefig('figs/python_fj_errors_filt.pdf', transparent=True, bbox_inches='tight', pad_inches=0)
#+END_SRC

#+name: fig:python_fj_errors_filt
#+caption: Raw and filtered measured errors of the fast jack motion
[[file:figs/python_fj_errors_filt.png]]

The measured fast jack motion (filtered) as a function of the IcePAP steps (desired position) is shown in Figure [[fig:python_pos_error_scan]] for the three fast jacks.

#+BEGIN_SRC jupyter-python :file python_pos_error_scan.png :exports none
plt.figure(dpi=150)
plt.clf
plt.plot(1e3*fjur[N_delay+1:-N_delay], 1e3*(fjur[N_delay+1:-N_delay]+fjur_e_filt), '-', label='fjur')
plt.plot(1e3*fjuh[N_delay+1:-N_delay], 1e3*(fjuh[N_delay+1:-N_delay]+fjuh_e_filt), '-', label='fjuh')
plt.plot(1e3*fjd[ N_delay+1:-N_delay],  1e3*(fjd[N_delay+1:-N_delay]+fjd_e_filt), '-', label='fjd')
plt.plot(1e3*fjur[N_delay+1:-N_delay], 1e3*(fjur[N_delay+1:-N_delay]), 'k--', label='Ideal')
plt.xlabel('IcePAP Step [mm]')
plt.ylabel('Measured Motion [mm]')
plt.xlim([24.68, 24.72])
plt.ylim([24.68, 24.72])
plt.legend(frameon=True)
plt.savefig('figs/python_pos_error_scan.pdf', transparent=True, bbox_inches='tight', pad_inches=0)
#+END_SRC

#+name: fig:python_pos_error_scan
#+caption: Measured fast jack motion as a function of the IcePAP step
[[file:figs/python_pos_error_scan.png]]

*** Get Only Interesting Data
Now the data corresponding to the acceleration phase are removed.

#+begin_src jupyter-python
# Remove the extreme part of the data corresponding to the acceleration phase
filt_array = np.where(np.logical_or(fjd[N_delay+1:-N_delay] > fjd[0] - 0.05e-3, fjd[N_delay+1:-N_delay] < fjd[-1] + 0.05e-3))

fjur_e_filt = np.delete(fjur_e_filt, filt_array)
fjuh_e_filt = np.delete(fjuh_e_filt, filt_array)
fjd_e_filt  = np.delete(fjd_e_filt,  filt_array)
time_filt   = np.delete(time_filt, filt_array)

fjur_filt = np.delete(fjur[N_delay+1:-N_delay], filt_array)
fjuh_filt = np.delete(fjuh[N_delay+1:-N_delay], filt_array)
fjd_filt  = np.delete(fjd[ N_delay+1:-N_delay], filt_array)
#+end_src

*** LUT creation
Now the LUT is initialized and computed.

#+begin_src jupyter-python
# Distance bewteen LUT points in [m]
lut_inc = 100e-9
#+end_src

#+begin_src jupyter-python
# Lut Initialization - First column is pos in [m]
lut_start = lut_inc*np.floor(np.min([[fjur_filt + fjur_e_filt], [fjuh_filt + fjuh_e_filt], [fjd_filt + fjd_e_filt]])/lut_inc)
lut_end = lut_inc*np.ceil(np.max([[fjur_filt + fjur_e_filt], [fjuh_filt + fjuh_e_filt], [fjd_filt + fjd_e_filt]])/lut_inc)

lut = np.arange(lut_start,lut_end,lut_inc)[:, np.newaxis] @ np.ones((1,4))
#+end_src

#+begin_src jupyter-python
# Build the LUT
for i in range(0, lut.shape[0]):
    idx = (np.abs(fjur_filt + fjur_e_filt - lut[i,0])).argmin()
    if idx > 3 and idx < np.size(fjur_filt) - 3:
        lut[i,1] = fjur_filt[idx];
    idx = (np.abs(fjuh_filt + fjuh_e_filt - lut[i,0])).argmin()
    if idx > 3 and idx < np.size(fjuh_filt) - 3:
        lut[i,2] = fjuh_filt[idx];
    idx = (np.abs(fjd_filt + fjd_e_filt - lut[i,0])).argmin()
    if idx > 3 and idx < np.size(fjd_filt) - 3:
        lut[i,3] = fjd_filt[idx];
#+end_src

#+begin_src jupyter-python
# Add points at both extremities of the LUT to make sure larger scans can be performed
# lut = np.append(lut, np.arange(lut_end+5e-6, lut_end+50e-6, 5e-6)[:, np.newaxis] @ np.ones((1,4)), axis=0)
# lut = np.insert(lut, 0, np.arange(lut_start-50e-6, lut_start-1e-6, 5e-6)[:, np.newaxis] @ np.ones((1,4)), axis=0)
lut = np.append(lut,    np.arange(lut_end+1e-3, 30e-3,     1e-3)[:, np.newaxis] @ np.ones((1,4)), axis=0)
lut = np.insert(lut, 0, np.arange(4e-3,         lut_start, 1e-3)[:, np.newaxis] @ np.ones((1,4)), axis=0)
#+end_src

The computed LUT is shown in Figure [[fig:python_lut_before_normalize_ends]].

There is a "step" at the extremities that will slow down the scans is the steps are within the trajectories.
#+BEGIN_SRC jupyter-python :file python_lut_before_normalize_ends.png :exports none
plt.figure(dpi=150)
plt.clf
plt.plot(1e3*lut[:,0], 1e6*(lut[:,1] - lut[:,0]), '.', label='fjur')
plt.plot(1e3*lut[:,0], 1e6*(lut[:,2] - lut[:,0]), '.', label='fjuh')
plt.plot(1e3*lut[:,0], 1e6*(lut[:,3] - lut[:,0]), '.', label='fjd')
plt.xlabel('IcePAP Step [mm]')
plt.ylabel('Output Step [um]')
plt.legend(frameon=True)
plt.xlim([23, 26])
plt.savefig('figs/python_lut_before_normalize_ends.pdf', transparent=True, bbox_inches='tight', pad_inches=0)
#+END_SRC

#+name: fig:python_lut_before_normalize_ends
#+caption: LUT before "normalization" of ends
[[file:figs/python_lut_before_normalize_ends.png]]

In order to deal with this issue, both ends of the LUT are shifted in order to compensate this step.
#+begin_src jupyter-python
# Step compensation of the start of the LUT
i = np.argmax(np.abs(lut[:,1] - lut[:,0]) > 100e-9)
ur_offset = lut[i,1] - lut[i,0]
lut[0:i,1] = lut[0:i,1] + ur_offset

i = np.argmax(np.abs(lut[:,2] - lut[:,0]) > 100e-9)
uh_offset = lut[i,2] - lut[i,0]
lut[0:i,2] = lut[0:i,2] + uh_offset

i = np.argmax(np.abs(lut[:,3] - lut[:,0]) > 100e-9)
d_offset = lut[i,3] - lut[i,0]
lut[0:i,3] = lut[0:i,3] + d_offset

# Step compensation of the end of the LUT
i = np.argmax(np.abs(lut[::-1,1] - lut[::-1,0]) > 100e-9)
ur_offset = lut[-i-1,1] - lut[-i-1,0]
lut[-i:,1] = lut[-i:,1] + ur_offset

i = np.argmax(np.abs(lut[::-1,2] - lut[::-1,0]) > 100e-9)
uh_offset = lut[-i-1,2] - lut[-i-1,0]
lut[-i:,2] = lut[-i:,2] + uh_offset

i = np.argmax(np.abs(lut[::-1,3] - lut[::-1,0]) > 100e-9)
d_offset = lut[-i-1,3] - lut[-i-1,0]
lut[-i:,3] = lut[-i:,3] + d_offset
#+end_src

The final LUT is displayed in Figure [[fig:python_lut_verif]].
The LUT is now smooth and trajectories larger than the LUT will be possible.
#+BEGIN_SRC jupyter-python :file python_lut_verif.png :exports none
plt.figure(dpi=150)
plt.clf
plt.plot(1e3*lut[:,0], 1e6*(lut[:,1] - lut[:,0]), '.', label='fjur')
plt.plot(1e3*lut[:,0], 1e6*(lut[:,2] - lut[:,0]), '.', label='fjuh')
plt.plot(1e3*lut[:,0], 1e6*(lut[:,3] - lut[:,0]), '.', label='fjd')
plt.xlabel('IcePAP Step [mm]')
plt.ylabel('Output Step [um]')
plt.legend(frameon=True)
plt.xlim([20, 30])
plt.savefig('figs/python_lut_verif.pdf', transparent=True, bbox_inches='tight', pad_inches=0)
#+END_SRC

#+name: fig:python_lut_verif
#+caption: Figure caption
[[file:figs/python_lut_verif.png]]

#+begin_src jupyter-python
# Convert from [m] to [mm]
lut = 1e3*lut;
#+end_src

The LUT is saved as a =.dat= file that will be loaded into BLISS.
#+begin_src jupyter-python
filename = "test_lut_python.dat"
print(f"Save LUT Table in {filename}")
np.savetxt(filename, lut)
#+end_src

** New Method (Python)
:PROPERTIES:
:header-args:jupyter-python+: :tangle python/src/computeLUTNew.py
:END:
*** Introduction                                                    :ignore:
In this section, the LUT is computed using Python.

*** Python Init                                            :noexport:ignore:
#+begin_src jupyter-python :exports both
<<p-init>>
#+end_src

#+begin_src jupyter-python :exports none
<<p-config>>
#+end_src

*** Load Data
A scan in mode A is performed and loaded.

#+begin_src jupyter-python
data = np.loadtxt("/home/thomas/mnt/data_id21/21Nov/blc13420/id21/LUT_constant_fj_vel/lut_const_fj_vel_17012022_1749.dat")
#+end_src

Useful data are extracted and converted to SI units.
#+begin_src jupyter-python
bragg = np.pi/180*data[:,0] # Bragg Angle [rad]
dz = 1e-9*data[:,1] # Distance between crystals [m]
dry = 1e-9*data[:,2] # dry [rad]
drx = 1e-9*data[:,3] # drx [rad]
fjur = 1e-8*data[:,4] # ur Fast Jack Step in [m]
fjuh = 1e-8*data[:,5] # uh Fast Jack Step in [m]
fjd = 1e-8*data[:,6] # d Fast Jack Step in [m]
time = 1e-4*np.arange(0, np.size(bragg), 1) # Time vector [s]
ddz = 10.5e-3/(2*np.cos(bragg)) - dz; # Z error between the two crystals [m]
#+end_src

*** Convert Data in the frame of the fast jack
The measured motion of the crystals using the interferometers are converted to the motion of the three jacks using the Jacobian matrix.

#+begin_src jupyter-python
# Actuator Jacobian
J_a_111 = np.array([[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[0,:] # [m]
fjuh_e = error[1,:] # [m]
fjd_e  = error[2,:] # [m]
#+end_src

*** Filter Data

In order to get rid of external disturbances and noise, the measured fast jack displacement errors are low pass filtered.

The filter parameters are defined below.
#+BEGIN_SRC jupyter-python
# Generate Low pass FIR Filter
sample_rate = 10000.0 # Sample Rate [Hz]
nyq_rate = sample_rate / 2.0 # Nyquist Rate [Hz]

cutoff_hz = 27 # The cutoff frequency of the filter [Hz]

# Window with specific ripple [dB] and width [Nyquist Fraction]
N, beta = kaiserord(60, 4/nyq_rate)

# Delay expressed in number of sample
N_delay = int((N-1)/2)

# Delay expressed in seconds
delay = N_delay / sample_rate
#+END_SRC

The filter is generated using the following command:
#+BEGIN_SRC jupyter-python
# Fitler generation
taps = firwin(N, cutoff_hz/nyq_rate, window=('kaiser', beta))
#+END_SRC

This filter will introduce a constant delay that is a function of its length:
#+BEGIN_SRC jupyter-python :results value replace
print("Length of the filter is %i\nDelay is %i samples (i.e. %.3f seconds)" % (N, N_delay, delay))
#+END_SRC

#+RESULTS:
: Length of the filter is 9065
: Delay is 4532 samples (i.e. 0.453 seconds)

The measured data is then filtered using the =lfilter= command.
The obtained raw and filtered data are displayed in Figure [[fig:python_fj_errors_filt]].
#+BEGIN_SRC jupyter-python
# Filtering of data, compensation of the delay introduced by the FIR filter
fjur_e_filt = lfilter(taps, 1.0, fjur_e)[N:]
fjuh_e_filt = lfilter(taps, 1.0, fjuh_e)[N:]
fjd_e_filt = lfilter(taps, 1.0, fjd_e)[N:]
time_filt = time[N_delay+1:-N_delay]
#+END_SRC

The measured fast jack motion (filtered) as a function of the IcePAP steps (desired position) is shown in Figure [[fig:python_pos_error_scan]] for the three fast jacks.

#+BEGIN_SRC jupyter-python :file python_pos_error_scan.png :exports none
plt.figure(dpi=150)
plt.clf
plt.plot(1e3*fjur[N_delay+1:-N_delay], 1e3*(fjur[N_delay+1:-N_delay]+fjur_e_filt), '-', label='fjur')
plt.plot(1e3*fjuh[N_delay+1:-N_delay], 1e3*(fjuh[N_delay+1:-N_delay]+fjuh_e_filt), '-', label='fjuh')
plt.plot(1e3*fjd[ N_delay+1:-N_delay],  1e3*(fjd[N_delay+1:-N_delay]+fjd_e_filt), '-', label='fjd')
plt.plot(1e3*fjur[N_delay+1:-N_delay], 1e3*(fjur[N_delay+1:-N_delay]), 'k--', label='Ideal')
plt.xlabel('IcePAP Step [mm]')
plt.ylabel('Measured Motion [mm]')
plt.xlim([24.68, 24.72])
plt.ylim([24.68, 24.72])
plt.legend(frameon=True)
plt.savefig('figs/python_pos_error_scan.pdf', transparent=True, bbox_inches='tight', pad_inches=0)
#+END_SRC

#+name: fig:python_pos_error_scan
#+caption: Measured fast jack motion as a function of the IcePAP step
[[file:figs/python_pos_error_scan.png]]

*** Get Only Interesting Data
Now the data corresponding to the acceleration phase are removed.

#+begin_src jupyter-python
# Remove the extreme part of the data corresponding to the acceleration phase
filt_array = np.where(np.logical_or(fjd[N_delay+1:-N_delay] > fjd[0] - 0.05e-3, fjd[N_delay+1:-N_delay] < fjd[-1] + 0.05e-3))

fjur_e_filt = np.delete(fjur_e_filt, filt_array)
fjuh_e_filt = np.delete(fjuh_e_filt, filt_array)
fjd_e_filt  = np.delete(fjd_e_filt,  filt_array)
time_filt   = np.delete(time_filt, filt_array)

fjur_filt = np.delete(fjur[N_delay+1:-N_delay], filt_array)
fjuh_filt = np.delete(fjuh[N_delay+1:-N_delay], filt_array)
fjd_filt  = np.delete(fjd[ N_delay+1:-N_delay], filt_array)
#+end_src

*** New LUT creation
#+begin_src jupyter-python
u, c = np.unique(fjur_filt, return_index=True)
lut_ur = np.stack((u + fjur_e_filt[c], u))

u, c = np.unique(fjuh_filt, return_index=True)
lut_uh = np.transpose(np.stack((u + fjuh_e_filt[c], u)))

u, c = np.unique(fjd_filt, return_index=True)
lut_d = np.transpose(np.stack((u + fjd_e_filt[c], u)))
#+end_src

#+begin_src jupyter-python :file python_new_lut.png :results none
# Figure - Correction made by the new LUT
plt.figure(figsize=(1200/150, 800/150), dpi=150)
plt.clf
plt.plot(1e3*lut_ur[:,0], 1e6*(lut_ur[:,1] - lut_ur[:,0]), '.', label='fjur')
plt.plot(1e3*lut_uh[:,0], 1e6*(lut_uh[:,1] - lut_uh[:,0]), '.', label='fjuh')
plt.plot(1e3*lut_d[:,0], 1e6*(lut_d[:,1] - lut_d[:,0]), '.', label='fjd')
plt.xlabel('IcePAP Step [mm]')
plt.ylabel('Output Step [um]')
plt.legend(frameon=True)
plt.xlim([24.5, 24.9])
plt.savefig('figs/python_new_lut.pdf', transparent=True, bbox_inches='tight', pad_inches=0)
#+end_src

#+name: fig:python_new_lut
#+caption: Correction made by the new LUT
[[file:figs/python_new_lut.png]]

#+begin_src jupyter-python :file python_new_lut.png :results none
# Figure - Correction made by the new LUT
plt.figure(figsize=(1200/150, 800/150), dpi=150)
plt.clf
plt.plot(1e3*lut_ur[:,0], 1e6*(lut_ur[:,1] - lut_ur[:,0]), '.', label='fjur')
plt.plot(1e3*lut[:,0], 1e6*(lut[:,1] - lut[:,0]), '.', label='fjur')
plt.xlabel('IcePAP Step [mm]')
plt.ylabel('Output Step [um]')
plt.legend(frameon=True)
plt.xlim([24.7, 24.71])
plt.ylim([-1.4, -0.8])
plt.savefig('figs/python_new_lut.pdf', transparent=True, bbox_inches='tight', pad_inches=0)
#+end_src

*** LUT creation
Now the LUT is initialized and computed.

#+begin_src jupyter-python
# Distance bewteen LUT points in [m]
lut_inc = 100e-9
#+end_src

#+begin_src jupyter-python
# Lut Initialization - First column is pos in [m]
lut_start = lut_inc*np.floor(np.min([[fjur_filt + fjur_e_filt], [fjuh_filt + fjuh_e_filt], [fjd_filt + fjd_e_filt]])/lut_inc)
lut_end = lut_inc*np.ceil(np.max([[fjur_filt + fjur_e_filt], [fjuh_filt + fjuh_e_filt], [fjd_filt + fjd_e_filt]])/lut_inc)

lut = np.arange(lut_start,lut_end,lut_inc)[:, np.newaxis] @ np.ones((1,4))
#+end_src

#+begin_src jupyter-python
# Build the LUT
for i in range(0, lut.shape[0]):
    idx = (np.abs(fjur_filt + fjur_e_filt - lut[i,0])).argmin()
    if idx > 3 and idx < np.size(fjur_filt) - 3:
        lut[i,1] = fjur_filt[idx];
    idx = (np.abs(fjuh_filt + fjuh_e_filt - lut[i,0])).argmin()
    if idx > 3 and idx < np.size(fjuh_filt) - 3:
        lut[i,2] = fjuh_filt[idx];
    idx = (np.abs(fjd_filt + fjd_e_filt - lut[i,0])).argmin()
    if idx > 3 and idx < np.size(fjd_filt) - 3:
        lut[i,3] = fjd_filt[idx];
#+end_src

#+begin_src jupyter-python
# Add points at both extremities of the LUT to make sure larger scans can be performed
# lut = np.append(lut, np.arange(lut_end+5e-6, lut_end+50e-6, 5e-6)[:, np.newaxis] @ np.ones((1,4)), axis=0)
# lut = np.insert(lut, 0, np.arange(lut_start-50e-6, lut_start-1e-6, 5e-6)[:, np.newaxis] @ np.ones((1,4)), axis=0)
lut = np.append(lut,    np.arange(lut_end+1e-3, 30e-3,     1e-3)[:, np.newaxis] @ np.ones((1,4)), axis=0)
lut = np.insert(lut, 0, np.arange(4e-3,         lut_start, 1e-3)[:, np.newaxis] @ np.ones((1,4)), axis=0)
#+end_src

The computed LUT is shown in Figure [[fig:python_lut_before_normalize_ends]].

There is a "step" at the extremities that will slow down the scans is the steps are within the trajectories.
#+BEGIN_SRC jupyter-python :file python_lut_before_normalize_ends.png :exports none
plt.figure(dpi=150)
plt.clf
plt.plot(1e3*lut[:,0], 1e6*(lut[:,1] - lut[:,0]), '.', label='fjur')
plt.plot(1e3*lut[:,0], 1e6*(lut[:,2] - lut[:,0]), '.', label='fjuh')
plt.plot(1e3*lut[:,0], 1e6*(lut[:,3] - lut[:,0]), '.', label='fjd')
plt.xlabel('IcePAP Step [mm]')
plt.ylabel('Output Step [um]')
plt.legend(frameon=True)
plt.xlim([23, 26])
plt.savefig('figs/python_lut_before_normalize_ends.pdf', transparent=True, bbox_inches='tight', pad_inches=0)
#+END_SRC

#+name: fig:python_lut_before_normalize_ends
#+caption: LUT before "normalization" of ends
[[file:figs/python_lut_before_normalize_ends.png]]

In order to deal with this issue, both ends of the LUT are shifted in order to compensate this step.
#+begin_src jupyter-python
# Step compensation of the start of the LUT
i = np.argmax(np.abs(lut[:,1] - lut[:,0]) > 100e-9)
ur_offset = lut[i,1] - lut[i,0]
lut[0:i,1] = lut[0:i,1] + ur_offset

i = np.argmax(np.abs(lut[:,2] - lut[:,0]) > 100e-9)
uh_offset = lut[i,2] - lut[i,0]
lut[0:i,2] = lut[0:i,2] + uh_offset

i = np.argmax(np.abs(lut[:,3] - lut[:,0]) > 100e-9)
d_offset = lut[i,3] - lut[i,0]
lut[0:i,3] = lut[0:i,3] + d_offset

# Step compensation of the end of the LUT
i = np.argmax(np.abs(lut[::-1,1] - lut[::-1,0]) > 100e-9)
ur_offset = lut[-i-1,1] - lut[-i-1,0]
lut[-i:,1] = lut[-i:,1] + ur_offset

i = np.argmax(np.abs(lut[::-1,2] - lut[::-1,0]) > 100e-9)
uh_offset = lut[-i-1,2] - lut[-i-1,0]
lut[-i:,2] = lut[-i:,2] + uh_offset

i = np.argmax(np.abs(lut[::-1,3] - lut[::-1,0]) > 100e-9)
d_offset = lut[-i-1,3] - lut[-i-1,0]
lut[-i:,3] = lut[-i:,3] + d_offset
#+end_src

The final LUT is displayed in Figure [[fig:python_lut_verif]].
The LUT is now smooth and trajectories larger than the LUT will be possible.
#+BEGIN_SRC jupyter-python :file python_lut_verif.png :exports none
plt.figure(dpi=150)
plt.clf
plt.plot(1e3*lut[:,0], 1e6*(lut[:,1] - lut[:,0]), '.', label='fjur')
plt.plot(1e3*lut[:,0], 1e6*(lut[:,2] - lut[:,0]), '.', label='fjuh')
plt.plot(1e3*lut[:,0], 1e6*(lut[:,3] - lut[:,0]), '.', label='fjd')
plt.xlabel('IcePAP Step [mm]')
plt.ylabel('Output Step [um]')
plt.legend(frameon=True)
plt.xlim([20, 30])
plt.savefig('figs/python_lut_verif.pdf', transparent=True, bbox_inches='tight', pad_inches=0)
#+END_SRC

#+name: fig:python_lut_verif
#+caption: Figure caption
[[file:figs/python_lut_verif.png]]

#+begin_src jupyter-python
# Convert from [m] to [mm]
lut = 1e3*lut;
#+end_src

The LUT is saved as a =.dat= file that will be loaded into BLISS.
#+begin_src jupyter-python
filename = "test_lut_python.dat"
print(f"Save LUT Table in {filename}")
np.savetxt(filename, lut)
#+end_src


*** Merge LUT
#+begin_src jupyter-python
lut_ur = np.transpose(np.loadtxt("/home/thomas/mnt/data_id21/tmp/LUT_full_new_ur.dat"))
lut_uh = np.transpose(np.loadtxt("/home/thomas/mnt/data_id21/tmp/LUT_full_new_uh.dat"))
lut_d = np.transpose(np.loadtxt("/home/thomas/mnt/data_id21/tmp/LUT_full_new_d.dat"))
#+end_src

#+BEGIN_SRC jupyter-python :file python_lut_before_normalize_ends.png :exports none
plt.figure(dpi=150)
plt.clf
plt.plot(lut_uh[0,:], 1e3*(lut_uh[1,:] - lut_uh[0,:]), '.', label='fjuh')
plt.xlabel('IcePAP Step [mm]')
plt.ylabel('Output Step [um]')
plt.legend(frameon=True)
plt.xlim([21.9, 25.15])
# plt.ylim([-5, -4])
plt.savefig('figs/python_lut_before_normalize_ends.pdf', transparent=True, bbox_inches='tight', pad_inches=0)
#+END_SRC

#+begin_src jupyter-python
merge_1 = (lut_1_ur[0,-1]+lut_2_ur[0,0])/2
merge_2 = (lut_2_ur[0,-1]+lut_3_ur[0,0])/2
#+end_src

#+begin_src jupyter-python
lut_ur = np.concatenate((
    lut_1_ur[:, lut_1_ur[0,:] < merge_1],
    lut_2_ur[:, np.logical_and(lut_2_ur[0,:] > merge_1, lut_2_ur[0,:] < merge_2)],
    lut_3_ur[:, lut_3_ur[0,:] > merge_2]
), axis=1)
#+end_src

#+BEGIN_SRC jupyter-python :file python_lut_before_normalize_ends.png :exports none
plt.figure(dpi=150)
plt.clf
plt.plot(lut_ur[:,0], 1e3*(lut_ur[:,1] - lut_ur[:,0]), '.', label='fjur')
plt.xlabel('IcePAP Step [mm]')
plt.ylabel('Output Step [um]')
plt.legend(frameon=True)
plt.xlim([23, 23.15])
plt.ylim([-5, -4])
plt.savefig('figs/python_lut_before_normalize_ends.pdf', transparent=True, bbox_inches='tight', pad_inches=0)
#+END_SRC

** Compare Python and Matlab LUT                                   :noexport:
*** 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

*** Test new LUT

#+begin_src matlab
lut_1_ur = importdata("/home/thomas/mnt/data_id21/tmp/LUT_1_ur.dat");
lut_1_uh = importdata("/home/thomas/mnt/data_id21/tmp/LUT_1_uh.dat");
lut_1_d  = importdata("/home/thomas/mnt/data_id21/tmp/LUT_1_d.dat");

lut_2_ur = importdata("/home/thomas/mnt/data_id21/tmp/LUT_2_ur.dat");
lut_2_uh = importdata("/home/thomas/mnt/data_id21/tmp/LUT_2_uh.dat");
lut_2_d  = importdata("/home/thomas/mnt/data_id21/tmp/LUT_2_d.dat");

lut_3_ur = importdata("/home/thomas/mnt/data_id21/tmp/LUT_3_ur.dat");
lut_3_uh = importdata("/home/thomas/mnt/data_id21/tmp/LUT_3_uh.dat");
lut_3_d  = importdata("/home/thomas/mnt/data_id21/tmp/LUT_3_d.dat");
#+end_src

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

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

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

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

*** Matlab LUT

#+begin_src matlab
%% Extract measurement Data make from BLISS
data_ol = extractDatData(sprintf("%s/21Nov/blc13420/id21/LUT_constant_fj_vel/lut_const_fj_vel_17012022_1749.dat", data_directory), ...
                     {"bragg", "dz", "dry", "drx", "fjur", "fjuh", "fjd"}, ...
                     [pi/180, 1e-9, 1e-9, 1e-9, 1e-8, 1e-8, 1e-8]);
#+end_src

A LUT is generated from this Data.
#+begin_src matlab
%% Generate LUT
data_lut = createLUT(data_ol, "./matlab/lut/lut_matlab_comp_with_python.dat", 'lut_inc', 100e-9);
#+end_src

#+begin_src matlab
lut_matlab = importdata("./matlab/lut/lut_matlab_comp_with_python.dat");
lut_python = importdata("test_lut_python.dat");
#+end_src

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

*** Test Python
#+begin_src matlab
lut_python = importdata("/home/thomas/mnt/tmp_14_days/berruyer/LUT_first_test_Ti.dat");
#+end_src

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

* Optimal Trajectory
<<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_mixed_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_pres.pdf', 'width', 'wide', 'height', 'normal');
#+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_mixed_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]]

* Constant Fast Jack velocity
<<sec:constant_fj_vel>>
** Introduction                                                      :ignore:
A new trajectory motor =fjstraj= has been created to be able to perform scans with constant Fast Jack velocity.

As explained in Section [[sec:optimal_traj_dist]], this can help with the filtering of the data as positioning errors with periods of $5\,\mu m$, $10\,\mu m$ and $20\,\mu m$ will be seen with a constant frequency in the time domain.
The frequency of these errors can be tuned by properly choosing the fast jack 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

** Analysis of measured motion
In this section, a scan with constant fast jack velocity is performed and the measurements are analyzed.

The measurements data are loaded and converted to SI units (mostly meters and radians).
#+begin_src matlab :exports none
%% Extract measurement Data make from BLISS
data_A = extractDatData(sprintf("%s/21Nov/blc13420/id21/LUT_constant_fj_vel/lut_const_fj_vel_12012022_1139.dat", data_directory), ...
                     {"bragg", "dz", "dry", "drx", "fjur", "fjuh", "fjd"}, ...
                      [pi/180, 1e-9, 1e-9, 1e-9, 1e-8, 1e-8, 1e-8]);
data_A.time = 1e-4*[0:1:length(data_A.bragg)-1];
data_A = processMeasData(data_A);
#+end_src

#+begin_src matlab :exports none
%% 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

#+begin_src matlab :exports none
%% Remove start and end of data
t_start = 1.5; % [s]
t_end   = 59; % [s]
t_i = data_A.time > t_start & data_A.time < t_end; % Indices
#+end_src

Bragg and Fast Jack velocity are computed and shown in Figure [[fig:constant_fj_vel_bragg_fj_vel]].
We can see that during the scan, the fast jack velocity is constant and equal to $0.125\,mm/s$ while the bragg velocity is increasing.
#+begin_src matlab :exports none
%% Bragg Velocity [rad/s]
data_A.bragg_vel = lsim(G_diff, data_A.bragg, data_A.time);
#+end_src

#+begin_src matlab :exports none
%% Fast Jack Velocity [m/s]
data_A.fjur_vel = lsim(G_diff, data_A.fjur, data_A.time);
data_A.fjuh_vel = lsim(G_diff, data_A.fjuh, data_A.time);
data_A.fjd_vel  = lsim(G_diff, data_A.fjd , data_A.time);
#+end_src

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

ax1 = nexttile();
plot(data_A.time(t_i), abs(180/pi*data_A.bragg_vel(t_i)))
xlabel('Time [s]'); ylabel('Bragg Velocity [deg/s]');

ax2 = nexttile();
hold on;
plot(data_A.time(t_i), 1e3*abs(data_A.fjur_vel(t_i)))
plot(data_A.time(t_i), 1e3*abs(data_A.fjuh_vel(t_i)))
plot(data_A.time(t_i), 1e3*abs(data_A.fjd_vel( t_i)))
hold off;
xlabel('Time [s]'); ylabel('Fast Jack Velocity [mm/s]');
#+end_src

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

#+name: fig:constant_fj_vel_bragg_fj_vel
#+caption: Bragg and Fast Jack velocity
#+RESULTS:
[[file:figs/constant_fj_vel_bragg_fj_vel.png]]

The frequency of the measured motion errors on =fjur= are computed as a function a time (spectrogram) and shown in Figure [[fig:constant_fj_vel_spectrogram]].
The vibrations linked to the motion of the bragg angle (more precisely due to =mcoil= motor) are clearly observed (purple lines).
The motion errors of the fast jacks have a constant frequency.
The frequency corresponding to the error period of $5\,\mu m$ is indicated by the dashed black line.

#+begin_src matlab :exports none
%% Spectrogram
figure;
hold on;
pspectrum(fjuh_e, 1e4, 'spectrogram', ...
          'FrequencyResolution', 1e0, ...
          'OverlapPercent',  99, ...
          'FrequencyLimits', [1, 400]);
plot(time, (1/(5e-6))*abs(fjur_vel), 'k--', ...
     'DisplayName', 'FJ - $5\mu$m')
plot(time, abs(fjur_vel)/765e-9, '--', 'color', colors(2,:), ...
     'DisplayName', 'Interf. Non-Lin.')
plot(time, 180/pi*50/0.2768*abs(bragg_vel), '--', 'color', colors(4,:), ...
     'DisplayName', 'Bragg')
plot(time, 2*180/pi*50/0.2768*abs(bragg_vel), '--', 'color', colors(4,:), ...
     'HandleVisibility', 'off')
plot(time, 4*180/pi*50/0.2768*abs(bragg_vel), '--', 'color', colors(4,:), ...
     'HandleVisibility', 'off')
hold off;
legend('location', 'northwest')
title('');
xlim([2.5, 57.5]); ylim([0, 400]);
caxis([-160, -140])
#+end_src

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

#+name: fig:constant_fj_vel_spectrogram
#+caption: Spectrogram of =fjsuh= during the constant Fast Jack velocity scan. Bragg (=mcoil= motor) disturbances can clearly by seen above 150Hz and they not seems to be a problem at low Bragg velocity.
#+RESULTS:
[[file:figs/constant_fj_vel_spectrogram.png]]

The (raw) measured positions of each fast jack are displayed as a function of the wanted position (i.e. IcePAP steps) in Figure [[fig:constant_fj_vel_pos_errors]].
It is clear that there are some high frequency vibrations/disturbances that are making the relation between the measured position and the wanted position not bijective.

#+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([15.99, 16.01]); ylim([15.99, 16.01]);
xticks([15.99 15.995 16 16.005 16.01]);
yticks([15.99 15.995 16 16.005 16.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/constant_fj_vel_pos_errors.pdf', 'width', 'full', 'height', 'tall');
#+end_src

#+name: fig:constant_fj_vel_pos_errors
#+caption: IcePAP steps and measured position during the scan with constant Fast Jack velocity
#+RESULTS:
[[file:figs/constant_fj_vel_pos_errors.png]]

The data is then filtered with a sharp low pass filter that filters everything above 30Hz such that the motion errors of the fast jacks are left untouched and all other disturbances are well attenuated.
The results are shown in Figure [[fig:constant_fj_vel_pos_errors_filtered_comp_raw]] where it is clear that the relation between the measured motion and the wanted motion is now a bijective function.
#+begin_src matlab :exports none
%% FIR Filter
Fs = round(1/((data_A.time(end)-data_A.time(1))/(length(data_A.time) - 1))); % Sampling Frequency [Hz]
fir_order = 5000; % Filter's order
delay = fir_order/2; % Delay induced by the filter
B_fir = firls(fir_order, ... % Filter's order
              [0 25/(Fs/2)  34/(Fs/2) 1], ... % Frequencies [Hz]
              [1 1          0          0]); % Wanted Magnitudes

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

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

#+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*data_A.fjur, 1e3*(data_A.fjur + data_A.fjur_e), 'k.', ...
    'DisplayName', 'Raw');
plot(1e3*data_A.fjuh, 1e3*(data_A.fjuh + data_A.fjuh_e), 'k.', ...
    'HandleVisibility', 'off');
plot(1e3*data_A.fjd,  1e3*(data_A.fjd + data_A.fjd_e), 'k.', ...
    'HandleVisibility', 'off');
set(gca,'ColorOrderIndex',1)
plot(1e3*data_A.fjur, 1e3*(data_A.fjur + data_A.fjur_e_filt), '-', ...
    'DisplayName', '$u_r$');
plot(1e3*data_A.fjuh, 1e3*(data_A.fjuh + data_A.fjuh_e_filt), '-', ...
    'DisplayName', '$u_h$');
plot(1e3*data_A.fjd,  1e3*(data_A.fjd + data_A.fjd_e_filt), '-', ...
    'DisplayName', '$d$');
plot(1e3*data_A.fjd,  1e3*data_A.fjd, 'k--', ...
    'DisplayName', 'Ref');
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Measured Position [mm]');
legend('location', 'southeast', 'FontSize', 8);
axis square;
xlim([15.99, 16.01]); ylim([15.99, 16.01]);
xticks([15.99 15.995 16 16.005 16.01]);
yticks([15.99 15.995 16 16.005 16.01]);
xtickangle(45);
ytickangle(90);

ax2 = nexttile();
hold on;
plot(1e3*data_A.fjur, 1e3*(data_A.fjur + data_A.fjur_e), 'k.', ...
    'DisplayName', 'Raw');
plot(1e3*data_A.fjuh, 1e3*(data_A.fjuh + data_A.fjuh_e), 'k.', ...
    'HandleVisibility', 'off');
plot(1e3*data_A.fjd,  1e3*(data_A.fjd + data_A.fjd_e), 'k.', ...
    'HandleVisibility', 'off');
set(gca,'ColorOrderIndex',1)
plot(1e3*data_A.fjur, 1e3*(data_A.fjur + data_A.fjur_e_filt), '-', ...
    'DisplayName', '$u_r$');
plot(1e3*data_A.fjuh, 1e3*(data_A.fjuh + data_A.fjuh_e_filt), '-', ...
    'DisplayName', '$u_h$');
plot(1e3*data_A.fjd,  1e3*(data_A.fjd + data_A.fjd_e_filt), '-', ...
    'DisplayName', '$d$');
plot(1e3*data_A.fjd,  1e3*data_A.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/constant_fj_vel_pos_errors_filtered_comp_raw.pdf', 'width', 'full', 'height', 'tall');
#+end_src

#+name: fig:constant_fj_vel_pos_errors_filtered_comp_raw
#+caption: IcePAP steps and measured position during the scan with constant Fast Jack velocity. Comparison of the raw and filtered data.
#+RESULTS:
[[file:figs/constant_fj_vel_pos_errors_filtered_comp_raw.png]]

If we only look at the measured position error of the fast jack (i.e. measured position minus the wanted position/IcePAP steps), we obtain the data of Figure [[fig:constant_fj_vel_pos_errors_filtered]].

The errors with a period of $5\,\mu m$ can be clearly observed.
#+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, 'k.', ...
    'DisplayName', 'Raw');
plot(1e3*fjuh, 1e6*fjuh_e, 'k.', ...
    'HandleVisibility', 'off');
plot(1e3*fjd,  1e6*fjd_e, 'k.', ...
    'HandleVisibility', 'off');
set(gca,'ColorOrderIndex',1)
plot(1e3*fjur, 1e6*fjur_e_filt, '-', ...
    'DisplayName', '$u_r$');
plot(1e3*fjuh, 1e6*fjuh_e_filt, '-', ...
    'DisplayName', '$u_h$');
plot(1e3*fjd,  1e6*fjd_e_filt, '-', ...
    'DisplayName', '$d$');
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Position Error [$\mu$m]');
legend('location', 'southeast', 'FontSize', 8);
axis square;
xlim([15.99, 16.01]);

ax2 = nexttile();
hold on;
plot(1e3*fjur, 1e6*fjur_e, 'k.', ...
    'DisplayName', 'Raw');
plot(1e3*fjuh, 1e6*fjuh_e, 'k.', ...
    'HandleVisibility', 'off');
plot(1e3*fjd,  1e6*fjd_e, 'k.', ...
    'HandleVisibility', 'off');
set(gca,'ColorOrderIndex',1)
plot(1e3*fjur, 1e6*fjur_e_filt, '-', ...
    'DisplayName', '$u_r$');
plot(1e3*fjuh, 1e6*fjuh_e_filt, '-', ...
    'DisplayName', '$u_h$');
plot(1e3*fjd,  1e6*fjd_e_filt, '-', ...
    '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/constant_fj_vel_pos_errors_filtered.pdf', 'width', 'full', 'height', 'tall');
#+end_src

#+name: fig:constant_fj_vel_pos_errors_filtered
#+caption: Raw and filtered measured position errors during the scan with constant Fast Jack velocity
#+RESULTS:
[[file:figs/constant_fj_vel_pos_errors_filtered.png]]

** LUT Creation
A Lookup Table is now computed from the filtered data with a point every $100\,nm$ of fast jack motion.
#+begin_src matlab :eval no
%% Generate LUT
createLUT(data_A, "lut/lut_const_fj_vel_12012022_1139.dat", "lut_inc", 100e-9);
#+end_src

#+begin_src matlab :exports none :tangle no
%% Generate LUT
createLUT(data_A, "./matlab/lut/lut_data_const_fj_vel_12012022_1139.dat", "lut_inc", 100e-9);
#+end_src

#+begin_src matlab :exports none
%% Load the generated LUT
data_lut = importdata("lut_data_const_fj_vel_12012022_1139.dat");
#+end_src

The obtained lookup table is displayed in Figure [[fig:constant_fj_vel_obtain_lut]].
#+begin_src matlab :exports none
%% Plot LUT Data
figure;
tiledlayout(1, 3, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([1,2]);
hold on;
plot(data_lut(:,1), 1e3*(data_lut(:,2)-data_lut(:,1)), '.', ...
    'DisplayName', '$u_r$');
plot(data_lut(:,1), 1e3*(data_lut(:,3)-data_lut(:,1)), '.', ...
    'DisplayName', '$u_h$');
plot(data_lut(:,1), 1e3*(data_lut(:,4)-data_lut(:,1)), '.', ...
    'DisplayName', '$d$');
hold off;
xlabel('IcePAP Step [mm]'); ylabel('Step Offset [$\mu$m]')
xlim([15, 22]);
legend('location', 'northeast');

ax2 = nexttile();
hold on;
plot(data_lut(:,1), 1e3*(data_lut(:,2)-data_lut(:,1)), '.', ...
    'DisplayName', '$u_r$');
plot(data_lut(:,1), 1e3*(data_lut(:,3)-data_lut(:,1)), '.', ...
    'DisplayName', '$u_h$');
plot(data_lut(:,1), 1e3*(data_lut(:,4)-data_lut(:,1)), '.', ...
    'DisplayName', '$d$');
hold off;
xlabel('IcePAP Step [mm]'); ylabel('Step Offset [$\mu$m]')
xlim([19.99, 20.01]);
#+end_src

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

#+name: fig:constant_fj_vel_obtain_lut
#+caption: Obtained Lookup Table data
#+RESULTS:
[[file:figs/constant_fj_vel_obtain_lut.png]]

** Comparison of errors in mode A and mode B
The Lookup Table is loaded in the IcePAP and a new scan is performed.

#+begin_src matlab :exports none
%% Load measurement in mode A
data_A = extractDatData(sprintf("%s/21Nov/blc13420/id21/LUT_constant_fj_vel/lut_const_fj_vel_14012022_1457.dat", data_directory), ...
                     {"bragg", "dz", "dry", "drx", "fjur", "fjuh", "fjd"}, ...
                     [pi/180, 1e-9, 1e-9, 1e-9, 1e-8, 1e-8, 1e-8]);
data_A.time = 1e-4*[1:1:length(data_A.bragg)];
data_A = processMeasData(data_A);
#+end_src

#+begin_src matlab :exports none
%% Load measurement in mode B
data_B = extractDatData(sprintf("%s/21Nov/blc13420/id21/LUT_constant_fj_vel/lut_const_fj_vel_14012022_1502.dat", data_directory), ...
                     {"bragg", "dz", "dry", "drx", "fjur", "fjuh", "fjd"}, ...
                     [pi/180, 1e-9, 1e-9, 1e-9, 1e-8, 1e-8, 1e-8]);
data_B.time = 1e-4*[1:1:length(data_A.bragg)];
data_B = processMeasData(data_B);
#+end_src

The measured position errors of the fast jacks are compared for the scan in mode A and in mode B in Figure [[fig:fj_constant_vel_comp_mode_A_B]].

#+begin_src matlab :exports none
%% Compare measured errors in mode A and mode B
figure;
tiledlayout(1, 3, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
plot(1e3*data_A.fjur,  1e6*data_A.fjur_e_filt, 'DisplayName', 'Mode A')
plot(1e3*data_B.fjur,  1e6*data_B.fjur_e_filt, 'DisplayName', 'Mode B')
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Position Error [$\mu$m]');
legend('location', 'northeast', 'FontSize', 8);
title('FJUR');

ax2 = nexttile();
hold on;
plot(1e3*data_A.fjuh,  1e6*data_A.fjuh_e_filt)
plot(1e3*data_B.fjuh,  1e6*data_B.fjuh_e_filt)
hold off;
xlabel('IcePAP Steps [mm]'); set(gca, 'YTickLabel',[]);
title('FJUH');

ax3 = nexttile();
hold on;
plot(1e3*data_A.fjd,  1e6*data_A.fjd_e_filt)
plot(1e3*data_B.fjd,  1e6*data_B.fjd_e_filt)
hold off;
xlabel('IcePAP Steps [mm]'); set(gca, 'YTickLabel',[]);
title('FJD');

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

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

#+name: fig:fj_constant_vel_comp_mode_A_B
#+caption: Comparison of the measured fast jack position errors in mode A and mode B
#+RESULTS:
[[file:figs/fj_constant_vel_comp_mode_A_B.png]]

** Test LUT just after making it
#+begin_src matlab :exports none
%% Extract measurement Data make from BLISS
data_A = extractDatData(sprintf("%s/21Nov/blc13420/id21/LUT_constant_fj_vel/lut_const_fj_vel_14012022_1720.dat", data_directory), ...
                        {"bragg", "dz", "dry", "drx", "fjur", "fjuh", "fjd"}, ...
                        [pi/180, 1e-9, 1e-9, 1e-9, 1e-8, 1e-8, 1e-8]);
data_A.time = 1e-4*[0:1:length(data_A.bragg)-1];
#+end_src

#+begin_src matlab
%% Generate LUT
createLUT(data_A, "matlab/lut/lut_data_const_fj_vel_14012022_1720.dat", "lut_inc", 100e-9);
#+end_src

#+begin_src bash
scp matlab/lut/lut_data_const_fj_vel_14012022_1720.dat opid21@lid21nano:/users/blissadm/local/beamline_configuration/DCM/CALIB/LUT/
#+end_src

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

#+begin_src matlab :exports none
%% Plot of the measured position of the FJ as a function of their wanted positions
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 Steps [mm]'); ylabel('Position Error [$\mu$m]');
legend('location', 'southeast', 'FontSize', 8);
#+end_src

#+begin_src matlab
data_files = {
    "lut_const_fj_vel_14012022_1725.dat",
    "lut_const_fj_vel_14012022_1726.dat",
    "lut_const_fj_vel_14012022_1727.dat",
    "lut_const_fj_vel_14012022_1728.dat",
    "lut_const_fj_vel_14012022_1730.dat"
};
#+end_src

#+begin_src matlab
data_400nm = {};

for i = 1:length(data_files)
    data_400nm{i} = extractDatData(sprintf("%s/21Nov/blc13420/id21/LUT_constant_fj_vel/%s", data_directory, data_files{i}), ...
                         {"bragg", "dz", "dry", "drx", "fjur", "fjuh", "fjd"}, ...
                         [pi/180, 1e-9, 1e-9, 1e-9, 1e-8, 1e-8, 1e-8]);

    data_400nm{i}.ddz  = 10.5e-3./(2*cos(data_400nm{i}.bragg)) - data_400nm{i}.dz;
    data_400nm{i}.time = 1e-4*[1:1:length(data_400nm{i}.bragg)];

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

    data_400nm{i}.fjur_e = error(1,:)'; % [m]
    data_400nm{i}.fjuh_e = error(2,:)'; % [m]
    data_400nm{i}.fjd_e  = error(3,:)'; % [m]

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

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

#+begin_src matlab
%% Re-sample data to have same data points in FJUR
for i = 1:length(data_files)
    [data_400nm{i}.fjur_e_resampl, data_400nm{i}.fjur_resampl] = resample(data_400nm{i}.fjur_e_filt, data_400nm{i}.fjur, 1/100e-9);
    [data_400nm{i}.fjuh_e_resampl, data_400nm{i}.fjuh_resampl] = resample(data_400nm{i}.fjuh_e_filt, data_400nm{i}.fjuh, 1/100e-9);
    [data_400nm{i}.fjd_e_resampl, data_400nm{i}.fjd_resampl] = resample(data_400nm{i}.fjd_e_filt, data_400nm{i}.fjd, 1/100e-9);
end
#+end_src

#+begin_src matlab
%% Mean Motion
fjur_400nm_e_mean = mean(cell2mat(cellfun(@(x) x.fjur_e_resampl, data_400nm, "UniformOutput", false)), 2);
fjuh_400nm_e_mean = mean(cell2mat(cellfun(@(x) x.fjuh_e_resampl, data_400nm, "UniformOutput", false)), 2);
fjd_400nm_e_mean = mean(cell2mat(cellfun(@(x) x.fjd_e_resampl, data_400nm, "UniformOutput", false)), 2);
#+end_src

#+begin_src matlab :exports none
%% Plot of the measured position of the FJ as a function of their wanted positions
figure;
hold on;
for i = 1:length(data_files)
    plot(1e3*data_400nm{i}.fjur, 1e6*(data_400nm{i}.fjur_e_filt), 'color', [colors(1,:),0.5], ...
        'HandleVisibility', 'off');
    plot(1e3*data_400nm{i}.fjuh, 1e6*(data_400nm{i}.fjuh_e_filt), 'color', [colors(2,:),0.5], ...
        'HandleVisibility', 'off');
    plot(1e3*data_400nm{i}.fjd, 1e6*(data_400nm{i}.fjd_e_filt), 'color', [colors(3,:),0.5], ...
        'HandleVisibility', 'off');
end
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Position Error [$\mu$m]');
legend('location', 'southeast', 'FontSize', 8);
#+end_src

#+begin_src matlab :exports none
%% Plot of the measured position of the FJ as a function of their wanted positions
figure;
hold on;
plot(1e3*data_400nm{1}.fjur_resampl,  1e6*(fjur_400nm_e_mean), '-', ...
    'DisplayName', 'fjur');
plot(1e3*data_400nm{1}.fjuh_resampl,  1e6*(fjuh_400nm_e_mean), '-', ...
    'DisplayName', 'fjuh');
plot(1e3*data_400nm{1}.fjd_resampl,  1e6*(fjd_400nm_e_mean), '-', ...
    'DisplayName', 'fjd');
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Repeatable part [$\mu$m]');
legend('location', 'southeast', 'FontSize', 8);
#+end_src

#+begin_src matlab
%% Compute RMS error in mode B with LUT every 400nmm
fjur_400nm_rms = 1e9*mean(cellfun(@(x) rms(detrend(x.fjur_e_resampl - fjur_e_mean, 0)), data_400nm))
#+end_src

#+begin_src matlab :results value replace :exports results :tangle no
sprintf('FJUR = %.1f [nm, RMS] in mode B after several minutes (1 um LUT increment)', fjur_400nm_rms);
#+end_src

#+RESULTS:
: FJUR = 76.2 [nm, RMS] in mode B after several minutes (1 um LUT increment)

Repeatable Part:
#+begin_src matlab

#+end_src

#+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;
for i = 1:length(data_files)
    plot(1e3*data_1u{i}.fjur, 1e6*(data_1u{i}.fjur_e_filt), 'color', [colors(1,:),0.5], ...
        'HandleVisibility', 'off');
end
plot(1e3*data_1u{1}.fjur_resampl,  1e6*(fjur_1u_e_mean), 'k-', ...
    'DisplayName', 'Mean');
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Position Error [$\mu$m]');
title('After 5 minutes');
legend('location', 'southeast', 'FontSize', 8);

ax2 = nexttile();
hold on;
for i = 1:length(data_files)
    plot(1e3*data_400nm{i}.fjur, 1e6*(data_400nm{i}.fjur_e_filt), 'color', [colors(1,:),0.5], ...
        'HandleVisibility', 'off');
end
plot(1e3*data_400nm{1}.fjur_resampl,  1e6*(fjur_400nm_e_mean), 'k-', ...
    'DisplayName', 'Mean');
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Position Error [$\mu$m]');
title('After 15 minutes');
legend('location', 'southeast', 'FontSize', 8);

linkaxes([ax1,ax2],'xy');
ylim([-0.4, 0.4]);
#+end_src

** Make a LUT based on mode B
#+begin_src matlab :exports none
%% Extract measurement Data make from BLISS
data_A = extractDatData(sprintf("%s/21Nov/blc13420/id21/LUT_constant_fj_vel/lut_const_fj_vel_14012022_1725.dat", data_directory), ...
                        {"bragg", "dz", "dry", "drx", "fjur", "fjuh", "fjd"}, ...
                        [pi/180, 1e-9, 1e-9, 1e-9, 1e-8, 1e-8, 1e-8]);
data_A.time = 1e-4*[0:1:length(data_A.bragg)-1];
#+end_src

#+begin_src matlab
%% Generate LUT
createLUT(data_A, "matlab/lut/lut_data_bis_const_fj_vel_14012022_1720.dat", "lut_inc", 100e-9);
#+end_src

#+begin_src matlab
data_lut_1 = importdata("lut_data_const_fj_vel_14012022_1720.dat");
data_lut_2 = importdata("lut_data_bis_const_fj_vel_14012022_1720.dat");
#+end_src

#+begin_src matlab :exports none
%% Plot of the measured position of the FJ as a function of their wanted positions
figure;
hold on;
plot(data_lut_1(:,1), 1e3*(data_lut_1(:,1)-data_lut_1(:,2)), '.', ...
    'DisplayName', '$u_r$');
plot(data_lut_1(:,1), 1e3*(data_lut_1(:,1)-data_lut_1(:,3)), '.', ...
    'DisplayName', '$u_h$');
plot(data_lut_1(:,1), 1e3*(data_lut_1(:,1)-data_lut_1(:,4)), '.', ...
    'DisplayName', '$d$');
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Position Error [$\mu$m]');
legend('location', 'southeast', 'FontSize', 8);
#+end_src

#+begin_src matlab :exports none
%% Plot of the measured position of the FJ as a function of their wanted positions
figure;
hold on;
plot(data_lut_2(:,1), 1e3*(data_lut_2(:,1)-data_lut_2(:,2)), '.', ...
    'DisplayName', '$u_r$');
plot(data_lut_2(:,1), 1e3*(data_lut_2(:,1)-data_lut_2(:,3)), '.', ...
    'DisplayName', '$u_h$');
plot(data_lut_2(:,1), 1e3*(data_lut_2(:,1)-data_lut_2(:,4)), '.', ...
    'DisplayName', '$d$');
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Position Error [$\mu$m]');
legend('location', 'southeast', 'FontSize', 8);
#+end_src

Update the LUT:
#+begin_src matlab
fj_start = max([data_lut_1(1,1), data_lut_2(1,1)]);
fj_end = min([data_lut_1(end,1), data_lut_2(end,1)]);

fj_i_1 = data_lut_1(:,1) >= fj_start & data_lut_1(:,1) <= fj_end;
fj_i_2 = data_lut_2(:,1) >= fj_start & data_lut_2(:,1) <= fj_end;

sum(fj_i_1) == sum(fj_i_2)
#+end_src

#+begin_src matlab
data_lut_merge = data_lut_1(fj_i_1, :);
data_lut_merge(:, 2) = data_lut_merge(:, 2) + (data_lut_2(fj_i_2, 2) - data_lut_2(fj_i_2, 1));
data_lut_merge(:, 3) = data_lut_merge(:, 3) + (data_lut_2(fj_i_2, 3) - data_lut_2(fj_i_2, 1));
data_lut_merge(:, 4) = data_lut_merge(:, 4) + (data_lut_2(fj_i_2, 4) - data_lut_2(fj_i_2, 1));
#+end_src

#+begin_src matlab :exports none
%% Plot of the measured position of the FJ as a function of their wanted positions
figure;
hold on;
plot(data_lut_1(:,1), 1e3*(data_lut_1(:,1)-data_lut_1(:,2)), '.', ...
    'DisplayName', '$u_r$');
plot(data_lut_merge(:,1), 1e3*(data_lut_merge(:,1)-data_lut_merge(:,2)), '.', ...
    'DisplayName', '$u_r$');
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Position Error [$\mu$m]');
legend('location', 'southeast', 'FontSize', 8);
#+end_src

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

fileID = fopen("matlab/lut/lut_data_merge_const_fj_vel_14012022_1720.dat", 'w');
fprintf(fileID, formatSpec, data_lut_merge');
fclose(fileID);
#+end_src

#+begin_src matlab
data_lut_merge = importdata("lut_data_merge_const_fj_vel_14012022_1720.dat");
#+end_src

Verify if it makes things better

#+begin_src matlab
data_files = {
    "lut_const_fj_vel_14012022_1816.dat"
};
#+end_src

#+begin_src matlab
data_it = {};

for i = 1:length(data_files)
    data_it{i} = extractDatData(sprintf("%s/21Nov/blc13420/id21/LUT_constant_fj_vel/%s", data_directory, data_files{i}), ...
                         {"bragg", "dz", "dry", "drx", "fjur", "fjuh", "fjd"}, ...
                         [pi/180, 1e-9, 1e-9, 1e-9, 1e-8, 1e-8, 1e-8]);

    data_it{i}.ddz  = 10.5e-3./(2*cos(data_it{i}.bragg)) - data_it{i}.dz;
    data_it{i}.time = 1e-4*[1:1:length(data_it{i}.bragg)];

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

    data_it{i}.fjur_e = error(1,:)'; % [m]
    data_it{i}.fjuh_e = error(2,:)'; % [m]
    data_it{i}.fjd_e  = error(3,:)'; % [m]

    %% Filtering all measured Fast Jack Position using the FIR filter
    data_it{i}.fjur_e_filt = filter(B_fir, 1, data_it{i}.fjur_e);
    data_it{i}.fjuh_e_filt = filter(B_fir, 1, data_it{i}.fjuh_e);
    data_it{i}.fjd_e_filt  = filter(B_fir, 1, data_it{i}.fjd_e);
    %% Compensation of the delay introduced by the FIR filter
    data_it{i}.fjur_e_filt(1:end-delay) = data_it{i}.fjur_e_filt(delay+1:end);
    data_it{i}.fjuh_e_filt(1:end-delay) = data_it{i}.fjuh_e_filt(delay+1:end);
    data_it{i}.fjd_e_filt( 1:end-delay) = data_it{i}.fjd_e_filt( delay+1:end);
end
#+end_src

#+begin_src matlab
%% Re-sample data to have same data points in FJUR
for i = 1:length(data_files)
    [data_it{i}.fjur_e_resampl, data_it{i}.fjur_resampl] = resample(data_it{i}.fjur_e_filt, data_it{i}.fjur, 1/100e-9);
    [data_it{i}.fjuh_e_resampl, data_it{i}.fjuh_resampl] = resample(data_it{i}.fjuh_e_filt, data_it{i}.fjuh, 1/100e-9);
    [data_it{i}.fjd_e_resampl, data_it{i}.fjd_resampl] = resample(data_it{i}.fjd_e_filt, data_it{i}.fjd, 1/100e-9);
end
#+end_src

#+begin_src matlab
%% Mean Motion
fjur_it_e_mean = mean(cell2mat(cellfun(@(x) x.fjur_e_resampl, data_it, "UniformOutput", false)), 2);
fjuh_it_e_mean = mean(cell2mat(cellfun(@(x) x.fjuh_e_resampl, data_it, "UniformOutput", false)), 2);
fjd_it_e_mean = mean(cell2mat(cellfun(@(x) x.fjd_e_resampl, data_it, "UniformOutput", false)), 2);
#+end_src

#+begin_src matlab :exports none
%% Plot of the measured position of the FJ as a function of their wanted positions
figure;
hold on;
for i = 1:length(data_files)
    plot(1e3*data_it{i}.fjur, 1e6*(data_it{i}.fjur_e_filt), 'color', [colors(1,:),0.5], ...
        'HandleVisibility', 'off');
    plot(1e3*data_it{i}.fjuh, 1e6*(data_it{i}.fjuh_e_filt), 'color', [colors(2,:),0.5], ...
        'HandleVisibility', 'off');
    plot(1e3*data_it{i}.fjd, 1e6*(data_it{i}.fjd_e_filt), 'color', [colors(3,:),0.5], ...
        'HandleVisibility', 'off');
end
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Position Error [$\mu$m]');
legend('location', 'southeast', 'FontSize', 8);
#+end_src

** TODO [#A] Repeatability of stepper errors
#+begin_src matlab :exports none
%% Extract measurement Data make from BLISS
data_A = extractDatData(sprintf("%s/21Nov/blc13420/id21/LUT_constant_fj_vel/lut_const_fj_vel_12012022_1139.dat", data_directory), ...
                     {"bragg", "dz", "dry", "drx", "fjur", "fjuh", "fjd"}, ...
                      [pi/180, 1e-9, 1e-9, 1e-9, 1e-8, 1e-8, 1e-8]);
data_A.time = 1e-4*[0:1:length(data_A.bragg)-1];
data_A = processMeasData(data_A);
#+end_src

#+begin_src matlab :exports none
%% FIR Filter
Fs = round(1/((data_A.time(end)-data_A.time(1))/(length(data_A.time) - 1))); % Sampling Frequency [Hz]
fir_order = 5000; % Filter's order
delay = fir_order/2; % Delay induced by the filter
B_fir = firls(fir_order, ... % Filter's order
              [0 25/(Fs/2)  34/(Fs/2) 1], ... % Frequencies [Hz]
              [1 1          0          0]); % Wanted Magnitudes
#+end_src

#+begin_src matlab :exports none
%% Filtering all measured Fast Jack Position using the FIR filter
data_A.fjur_e_filt = filter(B_fir, 1, data_A.fjur_e);
data_A.fjuh_e_filt = filter(B_fir, 1, data_A.fjuh_e);
data_A.fjd_e_filt  = filter(B_fir, 1, data_A.fjd_e);

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

#+begin_src matlab :exports none
%% FIR Filter
fir_order = 5000; % Filter's order
delay = fir_order/2; % Delay induced by the filter
B_fir = firls(fir_order, ... % Filter's order
              [0 4/(Fs/2)  6/(Fs/2) 1], ... % Frequencies [Hz]
              [0 0         1        1]); % Wanted Magnitudes
#+end_src

#+begin_src matlab :exports none
%% Filtering all measured Fast Jack Position using the FIR filter
data_A.fjur_e_filt = filter(B_fir, 1, data_A.fjur_e_filt);
data_A.fjuh_e_filt = filter(B_fir, 1, data_A.fjuh_e_filt);
data_A.fjd_e_filt  = filter(B_fir, 1, data_A.fjd_e_filt);

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

#+begin_src matlab :exports none
%% Plot of the measured position of the FJ as a function of their wanted positions
figure;
hold on;
plot(200*mod(1e3*data_A.fjur, 1), 1/(5e-6)*data_A.fjur_e_filt, '.', ...
    'DisplayName', '$u_r$');
plot(200*mod(1e3*data_A.fjuh, 1), 1/(5e-6)*data_A.fjuh_e_filt, '.', ...
    'DisplayName', '$u_h$');
plot(200*mod(1e3*data_A.fjd, 1),  1/(5e-6)*data_A.fjd_e_filt, '.', ...
    'DisplayName', '$d$');
hold off;
xlabel('Motor position [Step]'); ylabel('Position Error [step]');
xlim([0, 200]); ylim([-0.1, 0.1])
legend('location', 'southeast', 'FontSize', 8);
#+end_src

#+begin_src matlab :exports none
%% Plot of the measured position of the FJ as a function of their wanted positions
figure;
hold on;
plot(1/5*mod(1e6*data_A.fjur, 20), 1e6*data_A.fjur_e_filt, '.', ...
    'DisplayName', '$u_r$');
% plot(1e3*data_A.fjuh, 1e6*data_A.fjuh_e_filt, '-', ...
%     'DisplayName', '$u_h$');
% plot(1e3*data_A.fjd,  1e6*data_A.fjd_e_filt, '-', ...
%     'DisplayName', '$d$');
hold off;
xlabel('Full Step'); ylabel('Position Error [$\mu$m]');
legend('location', 'southeast', 'FontSize', 8);
#+end_src

#+begin_src matlab :exports none
%% Plot of the measured position of the FJ as a function of their wanted positions
figure;
hold on;
plot(mod(1e6*data_A.fjur, 10), 1e6*data_A.fjur_e_filt, '.', ...
    'DisplayName', '$u_r$');
% plot(1e3*data_A.fjuh, 1e6*data_A.fjuh_e_filt, '-', ...
%     'DisplayName', '$u_h$');
% plot(1e3*data_A.fjd,  1e6*data_A.fjd_e_filt, '-', ...
%     'DisplayName', '$d$');
hold off;
xlabel('IcePAP Steps [um]'); ylabel('Position Error [$\mu$m]');
legend('location', 'southeast', 'FontSize', 8);
#+end_src

#+begin_src matlab :exports none
%% Plot of the measured position of the FJ as a function of their wanted positions
figure;
hold on;
plot(1/5*mod(1e6*data_A.fjur, 5), 1e6*data_A.fjur_e_filt, '.', ...
    'DisplayName', '$u_r$');
% plot(1e3*data_A.fjuh, 1e6*data_A.fjuh_e_filt, '-', ...
%     'DisplayName', '$u_h$');
% plot(1e3*data_A.fjd,  1e6*data_A.fjd_e_filt, '-', ...
%     'DisplayName', '$d$');
hold off;
xlabel('Full step'); ylabel('Position Error [$\mu$m]');
legend('location', 'southeast', 'FontSize', 8);
#+end_src

#+begin_src matlab

#+end_src




















#+begin_src matlab :exports none
%% FIR Filter
fir_order = 5000; % Filter's order
delay = fir_order/2; % Delay induced by the filter
B_fir = firls(fir_order, ... % Filter's order
              [0 4/(Fs/2)  6/(Fs/2) 1], ... % Frequencies [Hz]
              [0 0         1        1]); % Wanted Magnitudes
#+end_src

#+begin_src matlab
%% Computation of filters' responses
[h_fir, f] = freqz(B_fir,     1, 10000, Fs);
#+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_fir),     'DisplayName', 'FIR');
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_fir));
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


* Effect of the number of points in the trajectory in mode B
<<sec:optimal_trajectory_mode_B>>
** Introduction                                                      :ignore:
The goal here is to see if the taken distance between points of the trajectory can affect the positioning accuracy of mode B.

To do so, a LUT is computed, and then several scans are performed with different distances between trajectory points.

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

** LUT
A first trajectory is performed to compute the Lookup Table.
#+begin_src matlab :exports none
%% Extract measurement Data make from BLISS
data_A = extractDatData(sprintf("%s/21Nov/blc13420/id21/LUT_constant_fj_vel/lut_const_fj_vel_14012022_1645.dat", data_directory), ...
                        {"bragg", "dz", "dry", "drx", "fjur", "fjuh", "fjd"}, ...
                        [pi/180, 1e-9, 1e-9, 1e-9, 1e-8, 1e-8, 1e-8]);
data_A.time = 1e-4*[0:1:length(data_A.bragg)-1];
data_A = processMeasData(data_A);
#+end_src

#+begin_src matlab :eval no
%% Generate LUT
createLUT(data_A, "lut/lut_const_fj_vel_14012022_1645.dat", "lut_inc", 250e-9);
#+end_src

#+begin_src matlab :exports none :tangle no
%% Generate LUT
createLUT(data_A, "./matlab/lut/lut_const_fj_vel_14012022_1645.dat", "lut_inc", 250e-9);
#+end_src

#+begin_src matlab :exports none
%% Load the generated LUT
data_lut = importdata("lut_const_fj_vel_14012022_1645.dat");
#+end_src

The obtained lookup table is displayed in Figure [[fig:lut_comp_nb_points_trajectory]].
#+begin_src matlab :exports none
%% Plot LUT Data
figure;
tiledlayout(1, 3, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([1,2]);
hold on;
plot(data_lut(:,1), 1e3*(data_lut(:,2)-data_lut(:,1)), '.', ...
    'DisplayName', '$u_r$');
plot(data_lut(:,1), 1e3*(data_lut(:,3)-data_lut(:,1)), '.', ...
    'DisplayName', '$u_h$');
plot(data_lut(:,1), 1e3*(data_lut(:,4)-data_lut(:,1)), '.', ...
    'DisplayName', '$d$');
hold off;
xlabel('IcePAP Step [mm]'); ylabel('Step Offset [$\mu$m]')
xlim([15, 22]);
legend('location', 'northeast');

ax2 = nexttile();
hold on;
plot(data_lut(:,1), 1e3*(data_lut(:,2)-data_lut(:,1)), '.', ...
    'DisplayName', '$u_r$');
plot(data_lut(:,1), 1e3*(data_lut(:,3)-data_lut(:,1)), '.', ...
    'DisplayName', '$u_h$');
plot(data_lut(:,1), 1e3*(data_lut(:,4)-data_lut(:,1)), '.', ...
    'DisplayName', '$d$');
hold off;
xlabel('IcePAP Step [mm]'); ylabel('Step Offset [$\mu$m]')
xlim([19.99, 20.01]);
#+end_src

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

#+name: fig:lut_comp_nb_points_trajectory
#+caption: Computed LUT that will be used for further tests about the effect of the number of points taken in the trajectory
#+RESULTS:
[[file:figs/lut_comp_nb_points_trajectory.png]]

** Trajectory with increment of $1\,\mu m$
A trajectory is loaded with 1000 points every millimeter of fast jack motion:
#+begin_src python :eval no
tdh.lut_constant_fj_vel(15.5, 21.5, pts_per_mm=1000, use_lut=True)
#+end_src

#+begin_src matlab :exports none
%% Data files in mode B with point every 1000nm
data_files_1u = {
    "lut_const_fj_vel_14012022_1652.dat",
    "lut_const_fj_vel_14012022_1654.dat",
    "lut_const_fj_vel_14012022_1656.dat",
    "lut_const_fj_vel_14012022_1657.dat",
    "lut_const_fj_vel_14012022_1659.dat"
};
#+end_src

#+begin_src matlab :exports none
%% Load Data
data_1u = {};

for i = 1:length(data_files_1u)
    data_1u{i} = extractDatData(sprintf("%s/21Nov/blc13420/id21/LUT_constant_fj_vel/%s", data_directory, data_files_1u{i}), ...
                         {"bragg", "dz", "dry", "drx", "fjur", "fjuh", "fjd"}, ...
                         [pi/180, 1e-9, 1e-9, 1e-9, 1e-8, 1e-8, 1e-8]);
    data_1u{i}.time = 1e-4*[1:1:length(data_1u{i}.bragg)];
    data_1u{i} = processMeasData(data_1u{i});
end
#+end_src

Several scans in mode B are performed and the results are shown in Figure
#+begin_src matlab :exports none
%% Plot of the measured position error of the FJ as a function of their wanted positions
figure;
hold on;
for i = 1:length(data_files_1u)
    plot(1e3*data_1u{i}.fjur, 1e9*(data_1u{i}.fjur_e_filt), 'color', [colors(1,:),0.5]);
end
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Position Error [nm]');
ylim([-400, 400]);
#+end_src

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

#+name: fig:fj_errors_mode_B_traj_inc_1u
#+caption: Measured position errors of the fast jacks
#+RESULTS:
[[file:figs/fj_errors_mode_B_traj_inc_1u.png]]

** Trajectory with increment of $0.4\,\mu m$
A trajectory is loaded with 2500 points every millimeter of fast jack motion:
#+begin_src python :eval no
tdh.lut_constant_fj_vel(15.5, 21.5, pts_per_mm=2500, use_lut=True)
#+end_src

The obtained errors on =fjur= are shown in Figure [[fig:fj_errors_mode_B_traj_inc_400nm]].
#+begin_src matlab :exports none
%% Data files in mode B with point every 400nm
data_files_400nm = {
    "lut_const_fj_vel_14012022_1702.dat",
    "lut_const_fj_vel_14012022_1704.dat",
    "lut_const_fj_vel_14012022_1705.dat",
    "lut_const_fj_vel_14012022_1707.dat",
    "lut_const_fj_vel_14012022_1708.dat"
};
#+end_src

#+begin_src matlab :exports none
%% Load and process all data
data_400nm = {};

for i = 1:length(data_files_400nm)
    data_400nm{i} = extractDatData(sprintf("%s/21Nov/blc13420/id21/LUT_constant_fj_vel/%s", data_directory, data_files_400nm{i}), ...
                         {"bragg", "dz", "dry", "drx", "fjur", "fjuh", "fjd"}, ...
                         [pi/180, 1e-9, 1e-9, 1e-9, 1e-8, 1e-8, 1e-8]);
    data_400nm{i}.time = 1e-4*[1:1:length(data_400nm{i}.bragg)];
    data_400nm{i} = processMeasData(data_400nm{i});
end
#+end_src

#+begin_src matlab :exports none
%% Plot of the measured position of the FJ as a function of their wanted positions
figure;
hold on;
for i = 1:length(data_files_400nm)
    plot(1e3*data_400nm{i}.fjur, 1e9*(data_400nm{i}.fjur_e_filt), 'color', [colors(1,:),0.5]);
end
hold off;
xlabel('IcePAP Steps [mm]'); ylabel('Position Error [$\mu$m]');
ylim([-400, 400]);
#+end_src

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

#+name: fig:fj_errors_mode_B_traj_inc_400nm
#+caption: Measured position errors of the fast jacks
#+RESULTS:
[[file:figs/fj_errors_mode_B_traj_inc_400nm.png]]

** Spatial Errors - Comparison
The spatial periods of errors for the two trajectories are compared in Figure [[fig:spatial_errors_comp_trajectory_points]].
Even though the trajectory with an increment of $0.4\,\mu m$ was done after the trajectory with an increment of $1\,\mu m$ (and therefore the errors in mode B should be higher), the errors for a period of $5\,\mu m$ are reduced.

It should be further investigated whether using small increments for the trajectory could help reducing the $5\,\mu m$ period errors.

#+begin_src matlab :exports none
%% Hanning window used for pwelch function
win = hanning(floor(5e-3/100e-9));

[Sxx, f] = pwelch(detrend(data_A.fjur_e_resampl, 0), win, 0, [], 1/100e-9);
data_A.S_fjur = Sxx;

%% Compute the PSD of estimated fjur errors in mode B
for i = 1:length(data_files_1u)
    [Sxx, ~] = pwelch(detrend(data_1u{i}.fjur_e_resampl, 0), win, 0, [], 1/100e-9);
    data_1u{i}.S_fjur = Sxx;
end
for i = 1:length(data_files_400nm)
    [Sxx, ~] = pwelch(detrend(data_400nm{i}.fjur_e_resampl, 0), win, 0, [], 1/100e-9);
    data_400nm{i}.S_fjur = Sxx;
end
#+end_src

#+begin_src matlab :exports none
%% ASD of measured errors with small spatial periods
figure;
tiledlayout(1, 3, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([1,2]);
hold on;
plot(1e6./f, 1e9*sqrt(data_A.S_fjur), 'k-', ...
    'DisplayName', 'mode A')
plot(1e6./f, 1e9*sqrt(data_1u{1}.S_fjur), 'color', [colors(2,:), 0.5], ...
    'DisplayName', '$1\mu$m inc.')
plot(1e6./f, 1e9*sqrt(data_400nm{1}.S_fjur), 'color', [colors(1,:), 0.5], ...
    'DisplayName', '400nm inc.')
for i = 2:length(data_files_1u)
    plot(1e6./f, 1e9*sqrt(data_1u{i}.S_fjur), 'color', [colors(2,:), 0.5], ...
        'HandleVisibility', 'off')
end
for i = 2:length(data_files_400nm)
    plot(1e6./f, 1e9*sqrt(data_400nm{i}.S_fjur), 'color', [colors(1,:), 0.5], ...
        'HandleVisibility', 'off')
end
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'lin');
xlabel('Spatial Period [$\mu$m]'); ylabel('ASD [nm/$1/\sqrt{m}$]');
xlim([4.5, 25]);
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile();
hold on;
plot(1e6./f, 1e9*sqrt(data_A.S_fjur), 'k-', ...
    'DisplayName', 'mode A')
plot(1e6./f, 1e9*sqrt(data_1u{1}.S_fjur), 'color', [colors(2,:), 0.5], ...
    'DisplayName', '$1\mu$m inc.')
plot(1e6./f, 1e9*sqrt(data_400nm{1}.S_fjur), 'color', [colors(1,:), 0.5], ...
    'DisplayName', '400nm inc.')
for i = 2:length(data_files_1u)
    plot(1e6./f, 1e9*sqrt(data_1u{i}.S_fjur), 'color', [colors(2,:), 0.5], ...
        'HandleVisibility', 'off')
end
for i = 2:length(data_files_400nm)
    plot(1e6./f, 1e9*sqrt(data_400nm{i}.S_fjur), 'color', [colors(1,:), 0.5], ...
        'HandleVisibility', 'off')
end
hold off;
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'lin');
xlabel('Spatial Period [$\mu$m]'); ylabel('ASD [nm/$1/\sqrt{m}$]');
xlim([4.98, 5.02]); ylim([0, 1.5]);
#+end_src

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

#+name: fig:spatial_errors_comp_trajectory_points
#+caption: Spectral density of the =fjur= measured position errors for both trajectories. For the errors with a spatial periods of $5\,\mu m$, taking smaller steps in the trajectory helps reducing the errors.
#+RESULTS:
[[file:figs/spatial_errors_comp_trajectory_points.png]]

* LUT for energy scans (XANES)
<<sec:lut_xanes>>
** Introduction                                                      :ignore:
In this section,

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

** Velocities

#+begin_src matlab
%% Scan parameters
scan_name = {'P',  'S',  'Cl',  'Cd', 'Ca', 'Ti', 'V',  'Cr', 'Mn', 'Fe', 'Cu'}; % Element Name
start_ene = 1e3*[2.14, 2.45, 2.895, 3.52, 3.95, 4.94, 5.45, 5.98, 6.52, 7.1 , 8.98]; % [ev]
end_ene   = 1e3*[2.19, 2.55, 2.995, 3.65, 4.15, 5.1 , 5.57, 6.14, 6.75, 7.25, 9.12]; % [ev]
step_ene  = [0.25, 0.25, 0.3,   0.3,  0.4,  0.4,  0.5,  0.5,  0.5, 0.5, 0.5]; % Scan Steps [ev]

dwell_time_min = 0.01; % corresponds to max velocity [s]
dwell_time_max = 0.1;  % corresponds to min velocity [s]
#+end_src

#+begin_src matlab
dspacing = 3.13501196169967; % [Angstrom]
#+end_src

#+begin_src matlab
%% Scan objects
scans = {};
for i = 1:length(start_ene)
    scans{i}.name = scan_name{i};

    scans{i}.traj_ene   = start_ene(i):step_ene(i):end_ene(i); % [eV]
    scans{i}.traj_bragg = asin(12398.4./scans{i}.traj_ene/2/dspacing); % [rad]
    scans{i}.traj_fjs   = 0.030427 - (10.5e-3)./(2*cos(scans{i}.traj_bragg)); % [m]

    scans{i}.time_slow = dwell_time_max*0:1:length(scans{i}.traj_ene)-1; % [s]
    scans{i}.time_fast = dwell_time_min*0:1:length(scans{i}.traj_ene)-1; % [s]

    scans{i}.vel_fast_bragg = abs([scans{i}.traj_bragg(2:end) - scans{i}.traj_bragg(1:end-1), 0]/dwell_time_min); % [rad/s]
    scans{i}.vel_fast_fjs = abs([scans{i}.traj_fjs(2:end) - scans{i}.traj_fjs(1:end-1), 0]/dwell_time_min); % [m/s]
end
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([1e3*cellfun(@(x) x.traj_fjs(1), scans); 1e3*cellfun(@(x) x.traj_fjs(end), scans); 1e3*cellfun(@(x) x.traj_fjs(end)-x.traj_fjs(1), scans); 1e3*cellfun(@(x) max(x.vel_fast_fjs), scans);]', scan_name, {'Scan Name', 'FJ min [mm]', 'FJ max [mm]', 'FJ stroke [mm]', 'Max FJ vel [mm/s]'}, ' %.3f ');
#+end_src

#+name: tab:typical_experiments_fj
#+caption: Fast Jack Stroke and Velocity for typical experiments
#+attr_latex: :environment tabularx :width \linewidth :align lXXXX
#+attr_latex: :center t :booktabs t
#+RESULTS:
| Scan Name | FJ min [mm] | FJ max [mm] | FJ stroke [mm] | Max FJ vel [mm/s] |
|-----------+-------------+-------------+----------------+-------------------|
| P         |      16.696 |      18.212 |          1.516 |             0.936 |
| S         |      21.535 |      22.112 |          0.577 |             0.169 |
| Cl        |      23.239 |      23.437 |          0.198 |             0.065 |
| Cd        |      24.081 |      24.181 |            0.1 |             0.025 |
| Ca        |      24.362 |      24.456 |          0.093 |             0.021 |
| Ti        |      24.698 |      24.731 |          0.033 |             0.009 |
| V         |      24.793 |      24.811 |          0.018 |             0.008 |
| Cr        |      24.864 |      24.882 |          0.017 |             0.006 |
| Mn        |      24.918 |      24.936 |          0.019 |             0.004 |
| Fe        |      24.961 |       24.97 |          0.009 |             0.003 |
| Cu        |      25.045 |      25.049 |          0.004 |             0.002 |

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([180/pi*cellfun(@(x) x.traj_bragg(end), scans);180/pi*cellfun(@(x) x.traj_bragg(1), scans);  180/pi*cellfun(@(x) x.traj_bragg(1)-x.traj_bragg(end), scans); 80/pi*cellfun(@(x) max(x.vel_fast_bragg), scans)]', scan_name, {'Scan Name', 'Bragg min [deg]', 'Bragg max [deg]', 'Bragg stroke [deg]', 'Max bragg vel [deg/s]'}, ' %.3f ');
#+end_src

#+name: tab:typical_experiments_bragg
#+caption: Bragg Stroke and Velocity for typical experiments
#+attr_latex: :environment tabularx :width \linewidth :align lXXXX
#+attr_latex: :center t :booktabs t
#+RESULTS:
| Scan Name | Bragg min [deg] | Bragg max [deg] | Bragg stroke [deg] | Max bragg vel [deg/s] |
|-----------+-----------------+-----------------+--------------------+-----------------------|
| P         |          64.545 |          67.521 |              2.976 |                 0.719 |
| S         |          50.846 |          53.814 |              2.968 |                 0.355 |
| Cl        |           41.32 |          43.082 |              1.762 |                 0.247 |
| Cd        |          32.804 |          34.178 |              1.374 |                 0.147 |
| Ca        |          28.456 |           30.04 |              1.584 |                 0.149 |
| Ti        |          22.813 |          23.596 |              0.783 |                  0.09 |
| V         |          20.794 |          21.274 |               0.48 |                 0.091 |
| Cr        |          18.787 |          19.309 |              0.522 |                 0.075 |
| Mn        |          17.035 |          17.655 |               0.62 |                 0.062 |
| Fe        |          15.828 |          16.171 |              0.343 |                 0.052 |
| Cu        |          12.522 |          12.721 |              0.198 |                 0.032 |

Based on Table [[tab:typical_experiments_bragg]].
- To work without =mcoil=, the maximum bragg stroke should be 16 degrees.
  Therefore, all the scans can be performed without =mcoil=.

Frequency of $5\mu m$ errors:
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([cellfun(@(x) max(x.vel_fast_fjs)./5e-6, scans)]', scan_name, {'Scan Name', 'Max freq. [Hz]'}, ' %.1f ');
#+end_src

#+RESULTS:
| Scan Name | Max freq. [Hz] |
|-----------+----------------|
| P         |          187.2 |
| S         |           33.9 |
| Cl        |           13.0 |
| Cd        |            5.0 |
| Ca        |            4.1 |
| Ti        |            1.8 |
| V         |            1.6 |
| Cr        |            1.1 |
| Mn        |            0.9 |
| Fe        |            0.6 |
| Cu        |            0.3 |

Estimation of maximum velocity such that the $5\mu m$ errors are reduced by a factor 50 (i.e. the frequency of this $5\mu m$ should be below 2Hz, see sensitivity function).

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([cellfun(@(x) dwell_time_min./(2/(max(x.vel_fast_fjs)./5e-6)), scans); cellfun(@(x) dwell_time_min./(10/(max(x.vel_fast_fjs)./5e-6)), scans)]', scan_name, {'Scan Name', 'Min time [s] - 2Hz', 'Min time [s] - 10Hz'}, ' %.4f ');
#+end_src

#+RESULTS:
| Scan Name | Min time [s] - 2Hz | Min time [s] - 10Hz |
|-----------+--------------------+---------------------|
| P         |             0.9358 |              0.1872 |
| S         |             0.1695 |              0.0339 |
| Cl        |             0.0651 |               0.013 |
| Cd        |             0.0249 |               0.005 |
| Ca        |             0.0205 |              0.0041 |
| Ti        |             0.0088 |              0.0018 |
| V         |             0.0078 |              0.0016 |
| Cr        |             0.0057 |              0.0011 |
| Mn        |             0.0043 |              0.0009 |
| Fe        |             0.0032 |              0.0006 |
| Cu        |             0.0015 |              0.0003 |

** test

#+begin_src matlab
%% Scan parameters
start_ene = 1e3*2.21; % [ev]
end_ene   = 1e3*2.23; % [ev]
step_ene  = 0.2;

dwell_time_min = 0.01; % corresponds to max velocity [s]
dwell_time_max = 0.1;  % corresponds to min velocity [s]
#+end_src

#+begin_src matlab
dspacing = 3.13501196169967; % [Angstrom]
#+end_src

#+begin_src matlab
%% Scan objects
traj_ene   = start_ene:step_ene:end_ene; % [eV]
traj_bragg = asin(12398.4./traj_ene/2/dspacing); % [rad]
traj_fjs   = 0.030427 - (10.5e-3)./(2*cos(traj_bragg)); % [m]

time_slow = dwell_time_max*0:1:length(traj_ene)-1; % [s]
time_fast = dwell_time_min*0:1:length(traj_ene)-1; % [s]

vel_fast_bragg = abs([traj_bragg(2:end) - traj_bragg(1:end-1), 0]/dwell_time_min); % [rad/s]
vel_fast_fjs = abs([traj_fjs(2:end) - traj_fjs(1:end-1), 0]/dwell_time_min); % [m/s]
#+end_src

#+begin_src matlab
figure; plot(time_fast, 1e3*vel_fast_fjs)
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([1e3*cellfun(@(x) x.traj_fjs(1), scans); 1e3*cellfun(@(x) x.traj_fjs(end), scans); 1e3*cellfun(@(x) x.traj_fjs(end)-x.traj_fjs(1), scans); 1e3*cellfun(@(x) max(x.vel_fast_fjs), scans);]', scan_name, {'Scan Name', 'FJ min [mm]', 'FJ max [mm]', 'FJ stroke [mm]', 'Max FJ vel [mm/s]'}, ' %.3f ');
#+end_src

#+name: tab:typical_experiments_fj
#+caption: Fast Jack Stroke and Velocity for typical experiments
#+attr_latex: :environment tabularx :width \linewidth :align lXXXX
#+attr_latex: :center t :booktabs t
#+RESULTS:
| Scan Name | FJ min [mm] | FJ max [mm] | FJ stroke [mm] | Max FJ vel [mm/s] |
|-----------+-------------+-------------+----------------+-------------------|
| P         |      16.696 |      18.212 |          1.516 |             0.936 |
| S         |      21.535 |      22.112 |          0.577 |             0.169 |
| Cl        |      23.239 |      23.437 |          0.198 |             0.065 |
| Cd        |      24.081 |      24.181 |            0.1 |             0.025 |
| Ca        |      24.362 |      24.456 |          0.093 |             0.021 |
| Ti        |      24.698 |      24.731 |          0.033 |             0.009 |
| V         |      24.793 |      24.811 |          0.018 |             0.008 |
| Cr        |      24.864 |      24.882 |          0.017 |             0.006 |
| Mn        |      24.918 |      24.936 |          0.019 |             0.004 |
| Fe        |      24.961 |       24.97 |          0.009 |             0.003 |
| Cu        |      25.045 |      25.049 |          0.004 |             0.002 |

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([180/pi*cellfun(@(x) x.traj_bragg(end), scans);180/pi*cellfun(@(x) x.traj_bragg(1), scans);  180/pi*cellfun(@(x) x.traj_bragg(1)-x.traj_bragg(end), scans); 80/pi*cellfun(@(x) max(x.vel_fast_bragg), scans)]', scan_name, {'Scan Name', 'Bragg min [deg]', 'Bragg max [deg]', 'Bragg stroke [deg]', 'Max bragg vel [deg/s]'}, ' %.3f ');
#+end_src

#+name: tab:typical_experiments_bragg
#+caption: Bragg Stroke and Velocity for typical experiments
#+attr_latex: :environment tabularx :width \linewidth :align lXXXX
#+attr_latex: :center t :booktabs t
#+RESULTS:
| Scan Name | Bragg min [deg] | Bragg max [deg] | Bragg stroke [deg] | Max bragg vel [deg/s] |
|-----------+-----------------+-----------------+--------------------+-----------------------|
| P         |          64.545 |          67.521 |              2.976 |                 0.719 |
| S         |          50.846 |          53.814 |              2.968 |                 0.355 |
| Cl        |           41.32 |          43.082 |              1.762 |                 0.247 |
| Cd        |          32.804 |          34.178 |              1.374 |                 0.147 |
| Ca        |          28.456 |           30.04 |              1.584 |                 0.149 |
| Ti        |          22.813 |          23.596 |              0.783 |                  0.09 |
| V         |          20.794 |          21.274 |               0.48 |                 0.091 |
| Cr        |          18.787 |          19.309 |              0.522 |                 0.075 |
| Mn        |          17.035 |          17.655 |               0.62 |                 0.062 |
| Fe        |          15.828 |          16.171 |              0.343 |                 0.052 |
| Cu        |          12.522 |          12.721 |              0.198 |                 0.032 |

Based on Table [[tab:typical_experiments_bragg]].
- To work without =mcoil=, the maximum bragg stroke should be 16 degrees.
  Therefore, all the scans can be performed without =mcoil=.

Frequency of $5\mu m$ errors:
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([cellfun(@(x) max(x.vel_fast_fjs)./5e-6, scans)]', scan_name, {'Scan Name', 'Max freq. [Hz]'}, ' %.1f ');
#+end_src

#+RESULTS:
| Scan Name | Max freq. [Hz] |
|-----------+----------------|
| P         |          187.2 |
| S         |           33.9 |
| Cl        |           13.0 |
| Cd        |            5.0 |
| Ca        |            4.1 |
| Ti        |            1.8 |
| V         |            1.6 |
| Cr        |            1.1 |
| Mn        |            0.9 |
| Fe        |            0.6 |
| Cu        |            0.3 |

Estimation of maximum velocity such that the $5\mu m$ errors are reduced by a factor 50 (i.e. the frequency of this $5\mu m$ should be below 2Hz, see sensitivity function).

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([cellfun(@(x) dwell_time_min./(2/(max(x.vel_fast_fjs)./5e-6)), scans); cellfun(@(x) dwell_time_min./(10/(max(x.vel_fast_fjs)./5e-6)), scans)]', scan_name, {'Scan Name', 'Min time [s] - 2Hz', 'Min time [s] - 10Hz'}, ' %.4f ');
#+end_src

#+RESULTS:
| Scan Name | Min time [s] - 2Hz | Min time [s] - 10Hz |
|-----------+--------------------+---------------------|
| P         |             0.9358 |              0.1872 |
| S         |             0.1695 |              0.0339 |
| Cl        |             0.0651 |               0.013 |
| Cd        |             0.0249 |               0.005 |
| Ca        |             0.0205 |              0.0041 |
| Ti        |             0.0088 |              0.0018 |
| V         |             0.0078 |              0.0016 |
| Cr        |             0.0057 |              0.0011 |
| Mn        |             0.0043 |              0.0009 |
| Fe        |             0.0032 |              0.0006 |
| Cu        |             0.0015 |              0.0003 |

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

** Merge LUT
#+begin_src matlab
lut_10_15 = importdata("LUT_220202_10_to_15_1.dat");
lut_15_45 = importdata("LUT_220202_15_to_45_1.dat");
lut_45_75 = importdata("LUT_220202_45_to_75_1.dat");
#+end_src

#+begin_src matlab :exports none
%% Plot LUT Data
figure;
hold on;
plot(lut_45_75(:,1), 1e3*(lut_45_75(:,2)-lut_45_75(:,1)), 'b.');
plot(lut_45_75(:,1), 1e3*(lut_45_75(:,3)-lut_45_75(:,1)), 'b.');
plot(lut_45_75(:,1), 1e3*(lut_45_75(:,4)-lut_45_75(:,1)), 'b.');
plot(lut_15_45(:,1), 1e3*(lut_15_45(:,3)-lut_15_45(:,1)), 'r.');
plot(lut_15_45(:,1), 1e3*(lut_15_45(:,2)-lut_15_45(:,1)), 'r.');
plot(lut_15_45(:,1), 1e3*(lut_15_45(:,4)-lut_15_45(:,1)), 'r.');
hold off;
xlabel('IcePAP Step [mm]'); ylabel('Step Offset [$\mu$m]')
#+end_src

#+begin_src matlab :exports none
%% Plot LUT Data
figure;
hold on;
plot(lut_45_75(:,1), 1e3*(lut_45_75(:,2)-lut_45_75(:,1)), '.', ...
    'DisplayName', '$u_r$');
plot(lut_15_45(:,1), 1e3*(lut_15_45(:,2)-lut_15_45(:,1)), '.', ...
    'DisplayName', '$u_r$');
plot(lut_10_15(:,1), 1e3*(lut_10_15(:,2)-lut_10_15(:,1)), '.', ...
    'DisplayName', '$u_r$');
hold off;
xlabel('IcePAP Step [mm]'); ylabel('Step Offset [$\mu$m]')
#+end_src

#+begin_src matlab
% [ur, uh, d]
a = [25.07, 25.09, 24.92];
b = [23.06, 23.1 , 22.93];
#+end_src

#+begin_src matlab
cond_45_75 = lut_45_75(:,1) < b(1);
cond_15_45 = lut_15_45(:,1) > b(1) & lut_15_45(:,1) < a(1);
cond_10_15 = lut_10_15(:,1) > a(1);

cond_45_75_ur = lut_45_75(:,1) < b(1);
cond_15_45_ur = lut_15_45(:,1) > b(1) & lut_15_45(:,1) < a(1);
cond_10_15_ur = lut_10_15(:,1) > a(1);

cond_45_75_uh = lut_45_75(:,1) < b(2);
cond_15_45_uh = lut_15_45(:,1) > b(2) & lut_15_45(:,1) < a(2);
cond_10_15_uh = lut_10_15(:,1) > a(2);

cond_45_75_d  = lut_45_75(:,1) < b(3);
cond_15_45_d  = lut_15_45(:,1) > b(3) & lut_15_45(:,1) < a(3);
cond_10_15_d  = lut_10_15(:,1) > a(3);
#+end_src

#+begin_src matlab
lut_ind = [lut_45_75(cond_45_75, 1);
           lut_15_45(cond_15_45, 1);
           lut_10_15(cond_10_15, 1)];

lut_ur = [lut_45_75(cond_45_75_ur, 2);
          lut_15_45(cond_15_45_ur, 2);
          lut_10_15(cond_10_15_ur, 2)];

lut_uh = [lut_45_75(cond_45_75_uh, 3);
          lut_15_45(cond_15_45_uh, 3);
          lut_10_15(cond_10_15_uh, 3)];

lut_d  = [lut_45_75(cond_45_75_d, 4);
          lut_15_45(cond_15_45_d, 4);
          lut_10_15(cond_10_15_d, 4)];
#+end_src

#+begin_src matlab
lut = [lut_ind, lut_ur, lut_uh, lut_d];
#+end_src

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

fileID = fopen("LUT_220202_10_to_75_1_merged.mat", 'w');
fprintf(fileID, formatSpec, lut');
fclose(fileID);
#+end_src

#+begin_src matlab :exports none
%% Plot LUT Data
figure;
hold on;
plot(lut(:,1), 1e3*(lut(:,2)-lut(:,1)), '.');
plot(lut(:,1), 1e3*(lut(:,3)-lut(:,1)), '.');
plot(lut(:,1), 1e3*(lut(:,4)-lut(:,1)), '.');
hold off;
xlabel('IcePAP Step [mm]'); ylabel('Step Offset [$\mu$m]')
#+end_src

#+begin_src matlab :exports none
%% Plot LUT Data
figure;
hold on;
plot(lut(:,1), lut(:,2), '.');
plot(lut(:,1), lut(:,3), '.');
plot(lut(:,1), lut(:,4), '.');
hold off;
xlabel('IcePAP Step [mm]'); ylabel('Step Offset [$\mu$m]')
#+end_src

#+begin_src matlab :exports none
%% Plot LUT Data
figure;
hold on;
plot(lut_10_15(:,1), 1e3*(lut_10_15(:,2)-lut_10_15(:,1)), 'b.');
plot(lut_10_15(:,1), 1e3*(lut_10_15(:,3)-lut_10_15(:,1)), 'b.');
plot(lut_10_15(:,1), 1e3*(lut_10_15(:,4)-lut_10_15(:,1)), 'b.');
plot(lut_15_45(:,1), 1e3*(lut_15_45(:,3)-lut_15_45(:,1)), 'r.');
plot(lut_15_45(:,1), 1e3*(lut_15_45(:,2)-lut_15_45(:,1)), 'r.');
plot(lut_15_45(:,1), 1e3*(lut_15_45(:,4)-lut_15_45(:,1)), 'r.');
hold off;
xlabel('IcePAP Step [mm]'); ylabel('Step Offset [$\mu$m]')
#+end_src

#+begin_src matlab :exports none
%% Plot LUT Data
figure;
hold on;
plot(lut_45_75(:,1), 1e3*(lut_45_75(:,2)-lut_45_75(:,1)), '.', ...
    'DisplayName', '$u_r$');
plot(lut_15_45(:,1), 1e3*(lut_15_45(:,2)-lut_15_45(:,1)), '.', ...
    'DisplayName', '$u_r$');
hold off;
xlabel('IcePAP Step [mm]'); ylabel('Step Offset [$\mu$m]')
#+end_src

#+begin_src matlab :exports none
%% Plot LUT Data
figure;
hold on;
plot(lut_45_75(:,1), 1e3*(lut_45_75(:,3)-lut_45_75(:,1)), '.', ...
    'DisplayName', '$u_r$');
plot(lut_15_45(:,1), 1e3*(lut_15_45(:,3)-lut_15_45(:,1)), '.', ...
    'DisplayName', '$u_r$');
hold off;
xlabel('IcePAP Step [mm]'); ylabel('Step Offset [$\mu$m]')
#+end_src

#+begin_src matlab :exports none
%% Plot LUT Data
figure;
hold on;
plot(lut_45_75(:,1), 1e3*(lut_45_75(:,4)-lut_45_75(:,1)), '.', ...
    'DisplayName', '$u_r$');
plot(lut_15_45(:,1), 1e3*(lut_15_45(:,4)-lut_15_45(:,1)), '.', ...
    'DisplayName', '$u_r$');
hold off;
xlabel('IcePAP Step [mm]'); ylabel('Step Offset [$\mu$m]')
#+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/lut/'); % Path for luts
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('./lut/'); % Path for luts
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);

%% Data directory
data_directory = "/home/thomas/mnt/data_id21";
#+END_SRC

** Python Packages
#+NAME: p-init
#+BEGIN_SRC python
# Import packages
import numpy as np
from scipy.signal import kaiserord, lfilter, firwin, freqz, firls
import matplotlib.pyplot as plt
#+END_SRC

#+NAME: p-config
#+BEGIN_SRC python
# Configuration
plt.style.use('seaborn-whitegrid')
#+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 [lut] = createLUT(data, 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:
#+end_src

*** Optional Parameters
#+begin_src matlab
arguments
    data
    lut_file
    args.plot    (1,1) logical {mustBeNumericOrLogical} = false
    args.update  (1,1) logical {mustBeNumericOrLogical} = false
    args.lut_inc (1,1) double {mustBeNumeric} = 1e-6 % size of one LUT step [m]
end
#+end_src

*** Load Data
#+begin_src matlab
%% Load Data
time  = data.time;
bragg = data.bragg;
dz    = data.dz;
dry   = data.dry;
drx   = data.drx;
fjur  = data.fjur;
fjuh  = data.fjuh;
fjd   = data.fjd;
#+end_src

#+begin_src matlab
%% Convert Data to Standard Units
% Z error between second crystal and first crystal [m]
ddz   = 10.5e-3./(2*cos(bragg)) - dz;
#+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 = 5000; % Filter's order
delay = fir_order/2; % Delay induced by the filter
B_fir = firls(fir_order, ... % Filter's order
              [0 25/(Fs/2)  34/(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 - First column is pos in [m]
lut_start = args.lut_inc*(round(max(min([fjur, fjuh, fjd]))/args.lut_inc)+1);
lut_end = args.lut_inc*(round(min(max([fjur, fjuh, fjd]))/args.lut_inc));

lut = [lut_start:args.lut_inc:lut_end]'*ones(1,4);
#+end_src

#+begin_src matlab
%% Build the LUT
for i = 1:length(lut)
    [~, i_data] = min(abs(fjur + fjur_e_filt - lut(i,1)));
    lut(i,2) = fjur(i_data);

    [~, i_data] = min(abs(fjuh + fjuh_e_filt - lut(i,1)));
    lut(i,3) = fjuh(i_data);

    [~, i_data] = min(abs(fjd + fjd_e_filt - lut(i,1)));
    lut(i,4) = fjd(i_data);
end

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

% for i = 1:length(lut)
%     % Poputate the LUT with the mean of the IcePAP steps
%     indices = fjd + fjd_e_filt > lut(i,1) - args.lut_inc/2 & ...
%               fjd + fjd_e_filt < lut(i,1) + args.lut_inc/2;
%     % 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

#+begin_src matlab
%% Add Time
data_struct.time = 1e-4*[1:1:length(data_struct.(names{1}))];
#+end_src

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

*** Function Description
#+begin_src matlab
function [data] = processMeasData(data, args)
% processMeasData -
%
% Syntax: processMeasData(data_file, lut_file, args)
%
% Inputs:
%    - data
#+end_src

*** Optional Parameters
#+begin_src matlab
arguments
    data
    args.fir_order (1,1) double {mustBeNumericOrLogical} = 5000
end
#+end_src

*** Process Data
#+begin_src matlab
%% Actuator Jacobian
J_a_111 = [1,  0.14, -0.0675
           1,  0.14,  0.1525
           1, -0.14,  0.0425];
#+end_src

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

#+begin_src matlab
data.ddz  = 10.5e-3./(2*cos(data.bragg)) - data.dz;

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

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

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

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

#+begin_src matlab
%% Re-sample data to have same data points in FJUR
[data.fjur_e_resampl, data.fjur_resampl] = resample(data.fjur_e_filt, data.fjur, 1/100e-9);
[data.fjuh_e_resampl, data.fjuh_resampl] = resample(data.fjuh_e_filt, data.fjuh, 1/100e-9);
[data.fjd_e_resampl,  data.fjd_resampl]  = resample(data.fjd_e_filt,  data.fjd,  1/100e-9);
#+end_src

* Bibliography                                                        :ignore:
#+latex: \printbibliography