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

#+BIND: org-latex-image-default-option "scale=1"
#+BIND: org-latex-image-default-width ""

#+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>
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="https://cdn.rawgit.com/dreampulse/computer-modern-web-font/master/fonts.css">

#+STARTUP: overview
#+OPTIONS: toc:2

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

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

#+LATEX_HEADER: \newcommand{\authorFirstName}{Thomas}
#+LATEX_HEADER: \newcommand{\authorLastName}{Dehaeze}
#+LATEX_HEADER: \newcommand{\authorEmail}{dehaeze.thomas@gmail.com}

#+LATEX_HEADER_EXTRA: \makeatletter
#+LATEX_HEADER_EXTRA: \preto\Gin@extensions{png,}
#+LATEX_HEADER_EXTRA: \DeclareGraphicsRule{.png}{pdf}{.pdf}{\noexpand\Gin@base.pdf}
#+LATEX_HEADER_EXTRA: \makeatother

#+LATEX_HEADER_EXTRA: \addbibresource{ref.bib}

#+PROPERTY: header-args:matlab  :session *MATLAB*
#+PROPERTY: header-args:matlab+ :tangle no
#+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

#+CSL_STYLE: ieee.csl
:END:

* Introduction                                                        :ignore:
In this document, the optimal and robust design of complementary filters is studied.

Two sensors are considered with both different noise characteristics and dynamical uncertainties represented by multiplicative input uncertainty.

- Section [[sec:optimal_comp_filters]]: the $\mathcal{H}_2$ synthesis is used to design complementary filters such that the RMS value of the super sensor's noise is minimized
- Section [[sec:comp_filter_robustness]]: the $\mathcal{H}_\infty$ synthesis is used to design complementary filters such that the super sensor's uncertainty is bonded to acceptable values
- Section [[sec:mixed_synthesis_sensor_fusion]]: the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis is used to both limit the super sensor's uncertainty and to lower the RMS value of the super sensor's noise

* Sensor Description
:PROPERTIES:
:header-args:matlab+: :tangle matlab/sensor_description.m
:header-args:matlab+: :comments org :mkdirp yes
:END:
<<sec:sensor_description>>

** Introduction                                                      :ignore:
In Figure [[fig:sensor_model_noise_uncertainty]] is shown a schematic of a sensor model that is used in the following study.
In this example, the measured quantity $x$ is the velocity of an object.

#+name: tab:sensor_signals
#+caption: Description of signals in Figure [[fig:sensor_model_noise_uncertainty]]
#+attr_latex: :environment tabular :align clc
#+attr_latex: :center t :booktabs t :float t
| *Notation*    | *Meaning*                       | *Unit*  |
|---------------+---------------------------------+---------|
| $x$           | Physical measured quantity      | $[m/s]$ |
| $\tilde{n}_i$ | White noise with unitary PSD    |         |
| $n_i$         | Shaped noise                    | $[m/s]$ |
| $v_i$         | Sensor output measurement       | $[V]$   |
| $\hat{x}_i$   | Estimate of $x$ from the sensor | $[m/s]$ |

#+name: tab:sensor_dynamical_blocks
#+caption: Description of Systems in Figure [[fig:sensor_model_noise_uncertainty]]
#+attr_latex: :environment tabular :align clc
#+attr_latex: :center t :booktabs t :float t
| *Notation*  | *Meaning*                                                                    | *Unit*            |
|-------------+------------------------------------------------------------------------------+-------------------|
| $\hat{G}_i$ | Nominal Sensor Dynamics                                                      | $[\frac{V}{m/s}]$ |
| $W_i$       | Weight representing the size of the uncertainty at each frequency            |                   |
| $\Delta_i$  | Any complex perturbation such that $\vert\vert\Delta_i\vert\vert_\infty < 1$ |                   |
| $N_i$       | Weight representing the sensor noise                                         | $[m/s]$           |

#+name: fig:sensor_model_noise_uncertainty
#+caption: Sensor Model
#+RESULTS:
[[file:figs-tikz/sensor_model_noise_uncertainty.png]]

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

** Sensor Dynamics
<<sec:sensor_dynamics>>
Let's consider two sensors measuring the velocity of an object.

The first sensor is an accelerometer.
Its nominal dynamics $\hat{G}_1(s)$ is defined below.
#+begin_src matlab
  m_acc = 0.01; % Inertial Mass [kg]
  c_acc = 5;    % Damping [N/(m/s)]
  k_acc = 1e5;  % Stiffness [N/m]
  g_acc = 1e5;  % Gain [V/m]

  G1 = g_acc*m_acc*s/(m_acc*s^2 + c_acc*s + k_acc); % Accelerometer Plant [V/(m/s)]
#+end_src

The second sensor is a displacement sensor, its nominal dynamics $\hat{G}_2(s)$ is defined below.
#+begin_src matlab
  w_pos = 2*pi*2e3; % Measurement Banwdith [rad/s]
  g_pos = 1e4; % Gain [V/m]

  G2 = g_pos/s/(1 + s/w_pos); % Position Sensor Plant [V/(m/s)]
#+end_src

These nominal dynamics are also taken as the model of the sensor dynamics.
The true sensor dynamics has some uncertainty associated to it and described in section [[sec:sensor_uncertainty]].

Both sensor dynamics in $[\frac{V}{m/s}]$ are shown in Figure [[fig:sensors_nominal_dynamics]].
#+begin_src matlab :exports none
  figure;
  % Magnitude
  ax1 = subplot(2,1,1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(G1, freqs, 'Hz'))), '-', 'DisplayName', '$G_1(j\omega)$');
  plot(freqs, abs(squeeze(freqresp(G2, freqs, 'Hz'))), '-', 'DisplayName', '$G_2(j\omega)$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Magnitude $[\frac{V}{m/s}]$'); set(gca, 'XTickLabel',[]);
  legend('location', 'northeast');
  hold off;

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

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

#+name: fig:sensors_nominal_dynamics
#+caption: Sensor nominal dynamics from the velocity of the object to the output voltage
#+RESULTS:
[[file:figs/sensors_nominal_dynamics.png]]

** Sensor Model Uncertainty
<<sec:sensor_uncertainty>>
The uncertainty on the sensor dynamics is described by multiplicative uncertainty (Figure [[fig:sensor_model_noise_uncertainty]]).

The true sensor dynamics $G_i(s)$ is then described by eqref:eq:sensor_dynamics_uncertainty.

\begin{equation}
  G_i(s) = \hat{G}_i(s) \left( 1 + W_i(s) \Delta_i(s) \right); \quad |\Delta_i(j\omega)| < 1 \forall \omega \label{eq:sensor_dynamics_uncertainty}
\end{equation}

The weights $W_i(s)$ representing the dynamical uncertainty are defined below and their magnitude is shown in Figure [[fig:sensors_uncertainty_weights]].
#+begin_src matlab
  W1 = createWeight('n', 2, 'w0', 2*pi*3,   'G0', 2, 'G1', 0.1,     'Gc', 1) * ...
       createWeight('n', 2, 'w0', 2*pi*1e3, 'G0', 1, 'G1', 4/0.1, 'Gc', 1/0.1);

  W2 = createWeight('n', 2, 'w0', 2*pi*1e2, 'G0', 0.05, 'G1', 4, 'Gc', 1);
#+end_src

The bode plot of the sensors nominal dynamics as well as their defined dynamical spread are shown in Figure [[fig:sensors_nominal_dynamics_and_uncertainty]].

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, abs(squeeze(freqresp(W1, freqs, 'Hz'))), 'DisplayName', '$|W_1(j\omega)|$');
  plot(freqs, abs(squeeze(freqresp(W2, freqs, 'Hz'))), 'DisplayName', '$|W_2(j\omega)|$');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  ylim([0, 5]);
  xlim([freqs(1), freqs(end)]);
  legend('location', 'northwest');
#+end_src

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

#+name: fig:sensors_uncertainty_weights
#+caption: Magnitude of the multiplicative uncertainty weights $|W_i(j\omega)|$
#+RESULTS:
[[file:figs/sensors_uncertainty_weights.png]]

#+begin_src matlab :exports none
  figure;
  % Magnitude
  ax1 = subplot(2,1,1);
  hold on;
  plotMagUncertainty(W1, freqs, 'G', G1, 'color_i', 1, 'DisplayName', '$G_1$');
  plotMagUncertainty(W2, freqs, 'G', G2, 'color_i', 2, 'DisplayName', '$G_2$');

  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(G1, freqs, 'Hz'))), 'DisplayName', '$\hat{G}_1$');
  plot(freqs, abs(squeeze(freqresp(G2, freqs, 'Hz'))), 'DisplayName', '$\hat{G}_2$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude $[\frac{V}{m/s}]$');
  ylim([1e-2, 2e3]);
  legend('location', 'northeast');
  hold off;

  % Phase
  ax2 = subplot(2,1,2);
  hold on;
  plotPhaseUncertainty(W1, freqs, 'G', G1, 'color_i', 1);
  plotPhaseUncertainty(W2, freqs, 'G', G2, 'color_i', 2);

  set(gca,'ColorOrderIndex',1)
  plot(freqs, 180/pi*angle(squeeze(freqresp(G1, freqs, 'Hz'))), 'DisplayName', '$\hat{G}_1$');
  plot(freqs, 180/pi*angle(squeeze(freqresp(G2, freqs, 'Hz'))), 'DisplayName', '$\hat{G}_2$');
  set(gca,'xscale','log');
  yticks(-180:90:180);
  ylim([-180 180]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
#+end_src

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

#+name: fig:sensors_nominal_dynamics_and_uncertainty
#+caption: Nominal Sensor Dynamics $\hat{G}_i$ (solid lines) as well as the spread of the dynamical uncertainty (background color)
#+RESULTS:
[[file:figs/sensors_nominal_dynamics_and_uncertainty.png]]

** Sensor Noise
<<sec:sensor_noise>>
The noise of the sensors $n_i$ are modelled by shaping a white noise with unitary PSD $\tilde{n}_i$ eqref:eq:unitary_noise_psd with a LTI transfer function $N_i(s)$ (Figure [[fig:sensor_model_noise_uncertainty]]).
\begin{equation}
  \Phi_{\tilde{n}_i}(\omega) = 1 \label{eq:unitary_noise_psd}
\end{equation}

The Power Spectral Density of the sensor noise $\Phi_{n_i}(\omega)$ is then computed using eqref:eq:sensor_noise_shaping and expressed in $[\frac{(m/s)^2}{Hz}]$.
\begin{equation}
  \Phi_{n_i}(\omega) = \left| N_i(j\omega) \right|^2 \Phi_{\tilde{n}_i}(\omega) \label{eq:sensor_noise_shaping}
\end{equation}

The weights $N_1$ and $N_2$ representing the amplitude spectral density of the sensor noises are defined below and shown in Figure [[fig:sensors_noise]].
#+begin_src matlab
  omegac = 0.15*2*pi; G0 = 1e-1; Ginf = 1e-6;
  N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4);

  omegac = 1000*2*pi; G0 = 1e-6; Ginf = 1e-3;
  N2 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4);
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, abs(squeeze(freqresp(N1, freqs, 'Hz'))), '-', 'DisplayName', '$|N_1(j\omega)|$');
  plot(freqs, abs(squeeze(freqresp(N2, freqs, 'Hz'))), '-', 'DisplayName', '$|N_2(j\omega)|$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Amplitude Spectral Density $\left[ \frac{m/s}{\sqrt{Hz}} \right]$');
  hold off;
  xlim([freqs(1), freqs(end)]);
  legend('location', 'northeast');
#+end_src

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

#+name: fig:sensors_noise
#+caption: Amplitude spectral density of the sensors $\sqrt{\Phi_{n_i}(\omega)} = |N_i(j\omega)|$
#+RESULTS:
[[file:figs/sensors_noise.png]]

** Save Model
All the dynamical systems representing the sensors are saved for further use.

#+begin_src matlab
  save('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
#+end_src

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

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

#+begin_src matlab
  load('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
#+end_src

** Complementary Filters
#+begin_src matlab
  wc = 2*pi*400;
  H1 = s/wc/(1 + s/wc);
  H2 = 1/(1 + s/wc);
#+end_src

#+begin_src matlab
  PSD_S1 = abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2;
  PSD_S2 = abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2;
  PSD_H2 = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;

  CPS_S1 = cumtrapz(freqs, PSD_S1);
  CPS_S2 = cumtrapz(freqs, PSD_S2);
  CPS_H2 = cumtrapz(freqs, PSD_H2);
#+end_src

#+begin_src matlab
  G2_u = G2*(1 + W2*ultidyn('Delta',[1 1]));
  G1_u = G1*(1 + W1*ultidyn('Delta',[1 1]));

  Gss_u = H1*inv(G1)*G1_u + H2*inv(G2)*G2_u;
#+end_src

#+begin_src matlab :exports none
  Dphi1 = 180/pi*asin(abs(squeeze(freqresp(W1, freqs, 'Hz'))));
  Dphi1(abs(squeeze(freqresp(W1, freqs, 'Hz'))) > 1) = 360;

  Dphi2 = 180/pi*asin(abs(squeeze(freqresp(W2, freqs, 'Hz'))));
  Dphi2(abs(squeeze(freqresp(W2, freqs, 'Hz'))) > 1) = 360;

  Dphi_ss = 180/pi*asin(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))));
  Dphi_ss(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))) > 1) = 360;

  figure;
  % Magnitude
  ax1 = subplot(2,1,1);
  hold on;
  p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W1, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W1, freqs, 'Hz'))), 1e-6))], 'w');
  p.FaceColor = [0 0.4470 0.7410];
  p.EdgeColor = 'none';
  p.FaceAlpha = 0.3;
  p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W2, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W2, freqs, 'Hz'))), 0.001))], 'w');
  p.FaceColor = [0.8500 0.3250 0.0980];
  p.EdgeColor = 'none';
  p.FaceAlpha = 0.3;
  p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))+abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))-abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 0.001))], 'w');
  p.EdgeColor = 'black';
  p.FaceAlpha = 0;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude');
  ylim([1e-2, 1e1]);
  hold off;

  % Phase
  ax2 = subplot(2,1,2);
  hold on;
  p = patch([freqs flip(freqs)], [Dphi1; flip(-Dphi1)], 'w');
  p.FaceColor = [0 0.4470 0.7410]; p.EdgeColor = 'none'; p.FaceAlpha = 0.3;
  p = patch([freqs flip(freqs)], [Dphi2; flip(-Dphi2)], 'w');
  p.FaceColor = [0.8500 0.3250 0.0980]; p.EdgeColor = 'none'; p.FaceAlpha = 0.3;
  p = patch([freqs flip(freqs)], [Dphi_ss; flip(-Dphi_ss)], 'w');
  p.EdgeColor = 'black'; p.FaceAlpha = 0;
  set(gca,'xscale','log');
  yticks(-180:90:180);
  ylim([-180 180]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
#+end_src


* Introduction to Sensor Fusion
<<sec:introduction_sensor_fusion>>

** Sensor Fusion Architecture
<<sec:sensor_fusion_architecture>>

The two sensors presented in Section [[sec:sensor_description]] are now merged together using complementary filters $H_1(s)$ and $H_2(s)$ to form a super sensor (Figure [[fig:sensor_fusion_noise_arch]]).

#+name: fig:sensor_fusion_noise_arch
#+caption: Sensor Fusion Architecture
[[file:figs-tikz/sensor_fusion_noise_arch.png]]

The complementary property of $H_1(s)$ and $H_2(s)$ means that the sum of their transfer function is equal to $1$ eqref:eq:complementary_property.

\begin{equation}
  H_1(s) + H_2(s) = 1 \label{eq:complementary_property}
\end{equation}

The super sensor estimate $\hat{x}$ is given by eqref:eq:super_sensor_estimate.

\begin{equation}
  \hat{x} = \left( H_1 \hat{G}_1^{-1} G_1 + H_2 \hat{G}_2^{-1} G_2 \right) x + \left( H_1 \hat{G}_1^{-1} G_1 N_1 \right) \tilde{n}_1 + \left( H_2 \hat{G}_2^{-1} G_2 N_2 \right) \tilde{n}_2 \label{eq:super_sensor_estimate}
\end{equation}

** Super Sensor Noise
<<sec:super_sensor_noise>>

If we first suppose that the models of the sensors $\hat{G}_i$ are very close to the true sensor dynamics $G_i$ eqref:eq:good_dynamical_model, we have that the super sensor estimate $\hat{x}$ is equals to the measured quantity $x$ plus the noise of the two sensors filtered out by the complementary filters eqref:eq:estimate_perfect_models.

\begin{equation}
  \hat{G}_i^{-1}(s) G_i(s) \approx 1 \label{eq:good_dynamical_model}
\end{equation}

\begin{equation}
  \hat{x} = x + \underbrace{\left( H_1 N_1 \right) \tilde{n}_1 + \left( H_2 N_2 \right) \tilde{n}_2}_{n} \label{eq:estimate_perfect_models}
\end{equation}

As the noise of both sensors are considered to be uncorrelated, the PSD of the super sensor noise is computed as follow:
\begin{equation}
  \Phi_n(\omega) = \left| H_1(j\omega) N_1(j\omega) \right|^2 + \left| H_2(j\omega) N_2(j\omega) \right|^2 \label{eq:super_sensor_psd_noise}
\end{equation}

And the Root Mean Square (RMS) value of the super sensor noise $\sigma_n$ is given by Equation eqref:eq:super_sensor_rms_noise.
\begin{equation}
  \sigma_n = \sqrt{\int_0^\infty \Phi_n(\omega) d\omega} \label{eq:super_sensor_rms_noise}
\end{equation}

** Super Sensor Dynamical Uncertainty
<<sec:super_sensor_dynamical_uncertainty>>

If we consider some dynamical uncertainty (the true system dynamics $G_i$ not being perfectly equal to our model $\hat{G}_i$) that we model by the use of multiplicative uncertainty (Figure [[fig:sensor_model_uncertainty]]), the super sensor dynamics is then equals to:

\begin{equation}
\begin{aligned}
  \frac{\hat{x}}{x} &= \Big(  H_1 \hat{G}_1^{-1} \hat{G}_1 (1 + W_1 \Delta_1) + H_2 \hat{G}_2^{-1} \hat{G}_2 (1 + W_2 \Delta_2) \Big) \\
                    &= \Big( H_1 (1 + W_1 \Delta_1) + H_2 (1 + W_2 \Delta_2) \Big) \\
                    &= \left( 1 + H_1 W_1 \Delta_1 + H_2 W_2 \Delta_2 \right), \quad \|\Delta_i\|_\infty<1
\end{aligned}
\end{equation}

#+name: fig:sensor_model_uncertainty
#+caption: Sensor Model including Dynamical Uncertainty
[[file:figs-tikz/sensor_model_uncertainty.png]]

The uncertainty set of the transfer function from $\hat{x}$ to $x$ at frequency $\omega$ is bounded in the complex plane by a circle centered on 1 and with a radius equal to $|W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)|$ as shown in Figure [[fig:uncertainty_set_super_sensor]].

#+name: fig:uncertainty_set_super_sensor
#+caption: Super Sensor model uncertainty displayed in the complex plane
[[file:figs-tikz/uncertainty_set_super_sensor.png]]

* Optimal Super Sensor Noise: $\mathcal{H}_2$ Synthesis
:PROPERTIES:
:header-args:matlab+: :tangle matlab/optimal_comp_filters.m
:header-args:matlab+: :comments org :mkdirp yes
:END:
<<sec:optimal_comp_filters>>

** Introduction                                                      :ignore:
In this section, the complementary filters $H_1(s)$ and $H_2(s)$ are designed in order to minimize the RMS value of super sensor noise $\sigma_n$.

#+name: fig:sensor_fusion_noise_arch
#+caption: Optimal Sensor Fusion Architecture
[[file:figs-tikz/sensor_fusion_noise_arch.png]]

The RMS value of the super sensor noise is (neglecting the model uncertainty):
\begin{equation}
\begin{aligned}
  \sigma_{n} &= \sqrt{\int_0^\infty |H_1(j\omega) N_1(j\omega)|^2 + |H_2(j\omega) N_2(j\omega)|^2 d\omega} \\
                   &= \left\| \begin{matrix} H_1 N_1 \\ H_2 N_2 \end{matrix} \right\|_2
\end{aligned}
\end{equation}

The goal is to design $H_1(s)$ and $H_2(s)$ such that $H_1(s) + H_2(s) = 1$ (complementary property) and such that $\left\| \begin{matrix} H_1 N_1 \\ H_2 N_2 \end{matrix} \right\|_2$ is minimized (minimized RMS value of the super sensor noise).
This is done using the $\mathcal{H}_2$ synthesis in Section [[sec:H2_synthesis]].

** 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
  addpath('src');
  load('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
#+end_src

** $\mathcal{H}_2$ Synthesis
<<sec:H2_synthesis>>

Consider the generalized plant $P_{\mathcal{H}_2}$ shown in Figure [[fig:h_two_optimal_fusion]] and described by Equation eqref:eq:H2_generalized_plant.

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

\begin{equation} \label{eq:H2_generalized_plant}
\begin{pmatrix}
  z_1 \\ z_2 \\ v
\end{pmatrix} = \underbrace{\begin{bmatrix}
  N_1 & -N_1 \\
  0   &  N_2 \\
  1   &  0
\end{bmatrix}}_{P_{\mathcal{H}_2}} \begin{pmatrix}
  w \\ u
\end{pmatrix}
\end{equation}

Applying the $\mathcal{H}_2$ synthesis on $P_{\mathcal{H}_2}$ will generate a filter $H_2(s)$ such that the $\mathcal{H}_2$ norm from $w$ to $(z_1,z_2)$ which is actually equals to $\sigma_n$ by defining $H_1(s) = 1 - H_2(s)$:
\begin{equation}
   \left\| \begin{matrix} z_1/w \\ z_2/w \end{matrix} \right\|_2 = \left\| \begin{matrix} N_1 (1 - H_2) \\ N_2 H_2 \end{matrix} \right\|_2 = \sigma_n \quad \text{with} \quad H_1(s) = 1 - H_2(s)
\end{equation}

We then have that the $\mathcal{H}_2$ synthesis applied on $P_{\mathcal{H}_2}$ generates two complementary filters $H_1(s)$ and $H_2(s)$ such that the RMS value of super sensor noise is minimized.

The generalized plant $P_{\mathcal{H}_2}$ is defined below
#+begin_src matlab
  PH2 = [N1 -N1;
         0   N2;
         1   0];
#+end_src

The $\mathcal{H}_2$ synthesis using the =h2syn= command
#+begin_src matlab
  [H2, ~, gamma] = h2syn(PH2, 1, 1);
#+end_src

Finally, $H_1(s)$ is defined as follows
#+begin_src matlab
  H1 = 1 - H2;
#+end_src

#+begin_src matlab :exports none
  % Filters are saved for further use
  save('./mat/H2_filters.mat', 'H2', 'H1');
#+end_src

The obtained complementary filters are shown in Figure [[fig:htwo_comp_filters]].
#+begin_src matlab :exports none
  figure;

  % Magnitude
  ax1 = subplot(2,1,1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), 'DisplayName', '$H_1$');
  plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), 'DisplayName', '$H_2$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude');
  hold off;
  legend('location', 'northeast');

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

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

#+name: fig:htwo_comp_filters
#+caption:  Obtained complementary filters using the $\mathcal{H}_2$ Synthesis
#+RESULTS:
[[file:figs/htwo_comp_filters.png]]

** Super Sensor Noise
<<sec:H2_super_sensor_noise>>

The Power Spectral Density of the individual sensors' noise $\Phi_{n_1}, \Phi_{n_2}$ and of the super sensor noise $\Phi_{n_{\mathcal{H}_2}}$ are computed below and shown in Figure [[fig:psd_sensors_htwo_synthesis]].
#+begin_src matlab
  PSD_S1 = abs(squeeze(freqresp(N1,    freqs, 'Hz'))).^2;
  PSD_S2 = abs(squeeze(freqresp(N2,    freqs, 'Hz'))).^2;
  PSD_H2 = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2 + ...
           abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
#+end_src

The corresponding Cumulative Power Spectrum $\Gamma_{n_1}$, $\Gamma_{n_2}$ and $\Gamma_{n_{\mathcal{H}_2}}$ (cumulative integration of the PSD eqref:eq:CPS_definition) are computed below and shown in Figure [[fig:cps_h2_synthesis]].
#+begin_src matlab
  CPS_S1 = cumtrapz(freqs, PSD_S1);
  CPS_S2 = cumtrapz(freqs, PSD_S2);
  CPS_H2 = cumtrapz(freqs, PSD_H2);
#+end_src

\begin{equation}
  \Gamma_n (\omega) = \int_0^\omega \Phi_n(\nu) d\nu \label{eq:CPS_definition}
\end{equation}

The RMS value of the individual sensors and of the super sensor are listed in Table [[tab:rms_noise_H2]].
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
  data2orgtable([sqrt(CPS_S1(end)); sqrt(CPS_S2(end)); sqrt(CPS_H2(end))], {'$\sigma_{n_1}$', '$\sigma_{n_2}$', '$\sigma_{n_{\mathcal{H}_2}}$'}, {'RMS value $[m/s]$'}, ' %.3f ');
#+end_src

#+name: tab:rms_noise_H2
#+caption: RMS value of the individual sensor noise and of the super sensor using the $\mathcal{H}_2$ Synthesis
#+attr_latex: :environment tabular :align cc
#+attr_latex: :center t :booktabs t :float t
#+RESULTS:
|                              | RMS value $[m/s]$ |
|------------------------------+-------------------|
| $\sigma_{n_1}$               |             0.015 |
| $\sigma_{n_2}$               |             0.080 |
| $\sigma_{n_{\mathcal{H}_2}}$ |             0.003 |

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, PSD_S1, '-',  'DisplayName', '$\Phi_{n_1}$');
  plot(freqs, PSD_S2, '-',  'DisplayName', '$\Phi_{n_2}$');
  plot(freqs, PSD_H2, 'k-', 'DisplayName', '$\Phi_{n_{\mathcal{H}_2}}$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Power Spectral Density [$(m/s)^2/Hz$]');
  hold off;
  xlim([freqs(1), freqs(end)]);
  legend('location', 'northeast');
#+end_src

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

#+name: fig:psd_sensors_htwo_synthesis
#+caption: Power Spectral Density of the estimated $\hat{x}$ using the two sensors alone and using the optimally fused signal
#+RESULTS:
[[file:figs/psd_sensors_htwo_synthesis.png]]

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, CPS_S1, '-',  'DisplayName', '$\Gamma_{n_1}$');
  plot(freqs, CPS_S2, '-',  'DisplayName', '$\Gamma_{n_2}$');
  plot(freqs, CPS_H2, 'k-', 'DisplayName', '$\Gamma_{n_{\mathcal{H}_2}}$');
  set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum $[(m/s)^2]$');
  hold off;
  xlim([2*freqs(1), freqs(end)]);
  legend('location', 'southeast');
#+end_src

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

#+name: fig:cps_h2_synthesis
#+caption: Cumulative Power Spectrum of individual sensors and super sensor using the $\mathcal{H}_2$ synthesis
#+RESULTS:
[[file:figs/cps_h2_synthesis.png]]

A time domain simulation is now performed.
The measured velocity $x$ is set to be a sweep sine with an amplitude of $0.1\ [m/s]$.
The velocity estimates from the two sensors and from the super sensors are shown in Figure [[fig:super_sensor_time_domain_h2]].
The resulting noises are displayed in Figure [[fig:sensor_noise_H2_time_domain]].

#+begin_src matlab :exports none
  Fs = 1e4;  % Sampling Frequency [Hz]
  Ts = 1/Fs; % Sampling Time [s]

  t = 0:Ts:2; % Time Vector [s]

  v = 0.1*sin((10*t).*t)'; % Velocity measured [m/s]

  % Generate noises in velocity corresponding to sensor 1 and 2:
  n1 = lsim(N1, sqrt(Fs/2)*randn(length(t), 1), t);
  n2 = lsim(N2, sqrt(Fs/2)*randn(length(t), 1), t);
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  set(gca,'ColorOrderIndex',2)
  plot(t, v + n2, 'DisplayName', '$\hat{x}_2$');
  set(gca,'ColorOrderIndex',1)
  plot(t, v + n1, 'DisplayName', '$\hat{x}_1$');
  set(gca,'ColorOrderIndex',3)
  plot(t, v + (lsim(H1, n1, t) + lsim(H2, n2, t)), 'DisplayName', '$\hat{x}$');
  plot(t, v, 'k--', 'DisplayName', '$x$');
  hold off;
  xlabel('Time [s]'); ylabel('Velocity [m/s]');
  legend('location', 'southwest');
  ylim([-0.3, 0.3]);
#+end_src

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

#+name: fig:super_sensor_time_domain_h2
#+caption: Noise of individual sensors and noise of the super sensor
#+RESULTS:
[[file:figs/super_sensor_time_domain_h2.png]]

#+begin_src matlab :exports none
  figure;
  hold on;
  set(gca,'ColorOrderIndex',2)
  plot(t, n2, 'DisplayName', '$n_2$');
  set(gca,'ColorOrderIndex',1)
  plot(t, n1, 'DisplayName', '$n_1$');
  set(gca,'ColorOrderIndex',3)
  plot(t, (lsim(H1, n1, t)+lsim(H2, n2, t)), '-', 'DisplayName', '$n$');
  hold off;
  xlabel('Time [s]'); ylabel('Sensor Noise [m/s]');
  legend();
  ylim([-0.2, 0.2]);
#+end_src

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

#+name: fig:sensor_noise_H2_time_domain
#+caption: Noise of the two sensors $n_1, n_2$ and noise of the super sensor $n$
#+RESULTS:
[[file:figs/sensor_noise_H2_time_domain.png]]

** Discrepancy between sensor dynamics and model
If we consider sensor dynamical uncertainty as explained in Section [[sec:sensor_uncertainty]], we can compute what would be the super sensor dynamical uncertainty when using the complementary filters obtained using the $\mathcal{H}_2$ Synthesis.

The super sensor dynamical uncertainty is shown in Figure [[fig:super_sensor_dynamical_uncertainty_H2]].

It is shown that the phase uncertainty is not bounded between 100Hz and 200Hz.
As a result the super sensor signal can not be used for feedback applications about 100Hz.
#+begin_src matlab :exports none
  Dphi_ss = 180/pi*asin(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))));
  Dphi_ss(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))) > 1) = 360;

  figure;
  % Magnitude
  ax1 = subplot(2,1,1);
  hold on;
  plotMagUncertainty(W1, freqs, 'color_i', 1, 'DisplayName', '$1 + W_1 \Delta_1$');
  plotMagUncertainty(W2, freqs, 'color_i', 2, 'DisplayName', '$1 + W_2 \Delta_2$');
  plot(freqs, 1 + abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))+abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 'k-', ...
       'DisplayName', '$1 + W_1 \Delta_1 + W_2 \Delta_2$')
  plot(freqs, max(1 - abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))-abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 0.001), 'k-', ...
       'HandleVisibility', 'off');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude');
  ylim([1e-2, 1e1]);
  legend('location', 'southeast');
  hold off;

  % Phase
  ax2 = subplot(2,1,2);
  hold on;
  plotPhaseUncertainty(W1, freqs, 'color_i', 1);
  plotPhaseUncertainty(W2, freqs, 'color_i', 2);
  plot(freqs,  Dphi_ss, 'k-');
  plot(freqs, -Dphi_ss, '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)]);
#+end_src

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

#+name: fig:super_sensor_dynamical_uncertainty_H2
#+caption: Super sensor dynamical uncertainty when using the $\mathcal{H}_2$ Synthesis
#+RESULTS:
[[file:figs/super_sensor_dynamical_uncertainty_H2.png]]

* Robust Sensor Fusion: $\mathcal{H}_\infty$ Synthesis
:PROPERTIES:
:header-args:matlab+: :tangle matlab/comp_filter_robustness.m
:header-args:matlab+: :comments org :mkdirp yes
:END:
<<sec:comp_filter_robustness>>

** Introduction                                                      :ignore:
We initially considered perfectly known sensor dynamics so that it can be perfectly inverted.

We now take into account the fact that the sensor dynamics is only partially known.
To do so, we model the uncertainty that we have on the sensor dynamics by multiplicative input uncertainty as shown in Figure [[fig:sensor_fusion_arch_uncertainty]].

#+name: fig:sensor_fusion_arch_uncertainty
#+caption: Sensor fusion architecture with sensor dynamics uncertainty
[[file:figs-tikz/sensor_fusion_arch_uncertainty.png]]

As explained in Section [[sec:sensor_uncertainty]], at each frequency $\omega$, the dynamical uncertainty of the super sensor can be represented in the complex plane by a circle with a radius equals to $|H_1(j\omega) W_1(j\omega)| + |H_2(j\omega) W_2(j\omega)|$ and centered on 1.

In order to specify a wanted upper bound on the dynamical uncertainty, a weight $W_u(s)$ is used where $1/|W_u(j\omega)|$ represents the maximum allowed radius of the uncertainty circle corresponding to the super sensor dynamics at a frequency $\omega$ eqref:eq:upper_bound_uncertainty.

\begin{align}
                  & |H_1(j\omega) W_1(j\omega)| + |H_2(j\omega) W_2(j\omega)| < \frac{1}{|W_u(j\omega)|}, \quad \forall \omega \label{eq:upper_bound_uncertainty} \\
  \Leftrightarrow & |H_1(j\omega) W_1(j\omega) W_u(j\omega)| + |H_2(j\omega) W_2(j\omega) W_u(j\omega)| < 1, \quad \forall\omega \label{eq:upper_bound_uncertainty_bis}
\end{align}


$|W_u(j\omega)|$ is also linked to the gain uncertainty $\Delta G$ eqref:eq:gain_uncertainty_bound and phase uncertainty $\Delta\phi$ eqref:eq:phase_uncertainty_bound of the super sensor.
\begin{align}
  \Delta G (\omega) &\le \frac{1}{|W_u(j\omega)|}, \quad \forall\omega \label{eq:gain_uncertainty_bound} \\
  \Delta \phi (\omega) &\le \arcsin\left(\frac{1}{|W_u(j\omega)|}\right), \quad \forall\omega \label{eq:phase_uncertainty_bound}
\end{align}

The choice of $W_u$ is presented in Section [[sec:weight_uncertainty]].


Condition eqref:eq:upper_bound_uncertainty_bis can almost be represented by eqref:eq:hinf_norm_uncertainty (within a factor $\sqrt{2}$).
\begin{equation}
  \left\| \begin{matrix} H_1(s) W_1(s) W_u(s) \\ H_2(s) W_2(s) W_u(s) \end{matrix} \right\|_\infty < 1 \label{eq:hinf_norm_uncertainty}
\end{equation}



The objective is to design $H_1(s)$ and $H_2(s)$ such that $H_1(s) + H_2(s) = 1$ (complementary property) and such that eqref:eq:hinf_norm_uncertainty is verified (bounded dynamical uncertainty).

This is done using the $\mathcal{H}_\infty$ synthesis in Section [[sec:Hinfinity_synthesis]].

** 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
  addpath('src');
  load('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
#+end_src

** Weighting Function used to bound the super sensor uncertainty
<<sec:weight_uncertainty>>

$W_u(s)$ is defined such that the super sensor phase uncertainty is less than 10 degrees below 100Hz eqref:eq:phase_uncertainy_bound_low_freq and is less than 180 degrees below 400Hz eqref:eq:phase_uncertainty_max.

\begin{align}
  \frac{1}{|W_u(j\omega)|} &< \sin\left(10 \frac{\pi}{180}\right), \quad \omega < 100\,\text{Hz} \label{eq:phase_uncertainy_bound_low_freq} \\
  \frac{1}{|W_u(j 2 \pi 400)|} &< 1 \label{eq:phase_uncertainty_max}
\end{align}

The uncertainty bounds of the two individual sensor as well as the wanted maximum uncertainty bounds of the super sensor are shown in Figure [[fig:weight_uncertainty_bounds_Wu]].

#+begin_src matlab
  Dphi = 10; % [deg]

  Wu = createWeight('n', 2, 'w0', 2*pi*4e2, 'G0', 1/sin(Dphi*pi/180), 'G1', 1/4, 'Gc', 1);
#+end_src

#+begin_src matlab :exports none
  % Wu is saved for further use
  save('./mat/Wu.mat', 'Wu');
#+end_src

#+begin_src matlab :exports none
  Dphi_Wu = 180/pi*asin(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))));
  Dphi_Wu(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))) > 1) = 360;

  figure;
  % Magnitude
  ax1 = subplot(2,1,1);
  hold on;
  plotMagUncertainty(W1, freqs, 'color_i', 1, 'DisplayName', '$1 + W_1 \Delta_1$');
  plotMagUncertainty(W2, freqs, 'color_i', 2, 'DisplayName', '$1 + W_2 \Delta_2$');
  plot(freqs, 1 + abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'k--', ...
       'DisplayName', '$1 + W_u^{-1} \Delta$')
  plot(freqs, 1 - abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'k--', ...
       'HandleVisibility', 'off')
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude');
  ylim([1e-2, 1e1]);
  legend('location', 'southeast');
  hold off;

  % Phase
  ax2 = subplot(2,1,2);
  hold on;
  plotPhaseUncertainty(W1, freqs, 'color_i', 1);
  plotPhaseUncertainty(W2, freqs, 'color_i', 2);
  plot(freqs,  Dphi_Wu, 'k--');
  plot(freqs, -Dphi_Wu, '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)]);
#+end_src

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

#+name: fig:weight_uncertainty_bounds_Wu
#+caption: Uncertainty region of the two sensors as well as the wanted maximum uncertainty of the super sensor (dashed lines)
#+RESULTS:
[[file:figs/weight_uncertainty_bounds_Wu.png]]

** $\mathcal{H}_\infty$ Synthesis
<<sec:Hinfinity_synthesis>>

The generalized plant $P_{\mathcal{H}_\infty}$ used for the $\mathcal{H}_\infty$ Synthesis of the complementary filters is shown in Figure [[fig:h_infinity_robust_fusion]] and is described by Equation eqref:eq:Hinf_generalized_plant.

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

\begin{equation} \label{eq:Hinf_generalized_plant}
\begin{pmatrix}
  z_1 \\ z_2 \\ v
\end{pmatrix} = \underbrace{\begin{bmatrix}
  W_u W_1 & -W_u W_1 \\
  0       &  W_u W_2 \\
  1       &  0
\end{bmatrix}}_{P_{\mathcal{H}_\infty}} \begin{pmatrix}
  w \\ u
\end{pmatrix}
\end{equation}

The generalized plant is defined below.
#+begin_src matlab
  P = [Wu*W1 -Wu*W1;
       0      Wu*W2;
       1      0];
#+end_src

And the $\mathcal{H}_\infty$ synthesis is performed using the =hinfsyn= command.
#+begin_src matlab :results output replace :exports both
  H2 = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'DISPLAY', 'on');
#+end_src

#+RESULTS:
#+begin_example
  Test bounds:  0.7071 <=  gamma  <=  1.291

    gamma        X>=0        Y>=0       rho(XY)<1    p/f
  9.554e-01     0.0e+00     0.0e+00     3.529e-16     p
  8.219e-01     0.0e+00     0.0e+00     5.204e-16     p
  7.624e-01     3.8e-17     0.0e+00     1.955e-15     p
  7.342e-01     0.0e+00     0.0e+00     5.612e-16     p
  7.205e-01     0.0e+00     0.0e+00     7.184e-16     p
  7.138e-01     0.0e+00     0.0e+00     0.000e+00     p
  7.104e-01     4.1e-16     0.0e+00     6.749e-15     p
  7.088e-01     0.0e+00     0.0e+00     2.794e-15     p
  7.079e-01     0.0e+00     0.0e+00     6.503e-16     p
  7.075e-01     0.0e+00     0.0e+00     4.302e-15     p

  Best performance (actual): 0.7071
#+end_example

The $\mathcal{H}_\infty$ is successful as the $\mathcal{H}_\infty$ norm of the "closed loop" transfer function from $(w)$ to $(z_1,\ z_2)$ is less than one.

$H_1(s)$ is then defined as the complementary of $H_2(s)$.
#+begin_src matlab
  H1 = 1 - H2;
#+end_src

#+begin_src matlab :exports none
  % Complementary filters are saved for further analysis
  save('./mat/Hinf_filters.mat', 'H2', 'H1');
#+end_src

The obtained complementary filters as well as the wanted upper bounds are shown in Figure [[fig:hinf_comp_filters]].

#+begin_src matlab :exports none
  figure;

  ax1 = subplot(2,1,1);
  hold on;
  plot(freqs, 1./abs(squeeze(freqresp(Wu*W1, freqs, 'Hz'))), '--', 'DisplayName', '$1/|W_uW_1|$');
  plot(freqs, 1./abs(squeeze(freqresp(Wu*W2, freqs, 'Hz'))), '--', 'DisplayName', '$1/|W_uW_2|$');

  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
  plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$');

  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Magnitude');
  set(gca, 'XTickLabel',[]);
  legend('location', 'northeast');

  ax2 = subplot(2,1,2);
  hold on;
  plot(freqs, 180/pi*phase(squeeze(freqresp(H1, freqs, 'Hz'))), '-');
  plot(freqs, 180/pi*phase(squeeze(freqresp(H2, freqs, 'Hz'))), '-');
  hold off;
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  set(gca, 'XScale', 'log');
  yticks([-360:90:360]);

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

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

#+name: fig:hinf_comp_filters
#+caption: Obtained complementary filters using the $\mathcal{H}_\infty$ Synthesis
#+RESULTS:
[[file:figs/hinf_comp_filters.png]]

** Super sensor uncertainty
The super sensor dynamical uncertainty is displayed in Figure [[fig:super_sensor_dynamical_uncertainty_Hinf]].
It is confirmed that the super sensor dynamical uncertainty is less than the maximum allowed uncertainty defined by the norm of $W_u(s)$.

The $\mathcal{H}_\infty$ synthesis thus allows to design filters such that the super sensor has specified bounded uncertainty.

#+begin_src matlab :exports none
  Dphi_Wu = 180/pi*asin(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))));
  Dphi_Wu(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))) > 1) = 360;

  Dphi_ss = 180/pi*asin(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))));
  Dphi_ss(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))) > 1) = 360;

  figure;
  % Magnitude
  ax1 = subplot(2,1,1);
  hold on;
  plotMagUncertainty(W1, freqs, 'color_i', 1, 'DisplayName', '$1 + W_1 \Delta_1$');
  plotMagUncertainty(W2, freqs, 'color_i', 2, 'DisplayName', '$1 + W_2 \Delta_2$');
  plot(freqs, 1 + abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))+abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 'k-', ...
       'DisplayName', '$1 + W_1 \Delta_1 + W_2 \Delta_2$')
  plot(freqs, max(1 - abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))-abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 0.001), 'k-', ...
       'HandleVisibility', 'off');
  plot(freqs, 1 + abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'k--', ...
       'DisplayName', '$1 + W_u^{-1}\Delta$')
  plot(freqs, 1 - abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'k--', ...
       'HandleVisibility', 'off')
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude');
  ylim([1e-2, 1e1]);
  legend('location', 'southeast');
  hold off;

  % Phase
  ax2 = subplot(2,1,2);
  hold on;
  plotPhaseUncertainty(W1, freqs, 'color_i', 1);
  plotPhaseUncertainty(W2, freqs, 'color_i', 2);
  plot(freqs,  Dphi_ss, 'k-');
  plot(freqs, -Dphi_ss, 'k-');
  plot(freqs,  Dphi_Wu, 'k--');
  plot(freqs, -Dphi_Wu, '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)]);
#+end_src

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

#+name: fig:super_sensor_dynamical_uncertainty_Hinf
#+caption: Super sensor dynamical uncertainty (solid curve) when using the $\mathcal{H}_\infty$ Synthesis
#+RESULTS:
[[file:figs/super_sensor_dynamical_uncertainty_Hinf.png]]

** Super sensor noise
We now compute the obtain Power Spectral Density of the super sensor's noise (Figure [[fig:psd_sensors_hinf_synthesis]]).

The obtained RMS of the super sensor noise in the $\mathcal{H}_2$ and $\mathcal{H}_\infty$ case are shown in Table [[tab:rms_noise_comp_H2_Hinf]].
As expected, the super sensor obtained from the $\mathcal{H}_\infty$ synthesis is much noisier than the super sensor obtained from the $\mathcal{H}_2$ synthesis.

#+begin_src matlab
  PSD_S2   = abs(squeeze(freqresp(N2,    freqs, 'Hz'))).^2;
  PSD_S1   = abs(squeeze(freqresp(N1,    freqs, 'Hz'))).^2;
  PSD_Hinf = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2 + ...
             abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
#+end_src

#+begin_src matlab :exports none
  H2_filters = load('./mat/H2_filters.mat', 'H2', 'H1');

  PSD_H2   = abs(squeeze(freqresp(N1*H2_filters.H1, freqs, 'Hz'))).^2 + ...
             abs(squeeze(freqresp(N2*H2_filters.H2, freqs, 'Hz'))).^2;
  CPS_H2   = cumtrapz(freqs, PSD_H2);
#+end_src

#+begin_src matlab :exports none
  CPS_S2   = cumtrapz(freqs, PSD_S2);
  CPS_S1   = cumtrapz(freqs, PSD_S1);
  CPS_Hinf = cumtrapz(freqs, PSD_Hinf);
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, PSD_S1,   '-',   'DisplayName', '$\Phi_{n_1}$');
  plot(freqs, PSD_S2,   '-',   'DisplayName', '$\Phi_{n_2}$');
  plot(freqs, PSD_H2,   'k-',  'DisplayName', '$\Phi_{n_{\mathcal{H}_2}}$');
  plot(freqs, PSD_Hinf, 'k--', 'DisplayName', '$\Phi_{n_{\mathcal{H}_\infty}}$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Power Spectral Density [$(m/s)^2/Hz$]');
  hold off;
  xlim([freqs(1), freqs(end)]);
  legend('location', 'northeast');
#+end_src

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

#+name: fig:psd_sensors_hinf_synthesis
#+caption: Power Spectral Density of the estimated $\hat{x}$ using the two sensors alone and using the
#+RESULTS:
[[file:figs/psd_sensors_hinf_synthesis.png]]

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
  data2orgtable([sqrt(CPS_H2(end)), sqrt(CPS_Hinf(end))]', {'Optimal: $\mathcal{H}_2$', 'Robust: $\mathcal{H}_\infty$'}, {'RMS [m/s]'}, ' %.1e ');
#+end_src

#+name: tab:rms_noise_comp_H2_Hinf
#+caption: Comparison of the obtained RMS noise of the super sensor
#+attr_latex: :environment tabular :align cc
#+attr_latex: :center t :booktabs t :float t
#+RESULTS:
|                              | RMS [m/s] |
|------------------------------+-----------|
| Optimal: $\mathcal{H}_2$     |    0.0027 |
| Robust: $\mathcal{H}_\infty$ |     0.041 |

** Conclusion
Using the $\mathcal{H}_\infty$ synthesis, the dynamical uncertainty of the super sensor can be bounded to acceptable values.

However, the RMS of the super sensor noise is not optimized as it was the case with the $\mathcal{H}_2$ synthesis

* Optimal and Robust Sensor Fusion: Mixed $\mathcal{H}_2/\mathcal{H}_\infty$ Synthesis
:PROPERTIES:
:header-args:matlab+: :tangle matlab/mixed_synthesis_sensor_fusion.m
:header-args:matlab+: :comments org :mkdirp yes
:END:
<<sec:mixed_synthesis_sensor_fusion>>
** Introduction                                                      :ignore:

#+name: fig:sensor_fusion_arch_full
#+caption: Sensor fusion architecture with sensor dynamics uncertainty
[[file:figs-tikz/sensor_fusion_arch_full.png]]

The goal is to design complementary filters such that:
- the maximum uncertainty of the super sensor is bounded
- the RMS value of the super sensor noise is minimized

To do so, we can use the Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis.

The Matlab function for that is =h2hinfsyn= ([[https://fr.mathworks.com/help/robust/ref/h2hinfsyn.html][doc]]).

** 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
  load('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
  load('./mat/Wu.mat', 'Wu');
#+end_src

** Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis
The synthesis architecture that is used here is shown in Figure [[fig:mixed_h2_hinf_synthesis]].

The controller $K$ is synthesized such that it:
- Keeps the $\mathcal{H}_\infty$ norm $G$ of the transfer function from $w$ to $z_\infty$ bellow some specified value
- Keeps the $\mathcal{H}_2$ norm $H$ of the transfer function from $w$ to $z_2$ bellow some specified value
- Minimizes a trade-off criterion of the form $W_1 G^2 + W_2 H^2$ where $W_1$ and $W_2$ are specified values

#+name: fig:mixed_h2_hinf_synthesis
#+caption: Mixed $\mathcal{H}_2/\mathcal{H}_\infty$ Synthesis
[[file:figs-tikz/mixed_h2_hinf_synthesis.png]]

Here, we define $P$ such that:
\begin{align*}
  \left\| \frac{z_\infty}{w} \right\|_\infty &= \left\| \begin{matrix}W_1(s) H_1(s) \\ W_2(s) H_2(s)\end{matrix} \right\|_\infty \\
  \left\| \frac{z_2}{w} \right\|_2           &= \left\| \begin{matrix}N_1(s) H_1(s) \\ N_2(s) H_2(s)\end{matrix} \right\|_2
\end{align*}

Then:
- we specify the maximum value for the $\mathcal{H}_\infty$ norm between $w$ and $z_\infty$ to be $1$
- we don't specify any maximum value for the $\mathcal{H}_2$ norm between $w$ and $z_2$
- we choose $W_1 = 0$ and $W_2 = 1$ such that the objective is to minimize the $\mathcal{H}_2$ norm between $w$ and $z_2$

The synthesis objective is to have:
\[ \left\| \frac{z_\infty}{w} \right\|_\infty = \left\| \begin{matrix}W_1(s) H_1(s) \\ W_2(s) H_2(s)\end{matrix} \right\|_\infty < 1 \]
and to minimize:
\[ \left\| \frac{z_2}{w} \right\|_2 = \left\| \begin{matrix}N_1(s) H_1(s) \\ N_2(s) H_2(s)\end{matrix} \right\|_2 \]
which is what we wanted.

We define the generalized plant that will be used for the mixed synthesis.
#+begin_src matlab
  W1u = ss(W2*Wu); W2u = ss(W1*Wu); % Weight on the uncertainty
  W1n = ss(N2); W2n = ss(N1); % Weight on the noise

  P = [W1u -W1u;
       0    W2u;
       W1n -W1n;
       0    W2n;
       1    0];
#+end_src

The mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis is performed below.
#+begin_src matlab
  Nmeas = 1; Ncon = 1; Nz2 = 2;

  [H1, ~, normz, ~] = h2hinfsyn(P, Nmeas, Ncon, Nz2, [0, 1], 'HINFMAX', 1, 'H2MAX', Inf, 'DKMAX', 100, 'TOL', 0.01, 'DISPLAY', 'on');

  H2 = 1 - H1;
#+end_src

#+begin_src matlab :exports none
  % The obtained filters are saved for further analysis
  save('./mat/H2_Hinf_filters.mat', 'H2', 'H1');
#+end_src

The obtained complementary filters are shown in Figure [[fig:htwo_hinf_comp_filters]].

#+begin_src matlab :exports none
  figure;

  ax1 = subplot(2,1,1);
  hold on;
  set(gca,'ColorOrderIndex',1)
  plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$W_1$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$W_2$');

  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$');

  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Magnitude');
  set(gca, 'XTickLabel',[]);
  ylim([1e-3, 2]);
  legend('location', 'southwest');

  ax2 = subplot(2,1,2);
  hold on;
  set(gca,'ColorOrderIndex',1)
  plot(freqs, 180/pi*phase(squeeze(freqresp(H2, freqs, 'Hz'))), '-');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, 180/pi*phase(squeeze(freqresp(H1, freqs, 'Hz'))), '-');
  hold off;
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  set(gca, 'XScale', 'log');
  yticks([-360:90:360]);

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

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

#+name: fig:htwo_hinf_comp_filters
#+caption: Obtained complementary filters after mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis
#+RESULTS:
[[file:figs/htwo_hinf_comp_filters.png]]

** Obtained Super Sensor's noise
The PSD and CPS of the super sensor's noise are shown in Figure [[fig:psd_sensors_htwo_hinf_synthesis]] and Figure [[fig:cps_h2_hinf_synthesis]] respectively.

#+begin_src matlab :exports none
  % The filters are loaded
  H2_filters      = load('./mat/H2_filters.mat',      'H2', 'H1');
  Hinf_filters    = load('./mat/Hinf_filters.mat',    'H2', 'H1');
#+end_src

#+begin_src matlab :exports none
  H2_filters = load('./mat/H2_filters.mat', 'H2', 'H1');

  PSD_H2   = abs(squeeze(freqresp(N1*H2_filters.H1, freqs, 'Hz'))).^2 + ...
             abs(squeeze(freqresp(N2*H2_filters.H2, freqs, 'Hz'))).^2;
  CPS_H2   = cumtrapz(freqs, PSD_H2);
#+end_src

#+begin_src matlab :exports none
  Hinf_filters = load('./mat/Hinf_filters.mat', 'H2', 'H1');

  PSD_Hinf   = abs(squeeze(freqresp(N1*Hinf_filters.H1, freqs, 'Hz'))).^2 + ...
               abs(squeeze(freqresp(N2*Hinf_filters.H2, freqs, 'Hz'))).^2;
  CPS_Hinf   = cumtrapz(freqs, PSD_Hinf);
#+end_src

#+begin_src matlab
  PSD_S2     = abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2;
  PSD_S1     = abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2;
  PSD_H2Hinf = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;

  CPS_S2     = cumtrapz(freqs, PSD_S2);
  CPS_S1     = cumtrapz(freqs, PSD_S1);
  CPS_H2Hinf = cumtrapz(freqs, PSD_H2Hinf);
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, PSD_S1,     '-',   'DisplayName', '$\Phi_{n_1}$');
  plot(freqs, PSD_S2,     '-',   'DisplayName', '$\Phi_{n_2}$');
  plot(freqs, PSD_H2,     'k-',  'DisplayName', '$\Phi_{n_{\mathcal{H}_2}}$');
  plot(freqs, PSD_Hinf,   'k--', 'DisplayName', '$\Phi_{n_{\mathcal{H}_\infty}}$');
  plot(freqs, PSD_H2Hinf, 'k-.', 'DisplayName', '$\Phi_{n_{\mathcal{H}_2/\mathcal{H}_\infty}}$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Power Spectral Density [$(m/s)^2/Hz$]');
  hold off;
  xlim([freqs(1), freqs(end)]);
  legend('location', 'northeast');
#+end_src

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

#+name: fig:psd_sensors_htwo_hinf_synthesis
#+CAPTION: Power Spectral Density of the Super Sensor obtained with the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis
#+RESULTS:
[[file:figs/psd_sensors_htwo_hinf_synthesis.png]]

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, CPS_S1,     '-',   'DisplayName', '$\Gamma_{n_1}$');
  plot(freqs, CPS_S2,     '-',   'DisplayName', '$\Gamma_{n_2}$');
  plot(freqs, CPS_H2,     'k-',  'DisplayName', '$\Gamma_{n_{\mathcal{H}_2}}$');
  plot(freqs, CPS_Hinf,   'k--', 'DisplayName', '$\Gamma_{n_{\mathcal{H}_\infty}}$');
  plot(freqs, CPS_H2Hinf, 'k-.', 'DisplayName', '$\Gamma_{n_{\mathcal{H}_2/\mathcal{H}_\infty}}$');
  set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum');
  hold off;
  xlim([2*freqs(1), freqs(end)]);
  legend('location', 'southeast');
#+end_src

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

#+name: fig:cps_h2_hinf_synthesis
#+CAPTION: Cumulative Power Spectrum of the Super Sensor obtained with the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis
#+RESULTS:
[[file:figs/cps_h2_hinf_synthesis.png]]

** Obtained Super Sensor's Uncertainty
The uncertainty on the super sensor's dynamics is shown in Figure

#+begin_src matlab :exports none
  Dphi_Wu = 180/pi*asin(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))));
  Dphi_Wu(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))) > 1) = 360;

  Dphi_ss = 180/pi*asin(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))));
  Dphi_ss(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))) > 1) = 360;

  figure;
  % Magnitude
  ax1 = subplot(2,1,1);
  hold on;
  plotMagUncertainty(W1, freqs, 'color_i', 1);
  plotMagUncertainty(W2, freqs, 'color_i', 2);
  p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))+abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))-abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 0.001))], 'w');
  p.EdgeColor = 'black'; p.FaceAlpha = 0;
  plot(freqs, 1 + abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'r--', ...
       'DisplayName', '$W_u$')
  plot(freqs, 1 - abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'r--', ...
       'HandleVisibility', 'off')
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude');
  ylim([1e-2, 1e1]);
  hold off;

  % Phase
  ax2 = subplot(2,1,2);
  hold on;
  plotPhaseUncertainty(W1, freqs, 'color_i', 1);
  plotPhaseUncertainty(W2, freqs, 'color_i', 2);
  p = patch([freqs flip(freqs)], [Dphi_ss; flip(-Dphi_ss)], 'w');
  p.EdgeColor = 'black'; p.FaceAlpha = 0;
  plot(freqs,  Dphi_Wu, 'r--');
  plot(freqs, -Dphi_Wu, 'r--');
  set(gca,'xscale','log');
  yticks(-180:90:180);
  ylim([-180 180]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
#+end_src

** Comparison Hinf H2 H2/Hinf
#+begin_src matlab
  H2_filters      = load('./mat/H2_filters.mat',      'H2', 'H1');
  Hinf_filters    = load('./mat/Hinf_filters.mat',    'H2', 'H1');
  H2_Hinf_filters = load('./mat/H2_Hinf_filters.mat', 'H2', 'H1');
#+end_src

#+begin_src matlab
  PSD_H2 = abs(squeeze(freqresp(N2*H2_filters.H2, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N1*H2_filters.H1, freqs, 'Hz'))).^2;
  CPS_H2 = cumtrapz(freqs, PSD_H2);

  PSD_Hinf = abs(squeeze(freqresp(N2*Hinf_filters.H2, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N1*Hinf_filters.H1, freqs, 'Hz'))).^2;
  CPS_Hinf = cumtrapz(freqs, PSD_Hinf);

  PSD_H2Hinf = abs(squeeze(freqresp(N2*H2_Hinf_filters.H2, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N1*H2_Hinf_filters.H1, freqs, 'Hz'))).^2;
  CPS_H2Hinf = cumtrapz(freqs, PSD_H2Hinf);
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
  data2orgtable([sqrt(CPS_H2(end)), sqrt(CPS_Hinf(end)), sqrt(CPS_H2Hinf(end))]', {'Optimal: $\mathcal{H}_2$', 'Robust: $\mathcal{H}_\infty$', 'Mixed: $\mathcal{H}_2/\mathcal{H}_\infty$'}, {'RMS [m/s]'}, ' %.1e ');
#+end_src

#+name: tab:rms_noise_comp
#+caption: Comparison of the obtained RMS noise of the super sensor
#+attr_latex: :environment tabular :align cc
#+attr_latex: :center t :booktabs t :float t
#+RESULTS:
|                                           | RMS [m/s] |
|-------------------------------------------+-----------|
| Optimal: $\mathcal{H}_2$                  |    0.0012 |
| Robust: $\mathcal{H}_\infty$              |     0.041 |
| Mixed: $\mathcal{H}_2/\mathcal{H}_\infty$ |     0.011 |

** Conclusion
This synthesis methods allows both to:
- limit the dynamical uncertainty of the super sensor
- minimize the RMS value of the estimation

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

This Matlab function is accessible [[file:src/createWeight.m][here]].

#+begin_src matlab
    function [W] = createWeight(args)
    % createWeight -
    %
    % Syntax: [in_data] = createWeight(in_data)
    %
    % Inputs:
    %    - n  - Weight Order
    %    - G0 - Low frequency Gain
    %    - G1 - High frequency Gain
    %    - Gc - Gain of W at frequency w0
    %    - w0 - Frequency at which |W(j w0)| = Gc
    %
    % Outputs:
    %    - W - Generated Weight

        arguments
            args.n  (1,1) double {mustBeInteger, mustBePositive} = 1
            args.G0 (1,1) double {mustBeNumeric, mustBePositive} = 0.1
            args.G1 (1,1) double {mustBeNumeric, mustBePositive} = 10
            args.Gc (1,1) double {mustBeNumeric, mustBePositive} = 1
            args.w0 (1,1) double {mustBeNumeric, mustBePositive} = 1
        end

      mustBeBetween(args.G0, args.Gc, args.G1);

      s = tf('s');

      W = (((1/args.w0)*sqrt((1-(args.G0/args.Gc)^(2/args.n))/(1-(args.Gc/args.G1)^(2/args.n)))*s + (args.G0/args.Gc)^(1/args.n))/((1/args.G1)^(1/args.n)*(1/args.w0)*sqrt((1-(args.G0/args.Gc)^(2/args.n))/(1-(args.Gc/args.G1)^(2/args.n)))*s + (1/args.Gc)^(1/args.n)))^args.n;

      end

      % Custom validation function
      function mustBeBetween(a,b,c)
          if ~((a > b && b > c) || (c > b && b > a))
              eid = 'createWeight:inputError';
              msg = 'Gc should be between G0 and G1.';
              throwAsCaller(MException(eid,msg))
          end
      end
#+end_src

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

This Matlab function is accessible [[file:src/plotMagUncertainty.m][here]].

#+begin_src matlab
  function [p] = plotMagUncertainty(W, freqs, args)
  % plotMagUncertainty -
  %
  % Syntax: [p] = plotMagUncertainty(W, freqs, args)
  %
  % Inputs:
  %    - W     - Multiplicative Uncertainty Weight
  %    - freqs - Frequency Vector [Hz]
  %    - args  - Optional Arguments:
  %      - G
  %      - color_i
  %      - opacity
  %
  % Outputs:
  %    - p - Plot Handle

  arguments
      W
      freqs double {mustBeNumeric, mustBeNonnegative}
      args.G = tf(1)
      args.color_i (1,1) double {mustBeInteger, mustBePositive} = 1
      args.opacity (1,1) double {mustBeNumeric, mustBeNonnegative} = 0.3
      args.DisplayName char = ''
  end

  % Get defaults colors
  colors = get(groot, 'defaultAxesColorOrder');

  p = patch([freqs flip(freqs)], ...
            [abs(squeeze(freqresp(args.G, freqs, 'Hz'))).*(1 + abs(squeeze(freqresp(W, freqs, 'Hz')))); ...
             flip(abs(squeeze(freqresp(args.G, freqs, 'Hz'))).*max(1 - abs(squeeze(freqresp(W, freqs, 'Hz'))), 1e-6))], 'w', ...
            'DisplayName', args.DisplayName);

  p.FaceColor = colors(args.color_i, :);
  p.EdgeColor = 'none';
  p.FaceAlpha = args.opacity;

  end
#+end_src

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

This Matlab function is accessible [[file:src/plotPhaseUncertainty.m][here]].

#+begin_src matlab
  function [p] = plotPhaseUncertainty(W, freqs, args)
  % plotPhaseUncertainty -
  %
  % Syntax: [p] = plotPhaseUncertainty(W, freqs, args)
  %
  % Inputs:
  %    - W     - Multiplicative Uncertainty Weight
  %    - freqs - Frequency Vector [Hz]
  %    - args  - Optional Arguments:
  %      - G
  %      - color_i
  %      - opacity
  %
  % Outputs:
  %    - p - Plot Handle

  arguments
      W
      freqs double {mustBeNumeric, mustBeNonnegative}
      args.G = tf(1)
      args.color_i (1,1) double {mustBeInteger, mustBePositive} = 1
      args.opacity (1,1) double {mustBeNumeric, mustBePositive} = 0.3
      args.DisplayName char = ''
  end

  % Get defaults colors
  colors = get(groot, 'defaultAxesColorOrder');

  % Compute Phase Uncertainty
  Dphi = 180/pi*asin(abs(squeeze(freqresp(W, freqs, 'Hz'))));
  Dphi(abs(squeeze(freqresp(W, freqs, 'Hz'))) > 1) = 360;

  % Compute Plant Phase
  G_ang = 180/pi*angle(squeeze(freqresp(args.G, freqs, 'Hz')));

  p = patch([freqs flip(freqs)], [G_ang+Dphi; flip(G_ang-Dphi)], 'w', ...
            'DisplayName', args.DisplayName);

  p.FaceColor = colors(args.color_i, :);
  p.EdgeColor = 'none';
  p.FaceAlpha = args.opacity;

  end
#+end_src

* Bibliography                                                        :ignore:
bibliographystyle:unsrt
bibliography:ref.bib

#+latex: \printbibliography