#+TITLE: Robust and Optimal Sensor Fusion - 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 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
: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
** Introduction                                                      :ignore:
In Figure [[fig:sensor_model_noise_uncertainty]] is shown a schematic of a sensor model that is used in the following study.

#+name: tab:sensor_signals
#+caption: Description of signals in Figure [[fig:sensor_model_noise_uncertainty]]
| *Notation*    | *Meaning*                       |
|---------------+---------------------------------|
| $x$           | Physical measured quantity      |
| $\tilde{n}_i$ | White noise with unitary PSD    |
| $n_i$         | Shaped noise                    |
| $v_i$         | Sensor output measurement       |
| $\hat{x}_i$   | Estimate of $x$ from the sensor |

#+name: tab:sensor_dynamical_blocks
#+caption: Description of Systems in Figure [[fig:sensor_model_noise_uncertainty]]
| *Notation*  | *Meaning*                                                                    |
|-------------+------------------------------------------------------------------------------|
| $\hat{G}_i$ | Nominal Sensor Dynamics                                                      |
| $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                                         |

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

In this example, the measured quantity $x$ is the velocity of an object.
The units of signals are listed in Table [[tab:signal_units]].
The units of systems are listed in Table [[tab:dynamical_block_units]].

#+name: tab:signal_units
#+caption: Units of signals in Figure [[fig:sensor_model_noise_uncertainty]]
| *Notation*    | *Unit*  |
|---------------+---------|
| $x$           | $[m/s]$ |
| $\tilde{n}_i$ |         |
| $n_i$         | $[m/s]$ |
| $v_i$         | $[V]$   |
| $\hat{x}_i$   | $[m/s]$ |


#+name: tab:dynamical_block_units
#+caption: Units of Systems in Figure [[fig:sensor_model_noise_uncertainty]]
| *Notation*       | *Unit*            |
|------------------+-------------------|
| $\hat{G}_i$      | $[\frac{V}{m/s}]$ |
| $\hat{G}_i^{-1}$ | $[\frac{m/s}{V}]$ |
| $W_i$            |                   |
| $\Delta_i$       |                   |
| $N_i$            | $[m/s]$           |

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


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

** Introduction                                                      :ignore:
The idea is to combine sensors that works in different frequency range using complementary filters.

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

Doing so, one "super sensor" is obtained that can have better noise characteristics than the individual sensors over a large frequency range.

The complementary filters have to be designed in order to minimize the effect noise of each sensor on the super sensor noise.

** ZIP file containing the data and matlab files                     :ignore:
#+begin_note
  The Matlab scripts is accessible [[file:matlab/optimal_comp_filters.m][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
  load('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
#+end_src

** H-Two Synthesis
As $\tilde{n}_1$ and $\tilde{n}_2$ are normalized white noise: $\Phi_{\tilde{n}_1}(\omega) = \Phi_{\tilde{n}_2}(\omega) = 1$ and we have:
\[ \sigma_{\hat{x}} = \sqrt{\int_0^\infty |H_1 N_1|^2(\omega) + |H_2 N_2|^2(\omega) d\omega} = \left\| \begin{matrix} H_1 N_1 \\ H_2 N_2 \end{matrix} \right\|_2 \]
Thus, the goal is to design $H_1(s)$ and $H_2(s)$ such that $H_1(s) + H_2(s) = 1$ and such that $\left\| \begin{matrix} H_1 N_1 \\ H_2 N_2 \end{matrix} \right\|_2$ is minimized.

For that, we use the $\mathcal{H}_2$ Synthesis.

We use the generalized plant architecture shown on Figure [[fig:h_two_optimal_fusion]].

#+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*}
\begin{pmatrix}
  z \\ v
\end{pmatrix} = \begin{pmatrix}
  0   &  N_2 & 1 \\
  N_1 & -N_2 & 0
\end{pmatrix} \begin{pmatrix}
  W_1 \\ W_2 \\ u
\end{pmatrix}
\end{equation*}

The transfer function from $[n_1, n_2]$ to $\hat{x}$ is:
\[ \begin{bmatrix} N_1 H_1 \\ N_2 (1 - H_1) \end{bmatrix} \]
If we define $H_2 = 1 - H_1$, we obtain:
\[ \begin{bmatrix} N_1 H_1 \\ N_2 H_2 \end{bmatrix} \]

Thus, if we minimize the $\mathcal{H}_2$ norm of this transfer function, we minimize the RMS value of $\hat{x}$.

We define the generalized plant $P$ on matlab as shown on figure [[fig:h_infinity_optimal_comp_filters]].
#+begin_src matlab
  P = [N1 -N1;
       0   N2;
       1   0];
#+end_src

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

Finally, we define $H_2(s) = 1 - H_1(s)$.
#+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 complementary filters obtained are shown on figure [[fig:htwo_comp_filters]].
#+begin_src matlab :exports none
  figure;
  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');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  xlim([freqs(1), freqs(end)]);
  legend('location', 'northeast');
#+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 ([[./figs/htwo_comp_filters.png][png]], [[./figs/htwo_comp_filters.pdf][pdf]])
#+RESULTS:
[[file:figs/htwo_comp_filters.png]]

** Sensor Noise
The PSD of the noise of the individual sensor and of the super sensor are shown in Figure [[fig:psd_sensors_htwo_synthesis]].

The Cumulative Power Spectrum (CPS) is shown on Figure [[fig:cps_h2_synthesis]].

The obtained RMS value of the super sensor is lower than the RMS value of the individual sensors.

#+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 :exports none
  figure;
  hold on;
  plot(freqs, PSD_S1, '-',  'DisplayName', '$\Phi_{\hat{x}_1}$');
  plot(freqs, PSD_S2, '-',  'DisplayName', '$\Phi_{\hat{x}_2}$');
  plot(freqs, PSD_H2, 'k-', 'DisplayName', '$\Phi_{\hat{x}_{\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', 'full', 'height', 'tall');
#+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 ([[./figs/psd_sensors_htwo_synthesis.png][png]], [[./figs/psd_sensors_htwo_synthesis.pdf][pdf]])
#+RESULTS:
[[file:figs/psd_sensors_htwo_synthesis.png]]

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, CPS_S1, '-',  'DisplayName', sprintf('$\\sigma_{\\hat{x}_1} = %.1e$ [m/s rms]', sqrt(CPS_S1(end))));
  plot(freqs, CPS_S2, '-',  'DisplayName', sprintf('$\\sigma_{\\hat{x}_2} = %.1e$ [m/s rms]', sqrt(CPS_S2(end))));
  plot(freqs, CPS_H2, 'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_2}} = %.1e$ [m/s rms]', sqrt(CPS_H2(end))));
  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_synthesis.pdf', 'width', 'full', 'height', 'tall');
#+end_src

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

#+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))]', {'Integrated Acceleration', 'Derived Position', 'Super Sensor - $\mathcal{H}_2$'}, {'RMS [m/s]'}, ' %.1e ');
#+end_src

#+RESULTS:
|                                | RMS [m/s] |
|--------------------------------+-----------|
| Integrated Acceleration        |     0.005 |
| Derived Position               |      0.08 |
| Super Sensor - $\mathcal{H}_2$ |    0.0012 |

** Time Domain Simulation
Parameters of the time domain simulation.
#+begin_src matlab
  Fs = 1e4; % Sampling Frequency [Hz]
  Ts = 1/Fs; % Sampling Time [s]

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

Time domain velocity.
#+begin_src matlab
  v = 0.1*sin((10*t).*t)';
#+end_src

Generate noises in velocity corresponding to sensor 1 and 2:
#+begin_src matlab
  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, n2, 'DisplayName', 'Differentiated Position');
  set(gca,'ColorOrderIndex',1)
  plot(t, n1, 'DisplayName', 'Integrated Acceleration');
  set(gca,'ColorOrderIndex',3)
  plot(t, (lsim(H1, n1, t)+lsim(H2, n2, t)), 'k-', 'DisplayName', 'Super Sensor');
  hold off;
  xlabel('Time [s]'); ylabel('Velocity [m/s]');
  legend();
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  set(gca,'ColorOrderIndex',2)
  plot(t, v+n2, 'DisplayName', 'Differentiated Position');
  set(gca,'ColorOrderIndex',1)
  plot(t, v+n1, 'DisplayName', 'Integrated Acceleration');
  set(gca,'ColorOrderIndex',3)
  plot(t, v+(lsim(H1, n1, t)+lsim(H2, n2, t)), 'DisplayName', 'Super Sensor');
  plot(t, v, 'k--', 'DisplayName', 'True Velocity');
  hold off;
  xlabel('Time [s]'); ylabel('Velocity [m/s]');
  legend();
  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', 'full', 'height', 'tall');
#+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]]

** Discrepancy between sensor dynamics and model
#+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);
  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;
  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;
  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

** Conclusion
From the above complementary filter design with the $\mathcal{H}_2$ and $\mathcal{H}_\infty$ synthesis, it still seems that the $\mathcal{H}_2$ synthesis gives the complementary filters that permits to obtain the minimal super sensor noise (when measuring with the $\mathcal{H}_2$ norm).

However, the synthesis does not take into account the robustness of the sensor fusion.

* Robust Sensor Fusion: $\mathcal{H}_\infty$ Synthesis with Acc and Pos
: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]]

The objective here is to design complementary filters $H_1(s)$ and $H_2(s)$ in order to minimize the dynamical uncertainty of the super sensor.

** ZIP file containing the data and matlab files                     :ignore:
#+begin_note
  The Matlab scripts is accessible [[file:matlab/comp_filter_robustness.m][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
  addpath('src');
  load('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
#+end_src

** Super Sensor Dynamical Uncertainty
In practical systems, the sensor dynamics has always some level of uncertainty.

The dynamics of the super sensor is represented by
\begin{align*}
  \frac{\hat{x}}{x} &= (1 + W_1 \Delta_1) H_1 + (1 + W_2 \Delta_2) H_2 \\
                    &= 1 + W_1 H_1 \Delta_1 + W_2 H_2 \Delta_2
\end{align*}
with $\Delta_i$ is any transfer function satisfying $\| \Delta_i \|_\infty < 1$.

We see that as soon as we have some uncertainty in the sensor dynamics, we have that the complementary filters have some effect on the transfer function from $x$ to $\hat{x}$.

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)|$ (figure [[fig:uncertainty_gain_phase_variation]]).

We then have that the angle introduced by the super sensor is bounded by $\arcsin(\epsilon)$:
\[ \angle \frac{\hat{x}}{x}(j\omega) \le \arcsin \Big(|W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)|\Big) \]

#+name: fig:uncertainty_gain_phase_variation
#+caption: Maximum phase variation
[[file:figs-tikz/uncertainty_gain_phase_variation.png]]

** Synthesis objective
The uncertainty region of the super sensor dynamics is represented by a circle in the complex plane as shown in Figure [[fig:uncertainty_gain_phase_variation]].

At each frequency $\omega$, the radius of the circle is $|W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)|$.

Thus, the phase shift $\Delta\phi(\omega)$ due to the super sensor uncertainty is bounded by:
\[ |\Delta\phi(\omega)| \leq \arcsin\big( |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| \big) \]

Let's define some allowed frequency depend phase shift $\Delta\phi_\text{max}(\omega) > 0$ such that:
\[ |\Delta\phi(\omega)| < \Delta\phi_\text{max}(\omega), \quad \forall\omega \]


If $H_1(s)$ and $H_2(s)$ are designed such that
\[ |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| < \sin\big( \Delta\phi_\text{max}(\omega) \big) \]

The maximum phase shift due to dynamic uncertainty at frequency $\omega$ will be $\Delta\phi_\text{max}(\omega)$.

** Requirements as an $\mathcal{H}_\infty$ norm
We now try to express this requirement in terms of an $\mathcal{H}_\infty$ norm.

Let's define one weight $W_\phi(s)$ that represents the maximum wanted phase uncertainty:
\[ |W_{\phi}(j\omega)|^{-1} \approx \sin(\Delta\phi_{\text{max}}(\omega)), \quad \forall\omega \]

Then:
\begin{align*}
                      & |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| < \sin\big( \Delta\phi_\text{max}(\omega) \big), \quad \forall\omega \\
  \Longleftrightarrow & |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| < |W_\phi(j\omega)|^{-1}, \quad \forall\omega \\
  \Longleftrightarrow & \left| W_1(j\omega) H_1(j\omega) W_\phi(j\omega) \right| + \left| W_2(j\omega) H_2(j\omega) W_\phi(j\omega) \right| < 1, \quad \forall\omega
\end{align*}

Which is approximately equivalent to (with an error of maximum $\sqrt{2}$):
#+name: eq:hinf_conf_phase_uncertainty
\begin{equation}
  \left\| \begin{matrix} W_1(s) W_\phi(s) H_1(s) \\ W_2(s) W_\phi(s) H_2(s) \end{matrix} \right\|_\infty < 1
\end{equation}

One should not forget that at frequency where both sensors has unknown dynamics ($|W_1(j\omega)| > 1$ and $|W_2(j\omega)| > 1$), the super sensor dynamics will also be unknown and the phase uncertainty cannot be bounded.
Thus, at these frequencies, $|W_\phi|$ should be smaller than $1$.

** Weighting Function used to bound the super sensor uncertainty
Let's define $W_\phi(s)$ in order to bound the maximum allowed phase uncertainty $\Delta\phi_\text{max}$ of the super sensor dynamics.

#+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
  save('./mat/Wu.mat', 'Wu');
#+end_src

#+begin_src matlab :exports none
  figure;
  % Magnitude
  ax1 = subplot(2,1,1);
  hold on;
  plotMagUncertainty(W1, freqs, 'color_i', 1);
  plotMagUncertainty(W2, freqs, 'color_i', 2);
  p = plotMagUncertainty(inv(Wu), freqs, 'color_i', 3);
  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;
  plotPhaseUncertainty(W1, freqs, 'color_i', 1);
  plotPhaseUncertainty(W2, freqs, 'color_i', 2);
  p = plotPhaseUncertainty(inv(Wu), freqs);
  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

The obtained upper bounds on the complementary filters in order to limit the phase uncertainty of the super sensor are represented in Figure [[fig:upper_bounds_comp_filter_max_phase_uncertainty]].

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, 1./abs(squeeze(freqresp(Wu*W1, freqs, 'Hz'))), '-', 'DisplayName', '$1/|W_1W_\phi|$');
  plot(freqs, 1./abs(squeeze(freqresp(Wu*W2, freqs, 'Hz'))), '-', 'DisplayName', '$1/|W_2W_\phi|$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  xlim([freqs(1), freqs(end)]);
  legend('location', 'northeast');
#+end_src

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

#+NAME: fig:upper_bounds_comp_filter_max_phase_uncertainty
#+CAPTION: Upper bounds on the complementary filters set in order to limit the maximum phase uncertainty of the super sensor to 30 degrees until 500Hz ([[./figs/upper_bounds_comp_filter_max_phase_uncertainty.png][png]], [[./figs/upper_bounds_comp_filter_max_phase_uncertainty.pdf][pdf]])
[[file:figs/upper_bounds_comp_filter_max_phase_uncertainty.png]]

** $\mathcal{H}_\infty$ Synthesis
The $\mathcal{H}_\infty$ synthesis architecture used for the complementary filters is shown in Figure [[fig:h_infinity_robust_fusion]].

#+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]]

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

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

#+RESULTS:
#+begin_example
[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');

  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

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

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

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

  ax1 = subplot(2,1,1);
  hold on;
  plot(freqs, 1./abs(squeeze(freqresp(Wu*W1, freqs, 'Hz'))), '--', 'DisplayName', '$|WuW_1|$');
  plot(freqs, 1./abs(squeeze(freqresp(Wu*W2, freqs, 'Hz'))), '--', 'DisplayName', '$|WuW_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)]);
  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_filter_hinf_uncertainty.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:comp_filter_hinf_uncertainty
#+CAPTION: Obtained complementary filters ([[./figs/comp_filter_hinf_uncertainty.png][png]], [[./figs/comp_filter_hinf_uncertainty.pdf][pdf]])
[[file:figs/comp_filter_hinf_uncertainty.png]]

** Super sensor uncertainty
#+begin_src matlab
  H2_filters = load('./mat/H2_filters.mat', 'H2', 'H1');
#+end_src

#+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;

  Dphi_ss_H2 = 180/pi*asin(abs(squeeze(freqresp(W2*H2_filters.H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H2_filters.H1, freqs, 'Hz'))));
  Dphi_ss_H2(abs(squeeze(freqresp(W2*H2_filters.H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H2_filters.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;
  p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W2*H2_filters.H2, freqs, 'Hz')))+abs(squeeze(freqresp(W1*H2_filters.H1, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W2*H2_filters.H2, freqs, 'Hz')))-abs(squeeze(freqresp(W1*H2_filters.H1, freqs, 'Hz'))), 0.001))], 'w');
  p.EdgeColor = 'black'; p.FaceAlpha = 0; p.LineStyle = '--';
  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;
  p = patch([freqs flip(freqs)], [Dphi_ss_H2; flip(-Dphi_ss_H2)], 'w');
  p.EdgeColor = 'black'; p.FaceAlpha = 0; p.LineStyle = '--';
  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

The uncertainty of the super sensor cannot be made smaller than both the individual sensor. Ideally, it would follow the minimum uncertainty of both sensors.

We here just used very wimple weights.
For instance, we could improve the dynamical uncertainty of the super sensor by making $|W_\phi(j\omega)|$ smaller bellow 2Hz where the dynamical uncertainty of the sensor 1 is small.

** Super sensor noise
We now compute the obtain Power Spectral Density of the super sensor's noise.
The noise characteristics of both individual sensor are defined below.

The PSD of both sensor and of the super sensor is shown in Figure [[fig:psd_sensors_hinf_synthesis]].
The CPS of both sensor and of the super sensor is shown in Figure [[fig:cps_sensors_hinf_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;
  PSD_H2   = abs(squeeze(freqresp(N1*H2_filters.H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2_filters.H2, freqs, 'Hz'))).^2;

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

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, PSD_S2,    '-',  'DisplayName', '$\Phi_{\hat{x}_{pos}}$');
  plot(freqs, PSD_S1,    '-',  'DisplayName', '$\Phi_{\hat{x}_{acc}}$');
  plot(freqs, PSD_Hinf, 'k-',  'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_\infty}}$');
  plot(freqs, PSD_H2,   'k--', 'DisplayName', '$\Phi_{\hat{x}_{\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

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

#+NAME: fig:psd_sensors_hinf_synthesis
#+CAPTION: Power Spectral Density of the obtained super sensor using the $\mathcal{H}_\infty$ synthesis ([[./figs/psd_sensors_hinf_synthesis.png][png]], [[./figs/psd_sensors_hinf_synthesis.pdf][pdf]])
[[file:figs/psd_sensors_hinf_synthesis.png]]

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, CPS_S2, '-',  'DisplayName', sprintf('$\\sigma_{\\hat{x}_{pos}} = %.1e$ [m/s rms]', sqrt(CPS_S2(end))));
  plot(freqs, CPS_S1, '-',  'DisplayName', sprintf('$\\sigma_{\\hat{x}_{acc}} = %.1e$ [m/s rms]', sqrt(CPS_S1(end))));
  plot(freqs, CPS_Hinf,  'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_\\infty}} = %.1e$ [m/s rms]', sqrt(CPS_Hinf(end))));
  set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum');
  hold off;
  xlim([2*freqs(1), freqs(end)]);
  % ylim([1e-10 1e-5]);
  legend('location', 'southeast');
#+end_src

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

#+NAME: fig:cps_sensors_hinf_synthesis
#+CAPTION: Cumulative Power Spectrum of the obtained super sensor using the $\mathcal{H}_\infty$ synthesis ([[./figs/cps_sensors_hinf_synthesis.png][png]], [[./figs/cps_sensors_hinf_synthesis.cps][cps]])
[[file:figs/cps_sensors_hinf_synthesis.png]]

** 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 with Acc and Pos
: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]]

** ZIP file containing the data and matlab files                    :ignore:
#+begin_note
  The Matlab scripts is accessible [[file:matlab/mixed_synthesis_sensor_fusion.m][here]].
#+end_note

** Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis - Introduction
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

** Noise characteristics and Uncertainty of the individual sensors
Both dynamical uncertainty and noise characteristics of the individual sensors are shown in Figure [[fig:mixed_synthesis_noise_uncertainty_sensors]].

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2, 1, 1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(N2, freqs, 'Hz'))), '-', 'DisplayName', '$|N_{pos}(j\omega)|$');
  plot(freqs, abs(squeeze(freqresp(N1, freqs, 'Hz'))), '-', 'DisplayName', '$|N_{acc}(j\omega)|$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  legend('location', 'northeast');

  ax2 = subplot(2, 1, 2);
  hold on;
  plot(freqs, abs(squeeze(freqresp(W2, freqs, 'Hz'))), '-', 'DisplayName', '$|W_{pos}(j\omega)|$');
  plot(freqs, abs(squeeze(freqresp(W1, freqs, 'Hz'))), '-', 'DisplayName', '$|W_{acc}(j\omega)|$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  legend('location', 'northeast');

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

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

#+NAME: fig:mixed_synthesis_noise_uncertainty_sensors
#+CAPTION: Noise characteristsics and Dynamical uncertainty of the individual sensors ([[./figs/mixed_synthesis_noise_uncertainty_sensors.png][png]], [[./figs/mixed_synthesis_noise_uncertainty_sensors.pdf][pdf]])
[[file:figs/mixed_synthesis_noise_uncertainty_sensors.png]]

** Weighting Functions on the uncertainty of the super sensor
We design weights for the $\mathcal{H}_\infty$ part of the synthesis in order to limit the dynamical uncertainty of the super sensor.
The maximum wanted multiplicative uncertainty is shown in Figure .The idea here is that we don't really need low uncertainty at low frequency but only near the crossover frequency that is suppose to be around 300Hz here.

** 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
  save('./mat/H2_Hinf_filters.mat', 'H2', 'H1');
#+end_src

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

#+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

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

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

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

#+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_S2, '-',  'DisplayName', '$\Phi_{\hat{x}_{pos}}$');
  plot(freqs, PSD_S1, '-',  'DisplayName', '$\Phi_{\hat{x}_{acc}}$');
  plot(freqs, PSD_H2Hinf,  'k-', 'DisplayName', '$\Phi_{\hat{x}_{\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

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

#+NAME: fig:psd_super_sensor_mixed_syn
#+CAPTION: Power Spectral Density of the Super Sensor obtained with the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis ([[./figs/psd_super_sensor_mixed_syn.png][png]], [[./figs/psd_super_sensor_mixed_syn.pdf][pdf]])
[[file:figs/psd_super_sensor_mixed_syn.png]]

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, CPS_S2, '-',  'DisplayName', sprintf('$\\sigma_{\\hat{x}_{pos}} = %.1e$ [m/s rms]', sqrt(CPS_S2(end))));
  plot(freqs, CPS_S1, '-',  'DisplayName', sprintf('$\\sigma_{\\hat{x}_{acc}} = %.1e$ [m/s rms]', sqrt(CPS_S1(end))));
  plot(freqs, CPS_H2Hinf, 'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_\\infty/\\mathcal{H}_\\infty}} = %.1e$ [m/s rms]', sqrt(CPS_H2Hinf(end))));
  set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum');
  hold off;
  xlim([2*freqs(1), freqs(end)]);
  % ylim([1e-10 1e-5]);
  legend('location', 'southeast');
#+end_src

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

#+NAME: fig:cps_super_sensor_mixed_syn
#+CAPTION: Cumulative Power Spectrum of the Super Sensor obtained with the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis ([[./figs/cps_super_sensor_mixed_syn.png][png]], [[./figs/cps_super_sensor_mixed_syn.pdf][pdf]])
[[file:figs/cps_super_sensor_mixed_syn.png]]

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

#+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);
  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;
  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;
  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

#+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

* Old                                                               :noexport:
** 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:
The idea is to combine sensors that works in different frequency range using complementary filters.

Doing so, one "super sensor" is obtained that can have better noise characteristics than the individual sensors over a large frequency range.

the complementary filters have to be designed in order to minimize the effect noise of each sensor on the super sensor noise.

*** zip file containing the data and matlab files                   :ignore:
#+begin_note
  the matlab scripts is accessible [[file:matlab/optimal_comp_filters.m][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

*** Architecture
Let's consider the sensor fusion architecture shown on figure [[fig:fusion_two_noisy_sensors_weights]] where two sensors (sensor 1 and sensor 2) are measuring the same quantity $x$ with different noise characteristics determined by $N_1(s)$ and $N_2(s)$.

$\tilde{n}_1$ and $\tilde{n}_2$ are normalized white noise:
#+name: eq:normalized_noise
\begin{equation}
  \Phi_{\tilde{n}_1}(\omega) = \Phi_{\tilde{n}_2}(\omega) = 1
\end{equation}

#+name: fig:fusion_two_noisy_sensors_weights
#+caption: Fusion of two sensors
[[file:figs-tikz/fusion_two_noisy_sensors_weights.png]]

We consider that the two sensor dynamics $G_1(s)$ and $G_2(s)$ are ideal:
#+name: eq:idea_dynamics
\begin{equation}
  G_1(s) = G_2(s) = 1
\end{equation}

We obtain the architecture of figure [[fig:sensor_fusion_noisy_perfect_dyn]].

#+name: fig:sensor_fusion_noisy_perfect_dyn
#+caption: Fusion of two sensors with ideal dynamics
[[file:figs-tikz/sensor_fusion_noisy_perfect_dyn.png]]

$H_1(s)$ and $H_2(s)$ are complementary filters:
#+name: eq:comp_filters_property
\begin{equation}
  H_1(s) + H_2(s) = 1
\end{equation}

The goal is to design $H_1(s)$ and $H_2(s)$ such that the effect of the noise sources $\tilde{n}_1$ and $\tilde{n}_2$ has the smallest possible effect on the estimation $\hat{x}$.

We have that the Power Spectral Density (PSD) of $\hat{x}$ is:
\[ \Phi_{\hat{x}}(\omega) = |H_1(j\omega) N_1(j\omega)|^2 \Phi_{\tilde{n}_1}(\omega) + |H_2(j\omega) N_2(j\omega)|^2 \Phi_{\tilde{n}_2}(\omega), \quad \forall \omega \]

And the goal is the minimize the Root Mean Square (RMS) value of $\hat{x}$:
#+name: eq:rms_value_estimation
\begin{equation}
  \sigma_{\hat{x}} = \sqrt{\int_0^\infty \Phi_{\hat{x}}(\omega) d\omega}
\end{equation}

*** Noise of the sensors
Let's define the noise characteristics of the two sensors by choosing $N_1$ and $N_2$:
- Sensor 1 characterized by $N_1(s)$ has low noise at low frequency (for instance a geophone)
- Sensor 2 characterized by $N_2(s)$ has low noise at high frequency (for instance an accelerometer)

#+begin_src matlab
  omegac = 100*2*pi; G0 = 1e-5; Ginf = 1e-4;
  N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/100);

  omegac = 1*2*pi; G0 = 1e-3; Ginf = 1e-8;
  N2 = ((sqrt(Ginf)*s/omegac + sqrt(G0))/(s/omegac + 1))^2/(1 + s/2/pi/4000)^2;
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, abs(squeeze(freqresp(N1, freqs, 'Hz'))), '-', 'DisplayName', '$N_1$');
  plot(freqs, abs(squeeze(freqresp(N2, freqs, 'Hz'))), '-', 'DisplayName', '$N_2$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  xlim([freqs(1), freqs(end)]);
  legend('location', 'northeast');
#+end_src

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

#+name: fig:noise_characteristics_sensors
#+caption: Noise Characteristics of the two sensors ([[./figs/noise_characteristics_sensors.png][png]], [[./figs/noise_characteristics_sensors.pdf][pdf]])
#+RESULTS:
[[file:figs/noise_characteristics_sensors.png]]

*** H-Two Synthesis
As $\tilde{n}_1$ and $\tilde{n}_2$ are normalized white noise: $\Phi_{\tilde{n}_1}(\omega) = \Phi_{\tilde{n}_2}(\omega) = 1$ and we have:
\[ \sigma_{\hat{x}} = \sqrt{\int_0^\infty |H_1 N_1|^2(\omega) + |H_2 N_2|^2(\omega) d\omega} = \left\| \begin{matrix} H_1 N_1 \\ H_2 N_2 \end{matrix} \right\|_2 \]
Thus, the goal is to design $H_1(s)$ and $H_2(s)$ such that $H_1(s) + H_2(s) = 1$ and such that $\left\| \begin{matrix} H_1 N_1 \\ H_2 N_2 \end{matrix} \right\|_2$ is minimized.

For that, we use the $\mathcal{H}_2$ Synthesis.

We use the generalized plant architecture shown on figure [[fig:h_infinity_optimal_comp_filters]].

#+name: fig:h_infinity_optimal_comp_filters
#+caption: $\mathcal{H}_2$ Synthesis - Generalized plant used for the optimal generation of complementary filters
[[file:figs-tikz/h_infinity_optimal_comp_filters.png]]

\begin{equation*}
\begin{pmatrix}
  z \\ v
\end{pmatrix} = \begin{pmatrix}
  0   &  N_2 & 1 \\
  N_1 & -N_2 & 0
\end{pmatrix} \begin{pmatrix}
  W_1 \\ W_2 \\ u
\end{pmatrix}
\end{equation*}

The transfer function from $[n_1, n_2]$ to $\hat{x}$ is:
\[ \begin{bmatrix} N_1 H_1 \\ N_2 (1 - H_1) \end{bmatrix} \]
If we define $H_2 = 1 - H_1$, we obtain:
\[ \begin{bmatrix} N_1 H_1 \\ N_2 H_2 \end{bmatrix} \]

Thus, if we minimize the $\mathcal{H}_2$ norm of this transfer function, we minimize the RMS value of $\hat{x}$.

We define the generalized plant $P$ on matlab as shown on figure [[fig:h_infinity_optimal_comp_filters]].
#+begin_src matlab
  P = [0   N2  1;
       N1 -N2  0];
#+end_src

And we do the $\mathcal{H}_2$ synthesis using the =h2syn= command.
#+begin_src matlab
  [H1, ~, gamma] = h2syn(P, 1, 1);
#+end_src

Finally, we define $H_2(s) = 1 - H_1(s)$.
#+begin_src matlab
  H2 = 1 - H1;
#+end_src

The complementary filters obtained are shown on figure [[fig:htwo_comp_filters]].

The PSD of the noise of the individual sensor and of the super sensor are shown in Figure [[fig:psd_sensors_htwo_synthesis]].

The Cumulative Power Spectrum (CPS) is shown on Figure [[fig:cps_h2_synthesis]].

The obtained RMS value of the super sensor is lower than the RMS value of the individual sensors.

#+begin_src matlab :exports none
  figure;
  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');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  xlim([freqs(1), freqs(end)]);
  legend('location', 'northeast');
#+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 ([[./figs/htwo_comp_filters.png][png]], [[./figs/htwo_comp_filters.pdf][pdf]])
#+RESULTS:
[[file:figs/htwo_comp_filters.png]]

#+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

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, PSD_S1, '-',  'DisplayName', '$\Phi_{\hat{x}_1}$');
  plot(freqs, PSD_S2, '-',  'DisplayName', '$\Phi_{\hat{x}_2}$');
  plot(freqs, PSD_H2, 'k-', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_2}}$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Power Spectral Density');
  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', 'full', 'height', 'tall');
#+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 ([[./figs/psd_sensors_htwo_synthesis.png][png]], [[./figs/psd_sensors_htwo_synthesis.pdf][pdf]])
#+RESULTS:
[[file:figs/psd_sensors_htwo_synthesis.png]]

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

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, CPS_S1, '-',  'DisplayName', sprintf('$\\sigma_{\\hat{x}_1} = %.1e$', sqrt(CPS_S1(end))));
  plot(freqs, CPS_S2, '-',  'DisplayName', sprintf('$\\sigma_{\\hat{x}_2} = %.1e$', sqrt(CPS_S2(end))));
  plot(freqs, CPS_H2, 'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_2}} = %.1e$', sqrt(CPS_H2(end))));
  set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum');
  hold off;
  xlim([2e-1, freqs(end)]);
  ylim([1e-10 1e-5]);
  legend('location', 'southeast');
#+end_src

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

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

*** Time Domain Simulation
Parameters of the time domain simulation.
#+begin_src matlab
  Fs = 1e3; % Sampling Frequency [Hz]
  Ts = 1/Fs; % Sampling Time [s]

  t = 0:Ts:5; % Time Vector [s]
#+end_src

Generate noises in velocity corresponding to sensor 1 and 2:
#+begin_src matlab
  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;
  plot(t, n1, 'DisplayName', 'Differentiated Position');
  plot(t, n2, 'DisplayName', 'Integrated Acceleration');
  plot(t, lsim(H1, n1, t)+lsim(H2, n2, t), 'DisplayName', 'Super Sensor');
  hold off;
  xlabel('Time [s]'); ylabel('Velocity [m/s]');
  legend();
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/super_sensor_time_domain_h2.pdf', 'width', 'full', 'height', 'tall');
#+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]]

*** Sensor Spurious Dynamics
#+begin_src matlab
  G2 = tf(1);

  w1 = 2*pi*10; xi1 = 0.2; z1 = 2*pi*20;
  G1 = (1 + 2*xi1*s/z1 + s^2/z1^2)/(1 + 2*xi1*s/w1 + s^2/w1^2);

  Gss = G1*H1 + G2*H2;
#+end_src

#+begin_src matlab :exports none
  figure;
  % Magnitude
  ax1 = subplot(2,1,1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(G1*H1, freqs, 'Hz'))), '-', 'DisplayName', '$G_1$');
  plot(freqs, abs(squeeze(freqresp(G2*H2, freqs, 'Hz'))), '-', 'DisplayName', '$G_2$');
  plot(freqs, abs(squeeze(freqresp(Gss, freqs, 'Hz'))), 'k-', 'DisplayName', 'SS Dynamics');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  legend('location', 'southwest');
  ylabel('Magnitude');
  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'))), '-');
  plot(freqs, 180/pi*angle(squeeze(freqresp(Gss, 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');
#+end_src

*** Obtained Super Sensor's noise uncertainty
We would like to verify if the obtained sensor fusion architecture is robust to change in the sensor dynamics.

To study the dynamical uncertainty on the super sensor, we defined some multiplicative uncertainty on both sensor dynamics.
Two weights $W_1(s)$ and $W_2(s)$ are used to described the amplitude of the dynamical uncertainty.

#+begin_src matlab
  omegac = 100*2*pi; G0 = 0.1; Ginf = 10;
  w1 = (Ginf*s/omegac + G0)/(s/omegac + 1);

  omegac = 0.2*2*pi; G0 = 5; Ginf = 0.1;
  w2 = (Ginf*s/omegac + G0)/(s/omegac + 1);
  omegac = 5000*2*pi; G0 = 1; Ginf = 50;
  w2 = w2*(Ginf*s/omegac + G0)/(s/omegac + 1);
#+end_src

The sensor uncertain models are defined below.
#+begin_src matlab
  G1 = 1 + w1*ultidyn('Delta',[1 1]);
  G2 = 1 + w2*ultidyn('Delta',[1 1]);
#+end_src

#+begin_src matlab :exports none
  % We here compute the maximum and minimum phase of both sensors
  Dphi1 = 180/pi*asin(abs(squeeze(freqresp(w1, freqs, 'Hz'))));
  Dphi2 = 180/pi*asin(abs(squeeze(freqresp(w2, freqs, 'Hz'))));
  Dphi1(abs(squeeze(freqresp(w1, freqs, 'Hz'))) > 1) = 190;
  Dphi2(abs(squeeze(freqresp(w2, freqs, 'Hz'))) > 1) = 190;
#+end_src

The super sensor uncertain model is defined below using the complementary filters obtained with the $\mathcal{H}_2$ synthesis.
The dynamical uncertainty bounds of the super sensor is shown in Figure [[fig:uncertainty_super_sensor_H2_syn]].
Right Half Plane zero might be introduced in the super sensor dynamics which will render the feedback system unstable.

#+begin_src matlab
  Gss = G1*H1 + G2*H2;
#+end_src

#+begin_src matlab :exports none
  Gsss = usample(Gss, 20);
#+end_src

#+begin_src matlab :exports none
  % We here compute the maximum and minimum phase of the super sensor
  Dphiss = 180/pi*asin(abs(squeeze(freqresp(w1*H1, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))));
  Dphiss(abs(squeeze(freqresp(w1*H1, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))) > 1) = 190;
#+end_src

#+begin_src matlab :exports none
  figure;
  % Magnitude
  ax1 = subplot(2,1,1);
  hold on;
  set(gca,'ColorOrderIndex',1);
  plot(freqs, 1 + abs(squeeze(freqresp(w1, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S1');
  set(gca,'ColorOrderIndex',1);
  plot(freqs, max(1 - abs(squeeze(freqresp(w1, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
  set(gca,'ColorOrderIndex',2);
  plot(freqs, 1 + abs(squeeze(freqresp(w2, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S2');
  set(gca,'ColorOrderIndex',2);
  plot(freqs, max(1 - abs(squeeze(freqresp(w2, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
  plot(freqs, 1 + abs(squeeze(freqresp(w1*H1, freqs, 'Hz'))) + abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))), 'k--', 'DisplayName', 'Bounds - SS');
  plot(freqs, max(1 - abs(squeeze(freqresp(w1*H1, freqs, 'Hz'))) - abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))), 0), 'k--', 'HandleVisibility', 'off');
  plot(freqs, abs(squeeze(freqresp(Gsss(1, 1, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'DisplayName', 'SS Dynamics');
  for i = 2:length(Gsss)
    plot(freqs, abs(squeeze(freqresp(Gsss(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'HandleVisibility', 'off');
  end
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  legend('location', 'southwest');
  ylabel('Magnitude');
  ylim([5e-2, 10]);
  hold off;

  % Phase
  ax2 = subplot(2,1,2);
  hold on;
  set(gca,'ColorOrderIndex',1);
  plot(freqs,  Dphi1, '--');
  set(gca,'ColorOrderIndex',1);
  plot(freqs, -Dphi1, '--');
  set(gca,'ColorOrderIndex',2);
  plot(freqs,  Dphi2, '--');
  set(gca,'ColorOrderIndex',2);
  plot(freqs, -Dphi2, '--');
  plot(freqs,  Dphiss, 'k--');
  plot(freqs, -Dphiss, 'k--');
  for i = 1:length(Gsss)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gsss(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2]);
  end
  set(gca,'xscale','log');
  yticks(-180:90:180);
  ylim([-180 180]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
#+end_src

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

#+NAME: fig:uncertainty_super_sensor_H2_syn
#+CAPTION: Uncertianty regions of both individual sensors and of the super sensor when using the $\mathcal{H}_2$ synthesis ([[./figs/uncertainty_super_sensor_H2_syn.png][png]], [[./figs/uncertainty_super_sensor_H2_syn.pdf][pdf]])
[[file:figs/uncertainty_super_sensor_H2_syn.png]]

*** Conclusion
From the above complementary filter design with the $\mathcal{H}_2$ and $\mathcal{H}_\infty$ synthesis, it still seems that the $\mathcal{H}_2$ synthesis gives the complementary filters that permits to obtain the minimal super sensor noise (when measuring with the $\mathcal{H}_2$ norm).

However, the synthesis does not take into account the robustness of the sensor fusion.

** 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_dynamic_uncertainty]].

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

The objective here is to design complementary filters $H_1(s)$ and $H_2(s)$ in order to minimize the dynamical uncertainty of the super sensor.

*** ZIP file containing the data and matlab files                   :ignore:
#+begin_note
  The Matlab scripts is accessible [[file:matlab/comp_filter_robustness.m][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

*** Super Sensor Dynamical Uncertainty
In practical systems, the sensor dynamics has always some level of uncertainty.
Let's represent that with multiplicative input uncertainty as shown on figure [[fig:sensor_fusion_dynamic_uncertainty]].

#+name: fig:sensor_fusion_dynamic_uncertainty
#+caption: Fusion of two sensors with input multiplicative uncertainty
[[file:figs-tikz/sensor_fusion_dynamic_uncertainty.png]]

The dynamics of the super sensor is represented by
\begin{align*}
  \frac{\hat{x}}{x} &= (1 + W_1 \Delta_1) H_1 + (1 + W_2 \Delta_2) H_2 \\
                    &= 1 + W_1 H_1 \Delta_1 + W_2 H_2 \Delta_2
\end{align*}
with $\Delta_i$ is any transfer function satisfying $\| \Delta_i \|_\infty < 1$.

We see that as soon as we have some uncertainty in the sensor dynamics, we have that the complementary filters have some effect on the transfer function from $x$ to $\hat{x}$.

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)|$ (figure [[fig:uncertainty_gain_phase_variation]]).

We then have that the angle introduced by the super sensor is bounded by $\arcsin(\epsilon)$:
\[ \angle \frac{\hat{x}}{x}(j\omega) \le \arcsin \Big(|W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)|\Big) \]

#+name: fig:uncertainty_gain_phase_variation
#+caption: Maximum phase variation
[[file:figs-tikz/uncertainty_gain_phase_variation.png]]

*** Dynamical uncertainty of the individual sensors
Let say we want to merge two sensors:
- sensor 1 that has unknown dynamics above 10Hz: $|W_1(j\omega)| > 1$ for $\omega > 10\text{ Hz}$
- sensor 2 that has unknown dynamics below 1Hz and above 1kHz $|W_2(j\omega)| > 1$ for $\omega < 1\text{ Hz}$ and $\omega > 1\text{ kHz}$

We define the weights that are used to characterize the dynamic uncertainty of the sensors.

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

#+begin_src matlab
  omegac = 100*2*pi; G0 = 0.1; Ginf = 10;
  w1 = (Ginf*s/omegac + G0)/(s/omegac + 1);

  omegac = 0.2*2*pi; G0 = 5; Ginf = 0.1;
  w2 = (Ginf*s/omegac + G0)/(s/omegac + 1);
  omegac = 5000*2*pi; G0 = 1; Ginf = 50;
  w2 = w2*(Ginf*s/omegac + G0)/(s/omegac + 1);
#+end_src

From the weights, we define the uncertain transfer functions of the sensors. Some of the uncertain dynamics of both sensors are shown on Figure [[fig:uncertainty_dynamics_sensors]] with the upper and lower bounds on the magnitude and on the phase.
#+begin_src matlab
  G1 = 1 + w1*ultidyn('Delta',[1 1]);
  G2 = 1 + w2*ultidyn('Delta',[1 1]);
#+end_src

#+begin_src matlab :exports none
  % Few random samples of the sensor dynamics are computed
  G1s = usample(G1, 10);
  G2s = usample(G2, 10);
#+end_src

#+begin_src matlab :exports none
  % We here compute the maximum and minimum phase of both sensors
  Dphi1 = 180/pi*asin(abs(squeeze(freqresp(w1, freqs, 'Hz'))));
  Dphi2 = 180/pi*asin(abs(squeeze(freqresp(w2, freqs, 'Hz'))));
  Dphi1(abs(squeeze(freqresp(w1, freqs, 'Hz'))) > 1) = 190;
  Dphi2(abs(squeeze(freqresp(w2, freqs, 'Hz'))) > 1) = 190;
#+end_src

#+begin_src matlab :exports none
  figure;
  % Magnitude
  ax1 = subplot(2,1,1);
  hold on;
  set(gca,'ColorOrderIndex',1);
  plot(freqs, 1 + abs(squeeze(freqresp(w1, freqs, 'Hz'))), '--');
  set(gca,'ColorOrderIndex',1);
  plot(freqs, max(1 - abs(squeeze(freqresp(w1, freqs, 'Hz'))), 0), '--');
  set(gca,'ColorOrderIndex',2);
  plot(freqs, 1 + abs(squeeze(freqresp(w2, freqs, 'Hz'))), '--');
  set(gca,'ColorOrderIndex',2);
  plot(freqs, max(1 - abs(squeeze(freqresp(w2, freqs, 'Hz'))), 0), '--');
  for i = 1:length(G1s)
    plot(freqs, abs(squeeze(freqresp(G1s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0.4470 0.7410 0.4]);
    plot(freqs, abs(squeeze(freqresp(G2s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0.8500 0.3250 0.0980 0.4]);
  end
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude');
  ylim([1e-1, 10]);
  hold off;

  % Phase
  ax2 = subplot(2,1,2);
  hold on;
  set(gca,'ColorOrderIndex',1);
  plot(freqs,  Dphi1, '--');
  set(gca,'ColorOrderIndex',1);
  plot(freqs, -Dphi1, '--');
  set(gca,'ColorOrderIndex',2);
  plot(freqs,  Dphi2, '--');
  set(gca,'ColorOrderIndex',2);
  plot(freqs, -Dphi2, '--');
  for i = 1:length(G1s)
    plot(freqs, 180/pi*angle(squeeze(freqresp(G1s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0.4470 0.7410 0.4]);
    plot(freqs, 180/pi*angle(squeeze(freqresp(G2s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0.8500 0.3250 0.0980 0.4]);
  end
  set(gca,'xscale','log');
  yticks(-180:90:180);
  ylim([-180 180]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
#+end_src

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

#+NAME: fig:uncertainty_dynamics_sensors
#+CAPTION: Dynamic uncertainty of the two sensors ([[./figs/uncertainty_dynamics_sensors.png][png]], [[./figs/uncertainty_dynamics_sensors.pdf][pdf]])
[[file:figs/uncertainty_dynamics_sensors.png]]

*** Synthesis objective
The uncertainty region of the super sensor dynamics is represented by a circle in the complex plane as shown in Figure [[fig:uncertainty_gain_phase_variation]].

At each frequency $\omega$, the radius of the circle is $|W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)|$.

Thus, the phase shift $\Delta\phi(\omega)$ due to the super sensor uncertainty is bounded by:
\[ |\Delta\phi(\omega)| \leq \arcsin\big( |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| \big) \]

Let's define some allowed frequency depend phase shift $\Delta\phi_\text{max}(\omega) > 0$ such that:
\[ |\Delta\phi(\omega)| < \Delta\phi_\text{max}(\omega), \quad \forall\omega \]


If $H_1(s)$ and $H_2(s)$ are designed such that
\[ |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| < \sin\big( \Delta\phi_\text{max}(\omega) \big) \]

The maximum phase shift due to dynamic uncertainty at frequency $\omega$ will be $\Delta\phi_\text{max}(\omega)$.

*** Requirements as an $\mathcal{H}_\infty$ norm
We now try to express this requirement in terms of an $\mathcal{H}_\infty$ norm.

Let's define one weight $W_\phi(s)$ that represents the maximum wanted phase uncertainty:
\[ |W_{\phi}(j\omega)|^{-1} \approx \sin(\Delta\phi_{\text{max}}(\omega)), \quad \forall\omega \]

Then:
\begin{align*}
                      & |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| < \sin\big( \Delta\phi_\text{max}(\omega) \big), \quad \forall\omega \\
  \Longleftrightarrow & |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| < |W_\phi(j\omega)|^{-1}, \quad \forall\omega \\
  \Longleftrightarrow & \left| W_1(j\omega) H_1(j\omega) W_\phi(j\omega) \right| + \left| W_2(j\omega) H_2(j\omega) W_\phi(j\omega) \right| < 1, \quad \forall\omega
\end{align*}

Which is approximately equivalent to (with an error of maximum $\sqrt{2}$):
#+name: eq:hinf_conf_phase_uncertainty
\begin{equation}
  \left\| \begin{matrix} W_1(s) W_\phi(s) H_1(s) \\ W_2(s) W_\phi(s) H_2(s) \end{matrix} \right\|_\infty < 1
\end{equation}

One should not forget that at frequency where both sensors has unknown dynamics ($|W_1(j\omega)| > 1$ and $|W_2(j\omega)| > 1$), the super sensor dynamics will also be unknown and the phase uncertainty cannot be bounded.
Thus, at these frequencies, $|W_\phi|$ should be smaller than $1$.

*** Weighting Function used to bound the super sensor uncertainty
Let's define $W_\phi(s)$ in order to bound the maximum allowed phase uncertainty $\Delta\phi_\text{max}$ of the super sensor dynamics.
The magnitude $|W_\phi(j\omega)|$ is shown in Figure [[fig:magnitude_wphi]] and the corresponding maximum allowed phase uncertainty of the super sensor dynamics of shown in Figure [[fig:maximum_wanted_phase_uncertainty]].

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

  n = 4; w0 = 2*pi*900; G0 = 1/sin(Dphi*pi/180); Ginf = 1/100; Gc = 1;
  wphi = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/Ginf)^(2/n)))*s + (G0/Gc)^(1/n))/((1/Ginf)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/Ginf)^(2/n)))*s + (1/Gc)^(1/n)))^n;

  W1 = w1*wphi;
  W2 = w2*wphi;
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, abs(squeeze(freqresp(wphi, freqs, 'Hz'))), '-', 'DisplayName', '$W_\phi(s)$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  xlim([freqs(1), freqs(end)]);
  legend('location', 'northeast');
#+end_src

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

#+NAME: fig:magnitude_wphi
#+CAPTION: Magnitude of the weght $W_\phi(s)$ that is used to bound the uncertainty of the super sensor ([[./figs/magnitude_wphi.png][png]], [[./figs/magnitude_wphi.pdf][pdf]])
[[file:figs/magnitude_wphi.png]]

#+begin_src matlab :exports none
  % We here compute the wanted maximum and minimum phase of the super sensor
  Dphimax = 180/pi*asin(1./abs(squeeze(freqresp(wphi, freqs, 'Hz'))));
  Dphimax(1./abs(squeeze(freqresp(wphi, freqs, 'Hz'))) > 1) = 190;
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs,  Dphimax, 'k--');
  plot(freqs, -Dphimax, 'k--');
  set(gca, 'XScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  xlim([freqs(1), freqs(end)]);
  ylim([-180 180]);
  yticks(-180:45:180);
#+end_src

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

#+NAME: fig:maximum_wanted_phase_uncertainty
#+CAPTION: Maximum wanted phase uncertainty using this weight ([[./figs/maximum_wanted_phase_uncertainty.png][png]], [[./figs/maximum_wanted_phase_uncertainty.pdf][pdf]])
[[file:figs/maximum_wanted_phase_uncertainty.png]]

The obtained upper bounds on the complementary filters in order to limit the phase uncertainty of the super sensor are represented in Figure [[fig:upper_bounds_comp_filter_max_phase_uncertainty]].

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '-', 'DisplayName', '$1/|W_1W_\phi|$');
  plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '-', 'DisplayName', '$1/|W_2W_\phi|$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  xlim([freqs(1), freqs(end)]);
  legend('location', 'northeast');
#+end_src

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

#+NAME: fig:upper_bounds_comp_filter_max_phase_uncertainty
#+CAPTION: Upper bounds on the complementary filters set in order to limit the maximum phase uncertainty of the super sensor to 30 degrees until 500Hz ([[./figs/upper_bounds_comp_filter_max_phase_uncertainty.png][png]], [[./figs/upper_bounds_comp_filter_max_phase_uncertainty.pdf][pdf]])
[[file:figs/upper_bounds_comp_filter_max_phase_uncertainty.png]]

*** $\mathcal{H}_\infty$ Synthesis
The $\mathcal{H}_\infty$ synthesis architecture used for the complementary filters is shown in Figure [[fig:h_infinity_robust_fusion]].

#+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]]

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

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

#+RESULTS:
#+begin_example
[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
Resetting value of Gamma min based on D_11, D_12, D_21 terms

Test bounds:      0.0447 <  gamma  <=      1.3318

  gamma    hamx_eig  xinf_eig  hamy_eig   yinf_eig   nrho_xy   p/f
    1.332   1.3e+01 -1.0e-14   1.3e+00   -2.6e-18    0.0000    p
    0.688   1.3e-11# ********   1.3e+00   -6.7e-15  ********    f
    1.010   1.1e+01 -1.5e-14   1.3e+00   -2.5e-14    0.0000    p
    0.849   6.9e-11# ********   1.3e+00   -2.3e-14  ********    f
    0.930   5.2e-12# ********   1.3e+00   -6.1e-18  ********    f
    0.970   5.6e-11# ********   1.3e+00   -2.3e-14  ********    f
    0.990   5.0e-11# ********   1.3e+00   -1.7e-17  ********    f
    1.000   2.1e-10# ********   1.3e+00    0.0e+00  ********    f
    1.005   1.9e-10# ********   1.3e+00   -3.7e-14  ********    f
    1.008   1.1e+01 -9.1e-15   1.3e+00    0.0e+00    0.0000    p
    1.006   1.2e-09# ********   1.3e+00   -6.9e-16  ********    f
    1.007   1.1e+01 -4.6e-15   1.3e+00   -1.8e-16    0.0000    p

 Gamma value achieved:     1.0069
#+end_example

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

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

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

  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
  set(gca,'ColorOrderIndex',2)
  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;
  set(gca,'ColorOrderIndex',1)
  plot(freqs, 180/pi*phase(squeeze(freqresp(H1, freqs, 'Hz'))), '-');
  set(gca,'ColorOrderIndex',2)
  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)]);
  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_filter_hinf_uncertainty.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:comp_filter_hinf_uncertainty
#+CAPTION: Obtained complementary filters ([[./figs/comp_filter_hinf_uncertainty.png][png]], [[./figs/comp_filter_hinf_uncertainty.pdf][pdf]])
[[file:figs/comp_filter_hinf_uncertainty.png]]

*** Super sensor uncertainty
We can now compute the uncertainty of the super sensor. The result is shown in Figure [[fig:super_sensor_uncertainty_bode_plot]].

#+begin_src matlab
  Gss = G1*H1 + G2*H2;
#+end_src

#+begin_src matlab :exports none
  Gsss = usample(Gss, 20);
#+end_src

#+begin_src matlab :exports none
  % We here compute the maximum and minimum phase of the super sensor
  Dphiss = 180/pi*asin(abs(squeeze(freqresp(w1*H1, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))));
  Dphiss(abs(squeeze(freqresp(w1*H1, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))) > 1) = 190;
#+end_src

#+begin_src matlab :exports none
  figure;
  % Magnitude
  ax1 = subplot(2,1,1);
  hold on;
  set(gca,'ColorOrderIndex',1);
  plot(freqs, 1 + abs(squeeze(freqresp(w1, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S1');
  set(gca,'ColorOrderIndex',1);
  plot(freqs, max(1 - abs(squeeze(freqresp(w1, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
  set(gca,'ColorOrderIndex',2);
  plot(freqs, 1 + abs(squeeze(freqresp(w2, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S2');
  set(gca,'ColorOrderIndex',2);
  plot(freqs, max(1 - abs(squeeze(freqresp(w2, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
  plot(freqs, 1 + abs(squeeze(freqresp(w1*H1+w2*H2, freqs, 'Hz'))), 'k--', 'DisplayName', 'Bounds - SS');
  plot(freqs, max(1 - abs(squeeze(freqresp(w1*H1+w2*H2, freqs, 'Hz'))), 0), 'k--', 'HandleVisibility', 'off');
  plot(freqs, abs(squeeze(freqresp(Gsss(1, 1, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'DisplayName', 'SS Dynamics');
  for i = 2:length(Gsss)
    plot(freqs, abs(squeeze(freqresp(Gsss(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'HandleVisibility', 'off');
  end
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  legend('location', 'southwest');
  ylabel('Magnitude');
  ylim([5e-2, 10]);
  hold off;

  % Phase
  ax2 = subplot(2,1,2);
  hold on;
  set(gca,'ColorOrderIndex',1);
  plot(freqs,  Dphi1, '--');
  set(gca,'ColorOrderIndex',1);
  plot(freqs, -Dphi1, '--');
  set(gca,'ColorOrderIndex',2);
  plot(freqs,  Dphi2, '--');
  set(gca,'ColorOrderIndex',2);
  plot(freqs, -Dphi2, '--');
  plot(freqs,  Dphiss, 'k--');
  plot(freqs, -Dphiss, 'k--');
  for i = 1:length(Gsss)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gsss(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2]);
  end
  set(gca,'xscale','log');
  yticks(-180:90:180);
  ylim([-180 180]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
#+end_src

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

#+NAME: fig:super_sensor_uncertainty_bode_plot
#+CAPTION: Uncertainty on the dynamics of the super sensor ([[./figs/super_sensor_uncertainty_bode_plot.png][png]], [[./figs/super_sensor_uncertainty_bode_plot.pdf][pdf]])
[[file:figs/super_sensor_uncertainty_bode_plot.png]]

The uncertainty of the super sensor cannot be made smaller than both the individual sensor. Ideally, it would follow the minimum uncertainty of both sensors.

We here just used very wimple weights.
For instance, we could improve the dynamical uncertainty of the super sensor by making $|W_\phi(j\omega)|$ smaller bellow 2Hz where the dynamical uncertainty of the sensor 1 is small.

*** Super sensor noise
We now compute the obtain Power Spectral Density of the super sensor's noise.
The noise characteristics of both individual sensor are defined below.

#+begin_src matlab
  omegac = 100*2*pi; G0 = 1e-5; Ginf = 1e-4;
  N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/100);

  omegac = 1*2*pi; G0 = 1e-3; Ginf = 1e-8;
  N2 = ((sqrt(Ginf)*s/omegac + sqrt(G0))/(s/omegac + 1))^2/(1 + s/2/pi/4000)^2;
#+end_src

The PSD of both sensor and of the super sensor is shown in Figure [[fig:psd_sensors_hinf_synthesis]].
The CPS of both sensor and of the super sensor is shown in Figure [[fig:cps_sensors_hinf_synthesis]].

#+begin_src matlab :exports none
  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

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

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

#+NAME: fig:psd_sensors_hinf_synthesis
#+CAPTION: Power Spectral Density of the obtained super sensor using the $\mathcal{H}_\infty$ synthesis ([[./figs/psd_sensors_hinf_synthesis.png][png]], [[./figs/psd_sensors_hinf_synthesis.pdf][pdf]])
[[file:figs/psd_sensors_hinf_synthesis.png]]

#+begin_src matlab :exports none
  CPS_S1 = 1/pi*cumtrapz(2*pi*freqs, PSD_S1);
  CPS_S2 = 1/pi*cumtrapz(2*pi*freqs, PSD_S2);
  CPS_H2 = 1/pi*cumtrapz(2*pi*freqs, PSD_H2);
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, CPS_S1, '-',  'DisplayName', sprintf('$\\sigma_{\\hat{x}_1} = %.1e$', sqrt(CPS_S1(end))));
  plot(freqs, CPS_S2, '-',  'DisplayName', sprintf('$\\sigma_{\\hat{x}_2} = %.1e$', sqrt(CPS_S2(end))));
  plot(freqs, CPS_H2, 'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_2}} = %.1e$', sqrt(CPS_H2(end))));
  set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum');
  hold off;
  xlim([2e-1, freqs(end)]);
  ylim([1e-10 1e-5]);
  legend('location', 'southeast');
#+end_src

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

#+NAME: fig:cps_sensors_hinf_synthesis
#+CAPTION: Cumulative Power Spectrum of the obtained super sensor using the $\mathcal{H}_\infty$ synthesis ([[./figs/cps_sensors_hinf_synthesis.png][png]], [[./figs/cps_sensors_hinf_synthesis.cps][cps]])
[[file:figs/cps_sensors_hinf_synthesis.png]]

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

*** First Basic Example with gain mismatch                        :noexport:
Let's consider two ideal sensors except one sensor has not an expected unity gain but a gain equal to $0.6$:
\begin{align*}
  G_1(s) &= 1 \\
  G_2(s) &= 0.6
\end{align*}

#+begin_src matlab
  G1 = 1;
  G2 = 0.6;
#+end_src

Two pairs of complementary filters are designed and shown on figure [[fig:comp_filters_robustness_test]].
The complementary filters shown in blue does not present a bump as the red ones but provides less sensor separation at high and low frequencies.

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

#+begin_src matlab :exports none
  w0 = 2*pi;
  alpha = 2;

  H1a = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
  H2a = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));

  w0 = 2*pi;
  alpha = 0.1;

  H1b = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
  H2b = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
#+end_src

#+begin_src matlab :exports none
  figure;
  % Magnitude
  ax1 = subplot(2,1,1);
  hold on;
  set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(H1a, freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(H2a, freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',2); plot(freqs, abs(squeeze(freqresp(H1b, freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',2); plot(freqs, abs(squeeze(freqresp(H2b, freqs, 'Hz'))));
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude');
  hold off;
  % Phase
  ax2 = subplot(2,1,2);
  hold on;
  set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(H1a, freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(H2a, freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',2); plot(freqs, 180/pi*angle(squeeze(freqresp(H1b, freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',2); plot(freqs, 180/pi*angle(squeeze(freqresp(H2b, freqs, 'Hz'))));
  set(gca,'xscale','log');
  yticks(-180:90:180);
  ylim([-180 180]);
  xlabel('Relative Frequency $\frac{\omega}{\omega_0}$'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
#+end_src

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

#+NAME: fig:comp_filters_robustness_test
#+CAPTION: The two complementary filters designed for the robustness test ([[./figs/comp_filters_robustness_test.png][png]], [[./figs/comp_filters_robustness_test.pdf][pdf]])
[[file:figs/comp_filters_robustness_test.png]]

We then compute the bode plot of the super sensor transfer function $H_1(s)G_1(s) + H_2(s)G_2(s)$ for both complementary filters pair (figure [[fig:tf_super_sensor_comp]]).

We see that the blue complementary filters with a lower maximum norm permits to limit the phase lag introduced by the gain mismatch.

#+begin_src matlab :exports none
  figure;
  % Magnitude
  ax1 = subplot(2,1,1);
  hold on;
  set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(H1a*G1 + H2a*G2, freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',2); plot(freqs, abs(squeeze(freqresp(H1b*G1 + H2b*G2, freqs, 'Hz'))));
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude');
  ylim([1e-1, 1e1]);
  hold off;
  % Phase
  ax2 = subplot(2,1,2);
  hold on;
  set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(H1a*G1 + H2a*G2, freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',2); plot(freqs, 180/pi*angle(squeeze(freqresp(H1b*G1 + H2b*G2, freqs, 'Hz'))));
  set(gca,'xscale','log');
  yticks(-180:90:180);
  ylim([-180 180]);
  xlabel('Relative Frequency $\frac{\omega}{\omega_0}$'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
#+end_src

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

#+NAME: fig:tf_super_sensor_comp
#+CAPTION: Comparison of the obtained super sensor transfer functions ([[./figs/tf_super_sensor_comp.png][png]], [[./figs/tf_super_sensor_comp.pdf][pdf]])
[[file:figs/tf_super_sensor_comp.png]]

** 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>>
*** ZIP file containing the data and matlab files                   :ignore:
#+begin_note
  The Matlab scripts is accessible [[file:matlab/mixed_synthesis_sensor_fusion.m][here]].
#+end_note

*** Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis - Introduction
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
  freqs = logspace(-1, 3, 1000);
#+end_src

*** Noise characteristics and Uncertainty of the individual sensors
We define the weights that are used to characterize the dynamic uncertainty of the sensors. This will be used for the $\mathcal{H}_\infty$ part of the synthesis.
#+begin_src matlab
  omegac = 100*2*pi; G0 = 0.1; Ginf = 10;
  w1 = (Ginf*s/omegac + G0)/(s/omegac + 1);

  omegac = 0.2*2*pi; G0 = 5; Ginf = 0.1;
  w2 = (Ginf*s/omegac + G0)/(s/omegac + 1);
  omegac = 5000*2*pi; G0 = 1; Ginf = 50;
  w2 = w2*(Ginf*s/omegac + G0)/(s/omegac + 1);
#+end_src

We define the noise characteristics of the two sensors by choosing $N_1$ and $N_2$. This will be used for the $\mathcal{H}_2$ part of the synthesis.
#+begin_src matlab
  omegac = 100*2*pi; G0 = 1e-5; Ginf = 1e-4;
  N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/100);

  omegac = 1*2*pi; G0 = 1e-3; Ginf = 1e-8;
  N2 = ((sqrt(Ginf)*s/omegac + sqrt(G0))/(s/omegac + 1))^2/(1 + s/2/pi/4000)^2;
#+end_src

Both dynamical uncertainty and noise characteristics of the individual sensors are shown in Figure [[fig:mixed_synthesis_noise_uncertainty_sensors]].

#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2, 1, 1);
  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('Magnitude');
  hold off;
  legend('location', 'northeast');

  ax2 = subplot(2, 1, 2);
  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)|$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  legend('location', 'northeast');

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

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

#+NAME: fig:mixed_synthesis_noise_uncertainty_sensors
#+CAPTION: Noise characteristsics and Dynamical uncertainty of the individual sensors ([[./figs/mixed_synthesis_noise_uncertainty_sensors.png][png]], [[./figs/mixed_synthesis_noise_uncertainty_sensors.pdf][pdf]])
[[file:figs/mixed_synthesis_noise_uncertainty_sensors.png]]

*** Weighting Functions on the uncertainty of the super sensor
We design weights for the $\mathcal{H}_\infty$ part of the synthesis in order to limit the dynamical uncertainty of the super sensor.
The maximum wanted multiplicative uncertainty is shown in Figure [[fig:mixed_syn_hinf_weight]]. The idea here is that we don't really need low uncertainty at low frequency but only near the crossover frequency that is suppose to be around 300Hz here.

#+begin_src matlab
  n = 4; w0 = 2*pi*900; G0 = 9; G1 = 1; Gc = 1.1;
  H = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n;
  wphi = 0.2*(s+3.142e04)/(s+628.3)/H;
#+end_src

#+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)|$');
  plot(freqs, 1./abs(squeeze(freqresp(wphi, freqs, 'Hz'))), 'k--', 'DisplayName', '$|Wu(j\omega)|^{-1}$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  legend('location', 'northeast');
  xlim([freqs(1), freqs(end)]);
#+end_src

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

#+NAME: fig:mixed_syn_hinf_weight
#+CAPTION: Wanted maximum module uncertainty of the super sensor ([[./figs/mixed_syn_hinf_weight.png][png]], [[./figs/mixed_syn_hinf_weight.pdf][pdf]])
[[file:figs/mixed_syn_hinf_weight.png]]

The equivalent Magnitude and Phase uncertainties are shown in Figure [[fig:mixed_syn_objective_hinf]].

#+begin_src matlab :exports none
  G1 = 1 + w1*ultidyn('Delta',[1 1]);
  G2 = 1 + w2*ultidyn('Delta',[1 1]);

  % Few random samples of the sensor dynamics are computed
  G1s = usample(G1, 10);
  G2s = usample(G2, 10);

  % We here compute the maximum and minimum phase of both sensors
  Dphi1 = 180/pi*asin(abs(squeeze(freqresp(w1, freqs, 'Hz'))));
  Dphi2 = 180/pi*asin(abs(squeeze(freqresp(w2, freqs, 'Hz'))));
  Dphi1(abs(squeeze(freqresp(w1, freqs, 'Hz'))) > 1) = 190;
  Dphi2(abs(squeeze(freqresp(w2, freqs, 'Hz'))) > 1) = 190;

  % We here compute the wanted maximum and minimum phase of the super sensor
  Dphimax = 180/pi*asin(1./abs(squeeze(freqresp(wphi, freqs, 'Hz'))));
  Dphimax(1./abs(squeeze(freqresp(wphi, freqs, 'Hz'))) > 1) = 190;

  figure;
  % Magnitude
  ax1 = subplot(2,1,1);
  hold on;
  set(gca,'ColorOrderIndex',1);
  plot(freqs, 1 + abs(squeeze(freqresp(w1, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S1');
  set(gca,'ColorOrderIndex',1);
  plot(freqs, max(1 - abs(squeeze(freqresp(w1, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
  set(gca,'ColorOrderIndex',2);
  plot(freqs, 1 + abs(squeeze(freqresp(w2, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S2');
  set(gca,'ColorOrderIndex',2);
  plot(freqs, max(1 - abs(squeeze(freqresp(w2, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
  plot(freqs, 1 + 1./abs(squeeze(freqresp(wphi, freqs, 'Hz'))), 'k--', 'DisplayName', 'Synthesis Obj.');
  plot(freqs, max(1 - 1./abs(squeeze(freqresp(wphi, freqs, 'Hz'))), 0), 'k--', 'HandleVisibility', 'off');
  for i = 1:length(G1s)
    plot(freqs, abs(squeeze(freqresp(G1s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0.4470 0.7410 0.4], 'HandleVisibility', 'off');
    plot(freqs, abs(squeeze(freqresp(G2s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0.8500 0.3250 0.0980 0.4], 'HandleVisibility', 'off');
  end
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude');
  ylim([1e-1, 10]);
  hold off;
  legend('location', 'southwest');

  % Phase
  ax2 = subplot(2,1,2);
  hold on;
  set(gca,'ColorOrderIndex',1);
  plot(freqs,  Dphi1, '--');
  set(gca,'ColorOrderIndex',1);
  plot(freqs, -Dphi1, '--');
  set(gca,'ColorOrderIndex',2);
  plot(freqs,  Dphi2, '--');
  set(gca,'ColorOrderIndex',2);
  plot(freqs, -Dphi2, '--');
  for i = 1:length(G1s)
    plot(freqs, 180/pi*angle(squeeze(freqresp(G1s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0.4470 0.7410 0.4]);
    plot(freqs, 180/pi*angle(squeeze(freqresp(G2s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0.8500 0.3250 0.0980 0.4]);
  end
  plot(freqs,  Dphimax, 'k--');
  plot(freqs, -Dphimax, 'k--');
  set(gca,'xscale','log');
  yticks(-180:90:180);
  ylim([-180 180]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
#+end_src

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

#+NAME: fig:mixed_syn_objective_hinf
#+CAPTION: $\mathcal{H}_\infty$ synthesis objective part of the mixed-synthesis ([[./figs/mixed_syn_objective_hinf.png][png]], [[./figs/mixed_syn_objective_hinf.pdf][pdf]])
[[file:figs/mixed_syn_objective_hinf.png]]

*** Mixed Synthesis Architecture
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 H2/H-Infinity 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(w1*wphi); W2u = ss(w2*wphi); % Weight on the uncertainty
  W1n = ss(N1); W2n = ss(N2); % Weight on the noise

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

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

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

  H1 = 1 - H2;
#+end_src

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

#+begin_src matlab :exports none
  figure;

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

  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
  set(gca,'ColorOrderIndex',2)
  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',[]);
  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(H1, freqs, 'Hz'))), '-');
  set(gca,'ColorOrderIndex',2)
  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)]);
  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_filters_mixed_synthesis.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

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

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

#+begin_src matlab :exports none
  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

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, PSD_S1, '-',  'DisplayName', '$\Phi_{\hat{x}_1}$');
  plot(freqs, PSD_S2, '-',  'DisplayName', '$\Phi_{\hat{x}_2}$');
  plot(freqs, PSD_H2, 'k-', 'DisplayName', '$\Phi_{\hat{x}}$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Power Spectral Density');
  hold off;
  xlim([freqs(1), freqs(end)]);
  legend('location', 'northeast');
#+end_src

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

#+NAME: fig:psd_super_sensor_mixed_syn
#+CAPTION: Power Spectral Density of the Super Sensor obtained with the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis ([[./figs/psd_super_sensor_mixed_syn.png][png]], [[./figs/psd_super_sensor_mixed_syn.pdf][pdf]])
[[file:figs/psd_super_sensor_mixed_syn.png]]


#+begin_src matlab :exports none
  CPS_S1 = 1/pi*cumtrapz(2*pi*freqs, PSD_S1);
  CPS_S2 = 1/pi*cumtrapz(2*pi*freqs, PSD_S2);
  CPS_H2 = 1/pi*cumtrapz(2*pi*freqs, PSD_H2);
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, CPS_S1, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_1} = %.1e$', sqrt(CPS_S1(end))));
  plot(freqs, CPS_S2, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_2} = %.1e$', sqrt(CPS_S2(end))));
  plot(freqs, CPS_H2, 'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_2}} = %.1e$', sqrt(CPS_H2(end))));
  set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum');
  hold off;
  xlim([2e-1, freqs(end)]);
  ylim([1e-10 1e-5]);
  legend('location', 'southeast');
#+end_src

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

#+NAME: fig:cps_super_sensor_mixed_syn
#+CAPTION: Cumulative Power Spectrum of the Super Sensor obtained with the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis ([[./figs/cps_super_sensor_mixed_syn.png][png]], [[./figs/cps_super_sensor_mixed_syn.pdf][pdf]])
[[file:figs/cps_super_sensor_mixed_syn.png]]

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

#+begin_src matlab :exports none
  G1 = 1 + w1*ultidyn('Delta',[1 1]);
  G2 = 1 + w2*ultidyn('Delta',[1 1]);

  Gss = G1*H1 + G2*H2;
  Gsss = usample(Gss, 20);

  % We here compute the maximum and minimum phase of the super sensor
  Dphiss = 180/pi*asin(abs(squeeze(freqresp(w1*H1, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))));
  Dphiss(abs(squeeze(freqresp(w1*H1, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))) > 1) = 190;

  % We here compute the maximum and minimum phase of both sensors
  Dphi1 = 180/pi*asin(abs(squeeze(freqresp(w1, freqs, 'Hz'))));
  Dphi2 = 180/pi*asin(abs(squeeze(freqresp(w2, freqs, 'Hz'))));
  Dphi1(abs(squeeze(freqresp(w1, freqs, 'Hz'))) > 1) = 190;
  Dphi2(abs(squeeze(freqresp(w2, freqs, 'Hz'))) > 1) = 190;
#+end_src

#+begin_src matlab :exports none
  figure;
  % Magnitude
  ax1 = subplot(2,1,1);
  hold on;
  set(gca,'ColorOrderIndex',1);
  plot(freqs, 1 + abs(squeeze(freqresp(w1, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S1');
  set(gca,'ColorOrderIndex',1);
  plot(freqs, max(1 - abs(squeeze(freqresp(w1, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
  set(gca,'ColorOrderIndex',2);
  plot(freqs, 1 + abs(squeeze(freqresp(w2, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S2');
  set(gca,'ColorOrderIndex',2);
  plot(freqs, max(1 - abs(squeeze(freqresp(w2, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
  plot(freqs, 1 + abs(squeeze(freqresp(w1*H1+w2*H2, freqs, 'Hz'))), 'k--', 'DisplayName', 'Bounds - SS');
  plot(freqs, max(1 - abs(squeeze(freqresp(w1*H1+w2*H2, freqs, 'Hz'))), 0), 'k--', 'HandleVisibility', 'off');
  plot(freqs, abs(squeeze(freqresp(Gsss(1, 1, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'DisplayName', 'SS Dynamics');
  for i = 2:length(Gsss)
    plot(freqs, abs(squeeze(freqresp(Gsss(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'HandleVisibility', 'off');
  end
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  legend('location', 'southwest');
  ylabel('Magnitude');
  ylim([5e-2, 10]);
  hold off;

  % Phase
  ax2 = subplot(2,1,2);
  hold on;
  set(gca,'ColorOrderIndex',1);
  plot(freqs,  Dphi1, '--');
  set(gca,'ColorOrderIndex',1);
  plot(freqs, -Dphi1, '--');
  set(gca,'ColorOrderIndex',2);
  plot(freqs,  Dphi2, '--');
  set(gca,'ColorOrderIndex',2);
  plot(freqs, -Dphi2, '--');
  plot(freqs,  Dphiss, 'k--');
  plot(freqs, -Dphiss, 'k--');
  for i = 1:length(Gsss)
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gsss(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2]);
  end
  set(gca,'xscale','log');
  yticks(-180:90:180);
  ylim([-180 180]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
#+end_src

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

#+NAME: fig:super_sensor_dyn_uncertainty_mixed_syn
#+CAPTION: Super Sensor Dynamical Uncertainty obtained with the mixed synthesis ([[./figs/super_sensor_dyn_uncertainty_mixed_syn.png][png]], [[./figs/super_sensor_dyn_uncertainty_mixed_syn.pdf][pdf]])
[[file:figs/super_sensor_dyn_uncertainty_mixed_syn.png]]

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

* Mixed Synthesis - LMI Optimization                                :noexport:
** Introduction
The following matlab scripts was written by Mohit.

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

** Noise characteristics and Uncertainty of the individual sensors
We define the weights that are used to characterize the dynamic uncertainty of the sensors. This will be used for the $\mathcal{H}_\infty$ part of the synthesis.
#+begin_src matlab
  omegac = 100*2*pi; G0 = 0.1; Ginf = 10;
  w1 = (Ginf*s/omegac + G0)/(s/omegac + 1);

  omegac = 0.2*2*pi; G0 = 5; Ginf = 0.1;
  w2 = (Ginf*s/omegac + G0)/(s/omegac + 1);
  omegac = 5000*2*pi; G0 = 1; Ginf = 50;
  w2 = w2*(Ginf*s/omegac + G0)/(s/omegac + 1);
#+end_src

We define the noise characteristics of the two sensors by choosing $N_1$ and $N_2$. This will be used for the $\mathcal{H}_2$ part of the synthesis.
#+begin_src matlab
  omegac = 100*2*pi; G0 = 1e-5; Ginf = 1e-4;
  N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/100);

  omegac = 1*2*pi; G0 = 1e-3; Ginf = 1e-8;
  N2 = ((sqrt(Ginf)*s/omegac + sqrt(G0))/(s/omegac + 1))^2/(1 + s/2/pi/4000)^2;
#+end_src

** Weights
The weights for the $\mathcal{H}_2$ and $\mathcal{H}_\infty$ part are defined below.

#+begin_src matlab
  n = 4; w0 = 2*pi*900; G0 = 9; G1 = 1; Gc = 1.1;
  H = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n;
  wphi = 0.2*(s+3.142e04)/(s+628.3)/H;

  W1u = ss(w1*wphi); W2u = ss(w2*wphi); % Weight on the uncertainty
  W1n = ss(N1); W2n = ss(N2); % Weight on the noise
#+end_src

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

** LMI Optimization
We are using the [[http://cvxr.com/cvx/][CVX toolbox]] to solve the optimization problem.

We first put the generalized plant in a State-space form.
#+begin_src matlab
  A = P.A;
  Bw = P.B(:,1);
  Bu = P.B(:,2);
  Cz1 = P.C(1:2,:); Dz1w = P.D(1:2,1); Dz1u = P.D(1:2,2); % Hinf
  Cz2 = P.C(3:4,:); Dz2w = P.D(1:2,1); Dz2u = P.D(1:2,2); % H2
  Cy = P.C(5,:); Dyw = P.D(5,1); Dyu = P.D(5,2);

  n = size(P.A,1);
  ny = 1; % number of measurements
  nu = 1; % number of control inputs
  nz = 2;
  nw = 1;

  Wtinf = 0;
  Wt2 = 1;
#+end_src

We Define all the variables.
#+begin_src matlab
  cvx_startup;

  cvx_begin sdp
  cvx_quiet true
  cvx_solver sedumi
  variable X(n,n) symmetric;
  variable Y(n,n) symmetric;
  variable W(nz,nz) symmetric;
  variable Ah(n,n);
  variable Bh(n,ny);
  variable Ch(nu,n);
  variable Dh(nu,ny);
  variable eta;
  variable gam;
#+end_src

We define the minimization objective.
#+begin_src matlab
  minimize Wt2*eta+Wtinf*gam % mix objective
  subject to:
#+end_src

The $\mathcal{H}_\infty$ constraint.
#+begin_src matlab
  gam<=1; % Keep the Hinf norm less than 1

  [ X, eye(n,n) ;
    eye(n,n), Y ] >= 0 ;

  [ A*X + Bu*Ch + X*A' + Ch'*Bu', A+Bu*Dh*Cy+Ah', Bw+Bu*Dh*Dyw, X*Cz1' + Ch'*Dz1u' ;
    (A+Bu*Dh*Cy+Ah')', Y*A + A'*Y + Bh*Cy + Cy'*Bh', Y*Bw + Bh*Dyw, (Cz1+Dz1u*Dh*Cy)' ;
    (Bw+Bu*Dh*Dyw)', Bw'*Y + Dyw'*Bh', -eye(nw,nw), (Dz1w+Dz1u*Dh*Dyw)' ;
    Cz1*X + Dz1u*Ch, Cz1+Dz1u*Dh*Cy, Dz1w+Dz1u*Dh*Dyw, -gam*eye(nz,nz)] <= 0 ;
#+end_src

The $\mathcal{H}_2$ constraint.
#+begin_src matlab
  trace(W) <= eta ;

  [ W, Cz2*X+Dz2u*Ch, Cz2*X+Dz2u*Ch;
    X*Cz2'+Ch'*Dz2u', X, eye(n,n) ;
    (Cz2*X+Dz2u*Ch)', eye(n,n), Y ] >= 0 ;

  [ A*X + Bu*Ch + X*A' + Ch'*Bu', A+Bu*Dh*Cy+Ah', Bw+Bu*Dh*Dyw ;
    (A+Bu*Dh*Cy+Ah')', Y*A + A'*Y + Bh*Cy + Cy'*Bh', Y*Bw + Bh*Dyw ;
    (Bw+Bu*Dh*Dyw)', Bw'*Y + Dyw'*Bh', -eye(nw,nw)] <= 0 ;
#+end_src

And we run the optimization.
#+begin_src matlab
  cvx_end
  cvx_status
#+end_src

#+begin_src matlab :exports none
  if(strcmp(cvx_status,'Inaccurate/Solved'))
      display('The solver was unable to make a determination to within the default numerical tolerance.');
      display('However, it determined that the results obtained satisfied a “relaxed” tolerance leve');
      display('and therefore may still be suitable for further use.');
  end
#+end_src

Finally, we can compute the obtained complementary filters.
#+begin_src matlab
  M = eye(n);
  N = inv(M)*(eye(n,n)-Y*X);
  Dk = Dh;
  Ck = (Ch-Dk*Cy*X)*inv(M');
  Bk = inv(N)*(Bh-Y*Bu*Dk);
  Ak = inv(N)*(Ah-Y*(A+Bu*Dk*Cy)*X-N*Bk*Cy*X-Y*Bu*Ck*M')*inv(M');

  H2 = tf(ss(Ak,Bk,Ck,Dk));
  H1 = 1 - H2;
#+end_src

** Result
The obtained complementary filters are compared with the required upper bounds on Figure [[fig:LMI_obtained_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(W1u, freqs, 'Hz'))), '--', 'DisplayName', '$W_1$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, 1./abs(squeeze(freqresp(W2u, freqs, 'Hz'))), '--', 'DisplayName', '$W_2$');

  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
  set(gca,'ColorOrderIndex',2)
  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',[]);
  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(H1, freqs, 'Hz'))), '-');
  set(gca,'ColorOrderIndex',2)
  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]);
  ylim([-180, 180]);

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

#+NAME: fig:LMI_obtained_comp_filters
#+CAPTION: Obtained complementary filters using the LMI optimization ([[./figs/LMI_obtained_comp_filters.png][png]], [[./figs/LMI_obtained_comp_filters.pdf][pdf]])
[[file:figs/LMI_obtained_comp_filters.png]]

** Comparison with the matlab Mixed Synthesis
The Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis is performed below.
#+begin_src matlab
  Nmeas = 1; Ncon = 1; Nz2 = 2;

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

  H1m = 1 - H2m;
#+end_src

The obtained filters are compare with the one obtained using the CVX toolbox in Figure [[]].

#+begin_src matlab :exports none
  figure;

  ax1 = subplot(2,1,1);
  hold on;
  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(H1m, freqs, 'Hz'))), '--', 'DisplayName', '$H_{1,\mathcal{H}_2/\mathcal{H}_\infty}$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, abs(squeeze(freqresp(H2m, freqs, 'Hz'))), '--', 'DisplayName', '$H_{2,\mathcal{H}_2/\mathcal{H}_\infty}$');

  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_{1, CVX}$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'DisplayName', '$H_{2, CVX}$');

  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(H1m, freqs, 'Hz'))), '--');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, 180/pi*phase(squeeze(freqresp(H2m, freqs, 'Hz'))), '--');

  set(gca,'ColorOrderIndex',1)
  plot(freqs, 180/pi*phase(squeeze(freqresp(H1, freqs, 'Hz'))), '-');
  set(gca,'ColorOrderIndex',2)
  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]);
  ylim([-180, 180]);

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

#+NAME: fig:compare_cvx_h2hinf_comp_filters
#+CAPTION: Comparison between the complementary filters obtained with the CVX toolbox and with the =h2hinfsyn= command ([[./figs/compare_cvx_h2hinf_comp_filters.png][png]], [[./figs/compare_cvx_h2hinf_comp_filters.pdf][pdf]])
[[file:figs/compare_cvx_h2hinf_comp_filters.png]]

** H-Infinity Objective
In terms of the $\mathcal{H}_\infty$ objective, both synthesis method are satisfying the requirements as shown in Figure [[fig:comp_cvx_h2i_hinf_norm]].

#+begin_src matlab :exports none
  figure;
  hold on;
  set(gca,'ColorOrderIndex',1)
  plot(freqs, 1./abs(squeeze(freqresp(W1u, freqs, 'Hz'))), '-.', 'DisplayName', '$1/W_{1u}$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, 1./abs(squeeze(freqresp(W2u, freqs, 'Hz'))), '-.', 'DisplayName', '$1/W_{2u}$');

  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(H1m, freqs, 'Hz'))), '--', 'DisplayName', '$H_{1,\mathcal{H}_2/\mathcal{H}_\infty}$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, abs(squeeze(freqresp(H2m, freqs, 'Hz'))), '--', 'DisplayName', '$H_{2,\mathcal{H}_2/\mathcal{H}_\infty}$');

  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_{1, CVX}$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'DisplayName', '$H_{2, CVX}$');

  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Magnitude');
  set(gca, 'XTickLabel',[]);
  ylim([1e-3, 2]);
  legend('location', 'southwest');
  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_cvx_h2i_hinf_norm.pdf" :var figsize="full-normal" :post pdf2svg(file=*this*, ext="png")
  <<plt-matlab>>
#+end_src

#+NAME: fig:comp_cvx_h2i_hinf_norm
#+CAPTION: H-Infinity norm requirement and results ([[./figs/comp_cvx_h2i_hinf_norm.png][png]], [[./figs/comp_cvx_h2i_hinf_norm.pdf][pdf]])
[[file:figs/comp_cvx_h2i_hinf_norm.png]]

** Obtained Super Sensor's noise
The PSD and CPS of the super sensor's noise obtained with the CVX toolbox and =h2hinfsyn= command are compared in Figure [[fig:psd_compare_cvx_h2i]] and [[fig:cps_compare_cvx_h2i]].

#+begin_src matlab :exports none
  PSD_S1 = abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2;
  PSD_S2 = abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2;
  PSD_cvx = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
  PSD_h2i = abs(squeeze(freqresp(N1*H1m, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2m, freqs, 'Hz'))).^2;
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, PSD_S1, '-',  'DisplayName', '$\Phi_{\hat{x}_1}$');
  plot(freqs, PSD_S2, '-',  'DisplayName', '$\Phi_{\hat{x}_2}$');
  plot(freqs, PSD_cvx, 'k-', 'DisplayName', '$\Phi_{\hat{x}, CVX}$');
  plot(freqs, PSD_h2i, 'k--', 'DisplayName', '$\Phi_{\hat{x}, \mathcal{H}_2/\mathcal{H}_\infty}$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Power Spectral Density');
  hold off;
  xlim([freqs(1), freqs(end)]);
  legend('location', 'northeast');
#+end_src

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

#+NAME: fig:psd_compare_cvx_h2i
#+CAPTION: Power Spectral Density of the Super Sensor obtained with the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis ([[./figs/psd_compare_cvx_h2i.png][png]], [[./figs/psd_compare_cvx_h2i.pdf][pdf]])
[[file:figs/psd_compare_cvx_h2i.png]]


#+begin_src matlab :exports none
  CPS_S1 = 1/pi*cumtrapz(2*pi*freqs, PSD_S1);
  CPS_S2 = 1/pi*cumtrapz(2*pi*freqs, PSD_S2);
  CPS_cvx = 1/pi*cumtrapz(2*pi*freqs, PSD_cvx);
  CPS_h2i = 1/pi*cumtrapz(2*pi*freqs, PSD_h2i);
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(freqs, CPS_S1, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_1} = %.1e$', sqrt(CPS_S1(end))));
  plot(freqs, CPS_S2, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_2} = %.1e$', sqrt(CPS_S2(end))));
  plot(freqs, CPS_cvx, 'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{CVX}} = %.1e$', sqrt(CPS_cvx(end))));
  plot(freqs, CPS_h2i, 'k--', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_2/\\mathcal{H}_\\infty}} = %.1e$', sqrt(CPS_h2i(end))));
  set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum');
  hold off;
  xlim([2e-1, freqs(end)]);
  ylim([1e-10 1e-5]);
  legend('location', 'southeast');
#+end_src

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

#+NAME: fig:cps_compare_cvx_h2i
#+CAPTION: Cumulative Power Spectrum of the Super Sensor obtained with the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis ([[./figs/cps_compare_cvx_h2i.png][png]], [[./figs/cps_compare_cvx_h2i.pdf][pdf]])
[[file:figs/cps_compare_cvx_h2i.png]]

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

#+begin_src matlab :exports none
  G1 = 1 + w1*ultidyn('Delta',[1 1]);
  G2 = 1 + w2*ultidyn('Delta',[1 1]);

  % We here compute the maximum and minimum phase of the super sensor
  Dphiss_cvx = 180/pi*asin(abs(squeeze(freqresp(w1*H1, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))));
  Dphiss_cvx(abs(squeeze(freqresp(w1*H1, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))) > 1) = 190;

  Dphiss_h2i = 180/pi*asin(abs(squeeze(freqresp(w1*H1m, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2m, freqs, 'Hz'))));
  Dphiss_h2i(abs(squeeze(freqresp(w1*H1m, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2m, freqs, 'Hz'))) > 1) = 190;

  % We here compute the maximum and minimum phase of both sensors
  Dphi1 = 180/pi*asin(abs(squeeze(freqresp(w1, freqs, 'Hz'))));
  Dphi2 = 180/pi*asin(abs(squeeze(freqresp(w2, freqs, 'Hz'))));
  Dphi1(abs(squeeze(freqresp(w1, freqs, 'Hz'))) > 1) = 190;
  Dphi2(abs(squeeze(freqresp(w2, freqs, 'Hz'))) > 1) = 190;
#+end_src

#+begin_src matlab :exports none
  figure;
  % Magnitude
  ax1 = subplot(2,1,1);
  hold on;
  set(gca,'ColorOrderIndex',1);
  plot(freqs, 1 + abs(squeeze(freqresp(w1, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S1');
  set(gca,'ColorOrderIndex',1);
  plot(freqs, max(1 - abs(squeeze(freqresp(w1, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
  set(gca,'ColorOrderIndex',2);
  plot(freqs, 1 + abs(squeeze(freqresp(w2, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S2');
  set(gca,'ColorOrderIndex',2);
  plot(freqs, max(1 - abs(squeeze(freqresp(w2, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
  plot(freqs, 1 + abs(squeeze(freqresp(w1*H1+w2*H2, freqs, 'Hz'))), 'k--', 'DisplayName', 'Bounds - CVX');
  plot(freqs, max(1 - abs(squeeze(freqresp(w1*H1+w2*H2, freqs, 'Hz'))), 0), 'k--', 'HandleVisibility', 'off');
  plot(freqs, 1 + abs(squeeze(freqresp(w1*H1m+w2*H2m, freqs, 'Hz'))), 'k-', 'DisplayName', 'Bounds - $\mathcal{H}_2/\mathcal{H}_\infty$');
  plot(freqs, max(1 - abs(squeeze(freqresp(w1*H1m+w2*H2m, freqs, 'Hz'))), 0), 'k-', 'HandleVisibility', 'off');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  legend('location', 'southwest');
  ylabel('Magnitude');
  ylim([5e-2, 10]);
  hold off;

  % Phase
  ax2 = subplot(2,1,2);
  hold on;
  set(gca,'ColorOrderIndex',1);
  plot(freqs,  Dphi1, '--');
  set(gca,'ColorOrderIndex',1);
  plot(freqs, -Dphi1, '--');
  set(gca,'ColorOrderIndex',2);
  plot(freqs,  Dphi2, '--');
  set(gca,'ColorOrderIndex',2);
  plot(freqs, -Dphi2, '--');
  plot(freqs,  Dphiss_cvx, 'k--');
  plot(freqs, -Dphiss_cvx, 'k--');
  plot(freqs,  Dphiss_h2i, 'k-');
  plot(freqs, -Dphiss_h2i, 'k-');
  set(gca,'xscale','log');
  yticks(-180:90:180);
  ylim([-180 180]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
#+end_src

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

#+NAME: fig:super_sensor_uncertainty_compare_cvx_h2i
#+CAPTION: Super Sensor Dynamical Uncertainty obtained with the mixed synthesis ([[./figs/super_sensor_uncertainty_compare_cvx_h2i.png][png]], [[./figs/super_sensor_uncertainty_compare_cvx_h2i.pdf][pdf]])
[[file:figs/super_sensor_uncertainty_compare_cvx_h2i.png]]

* Optimal And Robust Sensor Fusion in Practice                      :noexport:
<<sec:opti_robust_practice>>
** Introduction                                                     :ignore:
Here are the steps in order to apply optimal and robust sensor fusion:

- Measure the noise characteristics of the sensors to be merged (necessary for "optimal" part of the fusion)
- Measure/Estimate the dynamic uncertainty of the sensors (necessary for "robust" part of the fusion)
- Apply H2/H-infinity synthesis of the complementary filters

** Measurement of the noise characteristics of the sensors
*** Huddle Test
The technique to estimate the sensor noise is taken from cite:barzilai98_techn_measur_noise_sensor_presen.

Let's consider two sensors (sensor 1 and sensor 2) that are measuring the same quantity $x$ as shown in figure [[fig:huddle_test]].

#+NAME: fig:huddle_test
#+CAPTION: Huddle test block diagram
[[file:figs-tikz/huddle_test.png]]

Each sensor has uncorrelated noise $n_1$ and $n_2$ and internal dynamics $G_1(s)$ and $G_2(s)$ respectively.

We here suppose that each sensor has the same magnitude of instrumental noise: $n_1 = n_2 = n$.
We also assume that their dynamics is ideal: $G_1(s) = G_2(s) = 1$.

We then have:
#+NAME: eq:coh_bis
\begin{equation}
  \gamma_{\hat{x}_1\hat{x}_2}^2(\omega) = \frac{1}{1 + 2 \left( \frac{|\Phi_n(\omega)|}{|\Phi_{\hat{x}}(\omega)|} \right) + \left( \frac{|\Phi_n(\omega)|}{|\Phi_{\hat{x}}(\omega)|} \right)^2}
\end{equation}

Since the input signal $x$ and the instrumental noise $n$ are incoherent:
#+NAME: eq:incoherent_noise
\begin{equation}
  |\Phi_{\hat{x}}(\omega)| = |\Phi_n(\omega)| + |\Phi_x(\omega)|
\end{equation}

From equations eqref:eq:coh_bis and eqref:eq:incoherent_noise, we finally obtain
#+begin_important
#+NAME: eq:noise_psd
\begin{equation}
  |\Phi_n(\omega)| = |\Phi_{\hat{x}}(\omega)| \left( 1 - \sqrt{\gamma_{\hat{x}_1\hat{x}_2}^2(\omega)} \right)
\end{equation}
#+end_important

*** Weights that represents the noises' PSD

For further complementary filter synthesis, it is preferred to consider a normalized noise source $\tilde{n}$ that has a PSD equal to one ($\Phi_{\tilde{n}}(\omega) = 1$) and to use a weighting filter $N(s)$ in order to represent the frequency dependence of the noise.

The weighting filter $N(s)$ should be designed such that:
\begin{align*}
                      & \Phi_n(\omega) \approx |N(j\omega)|^2 \Phi_{\tilde{n}}(\omega) \quad \forall \omega \\
  \Longleftrightarrow & |N(j\omega)| \approx \sqrt{\Phi_n(\omega)} \quad \forall \omega
\end{align*}

These weighting filters can then be used to compare the noise level of sensors for the synthesis of complementary filters.

The sensor with a normalized noise input is shown in figure [[fig:one_sensor_normalized_noise]].

#+name: fig:one_sensor_normalized_noise
#+caption: One sensor with normalized noise
[[file:figs-tikz/one_sensor_normalized_noise.png]]

*** Comparison of the noises' PSD
Once the noise of the sensors to be merged have been characterized, the power spectral density of both sensors have to be compared.

Ideally, the PSD of the noise are such that:
\begin{align*}
  \Phi_{n_1}(\omega) &< \Phi_{n_2}(\omega) \text{ for } \omega < \omega_m \\
  \Phi_{n_1}(\omega) &> \Phi_{n_2}(\omega) \text{ for } \omega > \omega_m
\end{align*}

*** Computation of the coherence, power spectral density and cross spectral density of signals
The coherence between signals $x$ and $y$ is defined as follow
\[ \gamma^2_{xy}(\omega) = \frac{|\Phi_{xy}(\omega)|^2}{|\Phi_{x}(\omega)| |\Phi_{y}(\omega)|} \]
where $|\Phi_x(\omega)|$ is the output Power Spectral Density (PSD) of signal $x$ and $|\Phi_{xy}(\omega)|$ is the Cross Spectral Density (CSD) of signal $x$ and $y$.

The PSD and CSD are defined as follow:
\begin{align}
  |\Phi_x(\omega)|    &= \frac{2}{n_d T} \sum^{n_d}_{n=1} \left| X_k(\omega, T) \right|^2 \\
  |\Phi_{xy}(\omega)| &= \frac{2}{n_d T} \sum^{n_d}_{n=1} [ X_k^*(\omega, T) ] [ Y_k(\omega, T) ]
\end{align}
where:
- $n_d$ is the number for records averaged
- $T$ is the length of each record
- $X_k(\omega, T)$ is the finite Fourier transform of the $k^{\text{th}}$ record
- $X_k^*(\omega, T)$ is its complex conjugate

** Estimate the dynamic uncertainty of the sensors

Let's consider one sensor represented on figure [[fig:one_sensor_dyn_uncertainty]].

The dynamic uncertainty is represented by an input multiplicative uncertainty where $w(s)$ is a weight that represents the level of the uncertainty.

The goal is to accurately determine $w(s)$ for the sensors that have to be merged.

#+name: fig:one_sensor_dyn_uncertainty
#+caption: Sensor with dynamic uncertainty
[[file:figs-tikz/one_sensor_dyn_uncertainty.png]]

** Optimal and Robust synthesis of the complementary filters
Once the noise characteristics and dynamic uncertainty of both sensors have been determined and we have determined the following weighting functions:
- $W_1(s)$ and $W_2(s)$ representing the dynamic uncertainty of both sensors
- $N_1(s)$ and $N_2(s)$ representing the noise characteristics of both sensors

The goal is to design complementary filters $H_1(s)$ and $H_2(s)$ shown in figure [[fig:sensor_fusion_full]] such that:
- the uncertainty on the super sensor dynamics is minimized
- the noise sources $\tilde{n}_1$ and $\tilde{n}_2$ has the lowest possible effect on the estimation $\hat{x}$

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

* Real World Example of optimal sensor fusion                       :noexport:
** Introduction                                                      :ignore:
cite:moore19_capac_instr_sensor_fusion_high_bandW_nanop


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

** Sensor Noise                                                    :noexport:
#+begin_src matlab
  A1 = 19.13; % [uV2/Hz]
  A2 = 0.1632; % [uV2/Hz]
  A3 = 6.847; % [uV2/Hz]
  wnc = 3057; % [rad]
  wx = 7929; % [rad/s]
 
  Fx = 1/(1 - s/wx)/(1 - s/wx);
  [A B C D] = butter(2, 0.5, 'low');
  Fx = ss(A, B, C, D);

  Sq = A3*wnc/s + A3;
  Sx = A1*Fx + A2;
#+end_src

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

  figure;
  hold on;
  plot(freqs, abs(squeeze(freqresp(Sq, freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Sx, freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
#+end_src

** Matlab Code
Take an Accelerometer and a Geophone both measuring the absolute motion of a structure.

Parameters of the inertial sensors.
#+begin_src matlab
  m_acc = 0.01;
  k_acc = 1e6;
  c_acc = 20;

  m_geo = 1;
  k_geo = 1e3;
  c_geo = 10;
#+end_src

Transfer function from motion to measurement

For the accelerometer.
The measurement is the relative motion structure/inertial mass:
\[ \frac{d}{\ddot{w}} = \frac{-m}{ms^2 + cs + k} \]

For the geophone.
The measurement is the relative velocity structure/inertial mass:
\[ \frac{\dot{d}}{\dot{w}} = \frac{-ms^2}{ms^2 + cs + k} \]

#+begin_src matlab
  G_acc = -m_acc/(m_acc*s^2 + c_acc*s + k_acc); % [m/(m/s^2)]
  G_geo = -m_geo*s^2/(m_geo*s^2 + c_geo*s + k_geo); % [m/s/m/s]
#+end_src

Suppose the measure of the relative motion for the accelerometer (capacitive sensor for instance) has a white noise characteristic:
Suppose the measure of the relative velocity (current flowing through the coil) has a white noise characteristic:

Define the noise characteristics
#+begin_src matlab
  n = 1; w0 = 2*pi*5e3; G0 = 5e-12; G1 = 1e-15; Gc = G0/2;
  L_acc = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n;

  n = 1; w0 = 2*pi*5e3; G0 = 1e-6; G1 = 1e-8; Gc = G0/2;
  L_geo = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n;
#+end_src

Transfer function of the conversion to obtain the velocity:
#+begin_src matlab
  C_acc = (-k_acc/m_acc/(2*pi + s));
  C_geo = tf(-1);
#+end_src

Let's plot the noise of both sensors:
#+begin_src matlab :exports none
  freqs = logspace(-1, 4, 1000);

  figure;
  hold on;
  plot(freqs, abs(squeeze(freqresp(L_acc*C_acc, freqs, 'Hz'))), 'DisplayName', 'Acc');
  plot(freqs, abs(squeeze(freqresp(L_geo*C_geo, freqs, 'Hz'))), 'DisplayName', 'Geo');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Noise ASD [$m/s/\sqrt{Hz}$]');
  legend('location', 'northeast')
#+end_src

Dynamics of both sensors
#+begin_src matlab :exports none
  freqs = logspace(-1, 4, 1000);

  figure;
  hold on;
  plot(freqs, abs(squeeze(freqresp(s*G_acc*C_acc, freqs, 'Hz'))), 'DisplayName', 'Acc');
  plot(freqs, abs(squeeze(freqresp(G_geo*C_geo, freqs, 'Hz'))), 'DisplayName', 'Geo');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  legend('location', 'northeast')
#+end_src

** Time domain signals
#+begin_src matlab
  Fs = 1e4; % Sampling Frequency [Hz]
  Ts = 1/Fs; % Sampling Time [s]

  t = 0:Ts:10; % Time Vector [s]
#+end_src

#+begin_src matlab
  n_acc = lsim(L_acc*C_acc, sqrt(Fs/2)*randn(length(t), 1), t); % [m/s]
  n_geo = lsim(L_geo*C_geo, sqrt(Fs/2)*randn(length(t), 1), t); % [m/s]
#+end_src

#+begin_src matlab
  figure;
  hold on;
  plot(t, n_geo)
  plot(t, n_acc)
  hold off;
#+end_src

** H2 Synthesis
#+begin_src matlab
  N1 = L_acc*C_acc;
  N2 = L_geo*C_geo;
#+end_src

#+begin_src matlab
  bodeFig({N1, N2}, logspace(-1, 5, 1000))
#+end_src

#+begin_src matlab
  P = [0   N2  1;
       N1 -N2  0];
#+end_src

And we do the $\mathcal{H}_2$ synthesis using the =h2syn= command.
#+begin_src matlab
  [H1, ~, gamma] = h2syn(P, 1, 1);
#+end_src

Finally, we define $H_2(s) = 1 - H_1(s)$.
#+begin_src matlab
  H2 = 1 - H1;
#+end_src

#+begin_src matlab
  bodeFig({H1, H2}, struct('phase', true))
#+end_src

#+begin_src matlab
  n_acc_filt = lsim(H1, n_acc, t);
  n_geo_filt = lsim(H2, n_geo, t);
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
  data2orgtable([rms(n_acc), rms(n_geo), rms(n_acc_filt + n_geo_filt)]', {'Accelerometer', 'Geophone', 'Super Sensor'}, {'RMS'}, ' %.1e ');
#+end_src

#+RESULTS:
|               |     RMS |
|---------------+---------|
| Accelerometer | 9.7e-05 |
| Geophone      | 5.9e-05 |
| Super Sensor  | 1.5e-05 |

#+begin_src matlab
  figure;
  hold on;
  plot(t, n_geo)
  plot(t, n_acc)
  plot(t, n_acc_filt + n_geo_filt)
  hold off;
#+end_src

** Signal and Noise
Velocity Signal:
#+begin_src matlab
  v = lsim(1/(1 + s/2/pi/2), 1e-4*sqrt(Fs/2)*randn(length(t), 1), t);
  v = 1e-4 * sin(2*pi*100*t);
#+end_src

#+begin_src matlab
  v_acc = lsim(s*G_acc*C_acc, v, t) + n_acc;
  v_geo = lsim(G_geo*C_geo,   v, t) + n_geo;
#+end_src

#+begin_src matlab
  v_ss = lsim(H1, v_acc, t) + lsim(H2, v_geo, t);
#+end_src

#+begin_src matlab
  figure;
  hold on;
  plot(t, v_geo)
  plot(t, v_acc)
  plot(t, v_ss)
  plot(t, v, 'k--')
  hold off;
  xlim([1, 1+0.1])
#+end_src

** PSD and CPS
#+begin_src matlab
  nx = length(n_acc);
  na = 16;
  win = hanning(floor(nx/na));

  [p_acc, f] = pwelch(n_acc, win, 0, [], Fs);
  [p_geo, ~] = pwelch(n_geo, win, 0, [], Fs);
  [p_ss,  ~] = pwelch(n_acc_filt + n_geo_filt, win, 0, [], Fs);
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(f, p_acc, 'DisplayName', 'Accelerometer');
  plot(f, p_geo, 'DisplayName', 'Geophone');
  plot(f, p_ss,  'DisplayName', 'Super Sensor');
  hold off;
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]');
  ylabel('Power Spectral Density $\left[\frac{(m/s)^2}{Hz}\right]$');
  legend('location', 'southwest');
#+end_src

** Transfer function of the super sensor
#+begin_src matlab
  bodeFig({s*C_acc*G_acc, C_geo*G_geo, s*C_acc*G_acc*H1+C_geo*G_geo*H2}, struct('phase', true))
#+end_src

* 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, mustBePositive} = 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