diff --git a/matlab/index.org b/matlab/index.org index 7335e02..5c76a92 100644 --- a/matlab/index.org +++ b/matlab/index.org @@ -1048,8 +1048,13 @@ The obtained upper bounds on the complementary filters in order to limit the pha #+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]]. +The $\mathcal{H}_\infty$ synthesis architecture used is shown in Fig. [[fig:h_infinity_robust_fusion]]. +#+name: fig:h_infinity_robust_fusion +#+caption: Architecture used for $\mathcal{H}_\infty$ synthesis of complementary filters +[[file:figs/h_infinity_robust_fusion.png]] + +The generalized plant is defined below. #+begin_src matlab P = [W1 -W1; 0 W2; @@ -1089,6 +1094,8 @@ Test bounds: 0.0447 < gamma <= 1.3318 H1 = 1 - H2; #+end_src +The obtained complementary filters are shown in Fig. [[fig:comp_filter_hinf_uncertainty]]. + #+begin_src matlab :exports none figure; @@ -1214,6 +1221,327 @@ We can now compute the uncertainty of the super sensor. The result is shown in F 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. +* Optimal Sensor Fusion - Mixed Synthesis +** Matlab Init :noexport:ignore: +#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) + <> +#+end_src + +#+begin_src matlab :exports none :results silent :noweb yes + <> +#+end_src + +#+begin_src matlab + freqs = logspace(-1, 3, 1000); +#+end_src + +** Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis - Introduction +The goal is to design complementary filters such that: +- the maximum uncertainty on the super sensor is bounded +- the RMS value of the super sensor noise is minimized + +To do so, we can use the Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis. + +The Matlab function for that is =h2hinfsyn= ([[https://fr.mathworks.com/help/robust/ref/h2hinfsyn.html][doc]]). + +** Definition of the weights +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); + + 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 + +We define the noise characteristics of the two sensors by choosing $N_1$ and $N_2$. +#+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 + +We define the generalized plant that will be used for the mixed synthesis. +#+begin_src matlab + P = [W1 -W1; + 0 W2; + N1 -N1; + 0 N2; + 1 0]; + P = ss(P); +#+end_src + +** Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis +#+begin_src matlab :results output replace :exports both + Nmeas = 1; Ncon = 1; Nz2 = 2; + [K,~,normz,~] = h2hinfsyn(P, Nmeas, Ncon, Nz2, [1, 10], 'H2MAX', 2, 'HINFMAX', 2, 'DKMAX', 0, 'TOL', 0.001, 'DISPLAY', 'on'); +#+end_src + +#+RESULTS: +#+begin_example +Nmeas = 1; Ncon = 1; Nz2 = 2; +[K,~,normz,~] = h2hinfsyn(P, Nmeas, Ncon, Nz2, [1, 10], 'H2MAX', 2, 'HINFMAX', 2, 'DKMAX', 0, 'TOL', 0.001, 'DISPLAY', 'on'); + + Optimization of 1.000 * G^2 + 10.000 * H^2 : + ---------------------------------------------- + + Solver for linear objective minimization under LMI constraints + + Iterations : Best objective value so far + + 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12 + 13 + 14 + 15 + 16 + 17 + 18 + 19 + 20 + 21 + 22 + 23 + 24 + 25 + 26 + 27 39.619680 + 28 36.192540 + 29 35.073373 + 30 35.073373 + 31 33.083063 + 32 33.083063 + 33 33.083063 + 34 28.479232 + 35 28.479232 + 36 28.479232 + 37 26.389647 + 38 23.237872 + 39 17.636707 + 40 13.600053 + 41 10.293603 + 42 9.361705 + 43 9.361705 + 44 9.361705 +,* switching to QR + 45 9.361705 + 46 8.891291 + 47 8.891291 + 48 8.891291 + 49 6.892529 + 50 6.892529 + 51 5.425127 + 52 5.425127 + 53 3.499118 + 54 3.499118 + 55 3.249775 + 56 3.249775 + 57 2.893020 + 58 2.893020 +,*** new lower bound: 1.803459 + 59 2.583410 +,*** new lower bound: 1.976383 + 60 2.513031 +,*** new lower bound: 2.027973 + 61 2.441505 +,*** new lower bound: 2.067580 + 62 2.377727 +,*** new lower bound: 2.099478 + 63 2.342173 +,*** new lower bound: 2.125297 + 64 2.315672 +,*** new lower bound: 2.146487 + 65 2.295832 +,*** new lower bound: 2.163923 + 66 2.280942 +,*** new lower bound: 2.239118 + 67 2.265181 + 68 2.261547 + 69 2.259300 +,*** new lower bound: 2.240010 + 70 2.258097 +,*** new lower bound: 2.241899 + 71 2.257082 +,*** new lower bound: 2.243477 + 72 2.256225 +,*** new lower bound: 2.244794 + 73 2.255501 +,*** new lower bound: 2.245895 + 74 2.254596 +,*** new lower bound: 2.246815 + 75 2.254112 +,*** new lower bound: 2.247580 + 76 2.253705 +,*** new lower bound: 2.248219 + 77 2.253196 +,*** new lower bound: 2.251056 + + Result: feasible solution of required accuracy + best objective value: 2.253196 + guaranteed relative accuracy: 9.50e-04 + f-radius saturation: 30.962% of R = 1.00e+08 + + Guaranteed Hinf performance: 1.01e+00 + Guaranteed H2 performance: 2.49e-01 +#+end_example + +#+begin_src matlab + H2 = zpk(K); + H1 = 1-H2; +#+end_src + +#+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 + +** Obtained Super Sensor's noise +#+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 + +#+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 + +** Obtained Super Sensor's Uncertainty +#+begin_src matlab + G1 = 1 + w1*ultidyn('Delta',[1 1]); + G2 = 1 + w2*ultidyn('Delta',[1 1]); +#+end_src + +#+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 + % 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), '--'); + 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 + * Equivalent Super Sensor The goal here is to find the parameters of a single sensor that would best represent a super sensor.