diff --git a/matlab/index.org b/matlab/dehaeze21_desig_compl_filte_matlab.org similarity index 94% rename from matlab/index.org rename to matlab/dehaeze21_desig_compl_filte_matlab.org index 42edf83..a5e5bde 100644 --- a/matlab/index.org +++ b/matlab/dehaeze21_desig_compl_filte_matlab.org @@ -1,4 +1,4 @@ -#+TITLE: Complementary Filters Shaping Using $\mathcal{H}_\infty$ Synthesis - Matlab Computation +#+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 @@ -34,23 +34,12 @@ :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 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 -#+begin_note - Add the Matlab code use to obtain the results presented in the paper are accessible [[file:matlab.zip][here]] and presented below. -#+end_note - * H-Infinity synthesis of complementary filters :PROPERTIES: :header-args:matlab+: :tangle matlab/h_inf_synthesis_complementary_filters.m @@ -155,6 +144,7 @@ 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 results :results file replace @@ -176,23 +166,28 @@ W2 = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G #+begin_src matlab :exports none 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]'); ylabel('Magnitude'); +xlabel('Frequency [Hz]', 'FontSize', 10); ylabel('Magnitude', 'FontSize', 10); hold off; xlim([freqs(1), freqs(end)]); -ylim([1e-4, 20]); xticks([0.1, 1, 10, 100, 1000]); -leg = legend('location', 'southeast', 'FontSize', 8); +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 results :results file replace -exportFig('figs/weights_W1_W2.pdf', 'width', 'wide', 'height', 'normal'); +exportFig('figs/weights_W1_W2.pdf', 'width', 'half', 'height', 350); #+end_src #+name: fig:weights_W1_W2 @@ -279,22 +274,21 @@ tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2, 1]); hold on; set(gca,'ColorOrderIndex',1) -plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$w_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$'); +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$'); -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]); -yticks([1e-4, 1e-3, 1e-2, 1e-1, 1, 1e1]); -leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2); +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 @@ -305,16 +299,18 @@ 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'); +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 results :results file replace -exportFig('figs/hinf_filters_results.pdf', 'width', 'wide', 'height', 600); +exportFig('figs/hinf_filters_results.pdf', 'width', 700, 'height', 450); #+end_src #+name: fig:hinf_filters_results @@ -1352,7 +1348,7 @@ P = [ W1 0 1; #+end_src #+begin_src matlab :results output replace :exports both -[L, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'lmi', 'DISPLAY', 'on'); +[L, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on'); #+end_src #+begin_src matlab @@ -1368,20 +1364,80 @@ zpk(H2) #+RESULTS: #+begin_example zpk(H1) + ans = - (s+2.115e07) (s+153.6) (s+4.613) (s^2 + 6.858s + 12.03) - -------------------------------------------------------- - (s+2.117e07) (s^2 + 102.1s + 2732) (s^2 + 69.43s + 3271) + (s+3.842)^3 (s+153.6) (s+1.289e05) + ------------------------------------------------------- + (s+1.29e05) (s^2 + 102.1s + 2733) (s^2 + 69.45s + 3272) +Continuous-time zero/pole/gain model. zpk(H2) + ans = - 20455 (s+3425) (s+3318) (s^2 + 46.58s + 813.2) - -------------------------------------------------------- - (s+2.117e07) (s^2 + 102.1s + 2732) (s^2 + 69.43s + 3271) + 125.61 (s+3358)^2 (s^2 + 46.61s + 813.8) + ------------------------------------------------------- + (s+1.29e05) (s^2 + 102.1s + 2733) (s^2 + 69.45s + 3272) + +Continuous-time zero/pole/gain model. #+end_example +#+begin_src matlab :exports none +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 results :results file replace +exportFig('figs/hinf_filters_results_mixed_sensitivity.pdf', 'width', 700 , 'height', 600); +#+end_src + +#+name: fig:hinf_filters_results_mixed_sensitivity +#+caption: +#+RESULTS: +[[file:figs/hinf_filters_results_mixed_sensitivity.png]] + #+begin_src matlab :exports none freqs = logspace(-2, 4, 1000); @@ -2592,10 +2648,14 @@ bibliographystyle:unsrt bibliography:ref.bib * Functions +:PROPERTIES: +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + ** =generateWF=: Generate Weighting Functions :PROPERTIES: :header-args:matlab+: :tangle matlab/src/generateWF.m -:header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> @@ -2620,7 +2680,7 @@ function [W] = generateWF(args) % - w0 - Frequency at which |W(j w0)| = Gc [rad/s] % % Outputs: -% - W - Generated Weight +% - W - Generated Weighting Function #+end_src *** Optional Parameters @@ -2628,6 +2688,7 @@ function [W] = generateWF(args) :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 @@ -2635,7 +2696,19 @@ arguments 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. +#+name: eq:condition_params_formula +\begin{subequations} + \begin{align} + G_0 < 1 < G_\infty \text{ and } G_0 < G_c < G_\infty \label{eq:cond_formula_1}\\ + G_\infty < 1 < G_0 \text{ and } G_\infty < G_c < G_0 \label{eq:cond_formula_2} + \end{align} +\end{subequations} + +#+begin_src matlab +% Verification of correct relation between G0, Gc and Ginf mustBeBetween(args.G0, args.Gc, args.Ginf); #+end_src @@ -2644,10 +2717,22 @@ mustBeBetween(args.G0, args.Gc, args.Ginf); :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{ + \hfill{} \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 + ... @@ -2660,7 +2745,7 @@ W = (((1/args.w0)*sqrt((1-(args.G0/args.Gc)^(2/args.n))/(1-(args.Gc/args.Ginf)^( :UNNUMBERED: t :END: #+begin_src matlab -% Custom validation function +%% Custom validation function function mustBeBetween(a,b,c) if ~((a > b && b > c) || (c > b && b > a)) eid = 'createWeight:inputError'; @@ -2672,7 +2757,6 @@ function mustBeBetween(a,b,c) ** =generateCF=: Generate Complementary Filters :PROPERTIES: :header-args:matlab+: :tangle matlab/src/generateCF.m -:header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> @@ -2687,7 +2771,7 @@ This Matlab function is accessible [[file:matlab/src/generateCF.m][here]]. function [H1, H2] = generateCF(W1, W2, args) % createWeight - % -% Syntax: [W] = generateCF(args) +% Syntax: [H1, H2] = generateCF(W1, W2, args) % % Inputs: % - W1 - Weighting Function for H1 @@ -2706,6 +2790,7 @@ function [H1, H2] = generateCF(W1, W2, args) :UNNUMBERED: t :END: #+begin_src matlab +%% Argument validation arguments W1 W2 @@ -2719,15 +2804,18 @@ end :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