#+TITLE: Sensor Fusion Paper - Matlab Computation
:DRAWER:
#+HTML_LINK_HOME: ../index.html
#+HTML_LINK_UP: ../index.html

#+LATEX_CLASS: cleanreport
#+LATEX_CLASS_OPTIONS: [tocnp, secbreak, minted]

#+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: <script src="../js/jquery.min.js"></script>
#+HTML_HEAD: <script src="../js/bootstrap.min.js"></script>
#+HTML_HEAD: <script src="../js/jquery.stickytableheaders.min.js"></script>
#+HTML_HEAD: <script src="../js/readtheorg.js"></script>

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

* Introduction                                                       :ignore:
The control architecture studied here is shown on figure [[fig:sf_arch_class_prefilter]] where:
- $G^{\prime}$ is the plant to control
- $K = G^{-1} H_L^{-1}$ is the controller used with $G$ a model of the plant
- $H_L$ and $H_H$ are complementary filters ($H_L + H_H = 1$)
- $K_r$ is a pre-filter that can be added

#+name: fig:sf_arch_class_prefilter
#+caption: Control Architecture
[[file:figs-tikz/sf_arch_class_prefilter.png]]

Here is the outline of the =matlab= analysis for this control architecture:
- Section [[sec:plant]]: the plant model $G$ is defined
- Section [[sec:uncertainty]]: the plant uncertainty set $\Pi_I$ is defined using the multiplicative input uncertainty: $\Pi_I: \ G^\prime = G (1 + w_I \Delta)$. Thus the weight $w_I$ is defined such that the true system dynamics is included in the set $\Pi_I$
- Section [[sec:specifications]]: From the specifications on performance that are expressed in terms of upper bounds of $S$ and $T$, performance weights $w_S$ and $w_T$ are derived such that the goal is to obtain $|S| < \frac{1}{|w_S|}$ and $|T| < \frac{1}{|w_T|}, \ \forall \omega$
- Section [[sec:upper_bounds_filters]]: upper bounds on the magnitude of the complementary filters $|H_L|$ and $|H_H|$ are defined in order to ensure Nominal Performance (NP), Robust Stability (RS) and Robust Performance (RP)
- Then, $H_L$ and $H_H$ are synthesize such that $|H_L|$ and $|H_H|$ are within the specified bounds and such that $H_L + H_H = 1$ (complementary property). This is done using two techniques, first $\mathcal{H}_\infty$ (section [[sec:h_infinity]]) and then analytical formulas (section [[sec:analytical_formula]]). Resulting complementary filters for both methods are compared in section [[sec:comp_filters]].
- Section [[sec:controller_analysis]]: the obtain controller $K = G^{-1} H_H^{-1}$ is analyzed
- Section [[sec:nominal_stability_performance]]: the Nominal Stability (NS) and Nominal Performance conditions are verified
- Section [[sec:robustness_analysis]]: robust Stability and Robust Performance conditions are studied
- Section [[sec:pre_filter]]: a pre-filter that is used to limit the input usage due to the change of the reference is added
- Section [[sec:sisotool_controller]]: a controller is designed using =SISOTOOL= and then compared with the previously generated controller

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

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

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

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

#+begin_src matlab
  freqs = logspace(-1, 3, 1000);
#+end_src

* Definition of the plant
  <<sec:plant>>

The studied system consists of a solid positioned on top of a motorized uni-axial soft suspension.

The absolute position $x$ of the solid is measured using an inertial sensor and a force $F$ can be applied to the mass using a voice coil actuator.

The model of the system is represented on figure [[fig:mech_sys_alone]] where the mass of the solid is $m = 20\ [kg]$, the stiffness of the suspension is $k = 10^4\ [N/m]$ and the damping of the system is $c = 10^2\ [N/(m/s)]$.

#+name: fig:mech_sys_alone
#+caption: One degree of freedom system
[[file:figs-tikz/mech_sys_alone.png]]

The plant $G$ is defined on matlab and its bode plot is shown on figure [[fig:bode_plot_mech_sys]].

#+begin_src matlab
  m = 20;  % [kg]
  k = 1e4; % [N/m]
  c = 1e2; % [N/(m/s)]

  G = 1/(m*s^2 + c*s + k);
#+end_src

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

  ax2 = subaxis(2,1,2);
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(G, freqs, 'Hz'))), 'k-');
  hold off;
  yticks(-180:90:180);
  ylim([-180 180]);
  xlim([0.1, 1000]);
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  linkaxes([ax1,ax2],'x');
#+end_src

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

#+NAME: fig:bode_plot_mech_sys
#+CAPTION: Bode plot of $G$ ([[./figs/bode_plot_mech_sys.png][png]], [[./figs/bode_plot_mech_sys.pdf][pdf]])
[[file:figs/bode_plot_mech_sys.png]]

* Multiplicative input uncertainty
  <<sec:uncertainty>>
We choose to use the multiplicative input uncertainty to model the plant uncertainty:
\[ \Pi_I: \ G^\prime(s) = G(s) (1 + w_I(s) \Delta(s)),\text{ with } |\Delta(j\omega)| < 1 \ \forall \omega \]


The uncertainty weight $w_I$ has the following form:
\[ w_I(s) = \frac{\tau s + r_0}{(\tau/r_\infty) s + 1} \]
where $r_0=0.1$ is the relative uncertainty at steady-state, $1/\tau=80\text{Hz}$ is the frequency at which the relative uncertainty reaches 100%, and $r_\infty=10$ is the magnitude of the weight at high frequency.

We defined the uncertainty weight on matlab. Its bode plot is shown on figure [[fig:bode_wi]].

#+begin_src matlab
  r0 = 0.1;
  rinf = 10;
  tau = 1/2/pi/80;

  wI = (tau*s + r0)/((tau/rinf)*s+1);
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, abs(squeeze(freqresp(wI, freqs, 'Hz'))), 'k-');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  xlim([freqs(1), freqs(end)]);
  ylim([1e-1, 10]);
  xticks([0.1, 1, 10, 100, 1000])
#+end_src

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

#+NAME: fig:bode_wi
#+CAPTION: Bode plot of $w_I$ ([[./figs/bode_wi.png][png]], [[./figs/bode_wi.pdf][pdf]])
[[file:figs/bode_wi.png]]

The uncertain model is created with the =ultidyn= function. Elements in the uncertainty set $\Pi_I$ are computed and their bode plot is shown on figure [[fig:plant_uncertainty_bode_plot]].

#+begin_src matlab
  Delta = ultidyn('Delta', [1 1]);

  Gd = G*(1+wI*Delta);
  Gds = usample(Gd, 20);
#+end_src

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2,1,1);
  hold on;
  for i=1:length(Gds)
      plot(freqs, abs(squeeze(freqresp(Gds(:, :, i), freqs, 'Hz'))), '-', 'color', [0, 0, 0, 0.1]);
  end
  plot(freqs, abs(squeeze(freqresp(Gd, freqs, 'Hz'))), 'k-');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude [m/N]');
  hold off;
  % Phase
  ax2 = subplot(2,1,2);
  hold on;
  for i=1:length(Gds)
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gds(:, :, i), freqs, 'Hz')))), '-', 'color', [0, 0, 0, 0.1]);
  end
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G, freqs, 'Hz')))), 'k-');
  set(gca,'xscale','log');
  yticks(-360:90:180);
  ylim([-360 0]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
  xticks([0.1, 1, 10, 100, 1000])
#+end_src

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

#+NAME: fig:plant_uncertainty_bode_plot
#+CAPTION: Some elements in the uncertainty set $\Pi_I$ ([[./figs/plant_uncertainty_bode_plot.png][png]], [[./figs/plant_uncertainty_bode_plot.pdf][pdf]])
[[file:figs/plant_uncertainty_bode_plot.png]]

* Specifications and performance weights
  <<sec:specifications>>

The control objective is to isolate the displacement $x$ of the mass from the ground motion $w$.

The specifications are described below:
- at least a factor $10$ of disturbance rejection at $2\ \text{Hz}$ and with a slope of $2$ below $2\ \text{Hz}$ until a rejection of $10^3$
- the noise attenuation should be at least $10$ above $100\ \text{Hz}$ and with a slope of $-2$ above

These specifications can be represented as upper bounds on the closed loop transfer functions $S$ and $T$ (see figure [[fig:bode_requirements]]).

#+begin_src matlab :exports none
  figure;
  hold on;
  set(gca,'ColorOrderIndex',1)
  plot([100, 1000], [0.1, 0.001], ':', 'DisplayName', '$|T|$ - Upper bound');
  set(gca,'ColorOrderIndex',2)
  plot([0.1, 0.2, 2], [0.001, 0.001, 0.1], ':', 'DisplayName', '$|S|$ - Upper bound');

  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  xlim([freqs(1), freqs(end)]);
  ylim([1e-3, 10]);
  xticks([0.1, 1, 10, 100, 1000]);
  legend('location', 'northeast');
#+end_src

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

#+NAME: fig:bode_requirements
#+CAPTION: Upper bounds on $S$ and $T$ ([[./figs/bode_requirements.png][png]], [[./figs/bode_requirements.pdf][pdf]])
[[file:figs/bode_requirements.png]]

We now define two weights, $w_S(s)$ and $w_T(s)$ such that $1/|w_S|$ and $1/|w_T|$ are lower than the previously defined upper bounds.
Then, the performance specifications are satisfied if the following condition is valid:
\[ \big|S(j\omega)\big| < \frac{1}{|w_S(j\omega)|} ; \quad \big|T(j\omega)\big| < \frac{1}{|w_T(j\omega)|}, \quad \forall \omega \]

The weights are defined as follow. They magnitude is compared with the upper bounds on $S$ and $T$ on figure [[fig:compare_weights_upper_bounds_S_T]].
#+begin_src matlab
  wS = 1600/(s+0.13)^2;
  wT = 1000*((s/(2*pi*1000)))^2;
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  set(gca,'ColorOrderIndex',1)
  plot(freqs, 1./abs(squeeze(freqresp(wT, freqs, 'Hz'))), '--', 'DisplayName', '$1/|w_T|$');
  set(gca,'ColorOrderIndex',1)
  plot([100, 1000], [0.1, 0.001], ':', 'DisplayName', '$|T|$ - Upper bound');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, 1./abs(squeeze(freqresp(wS, freqs, 'Hz'))), '--', 'DisplayName', '$1/|w_S|$');
  set(gca,'ColorOrderIndex',2)
  plot([0.1, 0.2, 2], [0.001, 0.001, 0.1], ':', 'DisplayName', '$|S|$ - Upper bound');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  xlim([freqs(1), freqs(end)]);
  ylim([1e-4, 10]);
  xticks([0.1, 1, 10, 100, 1000]);
  legend('location', 'northeast');
#+end_src

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

#+NAME: fig:compare_weights_upper_bounds_S_T
#+CAPTION: Weights $w_S$ and $w_T$ with the upper bounds on $S$ and $T$ obtained from the specifications ([[./figs/compare_weights_upper_bounds_S_T.png][png]], [[./figs/compare_weights_upper_bounds_S_T.pdf][pdf]])
[[file:figs/compare_weights_upper_bounds_S_T.png]]

* Upper bounds on the norm of the complementary filters for NP, RS and RP
  <<sec:upper_bounds_filters>>

Now that we have defined $w_I$, $w_S$ and $w_T$, we can derive conditions for Nominal Performance, Robust Stability and Robust Performance ($j\omega$ is omitted here for readability):
\begin{align*}
  \text{NP} &\Leftrightarrow |H_H| < \frac{1}{|w_S|} \text{ and } |H_L| < \frac{1}{|w_T|} \quad \forall \omega \\
  \text{RS} &\Leftrightarrow |H_L| < \frac{1}{|w_I| (2 + |w_I|)} \quad \forall \omega \\
  \text{RP for } S &\Leftarrow |H_H| < \frac{1 + |w_I|}{|w_S| (2 + |w_I|)} \quad \forall \omega \\
  \text{RP for } T &\Leftrightarrow |H_L| < \frac{1}{|w_T| (1 + |w_I|) + |w_I|} \quad \forall \omega
\end{align*}

These conditions are upper bounds on the complementary filters used for control.

We plot these conditions on figure [[fig:weights_NP_RS_RP]].

#+begin_src matlab :exports none
  wT_resp = abs(squeeze(freqresp(wT, freqs, 'Hz')));
  wI_resp = abs(squeeze(freqresp(wI, freqs, 'Hz')));
  wS_resp = abs(squeeze(freqresp(wS, freqs, 'Hz')));
  Hh_resp = wT_resp.*(1 + wI_resp)./(wS_resp.*(wT_resp .* (1 + wI_resp) + wI_resp));
  Hl_resp = 1./(wT_resp .* (1 + wI_resp) + wI_resp);

  figure;
  hold on;
  plot(freqs, wS_resp .* Hh_resp + wI_resp .* Hl_resp, '--', 'DisplayName', 'NP - $H_L$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  xlim([freqs(1), freqs(end)]);
  ylim([1e-3, 10]);
  xticks([0.1, 1, 10, 100, 1000]);
  legend('location', 'northeast');
#+end_src

#+begin_src matlab :exports none
  wT_resp = abs(squeeze(freqresp(wT, freqs, 'Hz')));
  wI_resp = abs(squeeze(freqresp(wI, freqs, 'Hz')));
  wS_resp = abs(squeeze(freqresp(wS, freqs, 'Hz')));

  figure;
  hold on;
  set(gca,'ColorOrderIndex',1)
  plot(freqs, 1./wT_resp, '--', 'DisplayName', 'NP - $H_L$');
  set(gca,'ColorOrderIndex',1)
  plot(freqs, 1./(wT_resp .* (1 + wI_resp) + wI_resp), ':', 'DisplayName', 'RP for T - $H_L$');
  set(gca,'ColorOrderIndex',1)
  plot(freqs, 1./(wI_resp .* (2 + wI_resp)), '-.', 'DisplayName', 'RS - $H_L$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, 1./wS_resp, '--', 'DisplayName', 'NP - $H_H$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, (1 + wI_resp)./(wS_resp .* (2 + wI_resp)), ':', 'DisplayName', 'RP for S - $H_H$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, wT_resp.*(1 + wI_resp)./(wS_resp.*(wT_resp .* (1 + wI_resp) + wI_resp)), '-', 'DisplayName', 'RP for S - $H_H$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  xlim([freqs(1), freqs(end)]);
  ylim([1e-3, 10]);
  xticks([0.1, 1, 10, 100, 1000]);
  legend('location', 'northeast');
#+end_src

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

#+NAME: fig:weights_NP_RS_RP
#+CAPTION: Upper bounds on the norm of the complementary filters for NP, RS and RP ([[./figs/weights_NP_RS_RP.png][png]], [[./figs/weights_NP_RS_RP.pdf][pdf]])
[[file:figs/weights_NP_RS_RP.png]]

* H-Infinity synthesis of complementary filters
  <<sec:h_infinity>>

We here synthesize the complementary filters using the $\mathcal{H}_\infty$ synthesis.
The goal is to specify upper bounds on the norms of $H_L$ and $H_H$ while ensuring their complementary property ($H_L + H_H = 1$).

In order to do so, we use the generalized plant shown on figure [[fig:sf_hinf_filters_plant_b]] where $w_L$ and $w_H$ weighting transfer functions that will be used to shape $H_L$ and $H_H$ respectively.

#+name: fig:sf_hinf_filters_plant_b
#+caption: Generalized plant used for the $\mathcal{H}_\infty$ synthesis of the complementary filters
[[file:figs-tikz/sf_hinf_filters_plant_b.png]]

The $\mathcal{H}_\infty$ synthesis applied on this generalized plant will give a transfer function $H_L$ (figure [[fig:sf_hinf_filters_b]]) such that the $\mathcal{H}_\infty$ norm of the transfer function from $w$ to $[z_H,\ z_L]$ is less than one:
\[ \left\| \begin{array}{c} H_L w_L \\ (1 - H_L) w_H \end{array} \right\|_\infty < 1 \]

Thus, if the above condition is verified, we can define $H_H = 1 - H_L$ and we have that:
\[ \left\| \begin{array}{c} H_L w_L \\ H_H w_H \end{array} \right\|_\infty < 1 \]
Which is almost (with an maximum error of $\sqrt{2}$) equivalent to:
\begin{align*}
  |H_L| &< \frac{1}{|w_L|}, \quad \forall \omega \\
  |H_H| &< \frac{1}{|w_H|}, \quad \forall \omega
\end{align*}

We then see that $w_L$ and $w_H$ can be used to shape both $H_L$ and $H_H$ while ensuring (by definition of $H_H = 1 - H_L$) their complementary property.

#+name: fig:sf_hinf_filters_b
#+caption: $\mathcal{H}_\infty$ synthesis of the complementary filters
[[file:figs-tikz/sf_hinf_filters_b.png]]


Thus, if we choose $w_L$ and $w_H$ such that $1/|w_L|$ and $1/|w_H|$ lie below the upper bounds of figure [[fig:weights_NP_RS_RP]], we will ensure the NP, RS and RP of the controlled system.

Depending if we are interested only in NP, RS or RP, we can adjust the weights $w_L$ and $w_H$.

#+begin_src matlab
  omegab = 2*pi*9;
  wH = (omegab)^2/(s + omegab*sqrt(1e-5))^2;
  omegab = 2*pi*28;
  wL = (s + omegab/(4.5)^(1/3))^3/(s*(1e-4)^(1/3) + omegab)^3;
#+end_src

#+begin_src matlab :exports none
  wT_resp = abs(squeeze(freqresp(wT, freqs, 'Hz')));
  wI_resp = abs(squeeze(freqresp(wI, freqs, 'Hz')));
  wS_resp = abs(squeeze(freqresp(wS, freqs, 'Hz')));
  Hh_resp = wT_resp.*(1 + wI_resp)./(wS_resp.*(wT_resp .* (1 + wI_resp) + wI_resp));
  Hl_resp = 1./(wT_resp .* (1 + wI_resp) + wI_resp);

  figure;
  hold on;
  plot(freqs, wS_resp .* Hh_resp + wI_resp .* Hl_resp, '--', 'DisplayName', 'NP - $H_L$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  xlim([freqs(1), freqs(end)]);
  ylim([1e-3, 10]);
  xticks([0.1, 1, 10, 100, 1000]);
  legend('location', 'northeast');
#+end_src

#+begin_src matlab :exports none
  wH_resp = abs(squeeze(freqresp(wH, freqs, 'Hz')));
  wL_resp = abs(squeeze(freqresp(wL, freqs, 'Hz')));

  figure;
  hold on;
  plot(freqs, wH_resp .* Hh_resp + wL_resp.*Hl_resp, '--', 'DisplayName', 'NP - $H_L$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  xlim([freqs(1), freqs(end)]);
  ylim([1e-3, 10]);
  xticks([0.1, 1, 10, 100, 1000]);
  legend('location', 'northeast');
#+end_src


#+begin_src matlab :exports none
  figure;
  hold on;
  set(gca,'ColorOrderIndex',1)
  plot(freqs, 1./abs(squeeze(freqresp(wT, freqs, 'Hz'))), '--', 'DisplayName', 'NP - $H_L$');
  set(gca,'ColorOrderIndex',1)
  plot(freqs, 1./(abs(squeeze(freqresp(wT, freqs, 'Hz'))) .* (1 + abs(squeeze(freqresp(wI, freqs, 'Hz')))) + abs(squeeze(freqresp(wI, freqs, 'Hz')))), ':', 'DisplayName', 'RP for T - $H_L$');
  set(gca,'ColorOrderIndex',1)
  plot(freqs, 1./(abs(squeeze(freqresp(wI, freqs, 'Hz'))) .* (2 + abs(squeeze(freqresp(wI, freqs, 'Hz'))))), '-.', 'DisplayName', 'RS - $H_L$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, 1./abs(squeeze(freqresp(wS, freqs, 'Hz'))), '--', 'DisplayName', 'NP - $H_H$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, (1 + abs(squeeze(freqresp(wI, freqs, 'Hz'))))./(abs(squeeze(freqresp(wS, freqs, 'Hz'))) .* (2 + abs(squeeze(freqresp(wI, freqs, 'Hz'))))), ':', 'DisplayName', 'RP for S - $H_H$');

  set(gca,'ColorOrderIndex',1)
  plot(freqs, 1./abs(squeeze(freqresp(wL, freqs, 'Hz'))), '-', 'DisplayName', '$w_L$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, 1./abs(squeeze(freqresp(wH, freqs, 'Hz'))), '-', 'DisplayName', '$w_H$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  xlim([freqs(1), freqs(end)]);
  ylim([1e-3, 10]);
  xticks([0.1, 1, 10, 100, 1000]);
  legend('location', 'northeast');
#+end_src

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

#+NAME: fig:weights_wl_wh
#+CAPTION: Weights on the complementary filters $w_L$ and $w_H$ and the associated performance weights ([[./figs/weights_wl_wh.png][png]], [[./figs/weights_wl_wh.pdf][pdf]])
[[file:figs/weights_wl_wh.png]]

We define the generalized plant $P$ on matlab.
#+begin_src matlab
  P = [0   wL;
       wH -wH;
       1   0];
#+end_src

And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command.
#+begin_src matlab :results output replace :exports both
  [Hl_hinf, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
#+end_src

#+RESULTS:
#+begin_example
[Hl_hinf, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
Test bounds:      0.0000 <  gamma  <=      1.7285

  gamma    hamx_eig  xinf_eig  hamy_eig   yinf_eig   nrho_xy   p/f
    1.729   4.1e+01   8.4e-12   1.8e-01    0.0e+00    0.0000    p
    0.864   3.9e+01 -5.8e-02#  1.8e-01    0.0e+00    0.0000    f
    1.296   4.0e+01   8.4e-12   1.8e-01    0.0e+00    0.0000    p
    1.080   4.0e+01   8.5e-12   1.8e-01    0.0e+00    0.0000    p
    0.972   3.9e+01 -4.2e-01#  1.8e-01    0.0e+00    0.0000    f
    1.026   4.0e+01   8.5e-12   1.8e-01    0.0e+00    0.0000    p
    0.999   3.9e+01   8.5e-12   1.8e-01    0.0e+00    0.0000    p
    0.986   3.9e+01 -1.2e+00#  1.8e-01    0.0e+00    0.0000    f
    0.993   3.9e+01 -8.2e+00#  1.8e-01    0.0e+00    0.0000    f
    0.996   3.9e+01   8.5e-12   1.8e-01    0.0e+00    0.0000    p
    0.994   3.9e+01   8.5e-12   1.8e-01    0.0e+00    0.0000    p
    0.993   3.9e+01 -3.2e+01#  1.8e-01    0.0e+00    0.0000    f

 Gamma value achieved:     0.9942
#+end_example

We then define the high pass filter $H_H = 1 - H_L$. The bode plot of both $H_L$ and $H_H$ is shown on figure [[fig:hinf_filters_results]].
#+begin_src matlab
  Hh_hinf = 1 - Hl_hinf;
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  set(gca,'ColorOrderIndex',1)
  plot(freqs, 1./abs(squeeze(freqresp(wL, freqs, 'Hz'))), '--', 'DisplayName', '$w_L$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, 1./abs(squeeze(freqresp(wH, freqs, 'Hz'))), '--', 'DisplayName', '$w_H$');

  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(Hl_hinf, freqs, 'Hz'))), '-', 'DisplayName', '$H_L$ - $\mathcal{H}_\infty$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, abs(squeeze(freqresp(Hh_hinf, freqs, 'Hz'))), '-', 'DisplayName', '$H_H$ - $\mathcal{H}_\infty$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  xlim([freqs(1), freqs(end)]);
  ylim([1e-3, 10]);
  xticks([0.1, 1, 10, 100, 1000]);
  legend('location', 'northeast');
#+end_src

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

#+NAME: fig:hinf_filters_results
#+CAPTION: Obtained complementary filters using $\mathcal{H}_\infty$ synthesis ([[./figs/hinf_filters_results.png][png]], [[./figs/hinf_filters_results.pdf][pdf]])
[[file:figs/hinf_filters_results.png]]

* Complementary filters using analytical formula
  <<sec:analytical_formula>>

We here use analytical formula for the complementary filters $H_L$ and $H_H$.

The first two formulas that are used to generate complementary filters are:
\begin{align*}
  H_L(s) &= \frac{(1+\alpha) (\frac{s}{\omega_0})+1}{\left((\frac{s}{\omega_0})+1\right) \left((\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1\right)}\\
  H_H(s) &= \frac{(\frac{s}{\omega_0})^2 \left((\frac{s}{\omega_0})+1+\alpha\right)}{\left((\frac{s}{\omega_0})+1\right) \left((\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1\right)}
\end{align*}
where:
- $\omega_0$ is the blending frequency in rad/s.
- $\alpha$ is used to change the shape of the filters:
  - Small values for $\alpha$ will produce high magnitude of the filters $|H_L(j\omega)|$ and $|H_H(j\omega)|$ near $\omega_0$ but smaller value for $|H_L(j\omega)|$ above $\approx 1.5 \omega_0$ and for $|H_H(j\omega)|$ below $\approx 0.7 \omega_0$
  - A large $\alpha$ will do the opposite

This is illustrated on figure [[fig:comp_filters_param_alpha]].
As it is usually wanted to have the $\| S \|_\infty < 2$, $\alpha$ between $0.5$ and $1$ gives a good trade-off between the performance and the robustness.
The slope of those filters at high and low frequencies is $-2$ and $2$ respectively for $H_L$ and $H_H$.

#+begin_src matlab :exports none
  freqs_study = logspace(-2, 2, 10000);
  alphas = [0.1, 1, 10];
  w0 = 2*pi*1;

  figure;
  ax1 = subaxis(2,1,1);
  hold on;
  for i = 1:length(alphas)
    alpha = alphas(i);
    Hh2 = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
    Hl2 = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
    set(gca,'ColorOrderIndex',i);
    plot(freqs_study, abs(squeeze(freqresp(Hh2, freqs_study, 'Hz'))));
    set(gca,'ColorOrderIndex',i);
    plot(freqs_study, abs(squeeze(freqresp(Hl2, freqs_study, 'Hz'))));
  end
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude');
  hold off;
  ylim([1e-3, 20]);
  % Phase
  ax2 = subaxis(2,1,2);
  hold on;
  for i = 1:length(alphas)
    alpha = alphas(i);
    Hh2 = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
    Hl2 = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
    set(gca,'ColorOrderIndex',i);
    plot(freqs_study, 180/pi*angle(squeeze(freqresp(Hh2, freqs_study, 'Hz'))), 'DisplayName', sprintf('$\\alpha = %g$', alpha));
    set(gca,'ColorOrderIndex',i);
    plot(freqs_study, 180/pi*angle(squeeze(freqresp(Hl2, freqs_study, 'Hz'))), 'HandleVisibility', 'off');
  end
  set(gca,'xscale','log');
  yticks(-180:90:180);
  ylim([-180 180]);
  xlabel('Relative Frequency $\frac{\omega}{\omega_0}$'); ylabel('Phase [deg]');
  legend('Location', 'northeast');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs_study(1), freqs_study(end)]);
#+end_src

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

#+NAME: fig:comp_filters_param_alpha
#+CAPTION: Effect of the parameter $\alpha$ on the shape of the generated second order complementary filters ([[./figs/comp_filters_param_alpha.png][png]], [[./figs/comp_filters_param_alpha.pdf][pdf]])
[[file:figs/comp_filters_param_alpha.png]]


The parameters $\alpha$ and $\omega_0$ are chosen in order to have that the complementary filters stay below the defined upper bounds.

The obtained complementary filters are shown on figure [[fig:complementary_filters_second_order]].
The Robust Performance is not fulfilled for $T$, and we see that the RP condition as a slop of $-3$. We thus have to use different formula for the complementary filters here.

#+begin_src matlab
  w0 = 2*pi*13;
  alpha = 0.8;

  Hh2_ana = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
  Hl2_ana = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  set(gca,'ColorOrderIndex',1)
  plot(freqs, 1./abs(squeeze(freqresp(wT, freqs, 'Hz'))), '--', 'DisplayName', 'NP - $H_L$');
  set(gca,'ColorOrderIndex',1)
  plot(freqs, 1./(abs(squeeze(freqresp(wT, freqs, 'Hz'))) .* (1 + abs(squeeze(freqresp(wI, freqs, 'Hz')))) + abs(squeeze(freqresp(wI, freqs, 'Hz')))), ':', 'DisplayName', 'RP for T - $H_L$');
  set(gca,'ColorOrderIndex',1)
  plot(freqs, 1./(abs(squeeze(freqresp(wI, freqs, 'Hz'))) .* (2 + abs(squeeze(freqresp(wI, freqs, 'Hz'))))), '-.', 'DisplayName', 'RS - $H_L$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, 1./abs(squeeze(freqresp(wS, freqs, 'Hz'))), '--', 'DisplayName', 'NP - $H_H$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, (1 + abs(squeeze(freqresp(wI, freqs, 'Hz'))))./(abs(squeeze(freqresp(wS, freqs, 'Hz'))) .* (2 + abs(squeeze(freqresp(wI, freqs, 'Hz'))))), ':', 'DisplayName', 'RP for S - $H_H$');

  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(Hl2_ana, freqs, 'Hz'))), '-', 'DisplayName', '$H_L$ - Analytical');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, abs(squeeze(freqresp(Hh2_ana, freqs, 'Hz'))), '-', 'DisplayName', '$H_H$ - Analytical');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  xlim([freqs(1), freqs(end)]);
  ylim([1e-3, 10]);
  xticks([0.1, 1, 10, 100, 1000]);
  legend('location', 'northeast');
#+end_src

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

#+NAME: fig:complementary_filters_second_order
#+CAPTION: Second order complementary filters using the analytical formula ([[./figs/complementary_filters_second_order.png][png]], [[./figs/complementary_filters_second_order.pdf][pdf]])
[[file:figs/complementary_filters_second_order.png]]


The following formula gives complementary filters with slopes of $-3$ and $3$:
\begin{align*}
  H_L(s) &= \frac{\left(1+(\alpha+1)(\beta+1)\right) (\frac{s}{\omega_0})^2 + (1+\alpha+\beta)(\frac{s}{\omega_0}) + 1}{\left(\frac{s}{\omega_0} + 1\right) \left( (\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1 \right) \left( (\frac{s}{\omega_0})^2 + \beta (\frac{s}{\omega_0}) + 1 \right)}\\
  H_H(s) &= \frac{(\frac{s}{\omega_0})^3 \left( (\frac{s}{\omega_0})^2 + (1+\alpha+\beta) (\frac{s}{\omega_0}) + (1+(\alpha+1)(\beta+1)) \right)}{\left(\frac{s}{\omega_0} + 1\right) \left( (\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1 \right) \left( (\frac{s}{\omega_0})^2 + \beta (\frac{s}{\omega_0}) + 1 \right)}
\end{align*}

The parameters are:
- $\omega_0$ is the blending frequency in rad/s
- $\alpha$ and $\beta$ that are used to change the shape of the filters similarly to the parameter $\alpha$ for the second order complementary filters

The filters are defined below and the result is shown on figure [[fig:complementary_filters_third_order]] where we can see that the complementary filters are below the defined upper bounds.

#+begin_src matlab
  alpha = 1;
  beta = 10;
  w0 = 2*pi*14;

  Hh3_ana = (s/w0)^3 * ((s/w0)^2 + (1+alpha+beta)*(s/w0) + (1+(alpha+1)*(beta+1)))/((s/w0 + 1)*((s/w0)^2+alpha*(s/w0)+1)*((s/w0)^2+beta*(s/w0)+1));
  Hl3_ana = ((1+(alpha+1)*(beta+1))*(s/w0)^2 + (1+alpha+beta)*(s/w0) + 1)/((s/w0 + 1)*((s/w0)^2+alpha*(s/w0)+1)*((s/w0)^2+beta*(s/w0)+1));
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  set(gca,'ColorOrderIndex',1)
  plot(freqs, 1./abs(squeeze(freqresp(wT, freqs, 'Hz'))), '--', 'DisplayName', 'NP - $H_L$');
  set(gca,'ColorOrderIndex',1)
  plot(freqs, 1./(abs(squeeze(freqresp(wT, freqs, 'Hz'))) .* (1 + abs(squeeze(freqresp(wI, freqs, 'Hz')))) + abs(squeeze(freqresp(wI, freqs, 'Hz')))), ':', 'DisplayName', 'RP for T - $H_L$');
  set(gca,'ColorOrderIndex',1)
  plot(freqs, 1./(abs(squeeze(freqresp(wI, freqs, 'Hz'))) .* (2 + abs(squeeze(freqresp(wI, freqs, 'Hz'))))), '-.', 'DisplayName', 'RS - $H_L$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, 1./abs(squeeze(freqresp(wS, freqs, 'Hz'))), '--', 'DisplayName', 'NP - $H_H$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, (1 + abs(squeeze(freqresp(wI, freqs, 'Hz'))))./(abs(squeeze(freqresp(wS, freqs, 'Hz'))) .* (2 + abs(squeeze(freqresp(wI, freqs, 'Hz'))))), ':', 'DisplayName', 'RP for S - $H_H$');

  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(Hl3_ana, freqs, 'Hz'))), '-', 'DisplayName', '$H_L$ - Analytical');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, abs(squeeze(freqresp(Hh3_ana, freqs, 'Hz'))), '-', 'DisplayName', '$H_H$ - Analytical');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  xlim([freqs(1), freqs(end)]);
  ylim([1e-3, 10]);
  xticks([0.1, 1, 10, 100, 1000]);
  legend('location', 'northeast');
#+end_src

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

#+NAME: fig:complementary_filters_third_order
#+CAPTION: Third order complementary filters using the analytical formula ([[./figs/complementary_filters_third_order.png][png]], [[./figs/complementary_filters_third_order.pdf][pdf]])
[[file:figs/complementary_filters_third_order.png]]

* Comparison of complementary filters
  <<sec:comp_filters>>
The generated complementary filters using $\mathcal{H}_\infty$ and the analytical formulas are compared on figure [[fig:comp_hinf_analytical]].

Although they are very close to each other, there is some difference to note here:
- the analytical formula provides a very simple way to generate the complementary filters (and thus the controller), they could even be used to tune the controller online using the parameters $\alpha$ and $\omega_0$. However, these formula have the property that $|H_H|$ and $|H_L|$ are symmetrical with the frequency $\omega_0$ which may not be desirable.
- while the $\mathcal{H}_\infty$ synthesis of the complementary filters is not as straightforward as using the analytical formula, it provides a more optimized procedure to obtain the complementary filters

The complementary filters obtained with the $\mathcal{H}_\infty$ will be used for further analysis.

#+begin_src matlab :exports none
  figure;

  ax1 = subplot(2,1,1);
  hold on;
  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(Hl_hinf, freqs, 'Hz'))), '--');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, abs(squeeze(freqresp(Hh_hinf, freqs, 'Hz'))), '--');

  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(Hl2_ana, freqs, 'Hz'))), '-');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, abs(squeeze(freqresp(Hh2_ana, freqs, 'Hz'))), '-');

  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(Hl3_ana, freqs, 'Hz'))), ':');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, abs(squeeze(freqresp(Hh3_ana, freqs, 'Hz'))), ':');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Magnitude');
  hold off;
  ylim([1e-4, 10]);

  ax2 = subplot(2,1,2);
  hold on;
  set(gca,'ColorOrderIndex',1)
  plot(freqs, 180/pi*phase(squeeze(freqresp(Hl_hinf, freqs, 'Hz'))), '--', 'DisplayName', '$H_L$ - $\mathcal{H}_\infty$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, 180/pi*phase(squeeze(freqresp(Hh_hinf, freqs, 'Hz'))), '--', 'DisplayName', '$H_H$ - $\mathcal{H}_\infty$');

  set(gca,'ColorOrderIndex',1)
  plot(freqs, 180/pi*phase(squeeze(freqresp(Hl2_ana, freqs, 'Hz'))), '-', 'DisplayName', '$H_L$ - $2$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, 180/pi*phase(squeeze(freqresp(Hh2_ana, freqs, 'Hz'))), '-', 'DisplayName', '$H_H$ - $2$');

  set(gca,'ColorOrderIndex',1)
  plot(freqs, 180/pi*phase(squeeze(freqresp(Hl3_ana, freqs, 'Hz'))), ':', 'DisplayName', '$H_L$ - $3$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, 180/pi*phase(squeeze(freqresp(Hh3_ana, freqs, 'Hz')))+360, ':', 'DisplayName', '$H_H$ - $3$');
  set(gca, 'XScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  yticks([-360:90:360]);
  legend('location', 'northeast');

  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
  xticks([0.1, 1, 10, 100, 1000]);
#+end_src

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

#+NAME: fig:comp_hinf_analytical
#+CAPTION: Comparison of the complementary filters obtained with $\mathcal{H}_\infty$ synthesis and with the analytical formula ([[./figs/comp_hinf_analytical.png][png]], [[./figs/comp_hinf_analytical.pdf][pdf]])
[[file:figs/comp_hinf_analytical.png]]

* Controller Analysis
  <<sec:controller_analysis>>

The controller $K$ is computed from the plant model $G$ and the low pass filter $H_H$:
\[ K = G^{-1} H_H^{-1} \]

As this is not proper and thus realizable, a second order low pass filter is added with a crossover frequency much larger than the control bandwidth.

#+begin_src matlab
  omega = 2*pi*1000;
  K = 1/(Hh_hinf*G) * 1/((1+s/omega)*(1+s/omega+(s/omega)^2));
#+end_src

#+begin_src matlab :exports none
  K = zpk(minreal(K));
#+end_src

#+begin_src matlab :results output replace :exports results :wrap example
  zpk(K)
#+end_src

#+RESULTS:
#+begin_example
zpk(K)

ans =

        4.961e12 (s+9.915e04) (s^2 + 5s + 500) (s^2 + 284.6s + 2.135e04) (s^2 + 130.5s + 9887)
  --------------------------------------------------------------------------------------------------
  (s+9.914e04) (s+6283) (s^2 + 0.3576s + 0.03198) (s^2 + 413.8s + 6.398e04) (s^2 + 6283s + 3.948e07)

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

The bode plot of the controller is shown on figure [[fig:bode_plot_controller]]:
- two integrator are present at low frequency
- the resonance of the plant at $3.5\ \text{Hz}$ is inverted (notched)
- a lead is added at $10\ \text{Hz}$

#+begin_src matlab :exports none
  figure;
  % Magnitude
  ax1 = subplot(2,1,1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(K, freqs, 'Hz'))), 'k-');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude [N/m]');
  % ylim([1e3, 1e8])
  hold off;

  % Phase
  ax2 = subplot(2,1,2);
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(K, freqs, 'Hz'))), 'k-');
  set(gca,'xscale','log');
  yticks(-180:90:180);
  ylim([-180 180]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
  xticks([0.1, 1, 10, 100, 1000])
#+end_src

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

#+NAME: fig:bode_plot_controller
#+CAPTION: Bode plot of the obtained controller $K$ ([[./figs/bode_plot_controller.png][png]], [[./figs/bode_plot_controller.pdf][pdf]])
[[file:figs/bode_plot_controller.png]]

* Nominal Stability and Nominal Performance
  <<sec:nominal_stability_performance>>

The nominal stability of the system is first checked with the =allmargin= matlab command.

#+begin_src matlab :results output replace
  allmargin(K*G*Hl_hinf)
#+end_src

#+RESULTS:
#+begin_example
allmargin(K*G*Hl_hinf)
ans =
  struct with fields:

     GainMargin: 4.46426896164391
    GMFrequency: 243.854595348016
    PhaseMargin: 35.7045152899792
    PMFrequency: 88.3664383511655
    DelayMargin: 0.00705201387841809
    DMFrequency: 88.3664383511655
         Stable: 1
#+end_example

The system is stable and the stability margins are good.

The bode plot of the loop gain $L = K*G*H_L$ is shown on figure [[fig:bode_plot_loop_gain]].

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

  % Phase
  ax2 = subplot(2,1,2);
  hold on;
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(K*G*Hl_hinf, freqs, 'Hz')))), 'k-');
  set(gca,'xscale','log');
  yticks(-270:90:0);
  ylim([-270 0]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
  xticks([0.1, 1, 10, 100, 1000])
#+end_src

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

#+NAME: fig:bode_plot_loop_gain
#+CAPTION: Bode Plot of the Loop Gain $L = K G H_L$ ([[./figs/bode_plot_loop_gain.png][png]], [[./figs/bode_plot_loop_gain.pdf][pdf]])
[[file:figs/bode_plot_loop_gain.png]]

In order to check the Nominal Performance of the system, we compute the sensibility and the complementary sensibility transfer functions.

#+begin_src matlab
  S = 1/(K*G*Hl_hinf + 1);
  T = K*G*Hl_hinf/(K*G*Hl_hinf + 1);
#+end_src

We then compare their norms with the upper bounds on the performance of the system (figure [[fig:verification_NP]]).
As expected, we guarantee the Nominal Performance of the system.

#+begin_src matlab :exports none
  figure;
  hold on;
  set(gca,'ColorOrderIndex',1)
  plot([100, 1000], [0.1, 0.001], ':', 'DisplayName', 'Upper bound on $|T|$');
  set(gca,'ColorOrderIndex',2)
  plot([0.1, 0.2, 2], [0.001, 0.001, 0.1], ':', 'DisplayName', 'Upper bound on $|S|$');
  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(T, freqs, 'Hz'))), '-', 'DisplayName', '$T$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, abs(squeeze(freqresp(S, freqs, 'Hz'))), '-', 'DisplayName', '$S$');

  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  xlim([freqs(1), freqs(end)]);
  ylim([1e-4, 10]);
  xticks([0.1, 1, 10, 100, 1000]);
  legend('location', 'northeast');
#+end_src

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

#+NAME: fig:verification_NP
#+CAPTION: Bode plot of $S$ and $T$ in order to verify the nominal performance of the system ([[./figs/verification_NP.png][png]], [[./figs/verification_NP.pdf][pdf]])
[[file:figs/verification_NP.png]]

* Robust Stability and Robust Performance
  <<sec:robustness_analysis>>
In order to verify the Robust stability of the system, we can use the following equivalence:
\[ \text{RS} \Leftrightarrow \left| w_I T \right| < 1 \quad \forall \omega  \]

This is shown on figure [[fig:robust_stability]].

#+begin_src matlab :exports none
  Ts = Gds*K*Hl_hinf/(Gds*K*Hl_hinf + 1);
  Ss = 1/(Gds*K*Hl_hinf + 1);
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, abs(squeeze(freqresp(wI*T, freqs, 'Hz'))), 'k-');
  plot([freqs(1) freqs(end)], [1 1], 'k--');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]');
  ylim([0.02, 2])
#+end_src

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

#+NAME: fig:robust_stability
#+CAPTION: Robust Stability Check: $|w_I T| < 1, \quad \forall \omega$ ([[./figs/robust_stability.png][png]], [[./figs/robust_stability.pdf][pdf]])
[[file:figs-tikz/robust_stability.png]]

To check Robust Stability, we can also look at the loop gain of the uncertain system (figure [[fig:loop_gain_robustness]]) or the Nyquist plot (figure [[fig:nyquist_robustness]]).

#+begin_src matlab :results silent :exports none
  figure;
  ax2 = subplot(2,1,1);
  hold on;
  for i=1:length(Gds)
      plot(freqs, abs(squeeze(freqresp(Gds(:, :, i)*K*Hl_hinf, freqs, 'Hz'))), '-', 'color', [0, 0, 0, 0.1]);
  end
  plot(freqs, abs(squeeze(freqresp(G*K*Hl_hinf, freqs, 'Hz'))), 'k-');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude [m/N]');
  ylim([1e-4 1e4]);
  hold off;
  % Phase
  ax2 = subplot(2,1,2);
  hold on;
  for i=1:length(Gds)
      plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gds(:, :, i)*K*Hl_hinf, freqs, 'Hz')))), '-', 'color', [0, 0, 0, 0.1]);
  end
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G*K*Hl_hinf, freqs, 'Hz')))), 'k-');
  set(gca,'xscale','log');
  yticks(-360:90:180);
  ylim([-270 0]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
  xticks([0.1, 1, 10, 100, 1000])
#+end_src

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

#+NAME: fig:loop_gain_robustness
#+CAPTION: Loop Gain of the uncertain system ([[./figs/loop_gain_robustness.png][png]], [[./figs/loop_gain_robustness.pdf][pdf]])
[[file:figs/loop_gain_robustness.png]]


#+begin_src matlab :exports none
  freqs_nyquist = logspace(0, 4, 100);

  figure;
  hold on;
  for i=1:length(Gds)
      plot(real(squeeze(freqresp(Gds(:, :, i)*K*Hl_hinf, freqs_nyquist, 'Hz'))), imag(squeeze(freqresp(Gds(:, :, i)*K*Hl_hinf, freqs_nyquist, 'Hz'))), 'color', [0, 0, 0, 0.1]);
  end
  plot(real(squeeze(freqresp(G*K*Hl_hinf, freqs_nyquist, 'Hz'))), imag(squeeze(freqresp(G*K*Hl_hinf, freqs_nyquist, 'Hz'))), 'k');
  hold off;
  xlim([-1.4, 0.2]); ylim([-1.4, 0.2]);
  xticks(-1.4:0.2:0.2); yticks(-1.4:0.2:0.2);
  xlabel('Real Part'); ylabel('Imaginary Part');
#+end_src

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

#+NAME: fig:nyquist_robustness
#+CAPTION: Nyquist plot of the uncertain system ([[./figs/nyquist_robustness.png][png]], [[./figs/nyquist_robustness.pdf][pdf]])
[[file:figs/nyquist_robustness.png]]

The Robust Performance is verified by plotting $|S|$ and $|T|$ for the uncertain system along side the upper bounds defined for performance.
This is shown on figure [[fig:robust_performance_result]] and we can indeed confirmed that the robust performance property of the system is valid.

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, abs(squeeze(freqresp(Ts(:, :, i), freqs, 'Hz'))), 'color', [0, 0.4470, 0.7410, 0.1]     , 'DisplayName', '$|T|$ - Uncertain');
  plot(freqs, abs(squeeze(freqresp(Ss(:, :, i), freqs, 'Hz'))), 'color', [0.8500, 0.3250, 0.0980, 0.1], 'DisplayName', '$|S|$ - Uncertain');

  for i=2:length(Gds)
      plot(freqs, abs(squeeze(freqresp(Ts(:, :, i), freqs, 'Hz'))), 'color', [0, 0.4470, 0.7410, 0.1]     , 'HandleVisibility', 'off');
      plot(freqs, abs(squeeze(freqresp(Ss(:, :, i), freqs, 'Hz'))), 'color', [0.8500, 0.3250, 0.0980, 0.1], 'HandleVisibility', 'off');
  end

  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(G*K*Hl_hinf/(1+G*K*Hl_hinf), freqs, 'Hz'))), 'DisplayName', '$|T|$ - Nominal');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, abs(squeeze(freqresp(1/(1+G*K*Hl_hinf), freqs, 'Hz'))), 'DisplayName', '$|S|$ - Nominal');

  set(gca,'ColorOrderIndex',1)
  plot([100, 1000], [0.1, 0.001], ':', 'DisplayName', '$|T|$ - Upper bound');
  set(gca,'ColorOrderIndex',2)
  plot([0.1, 0.2, 2], [0.001, 0.001, 0.1], ':', 'DisplayName', '$|S|$ - Upper bound');

  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  hold off;
  xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
  xlim([freqs(1), freqs(end)]);
  ylim([1e-4, 5]);
  xticks([0.1, 1, 10, 100, 1000]);
  legend('location', 'northeastoutside');
#+end_src

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

#+NAME: fig:robust_performance_result
#+CAPTION: Verification of the Robust Performance ([[./figs/robust_performance_result.png][png]], [[./figs/robust_performance_result.pdf][pdf]])
[[file:figs/robust_performance_result.png]]

* Pre-filter
  <<sec:pre_filter>>

For now, we have not specified any performance requirements on the input usage due to change of reference.
Do limit the input usage due to change of reference, we can use a pre-filter $K_r$ as shown on figure [[fig:sf_arch_class_prefilter]].

If we want a response time of 0.01 second, we can choose a first order low pass filter with a crossover frequency at $1/0.01 = 100\ \text{Hz}$ as defined below.

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

We then run a simulation for a step of $1\mu m$ for the system without and with the pre-filter $K_r$ (figure [[fig:u_and_y_with_Kr]]).
This confirms that a pre-filter can be used to limit the input usage due to change of reference using this architecture.

#+begin_src matlab
  t  = linspace(0, 0.02, 1000);

  opts = stepDataOptions;
  opts.StepAmplitude = 1e-6;

  u  = step((K)/(1+G*K*Hl_hinf),      t, opts);
  uf = step((Kr*K)/(1+G*K*Hl_hinf),   t, opts);
  y  = step((K*G)/(1+G*K*Hl_hinf),    t, opts);
  yf = step((Kr*G*K)/(1+G*K*Hl_hinf), t, opts);
#+end_src

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2,1,1);
  hold on;
  plot(t, u, 'k--', 'DisplayName', 'Without Pre-filter');
  plot(t, uf, 'k-', 'DisplayName', 'With Pre-Filter');
  hold off;
  ylabel('Command Input [N]');
  set(gca, 'XTickLabel',[]);
  legend('location', 'northeast');

  ax2 = subplot(2,1,2);
  hold on;
  plot(t, y,  'k--');
  plot(t, yf, 'k-' );
  hold off;
  xlabel('Time [s]');
  ylabel('Output [m]');
#+end_src

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

#+NAME: fig:u_and_y_with_Kr
#+CAPTION: Input usage and response due to a step change of reference when using a pre-filter $K_r$ ([[./figs/u_and_y_with_Kr.png][png]], [[./figs/u_and_y_with_Kr.pdf][pdf]])
[[file:figs/u_and_y_with_Kr.png]]

* Controller using classical techniques
  <<sec:sisotool_controller>>
A controller is designed using =SISOTOOL= with a bandwidth of approximately $20\ \text{Hz}$ and with two integrator.

The obtained controller is shown below.
#+begin_src matlab
  Kf = 1.1814e12*(s+10.15)*(s+9.036)*(s+53.8)/(s^2*(s+216.1)*(s+1200)*(s+1864));
#+end_src

#+begin_src matlab :results output :exports results replace :wrap example
  zpk(Kf)
#+end_src

#+RESULTS:
#+begin_example
zpk(Kf)

ans =

  1.1814e12 (s+10.15) (s+9.036) (s+53.8)
  --------------------------------------
     s^2 (s+216.1) (s+1200) (s+1864)

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

The loop gain for both cases are compared on figure [[fig:loop_gain_compare]].

#+begin_src matlab :exports none
  figure;
  % Magnitude
  ax1 = subplot(2,1,1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(K*G*Hl_hinf, freqs, 'Hz'))),  'k--');
  plot(freqs, abs(squeeze(freqresp(Kf*G, freqs, 'Hz'))), 'k-');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude [N/m]');
  % ylim([1e3, 1e8])
  hold off;

  % Phase
  ax2 = subplot(2,1,2);
  hold on;
  plot(freqs, 180/pi*angle(squeeze(freqresp(K*G*Hl_hinf, freqs, 'Hz'))), 'k--', 'DisplayName', '$K G H_L$ - $\mathcal{H}_\infty$');
  plot(freqs, 180/pi*angle(squeeze(freqresp(Kf*G, freqs, 'Hz'))), 'k-', 'DisplayName', '$K G$ - SISOTOOL');
  set(gca,'xscale','log');
  yticks(-180:90:180);
  ylim([-180 180]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  legend('location', 'northwest');

  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
  xticks([0.1, 1, 10, 100, 1000])
#+end_src

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

#+NAME: fig:loop_gain_compare
#+CAPTION: Comparison of the Loop Gains ([[./figs/loop_gain_compare.png][png]], [[./figs/loop_gain_compare.pdf][pdf]])
[[file:figs/loop_gain_compare.png]]

The Robust Stability of the system is verified using the Nyquist plot on figure [[fig:nyquist_plot_sisotool_controller]].

#+begin_src matlab :exports none
  freqs_nyquist = logspace(0, 4, 100);

  figure;
  hold on;
  for i=1:length(Gds)
      plot(real(squeeze(freqresp(Gds(:, :, i)*Kf, freqs_nyquist, 'Hz'))), imag(squeeze(freqresp(Gds(:, :, i)*Kf, freqs_nyquist, 'Hz'))), 'color', [0, 0, 0, 0.1]);
  end
  plot(real(squeeze(freqresp(G*Kf, freqs_nyquist, 'Hz'))), imag(squeeze(freqresp(G*Kf, freqs_nyquist, 'Hz'))), 'k');
  hold off;
  xlim([-1.4, 0.2]); ylim([-1.4, 0.2]);
  xticks(-1.4:0.2:0.2); yticks(-1.4:0.2:0.2);
  xlabel('Real Part'); ylabel('Imaginary Part');
#+end_src

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

#+NAME: fig:nyquist_plot_sisotool_controller
#+CAPTION: Nyquist Plot of the uncertain system ([[./figs/nyquist_plot_sisotool_controller.png][png]], [[./figs/nyquist_plot_sisotool_controller.pdf][pdf]])
[[file:figs/nyquist_plot_sisotool_controller.png]]

The closed loop sensitivity and complementary sensitivity transfer functions are computed.
And finally, the Robust Performance of both systems are compared on figure [[fig:robust_performance_compare]].

#+begin_src matlab :exports none
  Sf = 1/(Kf*G + 1);
  Tf = Kf*G/(Kf*G + 1);

  Tfs = Gds*Kf/(Gds*Kf + 1);
  Sfs = 1/(Gds*Kf + 1);
#+end_src


#+begin_src matlab :exports none
  figure;
  ax1 = subplot(1, 2, 1);
  title('$K$ - SISOTOOL');
  hold on;

  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(Tf, freqs, 'Hz'))), 'DisplayName', '$|T|$ - Nominal');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, abs(squeeze(freqresp(Sf, freqs, 'Hz'))), 'DisplayName', '$|S|$ - Nominal');

  plot(freqs, abs(squeeze(freqresp(Tfs(:, :, i), freqs, 'Hz'))), 'color', [0, 0.4470, 0.7410, 0.1]     , 'DisplayName', '$|T|$ - Uncertain');
  plot(freqs, abs(squeeze(freqresp(Sfs(:, :, i), freqs, 'Hz'))), 'color', [0.8500, 0.3250, 0.0980, 0.1], 'DisplayName', '$|S|$ - Uncertain');

  for i=2:length(Gds)
      plot(freqs, abs(squeeze(freqresp(Tfs(:, :, i), freqs, 'Hz'))), 'color', [0, 0.4470, 0.7410, 0.1]     , 'HandleVisibility', 'off');
      plot(freqs, abs(squeeze(freqresp(Sfs(:, :, i), freqs, 'Hz'))), 'color', [0.8500, 0.3250, 0.0980, 0.1], 'HandleVisibility', 'off');
  end

  set(gca,'ColorOrderIndex',1)
  plot([100, 1000], [0.1, 0.001], ':', 'DisplayName', '$|T|$ - Upper bound');
  set(gca,'ColorOrderIndex',2)
  plot([0.1, 0.2, 2], [0.001, 0.001, 0.1], ':', 'DisplayName', '$|S|$ - Upper bound');

  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  hold off;
  xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
  xlim([freqs(1), freqs(end)]);
  xticks([0.1, 1, 10, 100, 1000]);

  ax2 = subplot(1, 2, 2);
  title('$K$ - complementary filters');
  hold on;
  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(T, freqs, 'Hz'))), 'DisplayName', '$|T|$ - Nominal');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, abs(squeeze(freqresp(S, freqs, 'Hz'))), 'DisplayName', '$|S|$ - Nominal');

  plot(freqs, abs(squeeze(freqresp(Ts(:, :, i), freqs, 'Hz'))), 'color', [0, 0.4470, 0.7410, 0.1]     , 'DisplayName', '$|T|$ - Uncertain');
  plot(freqs, abs(squeeze(freqresp(Ss(:, :, i), freqs, 'Hz'))), 'color', [0.8500, 0.3250, 0.0980, 0.1], 'DisplayName', '$|S|$ - Uncertain');

  for i=2:length(Gds)
      plot(freqs, abs(squeeze(freqresp(Ts(:, :, i), freqs, 'Hz'))), 'color', [0, 0.4470, 0.7410, 0.1]     , 'HandleVisibility', 'off');
      plot(freqs, abs(squeeze(freqresp(Ss(:, :, i), freqs, 'Hz'))), 'color', [0.8500, 0.3250, 0.0980, 0.1], 'HandleVisibility', 'off');
  end

  set(gca,'ColorOrderIndex',1)
  plot([100, 1000], [0.1, 0.001], ':', 'DisplayName', '$|T|$ - Upper bound');
  set(gca,'ColorOrderIndex',2)
  plot([0.1, 0.2, 2], [0.001, 0.001, 0.1], ':', 'DisplayName', '$|S|$ - Upper bound');

  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  hold off;
  xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
  xlim([freqs(1), freqs(end)]);
  xticks([0.1, 1, 10, 100, 1000]);

  linkaxes([ax1 ax2], 'y')
  ylim([1e-4, 10]);
#+end_src

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

#+NAME: fig:robust_performance_compare
#+CAPTION: Comparison of the Robust Performance for both controllers ([[./figs/robust_performance_compare.png][png]], [[./figs/robust_performance_compare.pdf][pdf]])
[[file:figs/robust_performance_compare.png]]