diff --git a/matlab/figs/magnitude_wphi.pdf b/matlab/figs/magnitude_wphi.pdf new file mode 100644 index 0000000..da37b02 Binary files /dev/null and b/matlab/figs/magnitude_wphi.pdf differ diff --git a/matlab/figs/magnitude_wphi.png b/matlab/figs/magnitude_wphi.png new file mode 100644 index 0000000..4de65a7 Binary files /dev/null and b/matlab/figs/magnitude_wphi.png differ diff --git a/matlab/figs/upper_bounds_comp_filter_max_phase_uncertainty.pdf b/matlab/figs/upper_bounds_comp_filter_max_phase_uncertainty.pdf index aeb1be9..2a0aac1 100644 Binary files a/matlab/figs/upper_bounds_comp_filter_max_phase_uncertainty.pdf and b/matlab/figs/upper_bounds_comp_filter_max_phase_uncertainty.pdf differ diff --git a/matlab/figs/upper_bounds_comp_filter_max_phase_uncertainty.png b/matlab/figs/upper_bounds_comp_filter_max_phase_uncertainty.png index 2d6e3de..96d74ab 100644 Binary files a/matlab/figs/upper_bounds_comp_filter_max_phase_uncertainty.png and b/matlab/figs/upper_bounds_comp_filter_max_phase_uncertainty.png differ diff --git a/matlab/index.html b/matlab/index.html index 1d47acb..3fa94e5 100644 --- a/matlab/index.html +++ b/matlab/index.html @@ -3,7 +3,7 @@ "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> - + Robust and Optimal Sensor Fusion - Matlab Computation @@ -279,88 +279,88 @@ for the JavaScript code in this tag.

Table of Contents

@@ -382,31 +382,31 @@ To achieve this, the sensors included in the filter should complement one anothe

When blending two sensors using complementary filters with unknown dynamics, phase lag may be introduced that renders the close-loop system unstable.

Then, three design methods for generating two complementary filters are proposed:

-
-

1 Optimal Sensor Fusion - Minimize the Super Sensor Noise

+
+

1 Optimal Sensor Fusion - Minimize the Super Sensor Noise

- +

The idea is to combine sensors that works in different frequency range using complementary filters. @@ -427,23 +427,23 @@ All the files (data and Matlab scripts) are accessible -

1.1 Architecture

+
+

1.1 Architecture

-Let's consider the sensor fusion architecture shown on figure 1 where two sensors (sensor 1 and sensor 2) are measuring the same quantity \(x\) with different noise characteristics determined by \(N_1(s)\) and \(N_2(s)\). +Let's consider the sensor fusion architecture shown on figure 1 where two sensors (sensor 1 and sensor 2) are measuring the same quantity \(x\) with different noise characteristics determined by \(N_1(s)\) and \(N_2(s)\).

\(\tilde{n}_1\) and \(\tilde{n}_2\) are normalized white noise:

\begin{equation} -\label{orgfdd39ec} +\label{orgcb7a1e1} \Phi_{\tilde{n}_1}(\omega) = \Phi_{\tilde{n}_1}(\omega) = 1 \end{equation} -
+

fusion_two_noisy_sensors_weights.png

Figure 1: Fusion of two sensors

@@ -453,16 +453,16 @@ Let's consider the sensor fusion architecture shown on figure 2. +We obtain the architecture of figure 2.

-
+

sensor_fusion_noisy_perfect_dyn.png

Figure 2: Fusion of two sensors with ideal dynamics

@@ -472,7 +472,7 @@ We obtain the architecture of figure 2. \(H_1(s)\) and \(H_2(s)\) are complementary filters:

\begin{equation} -\label{org60c56f0} +\label{orgc1eccc7} H_1(s) + H_2(s) = 1 \end{equation} @@ -489,14 +489,14 @@ We have that the Power Spectral Density (PSD) of \(\hat{x}\) is: And the goal is the minimize the Root Mean Square (RMS) value of \(\hat{x}\):

\begin{equation} -\label{org796f363} +\label{orgf5c5975} \sigma_{\hat{x}} = \sqrt{\int_0^\infty \Phi_{\hat{x}}(\omega) d\omega} \end{equation}
-
-

1.2 Noise of the sensors

+
+

1.2 Noise of the sensors

Let's define the noise characteristics of the two sensors by choosing \(N_1\) and \(N_2\): @@ -516,7 +516,7 @@ N2 = ( +

noise_characteristics_sensors.png

Figure 3: Noise Characteristics of the two sensors (png, pdf)

@@ -524,8 +524,8 @@ N2 = ( -

1.3 H-Two Synthesis

+
+

1.3 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: @@ -538,11 +538,11 @@ For that, we use the \(\mathcal{H}_2\) Synthesis.

-We use the generalized plant architecture shown on figure 4. +We use the generalized plant architecture shown on figure 4.

-
+

h_infinity_optimal_comp_filters.png

Figure 4: \(\mathcal{H}_2\) Synthesis - Generalized plant used for the optimal generation of complementary filters

@@ -560,7 +560,7 @@ Thus, if we minimize the \(\mathcal{H}_2\) norm of this transfer function, we mi

-We define the generalized plant \(P\) on matlab as shown on figure 4. +We define the generalized plant \(P\) on matlab as shown on figure 4. 

P = [0   N2  1;
@@ -585,15 +585,15 @@ Finally, we define \(H_2(s) = 1 - H_1(s)\).

-The complementary filters obtained are shown on figure 5. +The complementary filters obtained are shown on figure 5.

-The PSD of the noise of the individual sensor and of the super sensor are shown in Fig. 6. +The PSD of the noise of the individual sensor and of the super sensor are shown in Fig. 6.

-The Cumulative Power Spectrum (CPS) is shown on Fig. 7. +The Cumulative Power Spectrum (CPS) is shown on Fig. 7.

@@ -601,7 +601,7 @@ The obtained RMS value of the super sensor is lower than the RMS value of the in

-
+

htwo_comp_filters.png

Figure 5: Obtained complementary filters using the \(\mathcal{H}_2\) Synthesis (png, pdf)

@@ -615,7 +615,7 @@ PSD_H2 = abs(squeeze -
+

psd_sensors_htwo_synthesis.png

Figure 6: Power Spectral Density of the estimated \(\hat{x}\) using the two sensors alone and using the optimally fused signal (png, pdf)

@@ -629,7 +629,7 @@ CPS_H2 = 1 +

cps_h2_synthesis.png

Figure 7: Cumulative Power Spectrum of individual sensors and super sensor using the \(\mathcal{H}_2\) synthesis (png, pdf)

@@ -637,8 +637,8 @@ CPS_H2 = 1 -

1.4 H-Infinity Synthesis - method A

+
+

1.4 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\): @@ -662,12 +662,12 @@ We could try to approach that with the \(\mathcal{H}_\infty\) synthesis by using

-As shown on Fig. 3, the frequency where the two sensors have the same noise level is around 9Hz. +As shown on Fig. 3, 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. 8. +The weighting functions used as well as the obtained complementary filters are shown in Fig. 8.

@@ -736,7 +736,7 @@ Test bounds: 0.1000 < gamma <= 10500.0000
-
+

weights_comp_filters_Hinfa.png

Figure 8: Weights and Complementary Fitlers obtained (png, pdf)

@@ -754,8 +754,8 @@ CPS_Ha = 1 -

1.5 H-Infinity Synthesis - method B

+
+

1.5 H-Infinity Synthesis - method B

We have that: @@ -800,7 +800,7 @@ We use this as the weighting functions for the \(\mathcal{H}_\infty\) synthesis

-The weighting function and the obtained complementary filters are shown in Fig. 9. +The weighting function and the obtained complementary filters are shown in Fig. 9.

@@ -860,7 +860,7 @@ Test bounds: 0.0000 < gamma <= 32.8125
-
+

weights_comp_filters_Hinfb.png

Figure 9: Weights and Complementary Fitlers obtained (png, pdf)

@@ -874,26 +874,26 @@ CPS_Hb = 1 -

1.6 Comparison of the methods

+
+

1.6 Comparison of the methods

The three methods are now compared.

-The Power Spectral Density of the super sensors obtained with the complementary filters designed using the three methods are shown in Fig. 10. +The Power Spectral Density of the super sensors obtained with the complementary filters designed using the three methods are shown in Fig. 10.

-The Cumulative Power Spectrum for the same sensors are shown on Fig. 11. +The Cumulative Power Spectrum for the same sensors are shown on Fig. 11.

-The RMS value of the obtained super sensors are shown on table 1. +The RMS value of the obtained super sensors are shown on table 1.

- +
@@ -937,14 +937,14 @@ The RMS value of the obtained super sensors are shown on table + -
+

comparison_cps_noise.png

Figure 11: Comparison of the obtained Cumulative Power Spectrum using the three methods (png, pdf)

@@ -952,8 +952,8 @@ The RMS value of the obtained super sensors are shown on table -

1.7 Conclusion

+
+

1.7 Conclusion

From the above complementary filter design with the \(\mathcal{H}_2\) and \(\mathcal{H}_\infty\) synthesis, 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). @@ -962,11 +962,11 @@ From the above complementary filter design with the \(\mathcal{H}_2\) and \(\mat

-
-

2 Optimal Sensor Fusion - Minimize the Super Sensor Dynamical Uncertainty

+
+

2 Optimal Sensor Fusion - Minimize the Super Sensor Dynamical Uncertainty

- +

We initially considered perfectly known sensor dynamics so that it can be perfectly inverted. @@ -974,11 +974,11 @@ We initially considered perfectly known sensor dynamics so that it can be perfec

We now take into account the fact that the sensor dynamics is only partially known. -To do so, we model the uncertainty that we have on the sensor dynamics by multiplicative input uncertainty as shown in Fig. 12. +To do so, we model the uncertainty that we have on the sensor dynamics by multiplicative input uncertainty as shown in Fig. 12.

-
+

sensor_fusion_dynamic_uncertainty.png

Figure 12: Sensor fusion architecture with sensor dynamics uncertainty

@@ -989,22 +989,22 @@ The objective here is to design complementary filters \(H_1(s)\) and \(H_2(s)\)

-All the files (data and Matlab scripts) are accessible here. +All the files (data and Matlab scripts) are accessible here.

-
-

2.1 Unknown sensor dynamics dynamics

+
+

2.1 Unknown sensor dynamics dynamics

In practical systems, the sensor dynamics has always some level of uncertainty. -Let's represent that with multiplicative input uncertainty as shown on figure 12. +Let's represent that with multiplicative input uncertainty as shown on figure 12.

-
+

sensor_fusion_dynamic_uncertainty.png

Figure 13: Fusion of two sensors with input multiplicative uncertainty

@@ -1049,7 +1049,7 @@ Which is approximately the same as requiring

-The uncertainty set of the transfer function from \(\hat{x}\) to \(x\) is bounded in the complex plane by a circle centered on 1 and with a radius equal to \(\epsilon\) (figure 14). +The uncertainty set of the transfer function from \(\hat{x}\) to \(x\) is bounded in the complex plane by a circle centered on 1 and with a radius equal to \(\epsilon\) (figure 14).

@@ -1058,7 +1058,7 @@ We then have that the angle introduced by the super sensor is bounded by \(\arcs

-
+

uncertainty_gain_phase_variation.png

Figure 14: Maximum phase variation

@@ -1070,8 +1070,8 @@ Thus, we choose should choose \(\epsilon\) so that the maximum phase uncertainty
-
-

2.2 Design the complementary filters in order to limit the phase and gain uncertainty of the super sensor

+
+

2.2 Design the complementary filters in order to limit the phase and gain uncertainty of the super sensor

Let's say the two sensors dynamics have been identified with the associated uncertainty weights \(w_1(s)\) and \(w_2(s)\). @@ -1101,8 +1101,8 @@ This is of primary importance in order to ensure the stability of the feedback l

-
-

2.3 First Basic Example with gain mismatch

+
+

2.3 First Basic Example with gain mismatch

Let's consider two ideal sensors except one sensor has not an expected unity gain but a gain equal to \(0.6\): @@ -1119,19 +1119,19 @@ G2 = 0.15. +Two pairs of complementary filters are designed and shown on figure 15. 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.

-
+

comp_filters_robustness_test.png

Figure 15: The two complementary filters designed for the robustness test (png, pdf)

-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 16). +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 16).

@@ -1139,7 +1139,7 @@ We see that the blue complementary filters with a lower maximum norm permits to

-
+

tf_super_sensor_comp.png

Figure 16: Comparison of the obtained super sensor transfer functions (png, pdf)

@@ -1147,8 +1147,8 @@ We see that the blue complementary filters with a lower maximum norm permits to
-
-

2.4 More Complete example with dynamical uncertainty

+
+

2.4 More Complete example with dynamical uncertainty

We want to merge two sensors: @@ -1159,12 +1159,13 @@ We want to merge two sensors:

-
-

2.4.1 Dynamical uncertainty of the individual sensors

+
+

2.4.1 Dynamical uncertainty of the individual sensors

We define the weights that are used to characterize the dynamic uncertainty of the sensors.

+ 
omegac = 100*2*pi; G0 = 0.1; Ginf = 10;
 w1 = (Ginf*s/omegac + G0)/(s/omegac + 1);
@@ -1177,7 +1178,7 @@ w2 = w2*17 with the upper and lower bounds on the magnitude and on the phase.
+From the weights, we define the uncertain transfer functions of the sensors. Some of the uncertain dynamics of both sensors are shown on Fig. 17 with the upper and lower bounds on the magnitude and on the phase.
 

 

 
G1 = 1 + w1*ultidyn('Delta',[1 1]);
@@ -1186,7 +1187,7 @@ G2 = 1 
 

 
 
-

+

 
uncertainty_dynamics_sensors.png
 

 
Figure 17: Dynamic uncertainty of the two sensors (png, pdf)

@@ -1194,11 +1195,11 @@ G2 = 1 
 

 

 
-

-
2.4.2 Synthesis objective

+

+
2.4.2 Synthesis objective

 

 

-The uncertainty region of the super sensor dynamics is represented by a circle in the complex plane as shown in Fig. 14.
+The uncertainty region of the super sensor dynamics is represented by a circle in the complex plane as shown in Fig. 14.
 

 
 

@@ -1227,8 +1228,8 @@ The maximum phase shift due to dynamic uncertainty at frequency \(\omega\) will
 

 

 
-

-
2.4.3 Requirements as an \(\mathcal{H}_\infty\) norm

+

+
2.4.3 Requirements as an \(\mathcal{H}_\infty\) norm

 

 

 We know try to express this requirement in terms of an \(\mathcal{H}_\infty\) norm.
@@ -1249,10 +1250,10 @@ Then:
 \end{align*}
 
 

-Which is approximately equivalent to (with an approximation of maximum \(\sqrt{2}\)):
+Which is approximately equivalent to (with an error of maximum \(\sqrt{2}\)):
 

 \begin{equation}
-\label{org6f4cf45}
+\label{org9858a60}
   \left\| \begin{matrix} w_1(s) w_\phi(s) H_1(s) \\ w_2(s) w_\phi(s) H_2(s) \end{matrix} \right\|_\infty < 1
 \end{equation}
 
@@ -1263,11 +1264,11 @@ Thus, at these frequencies, \(|w_\phi|\) should be smaller than \(1\).
 

 

 
-

-
2.4.4 H-Infinity Synthesis

+

+
2.4.4 H-Infinity Synthesis

 

 

-Let's define \(w_\phi(s)\) such that the maximum allowed phase uncertainty \(\Delta\phi_\text{max}\) of the super sensor dynamics is \(30\text{ deg}\) until frequency \(\omega_0 = 500\text{ Hz}\)
+Let's define \(w_\phi(s)\) in order to bound the maximum allowed phase uncertainty \(\Delta\phi_\text{max}\) of the super sensor dynamics.
 

 
 

@@ -1282,25 +1283,32 @@ W2 = w2*wphi;
 

 
 
-

+

+
magnitude_wphi.png
+

+
Figure 18: Magnitude of the weght \(w_\phi(s)\) that is used to bound the uncertainty of the super sensor (png, pdf)

+

+
+
+

 
maximum_wanted_phase_uncertainty.png
 

-
Figure 18: Maximum wanted phase uncertainty using this weight (png, pdf)

+
Figure 19: Maximum wanted phase uncertainty using this weight (png, pdf)

 

 
 

-The obtained upper bounds on the complementary filters in order to limit the phase uncertainty of the super sensor are represented in Fig. 19.
+The obtained upper bounds on the complementary filters in order to limit the phase uncertainty of the super sensor are represented in Fig. 20.
 

 
 
-

+

 
upper_bounds_comp_filter_max_phase_uncertainty.png
 

-
Figure 19: Upper bounds on the complementary filters set in order to limit the maximum phase uncertainty of the super sensor to 30 degrees until 500Hz (png, pdf)

+
Figure 20: Upper bounds on the complementary filters set in order to limit the maximum phase uncertainty of the super sensor to 30 degrees until 500Hz (png, pdf)

 

 
 

-The \(\mathcal{H}_\infty\) synthesis is performed using the defined weights and the obtained complementary filters are shown in Fig. 20.
+The \(\mathcal{H}_\infty\) synthesis is performed using the defined weights and the obtained complementary filters are shown in Fig. 21.
 

 
 

@@ -1347,19 +1355,19 @@ Test bounds:      0.0447 <  gamma  <=      1.3318
 

 
 
-

+

 
comp_filter_hinf_uncertainty.png
 

-
Figure 20: Obtained complementary filters (png, pdf)

+
Figure 21: Obtained complementary filters (png, pdf)

 

 

 

 
-

-
2.4.5 Super sensor uncertainty

+

+
2.4.5 Super sensor uncertainty

 

 

-We can now compute the uncertainty of the super sensor. The result is shown in Fig. 21.
+We can now compute the uncertainty of the super sensor. The result is shown in Fig. 22.
 

 
 

@@ -1368,10 +1376,10 @@ We can now compute the uncertainty of the super sensor. The result is shown in F
 

 
 
-

+

 
super_sensor_uncertainty_bode_plot.png
 

-
Figure 21: Uncertainty on the dynamics of the super sensor (png, pdf)

+
Figure 22: Uncertainty on the dynamics of the super sensor (png, pdf)

 

 
 

@@ -1383,28 +1391,28 @@ We could for instance ask for less uncertainty at low frequency.
 

 

 
-

-
3 Equivalent Super Sensor

+

+
3 Equivalent Super Sensor

 

 

 The goal here is to find the parameters of a single sensor that would best represent a super sensor.
 

 

 
-

-
3.1 Sensor Fusion Architecture

+

+
3.1 Sensor Fusion Architecture

 

 

-Let consider figure 22 where two sensors are merged.
+Let consider figure 23 where two sensors are merged.
 The dynamic uncertainty of each sensor is represented by a weight \(w_i(s)\), the frequency characteristics each of the sensor noise is represented by the weights \(N_i(s)\).
 The noise sources \(\tilde{n}_i\) are considered to be white noise: \(\Phi_{\tilde{n}_i}(\omega) = 1, \ \forall\omega\).
 

 
 
-

+

 
sensor_fusion_full.png
 

-
Figure 22: Sensor fusion architecture (png, pdf).

+
Figure 23: Sensor fusion architecture (png, pdf).

 

 
 
@@ -1431,28 +1439,28 @@ And the noise of the super sensor is:
 

 

 
-

-
3.2 Equivalent Configuration

+

+
3.2 Equivalent Configuration

 

 

-We try to determine \(w_{ss}(s)\) and \(N_{ss}(s)\) such that the sensor on figure 23 is equivalent to the super sensor of figure 22.
+We try to determine \(w_{ss}(s)\) and \(N_{ss}(s)\) such that the sensor on figure 24 is equivalent to the super sensor of figure 23.
 

 
 
-

+

 
sensor_fusion_equivalent.png
 

-
Figure 23: Equivalent Super Sensor (png, pdf).

+
Figure 24: Equivalent Super Sensor (png, pdf).

 

 

 

 
-

-
3.3 Model the uncertainty of the super sensor

+

+
3.3 Model the uncertainty of the super sensor

 

 

-At each frequency \(\omega\), the uncertainty set of the super sensor shown on figure 22 is a circle centered on \(1\) with a radius equal to \(|H_1(j\omega) w_1(j\omega)| + |H_2(j\omega) w_2(j\omega)|\) on the complex plane.
-The uncertainty set of the sensor shown on figure 23 is a circle centered on \(1\) with a radius equal to \(|w_{ss}(j\omega)|\) on the complex plane.
+At each frequency \(\omega\), the uncertainty set of the super sensor shown on figure 23 is a circle centered on \(1\) with a radius equal to \(|H_1(j\omega) w_1(j\omega)| + |H_2(j\omega) w_2(j\omega)|\) on the complex plane.
+The uncertainty set of the sensor shown on figure 24 is a circle centered on \(1\) with a radius equal to \(|w_{ss}(j\omega)|\) on the complex plane.
 

 
 

@@ -1467,11 +1475,11 @@ Ideally, we want to find a weight \(w_{ss}(s)\) so that:
 

 

 
-

-
3.4 Model the noise of the super sensor

+

+
3.4 Model the noise of the super sensor

 

 

-The PSD of the estimation \(\hat{x}\) when \(x = 0\) of the configuration shown on figure 22 is:
+The PSD of the estimation \(\hat{x}\) when \(x = 0\) of the configuration shown on figure 23 is:
 

 \begin{align*}
   \Phi_{\hat{x}}(\omega) &= | H_1(j\omega) N_1(j\omega) |^2 \Phi_{\tilde{n}_1} + | H_2(j\omega) N_2(j\omega) |^2 \Phi_{\tilde{n}_2} \\
@@ -1479,7 +1487,7 @@ The PSD of the estimation \(\hat{x}\) when \(x = 0\) of the configuration shown
 \end{align*}
 
 

-The PSD of the estimation \(\hat{x}\) when \(x = 0\) of the configuration shown on figure 23 is:
+The PSD of the estimation \(\hat{x}\) when \(x = 0\) of the configuration shown on figure 24 is:
 

 \begin{align*}
   \Phi_{\hat{x}}(\omega) &= | N_{ss}(j\omega) |^2 \Phi_{\tilde{n}} \\
@@ -1498,8 +1506,8 @@ Ideally, we want to find a weight \(N_{ss}(s)\) such that:
 

 

 
-

-
3.5 First guess

+

+
3.5 First guess

 

 

 We could choose
@@ -1527,8 +1535,8 @@ and
 

 

 
-

-
4 Optimal And Robust Sensor Fusion in Practice

+

+
4 Optimal And Robust Sensor Fusion in Practice

 

 

 Here are the steps in order to apply optimal and robust sensor fusion:
@@ -1541,26 +1549,26 @@ Here are the steps in order to apply optimal and robust sensor fusion:
 
 

 
-

-
4.1 Measurement of the noise characteristics of the sensors

+

+
4.1 Measurement of the noise characteristics of the sensors

 

 

-

-
4.1.1 Huddle Test

+

+
4.1.1 Huddle Test

 

 

 The technique to estimate the sensor noise is taken from barzilai98_techn_measur_noise_sensor_presen.
 

 
 

-Let's consider two sensors (sensor 1 and sensor 2) that are measuring the same quantity \(x\) as shown in figure 24.
+Let's consider two sensors (sensor 1 and sensor 2) that are measuring the same quantity \(x\) as shown in figure 25.
 

 
 
-

+

 
huddle_test.png
 

-
Figure 24: Huddle test block diagram

+
Figure 25: Huddle test block diagram

 

 
 

@@ -1576,7 +1584,7 @@ We also assume that their dynamics is ideal: \(G_1(s) = G_2(s) = 1\).
 We then have:
 

 \begin{equation}
-\label{org51dae3b}
+\label{orgd3bc3e3}
   \gamma_{\hat{x}_1\hat{x}_2}^2(\omega) = \frac{1}{1 + 2 \left( \frac{|\Phi_n(\omega)|}{|\Phi_{\hat{x}}(\omega)|} \right) + \left( \frac{|\Phi_n(\omega)|}{|\Phi_{\hat{x}}(\omega)|} \right)^2}
 \end{equation}
 
@@ -1584,16 +1592,16 @@ We then have:
 Since the input signal \(x\) and the instrumental noise \(n\) are incoherent:
 

 \begin{equation}
-\label{orgec4c52a}
+\label{org3e2ed3a}
   |\Phi_{\hat{x}}(\omega)| = |\Phi_n(\omega)| + |\Phi_x(\omega)|
 \end{equation}
 
 

-From equations \eqref{org51dae3b} and \eqref{orgec4c52a}, we finally obtain
+From equations \eqref{orgd3bc3e3} and \eqref{org3e2ed3a}, we finally obtain
 

 

 \begin{equation}
-\label{orgdc4f127}
+\label{org53f418b}
   |\Phi_n(\omega)| = |\Phi_{\hat{x}}(\omega)| \left( 1 - \sqrt{\gamma_{\hat{x}_1\hat{x}_2}^2(\omega)} \right)
 \end{equation}
 
@@ -1601,8 +1609,8 @@ From equations \eqref{org51dae3b} and \eqref{orgec4c52a}, we finally obtain
 

 

 
-

-
4.1.2 Weights that represents the noises' PSD

+

+
4.1.2 Weights that represents the noises' PSD

 

 

 For further complementary filter synthesis, it is preferred to consider a normalized noise source \(\tilde{n}\) that has a PSD equal to one (\(\Phi_{\tilde{n}}(\omega) = 1\)) and to use a weighting filter \(N(s)\) in order to represent the frequency dependence of the noise.
@@ -1621,20 +1629,20 @@ These weighting filters can then be used to compare the noise level of sensors f
 

 
 

-The sensor with a normalized noise input is shown in figure 25.
+The sensor with a normalized noise input is shown in figure 26.
 

 
 
-

+

 
one_sensor_normalized_noise.png
 

-
Figure 25: One sensor with normalized noise

+
Figure 26: One sensor with normalized noise

 

 

 

 
-

-
4.1.3 Comparison of the noises' PSD

+

+
4.1.3 Comparison of the noises' PSD

 

 

 Once the noise of the sensors to be merged have been characterized, the power spectral density of both sensors have to be compared.
@@ -1650,8 +1658,8 @@ Ideally, the PSD of the noise are such that:
 

 

 
-

-
4.1.4 Computation of the coherence, power spectral density and cross spectral density of signals

+

+
4.1.4 Computation of the coherence, power spectral density and cross spectral density of signals

 

 

 The coherence between signals \(x\) and \(y\) is defined as follow
@@ -1679,11 +1687,11 @@ where:
 

 

 
-

-
4.2 Estimate the dynamic uncertainty of the sensors

+

+
4.2 Estimate the dynamic uncertainty of the sensors

 

 

-Let's consider one sensor represented on figure 26.
+Let's consider one sensor represented on figure 27.
 

 
 

@@ -1695,16 +1703,16 @@ The goal is to accurately determine \(w(s)\) for the sensors that have to be mer
 

 
 
-

+

 
one_sensor_dyn_uncertainty.png
 

-
Figure 26: Sensor with dynamic uncertainty

+
Figure 27: Sensor with dynamic uncertainty

 

 

 

 
-

-
4.3 Optimal and Robust synthesis of the complementary filters

+

+
4.3 Optimal and Robust synthesis of the complementary filters

 

 

 Once the noise characteristics and dynamic uncertainty of both sensors have been determined and we have determined the following weighting functions:
@@ -1715,7 +1723,7 @@ Once the noise characteristics and dynamic uncertainty of both sensors have been
 
 
 

-The goal is to design complementary filters \(H_1(s)\) and \(H_2(s)\) shown in figure 22 such that:
+The goal is to design complementary filters \(H_1(s)\) and \(H_2(s)\) shown in figure 23 such that:
 

 
    
 
  • the uncertainty on the super sensor dynamics is minimized
    • 
@@ -1723,24 +1731,24 @@ The goal is to design complementary filters \(H_1(s)\) and \(H_2(s)\) shown in f
 

 
 
-

+

 
sensor_fusion_full.png
 

-
Figure 27: Sensor fusion architecture with sensor dynamics uncertainty

+
Figure 28: Sensor fusion architecture with sensor dynamics uncertainty

 

 

 

 

 
-

-
5 Methods of complementary filter synthesis

+

+
5 Methods of complementary filter synthesis

 

 

-

-
5.1 Complementary filters using analytical formula

+

+
5.1 Complementary filters using analytical formula

 

 

-
+
 

 

 

@@ -1750,8 +1758,8 @@ All the files (data and Matlab scripts) are accessible 
-
5.1.1 Analytical 1st order complementary filters

+

+
5.1.1 Analytical 1st order complementary filters

 

 

 First order complementary filters are defined with following equations:
@@ -1762,7 +1770,7 @@ First order complementary filters are defined with following equations:
 \end{align}
 
 

-Their bode plot is shown figure 28.
+Their bode plot is shown figure 29.
 

 
 

@@ -1774,16 +1782,16 @@ Hl1 = 1
 

 
 
-

+

 
comp_filter_1st_order.png
 

-
Figure 28: Bode plot of first order complementary filter (png, pdf)

+
Figure 29: Bode plot of first order complementary filter (png, pdf)

 

 

 

 
-

-
5.1.2 Second Order Complementary Filters

+

+
5.1.2 Second Order Complementary Filters

 

 

 We here use analytical formula for the complementary filters \(H_L\) and \(H_H\).
@@ -1809,15 +1817,15 @@ where:
 
 
 

-This is illustrated on figure 29.
+This is illustrated on figure 30.
 The slope of those filters at high and low frequencies is \(-2\) and \(2\) respectively for \(H_L\) and \(H_H\).
 

 
 
-

+

 
comp_filters_param_alpha.png
 

-
Figure 29: Effect of the parameter \(\alpha\) on the shape of the generated second order complementary filters (png, pdf)

+
Figure 30: Effect of the parameter \(\alpha\) on the shape of the generated second order complementary filters (png, pdf)

 

 
 

@@ -1832,16 +1840,16 @@ xlabel('$
+

 
param_alpha_hinf_norm.png
 

-
Figure 30: Evolution of the H-Infinity norm of the complementary filters with the parameter \(\alpha\) (png, pdf)

+
Figure 31: Evolution of the H-Infinity norm of the complementary filters with the parameter \(\alpha\) (png, pdf)

 

 

 

 
-

-
5.1.3 Third Order Complementary Filters

+

+
5.1.3 Third Order Complementary Filters

 

 

 The following formula gives complementary filters with slopes of \(-3\) and \(3\):
@@ -1860,7 +1868,7 @@ The parameters are:
 
 
 

-The filters are defined below and the result is shown on figure 31.
+The filters are defined below and the result is shown on figure 32.
 

 
 

@@ -1874,20 +1882,20 @@ Hl3_ana = (
+

 
complementary_filters_third_order.png
 

-
Figure 31: Third order complementary filters using the analytical formula (png, pdf)

+
Figure 32: Third order complementary filters using the analytical formula (png, pdf)

 

 

 

 

 
-

-
5.2 H-Infinity synthesis of complementary filters

+

+
5.2 H-Infinity synthesis of complementary filters

 

 

-
+
 

 

 

@@ -1897,8 +1905,8 @@ All the files (data and Matlab scripts) are accessible 
-
5.2.1 Synthesis Architecture

+

+
5.2.1 Synthesis Architecture

 

 

 We here synthesize the complementary filters using the \(\mathcal{H}_\infty\) synthesis.
@@ -1906,18 +1914,18 @@ The goal is to specify upper bounds on the norms of \(H_L\) and \(H_H\) while en
 

 
 

-In order to do so, we use the generalized plant shown on figure 32 where \(w_L\) and \(w_H\) weighting transfer functions that will be used to shape \(H_L\) and \(H_H\) respectively.
+In order to do so, we use the generalized plant shown on figure 33 where \(w_L\) and \(w_H\) weighting transfer functions that will be used to shape \(H_L\) and \(H_H\) respectively.
 

 
 
-

+

 
sf_hinf_filters_plant_b.png
 

-
Figure 32: Generalized plant used for the \(\mathcal{H}_\infty\) synthesis of the complementary filters

+
Figure 33: Generalized plant used for the \(\mathcal{H}_\infty\) synthesis of the complementary filters

 

 
 

-The \(\mathcal{H}_\infty\) synthesis applied on this generalized plant will give a transfer function \(H_L\) (figure 33) such that the \(\mathcal{H}_\infty\) norm of the transfer function from \(w\) to \([z_H,\ z_L]\) is less than one:
+The \(\mathcal{H}_\infty\) synthesis applied on this generalized plant will give a transfer function \(H_L\) (figure 34) such that the \(\mathcal{H}_\infty\) norm of the transfer function from \(w\) to \([z_H,\ z_L]\) is less than one:
 \[ \left\| \begin{array}{c} H_L w_L \\ (1 - H_L) w_H \end{array} \right\|_\infty < 1 \]
 

 
@@ -1936,16 +1944,16 @@ We then see that \(w_L\) and \(w_H\) can be used to shape both \(H_L\) and \(H_H
 

 
 
-

+

 
sf_hinf_filters_b.png
 

-
Figure 33: \(\mathcal{H}_\infty\) synthesis of the complementary filters

+
Figure 34: \(\mathcal{H}_\infty\) synthesis of the complementary filters

 

 

 

 
-

-
5.2.2 Weights

+

+
5.2.2 Weights

 

 

 
omegab = 2*pi*9;
@@ -1956,16 +1964,16 @@ wL = (s 
+

 
weights_wl_wh.png
 

-
Figure 34: Weights on the complementary filters \(w_L\) and \(w_H\) and the associated performance weights (png, pdf)

+
Figure 35: Weights on the complementary filters \(w_L\) and \(w_H\) and the associated performance weights (png, pdf)

 

 

 

 
-

-
5.2.3 H-Infinity Synthesis

+

+
5.2.3 H-Infinity Synthesis

 

 

 We define the generalized plant \(P\) on matlab.
@@ -2007,7 +2015,7 @@ Test bounds:      0.0000 <  gamma  <=      1.7285

-We then define the high pass filter \(H_H = 1 - H_L\). The bode plot of both \(H_L\) and \(H_H\) is shown on figure 35. +We then define the high pass filter \(H_H = 1 - H_L\). The bode plot of both \(H_L\) and \(H_H\) is shown on figure 36. 

Hh_hinf = 1 - Hl_hinf;
@@ -2016,28 +2024,28 @@ We then define the high pass filter \(H_H = 1 - H_L\). The bode plot of both \(H
-
-

5.2.4 Obtained Complementary Filters

+
+

5.2.4 Obtained Complementary Filters

-The obtained complementary filters are shown on figure 35. +The obtained complementary filters are shown on figure 36.

-
+

hinf_filters_results.png

-

Figure 35: Obtained complementary filters using \(\mathcal{H}_\infty\) synthesis (png, pdf)

+

Figure 36: Obtained complementary filters using \(\mathcal{H}_\infty\) synthesis (png, pdf)

-
-

5.3 Feedback Control Architecture to generate Complementary Filters

+
+

5.3 Feedback Control Architecture to generate Complementary Filters

- +

The idea is here to use the fact that in a classical feedback architecture, \(S + T = 1\), in order to design complementary filters. @@ -2054,14 +2062,14 @@ All the files (data and Matlab scripts) are accessible -

5.3.1 Architecture

+
+

5.3.1 Architecture

-
+

complementary_filters_feedback_architecture.png

-

Figure 36: Architecture used to generate the complementary filters

+

Figure 37: Architecture used to generate the complementary filters

@@ -2085,8 +2093,8 @@ Which contains two integrator and a lead. \(\omega_c\) is used to tune the cross

-
-

5.3.2 Loop Gain Design

+ -
-

5.3.3 Complementary Filters Obtained

+
+

5.3.3 Complementary Filters Obtained

We then compute the resulting low pass and high pass filters. @@ -2121,25 +2129,25 @@ Hh = 1/

-
+

low_pass_high_pass_filters.png

-

Figure 38: Low pass and High pass filters \(H_L\) and \(H_H\) for different values of \(\alpha\) (png, pdf)

+

Figure 39: Low pass and High pass filters \(H_L\) and \(H_H\) for different values of \(\alpha\) (png, pdf)

-
-

5.4 Analytical Formula found in the literature

+
+

5.4 Analytical Formula found in the literature

- +

-
-

5.4.1 Analytical Formula

+
+

5.4.1 Analytical Formula

min15_compl_filter_desig_angle_estim @@ -2188,8 +2196,8 @@ Hh = 1/

-
-

5.4.2 Matlab

+
+

5.4.2 Matlab

omega0 = 1*2*pi; % [rad/s]
@@ -2207,16 +2215,16 @@ HL3 = (
+

 
comp_filters_literature.png
 

-
Figure 39: Comparison of some complementary filters found in the literature (png, pdf)

+
Figure 40: Comparison of some complementary filters found in the literature (png, pdf)
-
-

5.4.3 Discussion

+
+

5.4.3 Discussion

Analytical Formula found in the literature provides either no parameter for tuning the robustness / performance trade-off. @@ -2225,11 +2233,11 @@ Analytical Formula found in the literature provides either no parameter for tuni

-
-

5.5 Comparison of the different methods of synthesis

+
+

5.5 Comparison of the different methods of synthesis

- + The generated complementary filters using \(\mathcal{H}_\infty\) and the analytical formulas are very close to each other. However there is some difference to note here:

    @@ -2255,7 +2263,7 @@ The generated complementary filters using \(\mathcal{H}_\infty\) and the analyti

Author: Thomas Dehaeze

-

Created: 2019-08-29 jeu. 22:47

+

Created: 2019-08-30 ven. 09:17

Validate

diff --git a/matlab/index.org b/matlab/index.org index 831ad4c..7335e02 100644 --- a/matlab/index.org +++ b/matlab/index.org @@ -645,17 +645,8 @@ To do so, we model the uncertainty that we have on the sensor dynamics by multip The objective here is to design complementary filters $H_1(s)$ and $H_2(s)$ in order to minimize the dynamical uncertainty of the super sensor. ** ZIP file containing the data and matlab files :ignore: -#+begin_src bash :exports none :results none - if [ matlab/comp_filter_robustness.m -nt data/comp_filter_robustness.zip ]; then - cp matlab/comp_filter_robustness.m comp_filter_robustness.m; - zip data/comp_filter_robustness \ - comp_filter_robustness.m - rm comp_filter_robustness.m; - fi -#+end_src - #+begin_note - All the files (data and Matlab scripts) are accessible [[file:data/comp_filter_robustness.zip][here]]. + All the files (data and Matlab scripts) are accessible [[file:matlab/comp_filter_robustness.m][here]]. #+end_note ** Matlab Init :noexport:ignore: @@ -667,10 +658,6 @@ The objective here is to design complementary filters $H_1(s)$ and $H_2(s)$ in o <> #+end_src -#+begin_src matlab - freqs = logspace(-1, 3, 1000); -#+end_src - ** Unknown sensor dynamics dynamics In practical systems, the sensor dynamics has always some level of uncertainty. Let's represent that with multiplicative input uncertainty as shown on figure [[fig:sensor_fusion_dynamic_uncertainty]]. @@ -746,6 +733,10 @@ Let's consider two ideal sensors except one sensor has not an expected unity gai 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. +#+begin_src matlab :exports none + freqs = logspace(-1, 1, 1000); +#+end_src + #+begin_src matlab :exports none w0 = 2*pi; alpha = 2; @@ -761,8 +752,6 @@ The complementary filters shown in blue does not present a bump as the red ones #+end_src #+begin_src matlab :exports none - freqs = logspace(-1, 1, 1000); - figure; % Magnitude ax1 = subaxis(2,1,1); @@ -805,8 +794,6 @@ We then compute the bode plot of the super sensor transfer function $H_1(s)G_1(s We see that the blue complementary filters with a lower maximum norm permits to limit the phase lag introduced by the gain mismatch. #+begin_src matlab :exports none - freqs = logspace(-1, 1, 1000); - figure; % Magnitude ax1 = subaxis(2,1,1); @@ -848,6 +835,11 @@ We want to merge two sensors: *** Dynamical uncertainty of the individual sensors We define the weights that are used to characterize the dynamic uncertainty of the sensors. + +#+begin_src matlab :exports none + freqs = logspace(-1, 3, 1000); +#+end_src + #+begin_src matlab omegac = 100*2*pi; G0 = 0.1; Ginf = 10; w1 = (Ginf*s/omegac + G0)/(s/omegac + 1); @@ -963,7 +955,7 @@ Then: \Longleftrightarrow & \left| w_1(j\omega) H_1(j\omega) w_\phi(j\omega) \right| + \left| w_2(j\omega) H_2(j\omega) w_\phi(j\omega) \right| < 1, \quad \forall\omega \end{align*} -Which is approximately equivalent to (with an approximation of maximum $\sqrt{2}$): +Which is approximately equivalent to (with an error of maximum $\sqrt{2}$): #+name: eq:hinf_conf_phase_uncertainty \begin{equation} \left\| \begin{matrix} w_1(s) w_\phi(s) H_1(s) \\ w_2(s) w_\phi(s) H_2(s) \end{matrix} \right\|_\infty < 1 @@ -973,7 +965,7 @@ On should not forget that at frequency where both sensors has unknown dynamics ( Thus, at these frequencies, $|w_\phi|$ should be smaller than $1$. *** H-Infinity Synthesis -Let's define $w_\phi(s)$ such that the maximum allowed phase uncertainty $\Delta\phi_\text{max}$ of the super sensor dynamics is $30\text{ deg}$ until frequency $\omega_0 = 500\text{ Hz}$ +Let's define $w_\phi(s)$ in order to bound the maximum allowed phase uncertainty $\Delta\phi_\text{max}$ of the super sensor dynamics. #+begin_src matlab Dphi = 20; % [deg] @@ -985,6 +977,26 @@ Let's define $w_\phi(s)$ such that the maximum allowed phase uncertainty $\Delta W2 = w2*wphi; #+end_src +#+begin_src matlab :exports none + 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'); +#+end_src + +#+HEADER: :tangle no :exports results :results none :noweb yes +#+begin_src matlab :var filepath="figs/magnitude_wphi.pdf" :var figsize="full-normal" :post pdf2svg(file=*this*, ext="png") + <> +#+end_src + +#+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]] + #+begin_src matlab :exports none % We here compute the wanted maximum and minimum phase of the super sensor Dphimax = 180/pi*asin(1./abs(squeeze(freqresp(wphi, freqs, 'Hz')))); @@ -1028,7 +1040,7 @@ The obtained upper bounds on the complementary filters in order to limit the pha #+end_src #+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/upper_bounds_comp_filter_max_phase_uncertainty.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") +#+begin_src matlab :var filepath="figs/upper_bounds_comp_filter_max_phase_uncertainty.pdf" :var figsize="full-normal" :post pdf2svg(file=*this*, ext="png") <> #+end_src diff --git a/matlab/matlab/comp_filter_robustness.m b/matlab/matlab/comp_filter_robustness.m new file mode 100644 index 0000000..b6f705f --- /dev/null +++ b/matlab/matlab/comp_filter_robustness.m @@ -0,0 +1,378 @@ +%% Clear Workspace and Close figures +clear; close all; clc; + +%% Intialize Laplace variable +s = zpk('s'); + +% First Basic Example with gain mismatch +% 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 = subaxis(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 = subaxis(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 = subaxis(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 = subaxis(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)]); + +% Dynamical uncertainty of the individual sensors +% We define the weights that are used to characterize the dynamic uncertainty of the sensors. + + +freqs = logspace(-1, 3, 1000); + +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); + + + +% 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; + +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), '--'); +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 +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; +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 +set(gca,'xscale','log'); +yticks(-180:90:180); +ylim([-180 180]); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +linkaxes([ax1,ax2],'x'); + +% H-Infinity Synthesis +% Let's define $w_\phi(s)$ in order to bound the maximum allowed phase uncertainty $\Delta\phi_\text{max}$ of the super sensor dynamics. + + +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'); + + + +% #+NAME: fig:upper_bounds_comp_filter_max_phase_uncertainty +% #+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]]. + + +P = [W1 -W1; + 0 W2; + 1 0]; + + + +% And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command. + +[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', '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.0447 < gamma <= 1.3318 + +% 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 +% #+end_example + + +H1 = 1 - H2; + +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',[]); +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]); + +% 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]]. + + +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 = 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'); diff --git a/matlab/matlab/comp_filters_analytical.m b/matlab/matlab/comp_filters_analytical.m new file mode 100644 index 0000000..0c51a9a --- /dev/null +++ b/matlab/matlab/comp_filters_analytical.m @@ -0,0 +1,166 @@ +%% Clear Workspace and Close figures +clear; close all; clc; + +%% Intialize Laplace variable +s = zpk('s'); + +freqs = logspace(-1, 3, 1000); + +% Analytical 1st order complementary filters +% First order complementary filters are defined with following equations: +% \begin{align} +% H_L(s) = \frac{1}{1 + \frac{s}{\omega_0}}\\ +% H_H(s) = \frac{\frac{s}{\omega_0}}{1 + \frac{s}{\omega_0}} +% \end{align} + +% Their bode plot is shown figure [[fig:comp_filter_1st_order]]. + + +w0 = 2*pi; % [rad/s] + +Hh1 = (s/w0)/((s/w0)+1); +Hl1 = 1/((s/w0)+1); + +freqs = logspace(-2, 2, 1000); + +figure; +% Magnitude +ax1 = subaxis(2,1,1); +hold on; +set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(Hh1, freqs, 'Hz')))); +set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(Hl1, freqs, 'Hz')))); +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +set(gca, 'XTickLabel',[]); +ylabel('Magnitude'); +hold off; +% Phase +ax2 = subaxis(2,1,2); +hold on; +set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(Hh1, freqs, 'Hz')))); +set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(Hl1, 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)]); + +% Second Order Complementary Filters +% We here use analytical formula for the complementary filters $H_L$ and $H_H$. + +% The first two formulas that are used to generate complementary filters are: +% \begin{align*} +% H_L(s) &= \frac{(1+\alpha) (\frac{s}{\omega_0})+1}{\left((\frac{s}{\omega_0})+1\right) \left((\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1\right)}\\ +% H_H(s) &= \frac{(\frac{s}{\omega_0})^2 \left((\frac{s}{\omega_0})+1+\alpha\right)}{\left((\frac{s}{\omega_0})+1\right) \left((\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1\right)} +% \end{align*} +% where: +% - $\omega_0$ is the blending frequency in rad/s. +% - $\alpha$ is used to change the shape of the filters: +% - Small values for $\alpha$ will produce high magnitude of the filters $|H_L(j\omega)|$ and $|H_H(j\omega)|$ near $\omega_0$ but smaller value for $|H_L(j\omega)|$ above $\approx 1.5 \omega_0$ and for $|H_H(j\omega)|$ below $\approx 0.7 \omega_0$ +% - A large $\alpha$ will do the opposite + +% This is illustrated on figure [[fig:comp_filters_param_alpha]]. +% The slope of those filters at high and low frequencies is $-2$ and $2$ respectively for $H_L$ and $H_H$. + + +freqs_study = logspace(-2, 2, 10000); +alphas = [0.1, 1, 10]; +w0 = 2*pi*1; + +figure; +ax1 = subaxis(2,1,1); +hold on; +for i = 1:length(alphas) + alpha = alphas(i); + Hh2 = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1)); + Hl2 = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1)); + set(gca,'ColorOrderIndex',i); + plot(freqs_study, abs(squeeze(freqresp(Hh2, freqs_study, 'Hz')))); + set(gca,'ColorOrderIndex',i); + plot(freqs_study, abs(squeeze(freqresp(Hl2, freqs_study, 'Hz')))); +end +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +set(gca, 'XTickLabel',[]); +ylabel('Magnitude'); +hold off; +ylim([1e-3, 20]); +% Phase +ax2 = subaxis(2,1,2); +hold on; +for i = 1:length(alphas) + alpha = alphas(i); + Hh2 = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1)); + Hl2 = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1)); + set(gca,'ColorOrderIndex',i); + plot(freqs_study, 180/pi*angle(squeeze(freqresp(Hh2, freqs_study, 'Hz'))), 'DisplayName', sprintf('$\\alpha = %g$', alpha)); + set(gca,'ColorOrderIndex',i); + plot(freqs_study, 180/pi*angle(squeeze(freqresp(Hl2, freqs_study, 'Hz'))), 'HandleVisibility', 'off'); +end +set(gca,'xscale','log'); +yticks(-180:90:180); +ylim([-180 180]); +xlabel('Relative Frequency $\frac{\omega}{\omega_0}$'); ylabel('Phase [deg]'); +legend('Location', 'northeast'); +hold off; +linkaxes([ax1,ax2],'x'); +xlim([freqs_study(1), freqs_study(end)]); + + + +% #+NAME: fig:comp_filters_param_alpha +% #+CAPTION: Effect of the parameter $\alpha$ on the shape of the generated second order complementary filters ([[./figs/comp_filters_param_alpha.png][png]], [[./figs/comp_filters_param_alpha.pdf][pdf]]) +% [[file:figs/comp_filters_param_alpha.png]] + +% We now study the maximum norm of the filters function of the parameter $\alpha$. As we saw that the maximum norm of the filters is important for the robust merging of filters. + +alphas = logspace(-2, 2, 100); +w0 = 2*pi*1; +infnorms = zeros(size(alphas)); + +for i = 1:length(alphas) + alpha = alphas(i); + Hh2 = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1)); + Hl2 = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1)); + infnorms(i) = norm(Hh2, 'inf'); +end + +figure; +plot(alphas, infnorms) +set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log'); +xlabel('$\alpha$'); ylabel('$\|H_1\|_\infty$'); + +% Third Order Complementary Filters +% The following formula gives complementary filters with slopes of $-3$ and $3$: +% \begin{align*} +% H_L(s) &= \frac{\left(1+(\alpha+1)(\beta+1)\right) (\frac{s}{\omega_0})^2 + (1+\alpha+\beta)(\frac{s}{\omega_0}) + 1}{\left(\frac{s}{\omega_0} + 1\right) \left( (\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1 \right) \left( (\frac{s}{\omega_0})^2 + \beta (\frac{s}{\omega_0}) + 1 \right)}\\ +% H_H(s) &= \frac{(\frac{s}{\omega_0})^3 \left( (\frac{s}{\omega_0})^2 + (1+\alpha+\beta) (\frac{s}{\omega_0}) + (1+(\alpha+1)(\beta+1)) \right)}{\left(\frac{s}{\omega_0} + 1\right) \left( (\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1 \right) \left( (\frac{s}{\omega_0})^2 + \beta (\frac{s}{\omega_0}) + 1 \right)} +% \end{align*} + +% The parameters are: +% - $\omega_0$ is the blending frequency in rad/s +% - $\alpha$ and $\beta$ that are used to change the shape of the filters similarly to the parameter $\alpha$ for the second order complementary filters + +% The filters are defined below and the result is shown on figure [[fig:complementary_filters_third_order]]. + + +alpha = 1; +beta = 10; +w0 = 2*pi*14; + +Hh3_ana = (s/w0)^3 * ((s/w0)^2 + (1+alpha+beta)*(s/w0) + (1+(alpha+1)*(beta+1)))/((s/w0 + 1)*((s/w0)^2+alpha*(s/w0)+1)*((s/w0)^2+beta*(s/w0)+1)); +Hl3_ana = ((1+(alpha+1)*(beta+1))*(s/w0)^2 + (1+alpha+beta)*(s/w0) + 1)/((s/w0 + 1)*((s/w0)^2+alpha*(s/w0)+1)*((s/w0)^2+beta*(s/w0)+1)); + +figure; +hold on; +set(gca,'ColorOrderIndex',1) +plot(freqs, abs(squeeze(freqresp(Hl3_ana, freqs, 'Hz'))), '-', 'DisplayName', '$H_L$ - Analytical'); +set(gca,'ColorOrderIndex',2) +plot(freqs, abs(squeeze(freqresp(Hh3_ana, freqs, 'Hz'))), '-', 'DisplayName', '$H_H$ - Analytical'); +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Magnitude'); +hold off; +xlim([freqs(1), freqs(end)]); +ylim([1e-3, 10]); +xticks([0.1, 1, 10, 100, 1000]); +legend('location', 'northeast'); diff --git a/matlab/matlab/comp_filters_design.m b/matlab/matlab/comp_filters_design.m new file mode 100644 index 0000000..6f90b45 --- /dev/null +++ b/matlab/matlab/comp_filters_design.m @@ -0,0 +1,53 @@ +%% Clear Workspace and Close figures +clear; close all; clc; + +%% Intialize Laplace variable +s = zpk('s'); + +% Matlab + +omega0 = 1*2*pi; % [rad/s] +tau = 1/omega0; % [s] + +% From cite:corke04_inert_visual_sensin_system_small_auton_helic +HL1 = 1/(s/omega0 + 1); HH1 = s/omega0/(s/omega0 + 1); + +% From cite:jensen13_basic_uas +HL2 = (2*omega0*s + omega0^2)/(s+omega0)^2; HH2 = s^2/(s+omega0)^2; + +% From cite:shaw90_bandw_enhan_posit_measur_using_measur_accel +HL3 = (3*tau*s + 1)/(tau*s + 1)^3; HH3 = (tau^3*s^3 + 3*tau^2*s^2)/(tau*s + 1)^3; + +freqs = logspace(-1, 1, 1000); + +figure; +% Magnitude +ax1 = subaxis(2,1,1); +hold on; +set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(HH1, freqs, 'Hz')))); +set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(HL1, freqs, 'Hz')))); +set(gca,'ColorOrderIndex',2); plot(freqs, abs(squeeze(freqresp(HH2, freqs, 'Hz')))); +set(gca,'ColorOrderIndex',2); plot(freqs, abs(squeeze(freqresp(HL2, freqs, 'Hz')))); +set(gca,'ColorOrderIndex',3); plot(freqs, abs(squeeze(freqresp(HH3, freqs, 'Hz')))); +set(gca,'ColorOrderIndex',3); plot(freqs, abs(squeeze(freqresp(HL3, freqs, 'Hz')))); +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +set(gca, 'XTickLabel',[]); +ylabel('Magnitude'); +hold off; +ylim([1e-2 2]); +% Phase +ax2 = subaxis(2,1,2); +hold on; +set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(HH1, freqs, 'Hz')))); +set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(HL1, freqs, 'Hz')))); +set(gca,'ColorOrderIndex',2); plot(freqs, 180/pi*angle(squeeze(freqresp(HH2, freqs, 'Hz')))); +set(gca,'ColorOrderIndex',2); plot(freqs, 180/pi*angle(squeeze(freqresp(HL2, freqs, 'Hz')))); +set(gca,'ColorOrderIndex',3); plot(freqs, 180/pi*angle(squeeze(freqresp(HH3, freqs, 'Hz')))); +set(gca,'ColorOrderIndex',3); plot(freqs, 180/pi*angle(squeeze(freqresp(HL3, 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)]); diff --git a/matlab/matlab/feedback_generate_comp_filters.m b/matlab/matlab/feedback_generate_comp_filters.m new file mode 100644 index 0000000..65891fb --- /dev/null +++ b/matlab/matlab/feedback_generate_comp_filters.m @@ -0,0 +1,58 @@ +%% Clear Workspace and Close figures +clear; close all; clc; + +%% Intialize Laplace variable +s = zpk('s'); + +freqs = logspace(-2, 2, 1000); + +% Loop Gain Design +% Let's first define the loop gain $L$. + +wc = 2*pi*1; +alpha = 2; + +L = (wc/s)^2 * (s/(wc/alpha) + 1)/(s/wc + alpha); + +figure; + +ax1 = subplot(2,1,1); +plot(freqs, abs(squeeze(freqresp(L, freqs, 'Hz'))), '-'); +ylabel('Magnitude'); +set(gca, 'XScale', 'log'); +set(gca, 'YScale', 'log'); + +ax2 = subplot(2,1,2); +plot(freqs, 180/pi*phase(squeeze(freqresp(L, freqs, 'Hz'))), '--'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +set(gca, 'XScale', 'log'); +ylim([-180, 0]); +yticks([-360:90:360]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); +xticks([0.1, 1, 10, 100, 1000]); + +% Complementary Filters Obtained +% We then compute the resulting low pass and high pass filters. + +Hl = L/(L + 1); +Hh = 1/(L + 1); + +alphas = [1, 2, 10]; + +figure; +hold on; +for i = 1:length(alphas) + alpha = alphas(i); + L = (wc/s)^2 * (s/(wc/alpha) + 1)/(s/wc + alpha); + Hl = L/(L + 1); + Hh = 1/(L + 1); + set(gca,'ColorOrderIndex',i) + plot(freqs, abs(squeeze(freqresp(Hl, freqs, 'Hz'))), 'DisplayName', sprintf('$\\alpha = %.0f$', alpha)); + set(gca,'ColorOrderIndex',i) + plot(freqs, abs(squeeze(freqresp(Hh, freqs, 'Hz'))), 'HandleVisibility', 'off'); +end +set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude') +legend('location', 'northeast'); diff --git a/matlab/matlab/h_inf_synthesis_complementary_filters.m b/matlab/matlab/h_inf_synthesis_complementary_filters.m new file mode 100644 index 0000000..f0ee8b5 --- /dev/null +++ b/matlab/matlab/h_inf_synthesis_complementary_filters.m @@ -0,0 +1,94 @@ +%% Clear Workspace and Close figures +clear; close all; clc; + +%% Intialize Laplace variable +s = zpk('s'); + +freqs = logspace(-1, 3, 1000); + +% Weights + + +omegab = 2*pi*9; +wH = (omegab)^2/(s + omegab*sqrt(1e-5))^2; +omegab = 2*pi*28; +wL = (s + omegab/(4.5)^(1/3))^3/(s*(1e-4)^(1/3) + omegab)^3; + +figure; +hold on; +set(gca,'ColorOrderIndex',1) +plot(freqs, 1./abs(squeeze(freqresp(wL, freqs, 'Hz'))), '-', 'DisplayName', '$w_L$'); +set(gca,'ColorOrderIndex',2) +plot(freqs, 1./abs(squeeze(freqresp(wH, freqs, 'Hz'))), '-', 'DisplayName', '$w_H$'); +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Magnitude'); +hold off; +xlim([freqs(1), freqs(end)]); +ylim([1e-3, 10]); +xticks([0.1, 1, 10, 100, 1000]); +legend('location', 'northeast'); + +% H-Infinity Synthesis +% We define the generalized plant $P$ on matlab. + +P = [0 wL; + wH -wH; + 1 0]; + + + +% And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command. + +[Hl_hinf, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on'); + + + +% #+RESULTS: +% #+begin_example +% [Hl_hinf, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on'); +% Test bounds: 0.0000 < gamma <= 1.7285 + +% gamma hamx_eig xinf_eig hamy_eig yinf_eig nrho_xy p/f +% 1.729 4.1e+01 8.4e-12 1.8e-01 0.0e+00 0.0000 p +% 0.864 3.9e+01 -5.8e-02# 1.8e-01 0.0e+00 0.0000 f +% 1.296 4.0e+01 8.4e-12 1.8e-01 0.0e+00 0.0000 p +% 1.080 4.0e+01 8.5e-12 1.8e-01 0.0e+00 0.0000 p +% 0.972 3.9e+01 -4.2e-01# 1.8e-01 0.0e+00 0.0000 f +% 1.026 4.0e+01 8.5e-12 1.8e-01 0.0e+00 0.0000 p +% 0.999 3.9e+01 8.5e-12 1.8e-01 0.0e+00 0.0000 p +% 0.986 3.9e+01 -1.2e+00# 1.8e-01 0.0e+00 0.0000 f +% 0.993 3.9e+01 -8.2e+00# 1.8e-01 0.0e+00 0.0000 f +% 0.996 3.9e+01 8.5e-12 1.8e-01 0.0e+00 0.0000 p +% 0.994 3.9e+01 8.5e-12 1.8e-01 0.0e+00 0.0000 p +% 0.993 3.9e+01 -3.2e+01# 1.8e-01 0.0e+00 0.0000 f + +% Gamma value achieved: 0.9942 +% #+end_example + +% We then define the high pass filter $H_H = 1 - H_L$. The bode plot of both $H_L$ and $H_H$ is shown on figure [[fig:hinf_filters_results]]. + +Hh_hinf = 1 - Hl_hinf; + +% Obtained Complementary Filters + +% The obtained complementary filters are shown on figure [[fig:hinf_filters_results]]. + + +figure; +hold on; +set(gca,'ColorOrderIndex',1) +plot(freqs, 1./abs(squeeze(freqresp(wL, freqs, 'Hz'))), '--', 'DisplayName', '$w_L$'); +set(gca,'ColorOrderIndex',2) +plot(freqs, 1./abs(squeeze(freqresp(wH, freqs, 'Hz'))), '--', 'DisplayName', '$w_H$'); + +set(gca,'ColorOrderIndex',1) +plot(freqs, abs(squeeze(freqresp(Hl_hinf, freqs, 'Hz'))), '-', 'DisplayName', '$H_L$ - $\mathcal{H}_\infty$'); +set(gca,'ColorOrderIndex',2) +plot(freqs, abs(squeeze(freqresp(Hh_hinf, freqs, 'Hz'))), '-', 'DisplayName', '$H_H$ - $\mathcal{H}_\infty$'); +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Magnitude'); +hold off; +xlim([freqs(1), freqs(end)]); +ylim([1e-3, 10]); +xticks([0.1, 1, 10, 100, 1000]); +legend('location', 'northeast'); diff --git a/matlab/matlab/optimal_comp_filters.m b/matlab/matlab/optimal_comp_filters.m new file mode 100644 index 0000000..51fa6fa --- /dev/null +++ b/matlab/matlab/optimal_comp_filters.m @@ -0,0 +1,432 @@ +%% Clear Workspace and Close figures +clear; close all; clc; + +%% Intialize Laplace variable +s = zpk('s'); + +freqs = logspace(-1, 3, 1000); + +% 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) + + +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; + +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]] + +% 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} \] + +% Thus, if we minimize the $\mathcal{H}_2$ norm of this transfer function, we minimize the RMS value of $\hat{x}$. + +% We define the generalized plant $P$ on matlab as shown on figure [[fig:h_infinity_optimal_comp_filters]]. + +P = [0 N2 1; + N1 -N2 0]; + + + +% And we do the $\mathcal{H}_2$ synthesis using the =h2syn= command. + +[H1, ~, gamma] = h2syn(P, 1, 1); + + + +% Finally, we define $H_2(s) = 1 - H_1(s)$. + +H2 = 1 - 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; + +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); + + + +% #+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 | + + + +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, PSD_Ha, 'k--', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_\infty},a}$'); +plot(freqs, PSD_Hb, 'k-.', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_\infty},b}$'); +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, 'k-', '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)))); +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');
Table 1: RMS value of the estimation error when using the sensor individually and when using the two sensor merged using the optimal complementary filters