#+TITLE: Control Requirements
:DRAWER:
#+STARTUP: overview

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

#+HTML_LINK_HOME: ./index.html
#+HTML_LINK_UP: ./index.html

#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./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>

#+HTML_MATHJAX: align: center tagside: right font: TeX

#+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 no
#+PROPERTY: header-args:matlab+ :mkdirp yes

#+PROPERTY: header-args:shell  :eval no-export

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

* Introduction                                                        :ignore:
The goal here is to write clear specifications for the NASS.

This can then be used for the control synthesis and for the design of the nano-hexapod.

Ideal, specifications on the norm of closed loop transfer function should be written.

* Simplify Model for the Nano-Hexapod
** Model of the nano-hexapod
Let's consider the simple mechanical system in Figure [[fig:nass_simple_model]].

#+begin_src latex :file nass_simple_model.pdf
  \begin{tikzpicture}
    % Parameters
    \def\massw{3}
    \def\massh{1}
    \def\spaceh{2}

    % Ground
    \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){$m$};

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

    % Force Sensor
    \node[forcesensor={\massw}{0.2}] (fsens) at (0, \spaceh){};
    \node[right] at (fsens.east) {$F_m$};

    % Displacements
    \draw[dashed] (0.5*\massw, 0) -- ++(0.2*\massw, 0);
    \draw[->] (0.6*\massw, 0) -- ++(0, 0.2*\spaceh) node[below right]{$x_\mu$};

    % Displacements
    \draw[dashed] (0.5*\massw, \spaceh+\massh) -- ++(0.2*\massw, 0);
    \draw[->] (0.6*\massw, \spaceh+\massh) -- ++(0, 0.2*\spaceh) node[below right]{$x$};

    \draw[<->] (-0.5*\massw+-1.5*\dispw, 0) -- node[midway, left]{$d$} ++(0, \spaceh);

    % Direct forces
    \draw[->] (0, \spaceh+\massh) node[branch]{} -- ++(0, 0.4*\spaceh) node[below right]{$F_d$};
  \end{tikzpicture}
#+end_src

#+name: fig:nass_simple_model
#+caption: Simplified mechanical system for the nano-hexapod
#+RESULTS:
[[file:figs/nass_simple_model.png]]

The signals are described in table [[tab:general_plant_signals]].

#+name: tab:general_plant_signals
#+caption: Signals definition for the generalized plant
|                     | *Symbol* | *Meaning*                              |
|---------------------+----------+----------------------------------------|
| *Exogenous Inputs*  | $x_\mu$  | Motion of the $\nu$-hexapod's base     |
|                     | $F_d$    | External Forces applied to the Payload |
|                     | $r$      | Reference signal for tracking          |
|---------------------+----------+----------------------------------------|
| *Exogenous Outputs* | $x$      | Absolute Motion of the Payload         |
|---------------------+----------+----------------------------------------|
| *Sensed Outputs*    | $F_m$    | Force Sensors in each leg              |
|                     | $d$      | Measured displacement of each leg      |
|                     | $x$      | Absolute Motion of the Payload         |
|---------------------+----------+----------------------------------------|
| *Control Signals*   | $F$      | Actuator Inputs                        |

For the nano-hexapod alone, we have the following equations:
\[ \begin{align*}
  x   &= \frac{1}{ms^2 + k}    F + \frac{1}{ms^2 + k} F_d + \frac{k}{ms^2 + k} x_\mu \\
  F_m &= \frac{ms^2}{ms^2 + k} F - \frac{k}{ms^2 + k} F_d + \frac{k m s^2}{ms^2 + k} x_\mu \\
  d   &= \frac{1}{ms^2 + k}    F + \frac{1}{ms^2 + k} F_d - \frac{ms^2}{ms^2 + k} x_\mu
\end{align*} \]

We can write the equations function of $\omega_\nu = \sqrt{\frac{k}{m}}$:
\[ \begin{align*}
  x   &= \frac{1/k}{1 + \frac{s^2}{\omega_\nu^2}}    F + \frac{1/k}{1 + \frac{s^2}{\omega_\nu^2}}    F_d + \frac{1}{1 + \frac{s^2}{\omega_\nu^2}} x_\mu \\
  F_m &= \frac{\frac{s^2}{\omega_\nu^2}}{1 + \frac{s^2}{\omega_\nu^2}} F - \frac{1}{1 + \frac{s^2}{\omega_\nu^2}} F_d + \frac{k \frac{s^2}{\omega_\nu^2}}{1 + \frac{s^2}{\omega_\nu^2}} x_\mu \\
  d   &= \frac{1/k}{1 + \frac{s^2}{\omega_\nu^2}}    F + \frac{1/k}{1 + \frac{s^2}{\omega_\nu^2}}    F_d - \frac{\frac{s^2}{\omega_\nu^2}}{1 + \frac{s^2}{\omega_\nu^2}} x_\mu
\end{align*} \]


*Assumptions*:
- the forces applied by the nano-hexapod have no influence on the micro-station, specifically on the displacement of the top platform of the micro-hexapod.

This means that the nano-hexapod can be considered separately from the micro-station and that the motion $x_\mu$ is imposed and considered as an external input.

The nano-hexapod can thus be represented as in Figure [[fig:nano_station_inputs_outputs]].

#+begin_src latex :file nano_station_inputs_outputs.pdf
  \begin{tikzpicture}
    \node[block={2.0cm}{2.0cm}] (P) {};
    \node[above] at (P.north) {$\nu$-hexapod};

    % Input and outputs coordinates
    \coordinate[] (inputFd) at ($(P.south west)!0.8!(P.north west)$);
    \coordinate[] (inputw)  at ($(P.south west)!0.5!(P.north west)$);
    \coordinate[] (inputF)  at ($(P.south west)!0.2!(P.north west)$);
    \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
    \coordinate[] (outputd) at ($(P.south east)!0.5!(P.north east)$);
    \coordinate[] (outputx) at ($(P.south east)!0.2!(P.north east)$);

    % Connections and labels
    \draw[<-] (inputFd) -- ++(-0.8, 0) node[above right]{$F_d$};
    \draw[<-] (inputw)  -- ++(-0.8, 0) node[above right]{$x_\mu$};
    \draw[<-] (inputF)  -- ++(-0.8, 0) node[above right]{$F$};

    \draw[->] (outputF) -- ++(0.8, 0) node[above left]{$F_m$};
    \draw[->] (outputd) -- ++(0.8, 0) node[above left]{$d$};
    \draw[->] (outputx) -- ++(0.8, 0) node[above left]{$x$};
  \end{tikzpicture}
#+end_src

#+name: fig:nano_station_inputs_outputs
#+caption: Block representation of the nano-hexapod

** How to include Ground Motion in the model?
What we measure is not the absolute motion $x$, but the relative motion $x - w$ where $w$ is the motion of the granite.

Also, $w$ induces some motion $x_\mu$ through the transmissibility of the micro-station.

#+begin_src latex :file nano_station_inputs_outputs_ground_motion.pdf
  \begin{tikzpicture}
    \node[block={3.0cm}{3.0cm}] (P) {};
    \node[above] at (P.north) {$\nu$-hexapod};

    % Input and outputs coordinates
    \coordinate[] (inputFd) at ($(P.south west)!0.95!(P.north west)$);
    \coordinate[] (inputw)  at ($(P.south west)!0.65!(P.north west)$);
    \coordinate[] (inputxm) at ($(P.south west)!0.35!(P.north west)$);
    \coordinate[] (inputF)  at ($(P.south west)!0.05!(P.north west)$);
    \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
    \coordinate[] (outputd) at ($(P.south east)!0.5!(P.north east)$);
    \coordinate[] (outputx) at ($(P.south east)!0.2!(P.north east)$);

    % Connections and labels
    \draw[<-] (inputFd) -- ++(-1.4, 0) node[above right]{$F_d$};
    \draw[<-] (inputw)  -- ++(-1.4, 0) node[above right]{$w$};
    \draw[<-] (inputxm) -- ++(-1.4, 0) node[above right]{$x_\mu$};
    \draw[<-] (inputF)  -- ++(-1.4, 0) node[above right]{$F$};

    \draw[->] (outputF) -- ++(1.4, 0) node[above left]{$F_m$};
    \draw[->] (outputd) -- ++(1.4, 0) node[above left]{$d$};
    \draw[->] (outputx) -- ++(1.4, 0) node[above]{$y = x-w$};
  \end{tikzpicture}
#+end_src

** Motion of the micro-station
As explained, we consider $x_\mu$ as an external input ($F$ has no influence on $x_\mu$).

$x_\mu$ is the motion of the micro-station's top platform due to the motion of each stage of the micro-station.

We consider that $x_\mu$ has the following form:
\[ x_\mu = T_\mu r + d_\mu \]
where:
- $T_\mu r$ corresponds to the response of the stages due to the reference $r$
- $d_\mu$ is the motion of the hexapod due to all the vibrations of the stages


$T_\mu$ can be considered to be a low pass filter with a bandwidth corresponding approximatively to the bandwidth of the micro-station's stages.
To simplify, we can consider $T_\mu$ to be a first order low pass filter:
\[ T_\mu = \frac{1}{1 + s/\omega_\mu} \]
where $\omega_\mu$ corresponds to the tracking speed of the micro-station.


What is important to note is that while $x_\mu$ is viewed as a perturbation from the nano-hexapod point of view, $x_\mu$ *does* depend on the reference signal $r$.

Also, here, we suppose that the granite is not moving.

If we now include the motion of the granite $w$, we obtain the block diagram shown in Figure [[fig:nano_station_ground_motion]].

#+begin_src latex :file nano_station_ground_motion.pdf
  \begin{tikzpicture}
    \node[block={4.0cm}{4.0cm}] (P) {};
    \node[above] at (P.north) {$\nu$-hexapod};

    % Input and outputs coordinates
    \coordinate[] (inputFd) at ($(P.south west)!0.8!(P.north west)$);
    \coordinate[] (inputw)  at ($(P.south west)!0.5!(P.north west)$);
    \coordinate[] (inputF)  at ($(P.south west)!0.2!(P.north west)$);
    \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
    \coordinate[] (outputd) at ($(P.south east)!0.5!(P.north east)$);
    \coordinate[] (outputx) at ($(P.south east)!0.2!(P.north east)$);

    % Connections and labels
    \draw[<-] (inputFd) -- ++(-0.8, 0) node[above right]{$F_d$};
    \draw[<-] (inputF)  -- ++(-0.8, 0) node[above right]{$F$};

    \draw[->] (outputF) -- ++(0.8, 0) node[above left]{$F_m$};
    \draw[->] (outputd) -- ++(0.8, 0) node[above left]{$d$};

    \node[addb, left= of inputw] (add) {};
    \node[block, left= of add] (Tmu) {$T_\mu$};
    \node[addb, above= of add] (addw) {};
    \node[block, left= of addw] (Tw) {$T_w$};

    \node[addb={+}{}{-}{}{}, right= of outputx] (addx) {};
    \draw[->] (outputx) -- (addx.west) node[above left]{$x$};
    \draw[->] (addx.east) -- ++(0.8, 0) node[above left]{$y$};

    \draw[<-] (Tmu.west) -- ++(-0.8, 0)node[above right]{$r$};

    \draw[->] (Tmu.east) -- (add.west);
    \draw[->] (add.east) -- (inputw) node[above left]{$x_\mu$};
    \draw[->] ($(addw.north) + (0, 0.8)$) node[below right]{$d_\mu$} -- (addw.north);
    \draw[->] (addw.south) -- (add.north);

    \draw[->] ($(Tw.west) + (-1, 0)$) node[above right]{$w$} -- (Tw.west);
    \draw[->] ($(Tw.west) + (-0.4, 0)$) node[branch]{} -- ++(0, 1.4) -| (addx.north);
    \draw[->] (Tw.east) -- (addw.west);
  \end{tikzpicture}
#+end_src

#+name: fig:nano_station_ground_motion
#+caption: Ground Motion $w$ included
#+RESULTS:
[[file:figs/nano_station_ground_motion.png]]

$T_w$ is the mechanical transmissibility of the micro-station.
We can approximate this transfer function by a second order low pass filter:
\[ T_w = \frac{1}{1 + 2 \xi s/\omega_0 + s^2/\omega_0^2} \]

** Notes on the signals                                            :noexport:
Let's consider the reference tracking problem:

The reference $r$ is a sinusoidal signal at 1Hz with an amplitude up to 10mm.
We want to position error $\epsilon  = x - r$ to be less than 10nm.

Thus, if we don't take into account the fact that the perturbation $x_\mu$ does depend on the reference $r$, we would conclude that we need the sensitivity function of the nano-hexapod $S$:
\[ |S| < - 60dB\quad\text{at 1 Hz} \]
And thus we would need a bandwidth of approximatively 1kHz which is not realistic.

* Control with the Stiff Nano-Hexapod
** 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

** Definition of the values
Let's define the mass and stiffness of the nano-hexapod.
#+begin_src matlab
  m = 50; % [kg]
  k = 1e7; % [N/m]
#+end_src

Let's define the Plant as shown in Figure [[fig:nano_station_inputs_outputs]]:
#+begin_src matlab
  Gn = 1/(m*s^2 + k)*[-k, k*m*s^2, m*s^2; 1, -m*s^2, 1; 1, k, 1];
  Gn.InputName  = {'Fd', 'xmu', 'F'};
  Gn.OutputName = {'Fm', 'd', 'x'};
#+end_src

Now, define the transmissibility transfer function $T_\mu$ corresponding to the micro-station motion.
#+begin_src matlab
  wmu = 2*pi*50; % [rad/s]

  Tmu = 1/(1 + s/wmu);
  Tmu.InputName = {'r1'};
  Tmu.OutputName = {'ymu'};
#+end_src

#+begin_src matlab
  w0 = 2*pi*40;
  xi = 0.5;
  Tw = 1/(1 + 2*xi*s/w0 + s^2/w0^2);
  Tw.InputName = {'w1'};
  Tw.OutputName = {'dw'};
#+end_src

We add the fact that $x_\mu = d_\mu + T_\mu r + T_w w$:
#+begin_src matlab
  Wsplit = [tf(1); tf(1)];
  Wsplit.InputName = {'w'};
  Wsplit.OutputName = {'w1', 'w2'};

  S = sumblk('xmu = ymu + dmu + dw');

  Sw = sumblk('y = x - w2');

  Gpz = connect(Gn, S, Wsplit, Tw, Tmu, Sw, {'Fd', 'dmu', 'r1', 'F', 'w'}, {'Fm', 'd', 'y'});
#+end_src

** Control using $d$
*** Control Architecture
Let's consider a feedback loop using $d$ as shown in Figure [[fig:nano_station_control_d]].

#+begin_src latex :file nano_station_control_d.pdf
  \begin{tikzpicture}
    \node[block={4.0cm}{4.0cm}] (P) {};
    \node[above] at (P.north) {$\nu$-hexapod};

    % Input and outputs coordinates
    \coordinate[] (inputFd) at ($(P.south west)!0.8!(P.north west)$);
    \coordinate[] (inputw)  at ($(P.south west)!0.5!(P.north west)$);
    \coordinate[] (inputF)  at ($(P.south west)!0.2!(P.north west)$);
    \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
    \coordinate[] (outputd) at ($(P.south east)!0.5!(P.north east)$);
    \coordinate[] (outputx) at ($(P.south east)!0.2!(P.north east)$);

    % Connections and labels
    \draw[<-] (inputFd) -- ++(-0.8, 0) node[above right]{$F_d$};
    \draw[<-] (inputw)  -- ++(-0.8, 0) node[above right]{$x_\mu$};

    \draw[->] (outputF) -- ++(0.8, 0) node[above left]{$F_m$};
    \draw[->] (outputd) -- ++(1.6, 0) node[above left]{$d$};
    \draw[->] (outputx) -- ++(0.8, 0) node[above left]{$x$};

    \node[block, below=0.3 of P] (K) {$K_d$};

    \node[addb={+}{}{}{}{-}, left= of inputF] (addF) {};

    \draw[->] ($(outputd) + (1.1, 0)$) node[branch]{} |- (K.east);
    \draw[->] (K.west) -| (addF.south);
    \draw[->] (addF.east) -- (inputF) node[above left]{$F$};
    \draw[<-] (addF.west) -- ++(-0.8, 0)node[above right]{$F^\prime$};
  \end{tikzpicture}
#+end_src

#+name: fig:nano_station_control_d
#+caption: Feedback diagram using $d$
#+RESULTS:
[[file:figs/nano_station_control_d.png]]

*** Analytical Analysis
Let's apply a direct velocity feedback by deriving $d$:
\[ F = F^\prime - g s d \]

Thus:
\[ d = \frac{1}{ms^2 + gs + k} F^\prime + \frac{1}{ms^2 + gs + k} F_d - \frac{ms^2}{ms^2 + gs + k} x_\mu \]

\[ F = \frac{ms^2 + k}{ms^2 + gs + k} F^\prime - \frac{gs}{ms^2 + gs + k} F_d + \frac{mgs^3}{ms^2 + gs + k} x_\mu \]

and
\[ x = \frac{1}{ms^2 + k} (\frac{ms^2 + k}{ms^2 + gs + k} F^\prime - \frac{gs}{ms^2 + gs + k} F_d + \frac{mgs^3}{ms^2 + gs + k} x_\mu) + \frac{1}{ms^2 + k} F_d + \frac{k}{ms^2 + k} x_\mu \]


\[ x = \frac{ms^2 + k}{(ms^2 + k) (ms^2 + gs + k)} F^\prime + \frac{ms^2 + k}{(ms^2 + k) (ms^2 + gs + k)} F_d + \frac{mgs^3 + k(ms^2 + gs + k)}{(ms^2 + k) (ms^2 + gs + k)} x_\mu \]

And we finally obtain:
\[ x = \frac{1}{ms^2 + gs + k} F^\prime + \frac{1}{ms^2 + gs + k} F_d + \frac{gs + k}{ms^2 + gs + k} x_\mu \]

#+begin_src matlab
  K_dvf = 2*sqrt(k*m)*s;
  K_dvf.InputName = {'d'};
  K_dvf.OutputName = {'F'};

  Gpz_dvf = feedback(Gpz, K_dvf, 'name');
#+end_src

Now let's consider that $x_\mu = d_\mu + T_\mu r$

\[ x = \frac{1}{ms^2 + gs + k} F^\prime + \frac{1}{ms^2 + gs + k} F_d + \frac{gs + k}{ms^2 + gs + k} d_\mu + T_\mu \frac{gs + k}{ms^2 + gs + k} r \]

And $\epsilon = r - x$:
\[ \epsilon = \frac{1}{ms^2 + gs + k} F^\prime + \frac{1}{ms^2 + gs + k} F_d + \frac{gs + k}{ms^2 + gs + k} d_\mu + \frac{ms^2 + gs + k - T_\mu (gs + k)}{ms^2 + gs + k} r \]

#+begin_important
\[ \epsilon = \frac{1}{ms^2 + gs + k} F^\prime + \frac{1}{ms^2 + gs + k} F_d + \frac{gs + k}{ms^2 + gs + k} d_\mu + \frac{ms^2 - S_\mu(gs + k)}{ms^2 + gs + k} r \]
#+end_important

** Control using $F_m$
*** Control Architecture
Let's consider a feedback loop using $Fm$ as shown in Figure [[fig:nano_station_control_Fm]].

#+begin_src latex :file nano_station_control_Fm.pdf
  \begin{tikzpicture}
    \node[block={4.0cm}{4.0cm}] (P) {};
    \node[above] at (P.north) {$\nu$-hexapod};

    % Input and outputs coordinates
    \coordinate[] (inputFd) at ($(P.south west)!0.8!(P.north west)$);
    \coordinate[] (inputw)  at ($(P.south west)!0.5!(P.north west)$);
    \coordinate[] (inputF)  at ($(P.south west)!0.2!(P.north west)$);
    \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
    \coordinate[] (outputd) at ($(P.south east)!0.5!(P.north east)$);
    \coordinate[] (outputx) at ($(P.south east)!0.2!(P.north east)$);

    % Connections and labels
    \draw[<-] (inputFd) -- ++(-0.8, 0) node[above right]{$F_d$};
    \draw[<-] (inputw)  -- ++(-0.8, 0) node[above right]{$x_\mu$};

    \draw[->] (outputF) -- ++(1.6, 0) node[above left]{$F_m$};
    \draw[->] (outputd) -- ++(0.8, 0) node[above left]{$d$};
    \draw[->] (outputx) -- ++(0.8, 0) node[above left]{$x$};

    \node[block, below=0.3 of P] (K) {$K_{F_m}$};

    \node[addb={+}{}{}{}{-}, left= of inputF] (addF) {};

    \draw[->] ($(outputF) + (1.1, 0)$) node[branch]{} |- (K.east);
    \draw[->] (K.west) -| (addF.south);
    \draw[->] (addF.east) -- (inputF) node[above left]{$F$};
    \draw[<-] (addF.west) -- ++(-0.8, 0)node[above right]{$F^\prime$};
  \end{tikzpicture}
#+end_src

#+name: fig:nano_station_control_Fm
#+caption: Feedback diagram using $F_m$
#+RESULTS:
[[file:figs/nano_station_control_Fm.png]]

*** Pure Integrator
Let's apply integral force feedback by integration $F_m$:
\[ F = F^\prime - \frac{g}{s} F_m \]

And we finally obtain:
\[ x = \frac{1}{ms^2 + mgs + k} F^\prime + \frac{1 + \frac{g}{s}}{ms^2 + mgs + k} F_d + \frac{k}{ms^2 + mgs + k} x_\mu \]

#+begin_src matlab
  K_iff = 2*sqrt(k/m)/s;
  K_iff.InputName = {'Fm'};
  K_iff.OutputName = {'F'};

  Gpz_iff = feedback(Gpz, K_iff, 'name');
#+end_src

Now let's consider that $x_\mu = d_\mu + T_\mu r$

\[ x = \frac{1}{ms^2 + mgs + k} F^\prime + \frac{1 + \frac{g}{s}}{ms^2 + mgs + k} F_d + \frac{k}{ms^2 + mgs + k} d_\mu + \frac{T_\mu k}{ms^2 + mgs + k} r \]

And $\epsilon = r - x$:
\[ \epsilon = \frac{1}{ms^2 + mgs + k} F^\prime + \frac{1 + \frac{g}{s}}{ms^2 + mgs + k} F_d + \frac{k}{ms^2 + mgs + k} d_\mu + \frac{ms^2 + mgs + k - T_\mu k}{ms^2 + mgs + k} r \]

#+begin_important
\[ \epsilon = \frac{1}{ms^2 + mgs + k} F^\prime + \frac{1 + \frac{g}{s}}{ms^2 + mgs + k} F_d + \frac{k}{ms^2 + mgs + k} d_\mu + \frac{ms^2 + mgs + S_\mu k}{ms^2 + mgs + k} r \]
#+end_important

*** Low pass filter
Instead of a pure integrator, let's use a low pass filter with a cut-off frequency above the bandwidth of the micro-station $\omega_mu$
#+begin_src matlab
  % K_iff = (2*sqrt(k/m)/(2*wmu))*(1/(1 + s/(2*wmu)));
  % K_iff.InputName = {'Fm'};
  % K_iff.OutputName = {'F'};

  % Gpz_iff = feedback(Gpz, K_iff, 'name');
#+end_src

** Comparison
#+begin_src matlab :exports none
  figure;
  subplot(2, 2, 1);
  title('Effect of Ground motion ($\epsilon/w$)');
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gpz('y', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_iff('y', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_dvf('y', 'w'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
  xlim([freqs(1), freqs(end)]);

  subplot(2, 2, 2);
  title('Compliance ($\epsilon/F_d$)');
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gpz('y', 'Fd'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_iff('y', 'Fd'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_dvf('y', 'Fd'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
  xlim([freqs(1), freqs(end)]);

  subplot(2, 2, 3);
  title('Vibration Filtering ($\epsilon/d_\mu$)');
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gpz('y', 'dmu'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_iff('y', 'dmu'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_dvf('y', 'dmu'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
  xlim([freqs(1), freqs(end)]);

  subplot(2, 2, 4);
  title('Reference Tracking ($\epsilon/r$)');
  hold on;
  plot(freqs, abs(squeeze(freqresp(1 - Gpz('y', 'r'), freqs, 'Hz'))),            'DisplayName', 'OL' );
  plot(freqs, abs(squeeze(freqresp(1 - Gpz_iff('y', 'r'), freqs, 'Hz'))),        'DisplayName', 'IFF');
  plot(freqs, abs(squeeze(freqresp(1 - Gpz_dvf('y', 'r'), freqs, 'Hz'))),        'DisplayName', 'DVF');
  plot(freqs, abs(squeeze(freqresp(1 - Tmu, freqs, 'Hz'))),             'k--', 'DisplayName', '$S_\mu$');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
  legend('location', 'northwest');
  xlim([freqs(1), freqs(end)]);
#+end_src

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

#+name: fig:comp_iff_dvf_simplified
#+caption: Obtained transfer functions for DVF and IFF ([[./figs/comp_iff_dvf_simplified.png][png]], [[./figs/comp_iff_dvf_simplified.pdf][pdf]])
[[file:figs/comp_iff_dvf_simplified.png]]

|     | $d_\mu$                            | $F_d$                             | $w$                                  |
|-----+------------------------------------+-----------------------------------+--------------------------------------|
| IFF | Better filtering of the vibrations | More sensitive to External forces |                                      |
| DVF | inverse                            | inverse                           | Little bit better at low frequencies |

** Control using $x$
*** Analytical analysis
Let's first consider that only the output $x$ is used for feedback (Figure [[fig:nano_station_control_x]])

#+begin_src latex :file nano_station_control_x.pdf
  \begin{tikzpicture}
    \node[block={4.0cm}{4.0cm}] (P) {};
    \node[above] at (P.north) {$\nu$-hexapod};

    % Input and outputs coordinates
    \coordinate[] (inputFd) at ($(P.south west)!0.8!(P.north west)$);
    \coordinate[] (inputw)  at ($(P.south west)!0.5!(P.north west)$);
    \coordinate[] (inputF)  at ($(P.south west)!0.2!(P.north west)$);
    \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
    \coordinate[] (outputd) at ($(P.south east)!0.5!(P.north east)$);
    \coordinate[] (outputx) at ($(P.south east)!0.2!(P.north east)$);

    % Connections and labels
    \draw[<-] (inputFd) -- ++(-0.8, 0) node[above right]{$F_d$};

    \draw[->] (outputF) -- ++(0.8, 0) node[above left]{$F_m$};
    \draw[->] (outputd) -- ++(0.8, 0) node[above left]{$d$};
    \draw[->] (outputx) -- ++(0.8, 0) node[above left]{$x$};

    \node[block, left= of inputF] (K) {$K$};
    \node[addb={+}{}{}{}{-}, left= of K] (addfb) {};
    \node[addb, left= of inputw] (add) {};
    \node[block, left= of add] (Tmu) {$T_\mu$};

    \draw[->] ($(outputx) + (0.3, 0)$) node[branch]{} -- ++(0, -1) -| (addfb.south);
    \draw[->] (addfb.east) -- (K.west) node[above left]{$\epsilon$};
    \draw[->] (K.east) -- (inputF) node[above left]{$F$};

    \draw[->] ($(addfb.west) + (-1.6, 0)$)node[above right]{$r$} -- (addfb.west);

    \draw[->] ($(addfb.west) + (-0.8, 0)$)node[branch]{} |- (Tmu.west);
    \draw[->] (Tmu.east) -- (add.west);
    \draw[->] (add.east) -- (inputw) node[above left]{$x_\mu$};
    \draw[->] ($(add.north) + (0, 0.8)$) node[below right]{$d_\mu$} -- (add.north);
  \end{tikzpicture}
#+end_src

#+name: fig:nano_station_control_x
#+caption: Feedback diagram using $x$
#+RESULTS:
[[file:figs/nano_station_control_x.png]]

We then have:
\[ \epsilon &= r - G_{\frac{x}{F}} K \epsilon - G_{\frac{x}{F_d}} F_d - G_{\frac{x}{x_\mu}} d_\mu - G_{\frac{x}{x_\mu}} T_\mu r \]

And then:
#+begin_important
\[ \epsilon = \frac{-G_{\frac{x}{F_d}}}{1 + G_{\frac{x}{F}}K} F_d +  \frac{-G_{\frac{x}{x_\mu}}}{1 + G_{\frac{x}{F}}K} d_\mu + \frac{1 - G_{\frac{x}{x_\mu}} T_\mu}{1 + G_{\frac{x}{F}}K} r \]
#+end_important

With $S = \frac{1}{1 + G_{\frac{x}{F}} K}$, we have:
\[ \epsilon = - S G_{\frac{x}{F_d}} F_d - S G_{\frac{x}{x_\mu}} d_\mu + S (1 - G_{\frac{x}{x_\mu}} T_\mu) r \]

We have 3 terms that we would like to have small by design:
- $G_{\frac{x}{F_d}} = \frac{1}{ms^2 + k}$: thus $k$ and $m$ should be high to lower the effect of direct forces $F_d$
- $G_{\frac{x}{x_\mu}} = \frac{k}{ms^2 + k} = \frac{1}{1 + \frac{s^2}{\omega_\nu^2}}$: $\omega_\nu$ should be small enough such that it filters out the vibrations of the micro-station
- $1 - G_{\frac{x}{x_\mu}} T_\mu$

\[ 1 - G_{\frac{x}{x_\mu}} T_\mu = 1 - \frac{1}{1 + \frac{s^2}{\omega_\nu^2}} T_\mu \]

We can approximate $T_\mu \approx \frac{1}{1 + \frac{s}{\omega_\mu}}$ to have:
\begin{align*}
 1 - G_{\frac{x}{x_\mu}} T_\mu &= 1 - \frac{1}{1 + \frac{s^2}{\omega_\nu^2}} \frac{1}{1 + \frac{s}{\omega_\mu}} \\
                               &\approx \frac{\frac{s}{\omega_\mu}}{1 + \frac{s}{\omega_\mu}} = S_\mu \text{ if } \omega_\nu > \omega_\mu \\
                               &\approx \frac{\frac{s^2}{\omega_\nu^2}}{1 + \frac{s^2}{\omega_\nu^2}} =  \text{ if } \omega_\nu < \omega_\mu
\end{align*}

In our case, we have $\omega_\nu > \omega_\mu$ and thus we cannot lower this term.

Some implications on the design are summarized on table [[tab:design_decommentation]].

#+name: tab:design_decommentation
#+caption: Design recommendation
| Exogenous Outputs | Design recommendation                     |
|-------------------+-------------------------------------------|
| $F_d$             | high $k$, high $m$                        |
| $d_\mu$           | low $k$, high $m$                         |
| $r$               | no influence if $\omega_\nu > \omega_\mu$ |

*** Control implementation
Controller for the damped plant using DVF.
#+begin_src matlab
  wb = 2*pi*50; % control bandwidth [rad/s]

  % Lead
  h = 2.0;
  wz = wb/h; % [rad/s]
  wp = wb*h; % [rad/s]

  H = 1/h*(1 + s/wz)/(1 + s/wp);

  % Integrator until 10Hz
  Hi = (1 + s/2/pi/10)/(s/2/pi/10);

  K = Hi*H*(1/s);

  Kpz_dvf = K/abs(freqresp(K*Gpz_dvf('y', 'F'), wb));
  Kpz_dvf.InputName = {'e'};
  Kpz_dvf.OutputName = {'Fi'};
#+end_src

Controller for the damped plant using IFF.
#+begin_src matlab
  wb = 2*pi*50; % control bandwidth [rad/s]

  % Lead
  h = 2.0;
  wz = wb/h; % [rad/s]
  wp = wb*h; % [rad/s]

  H = 1/h*(1 + s/wz)/(1 + s/wp);

  % Integrator until 10Hz
  Hi = (1 + s/2/pi/10)/(s/2/pi/10);

  K = Hi*H*(1/s);

  Kpz_iff = K/abs(freqresp(K*Gpz_iff('y', 'F'), wb));
  Kpz_iff.InputName = {'e'};
  Kpz_iff.OutputName = {'Fi'};
#+end_src

Loop gain
#+begin_src matlab :exports none
  figure;
  title('Transfer function from $F^\prime$ to $\epsilon$');
  subplot(2, 1, 1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Kpz_dvf*Gpz_dvf('y', 'F'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Kpz_iff*Gpz_iff('y', 'F'), freqs, 'Hz'))), '--');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Loop Gain'); xlabel('Frequency [Hz]');

  subplot(2, 1, 2);
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(Kpz_dvf*Gpz_dvf('y', 'F'), freqs, 'Hz'))), 'DisplayName', '$L_{DVF}$');
  plot(freqs, 180/pi*angle(squeeze(freqresp(Kpz_iff*Gpz_iff('y', 'F'), freqs, 'Hz'))), '--', 'DisplayName', '$L_{IFF}$');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-180, 180]);
  yticks([-180, -90, 0, 90, 180]);
  legend('location', 'northwest');
#+end_src

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

#+name: fig:simple_loop_gain_pz
#+caption: Loop Gain ([[./figs/simple_loop_gain_pz.png][png]], [[./figs/simple_loop_gain_pz.pdf][pdf]])
[[file:figs/simple_loop_gain_pz.png]]


Let's connect all the systems as shown in Figure [[fig:nano_station_control_x]].
#+begin_src matlab
  Sfb = sumblk('e = r2 - y');

  R = [tf(1); tf(1)];
  R.InputName = {'r'};
  R.OutputName = {'r1', 'r2'};

  F = [tf(1); tf(1)];
  F.InputName = {'Fi'};
  F.OutputName = {'F', 'Fu'};

  Gpz_fb_dvf = connect(Gpz_dvf, Kpz_dvf, R, Sfb, F, {'r', 'dmu', 'Fd', 'w'}, {'y', 'e', 'Fm', 'd', 'Fu'});
  Gpz_fb_iff = connect(Gpz_iff, Kpz_iff, R, Sfb, F, {'r', 'dmu', 'Fd', 'w'}, {'y', 'e', 'Fm', 'd', 'Fu'});
#+end_src

*** Results
#+begin_src matlab :exports none
  figure;
  subplot(2, 2, 1);
  title('Effect of Ground Motion ($\epsilon/w$)');
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gpz('y', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_iff('y', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_dvf('y', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_fb_iff('y', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_fb_dvf('y', 'w'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
  xlim([freqs(1), freqs(end)]);

  subplot(2, 2, 2);
  title('Compliance ($\epsilon/F_d$)');
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gpz('y', 'Fd'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_iff('y', 'Fd'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_dvf('y', 'Fd'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_fb_iff('y', 'Fd'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_fb_dvf('y', 'Fd'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
  xlim([freqs(1), freqs(end)]);

  subplot(2, 2, 3);
  title('Vibration Filtering ($\epsilon/d_\mu$)');
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gpz('y', 'dmu'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_iff('y', 'dmu'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_dvf('y', 'dmu'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_fb_iff('y', 'dmu'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_fb_dvf('y', 'dmu'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
  xlim([freqs(1), freqs(end)]);

  subplot(2, 2, 4);
  title('Reference Tracking ($\epsilon/r$)');
  hold on;
  plot(freqs, abs(squeeze(freqresp(1 - Gpz('y', 'r'), freqs, 'Hz'))),     'DisplayName', 'OL' );
  plot(freqs, abs(squeeze(freqresp(1 - Gpz_iff('y', 'r'), freqs, 'Hz'))), 'DisplayName', 'IFF');
  plot(freqs, abs(squeeze(freqresp(1 - Gpz_dvf('y', 'r'), freqs, 'Hz'))), 'DisplayName', 'DVF');
  plot(freqs, abs(squeeze(freqresp(1 - Gpz_fb_iff('y', 'r'), freqs, 'Hz'))),  'DisplayName', 'HAC-IFF');
  plot(freqs, abs(squeeze(freqresp(1 - Gpz_fb_dvf('y', 'r'), freqs, 'Hz'))),  'DisplayName', 'HAC-DVF');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
  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/simple_hac_lac_results.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:simple_hac_lac_results
#+caption: Obtained closed-loop transfer functions ([[./figs/simple_hac_lac_results.png][png]], [[./figs/simple_hac_lac_results.pdf][pdf]])
[[file:figs/simple_hac_lac_results.png]]

|     | Reference Tracking | Vibration Filtering              | Compliance                       |
|-----+--------------------+----------------------------------+----------------------------------|
| DVF | Similar behavior   |                                  | Better for $\omega < \omega_\nu$ |
| IFF | Similar behavior   | Better for $\omega > \omega_\nu$ |                                  |

** Two degree of freedom control                                   :noexport:
Let's try to implement the control architecture shown in Figure [[fig:nano_station_control_2dof_x]].

The pre-filter $K_r$ is added in order to improve the reference tracking performances.

#+begin_src latex :file nano_station_control_2dof_x.pdf
  \begin{tikzpicture}
    \node[block={4.0cm}{4.0cm}] (P) {};
    \node[above] at (P.north) {$\nu$-hexapod};

    % Input and outputs coordinates
    \coordinate[] (inputFd) at ($(P.south west)!0.8!(P.north west)$);
    \coordinate[] (inputw)  at ($(P.south west)!0.5!(P.north west)$);
    \coordinate[] (inputF)  at ($(P.south west)!0.2!(P.north west)$);
    \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
    \coordinate[] (outputd) at ($(P.south east)!0.5!(P.north east)$);
    \coordinate[] (outputx) at ($(P.south east)!0.2!(P.north east)$);

    % Connections and labels
    \draw[<-] (inputFd) -- ++(-0.8, 0) node[above right]{$F_d$};

    \draw[->] (outputF) -- ++(0.8, 0) node[above left]{$F_m$};
    \draw[->] (outputd) -- ++(0.8, 0) node[above left]{$d$};
    \draw[->] (outputx) -- ++(0.8, 0) node[above left]{$x$};

    \node[block, left= of inputF] (K) {$K$};
    \node[addb={+}{}{}{}{-}, left= of K] (addfb) {};
    \node[block, left= of addfb] (Kr) {$K_r$};
    \node[addb, left= of inputw] (add) {};
    \node[block, left= of add] (Tmu) {$T_\mu$};

    \draw[->] ($(outputx) + (0.3, 0)$) node[branch]{} -- ++(0, -1) -| (addfb.south);
    \draw[->] (addfb.east) -- (K.west) node[above left]{$\epsilon$};
    \draw[->] (K.east) -- (inputF) node[above left]{$F$};

    \draw[->] ($(Kr.west) + (-1.6, 0)$)node[above right]{$r$} -- (Kr.west);
    \draw[->] (Kr.east) -- (addfb.west);

    \draw[->] ($(Kr.west) + (-0.8, 0)$)node[branch]{} |- (Tmu.west);
    \draw[->] (Tmu.east) -- (add.west);
    \draw[->] (add.east) -- (inputw) node[above left]{$x_\mu$};
    \draw[->] ($(add.north) + (0, 0.8)$) node[below right]{$d_\mu$} -- (add.north);
  \end{tikzpicture}
#+end_src

#+name: fig:nano_station_control_2dof_x
#+caption: Two degrees of freedom feedback control
#+RESULTS:
[[file:figs/nano_station_control_2dof_x.png]]

In order to design the pre-filter $K_r$, the dynamics of the system should be known quite precisely (Dynamics of the nano-hexapod + $T_\mu$).
#+begin_src matlab
  Krpz = inv(Gpz_fb('y', 'r'));

  Krpz.InputName = {'r2'};
  Krpz.OutputName = {'r3'};
#+end_src

#+begin_src matlab
  Sfb = sumblk('e = r3 - y');

  R = [tf(1); tf(1)];
  R.InputName = {'r'};
  R.OutputName = {'r1', 'r2'};

  Gpz_2dof = connect(Gpz_dvf, Krpz, Kpz, R, Sfb, {'r', 'dmu', 'Fd', 'w'}, {'y', 'e', 'Fm', 'd'});
#+end_src

#+begin_src matlab :exports none
  figure;
  subplot(2, 2, 1);
  title('Effect of Ground Motion ($\epsilon/w$)');
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gpz('y', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_iff('y', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_dvf('y', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_fb('y', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_2dof('y', 'w'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');

  subplot(2, 2, 2);
  title('Compliance ($\epsilon/F_d$)');
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gpz('y', 'Fd'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_iff('y', 'Fd'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_dvf('y', 'Fd'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_fb('y', 'Fd'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_2dof('y', 'Fd'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');

  subplot(2, 2, 3);
  title('Vibration Filtering ($\epsilon/d_\mu$)');
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gpz('y', 'dmu'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_iff('y', 'dmu'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_dvf('y', 'dmu'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_fb('y', 'dmu'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gpz_2dof('y', 'dmu'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');

  subplot(2, 2, 4);
  title('Reference Tracking ($\epsilon/r$)');
  hold on;
  plot(freqs, abs(squeeze(freqresp(1 - Gpz('y', 'r'), freqs, 'Hz'))),     'DisplayName', 'OL' );
  plot(freqs, abs(squeeze(freqresp(1 - Gpz_iff('y', 'r'), freqs, 'Hz'))), 'DisplayName', 'IFF');
  plot(freqs, abs(squeeze(freqresp(1 - Gpz_dvf('y', 'r'), freqs, 'Hz'))), 'DisplayName', 'DVF');
  plot(freqs, abs(squeeze(freqresp(1 - Gpz_fb('y', 'r'), freqs, 'Hz'))),   'DisplayName', 'FB');
  plot(freqs, abs(squeeze(freqresp(1 - Gpz_2dof('y', 'r'), freqs, 'Hz'))), 'DisplayName', '2-DOF');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
  legend('location', 'northwest');
#+end_src

* Comparison with the use of a Soft nano-hexapod
#+begin_src matlab
  m = 50; % [kg]
  k = 1e3; % [N/m]

  Gn = 1/(m*s^2 + k)*[-k, k*m*s^2, m*s^2; 1, -m*s^2, 1; 1, k, 1];
  Gn.InputName  = {'Fd', 'xmu', 'F'};
  Gn.OutputName = {'Fm', 'd', 'x'};
#+end_src

#+begin_src matlab
  wmu = 2*pi*50; % [rad/s]

  Tmu = 1/(1 + s/wmu);
  Tmu.InputName = {'r1'};
  Tmu.OutputName = {'ymu'};
#+end_src

#+begin_src matlab
  w0 = 2*pi*40;
  xi = 0.5;
  Tw = 1/(1 + 2*xi*s/w0 + s^2/w0^2);
  Tw.InputName = {'w1'};
  Tw.OutputName = {'dw'};
#+end_src

#+begin_src matlab
  Wsplit = [tf(1); tf(1)];
  Wsplit.InputName = {'w'};
  Wsplit.OutputName = {'w1', 'w2'};

  S = sumblk('xmu = ymu + dmu + dw');

  Sw = sumblk('y = x - w2');

  Gvc = connect(Gn, S, Wsplit, Tw, Tmu, Sw, {'Fd', 'dmu', 'r1', 'F', 'w'}, {'Fm', 'd', 'y'});
#+end_src

#+begin_src matlab
  Kvc_dvf = 2*sqrt(k*m)*s;
  Kvc_dvf.InputName = {'d'};
  Kvc_dvf.OutputName = {'F'};

  Gvc_dvf = feedback(Gvc, Kvc_dvf, 'name');

  Kvc_iff = 2*sqrt(k/m)/s;
  Kvc_iff.InputName = {'Fm'};
  Kvc_iff.OutputName = {'F'};

  Gvc_iff = feedback(Gvc, Kvc_iff, 'name');
#+end_src

#+begin_src matlab
  wb = 2*pi*100; % control bandwidth [rad/s]

  % Lead
  h = 2.0;
  wz = wb/h; % [rad/s]
  wp = wb*h; % [rad/s]

  H = 1/h*(1 + s/wz)/(1 + s/wp);

  Kvc_dvf = H/abs(freqresp(H*Gvc_dvf('y', 'F'), wb));
  Kvc_dvf.InputName = {'e'};
  Kvc_dvf.OutputName = {'Fi'};

  Kvc_iff = H/abs(freqresp(H*Gvc_iff('y', 'F'), wb));
  Kvc_iff.InputName = {'e'};
  Kvc_iff.OutputName = {'Fi'};
#+end_src

#+begin_src matlab
  Sfb = sumblk('e = r2 - y');

  R = [tf(1); tf(1)];
  R.InputName = {'r'};
  R.OutputName = {'r1', 'r2'};

  F = [tf(1); tf(1)];
  F.InputName = {'Fi'};
  F.OutputName = {'F', 'Fu'};


  Gvc_fb_dvf = connect(Gvc_dvf, Kvc_dvf, R, Sfb, F, {'r', 'dmu', 'Fd', 'w'}, {'y', 'e', 'Fm', 'd', 'Fu'});
  Gvc_fb_iff = connect(Gvc_iff, Kvc_iff, R, Sfb, F, {'r', 'dmu', 'Fd', 'w'}, {'y', 'e', 'Fm', 'd', 'Fu'});
#+end_src

#+begin_src matlab :exports none
  figure;
  subplot(2, 2, 1);
  title('Effect of Ground Motion ($\epsilon/w$)');
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gvc('y', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gvc_iff('y', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gvc_dvf('y', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gvc_fb_iff('y', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gvc_fb_dvf('y', 'w'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
  xlim([freqs(1), freqs(end)]);

  subplot(2, 2, 2);
  title('Compliance ($\epsilon/F_d$)');
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gvc('y', 'Fd'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gvc_iff('y', 'Fd'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gvc_dvf('y', 'Fd'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gvc_fb_iff('y', 'Fd'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gvc_fb_dvf('y', 'Fd'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
  xlim([freqs(1), freqs(end)]);

  subplot(2, 2, 3);
  title('Vibration Filtering ($\epsilon/d_\mu$)');
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gvc('y', 'dmu'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gvc_iff('y', 'dmu'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gvc_dvf('y', 'dmu'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gvc_fb_iff('y', 'dmu'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gvc_fb_dvf('y', 'dmu'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
  xlim([freqs(1), freqs(end)]);

  subplot(2, 2, 4);
  title('Reference Tracking ($\epsilon/r$)');
  hold on;
  plot(freqs, abs(squeeze(freqresp(1 - Gvc('y', 'r'), freqs, 'Hz'))),     'DisplayName', 'OL' );
  plot(freqs, abs(squeeze(freqresp(1 - Gvc_iff('y', 'r'), freqs, 'Hz'))), 'DisplayName', 'IFF');
  plot(freqs, abs(squeeze(freqresp(1 - Gvc_dvf('y', 'r'), freqs, 'Hz'))), 'DisplayName', 'DVF');
  plot(freqs, abs(squeeze(freqresp(1 - Gvc_fb_iff('y', 'r'), freqs, 'Hz'))), 'DisplayName', 'HAC-IFF');
  plot(freqs, abs(squeeze(freqresp(1 - Gvc_fb_dvf('y', 'r'), freqs, 'Hz'))), 'DisplayName', 'HAC-DVF');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
  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/simple_hac_lac_results_soft.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:simple_hac_lac_results_soft
#+caption: Obtained closed-loop transfer functions ([[./figs/simple_hac_lac_results_soft.png][png]], [[./figs/simple_hac_lac_results_soft.pdf][pdf]])
[[file:figs/simple_hac_lac_results_soft.png]]

|     | Reference Tracking | Vibration Filtering              | Compliance                       |
|-----+--------------------+----------------------------------+----------------------------------|
| DVF | Similar behavior   |                                  | Better for $\omega < \omega_\nu$ |
| IFF | Similar behavior   | Better for $\omega > \omega_\nu$ |                                  |

#+begin_src matlab :exports none
  figure;
  subplot(2, 2, 1);
  title('Effect of Ground Motion ($\epsilon/w$)');
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gpz_fb_dvf('y', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gvc_fb_dvf('y', 'w'), freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',1);
  plot(freqs, abs(squeeze(freqresp(Gpz_fb_iff('y', 'w'), freqs, 'Hz'))), '--');
  plot(freqs, abs(squeeze(freqresp(Gvc_fb_iff('y', 'w'), freqs, 'Hz'))), '--');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
  xlim([freqs(1), freqs(end)]);

  subplot(2, 2, 2);
  title('Compliance ($\epsilon/F_d$)');
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gpz_fb_dvf('y', 'Fd'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gvc_fb_dvf('y', 'Fd'), freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',1);
  plot(freqs, abs(squeeze(freqresp(Gpz_fb_iff('y', 'Fd'), freqs, 'Hz'))), '--');
  plot(freqs, abs(squeeze(freqresp(Gvc_fb_iff('y', 'Fd'), freqs, 'Hz'))), '--');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
  xlim([freqs(1), freqs(end)]);

  subplot(2, 2, 3);
  title('Vibration Filtering ($\epsilon/d_\mu$)');
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gpz_fb_dvf('y', 'dmu'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gvc_fb_dvf('y', 'dmu'), freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',1);
  plot(freqs, abs(squeeze(freqresp(Gpz_fb_iff('y', 'dmu'), freqs, 'Hz'))), '--');
  plot(freqs, abs(squeeze(freqresp(Gvc_fb_iff('y', 'dmu'), freqs, 'Hz'))), '--');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
  xlim([freqs(1), freqs(end)]);

  subplot(2, 2, 4);
  title('Reference Tracking ($\epsilon/r$)');
  hold on;
  plot(freqs, abs(squeeze(freqresp(1 - Gpz_fb_dvf('y', 'r'), freqs, 'Hz'))), 'DisplayName', 'HAC-DVF PZ');
  plot(freqs, abs(squeeze(freqresp(1 - Gvc_fb_dvf('y', 'r'), freqs, 'Hz'))), 'DisplayName', 'HAC-DVF VC');
  set(gca,'ColorOrderIndex',1);
  plot(freqs, abs(squeeze(freqresp(1 - Gpz_fb_iff('y', 'r'), freqs, 'Hz'))), '--', 'DisplayName', 'HAC-IFF PZ');
  plot(freqs, abs(squeeze(freqresp(1 - Gvc_fb_iff('y', 'r'), freqs, 'Hz'))), '--', 'DisplayName', 'HAC-IFF VC');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
  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/simple_comp_vc_pz.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:simple_comp_vc_pz
#+caption: Comparison of the closed-loop transfer functions for Soft and Stiff nano-hexapod ([[./figs/simple_comp_vc_pz.png][png]], [[./figs/simple_comp_vc_pz.pdf][pdf]])
[[file:figs/simple_comp_vc_pz.png]]

|                       |  <c>   |   <c>   |
|                       | *Soft* | *Stiff* |
|-----------------------+--------+---------|
| *Reference Tracking*  |   =    |    =    |
| *Ground Motion*       |   =    |    =    |
| *Vibration Isolation* |   +    |    -    |
| *Compliance*          |   -    |    +    |

* Estimate the level of vibration
#+begin_src matlab
  gm  = load('./mat/psd_gm.mat', 'f', 'psd_gm');
  rz  = load('./mat/pxsp_r.mat', 'f', 'pxsp_r');
  tyz = load('./mat/pxz_ty_r.mat', 'f', 'pxz_ty_r');
#+end_src

#+begin_src matlab :exports none
  f = gm.f(2:end);

  psd_gm = gm.psd_gm(2:end); % PSD of ground motion [m^2/Hz]
  psd_rz = rz.pxsp_r(2:end)./(2*pi*f.^2); % PSD of hexapod's motion due to Rz [m^2/Hz]
  psd_ty = tyz.pxz_ty_r(2:end)./(2*pi*f.^2); % PSD of hexapod's motion due to Ty [m^2/Hz]
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(f, sqrt(psd_rz), 'DisplayName', 'Rz');
  plot(f, sqrt(psd_ty), 'DisplayName', 'Ty');
  plot(f, sqrt(psd_gm), 'DisplayName', 'Gm');
  hold off;
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('ASD of the displacement $\left[\frac{m}{\sqrt{Hz}}\right]$')
  legend('Location', 'southwest');
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(f, flip(sqrt(-cumtrapz(flip(f), flip(psd_rz)))), 'DisplayName', 'Rz');
  plot(f, flip(sqrt(-cumtrapz(flip(f), flip(psd_ty)))), 'DisplayName', 'Ty');
  plot(f, flip(sqrt(-cumtrapz(flip(f), flip(psd_ty + psd_rz)))), 'k-', 'DisplayName', 'tot');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('CAS of $x_\mu$ [m]')
  xlim([freqs(1), freqs(end)]);
  legend('Location', 'southwest');
#+end_src

If we note the PSD $\Gamma$:
\[ \Gamma_y = |G_{\frac{y}{w}}|^2 \Gamma_w +  |G_{\frac{y}{x_\mu}}|^2 \Gamma_{x_\mu} \]

#+begin_src matlab
  x_pz = abs(squeeze(freqresp(Gpz_fb_iff('y', 'dmu'), f, 'Hz'))).^2.*(psd_rz + psd_ty) + abs(squeeze(freqresp(Gpz_fb_iff('y', 'w'), f, 'Hz'))).^2.*(psd_gm);
  x_vc = abs(squeeze(freqresp(Gvc_fb_iff('y', 'dmu'), f, 'Hz'))).^2.*(psd_rz + psd_ty) + abs(squeeze(freqresp(Gvc_fb_iff('y', 'w'), f, 'Hz'))).^2.*(psd_gm);
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(f, sqrt(x_pz));
  plot(f, sqrt(x_vc));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('ASD of the displacement $\left[\frac{m}{\sqrt{Hz}}\right]$')
  xlim([freqs(1), freqs(end)]);
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(f, flip(sqrt(-cumtrapz(flip(f), flip(x_pz)))), 'DisplayName', 'PZ');
  plot(f, flip(sqrt(-cumtrapz(flip(f), flip(x_vc)))), 'DisplayName', 'VC');
  plot(f, flip(sqrt(-cumtrapz(flip(f), flip(psd_rz + psd_ty + psd_gm.*abs(squeeze(freqresp(Tw, f, 'Hz')).^2))))), 'DisplayName', 'OL');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('CAS of the relative displacement $[m]$')
  xlim([freqs(1), freqs(end)]);
  legend();
#+end_src

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

#+name: fig:simple_asd_motion_error
#+caption: ASD of the position error due to Ground Motion and Vibration ([[./figs/simple_asd_motion_error.png][png]], [[./figs/simple_asd_motion_error.pdf][pdf]])
[[file:figs/simple_asd_motion_error.png]]

Actuator usage
#+begin_src matlab
  F_pz = abs(squeeze(freqresp(Gpz_fb_iff('Fu', 'dmu'), f, 'Hz'))).^2.*(psd_rz + psd_ty) + abs(squeeze(freqresp(Gpz_fb_iff('Fu', 'w'), f, 'Hz'))).^2.*(psd_gm);
  F_vc = abs(squeeze(freqresp(Gvc_fb_iff('Fu', 'dmu'), f, 'Hz'))).^2.*(psd_rz + psd_ty) + abs(squeeze(freqresp(Gvc_fb_iff('Fu', 'w'), f, 'Hz'))).^2.*(psd_gm);
#+end_src

#+begin_src matlab :results output replace
  sqrt(trapz(f, F_pz))
  sqrt(trapz(f, F_vc))
#+end_src

#+RESULTS:
: sqrt(trapz(f, F_pz))
: ans =
:           84.8961762069446
: sqrt(trapz(f, F_vc))
: ans =
:         0.0387785981815527

* Requirements on the norm of closed-loop transfer functions
** Approximation of the ASD of perturbations
#+begin_src matlab
  G_rz = 1e-9*1/(1 + s/2/pi/0.5)^2*(s + 2*pi*1)*(s + 2*pi*10)*(1/((1 + s/2/pi/100)^2));
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(f, sqrt(psd_rz), 'DisplayName', 'Rz');
  plot(freqs, abs(squeeze(freqresp(G_rz, freqs, 'Hz'))));
  hold off;
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('ASD of the displacement $\left[\frac{m}{\sqrt{Hz}}\right]$')
  legend('Location', 'southwest');
#+end_src

#+begin_src matlab
  G_gm = 1e-8*1/s^2*(s + 2*pi*1)^2*(1/((1 + s/2/pi/10)^3));
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(f, sqrt(psd_gm), 'DisplayName', 'Rz');
  plot(freqs, abs(squeeze(freqresp(G_gm, freqs, 'Hz'))));
  hold off;
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('ASD of the displacement $\left[\frac{m}{\sqrt{Hz}}\right]$')
  legend('Location', 'southwest');
#+end_src

** Wanted ASD of outputs
Wanted ASD of motion error
#+begin_src matlab
  y_wanted = 100e-9; % 10nm rms wanted
  y_bw = 2*pi*100; % bandwidth [rad/s]

  G_y = 2*y_wanted/sqrt(y_bw) * (1 + s/y_bw/10) / (1 + s/y_bw);
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G_y, freqs, 'Hz'))));
  hold off;
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('ASD of the displacement $\left[\frac{m}{\sqrt{Hz}}\right]$')
  legend('Location', 'southwest');
#+end_src

#+begin_src matlab :results output replace
  sqrt(trapz(f, abs(squeeze(freqresp(G_y, f, 'Hz'))).^2))
#+end_src

#+RESULTS:
: sqrt(trapz(f, abs(squeeze(freqresp(G_y, f, 'Hz'))).^2))
: ans =
:       9.47118350214793e-08

** Limiting the bandwidth
#+begin_src matlab
  wF = 2*pi*10;
  G_F = 100000*(wF + s)^2;
#+end_src

** Generalized Weighted plant
Let's create a generalized weighted plant for controller synthesis.

Let's start simple:
|                     | *Symbol* | *Meaning*                          |
|---------------------+----------+------------------------------------|
| *Exogenous Inputs*  | $x_\mu$  | Motion of the $\nu$-hexapod's base |
|---------------------+----------+------------------------------------|
| *Exogenous Outputs* | $y$      | Motion error of the Payload        |
|---------------------+----------+------------------------------------|
| *Sensed Outputs*    | $y$      | Motion error of the Payload        |
|---------------------+----------+------------------------------------|
| *Control Signals*   | $F$      | Actuator Inputs                    |

Add $F$ as output.
#+begin_src matlab
  F = [tf(1); tf(1)];
  F.InputName = {'Fi'};
  F.OutputName = {'F', 'Fu'};

  P_pz = connect(F, Gpz_dvf, {'dmu', 'Fi'}, {'y', 'Fu', 'y'})
  P_vc = connect(F, Gvc_dvf, {'dmu', 'Fi'}, {'y', 'Fu', 'y'})
#+end_src

Normalization.

We multiply the plant input by $G_{rz}$ and the plant output by $G_y^{-1}$:
#+begin_src matlab
  P_pz_norm = blkdiag(inv(G_y), inv(G_F), 1)*P_pz*blkdiag(G_rz, 1);
  P_pz_norm.OutputName = {'z', 'F', 'y'};
  P_pz_norm.InputName  = {'w', 'u'};

  P_vc_norm = blkdiag(inv(G_y), inv(G_F), 1)*P_vc*blkdiag(G_rz, 1);
  P_vc_norm.OutputName = {'z', 'F', 'y'};
  P_vc_norm.InputName  = {'w', 'u'};
#+end_src

** Synthesis
#+begin_src matlab
  [Kpz_dvf,CL_vc,~] = hinfsyn(minreal(P_pz_norm), 1, 1, 'TOLGAM', 0.001, 'METHOD', 'LMI', 'DISPLAY', 'on');
  Kpz_dvf.InputName = {'e'};
  Kpz_dvf.OutputName = {'Fi'};

  [Kvc_dvf,CL_pz,~] = hinfsyn(minreal(P_vc_norm), 1, 1, 'TOLGAM', 0.001, 'METHOD', 'LMI', 'DISPLAY', 'on');
  Kvc_dvf.InputName = {'e'};
  Kvc_dvf.OutputName = {'Fi'};
#+end_src

** Loop Gain
#+begin_src matlab :exports none
  figure;
  title('Transfer function from $F^\prime$ to $\epsilon$');
  subplot(2, 1, 1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Kpz_dvf*Gpz_dvf('y', 'F'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Kvc_dvf*Gvc_dvf('y', 'F'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Loop Gain'); xlabel('Frequency [Hz]');

  subplot(2, 1, 2);
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(-Kpz_dvf*Gpz_dvf('y', 'F'), freqs, 'Hz'))), 'DisplayName', '$PZ$');
  plot(freqs, 180/pi*angle(squeeze(freqresp(-Kvc_dvf*Gvc_dvf('y', 'F'), freqs, 'Hz'))), 'DisplayName', '$VC$');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
  ylim([-180, 180]);
  yticks([-180, -90, 0, 90, 180]);
  legend('location', 'northwest');
#+end_src

#+begin_src matlab
  Sfb = sumblk('e = r2 - y');

  R = [tf(1); tf(1)];
  R.InputName = {'r'};
  R.OutputName = {'r1', 'r2'};

  F = [tf(1); tf(1)];
  F.InputName = {'Fi'};
  F.OutputName = {'F', 'Fu'};

  Gpz_fb_dvf = connect(Gpz_dvf, -Kpz_dvf, R, Sfb, F, {'r', 'dmu', 'Fd', 'w'}, {'y', 'e', 'Fm', 'd', 'Fu'});
  Gvc_fb_dvf = connect(Gvc_dvf, -Kvc_dvf, R, Sfb, F, {'r', 'dmu', 'Fd', 'w'}, {'y', 'e', 'Fm', 'd', 'Fu'});
#+end_src

** Results
#+begin_src matlab :exports none
  freqs = logspace(-6, 8, 1000);

  figure;
  subplot(2, 2, 1);
  title('Effect of Ground Motion ($\epsilon/w$)');
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gpz_fb_dvf('y', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gvc_fb_dvf('y', 'w'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
  xlim([freqs(1), freqs(end)]);

  subplot(2, 2, 2);
  title('Compliance ($\epsilon/F_d$)');
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gpz_fb_dvf('y', 'Fd'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gvc_fb_dvf('y', 'Fd'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
  xlim([freqs(1), freqs(end)]);

  subplot(2, 2, 3);
  title('Vibration Filtering ($\epsilon/d_\mu$)');
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gpz_fb_dvf('y', 'dmu'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Gvc_fb_dvf('y', 'dmu'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(G_y*inv(G_rz), freqs, 'Hz'))), 'k--');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
  xlim([freqs(1), freqs(end)]);

  subplot(2, 2, 4);
  title('Reference Tracking ($\epsilon/r$)');
  hold on;
  plot(freqs, abs(squeeze(freqresp(1 - Gpz_fb_dvf('y', 'r'), freqs, 'Hz'))), 'DisplayName', 'HAC-DVF PZ');
  plot(freqs, abs(squeeze(freqresp(1 - Gvc_fb_dvf('y', 'r'), freqs, 'Hz'))), 'DisplayName', 'HAC-DVF VC');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
  xlim([freqs(1), freqs(end)]);
  legend('location', 'southeast');
#+end_src

** Requirements
| reference tracking  | $\epsilon/r$ | -120dB at 1Hz    |
| vibration isolation | $x/x_\mu$    | -60dB above 10Hz |
| compliance          | $x/F_d$      |                  |