css/htmlize.css

@@ -143,42 +143,3 @@
 .org-widget-field { /* widget-field */ background-color: #d9d9d9; }
 .org-widget-inactive { /* widget-inactive */ color: #7f7f7f; }
 .org-widget-single-line-field { /* widget-single-line-field */ background-color: #d9d9d9; }
-
-
-pre                                      {background-color:#FFFFFF;}
-pre span.org-builtin                     {color:#006FE0;font-weight:bold;}
-pre span.org-string                      {color:#008000;}
-pre span.org-doc                         {color:#008000;}
-pre span.org-keyword                     {color:#0000FF;}
-pre span.org-variable-name               {color:#BA36A5;}
-pre span.org-function-name               {color:#006699;}
-pre span.org-type                        {color:#6434A3;}
-pre span.org-preprocessor                {color:#808080;font-weight:bold;}
-pre span.org-constant                    {color:#D0372D;}
-pre span.org-comment-delimiter           {color:#8D8D84;}
-pre span.org-comment                     {color:#8D8D84;font-style:italic}
-pre span.org-outshine-level-1            {color:#8D8D84;font-style:italic}
-pre span.org-outshine-level-2            {color:#8D8D84;font-style:italic}
-pre span.org-outshine-level-3            {color:#8D8D84;font-style:italic}
-pre span.org-outshine-level-4            {color:#8D8D84;font-style:italic}
-pre span.org-outshine-level-5            {color:#8D8D84;font-style:italic}
-pre span.org-outshine-level-6            {color:#8D8D84;font-style:italic}
-pre span.org-outshine-level-7            {color:#8D8D84;font-style:italic}
-pre span.org-outshine-level-8            {color:#8D8D84;font-style:italic}
-pre span.org-outshine-level-9            {color:#8D8D84;font-style:italic}
-pre span.org-rainbow-delimiters-depth-1  {color:#707183;}
-pre span.org-rainbow-delimiters-depth-2  {color:#7388d6;}
-pre span.org-rainbow-delimiters-depth-3  {color:#909183;}
-pre span.org-rainbow-delimiters-depth-4  {color:#709870;}
-pre span.org-rainbow-delimiters-depth-5  {color:#907373;}
-pre span.org-rainbow-delimiters-depth-6  {color:#6276ba;}
-pre span.org-rainbow-delimiters-depth-7  {color:#858580;}
-pre span.org-rainbow-delimiters-depth-8  {color:#80a880;}
-pre span.org-rainbow-delimiters-depth-9  {color:#887070;}
-pre span.org-sh-quoted-exec              {color:#FF1493;}
-pre span.org-diff-added                  {color:#008000;}
-pre span.org-diff-changed                {color:#0000FF;}
-pre span.org-diff-header                 {color:#800000;}
-pre span.org-diff-hunk-header            {color:#990099;}
-pre span.org-diff-none                   {color:#545454;}
-pre span.org-diff-removed                {color:#A60000;}

css/readtheorg.css

@@ -345,11 +345,9 @@ table{
     border-collapse:collapse;
     border-spacing:0;
     empty-cells:show;
-    border-bottom:1px solid #e1e4e5;
-    margin-right: auto;
-    margin-left: auto;
     margin-bottom:24px;
-    /* margin: 0 auto; */
+    border-bottom:1px solid #e1e4e5;
+    margin: 0 auto;
 }
 
 td{
@@ -1103,32 +1101,3 @@ h2.footnotes{
     margin-bottom: 24px;
     font-family:"Roboto Slab","ff-tisa-web-pro","Georgia",Arial,sans-serif;
 }
-
-
-
-details {
-    /* color: #2980B9; */
-    background: #fbfbfb;
-    border: 1px solid  #c9c9c9;
-    border-radius: 3px;
-    padding:  0.25em;
-    margin-bottom: 1.0em;
-}
-details pre.src {
-    border: 0;
-    background: none;
-    margin: 0;
-}
-details pre.src-lisp::before { content: ""; }
-summary {
-    outline: 0;
-    color: #c9c9c9;
-}
-summary::after {
-    font-size: 0.85em;
-    color: #c9c9c9;
-    display: inline-block;
-    float: right;
-    content: "Click to fold/unfold";
-    padding-right:  0.5em;
-}

matlab/index.org

@@ -23,10 +23,6 @@
 #+LATEX_CLASS: cleanreport
 #+LATEX_CLASS_OPTIONS: [tocnp, minted, secbreak]
 
-#+LATEX_HEADER_EXTRA: \usepackage[cache=false]{minted}
-#+LATEX_HEADER_EXTRA: \usemintedstyle{autumn}
-#+LATEX_HEADER_EXTRA: \setminted[matlab]{linenos=true, breaklines=true, tabsize=4, fontsize=\scriptsize, autogobble=true}
-
 #+LATEX_HEADER: \newcommand{\authorFirstName}{Thomas}
 #+LATEX_HEADER: \newcommand{\authorLastName}{Dehaeze}
 #+LATEX_HEADER: \newcommand{\authorEmail}{dehaeze.thomas@gmail.com}
@@ -75,16 +71,13 @@ In this example, the measured quantity $x$ is the velocity of an object.
 #+caption: Description of signals in Figure [[fig:sensor_model_noise_uncertainty]]
 #+attr_latex: :environment tabular :align clc
 #+attr_latex: :center t :booktabs t :float t
-| *Notation*       | *Meaning*                         | *Unit*                    |
+| *Notation*    | *Meaning*                       | *Unit*  |
-|------------------+-----------------------------------+---------------------------|
+|---------------+---------------------------------+---------|
-| $x$              | Physical measured quantity        | $[m/s]$                   |
+| $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                      | $[m/s]$                   |
+| $n_i$         | Shaped noise                    | $[m/s]$ |
-| $v_i$            | Sensor output measurement         | $[V]$                     |
+| $v_i$         | Sensor output measurement       | $[V]$   |
-| $\hat{x}_i$      | Estimate of $x$ from the sensor   | $[m/s]$                   |
+| $\hat{x}_i$   | Estimate of $x$ from the sensor | $[m/s]$ |
-| $\Phi_n(\omega)$ | Power Spectral Density of $n$     | $[\frac{(m/s)^2}{Hz}]$    |
-| $\phi_n(\omega)$ | Amplitude Spectral Density of $n$ | $[\frac{m/s}{\sqrt{Hz}}]$ |
-| $\sigma_n$       | Root Mean Square Value of $n$     | $[m/s\ rms]$              |
 
 #+name: tab:sensor_dynamical_blocks
 #+caption: Description of Systems in Figure [[fig:sensor_model_noise_uncertainty]]
@@ -151,8 +144,8 @@ Both sensor dynamics in $[\frac{V}{m/s}]$ are shown in Figure [[fig:sensors_nomi
   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 $\left[\frac{V}{m/s}\right]$'); set(gca, 'XTickLabel',[]);
+  ylabel('Magnitude $[\frac{V}{m/s}]$'); set(gca, 'XTickLabel',[]);
-  legend('location', 'northeast', 'FontSize', 8);
+  legend('location', 'northeast');
   hold off;
 
   % Phase
@@ -170,7 +163,7 @@ Both sensor dynamics in $[\frac{V}{m/s}]$ are shown in Figure [[fig:sensors_nomi
 #+end_src
 
 #+begin_src matlab :tangle no :exports results :results file replace
-  exportFig('figs/sensors_nominal_dynamics.pdf', 'width', 'half', 'height', 'tall');
+  exportFig('figs/sensors_nominal_dynamics.pdf', 'width', 'full', 'height', 'full');
 #+end_src
 
 #+name: fig:sensors_nominal_dynamics
@@ -208,11 +201,11 @@ The bode plot of the sensors nominal dynamics as well as their defined dynamical
   xlabel('Frequency [Hz]'); ylabel('Magnitude');
   ylim([0, 5]);
   xlim([freqs(1), freqs(end)]);
-  legend('location', 'northwest', 'FontSize', 8);
+  legend('location', 'northwest');
 #+end_src
 
 #+begin_src matlab :tangle no :exports results :results file replace
-  exportFig('figs/sensors_uncertainty_weights.pdf', 'width', 'half', 'height', 'short');
+  exportFig('figs/sensors_uncertainty_weights.pdf', 'width', 'wide', 'height', 'normal');
 #+end_src
 
 #+name: fig:sensors_uncertainty_weights
@@ -235,9 +228,8 @@ The bode plot of the sensors nominal dynamics as well as their defined dynamical
   set(gca, 'XTickLabel',[]);
   ylabel('Magnitude $[\frac{V}{m/s}]$');
   ylim([1e-2, 2e3]);
-  legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 2);
+  legend('location', 'northeast');
   hold off;
-  ylim([1e-2, 1e4])
 
   % Phase
   ax2 = subplot(2,1,2);
@@ -258,7 +250,7 @@ The bode plot of the sensors nominal dynamics as well as their defined dynamical
 #+end_src
 
 #+begin_src matlab :tangle no :exports results :results file replace
-  exportFig('figs/sensors_nominal_dynamics_and_uncertainty.pdf', 'width', 'half', 'height', 'tall');
+  exportFig('figs/sensors_nominal_dynamics_and_uncertainty.pdf', 'width', 'full', 'height', 'full');
 #+end_src
 
 #+name: fig:sensors_nominal_dynamics_and_uncertainty
@@ -293,14 +285,14 @@ The weights $N_1$ and $N_2$ representing the amplitude spectral density of the s
   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('ASD $\left[ \frac{m/s}{\sqrt{Hz}} \right]$');
+  xlabel('Frequency [Hz]'); ylabel('Amplitude Spectral Density $\left[ \frac{m/s}{\sqrt{Hz}} \right]$');
   hold off;
   xlim([freqs(1), freqs(end)]);
-  legend('location', 'northeast', 'FontSize', 8);
+  legend('location', 'northeast');
 #+end_src
 
 #+begin_src matlab :tangle no :exports results :results file replace
-  exportFig('figs/sensors_noise.pdf', 'width', 'half', 'height', 'short');
+  exportFig('figs/sensors_noise.pdf', 'width', 'normal', 'height', 'normal');
 #+end_src
 
 #+name: fig:sensors_noise
@@ -395,6 +387,10 @@ The uncertainty set of the transfer function from $\hat{x}$ to $x$ at frequency
 ** Introduction                                                      :ignore:
 In this section, the complementary filters $H_1(s)$ and $H_2(s)$ are designed in order to minimize the RMS value of super sensor noise $\sigma_n$.
 
+#+name: fig:sensor_fusion_noise_arch
+#+caption: Optimal Sensor Fusion Architecture
+[[file:figs-tikz/sensor_fusion_noise_arch.png]]
+
 The RMS value of the super sensor noise is (neglecting the model uncertainty):
 \begin{equation}
 \begin{aligned}
@@ -483,7 +479,7 @@ The obtained complementary filters are shown in Figure [[fig:htwo_comp_filters]]
   set(gca, 'XTickLabel',[]);
   ylabel('Magnitude');
   hold off;
-  legend('location', 'northeast', 'FontSize', 8);
+  legend('location', 'northeast');
 
   % Phase
   ax2 = subplot(2,1,2);
@@ -500,7 +496,7 @@ The obtained complementary filters are shown in Figure [[fig:htwo_comp_filters]]
 #+end_src
 
 #+begin_src matlab :tangle no :exports results :results file replace
-  exportFig('figs/htwo_comp_filters.pdf', 'width', 'half', 'height', 'tall');
+  exportFig('figs/htwo_comp_filters.pdf', 'width', 'full', 'height', 'tall');
 #+end_src
 
 #+name: fig:htwo_comp_filters
@@ -511,7 +507,7 @@ The obtained complementary filters are shown in Figure [[fig:htwo_comp_filters]]
 ** 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.
+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;
@@ -519,8 +515,6 @@ The Power Spectral Density of the individual sensors' noise $\Phi_{n_1}, \Phi_{n
            abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
 #+end_src
 
-The obtained ASD are shown in Figure [[fig:psd_sensors_htwo_synthesis]].
-
 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(trapz(freqs, PSD_S1)); sqrt(trapz(freqs, PSD_S2)); sqrt(trapz(freqs, PSD_H2))], ...
@@ -542,18 +536,18 @@ The RMS value of the individual sensors and of the super sensor are listed in Ta
 #+begin_src matlab :exports none
   figure;
   hold on;
-  plot(freqs, sqrt(PSD_S1), '-',  'DisplayName', '$\phi_{n_1}$');
+  plot(freqs, PSD_S1, '-',  'DisplayName', '$\Phi_{n_1}$');
-  plot(freqs, sqrt(PSD_S2), '-',  'DisplayName', '$\phi_{n_2}$');
+  plot(freqs, PSD_S2, '-',  'DisplayName', '$\Phi_{n_2}$');
-  plot(freqs, sqrt(PSD_H2), 'k-', 'DisplayName', '$\phi_{n_{\mathcal{H}_2}}$');
+  plot(freqs, PSD_H2, 'k-', 'DisplayName', '$\Phi_{n_{\mathcal{H}_2}}$');
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  xlabel('Frequency [Hz]'); ylabel('ASD $\left[ \frac{m/s}{\sqrt{Hz}} \right]$');
+  xlabel('Frequency [Hz]'); ylabel('Power Spectral Density $\left[ \frac{(m/s)^2}{Hz} \right]$');
   hold off;
   xlim([freqs(1), freqs(end)]);
-  legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
+  legend('location', 'northeast');
 #+end_src
 
 #+begin_src matlab :tangle no :exports results :results file replace
-  exportFig('figs/psd_sensors_htwo_synthesis.pdf', 'width', 'half', 'height', 'short');
+  exportFig('figs/psd_sensors_htwo_synthesis.pdf', 'width', 'wide', 'height', 'normal');
 #+end_src
 
 #+name: fig:psd_sensors_htwo_synthesis
@@ -591,12 +585,12 @@ The resulting noises are displayed in Figure [[fig:sensor_noise_H2_time_domain]]
   plot(t, v, 'k--', 'DisplayName', '$x$');
   hold off;
   xlabel('Time [s]'); ylabel('Velocity [m/s]');
-  legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
+  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', 'half', 'height', 'normal');
+  exportFig('figs/super_sensor_time_domain_h2.pdf', 'width', 'wide', 'height', 'normal');
 #+end_src
 
 #+name: fig:super_sensor_time_domain_h2
@@ -615,12 +609,12 @@ The resulting noises are displayed in Figure [[fig:sensor_noise_H2_time_domain]]
   plot(t, (lsim(H1, n1, t)+lsim(H2, n2, t)), '-', 'DisplayName', '$n$');
   hold off;
   xlabel('Time [s]'); ylabel('Sensor Noise [m/s]');
-  legend('FontSize', 8);
+  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', 'half', 'height', 'normal');
+  exportFig('figs/sensor_noise_H2_time_domain.pdf', 'width', 'wide', 'height', 'normal');
 #+end_src
 
 #+name: fig:sensor_noise_H2_time_domain
@@ -653,7 +647,7 @@ As a result the super sensor signal can not be used for feedback applications ab
   set(gca, 'XTickLabel',[]);
   ylabel('Magnitude');
   ylim([1e-2, 1e1]);
-  legend('location', 'southeast', 'FontSize', 8);
+  legend('location', 'southeast');
   hold off;
 
   % Phase
@@ -673,7 +667,7 @@ As a result the super sensor signal can not be used for feedback applications ab
 #+end_src
 
 #+begin_src matlab :tangle no :exports results :results file replace
-  exportFig('figs/super_sensor_dynamical_uncertainty_H2.pdf', 'width', 'half', 'height', 'tall');
+  exportFig('figs/super_sensor_dynamical_uncertainty_H2.pdf', 'width', 'full', 'height', 'full');
 #+end_src
 
 #+name: fig:super_sensor_dynamical_uncertainty_H2
@@ -783,7 +777,7 @@ The uncertainty bounds of the two individual sensor as well as the wanted maximu
   set(gca, 'XTickLabel',[]);
   ylabel('Magnitude');
   ylim([1e-2, 1e1]);
-  legend('location', 'southeast', 'FontSize', 8);
+  legend('location', 'southeast');
   hold off;
 
   % Phase
@@ -803,7 +797,7 @@ The uncertainty bounds of the two individual sensor as well as the wanted maximu
 #+end_src
 
 #+begin_src matlab :tangle no :exports results :results file replace
-  exportFig('figs/weight_uncertainty_bounds_Wu.pdf', 'width', 'half', 'height', 'tall');
+  exportFig('figs/weight_uncertainty_bounds_Wu.pdf', 'width', 'full', 'height', 'full');
 #+end_src
 
 #+name: fig:weight_uncertainty_bounds_Wu
@@ -884,15 +878,16 @@ The obtained complementary filters as well as the wanted upper bounds are shown
   hold on;
   plot(freqs, 1./abs(squeeze(freqresp(Wu*W1, freqs, 'Hz'))), '--', 'DisplayName', '$1/|W_uW_1|$');
   plot(freqs, 1./abs(squeeze(freqresp(Wu*W2, freqs, 'Hz'))), '--', 'DisplayName', '$1/|W_uW_2|$');
+
   set(gca,'ColorOrderIndex',1)
-  plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$|H_1|$');
+  plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
-  plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'DisplayName', '$|H_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', 'FontSize', 8, 'NumColumns', 2);
+  legend('location', 'northeast');
 
   ax2 = subplot(2,1,2);
   hold on;
@@ -908,7 +903,7 @@ The obtained complementary filters as well as the wanted upper bounds are shown
 #+end_src
 
 #+begin_src matlab :tangle no :exports results :results file replace
-  exportFig('figs/hinf_comp_filters.pdf', 'width', 'half', 'height', 'tall');
+  exportFig('figs/hinf_comp_filters.pdf', 'width', 'full', 'height', 'full');
 #+end_src
 
 #+name: fig:hinf_comp_filters
@@ -947,7 +942,7 @@ The $\mathcal{H}_\infty$ synthesis thus allows to design filters such that the s
   set(gca, 'XTickLabel',[]);
   ylabel('Magnitude');
   ylim([1e-2, 1e1]);
-  legend('location', 'southeast', 'FontSize', 8);
+  legend('location', 'southeast');
   hold off;
 
   % Phase
@@ -969,7 +964,7 @@ The $\mathcal{H}_\infty$ synthesis thus allows to design filters such that the s
 #+end_src
 
 #+begin_src matlab :tangle no :exports results :results file replace
-  exportFig('figs/super_sensor_dynamical_uncertainty_Hinf.pdf', 'width', 'half', 'height', 'tall');
+  exportFig('figs/super_sensor_dynamical_uncertainty_Hinf.pdf', 'width', 'full', 'height', 'full');
 #+end_src
 
 #+name: fig:super_sensor_dynamical_uncertainty_Hinf
@@ -978,8 +973,10 @@ The $\mathcal{H}_\infty$ synthesis thus allows to design filters such that the s
 [[file:figs/super_sensor_dynamical_uncertainty_Hinf.png]]
 
 ** Super sensor noise
-We now compute the obtain Power Spectral Density of the super sensor's noise.
+We now compute the obtain Power Spectral Density of the super sensor's noise (Figure [[fig:psd_sensors_hinf_synthesis]]).
-The Amplitude Spectral Densities are shown in Figure [[fig:psd_sensors_hinf_synthesis]].
+
+The obtained RMS of the super sensor noise in the $\mathcal{H}_2$ and $\mathcal{H}_\infty$ case are shown in Table [[tab:rms_noise_comp_H2_Hinf]].
+As expected, the super sensor obtained from the $\mathcal{H}_\infty$ synthesis is much noisier than the super sensor obtained from the $\mathcal{H}_2$ synthesis.
 
 #+begin_src matlab
   PSD_S2   = abs(squeeze(freqresp(N2,    freqs, 'Hz'))).^2;
@@ -988,9 +985,6 @@ The Amplitude Spectral Densities are shown in Figure [[fig:psd_sensors_hinf_synt
              abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
 #+end_src
 
-The obtained RMS of the super sensor noise in the $\mathcal{H}_2$ and $\mathcal{H}_\infty$ case are shown in Table [[tab:rms_noise_comp_H2_Hinf]].
-As expected, the super sensor obtained from the $\mathcal{H}_\infty$ synthesis is much noisier than the super sensor obtained from the $\mathcal{H}_2$ synthesis.
-
 #+begin_src matlab :exports none
   H2_filters = load('./mat/H2_filters.mat', 'H2', 'H1');
 
@@ -1001,19 +995,19 @@ As expected, the super sensor obtained from the $\mathcal{H}_\infty$ synthesis i
 #+begin_src matlab :exports none
   figure;
   hold on;
-  plot(freqs, sqrt(PSD_S1),   '-',   'DisplayName', '$\phi_{n_1}$');
+  plot(freqs, PSD_S1,   '-',   'DisplayName', '$\Phi_{n_1}$');
-  plot(freqs, sqrt(PSD_S2),   '-',   'DisplayName', '$\phi_{n_2}$');
+  plot(freqs, PSD_S2,   '-',   'DisplayName', '$\Phi_{n_2}$');
-  plot(freqs, sqrt(PSD_H2),   'k-',  'DisplayName', '$\phi_{n_{\mathcal{H}_2}}$');
+  plot(freqs, PSD_H2,   'k-',  'DisplayName', '$\Phi_{n_{\mathcal{H}_2}}$');
-  plot(freqs, sqrt(PSD_Hinf), 'k--', 'DisplayName', '$\phi_{n_{\mathcal{H}_\infty}}$');
+  plot(freqs, PSD_Hinf, 'k--', 'DisplayName', '$\Phi_{n_{\mathcal{H}_\infty}}$');
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  xlabel('Frequency [Hz]'); ylabel('ASD $\left[ \frac{m/s}{\sqrt{Hz}} \right]$');
+  xlabel('Frequency [Hz]'); ylabel('Power Spectral Density $\left[ \frac{(m/s)^2}{Hz} \right]$');
   hold off;
   xlim([freqs(1), freqs(end)]);
-  legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
+  legend('location', 'northeast');
 #+end_src
 
 #+begin_src matlab :tangle no :exports results :results file replace
-  exportFig('figs/psd_sensors_hinf_synthesis.pdf', 'width', 'half', 'height', 'normal');
+  exportFig('figs/psd_sensors_hinf_synthesis.pdf', 'width', 'wide', 'height', 'normal');
 #+end_src
 
 #+name: fig:psd_sensors_hinf_synthesis
@@ -1141,7 +1135,7 @@ The obtained complementary filters are shown in Figure [[fig:htwo_hinf_comp_filt
   ylabel('Magnitude');
   set(gca, 'XTickLabel',[]);
   ylim([1e-3, 2]);
-  legend('location', 'southeast', 'FontSize', 8);
+  legend('location', 'southwest');
 
   ax2 = subplot(2,1,2);
   hold on;
@@ -1157,7 +1151,7 @@ The obtained complementary filters are shown in Figure [[fig:htwo_hinf_comp_filt
 #+end_src
 
 #+begin_src matlab :tangle no :exports results :results file replace
-  exportFig('figs/htwo_hinf_comp_filters.pdf', 'width', 'half', 'height', 'tall');
+  exportFig('figs/htwo_hinf_comp_filters.pdf', 'width', 'full', 'height', 'full');
 #+end_src
 
 #+name: fig:htwo_hinf_comp_filters
@@ -1166,7 +1160,7 @@ The obtained complementary filters are shown in Figure [[fig:htwo_hinf_comp_filt
 [[file:figs/htwo_hinf_comp_filters.png]]
 
 ** Obtained Super Sensor's noise
-The Amplitude Spectral Density of the super sensor's noise is shown in Figure [[fig:psd_sensors_htwo_hinf_synthesis]].
+The Power Spectral Density of the super sensor's noise is shown in Figure [[fig:psd_sensors_htwo_hinf_synthesis]].
 
 A time domain simulation is shown in Figure [[fig:super_sensor_time_domain_h2_hinf]].
 
@@ -1196,20 +1190,20 @@ The RMS values of the super sensor noise for the presented three synthesis are l
 #+begin_src matlab :exports none
   figure;
   hold on;
-  plot(freqs, sqrt(PSD_S1),     '-',   'DisplayName', '$\Phi_{n_1}$');
+  plot(freqs, PSD_S1,     '-',   'DisplayName', '$\Phi_{n_1}$');
-  plot(freqs, sqrt(PSD_S2),     '-',   'DisplayName', '$\Phi_{n_2}$');
+  plot(freqs, PSD_S2,     '-',   'DisplayName', '$\Phi_{n_2}$');
-  plot(freqs, sqrt(PSD_H2),     'k-',  'DisplayName', '$\Phi_{n_{\mathcal{H}_2}}$');
+  plot(freqs, PSD_H2,     'k-',  'DisplayName', '$\Phi_{n_{\mathcal{H}_2}}$');
-  plot(freqs, sqrt(PSD_Hinf),   'k--', 'DisplayName', '$\Phi_{n_{\mathcal{H}_\infty}}$');
+  plot(freqs, PSD_Hinf,   'k--', 'DisplayName', '$\Phi_{n_{\mathcal{H}_\infty}}$');
-  plot(freqs, sqrt(PSD_H2Hinf), 'k-.', 'DisplayName', '$\Phi_{n_{\mathcal{H}_2/\mathcal{H}_\infty}}$');
+  plot(freqs, PSD_H2Hinf, 'k-.', 'DisplayName', '$\Phi_{n_{\mathcal{H}_2/\mathcal{H}_\infty}}$');
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  xlabel('Frequency [Hz]'); ylabel('ASD $\left[ \frac{m/s}{\sqrt{Hz}} \right]$');
+  xlabel('Frequency [Hz]'); ylabel('Power Spectral Density [$(m/s)^2/Hz$]');
   hold off;
   xlim([freqs(1), freqs(end)]);
-  legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 3);
+  legend('location', 'northeast');
 #+end_src
 
 #+begin_src matlab :tangle no :exports results :results file replace
-  exportFig('figs/psd_sensors_htwo_hinf_synthesis.pdf', 'width', 'half', 'height', 'normal');
+  exportFig('figs/psd_sensors_htwo_hinf_synthesis.pdf', 'width', 'wide', 'height', 'normal');
 #+end_src
 
 #+name: fig:psd_sensors_htwo_hinf_synthesis
@@ -1242,12 +1236,12 @@ The RMS values of the super sensor noise for the presented three synthesis are l
   plot(t, v, 'k--', 'DisplayName', '$x$');
   hold off;
   xlabel('Time [s]'); ylabel('Velocity [m/s]');
-  legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 3);
+  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_hinf.pdf', 'width', 'half', 'height', 'normal');
+  exportFig('figs/super_sensor_time_domain_h2_hinf.pdf', 'width', 'wide', 'height', 'normal');
 #+end_src
 
 #+name: fig:super_sensor_time_domain_h2_hinf
@@ -1300,7 +1294,7 @@ The uncertainty on the super sensor's dynamics is shown in Figure [[fig:super_se
   set(gca, 'XTickLabel',[]);
   ylabel('Magnitude');
   ylim([1e-2, 1e1]);
-  legend('location', 'southeast', 'FontSize', 8);
+  legend('location', 'southeast');
   hold off;
 
   % Phase
@@ -1322,7 +1316,7 @@ The uncertainty on the super sensor's dynamics is shown in Figure [[fig:super_se
 #+end_src
 
 #+begin_src matlab :tangle no :exports results :results file replace
-  exportFig('figs/super_sensor_dynamical_uncertainty_Htwo_Hinf.pdf', 'width', 'half', 'height', 'tall');
+  exportFig('figs/super_sensor_dynamical_uncertainty_Htwo_Hinf.pdf', 'width', 'full', 'height', 'full');
 #+end_src
 
 #+name: fig:super_sensor_dynamical_uncertainty_Htwo_Hinf

matlab/matlab/comp_filter_robustness.m

@@ -4,151 +4,226 @@ clear; close all; clc;
 %% Intialize Laplace variable
 s = zpk('s');
 
-addpath('src');
+% Dynamical uncertainty of the individual sensors
-load('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
+% 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}$
 
-% Weighting Function used to bound the super sensor uncertainty
+% We define the weights that are used to characterize the dynamic uncertainty of the sensors.
-% <<sec:weight_uncertainty>>
-
-% $W_u(s)$ is defined such that the super sensor phase uncertainty is less than 10 degrees below 100Hz eqref:eq:phase_uncertainy_bound_low_freq and is less than 180 degrees below 400Hz eqref:eq:phase_uncertainty_max.
-
-% \begin{align}
-%   \frac{1}{|W_u(j\omega)|} &< \sin\left(10 \frac{\pi}{180}\right), \quad \omega < 100\,\text{Hz} \label{eq:phase_uncertainy_bound_low_freq} \\
-%   \frac{1}{|W_u(j 2 \pi 400)|} &< 1 \label{eq:phase_uncertainty_max}
-% \end{align}
-
-% The uncertainty bounds of the two individual sensor as well as the wanted maximum uncertainty bounds of the super sensor are shown in Figure [[fig:weight_uncertainty_bounds_Wu]].
 
 
-Dphi = 10; % [deg]
+freqs = logspace(-1, 3, 1000);
 
-Wu = createWeight('n', 2, 'w0', 2*pi*4e2, 'G0', 1/sin(Dphi*pi/180), 'G1', 1/4, 'Gc', 1);
+omegac = 100*2*pi; G0 = 0.1; Ginf = 10;
+w1 = (Ginf*s/omegac + G0)/(s/omegac + 1);
 
-% Wu is saved for further use
+omegac = 0.2*2*pi; G0 = 5; Ginf = 0.1;
-save('./mat/Wu.mat', 'Wu');
+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);
 
-Dphi_Wu = 180/pi*asin(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))));
+
-Dphi_Wu(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))) > 1) = 360;
+
+% From the weights, we define the uncertain transfer functions of the sensors. Some of the uncertain dynamics of both sensors are shown on Fig. [[fig:uncertainty_dynamics_sensors]] with the upper and lower bounds on the magnitude and on the phase.
+
+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;
 
 figure;
 % Magnitude
 ax1 = subplot(2,1,1);
 hold on;
-plotMagUncertainty(W1, freqs, 'color_i', 1, 'DisplayName', '$1 + W_1 \Delta_1$');
+set(gca,'ColorOrderIndex',1);
-plotMagUncertainty(W2, freqs, 'color_i', 2, 'DisplayName', '$1 + W_2 \Delta_2$');
+plot(freqs, 1 + abs(squeeze(freqresp(w1, freqs, 'Hz'))), '--');
-plot(freqs, 1 + abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'k--', ...
+set(gca,'ColorOrderIndex',1);
-     'DisplayName', '$1 + W_u^{-1} \Delta$')
+plot(freqs, max(1 - abs(squeeze(freqresp(w1, freqs, 'Hz'))), 0), '--');
-plot(freqs, 1 - abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'k--', ...
+set(gca,'ColorOrderIndex',2);
-     'HandleVisibility', 'off')
+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-2, 1e1]);
+ylim([1e-1, 10]);
-legend('location', 'southeast', 'FontSize', 8);
 hold off;
 
 % Phase
 ax2 = subplot(2,1,2);
 hold on;
-plotPhaseUncertainty(W1, freqs, 'color_i', 1);
+set(gca,'ColorOrderIndex',1);
-plotPhaseUncertainty(W2, freqs, 'color_i', 2);
+plot(freqs,  Dphi1, '--');
-plot(freqs,  Dphi_Wu, 'k--');
+set(gca,'ColorOrderIndex',1);
-plot(freqs, -Dphi_Wu, 'k--');
+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');
+
+% 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 Fig. [[fig:magnitude_wphi]] and the corresponding maximum allowed phase uncertainty of the super sensor dynamics of shown in Fig. [[fig:maximum_wanted_phase_uncertainty]].
+
+
+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;
+
+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');
+
+
+
+% #+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]]
+
+
+% 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;
+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);
+
+
+
+% #+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 Fig. [[fig:upper_bounds_comp_filter_max_phase_uncertainty]].
+
+
+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');
 
 % $\mathcal{H}_\infty$ Synthesis
-% <<sec:Hinfinity_synthesis>>
+% The $\mathcal{H}_\infty$ synthesis architecture used for the complementary filters is shown in Fig. [[fig:h_infinity_robust_fusion]].
-
-% The generalized plant $P_{\mathcal{H}_\infty}$ used for the $\mathcal{H}_\infty$ Synthesis of the complementary filters is shown in Figure [[fig:h_infinity_robust_fusion]] and is described by Equation eqref:eq:Hinf_generalized_plant.
 
 % #+name: fig:h_infinity_robust_fusion
 % #+caption: Architecture used for $\mathcal{H}_\infty$ synthesis of complementary filters
 % [[file:figs-tikz/h_infinity_robust_fusion.png]]
 
-% \begin{equation} \label{eq:Hinf_generalized_plant}
-% \begin{pmatrix}
-%   z_1 \\ z_2 \\ v
-% \end{pmatrix} = \underbrace{\begin{bmatrix}
-%   W_u W_1 & -W_u W_1 \\
-%   0       &  W_u W_2 \\
-%   1       &  0
-% \end{bmatrix}}_{P_{\mathcal{H}_\infty}} \begin{pmatrix}
-%   w \\ u
-% \end{pmatrix}
-% \end{equation}
-
 % The generalized plant is defined below.
 
-P = [Wu*W1 -Wu*W1;
+P = [W1 -W1;
-     0      Wu*W2;
+     0   W2;
-     1      0];
+     1   0];
 
 
 
-% And the $\mathcal{H}_\infty$ synthesis is performed using the =hinfsyn= command.
+% And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command.
 
-H2 = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'DISPLAY', 'on');
+[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
 
 
 
 % #+RESULTS:
 % #+begin_example
-%   Test bounds:  0.7071 <=  gamma  <=  1.291
+% [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
 
-%     gamma        X>=0        Y>=0       rho(XY)<1    p/f
+% Test bounds:      0.0447 <  gamma  <=      1.3318
-%   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
+%   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
 
-% The $\mathcal{H}_\infty$ is successful as the $\mathcal{H}_\infty$ norm of the "closed loop" transfer function from $(w)$ to $(z_1,\ z_2)$ is less than one.
+% And $H_1(s)$ is defined as the complementary of $H_2(s)$.
-
-% $H_1(s)$ is then defined as the complementary of $H_2(s)$.
 
 H1 = 1 - H2;
 
-% Complementary filters are saved for further analysis
-save('./mat/Hinf_filters.mat', 'H2', 'H1');
 
 
-
+% The obtained complementary filters are shown in Fig. [[fig:comp_filter_hinf_uncertainty]].
-% The obtained complementary filters as well as the wanted upper bounds are shown in Figure [[fig:hinf_comp_filters]].
-
 
 figure;
 
 ax1 = subplot(2,1,1);
 hold on;
-plot(freqs, 1./abs(squeeze(freqresp(Wu*W1, freqs, 'Hz'))), '--', 'DisplayName', '$1/|W_uW_1|$');
-plot(freqs, 1./abs(squeeze(freqresp(Wu*W2, freqs, 'Hz'))), '--', 'DisplayName', '$1/|W_uW_2|$');
 set(gca,'ColorOrderIndex',1)
-plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$|H_1|$');
+plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$W_1$');
-plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'DisplayName', '$|H_2|$');
+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', 'FontSize', 8, 'NumColumns', 2);
+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]');
@@ -157,87 +232,212 @@ yticks([-360:90:360]);
 
 linkaxes([ax1,ax2],'x');
 xlim([freqs(1), freqs(end)]);
+xticks([0.1, 1, 10, 100, 1000]);
 
 % Super sensor uncertainty
-% The super sensor dynamical uncertainty is displayed in Figure [[fig:super_sensor_dynamical_uncertainty_Hinf]].
+% We can now compute the uncertainty of the super sensor. The result is shown in Fig. [[fig:super_sensor_uncertainty_bode_plot]].
-% It is confirmed that the super sensor dynamical uncertainty is less than the maximum allowed uncertainty defined by the norm of $W_u(s)$.
-
-% The $\mathcal{H}_\infty$ synthesis thus allows to design filters such that the super sensor has specified bounded uncertainty.
 
 
-Dphi_Wu = 180/pi*asin(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))));
+Gss = G1*H1 + G2*H2;
-Dphi_Wu(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))) > 1) = 360;
 
-Dphi_ss = 180/pi*asin(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))));
+Gsss = usample(Gss, 20);
-Dphi_ss(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))) > 1) = 360;
+
+% 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;
 
 figure;
 % Magnitude
 ax1 = subplot(2,1,1);
 hold on;
-plotMagUncertainty(W1, freqs, 'color_i', 1, 'DisplayName', '$1 + W_1 \Delta_1$');
+set(gca,'ColorOrderIndex',1);
-plotMagUncertainty(W2, freqs, 'color_i', 2, 'DisplayName', '$1 + W_2 \Delta_2$');
+plot(freqs, 1 + abs(squeeze(freqresp(w1, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S1');
-plot(freqs, 1 + abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))+abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 'k-', ...
+set(gca,'ColorOrderIndex',1);
-     'DisplayName', '$1 + W_1 \Delta_1 + W_2 \Delta_2$')
+plot(freqs, max(1 - abs(squeeze(freqresp(w1, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
-plot(freqs, max(1 - abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))-abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 0.001), 'k-', ...
+set(gca,'ColorOrderIndex',2);
-     'HandleVisibility', 'off');
+plot(freqs, 1 + abs(squeeze(freqresp(w2, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S2');
-plot(freqs, 1 + abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'k--', ...
+set(gca,'ColorOrderIndex',2);
-     'DisplayName', '$1 + W_u^{-1}\Delta$')
+plot(freqs, max(1 - abs(squeeze(freqresp(w2, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
-plot(freqs, 1 - abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'k--', ...
+plot(freqs, 1 + abs(squeeze(freqresp(w1*H1+w2*H2, freqs, 'Hz'))), 'k--', 'DisplayName', 'Bounds - SS');
-     'HandleVisibility', 'off')
+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([1e-2, 1e1]);
+ylim([5e-2, 10]);
-legend('location', 'southeast', 'FontSize', 8);
 hold off;
 
 % Phase
 ax2 = subplot(2,1,2);
 hold on;
-plotPhaseUncertainty(W1, freqs, 'color_i', 1);
+set(gca,'ColorOrderIndex',1);
-plotPhaseUncertainty(W2, freqs, 'color_i', 2);
+plot(freqs,  Dphi1, '--');
-plot(freqs,  Dphi_ss, 'k-');
+set(gca,'ColorOrderIndex',1);
-plot(freqs, -Dphi_ss, 'k-');
+plot(freqs, -Dphi1, '--');
-plot(freqs,  Dphi_Wu, 'k--');
+set(gca,'ColorOrderIndex',2);
-plot(freqs, -Dphi_Wu, 'k--');
+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');
-xlim([freqs(1), freqs(end)]);
 
 % Super sensor noise
 % We now compute the obtain Power Spectral Density of the super sensor's noise.
-% The Amplitude Spectral Densities are shown in Figure [[fig:psd_sensors_hinf_synthesis]].
+% The noise characteristics of both individual sensor are defined below.
 
 
-PSD_S2   = abs(squeeze(freqresp(N2,    freqs, 'Hz'))).^2;
+omegac = 100*2*pi; G0 = 1e-5; Ginf = 1e-4;
-PSD_S1   = abs(squeeze(freqresp(N1,    freqs, 'Hz'))).^2;
+N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/100);
-PSD_Hinf = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2 + ...
+
-           abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
+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;
 
 
 
-% The obtained RMS of the super sensor noise in the $\mathcal{H}_2$ and $\mathcal{H}_\infty$ case are shown in Table [[tab:rms_noise_comp_H2_Hinf]].
+% The PSD of both sensor and of the super sensor is shown in Fig. [[fig:psd_sensors_hinf_synthesis]].
-% As expected, the super sensor obtained from the $\mathcal{H}_\infty$ synthesis is much noisier than the super sensor obtained from the $\mathcal{H}_2$ synthesis.
+% The CPS of both sensor and of the super sensor is shown in Fig. [[fig:cps_sensors_hinf_synthesis]].
 
 
-H2_filters = load('./mat/H2_filters.mat', 'H2', 'H1');
+PSD_S1 = abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2;
-
+PSD_S2 = abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2;
-PSD_H2   = abs(squeeze(freqresp(N1*H2_filters.H1, freqs, 'Hz'))).^2 + ...
+PSD_H2 = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
-           abs(squeeze(freqresp(N2*H2_filters.H2, freqs, 'Hz'))).^2;
 
 figure;
 hold on;
-plot(freqs, sqrt(PSD_S1),   '-',   'DisplayName', '$\phi_{n_1}$');
+plot(freqs, PSD_S1, '-',  'DisplayName', '$\Phi_{\hat{x}_1}$');
-plot(freqs, sqrt(PSD_S2),   '-',   'DisplayName', '$\phi_{n_2}$');
+plot(freqs, PSD_S2, '-',  'DisplayName', '$\Phi_{\hat{x}_2}$');
-plot(freqs, sqrt(PSD_H2),   'k-',  'DisplayName', '$\phi_{n_{\mathcal{H}_2}}$');
+plot(freqs, PSD_H2, 'k-', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_2}}$');
-plot(freqs, sqrt(PSD_Hinf), 'k--', 'DisplayName', '$\phi_{n_{\mathcal{H}_\infty}}$');
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('ASD $\left[ \frac{m/s}{\sqrt{Hz}} \right]$');
+xlabel('Frequency [Hz]'); ylabel('Power Spectral Density');
 hold off;
 xlim([freqs(1), freqs(end)]);
-legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
+legend('location', 'northeast');
+
+
+
+% #+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]]
+
+
+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);
+
+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');
+
+% 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*}
+
+
+G1 = 1;
+G2 = 0.6;
+
+
+
+% 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.
+
+
+freqs = logspace(-1, 1, 1000);
+
+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));
+
+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)]);
+
+
+
+% #+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.
+
+
+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)]);

matlab/matlab/comp_filters_analytical.m

@@ -23,9 +23,6 @@ Hl1 = 1/((s/w0)+1);
 
 freqs = logspace(-2, 2, 1000);
 
-freqs
-
-
 figure;
 % Magnitude
 ax1 = subplot(2,1,1);

matlab/matlab/mixed_synthesis_sensor_fusion.m

@@ -4,65 +4,217 @@ clear; close all; clc;
 %% Intialize Laplace variable
 s = zpk('s');
 
-load('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
+freqs = logspace(-1, 3, 1000);
-load('./mat/Wu.mat', 'Wu');
 
-% Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis
+% Noise characteristics and Uncertainty of the individual sensors
-% <<sec:H2_Hinf_synthesis>>
+% 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.
 
-% The synthesis architecture that is used here is shown in Figure [[fig:mixed_h2_hinf_synthesis]].
+omegac = 100*2*pi; G0 = 0.1; Ginf = 10;
+w1 = (Ginf*s/omegac + G0)/(s/omegac + 1);
 
-% The filter $H_2(s)$ is synthesized such that it:
+omegac = 0.2*2*pi; G0 = 5; Ginf = 0.1;
-% - keeps the $\mathcal{H}_\infty$ norm of the transfer function from $w$ to $z_{\mathcal{H}_\infty}$ bellow some specified value
+w2 = (Ginf*s/omegac + G0)/(s/omegac + 1);
-% - minimizes the $\mathcal{H}_2$ norm of the transfer function from $w$ to $z_{\mathcal{H}_2}$
+omegac = 5000*2*pi; G0 = 1; Ginf = 50;
+w2 = w2*(Ginf*s/omegac + G0)/(s/omegac + 1);
+
+
+
+% 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.
+
+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;
+
+
+
+% Both dynamical uncertainty and noise characteristics of the individual sensors are shown in Fig. [[fig:mixed_synthesis_noise_uncertainty_sensors]].
+
+
+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)]);
+
+% 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 Fig. [[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.
+
+
+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;
+
+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', '$|w_u(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)]);
+
+
+
+% #+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 Fig. [[fig:mixed_syn_objective_hinf]].
+
+
+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');
+
+% Mixed Synthesis Architecture
+% The synthesis architecture that is used here is shown in Fig. [[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
+% #+caption: Mixed H2/H-Infinity Synthesis
 % [[file:figs-tikz/mixed_h2_hinf_synthesis.png]]
 
-% Let's see that
+% Here, we define $P$ such that:
-% with $H_1(s)= 1 - H_2(s)$
+% \begin{align*}
-% \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_\infty}{w} \right\|_\infty &= \left\| \begin{matrix}H_1(s) W_1(s) W_u(s)\\ H_2(s) W_2(s) W_u(s)\end{matrix} \right\|_\infty \\
+%   \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
-%   \left\| \frac{z_2}{w} \right\|_2           &= \left\| \begin{matrix}H_1(s) N_1(s)       \\ H_2(s) N_2(s)\end{matrix} \right\|_2 = \sigma_n
+% \end{align*}
-% \end{align}
 
-% The generalized plant $P_{\mathcal{H}_2/\mathcal{H}_\infty}$ is defined below
+% 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$
 
-W1u = ss(W2*Wu); W2u = ss(W1*Wu); % Weight on the uncertainty
+% The synthesis objective is to have:
-W1n = ss(N2); W2n = ss(N1); % Weight on the noise
+% \[ \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.
 
-P = [Wu*W1 -Wu*W1;
+% We define the generalized plant that will be used for the mixed synthesis.
-     0      Wu*W2;
-     N1    -N1;
-     0      N2;
-     1      0];
 
+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];
 
-% And the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis is performed.
+% Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis
+% The mixed $\mathcal{H}_2$/$\mathcal{H}_\infty$ synthesis is performed below.
 
-[H2, ~] = h2hinfsyn(ss(P), 1, 1, 2, [0, 1], 'HINFMAX', 1, 'H2MAX', Inf, 'DKMAX', 100, 'TOL', 1e-3, 'DISPLAY', 'on');
+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;
 
-% The obtained filters are saved for further analysis
-save('./mat/H2_Hinf_filters.mat', 'H2', 'H1');
 
 
-
+% The obtained complementary filters are shown in Fig. [[fig:comp_filters_mixed_synthesis]].
-% The obtained complementary filters are shown in Figure [[fig:htwo_hinf_comp_filters]].
 
 
 figure;
 
 ax1 = subplot(2,1,1);
 hold on;
-plot(freqs, 1./abs(squeeze(freqresp(Wu*W1, freqs, 'Hz'))), '--', 'DisplayName', '$1/|W_uW_1|$');
+set(gca,'ColorOrderIndex',1)
-plot(freqs, 1./abs(squeeze(freqresp(Wu*W2, freqs, 'Hz'))), '--', 'DisplayName', '$1/|W_uW_2|$');
+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;
@@ -70,11 +222,13 @@ set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
 ylabel('Magnitude');
 set(gca, 'XTickLabel',[]);
 ylim([1e-3, 2]);
-legend('location', 'southeast', 'FontSize', 8);
+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]');
@@ -83,120 +237,115 @@ yticks([-360:90:360]);
 
 linkaxes([ax1,ax2],'x');
 xlim([freqs(1), freqs(end)]);
+xticks([0.1, 1, 10, 100, 1000]);
 
 % Obtained Super Sensor's noise
-% The Amplitude Spectral Density of the super sensor's noise is shown in Figure [[fig:psd_sensors_htwo_hinf_synthesis]].
+% The PSD and CPS of the super sensor's noise are shown in Fig. [[fig:psd_super_sensor_mixed_syn]] and Fig. [[fig:cps_super_sensor_mixed_syn]] respectively.
-
-% A time domain simulation is shown in Figure [[fig:super_sensor_time_domain_h2_hinf]].
-
-% The RMS values of the super sensor noise for the presented three synthesis are listed in Table [[tab:rms_noise_comp]].
 
 
-PSD_S2     = abs(squeeze(freqresp(N2,    freqs, 'Hz'))).^2;
+PSD_S1 = abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2;
-PSD_S1     = abs(squeeze(freqresp(N1,    freqs, 'Hz'))).^2;
+PSD_S2 = abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2;
-PSD_H2Hinf = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2 + ...
+PSD_H2 = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
-             abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
-
-H2_filters = load('./mat/H2_filters.mat', 'H2', 'H1');
-
-PSD_H2   = abs(squeeze(freqresp(N1*H2_filters.H1, freqs, 'Hz'))).^2 + ...
-           abs(squeeze(freqresp(N2*H2_filters.H2, freqs, 'Hz'))).^2;
-
-Hinf_filters = load('./mat/Hinf_filters.mat', 'H2', 'H1');
-
-PSD_Hinf   = abs(squeeze(freqresp(N1*Hinf_filters.H1, freqs, 'Hz'))).^2 + ...
-             abs(squeeze(freqresp(N2*Hinf_filters.H2, freqs, 'Hz'))).^2;
 
 figure;
 hold on;
-plot(freqs, sqrt(PSD_S1),     '-',   'DisplayName', '$\Phi_{n_1}$');
+plot(freqs, PSD_S1, '-',  'DisplayName', '$\Phi_{\hat{x}_1}$');
-plot(freqs, sqrt(PSD_S2),     '-',   'DisplayName', '$\Phi_{n_2}$');
+plot(freqs, PSD_S2, '-',  'DisplayName', '$\Phi_{\hat{x}_2}$');
-plot(freqs, sqrt(PSD_H2),     'k-',  'DisplayName', '$\Phi_{n_{\mathcal{H}_2}}$');
+plot(freqs, PSD_H2, 'k-', 'DisplayName', '$\Phi_{\hat{x}}$');
-plot(freqs, sqrt(PSD_Hinf),   'k--', 'DisplayName', '$\Phi_{n_{\mathcal{H}_\infty}}$');
-plot(freqs, sqrt(PSD_H2Hinf), 'k-.', 'DisplayName', '$\Phi_{n_{\mathcal{H}_2/\mathcal{H}_\infty}}$');
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('ASD $\left[ \frac{m/s}{\sqrt{Hz}} \right]$');
+xlabel('Frequency [Hz]'); ylabel('Power Spectral Density');
 hold off;
 xlim([freqs(1), freqs(end)]);
-legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 3);
+legend('location', 'northeast');
 
 
 
-% #+name: fig:psd_sensors_htwo_hinf_synthesis
+% #+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
+% #+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]])
-% #+RESULTS:
+% [[file:figs/psd_super_sensor_mixed_syn.png]]
-% [[file:figs/psd_sensors_htwo_hinf_synthesis.png]]
 
 
-Fs = 1e4;  % Sampling Frequency [Hz]
-Ts = 1/Fs; % Sampling Time [s]
 
-t = 0:Ts:2; % Time Vector [s]
+CPS_S1 = 1/pi*cumtrapz(2*pi*freqs, PSD_S1);
-
+CPS_S2 = 1/pi*cumtrapz(2*pi*freqs, PSD_S2);
-v = 0.1*sin((10*t).*t)'; % Velocity measured [m/s]
+CPS_H2 = 1/pi*cumtrapz(2*pi*freqs, PSD_H2);
-
-% 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);
 
 figure;
 hold on;
-set(gca,'ColorOrderIndex',2)
+plot(freqs, CPS_S1, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_1} = %.1e$', sqrt(CPS_S1(end))));
-plot(t, v + n2, 'DisplayName', '$\hat{x}_2$');
+plot(freqs, CPS_S2, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_2} = %.1e$', sqrt(CPS_S2(end))));
-set(gca,'ColorOrderIndex',1)
+plot(freqs, CPS_H2, 'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_2}} = %.1e$', sqrt(CPS_H2(end))));
-plot(t, v + n1, 'DisplayName', '$\hat{x}_1$');
+set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
-set(gca,'ColorOrderIndex',3)
+xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum');
-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]');
+xlim([2e-1, freqs(end)]);
-legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 3);
+ylim([1e-10 1e-5]);
-ylim([-0.3, 0.3]);
+legend('location', 'southeast');
 
 % Obtained Super Sensor's Uncertainty
-% The uncertainty on the super sensor's dynamics is shown in Figure [[fig:super_sensor_dynamical_uncertainty_Htwo_Hinf]].
+% The uncertainty on the super sensor's dynamics is shown in Fig. [[]].
 
 
-Dphi_Wu = 180/pi*asin(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))));
+G1 = 1 + w1*ultidyn('Delta',[1 1]);
-Dphi_Wu(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))) > 1) = 360;
+G2 = 1 + w2*ultidyn('Delta',[1 1]);
 
-Dphi_ss = 180/pi*asin(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))));
+Gss = G1*H1 + G2*H2;
-Dphi_ss(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))) > 1) = 360;
+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;
 
 figure;
 % Magnitude
 ax1 = subplot(2,1,1);
 hold on;
-plotMagUncertainty(W1, freqs, 'color_i', 1, 'DisplayName', '$1 + W_1 \Delta_1$');
+set(gca,'ColorOrderIndex',1);
-plotMagUncertainty(W2, freqs, 'color_i', 2, 'DisplayName', '$1 + W_2 \Delta_2$');
+plot(freqs, 1 + abs(squeeze(freqresp(w1, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S1');
-plot(freqs, 1 + abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))+abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 'k-', ...
+set(gca,'ColorOrderIndex',1);
-     'DisplayName', '$1 + W_1 \Delta_1 + W_2 \Delta_2$')
+plot(freqs, max(1 - abs(squeeze(freqresp(w1, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
-plot(freqs, max(1 - abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))-abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 0.001), 'k-', ...
+set(gca,'ColorOrderIndex',2);
-     'HandleVisibility', 'off');
+plot(freqs, 1 + abs(squeeze(freqresp(w2, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S2');
-plot(freqs, 1 + abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'k--', ...
+set(gca,'ColorOrderIndex',2);
-     'DisplayName', '$1 + W_u^{-1}\Delta$')
+plot(freqs, max(1 - abs(squeeze(freqresp(w2, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
-plot(freqs, 1 - abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'k--', ...
+plot(freqs, 1 + abs(squeeze(freqresp(w1*H1+w2*H2, freqs, 'Hz'))), 'k--', 'DisplayName', 'Bounds - SS');
-     'HandleVisibility', 'off')
+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([1e-2, 1e1]);
+ylim([5e-2, 10]);
-legend('location', 'southeast', 'FontSize', 8);
 hold off;
 
 % Phase
 ax2 = subplot(2,1,2);
 hold on;
-plotPhaseUncertainty(W1, freqs, 'color_i', 1);
+set(gca,'ColorOrderIndex',1);
-plotPhaseUncertainty(W2, freqs, 'color_i', 2);
+plot(freqs,  Dphi1, '--');
-plot(freqs,  Dphi_ss, 'k-');
+set(gca,'ColorOrderIndex',1);
-plot(freqs, -Dphi_ss, 'k-');
+plot(freqs, -Dphi1, '--');
-plot(freqs,  Dphi_Wu, 'k--');
+set(gca,'ColorOrderIndex',2);
-plot(freqs, -Dphi_Wu, 'k--');
+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');
-xlim([freqs(1), freqs(end)]);

matlab/matlab/optimal_comp_filters.m

@@ -4,221 +4,629 @@ clear; close all; clc;
 %% Intialize Laplace variable
 s = zpk('s');
 
-addpath('src');
+freqs = logspace(-1, 3, 1000);
-load('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
 
-% $\mathcal{H}_2$ Synthesis
+% Noise of the sensors
-% <<sec:H2_synthesis>>
+% 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)
 
-% Consider the generalized plant $P_{\mathcal{H}_2}$ shown in Figure [[fig:h_two_optimal_fusion]] and described by Equation eqref:eq:H2_generalized_plant.
 
-% #+name: fig:h_two_optimal_fusion
+omegac = 100*2*pi; G0 = 1e-5; Ginf = 1e-4;
-% #+caption: Architecture used for $\mathcal{H}_\infty$ synthesis of complementary filters
+N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/100);
-% [[file:figs-tikz/h_two_optimal_fusion.png]]
 
-% \begin{equation} \label{eq:H2_generalized_plant}
+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;
+
+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');
+
+% 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_1 \\ z_2 \\ v
+%   z \\ v
-% \end{pmatrix} = \underbrace{\begin{bmatrix}
+% \end{pmatrix} = \begin{pmatrix}
-%   N_1 & -N_1 \\
+%   0   &  N_2 & 1 \\
-%   0   &  N_2 \\
+%   N_1 & -N_2 & 0
-%   1   &  0
+% \end{pmatrix} \begin{pmatrix}
-% \end{bmatrix}}_{P_{\mathcal{H}_2}} \begin{pmatrix}
+%   w_1 \\ w_2 \\ u
-%   w \\ u
 % \end{pmatrix}
-% \end{equation}
+% \end{equation*}
 
-% Applying the $\mathcal{H}_2$ synthesis on $P_{\mathcal{H}_2}$ will generate a filter $H_2(s)$ such that the $\mathcal{H}_2$ norm from $w$ to $(z_1,z_2)$ which is actually equals to $\sigma_n$ by defining $H_1(s) = 1 - H_2(s)$:
+% The transfer function from $[n_1, n_2]$ to $\hat{x}$ is:
-% \begin{equation}
+% \[ \begin{bmatrix} N_1 H_1 \\ N_2 (1 - H_1) \end{bmatrix} \]
-%    \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)
+% If we define $H_2 = 1 - H_1$, we obtain:
-% \end{equation}
+% \[ \begin{bmatrix} N_1 H_1 \\ N_2 H_2 \end{bmatrix} \]
 
-% 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.
+% Thus, if we minimize the $\mathcal{H}_2$ norm of this transfer function, we minimize the RMS value of $\hat{x}$.
 
-% The generalized plant $P_{\mathcal{H}_2}$ is defined below
+% We define the generalized plant $P$ on matlab as shown on figure [[fig:h_infinity_optimal_comp_filters]].
 
-PH2 = [N1 -N1;
+P = [0   N2  1;
-       0   N2;
+     N1 -N2  0];
-       1   0];
 
 
 
-% The $\mathcal{H}_2$ synthesis using the =h2syn= command
+% And we do the $\mathcal{H}_2$ synthesis using the =h2syn= command.
 
-[H2, ~, gamma] = h2syn(PH2, 1, 1);
+[H1, ~, gamma] = h2syn(P, 1, 1);
 
 
 
-% Finally, $H_1(s)$ is defined as follows
+% Finally, we define $H_2(s) = 1 - H_1(s)$.
 
-H1 = 1 - H2;
+H2 = 1 - H1;
-
-% Filters are saved for further use
-save('./mat/H2_filters.mat', 'H2', 'H1');
 
 
 
-% The obtained complementary filters are shown in Figure [[fig:htwo_comp_filters]].
+% The complementary filters obtained are shown on figure [[fig:htwo_comp_filters]].
 
-figure;
+% The PSD of the noise of the individual sensor and of the super sensor are shown in Fig. [[fig:psd_sensors_htwo_synthesis]].
 
-% Magnitude
+% The Cumulative Power Spectrum (CPS) is shown on Fig. [[fig:cps_h2_synthesis]].
-ax1 = subplot(2,1,1);
-hold on;
-plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), 'DisplayName', '$H_1$');
-plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), 'DisplayName', '$H_2$');
-set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-set(gca, 'XTickLabel',[]);
-ylabel('Magnitude');
-hold off;
-legend('location', 'northeast', 'FontSize', 8);
 
-% Phase
+% The obtained RMS value of the super sensor is lower than the RMS value of the individual sensors.
-ax2 = subplot(2,1,2);
-hold on;
-plot(freqs, 180/pi*angle(squeeze(freqresp(H1, freqs, 'Hz'))));
-plot(freqs, 180/pi*angle(squeeze(freqresp(H2, freqs, 'Hz'))));
-set(gca,'xscale','log');
-yticks(-180:90:180);
-ylim([-180 180]);
-xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-hold off;
-linkaxes([ax1,ax2],'x');
-xlim([freqs(1), freqs(end)]);
-
-% 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.
-
-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;
-
-
-
-% #+name: tab:rms_noise_H2
-% #+caption: RMS value of the individual sensor noise and of the super sensor using the $\mathcal{H}_2$ Synthesis
-% #+attr_latex: :environment tabular :align cc
-% #+attr_latex: :center t :booktabs t :float t
-% #+RESULTS:
-% |                              | RMS value $[m/s]$ |
-% |------------------------------+-------------------|
-% | $\sigma_{n_1}$               |             0.015 |
-% | $\sigma_{n_2}$               |             0.080 |
-% | $\sigma_{n_{\mathcal{H}_2}}$ |             0.003 |
 
 
 figure;
 hold on;
-plot(freqs, sqrt(PSD_S1), '-',  'DisplayName', '$\phi_{n_1}$');
+plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
-plot(freqs, sqrt(PSD_S2), '-',  'DisplayName', '$\phi_{n_2}$');
+plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$');
-plot(freqs, sqrt(PSD_H2), 'k-', 'DisplayName', '$\phi_{n_{\mathcal{H}_2}}$');
 set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-xlabel('Frequency [Hz]'); ylabel('ASD $\left[ \frac{m/s}{\sqrt{Hz}} \right]$');
+xlabel('Frequency [Hz]'); ylabel('Magnitude');
 hold off;
 xlim([freqs(1), freqs(end)]);
-legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
+legend('location', 'northeast');
 
 
 
-% #+name: fig:psd_sensors_htwo_synthesis
+% #+NAME: fig:htwo_comp_filters
-% #+caption: Power Spectral Density of the estimated $\hat{x}$ using the two sensors alone and using the optimally fused signal
+% #+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]]
+
+
+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;
+
+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');
+
+
+
+% #+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]])
 % [[file:figs/psd_sensors_htwo_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]].
 
-
+CPS_S1 = 1/pi*cumtrapz(2*pi*freqs, PSD_S1);
-Fs = 1e4;  % Sampling Frequency [Hz]
+CPS_S2 = 1/pi*cumtrapz(2*pi*freqs, PSD_S2);
-Ts = 1/Fs; % Sampling Time [s]
+CPS_H2 = 1/pi*cumtrapz(2*pi*freqs, PSD_H2);
-
-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);
 
 figure;
 hold on;
-set(gca,'ColorOrderIndex',2)
+plot(freqs, CPS_S1, '-',  'DisplayName', sprintf('$\\sigma_{\\hat{x}_1} = %.1e$', sqrt(CPS_S1(end))));
-plot(t, v + n2, 'DisplayName', '$\hat{x}_2$');
+plot(freqs, CPS_S2, '-',  'DisplayName', sprintf('$\\sigma_{\\hat{x}_2} = %.1e$', sqrt(CPS_S2(end))));
-set(gca,'ColorOrderIndex',1)
+plot(freqs, CPS_H2, 'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_2}} = %.1e$', sqrt(CPS_H2(end))));
-plot(t, v + n1, 'DisplayName', '$\hat{x}_1$');
+set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
-set(gca,'ColorOrderIndex',3)
+xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum');
-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]');
+xlim([2e-1, freqs(end)]);
-legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
+ylim([1e-10 1e-5]);
-ylim([-0.3, 0.3]);
+legend('location', 'southeast');
+
+% H-Infinity Synthesis - method A
+% Another objective that we may have is that the noise of the super sensor $n_{SS}$ is following the minimum of the noise of the two sensors $n_1$ and $n_2$:
+% \[ \Gamma_{n_{ss}}(\omega) = \min(\Gamma_{n_1}(\omega),\ \Gamma_{n_2}(\omega)) \]
+
+% In order to obtain that ideal case, we need that the complementary filters be designed such that:
+% \begin{align*}
+%   & |H_1(j\omega)| = 1 \text{ and } |H_2(j\omega)| = 0 \text{ at frequencies where } \Gamma_{n_1}(\omega) < \Gamma_{n_2}(\omega) \\
+%   & |H_1(j\omega)| = 0 \text{ and } |H_2(j\omega)| = 1 \text{ at frequencies where } \Gamma_{n_1}(\omega) > \Gamma_{n_2}(\omega)
+% \end{align*}
+
+% Which is indeed impossible in practice.
+
+% We could try to approach that with the $\mathcal{H}_\infty$ synthesis by using high order filters.
+
+% As shown on Fig. [[fig:noise_characteristics_sensors]], the frequency where the two sensors have the same noise level is around 9Hz.
+% We will thus choose weighting functions such that the merging frequency is around 9Hz.
+
+% The weighting functions used as well as the obtained complementary filters are shown in Fig. [[fig:weights_comp_filters_Hinfa]].
+
+
+n = 5; w0 = 2*pi*10; G0 = 1/10; G1 = 10000; Gc = 1/2;
+W1a = (((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 = 5; w0 = 2*pi*8; G0 = 1000; G1 = 0.1; Gc = 1/2;
+W2a = (((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;
+
+P = [W1a -W1a;
+     0    W2a;
+     1    0];
+
+
+
+% And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command.
+
+[H2a, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
 
 
 
-% #+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_example
+% [H2a, ~, 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.1000 <  gamma  <=  10500.0000
+
+%   gamma    hamx_eig  xinf_eig  hamy_eig   yinf_eig   nrho_xy   p/f
+% 1.050e+04   2.1e+01 -3.0e-07   7.8e+00   -1.3e-15    0.0000    p
+% 5.250e+03   2.1e+01 -1.5e-08   7.8e+00   -5.8e-14    0.0000    p
+% 2.625e+03   2.1e+01   2.5e-10   7.8e+00   -3.7e-12    0.0000    p
+% 1.313e+03   2.1e+01 -3.2e-11   7.8e+00   -7.3e-14    0.0000    p
+%   656.344   2.1e+01 -2.2e-10   7.8e+00   -1.1e-15    0.0000    p
+%   328.222   2.1e+01 -1.1e-10   7.8e+00   -1.2e-15    0.0000    p
+%   164.161   2.1e+01 -2.4e-08   7.8e+00   -8.9e-16    0.0000    p
+%    82.130   2.1e+01   2.0e-10   7.8e+00   -9.1e-31    0.0000    p
+%    41.115   2.1e+01 -6.8e-09   7.8e+00   -4.1e-13    0.0000    p
+%    20.608   2.1e+01   3.3e-10   7.8e+00   -1.4e-12    0.0000    p
+%    10.354   2.1e+01 -9.8e-09   7.8e+00   -1.8e-15    0.0000    p
+%     5.227   2.1e+01 -4.1e-09   7.8e+00   -2.5e-12    0.0000    p
+%     2.663   2.1e+01   2.7e-10   7.8e+00   -4.0e-14    0.0000    p
+%     1.382   2.1e+01 -3.2e+05#  7.8e+00   -3.5e-14    0.0000    f
+%     2.023   2.1e+01 -5.0e-10   7.8e+00    0.0e+00    0.0000    p
+%     1.702   2.1e+01 -2.4e+07#  7.8e+00   -1.6e-13    0.0000    f
+%     1.862   2.1e+01 -6.0e+08#  7.8e+00   -1.0e-12    0.0000    f
+%     1.942   2.1e+01 -2.8e-09   7.8e+00   -8.1e-14    0.0000    p
+%     1.902   2.1e+01 -2.5e-09   7.8e+00   -1.1e-13    0.0000    p
+%     1.882   2.1e+01 -9.3e-09   7.8e+00   -2.0e-15    0.0001    p
+%     1.872   2.1e+01 -1.3e+09#  7.8e+00   -3.6e-22    0.0000    f
+%     1.877   2.1e+01 -2.6e+09#  7.8e+00   -1.2e-13    0.0000    f
+%     1.880   2.1e+01 -5.6e+09#  7.8e+00   -1.4e-13    0.0000    f
+%     1.881   2.1e+01 -1.2e+10#  7.8e+00   -3.3e-12    0.0000    f
+%     1.882   2.1e+01 -3.2e+10#  7.8e+00   -8.5e-14    0.0001    f
+
+%  Gamma value achieved:     1.8824
+% #+end_example
+
+
+H1a = 1 - H2a;
+
+figure;
+
+ax1 = subplot(2,1,1);
+hold on;
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 1./abs(squeeze(freqresp(W1a, freqs, 'Hz'))), '--', 'DisplayName', '$w_1$');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, 1./abs(squeeze(freqresp(W2a, freqs, 'Hz'))), '--', 'DisplayName', '$w_2$');
+
+set(gca,'ColorOrderIndex',1)
+plot(freqs, abs(squeeze(freqresp(H1a, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, abs(squeeze(freqresp(H2a, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$');
+
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Magnitude');
+set(gca, 'XTickLabel',[]);
+ylim([5e-4, 20]);
+legend('location', 'northeast');
+
+ax2 = subplot(2,1,2);
+hold on;
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 180/pi*phase(squeeze(freqresp(H1a, freqs, 'Hz'))), '-');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, 180/pi*phase(squeeze(freqresp(H2a, 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]);
+
+
+
+% #+NAME: fig:weights_comp_filters_Hinfa
+% #+CAPTION: Weights and Complementary Fitlers obtained ([[./figs/weights_comp_filters_Hinfa.png][png]], [[./figs/weights_comp_filters_Hinfa.pdf][pdf]])
+% [[file:figs/weights_comp_filters_Hinfa.png]]
+
+% We then compute the Power Spectral Density as well as the Cumulative Power Spectrum.
+
+
+PSD_Ha = abs(squeeze(freqresp(N1*H1a, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2a, freqs, 'Hz'))).^2;
+CPS_Ha = 1/pi*cumtrapz(2*pi*freqs, PSD_Ha);
+
+% H-Infinity Synthesis - method B
+% We have that:
+% \[ \Phi_{\hat{x}}(\omega) = \left|H_1(j\omega) N_1(j\omega)\right|^2 + \left|H_2(j\omega) N_2(j\omega)\right|^2 \]
+
+% Then, at frequencies where $|H_1(j\omega)| < |H_2(j\omega)|$ we would like that $|N_1(j\omega)| = 1$ and $|N_2(j\omega)| = 0$ as we discussed before.
+% Then $|H_1 N_1|^2 + |H_2 N_2|^2 = |N_1|^2$.
+
+% We know that this is impossible in practice. A more realistic choice is to design $H_2(s)$ such that when $|N_2(j\omega)| > |N_1(j\omega)|$, we have that:
+% \[ |H_2 N_2|^2 = \epsilon |H_1 N_1|^2 \]
+
+% Which is equivalent to have (by supposing $|H_1| \approx 1$):
+% \[ |H_2| = \sqrt{\epsilon} \frac{|N_1|}{|N_2|} \]
+
+% And we have:
+% \begin{align*}
+%   \Phi_{\hat{x}} &= \left|H_1 N_1\right|^2 + |H_2 N_2|^2 \\
+%                 &= (1 + \epsilon) \left| H_1 N_1 \right|^2 \\
+%                 &\approx \left|N_1\right|^2
+% \end{align*}
+
+% Similarly, we design $H_1(s)$ such that at frequencies where $|N_1| > |N_2|$:
+% \[ |H_1| = \sqrt{\epsilon} \frac{|N_2|}{|N_1|} \]
+
+% For instance, is we take $\epsilon = 1$, then the PSD of $\hat{x}$ is increased by just by a factor $\sqrt{2}$ over the all frequencies from the idea case.
+
+% We use this as the weighting functions for the $\mathcal{H}_\infty$ synthesis of the complementary filters.
+
+% The weighting function and the obtained complementary filters are shown in Fig. [[fig:weights_comp_filters_Hinfb]].
+
+
+epsilon = 2;
+
+W1b = 1/epsilon*N1/N2;
+W2b = 1/epsilon*N2/N1;
+
+W1b = W1b/(1 + s/2/pi/1000); % this is added so that it is proper
+
+P = [W1b -W1b;
+     0    W2b;
+     1    0];
+
+
+
+% And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command.
+
+[H2b, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
+
+
+
+% #+RESULTS:
+% #+begin_example
+% [H2b, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
+% Test bounds:      0.0000 <  gamma  <=     32.8125
+
+%   gamma    hamx_eig  xinf_eig  hamy_eig   yinf_eig   nrho_xy   p/f
+%    32.812   1.8e+01   3.4e-10   6.3e+00   -2.9e-13    0.0000    p
+%    16.406   1.8e+01   3.4e-10   6.3e+00   -1.2e-15    0.0000    p
+%     8.203   1.8e+01   3.3e-10   6.3e+00   -2.6e-13    0.0000    p
+%     4.102   1.8e+01   3.3e-10   6.3e+00   -2.1e-13    0.0000    p
+%     2.051   1.7e+01   3.4e-10   6.3e+00   -7.2e-16    0.0000    p
+%     1.025   1.6e+01 -1.3e+06#  6.3e+00   -8.3e-14    0.0000    f
+%     1.538   1.7e+01   3.4e-10   6.3e+00   -2.0e-13    0.0000    p
+%     1.282   1.7e+01   3.4e-10   6.3e+00   -7.9e-17    0.0000    p
+%     1.154   1.7e+01   3.6e-10   6.3e+00   -1.8e-13    0.0000    p
+%     1.089   1.7e+01 -3.4e+06#  6.3e+00   -1.7e-13    0.0000    f
+%     1.122   1.7e+01 -1.0e+07#  6.3e+00   -3.2e-13    0.0000    f
+%     1.138   1.7e+01 -1.3e+08#  6.3e+00   -1.8e-13    0.0000    f
+%     1.146   1.7e+01   3.2e-10   6.3e+00   -3.0e-13    0.0000    p
+%     1.142   1.7e+01   5.5e-10   6.3e+00   -2.8e-13    0.0000    p
+%     1.140   1.7e+01 -1.5e-10   6.3e+00   -2.3e-13    0.0000    p
+%     1.139   1.7e+01 -4.8e+08#  6.3e+00   -6.2e-14    0.0000    f
+%     1.139   1.7e+01   1.3e-09   6.3e+00   -8.9e-17    0.0000    p
+
+%  Gamma value achieved:     1.1390
+% #+end_example
+
+
+H1b = 1 - H2b;
+
+figure;
+
+ax1 = subplot(2,1,1);
+hold on;
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 1./abs(squeeze(freqresp(W1b, freqs, 'Hz'))), '--', 'DisplayName', '$w_1$');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, 1./abs(squeeze(freqresp(W2b, freqs, 'Hz'))), '--', 'DisplayName', '$w_2$');
+
+set(gca,'ColorOrderIndex',1)
+plot(freqs, abs(squeeze(freqresp(H1b, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, abs(squeeze(freqresp(H2b, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$');
+
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Magnitude');
+set(gca, 'XTickLabel',[]);
+ylim([5e-4, 20]);
+legend('location', 'northeast');
+
+ax2 = subplot(2,1,2);
+hold on;
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 180/pi*phase(squeeze(freqresp(H1b, freqs, 'Hz'))), '-');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, 180/pi*phase(squeeze(freqresp(H2b, 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]);
+
+
+
+% #+NAME: fig:weights_comp_filters_Hinfb
+% #+CAPTION: Weights and Complementary Fitlers obtained ([[./figs/weights_comp_filters_Hinfb.png][png]], [[./figs/weights_comp_filters_Hinfb.pdf][pdf]])
+% [[file:figs/weights_comp_filters_Hinfb.png]]
+
+
+PSD_Hb = abs(squeeze(freqresp(N1*H1b, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2b, freqs, 'Hz'))).^2;
+CPS_Hb = 1/pi*cumtrapz(2*pi*freqs, PSD_Hb);
+
+% H-Infinity Synthesis - method C
+
+Wp = 0.56*(inv(N1)+inv(N2))/(1 + s/2/pi/1000);
+
+W1c = N1*Wp;
+W2c = N2*Wp;
+
+P = [W1c -W1c;
+     0    W2c;
+     1    0];
+
+
+
+% And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command.
+
+[H2c, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
+
+
+
+% #+RESULTS:
+% #+begin_example
+% [H2c, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
+% Test bounds:      0.0000 <  gamma  <=     36.7543
+
+%   gamma    hamx_eig  xinf_eig  hamy_eig   yinf_eig   nrho_xy   p/f
+%    36.754   5.7e+00 -1.0e-13   6.3e+00   -6.2e-25    0.0000    p
+%    18.377   5.7e+00 -1.4e-12   6.3e+00   -1.8e-13    0.0000    p
+%     9.189   5.7e+00 -4.3e-13   6.3e+00   -4.7e-15    0.0000    p
+%     4.594   5.7e+00 -9.4e-13   6.3e+00   -4.7e-15    0.0000    p
+%     2.297   5.7e+00 -1.3e-16   6.3e+00   -6.8e-14    0.0000    p
+%     1.149   5.7e+00 -1.6e-17   6.3e+00   -1.5e-15    0.0000    p
+%     0.574   5.7e+00 -5.2e+02#  6.3e+00   -5.9e-14    0.0000    f
+%     0.861   5.7e+00 -3.1e+04#  6.3e+00   -3.8e-14    0.0000    f
+%     1.005   5.7e+00 -1.6e-12   6.3e+00   -1.1e-14    0.0000    p
+%     0.933   5.7e+00 -1.1e+05#  6.3e+00   -7.2e-14    0.0000    f
+%     0.969   5.7e+00 -3.3e+05#  6.3e+00   -5.6e-14    0.0000    f
+%     0.987   5.7e+00 -1.2e+06#  6.3e+00   -4.5e-15    0.0000    f
+%     0.996   5.7e+00 -6.5e-16   6.3e+00   -1.7e-15    0.0000    p
+%     0.992   5.7e+00 -2.9e+06#  6.3e+00   -6.1e-14    0.0000    f
+%     0.994   5.7e+00 -9.7e+06#  6.3e+00   -3.0e-16    0.0000    f
+%     0.995   5.7e+00 -8.0e-10   6.3e+00   -1.9e-13    0.0000    p
+%     0.994   5.7e+00 -2.3e+07#  6.3e+00   -4.3e-14    0.0000    f
+
+%  Gamma value achieved:     0.9949
+% #+end_example
+
+
+H1c = 1 - H2c;
+
+figure;
+
+ax1 = subplot(2,1,1);
+hold on;
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 1./abs(squeeze(freqresp(W1c, freqs, 'Hz'))), '--', 'DisplayName', '$w_1$');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, 1./abs(squeeze(freqresp(W2c, freqs, 'Hz'))), '--', 'DisplayName', '$w_2$');
+
+set(gca,'ColorOrderIndex',1)
+plot(freqs, abs(squeeze(freqresp(H1c, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, abs(squeeze(freqresp(H2c, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$');
+
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Magnitude');
+set(gca, 'XTickLabel',[]);
+ylim([5e-4, 20]);
+legend('location', 'northeast');
+
+ax2 = subplot(2,1,2);
+hold on;
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 180/pi*phase(squeeze(freqresp(H1c, freqs, 'Hz'))), '-');
+set(gca,'ColorOrderIndex',2)
+plot(freqs, 180/pi*phase(squeeze(freqresp(H2c, 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]);
+
+
+
+% #+NAME: fig:weights_comp_filters_Hinfc
+% #+CAPTION: Weights and Complementary Fitlers obtained ([[./figs/weights_comp_filters_Hinfc.png][png]], [[./figs/weights_comp_filters_Hinfc.pdf][pdf]])
+% [[file:figs/weights_comp_filters_Hinfc.png]]
+
+
+PSD_Hc = abs(squeeze(freqresp(N1*H1c, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2c, freqs, 'Hz'))).^2;
+CPS_Hc = 1/pi*cumtrapz(2*pi*freqs, PSD_Hc);
+
+
+
+% #+name: tab:rms_results
+% #+caption: RMS value of the estimation error when using the sensor individually and when using the two sensor merged using the optimal complementary filters
+% #+RESULTS:
+% |              | rms value |
+% |--------------+-----------|
+% | Sensor 1     |   1.3e-03 |
+% | Sensor 2     |   1.3e-03 |
+% | H2 Fusion    |   1.2e-04 |
+% | H-Infinity a |   2.4e-04 |
+% | H-Infinity b |   1.4e-04 |
+% | H-Infinity c |   2.2e-04 |
+
 
 
 figure;
 hold on;
-set(gca,'ColorOrderIndex',2)
+plot(freqs, PSD_S1, '-',  'DisplayName', '$\Phi_{\hat{x}_1}$');
-plot(t, n2, 'DisplayName', '$n_2$');
+plot(freqs, PSD_S2, '-',  'DisplayName', '$\Phi_{\hat{x}_2}$');
-set(gca,'ColorOrderIndex',1)
+plot(freqs, PSD_H2, 'r-', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_2}}$');
-plot(t, n1, 'DisplayName', '$n_1$');
+plot(freqs, PSD_Ha, 'k-', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_\infty},a}$');
-set(gca,'ColorOrderIndex',3)
+plot(freqs, PSD_Hb, 'k--', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_\infty},b}$');
-plot(t, (lsim(H1, n1, t)+lsim(H2, n2, t)), '-', 'DisplayName', '$n$');
+plot(freqs, PSD_Hc, 'k-.', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_\infty},c}$');
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+xlabel('Frequency [Hz]'); ylabel('Power Spectral Density');
 hold off;
-xlabel('Time [s]'); ylabel('Sensor Noise [m/s]');
+xlim([freqs(1), freqs(end)]);
-legend('FontSize', 8);
+legend('location', 'northeast');
-ylim([-0.2, 0.2]);
 
-% 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.
+% #+NAME: fig:comparison_psd_noise
-% As a result the super sensor signal can not be used for feedback applications about 100Hz.
+% #+CAPTION: Comparison of the obtained Power Spectral Density using the three methods ([[./figs/comparison_psd_noise.png][png]], [[./figs/comparison_psd_noise.pdf][pdf]])
+% [[file:figs/comparison_psd_noise.png]]
 
-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;
+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, 'r-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_2}} = %.1e$', sqrt(CPS_H2(end))));
+plot(freqs, CPS_Ha, 'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_\\infty, a}} = %.1e$', sqrt(CPS_Ha(end))));
+plot(freqs, CPS_Hb, 'k--', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_\\infty, b}} = %.1e$', sqrt(CPS_Hb(end))));
+plot(freqs, CPS_Hc, 'k-.', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_\\infty, c}} = %.1e$', sqrt(CPS_Hc(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');
+
+% 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.
+
+
+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);
+
+
+
+% The sensor uncertain models are defined below.
+
+G1 = 1 + w1*ultidyn('Delta',[1 1]);
+G2 = 1 + w2*ultidyn('Delta',[1 1]);
+
+% 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;
+
+
+
+% 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 Fig. [[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.
+
+
+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;
 
 figure;
 % Magnitude
 ax1 = subplot(2,1,1);
 hold on;
-plotMagUncertainty(W1, freqs, 'color_i', 1, 'DisplayName', '$1 + W_1 \Delta_1$');
+set(gca,'ColorOrderIndex',1);
-plotMagUncertainty(W2, freqs, 'color_i', 2, 'DisplayName', '$1 + W_2 \Delta_2$');
+plot(freqs, 1 + abs(squeeze(freqresp(w1, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S1');
-plot(freqs, 1 + abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))+abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 'k-', ...
+set(gca,'ColorOrderIndex',1);
-     'DisplayName', '$1 + W_1 \Delta_1 + W_2 \Delta_2$')
+plot(freqs, max(1 - abs(squeeze(freqresp(w1, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
-plot(freqs, max(1 - abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))-abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 0.001), 'k-', ...
+set(gca,'ColorOrderIndex',2);
-     'HandleVisibility', 'off');
+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([1e-2, 1e1]);
+ylim([5e-2, 10]);
-legend('location', 'southeast', 'FontSize', 8);
 hold off;
 
 % Phase
 ax2 = subplot(2,1,2);
 hold on;
-plotPhaseUncertainty(W1, freqs, 'color_i', 1);
+set(gca,'ColorOrderIndex',1);
-plotPhaseUncertainty(W2, freqs, 'color_i', 2);
+plot(freqs,  Dphi1, '--');
-plot(freqs,  Dphi_ss, 'k-');
+set(gca,'ColorOrderIndex',1);
-plot(freqs, -Dphi_ss, 'k-');
+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');
-xlim([freqs(1), freqs(end)]);

matlab/matlab/sensor_description.m

@@ -1,174 +0,0 @@
-%% Clear Workspace and Close figures
-clear; close all; clc;
-
-%% Intialize Laplace variable
-s = zpk('s');
-
-addpath('src');
-freqs = logspace(0, 4, 1000);
-
-% 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.
-
-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)]
-
-
-
-% The second sensor is a displacement sensor, its nominal dynamics $\hat{G}_2(s)$ is defined below.
-
-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)]
-
-
-
-% 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]].
-
-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 $\left[\frac{V}{m/s}\right]$'); set(gca, 'XTickLabel',[]);
-legend('location', 'northeast', 'FontSize', 8);
-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)]);
-
-% 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]].
-
-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);
-
-
-
-% 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]].
-
-
-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', 'FontSize', 8);
-
-
-
-% #+name: fig:sensors_uncertainty_weights
-% #+caption: Magnitude of the multiplicative uncertainty weights $|W_i(j\omega)|$
-% #+RESULTS:
-% [[file:figs/sensors_uncertainty_weights.png]]
-
-
-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', 'northwest', 'FontSize', 8, 'NumColumns', 2);
-hold off;
-ylim([1e-2, 1e4])
-
-% 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)]);
-
-% 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]].
-
-omegac = 0.15*2*pi; G0 = 1e-1; Ginf = 1e-6;
-N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4);
-
-omegac = 1000*2*pi; G0 = 1e-6; Ginf = 1e-3;
-N2 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4);
-
-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('ASD $\left[ \frac{m/s}{\sqrt{Hz}} \right]$');
-hold off;
-xlim([freqs(1), freqs(end)]);
-legend('location', 'northeast', 'FontSize', 8);
-
-% Save Model
-% All the dynamical systems representing the sensors are saved for further use.
-
-
-save('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');

paper/paper.org

@@ -96,6 +96,7 @@
 <<sec:optimal_fusion>>
 
 ** Sensor Model
+
 Let's consider a sensor measuring a physical quantity $x$ (Figure ref:fig:sensor_model_noise).
 The sensor has an internal dynamics which is here modelled with a Linear Time Invariant (LTI) system transfer function $G_i(s)$.
 
@@ -130,6 +131,7 @@ In order to obtain an estimate $\hat{x}_i$ of $x$, a model $\hat{G}_i$ of the (t
 [[file:figs/sensor_model_noise.pdf]]
 
 ** Sensor Fusion Architecture
+
 Let's now consider two sensors measuring the same physical quantity $x$ but with different dynamics $(G_1, G_2)$ and noise characteristics $(N_1, N_2)$ (Figure ref:fig:sensor_fusion_noise_arch).
 
 The noise sources $\tilde{n}_1$ and $\tilde{n}_2$ are considered to be uncorrelated.
@@ -193,8 +195,8 @@ This can be cast into an $\mathcal{H}_2$ synthesis problem by considering the fo
 \begin{pmatrix}
   z_1 \\ z_2 \\ v
 \end{pmatrix} = \underbrace{\begin{bmatrix}
-  N_1 & -N_1 \\
+  N_1 & N_1 \\
-  0   &  N_2 \\
+  0   & N_2 \\
   1   &  0
 \end{bmatrix}}_{P_{\mathcal{H}_2}} \begin{pmatrix}
   w \\ u
@@ -221,11 +223,6 @@ We then have that the $\mathcal{H}_2$ synthesis applied on $P_{\mathcal{H}_2}$ g
 
 ** Example
 
-#+name: fig:figure_name
-#+caption: Figure caption
-#+attr_latex: :scale 1
-[[file:figs/sensors_nominal_dynamics.pdf]]
-
 ** Robustness Problem
 
 * Robust Sensor Fusion: $\mathcal{H}_\infty$ Synthesis
@@ -273,6 +270,7 @@ As $H_1$ and $H_2$ are complementary filters, we finally have:
 [[file:figs/sensor_fusion_arch_uncertainty.pdf]]
 
 ** Super Sensor Dynamical Uncertainty
+
 The uncertainty set of the transfer function from $\hat{x}$ to $x$ at frequency $\omega$ is bounded in the complex plane by a circle centered on 1 and with a radius equal to $|W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)|$ as shown in Figure ref:fig:uncertainty_set_super_sensor.
 
 
@@ -308,9 +306,9 @@ This problem can thus be dealt with an $\mathcal{H}_\infty$ synthesis problem by
 \begin{pmatrix}
   z_1 \\ z_2 \\ v
 \end{pmatrix} = \underbrace{\begin{bmatrix}
-  W_u W_1 & -W_u W_1 \\
+  W_u W_1 & W_u W_1 \\
-  0       &  W_u W_2 \\
+  0       & W_u W_2 \\
-  1       &  0
+  1       & 0
 \end{bmatrix}}_{P_{\mathcal{H}_\infty}} \begin{pmatrix}
   w \\ u
 \end{pmatrix}