#+TITLE: A new method of designing complementary filters for sensor fusion using the $\mathcal{H}_\infty$ synthesis - Matlab Computation
:DRAWER:
#+HTML_LINK_HOME: ../index.html
#+HTML_LINK_UP:   ../index.html

#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="https://research.tdehaeze.xyz/css/style.css"/>
#+HTML_HEAD: <script type="text/javascript" src="https://research.tdehaeze.xyz/js/script.js"></script>

#+LaTeX_CLASS: scrreprt
#+LaTeX_HEADER_EXTRA: \input{/home/thomas/Cloud/org-theme/preamble.tex}

#+PROPERTY: header-args:matlab  :session *MATLAB*
#+PROPERTY: header-args:matlab+ :comments none
#+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:

This file is the Matlab file for the paper cite:dehaeze21_new_method_desig_compl_filter.

This document is divided into several sections:
- in section [[#sec:h_inf_synthesis_complementary_filters]], the $\mathcal{H}_\infty$ synthesis is used for generating two complementary filters
- in section [[#sec:three_comp_filters]], a method using the $\mathcal{H}_\infty$ synthesis is proposed to shape three of more complementary filters
- in section [[#sec:comp_filters_ligo]], the $\mathcal{H}_\infty$ synthesis is used and compared with FIR complementary filters used for LIGO
- in section [[#sec:closed_loop_complementary_filters]]

* H-Infinity synthesis of complementary filters
:PROPERTIES:
:header-args:matlab+: :tangle matlab/1_synthesis_complementary_filters.m
:CUSTOM_ID: sec:h_inf_synthesis_complementary_filters
:END:

** Introduction                                                      :ignore:
#+begin_note
  The Matlab file corresponding to this section is accessible [[file:matlab/1_synthesis_complementary_filters.m][here]].
#+end_note

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

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

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

#+begin_src matlab :tangle no
addpath('./matlab');
addpath('./matlab/src');
#+end_src

#+begin_src matlab :eval no
%% Add functions to path
addpath('./src');
#+end_src

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

In order to do so, we use the generalized plant shown on figure [[fig:h_infinity_robst_fusion]] where $W_1(s)$ and $W_2(s)$ are weighting transfer functions that will be used to shape $H_1(s)$ and $H_2(s)$ respectively.

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

The $\mathcal{H}_\infty$ synthesis applied on this generalized plant will give a transfer function $H_2$ (figure [[fig:h_infinity_robst_fusion]]) such that the $\mathcal{H}_\infty$ norm of the transfer function from $w$ to $[z_1,\ z_2]$ is less than one:
\[ \left\| \begin{array}{c} (1 - H_2(s)) W_1(s) \\ H_2(s) W_2(s) \end{array} \right\|_\infty < 1 \]

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

We then see that $W_1(s)$ and $W_2(s)$ can be used to shape both $H_1(s)$ and $H_2(s)$ while ensuring their complementary property by the definition of $H_1(s) = 1 - H_2(s)$.

** Design of Weighting Function
A formula is proposed to help the design of the weighting functions:
\begin{equation}
  W(s) = \left( \frac{
           \frac{1}{\omega_0} \sqrt{\frac{1 - \left(\frac{G_0}{G_c}\right)^{\frac{2}{n}}}{1 - \left(\frac{G_c}{G_\infty}\right)^{\frac{2}{n}}}} s + \left(\frac{G_0}{G_c}\right)^{\frac{1}{n}}
         }{
           \left(\frac{1}{G_\infty}\right)^{\frac{1}{n}} \frac{1}{\omega_0} \sqrt{\frac{1 - \left(\frac{G_0}{G_c}\right)^{\frac{2}{n}}}{1 - \left(\frac{G_c}{G_\infty}\right)^{\frac{2}{n}}}} s + \left(\frac{1}{G_c}\right)^{\frac{1}{n}}
         }\right)^n
\end{equation}

The parameters permits to specify:
- the low frequency gain: $G_0 = lim_{\omega \to 0} |W(j\omega)|$
- the high frequency gain: $G_\infty = lim_{\omega \to \infty} |W(j\omega)|$
- the absolute gain at $\omega_0$: $G_c = |W(j\omega_0)|$
- the absolute slope between high and low frequency: $n$

The general shape of a weighting function generated using the formula is shown in figure [[fig:weight_formula]].

#+begin_src matlab :exports none
%% Weighting Function Design
% Parameters
n = 3; w0 = 2*pi*10; G0 = 1e-3; G1 = 1e1; Gc = 2;

% Formulas
W = (((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 :exports none
%% Magnitude of the weighting function with parameters
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(W, freqs, 'Hz'))), 'k-');

plot([1e-3 1e0], [G0 G0], 'k--', 'LineWidth', 1)
text(1e0, G0, '$\quad G_0$')

plot([1e1 1e3], [G1 G1], 'k--', 'LineWidth', 1)
text(1e1,G1,'$G_{\infty}\quad$','HorizontalAlignment', 'right')

plot([w0/2/pi w0/2/pi], [1 2*Gc], 'k--', 'LineWidth', 1)
text(w0/2/pi,1,'$\omega_c$','VerticalAlignment', 'top', 'HorizontalAlignment', 'center')

plot([w0/2/pi/2  2*w0/2/pi], [Gc Gc], 'k--', 'LineWidth', 1)
text(w0/2/pi/2, Gc, '$G_c \quad$','HorizontalAlignment', 'right')

text(w0/5/pi/2, abs(evalfr(W, j*w0/5)), 'Slope: $n \quad$', 'HorizontalAlignment', 'right')

text(w0/2/pi, abs(evalfr(W, j*w0)), '$\bullet$', 'HorizontalAlignment', 'center')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
xlim([freqs(1), freqs(end)]);
ylim([5e-4, 20]);
yticks([1e-4, 1e-3, 1e-2, 1e-1, 1, 1e1]);
#+end_src

#+begin_src matlab :tangle no :exports none
exportFig('figs-journal/weight_formula.pdf', 'width', 'wide', 'height', 'normal');
#+end_src

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

#+name: fig:weight_formula
#+caption: Gain of the Weighting Function formula
#+RESULTS:
[[file:figs/weight_formula.png]]

** Example

#+begin_src matlab
%% Design of the Weighting Functions
W1 = generateWF('n', 3, 'w0', 2*pi*10, 'G0', 1000, 'Ginf', 1/10, 'Gc', 0.45);
W2 = generateWF('n', 2, 'w0', 2*pi*10, 'G0', 1/10, 'Ginf', 1000, 'Gc', 0.45);
#+end_src

#+begin_src matlab :exports none
%% Plot of the Weighting function magnitude
figure;
tiledlayout(1, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile();
hold on;
set(gca,'ColorOrderIndex',1)
plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$|W_1|^{-1}$');
set(gca,'ColorOrderIndex',2)
plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$|W_2|^{-1}$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]', 'FontSize', 10); ylabel('Magnitude', 'FontSize', 10);
hold off;
xlim([freqs(1), freqs(end)]);
xticks([0.1, 1, 10, 100, 1000]);
ylim([8e-4, 20]);
yticks([1e-3, 1e-2, 1e-1, 1, 1e1]);
yticklabels({'', '$10^{-2}$', '', '$10^0$', ''});
ax1.FontSize = 9;
leg = legend('location', 'south', 'FontSize', 8);
leg.ItemTokenSize(1) = 18;
#+end_src

#+begin_src matlab :tangle no :exports none
exportFig('figs-journal/weights_W1_W2.pdf', 'width', 'half', 'height', 350);
#+end_src

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

#+name: fig:weights_W1_W2
#+caption: Weights on the complementary filters $W_1$ and $W_2$ and the associated performance weights
#+RESULTS:
[[file:figs/weights_W1_W2.png]]

** H-Infinity Synthesis
We define the generalized plant $P$ on matlab.
#+begin_src matlab
%% Generalized Plant
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
%% H-Infinity Synthesis
[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
#+end_src

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

  Test bounds:  0.3223 <=  gamma  <=  1000

    gamma        X>=0        Y>=0       rho(XY)<1    p/f
  1.795e+01     1.4e-07     0.0e+00     1.481e-16     p
  2.406e+00     1.4e-07     0.0e+00     3.604e-15     p
  8.806e-01    -3.1e+02 #  -1.4e-16     7.370e-19     f
  1.456e+00     1.4e-07     0.0e+00     1.499e-18     p
  1.132e+00     1.4e-07     0.0e+00     8.587e-15     p
  9.985e-01     1.4e-07     0.0e+00     2.331e-13     p
  9.377e-01    -7.7e+02 #  -6.6e-17     3.744e-14     f
  9.676e-01    -2.0e+03 #  -5.7e-17     1.046e-13     f
  9.829e-01    -6.6e+03 #  -1.1e-16     2.949e-13     f
  9.907e-01     1.4e-07     0.0e+00     2.374e-19     p
  9.868e-01    -1.6e+04 #  -6.4e-17     5.331e-14     f
  9.887e-01    -5.1e+04 #  -1.5e-17     2.703e-19     f
  9.897e-01     1.4e-07     0.0e+00     1.583e-11     p
  Limiting gains...
  9.897e-01     1.5e-07     0.0e+00     1.183e-12     p
  9.897e-01     6.9e-07     0.0e+00     1.365e-12     p

  Best performance (actual): 0.9897
#+end_example

We then define the high pass filter $H_1 = 1 - H_2$. The bode plot of both $H_1$ and $H_2$ is shown on figure [[fig:hinf_filters_results]].

#+begin_src matlab
%% Define H1 to be the complementary of H2
H1 = 1 - H2;
#+end_src

Or one can just used to =generateCF= Matlab function:
#+begin_src matlab :tangle no :eval no
[H1, H2] = generateCF(W1, W2);
#+end_src

** Obtained Complementary Filters
The obtained complementary filters are shown on figure [[fig:hinf_filters_results]].

#+begin_src matlab :results output replace :exports results :tangle no
zpk(H1)
zpk(H2)
#+end_src

#+RESULTS:
#+begin_example
zpk(H1)
ans =

            (s+1.289e05) (s+153.6) (s+3.842)^3
  -------------------------------------------------------
  (s+1.29e05) (s^2 + 102.1s + 2733) (s^2 + 69.45s + 3272)

zpk(H2)
ans =

         125.61 (s+3358)^2 (s^2 + 46.61s + 813.8)
  -------------------------------------------------------
  (s+1.29e05) (s^2 + 102.1s + 2733) (s^2 + 69.45s + 3272)
#+end_example

#+begin_src matlab :exports none
%% Bode plot of the complementary filters
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

% Magnitude
ax1 = nexttile([2, 1]);
hold on;
set(gca,'ColorOrderIndex',1)
plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$|W_1|^{-1}$');
set(gca,'ColorOrderIndex',2)
plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$|W_2|^{-1}$');

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');
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
ylim([8e-4, 20]);
yticks([1e-3, 1e-2, 1e-1, 1, 1e1]);
yticklabels({'', '$10^{-2}$', '', '$10^0$', ''})
leg = legend('location', 'south', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 18;

% Phase
ax2 = nexttile;
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;
set(gca, 'XScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
yticks([-180:90:180]);
ylim([-180, 200])
yticklabels({'-180', '', '0', '', '180'})

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

#+begin_src matlab :tangle no :exports none
exportFig('figs-journal/hinf_filters_results.pdf', 'width', 700, 'height', 450);
#+end_src

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

#+name: fig:hinf_filters_results
#+caption: Obtained complementary filters using $\mathcal{H}_\infty$ synthesis
#+RESULTS:
[[file:figs/hinf_filters_results.png]]

* Design of complementary filters used in the Active Vibration Isolation System at the LIGO
:PROPERTIES:
:header-args:matlab+: :tangle matlab/2_ligo_complementary_filters.m
:CUSTOM_ID: sec:comp_filters_ligo
:END:

** Introduction                                                      :ignore:
#+begin_note
  The Matlab file corresponding to this section is accessible [[file:matlab/2_ligo_complementary_filters.m][here]].
#+end_note

Let's try to design complementary filters that are corresponding to the complementary filters design for the LIGO and described in cite:hua05_low_ligo.

The FIR complementary filters designed in cite:hua05_low_ligo are of order 512.

** 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
%% Initialize Frequency Vector
freqs = logspace(-3, 0, 1000);
#+end_src

#+begin_src matlab :tangle no
addpath('./matlab');
addpath('./matlab/src');
#+end_src

#+begin_src matlab :eval no
%% Add functions to path
addpath('./src');
#+end_src

** Specifications
The specifications for the filters are:
1. From $0$ to $0.008\text{ Hz}$,the magnitude of the filter’s transfer function should be less than or equal to $8 \times 10^{-3}$
2. From $0.008\text{ Hz}$ to $0.04\text{ Hz}$, it attenuates the input signal proportional to frequency cubed
3. Between $0.04\text{ Hz}$ and $0.1\text{ Hz}$, the magnitude of the transfer function should be less than 3
4. Above $0.1\text{ Hz}$, the maximum of the magnitude of the complement filter should be as close to zero as possible. In our system, we would like to have the magnitude of the complementary filter to be less than $0.1$. As the filters obtained in cite:hua05_low_ligo have a magnitude of $0.045$, we will set that as our requirement

The specifications are translated in upper bounds of the complementary filters are shown on figure [[fig:ligo_specifications]].

#+begin_src matlab :exports none
%% Upper bounds for the complementary filters
figure;
hold on;
set(gca,'ColorOrderIndex',1)
plot([0.0001, 0.008], [8e-3, 8e-3], ':', 'DisplayName', 'Spec. on $H_H$');
set(gca,'ColorOrderIndex',1)
plot([0.008 0.04], [8e-3, 1], ':', 'HandleVisibility', 'off');
set(gca,'ColorOrderIndex',1)
plot([0.04 0.1], [3, 3], ':', 'HandleVisibility', 'off');
set(gca,'ColorOrderIndex',2)
plot([0.1, 10], [0.045, 0.045], ':', 'DisplayName', 'Spec. on $H_L$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
xlim([freqs(1), freqs(end)]);
ylim([1e-4, 10]);
leg = legend('location', 'southeast', 'FontSize', 8);
leg.ItemTokenSize(1) = 18;
#+end_src

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

#+name: fig:ligo_specifications
#+caption: Specification for the LIGO complementary filters
#+RESULTS:
[[file:figs/ligo_specifications.png]]

** FIR Filter
We here try to implement the FIR complementary filter synthesis as explained in cite:hua05_low_ligo.
For that, we use the [[http://cvxr.com/cvx/][CVX matlab Toolbox]].

We setup the CVX toolbox and use the =SeDuMi= solver.
#+begin_src matlab
%% Initialized CVX
cvx_startup;
cvx_solver sedumi;
#+end_src

We define the frequency vectors on which we will constrain the norm of the FIR filter.
#+begin_src matlab
%% Frequency vectors
w1 = 0:4.06e-4:0.008;
w2 = 0.008:4.06e-4:0.04;
w3 = 0.04:8.12e-4:0.1;
w4 = 0.1:8.12e-4:0.83;
#+end_src

We then define the order of the FIR filter.
#+begin_src matlab
%% Filter order
n = 512;
#+end_src

#+begin_src matlab
%% Initialization of filter responses
A1 = [ones(length(w1),1),  cos(kron(w1'.*(2*pi),[1:n-1]))];
A2 = [ones(length(w2),1),  cos(kron(w2'.*(2*pi),[1:n-1]))];
A3 = [ones(length(w3),1),  cos(kron(w3'.*(2*pi),[1:n-1]))];
A4 = [ones(length(w4),1),  cos(kron(w4'.*(2*pi),[1:n-1]))];

B1 = [zeros(length(w1),1), sin(kron(w1'.*(2*pi),[1:n-1]))];
B2 = [zeros(length(w2),1), sin(kron(w2'.*(2*pi),[1:n-1]))];
B3 = [zeros(length(w3),1), sin(kron(w3'.*(2*pi),[1:n-1]))];
B4 = [zeros(length(w4),1), sin(kron(w4'.*(2*pi),[1:n-1]))];
#+end_src

We run the convex optimization.
#+begin_src matlab :results output replace :wrap example
%% Convex optimization
cvx_begin

variable y(n+1,1)

% t
maximize(-y(1))

for i = 1:length(w1)
    norm([0 A1(i,:); 0 B1(i,:)]*y) <= 8e-3;
end

for  i = 1:length(w2)
    norm([0 A2(i,:); 0 B2(i,:)]*y) <= 8e-3*(2*pi*w2(i)/(0.008*2*pi))^3;
end

for i = 1:length(w3)
    norm([0 A3(i,:); 0 B3(i,:)]*y) <= 3;
end

for i = 1:length(w4)
    norm([[1 0]'- [0 A4(i,:); 0 B4(i,:)]*y]) <= y(1);
end

cvx_end

h = y(2:end);
#+end_src

#+RESULTS:
#+begin_example
cvx_begin
variable y(n+1,1)
% t
maximize(-y(1))
for i = 1:length(w1)
    norm([0 A1(i,:); 0 B1(i,:)]*y) <= 8e-3;
end
for  i = 1:length(w2)
    norm([0 A2(i,:); 0 B2(i,:)]*y) <= 8e-3*(2*pi*w2(i)/(0.008*2*pi))^3;
end
for i = 1:length(w3)
    norm([0 A3(i,:); 0 B3(i,:)]*y) <= 3;
end
for i = 1:length(w4)
    norm([[1 0]'- [0 A4(i,:); 0 B4(i,:)]*y]) <= y(1);
end
cvx_end

Calling SeDuMi 1.34: 4291 variables, 1586 equality constraints
   For improved efficiency, SeDuMi is solving the dual problem.
------------------------------------------------------------
SeDuMi 1.34 (beta) by AdvOL, 2005-2008 and Jos F. Sturm, 1998-2003.
Alg = 2: xz-corrector, Adaptive Step-Differentiation, theta = 0.250, beta = 0.500
eqs m = 1586, order n = 3220, dim = 4292, blocks = 1073
nnz(A) = 1100727 + 0, nnz(ADA) = 1364794, nnz(L) = 683190
 it :     b*y       gap    delta  rate   t/tP*  t/tD*   feas cg cg  prec
  0 :            4.11E+02 0.000
  1 :  -2.58E+00 1.25E+02 0.000 0.3049 0.9000 0.9000   4.87  1  1  3.0E+02
  2 :  -2.36E+00 3.90E+01 0.000 0.3118 0.9000 0.9000   1.83  1  1  6.6E+01
  3 :  -1.69E+00 1.31E+01 0.000 0.3354 0.9000 0.9000   1.76  1  1  1.5E+01
  4 :  -8.60E-01 7.10E+00 0.000 0.5424 0.9000 0.9000   2.48  1  1  4.8E+00
  5 :  -4.91E-01 5.44E+00 0.000 0.7661 0.9000 0.9000   3.12  1  1  2.5E+00
  6 :  -2.96E-01 3.88E+00 0.000 0.7140 0.9000 0.9000   2.62  1  1  1.4E+00
  7 :  -1.98E-01 2.82E+00 0.000 0.7271 0.9000 0.9000   2.14  1  1  8.5E-01
  8 :  -1.39E-01 2.00E+00 0.000 0.7092 0.9000 0.9000   1.78  1  1  5.4E-01
  9 :  -9.99E-02 1.30E+00 0.000 0.6494 0.9000 0.9000   1.51  1  1  3.3E-01
 10 :  -7.57E-02 8.03E-01 0.000 0.6175 0.9000 0.9000   1.31  1  1  2.0E-01
 11 :  -5.99E-02 4.22E-01 0.000 0.5257 0.9000 0.9000   1.17  1  1  1.0E-01
 12 :  -5.28E-02 2.45E-01 0.000 0.5808 0.9000 0.9000   1.08  1  1  5.9E-02
 13 :  -4.82E-02 1.28E-01 0.000 0.5218 0.9000 0.9000   1.05  1  1  3.1E-02
 14 :  -4.56E-02 5.65E-02 0.000 0.4417 0.9045 0.9000   1.02  1  1  1.4E-02
 15 :  -4.43E-02 2.41E-02 0.000 0.4265 0.9004 0.9000   1.01  1  1  6.0E-03
 16 :  -4.37E-02 8.90E-03 0.000 0.3690 0.9070 0.9000   1.00  1  1  2.3E-03
 17 :  -4.35E-02 3.24E-03 0.000 0.3641 0.9164 0.9000   1.00  1  1  9.5E-04
 18 :  -4.34E-02 1.55E-03 0.000 0.4788 0.9086 0.9000   1.00  1  1  4.7E-04
 19 :  -4.34E-02 8.77E-04 0.000 0.5653 0.9169 0.9000   1.00  1  1  2.8E-04
 20 :  -4.34E-02 5.05E-04 0.000 0.5754 0.9034 0.9000   1.00  1  1  1.6E-04
 21 :  -4.34E-02 2.94E-04 0.000 0.5829 0.9136 0.9000   1.00  1  1  9.9E-05
 22 :  -4.34E-02 1.63E-04 0.015 0.5548 0.9000 0.0000   1.00  1  1  6.6E-05
 23 :  -4.33E-02 9.42E-05 0.000 0.5774 0.9053 0.9000   1.00  1  1  3.9E-05
 24 :  -4.33E-02 6.27E-05 0.000 0.6658 0.9148 0.9000   1.00  1  1  2.6E-05
 25 :  -4.33E-02 3.75E-05 0.000 0.5972 0.9187 0.9000   1.00  1  1  1.6E-05
 26 :  -4.33E-02 1.89E-05 0.000 0.5041 0.9117 0.9000   1.00  1  1  8.6E-06
 27 :  -4.33E-02 9.72E-06 0.000 0.5149 0.9050 0.9000   1.00  1  1  4.5E-06
 28 :  -4.33E-02 2.94E-06 0.000 0.3021 0.9194 0.9000   1.00  1  1  1.5E-06
 29 :  -4.33E-02 9.73E-07 0.000 0.3312 0.9189 0.9000   1.00  2  2  5.3E-07
 30 :  -4.33E-02 2.82E-07 0.000 0.2895 0.9063 0.9000   1.00  2  2  1.6E-07
 31 :  -4.33E-02 8.05E-08 0.000 0.2859 0.9049 0.9000   1.00  2  2  4.7E-08
 32 :  -4.33E-02 1.43E-08 0.000 0.1772 0.9059 0.9000   1.00  2  2  8.8E-09

iter seconds digits       c*x               b*y
 32     49.4   6.8 -4.3334083581e-02 -4.3334090214e-02
|Ax-b| =   3.7e-09, [Ay-c]_+ =   1.1E-10, |x|=  1.0e+00, |y|=  2.6e+00

Detailed timing (sec)
   Pre          IPM          Post
3.902E+00    4.576E+01    1.035E-02
Max-norms: ||b||=1, ||c|| = 3,
Cholesky |add|=0, |skip| = 0, ||L.L|| = 4.26267.
------------------------------------------------------------
Status: Solved
Optimal value (cvx_optval): -0.0433341
h = y(2:end);
#+end_example

Finally, we compute the filter response over the frequency vector defined and the result is shown on figure [[fig:fir_filter_ligo]] which is very close to the filters obtain in cite:hua05_low_ligo.

#+begin_src matlab
%% Combine the frequency vectors to form the obtained filter
w = [w1 w2 w3 w4];
H = [exp(-j*kron(w'.*2*pi,[0:n-1]))]*h;
#+end_src

#+begin_src matlab :exports none
%% Bode plot of the obtained complementary filters
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

% Magnitude
ax1 = nexttile([2, 1]);
hold on;
set(gca,'ColorOrderIndex',1)
plot(w, abs(1-H), '-', 'DisplayName', '$L_1$');
plot([0.1, 10], [0.045, 0.045], 'k:', 'DisplayName', 'Spec. on $L_1$');

set(gca,'ColorOrderIndex',2)
plot(w, abs(H), '-', 'DisplayName', '$H_1$');
plot([0.0001, 0.008], [8e-3, 8e-3], 'k--', 'DisplayName', 'Spec. on $H_1$');
plot([0.008 0.04], [8e-3, 1],       'k--', 'HandleVisibility', 'off');
plot([0.04 0.1], [3, 3],            'k--', 'HandleVisibility', 'off');

set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
hold off;
ylim([5e-3, 10]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 16;

% Phase
ax2 = nexttile;
hold on;
plot(w, 180/pi*unwrap(angle(1-H)), '-');
plot(w, 180/pi*unwrap(angle(H)), '-');
hold off;
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
set(gca, 'XScale', 'log');
yticks([-360:180:180]); ylim([-380, 200]);

linkaxes([ax1,ax2],'x');
xlim([1e-3, 1]);
#+end_src

#+begin_src matlab :tangle no :exports none
exportFig('figs-journal/fir_filter_ligo.pdf', 'width', 'wide', 'height', 600);
#+end_src

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

#+name: fig:fir_filter_ligo
#+caption: FIR Complementary filters obtain after convex optimization
#+RESULTS:
[[file:figs/fir_filter_ligo.png]]

** Weights
We design weights that will be used for the $\mathcal{H}_\infty$ synthesis of the complementary filters.
These weights will determine the order of the obtained filters.
Here are the requirements on the filters:
- reasonable order
- to be as close as possible to the specified upper bounds
- stable minimum phase

The bode plot of the weights is shown on figure [[fig:ligo_weights]].

#+begin_src matlab :exports none
%% Design of the weight for the high pass filter
w1 = 2*pi*0.008; x1 = 0.35;
w2 = 2*pi*0.04;  x2 = 0.5;
w3 = 2*pi*0.05;  x3 = 0.5;

% Slope of +3 from w1
wH = 0.008*(s^2/w1^2 + 2*x1/w1*s + 1)*(s/w1 + 1);
% Little bump from w2 to w3
wH = wH*(s^2/w2^2 + 2*x2/w2*s + 1)/(s^2/w3^2 + 2*x3/w3*s + 1);
% No Slope at high frequencies
wH = wH/(s^2/w3^2 + 2*x3/w3*s + 1)/(s/w3 + 1);
% Little bump between w2 and w3
w0 = 2*pi*0.045; xi = 0.1; A = 2; n = 1;
wH = wH*((s^2 + 2*w0*xi*A^(1/n)*s + w0^2)/(s^2 + 2*w0*xi*s + w0^2))^n;

wH = 1/wH;
wH = minreal(ss(wH));
#+end_src

#+begin_src matlab :exports none
%% Design of the weight for the low pass filter
n = 20; % Filter order
Rp = 1; % Peak to peak passband ripple
Wp = 2*pi*0.102; % Edge frequency

% Chebyshev Type I filter design
[b,a] = cheby1(n, Rp, Wp, 'high', 's');
wL = 0.04*tf(a, b);

wL = 1/wL;
wL = minreal(ss(wL));
#+end_src

#+begin_src matlab :exports none
%% Magnitude of the designed Weights and initial specifications
figure;
hold on;
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(inv(wL), freqs, 'Hz'))), '-', 'DisplayName', '$|W_L|^{-1}$');
plot([0.1, 10], [0.045, 0.045],     'k:',  'DisplayName', 'Spec. on $L_1$');

set(gca,'ColorOrderIndex',2);
plot(freqs, abs(squeeze(freqresp(inv(wH), freqs, 'Hz'))), '-', 'DisplayName', '$|W_H|^{-1}$');
plot([0.0001, 0.008], [8e-3, 8e-3], 'k--', 'DisplayName', 'Spec. on $H_1$');
plot([0.008 0.04], [8e-3, 1],       'k--', 'HandleVisibility', 'off');
plot([0.04 0.1], [3, 3],            'k--', 'HandleVisibility', 'off');

set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
xlim([freqs(1), freqs(end)]);
ylim([5e-3, 10]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 16;
#+end_src

#+begin_src matlab :tangle no :exports none
exportFig('figs-journal/ligo_weights.pdf', 'width', 'wide', 'height', 'normal');
#+end_src

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

#+name: fig:ligo_weights
#+caption: Weights for the $\mathcal{H}_\infty$ synthesis
#+RESULTS:
[[file:figs/ligo_weights.png]]

** H-Infinity Synthesis
We define the generalized plant as shown on figure [[fig:h_infinity_robst_fusion]].
#+begin_src matlab
%% Generalized plant for the H-infinity Synthesis
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 :wrap example
%% Standard H-Infinity synthesis
[Hl, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
#+end_src

#+RESULTS:
#+begin_example
[Hl, ~, 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.3276 <  gamma  <=      1.8063

  gamma    hamx_eig  xinf_eig  hamy_eig   yinf_eig   nrho_xy   p/f
    1.806   1.4e-02 -1.7e-16   3.6e-03   -4.8e-12    0.0000    p
    1.067   1.3e-02 -4.2e-14   3.6e-03   -1.9e-12    0.0000    p
    0.697   1.3e-02 -3.0e-01#  3.6e-03   -3.5e-11    0.0000    f
    0.882   1.3e-02 -9.5e-01#  3.6e-03   -1.2e-34    0.0000    f
    0.975   1.3e-02 -2.7e+00#  3.6e-03   -1.6e-12    0.0000    f
    1.021   1.3e-02 -8.7e+00#  3.6e-03   -4.5e-16    0.0000    f
    1.044   1.3e-02 -6.5e-14   3.6e-03   -3.0e-15    0.0000    p
    1.032   1.3e-02 -1.8e+01#  3.6e-03    0.0e+00    0.0000    f
    1.038   1.3e-02 -3.8e+01#  3.6e-03    0.0e+00    0.0000    f
    1.041   1.3e-02 -8.3e+01#  3.6e-03   -2.9e-33    0.0000    f
    1.042   1.3e-02 -1.9e+02#  3.6e-03   -3.4e-11    0.0000    f
    1.043   1.3e-02 -5.3e+02#  3.6e-03   -7.5e-13    0.0000    f

 Gamma value achieved:     1.0439
#+end_example

The high pass filter is defined as $H_H = 1 - H_L$.
#+begin_src matlab
%% High pass filter as the complementary of the low pass filter
Hh = 1 - Hl;
#+end_src

#+begin_src matlab :exports none
%% Minimum realization of the filters
Hh = minreal(Hh);
Hl = minreal(Hl);
#+end_src

The size of the filters is shown below.

#+begin_src matlab :exports results :results output replace :wrap example :tangle no
size(Hh), size(Hl)
#+end_src

#+RESULTS:
#+begin_example
size(Hh), size(Hl)
State-space model with 1 outputs, 1 inputs, and 27 states.
State-space model with 1 outputs, 1 inputs, and 27 states.
#+end_example

The bode plot of the obtained filters as shown on figure [[fig:hinf_synthesis_ligo_results]].

#+begin_src matlab :exports none
%% Bode plot of the obtained filters and comparison with the upper bounds
figure;
hold on;
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Hl, freqs, 'Hz'))), '-', 'DisplayName', '$L_1^\prime$');
plot([0.1, 10], [0.045, 0.045],     'k:',  'DisplayName', 'Spec. on $L_1$');

set(gca,'ColorOrderIndex',2);
plot(freqs, abs(squeeze(freqresp(Hh, freqs, 'Hz'))), '-', 'DisplayName', '$H_1^\prime$');
plot([0.0001, 0.008], [8e-3, 8e-3], 'k--', 'DisplayName', 'Spec. on $H_1$');
plot([0.008 0.04], [8e-3, 1],       'k--', 'HandleVisibility', 'off');
plot([0.04 0.1], [3, 3],            'k--', 'HandleVisibility', 'off');

set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
xlim([freqs(1), freqs(end)]);
ylim([5e-3, 10]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 16;
#+end_src

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

#+name: fig:hinf_synthesis_ligo_results
#+caption: Obtained complementary filters using the $\mathcal{H}_\infty$ synthesis
#+RESULTS:
[[file:figs/hinf_synthesis_ligo_results.png]]

** Compare FIR and H-Infinity Filters
Let's now compare the FIR filters designed in cite:hua05_low_ligo and the one obtained with the $\mathcal{H}_\infty$ synthesis on figure [[fig:comp_fir_ligo_hinf]].

#+begin_src matlab :exports none
%% Comparison of the complementary filters obtained with H-infinity and with CVX
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

% Magnitude
ax1 = nexttile([2, 1]);
hold on;
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Hl, freqs, 'Hz'))), '-', ...
     'DisplayName', '$L_1(s)$ - $\mathcal{H}_\infty$');
set(gca,'ColorOrderIndex',2);
plot(freqs, abs(squeeze(freqresp(Hh, freqs, 'Hz'))), '-', ...
     'DisplayName', '$H_1(s)$ - $\mathcal{H}_\infty$');

set(gca,'ColorOrderIndex',1);
plot(w, abs(1-H), '--', ...
     'DisplayName', '$L_1(s)$ - FIR');
set(gca,'ColorOrderIndex',2);
plot(w, abs(H), '--', ...
     'DisplayName', '$H_1(s)$ - FIR');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude');
set(gca, 'XTickLabel',[]);
ylim([5e-3, 10]);
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 16;

% Phase
ax2 = nexttile;
hold on;
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Hl, freqs, 'Hz')))), '-');
set(gca,'ColorOrderIndex',2);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Hh, freqs, 'Hz')))), '-');

set(gca,'ColorOrderIndex',1);
plot(w, 180/pi*unwrap(angle(1-H)), '--');
set(gca,'ColorOrderIndex',2);
plot(w, 180/pi*unwrap(angle(H)), '--');
set(gca, 'XScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks([-360:180:180]); ylim([-380, 200]);

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

#+begin_src matlab :tangle no :exports none
exportFig('figs-journal/comp_fir_ligo_hinf.pdf', 'width', 'wide', 'height', 600);
#+end_src

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

#+name: fig:comp_fir_ligo_hinf
#+caption: Comparison between the FIR filters developped for LIGO and the $\mathcal{H}_\infty$ complementary filters
#+RESULTS:
[[file:figs/comp_fir_ligo_hinf.png]]

* "Closed-Loop" complementary filters
:PROPERTIES:
:header-args:matlab+: :tangle matlab/3_closed_loop_complementary_filters.m
:CUSTOM_ID: sec:closed_loop_complementary_filters
:END:

** Introduction                                                      :ignore:
#+begin_note
  The Matlab file corresponding to this section is accessible [[file:matlab/3_closed_loop_complementary_filters.m][here]].
#+end_note

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

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

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

#+begin_src matlab :tangle no
addpath('./matlab');
addpath('./matlab/src');
#+end_src

#+begin_src matlab :eval no
%% Add functions to path
addpath('./src');
#+end_src

** Using Feedback architecture
#+begin_src matlab
%% Design of the Weighting Functions
W1 = generateWF('n', 3, 'w0', 2*pi*10, 'G0', 1000, 'Ginf', 1/10, 'Gc', 0.45);
W2 = generateWF('n', 2, 'w0', 2*pi*10, 'G0', 1/10, 'Ginf', 1000, 'Gc', 0.45);
#+end_src

Let's first synthesize $H_1(s)$:
#+begin_src matlab
%% Generalized plant for "closed-loop" complementary filter synthesis
P = [ W1 0   1;
     -W1 W2 -1];
#+end_src

#+begin_src matlab :results output replace :exports both
%% Standard H-Infinity Synthesis
[L, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
#+end_src


#+begin_src matlab
%% Complementary filters
H1 = inv(1 + L);
H2 = 1 - H1;
#+end_src

#+begin_src matlab :results output replace :exports results :tangle no
zpk(H1)
zpk(H2)
#+end_src

#+RESULTS:
#+begin_example
zpk(H1) =
            (s+3.842)^3 (s+153.6) (s+1.289e05)
  -------------------------------------------------------
  (s+1.29e05) (s^2 + 102.1s + 2733) (s^2 + 69.45s + 3272)

zpk(H2) =
         125.61 (s+3358)^2 (s^2 + 46.61s + 813.8)
  -------------------------------------------------------
  (s+1.29e05) (s^2 + 102.1s + 2733) (s^2 + 69.45s + 3272)
#+end_example

#+begin_src matlab :exports none
%% Bode plot of the obtained Complementary filters with upper-bounds
freqs = logspace(-1, 3, 1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

% Magnitude
ax1 = nexttile([2, 1]);
hold on;
set(gca,'ColorOrderIndex',1)
plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$|W_1|^{-1}$');
set(gca,'ColorOrderIndex',2)
plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$|W_2|^{-1}$');

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$');

plot(freqs, abs(squeeze(freqresp(L, freqs, 'Hz'))), 'k--', 'DisplayName', '$|L|$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
ylim([1e-3, 1e3]);
yticks([1e-3, 1e-2, 1e-1, 1, 1e1, 1e2, 1e3]);
yticklabels({'', '$10^{-2}$', '', '$10^0$', '', '$10^2$', ''});
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 3);
leg.ItemTokenSize(1) = 18;

% Phase
ax2 = nexttile;
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;
set(gca, 'XScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
yticks([-180:90:180]);
ylim([-180, 200])
yticklabels({'-180', '', '0', '', '180'})

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

#+begin_src matlab :tangle no :exports none
exportFig('figs-journal/hinf_filters_results_mixed_sensitivity.pdf', 'width', 700, 'height', 600);
#+end_src

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

#+name: fig:hinf_filters_results_mixed_sensitivity
#+caption:
#+RESULTS:
[[file:figs/hinf_filters_results_mixed_sensitivity.png]]

* Synthesis of three complementary filters
:PROPERTIES:
:header-args:matlab+: :tangle matlab/4_three_complementary_filters.m
:header-args:matlab+: :comments org :mkdirp yes
:CUSTOM_ID: sec:three_comp_filters
:END:

** Introduction                                                      :ignore:
#+begin_note
  The Matlab file corresponding to this section is accessible [[file:matlab/4_three_complementary_filters.m][here]].
#+end_note

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

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

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

#+begin_src matlab :tangle no
addpath('./matlab');
addpath('./matlab/src');
#+end_src

#+begin_src matlab :eval no
addpath('./src');
#+end_src

** Theory
We want:
\begin{align*}
  & |H_1(j\omega)| < 1/|W_1(j\omega)|, \quad \forall\omega\\
  & |H_2(j\omega)| < 1/|W_2(j\omega)|, \quad \forall\omega\\
  & |H_3(j\omega)| < 1/|W_3(j\omega)|, \quad \forall\omega\\
  & H_1(s) + H_2(s) + H_3(s) = 1
\end{align*}

For that, we use the $\mathcal{H}_\infty$ synthesis with the architecture shown on figure [[fig:comp_filter_three_hinf]].

#+name: fig:comp_filter_three_hinf
#+caption: Generalized architecture for generating 3 complementary filters
[[file:figs-journal/comp_filter_three_hinf_fb.png]]

The $\mathcal{H}_\infty$ objective is:
\begin{align*}
  & |(1 - H_2(j\omega) - H_3(j\omega)) W_1(j\omega)| < 1, \quad \forall\omega\\
  & |H_2(j\omega) W_2(j\omega)| < 1, \quad \forall\omega\\
  & |H_3(j\omega) W_3(j\omega)| < 1, \quad \forall\omega\\
\end{align*}

And thus if we choose $H_1 = 1 - H_2 - H_3$ we have solved the problem.

** Weights
First we define the weights.
#+begin_src matlab
%% Design of the Weighting Functions
W1 = generateWF('n', 2, 'w0', 2*pi*1, 'G0', 1/10, 'Ginf', 1000, 'Gc', 0.5);
W2 = 0.22*(1 + s/2/pi/1)^2/(sqrt(1e-4) + s/2/pi/1)^2*(1 + s/2/pi/10)^2/(1 + s/2/pi/1000)^2;
W3 = generateWF('n', 3, 'w0', 2*pi*10, 'G0', 1000, 'Ginf', 1/10, 'Gc', 0.5);
#+end_src

#+begin_src matlab :exports none
%% Inverse magnitude of the weighting functions
figure;
hold on;
set(gca,'ColorOrderIndex',1)
plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$|W_1|^{-1}$');
set(gca,'ColorOrderIndex',2)
plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$|W_2|^{-1}$');
set(gca,'ColorOrderIndex',3)
plot(freqs, 1./abs(squeeze(freqresp(W3, freqs, 'Hz'))), '--', 'DisplayName', '$|W_3|^{-1}$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
xlim([freqs(1), freqs(end)]); ylim([2e-4, 1.3e1])
leg = legend('location', 'northeast', 'FontSize', 8);
leg.ItemTokenSize(1) = 18;
#+end_src

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

#+name: fig:three_weighting_functions
#+caption: Three weighting functions used for the $\mathcal{H}_\infty$ synthesis of the complementary filters
#+RESULTS:
[[file:figs/three_weighting_functions.png]]

** H-Infinity Synthesis
Then we create the generalized plant =P=.
#+begin_src matlab
%% Generalized plant for the synthesis of 3 complementary filters
P = [W1 -W1 -W1;
     0   W2  0 ;
     0   0   W3;
     1   0   0];
#+end_src

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

#+RESULTS:
#+begin_example
[H, ~, gamma, ~] = hinfsyn(P, 1, 2,'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  <=   1050.0000

  gamma    hamx_eig  xinf_eig  hamy_eig   yinf_eig   nrho_xy   p/f
1.050e+03   3.2e+00   4.5e-13   6.3e-02   -1.2e-11    0.0000    p
  525.050   3.2e+00   1.3e-13   6.3e-02    0.0e+00    0.0000    p
  262.575   3.2e+00   2.1e-12   6.3e-02   -1.5e-13    0.0000    p
  131.337   3.2e+00   1.1e-12   6.3e-02   -7.2e-29    0.0000    p
   65.719   3.2e+00   2.0e-12   6.3e-02    0.0e+00    0.0000    p
   32.909   3.2e+00   7.4e-13   6.3e-02   -5.9e-13    0.0000    p
   16.505   3.2e+00   1.4e-12   6.3e-02    0.0e+00    0.0000    p
    8.302   3.2e+00   1.6e-12   6.3e-02    0.0e+00    0.0000    p
    4.201   3.2e+00   1.6e-12   6.3e-02    0.0e+00    0.0000    p
    2.151   3.2e+00   1.6e-12   6.3e-02    0.0e+00    0.0000    p
    1.125   3.2e+00   2.8e-12   6.3e-02    0.0e+00    0.0000    p
    0.613   3.0e+00 -2.5e+03#  6.3e-02    0.0e+00    0.0000    f
    0.869   3.1e+00 -2.9e+01#  6.3e-02    0.0e+00    0.0000    f
    0.997   3.2e+00   1.9e-12   6.3e-02    0.0e+00    0.0000    p
    0.933   3.1e+00 -6.9e+02#  6.3e-02    0.0e+00    0.0000    f
    0.965   3.1e+00 -3.0e+03#  6.3e-02    0.0e+00    0.0000    f
    0.981   3.1e+00 -8.6e+03#  6.3e-02    0.0e+00    0.0000    f
    0.989   3.2e+00 -2.7e+04#  6.3e-02    0.0e+00    0.0000    f
    0.993   3.2e+00 -5.7e+05#  6.3e-02    0.0e+00    0.0000    f
    0.995   3.2e+00   2.2e-12   6.3e-02    0.0e+00    0.0000    p
    0.994   3.2e+00   1.6e-12   6.3e-02    0.0e+00    0.0000    p
    0.994   3.2e+00   1.0e-12   6.3e-02    0.0e+00    0.0000    p

 Gamma value achieved:     0.9936
#+end_example

** Obtained Complementary Filters
The obtained filters are:
#+begin_src matlab
%%
H2 = tf(H(1));
H3 = tf(H(2));
H1 = 1 - H2 - H3;
#+end_src

#+begin_src matlab :exports none
%% Bode plot of the obtained complementary filters
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');

% Magnitude
ax1 = nexttile([2, 1]);
hold on;
set(gca,'ColorOrderIndex',1)
plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$|W_1|^{-1}$');
set(gca,'ColorOrderIndex',2)
plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$|W_2|^{-1}$');
set(gca,'ColorOrderIndex',3)
plot(freqs, 1./abs(squeeze(freqresp(W3, freqs, 'Hz'))), '--', 'DisplayName', '$|W_3|^{-1}$');
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$');
set(gca,'ColorOrderIndex',3)
plot(freqs, abs(squeeze(freqresp(H3, freqs, 'Hz'))), '-', 'DisplayName', '$H_3$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude');
set(gca, 'XTickLabel',[]);
ylim([1e-4, 20]);
leg = legend('location', 'northeast', 'FontSize', 8);
leg.ItemTokenSize(1) = 18;

% Phase
ax2 = nexttile;
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'))));
set(gca,'ColorOrderIndex',3)
plot(freqs, 180/pi*phase(squeeze(freqresp(H3, freqs, 'Hz'))));
hold off;
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
set(gca, 'XScale', 'log');
yticks([-180:90:180]); ylim([-220, 220]);

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

#+begin_src matlab :tangle no :exports none
exportFig('figs-journal/three_complementary_filters_results.pdf', 'width', 'wide', 'height', 600);
#+end_src

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

#+name: fig:three_complementary_filters_results
#+caption: The three complementary filters obtained after $\mathcal{H}_\infty$ synthesis
#+RESULTS:
[[file:figs/three_complementary_filters_results.png]]

* Bibliography                                                        :ignore:
:PROPERTIES:
:CUSTOM_ID: sec:bibliography
:END:
bibliographystyle:unsrt
bibliography:ref.bib

* Functions
:PROPERTIES:
:header-args:matlab+: :comments none :mkdirp yes :eval no
:CUSTOM_ID: sec:functions
:END:

** =generateWF=: Generate Weighting Functions
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/generateWF.m
:END:
<<sec:generateWF>>

This Matlab function is accessible [[file:matlab/src/generateWF.m][here]].

*** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
function [W] = generateWF(args)
% createWeight -
%
% Syntax: [W] = generateWeight(args)
%
% Inputs:
%    - n  - Weight Order (integer)
%    - G0 - Low frequency Gain
%    - G1 - High frequency Gain
%    - Gc - Gain of the weight at frequency w0
%    - w0 - Frequency at which |W(j w0)| = Gc [rad/s]
%
% Outputs:
%    - W - Generated Weighting Function
#+end_src

*** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
%% Argument validation
arguments
    args.n    (1,1) double {mustBeInteger, mustBePositive} = 1
    args.G0   (1,1) double {mustBeNumeric, mustBePositive} = 0.1
    args.Ginf (1,1) double {mustBeNumeric, mustBePositive} = 10
    args.Gc   (1,1) double {mustBeNumeric, mustBePositive} = 1
    args.w0   (1,1) double {mustBeNumeric, mustBePositive} = 1
end
#+end_src

Verification that the parameters $G_0$, $G_c$ and $G_\infty$ are satisfy condition eqref:eq:cond_formula_1 or eqref:eq:cond_formula_2.
\begin{equation}
  G_0 < 1 < G_\infty \text{ and } G_0 < G_c < G_\infty \label{eq:cond_formula_1}
\end{equation}
\begin{equation}
  G_\infty < 1 < G_0 \text{ and } G_\infty < G_c < G_0 \label{eq:cond_formula_2}
\end{equation}

#+begin_src matlab
% Verification of correct relation between G0, Gc and Ginf
mustBeBetween(args.G0, args.Gc, args.Ginf);
#+end_src

*** Generate the Weighting function
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
%% Initialize the Laplace variable
s = zpk('s');
#+end_src

The weighting function formula use is:
#+name: eq:weight_formula
\begin{equation}
  W(s) = \left( \frac{
           \frac{1}{\omega_c} \sqrt{\frac{1 - \left(\frac{G_0}{G_c}\right)^{\frac{2}{n}}}{1 - \left(\frac{G_c}{G_\infty}\right)^{\frac{2}{n}}}} s + \left(\frac{G_0}{G_c}\right)^{\frac{1}{n}}
         }{
           \left(\frac{1}{G_\infty}\right)^{\frac{1}{n}} \frac{1}{\omega_c} \sqrt{\frac{1 - \left(\frac{G_0}{G_c}\right)^{\frac{2}{n}}}{1 - \left(\frac{G_c}{G_\infty}\right)^{\frac{2}{n}}}} s + \left(\frac{1}{G_c}\right)^{\frac{1}{n}}
         }\right)^n
\end{equation}

#+begin_src matlab
%% Create the weighting function according to formula
W = (((1/args.w0)*sqrt((1-(args.G0/args.Gc)^(2/args.n))/(1-(args.Gc/args.Ginf)^(2/args.n)))*s + ...
      (args.G0/args.Gc)^(1/args.n))/...
     ((1/args.Ginf)^(1/args.n)*(1/args.w0)*sqrt((1-(args.G0/args.Gc)^(2/args.n))/(1-(args.Gc/args.Ginf)^(2/args.n)))*s + ...
      (1/args.Gc)^(1/args.n)))^args.n;
#+end_src


*** Verification of the $G_0$, $G_c$ and $G_\infty$ gains
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
%% Custom validation function
function mustBeBetween(a,b,c)
    if ~((a > b && b > c) || (c > b && b > a))
        eid = 'createWeight:inputError';
        msg = 'Gc should be between G0 and Ginf.';
        throwAsCaller(MException(eid,msg))
    end
#+end_src

** =generateCF=: Generate Complementary Filters
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/generateCF.m
:END:
<<sec:generateCF>>

This Matlab function is accessible [[file:matlab/src/generateCF.m][here]].

*** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
function [H1, H2] = generateCF(W1, W2, args)
% createWeight -
%
% Syntax: [H1, H2] = generateCF(W1, W2, args)
%
% Inputs:
%    - W1 - Weighting Function for H1
%    - W2 - Weighting Function for H2
%    - args:
%      - method  - H-Infinity solver ('lmi' or 'ric')
%      - display - Display synthesis results ('on' or 'off')
%
% Outputs:
%    - H1 - Generated H1 Filter
%    - H2 - Generated H2 Filter
#+end_src

*** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
%% Argument validation
arguments
    W1
    W2
    args.method  char {mustBeMember(args.method,{'lmi', 'ric'})} = 'ric'
    args.display char {mustBeMember(args.display,{'on', 'off'})} = 'on'
end
#+end_src

*** H-Infinity Synthesis
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
%% The generalized plant is defined
P = [W1 -W1;
     0   W2;
     1   0];
#+end_src

#+begin_src matlab :results output replace :exports both
%% The standard H-infinity synthesis is performed
[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', args.method, 'DISPLAY', args.display);
#+end_src

#+begin_src matlab
%% H1 is defined as the complementary of H2
H1 = 1 - H2;
#+end_src
