tdehaeze/dehaeze20_optim_robus_compl_filte
1
0
Fork 0
Code Issues Pull Requests Releases Activity

Update tangled code

Browse Source
This commit is contained in:
Thomas Dehaeze
2020-10-05 11:47:09 +02:00
parent 5f1f33144e
commit 41f51e423c
4 changed files with 416 additions and 1170 deletions
Download Patch File Download Diff File Expand all files Collapse all files

442
matlab/matlab/comp_filter_robustness.m
View File

@@ -4,226 +4,151 @@ clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
% Dynamical uncertainty of the individual sensors
% Let say 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}$
addpath('src');
load('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
% We define the weights that are used to characterize the dynamic uncertainty of the sensors.
% Weighting Function used to bound the super sensor uncertainty
% <<sec:weight_uncertainty>>
% $W_u(s)$ is defined such that the super sensor phase uncertainty is less than 10 degrees below 100Hz eqref:eq:phase_uncertainy_bound_low_freq and is less than 180 degrees below 400Hz eqref:eq:phase_uncertainty_max.
% \begin{align}
% \frac{1}{|W_u(j\omega)|} &< \sin\left(10 \frac{\pi}{180}\right), \quad \omega < 100\,\text{Hz} \label{eq:phase_uncertainy_bound_low_freq} \\
% \frac{1}{|W_u(j 2 \pi 400)|} &< 1 \label{eq:phase_uncertainty_max}
% \end{align}
% The uncertainty bounds of the two individual sensor as well as the wanted maximum uncertainty bounds of the super sensor are shown in Figure [[fig:weight_uncertainty_bounds_Wu]].
freqs = logspace(-1, 3, 1000);
Dphi = 10; % [deg]
omegac = 100*2*pi; G0 = 0.1; Ginf = 10;
w1 = (Ginf*s/omegac + G0)/(s/omegac + 1);
Wu = createWeight('n', 2, 'w0', 2*pi*4e2, 'G0', 1/sin(Dphi*pi/180), 'G1', 1/4, 'Gc', 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);
% Wu is saved for further use
save('./mat/Wu.mat', 'Wu');
% 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.
G1 = 1 + w1*ultidyn('Delta',[1 1]);
G2 = 1 + w2*ultidyn('Delta',[1 1]);
% Few random samples of the sensor dynamics are computed
G1s = usample(G1, 10);
G2s = usample(G2, 10);
% 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;
Dphi_Wu = 180/pi*asin(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))));
Dphi_Wu(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))) > 1) = 360;
figure;
% Magnitude
ax1 = subplot(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
plotMagUncertainty(W1, freqs, 'color_i', 1, 'DisplayName', '$1 + W_1 \Delta_1$');
plotMagUncertainty(W2, freqs, 'color_i', 2, 'DisplayName', '$1 + W_2 \Delta_2$');
plot(freqs, 1 + abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'k--', ...
'DisplayName', '$1 + W_u^{-1} \Delta$')
plot(freqs, 1 - abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'k--', ...
'HandleVisibility', 'off')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');
ylim([1e-1, 10]);
ylim([1e-2, 1e1]);
legend('location', 'southeast', 'FontSize', 8);
hold off;
% Phase
ax2 = subplot(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
plotPhaseUncertainty(W1, freqs, 'color_i', 1);
plotPhaseUncertainty(W2, freqs, 'color_i', 2);
plot(freqs, Dphi_Wu, 'k--');
plot(freqs, -Dphi_Wu, 'k--');
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
% Weighting Function used to bound the super sensor uncertainty
% Let's define $w_\phi(s)$ in order to bound the maximum allowed phase uncertainty $\Delta\phi_\text{max}$ of the super sensor dynamics.
% The magnitude $|w_\phi(j\omega)|$ is shown in Fig. [[fig:magnitude_wphi]] and the corresponding maximum allowed phase uncertainty of the super sensor dynamics of shown in Fig. [[fig:maximum_wanted_phase_uncertainty]].
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;
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(wphi, freqs, 'Hz'))), '-', 'DisplayName', '$w_\phi(s)$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
xlim([freqs(1), freqs(end)]);
legend('location', 'northeast');
% #+NAME: fig:magnitude_wphi
% #+CAPTION: Magnitude of the weght $w_\phi(s)$ that is used to bound the uncertainty of the super sensor ([[./figs/magnitude_wphi.png][png]], [[./figs/magnitude_wphi.pdf][pdf]])
% [[file:figs/magnitude_wphi.png]]
% 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;
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);
% #+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]].
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');
% $\mathcal{H}_\infty$ Synthesis
% The $\mathcal{H}_\infty$ synthesis architecture used for the complementary filters is shown in Fig. [[fig:h_infinity_robust_fusion]].
% <<sec:Hinfinity_synthesis>>
% The generalized plant $P_{\mathcal{H}_\infty}$ used for the $\mathcal{H}_\infty$ Synthesis of the complementary filters is shown in Figure [[fig:h_infinity_robust_fusion]] and is described by Equation eqref:eq:Hinf_generalized_plant.
% #+name: fig:h_infinity_robust_fusion
% #+caption: Architecture used for $\mathcal{H}_\infty$ synthesis of complementary filters
% [[file:figs-tikz/h_infinity_robust_fusion.png]]
% \begin{equation} \label{eq:Hinf_generalized_plant}
% \begin{pmatrix}
% z_1 \\ z_2 \\ v
% \end{pmatrix} = \underbrace{\begin{bmatrix}
% W_u W_1 & -W_u W_1 \\
% 0 & W_u W_2 \\
% 1 & 0
% \end{bmatrix}}_{P_{\mathcal{H}_\infty}} \begin{pmatrix}
% w \\ u
% \end{pmatrix}
% \end{equation}
% The generalized plant is defined below.
P = [W1 -W1;
0 W2;
1 0];
P = [Wu*W1 -Wu*W1;
0 Wu*W2;
1 0];
% And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command.
% And the $\mathcal{H}_\infty$ synthesis is performed using the =hinfsyn= command.
[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
H2 = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'DISPLAY', 'on');
% #+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.7071 <= gamma <= 1.291
% Test bounds: 0.0447 < gamma <= 1.3318
% gamma X>=0 Y>=0 rho(XY)<1 p/f
% 9.554e-01 0.0e+00 0.0e+00 3.529e-16 p
% 8.219e-01 0.0e+00 0.0e+00 5.204e-16 p
% 7.624e-01 3.8e-17 0.0e+00 1.955e-15 p
% 7.342e-01 0.0e+00 0.0e+00 5.612e-16 p
% 7.205e-01 0.0e+00 0.0e+00 7.184e-16 p
% 7.138e-01 0.0e+00 0.0e+00 0.000e+00 p
% 7.104e-01 4.1e-16 0.0e+00 6.749e-15 p
% 7.088e-01 0.0e+00 0.0e+00 2.794e-15 p
% 7.079e-01 0.0e+00 0.0e+00 6.503e-16 p
% 7.075e-01 0.0e+00 0.0e+00 4.302e-15 p
% 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
% Best performance (actual): 0.7071
% #+end_example
% And $H_1(s)$ is defined as the complementary of $H_2(s)$.
% The $\mathcal{H}_\infty$ is successful as the $\mathcal{H}_\infty$ norm of the "closed loop" transfer function from $(w)$ to $(z_1,\ z_2)$ is less than one.
% $H_1(s)$ is then defined as the complementary of $H_2(s)$.
H1 = 1 - H2;
% Complementary filters are saved for further analysis
save('./mat/Hinf_filters.mat', 'H2', 'H1');
% The obtained complementary filters are shown in Fig. [[fig:comp_filter_hinf_uncertainty]].
% The obtained complementary filters as well as the wanted upper bounds are shown in Figure [[fig:hinf_comp_filters]].
figure;
ax1 = subplot(2,1,1);
hold on;
plot(freqs, 1./abs(squeeze(freqresp(Wu*W1, freqs, 'Hz'))), '--', 'DisplayName', '$1/|W_uW_1|$');
plot(freqs, 1./abs(squeeze(freqresp(Wu*W2, freqs, 'Hz'))), '--', 'DisplayName', '$1/|W_uW_2|$');
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$');
plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$|H_1|$');
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');
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
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]');
@@ -232,212 +157,87 @@ yticks([-360:90:360]);
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
xticks([0.1, 1, 10, 100, 1000]);
% 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]].
% The super sensor dynamical uncertainty is displayed in Figure [[fig:super_sensor_dynamical_uncertainty_Hinf]].
% It is confirmed that the super sensor dynamical uncertainty is less than the maximum allowed uncertainty defined by the norm of $W_u(s)$.
% The $\mathcal{H}_\infty$ synthesis thus allows to design filters such that the super sensor has specified bounded uncertainty.
Gss = G1*H1 + G2*H2;
Dphi_Wu = 180/pi*asin(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))));
Dphi_Wu(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))) > 1) = 360;
Gsss = usample(Gss, 20);
% 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;
Dphi_ss = 180/pi*asin(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))));
Dphi_ss(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))) > 1) = 360;
figure;
% Magnitude
ax1 = subplot(2,1,1);
hold on;
set(gca,'ColorOrderIndex',1);
plot(freqs, 1 + abs(squeeze(freqresp(w1, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S1');
set(gca,'ColorOrderIndex',1);
plot(freqs, max(1 - abs(squeeze(freqresp(w1, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
set(gca,'ColorOrderIndex',2);
plot(freqs, 1 + abs(squeeze(freqresp(w2, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S2');
set(gca,'ColorOrderIndex',2);
plot(freqs, max(1 - abs(squeeze(freqresp(w2, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
plot(freqs, 1 + abs(squeeze(freqresp(w1*H1+w2*H2, freqs, 'Hz'))), 'k--', 'DisplayName', 'Bounds - SS');
plot(freqs, max(1 - abs(squeeze(freqresp(w1*H1+w2*H2, freqs, 'Hz'))), 0), 'k--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(Gsss(1, 1, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'DisplayName', 'SS Dynamics');
for i = 2:length(Gsss)
plot(freqs, abs(squeeze(freqresp(Gsss(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'HandleVisibility', 'off');
end
plotMagUncertainty(W1, freqs, 'color_i', 1, 'DisplayName', '$1 + W_1 \Delta_1$');
plotMagUncertainty(W2, freqs, 'color_i', 2, 'DisplayName', '$1 + W_2 \Delta_2$');
plot(freqs, 1 + abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))+abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 'k-', ...
'DisplayName', '$1 + W_1 \Delta_1 + W_2 \Delta_2$')
plot(freqs, max(1 - abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))-abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 0.001), 'k-', ...
'HandleVisibility', 'off');
plot(freqs, 1 + abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'k--', ...
'DisplayName', '$1 + W_u^{-1}\Delta$')
plot(freqs, 1 - abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'k--', ...
'HandleVisibility', 'off')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
legend('location', 'southwest');
ylabel('Magnitude');
ylim([5e-2, 10]);
ylim([1e-2, 1e1]);
legend('location', 'southeast', 'FontSize', 8);
hold off;
% Phase
ax2 = subplot(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, '--');
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
plotPhaseUncertainty(W1, freqs, 'color_i', 1);
plotPhaseUncertainty(W2, freqs, 'color_i', 2);
plot(freqs, Dphi_ss, 'k-');
plot(freqs, -Dphi_ss, 'k-');
plot(freqs, Dphi_Wu, 'k--');
plot(freqs, -Dphi_Wu, 'k--');
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
% Super sensor noise
% We now compute the obtain Power Spectral Density of the super sensor's noise.
% The noise characteristics of both individual sensor are defined below.
% The Amplitude Spectral Densities are shown in Figure [[fig:psd_sensors_hinf_synthesis]].
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;
PSD_S2 = abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2;
PSD_S1 = abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2;
PSD_Hinf = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2 + ...
abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
% The PSD of both sensor and of the super sensor is shown in Fig. [[fig:psd_sensors_hinf_synthesis]].
% The CPS of both sensor and of the super sensor is shown in Fig. [[fig:cps_sensors_hinf_synthesis]].
% The obtained RMS of the super sensor noise in the $\mathcal{H}_2$ and $\mathcal{H}_\infty$ case are shown in Table [[tab:rms_noise_comp_H2_Hinf]].
% As expected, the super sensor obtained from the $\mathcal{H}_\infty$ synthesis is much noisier than the super sensor obtained from the $\mathcal{H}_2$ synthesis.
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;
H2_filters = load('./mat/H2_filters.mat', 'H2', 'H1');
PSD_H2 = abs(squeeze(freqresp(N1*H2_filters.H1, freqs, 'Hz'))).^2 + ...
abs(squeeze(freqresp(N2*H2_filters.H2, freqs, 'Hz'))).^2;
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, sqrt(PSD_S1), '-', 'DisplayName', '$\phi_{n_1}$');
plot(freqs, sqrt(PSD_S2), '-', 'DisplayName', '$\phi_{n_2}$');
plot(freqs, sqrt(PSD_H2), 'k-', 'DisplayName', '$\phi_{n_{\mathcal{H}_2}}$');
plot(freqs, sqrt(PSD_Hinf), 'k--', 'DisplayName', '$\phi_{n_{\mathcal{H}_\infty}}$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Power Spectral Density');
xlabel('Frequency [Hz]'); ylabel('ASD $\left[ \frac{m/s}{\sqrt{Hz}} \right]$');
hold off;
xlim([freqs(1), freqs(end)]);
legend('location', 'northeast');
% #+NAME: fig:psd_sensors_hinf_synthesis
% #+CAPTION: Power Spectral Density of the obtained super sensor using the $\mathcal{H}_\infty$ synthesis ([[./figs/psd_sensors_hinf_synthesis.png][png]], [[./figs/psd_sensors_hinf_synthesis.pdf][pdf]])
% [[file:figs/psd_sensors_hinf_synthesis.png]]
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);
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');
% First Basic Example with gain mismatch :noexport:
% 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*}
G1 = 1;
G2 = 0.6;
% 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.
freqs = logspace(-1, 1, 1000);
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));
figure;
% Magnitude
ax1 = subplot(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 = subplot(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)]);
% #+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.
figure;
% Magnitude
ax1 = subplot(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 = subplot(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)]);
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);

3
matlab/matlab/comp_filters_analytical.m
View File

@@ -23,6 +23,9 @@ Hl1 = 1/((s/w0)+1);
freqs = logspace(-2, 2, 1000);
freqs
figure;
% Magnitude
ax1 = subplot(2,1,1);

379
matlab/matlab/mixed_synthesis_sensor_fusion.m
View File

@@ -4,217 +4,65 @@ clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
freqs = logspace(-1, 3, 1000);
% Noise characteristics and Uncertainty of the individual sensors
% We define the weights that are used to characterize the dynamic uncertainty of the sensors. This will be used for the $\mathcal{H}_\infty$ part of the synthesis.
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);
% We define the noise characteristics of the two sensors by choosing $N_1$ and $N_2$. This will be used for the $\mathcal{H}_2$ part of the synthesis.
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;
% Both dynamical uncertainty and noise characteristics of the individual sensors are shown in Fig. [[fig:mixed_synthesis_noise_uncertainty_sensors]].
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(N1, freqs, 'Hz'))), '-', 'DisplayName', '$|N_1(j\omega)|$');
plot(freqs, abs(squeeze(freqresp(N2, freqs, 'Hz'))), '-', 'DisplayName', '$|N_2(j\omega)|$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
legend('location', 'northeast');
ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, abs(squeeze(freqresp(w1, freqs, 'Hz'))), '-', 'DisplayName', '$|w_1(j\omega)|$');
plot(freqs, abs(squeeze(freqresp(w2, freqs, 'Hz'))), '-', 'DisplayName', '$|w_2(j\omega)|$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
legend('location', 'northeast');
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
% Weighting Functions on the uncertainty of the super sensor
% We design weights for the $\mathcal{H}_\infty$ part of the synthesis in order to limit the dynamical uncertainty of the super sensor.
% The maximum wanted multiplicative uncertainty is shown in Fig. [[fig:mixed_syn_hinf_weight]]. The idea here is that we don't really need low uncertainty at low frequency but only near the crossover frequency that is suppose to be around 300Hz here.
n = 4; w0 = 2*pi*900; G0 = 9; G1 = 1; Gc = 1.1;
H = (((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;
wphi = 0.2*(s+3.142e04)/(s+628.3)/H;
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(w1, freqs, 'Hz'))), '-', 'DisplayName', '$|w_1(j\omega)|$');
plot(freqs, abs(squeeze(freqresp(w2, freqs, 'Hz'))), '-', 'DisplayName', '$|w_2(j\omega)|$');
plot(freqs, 1./abs(squeeze(freqresp(wphi, freqs, 'Hz'))), 'k--', 'DisplayName', '$|w_u(j\omega)|^{-1}$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
legend('location', 'northeast');
xlim([freqs(1), freqs(end)]);
% #+NAME: fig:mixed_syn_hinf_weight
% #+CAPTION: Wanted maximum module uncertainty of the super sensor ([[./figs/mixed_syn_hinf_weight.png][png]], [[./figs/mixed_syn_hinf_weight.pdf][pdf]])
% [[file:figs/mixed_syn_hinf_weight.png]]
% The equivalent Magnitude and Phase uncertainties are shown in Fig. [[fig:mixed_syn_objective_hinf]].
G1 = 1 + w1*ultidyn('Delta',[1 1]);
G2 = 1 + w2*ultidyn('Delta',[1 1]);
% Few random samples of the sensor dynamics are computed
G1s = usample(G1, 10);
G2s = usample(G2, 10);
% 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;
% 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;
figure;
% Magnitude
ax1 = subplot(2,1,1);
hold on;
set(gca,'ColorOrderIndex',1);
plot(freqs, 1 + abs(squeeze(freqresp(w1, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S1');
set(gca,'ColorOrderIndex',1);
plot(freqs, max(1 - abs(squeeze(freqresp(w1, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
set(gca,'ColorOrderIndex',2);
plot(freqs, 1 + abs(squeeze(freqresp(w2, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S2');
set(gca,'ColorOrderIndex',2);
plot(freqs, max(1 - abs(squeeze(freqresp(w2, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
plot(freqs, 1 + 1./abs(squeeze(freqresp(wphi, freqs, 'Hz'))), 'k--', 'DisplayName', 'Synthesis Obj.');
plot(freqs, max(1 - 1./abs(squeeze(freqresp(wphi, freqs, 'Hz'))), 0), 'k--', 'HandleVisibility', 'off');
for i = 1:length(G1s)
plot(freqs, abs(squeeze(freqresp(G1s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0.4470 0.7410 0.4], 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G2s(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0.8500 0.3250 0.0980 0.4], 'HandleVisibility', 'off');
end
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');
ylim([1e-1, 10]);
hold off;
legend('location', 'southwest');
% Phase
ax2 = subplot(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
plot(freqs, Dphimax, 'k--');
plot(freqs, -Dphimax, 'k--');
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
% Mixed Synthesis Architecture
% The synthesis architecture that is used here is shown in Fig. [[fig:mixed_h2_hinf_synthesis]].
% The controller $K$ is synthesized such that it:
% - Keeps the $\mathcal{H}_\infty$ norm $G$ of the transfer function from $w$ to $z_\infty$ bellow some specified value
% - Keeps the $\mathcal{H}_2$ norm $H$ of the transfer function from $w$ to $z_2$ bellow some specified value
% - Minimizes a trade-off criterion of the form $W_1 G^2 + W_2 H^2$ where $W_1$ and $W_2$ are specified values
% #+name: fig:mixed_h2_hinf_synthesis
% #+caption: Mixed H2/H-Infinity Synthesis
% [[file:figs-tikz/mixed_h2_hinf_synthesis.png]]
% Here, we define $P$ such that:
% \begin{align*}
% \left\| \frac{z_\infty}{w} \right\|_\infty &= \left\| \begin{matrix}W_1(s) H_1(s) \\ W_2(s) H_2(s)\end{matrix} \right\|_\infty \\
% \left\| \frac{z_2}{w} \right\|_2 &= \left\| \begin{matrix}N_1(s) H_1(s) \\ N_2(s) H_2(s)\end{matrix} \right\|_2
% \end{align*}
% Then:
% - we specify the maximum value for the $\mathcal{H}_\infty$ norm between $w$ and $z_\infty$ to be $1$
% - we don't specify any maximum value for the $\mathcal{H}_2$ norm between $w$ and $z_2$
% - we choose $W_1 = 0$ and $W_2 = 1$ such that the objective is to minimize the $\mathcal{H}_2$ norm between $w$ and $z_2$
% The synthesis objective is to have:
% \[ \left\| \frac{z_\infty}{w} \right\|_\infty = \left\| \begin{matrix}W_1(s) H_1(s) \\ W_2(s) H_2(s)\end{matrix} \right\|_\infty < 1 \]
% and to minimize:
% \[ \left\| \frac{z_2}{w} \right\|_2 = \left\| \begin{matrix}N_1(s) H_1(s) \\ N_2(s) H_2(s)\end{matrix} \right\|_2 \]
% which is what we wanted.
% We define the generalized plant that will be used for the mixed synthesis.
W1u = ss(w1*wphi); W2u = ss(w2*wphi); % Weight on the uncertainty
W1n = ss(N1); W2n = ss(N2); % Weight on the noise
P = [W1u -W1u;
0 W2u;
W1n -W1n;
0 W2n;
1 0];
load('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
load('./mat/Wu.mat', 'Wu');
% Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis
% The mixed $\mathcal{H}_2$/$\mathcal{H}_\infty$ synthesis is performed below.
% <<sec:H2_Hinf_synthesis>>
Nmeas = 1; Ncon = 1; Nz2 = 2;
% The synthesis architecture that is used here is shown in Figure [[fig:mixed_h2_hinf_synthesis]].
[H2,~,normz,~] = h2hinfsyn(P, Nmeas, Ncon, Nz2, [0, 1], 'HINFMAX', 1, 'H2MAX', Inf, 'DKMAX', 100, 'TOL', 0.01, 'DISPLAY', 'on');
% The filter $H_2(s)$ is synthesized such that it:
% - keeps the $\mathcal{H}_\infty$ norm of the transfer function from $w$ to $z_{\mathcal{H}_\infty}$ bellow some specified value
% - minimizes the $\mathcal{H}_2$ norm of the transfer function from $w$ to $z_{\mathcal{H}_2}$
% #+name: fig:mixed_h2_hinf_synthesis
% #+caption: Mixed $\mathcal{H}_2/\mathcal{H}_\infty$ Synthesis
% [[file:figs-tikz/mixed_h2_hinf_synthesis.png]]
% Let's see that
% with $H_1(s)= 1 - H_2(s)$
% \begin{align}
% \left\| \frac{z_\infty}{w} \right\|_\infty &= \left\| \begin{matrix}H_1(s) W_1(s) W_u(s)\\ H_2(s) W_2(s) W_u(s)\end{matrix} \right\|_\infty \\
% \left\| \frac{z_2}{w} \right\|_2 &= \left\| \begin{matrix}H_1(s) N_1(s) \\ H_2(s) N_2(s)\end{matrix} \right\|_2 = \sigma_n
% \end{align}
% The generalized plant $P_{\mathcal{H}_2/\mathcal{H}_\infty}$ is defined below
W1u = ss(W2*Wu); W2u = ss(W1*Wu); % Weight on the uncertainty
W1n = ss(N2); W2n = ss(N1); % Weight on the noise
P = [Wu*W1 -Wu*W1;
0 Wu*W2;
N1 -N1;
0 N2;
1 0];
% And the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis is performed.
[H2, ~] = h2hinfsyn(ss(P), 1, 1, 2, [0, 1], 'HINFMAX', 1, 'H2MAX', Inf, 'DKMAX', 100, 'TOL', 1e-3, 'DISPLAY', 'on');
H1 = 1 - H2;
% The obtained filters are saved for further analysis
save('./mat/H2_Hinf_filters.mat', 'H2', 'H1');
% The obtained complementary filters are shown in Fig. [[fig:comp_filters_mixed_synthesis]].
% The obtained complementary filters are shown in Figure [[fig:htwo_hinf_comp_filters]].
figure;
ax1 = subplot(2,1,1);
hold on;
set(gca,'ColorOrderIndex',1)
plot(freqs, 1./abs(squeeze(freqresp(W1u, freqs, 'Hz'))), '--', 'DisplayName', '$W_1$');
set(gca,'ColorOrderIndex',2)
plot(freqs, 1./abs(squeeze(freqresp(W2u, freqs, 'Hz'))), '--', 'DisplayName', '$W_2$');
plot(freqs, 1./abs(squeeze(freqresp(Wu*W1, freqs, 'Hz'))), '--', 'DisplayName', '$1/|W_uW_1|$');
plot(freqs, 1./abs(squeeze(freqresp(Wu*W2, freqs, 'Hz'))), '--', 'DisplayName', '$1/|W_uW_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;
@@ -222,13 +70,11 @@ set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude');
set(gca, 'XTickLabel',[]);
ylim([1e-3, 2]);
legend('location', 'southwest');
legend('location', 'southeast', 'FontSize', 8);
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]');
@@ -237,115 +83,120 @@ yticks([-360:90:360]);
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
xticks([0.1, 1, 10, 100, 1000]);
% Obtained Super Sensor's noise
% The PSD and CPS of the super sensor's noise are shown in Fig. [[fig:psd_super_sensor_mixed_syn]] and Fig. [[fig:cps_super_sensor_mixed_syn]] respectively.
% The Amplitude Spectral Density of the super sensor's noise is shown in Figure [[fig:psd_sensors_htwo_hinf_synthesis]].
% A time domain simulation is shown in Figure [[fig:super_sensor_time_domain_h2_hinf]].
% The RMS values of the super sensor noise for the presented three synthesis are listed in Table [[tab:rms_noise_comp]].
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;
PSD_S2 = abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2;
PSD_S1 = abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2;
PSD_H2Hinf = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2 + ...
abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
H2_filters = load('./mat/H2_filters.mat', 'H2', 'H1');
PSD_H2 = abs(squeeze(freqresp(N1*H2_filters.H1, freqs, 'Hz'))).^2 + ...
abs(squeeze(freqresp(N2*H2_filters.H2, freqs, 'Hz'))).^2;
Hinf_filters = load('./mat/Hinf_filters.mat', 'H2', 'H1');
PSD_Hinf = abs(squeeze(freqresp(N1*Hinf_filters.H1, freqs, 'Hz'))).^2 + ...
abs(squeeze(freqresp(N2*Hinf_filters.H2, freqs, 'Hz'))).^2;
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}}$');
plot(freqs, sqrt(PSD_S1), '-', 'DisplayName', '$\Phi_{n_1}$');
plot(freqs, sqrt(PSD_S2), '-', 'DisplayName', '$\Phi_{n_2}$');
plot(freqs, sqrt(PSD_H2), 'k-', 'DisplayName', '$\Phi_{n_{\mathcal{H}_2}}$');
plot(freqs, sqrt(PSD_Hinf), 'k--', 'DisplayName', '$\Phi_{n_{\mathcal{H}_\infty}}$');
plot(freqs, sqrt(PSD_H2Hinf), 'k-.', 'DisplayName', '$\Phi_{n_{\mathcal{H}_2/\mathcal{H}_\infty}}$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Power Spectral Density');
xlabel('Frequency [Hz]'); ylabel('ASD $\left[ \frac{m/s}{\sqrt{Hz}} \right]$');
hold off;
xlim([freqs(1), freqs(end)]);
legend('location', 'northeast');
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 3);
% #+NAME: fig:psd_super_sensor_mixed_syn
% #+CAPTION: Power Spectral Density of the Super Sensor obtained with the mixed $\mathcal{H}_2$/$\mathcal{H}_\infty$ synthesis ([[./figs/psd_super_sensor_mixed_syn.png][png]], [[./figs/psd_super_sensor_mixed_syn.pdf][pdf]])
% [[file:figs/psd_super_sensor_mixed_syn.png]]
% #+name: fig:psd_sensors_htwo_hinf_synthesis
% #+CAPTION: Power Spectral Density of the Super Sensor obtained with the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis
% #+RESULTS:
% [[file:figs/psd_sensors_htwo_hinf_synthesis.png]]
Fs = 1e4; % Sampling Frequency [Hz]
Ts = 1/Fs; % Sampling Time [s]
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);
t = 0:Ts:2; % Time Vector [s]
v = 0.1*sin((10*t).*t)'; % Velocity measured [m/s]
% Generate noises in velocity corresponding to sensor 1 and 2:
n1 = lsim(N1, sqrt(Fs/2)*randn(length(t), 1), t);
n2 = lsim(N2, sqrt(Fs/2)*randn(length(t), 1), t);
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');
set(gca,'ColorOrderIndex',2)
plot(t, v + n2, 'DisplayName', '$\hat{x}_2$');
set(gca,'ColorOrderIndex',1)
plot(t, v + n1, 'DisplayName', '$\hat{x}_1$');
set(gca,'ColorOrderIndex',3)
plot(t, v + (lsim(H1, n1, t) + lsim(H2, n2, t)), 'DisplayName', '$\hat{x}$');
plot(t, v, 'k--', 'DisplayName', '$x$');
hold off;
xlim([2e-1, freqs(end)]);
ylim([1e-10 1e-5]);
legend('location', 'southeast');
xlabel('Time [s]'); ylabel('Velocity [m/s]');
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 3);
ylim([-0.3, 0.3]);
% Obtained Super Sensor's Uncertainty
% The uncertainty on the super sensor's dynamics is shown in Fig. [[]].
% The uncertainty on the super sensor's dynamics is shown in Figure [[fig:super_sensor_dynamical_uncertainty_Htwo_Hinf]].
G1 = 1 + w1*ultidyn('Delta',[1 1]);
G2 = 1 + w2*ultidyn('Delta',[1 1]);
Dphi_Wu = 180/pi*asin(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))));
Dphi_Wu(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))) > 1) = 360;
Gss = G1*H1 + G2*H2;
Gsss = usample(Gss, 20);
% 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;
% 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;
Dphi_ss = 180/pi*asin(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))));
Dphi_ss(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))) > 1) = 360;
figure;
% Magnitude
ax1 = subplot(2,1,1);
hold on;
set(gca,'ColorOrderIndex',1);
plot(freqs, 1 + abs(squeeze(freqresp(w1, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S1');
set(gca,'ColorOrderIndex',1);
plot(freqs, max(1 - abs(squeeze(freqresp(w1, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
set(gca,'ColorOrderIndex',2);
plot(freqs, 1 + abs(squeeze(freqresp(w2, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S2');
set(gca,'ColorOrderIndex',2);
plot(freqs, max(1 - abs(squeeze(freqresp(w2, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
plot(freqs, 1 + abs(squeeze(freqresp(w1*H1+w2*H2, freqs, 'Hz'))), 'k--', 'DisplayName', 'Bounds - SS');
plot(freqs, max(1 - abs(squeeze(freqresp(w1*H1+w2*H2, freqs, 'Hz'))), 0), 'k--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(Gsss(1, 1, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'DisplayName', 'SS Dynamics');
for i = 2:length(Gsss)
plot(freqs, abs(squeeze(freqresp(Gsss(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'HandleVisibility', 'off');
end
plotMagUncertainty(W1, freqs, 'color_i', 1, 'DisplayName', '$1 + W_1 \Delta_1$');
plotMagUncertainty(W2, freqs, 'color_i', 2, 'DisplayName', '$1 + W_2 \Delta_2$');
plot(freqs, 1 + abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))+abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 'k-', ...
'DisplayName', '$1 + W_1 \Delta_1 + W_2 \Delta_2$')
plot(freqs, max(1 - abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))-abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 0.001), 'k-', ...
'HandleVisibility', 'off');
plot(freqs, 1 + abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'k--', ...
'DisplayName', '$1 + W_u^{-1}\Delta$')
plot(freqs, 1 - abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'k--', ...
'HandleVisibility', 'off')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
legend('location', 'southwest');
ylabel('Magnitude');
ylim([5e-2, 10]);
ylim([1e-2, 1e1]);
legend('location', 'southeast', 'FontSize', 8);
hold off;
% Phase
ax2 = subplot(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, '--');
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
plotPhaseUncertainty(W1, freqs, 'color_i', 1);
plotPhaseUncertainty(W2, freqs, 'color_i', 2);
plot(freqs, Dphi_ss, 'k-');
plot(freqs, -Dphi_ss, 'k-');
plot(freqs, Dphi_Wu, 'k--');
plot(freqs, -Dphi_Wu, 'k--');
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);

762
matlab/matlab/optimal_comp_filters.m
View File

@@ -4,629 +4,221 @@ clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
freqs = logspace(-1, 3, 1000);
addpath('src');
load('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
% 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)
% $\mathcal{H}_2$ Synthesis
% <<sec:H2_synthesis>>
% Consider the generalized plant $P_{\mathcal{H}_2}$ shown in Figure [[fig:h_two_optimal_fusion]] and described by Equation eqref:eq:H2_generalized_plant.
omegac = 100*2*pi; G0 = 1e-5; Ginf = 1e-4;
N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/100);
% #+name: fig:h_two_optimal_fusion
% #+caption: Architecture used for $\mathcal{H}_\infty$ synthesis of complementary filters
% [[file:figs-tikz/h_two_optimal_fusion.png]]
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;
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');
% 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]]
% \begin{equation*}
% \begin{equation} \label{eq:H2_generalized_plant}
% \begin{pmatrix}
% z \\ v
% \end{pmatrix} = \begin{pmatrix}
% 0 & N_2 & 1 \\
% N_1 & -N_2 & 0
% \end{pmatrix} \begin{pmatrix}
% w_1 \\ w_2 \\ u
% z_1 \\ z_2 \\ v
% \end{pmatrix} = \underbrace{\begin{bmatrix}
% N_1 & -N_1 \\
% 0 & N_2 \\
% 1 & 0
% \end{bmatrix}}_{P_{\mathcal{H}_2}} \begin{pmatrix}
% w \\ u
% \end{pmatrix}
% \end{equation*}
% \end{equation}
% 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} \]
% Applying the $\mathcal{H}_2$ synthesis on $P_{\mathcal{H}_2}$ will generate a filter $H_2(s)$ such that the $\mathcal{H}_2$ norm from $w$ to $(z_1,z_2)$ which is actually equals to $\sigma_n$ by defining $H_1(s) = 1 - H_2(s)$:
% \begin{equation}
% \left\| \begin{matrix} z_1/w \\ z_2/w \end{matrix} \right\|_2 = \left\| \begin{matrix} N_1 (1 - H_2) \\ N_2 H_2 \end{matrix} \right\|_2 = \sigma_n \quad \text{with} \quad H_1(s) = 1 - H_2(s)
% \end{equation}
% Thus, if we minimize the $\mathcal{H}_2$ norm of this transfer function, we minimize the RMS value of $\hat{x}$.
% We then have that the $\mathcal{H}_2$ synthesis applied on $P_{\mathcal{H}_2}$ generates two complementary filters $H_1(s)$ and $H_2(s)$ such that the RMS value of super sensor noise is minimized.
% We define the generalized plant $P$ on matlab as shown on figure [[fig:h_infinity_optimal_comp_filters]].
% The generalized plant $P_{\mathcal{H}_2}$ is defined below
P = [0 N2 1;
N1 -N2 0];
PH2 = [N1 -N1;
0 N2;
1 0];
% And we do the $\mathcal{H}_2$ synthesis using the =h2syn= command.
% The $\mathcal{H}_2$ synthesis using the =h2syn= command
[H1, ~, gamma] = h2syn(P, 1, 1);
[H2, ~, gamma] = h2syn(PH2, 1, 1);
% Finally, we define $H_2(s) = 1 - H_1(s)$.
% Finally, $H_1(s)$ is defined as follows
H2 = 1 - H1;
H1 = 1 - H2;
% Filters are saved for further use
save('./mat/H2_filters.mat', 'H2', 'H1');
% 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.
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');
% #+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]]
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;
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');
% #+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]]
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);
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');
% 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]].
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;
P = [W1a -W1a;
0 W2a;
1 0];
% And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command.
[H2a, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
% #+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
H1a = 1 - H2a;
% The obtained complementary filters are shown in Figure [[fig:htwo_comp_filters]].
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]);
% #+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.
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);
% 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]].
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
P = [W1b -W1b;
0 W2b;
1 0];
% And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command.
[H2b, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
% #+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
H1b = 1 - H2b;
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]);
% #+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]]
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);
% H-Infinity Synthesis - method C
Wp = 0.56*(inv(N1)+inv(N2))/(1 + s/2/pi/1000);
W1c = N1*Wp;
W2c = N2*Wp;
P = [W1c -W1c;
0 W2c;
1 0];
% And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command.
[H2c, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
% #+RESULTS:
% #+begin_example
% [H2c, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
% Test bounds: 0.0000 < gamma <= 36.7543
% gamma hamx_eig xinf_eig hamy_eig yinf_eig nrho_xy p/f
% 36.754 5.7e+00 -1.0e-13 6.3e+00 -6.2e-25 0.0000 p
% 18.377 5.7e+00 -1.4e-12 6.3e+00 -1.8e-13 0.0000 p
% 9.189 5.7e+00 -4.3e-13 6.3e+00 -4.7e-15 0.0000 p
% 4.594 5.7e+00 -9.4e-13 6.3e+00 -4.7e-15 0.0000 p
% 2.297 5.7e+00 -1.3e-16 6.3e+00 -6.8e-14 0.0000 p
% 1.149 5.7e+00 -1.6e-17 6.3e+00 -1.5e-15 0.0000 p
% 0.574 5.7e+00 -5.2e+02# 6.3e+00 -5.9e-14 0.0000 f
% 0.861 5.7e+00 -3.1e+04# 6.3e+00 -3.8e-14 0.0000 f
% 1.005 5.7e+00 -1.6e-12 6.3e+00 -1.1e-14 0.0000 p
% 0.933 5.7e+00 -1.1e+05# 6.3e+00 -7.2e-14 0.0000 f
% 0.969 5.7e+00 -3.3e+05# 6.3e+00 -5.6e-14 0.0000 f
% 0.987 5.7e+00 -1.2e+06# 6.3e+00 -4.5e-15 0.0000 f
% 0.996 5.7e+00 -6.5e-16 6.3e+00 -1.7e-15 0.0000 p
% 0.992 5.7e+00 -2.9e+06# 6.3e+00 -6.1e-14 0.0000 f
% 0.994 5.7e+00 -9.7e+06# 6.3e+00 -3.0e-16 0.0000 f
% 0.995 5.7e+00 -8.0e-10 6.3e+00 -1.9e-13 0.0000 p
% 0.994 5.7e+00 -2.3e+07# 6.3e+00 -4.3e-14 0.0000 f
% Gamma value achieved: 0.9949
% #+end_example
H1c = 1 - H2c;
figure;
ax1 = subplot(2,1,1);
hold on;
set(gca,'ColorOrderIndex',1)
plot(freqs, 1./abs(squeeze(freqresp(W1c, freqs, 'Hz'))), '--', 'DisplayName', '$w_1$');
set(gca,'ColorOrderIndex',2)
plot(freqs, 1./abs(squeeze(freqresp(W2c, freqs, 'Hz'))), '--', 'DisplayName', '$w_2$');
set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(H1c, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
set(gca,'ColorOrderIndex',2)
plot(freqs, abs(squeeze(freqresp(H2c, 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(H1c, freqs, 'Hz'))), '-');
set(gca,'ColorOrderIndex',2)
plot(freqs, 180/pi*phase(squeeze(freqresp(H2c, 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]);
% #+NAME: fig:weights_comp_filters_Hinfc
% #+CAPTION: Weights and Complementary Fitlers obtained ([[./figs/weights_comp_filters_Hinfc.png][png]], [[./figs/weights_comp_filters_Hinfc.pdf][pdf]])
% [[file:figs/weights_comp_filters_Hinfc.png]]
PSD_Hc = abs(squeeze(freqresp(N1*H1c, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2c, freqs, 'Hz'))).^2;
CPS_Hc = 1/pi*cumtrapz(2*pi*freqs, PSD_Hc);
% #+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 |
% | H-Infinity c | 2.2e-04 |
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, 'r-', '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}$');
plot(freqs, PSD_Hc, 'k-.', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_\infty},c}$');
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');
% #+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]]
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, 'r-', '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))));
plot(freqs, CPS_Hc, 'k-.', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_\\infty, c}} = %.1e$', sqrt(CPS_Hc(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');
% Obtained Super Sensor's noise uncertainty
% We would like to verify if the obtained sensor fusion architecture is robust to change in the sensor dynamics.
% To study the dynamical uncertainty on the super sensor, we defined some multiplicative uncertainty on both sensor dynamics.
% Two weights $w_1(s)$ and $w_2(s)$ are used to described the amplitude of the dynamical uncertainty.
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);
% The sensor uncertain models are defined below.
G1 = 1 + w1*ultidyn('Delta',[1 1]);
G2 = 1 + w2*ultidyn('Delta',[1 1]);
% 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;
% The super sensor uncertain model is defined below using the complementary filters obtained with the $\mathcal{H}_2$ synthesis.
% The dynamical uncertainty bounds of the super sensor is shown in Fig. [[fig:uncertainty_super_sensor_H2_syn]].
% Right Half Plane zero might be introduced in the super sensor dynamics which will render the feedback system unstable.
Gss = G1*H1 + G2*H2;
Gsss = usample(Gss, 20);
% 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;
figure;
% Magnitude
ax1 = subplot(2,1,1);
hold on;
set(gca,'ColorOrderIndex',1);
plot(freqs, 1 + abs(squeeze(freqresp(w1, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S1');
set(gca,'ColorOrderIndex',1);
plot(freqs, max(1 - abs(squeeze(freqresp(w1, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
set(gca,'ColorOrderIndex',2);
plot(freqs, 1 + abs(squeeze(freqresp(w2, freqs, 'Hz'))), '--', 'DisplayName', 'Bounds - S2');
set(gca,'ColorOrderIndex',2);
plot(freqs, max(1 - abs(squeeze(freqresp(w2, freqs, 'Hz'))), 0), '--', 'HandleVisibility', 'off');
plot(freqs, 1 + abs(squeeze(freqresp(w1*H1, freqs, 'Hz'))) + abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))), 'k--', 'DisplayName', 'Bounds - SS');
plot(freqs, max(1 - abs(squeeze(freqresp(w1*H1, freqs, 'Hz'))) - abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))), 0), 'k--', 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(Gsss(1, 1, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'DisplayName', 'SS Dynamics');
for i = 2:length(Gsss)
plot(freqs, abs(squeeze(freqresp(Gsss(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'HandleVisibility', 'off');
end
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');
set(gca, 'XTickLabel',[]);
legend('location', 'southwest');
ylabel('Magnitude');
ylim([5e-2, 10]);
hold off;
legend('location', 'northeast', 'FontSize', 8);
% Phase
ax2 = subplot(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, '--');
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
plot(freqs, 180/pi*angle(squeeze(freqresp(H1, freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(H2, freqs, 'Hz'))));
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
% Super Sensor Noise
% <<sec:H2_super_sensor_noise>>
% The Power Spectral Density of the individual sensors' noise $\Phi_{n_1}, \Phi_{n_2}$ and of the super sensor noise $\Phi_{n_{\mathcal{H}_2}}$ are computed below.
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;
% #+name: tab:rms_noise_H2
% #+caption: RMS value of the individual sensor noise and of the super sensor using the $\mathcal{H}_2$ Synthesis
% #+attr_latex: :environment tabular :align cc
% #+attr_latex: :center t :booktabs t :float t
% #+RESULTS:
% | | RMS value $[m/s]$ |
% |------------------------------+-------------------|
% | $\sigma_{n_1}$ | 0.015 |
% | $\sigma_{n_2}$ | 0.080 |
% | $\sigma_{n_{\mathcal{H}_2}}$ | 0.003 |
figure;
hold on;
plot(freqs, sqrt(PSD_S1), '-', 'DisplayName', '$\phi_{n_1}$');
plot(freqs, sqrt(PSD_S2), '-', 'DisplayName', '$\phi_{n_2}$');
plot(freqs, sqrt(PSD_H2), 'k-', 'DisplayName', '$\phi_{n_{\mathcal{H}_2}}$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('ASD $\left[ \frac{m/s}{\sqrt{Hz}} \right]$');
hold off;
xlim([freqs(1), freqs(end)]);
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
% #+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
% #+RESULTS:
% [[file:figs/psd_sensors_htwo_synthesis.png]]
% A time domain simulation is now performed.
% The measured velocity $x$ is set to be a sweep sine with an amplitude of $0.1\ [m/s]$.
% The velocity estimates from the two sensors and from the super sensors are shown in Figure [[fig:super_sensor_time_domain_h2]].
% The resulting noises are displayed in Figure [[fig:sensor_noise_H2_time_domain]].
Fs = 1e4; % Sampling Frequency [Hz]
Ts = 1/Fs; % Sampling Time [s]
t = 0:Ts:2; % Time Vector [s]
v = 0.1*sin((10*t).*t)'; % Velocity measured [m/s]
% Generate noises in velocity corresponding to sensor 1 and 2:
n1 = lsim(N1, sqrt(Fs/2)*randn(length(t), 1), t);
n2 = lsim(N2, sqrt(Fs/2)*randn(length(t), 1), t);
figure;
hold on;
set(gca,'ColorOrderIndex',2)
plot(t, v + n2, 'DisplayName', '$\hat{x}_2$');
set(gca,'ColorOrderIndex',1)
plot(t, v + n1, 'DisplayName', '$\hat{x}_1$');
set(gca,'ColorOrderIndex',3)
plot(t, v + (lsim(H1, n1, t) + lsim(H2, n2, t)), 'DisplayName', '$\hat{x}$');
plot(t, v, 'k--', 'DisplayName', '$x$');
hold off;
xlabel('Time [s]'); ylabel('Velocity [m/s]');
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
ylim([-0.3, 0.3]);
% #+name: fig:super_sensor_time_domain_h2
% #+caption: Noise of individual sensors and noise of the super sensor
% #+RESULTS:
% [[file:figs/super_sensor_time_domain_h2.png]]
figure;
hold on;
set(gca,'ColorOrderIndex',2)
plot(t, n2, 'DisplayName', '$n_2$');
set(gca,'ColorOrderIndex',1)
plot(t, n1, 'DisplayName', '$n_1$');
set(gca,'ColorOrderIndex',3)
plot(t, (lsim(H1, n1, t)+lsim(H2, n2, t)), '-', 'DisplayName', '$n$');
hold off;
xlabel('Time [s]'); ylabel('Sensor Noise [m/s]');
legend('FontSize', 8);
ylim([-0.2, 0.2]);
% Discrepancy between sensor dynamics and model
% If we consider sensor dynamical uncertainty as explained in Section [[sec:sensor_uncertainty]], we can compute what would be the super sensor dynamical uncertainty when using the complementary filters obtained using the $\mathcal{H}_2$ Synthesis.
% The super sensor dynamical uncertainty is shown in Figure [[fig:super_sensor_dynamical_uncertainty_H2]].
% It is shown that the phase uncertainty is not bounded between 100Hz and 200Hz.
% As a result the super sensor signal can not be used for feedback applications about 100Hz.
Dphi_ss = 180/pi*asin(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))));
Dphi_ss(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))) > 1) = 360;
figure;
% Magnitude
ax1 = subplot(2,1,1);
hold on;
plotMagUncertainty(W1, freqs, 'color_i', 1, 'DisplayName', '$1 + W_1 \Delta_1$');
plotMagUncertainty(W2, freqs, 'color_i', 2, 'DisplayName', '$1 + W_2 \Delta_2$');
plot(freqs, 1 + abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))+abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 'k-', ...
'DisplayName', '$1 + W_1 \Delta_1 + W_2 \Delta_2$')
plot(freqs, max(1 - abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))-abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 0.001), 'k-', ...
'HandleVisibility', 'off');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');
ylim([1e-2, 1e1]);
legend('location', 'southeast', 'FontSize', 8);
hold off;
% Phase
ax2 = subplot(2,1,2);
hold on;
plotPhaseUncertainty(W1, freqs, 'color_i', 1);
plotPhaseUncertainty(W2, freqs, 'color_i', 2);
plot(freqs, Dphi_ss, 'k-');
plot(freqs, -Dphi_ss, 'k-');
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);