Worked on H-Infinity Synthesis

matlab/index.org

@@ -616,7 +616,7 @@ The corresponding Cumulative Power Spectrum $\Gamma_{n_1}$, $\Gamma_{n_2}$ and $
 
 The RMS value of the individual sensors and of the super sensor are listed in Table [[tab:rms_noise_H2]].
 #+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-  data2orgtable([sqrt(CPS_S1(end)); sqrt(CPS_S2(end)); sqrt(CPS_H2(end))], {'$\sigma_{n_1}$', '$\sigma_{n_2}$', '$\sigma_{n_{\mathcal{H}_2}}$'}, {'RMS value $[m/s]$'}, ' %.1e ');
+  data2orgtable([sqrt(CPS_S1(end)); sqrt(CPS_S2(end)); sqrt(CPS_H2(end))], {'$\sigma_{n_1}$', '$\sigma_{n_2}$', '$\sigma_{n_{\mathcal{H}_2}}$'}, {'RMS value $[m/s]$'}, ' %.3f ');
 #+end_src
 
 #+name: tab:rms_noise_H2
@@ -627,8 +627,8 @@ The RMS value of the individual sensors and of the super sensor are listed in Ta
 |                              | RMS value $[m/s]$ |
 |------------------------------+-------------------|
 | $\sigma_{n_1}$               |             0.015 |
-| $\sigma_{n_2}$               |              0.08 |
+| $\sigma_{n_2}$               |             0.080 |
-| $\sigma_{n_{\mathcal{H}_2}}$ |            0.0027 |
+| $\sigma_{n_{\mathcal{H}_2}}$ |             0.003 |
 
 #+begin_src matlab :exports none
   figure;
@@ -811,7 +811,35 @@ To do so, we model the uncertainty that we have on the sensor dynamics by multip
 #+caption: Sensor fusion architecture with sensor dynamics uncertainty
 [[file:figs-tikz/sensor_fusion_arch_uncertainty.png]]
 
-The objective here is to design complementary filters $H_1(s)$ and $H_2(s)$ in order to bound the dynamical uncertainty of the super sensor to acceptable values.
+As explained in Section [[sec:sensor_uncertainty]], at each frequency $\omega$, the dynamical uncertainty of the super sensor can be represented in the complex plane by a circle with a radius equals to $|H_1(j\omega) W_1(j\omega)| + |H_2(j\omega) W_2(j\omega)|$ and centered on 1.
+
+In order to specify a wanted upper bound on the dynamical uncertainty, a weight $W_u(s)$ is used where $1/|W_u(j\omega)|$ represents the maximum allowed radius of the uncertainty circle corresponding to the super sensor dynamics at a frequency $\omega$ eqref:eq:upper_bound_uncertainty.
+
+\begin{align}
+                  & |H_1(j\omega) W_1(j\omega)| + |H_2(j\omega) W_2(j\omega)| < \frac{1}{|W_u(j\omega)|}, \quad \forall \omega \label{eq:upper_bound_uncertainty} \\
+  \Leftrightarrow & |H_1(j\omega) W_1(j\omega) W_u(j\omega)| + |H_2(j\omega) W_2(j\omega) W_u(j\omega)| < 1, \quad \forall\omega \label{eq:upper_bound_uncertainty_bis}
+\end{align}
+
+
+$|W_u(j\omega)|$ is also linked to the gain uncertainty $\Delta G$ eqref:eq:gain_uncertainty_bound and phase uncertainty $\Delta\phi$ eqref:eq:phase_uncertainty_bound of the super sensor.
+\begin{align}
+  \Delta G (\omega) &\le \frac{1}{|W_u(j\omega)|}, \quad \forall\omega \label{eq:gain_uncertainty_bound} \\
+  \Delta \phi (\omega) &\le \arcsin\left(\frac{1}{|W_u(j\omega)|}\right), \quad \forall\omega \label{eq:phase_uncertainty_bound}
+\end{align}
+
+The choice of $W_u$ is presented in Section [[sec:weight_uncertainty]].
+
+
+Condition eqref:eq:upper_bound_uncertainty_bis can almost be represented by eqref:eq:hinf_norm_uncertainty (within a factor $\sqrt{2}$).
+\begin{equation}
+  \left\| \begin{matrix} H_1(s) W_1(s) W_u(s) \\ H_2(s) W_2(s) W_u(s) \end{matrix} \right\|_\infty < 1 \label{eq:hinf_norm_uncertainty}
+\end{equation}
+
+
+
+The objective is to design $H_1(s)$ and $H_2(s)$ such that $H_1(s) + H_2(s) = 1$ (complementary property) and such that eqref:eq:hinf_norm_uncertainty is verified (bounded dynamical uncertainty).
+
+This is done using the $\mathcal{H}_\infty$ synthesis in Section [[sec:Hinfinity_synthesis]].
 
 ** Matlab Init                                              :noexport:ignore:
 #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
@@ -827,68 +855,17 @@ The objective here is to design complementary filters $H_1(s)$ and $H_2(s)$ in o
   load('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
 #+end_src
 
-** Super Sensor Dynamical Uncertainty
-In practical systems, the sensor dynamics has always some level of uncertainty.
-
-The dynamics of the super sensor is represented by
-\begin{align*}
-  \frac{\hat{x}}{x} &= (1 + W_1 \Delta_1) H_1 + (1 + W_2 \Delta_2) H_2 \\
-                    &= 1 + W_1 H_1 \Delta_1 + W_2 H_2 \Delta_2
-\end{align*}
-with $\Delta_i$ is any transfer function satisfying $\| \Delta_i \|_\infty < 1$.
-
-We see that as soon as we have some uncertainty in the sensor dynamics, we have that the complementary filters have some effect on the transfer function from $x$ to $\hat{x}$.
-
-The uncertainty set of the transfer function from $\hat{x}$ to $x$ at frequency $\omega$ is bounded in the complex plane by a circle centered on 1 and with a radius equal to $|W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)|$ (figure [[fig:uncertainty_gain_phase_variation]]).
-
-We then have that the angle introduced by the super sensor is bounded by $\arcsin(\epsilon)$:
-\[ \angle \frac{\hat{x}}{x}(j\omega) \le \arcsin \Big(|W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)|\Big) \]
-
-#+name: fig:uncertainty_gain_phase_variation
-#+caption: Maximum phase variation
-[[file:figs-tikz/uncertainty_gain_phase_variation.png]]
-
-** Synthesis objective
-The uncertainty region of the super sensor dynamics is represented by a circle in the complex plane as shown in Figure [[fig:uncertainty_gain_phase_variation]].
-
-At each frequency $\omega$, the radius of the circle is $|W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)|$.
-
-Thus, the phase shift $\Delta\phi(\omega)$ due to the super sensor uncertainty is bounded by:
-\[ |\Delta\phi(\omega)| \leq \arcsin\big( |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| \big) \]
-
-Let's define some allowed frequency depend phase shift $\Delta\phi_\text{max}(\omega) > 0$ such that:
-\[ |\Delta\phi(\omega)| < \Delta\phi_\text{max}(\omega), \quad \forall\omega \]
-
-
-If $H_1(s)$ and $H_2(s)$ are designed such that
-\[ |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| < \sin\big( \Delta\phi_\text{max}(\omega) \big) \]
-
-The maximum phase shift due to dynamic uncertainty at frequency $\omega$ will be $\Delta\phi_\text{max}(\omega)$.
-
-** Requirements as an $\mathcal{H}_\infty$ norm
-We now try to express this requirement in terms of an $\mathcal{H}_\infty$ norm.
-
-Let's define one weight $W_\phi(s)$ that represents the maximum wanted phase uncertainty:
-\[ |W_{\phi}(j\omega)|^{-1} \approx \sin(\Delta\phi_{\text{max}}(\omega)), \quad \forall\omega \]
-
-Then:
-\begin{align*}
-                      & |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| < \sin\big( \Delta\phi_\text{max}(\omega) \big), \quad \forall\omega \\
-  \Longleftrightarrow & |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| < |W_\phi(j\omega)|^{-1}, \quad \forall\omega \\
-  \Longleftrightarrow & \left| W_1(j\omega) H_1(j\omega) W_\phi(j\omega) \right| + \left| W_2(j\omega) H_2(j\omega) W_\phi(j\omega) \right| < 1, \quad \forall\omega
-\end{align*}
-
-Which is approximately equivalent to (with an error of maximum $\sqrt{2}$):
-#+name: eq:hinf_conf_phase_uncertainty
-\begin{equation}
-  \left\| \begin{matrix} W_1(s) W_\phi(s) H_1(s) \\ W_2(s) W_\phi(s) H_2(s) \end{matrix} \right\|_\infty < 1
-\end{equation}
-
-One should not forget that at frequency where both sensors has unknown dynamics ($|W_1(j\omega)| > 1$ and $|W_2(j\omega)| > 1$), the super sensor dynamics will also be unknown and the phase uncertainty cannot be bounded.
-Thus, at these frequencies, $|W_\phi|$ should be smaller than $1$.
-
 ** Weighting Function used to bound the super sensor uncertainty
-Let's define $W_\phi(s)$ in order to bound the maximum allowed phase uncertainty $\Delta\phi_\text{max}$ of the super sensor dynamics.
+<<sec:weight_uncertainty>>
+
+$W_u(s)$ is defined such that the super sensor phase uncertainty is less than 10 degrees below 100Hz eqref:eq:phase_uncertainy_bound_low_freq and is less than 180 degrees below 400Hz eqref:eq:phase_uncertainty_max.
+
+\begin{align}
+  \frac{1}{|W_u(j\omega)|} &< \sin\left(10 \frac{\pi}{180}\right), \quad \omega < 100\,\text{Hz} \label{eq:phase_uncertainy_bound_low_freq} \\
+  \frac{1}{|W_u(j 2 \pi 400)|} &< 1 \label{eq:phase_uncertainty_max}
+\end{align}
+
+The uncertainty bounds of the two individual sensor as well as the wanted maximum uncertainty bounds of the super sensor are shown in Figure [[fig:weight_uncertainty_bounds_Wu]].
 
 #+begin_src matlab
   Dphi = 10; % [deg]
@@ -896,23 +873,30 @@ Let's define $W_\phi(s)$ in order to bound the maximum allowed phase uncertainty
   Wu = createWeight('n', 2, 'w0', 2*pi*4e2, 'G0', 1/sin(Dphi*pi/180), 'G1', 1/4, 'Gc', 1);
 #+end_src
 
-#+begin_src matlab
+#+begin_src matlab :exports none
+  % Wu is saved for further use
   save('./mat/Wu.mat', 'Wu');
 #+end_src
 
 #+begin_src matlab :exports none
+  Dphi_Wu = 180/pi*asin(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))));
+  Dphi_Wu(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))) > 1) = 360;
+
   figure;
   % Magnitude
   ax1 = subplot(2,1,1);
   hold on;
-  plotMagUncertainty(W1, freqs, 'color_i', 1);
+  plotMagUncertainty(W1, freqs, 'color_i', 1, 'DisplayName', '$1 + W_1 \Delta_1$');
-  plotMagUncertainty(W2, freqs, 'color_i', 2);
+  plotMagUncertainty(W2, freqs, 'color_i', 2, 'DisplayName', '$1 + W_2 \Delta_2$');
-  p = plotMagUncertainty(inv(Wu), freqs, 'color_i', 3);
+  plot(freqs, 1 + abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'k--', ...
-  p.EdgeColor = 'black'; p.FaceAlpha = 0;
+       'DisplayName', '$1 + W_u^{-1} \Delta$')
+  plot(freqs, 1 - abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'k--', ...
+       'HandleVisibility', 'off')
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
   set(gca, 'XTickLabel',[]);
   ylabel('Magnitude');
   ylim([1e-2, 1e1]);
+  legend('location', 'southeast');
   hold off;
 
   % Phase
@@ -920,8 +904,8 @@ Let's define $W_\phi(s)$ in order to bound the maximum allowed phase uncertainty
   hold on;
   plotPhaseUncertainty(W1, freqs, 'color_i', 1);
   plotPhaseUncertainty(W2, freqs, 'color_i', 2);
-  p = plotPhaseUncertainty(inv(Wu), freqs);
+  plot(freqs,  Dphi_Wu, 'k--');
-  p.EdgeColor = 'black'; p.FaceAlpha = 0;
+  plot(freqs, -Dphi_Wu, 'k--');
   set(gca,'xscale','log');
   yticks(-180:90:180);
   ylim([-180 180]);
@@ -931,36 +915,36 @@ Let's define $W_\phi(s)$ in order to bound the maximum allowed phase uncertainty
   xlim([freqs(1), freqs(end)]);
 #+end_src
 
-The obtained upper bounds on the complementary filters in order to limit the phase uncertainty of the super sensor are represented in Figure [[fig:upper_bounds_comp_filter_max_phase_uncertainty]].
+#+begin_src matlab :tangle no :exports results :results file replace
-
+  exportFig('figs/weight_uncertainty_bounds_Wu.pdf', 'width', 'full', 'height', 'full');
-#+begin_src matlab :exports none
-  figure;
-  hold on;
-  plot(freqs, 1./abs(squeeze(freqresp(Wu*W1, freqs, 'Hz'))), '-', 'DisplayName', '$1/|W_1W_\phi|$');
-  plot(freqs, 1./abs(squeeze(freqresp(Wu*W2, freqs, 'Hz'))), '-', 'DisplayName', '$1/|W_2W_\phi|$');
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  xlabel('Frequency [Hz]'); ylabel('Magnitude');
-  hold off;
-  xlim([freqs(1), freqs(end)]);
-  legend('location', 'northeast');
 #+end_src
 
-#+HEADER: :tangle no :exports results :results none :noweb yes
+#+name: fig:weight_uncertainty_bounds_Wu
-#+begin_src matlab :var filepath="figs/upper_bounds_comp_filter_max_phase_uncertainty.pdf" :var figsize="full-normal" :post pdf2svg(file=*this*, ext="png")
+#+caption: Uncertainty region of the two sensors as well as the wanted maximum uncertainty of the super sensor (dashed lines)
-  <<plt-matlab>>
+#+RESULTS:
-#+end_src
+[[file:figs/weight_uncertainty_bounds_Wu.png]]
-
-#+NAME: fig:upper_bounds_comp_filter_max_phase_uncertainty
-#+CAPTION: Upper bounds on the complementary filters set in order to limit the maximum phase uncertainty of the super sensor to 30 degrees until 500Hz
-[[file:figs/upper_bounds_comp_filter_max_phase_uncertainty.png]]
 
 ** $\mathcal{H}_\infty$ Synthesis
-The $\mathcal{H}_\infty$ synthesis architecture used for the complementary filters is shown in Figure [[fig:h_infinity_robust_fusion]].
+<<sec:Hinfinity_synthesis>>
+
+The generalized plant $P_{\mathcal{H}_\infty}$ used for the $\mathcal{H}_\infty$ Synthesis of the complementary filters is shown in Figure [[fig:h_infinity_robust_fusion]] and is described by Equation eqref:eq:Hinf_generalized_plant.
 
 #+name: fig:h_infinity_robust_fusion
 #+caption: Architecture used for $\mathcal{H}_\infty$ synthesis of complementary filters
 [[file:figs-tikz/h_infinity_robust_fusion.png]]
 
+\begin{equation} \label{eq:Hinf_generalized_plant}
+\begin{pmatrix}
+  z_1 \\ z_2 \\ v
+\end{pmatrix} = \underbrace{\begin{bmatrix}
+  W_u W_1 & -W_u W_1 \\
+  0       &  W_u W_2 \\
+  1       &  0
+\end{bmatrix}}_{P_{\mathcal{H}_\infty}} \begin{pmatrix}
+  w \\ u
+\end{pmatrix}
+\end{equation}
+
 The generalized plant is defined below.
 #+begin_src matlab
   P = [Wu*W1 -Wu*W1;
@@ -968,15 +952,13 @@ The generalized plant is defined below.
        1      0];
 #+end_src
 
-And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command.
+And the $\mathcal{H}_\infty$ synthesis is performed using the =hinfsyn= command.
 #+begin_src matlab :results output replace :exports both
-  [H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
+  H2 = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'DISPLAY', 'on');
 #+end_src
 
 #+RESULTS:
 #+begin_example
-[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
-
   Test bounds:  0.7071 <=  gamma  <=  1.291
 
     gamma        X>=0        Y>=0       rho(XY)<1    p/f
@@ -994,23 +976,27 @@ And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command.
   Best performance (actual): 0.7071
 #+end_example
 
-And $H_1(s)$ is defined as the complementary of $H_2(s)$.
+The $\mathcal{H}_\infty$ is successful as the $\mathcal{H}_\infty$ norm of the "closed loop" transfer function from $(w)$ to $(z_1,\ z_2)$ is less than one.
+
+$H_1(s)$ is then defined as the complementary of $H_2(s)$.
 #+begin_src matlab
   H1 = 1 - H2;
 #+end_src
 
 #+begin_src matlab :exports none
+  % Complementary filters are saved for further analysis
   save('./mat/Hinf_filters.mat', 'H2', 'H1');
 #+end_src
 
-The obtained complementary filters are shown in Figure [[fig:comp_filter_hinf_uncertainty]].
+The obtained complementary filters as well as the wanted upper bounds are shown in Figure [[fig:hinf_comp_filters]].
+
 #+begin_src matlab :exports none
   figure;
 
   ax1 = subplot(2,1,1);
   hold on;
-  plot(freqs, 1./abs(squeeze(freqresp(Wu*W1, freqs, 'Hz'))), '--', 'DisplayName', '$|WuW_1|$');
+  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', '$|WuW_2|$');
+  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$');
@@ -1033,44 +1019,49 @@ The obtained complementary filters are shown in Figure [[fig:comp_filter_hinf_un
 
   linkaxes([ax1,ax2],'x');
   xlim([freqs(1), freqs(end)]);
-  xticks([0.1, 1, 10, 100, 1000]);
 #+end_src
 
-#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :tangle no :exports results :results file replace
-#+begin_src matlab :var filepath="figs/comp_filter_hinf_uncertainty.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  exportFig('figs/hinf_comp_filters.pdf', 'width', 'full', 'height', 'full');
-  <<plt-matlab>>
 #+end_src
 
-#+NAME: fig:comp_filter_hinf_uncertainty
+#+name: fig:hinf_comp_filters
-#+CAPTION: Obtained complementary filters
+#+caption: Obtained complementary filters using the $\mathcal{H}_\infty$ Synthesis
-[[file:figs/comp_filter_hinf_uncertainty.png]]
+#+RESULTS:
+[[file:figs/hinf_comp_filters.png]]
 
 ** Super sensor uncertainty
-#+begin_src matlab
+The super sensor dynamical uncertainty is displayed in Figure [[fig:super_sensor_dynamical_uncertainty_Hinf]].
-  H2_filters = load('./mat/H2_filters.mat', 'H2', 'H1');
+It is confirmed that the super sensor dynamical uncertainty is less than the maximum allowed uncertainty defined by the norm of $W_u(s)$.
-#+end_src
+
+The $\mathcal{H}_\infty$ synthesis thus allows to design filters such that the super sensor has specified bounded uncertainty.
 
 #+begin_src matlab :exports none
+  Dphi_Wu = 180/pi*asin(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))));
+  Dphi_Wu(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))) > 1) = 360;
+
   Dphi_ss = 180/pi*asin(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))));
   Dphi_ss(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))) > 1) = 360;
 
-  Dphi_ss_H2 = 180/pi*asin(abs(squeeze(freqresp(W2*H2_filters.H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H2_filters.H1, freqs, 'Hz'))));
-  Dphi_ss_H2(abs(squeeze(freqresp(W2*H2_filters.H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H2_filters.H1, freqs, 'Hz'))) > 1) = 360;
-
   figure;
   % Magnitude
   ax1 = subplot(2,1,1);
   hold on;
-  plotMagUncertainty(W1, freqs, 'color_i', 1);
+  plotMagUncertainty(W1, freqs, 'color_i', 1, 'DisplayName', '$1 + W_1 \Delta_1$');
-  plotMagUncertainty(W2, freqs, 'color_i', 2);
+  plotMagUncertainty(W2, freqs, 'color_i', 2, 'DisplayName', '$1 + W_2 \Delta_2$');
-  p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))+abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))-abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 0.001))], 'w');
+  plot(freqs, 1 + abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))+abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 'k-', ...
-  p.EdgeColor = 'black'; p.FaceAlpha = 0;
+       'DisplayName', '$1 + W_1 \Delta_1 + W_2 \Delta_2$')
-  p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W2*H2_filters.H2, freqs, 'Hz')))+abs(squeeze(freqresp(W1*H2_filters.H1, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W2*H2_filters.H2, freqs, 'Hz')))-abs(squeeze(freqresp(W1*H2_filters.H1, freqs, 'Hz'))), 0.001))], 'w');
+  plot(freqs, max(1 - abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))-abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 0.001), 'k-', ...
-  p.EdgeColor = 'black'; p.FaceAlpha = 0; p.LineStyle = '--';
+       'HandleVisibility', 'off');
+  plot(freqs, 1 + abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'k--', ...
+       'DisplayName', '$1 + W_u^{-1}\Delta$')
+  plot(freqs, 1 - abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'k--', ...
+       'HandleVisibility', 'off')
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
   set(gca, 'XTickLabel',[]);
   ylabel('Magnitude');
   ylim([1e-2, 1e1]);
+  legend('location', 'southeast');
   hold off;
 
   % Phase
@@ -1078,10 +1069,10 @@ The obtained complementary filters are shown in Figure [[fig:comp_filter_hinf_un
   hold on;
   plotPhaseUncertainty(W1, freqs, 'color_i', 1);
   plotPhaseUncertainty(W2, freqs, 'color_i', 2);
-  p = patch([freqs flip(freqs)], [Dphi_ss; flip(-Dphi_ss)], 'w');
+  plot(freqs,  Dphi_ss, 'k-');
-  p.EdgeColor = 'black'; p.FaceAlpha = 0;
+  plot(freqs, -Dphi_ss, 'k-');
-  p = patch([freqs flip(freqs)], [Dphi_ss_H2; flip(-Dphi_ss_H2)], 'w');
+  plot(freqs,  Dphi_Wu, 'k--');
-  p.EdgeColor = 'black'; p.FaceAlpha = 0; p.LineStyle = '--';
+  plot(freqs, -Dphi_Wu, 'k--');
   set(gca,'xscale','log');
   yticks(-180:90:180);
   ylim([-180 180]);
@@ -1091,37 +1082,49 @@ The obtained complementary filters are shown in Figure [[fig:comp_filter_hinf_un
   xlim([freqs(1), freqs(end)]);
 #+end_src
 
-The uncertainty of the super sensor cannot be made smaller than both the individual sensor. Ideally, it would follow the minimum uncertainty of both sensors.
+#+begin_src matlab :tangle no :exports results :results file replace
+  exportFig('figs/super_sensor_dynamical_uncertainty_Hinf.pdf', 'width', 'full', 'height', 'full');
+#+end_src
 
-We here just used very wimple weights.
+#+name: fig:super_sensor_dynamical_uncertainty_Hinf
-For instance, we could improve the dynamical uncertainty of the super sensor by making $|W_\phi(j\omega)|$ smaller bellow 2Hz where the dynamical uncertainty of the sensor 1 is small.
+#+caption: Super sensor dynamical uncertainty (solid curve) when using the $\mathcal{H}_\infty$ Synthesis
+#+RESULTS:
+[[file:figs/super_sensor_dynamical_uncertainty_Hinf.png]]
 
 ** Super sensor noise
-We now compute the obtain Power Spectral Density of the super sensor's noise.
+We now compute the obtain Power Spectral Density of the super sensor's noise (Figure [[fig:psd_sensors_hinf_synthesis]]).
-The noise characteristics of both individual sensor are defined below.
 
-The PSD of both sensor and of the super sensor is shown in Figure [[fig:psd_sensors_hinf_synthesis]].
+The 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 CPS of both sensor and of the super sensor is shown in Figure [[fig:cps_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.
 
 #+begin_src matlab
   PSD_S2   = abs(squeeze(freqresp(N2,    freqs, 'Hz'))).^2;
   PSD_S1   = abs(squeeze(freqresp(N1,    freqs, 'Hz'))).^2;
-  PSD_Hinf = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
+  PSD_Hinf = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2 + ...
-  PSD_H2   = abs(squeeze(freqresp(N1*H2_filters.H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2_filters.H2, freqs, 'Hz'))).^2;
+             abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
+#+end_src
 
+#+begin_src matlab :exports none
+  H2_filters = load('./mat/H2_filters.mat', 'H2', 'H1');
+
+  PSD_H2   = abs(squeeze(freqresp(N1*H2_filters.H1, freqs, 'Hz'))).^2 + ...
+             abs(squeeze(freqresp(N2*H2_filters.H2, freqs, 'Hz'))).^2;
+  CPS_H2   = cumtrapz(freqs, PSD_H2);
+#+end_src
+
+#+begin_src matlab :exports none
   CPS_S2   = cumtrapz(freqs, PSD_S2);
   CPS_S1   = cumtrapz(freqs, PSD_S1);
   CPS_Hinf = cumtrapz(freqs, PSD_Hinf);
-  CPS_H2   = cumtrapz(freqs, PSD_H2);
 #+end_src
 
 #+begin_src matlab :exports none
   figure;
   hold on;
-  plot(freqs, PSD_S2,    '-',  'DisplayName', '$\Phi_{\hat{x}_{pos}}$');
+  plot(freqs, PSD_S1,   '-',   'DisplayName', '$\Phi_{n_1}$');
-  plot(freqs, PSD_S1,    '-',  'DisplayName', '$\Phi_{\hat{x}_{acc}}$');
+  plot(freqs, PSD_S2,   '-',   'DisplayName', '$\Phi_{n_2}$');
-  plot(freqs, PSD_Hinf, 'k-',  'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_\infty}}$');
+  plot(freqs, PSD_H2,   'k-',  'DisplayName', '$\Phi_{n_{\mathcal{H}_2}}$');
-  plot(freqs, PSD_H2,   'k--', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_2}}$');
+  plot(freqs, PSD_Hinf, 'k--', 'DisplayName', '$\Phi_{n_{\mathcal{H}_\infty}}$');
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
   xlabel('Frequency [Hz]'); ylabel('Power Spectral Density [$(m/s)^2/Hz$]');
   hold off;
@@ -1129,37 +1132,28 @@ The CPS of both sensor and of the super sensor is shown in Figure [[fig:cps_sens
   legend('location', 'northeast');
 #+end_src
 
-#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :tangle no :exports results :results file replace
-#+begin_src matlab :var filepath="figs/psd_sensors_hinf_synthesis.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  exportFig('figs/psd_sensors_hinf_synthesis.pdf', 'width', 'wide', 'height', 'normal');
-  <<plt-matlab>>
 #+end_src
 
-#+NAME: fig:psd_sensors_hinf_synthesis
+#+name: fig:psd_sensors_hinf_synthesis
-#+CAPTION: Power Spectral Density of the obtained super sensor using the $\mathcal{H}_\infty$ synthesis
+#+caption: Power Spectral Density of the estimated $\hat{x}$ using the two sensors alone and using the
+#+RESULTS:
 [[file:figs/psd_sensors_hinf_synthesis.png]]
 
-#+begin_src matlab :exports none
+#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
-  figure;
+  data2orgtable([sqrt(CPS_H2(end)), sqrt(CPS_Hinf(end))]', {'Optimal: $\mathcal{H}_2$', 'Robust: $\mathcal{H}_\infty$'}, {'RMS [m/s]'}, ' %.1e ');
-  hold on;
-  plot(freqs, CPS_S2, '-',  'DisplayName', sprintf('$\\sigma_{\\hat{x}_{pos}} = %.1e$ [m/s rms]', sqrt(CPS_S2(end))));
-  plot(freqs, CPS_S1, '-',  'DisplayName', sprintf('$\\sigma_{\\hat{x}_{acc}} = %.1e$ [m/s rms]', sqrt(CPS_S1(end))));
-  plot(freqs, CPS_Hinf,  'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_\\infty}} = %.1e$ [m/s rms]', sqrt(CPS_Hinf(end))));
-  set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
-  xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum');
-  hold off;
-  xlim([2*freqs(1), freqs(end)]);
-  % ylim([1e-10 1e-5]);
-  legend('location', 'southeast');
 #+end_src
 
-#+HEADER: :tangle no :exports results :results none :noweb yes
+#+name: tab:rms_noise_comp_H2_Hinf
-#+begin_src matlab :var filepath="figs/cps_sensors_hinf_synthesis.cps" :var figsize="full-tall" :post cps2svg(file=*this*, ext="png")
+#+caption: Comparison of the obtained RMS noise of the super sensor
-  <<plt-matlab>>
+#+attr_latex: :environment tabular :align cc
-#+end_src
+#+attr_latex: :center t :booktabs t :float t
-
+#+RESULTS:
-#+NAME: fig:cps_sensors_hinf_synthesis
+|                              | RMS [m/s] |
-#+CAPTION: Cumulative Power Spectrum of the obtained super sensor using the $\mathcal{H}_\infty$ synthesis
+|------------------------------+-----------|
-[[file:figs/cps_sensors_hinf_synthesis.png]]
+| Optimal: $\mathcal{H}_2$     |    0.0027 |
+| Robust: $\mathcal{H}_\infty$ |     0.041 |
 
 ** Conclusion
 Using the $\mathcal{H}_\infty$ synthesis, the dynamical uncertainty of the super sensor can be bounded to acceptable values.
@@ -1178,7 +1172,6 @@ However, the RMS of the super sensor noise is not optimized as it was the case w
 #+caption: Sensor fusion architecture with sensor dynamics uncertainty
 [[file:figs-tikz/sensor_fusion_arch_full.png]]
 
-** Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis - Introduction
 The goal is to design complementary filters such that:
 - the maximum uncertainty of the super sensor is bounded
 - the RMS value of the super sensor noise is minimized
@@ -1201,46 +1194,6 @@ The Matlab function for that is =h2hinfsyn= ([[https://fr.mathworks.com/help/rob
   load('./mat/Wu.mat', 'Wu');
 #+end_src
 
-** Noise characteristics and Uncertainty of the individual sensors
-Both dynamical uncertainty and noise characteristics of the individual sensors are shown in Figure [[fig:mixed_synthesis_noise_uncertainty_sensors]].
-
-#+begin_src matlab :exports none
-  figure;
-  ax1 = subplot(2, 1, 1);
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(N2, freqs, 'Hz'))), '-', 'DisplayName', '$|N_{pos}(j\omega)|$');
-  plot(freqs, abs(squeeze(freqresp(N1, freqs, 'Hz'))), '-', 'DisplayName', '$|N_{acc}(j\omega)|$');
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  xlabel('Frequency [Hz]'); ylabel('Magnitude');
-  hold off;
-  legend('location', 'northeast');
-
-  ax2 = subplot(2, 1, 2);
-  hold on;
-  plot(freqs, abs(squeeze(freqresp(W2, freqs, 'Hz'))), '-', 'DisplayName', '$|W_{pos}(j\omega)|$');
-  plot(freqs, abs(squeeze(freqresp(W1, freqs, 'Hz'))), '-', 'DisplayName', '$|W_{acc}(j\omega)|$');
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  xlabel('Frequency [Hz]'); ylabel('Magnitude');
-  hold off;
-  legend('location', 'northeast');
-
-  linkaxes([ax1,ax2],'x');
-  xlim([freqs(1), freqs(end)]);
-#+end_src
-
-#+HEADER: :tangle no :exports results :results none :noweb yes
-#+begin_src matlab :var filepath="figs/mixed_synthesis_noise_uncertainty_sensors.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
-  <<plt-matlab>>
-#+end_src
-
-#+NAME: fig:mixed_synthesis_noise_uncertainty_sensors
-#+CAPTION: Noise characteristsics and Dynamical uncertainty of the individual sensors
-[[file:figs/mixed_synthesis_noise_uncertainty_sensors.png]]
-
-** Weighting Functions on the uncertainty of the super sensor
-We design weights for the $\mathcal{H}_\infty$ part of the synthesis in order to limit the dynamical uncertainty of the super sensor.
-The maximum wanted multiplicative uncertainty is shown in Figure .The idea here is that we don't really need low uncertainty at low frequency but only near the crossover frequency that is suppose to be around 300Hz here.
-
 ** Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis
 The synthesis architecture that is used here is shown in Figure [[fig:mixed_h2_hinf_synthesis]].
 
@@ -1292,10 +1245,11 @@ The mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis is performed below.
 #+end_src
 
 #+begin_src matlab :exports none
+  % The obtained filters are saved for further analysis
   save('./mat/H2_Hinf_filters.mat', 'H2', 'H1');
 #+end_src
 
-The obtained complementary filters are shown in Figure [[fig:comp_filters_mixed_synthesis]].
+The obtained complementary filters are shown in Figure [[fig:htwo_hinf_comp_filters]].
 
 #+begin_src matlab :exports none
   figure;
@@ -1335,17 +1289,39 @@ The obtained complementary filters are shown in Figure [[fig:comp_filters_mixed_
   xticks([0.1, 1, 10, 100, 1000]);
 #+end_src
 
-#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :tangle no :exports results :results file replace
-#+begin_src matlab :var filepath="figs/comp_filters_mixed_synthesis.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  exportFig('figs/htwo_hinf_comp_filters.pdf', 'width', 'full', 'height', 'full');
-  <<plt-matlab>>
 #+end_src
 
-#+NAME: fig:comp_filters_mixed_synthesis
+#+name: fig:htwo_hinf_comp_filters
-#+CAPTION: Obtained complementary filters after mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis
+#+caption: Obtained complementary filters after mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis
-[[file:figs/comp_filters_mixed_synthesis.png]]
+#+RESULTS:
+[[file:figs/htwo_hinf_comp_filters.png]]
 
 ** Obtained Super Sensor's noise
-The PSD and CPS of the super sensor's noise are shown in Figure [[fig:psd_super_sensor_mixed_syn]] and Figure [[fig:cps_super_sensor_mixed_syn]] respectively.
+The PSD and CPS of the super sensor's noise are shown in Figure [[fig:psd_sensors_htwo_hinf_synthesis]] and Figure [[fig:cps_h2_hinf_synthesis]] respectively.
+
+#+begin_src matlab :exports none
+  % The filters are loaded
+  H2_filters      = load('./mat/H2_filters.mat',      'H2', 'H1');
+  Hinf_filters    = load('./mat/Hinf_filters.mat',    'H2', 'H1');
+#+end_src
+
+#+begin_src matlab :exports none
+  H2_filters = load('./mat/H2_filters.mat', 'H2', 'H1');
+
+  PSD_H2   = abs(squeeze(freqresp(N1*H2_filters.H1, freqs, 'Hz'))).^2 + ...
+             abs(squeeze(freqresp(N2*H2_filters.H2, freqs, 'Hz'))).^2;
+  CPS_H2   = cumtrapz(freqs, PSD_H2);
+#+end_src
+
+#+begin_src matlab :exports none
+  Hinf_filters = load('./mat/Hinf_filters.mat', 'H2', 'H1');
+
+  PSD_Hinf   = abs(squeeze(freqresp(N1*Hinf_filters.H1, freqs, 'Hz'))).^2 + ...
+               abs(squeeze(freqresp(N2*Hinf_filters.H2, freqs, 'Hz'))).^2;
+  CPS_Hinf   = cumtrapz(freqs, PSD_Hinf);
+#+end_src
 
 #+begin_src matlab
   PSD_S2     = abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2;
@@ -1360,9 +1336,11 @@ The PSD and CPS of the super sensor's noise are shown in Figure [[fig:psd_super_
 #+begin_src matlab :exports none
   figure;
   hold on;
-  plot(freqs, PSD_S2, '-',  'DisplayName', '$\Phi_{\hat{x}_{pos}}$');
+  plot(freqs, PSD_S1,     '-',   'DisplayName', '$\Phi_{n_1}$');
-  plot(freqs, PSD_S1, '-',  'DisplayName', '$\Phi_{\hat{x}_{acc}}$');
+  plot(freqs, PSD_S2,     '-',   'DisplayName', '$\Phi_{n_2}$');
-  plot(freqs, PSD_H2Hinf,  'k-', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_2/\mathcal{H}_\infty}}$');
+  plot(freqs, PSD_H2,     'k-',  'DisplayName', '$\Phi_{n_{\mathcal{H}_2}}$');
+  plot(freqs, PSD_Hinf,   'k--', 'DisplayName', '$\Phi_{n_{\mathcal{H}_\infty}}$');
+  plot(freqs, PSD_H2Hinf, 'k-.', 'DisplayName', '$\Phi_{n_{\mathcal{H}_2/\mathcal{H}_\infty}}$');
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
   xlabel('Frequency [Hz]'); ylabel('Power Spectral Density [$(m/s)^2/Hz$]');
   hold off;
@@ -1370,42 +1348,46 @@ The PSD and CPS of the super sensor's noise are shown in Figure [[fig:psd_super_
   legend('location', 'northeast');
 #+end_src
 
-#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :tangle no :exports results :results file replace
-#+begin_src matlab :var filepath="figs/psd_super_sensor_mixed_syn.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  exportFig('figs/psd_sensors_htwo_hinf_synthesis.pdf', 'width', 'wide', 'height', 'normal');
-  <<plt-matlab>>
 #+end_src
 
-#+NAME: fig:psd_super_sensor_mixed_syn
+#+name: fig:psd_sensors_htwo_hinf_synthesis
 #+CAPTION: Power Spectral Density of the Super Sensor obtained with the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis
-[[file:figs/psd_super_sensor_mixed_syn.png]]
+#+RESULTS:
+[[file:figs/psd_sensors_htwo_hinf_synthesis.png]]
 
 #+begin_src matlab :exports none
   figure;
   hold on;
-  plot(freqs, CPS_S2, '-',  'DisplayName', sprintf('$\\sigma_{\\hat{x}_{pos}} = %.1e$ [m/s rms]', sqrt(CPS_S2(end))));
+  plot(freqs, CPS_S1,     '-',   'DisplayName', '$\Gamma_{n_1}$');
-  plot(freqs, CPS_S1, '-',  'DisplayName', sprintf('$\\sigma_{\\hat{x}_{acc}} = %.1e$ [m/s rms]', sqrt(CPS_S1(end))));
+  plot(freqs, CPS_S2,     '-',   'DisplayName', '$\Gamma_{n_2}$');
-  plot(freqs, CPS_H2Hinf, 'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_\\infty/\\mathcal{H}_\\infty}} = %.1e$ [m/s rms]', sqrt(CPS_H2Hinf(end))));
+  plot(freqs, CPS_H2,     'k-',  'DisplayName', '$\Gamma_{n_{\mathcal{H}_2}}$');
+  plot(freqs, CPS_Hinf,   'k--', 'DisplayName', '$\Gamma_{n_{\mathcal{H}_\infty}}$');
+  plot(freqs, CPS_H2Hinf, 'k-.', 'DisplayName', '$\Gamma_{n_{\mathcal{H}_2/\mathcal{H}_\infty}}$');
   set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
   xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum');
   hold off;
   xlim([2*freqs(1), freqs(end)]);
-  % ylim([1e-10 1e-5]);
   legend('location', 'southeast');
 #+end_src
 
-#+HEADER: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :tangle no :exports results :results file replace
-#+begin_src matlab :var filepath="figs/cps_super_sensor_mixed_syn.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
+  exportFig('figs/cps_h2_hinf_synthesis.pdf', 'width', 'wide', 'height', 'normal');
-  <<plt-matlab>>
 #+end_src
 
-#+NAME: fig:cps_super_sensor_mixed_syn
+#+name: fig:cps_h2_hinf_synthesis
 #+CAPTION: Cumulative Power Spectrum of the Super Sensor obtained with the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis
-[[file:figs/cps_super_sensor_mixed_syn.png]]
+#+RESULTS:
+[[file:figs/cps_h2_hinf_synthesis.png]]
 
 ** Obtained Super Sensor's Uncertainty
 The uncertainty on the super sensor's dynamics is shown in Figure
 
 #+begin_src matlab :exports none
+  Dphi_Wu = 180/pi*asin(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))));
+  Dphi_Wu(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))) > 1) = 360;
+
   Dphi_ss = 180/pi*asin(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))));
   Dphi_ss(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))) > 1) = 360;
 
@@ -1417,6 +1399,10 @@ The uncertainty on the super sensor's dynamics is shown in Figure
   plotMagUncertainty(W2, freqs, 'color_i', 2);
   p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))+abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))-abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 0.001))], 'w');
   p.EdgeColor = 'black'; p.FaceAlpha = 0;
+  plot(freqs, 1 + abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'r--', ...
+       'DisplayName', '$W_u$')
+  plot(freqs, 1 - abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'r--', ...
+       'HandleVisibility', 'off')
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
   set(gca, 'XTickLabel',[]);
   ylabel('Magnitude');
@@ -1430,6 +1416,8 @@ The uncertainty on the super sensor's dynamics is shown in Figure
   plotPhaseUncertainty(W2, freqs, 'color_i', 2);
   p = patch([freqs flip(freqs)], [Dphi_ss; flip(-Dphi_ss)], 'w');
   p.EdgeColor = 'black'; p.FaceAlpha = 0;
+  plot(freqs,  Dphi_Wu, 'r--');
+  plot(freqs, -Dphi_Wu, 'r--');
   set(gca,'xscale','log');
   yticks(-180:90:180);
   ylim([-180 180]);