#+TITLE: Cercalo Test Bench
:DRAWER:
#+STARTUP: overview

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

#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/zenburn.css"/>
#+HTML_HEAD: <script type="text/javascript" src="./js/jquery.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="./js/bootstrap.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="./js/jquery.stickytableheaders.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="./js/readtheorg.js"></script>

#+PROPERTY: header-args:latex  :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{/home/thomas/Cloud/These/LaTeX/}{config.tex}")
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
#+PROPERTY: header-args:latex+ :eval no-export
#+PROPERTY: header-args:latex+ :exports both
#+PROPERTY: header-args:latex+ :mkdirp yes
#+PROPERTY: header-args:latex+ :noweb yes
#+PROPERTY: header-args:latex+ :output-dir figs
#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png")

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

* Introduction
** Block Diagram
The block diagram of the setup to be controlled is shown in Fig. [[fig:block_diagram_simplify]].

#+begin_src latex :file cercalo_diagram_simplify.pdf :exports results
  \begin{tikzpicture}
    \node[DAC] (dac) at (0, 0) {};
    \node[block, right=1.2 of dac] (Gi) {$G_i$};
    \node[block, right=1.5 of Gi] (Gc) {$G_c$};
    \node[addb, right=0.5 of Gc] (add) {};
    \node[block, above= of add] (Gn) {$G_n$};
    \node[ADC, right=1.2 of add] (adc) {};
    \coordinate (GiGc) at ($0.8*(Gi.east) + 0.2*(Gc.west)$);
    \node[block] (Zc) at (GiGc|-Gn) {$Z_c$};
    \node[block, above=1.2 of Zc] (Ga) {$G_a$};
    \node[block, right= of adc] (Gd) {${G_{d}}^{-1}$};

    \node[above, align=center] at (Gi.north) {Current\\Amplifier};
    \node[left, align=right] at (Zc.west) {Cercalo's\\Impedance};
    \node[left, align=right] at (Ga.west) {Voltage\\Amplifier};
    \node[above, align=center] at (Gc.north) {Cercalo};
    \node[left, align=right] at (Gn.west) {Newport};

    \draw[->] ($(dac.west) + (-1, 0)$) --node[midway, sloped]{$/$} (dac.west);
    \draw[->] (dac.east) -- (Gi.west) node[above left]{$\begin{bmatrix}U_{c,h} \\ U_{c,v}\end{bmatrix}$} node[below left]{$[V]$};
    \draw[->] (Gi.east) -- (Gc.west) node[above left]{$\begin{bmatrix}I_{c,h} \\ I_{c,v}\end{bmatrix}$} node[below left]{$[A]$};
    \draw[->] (Gc.east) -- (add.west);
    \draw[->] (Gn.south) -- (add.north);
    \draw[->] (GiGc)node[branch]{} -- (Zc.south);
    \draw[->] (Zc.north) -- (Ga.south) node[below right]{$\begin{bmatrix}\tilde{V}_{c,h} \\ \tilde{V}_{c,v}\end{bmatrix}$} node[below left]{$[V]$};
    \draw[->] (Ga.north) -- ++(0, 1.2) node[below right]{$\begin{bmatrix}V_{c,h} \\ V_{c,v}\end{bmatrix}$} node[below left]{$[V]$};
    \draw[->] ($(Gn.north) + (0, 1.2)$) -- (Gn.north) node[above right]{$\begin{bmatrix}U_{n,h} \\ U_{n,v}\end{bmatrix}$} node[above left]{$[V]$};
    \draw[->] (add.east) -- (adc.west) node[above left]{$\begin{bmatrix}V_{p,h} \\ V_{p,v}\end{bmatrix}$} node[below left]{$[V]$};
    \draw[->] (adc.east) --node[midway, sloped]{$/$} (Gd.west);
    \draw[->] (Gd.east) --node[midway, sloped]{$/$} ++(1, 0) node[above left]{$\begin{bmatrix} \theta_h \\ \theta_v \end{bmatrix}$} node[below left]{$[rad]$};
  \end{tikzpicture}
#+end_src

#+name: fig:block_diagram_simplify
#+caption: Block Diagram of the Experimental Setup
#+RESULTS:
[[file:figs/cercalo_diagram_simplify.png]]

The transfer functions in the system are:
- *Current Amplifier*: from the voltage set by the DAC to the current going to the Cercalo's inductors
  \[ G_i = \begin{bmatrix} G_{i,h} & 0 \\ 0 & G_{i,v} \end{bmatrix} \text{ in } \left[ \frac{A}{V} \right] \]
  \[ \begin{bmatrix} I_{c,h} \\ I_{c,v} \end{bmatrix} = G_i \begin{bmatrix} U_{c,h} \\ U_{c,v} \end{bmatrix} \]
- *Impedance of the Cercalo* that converts the current going to the cercalo to the voltage across the cercalo:
  \[ Z_c = \begin{bmatrix} Z_{c,h} & 0 \\ 0 & Z_{c,v} \end{bmatrix} \text{ in } \left[ \frac{V}{A} \right] \]
  \[ \begin{bmatrix} \tilde{V}_{c,h} \\ \tilde{V}_{c,v} \end{bmatrix} = Z_c \begin{bmatrix} I_{c,h} \\ I_{c,v} \end{bmatrix} \]
- *Voltage Amplifier*: from the voltage across the Cercalo inductors to the measured voltage
  \[ G_a = \begin{bmatrix} G_{a,h} & 0 \\ 0 & G_{a,v} \end{bmatrix} \text{ in } \left[ \frac{V}{V} \right] \]
  \[ \begin{bmatrix} V_{c,h} \\ V_{c,v} \end{bmatrix} = G_a \begin{bmatrix} \tilde{V}_{c,h} \\ \tilde{V}_{c,v} \end{bmatrix} \]
- *Cercalo*: Transfer function from the current going through the cercalo inductors to the 4 quadrant measurement
  \[ G_c = \begin{bmatrix} G_{\frac{V_{p,h}}{\tilde{U}_{c,h}}} & G_{\frac{V_{p,h}}{\tilde{U}_{c,v}}} \\ G_{\frac{V_{p,v}}{\tilde{U}_{c,h}}} & G_{\frac{V_{p,v}}{\tilde{U}_{c,v}}} \end{bmatrix} \text{ in } \left[ \frac{V}{A} \right] \]
  \[ \begin{bmatrix} V_{p,h} \\ V_{p,v} \end{bmatrix} = G_c \begin{bmatrix} I_{c,h} \\ I_{c,v} \end{bmatrix} \]
- *Newport* Transfer function from the command signal of the Newport to the 4 quadrant measurement
  \[ G_n = \begin{bmatrix} G_{\frac{V_{p,h}}{U_{n,h}}} & G_{\frac{V_{p,h}}{U_{n,v}}} \\ G_{\frac{V_{p,v}}{U_{n,h}}} & G_{\frac{V_{n,v}}{U_{n,v}}} \end{bmatrix} \text{ in } \left[ \frac{V}{V} \right] \]
  \[ \begin{bmatrix} V_{p,h} \\ V_{p,v} \end{bmatrix} = G_c \begin{bmatrix} V_{n,h} \\ V_{n,v} \end{bmatrix} \]
- *4 Quadrant Diode*: the gain of the 4 quadrant diode in [V/rad] is inverse in order to obtain the physical angle of the beam
  \[ G_d = \begin{bmatrix} G_{d,h} & 0 \\ 0 & G_{d,v} \end{bmatrix} \text{ in } \left[\frac{V}{rad}\right] \]

The block diagram with each transfer function is shown in Fig. [[fig:block_diagram]].
#+begin_src latex :file cercalo_diagram.pdf :exports results
  \begin{tikzpicture}
    \node[DAC] (dac) at (0, 0) {};
    \node[block, right=1.5 of dac] (Gi) {$\begin{bmatrix} G_{i,h} & 0 \\ 0 & G_{i,v} \end{bmatrix}$};
    \node[block, right=1.8 of Gi] (Gc) {$\begin{bmatrix} G_{\frac{V_{p,h}}{\tilde{U}_{c,h}}} & G_{\frac{V_{p,h}}{\tilde{U}_{c,v}}} \\ G_{\frac{V_{p,v}}{\tilde{U}_{c,h}}} & G_{\frac{V_{p,v}}{\tilde{U}_{c,v}}} \end{bmatrix}$};
    \node[addb, right= of Gc] (add) {};
    \node[block, above= of add] (Gn) {$\begin{bmatrix} G_{\frac{V_{p,h}}{U_{n,h}}} & G_{\frac{V_{p,h}}{U_{n,v}}} \\ G_{\frac{V_{p,v}}{U_{n,h}}} & G_{\frac{V_{n,v}}{U_{n,v}}} \end{bmatrix}$};
    \node[ADC, right = 1.5 of add] (adc) {};
    \coordinate (GiGc) at ($0.7*(Gi.east) + 0.3*(Gc.west)$);
    \node[block] (Zc) at (GiGc|-Gn) {$\begin{bmatrix} Z_{c,h} & 0 \\ 0 & Z_{c,v} \end{bmatrix}$};
    \node[block, above=1.2 of Zc] (Ga) {$\begin{bmatrix} G_{a,h} & 0 \\ 0 & G_{a,v} \end{bmatrix}$};
    \node[block, right= of adc] (Gd) {$\begin{bmatrix} G_{d,h}^{-1} & 0 \\ 0 & G_{d,v}^{-1} \end{bmatrix}$};

    \node[above, align=center] at (Gi.north) {Current\\Amplifier};
    \node[above, align=center] at (Gc.north) {Cercalo};
    \node[left, align=right] at (Gn.west) {Newport};

    \draw[->] ($(dac.west) + (-1, 0)$) --node[midway, sloped]{$/$} (dac.west);
    \draw[->] (dac.east) -- (Gi.west) node[above left]{$\begin{bmatrix}U_{c,h} \\ U_{c,v}\end{bmatrix}$};
    \draw[->] (Gi.east) -- (Gc.west) node[above left]{$\begin{bmatrix}I_{c,h} \\ I_{c,v}\end{bmatrix}$};
    \draw[->] (Gc.east) -- (add.west);
    \draw[->] (Gn.south) -- (add.north);
    \draw[->] (GiGc)node[branch]{} -- (Zc.south);
    \draw[->] (Zc.north) -- (Ga.south) node[below right]{$\begin{bmatrix}\tilde{V}_{c,h} \\ \tilde{V}_{c,v}\end{bmatrix}$};
    \draw[->] (Ga.north) -- ++(0, 1.5) node[below right]{$\begin{bmatrix}V_{c,h} \\ V_{c,v}\end{bmatrix}$};
    \draw[->] ($(Gn.north) + (0, 1.5)$) -- (Gn.north) node[above right]{$\begin{bmatrix}U_{n,h} \\ U_{n,v}\end{bmatrix}$};
    \draw[->] (add.east) -- (adc.west) node[above left]{$\begin{bmatrix}V_{p,h} \\ V_{p,v}\end{bmatrix}$};
    \draw[->] (adc.east) --node[midway, sloped]{$/$} (Gd.west);
    \draw[->] (Gd.east) --node[midway, sloped]{$/$} ++(1, 0) node[above left]{$\begin{bmatrix} \theta_h \\ \theta_v \end{bmatrix}$} node[below left]{$[rad]$};
  \end{tikzpicture}
#+end_src

#+name: fig:block_diagram
#+caption: Block Diagram of the Experimental Setup with detailed dynamics
#+RESULTS:
[[file:figs/cercalo_diagram.png]]

** Cercalo
From the Cercalo documentation, we have the parameters shown on table [[tab:cercalo_parameters]].

#+name: tab:cercalo_parameters
#+caption: Cercalo Parameters
|                  | Maximum Stroke [deg] | Resonance Frequency [Hz] | DC Gain [mA/deg] | Gain at resonance [deg/V] | RC Resistance [Ohm] |
|------------------+----------------------+--------------------------+------------------+---------------------------+---------------------|
| AX1 (Horizontal) |                    5 |                   411.13 |             28.4 |                     382.9 |                9.41 |
| AX2 (Vertical)   |                    5 |                    252.5 |             35.2 |                     350.4 |                     |

The Inductance and DC resistance of the two axis of the Cercalo have been measured:
- $L_{c,h} = 0.1\ \text{mH}$
- $L_{c,v} = 0.1\ \text{mH}$
- $R_{c,h} = 9.3\ \Omega$
- $R_{c,v} = 8.3\ \Omega$

Let's first consider the *horizontal direction* and we try to model the Cercalo by a spring/mass/damper system (Fig. [[fig:mech_cercalo]]).

#+begin_src latex :file mech_cercalo.pdf :exports results
  \begin{tikzpicture}
    \def\massw{2.2}   % Width of the masses
    \def\massh{0.8}   % Height of the masses
    \def\spaceh{1.4}  % Height of the springs/dampers
    \def\dispw{0.3}   % Width of the dashed line for the displacement
    \def\disph{0.5}   % Height of the arrow for the displacements
    \def\bracs{0.05}  % Brace spacing vertically
    \def\brach{-10pt} % Brace shift horizontaly

    \draw (-0.5*\massw, 0) -- (0.5*\massw, 0);

    % Mass
    \draw[fill=white] (-0.5*\massw, \spaceh) rectangle (0.5*\massw, \spaceh+\massh) node[pos=0.5]{$m$};

    % Spring, Damper, and Actuator
    \draw[spring] (-0.4*\massw, 0) -- (-0.4*\massw, \spaceh) node[midway, left=0.1]{$k$};
    \draw[damper] (0, 0)           -- ( 0, \spaceh)          node[midway, left=0.2]{$c$};
    \draw[actuator] ( 0.4*\massw, 0) -- (	0.4*\massw, \spaceh) node[midway, left=0.1](F){$F$};

    % Displacements
    \draw[dashed] (0.5*\massw, \spaceh) -- ++(\dispw, 0);
    \draw[->] (0.5*\massw+0.5*\dispw, \spaceh) -- ++(0, \disph) node[right]{$x$};
  \end{tikzpicture}
#+end_src

#+name: fig:mech_cercalo
#+caption: 1 degree-of-freedom model of the Cercalo
#+RESULTS:
[[file:figs/mech_cercalo.png]]


The equation of motion is:
\begin{align*}
  \frac{x}{F} &= \frac{1}{k + c s + m s^2} \\
              &= \frac{G_0}{1 + 2 \xi \frac{s}{\omega_0} + \frac{s^2}{\omega_0^2}}
\end{align*}
with:
- $G_0 = 1/k$ is the gain at DC in rad/N
- $\xi = \frac{c}{2 \sqrt{km}}$ is the damping ratio of the system
- $\omega_0 = \sqrt{\frac{k}{m}}$ is the resonance frequency in rad

The force $F$ applied to the mass is proportional to the current $I$ flowing through the voice coils:
\[ \frac{F}{I} = \alpha \]
with $\alpha$ is in $N/A$ and is to be determined.

The current $I$ is also proportional to the voltage at the output of the buffer:
\begin{align*}
  \frac{I_c}{U_c} &= \frac{1}{(R + R_c) + L_c s} \\
                  &\approx 0.02 \left[ \frac{A}{V} \right]
\end{align*}

Let's try to determine the equivalent mass and spring values.
From table [[tab:cercalo_parameters]], for the horizontal direction:
\[ \left| \frac{x}{I} \right|(0) = \left| \alpha \frac{x}{F} \right|(0) = 28.4\ \frac{mA}{deg} = 1.63\ \frac{A}{rad} \]

So:
\[ \alpha \frac{1}{k} = 1.63 \Longleftrightarrow k = \frac{\alpha}{1.63} \left[\frac{N}{rad}\right] \]

We also know the resonance frequency:
\[ \omega_0 = 411.1\ \text{Hz} = 2583\ \frac{rad}{s} \]

And the gain at resonance:
\begin{align*}
  \left| \frac{x}{U_c} \right|(j\omega_0) &= \left| 0.02 \frac{x}{I_c} \right| (j\omega_0) \\
                                          &= \left| 0.02 \alpha \frac{x}{F} \right| (j\omega_0) \\
                                          &= 0.02 \alpha \frac{1/k}{2\xi} \\
                                          &= 282.9\ \left[\frac{deg}{V}\right] \\
                                          &= 4.938\ \left[\frac{rad}{V}\right]
\end{align*}

Thus:
\begin{align*}
  & \frac{\alpha}{2 \xi k} = 245 \\
  \Leftrightarrow & \frac{1.63}{2 \xi} = 245 \\
  \Leftrightarrow & \xi = 0.0033 \\
  \Leftrightarrow & \xi = 0.33 \%
\end{align*}

#+begin_important
\begin{align*}
  G_0 &= \frac{1.63}{\alpha}\ \frac{rad}{N} \\
  \xi &= 0.0033 \\
  \omega_0 &= 2583\ \frac{rad}{s}
\end{align*}
and in terms of the physical properties:
\begin{align*}
  k   &= \frac{\alpha}{1.63}\ \frac{N}{rad} \\
  \xi &= 0.0033 \\
  m   &= \frac{\alpha}{1.1 \cdot 10^7}\ \frac{kg}{m^2}
\end{align*}

Thus, we have to determine $\alpha$.
This can be done experimentally by determining the gain at DC or at resonance of the system.
For that, we need to know the angle of the mirror, thus we need to *calibrate* the photo-diodes.
This will be done using the Newport.
#+end_important

** Optical Setup
** Newport
Parameters of the Newport are shown in Fig. [[fig:newport_doc]].

It's dynamics for small angle excitation is shown in Fig. [[fig:newport_gain]].

And we have:
\begin{align*}
  G_{n, h}(0) &= 2.62 \cdot 10^{-3}\ \frac{rad}{V} \\
  G_{n, v}(0) &= 2.62 \cdot 10^{-3}\ \frac{rad}{V}
\end{align*}

#+name: fig:newport_doc
#+caption: Documentation of the Newport
[[file:figs/newport_doc.png]]

#+name: fig:newport_gain
#+caption: Transfer function of the Newport
[[file:figs/newport_gain.png]]

** 4 quadrant Diode
The front view of the 4 quadrant photo-diode is shown in Fig. [[fig:4qd_naming]].

#+begin_src latex :file 4qd_naming.pdf :exports results
  \begin{tikzpicture}
    \node[draw, circle, minimum size=3cm] (c) at (0, 0){};
    \draw[] (c.north) -- (c.south);
    \draw[] (c.west) -- (c.east);
    \node[] at (-0.6,  0.6){\huge 1};
    \node[] at ( 0.6,  0.6){\huge 2};
    \node[] at (-0.6, -0.6){\huge 3};
    \node[] at ( 0.6, -0.6){\huge 4};
  \end{tikzpicture}
#+end_src

#+name: fig:4qd_naming
#+caption: Front view of the 4QD
#+RESULTS:
[[file:figs/4qd_naming.png]]

Each of the photo-diode is amplified using a 4-channel amplifier as shown in Fig. [[fig:4qd_amplifier]].
#+begin_src latex :file 4qd_amplifier.pdf :exports results
  \begin{tikzpicture}
    \node[draw, minimum width=2cm, minimum height=1.5cm] (ampl) at (0, 0){Amp};
    \node[above right] at (ampl.north west){\huge 2};
    \node[above left] at (ampl.north east){\huge 1};
    \node[below right] at (ampl.south west){\huge 4};
    \node[below left] at (ampl.south east){\huge 3};
  \end{tikzpicture}
#+end_src

#+name: fig:4qd_amplifier
#+caption: Wiring of the amplifier. The amplifier is located on the bottom right of the board
#+RESULTS:
[[file:figs/4qd_amplifier.png]]

** ADC/DAC
Let's compute the theoretical noise of the ADC/DAC.

\begin{align*}
  \Delta V &= 20 V \\
  n &= 16bits \\
  q &= \Delta V/2^n = 305 \mu V \\
  f_N &= 10kHz \\
  \Gamma_n &= \frac{q^2}{12 f_N} = 7.76 \cdot 10^{-13} \frac{V^2}{Hz}
\end{align*}
with $\Delta V$ the total range of the ADC, $n$ its number of bits, $q$ the quantization, $f_N$ the sampling frequency and $\Gamma_n$ its theoretical Power Spectral Density.

* Identification of the system dynamics
:PROPERTIES:
:header-args:matlab+: :tangle matlab/cercalo_identification.m
:header-args:matlab+: :comments org :mkdirp yes
:END:
<<sec:cercalo_identification>>
** Introduction                                                     :ignore:
In this section, we seek to identify all the blocks as shown in Fig. [[fig:block_diagram_simplify]].

| Signal                                              | Name  | Unit |
|-----------------------------------------------------+-------+------|
| Voltage Sent to Cercalo - Horizontal                | =Uch= | [V]  |
| Voltage Sent to Cercalo - Vertical                  | =Ucv= | [V]  |
| Voltage Sent to Newport - Horizontal                | =Unh= | [V]  |
| Voltage Sent to Newport - Vertical                  | =Unv= | [V]  |
|-----------------------------------------------------+-------+------|
| 4Q Photodiode Measurement - Horizontal              | =Vph= | [V]  |
| 4Q Photodiode Measurement - Vertical                | =Vpv= | [V]  |
| Measured Voltage across the Inductance - Horizontal | =Vch= | [V]  |
| Measured Voltage across the Inductance - Vertical   | =Vcv= | [V]  |
| Newport Metrology - Horizontal                      | =Vnh= | [V]  |
| Newport Metrology - Vertical                        | =Vnv= | [V]  |
|-----------------------------------------------------+-------+------|
| Attocube Measurement                                | =Va=  | [m]  |

** ZIP file containing the data and matlab files                    :ignore:
#+begin_src bash :exports none :results none
  if [ matlab/cercalo_identification.m -nt data/cercalo_identification.zip ]; then
    cp matlab/cercalo_identification.m cercalo_identification.m;
    zip data/cercalo_identification \
        mat/data_cal_pd_h.mat \
        mat/data_cal_pd_v.mat \
        mat/data_uch.mat \
        mat/data_ucv.mat \
        mat/data_unh.mat \
        mat/data_unv.mat \
        cercalo_identification.m
    rm cercalo_identification.m;
  fi
#+end_src

#+begin_note
  All the files (data and Matlab scripts) are accessible [[file:data/cercalo_identification.zip][here]].
#+end_note

** 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
  fs = 1e4;
  Ts = 1/fs;
  freqs = logspace(1, 3, 1000);
#+end_src

** Calibration of the 4 Quadrant Diode
*** Introduction                                                   :ignore:
Prior to any dynamic identification, we would like to be able to determine the meaning of the 4 quadrant diode measurement.
For instance, instead of obtaining transfer function in [V/V] from the input of the cercalo to the measurement voltage of the 4QD, we would like to obtain the transfer function in [rad/V].
This will give insight to physical interpretation.

To calibrate the 4 quadrant photo-diode, we can use the metrology included in the Newport.
We can choose precisely the angle of the Newport mirror and see what is the value measured by the 4 Quadrant Diode.
We then should be able to obtain the "gain" of the 4QD in [V/rad].

*** Input / Output data
The identification data is loaded
#+begin_src matlab
  uh = load('mat/data_cal_pd_h.mat', 't', 'Vph', 'Vpv', 'Vnh');
  uv = load('mat/data_cal_pd_v.mat', 't', 'Vph', 'Vpv', 'Vnv');
#+end_src

We remove the first seconds where the Cercalo is turned on.
#+begin_src matlab
  t0 = 1;

  uh.Vph(uh.t<t0) = [];
  uh.Vpv(uh.t<t0) = [];
  uh.Vnh(uh.t<t0) = [];
  uh.t(uh.t<t0)   = [];
  uh.t = uh.t - uh.t(1); % We start at t=0

  t0 = 1;

  uv.Vph(uv.t<t0) = [];
  uv.Vpv(uv.t<t0) = [];
  uv.Vnv(uv.t<t0) = [];
  uv.t(uv.t<t0) = [];
  uv.t = uv.t - uv.t(1); % We start at t=0
#+end_src

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(1, 2, 1);
  plot(uh.t, uh.Vnh, 'DisplayName', '$Vn_h$');
  xlabel('Time [s]');
  ylabel('Amplitude [V]');
  legend();

  ax2 = subplot(1, 2, 2);
  hold on;
  plot(uh.t, uh.Vph, 'DisplayName', '$Vp_h$');
  plot(uh.t, uh.Vpv, 'DisplayName', '$Vp_v$');
  hold off;
  xlabel('Time [s]');
  ylabel('Amplitude [V]');
  legend();

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

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/calib_4qd_h.pdf" :var figsize="full-normal" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:calib_4qd_h
#+CAPTION: Identification signals when exciting the horizontal direction ([[./figs/calib_4qd_h.png][png]], [[./figs/calib_4qd_h.pdf][pdf]])
[[file:figs/calib_4qd_h.png]]


#+begin_src matlab :exports none
  figure;
  ax1 = subplot(1, 2, 1);
  plot(uv.t, uv.Vnv, 'DisplayName', '$Vn_v$');
  xlabel('Time [s]');
  ylabel('Amplitude [V]');

  ax2 = subplot(1, 2, 2);
  hold on;
  plot(uv.t, uv.Vpv, 'DisplayName', '$Vp_v$');
  plot(uv.t, uv.Vph, 'DisplayName', '$Vp_h$');
  hold off;
  xlabel('Time [s]');
  ylabel('Amplitude [V]');
  legend();

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

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/calib_4qd_v.pdf" :var figsize="full-normal" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:calib_4qd_v
#+CAPTION: Identification signals when exciting in the vertical direction ([[./figs/calib_4qd_v.png][png]], [[./figs/calib_4qd_v.pdf][pdf]])
[[file:figs/calib_4qd_v.png]]

*** Linear Regression to obtain the gain of the 4QD
We plot the angle of mirror

Gain of the Newport metrology in [rad/V].
#+begin_src matlab
  gn0 = 2.62e-3;
#+end_src

The angular displacement of the beam is twice the angular displacement of the Newport mirror.

We do a linear regression
\[ y = a x + b \]
where:
- $y$ is the measured voltage of the 4QD in [V]
- $x$ is the beam angle (twice the mirror angle) in [rad]
- $a$ is the identified gain of the 4QD in [rad/V]

The linear regression is shown in Fig. [[fig:4qd_linear_reg]].

#+begin_src matlab
  bh = [ones(size(uh.Vnh)) 2*gn0*uh.Vnh]\uh.Vph;
  bv = [ones(size(uv.Vnv)) 2*gn0*uv.Vnv]\uv.Vpv;
#+end_src

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(1, 2, 1);
  hold on;
  plot(2*gn0*uh.Vnh, uh.Vph, 'o', 'DisplayName', 'Exp. data');
  plot(2*gn0*[min(uh.Vnh) max(uh.Vnh)], 2*gn0*[min(uh.Vnh) max(uh.Vnh)].*bh(2) + bh(1), 'k--', 'DisplayName', sprintf('%.1e x + %.1e', bh(2), bh(1)))
  hold off;
  xlabel('$\alpha_{0,h}$ [rad]'); ylabel('$Vp_h$ [V]');
  legend();

  ax2 = subplot(1, 2, 2);
  hold on;
  plot(2*gn0*uv.Vnv, uv.Vpv, 'o', 'DisplayName', 'Exp. data');
  plot(2*gn0*[min(uv.Vnv) max(uv.Vnv)], 2*gn0*[min(uv.Vnv) max(uv.Vnv)].*bv(2) + bv(1), 'k--', 'DisplayName', sprintf('%.1e x + %.1e', bv(2), bv(1)))
  hold off;
  xlabel('$\alpha_{0,v}$ [rad]'); ylabel('$Vp_v$ [V]');
  legend();
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/4qd_linear_reg.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:4qd_linear_reg
#+CAPTION: Linear Regression ([[./figs/4qd_linear_reg.png][png]], [[./figs/4qd_linear_reg.pdf][pdf]])
[[file:figs/4qd_linear_reg.png]]

Thus, we obtain the "gain of the 4 quadrant photo-diode as shown on table [[tab:gain_4qd]].

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
  data2orgtable([bh(2), bv(2)], {}, {'Horizontal [V/rad]', 'Vertical [V/rad]'}, ' %.1f ');
#+end_src

#+name: tab:gain_4qd
#+caption: Identified Gain of the 4 quadrant diode
#+RESULTS:
| Horizontal [V/rad] | Vertical [V/rad] |
|--------------------+------------------|
|              -31.0 |             36.3 |

#+begin_src matlab
  Gd = tf([bh(2) 0 ;
           0     bv(2)]);
#+end_src

We obtain:
\begin{align*}
  \frac{V_{qd,h}}{\alpha_{0,h}} &\approx 0.032\ \left[ \frac{rad}{V} \right] \\
                                &\approx 32.3\ \left[ \frac{\mu rad}{mV} \right]
\end{align*}

\begin{align*}
  \frac{V_{qd,v}}{\alpha_{0,v}} &\approx 0.028\ \left[ \frac{rad}{V} \right] \\
                                &\approx 27.6\ \left[ \frac{\mu rad}{mV} \right]
\end{align*}

** Identification of the Cercalo Impedance, Current Amplifier and Voltage Amplifier dynamics
*** Introduction                                                   :ignore:
We wish here to determine $G_i$ and $G_a$ shown in Fig. [[fig:block_diagram_simplify]].

We ignore the electro-mechanical coupling.

*** Electrical Schematic
The schematic of the electrical circuit used to drive the Cercalo is shown in Fig. [[fig:current_amplifier]].

#+begin_src latex :file cercalo_amplifier.pdf :exports results
  \begin{circuitikz}[]
    \ctikzset{bipoles/length=1.0cm}
    \draw
    (0, -2) node[ground]{} to[vco, V=$U_c$] (0, 0)
    to [amp, t={1},i^>=$I_c$, l=BUF] ++(2, 0)
    to [R=$R$] ++(2, 0) coordinate(A)
    to [L=$L_c$] ++(0, -1)
    to [R=$R_c$] ++(0, -1) coordinate(B) node[ground]{}
    ;
    \draw (A) to [amp, i>^=$0$, l={60dB}] ++ (2, 0);

    \draw[->] ($(B)+(-0.4, 0)$) -- node[midway, left]{$\tilde{V}_c$} ($(A)+(-0.4, 0)$);
    \draw[->] ($(B)+(2, 0)$) -- node[midway, left]{$V_c$} ($(A)+(2, 0)$);
  \end{circuitikz}
#+end_src

#+name: fig:current_amplifier
#+caption: Current Amplifier Schematic
#+RESULTS:
[[file:figs/cercalo_amplifier.png]]

The elements are:
- $U_c$: the voltage generated by the DAC
- BUF: is a unity-gain open-loop buffer that allows to increase the output current
- $R$: a chosen resistor that will determine the gain of the current amplifier
- $L_c$: inductor present in the Cercalo
- $R_c$: resistance of the inductor
- $\tilde{V}_c$: voltage measured across the Cercalo's inductor
- $V_c$: amplified voltage measured across the Cercalo's inductor
- $I_c$ is the current going through the Cercalo's inductor


The values of the components have been measured for the horizontal and vertical directions:
- $R_h = 41 \Omega$
- $L_{c,h} = 0.1 mH$
- $R_{c,h} = 9.3 \Omega$
- $R_v = 41 \Omega$
- $L_{c,v} = 0.1 mH$
- $R_{c,v} = 8.3 \Omega$

Let's first determine the transfer function from $U_c$ to $I_c$.

We have that:
\[ U_c = (R + R_c) I_c + L_c s I_c \]

Thus:
\begin{align}
  G_i(s) &= \frac{I_c}{U_c} \\
            &= \frac{1}{(R + R_c) + L_c s} \\
            &= \frac{G_{i,0}}{1 + s/\omega_0}
\end{align}
with
- $G_{i,0} = \frac{1}{R + R_c}$
- $\omega_0 = \frac{R + R_c}{L_c}$


Now, determine the transfer function from $I_c$ to $\tilde{V}_c$:
\[ \tilde{V}_C = R_c I_c + L_c s I_c \]
Thus:
\begin{align}
  Z_c(s) &= \frac{\tilde{V}_c}{I_c} \\
         &= R_c + L_c s
\end{align}


Finally, the transfer function of the voltage amplifier $G_a$ is simply a low pass filter:
\begin{align}
  G_a(s) &= \frac{V_c}{\tilde{V}_c} \\
         &= \frac{G_{a,0}}{1 + s/\omega_c}
\end{align}
with
- $G_{a,0}$ is the gain 1000 (60dB)
- $\omega_c$ is the cut-off frequency of the voltage amplifier set to 1000Hz

*** Theoretical Transfer Functions
The values of the components in the current amplifier have been measured.
#+begin_src matlab
  Rh = 41; % [Ohm]
  Lch = 0.1e-3; % [H]
  Rch = 9.3; % [Ohm]

  Rv = 41; % [Ohm]
  Lcv = 0.1e-3; % [H]
  Rcv = 8.3; % [Ohm]
#+end_src

\begin{align*}
  G_i(s) &= \frac{1}{(R + R_c) + L_c s} \\
  Z_c(s) &= R_c + L_c s \\
  G_a(s) &= \frac{1000}{1 + s/\omega_c}
\end{align*}

#+begin_src matlab
  Gi = blkdiag(1/(Rh + Rch + Lch * s), 1/(Rv + Rcv + Lcv * s));
  Zc = blkdiag(Rch+Lch*s, Rcv+Lcv*s);
  Ga = blkdiag(1000/(1 + s/2/pi/1000), 1000/(1 + s/2/pi/1000));
#+end_src

#+begin_src matlab :exports none
  freqs = logspace(1, 4, 1000);

  figure;

  ax1 = subplot(1, 3, 1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gi(1,1), freqs, 'Hz'))), 'DisplayName', '$G_{i, h} = \frac{I_{c,h}}{U_{c,h}}$')
  plot(freqs, abs(squeeze(freqresp(Gi(2,2), freqs, 'Hz'))), 'DisplayName', '$G_{i, v} = \frac{I_{c,v}}{U_{c,v}}$')
  hold off;
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Amplitude [A/V]');
  legend('location', 'northwest');
  ylim([0.01, 1]);

  ax2 = subplot(1, 3, 2);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Zc(1,1), freqs, 'Hz'))), 'DisplayName', '$Z_{c, h} = \frac{\tilde{V}_{c,h}}{I_{c,h}}$')
  plot(freqs, abs(squeeze(freqresp(Zc(2,2), freqs, 'Hz'))), 'DisplayName', '$Z_{c, v} = \frac{\tilde{V}_{c,v}}{I_{c,v}}$')
  hold off;
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Amplitude [V/A]');
  legend('location', 'southwest');
  ylim([1, 100]);

  ax3 = subplot(1, 3, 3);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Ga(1,1), freqs, 'Hz'))), 'DisplayName', '$G_{a, h} = \frac{V_{c,h}}{\tilde{V}_{c,h}}$')
  plot(freqs, abs(squeeze(freqresp(Ga(2,2), freqs, 'Hz'))), 'DisplayName', '$G_{a, v} = \frac{V_{c,v}}{\tilde{V}_{c,v}}$')
  hold off;
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Amplitude [V/V]');
  legend('location', 'southwest');
  ylim([10, 1000]);

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

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/current_amplifier_tf.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:current_amplifier_tf
#+CAPTION: Transfer function for the current amplifier ([[./figs/current_amplifier_tf.png][png]], [[./figs/current_amplifier_tf.pdf][pdf]])
[[file:figs/current_amplifier_tf.png]]

#+begin_important
Over the frequency band of interest, the current amplifier transfer function $G_i$ can be considered as constant.
This is the same for the impedance $Z_c$.
#+end_important

#+begin_src matlab
  Gi = tf(blkdiag(1/(Rh + Rch), 1/(Rv + Rcv)));
  Zc = tf(blkdiag(Rch, Rcv));
#+end_src

*** Identified Transfer Functions
Noise is generated using the DAC ($[U_{c,h}\ U_{c,v}]$) and we measure the output of the voltage amplifier $[V_{c,h}, V_{c,v}]$.
From that, we should be able to identify $G_a Z_c G_i$.

The identification data is loaded.
#+begin_src matlab
  uh = load('mat/data_uch.mat', 't', 'Uch', 'Vch');
  uv = load('mat/data_ucv.mat', 't', 'Ucv', 'Vcv');
#+end_src

We remove the first seconds where the Cercalo is turned on.
#+begin_src matlab :exports none
  t0 = 1;

  uh.Uch(uh.t<t0) = [];
  uh.Vch(uh.t<t0) = [];
  uh.t(uh.t<t0)   = [];
  uh.t = uh.t - uh.t(1); % We start at t=0

  t0 = 1;

  uv.Ucv(uv.t<t0) = [];
  uv.Vcv(uv.t<t0) = [];
  uv.t(uv.t<t0)   = [];
  uv.t = uv.t - uv.t(1); % We start at t=0
#+end_src

#+begin_src matlab
  win = hanning(ceil(1*fs));
  [GaZcGi_h, f] = tfestimate(uh.Uch, uh.Vch, win, [], [], fs);
  [GaZcGi_v, ~] = tfestimate(uv.Ucv, uv.Vcv, win, [], [], fs);
#+end_src

#+begin_src matlab :exports none
  GaZcGi_resp = abs(squeeze(freqresp(Ga*Zc*Gi, f, 'Hz')));

  figure;

  ax1 = subplot(1, 2, 1);
  hold on;
  plot(f, abs(GaZcGi_h), 'k-', 'DisplayName', 'Identified')
  plot(f, squeeze(GaZcGi_resp(1,1,:)), 'k--', 'DisplayName', 'Theoretical')
  title('FRF $G_{i,h} Z_{c,h} G_{a,h} = \frac{V_{c,h}}{U_{c,h}}$')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude'); xlabel('Frequency [Hz]');
  legend();
  hold off;

  ax2 = subplot(1, 2, 2);
  hold on;
  plot(f, abs(GaZcGi_v), 'k-', 'DisplayName', 'Identified')
  plot(f, squeeze(GaZcGi_resp(2,2,:)), 'k--', 'DisplayName', 'Theoretical')
  title('FRF $G_{a,v} Z_{c,v} G_{i,v} = \frac{V_{c,v}}{U_{c,v}}$')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude'); xlabel('Frequency [Hz]');
  legend();
  hold off;

  linkaxes([ax1,ax2],'xy');
  xlim([1, 2000]);
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/current_amplifier_comp_theory_id.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:current_amplifier_comp_theory_id
#+CAPTION: Identified and Theoretical Transfer Function $G_a G_i$ ([[./figs/current_amplifier_comp_theory_id.png][png]], [[./figs/current_amplifier_comp_theory_id.pdf][pdf]])
[[file:figs/current_amplifier_comp_theory_id.png]]

There is a gain mismatch, that is probably due to bad identification of the inductance and resistance measurement of the cercalo inductors.
Thus, we suppose $G_a$ is perfectly known (the gain and cut-off frequency of the voltage amplifier is very accurate) and that $G_i$ is also well determined as it mainly depends on the resistor used in the amplifier that is well measured.
#+begin_src matlab
  Gi_resp_h = abs(GaZcGi_h)./squeeze(abs(freqresp(Ga(1,1)*Zc(1,1), f, 'Hz')));
  Gi_resp_v = abs(GaZcGi_v)./squeeze(abs(freqresp(Ga(2,2)*Zc(2,2), f, 'Hz')));
  Gi = tf(blkdiag(mean(Gi_resp_h(f>20 & f<200)), mean(Gi_resp_v(f>20 & f<200))));
#+end_src

#+begin_src matlab :exports none
  GaZcGi_resp = abs(squeeze(freqresp(Ga*Zc*Gi, f, 'Hz')));

  figure;

  ax1 = subplot(1, 2, 1);
  hold on;
  plot(f, abs(GaZcGi_h), 'k-', 'DisplayName', 'Identified')
  plot(f, squeeze(GaZcGi_resp(1,1,:)), 'k--', 'DisplayName', 'Theoretical')
  title('FRF $G_{i,h} Z_{c,h} G_{a,h} = \frac{V_{c,h}}{U_{c,h}}$')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude'); xlabel('Frequency [Hz]');
  legend();
  hold off;

  ax2 = subplot(1, 2, 2);
  hold on;
  plot(f, abs(GaZcGi_v), 'k-', 'DisplayName', 'Identified')
  plot(f, squeeze(GaZcGi_resp(2,2,:)), 'k--', 'DisplayName', 'Theoretical')
  title('FRF $G_{a,v} Z_{c,v} G_{i,v} = \frac{V_{c,v}}{U_{c,v}}$')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude'); xlabel('Frequency [Hz]');
  legend();
  hold off;

  linkaxes([ax1,ax2],'xy');
  xlim([1, 2000]);
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/current_amplifier_comp_theory_id_bis.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:current_amplifier_comp_theory_id_bis
#+CAPTION: Identified and Theoretical Transfer Function $G_a G_i$ ([[./figs/current_amplifier_comp_theory_id_bis.png][png]], [[./figs/current_amplifier_comp_theory_id_bis.pdf][pdf]])
[[file:figs/current_amplifier_comp_theory_id_bis.png]]

Finally, we have the following transfer functions:
#+begin_src matlab :results output replace :exports results
  Gi,Zc,Ga
#+end_src

#+RESULTS:
#+begin_example
ans = filepath;
if ischar(ans), fid = fopen('/tmp/babel-ZKMGJu/matlab-FA7h5L', 'w'); fprintf(fid, '%s\n', ans); fclose(fid);
else, dlmwrite('/tmp/babel-ZKMGJu/matlab-FA7h5L', ans, '\t')
end
'org_babel_eoe'
Gi,Zc,Ga
'org_babel_eoe'
ans = filepath;
if ischar(ans), fid = fopen('/tmp/babel-ZKMGJu/matlab-FA7h5L', 'w'); fprintf(fid, '%s\n', ans); fclose(fid);
else, dlmwrite('/tmp/babel-ZKMGJu/matlab-FA7h5L', ans, '\t')
end
'org_babel_eoe'
ans =
    'org_babel_eoe'
Gi,Zc,Ga

Gi =

  From input 1 to output...
   1:  0.01275

   2:  0

  From input 2 to output...
   1:  0

   2:  0.01382

Static gain.


Zc =

  From input 1 to output...
   1:  9.3

   2:  0

  From input 2 to output...
   1:  0

   2:  8.3

Static gain.


Ga =

  From input 1 to output...
       6.2832e+06
   1:  ----------
        (s+6283)

   2:  0

  From input 2 to output...
   1:  0

       6.2832e+06
   2:  ----------
        (s+6283)

Continuous-time zero/pole/gain model.
#+end_example

** Identification of the Cercalo Dynamics
*** Introduction                                                   :ignore:
We now wish to identify the dynamics of the Cercalo identified by $G_c$ on the block diagram in Fig. [[fig:block_diagram_simplify]].

To do so, we inject some noise at the input of the current amplifier $[U_{c,h},\ U_{c,v}]$ (one input after the other) and we measure simultaneously the output of the 4QD $[V_{p,h},\ V_{p,v}]$.

The transfer function obtained will be $G_c G_i$, and because we have already identified $G_i$, we can obtain $G_c$ by multiplying the obtained transfer function matrix by ${G_i}^{-1}$.

*** Input / Output data
The identification data is loaded
#+begin_src matlab
  uh = load('mat/data_uch.mat', 't', 'Uch', 'Vph', 'Vpv');
  uv = load('mat/data_ucv.mat', 't', 'Ucv', 'Vph', 'Vpv');
#+end_src

We remove the first seconds where the Cercalo is turned on.
#+begin_src matlab
  t0 = 1;

  uh.Uch(uh.t<t0) = [];
  uh.Vph(uh.t<t0) = [];
  uh.Vpv(uh.t<t0) = [];
  uh.t(uh.t<t0)   = [];
  uh.t = uh.t - uh.t(1); % We start at t=0

  t0 = 1;

  uv.Ucv(uv.t<t0) = [];
  uv.Vph(uv.t<t0) = [];
  uv.Vpv(uv.t<t0) = [];
  uv.t(uv.t<t0)   = [];

  uv.t = uv.t - uv.t(1); % We start at t=0
#+end_src

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(1, 2, 1);
  plot(uh.t, uh.Uch, 'DisplayName', '$Uc_h$');
  xlabel('Time [s]');
  ylabel('Amplitude [V]');
  legend();

  ax2 = subplot(1, 2, 2);
  hold on;
  plot(uh.t, uh.Vph, 'DisplayName', '$Vp_h$');
  plot(uh.t, uh.Vpv, 'DisplayName', '$Vp_v$');
  hold off;
  xlabel('Time [s]');
  ylabel('Amplitude [V]');
  legend();

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

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/identification_uh.pdf" :var figsize="full-normal" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:identification_uh
#+CAPTION: Identification signals when exciting the horizontal direction ([[./figs/identification_uh.png][png]], [[./figs/identification_uh.pdf][pdf]])
[[file:figs/identification_uh.png]]


#+begin_src matlab :exports none
  figure;
  ax1 = subplot(1, 2, 1);
  plot(uv.t, uv.Ucv, 'DisplayName', '$Uc_v$');
  xlabel('Time [s]');
  ylabel('Amplitude [V]');

  ax2 = subplot(1, 2, 2);
  hold on;
  plot(uv.t, uv.Vpv, 'DisplayName', '$Vp_v$');
  plot(uv.t, uv.Vph, 'DisplayName', '$Vp_h$');
  hold off;
  xlabel('Time [s]');
  ylabel('Amplitude [V]');
  legend();

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

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/identification_uv.pdf" :var figsize="full-normal" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:identification_uv
#+CAPTION: Identification signals when exciting in the vertical direction ([[./figs/identification_uv.png][png]], [[./figs/identification_uv.pdf][pdf]])
[[file:figs/identification_uv.png]]

*** Coherence
The window used for the spectral analysis is an =hanning= windows with temporal size equal to 1 second.
#+begin_src matlab
  win = hanning(ceil(1*fs));
#+end_src

#+begin_src matlab
  [coh_Uch_Vph, f] = mscohere(uh.Uch, uh.Vph, win, [], [], fs);
  [coh_Uch_Vpv, ~] = mscohere(uh.Uch, uh.Vpv, win, [], [], fs);
  [coh_Ucv_Vph, ~] = mscohere(uv.Ucv, uv.Vph, win, [], [], fs);
  [coh_Ucv_Vpv, ~] = mscohere(uv.Ucv, uv.Vpv, win, [], [], fs);
#+end_src

#+begin_src matlab :exports none
  figure;
  ax11 = subplot(2, 2, 1);
  hold on;
  plot(f, coh_Uch_Vph)
  set(gca, 'Xscale', 'log');
  title('Coherence $\frac{Vp_h}{Uc_h}$')
  ylabel('Coherence')
  hold off;

  ax12 = subplot(2, 2, 2);
  hold on;
  plot(f, coh_Ucv_Vph)
  set(gca, 'Xscale', 'log');
  title('Coherence $\frac{Vp_h}{Uc_v}$')
  hold off;

  ax21 = subplot(2, 2, 3);
  hold on;
  plot(f, coh_Uch_Vpv)
  set(gca, 'Xscale', 'log');
  title('Coherence $\frac{Vp_v}{Uc_h}$')
  ylabel('Coherence')
  xlabel('Frequency [Hz]')
  hold off;

  ax22 = subplot(2, 2, 4);
  hold on;
  plot(f, coh_Ucv_Vpv)
  set(gca, 'Xscale', 'log');
  title('Coherence $\frac{Vp_v}{Uc_v}$')
  xlabel('Frequency [Hz]')
  hold off;

  linkaxes([ax11,ax12,ax21,ax22],'xy');
  xlim([10, 1000]); ylim([0, 1]);
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/coh_cercalo.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:coh_cercalo
#+CAPTION: Coherence ([[./figs/coh_cercalo.png][png]], [[./figs/coh_cercalo.pdf][pdf]])
[[file:figs/coh_cercalo.png]]

*** Estimation of the Frequency Response Function Matrix
We compute an estimate of the transfer functions.
#+begin_src matlab
  [tf_Uch_Vph, f] = tfestimate(uh.Uch, uh.Vph, win, [], [], fs);
  [tf_Uch_Vpv, ~] = tfestimate(uh.Uch, uh.Vpv, win, [], [], fs);
  [tf_Ucv_Vph, ~] = tfestimate(uv.Ucv, uv.Vph, win, [], [], fs);
  [tf_Ucv_Vpv, ~] = tfestimate(uv.Ucv, uv.Vpv, win, [], [], fs);
#+end_src

#+begin_src matlab :exports none
  figure;
  ax11 = subplot(2, 2, 1);
  hold on;
  plot(f, abs(tf_Uch_Vph))
  title('Frequency Response Function $\frac{Vp_h}{Uc_h}$')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude [V/V]')
  hold off;

  ax12 = subplot(2, 2, 2);
  hold on;
  plot(f, abs(tf_Ucv_Vph))
  title('Frequency Response Function $\frac{Vp_h}{Uc_v}$')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  hold off;

  ax21 = subplot(2, 2, 3);
  hold on;
  plot(f, abs(tf_Uch_Vpv))
  title('Frequency Response Function $\frac{Vp_v}{Uc_h}$')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude [V/V]')
  xlabel('Frequency [Hz]')
  hold off;

  ax22 = subplot(2, 2, 4);
  hold on;
  plot(f, abs(tf_Ucv_Vpv))
  title('Frequency Response Function $\frac{Vp_v}{Uc_v}$')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  xlabel('Frequency [Hz]')
  hold off;

  linkaxes([ax11,ax12,ax21,ax22],'xy');
  xlim([10, 1000]); ylim([1e-2, 1e3]);
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/frf_cercalo_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:frf_cercalo_gain
#+CAPTION: Frequency Response Matrix ([[./figs/frf_cercalo_gain.png][png]], [[./figs/frf_cercalo_gain.pdf][pdf]])
[[file:figs/frf_cercalo_gain.png]]

#+begin_src matlab :exports none
  figure;
  ax11 = subplot(2, 2, 1);
  hold on;
  plot(f, 180/pi*unwrap(angle(tf_Uch_Vph)))
  title('Frequency Response Function $\frac{Vp_h}{Uc_h}$')
  set(gca, 'Xscale', 'log');
  ylabel('Phase [deg]')
  yticks(-180:90:180);
  hold off;

  ax12 = subplot(2, 2, 2);
  hold on;
  plot(f, 180/pi*unwrap(angle(tf_Ucv_Vph)))
  title('Frequency Response Function $\frac{Vp_h}{Uc_v}$')
  set(gca, 'Xscale', 'log');
  yticks(-180:90:180);
  hold off;

  ax21 = subplot(2, 2, 3);
  hold on;
  plot(f, 180/pi*unwrap(angle(tf_Uch_Vpv)))
  title('Frequency Response Function $\frac{Vp_v}{Uc_h}$')
  set(gca, 'Xscale', 'log');
  ylabel('Phase [deg]')
  xlabel('Frequency [Hz]')
  yticks(-180:90:180);
  hold off;

  ax22 = subplot(2, 2, 4);
  hold on;
  plot(f, 180/pi*unwrap(angle(tf_Ucv_Vpv)))
  title('Frequency Response Function $\frac{Vp_v}{Uc_v}$')
  set(gca, 'Xscale', 'log');
  xlabel('Frequency [Hz]')
  yticks(-180:90:180);
  hold off;

  linkaxes([ax11,ax12,ax21,ax22],'xy');
  xlim([10, 1000]); ylim([-200, 200]);
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/frf_cercalo_phase.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:frf_cercalo_phase
#+CAPTION: Frequency Response Matrix_Phase ([[./figs/frf_cercalo_phase.png][png]], [[./figs/frf_cercalo_phase.pdf][pdf]])
[[file:figs/frf_cercalo_phase.png]]

*** Time Delay
Now, we would like to remove the time delay included in the FRF prior to the model extraction.

Estimation of the time delay:
#+begin_src matlab
  Ts_delay = Ts; % [s]

  G_delay = tf(1, 1, 'InputDelay', Ts_delay);

  G_delay_resp = squeeze(freqresp(G_delay, f, 'Hz'));
#+end_src

We then remove the time delay from the frequency response function.
#+begin_src matlab
  tf_Uch_Vph = tf_Uch_Vph./G_delay_resp;
  tf_Uch_Vpv = tf_Uch_Vpv./G_delay_resp;
  tf_Ucv_Vph = tf_Ucv_Vph./G_delay_resp;
  tf_Ucv_Vpv = tf_Ucv_Vpv./G_delay_resp;
#+end_src

*** Extraction of a transfer function matrix
First we define the initial guess for the resonance frequencies and the weights associated.
#+begin_src matlab
  freqs_res_uh = [410]; % [Hz]
  freqs_res_uv = [250]; % [Hz]
#+end_src

We then make an initial guess on the complex values of the poles.
#+begin_src matlab
  xi = 0.001; % Approximate modal damping
  poles_uh = [2*pi*freqs_res_uh*(xi + 1i), 2*pi*freqs_res_uh*(xi - 1i)];
  poles_uv = [2*pi*freqs_res_uv*(xi + 1i), 2*pi*freqs_res_uv*(xi - 1i)];
#+end_src

We then define the weight that will be used for the fitting.
Basically, we want more weight around the resonance and at low frequency (below the first resonance).
Also, we want more importance where we have a better coherence.
Finally, we ignore data above some frequency.
#+begin_src matlab
  weight_Uch_Vph = coh_Uch_Vph';
  weight_Uch_Vpv = coh_Uch_Vpv';
  weight_Ucv_Vph = coh_Ucv_Vph';
  weight_Ucv_Vpv = coh_Ucv_Vpv';

  alpha = 0.1;

  for freq_i = 1:length(freqs_res_uh)
    weight_Uch_Vph(f>(1-alpha)*freqs_res_uh(freq_i) & f<(1 + alpha)*freqs_res_uh(freq_i)) = 10;
    weight_Uch_Vpv(f>(1-alpha)*freqs_res_uh(freq_i) & f<(1 + alpha)*freqs_res_uh(freq_i)) = 10;
    weight_Ucv_Vph(f>(1-alpha)*freqs_res_uv(freq_i) & f<(1 + alpha)*freqs_res_uv(freq_i)) = 10;
    weight_Ucv_Vpv(f>(1-alpha)*freqs_res_uv(freq_i) & f<(1 + alpha)*freqs_res_uv(freq_i)) = 10;
  end

  weight_Uch_Vph(f>1000) = 0;
  weight_Uch_Vpv(f>1000) = 0;
  weight_Ucv_Vph(f>1000) = 0;
  weight_Ucv_Vpv(f>1000) = 0;
#+end_src

The weights are shown in Fig. [[fig:weights_cercalo]].

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(f, weight_Uch_Vph, 'DisplayName', '$W_{U_{ch},V_{ph}}$');
  plot(f, weight_Uch_Vpv, 'DisplayName', '$W_{U_{ch},V_{pv}}$');
  plot(f, weight_Ucv_Vph, 'DisplayName', '$W_{U_{cv},V_{ph}}$');
  plot(f, weight_Ucv_Vpv, 'DisplayName', '$W_{U_{cv},V_{pv}}$');
  hold off;
  xlabel('Frequency [Hz]'); ylabel('Weight Amplitude');
  set(gca, 'xscale', 'log');
  xlim([f(1), f(end)]);
  legend('location', 'northwest');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/weights_cercalo.pdf" :var figsize="wide-normal" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:weights_cercalo
#+CAPTION: Weights amplitude ([[./figs/weights_cercalo.png][png]], [[./figs/weights_cercalo.pdf][pdf]])
[[file:figs/weights_cercalo.png]]

When we set some options for =vfit3=.
#+begin_src matlab
  opts = struct();

  opts.stable = 1;    % Enforce stable poles
  opts.asymp = 1;     % Force D matrix to be null
  opts.relax = 1;     % Use vector fitting with relaxed non-triviality constraint
  opts.skip_pole = 0; % Do NOT skip pole identification
  opts.skip_res = 0;  % Do NOT skip identification of residues (C,D,E)
  opts.cmplx_ss = 0;  % Create real state space model with block diagonal A

  opts.spy1 = 0;      % No plotting for first stage of vector fitting
  opts.spy2 = 0;      % Create magnitude plot for fitting of f(s)
#+end_src

We define the number of iteration.
#+begin_src matlab
  Niter = 5;
#+end_src

An we run the =vectfit3= algorithm.
#+begin_src matlab
  for iter = 1:Niter
    [SER_Uch_Vph, poles, ~, fit_Uch_Vph] = vectfit3(tf_Uch_Vph.', 1i*2*pi*f, poles_uh, weight_Uch_Vph, opts);
  end
  for iter = 1:Niter
    [SER_Uch_Vpv, poles, ~, fit_Uch_Vpv] = vectfit3(tf_Uch_Vpv.', 1i*2*pi*f, poles_uh, weight_Uch_Vpv, opts);
  end
  for iter = 1:Niter
    [SER_Ucv_Vph, poles, ~, fit_Ucv_Vph] = vectfit3(tf_Ucv_Vph.', 1i*2*pi*f, poles_uv, weight_Ucv_Vph, opts);
  end
  for iter = 1:Niter
    [SER_Ucv_Vpv, poles, ~, fit_Ucv_Vpv] = vectfit3(tf_Ucv_Vpv.', 1i*2*pi*f, poles_uv, weight_Ucv_Vpv, opts);
  end
#+end_src

#+begin_src matlab :exports none
  figure;
  ax11 = subplot(2, 2, 1);
  hold on;
  plot(f, abs(tf_Uch_Vph))
  plot(f, abs(fit_Uch_Vph))
  title('Frequency Response Function $\frac{Vp_h}{Uc_h}$')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude [V/V]')
  hold off;

  ax12 = subplot(2, 2, 2);
  hold on;
  plot(f, abs(tf_Ucv_Vph))
  plot(f, abs(fit_Ucv_Vph))
  title('Frequency Response Function $\frac{Vp_h}{Uc_v}$')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  hold off;

  ax21 = subplot(2, 2, 3);
  hold on;
  plot(f, abs(tf_Uch_Vpv))
  plot(f, abs(fit_Uch_Vpv))
  title('Frequency Response Function $\frac{Vp_v}{Uc_h}$')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude [V/V]')
  xlabel('Frequency [Hz]')
  hold off;

  ax22 = subplot(2, 2, 4);
  hold on;
  plot(f, abs(tf_Ucv_Vpv))
  plot(f, abs(fit_Ucv_Vpv))
  title('Frequency Response Function $\frac{Vp_v}{Uc_v}$')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  xlabel('Frequency [Hz]')
  hold off;

  linkaxes([ax11,ax12,ax21,ax22],'xy');
  xlim([10, 1000]); ylim([1e-2, 1e3]);
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/identification_matrix_fit.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:identification_matrix_fit
#+CAPTION: Transfer Function Extraction of the FRF matrix ([[./figs/identification_matrix_fit.png][png]], [[./figs/identification_matrix_fit.pdf][pdf]])
[[file:figs/identification_matrix_fit.png]]


#+begin_src matlab :exports none
  figure;
  ax11 = subplot(2, 2, 1);
  hold on;
  plot(f, 180/pi*unwrap(angle(tf_Uch_Vph)))
  plot(f, 180/pi*unwrap(angle(fit_Uch_Vph)))
  title('Frequency Response Function $\frac{Vp_h}{Uc_h}$')
  set(gca, 'Xscale', 'log');
  ylabel('Phase [deg]')
  yticks(-180:90:180);
  hold off;

  ax12 = subplot(2, 2, 2);
  hold on;
  plot(f, 180/pi*unwrap(angle(tf_Ucv_Vph)))
  plot(f, 180/pi*unwrap(angle(fit_Ucv_Vph)))
  title('Frequency Response Function $\frac{Vp_h}{Uc_v}$')
  set(gca, 'Xscale', 'log');
  yticks(-180:90:180);
  hold off;

  ax21 = subplot(2, 2, 3);
  hold on;
  plot(f, 180/pi*unwrap(angle(tf_Uch_Vpv)))
  plot(f, 180/pi*unwrap(angle(fit_Uch_Vpv)))
  title('Frequency Response Function $\frac{Vp_v}{Uc_h}$')
  set(gca, 'Xscale', 'log');
  ylabel('Phase [deg]')
  xlabel('Frequency [Hz]')
  yticks(-180:90:180);
  hold off;

  ax22 = subplot(2, 2, 4);
  hold on;
  plot(f, 180/pi*unwrap(angle(tf_Ucv_Vpv)))
  plot(f, 180/pi*unwrap(angle(fit_Ucv_Vpv)))
  title('Frequency Response Function $\frac{Vp_v}{Uc_v}$')
  set(gca, 'Xscale', 'log');
  xlabel('Frequency [Hz]')
  yticks(-180:90:180);
  hold off;

  linkaxes([ax11,ax12,ax21,ax22],'xy');
  xlim([10, 1000]); ylim([-200, 200]);
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/identification_matrix_fit_phase.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:identification_matrix_fit_phase
#+CAPTION: Transfer Function Extraction of the FRF matrix ([[./figs/identification_matrix_fit_phase.png][png]], [[./figs/identification_matrix_fit_phase.pdf][pdf]])
[[file:figs/identification_matrix_fit_phase.png]]

And finally, we create the identified $G_c$ matrix by multiplying by ${G_i}^{-1}$.
#+begin_src matlab
  G_Uch_Vph = tf(minreal(ss(full(SER_Uch_Vph.A),SER_Uch_Vph.B,SER_Uch_Vph.C,SER_Uch_Vph.D)));
  G_Ucv_Vph = tf(minreal(ss(full(SER_Ucv_Vph.A),SER_Ucv_Vph.B,SER_Ucv_Vph.C,SER_Ucv_Vph.D)));
  G_Uch_Vpv = tf(minreal(ss(full(SER_Uch_Vpv.A),SER_Uch_Vpv.B,SER_Uch_Vpv.C,SER_Uch_Vpv.D)));
  G_Ucv_Vpv = tf(minreal(ss(full(SER_Ucv_Vpv.A),SER_Ucv_Vpv.B,SER_Ucv_Vpv.C,SER_Ucv_Vpv.D)));

  Gc = [G_Uch_Vph, G_Ucv_Vph;
        G_Uch_Vpv, G_Ucv_Vpv]*inv(Gi);
#+end_src

** Identification of the Newport Dynamics
*** Introduction                                                   :ignore:
We here identify the transfer function from a reference sent to the Newport $[U_{n,h},\ U_{n,v}]$ to the measurement made by the 4QD $[V_{p,h},\ V_{p,v}]$.

To do so, we inject noise to the Newport $[U_{n,h},\ U_{n,v}]$ and we record the 4QD measurement $[V_{p,h},\ V_{p,v}]$.

*** Input / Output data
The identification data is loaded
#+begin_src matlab
  uh = load('mat/data_unh.mat', 't', 'Unh', 'Vph', 'Vpv');
  uv = load('mat/data_unv.mat', 't', 'Unv', 'Vph', 'Vpv');
#+end_src

We remove the first seconds where the Cercalo is turned on.
#+begin_src matlab
  t0 = 3;

  uh.Unh(uh.t<t0) = [];
  uh.Vph(uh.t<t0) = [];
  uh.Vpv(uh.t<t0) = [];
  uh.t(uh.t<t0)   = [];
  uh.t = uh.t - uh.t(1); % We start at t=0

  t0 = 1.5;

  uv.Unv(uv.t<t0) = [];
  uv.Vph(uv.t<t0) = [];
  uv.Vpv(uv.t<t0) = [];
  uv.t(uv.t<t0)   = [];

  uv.t = uv.t - uv.t(1); % We start at t=0
#+end_src

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(1, 2, 1);
  plot(uh.t, uh.Unh, 'DisplayName', '$Un_h$');
  xlabel('Time [s]');
  ylabel('Amplitude [V]');
  legend();

  ax2 = subplot(1, 2, 2);
  hold on;
  plot(uh.t, uh.Vph, 'DisplayName', '$Vp_h$');
  plot(uh.t, uh.Vpv, 'DisplayName', '$Vp_v$');
  hold off;
  xlabel('Time [s]');
  ylabel('Amplitude [V]');
  legend();

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

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/identification_unh.pdf" :var figsize="full-normal" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:identification_unh
#+CAPTION: Identification signals when exciting the horizontal direction ([[./figs/identification_unh.png][png]], [[./figs/identification_unh.pdf][pdf]])
[[file:figs/identification_unh.png]]

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(1, 2, 1);
  plot(uv.t, uv.Unv, 'DisplayName', '$Un_v$');
  xlabel('Time [s]');
  ylabel('Amplitude [V]');

  ax2 = subplot(1, 2, 2);
  hold on;
  plot(uv.t, uv.Vpv, 'DisplayName', '$Vp_v$');
  plot(uv.t, uv.Vph, 'DisplayName', '$Vp_h$');
  hold off;
  xlabel('Time [s]');
  ylabel('Amplitude [V]');
  legend();

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

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/identification_unv.pdf" :var figsize="full-normal" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:identification_unv
#+CAPTION: Identification signals when exciting in the vertical direction ([[./figs/identification_unv.png][png]], [[./figs/identification_unv.pdf][pdf]])
[[file:figs/identification_unv.png]]

*** Coherence
The window used for the spectral analysis is an =hanning= windows with temporal size equal to 1 second.
#+begin_src matlab
  win = hanning(ceil(1*fs));
#+end_src

#+begin_src matlab
  [coh_Unh_Vph, f] = mscohere(uh.Unh, uh.Vph, win, [], [], fs);
  [coh_Unh_Vpv, ~] = mscohere(uh.Unh, uh.Vpv, win, [], [], fs);
  [coh_Unv_Vph, ~] = mscohere(uv.Unv, uv.Vph, win, [], [], fs);
  [coh_Unv_Vpv, ~] = mscohere(uv.Unv, uv.Vpv, win, [], [], fs);
#+end_src

#+begin_src matlab :exports none
  figure;
  ax11 = subplot(2, 2, 1);
  hold on;
  plot(f, coh_Unh_Vph)
  set(gca, 'Xscale', 'log');
  title('Coherence $\frac{Vp_h}{Un_h}$')
  ylabel('Coherence')
  hold off;

  ax12 = subplot(2, 2, 2);
  hold on;
  plot(f, coh_Unv_Vph)
  set(gca, 'Xscale', 'log');
  title('Coherence $\frac{Vp_h}{Un_v}$')
  hold off;

  ax21 = subplot(2, 2, 3);
  hold on;
  plot(f, coh_Unh_Vpv)
  set(gca, 'Xscale', 'log');
  title('Coherence $\frac{Vp_v}{Un_h}$')
  ylabel('Coherence')
  xlabel('Frequency [Hz]')
  hold off;

  ax22 = subplot(2, 2, 4);
  hold on;
  plot(f, coh_Unv_Vpv)
  set(gca, 'Xscale', 'log');
  title('Coherence $\frac{Vp_v}{Un_v}$')
  xlabel('Frequency [Hz]')
  hold off;

  linkaxes([ax11,ax12,ax21,ax22],'xy');
  xlim([10, 1000]); ylim([0, 1]);
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/id_newport_coherence.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:id_newport_coherence
#+CAPTION: Coherence ([[./figs/id_newport_coherence.png][png]], [[./figs/id_newport_coherence.pdf][pdf]])
[[file:figs/id_newport_coherence.png]]

*** Estimation of the Frequency Response Function Matrix
We compute an estimate of the transfer functions.
#+begin_src matlab
  [tf_Unh_Vph, f] = tfestimate(uh.Unh, uh.Vph, win, [], [], fs);
  [tf_Unh_Vpv, ~] = tfestimate(uh.Unh, uh.Vpv, win, [], [], fs);
  [tf_Unv_Vph, ~] = tfestimate(uv.Unv, uv.Vph, win, [], [], fs);
  [tf_Unv_Vpv, ~] = tfestimate(uv.Unv, uv.Vpv, win, [], [], fs);
#+end_src

#+begin_src matlab :exports none
  figure;
  ax11 = subplot(2, 2, 1);
  hold on;
  plot(f, abs(tf_Unh_Vph))
  title('Frequency Response Function $\frac{Vp_h}{Un_h}$')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude [V/V]')
  hold off;

  ax12 = subplot(2, 2, 2);
  hold on;
  plot(f, abs(tf_Unv_Vph))
  title('Frequency Response Function $\frac{Vp_h}{Un_v}$')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  hold off;

  ax21 = subplot(2, 2, 3);
  hold on;
  plot(f, abs(tf_Unh_Vpv))
  title('Frequency Response Function $\frac{Vp_v}{Un_h}$')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  ylabel('Amplitude [V/V]')
  xlabel('Frequency [Hz]')
  hold off;

  ax22 = subplot(2, 2, 4);
  hold on;
  plot(f, abs(tf_Unv_Vpv))
  title('Frequency Response Function $\frac{Vp_v}{Un_v}$')
  set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
  xlabel('Frequency [Hz]')
  hold off;

  linkaxes([ax11,ax12,ax21,ax22],'xy');
  xlim([10, 1000]); ylim([1e-4, 1e1]);
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/frf_newport_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:frf_newport_gain
#+CAPTION: Frequency Response Matrix ([[./figs/frf_newport_gain.png][png]], [[./figs/frf_newport_gain.pdf][pdf]])
[[file:figs/frf_newport_gain.png]]

#+begin_src matlab :exports none
  figure;
  ax11 = subplot(2, 2, 1);
  hold on;
  plot(f, 180/pi*unwrap(angle(tf_Unh_Vph)))
  title('Frequency Response Function $\frac{Vp_h}{Un_h}$')
  set(gca, 'Xscale', 'log');
  ylabel('Phase [deg]')
  yticks(-180:90:180);
  hold off;

  ax12 = subplot(2, 2, 2);
  hold on;
  plot(f, 180/pi*unwrap(angle(tf_Unv_Vph)))
  title('Frequency Response Function $\frac{Vp_h}{Un_v}$')
  set(gca, 'Xscale', 'log');
  yticks(-180:90:180);
  hold off;

  ax21 = subplot(2, 2, 3);
  hold on;
  plot(f, 180/pi*unwrap(angle(tf_Unh_Vpv)))
  title('Frequency Response Function $\frac{Vp_v}{Un_h}$')
  set(gca, 'Xscale', 'log');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  yticks(-180:90:180);
  hold off;

  ax22 = subplot(2, 2, 4);
  hold on;
  plot(f, 180/pi*unwrap(angle(tf_Unv_Vpv)))
  title('Frequency Response Function $\frac{Vp_v}{Un_v}$')
  set(gca, 'Xscale', 'log');
  xlabel('Frequency [Hz]');
  yticks(-180:90:180);
  hold off;

  linkaxes([ax11,ax12,ax21,ax22],'xy');
  xlim([10, 1000]); ylim([-200, 200]);
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/frf_newport_phase.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:frf_newport_phase
#+CAPTION: Frequency Response Matrix Phase ([[./figs/frf_newport_phase.png][png]], [[./figs/frf_newport_phase.pdf][pdf]])
[[file:figs/frf_newport_phase.png]]

*** Time Delay
Now, we would like to remove the time delay included in the FRF prior to the model extraction.

Estimation of the time delay:
#+begin_src matlab
  Ts_delay = 0.0005; % [s]

  G_delay = tf(1, 1, 'InputDelay', Ts_delay);

  G_delay_resp = squeeze(freqresp(G_delay, f, 'Hz'));
#+end_src

We then remove the time delay from the frequency response function.
#+begin_src matlab :exports none
  figure;
  ax11 = subplot(2, 2, 1);
  hold on;
  plot(f, 180/pi*unwrap(angle(tf_Unh_Vph)))
  plot(f, 180/pi*unwrap(angle(tf_Unh_Vph./G_delay_resp)))
  title('Frequency Response Function $\frac{Vp_h}{Un_h}$')
  set(gca, 'Xscale', 'log');
  ylabel('Phase [deg]')
  yticks(-180:90:180);
  hold off;

  ax12 = subplot(2, 2, 2);
  hold on;
  plot(f, 180/pi*unwrap(angle(tf_Unv_Vph)))
  plot(f, 180/pi*unwrap(angle(tf_Unv_Vph./G_delay_resp)))
  title('Frequency Response Function $\frac{Vp_h}{Un_v}$')
  set(gca, 'Xscale', 'log');
  yticks(-180:90:180);
  hold off;

  ax21 = subplot(2, 2, 3);
  hold on;
  plot(f, 180/pi*unwrap(angle(tf_Unh_Vpv)))
  plot(f, 180/pi*unwrap(angle(tf_Unh_Vpv./G_delay_resp)))
  title('Frequency Response Function $\frac{Vp_v}{Un_h}$')
  set(gca, 'Xscale', 'log');
  ylabel('Phase [deg]')
  xlabel('Frequency [Hz]')
  yticks(-180:90:180);
  hold off;

  ax22 = subplot(2, 2, 4);
  hold on;
  plot(f, 180/pi*unwrap(angle(tf_Unv_Vpv)))
  plot(f, 180/pi*unwrap(angle(tf_Unv_Vpv./G_delay_resp)))
  title('Frequency Response Function $\frac{Vp_v}{Un_v}$')
  set(gca, 'Xscale', 'log');
  xlabel('Frequency [Hz]')
  yticks(-180:90:180);
  hold off;

  linkaxes([ax11,ax12,ax21,ax22],'xy');
  xlim([10, 1000]); ylim([-200, 200]);
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/time_delay_newport.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:time_delay_newport
#+CAPTION: Phase change due to time-delay in the Newport dynamics ([[./figs/time_delay_newport.png][png]], [[./figs/time_delay_newport.pdf][pdf]])
[[file:figs/time_delay_newport.png]]

*** Extraction of a transfer function matrix
From Fig. [[fig:frf_newport_gain]], it seems reasonable to model the Newport dynamics as diagonal and constant.

#+begin_src matlab
  Gn = blkdiag(tf(mean(abs(tf_Unh_Vph(f>10 & f<100)))), tf(mean(abs(tf_Unv_Vpv(f>10 & f<100)))));
#+end_src

** Full System
We now have identified:
- $G_i$
- $G_a$
- $G_c$
- $G_n$
- $G_d$

We name the input and output of each transfer function:
#+begin_src matlab
  Gi.InputName = {'Uch', 'Ucv'};
  Gi.OutputName = {'Ich', 'Icv'};

  Zc.InputName = {'Ich', 'Icv'};
  Zc.OutputName = {'Vtch', 'Vtcv'};

  Ga.InputName = {'Vtch', 'Vtcv'};
  Ga.OutputName = {'Vch', 'Vcv'};

  Gc.InputName = {'Ich', 'Icv'};
  Gc.OutputName = {'Vpch', 'Vpcv'};

  Gn.InputName = {'Unh', 'Unv'};
  Gn.OutputName = {'Vpnh', 'Vpnv'};

  Gd.InputName = {'Rh', 'Rv'};
  Gd.OutputName = {'Vph', 'Vpv'};
#+end_src

#+begin_src matlab
  Sh = sumblk('Vph = Vpch + Vpnh');
  Sv = sumblk('Vpv = Vpcv + Vpnv');
#+end_src

#+begin_src matlab
  inputs  = {'Uch', 'Ucv', 'Unh', 'Unv'};
  outputs = {'Vch', 'Vcv', 'Ich', 'Icv', 'Rh', 'Rv', 'Vph', 'Vpv'};

  sys = connect(Gi, Zc, Ga, Gc, Gn, inv(Gd), Sh, Sv, inputs, outputs);
#+end_src

The file =mat/plant.mat= is accessible [[./mat/plant.mat][here]].
#+begin_src matlab
  save('mat/plant.mat', 'sys', 'Gi', 'Zc', 'Ga', 'Gc', 'Gn', 'Gd');
#+end_src

* Active Damping
** 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
  freqs = logspace(1, 3, 1000);
#+end_src

** Load Plant
#+begin_src matlab
  load('mat/plant.mat', 'sys', 'Gi', 'Zc', 'Ga', 'Gc', 'Gn', 'Gd');
#+end_src

** Test
#+begin_src matlab
  bode(sys({'Vch', 'Vcv'}, {'Uch', 'Ucv'}));
#+end_src

#+begin_src matlab
  Kppf = blkdiag(-10000/s, tf(0));

  Kppf.InputName  = {'Vch', 'Vcv'};
  Kppf.OutputName = {'Uch', 'Ucv'};
#+end_src

#+begin_src matlab :exports none
  figure;
  % Magnitude
  ax1 = subaxis(2,1,1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(G, freqs, 'Hz'))), 'k-');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude [dB]');
  hold off;

  % Phase
  ax2 = subaxis(2,1,2);
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(G, freqs, 'Hz'))), 'k-');
  set(gca,'xscale','log');
  yticks(-360:90:180);
  ylim([-360 0]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;

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

#+begin_src matlab
  inputs  = {'Uch', 'Ucv', 'Unh', 'Unv'};
  outputs = {'Ich', 'Icv', 'Rh', 'Rv', 'Vph', 'Vpv'};

  sys_cl = connect(sys, Kppf, inputs, outputs);

  figure; bode(sys_cl({'Vph', 'Vpv'}, {'Uch', 'Ucv'}), sys({'Vph', 'Vpv'}, {'Uch', 'Ucv'}))
#+end_src

* Huddle Test
** Matlab Init                                              :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
  <<matlab-dir>>
#+end_src

#+begin_src matlab :exports none :results silent :noweb yes
  <<matlab-init>>
#+end_src

** Load data
We load the data taken during the Huddle Test.
#+begin_src matlab
  load('mat/data_huddle_test.mat', ...
       't', 'Uch', 'Ucv', ...
       'Unh', 'Unv', ...
       'Vph', 'Vpv', ...
       'Vch', 'Vcv', ...
       'Vnh', 'Vnv', ...
       'Va');
#+end_src

** Pre-processing
We remove the first second of data where everything is settling down.
#+begin_src matlab
  t0 = 1;

  Uch(t<t0) = [];
  Ucv(t<t0) = [];
  Unh(t<t0) = [];
  Unv(t<t0) = [];
  Vph(t<t0) = [];
  Vpv(t<t0) = [];
  Vch(t<t0) = [];
  Vcv(t<t0) = [];
  Vnh(t<t0) = [];
  Vnv(t<t0) = [];
  Va(t<t0)  = [];
  t(t<t0)   = [];

  t = t - t(1); % We start at t=0
#+end_src

** Time Domain Data
#+begin_src matlab :exports none
  figure;
  hold on;
  plot(t, Uch, 'DisplayName', '$Vp_h$');
  plot(t, Ucv, 'DisplayName', '$Vp_v$');
  hold off;
  xlabel('Time [s]');
  ylabel('Amplitude [V]');
  xlim([t(1), t(end)]);
  legend();
#+end_src

We compute the Power Spectral Density of the horizontal and vertical positions of the beam as measured by the 4 quadrant diode.
#+begin_src matlab
  [psd_Vph, f] = pwelch(Vph, hanning(ceil(1*fs)), [], [], fs);
  [psd_Vpv, ~] = pwelch(Vpv, hanning(ceil(1*fs)), [], [], fs);
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(f, sqrt(psd_Vph), 'DisplayName', '$\Gamma_{Vp_h}$');
  plot(f, sqrt(psd_Vpv), 'DisplayName', '$\Gamma_{Vp_v}$');
  hold off;
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{V}{\sqrt{Hz}}\right]$')
  legend('Location', 'southwest');
  xlim([1, 1000]);
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(t, Vch, 'DisplayName', '$Vc_h$');
  plot(t, Vcv, 'DisplayName', '$Vc_v$');
  hold off;
  xlabel('Time [s]');
  ylabel('Amplitude [V]');
  xlim([t(1), t(end)]);
  legend();
#+end_src

We compute the Power Spectral Density of the voltage across the inductance used for horizontal and vertical positioning of the Cercalo.
#+begin_src matlab
  [psd_Vch, f] = pwelch(Vch, hanning(ceil(1*fs)), [], [], fs);
  [psd_Vcv, ~] = pwelch(Vcv, hanning(ceil(1*fs)), [], [], fs);
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(f, sqrt(psd_Vch), 'DisplayName', '$\Gamma_{Vc_h}$');
  plot(f, sqrt(psd_Vcv), 'DisplayName', '$\Gamma_{Vc_v}$');
  hold off;
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{V}{\sqrt{Hz}}\right]$')
  legend('Location', 'southwest');
  xlim([1, 1000]);
#+end_src

* Plant Scaling
|                        | Value | Unit        |   |
|------------------------+-------+-------------+---|
| Expected perturbations |     1 | [V]         | $U_n$ |
| Maximum input usage    |    10 | [V]         | $U_c$ |
| Maximum wanted error   |    10 | [$\mu rad$] | $\theta$ |
| Measured noise         |     5 | [$\mu rad$] |   |

** General Configuration

* Plant Analysis
** Matlab Init                                              :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
  <<matlab-dir>>
#+end_src

#+begin_src matlab :exports none :results silent :noweb yes
  <<matlab-init>>
#+end_src

** Load Plant
#+begin_src matlab
  load('mat/plant.mat', 'G');
#+end_src

** RGA-Number
#+begin_src matlab
  freqs = logspace(2, 4, 1000);
  G_resp = freqresp(G, freqs, 'Hz');
  A = zeros(size(G_resp));
  RGAnum = zeros(1, length(freqs));

  for i = 1:length(freqs)
    A(:, :, i) = G_resp(:, :, i).*inv(G_resp(:, :, i))';
    RGAnum(i) = sum(sum(abs(A(:, :, i)-eye(2))));
  end
  % RGA = G0.*inv(G0)';
#+end_src

#+begin_src matlab
  figure;
  plot(freqs, RGAnum);
  set(gca, 'xscale', 'log');
#+end_src

#+begin_src matlab
  U = zeros(2, 2, length(freqs));
  S = zeros(2, 2, length(freqs))
  V = zeros(2, 2, length(freqs));

  for i = 1:length(freqs)
    [Ui, Si, Vi] = svd(G_resp(:, :, i));
    U(:, :, i) = Ui;
    S(:, :, i) = Si;
    V(:, :, i) = Vi;
  end
#+end_src

** Rotation Matrix
#+begin_src matlab
  G0 = freqresp(G, 0);
#+end_src

* Control Objective
The maximum expected stroke is $y_\text{max} = 3mm \approx 5e^{-2} rad$ at $1Hz$.
The maximum wanted error is $e_\text{max} = 10 \mu rad$.

Thus, we require the sensitivity function at $\omega_0 = 1\text{ Hz}$:
\begin{align*}
  |S(j\omega_0)| &< \left| \frac{e_\text{max}}{y_\text{max}} \right| \\
                 &< 2 \cdot 10^{-4}
\end{align*}

In terms of loop gain, this is equivalent to:
\[ |L(j\omega_0)| > 5 \cdot 10^{3} \]

* Decentralized Control
:PROPERTIES:
:header-args:matlab+: :tangle matlab/decentralized_control.m
:header-args:matlab+: :comments org :mkdirp yes
:END:
<<sec:decentralized_control>>

** Introduction                                                     :ignore:
In this section, we try to implement a simple decentralized controller.

** ZIP file containing the data and matlab files                     :ignore:
#+begin_src bash :exports none :results none
  if [ matlab/decentralized_control.m -nt data/decentralized_control.zip ]; then
    cp matlab/decentralized_control.m decentralized_control.m;
    zip data/decentralized_control \
        mat/plant.mat \
        decentralized_control.m
    rm decentralized_control.m;
  fi
#+end_src

#+begin_note
  All the files (data and Matlab scripts) are accessible [[file:data/decentralized_control.zip][here]].
#+end_note

** 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
  freqs = logspace(0, 3, 1000);
#+end_src

** Load Plant
#+begin_src matlab
  load('mat/plant.mat', 'sys', 'Gi', 'Zc', 'Ga', 'Gc', 'Gn', 'Gd');
#+end_src

** Diagonal Controller
Using =SISOTOOL=, a diagonal controller is designed.
The two SISO loop gains are shown in Fig. [[fig:diag_contr_loop_gain]].
#+begin_src matlab
  Kh = -0.25598*(s+112)*(s^2 + 15.93*s + 6.686e06)/((s^2*(s+352.5)*(1+s/2/pi/2000)));
  Kv = 10207*(s+55.15)*(s^2 + 17.45*s + 2.491e06)/(s^2*(s+491.2)*(s+7695));

  K = blkdiag(Kh, Kv);
  K.InputName  = {'Rh', 'Rv'};
  K.OutputName = {'Uch', 'Ucv'};
#+end_src

#+begin_src matlab :exports none
  figure;
  % Magnitude
  ax1 = subaxis(2,1,1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Kh*sys('Rh', 'Uch'), freqs, 'Hz'))), 'DisplayName', '$L_h = K_h G_{d,h}^{-1} G_{\frac{V_{p,h}}{\tilde{U}_{c,h}}} G_{i,h} $');
  plot(freqs, abs(squeeze(freqresp(Kv*sys('Rv', 'Ucv'), freqs, 'Hz'))), 'DisplayName', '$L_v = K_v G_{d,v}^{-1} G_{\frac{V_{p,v}}{\tilde{U}_{c,v}}} G_{i,v} $');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude [dB]');
  hold off;
  legend('location', 'northeast');

  % Phase
  ax2 = subaxis(2,1,2);
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(Kh*sys('Rh', 'Uch'), freqs, 'Hz'))));
  plot(freqs, 180/pi*angle(squeeze(freqresp(Kv*sys('Rv', 'Ucv'), freqs, 'Hz'))));
  set(gca,'xscale','log');
  yticks(-180:90:180);
  ylim([-180 180]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;

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

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/diag_contr_loop_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:diag_contr_loop_gain
#+CAPTION: Loop Gain using the Decentralized Diagonal Controller ([[./figs/diag_contr_loop_gain.png][png]], [[./figs/diag_contr_loop_gain.pdf][pdf]])
[[file:figs/diag_contr_loop_gain.png]]

We then close the loop and we look at the transfer function from the Newport rotation signal to the beam angle (Fig. [[fig:diag_contr_effect_newport]]).
#+begin_src matlab
  inputs  = {'Uch', 'Ucv', 'Unh', 'Unv'};
  outputs = {'Vch', 'Vcv', 'Ich', 'Icv', 'Rh', 'Rv', 'Vph', 'Vpv'};

  sys_cl = connect(sys, -K, inputs, outputs);
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  set(gca,'ColorOrderIndex',1);
  plot(freqs, abs(squeeze(freqresp(sys('Rh', 'Unh'), freqs, 'Hz'))), '-', 'DisplayName', 'OL - $R_h/U_{n,h}$');
  set(gca,'ColorOrderIndex',1);
  plot(freqs, abs(squeeze(freqresp(sys_cl('Rh', 'Unh'), freqs, 'Hz'))), '--', 'DisplayName', 'CL - $R_h/U_{n,h}$');
  set(gca,'ColorOrderIndex',2);
  plot(freqs, abs(squeeze(freqresp(sys('Rv', 'Unv'), freqs, 'Hz'))), '-', 'DisplayName', 'OL - $R_v/U_{n,v}$');
  set(gca,'ColorOrderIndex',2);
  plot(freqs, abs(squeeze(freqresp(sys_cl('Rv', 'Unv'), freqs, 'Hz'))), '--', 'DisplayName', 'CL - $R_v/U_{n,v}$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude [dB]');
  hold off;
  xlim([freqs(1), freqs(end)]);
  legend('location', 'southeast');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/diag_contr_effect_newport.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:diag_contr_effect_newport
#+CAPTION: Effect of the Newport rotation on the beam position when the loop is closed using the Decentralized Diagonal Controller ([[./figs/diag_contr_effect_newport.png][png]], [[./figs/diag_contr_effect_newport.pdf][pdf]])
[[file:figs/diag_contr_effect_newport.png]]

** Save the Controller
#+begin_src matlab
  Kd = c2d(K, 1e-4, 'tustin');
#+end_src

The diagonal controller is accessible [[./mat/K_diag.mat][here]].
#+begin_src matlab
  save('mat/K_diag.mat', 'K', 'Kd');
#+end_src

* Newport Control
** Introduction                                                     :ignore:
In this section, we try to implement a simple decentralized controller for the Newport.
This can be used to align the 4QD:
- once there is a signal from the 4QD, the Newport feedback loop is closed
- thus, the Newport is positioned such that the beam hits the center of the 4QD
- then we can move the 4QD manually in X-Y plane in order to cancel the command signal of the Newport
- finally, we are sure to be aligned when the command signal of the Newport is 0

** 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
  freqs = logspace(0, 2, 1000);
#+end_src

** Load Plant
#+begin_src matlab
  load('mat/plant.mat', 'Gn', 'Gd');
#+end_src

** Analysis
The plant is basically a constant until frequencies up to the required bandwidth.

We get that constant value.
#+begin_src matlab
  Gn0 = freqresp(inv(Gd)*Gn, 0);
#+end_src

We design two controller containing 2 integrators and one lead near the crossover frequency set to 10Hz.
#+begin_src matlab
  h = 2;
  w0 = 2*pi*10;

  Knh = 1/Gn0(1,1) * (w0/s)^2 * (1 + s/w0*h)/(1 + s/w0/h)/h;
  Knv = 1/Gn0(2,2) * (w0/s)^2 * (1 + s/w0*h)/(1 + s/w0/h)/h;
#+end_src

#+begin_src matlab :exports none
  figure;

  hold on;
  plot(freqs, abs(squeeze(freqresp(Gn0(1,1)*Knh, freqs, 'Hz'))))
  hold off;
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Loop Gain');
#+end_src

#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/loop_gain_newport.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:loop_gain_newport
#+CAPTION: Diagonal Loop Gain for the Newport ([[./figs/loop_gain_newport.png][png]], [[./figs/loop_gain_newport.pdf][pdf]])
[[file:figs/loop_gain_newport.png]]

** Save
#+begin_src matlab
  Kn = blkdiag(Knh, Knv);
  Knd = c2d(Kn, 1e-4, 'tustin');
#+end_src


The controllers can be downloaded [[./mat/K_newport.mat][here]].

#+begin_src matlab
  save('mat/K_newport.mat', 'Kn', 'Knd');
#+end_src

* Measuement of the non-repeatability
** Introduction                                                     :ignore:
- Explanation of the procedure
- List all sources of error and their effects on the Attocube measurement
- Think about how to determine the value of the individual sources of error

** 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
  fs = 1e4;
#+end_src

** Data Load
#+begin_src matlab
  uh = load('mat/data_rep_h.mat', ...
       't', 'Uch', 'Ucv', ...
       'Unh', 'Unv', ...
       'Vph', 'Vpv', ...
       'Vnh', 'Vnv', ...
       'Va');

  uv = load('mat/data_rep_v.mat', ...
       't', 'Uch', 'Ucv', ...
       'Unh', 'Unv', ...
       'Vph', 'Vpv', ...
       'Vnh', 'Vnv', ...
       'Va');
#+end_src

#+begin_src matlab
  % Let's start one second after the first command in the system
  i0 = find(uh.Unh ~= 0, 1) + fs;
  iend = i0+fs*floor((length(uh.t)-i0)/fs);

  uh.Uch([1:i0-1, iend:end]) = [];
  uh.Ucv([1:i0-1, iend:end]) = [];
  uh.Unh([1:i0-1, iend:end]) = [];
  uh.Unv([1:i0-1, iend:end]) = [];
  uh.Vph([1:i0-1, iend:end]) = [];
  uh.Vpv([1:i0-1, iend:end]) = [];
  uh.Vnh([1:i0-1, iend:end]) = [];
  uh.Vnv([1:i0-1, iend:end]) = [];
  uh.Va ([1:i0-1, iend:end]) = [];
  uh.t  ([1:i0-1, iend:end]) = [];

  % We reset the time t
  uh.t = uh.t - uh.t(1);
#+end_src

#+begin_src matlab
  % Let's start one second after the first command in the system
  i0 = find(uv.Unv ~= 0, 1) + fs;
  iend = i0+fs*floor((length(uv.t)-i0)/fs);

  uv.Uch([1:i0-1, iend:end]) = [];
  uv.Ucv([1:i0-1, iend:end]) = [];
  uv.Unh([1:i0-1, iend:end]) = [];
  uv.Unv([1:i0-1, iend:end]) = [];
  uv.Vph([1:i0-1, iend:end]) = [];
  uv.Vpv([1:i0-1, iend:end]) = [];
  uv.Vnh([1:i0-1, iend:end]) = [];
  uv.Vnv([1:i0-1, iend:end]) = [];
  uv.Va ([1:i0-1, iend:end]) = [];
  uv.t  ([1:i0-1, iend:end]) = [];

  % We reset the time t
  uv.t = uv.t - uv.t(1);
#+end_src

** Verify Tracking Angle Error
Let's verify that the positioning error of the beam is small and what could be the effect on the distance measured by the intereferometer.

#+begin_src matlab
  load('./mat/plant.mat', 'Gd');
#+end_src

#+begin_src matlab
  figure;
  ax1 = subplot(1, 2, 1);
  hold on;
  plot(uh.t(1:2*fs), 1e6*uh.Vph(1:2*fs)/freqresp(Gd(1,1), 0), 'DisplayName', '$\theta_{h}$');
  plot(uh.t(1:2*fs), 1e6*uh.Vpv(1:2*fs)/freqresp(Gd(2,2), 0), 'DisplayName', '$\theta_{v}$');
  hold off;
  xlabel('Time [s]'); ylabel('$\theta$ [$\mu$ rad]');
  title('Newport Tilt - Horizontal Direction');
  legend();

  ax2 = subplot(1, 2, 2);
  hold on;
  plot(uv.t(1:2*fs), 1e6*uv.Vph(1:2*fs)/freqresp(Gd(1,1), 0), 'DisplayName', '$\theta_{h}$');
  plot(uv.t(1:2*fs), 1e6*uv.Vpv(1:2*fs)/freqresp(Gd(2,2), 0), 'DisplayName', '$\theta_{v}$');
  hold off;
  xlabel('Time [s]'); ylabel('$\theta$ [$\mu$ rad]');
  title('Newport Tilt - Vertical Direction');
  legend();

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

Let's compute the PSD of the error to see the frequency content.

#+begin_src matlab
  [psd_UhRh, f] = pwelch(uh.Vph/freqresp(Gd(1,1), 0), hanning(ceil(1*fs)), [], [], fs);
  [psd_UhRv, ~] = pwelch(uh.Vpv/freqresp(Gd(2,2), 0), hanning(ceil(1*fs)), [], [], fs);
  [psd_UvRh, ~] = pwelch(uv.Vph/freqresp(Gd(1,1), 0), hanning(ceil(1*fs)), [], [], fs);
  [psd_UvRv, ~] = pwelch(uv.Vpv/freqresp(Gd(2,2), 0), hanning(ceil(1*fs)), [], [], fs);
#+end_src

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(1, 2, 1);
  hold on;
  plot(f, sqrt(psd_UhRh), 'DisplayName', '$\Gamma_{\theta_h}$');
  plot(f, sqrt(psd_UhRv), 'DisplayName', '$\Gamma_{\theta_v}$');
  hold off;
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{rad}{\sqrt{Hz}}\right]$')
  legend('Location', 'southwest');
  title('Newport Tilt - Horizontal Direction');

  ax2 = subplot(1, 2, 2);
  hold on;
  plot(f, sqrt(psd_UvRh), 'DisplayName', '$\Gamma_{\theta_h}$');
  plot(f, sqrt(psd_UvRv), 'DisplayName', '$\Gamma_{\theta_v}$');
  hold off;
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{rad}{\sqrt{Hz}}\right]$')
  legend('Location', 'southwest');
  title('Newport Tilt - Vertical Direction');

  linkaxes([ax1,ax2],'xy');
  xlim([1, 1000]);
#+end_src

Let's convert that to errors in distance

\[ \Delta L = L^\prime - L = \frac{L}{\cos(\alpha)} - L \approx \frac{L \alpha^2}{2} \]
with
- $L$ is the nominal distance traveled by the beam
- $L^\prime$ is the distance traveled by the beam with an angle error
- $\alpha$ is the angle error

#+begin_src matlab
  L = 0.1; % [m]
#+end_src

#+begin_src matlab
  [psd_UhLh, f] = pwelch(0.5*L*(uh.Vph/freqresp(Gd(1,1), 0)).^2, hanning(ceil(1*fs)), [], [], fs);
  [psd_UhLv, ~] = pwelch(0.5*L*(uh.Vpv/freqresp(Gd(2,2), 0)).^2, hanning(ceil(1*fs)), [], [], fs);
  [psd_UvLh, ~] = pwelch(0.5*L*(uv.Vph/freqresp(Gd(1,1), 0)).^2, hanning(ceil(1*fs)), [], [], fs);
  [psd_UvLv, ~] = pwelch(0.5*L*(uv.Vpv/freqresp(Gd(2,2), 0)).^2, hanning(ceil(1*fs)), [], [], fs);
#+end_src

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(1, 2, 1);
  hold on;
  plot(f, sqrt(psd_UhLh), 'DisplayName', '$\Gamma_{L_h}$');
  plot(f, sqrt(psd_UhLv), 'DisplayName', '$\Gamma_{L_v}$');
  hold off;
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{m}{\sqrt{Hz}}\right]$')
  legend('Location', 'southwest');
  title('Newport Tilt - Horizontal Direction');

  ax2 = subplot(1, 2, 2);
  hold on;
  plot(f, sqrt(psd_UvLh), 'DisplayName', '$\Gamma_{L_h}$');
  plot(f, sqrt(psd_UvLv), 'DisplayName', '$\Gamma_{L_v}$');
  hold off;
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{m}{\sqrt{Hz}}\right]$')
  legend('Location', 'southwest');
  title('Newport Tilt - Vertical Direction');

  linkaxes([ax1,ax2],'xy');
  xlim([1, 1000]);
#+end_src

Now, compare that with the PSD of the measured distance by the interferometer.
#+begin_src matlab
  [psd_Lh, f] = pwelch(uh.Va, hanning(ceil(1*fs)), [], [], fs);
  [psd_Lv, ~] = pwelch(uv.Va, hanning(ceil(1*fs)), [], [], fs);
#+end_src

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(1, 2, 1);
  hold on;
  plot(f, sqrt(psd_UhLh), 'DisplayName', '$\Gamma_{L_h}$');
  plot(f, sqrt(psd_UhLv), 'DisplayName', '$\Gamma_{L_v}$');
  plot(f, sqrt(psd_Lh), '--k', 'DisplayName', '$\Gamma_{L_h}$');
  hold off;
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{m}{\sqrt{Hz}}\right]$')
  legend('Location', 'southwest');
  title('Newport Tilt - Horizontal Direction');

  ax2 = subplot(1, 2, 2);
  hold on;
  plot(f, sqrt(psd_UvLh), 'DisplayName', '$\Gamma_{L_h}$');
  plot(f, sqrt(psd_UvLv), 'DisplayName', '$\Gamma_{L_v}$');
  plot(f, sqrt(psd_Lv), '--k', 'DisplayName', '$\Gamma_{L_h}$');
  hold off;
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('ASD $\left[\frac{m}{\sqrt{Hz}}\right]$')
  legend('Location', 'southwest');
  title('Newport Tilt - Vertical Direction');

  linkaxes([ax1,ax2],'xy');
  xlim([1, 1000]);
#+end_src

** Processing
First, we get the mean value as measured by the interferometer for each value of the Newport angle.
#+begin_src matlab
  Vahm = mean(reshape(uh.Va, [fs floor(length(uh.t)/fs)]),2);
  Unhm = mean(reshape(uh.Unh, [fs floor(length(uh.t)/fs)]),2);

  Vavm = mean(reshape(uv.Va, [fs floor(length(uv.t)/fs)]),2);
  Unvm = mean(reshape(uv.Unv, [fs floor(length(uv.t)/fs)]),2);
#+end_src

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(1, 2, 1);
  hold on;
  plot(uh.Unh, uh.Va);
  plot(Unhm, Vahm)
  hold off;
  xlabel('$V_{n,h}$ [V]'); ylabel('$V_a$ [m]');

  ax2 = subplot(1, 2, 2);
  hold on;
  plot(uv.Unv, uv.Va);
  plot(Unvm, Vavm)
  hold off;
  xlabel('$V_{n,v}$ [V]'); ylabel('$V_a$ [m]');
#+end_src

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(1, 2, 1);
  hold on;
  plot(uh.Unh, 1e9*(uh.Va - repmat(Vahm, length(uh.t)/length(Vahm),1)));
  hold off;
  xlabel('$V_{n,h}$ [V]'); ylabel('$V_a$ [nm]');

  ax2 = subplot(1, 2, 2);
  hold on;
  plot(uv.Unv, 1e9*(uv.Va - repmat(Vavm, length(uv.t)/length(Vavm),1)));
  hold off;
  xlabel('$V_{n,v}$ [V]'); ylabel('$V_a$ [nm]');

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

We then subtract
#+begin_src matlab
  figure;
  hold on;
  plot(uh.Unh, 1e9*(uh.Va - repmat(Vam, length(uh.t)/length(Vam),1)))
  hold off;
  xlabel('$V_{n,h}$ [V]'); ylabel('$V_a$ [nm]');
#+end_src

** Some Plots
#+begin_src matlab
  figure;
  hold on;
  plot(uh.Unh, uh.Va);
  plot(Unhm, Vam)
  hold off;
  xlabel('$V_{n,h}$ [V]'); ylabel('$V_a$ [m]');
#+end_src

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(1, 2, 1);
  hold on;
  plot(uh.Vnh(1:fs/2), uh.Va(1:fs/2));
  hold off;
  xlabel('$V_{n,h}$ [V]'); ylabel('$V_a$ [m]');

  ax2 = subplot(1, 2, 2);
  hold on;
  plot(uv.Vnv, uv.Va);
  hold off;
  xlabel('$V_{n,v}$ [V]'); ylabel('$V_a$ [m]');
#+end_src


#+begin_src matlab :exports none
  figure;
  ax1 = subplot(1, 2, 1);
  hold on;
  plot(uh.Vnh, uh.Va);
  hold off;
  xlabel('$V_{n,h}$ [V]'); ylabel('$V_a$ [m]');

  ax2 = subplot(1, 2, 2);
  hold on;
  plot(uv.Vnv, uv.Va);
  hold off;
  xlabel('$V_{n,v}$ [V]'); ylabel('$V_a$ [m]');
#+end_src

** Repeatability
#+begin_src matlab :exports none
  figure;
  ax1 = subplot(1, 2, 1);
  hold on;
  plot(Vnh, Va);
  hold off;
  xlabel('$V_{n,h}$ [V]'); ylabel('$V_a$ [m]');

  ax2 = subplot(1, 2, 2);
  hold on;
  plot(Vnv, Va);
  hold off;
  xlabel('$V_{n,v}$ [V]'); ylabel('$V_a$ [m]');
#+end_src


#+begin_src matlab
  bh = [ones(size(Vnh)) Vnh]\Vph;
  bv = [ones(size(Vnv)) Vnv]\Vpv;
#+end_src

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(1, 2, 1);
  hold on;
  plot(2*gn0*uh.Vnh, uh.Vph, 'o', 'DisplayName', 'Exp. data');
  plot(2*gn0*[min(uh.Vnh) max(uh.Vnh)], 2*gn0*[min(uh.Vnh) max(uh.Vnh)].*bh(2) + bh(1), 'k--', 'DisplayName', sprintf('%.1e x + %.1e', bh(2), bh(1)))
  hold off;
  xlabel('$\alpha_{0,h}$ [rad]'); ylabel('$Vp_h$ [V]');
  legend();

  ax2 = subplot(1, 2, 2);
  hold on;
  plot(2*gn0*uv.Vnv, uv.Vpv, 'o', 'DisplayName', 'Exp. data');
  plot(2*gn0*[min(uv.Vnv) max(uv.Vnv)], 2*gn0*[min(uv.Vnv) max(uv.Vnv)].*bv(2) + bv(1), 'k--', 'DisplayName', sprintf('%.1e x + %.1e', bv(2), bv(1)))
  hold off;
  xlabel('$\alpha_{0,v}$ [rad]'); ylabel('$Vp_v$ [V]');
  legend();
#+end_src