From 665cc66e38b8019402bf8ee1a0cb40c96b04327f Mon Sep 17 00:00:00 2001 From: Thomas Dehaeze Date: Thu, 29 Aug 2019 11:22:03 +0200 Subject: [PATCH] Add H-Infinity synthesis to minimize the super sensor noise RMS --- matlab/index.org | 330 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 330 insertions(+) diff --git a/matlab/index.org b/matlab/index.org index 964afc1..9889f23 100644 --- a/matlab/index.org +++ b/matlab/index.org @@ -250,6 +250,336 @@ The optimal sensor fusion has permitted to reduced the RMS value of the estimati | Sensor 2 | 1.3e-03 | | Optimal Sensor Fusion | 1.5e-04 | +** H-Infinity Synthesis +*** First idea +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 use filters with the highest possible order to approximate that. +However, we may have robustness and implementation problems. + +#+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/4000); + + 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 + n = 5; w0 = 2*pi*10; G0 = 1/10; G1 = 10000; Gc = 1/2; + W1 = (((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; + W2 = (((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 + figure; + hold on; + plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$W_1 = \frac{N_1}{N_2}$'); + plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$W_2 = \frac{N_2}{N_1}$'); + 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 + +#+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.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 + 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',[]); + 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(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 + +#+begin_src matlab :exports none + figure; + hold on; + plot(freqs, abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2, '-', 'DisplayName', '$|N_1|^2$'); + plot(freqs, abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2, '-', 'DisplayName', '$|N_2|^2$'); + plot(freqs, abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2, 'k-', 'DisplayName', '$|N_1H_1|^2+|N_2H_2|^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 + +#+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)], {'Sensor 1', 'Sensor 2', 'Optimal Sensor Fusion'}, {'rms value'}, ' %.1e'); +#+end_src + +#+RESULTS: +| | rms value | +|-----------------------+-----------| +| Sensor 1 | 1.1e-02 | +| Sensor 2 | 1.3e-03 | +| Optimal Sensor Fusion | 3.2e-04 | + +*** Second idea +We have that: +\[ \Phi_{n_{ss}} = \left|H_1\right|^2 \Phi_{n_1} + \left|H_2\right|^2 \Phi_{n_2} \] + +We model the Power Spectral Densities of the two sensors by weighting functions $N_1(s)$ and $N_2(s)$. +\[ \Phi_{n_{ss}} = \left|H_1 N_1\right|^2 + \left|H_2\right N_2|^2 \] + +Then, at frequencies where $|H_1| < |H_2|$ we would like that $|N_1| = 1$ and $|N_2| = 0$ as we discussed before. +Then $|H_1 N_1|^2 + |H_2 N_2|^2 = |H_1 N_1|^2$. + +However, if we design $H_2$ such that when $|N_1| < |N_2|$, we have that: +\[ |H_2 N_2|^2 = \epsilon |H_1 N_1|^2 \] +And we have: +\begin{align*} + \Phi_{n_{ss}} &= \left|H_1 N_1\right|^2 + |H_2 N_2|^2 \\ + &= (1 + \epsilon)\left|H_1 N_1\right|^2 \\ + &\approx \left|H_1 N_1\right|^2 +\end{align*} + +We have then that: +\[ |H_2 N_2| = \sqrt{\epsilon} |H_1 N_1| \] +We suppose that $|H_1| = 1$: +\[ |H_2| = \sqrt{\epsilon} \frac{|N_1|}{|N_2|} \] +We can use that for the maximum magnitude of $|H_2|$ at frequencies where $|N_1| < |N_2|$ + +Similarly, we have that the maximum magnitude of $|H_1|$ at frequencies where $|N_1| > |N_2|$ is: +\[ |H_1| = \sqrt{\epsilon} \frac{|N_2|}{|N_1|} \] + +We could take $\epsilon = 1$, then we increase the noise of the super sensor just by a factor $\sqrt{2}$ over the all bandwidth from the idea case. + +This could be used as weighting functions for the $\mathcal{H}_\infty$ synthesis of the complementary filters. + +#+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/4000); + + 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 + W1 = N1/N2; + W2 = N2/N1; + + W1 = W1/(1 + s/2/pi/1000); % this is added so that it is proper +#+end_src + +#+begin_src matlab :exports none + figure; + hold on; + plot(freqs, abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$W_1 = \frac{N_1}{N_2}$'); + plot(freqs, abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$W_2 = \frac{N_2}{N_1}$'); + 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 + +#+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'); +Test bounds: 0.0000 < gamma <= 2625.0000 + + gamma hamx_eig xinf_eig hamy_eig yinf_eig nrho_xy p/f +2.625e+03 1.8e+01 -2.3e-12 6.3e+00 -1.7e-14 0.0000 p +1.313e+03 1.8e+01 -3.3e-12 6.3e+00 -5.9e-13 0.0000 p + 656.250 1.8e+01 -1.8e-12 6.3e+00 -1.2e-13 0.0000 p + 328.125 1.8e+01 -8.3e-13 6.3e+00 -7.5e-14 0.0000 p + 164.063 1.8e+01 -3.8e-13 6.3e+00 -6.1e-14 0.0000 p + 82.031 1.8e+01 -3.3e-22 6.3e+00 -5.4e-14 0.0000 p + 41.016 1.8e+01 -3.4e-12 6.3e+00 -1.4e-16 0.0000 p + 20.508 1.8e+01 -6.4e-14 6.3e+00 -4.4e-13 0.0000 p + 10.254 1.8e+01 -1.4e-13 6.3e+00 -4.9e-14 0.0000 p + 5.127 1.7e+01 -1.1e-12 6.3e+00 -1.6e-13 0.0000 p + 2.563 1.7e+01 -6.5e-13 6.3e+00 -7.3e-14 0.0000 p + 1.282 1.4e+01 -8.9e+04# 6.3e+00 -1.6e-14 0.0000 f + 1.923 1.6e+01 -4.6e+05# 6.3e+00 -2.1e-13 0.0000 f + 2.243 1.7e+01 -1.9e+06# 6.3e+00 -9.9e-14 0.0000 f + 2.403 1.7e+01 -3.7e-11 6.3e+00 -2.1e-12 0.0000 p + 2.323 1.7e+01 -4.6e+06# 6.3e+00 -5.8e-14 0.0000 f + 2.363 1.7e+01 -1.2e+07# 6.3e+00 -3.3e-14 0.0000 f + 2.383 1.7e+01 -6.8e+07# 6.3e+00 -1.2e-13 0.0000 f + 2.393 1.7e+01 -5.5e-11 6.3e+00 -1.8e-12 0.0000 p + 2.388 1.7e+01 1.3e-22 6.3e+00 -2.5e-15 0.0000 p + 2.386 1.7e+01 -1.5e+08# 6.3e+00 -1.4e-13 0.0000 f + 2.387 1.7e+01 -4.0e+08# 6.3e+00 -1.0e-13 0.0000 f + 2.388 1.7e+01 -2.1e+09# 6.3e+00 -4.2e-13 0.0000 f + + Gamma value achieved: 2.3882 +#+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',[]); + 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(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 + +#+begin_src matlab :exports none + figure; + hold on; + plot(freqs, abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2, '-', 'DisplayName', '$|N_1|^2$'); + plot(freqs, abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2, '-', 'DisplayName', '$|N_2|^2$'); + plot(freqs, abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2, 'k-', 'DisplayName', '$|N_1H_1|^2+|N_2H_2|^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 + +#+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)], {'Sensor 1', 'Sensor 2', 'Optimal Sensor Fusion'}, {'rms value'}, ' %.1e'); +#+end_src + +#+RESULTS: +| | rms value | +|-----------------------+-----------| +| Sensor 1 | 1.1e-02 | +| Sensor 2 | 1.3e-03 | +| Optimal Sensor Fusion | 1.9e-04 | + +*** Conclusion +From the above two experiments with $\mathcal{H}_\infty$ 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) + * Robustness to sensor dynamics uncertainty :PROPERTIES: :header-args:matlab+: :tangle matlab/comp_filter_robustness.m