#+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] (Amp) at (GiGc|-Gn) {$G_a$};

    \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}\tilde{U}_{c,h} \\ \tilde{U}_{c,v}\end{bmatrix}$};
    \draw[->] (Gc.east) -- (add.west);
    \draw[->] (Gn.south) -- (add.north);
    \draw[->] (GiGc)node[branch]{} -- (Amp.south);
    \draw[->] (Amp.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]{$/$} ++(1, 0);
  \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 voltage across the Cercalo inductors
  \[ G_i = \begin{bmatrix} G_{i,h} & 0 \\ 0 & G_{i,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} \]
- *Cercalo*: Transfer function from the Voltage across 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} \]
- *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} \]

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] (Amp) at (GiGc|-Gn) {$\begin{bmatrix} G_{a,h} & 0 \\ 0 & G_{a,v} \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}\tilde{U}_{c,h} \\ \tilde{U}_{c,v}\end{bmatrix}$};
    \draw[->] (Gc.east) -- (add.west);
    \draw[->] (Gn.south) -- (add.north);
    \draw[->] (GiGc)node[branch]{} -- (Amp.south);
    \draw[->] (Amp.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]{$/$} ++(1, 0);
  \end{tikzpicture}
#+end_src

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

** Amplifier for the Cercalo
#+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[->] ($(B)+(0.8, 0)$) -- node[midway, right]{$V_c$} ($(A)+(0.8, 0)$);
  \end{circuitikz}
#+end_src

#+RESULTS:
[[file:figs/cercalo_amplifier.png]]

The value of the resistor in series with the buffer have been measured for both axis.
- $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$

We want to find the transfer function from $U_c$ to $V_L$ and from $U_c$ to $i_c$.

We have that:
\begin{align*}
  V_C &= R_c i + L_c s i \\
  U_c &= (R + R_c) i + L_c s i
\end{align*}

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

And
\begin{align}
  \frac{V_c}{U_c} &= \frac{R_c + L_c s}{(R + R_c) + L_c s} \\
                  &= \frac{\frac{R_c}{R + R_c} + \frac{L_c}{R + R_c} s}{1 + \frac{L_c}{R + R_c} s} \\
                  &= \frac{G_0 + s/\omega_0}{1 + s/\omega_0} \\
\end{align}
with
- $G_0 = \frac{R_c}{R + R_c}$
- $\omega_0 = \frac{R + R_c}{L_c}$

Let's verify that the electrical circuit behaves as a constant current amplifier in the frequency band of interest.

#+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_src matlab
  Gih = 1/(Rh + Rch + Lch * s);
  Gvh = (Rch + Lch * s)/(Rh + Rch + Lch * s);

  Giv = 1/(Rv + Rcv + Lcv * s);
  Gvv = (Rcv + Lcv * s)/(Rv + Rcv + Lcv * s);
#+end_src

#+begin_src matlab
  Gih0 = freqresp(Gih, 0);
  Gvh0 = freqresp(Gvh, 0);
  Giv0 = freqresp(Giv, 0);
  Gvv0 = freqresp(Gvv, 0);
#+end_src

#+begin_src matlab :exports none
  freqs = logspace(2, 5, 1000);

  figure;

  ax1 = subplot(1, 2, 1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gvh, freqs, 'Hz'))), 'DisplayName', ['$\frac{\tilde{U}_{c,h}}{U_{c,h}}$, ', sprintf('$G_{v,h}(0)=%.3f V/V$', Gvh0)])
  plot(freqs, abs(squeeze(freqresp(Gvv, freqs, 'Hz'))), 'DisplayName', ['$\frac{\tilde{U}_{c,v}}{U_{c,v}}$, ', sprintf('$G_{v,v}(0)=%.3f V/V$', Gvv0)])
  hold off;
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Amplitude [V/V]');
  legend('location', 'northwest');

  ax2 = subplot(1, 2, 2);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gih, freqs, 'Hz'))), 'DisplayName', ['$\frac{i_{c,h}}{U_{c,h}}$, ', sprintf('$G_{i,h}(0) = %.3f A/V$', Gih0)])
  plot(freqs, abs(squeeze(freqresp(Giv, freqs, 'Hz'))), 'DisplayName', ['$\frac{i_{c,v}}{U_{c,v}}$, ', sprintf('$G_{i,v}(0) = %.3f A/V$', Giv0)])
  hold off;
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Amplitude [A/V]');
  legend('location', 'southwest');

  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/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
The current amplifier has a constant gain over all the frequency band of interest.
\[ G_i(s) = \begin{bmatrix} 0.02 & 0 \\ 0 & 0.02 \end{bmatrix}\quad \left[\frac{A}{V}\right] \]
\[ G_a(s) = \begin{bmatrix} 0.185 & 0 \\ 0 & 0.168 \end{bmatrix} \left[\frac{V}{V}\right] \]
#+end_important

** 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
** 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
:PROPERTIES:
:header-args:matlab+: :tangle matlab/plant_identification.m
:header-args:matlab+: :comments org :mkdirp yes
:END:
<<sec:plant_identification>>
** Introduction                                                     :ignore:

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

#+begin_note
  All the files (data and Matlab scripts) are accessible [[file:data/plant_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

** Excitation Data
#+begin_src matlab
  fs = 1e4;
  Ts = 1/fs;
#+end_src

We generate white noise with the "random number" simulink block, and we filter that noise.

#+begin_src matlab
  Gi = (1)/(1+s/2/pi/100);
#+end_src

#+begin_src matlab :results output replace
  c2d(Gi, Ts, 'tustin')
#+end_src

#+RESULTS:
#+begin_example
c2d(Gi, Ts, 'tustin')

ans =

  0.030459 (z+1)
  --------------
    (z-0.9391)

Sample time: 0.0001 seconds
Discrete-time zero/pole/gain model.
#+end_example

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

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

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

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(t, Vph, 'DisplayName', '$Vp_h$');
  plot(t, Vpv, '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 :results 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 :results 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

** Input / Output data
The identification data is loaded
#+begin_src matlab
  uh = load('mat/data_uh.mat', ...
            't', 'Uch', 'Ucv', ...
            'Unh', 'Unv', ...
            'Vph', 'Vpv', ...
            'Vch', 'Vcv', ...
            'Vnh', 'Vnv', ...
            'Va');

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

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

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

  uh.t = uh.t - uh.t(1); % We start at t=0

  t0 = 1;

  uv.Uch(t<t0) = [];
  uv.Ucv(t<t0) = [];
  uv.Unh(t<t0) = [];
  uv.Unv(t<t0) = [];
  uv.Vph(t<t0) = [];
  uv.Vpv(t<t0) = [];
  uv.Vch(t<t0) = [];
  uv.Vcv(t<t0) = [];
  uv.Vnh(t<t0) = [];
  uv.Vnv(t<t0) = [];
  uv.Va(t<t0)  = [];
  uv.t(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]]

** Estimation of the Frequency Response Function Matrix
#+begin_src matlab
  win = hanning(ceil(1*fs));
#+end_src

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')
  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')
  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/frequency_response_matrix.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

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

** Coherence
#+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/identification_coherence.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

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


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

From the number of resonance frequency we want to fit, we define the order =N= of the system we want to obtain.
#+begin_src matlab
  N = 2;
#+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.
#+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)
    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
#+end_src

Ignore data above some frequency.
#+begin_src matlab
  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

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(f, weight_Uch_Vph);
  plot(f, weight_Uch_Vpv);
  plot(f, weight_Ucv_Vph);
  plot(f, weight_Ucv_Vpv);
  hold off;
  xlabel('Frequency [Hz]');
  xlabel('Weight Amplitude');
  set(gca, 'xscale', 'log');
  xlim([f(1), f(end)]);
#+end_src

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

#+NAME: fig:weights
#+CAPTION: Weights amplitude ([[./figs/weights.png][png]], [[./figs/weights.pdf][pdf]])
[[file:figs/weights.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')
  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')
  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 state space model:
#+begin_src matlab
  G_uh_xh = minreal(ss(full(SER_uh_xh.A),SER_uh_xh.B,SER_uh_xh.C,SER_uh_xh.D));
  G_uv_xh = minreal(ss(full(SER_uv_xh.A),SER_uv_xh.B,SER_uv_xh.C,SER_uv_xh.D));
  G_uh_xv = minreal(ss(full(SER_uh_xv.A),SER_uh_xv.B,SER_uh_xv.C,SER_uh_xv.D));
  G_uv_xv = minreal(ss(full(SER_uv_xv.A),SER_uv_xv.B,SER_uv_xv.C,SER_uv_xv.D));

  G = [G_uh_xh, G_uv_xh;
       G_uh_xv, G_uv_xv];
#+end_src

#+begin_src matlab
  save('mat/plant.mat', 'G');
#+end_src

* Plant Scaling
- measured noise
- expected perturbations
- maximum input usage
- maximum wanted error

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

* Control Design
* Measurement of the non-repeatability