Rework H2 synthesis section

matlab/index.org

@@ -12,6 +12,7 @@
 #+HTML_HEAD: <script src="../js/bootstrap.min.js"></script>
 #+HTML_HEAD: <script src="../js/jquery.stickytableheaders.min.js"></script>
 #+HTML_HEAD: <script src="../js/readtheorg.js"></script>
+#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="https://cdn.rawgit.com/dreampulse/computer-modern-web-font/master/fonts.css">
 
 #+PROPERTY: header-args:matlab  :session *MATLAB*
 #+PROPERTY: header-args:matlab+ :tangle no
@@ -42,56 +43,32 @@ Two sensors are considered with both different noise characteristics and dynamic
 
 ** Introduction                                                      :ignore:
 In Figure [[fig:sensor_model_noise_uncertainty]] is shown a schematic of a sensor model that is used in the following study.
+In this example, the measured quantity $x$ is the velocity of an object.
 
 #+name: tab:sensor_signals
 #+caption: Description of signals in Figure [[fig:sensor_model_noise_uncertainty]]
-| *Notation*    | *Meaning*                       |
+| *Notation*    | *Meaning*                       | *Unit*  |
-|---------------+---------------------------------|
+|---------------+---------------------------------+---------|
-| $x$           | Physical measured quantity      |
+| $x$           | Physical measured quantity      | $[m/s]$ |
-| $\tilde{n}_i$ | White noise with unitary PSD    |
+| $\tilde{n}_i$ | White noise with unitary PSD    |         |
-| $n_i$         | Shaped noise                    |
+| $n_i$         | Shaped noise                    | $[m/s]$ |
-| $v_i$         | Sensor output measurement       |
+| $v_i$         | Sensor output measurement       | $[V]$   |
-| $\hat{x}_i$   | Estimate of $x$ from the sensor |
+| $\hat{x}_i$   | Estimate of $x$ from the sensor | $[m/s]$ |
 
 #+name: tab:sensor_dynamical_blocks
 #+caption: Description of Systems in Figure [[fig:sensor_model_noise_uncertainty]]
-| *Notation*  | *Meaning*                                                                    |
+| *Notation*  | *Meaning*                                                                    | *Unit*            |
-|-------------+------------------------------------------------------------------------------|
+|-------------+------------------------------------------------------------------------------+-------------------|
-| $\hat{G}_i$ | Nominal Sensor Dynamics                                                      |
+| $\hat{G}_i$ | Nominal Sensor Dynamics                                                      | $[\frac{V}{m/s}]$ |
-| $W_i$       | Weight representing the size of the uncertainty at each frequency            |
+| $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$ |
+| $\Delta_i$  | Any complex perturbation such that $\vert\vert\Delta_i\vert\vert_\infty < 1$ |                   |
-| $N_i$       | Weight representing the sensor noise                                         |
+| $N_i$       | Weight representing the sensor noise                                         | $[m/s]$           |
 
 #+name: fig:sensor_model_noise_uncertainty
 #+caption: Sensor Model
 #+RESULTS:
 [[file:figs-tikz/sensor_model_noise_uncertainty.png]]
 
-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>>
@@ -269,7 +246,7 @@ The Power Spectral Density of the sensor noise $\Phi_{n_i}(\omega)$ is then comp
 
 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;
+  omegac = 0.15*2*pi; G0 = 1e-1; Ginf = 1e-6;
   N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4);
 
   omegac = 1000*2*pi; G0 = 1e-6; Ginf = 1e-3;
@@ -431,10 +408,10 @@ If we first suppose that the models of the sensors $\hat{G}_i$ are very close to
 
 As the noise of both sensors are considered to be uncorrelated, the PSD of the super sensor noise is computed as follow:
 \begin{equation}
-  \Phi_n(\omega) = \left| H_1 N_1 \right|^2 + \left| H_2 N_2 \right|^2 \label{eq:super_sensor_psd_noise}
+  \Phi_n(\omega) = \left| H_1(j\omega) N_1(j\omega) \right|^2 + \left| H_2(j\omega) N_2(j\omega) \right|^2 \label{eq:super_sensor_psd_noise}
 \end{equation}
 
-And the Root Mean Square (RMS) value of the super sensor noise $\sigma_n$ is given by eqref:eq:super_sensor_rms_noise.
+And the Root Mean Square (RMS) value of the super sensor noise $\sigma_n$ is given by Equation eqref:eq:super_sensor_rms_noise.
 \begin{equation}
   \sigma_n = \sqrt{\int_0^\infty \Phi_n(\omega) d\omega} \label{eq:super_sensor_rms_noise}
 \end{equation}
@@ -470,16 +447,23 @@ The uncertainty set of the transfer function from $\hat{x}$ to $x$ at frequency
 <<sec:optimal_comp_filters>>
 
 ** Introduction                                                      :ignore:
-In this section, the two sensors are merged together using complementary
+In this section, the complementary filters $H_1(s)$ and $H_2(s)$ are designed in order to minimize the RMS value of super sensor noise $\sigma_n$.
-
-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.
 
 #+name: fig:sensor_fusion_noise_arch
 #+caption: Optimal Sensor Fusion Architecture
 [[file:figs-tikz/sensor_fusion_noise_arch.png]]
 
+The RMS value of the super sensor noise is (neglecting the model uncertainty):
+\begin{equation}
+\begin{aligned}
+  \sigma_{n} &= \sqrt{\int_0^\infty |H_1(j\omega) N_1(j\omega)|^2 + |H_2(j\omega) N_2(j\omega)|^2 d\omega} \\
+                   &= \left\| \begin{matrix} H_1 N_1 \\ H_2 N_2 \end{matrix} \right\|_2
+\end{aligned}
+\end{equation}
+
+The goal is to design $H_1(s)$ and $H_2(s)$ such that $H_1(s) + H_2(s) = 1$ (complementary property) and such that $\left\| \begin{matrix} H_1 N_1 \\ H_2 N_2 \end{matrix} \right\|_2$ is minimized (minimized RMS value of the super sensor noise).
+This is done using the $\mathcal{H}_2$ synthesis in Section [[sec:H2_synthesis]].
+
 ** Matlab Init                                              :noexport:ignore:
 #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
   <<matlab-dir>>
@@ -490,54 +474,51 @@ The complementary filters have to be designed in order to minimize the effect no
 #+end_src
 
 #+begin_src matlab
+  addpath('src');
   load('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
 #+end_src
 
-** H-Two Synthesis
+** $\mathcal{H}_2$ 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:
+<<sec:H2_synthesis>>
-\[ \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.
+Consider the generalized plant $P_{\mathcal{H}_2}$ shown in Figure [[fig:h_two_optimal_fusion]] and described by Equation eqref:eq:H2_generalized_plant.
-
-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} \label{eq:H2_generalized_plant}
-\begin{equation*}
 \begin{pmatrix}
-  z \\ v
+  z_1 \\ z_2 \\ v
-\end{pmatrix} = \begin{pmatrix}
+\end{pmatrix} = \underbrace{\begin{bmatrix}
-  0   &  N_2 & 1 \\
+  N_1 & -N_1 \\
-  N_1 & -N_2 & 0
+  0   &  N_2 \\
-\end{pmatrix} \begin{pmatrix}
+  1   &  0
-  W_1 \\ W_2 \\ u
+\end{bmatrix}}_{P_{\mathcal{H}_2}} \begin{pmatrix}
+  w \\ u
 \end{pmatrix}
-\end{equation*}
+\end{equation}
 
-The transfer function from $[n_1, n_2]$ to $\hat{x}$ is:
+Applying the $\mathcal{H}_2$ synthesis on $P_{\mathcal{H}_2}$ will generate a filter $H_2(s)$ such that the $\mathcal{H}_2$ norm from $w$ to $(z_1,z_2)$ which is actually equals to $\sigma_n$ by defining $H_1(s) = 1 - H_2(s)$:
-\[ \begin{bmatrix} N_1 H_1 \\ N_2 (1 - H_1) \end{bmatrix} \]
+\begin{equation}
-If we define $H_2 = 1 - H_1$, we obtain:
+   \left\| \begin{matrix} z_1/w \\ z_2/w \end{matrix} \right\|_2 = \left\| \begin{matrix} N_1 (1 - H_2) \\ N_2 H_2 \end{matrix} \right\|_2 = \sigma_n \quad \text{with} \quad H_1(s) = 1 - H_2(s)
-\[ \begin{bmatrix} N_1 H_1 \\ N_2 H_2 \end{bmatrix} \]
+\end{equation}
 
-Thus, if we minimize the $\mathcal{H}_2$ norm of this transfer function, we minimize the RMS value of $\hat{x}$.
+We then have that the $\mathcal{H}_2$ synthesis applied on $P_{\mathcal{H}_2}$ generates two complementary filters $H_1(s)$ and $H_2(s)$ such that the RMS value of super sensor noise is minimized.
 
-We define the generalized plant $P$ on matlab as shown on Figure
+The generalized plant $P_{\mathcal{H}_2}$ is defined below
 #+begin_src matlab
-  P = [N1 -N1;
+  PH2 = [N1 -N1;
-       0   N2;
+         0   N2;
-       1   0];
+         1   0];
 #+end_src
 
-And we do the $\mathcal{H}_2$ synthesis using the =h2syn= command.
+The $\mathcal{H}_2$ synthesis using the =h2syn= command
 #+begin_src matlab
-  [H2, ~, gamma] = h2syn(P, 1, 1);
+  [H2, ~, gamma] = h2syn(PH2, 1, 1);
 #+end_src
 
-Finally, we define $H_2(s) = 1 - H_1(s)$.
+Finally, $H_1(s)$ is defined as follows
 #+begin_src matlab
   H1 = 1 - H2;
 #+end_src
@@ -547,185 +528,26 @@ Finally, we define $H_2(s) = 1 - H_1(s)$.
   save('./mat/H2_filters.mat', 'H2', 'H1');
 #+end_src
 
-The complementary filters obtained are shown on Figure
+The obtained complementary filters are shown in 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);
+  plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), 'DisplayName', '$H_1$');
-  plotMagUncertainty(W2, freqs, 'color_i', 2);
+  plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), 'DisplayName', '$H_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;
+  legend('location', 'northeast');
 
   % Phase
   ax2 = subplot(2,1,2);
   hold on;
-  plotPhaseUncertainty(W1, freqs, 'color_i', 1);
+  plot(freqs, 180/pi*angle(squeeze(freqresp(H1, freqs, 'Hz'))));
-  plotPhaseUncertainty(W2, freqs, 'color_i', 2);
+  plot(freqs, 180/pi*angle(squeeze(freqresp(H2, freqs, 'Hz'))));
-  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]);
@@ -735,10 +557,214 @@ Generate noises in velocity corresponding to sensor 1 and 2:
   xlim([freqs(1), freqs(end)]);
 #+end_src
 
-** Conclusion
+#+begin_src matlab :tangle no :exports results :results file replace
-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).
+  exportFig('figs/htwo_comp_filters.pdf', 'width', 'full', 'height', 'tall');
+#+end_src
 
-However, the synthesis does not take into account the robustness of the sensor fusion.
+#+name: fig:htwo_comp_filters
+#+caption:  Obtained complementary filters using the $\mathcal{H}_2$ Synthesis
+#+RESULTS:
+[[file:figs/htwo_comp_filters.png]]
+
+** Super Sensor Noise
+<<sec:H2_super_sensor_noise>>
+
+The Power Spectral Density of the individual sensors' noise $\Phi_{n_1}, \Phi_{n_2}$ and of the super sensor noise $\Phi_{n_{\mathcal{H}_2}}$ are computed below and shown in Figure [[fig:psd_sensors_htwo_synthesis]].
+#+begin_src matlab
+  PSD_S1 = abs(squeeze(freqresp(N1,    freqs, 'Hz'))).^2;
+  PSD_S2 = abs(squeeze(freqresp(N2,    freqs, 'Hz'))).^2;
+  PSD_H2 = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2 + ...
+           abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
+#+end_src
+
+The corresponding Cumulative Power Spectrum $\Gamma_{n_1}$, $\Gamma_{n_2}$ and $\Gamma_{n_{\mathcal{H}_2}}$ (cumulative integration of the PSD eqref:eq:CPS_definition) are computed below and shown in Figure [[fig:cps_h2_synthesis]].
+#+begin_src matlab
+  CPS_S1 = cumtrapz(freqs, PSD_S1);
+  CPS_S2 = cumtrapz(freqs, PSD_S2);
+  CPS_H2 = cumtrapz(freqs, PSD_H2);
+#+end_src
+
+\begin{equation}
+  \Gamma_n (\omega) = \int_0^\omega \Phi_n(\nu) d\nu \label{eq:CPS_definition}
+\end{equation}
+
+The RMS value of the individual sensors and of the super sensor are listed in Table [[tab:rms_noise_H2]].
+#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
+  data2orgtable([sqrt(CPS_S1(end)); sqrt(CPS_S2(end)); sqrt(CPS_H2(end))], {'$\sigma_{n_1}$', '$\sigma_{n_2}$', '$\sigma_{n_{\mathcal{H}_2}}$'}, {'RMS value $[m/s]$'}, ' %.1e ');
+#+end_src
+
+#+name: tab:rms_noise_H2
+#+caption: RMS value of the individual sensor noise and of the super sensor using the $\mathcal{H}_2$ Synthesis
+#+RESULTS:
+|                              | RMS value $[m/s]$ |
+|------------------------------+-------------------|
+| $\sigma_{n_1}$               |             0.015 |
+| $\sigma_{n_2}$               |              0.08 |
+| $\sigma_{n_{\mathcal{H}_2}}$ |            0.0027 |
+
+#+begin_src matlab :exports none
+  figure;
+  hold on;
+  plot(freqs, PSD_S1, '-',  'DisplayName', '$\Phi_{n_1}$');
+  plot(freqs, PSD_S2, '-',  'DisplayName', '$\Phi_{n_2}$');
+  plot(freqs, PSD_H2, 'k-', 'DisplayName', '$\Phi_{n_{\mathcal{H}_2}}$');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  xlabel('Frequency [Hz]'); ylabel('Power Spectral Density [$(m/s)^2/Hz$]');
+  hold off;
+  xlim([freqs(1), freqs(end)]);
+  legend('location', 'northeast');
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+  exportFig('figs/psd_sensors_htwo_synthesis.pdf', 'width', 'wide', 'height', 'normal');
+#+end_src
+
+#+name: fig:psd_sensors_htwo_synthesis
+#+caption: Power Spectral Density of the estimated $\hat{x}$ using the two sensors alone and using the optimally fused signal
+#+RESULTS:
+[[file:figs/psd_sensors_htwo_synthesis.png]]
+
+#+begin_src matlab :exports none
+  figure;
+  hold on;
+  plot(freqs, CPS_S1, '-',  'DisplayName', '$\Gamma_{n_1}$');
+  plot(freqs, CPS_S2, '-',  'DisplayName', '$\Gamma_{n_2}$');
+  plot(freqs, CPS_H2, 'k-', 'DisplayName', '$\Gamma_{n_{\mathcal{H}_2}}$');
+  set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
+  xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum $[(m/s)^2]$');
+  hold off;
+  xlim([2*freqs(1), freqs(end)]);
+  legend('location', 'southeast');
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+  exportFig('figs/cps_h2_synthesis.pdf', 'width', 'wide', 'height', 'normal');
+#+end_src
+
+#+name: fig:cps_h2_synthesis
+#+caption: Cumulative Power Spectrum of individual sensors and super sensor using the $\mathcal{H}_2$ synthesis
+#+RESULTS:
+[[file:figs/cps_h2_synthesis.png]]
+
+A time domain simulation is now performed.
+The measured velocity $x$ is set to be a sweep sine with an amplitude of $0.1\ [m/s]$.
+The velocity estimates from the two sensors and from the super sensors are shown in Figure [[fig:super_sensor_time_domain_h2]].
+The resulting noises are displayed in Figure [[fig:sensor_noise_H2_time_domain]].
+
+#+begin_src matlab :exports none
+  Fs = 1e4;  % Sampling Frequency [Hz]
+  Ts = 1/Fs; % Sampling Time [s]
+
+  t = 0:Ts:2; % Time Vector [s]
+
+  v = 0.1*sin((10*t).*t)'; % Velocity measured [m/s]
+
+  % Generate noises in velocity corresponding to sensor 1 and 2:
+  n1 = lsim(N1, sqrt(Fs/2)*randn(length(t), 1), t);
+  n2 = lsim(N2, sqrt(Fs/2)*randn(length(t), 1), t);
+#+end_src
+
+#+begin_src matlab :exports none
+  figure;
+  hold on;
+  set(gca,'ColorOrderIndex',2)
+  plot(t, v + n2, 'DisplayName', '$\hat{x}_2$');
+  set(gca,'ColorOrderIndex',1)
+  plot(t, v + n1, 'DisplayName', '$\hat{x}_1$');
+  set(gca,'ColorOrderIndex',3)
+  plot(t, v + (lsim(H1, n1, t) + lsim(H2, n2, t)), 'DisplayName', '$\hat{x}$');
+  plot(t, v, 'k--', 'DisplayName', '$x$');
+  hold off;
+  xlabel('Time [s]'); ylabel('Velocity [m/s]');
+  legend('location', 'southwest');
+  ylim([-0.3, 0.3]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+  exportFig('figs/super_sensor_time_domain_h2.pdf', 'width', 'wide', 'height', 'normal');
+#+end_src
+
+#+name: fig:super_sensor_time_domain_h2
+#+caption: Noise of individual sensors and noise of the super sensor
+#+RESULTS:
+[[file:figs/super_sensor_time_domain_h2.png]]
+
+#+begin_src matlab :exports none
+  figure;
+  hold on;
+  set(gca,'ColorOrderIndex',2)
+  plot(t, n2, 'DisplayName', '$n_2$');
+  set(gca,'ColorOrderIndex',1)
+  plot(t, n1, 'DisplayName', '$n_1$');
+  set(gca,'ColorOrderIndex',3)
+  plot(t, (lsim(H1, n1, t)+lsim(H2, n2, t)), '-', 'DisplayName', '$n$');
+  hold off;
+  xlabel('Time [s]'); ylabel('Sensor Noise [m/s]');
+  legend();
+  ylim([-0.2, 0.2]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+  exportFig('figs/sensor_noise_H2_time_domain.pdf', 'width', 'wide', 'height', 'normal');
+#+end_src
+
+#+name: fig:sensor_noise_H2_time_domain
+#+caption:
+#+RESULTS:
+[[file:figs/sensor_noise_H2_time_domain.png]]
+
+** Discrepancy between sensor dynamics and model
+If we consider sensor dynamical uncertainty as explained in Section [[sec:sensor_uncertainty]], we can compute what would be the super sensor dynamical uncertainty when using the complementary filters obtained using the $\mathcal{H}_2$ Synthesis.
+
+The super sensor dynamical uncertainty is shown in Figure [[fig:super_sensor_dynamical_uncertainty_H2]].
+
+It is shown that the phase uncertainty is not bounded between 100Hz and 200Hz.
+As a result the super sensor signal can not be used for feedback applications about 100Hz.
+#+begin_src matlab :exports none
+  Dphi_ss = 180/pi*asin(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))));
+  Dphi_ss(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))) > 1) = 360;
+
+  figure;
+  % Magnitude
+  ax1 = subplot(2,1,1);
+  hold on;
+  plotMagUncertainty(W1, freqs, 'color_i', 1, 'DisplayName', '$1 + W_1 \Delta_1$');
+  plotMagUncertainty(W2, freqs, 'color_i', 2, 'DisplayName', '$1 + W_2 \Delta_2$');
+  plot(freqs, 1 + abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))+abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 'k-', ...
+       'DisplayName', '$1 + W_1 \Delta_1 + W_2 \Delta_2$')
+  plot(freqs, max(1 - abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))-abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 0.001), 'k-', ...
+       'HandleVisibility', 'off');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  set(gca, 'XTickLabel',[]);
+  ylabel('Magnitude');
+  ylim([1e-2, 1e1]);
+  legend('location', 'southeast');
+  hold off;
+
+  % Phase
+  ax2 = subplot(2,1,2);
+  hold on;
+  plotPhaseUncertainty(W1, freqs, 'color_i', 1);
+  plotPhaseUncertainty(W2, freqs, 'color_i', 2);
+  plot(freqs,  Dphi_ss, 'k-');
+  plot(freqs, -Dphi_ss, 'k-');
+  set(gca,'xscale','log');
+  yticks(-180:90:180);
+  ylim([-180 180]);
+  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+  hold off;
+  linkaxes([ax1,ax2],'x');
+  xlim([freqs(1), freqs(end)]);
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+  exportFig('figs/super_sensor_dynamical_uncertainty_H2.pdf', 'width', 'full', 'height', 'full');
+#+end_src
+
+#+name: fig:super_sensor_dynamical_uncertainty_H2
+#+caption: Super sensor dynamical uncertainty when using the $\mathcal{H}_2$ Synthesis
+#+RESULTS:
+[[file:figs/super_sensor_dynamical_uncertainty_H2.png]]
 
 * Robust Sensor Fusion: $\mathcal{H}_\infty$ Synthesis
 :PROPERTIES:
@@ -757,7 +783,7 @@ To do so, we model the uncertainty that we have on the sensor dynamics by multip
 #+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.
+The objective here is to design complementary filters $H_1(s)$ and $H_2(s)$ in order to bound the dynamical uncertainty of the super sensor to acceptable values.
 
 ** Matlab Init                                              :noexport:ignore:
 #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
@@ -897,7 +923,7 @@ The obtained upper bounds on the complementary filters in order to limit the pha
 #+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]])
+#+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
 [[file:figs/upper_bounds_comp_filter_max_phase_uncertainty.png]]
 
 ** $\mathcal{H}_\infty$ Synthesis
@@ -910,8 +936,8 @@ The $\mathcal{H}_\infty$ synthesis architecture used for the complementary filte
 The generalized plant is defined below.
 #+begin_src matlab
   P = [Wu*W1 -Wu*W1;
-       0       Wu*W2;
+       0      Wu*W2;
-       1       0];
+       1      0];
 #+end_src
 
 And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command.
@@ -988,7 +1014,7 @@ The obtained complementary filters are shown in Figure [[fig:comp_filter_hinf_un
 #+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]])
+#+CAPTION: Obtained complementary filters
 [[file:figs/comp_filter_hinf_uncertainty.png]]
 
 ** Super sensor uncertainty
@@ -1081,7 +1107,7 @@ The CPS of both sensor and of the super sensor is shown in Figure [[fig:cps_sens
 #+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]])
+#+CAPTION: Power Spectral Density of the obtained super sensor using the $\mathcal{H}_\infty$ synthesis
 [[file:figs/psd_sensors_hinf_synthesis.png]]
 
 #+begin_src matlab :exports none
@@ -1104,7 +1130,7 @@ The CPS of both sensor and of the super sensor is shown in Figure [[fig:cps_sens
 #+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]])
+#+CAPTION: Cumulative Power Spectrum of the obtained super sensor using the $\mathcal{H}_\infty$ synthesis
 [[file:figs/cps_sensors_hinf_synthesis.png]]
 
 ** Conclusion
@@ -1180,7 +1206,7 @@ Both dynamical uncertainty and noise characteristics of the individual sensors a
 #+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]])
+#+CAPTION: Noise characteristsics and Dynamical uncertainty of the individual sensors
 [[file:figs/mixed_synthesis_noise_uncertainty_sensors.png]]
 
 ** Weighting Functions on the uncertainty of the super sensor
@@ -1287,7 +1313,7 @@ The obtained complementary filters are shown in Figure [[fig:comp_filters_mixed_
 #+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]])
+#+CAPTION: Obtained complementary filters after mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis
 [[file:figs/comp_filters_mixed_synthesis.png]]
 
 ** Obtained Super Sensor's noise
@@ -1322,7 +1348,7 @@ The PSD and CPS of the super sensor's noise are shown in Figure [[fig:psd_super_
 #+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]])
+#+CAPTION: Power Spectral Density of the Super Sensor obtained with the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis
 [[file:figs/psd_super_sensor_mixed_syn.png]]
 
 #+begin_src matlab :exports none
@@ -1345,7 +1371,7 @@ The PSD and CPS of the super sensor's noise are shown in Figure [[fig:psd_super_
 #+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]])
+#+CAPTION: Cumulative Power Spectrum of the Super Sensor obtained with the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis
 [[file:figs/cps_super_sensor_mixed_syn.png]]
 
 ** Obtained Super Sensor's Uncertainty
@@ -1503,7 +1529,7 @@ This Matlab function is accessible [[file:src/plotMagUncertainty.m][here]].
       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.opacity (1,1) double {mustBeNumeric, mustBeNonnegative} = 0.3
       args.DisplayName char = ''
   end
 

matlab/src/plotMagUncertainty.m

@@ -19,7 +19,7 @@ arguments
     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.opacity (1,1) double {mustBeNumeric, mustBeNonnegative} = 0.3
     args.DisplayName char = ''
 end