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

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

#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
#+HTML_HEAD: <script src="../js/jquery.min.js"></script>
#+HTML_HEAD: <script src="../js/bootstrap.min.js"></script>
#+HTML_HEAD: <script src="../js/jquery.stickytableheaders.min.js"></script>
#+HTML_HEAD: <script src="../js/readtheorg.js"></script>

#+PROPERTY: header-args:matlab  :session *MATLAB*
#+PROPERTY: header-args:matlab+ :tangle matlab/comp_filters_design.m
#+PROPERTY: header-args:matlab+ :comments org
#+PROPERTY: header-args:matlab+ :exports both
#+PROPERTY: header-args:matlab+ :results none
#+PROPERTY: header-args:matlab+ :eval no-export
#+PROPERTY: header-args:matlab+ :noweb yes
#+PROPERTY: header-args:matlab+ :mkdirp yes
#+PROPERTY: header-args:matlab+ :output-dir figs
:END:

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

One use of complementary filter is described below:
#+begin_quote
  The basic idea of a complementary filter involves taking two or more sensors, filtering out unreliable frequencies for each sensor, and combining the filtered outputs to get a better estimate throughout the entire bandwidth of the system.
  To achieve this, the sensors included in the filter should complement one another by performing better over specific parts of the system bandwidth.
#+end_quote

- in section [[sec:optimal_comp_filters]], the optimal design of the complementary filters in order to obtain the lowest resulting "super sensor" noise is studied

When blending two sensors using complementary filters with unknown dynamics, phase lag may be introduced that renders the close-loop system unstable.
- in section [[sec:comp_filter_robustness]], the blending robustness to sensor dynamic uncertainty is studied.

Then, three design methods for generating two complementary filters are proposed:
- in section [[sec:comp_filters_analytical]], analytical formulas are proposed
- in section [[sec:h_inf_synthesis_complementary_filters]], the $\mathcal{H}_\infty$ synthesis is used
- in section [[sec:feedback_generate_comp_filters]], the classical feedback architecture is used
- in section [[sec:analytical_formula_literature]], analytical formulas found in the literature are listed

* Optimal Sensor Fusion - Minimize the Super Sensor Noise
  :PROPERTIES:
  :header-args:matlab+: :tangle matlab/optimal_comp_filters.m
  :header-args:matlab+: :comments org :mkdirp yes
  :END:
  <<sec:optimal_comp_filters>>

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

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

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

** ZIP file containing the data and matlab files                     :ignore:
#+begin_src bash :exports none :results none
  if [ matlab/optimal_comp_filters.m -nt data/optimal_comp_filters.zip ]; then
    cp matlab/optimal_comp_filters.m optimal_comp_filters.m;
    zip data/optimal_comp_filters \
        optimal_comp_filters.m
    rm optimal_comp_filters.m;
  fi
#+end_src

#+begin_note
  All the files (data and Matlab scripts) are accessible [[file:data/optimal_comp_filters.zip][here]].
#+end_note

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

#+NAME: fig:htwo_comp_filters
#+CAPTION: Obtained complementary filters using the $\mathcal{H}_2$ Synthesis ([[./figs/htwo_comp_filters.png][png]], [[./figs/htwo_comp_filters.pdf][pdf]])
[[file:figs/htwo_comp_filters.png]]

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

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

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

#+NAME: fig:psd_sensors_htwo_synthesis
#+CAPTION: Power Spectral Density of the estimated $\hat{x}$ using the two sensors alone and using the optimally fused signal ([[./figs/psd_sensors_htwo_synthesis.png][png]], [[./figs/psd_sensors_htwo_synthesis.pdf][pdf]])
[[file:figs/psd_sensors_htwo_synthesis.png]]

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

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

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

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

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

#+begin_src matlab
  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;
#+end_src

#+begin_src matlab
  P = [W1a -W1a;
       0    W2a;
       1    0];
#+end_src

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

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

#+begin_src matlab
  H1a = 1 - H2a;
#+end_src

#+begin_src matlab :exports none
  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]);
#+end_src

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

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

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

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

#+begin_src matlab
  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
#+end_src

#+begin_src matlab
  P = [W1b -W1b;
       0    W2b;
       1    0];
#+end_src

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

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

#+begin_src matlab
  H1b = 1 - H2b;
#+end_src

#+begin_src matlab :exports none
  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]);
#+end_src

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

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

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

** Comparison of the methods
The three methods are now compared.

The Power Spectral Density of the super sensors obtained with the complementary filters designed using the three methods are shown in Fig. [[fig:comparison_psd_noise]].

The Cumulative Power Spectrum for the same sensors are shown on Fig. [[fig:comparison_cps_noise]].

The RMS value of the obtained super sensors are shown on table [[tab:rms_results]].

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
  data2orgtable([norm([N1], 2) ; norm([N2], 2) ; norm([N1*H1, N2*H2], 2) ; norm([N1*H1a, N2*H2a], 2) ; norm([N1*H1b, N2*H2b], 2)], {'Sensor 1', 'Sensor 2', 'H2 Fusion', 'H-Infinity a', 'H-Infinity b'}, {'rms value'}, ' %.1e');
#+end_src

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


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

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

#+NAME: fig:comparison_psd_noise
#+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]]

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

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

#+NAME: fig:comparison_cps_noise
#+CAPTION: Comparison of the obtained Cumulative Power Spectrum using the three methods ([[./figs/comparison_cps_noise.png][png]], [[./figs/comparison_cps_noise.pdf][pdf]])
[[file:figs/comparison_cps_noise.png]]

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

* Optimal Sensor Fusion - Minimize the Super Sensor Dynamical Uncertainty
  :PROPERTIES:
  :header-args:matlab+: :tangle matlab/comp_filter_robustness.m
  :header-args:matlab+: :comments org :mkdirp yes
  :END:
  <<sec:comp_filter_robustness>>

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

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

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

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

** ZIP file containing the data and matlab files                    :ignore:
#+begin_src bash :exports none :results none
  if [ matlab/comp_filter_robustness.m -nt data/comp_filter_robustness.zip ]; then
    cp matlab/comp_filter_robustness.m comp_filter_robustness.m;
    zip data/comp_filter_robustness \
        comp_filter_robustness.m
    rm comp_filter_robustness.m;
  fi
#+end_src

#+begin_note
  All the files (data and Matlab scripts) are accessible [[file:data/comp_filter_robustness.zip][here]].
#+end_note

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

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

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

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

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

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

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

We want that the super sensor transfer function has a gain of 1 and no phase variation over all the frequencies:
\[ \frac{\hat{x}}{x} \approx 1 \]

Thus, we want that
\begin{align*}
                      & |W_1 H_1 \Delta_1 + W_2 H_2 \Delta_2| < \epsilon \quad \forall \omega,  \forall \Delta_i, \|\Delta_i\|_\infty < 1 \\
  \Longleftrightarrow & |W_1 H_1| + |W_2 H_2| < \epsilon \quad \forall \omega
\end{align*}

Which is approximately the same as requiring
\[ \left\| \begin{matrix} W_1 H_1 \\ W_2 H_2 \end{matrix} \right\|_\infty < \epsilon  \]


*How small should we choose $\epsilon$?*

The uncertainty set of the transfer function from $\hat{x}$ to $x$ is bounded in the complex plane by a circle centered on 1 and with a radius equal to $\epsilon$ (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} \le \arcsin (\epsilon) \quad \forall \omega \]

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

Thus, we choose should choose $\epsilon$ so that the maximum phase uncertainty introduced by the sensors is of an acceptable value.

** Design the complementary filters in order to limit the phase and gain uncertainty of the super sensor
Let's say the two sensors dynamics have been identified with the associated uncertainty weights $w_1(s)$ and $w_2(s)$.

If we want to have a maximum phase introduced by the sensors of 20 degrees, we have to design $H_1(s)$ and $H_2(s)$ such that:
\begin{align*}
                      & arcsin\Big( |H_1(j\omega) w_1(j\omega)| + |H_2(j\omega) w_2(j\omega)| \Big) < 20 \text{ deg} \quad \forall\omega \\
  \Longleftrightarrow & |H_1 w_1| + |H_2 w_2| < 0.34 \quad \forall\omega
\end{align*}

We can do that with the $\mathcal{H}_\infty$ synthesis by setting upper bounds on the complementary filters using weights that corresponds to the sensor dynamics uncertainty.

Basically, at frequencies where $|w_i(j\omega)|$ is large, $|H_i(j\omega)|$ has to be made small.
Thus, by limiting the norm of the complementary filters, we can limit the maximum unwanted phase introduced by the uncertainty on the sensors dynamics.

This is of primary importance in order to ensure the stability of the feedback loop using the super sensor signal.


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

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

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

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

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

  w0 = 2*pi;
  alpha = 0.1;

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

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

  figure;
  % Magnitude
  ax1 = subaxis(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 = subaxis(2,1,2);
  hold on;
  set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(H1a, freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(H2a, freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',2); plot(freqs, 180/pi*angle(squeeze(freqresp(H1b, freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',2); plot(freqs, 180/pi*angle(squeeze(freqresp(H2b, freqs, 'Hz'))));
  set(gca,'xscale','log');
  yticks(-180:90:180);
  ylim([-180 180]);
  xlabel('Relative Frequency $\frac{\omega}{\omega_0}$'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
#+end_src

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

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

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

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

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

  figure;
  % Magnitude
  ax1 = subaxis(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 = subaxis(2,1,2);
  hold on;
  set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(H1a*G1 + H2a*G2, freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',2); plot(freqs, 180/pi*angle(squeeze(freqresp(H1b*G1 + H2b*G2, freqs, 'Hz'))));
  set(gca,'xscale','log');
  yticks(-180:90:180);
  ylim([-180 180]);
  xlabel('Relative Frequency $\frac{\omega}{\omega_0}$'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
#+end_src

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

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

** More Complete example with dynamical uncertainty
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}$

*** Dynamical uncertainty of the individual sensors
We define the weights that are used to characterize the dynamic uncertainty of the sensors.
#+begin_src matlab
  omegac = 100*2*pi; G0 = 0.1; Ginf = 10;
  w1 = (Ginf*s/omegac + G0)/(s/omegac + 1);

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

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

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

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

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

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

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

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

*** Synthesis objective
The uncertainty region of the super sensor dynamics is represented by a circle in the complex plane as shown in Fig. [[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 know try to express this requirement in terms of an $\mathcal{H}_\infty$ norm.

Let 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 approximation 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}

On 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$.

*** H-Infinity Synthesis
Let's define $w_\phi(s)$ such that the maximum allowed phase uncertainty $\Delta\phi_\text{max}$ of the super sensor dynamics is $30\text{ deg}$ until frequency $\omega_0 = 500\text{ Hz}$

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

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

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

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

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

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

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

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

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

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

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

The $\mathcal{H}_\infty$ synthesis is performed using the defined weights and the obtained complementary filters are shown in Fig. [[fig:comp_filter_hinf_uncertainty]].

#+begin_src matlab
  P = [W1 -W1;
       0   W2;
       1   0];
#+end_src

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

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

Test bounds:      0.0447 <  gamma  <=      1.3318

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

 Gamma value achieved:     1.0069
#+end_example

#+begin_src matlab
  H1 = 1 - H2;
#+end_src

#+begin_src matlab :exports none
  figure;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

We here just used very wimple weights. We could shape the dynamical uncertainty of the super sensor by using more complex weights.
We could for instance ask for less uncertainty at low frequency.

* Equivalent Super Sensor

The goal here is to find the parameters of a single sensor that would best represent a super sensor.

** Sensor Fusion Architecture
Let consider figure [[fig:sensor_fusion_full]] where two sensors are merged.
The dynamic uncertainty of each sensor is represented by a weight $w_i(s)$, the frequency characteristics each of the sensor noise is represented by the weights $N_i(s)$.
The noise sources $\tilde{n}_i$ are considered to be white noise: $\Phi_{\tilde{n}_i}(\omega) = 1, \ \forall\omega$.

#+name: fig:sensor_fusion_full
#+caption: Sensor fusion architecture ([[./figs/sensor_fusion_full.png][png]], [[./figs/sensor_fusion_full.pdf][pdf]]).
#+RESULTS:
[[file:figs-tikz/sensor_fusion_full.png]]


\begin{align*}
  \hat{x} &= H_1(s) N_1(s) \tilde{n}_1 + H_2(s) N_2(s) \tilde{n}_2 \\
          &\quad \quad + \Big(H_1(s) \big(1 + w_1(s) \Delta_1(s)\big) + H_2(s) \big(1 + w_2(s) \Delta_2(s)\big)\Big) x \\
          &= H_1(s) N_1(s) \tilde{n}_1 + H_2(s) N_2(s) \tilde{n}_2 \\
          &\quad \quad + \big(1 + H_1(s) w_1(s) \Delta_1(s) + H_2(s) w_2(s) \Delta_2(s)\big) x
\end{align*}

To the dynamics of the super sensor is:
\begin{equation}
  \frac{\hat{x}}{x} = 1 + H_1(s) w_1(s) \Delta_1(s) + H_2(s) w_2(s) \Delta_2(s)
\end{equation}

And the noise of the super sensor is:
\begin{equation}
  n_{ss} = H_1(s) N_1(s) \tilde{n}_1 + H_2(s) N_2(s) \tilde{n}_2
\end{equation}

** Equivalent Configuration
We try to determine $w_{ss}(s)$ and $N_{ss}(s)$ such that the sensor on figure [[fig:sensor_fusion_equivalent]] is equivalent to the super sensor of figure [[fig:sensor_fusion_full]].

#+name: fig:sensor_fusion_equivalent
#+caption: Equivalent Super Sensor ([[./figs/sensor_fusion_equivalent.png][png]], [[./figs/sensor_fusion_equivalent.pdf][pdf]]).
#+RESULTS:
[[file:figs-tikz/sensor_fusion_equivalent.png]]

** Model the uncertainty of the super sensor
At each frequency $\omega$, the uncertainty set of the super sensor shown on figure [[fig:sensor_fusion_full]] is a circle centered on $1$ with a radius equal to $|H_1(j\omega) w_1(j\omega)| + |H_2(j\omega) w_2(j\omega)|$ on the complex plane.
The uncertainty set of the sensor shown on figure [[fig:sensor_fusion_equivalent]] is a circle centered on $1$ with a radius equal to $|w_{ss}(j\omega)|$ on the complex plane.

Ideally, we want to find a weight $w_{ss}(s)$ so that:
#+begin_important
  \[ |w_{ss}(j\omega)| = |H_1(j\omega) w_1(j\omega)| + |H_2(j\omega) w_2(j\omega)|, \quad \forall\omega \]
#+end_important

** Model the noise of the super sensor
The PSD of the estimation $\hat{x}$ when $x = 0$ of the configuration shown on figure [[fig:sensor_fusion_full]] is:
\begin{align*}
  \Phi_{\hat{x}}(\omega) &= | H_1(j\omega) N_1(j\omega) |^2 \Phi_{\tilde{n}_1} + | H_2(j\omega) N_2(j\omega) |^2 \Phi_{\tilde{n}_2} \\
                         &= | H_1(j\omega) N_1(j\omega) |^2 + | H_2(j\omega) N_2(j\omega) |^2
\end{align*}

The PSD of the estimation $\hat{x}$ when $x = 0$ of the configuration shown on figure [[fig:sensor_fusion_equivalent]] is:
\begin{align*}
  \Phi_{\hat{x}}(\omega) &= | N_{ss}(j\omega) |^2 \Phi_{\tilde{n}} \\
                         &= | N_{ss}(j\omega) |^2
\end{align*}

Ideally, we want to find a weight $N_{ss}(s)$ such that:
#+begin_important
  \[ |N_{ss}(j\omega)|^2 = | H_1(j\omega) N_1(j\omega) |^2 + | H_2(j\omega) N_2(j\omega) |^2 \quad \forall\omega \]
#+end_important

** First guess
We could choose
\begin{align*}
  w_{ss}(s) &= H_1(s) w_1(s) + H_2(s) w_2(s) \\
  N_{ss}(s) &= H_1(s) N_1(s) + H_2(s) N_2(s)
\end{align*}

But we would have:
\begin{align*}
  |w_{ss}(j\omega)| &= |H_1(j\omega) w_1(j\omega) + H_2(j\omega) w_2(j\omega)|, \quad \forall\omega \\
                    &\neq |H_1(j\omega) w_1(j\omega)| + |H_2(j\omega) w_2(j\omega)|, \quad \forall\omega
\end{align*}
and
\begin{align*}
  |N_{ss}(j\omega)|^2 &= | H_1(j\omega) N_1(j\omega) + H_2(j\omega) N_2(j\omega) |^2 \quad \forall\omega \\
                      &\neq | H_1(j\omega) N_1(j\omega)|^2 + |H_2(j\omega) N_2(j\omega) |^2 \quad \forall\omega \\
\end{align*}

* Optimal And Robust Sensor Fusion in Practice

Here are the steps in order to apply optimal and robust sensor fusion:

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

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

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

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

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

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

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

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

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

*** Weights that represents the noises' PSD

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

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

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

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

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

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

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

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

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

** Estimate the dynamic uncertainty of the sensors

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

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

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

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

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

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

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

* Methods of complementary filter synthesis
** Complementary filters using analytical formula
  :PROPERTIES:
  :header-args:matlab+: :tangle matlab/comp_filters_analytical.m
  :header-args:matlab+: :comments org :mkdirp yes
  :END:
  <<sec:comp_filters_analytical>>

*** Introduction                                                   :ignore:
*** ZIP file containing the data and matlab files                  :ignore:
#+begin_src bash :exports none :results none
  if [ matlab/comp_filters_analytical.m -nt data/comp_filters_analytical.zip ]; then
    cp matlab/comp_filters_analytical.m comp_filters_analytical.m;
    zip data/comp_filters_analytical \
        comp_filters_analytical.m
    rm comp_filters_analytical.m;
  fi
#+end_src

#+begin_note
  All the files (data and Matlab scripts) are accessible [[file:data/comp_filters_analytical.zip][here]].
#+end_note

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

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

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

*** Analytical 1st order complementary filters
First order complementary filters are defined with following equations:
\begin{align}
  H_L(s) = \frac{1}{1 + \frac{s}{\omega_0}}\\
  H_H(s) = \frac{\frac{s}{\omega_0}}{1 + \frac{s}{\omega_0}}
\end{align}

Their bode plot is shown figure [[fig:comp_filter_1st_order]].

#+begin_src matlab
  w0 = 2*pi; % [rad/s]

  Hh1 = (s/w0)/((s/w0)+1);
  Hl1 = 1/((s/w0)+1);
#+end_src

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

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

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

#+NAME: fig:comp_filter_1st_order
#+CAPTION: Bode plot of first order complementary filter ([[./figs/comp_filter_1st_order.png][png]], [[./figs/comp_filter_1st_order.pdf][pdf]])
[[file:figs/comp_filter_1st_order.png]]

*** Second Order Complementary Filters
We here use analytical formula for the complementary filters $H_L$ and $H_H$.

The first two formulas that are used to generate complementary filters are:
\begin{align*}
  H_L(s) &= \frac{(1+\alpha) (\frac{s}{\omega_0})+1}{\left((\frac{s}{\omega_0})+1\right) \left((\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1\right)}\\
  H_H(s) &= \frac{(\frac{s}{\omega_0})^2 \left((\frac{s}{\omega_0})+1+\alpha\right)}{\left((\frac{s}{\omega_0})+1\right) \left((\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1\right)}
\end{align*}
where:
- $\omega_0$ is the blending frequency in rad/s.
- $\alpha$ is used to change the shape of the filters:
  - Small values for $\alpha$ will produce high magnitude of the filters $|H_L(j\omega)|$ and $|H_H(j\omega)|$ near $\omega_0$ but smaller value for $|H_L(j\omega)|$ above $\approx 1.5 \omega_0$ and for $|H_H(j\omega)|$ below $\approx 0.7 \omega_0$
  - A large $\alpha$ will do the opposite

This is illustrated on figure [[fig:comp_filters_param_alpha]].
The slope of those filters at high and low frequencies is $-2$ and $2$ respectively for $H_L$ and $H_H$.

#+begin_src matlab :exports none
  freqs_study = logspace(-2, 2, 10000);
  alphas = [0.1, 1, 10];
  w0 = 2*pi*1;

  figure;
  ax1 = subaxis(2,1,1);
  hold on;
  for i = 1:length(alphas)
    alpha = alphas(i);
    Hh2 = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
    Hl2 = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
    set(gca,'ColorOrderIndex',i);
    plot(freqs_study, abs(squeeze(freqresp(Hh2, freqs_study, 'Hz'))));
    set(gca,'ColorOrderIndex',i);
    plot(freqs_study, abs(squeeze(freqresp(Hl2, freqs_study, 'Hz'))));
  end
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude');
  hold off;
  ylim([1e-3, 20]);
  % Phase
  ax2 = subaxis(2,1,2);
  hold on;
  for i = 1:length(alphas)
    alpha = alphas(i);
    Hh2 = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
    Hl2 = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
    set(gca,'ColorOrderIndex',i);
    plot(freqs_study, 180/pi*angle(squeeze(freqresp(Hh2, freqs_study, 'Hz'))), 'DisplayName', sprintf('$\\alpha = %g$', alpha));
    set(gca,'ColorOrderIndex',i);
    plot(freqs_study, 180/pi*angle(squeeze(freqresp(Hl2, freqs_study, 'Hz'))), 'HandleVisibility', 'off');
  end
  set(gca,'xscale','log');
  yticks(-180:90:180);
  ylim([-180 180]);
  xlabel('Relative Frequency $\frac{\omega}{\omega_0}$'); ylabel('Phase [deg]');
  legend('Location', 'northeast');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs_study(1), freqs_study(end)]);
#+end_src

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

#+NAME: fig:comp_filters_param_alpha
#+CAPTION: Effect of the parameter $\alpha$ on the shape of the generated second order complementary filters ([[./figs/comp_filters_param_alpha.png][png]], [[./figs/comp_filters_param_alpha.pdf][pdf]])
[[file:figs/comp_filters_param_alpha.png]]

We now study the maximum norm of the filters function of the parameter $\alpha$. As we saw that the maximum norm of the filters is important for the robust merging of filters.
#+begin_src matlab :exports none
  alphas = logspace(-2, 2, 100);
  w0 = 2*pi*1;
  infnorms = zeros(size(alphas));

  for i = 1:length(alphas)
    alpha = alphas(i);
    Hh2 = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
    Hl2 = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
    infnorms(i) = norm(Hh2, 'inf');
  end
#+end_src

#+begin_src matlab
  figure;
  plot(alphas, infnorms)
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  xlabel('$\alpha$'); ylabel('$\|H_1\|_\infty$');
#+end_src

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

#+NAME: fig:param_alpha_hinf_norm
#+CAPTION: Evolution of the H-Infinity norm of the complementary filters with the parameter $\alpha$ ([[./figs/param_alpha_hinf_norm.png][png]], [[./figs/param_alpha_hinf_norm.pdf][pdf]])
[[file:figs/param_alpha_hinf_norm.png]]

*** Third Order Complementary Filters
The following formula gives complementary filters with slopes of $-3$ and $3$:
\begin{align*}
  H_L(s) &= \frac{\left(1+(\alpha+1)(\beta+1)\right) (\frac{s}{\omega_0})^2 + (1+\alpha+\beta)(\frac{s}{\omega_0}) + 1}{\left(\frac{s}{\omega_0} + 1\right) \left( (\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1 \right) \left( (\frac{s}{\omega_0})^2 + \beta (\frac{s}{\omega_0}) + 1 \right)}\\
  H_H(s) &= \frac{(\frac{s}{\omega_0})^3 \left( (\frac{s}{\omega_0})^2 + (1+\alpha+\beta) (\frac{s}{\omega_0}) + (1+(\alpha+1)(\beta+1)) \right)}{\left(\frac{s}{\omega_0} + 1\right) \left( (\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1 \right) \left( (\frac{s}{\omega_0})^2 + \beta (\frac{s}{\omega_0}) + 1 \right)}
\end{align*}

The parameters are:
- $\omega_0$ is the blending frequency in rad/s
- $\alpha$ and $\beta$ that are used to change the shape of the filters similarly to the parameter $\alpha$ for the second order complementary filters

The filters are defined below and the result is shown on figure [[fig:complementary_filters_third_order]].

#+begin_src matlab
  alpha = 1;
  beta = 10;
  w0 = 2*pi*14;

  Hh3_ana = (s/w0)^3 * ((s/w0)^2 + (1+alpha+beta)*(s/w0) + (1+(alpha+1)*(beta+1)))/((s/w0 + 1)*((s/w0)^2+alpha*(s/w0)+1)*((s/w0)^2+beta*(s/w0)+1));
  Hl3_ana = ((1+(alpha+1)*(beta+1))*(s/w0)^2 + (1+alpha+beta)*(s/w0) + 1)/((s/w0 + 1)*((s/w0)^2+alpha*(s/w0)+1)*((s/w0)^2+beta*(s/w0)+1));
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(Hl3_ana, freqs, 'Hz'))), '-', 'DisplayName', '$H_L$ - Analytical');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, abs(squeeze(freqresp(Hh3_ana, freqs, 'Hz'))), '-', 'DisplayName', '$H_H$ - Analytical');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  xlim([freqs(1), freqs(end)]);
  ylim([1e-3, 10]);
  xticks([0.1, 1, 10, 100, 1000]);
  legend('location', 'northeast');
#+end_src

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

#+NAME: fig:complementary_filters_third_order
#+CAPTION: Third order complementary filters using the analytical formula ([[./figs/complementary_filters_third_order.png][png]], [[./figs/complementary_filters_third_order.pdf][pdf]])
[[file:figs/complementary_filters_third_order.png]]

** H-Infinity synthesis of complementary filters
  :PROPERTIES:
  :header-args:matlab+: :tangle matlab/h_inf_synthesis_complementary_filters.m
  :header-args:matlab+: :comments org :mkdirp yes
  :END:
  <<sec:h_inf_synthesis_complementary_filters>>

*** Introduction                                                   :ignore:
*** ZIP file containing the data and matlab files                  :ignore:
#+begin_src bash :exports none :results none
  if [ matlab/h_inf_synthesis_complementary_filters.m -nt data/h_inf_synthesis_complementary_filters.zip ]; then
    cp matlab/h_inf_synthesis_complementary_filters.m h_inf_synthesis_complementary_filters.m;
    zip data/h_inf_synthesis_complementary_filters \
        h_inf_synthesis_complementary_filters.m
    rm h_inf_synthesis_complementary_filters.m;
  fi
#+end_src

#+begin_note
  All the files (data and Matlab scripts) are accessible [[file:data/h_inf_synthesis_complementary_filters.zip][here]].
#+end_note

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

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

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

*** Synthesis Architecture
We here synthesize the complementary filters using the $\mathcal{H}_\infty$ synthesis.
The goal is to specify upper bounds on the norms of $H_L$ and $H_H$ while ensuring their complementary property ($H_L + H_H = 1$).

In order to do so, we use the generalized plant shown on figure [[fig:sf_hinf_filters_plant_b]] where $w_L$ and $w_H$ weighting transfer functions that will be used to shape $H_L$ and $H_H$ respectively.

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

The $\mathcal{H}_\infty$ synthesis applied on this generalized plant will give a transfer function $H_L$ (figure [[fig:sf_hinf_filters_b]]) such that the $\mathcal{H}_\infty$ norm of the transfer function from $w$ to $[z_H,\ z_L]$ is less than one:
\[ \left\| \begin{array}{c} H_L w_L \\ (1 - H_L) w_H \end{array} \right\|_\infty < 1 \]

Thus, if the above condition is verified, we can define $H_H = 1 - H_L$ and we have that:
\[ \left\| \begin{array}{c} H_L w_L \\ H_H w_H \end{array} \right\|_\infty < 1 \]
Which is almost (with an maximum error of $\sqrt{2}$) equivalent to:
\begin{align*}
  |H_L| &< \frac{1}{|w_L|}, \quad \forall \omega \\
  |H_H| &< \frac{1}{|w_H|}, \quad \forall \omega
\end{align*}

We then see that $w_L$ and $w_H$ can be used to shape both $H_L$ and $H_H$ while ensuring (by definition of $H_H = 1 - H_L$) their complementary property.

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

*** Weights

#+begin_src matlab
  omegab = 2*pi*9;
  wH = (omegab)^2/(s + omegab*sqrt(1e-5))^2;
  omegab = 2*pi*28;
  wL = (s + omegab/(4.5)^(1/3))^3/(s*(1e-4)^(1/3) + omegab)^3;
#+end_src

#+begin_src matlab :exports none
  figure;
  hold on;
  set(gca,'ColorOrderIndex',1)
  plot(freqs, 1./abs(squeeze(freqresp(wL, freqs, 'Hz'))), '-', 'DisplayName', '$w_L$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, 1./abs(squeeze(freqresp(wH, freqs, 'Hz'))), '-', 'DisplayName', '$w_H$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  xlim([freqs(1), freqs(end)]);
  ylim([1e-3, 10]);
  xticks([0.1, 1, 10, 100, 1000]);
  legend('location', 'northeast');
#+end_src

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

#+NAME: fig:weights_wl_wh
#+CAPTION: Weights on the complementary filters $w_L$ and $w_H$ and the associated performance weights ([[./figs/weights_wl_wh.png][png]], [[./figs/weights_wl_wh.pdf][pdf]])
[[file:figs/weights_wl_wh.png]]

*** H-Infinity Synthesis
We define the generalized plant $P$ on matlab.
#+begin_src matlab
  P = [0   wL;
       wH -wH;
       1   0];
#+end_src

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

#+RESULTS:
#+begin_example
[Hl_hinf, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
Test bounds:      0.0000 <  gamma  <=      1.7285

  gamma    hamx_eig  xinf_eig  hamy_eig   yinf_eig   nrho_xy   p/f
    1.729   4.1e+01   8.4e-12   1.8e-01    0.0e+00    0.0000    p
    0.864   3.9e+01 -5.8e-02#  1.8e-01    0.0e+00    0.0000    f
    1.296   4.0e+01   8.4e-12   1.8e-01    0.0e+00    0.0000    p
    1.080   4.0e+01   8.5e-12   1.8e-01    0.0e+00    0.0000    p
    0.972   3.9e+01 -4.2e-01#  1.8e-01    0.0e+00    0.0000    f
    1.026   4.0e+01   8.5e-12   1.8e-01    0.0e+00    0.0000    p
    0.999   3.9e+01   8.5e-12   1.8e-01    0.0e+00    0.0000    p
    0.986   3.9e+01 -1.2e+00#  1.8e-01    0.0e+00    0.0000    f
    0.993   3.9e+01 -8.2e+00#  1.8e-01    0.0e+00    0.0000    f
    0.996   3.9e+01   8.5e-12   1.8e-01    0.0e+00    0.0000    p
    0.994   3.9e+01   8.5e-12   1.8e-01    0.0e+00    0.0000    p
    0.993   3.9e+01 -3.2e+01#  1.8e-01    0.0e+00    0.0000    f

 Gamma value achieved:     0.9942
#+end_example

We then define the high pass filter $H_H = 1 - H_L$. The bode plot of both $H_L$ and $H_H$ is shown on figure [[fig:hinf_filters_results]].
#+begin_src matlab
  Hh_hinf = 1 - Hl_hinf;
#+end_src

*** Obtained Complementary Filters

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

#+begin_src matlab :exports none
  figure;
  hold on;
  set(gca,'ColorOrderIndex',1)
  plot(freqs, 1./abs(squeeze(freqresp(wL, freqs, 'Hz'))), '--', 'DisplayName', '$w_L$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, 1./abs(squeeze(freqresp(wH, freqs, 'Hz'))), '--', 'DisplayName', '$w_H$');

  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(Hl_hinf, freqs, 'Hz'))), '-', 'DisplayName', '$H_L$ - $\mathcal{H}_\infty$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, abs(squeeze(freqresp(Hh_hinf, freqs, 'Hz'))), '-', 'DisplayName', '$H_H$ - $\mathcal{H}_\infty$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Magnitude');
  hold off;
  xlim([freqs(1), freqs(end)]);
  ylim([1e-3, 10]);
  xticks([0.1, 1, 10, 100, 1000]);
  legend('location', 'northeast');
#+end_src

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

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

** Feedback Control Architecture to generate Complementary Filters
  :PROPERTIES:
  :header-args:matlab+: :tangle matlab/feedback_generate_comp_filters.m
  :header-args:matlab+: :comments org :mkdirp yes
  :END:
  <<sec:feedback_generate_comp_filters>>

*** Introduction                                                   :ignore:
The idea is here to use the fact that in a classical feedback architecture, $S + T = 1$, in order to design complementary filters.

Thus, all the tools that has been developed for classical feedback control can be used for complementary filter design.

*** ZIP file containing the data and matlab files                  :ignore:
#+begin_src bash :exports none :results none
  if [ matlab/feedback_generate_comp_filters.m -nt data/feedback_generate_comp_filters.zip ]; then
    cp matlab/feedback_generate_comp_filters.m feedback_generate_comp_filters.m;
    zip data/feedback_generate_comp_filters \
        feedback_generate_comp_filters.m
    rm feedback_generate_comp_filters.m;
  fi
#+end_src

#+begin_note
  All the files (data and Matlab scripts) are accessible [[file:data/feedback_generate_comp_filters.zip][here]].
#+end_note

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

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

#+begin_src matlab
  freqs = logspace(-2, 2, 1000);
#+end_src

*** Architecture
#+name: fig:complementary_filters_feedback_architecture
#+caption: Architecture used to generate the complementary filters
[[file:figs-tikz/complementary_filters_feedback_architecture.png]]

We have:
\[ y = \underbrace{\frac{L}{L + 1}}_{H_L} y_1 + \underbrace{\frac{1}{L + 1}}_{H_H} y_2 \]
with $H_L + H_H = 1$.

The only thing to design is $L$ such that the complementary filters are stable with the wanted shape.

A simple choice is:
\[ L = \left(\frac{\omega_c}{s}\right)^2 \frac{\frac{s}{\omega_c / \alpha} + 1}{\frac{s}{\omega_c} + \alpha} \]

Which contains two integrator and a lead. $\omega_c$ is used to tune the crossover frequency and $\alpha$ the trade-off "bump" around blending frequency and filtering away from blending frequency.

*** Loop Gain Design
Let's first define the loop gain $L$.
#+begin_src matlab
  wc = 2*pi*1;
  alpha = 2;

  L = (wc/s)^2 * (s/(wc/alpha) + 1)/(s/wc + alpha);
#+end_src

#+begin_src matlab :exports none
  figure;

  ax1 = subplot(2,1,1);
  plot(freqs, abs(squeeze(freqresp(L, freqs, 'Hz'))), '-');
  ylabel('Magnitude');
  set(gca, 'XScale', 'log');
  set(gca, 'YScale', 'log');

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

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

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

#+NAME: fig:loop_gain_bode_plot
#+CAPTION: Bode plot of the loop gain $L$ ([[./figs/loop_gain_bode_plot.png][png]], [[./figs/loop_gain_bode_plot.pdf][pdf]])
[[file:figs/loop_gain_bode_plot.png]]

*** Complementary Filters Obtained
We then compute the resulting low pass and high pass filters.
#+begin_src matlab
  Hl = L/(L + 1);
  Hh = 1/(L + 1);
#+end_src

#+begin_src matlab :exports none
  alphas = [1, 2, 10];

  figure;
  hold on;
  for i = 1:length(alphas)
    alpha = alphas(i);
    L = (wc/s)^2 * (s/(wc/alpha) + 1)/(s/wc + alpha);
    Hl = L/(L + 1);
    Hh = 1/(L + 1);
    set(gca,'ColorOrderIndex',i)
    plot(freqs, abs(squeeze(freqresp(Hl, freqs, 'Hz'))), 'DisplayName', sprintf('$\\alpha = %.0f$', alpha));
    set(gca,'ColorOrderIndex',i)
    plot(freqs, abs(squeeze(freqresp(Hh, freqs, 'Hz'))), 'HandleVisibility', 'off');
  end
  set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Amplitude')
  legend('location', 'northeast');
#+end_src

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

#+NAME: fig:low_pass_high_pass_filters
#+CAPTION: Low pass and High pass filters $H_L$ and $H_H$ for different values of $\alpha$ ([[./figs/low_pass_high_pass_filters.png][png]], [[./figs/low_pass_high_pass_filters.pdf][pdf]])
[[file:figs/low_pass_high_pass_filters.png]]

** Analytical Formula found in the literature
  <<sec:analytical_formula_literature>>

*** Analytical Formula
cite:min15_compl_filter_desig_angle_estim
\begin{align*}
  H_L(s) = \frac{K_p s + K_i}{s^2 + K_p s + K_i} \\
  H_H(s) = \frac{s^2}{s^2 + K_p s + K_i}
\end{align*}

cite:corke04_inert_visual_sensin_system_small_auton_helic
\begin{align*}
  H_L(s) = \frac{1}{s/p + 1} \\
  H_H(s) = \frac{s/p}{s/p + 1}
\end{align*}

cite:jensen13_basic_uas
\begin{align*}
  H_L(s) = \frac{2 \omega_0 s + \omega_0^2}{(s + \omega_0)^2} \\
  H_H(s) = \frac{s^2}{(s + \omega_0)^2}
\end{align*}

\begin{align*}
  H_L(s) = \frac{C(s)}{C(s) + s} \\
  H_H(s) = \frac{s}{C(s) + s}
\end{align*}

cite:shaw90_bandw_enhan_posit_measur_using_measur_accel
\begin{align*}
  H_L(s) = \frac{3 \tau s + 1}{(\tau s + 1)^3} \\
  H_H(s) = \frac{\tau^3 s^3 + 3 \tau^2 s^2}{(\tau s + 1)^3}
\end{align*}

cite:baerveldt97_low_cost_low_weigh_attit
\begin{align*}
  H_L(s) = \frac{2 \tau s + 1}{(\tau s + 1)^2} \\
  H_H(s) = \frac{\tau^2 s^2}{(\tau s + 1)^2}
\end{align*}

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

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

*** Matlab
#+begin_src matlab
  omega0 = 1*2*pi; % [rad/s]
  tau = 1/omega0; % [s]

  % From cite:corke04_inert_visual_sensin_system_small_auton_helic
  HL1 = 1/(s/omega0 + 1); HH1 = s/omega0/(s/omega0 + 1);

  % From cite:jensen13_basic_uas
  HL2 = (2*omega0*s + omega0^2)/(s+omega0)^2; HH2 = s^2/(s+omega0)^2;

  % From cite:shaw90_bandw_enhan_posit_measur_using_measur_accel
  HL3 = (3*tau*s + 1)/(tau*s + 1)^3; HH3 = (tau^3*s^3 + 3*tau^2*s^2)/(tau*s + 1)^3;
#+end_src

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

  figure;
  % Magnitude
  ax1 = subaxis(2,1,1);
  hold on;
  set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(HH1, freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(HL1, freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',2); plot(freqs, abs(squeeze(freqresp(HH2, freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',2); plot(freqs, abs(squeeze(freqresp(HL2, freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',3); plot(freqs, abs(squeeze(freqresp(HH3, freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',3); plot(freqs, abs(squeeze(freqresp(HL3, freqs, 'Hz'))));
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  set(gca, 'XTickLabel',[]);
  ylabel('Magnitude');
  hold off;
  ylim([1e-2 2]);
  % Phase
  ax2 = subaxis(2,1,2);
  hold on;
  set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(HH1, freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(HL1, freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',2); plot(freqs, 180/pi*angle(squeeze(freqresp(HH2, freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',2); plot(freqs, 180/pi*angle(squeeze(freqresp(HL2, freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',3); plot(freqs, 180/pi*angle(squeeze(freqresp(HH3, freqs, 'Hz'))));
  set(gca,'ColorOrderIndex',3); plot(freqs, 180/pi*angle(squeeze(freqresp(HL3, freqs, 'Hz'))));
  set(gca,'xscale','log');
  yticks(-180:90:180);
  ylim([-180 180]);
  xlabel('Relative Frequency $\frac{\omega}{\omega_0}$'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
#+end_src

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

#+NAME: fig:comp_filters_literature
#+CAPTION: Comparison of some complementary filters found in the literature ([[./figs/comp_filters_literature.png][png]], [[./figs/comp_filters_literature.pdf][pdf]])
[[file:figs/comp_filters_literature.png]]

*** Discussion
Analytical Formula found in the literature provides either no parameter for tuning the robustness / performance trade-off.

** Comparison of the different methods of synthesis
  <<sec:discussion>>
The generated complementary filters using $\mathcal{H}_\infty$ and the analytical formulas are very close to each other. However there is some difference to note here:
- the analytical formula provides a very simple way to generate the complementary filters (and thus the controller), they could even be used to tune the controller online using the parameters $\alpha$ and $\omega_0$. However, these formula have the property that $|H_H|$ and $|H_L|$ are symmetrical with the frequency $\omega_0$ which may not be desirable.
- while the $\mathcal{H}_\infty$ synthesis of the complementary filters is not as straightforward as using the analytical formula, it provides a more optimized procedure to obtain the complementary filters

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