From 9c7f96dd02e93cf634e06c1703e27ef7a196ebfc Mon Sep 17 00:00:00 2001 From: Thomas Dehaeze Date: Wed, 30 Sep 2020 08:47:27 +0200 Subject: [PATCH] Reworked the matlab file --- matlab/index.html | 2704 +++-------------------------- matlab/index.org | 4155 ++++++++++++++++++++------------------------- 2 files changed, 2141 insertions(+), 4718 deletions(-) diff --git a/matlab/index.html b/matlab/index.html index cbf7471..ac3c157 100644 --- a/matlab/index.html +++ b/matlab/index.html @@ -3,251 +3,25 @@ "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> - + - Robust and Optimal Sensor Fusion - Matlab Computation - - - - - + +
@@ -260,122 +34,38 @@ for the JavaScript code in this tag.

Table of Contents

@@ -391,27 +81,23 @@ Two sensors are considered with both different noise characteristics and dynamic

    -
  • in section 1: the \(\mathcal{H}_2\) synthesis is used to design complementary filters such that the RMS value of the super sensor's noise is minimized
    • -
  • in section 2: the \(\mathcal{H}_\infty\) synthesis is used to design complementary filters such that the super sensor's uncertainty is bonded to acceptable values
    • -
  • in section 3: the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis is used to both limit the super sensor's uncertainty and to lower the RMS value of the super sensor's noise
    • -
  • in section 5: the \(\mathcal{H}_\infty\) synthesis is used for both limiting the noise and uncertainty of the super sensor
    • -
  • in section 6: we try to find the characteristics of the super sensor from the characteristics of the individual sensors and of the complementary filters
    • -
  • in section 7: a methodology is proposed to apply optimal and robust sensor fusion in practice
    • -
  • in section 8: methods of complementary filter synthesis are proposed
    • +
  • Section 1: the \(\mathcal{H}_2\) synthesis is used to design complementary filters such that the RMS value of the super sensor’s noise is minimized
    • +
  • Section 2: the \(\mathcal{H}_\infty\) synthesis is used to design complementary filters such that the super sensor’s uncertainty is bonded to acceptable values
    • +
  • Section 3: the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis is used to both limit the super sensor’s uncertainty and to lower the RMS value of the super sensor’s noise
-
-

1 Optimal Sensor Fusion - Minimize the Super Sensor Noise

+
+

1 Optimal Super Sensor Noise: \(\mathcal{H}_2\) Synthesis

- +

The idea is to combine sensors that works in different frequency range using complementary filters.

-Doing so, one "super sensor" is obtained that can have better noise characteristics than the individual sensors over a large frequency range. +Doing so, one “super sensor” is obtained that can have better noise characteristics than the individual sensors over a large frequency range.

@@ -425,23 +111,23 @@ The Matlab scripts is accessible here

-
-

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{orge64e355} - \Phi_{\tilde{n}_1}(\omega) = \Phi_{\tilde{n}_1}(\omega) = 1 +\label{orga7ad7f8} + \Phi_{\tilde{n}_1}(\omega) = \Phi_{\tilde{n}_2}(\omega) = 1 \end{equation} -
+

fusion_two_noisy_sensors_weights.png

Figure 1: Fusion of two sensors

@@ -451,16 +137,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

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

\begin{equation} -\label{org9f8097b} +\label{orga71be68} H_1(s) + H_2(s) = 1 \end{equation} @@ -487,17 +173,17 @@ 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{orgc722a2b} +\label{orgc926b79} \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\): +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)
    • @@ -505,16 +191,16 @@ Let's define the noise characteristics of the two sensors by choosing \(N_1\) an
-
omegac = 100*2*pi; G0 = 1e-5; Ginf = 1e-4;
-N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/100);
+
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;
+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;
-
+

noise_characteristics_sensors.png

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

@@ -522,8 +208,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: @@ -536,11 +222,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

@@ -569,11 +255,11 @@ 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;
-     N1 -N2  0];
+
P = [0   N2  1;
+     N1 -N2  0];
@@ -581,7 +267,7 @@ We define the generalized plant \(P\) on matlab as shown on figure -
[H1, ~, gamma] = h2syn(P, 1, 1);
+
[H1, ~, gamma] = h2syn(P, 1, 1);
@@ -589,20 +275,20 @@ And we do the \(\mathcal{H}_2\) synthesis using the h2syn command. Finally, we define \(H_2(s) = 1 - H_1(s)\).

-
H2 = 1 - H1;
+
H2 = 1 - H1;

-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.

@@ -610,35 +296,35 @@ 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)

-
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_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_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)

-
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);
+
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);
-
+

cps_h2_synthesis.png

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

@@ -646,431 +332,10 @@ CPS_H2 = 1 -

1.4 Alternative H-Two Synthesis

+
+

1.4 Obtained Super Sensor’s noise uncertainty

-An alternative Alternative formulation of the \(\mathcal{H}_2\) synthesis is shown in Fig. 8. -

- - -
-

h_infinity_optimal_comp_filters_bis.png -

-

Figure 8: Alternative formulation of the \(\mathcal{H}_2\) synthesis

-
- -\begin{equation*} -\begin{pmatrix} - z_1 \\ z_2 \\ v -\end{pmatrix} = \begin{pmatrix} - N_1 & -N_1 \\ - 0 & N_2 \\ - 1 & 0 -\end{pmatrix} \begin{pmatrix} - w \\ u -\end{pmatrix} -\end{equation*} -
-
- - -
-

1.5 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. 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. 9. -

- -
-
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');
-
-
- -
-[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
-
- -
-
H1a = 1 - H2a;
-
-
- - -
-

weights_comp_filters_Hinfa.png -

-

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

-
- -

-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);
-
-
-
-
- -
-

1.6 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. 10. -

- -
-
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');
-
-
- -
-[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
-
- -
-
H1b = 1 - H2b;
-
-
- - -
-

weights_comp_filters_Hinfb.png -

-

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

-
- -
-
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);
-
-
-
-
- -
-

1.7 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');
-
-
- -
-[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
-
- -
-
H1c = 1 - H2c;
-
-
- - -
-

weights_comp_filters_Hinfc.png -

-

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

-
- -
-
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);
-
-
-
-
- -
-

1.8 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. 12. -

- -

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

- -

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

- - - - --- -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
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
 rms value
Sensor 11.3e-03
Sensor 21.3e-03
H2 Fusion1.2e-04
H-Infinity a2.4e-04
H-Infinity b1.4e-04
H-Infinity c2.2e-04
- - - -
-

comparison_psd_noise.png -

-

Figure 12: Comparison of the obtained Power Spectral Density using the three methods (png, pdf)

-
- - -
-

comparison_cps_noise.png -

-

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

-
-
-
- -
-

1.9 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.

@@ -1080,13 +345,13 @@ Two weights \(w_1(s)\) and \(w_2(s)\) are used to described the amplitude of the

-
omegac = 100*2*pi; G0 = 0.1; Ginf = 10;
-w1 = (Ginf*s/omegac + G0)/(s/omegac + 1);
+
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);
+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);
@@ -1094,34 +359,34 @@ w2 = w2* -
G1 = 1 + w1*ultidyn('Delta',[1 1]);
-G2 = 1 + w2*ultidyn('Delta',[1 1]);
+
G1 = 1 + w1*ultidyn('Delta',[1 1]);
+G2 = 1 + w2*ultidyn('Delta',[1 1]);

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. 14. +The dynamical uncertainty bounds of the super sensor is shown in Fig. 8. Right Half Plane zero might be introduced in the super sensor dynamics which will render the feedback system unstable.

-
Gss = G1*H1 + G2*H2;
+
Gss = G1*H1 + G2*H2;
-
+

uncertainty_super_sensor_H2_syn.png

-

Figure 14: Uncertianty regions of both individual sensors and of the super sensor when using the \(\mathcal{H}_2\) synthesis (png, pdf)

+

Figure 8: Uncertianty regions of both individual sensors and of the super sensor when using the \(\mathcal{H}_2\) synthesis (png, pdf)

-
-

1.10 Conclusion

-
+
+

1.5 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).

@@ -1133,11 +398,11 @@ However, the synthesis does not take into account the robustness of the sensor f
-
-

2 Optimal Sensor Fusion - Minimize the Super Sensor Dynamical Uncertainty

+
+

2 Robust Sensor Fusion: \(\mathcal{H}_\infty\) Synthesis

- +

We initially considered perfectly known sensor dynamics so that it can be perfectly inverted. @@ -1145,14 +410,14 @@ 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. 15. +To do so, we model the uncertainty that we have on the sensor dynamics by multiplicative input uncertainty as shown in Fig. 9.

-
+

sensor_fusion_dynamic_uncertainty.png

-

Figure 15: Sensor fusion architecture with sensor dynamics uncertainty

+

Figure 9: Sensor fusion architecture with sensor dynamics uncertainty

@@ -1166,19 +431,19 @@ The Matlab scripts is accessible here<

-
-

2.1 Super Sensor Dynamical Uncertainty

+
+

2.1 Super Sensor Dynamical Uncertainty

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

-
+

sensor_fusion_dynamic_uncertainty.png

-

Figure 16: Fusion of two sensors with input multiplicative uncertainty

+

Figure 10: Fusion of two sensors with input multiplicative uncertainty

@@ -1197,7 +462,7 @@ We see that as soon as we have some uncertainty in the sensor dynamics, we have

-The uncertainty set of the transfer function from \(\hat{x}\) to \(x\) at frequency \(\omega\) is bounded in the complex plane by a circle centered on 1 and with a radius equal to \(|w_1(j\omega) H_1(j\omega)| + |w_2(j\omega) H_2(j\omega)|\) (figure 17). +The uncertainty set of the transfer function from \(\hat{x}\) to \(x\) at frequency \(\omega\) is bounded in the complex plane by a circle centered on 1 and with a radius equal to \(|w_1(j\omega) H_1(j\omega)| + |w_2(j\omega) H_2(j\omega)|\) (figure 11).

@@ -1206,16 +471,16 @@ We then have that the angle introduced by the super sensor is bounded by \(\arcs

-
+

uncertainty_gain_phase_variation.png

-

Figure 17: Maximum phase variation

+

Figure 11: Maximum phase variation

-
-

2.2 Dynamical uncertainty of the individual sensors

+
+

2.2 Dynamical uncertainty of the individual sensors

Let say we want to merge two sensors: @@ -1230,39 +495,39 @@ We define the weights that are used to characterize the dynamic uncertainty of t

-
omegac = 100*2*pi; G0 = 0.1; Ginf = 10;
-w1 = (Ginf*s/omegac + G0)/(s/omegac + 1);
+
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);
+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. 18 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. 12 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]);
+
G1 = 1 + w1*ultidyn('Delta',[1 1]);
+G2 = 1 + w2*ultidyn('Delta',[1 1]);
-
+

uncertainty_dynamics_sensors.png

-

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

+

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

-
-

2.3 Synthesis objective

+
+

2.3 Synthesis objective

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

@@ -1275,7 +540,7 @@ Thus, the phase shift \(\Delta\phi(\omega)\) due to the super sensor uncertainty

-Let's define some allowed frequency depend phase shift \(\Delta\phi_\text{max}(\omega) > 0\) such that: +Let’s define some allowed frequency depend phase shift \(\Delta\phi_\text{max}(\omega) > 0\) such that: \[ |\Delta\phi(\omega)| < \Delta\phi_\text{max}(\omega), \quad \forall\omega \]

@@ -1291,15 +556,15 @@ The maximum phase shift due to dynamic uncertainty at frequency \(\omega\) will
-
-

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

+
+

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

-We know try to express this requirement in terms of an \(\mathcal{H}_\infty\) norm. +We now try to express this requirement in terms of an \(\mathcal{H}_\infty\) norm.

-Let's define one weight \(w_\phi(s)\) that represents the maximum wanted phase uncertainty: +Let’s define one weight \(w_\phi(s)\) that represents the maximum wanted phase uncertainty: \[ |w_{\phi}(j\omega)|^{-1} \approx \sin(\Delta\phi_{\text{max}}(\omega)), \quad \forall\omega \]

@@ -1316,7 +581,7 @@ Then: Which is approximately equivalent to (with an error of maximum \(\sqrt{2}\)):

\begin{equation} -\label{org58312d5} +\label{org559e8db} \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} @@ -1327,73 +592,73 @@ Thus, at these frequencies, \(|w_\phi|\) should be smaller than \(1\).
-
-

2.5 Weighting Function used to bound the super sensor uncertainty

+
+

2.5 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. 19 and the corresponding maximum allowed phase uncertainty of the super sensor dynamics of shown in Fig. 20. +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. 13 and the corresponding maximum allowed phase uncertainty of the super sensor dynamics of shown in Fig. 14.

-
Dphi = 20; % [deg]
+
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;
+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;
+W1 = w1*wphi;
+W2 = w2*wphi;
-
+

magnitude_wphi.png

-

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

+

Figure 13: 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 20: Maximum wanted phase uncertainty using this weight (png, pdf)

+

Figure 14: 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. 21. +The obtained upper bounds on the complementary filters in order to limit the phase uncertainty of the super sensor are represented in Fig. 15.

-
+

upper_bounds_comp_filter_max_phase_uncertainty.png

-

Figure 21: 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 15: 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)

-
-

2.6 \(\mathcal{H}_\infty\) Synthesis

+
+

2.6 \(\mathcal{H}_\infty\) Synthesis

-The \(\mathcal{H}_\infty\) synthesis architecture used for the complementary filters is shown in Fig. 22. +The \(\mathcal{H}_\infty\) synthesis architecture used for the complementary filters is shown in Fig. 16.

-
+

h_infinity_robust_fusion.png

-

Figure 22: Architecture used for \(\mathcal{H}_\infty\) synthesis of complementary filters

+

Figure 16: Architecture used for \(\mathcal{H}_\infty\) synthesis of complementary filters

The generalized plant is defined below.

-
P = [W1 -W1;
-     0   W2;
-     1   0];
+
P = [W1 -W1;
+     0   W2;
+     1   0];
@@ -1401,7 +666,7 @@ The generalized plant is defined below. 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');
+
[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
@@ -1432,39 +697,39 @@ Test bounds: 0.0447 < gamma <= 1.3318 And \(H_1(s)\) is defined as the complementary of \(H_2(s)\).

-
H1 = 1 - H2;
+
H1 = 1 - H2;

-The obtained complementary filters are shown in Fig. 23. +The obtained complementary filters are shown in Fig. 17.

-
+

comp_filter_hinf_uncertainty.png

-

Figure 23: Obtained complementary filters (png, pdf)

+

Figure 17: Obtained complementary filters (png, pdf)

-
-

2.7 Super sensor uncertainty

+
+

2.7 Super sensor uncertainty

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

-
Gss = G1*H1 + G2*H2;
+
Gss = G1*H1 + G2*H2;
-
+

super_sensor_uncertainty_bode_plot.png

-

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

+

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

@@ -1478,46 +743,46 @@ For instance, we could improve the dynamical uncertainty of the super sensor by

-
-

2.8 Super sensor noise

+
+

2.8 Super sensor noise

-We now compute the obtain Power Spectral Density of the super sensor's 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.

-
omegac = 100*2*pi; G0 = 1e-5; Ginf = 1e-4;
-N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/100);
+
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;
+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;

-The PSD of both sensor and of the super sensor is shown in Fig. 25. -The CPS of both sensor and of the super sensor is shown in Fig. 26. +The PSD of both sensor and of the super sensor is shown in Fig. 19. +The CPS of both sensor and of the super sensor is shown in Fig. 20.

-
+

psd_sensors_hinf_synthesis.png

-

Figure 25: Power Spectral Density of the obtained super sensor using the \(\mathcal{H}_\infty\) synthesis (png, pdf)

+

Figure 19: Power Spectral Density of the obtained super sensor using the \(\mathcal{H}_\infty\) synthesis (png, pdf)

-
+

cps_sensors_hinf_synthesis.png

-

Figure 26: Cumulative Power Spectrum of the obtained super sensor using the \(\mathcal{H}_\infty\) synthesis (png, cps)

+

Figure 20: Cumulative Power Spectrum of the obtained super sensor using the \(\mathcal{H}_\infty\) synthesis (png, cps)

-
-

2.9 Conclusion

+
+

2.9 Conclusion

Using the \(\mathcal{H}_\infty\) synthesis, the dynamical uncertainty of the super sensor can be bounded to acceptable values. @@ -1530,11 +795,11 @@ However, the RMS of the super sensor noise is not optimized as it was the case w

-
-

3 Optimal Sensor Fusion - Mixed Synthesis

+
+

3 Optimal and Robust Sensor Fusion: Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis

- +

@@ -1543,8 +808,8 @@ The Matlab scripts is accessible -

3.1 Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis - Introduction

+
+

3.1 Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis - Introduction

The goal is to design complementary filters such that: @@ -1564,20 +829,20 @@ The Matlab function for that is h2hinfsyn ( -

3.2 Noise characteristics and Uncertainty of the individual sensors

+
+

3.2 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 = 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);
+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);
@@ -1585,67 +850,67 @@ w2 = w2* -
omegac = 100*2*pi; G0 = 1e-5; Ginf = 1e-4;
-N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/100);
+
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;
+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. 27. +Both dynamical uncertainty and noise characteristics of the individual sensors are shown in Fig. 21.

-
+

mixed_synthesis_noise_uncertainty_sensors.png

-

Figure 27: Noise characteristsics and Dynamical uncertainty of the individual sensors (png, pdf)

+

Figure 21: Noise characteristsics and Dynamical uncertainty of the individual sensors (png, pdf)

-
-

3.3 Weighting Functions on the uncertainty of the super sensor

+
+

3.3 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. 28. 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. +The maximum wanted multiplicative uncertainty is shown in Fig. 22. 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;
+
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;
-
+

mixed_syn_hinf_weight.png

-

Figure 28: Wanted maximum module uncertainty of the super sensor (png, pdf)

+

Figure 22: Wanted maximum module uncertainty of the super sensor (png, pdf)

-The equivalent Magnitude and Phase uncertainties are shown in Fig. 29. +The equivalent Magnitude and Phase uncertainties are shown in Fig. 23.

-
+

mixed_syn_objective_hinf.png

-

Figure 29: \(\mathcal{H}_\infty\) synthesis objective part of the mixed-synthesis (png, pdf)

+

Figure 23: \(\mathcal{H}_\infty\) synthesis objective part of the mixed-synthesis (png, pdf)

-
-

3.4 Mixed Synthesis Architecture

+
+

3.4 Mixed Synthesis Architecture

-The synthesis architecture that is used here is shown in Fig. 30. +The synthesis architecture that is used here is shown in Fig. 24.

@@ -1658,10 +923,10 @@ The controller \(K\) is synthesized such that it: -

+

mixed_h2_hinf_synthesis.png

-

Figure 30: Mixed H2/H-Infinity Synthesis

+

Figure 24: Mixed H2/H-Infinity Synthesis

@@ -1677,7 +942,7 @@ 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 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\)
@@ -1693,89 +958,89 @@ 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
+
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];
+P = [W1u -W1u;
+     0    W2u;
+     W1n -W1n;
+     0    W2n;
+     1    0];
-
-

3.5 Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis

+
+

3.5 Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis

-The mixed $\mathcal{H}2$/\(\mathcal{H}_\infty\) synthesis is performed below. +The mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis is performed below.

-
Nmeas = 1; Ncon = 1; Nz2 = 2;
+
Nmeas = 1; Ncon = 1; Nz2 = 2;
 
-[H2,~,normz,~] = h2hinfsyn(P, Nmeas, Ncon, Nz2, [0, 1], 'HINFMAX', 1, 'H2MAX', Inf, 'DKMAX', 100, 'TOL', 0.01, 'DISPLAY', 'on');
+[H2,~,normz,~] = h2hinfsyn(P, Nmeas, Ncon, Nz2, [0, 1], 'HINFMAX', 1, 'H2MAX', Inf, 'DKMAX', 100, 'TOL', 0.01, 'DISPLAY', 'on');
 
-H1 = 1 - H2;
+H1 = 1 - H2;

-The obtained complementary filters are shown in Fig. 31. +The obtained complementary filters are shown in Fig. 25.

-
+

comp_filters_mixed_synthesis.png

-

Figure 31: Obtained complementary filters after mixed $\mathcal{H}2$/\(\mathcal{H}_\infty\) synthesis (png, pdf)

+

Figure 25: Obtained complementary filters after mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis (png, pdf)

-
-

3.6 Obtained Super Sensor's noise

+
+

3.6 Obtained Super Sensor’s noise

-The PSD and CPS of the super sensor's noise are shown in Fig. 32 and Fig. 33 respectively. +The PSD and CPS of the super sensor’s noise are shown in Fig. 26 and Fig. 27 respectively.

-
+

psd_super_sensor_mixed_syn.png

-

Figure 32: Power Spectral Density of the Super Sensor obtained with the mixed $\mathcal{H}2$/\(\mathcal{H}_\infty\) synthesis (png, pdf)

+

Figure 26: Power Spectral Density of the Super Sensor obtained with the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis (png, pdf)

-
+

cps_super_sensor_mixed_syn.png

-

Figure 33: Cumulative Power Spectrum of the Super Sensor obtained with the mixed $\mathcal{H}2$/\(\mathcal{H}_\infty\) synthesis (png, pdf)

+

Figure 27: Cumulative Power Spectrum of the Super Sensor obtained with the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis (png, pdf)

-
-

3.7 Obtained Super Sensor's Uncertainty

+
+

3.7 Obtained Super Sensor’s Uncertainty

-The uncertainty on the super sensor's dynamics is shown in Fig. 34. +The uncertainty on the super sensor’s dynamics is shown in Fig. 28.

-
+

super_sensor_dyn_uncertainty_mixed_syn.png

-

Figure 34: Super Sensor Dynamical Uncertainty obtained with the mixed synthesis (png, pdf)

+

Figure 28: Super Sensor Dynamical Uncertainty obtained with the mixed synthesis (png, pdf)

-
-

3.8 Conclusion

+
+

3.8 Conclusion

This synthesis methods allows both to: @@ -1788,1476 +1053,19 @@ This synthesis methods allows both to:

-
-

4 Mixed Synthesis - LMI Optimization

-
-
-
-

4.1 Introduction

-
-

-The following matlab scripts was written by Mohit. -

-
-
- -
-

4.2 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;
-
-
-
-
- -
-

4.3 Weights

-
-

-The weights for the \(\mathcal{H}_2\) and \(\mathcal{H}_\infty\) part are defined below. -

- -
-
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;
-
-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];
-
-
-
-
- -
-

4.4 LMI Optimization

-
-

-We are using the CVX toolbox to solve the optimization problem. -

- -

-We first put the generalized plant in a State-space form. -

-
-
A = P.A;
-Bw = P.B(:,1);
-Bu = P.B(:,2);
-Cz1 = P.C(1:2,:); Dz1w = P.D(1:2,1); Dz1u = P.D(1:2,2); % Hinf
-Cz2 = P.C(3:4,:); Dz2w = P.D(1:2,1); Dz2u = P.D(1:2,2); % H2
-Cy = P.C(5,:); Dyw = P.D(5,1); Dyu = P.D(5,2);
-
-n = size(P.A,1);
-ny = 1; % number of measurements
-nu = 1; % number of control inputs
-nz = 2;
-nw = 1;
-
-Wtinf = 0;
-Wt2 = 1;
-
-
- -

-We Define all the variables. -

-
-
cvx_startup;
-
-cvx_begin sdp
-cvx_quiet true
-cvx_solver sedumi
-variable X(n,n) symmetric;
-variable Y(n,n) symmetric;
-variable W(nz,nz) symmetric;
-variable Ah(n,n);
-variable Bh(n,ny);
-variable Ch(nu,n);
-variable Dh(nu,ny);
-variable eta;
-variable gam;
-
-
- -

-We define the minimization objective. -

-
-
minimize Wt2*eta+Wtinf*gam % mix objective
-subject to:
-
-
- -

-The \(\mathcal{H}_\infty\) constraint. -

-
-
gam<=1; % Keep the Hinf norm less than 1
-
-[ X, eye(n,n) ;
-  eye(n,n), Y ] >= 0 ;
-
-[ A*X + Bu*Ch + X*A' + Ch'*Bu', A+Bu*Dh*Cy+Ah', Bw+Bu*Dh*Dyw, X*Cz1' + Ch'*Dz1u' ;
-  (A+Bu*Dh*Cy+Ah')', Y*A + A'*Y + Bh*Cy + Cy'*Bh', Y*Bw + Bh*Dyw, (Cz1+Dz1u*Dh*Cy)' ;
-  (Bw+Bu*Dh*Dyw)', Bw'*Y + Dyw'*Bh', -eye(nw,nw), (Dz1w+Dz1u*Dh*Dyw)' ;
-  Cz1*X + Dz1u*Ch, Cz1+Dz1u*Dh*Cy, Dz1w+Dz1u*Dh*Dyw, -gam*eye(nz,nz)] <= 0 ;
-
-
- -

-The \(\mathcal{H}_2\) constraint. -

-
-
trace(W) <= eta ;
-
-[ W, Cz2*X+Dz2u*Ch, Cz2*X+Dz2u*Ch;
-  X*Cz2'+Ch'*Dz2u', X, eye(n,n) ;
-  (Cz2*X+Dz2u*Ch)', eye(n,n), Y ] >= 0 ;
-
-[ A*X + Bu*Ch + X*A' + Ch'*Bu', A+Bu*Dh*Cy+Ah', Bw+Bu*Dh*Dyw ;
-  (A+Bu*Dh*Cy+Ah')', Y*A + A'*Y + Bh*Cy + Cy'*Bh', Y*Bw + Bh*Dyw ;
-  (Bw+Bu*Dh*Dyw)', Bw'*Y + Dyw'*Bh', -eye(nw,nw)] <= 0 ;
-
-
- -

-And we run the optimization. -

-
-
cvx_end
-cvx_status
-
-
- -

-Finally, we can compute the obtained complementary filters. -

-
-
M = eye(n);
-N = inv(M)*(eye(n,n)-Y*X);
-Dk = Dh;
-Ck = (Ch-Dk*Cy*X)*inv(M');
-Bk = inv(N)*(Bh-Y*Bu*Dk);
-Ak = inv(N)*(Ah-Y*(A+Bu*Dk*Cy)*X-N*Bk*Cy*X-Y*Bu*Ck*M')*inv(M');
-
-H2 = tf(ss(Ak,Bk,Ck,Dk));
-H1 = 1 - H2;
-
-
-
-
- -
-

4.5 Result

-
-

-The obtained complementary filters are compared with the required upper bounds on Fig. 35. -

- - -
-

LMI_obtained_comp_filters.png -

-

Figure 35: Obtained complementary filters using the LMI optimization (png, pdf)

-
-
-
- -
-

4.6 Comparison with the matlab Mixed Synthesis

-
-

-The Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis is performed below. -

-
-
Nmeas = 1; Ncon = 1; Nz2 = 2;
-
-[H2m,~,normz,~] = h2hinfsyn(P, Nmeas, Ncon, Nz2, [0, 1], 'HINFMAX', 1, 'H2MAX', Inf, 'DKMAX', 100, 'TOL', 0.01, 'DISPLAY', 'on');
-
-H1m = 1 - H2m;
-
-
- -

-The obtained filters are compare with the one obtained using the CVX toolbox in Fig. [[]]. -

- - -
-

compare_cvx_h2hinf_comp_filters.png -

-

Figure 36: Comparison between the complementary filters obtained with the CVX toolbox and with the h2hinfsyn command (png, pdf)

-
-
-
- -
-

4.7 H-Infinity Objective

-
-

-In terms of the \(\mathcal{H}_\infty\) objective, both synthesis method are satisfying the requirements as shown in Fig. 37. -

- - -
-

comp_cvx_h2i_hinf_norm.png -

-

Figure 37: H-Infinity norm requirement and results (png, pdf)

-
-
-
- -
-

4.8 Obtained Super Sensor's noise

-
-

-The PSD and CPS of the super sensor's noise obtained with the CVX toolbox and h2hinfsyn command are compared in Fig. 38 and 39. -

- - -
-

psd_compare_cvx_h2i.png -

-

Figure 38: Power Spectral Density of the Super Sensor obtained with the mixed $\mathcal{H}2$/\(\mathcal{H}_\infty\) synthesis (png, pdf)

-
- - - -
-

cps_compare_cvx_h2i.png -

-

Figure 39: Cumulative Power Spectrum of the Super Sensor obtained with the mixed $\mathcal{H}2$/\(\mathcal{H}_\infty\) synthesis (png, pdf)

-
-
-
- -
-

4.9 Obtained Super Sensor's Uncertainty

-
-

-The uncertainty on the super sensor's dynamics is shown in Fig. [[]]. -

- - -
-

super_sensor_uncertainty_compare_cvx_h2i.png -

-

Figure 40: Super Sensor Dynamical Uncertainty obtained with the mixed synthesis (png, pdf)

-
-
-
-
- -
-

5 H-Infinity synthesis to ensure both performance and robustness

-
-

- -

-
-

-The Matlab scripts is accessible here. -

- -
-
- -
-

5.1 Introduction

-
-

-The idea is to use only the \(\mathcal{H}_\infty\) norm to express both the maximum wanted super sensor uncertainty and the fact that we want to minimize the super sensor's noise. -

- -

-For performance, we may want to obtain a super sensor's noise that is close to the minimum of the individual sensor noises. -

- -

-The noise of the super sensor is: -\[ |N_{ss}(j\omega)|^2 = | H_1(j\omega) N_1(j\omega) |^2 + | H_2(j\omega) N_2(j\omega) |^2 \quad \forall\omega \] -

- -

-The minimum noise that we can obtain follows the minimum noise of the individual sensor: -

-\begin{align*} - & |N_{ss}(j\omega)| \approx |N_1(j\omega)| \quad \text{when} \quad |N_1(j\omega)| < |N_2(j\omega)| \\ - & |N_{ss}(j\omega)| \approx |N_2(j\omega)| \quad \text{when} \quad |N_2(j\omega)| < |N_1(j\omega)| -\end{align*} - -

-To do so, we want to design the complementary filters such that: -

-\begin{align*} - & |H_2(j\omega)| \ll 1 \quad \text{when} \quad |N_1(j\omega)| < |N_2(j\omega)| \\ - & |H_1(j\omega)| \ll 1 \quad \text{when} \quad |N_2(j\omega)| < |N_1(j\omega)| -\end{align*} - - - - -

-For the uncertainty of the super sensor. -The equivalent super sensor uncertainty is: -\[ |w_{ss}(j\omega)| = |H_1(j\omega) w_1(j\omega)| + |H_2(j\omega) w_2(j\omega)|, \quad \forall\omega \] -

- -

-The minimum uncertainty that we can obtain follows the minimum uncertainty of the individual sensor: -

-\begin{align*} - & |w_{ss}(j\omega)| \approx |w_1(j\omega)| \quad \text{when} \quad |w_1(j\omega)| < |w_2(j\omega)| \\ - & |w_{ss}(j\omega)| \approx |w_2(j\omega)| \quad \text{when} \quad |w_2(j\omega)| < |w_1(j\omega)| -\end{align*} - -

-To do so, we want to design the complementary filters such that: -

-\begin{align*} - & |H_2(j\omega)| \ll 1 \quad \text{when} \quad |w_1(j\omega)| < |w_2(j\omega)| \\ - & |H_1(j\omega)| \ll 1 \quad \text{when} \quad |w_2(j\omega)| < |w_1(j\omega)| -\end{align*} - - -

-Of course, the conditions for performance and uncertainty may not be compatible. -

- -

-We may not want to follow the minimum uncertainty. -

-
-
- -
-

5.2 Dynamical uncertainty and Noise level of the individual sensors

-
-

-Uncertainty on the individual sensors: -

-
-
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);
-
-
- -

-Noise level of the individual sensors: -

-
-
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;
-
-
- - -
-

noise_uncertainty_sensors_hinf.png -

-

Figure 41: Noise and Uncertainty characteristics of the sensors (png, pdf)

-
-
-
- -
-

5.3 Weights for uncertainty and performance

-
-

-We design weights that are used to describe the wanted upper bound on the super sensor's noise and super sensor's uncertainty. -

- -

-Weight on the uncertainty: -

-
-
n = 4; w0 = 2*pi*500; G0 = 6; 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;
-
-Wu = 0.2*(s+3.142e04)/(s+628.3)/H;
-
-
- -

-Weight on the performance: -

-
-
n = 1; w0 = 2*pi*9; A = 6;
-a = sqrt(2*A^(2/n) - 1 + 2*A^(1/n)*sqrt(A^(2/n) - 1));
-G = ((1 + s/(w0/a))*(1 + s/(w0*a))/(1 + s/w0)^2)^n;
-
-n = 2; w0 = 2*pi*9; G0 = 1e-2; G1 = 1; Gc = 5e-1;
-G2 = (((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;
-
-Wp = inv(G2)*inv(G)*inv(N2);
-
-
- -

-The noise and uncertainty weights of the individual sensors and the asked noise/uncertainty of the super sensor are displayed in Fig. 42. -

- -
-

charac_sensors_weights.png -

-

Figure 42: Upper bounds on the super sensor's noise and super sensor's uncertainty (png, pdf)

-
- - -

-The corresponding maximum norms of the filters to have the perf/robust asked are shown in Fig. 43. -

- -
-

upper_bound_complementary_filters_perf_robust.png -

-

Figure 43: Upper bounds on the complementary filters (png, pdf)

-
-
-
- -
-

5.4 H-infinity synthesis with 4 outputs corresponding to the 4 weights

-
-

-We do the \(\mathcal{H}_\infty\) synthesis with 4 weights and 4 outputs. -

- -\begin{equation*} - \left\| \begin{matrix} - W_{1p}(s) (1 - N_2(s)) \\ - W_{2p}(s) N_2(s) \\ - W_{1u}(s) (1 - N_2(s)) \\ - W_{2u}(s) N_2(s) - \end{matrix} \right\|_\infty < 1 -\end{equation*} - - -
-
W1p = N1*Wp/(1+s/2/pi/1000); % Used to render W1p proper
-W2p = N2*Wp;
-W1u = w1*Wu;
-W2u = w2*Wu;
-
-
- -
-
P = [W1p -W1p;
-     0    W2p;
-     W1u -W1u;
-     0    W2u;
-     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');
-
-
- -
-[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:      1.4139 <  gamma  <=     65.6899
-
-  gamma    hamx_eig  xinf_eig  hamy_eig   yinf_eig   nrho_xy   p/f
-   65.690   1.3e+00 -6.7e-15   1.3e+00   -4.5e-13    0.0000    p
-   33.552   1.3e+00 -9.4e-15   1.3e+00   -3.7e-14    0.0000    p
-   17.483   1.3e+00 -5.6e-16   1.3e+00   -4.8e-13    0.0000    p
-    9.448   1.3e+00 -3.2e-15   1.3e+00   -1.2e-13    0.0000    p
-    5.431   1.3e+00 -2.3e-16   1.3e+00   -3.6e-13    0.0000    p
-    3.422   1.3e+00 -7.3e-16   1.3e+00   -2.6e-15    0.0000    p
-    2.418   1.3e+00   9.3e-17   1.3e+00   -3.0e-14    0.0000    p
-    1.916   1.3e+00   2.4e-17   1.3e+00   -2.2e-14    0.0000    p
-    1.665   1.3e+00 -2.5e-16   1.3e+00   -2.1e-14    0.0000    p
-    1.539   1.3e+00 -6.9e-15   1.3e+00   -5.3e-14    0.0000    p
-    1.477   1.3e+00 -2.1e-14   1.3e+00   -2.3e-13    0.0000    p
-    1.445   1.3e+00 -1.3e-16   1.3e+00   -2.6e-15    0.0000    p
-    1.430   1.3e+00 -4.9e-13   1.3e+00   -2.2e-13    0.0000    p
-    1.422   1.3e+00 -1.2e+08#  1.3e+00   -2.6e-13    0.0000    f
-    1.426   1.3e+00 -6.3e-13   1.3e+00   -3.3e-14    0.0000    p
-    1.424   1.3e+00 -3.4e+08#  1.3e+00   -4.5e-14    0.0000    f
-    1.425   1.3e+00 -1.7e+09#  1.3e+00   -5.2e-13    0.0000    f
-
- Gamma value achieved:     1.4256
-
- -
-
H1 = 1 - H2;
-
-
- -

-The obtained complementary filters with the upper bounds are shown in Fig. 44. -

- -
-

hinf_result_comp_filters_4_outputs.png -

-

Figure 44: caption (png, pdf)

-
- - - - -
-

upper_bounds_perf_robust_result_4_outputs.png -

-

Figure 45: Obtained PSD and uncertainty with the corresponding upper bounds (png, pdf)

-
- - -
-

4outputs_hinf_psd_cps2svg.png -

-

Figure 46: PSD and CPS (png, pdf)

-
- - - -
-

4outputs_uncertainty.png -

-

Figure 47: Dynamical uncertainty (png, pdf)

-
-
-
- -
-

5.5 Conclusion

-
-

-The \(\mathcal{H}_\infty\) synthesis has been used to design complementary filters that permits to robustly merge sensors while ensuring a maximum noise level. -However, no guarantee is made that the RMS value of the super sensor's noise is minimized. -

-
-
-
- -
-

6 Equivalent Super Sensor

-
-

- -

-

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

-
-
-

6.1 Sensor Fusion Architecture

-
-

-Let consider figure 48 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 48: Sensor fusion architecture (png, pdf).

-
- - -\begin{align*} - \hat{x} &= H_1(s) N_1(s) \tilde{n}_1 + H_2(s) N_2(s) \tilde{n}_2 \\ - &\quad \quad + \Big(H_1(s) \big(1 + w_1(s) \Delta_1(s)\big) + H_2(s) \big(1 + w_2(s) \Delta_2(s)\big)\Big) x \\ - &= H_1(s) N_1(s) \tilde{n}_1 + H_2(s) N_2(s) \tilde{n}_2 \\ - &\quad \quad + \big(1 + H_1(s) w_1(s) \Delta_1(s) + H_2(s) w_2(s) \Delta_2(s)\big) x -\end{align*} - -

-To the dynamics of the super sensor is: -

-\begin{equation} - \frac{\hat{x}}{x} = 1 + H_1(s) w_1(s) \Delta_1(s) + H_2(s) w_2(s) \Delta_2(s) -\end{equation} - -

-And the noise of the super sensor is: -

-\begin{equation} - n_{ss} = H_1(s) N_1(s) \tilde{n}_1 + H_2(s) N_2(s) \tilde{n}_2 -\end{equation} -
-
- -
-

6.2 Equivalent Configuration

-
-

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

- - -
-

sensor_fusion_equivalent.png -

-

Figure 49: Equivalent Super Sensor (png, pdf).

-
-
-
- -
-

6.3 Model the uncertainty of the super sensor

-
-

-At each frequency \(\omega\), the uncertainty set of the super sensor shown on figure 48 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 49 is a circle centered on \(1\) with a radius equal to \(|w_{ss}(j\omega)|\) on the complex plane. -

- -

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

-
-

-\[ |w_{ss}(j\omega)| = |H_1(j\omega) w_1(j\omega)| + |H_2(j\omega) w_2(j\omega)|, \quad \forall\omega \] -

- -
-
-
- -
-

6.4 Model the noise of the super sensor

-
-

-The PSD of the estimation \(\hat{x}\) when \(x = 0\) of the configuration shown on figure 48 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} \\ - &= | H_1(j\omega) N_1(j\omega) |^2 + | H_2(j\omega) N_2(j\omega) |^2 -\end{align*} - -

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

-\begin{align*} - \Phi_{\hat{x}}(\omega) &= | N_{ss}(j\omega) |^2 \Phi_{\tilde{n}} \\ - &= | N_{ss}(j\omega) |^2 -\end{align*} - -

-Ideally, we want to find a weight \(N_{ss}(s)\) such that: -

-
-

-\[ |N_{ss}(j\omega)|^2 = | H_1(j\omega) N_1(j\omega) |^2 + | H_2(j\omega) N_2(j\omega) |^2 \quad \forall\omega \] -

- -
-
-
- -
-

6.5 First guess

-
-

-We could choose -

-\begin{align*} - w_{ss}(s) &= H_1(s) w_1(s) + H_2(s) w_2(s) \\ - N_{ss}(s) &= H_1(s) N_1(s) + H_2(s) N_2(s) -\end{align*} - -

-But we would have: -

-\begin{align*} - |w_{ss}(j\omega)| &= |H_1(j\omega) w_1(j\omega) + H_2(j\omega) w_2(j\omega)|, \quad \forall\omega \\ - &\neq |H_1(j\omega) w_1(j\omega)| + |H_2(j\omega) w_2(j\omega)|, \quad \forall\omega -\end{align*} -

-and -

-\begin{align*} - |N_{ss}(j\omega)|^2 &= | H_1(j\omega) N_1(j\omega) + H_2(j\omega) N_2(j\omega) |^2 \quad \forall\omega \\ - &\neq | H_1(j\omega) N_1(j\omega)|^2 + |H_2(j\omega) N_2(j\omega) |^2 \quad \forall\omega \\ -\end{align*} -
-
-
- -
-

7 Optimal And Robust Sensor Fusion in Practice

-
-

- -

-

-Here are the steps in order to apply optimal and robust sensor fusion: -

- -
    -
  • Measure the noise characteristics of the sensors to be merged (necessary for "optimal" part of the fusion)
    • -
  • Measure/Estimate the dynamic uncertainty of the sensors (necessary for "robust" part of the fusion)
    • -
  • Apply H2/H-infinity synthesis of the complementary filters
    • -
-
-
-

7.1 Measurement of the noise characteristics of the sensors

-
-
-
-

7.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 50. -

- - -
-

huddle_test.png -

-

Figure 50: Huddle test block diagram

-
- -

-Each sensor has uncorrelated noise \(n_1\) and \(n_2\) and internal dynamics \(G_1(s)\) and \(G_2(s)\) respectively. -

- -

-We here suppose that each sensor has the same magnitude of instrumental noise: \(n_1 = n_2 = n\). -We also assume that their dynamics is ideal: \(G_1(s) = G_2(s) = 1\). -

- -

-We then have: -

-\begin{equation} -\label{orgac9254d} - \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} - -

-Since the input signal \(x\) and the instrumental noise \(n\) are incoherent: -

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

-From equations \eqref{orgac9254d} and \eqref{org86d1282}, we finally obtain -

-
-\begin{equation} -\label{org8797474} - |\Phi_n(\omega)| = |\Phi_{\hat{x}}(\omega)| \left( 1 - \sqrt{\gamma_{\hat{x}_1\hat{x}_2}^2(\omega)} \right) -\end{equation} - -
-
-
- -
-

7.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. -

- -

-The weighting filter \(N(s)\) should be designed such that: -

-\begin{align*} - & \Phi_n(\omega) \approx |N(j\omega)|^2 \Phi_{\tilde{n}}(\omega) \quad \forall \omega \\ - \Longleftrightarrow & |N(j\omega)| \approx \sqrt{\Phi_n(\omega)} \quad \forall \omega -\end{align*} - -

-These weighting filters can then be used to compare the noise level of sensors for the synthesis of complementary filters. -

- -

-The sensor with a normalized noise input is shown in figure 51. -

- - -
-

one_sensor_normalized_noise.png -

-

Figure 51: One sensor with normalized noise

-
-
-
- -
-

7.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. -

- -

-Ideally, the PSD of the noise are such that: -

-\begin{align*} - \Phi_{n_1}(\omega) &< \Phi_{n_2}(\omega) \text{ for } \omega < \omega_m \\ - \Phi_{n_1}(\omega) &> \Phi_{n_2}(\omega) \text{ for } \omega > \omega_m -\end{align*} -
-
- -
-

7.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 -\[ \gamma^2_{xy}(\omega) = \frac{|\Phi_{xy}(\omega)|^2}{|\Phi_{x}(\omega)| |\Phi_{y}(\omega)|} \] -where \(|\Phi_x(\omega)|\) is the output Power Spectral Density (PSD) of signal \(x\) and \(|\Phi_{xy}(\omega)|\) is the Cross Spectral Density (CSD) of signal \(x\) and \(y\). -

- -

-The PSD and CSD are defined as follow: -

-\begin{align} - |\Phi_x(\omega)| &= \frac{2}{n_d T} \sum^{n_d}_{n=1} \left| X_k(\omega, T) \right|^2 \\ - |\Phi_{xy}(\omega)| &= \frac{2}{n_d T} \sum^{n_d}_{n=1} [ X_k^*(\omega, T) ] [ Y_k(\omega, T) ] -\end{align} -

-where: -

-
    -
  • \(n_d\) is the number for records averaged
    • -
  • \(T\) is the length of each record
    • -
  • \(X_k(\omega, T)\) is the finite Fourier transform of the \(k^{\text{th}}\) record
    • -
  • \(X_k^*(\omega, T)\) is its complex conjugate
    • -
-
-
-
- -
-

7.2 Estimate the dynamic uncertainty of the sensors

-
-

-Let's consider one sensor represented on figure 52. -

- -

-The dynamic uncertainty is represented by an input multiplicative uncertainty where \(w(s)\) is a weight that represents the level of the uncertainty. -

-

-The goal is to accurately determine \(w(s)\) for the sensors that have to be merged. -

- - -
-

one_sensor_dyn_uncertainty.png -

-

Figure 52: Sensor with dynamic uncertainty

-
-
-
- -
-

7.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: -

-
    -
  • \(w_1(s)\) and \(w_2(s)\) representing the dynamic uncertainty of both sensors
    • -
  • \(N_1(s)\) and \(N_2(s)\) representing the noise characteristics of both sensors
    • -
- -

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

-
    -
  • the uncertainty on the super sensor dynamics is minimized
    • -
  • the noise sources \(\tilde{n}_1\) and \(\tilde{n}_2\) has the lowest possible effect on the estimation \(\hat{x}\)
    • -
- - -
-

sensor_fusion_full.png -

-

Figure 53: Sensor fusion architecture with sensor dynamics uncertainty

-
-
-
-
- -
-

8 Methods of complementary filter synthesis

-
-

- -

-
-
-

8.1 Complementary filters using analytical formula

-
-

- -

-
-

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

- -
-
- -
-

8.1.1 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 54. -

- -
-
w0 = 2*pi; % [rad/s]
-
-Hh1 = (s/w0)/((s/w0)+1);
-Hl1 = 1/((s/w0)+1);
-
-
- - -
-

comp_filter_1st_order.png -

-

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

-
-
-
- -
-

8.1.2 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 55. -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 55: Effect of the parameter \(\alpha\) on the shape of the generated second order complementary filters (png, pdf)

-
- -

-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. -

-
-
figure;
-plot(alphas, infnorms)
-set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
-xlabel('$\alpha$'); ylabel('$\|H_1\|_\infty$');
-
-
- - -
-

param_alpha_hinf_norm.png -

-

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

-
-
-
- -
-

8.1.3 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 57. -

- -
-
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));
-
-
- - -
-

complementary_filters_third_order.png -

-

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

-
-
-
-
- -
-

8.2 H-Infinity synthesis of complementary filters

-
-

- -

-
-

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

- -
-
- -
-

8.2.1 Synthesis Architecture

-
-

-We here synthesize the complementary filters using the \(\mathcal{H}_\infty\) synthesis. -The goal is to specify upper bounds on the norms of \(H_L\) and \(H_H\) while ensuring their complementary property (\(H_L + H_H = 1\)). -

- -

-In order to do so, we use the generalized plant shown on figure 58 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 58: 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 59) 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 \] -

- -

-Thus, if the above condition is verified, we can define \(H_H = 1 - H_L\) and we have that: -\[ \left\| \begin{array}{c} H_L w_L \\ H_H w_H \end{array} \right\|_\infty < 1 \] -Which is almost (with an maximum error of \(\sqrt{2}\)) equivalent to: -

-\begin{align*} - |H_L| &< \frac{1}{|w_L|}, \quad \forall \omega \\ - |H_H| &< \frac{1}{|w_H|}, \quad \forall \omega -\end{align*} - -

-We then see that \(w_L\) and \(w_H\) can be used to shape both \(H_L\) and \(H_H\) while ensuring (by definition of \(H_H = 1 - H_L\)) their complementary property. -

- - -
-

sf_hinf_filters_b.png -

-

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

-
-
-
- -
-

8.2.2 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;
-
-
- - -
-

weights_wl_wh.png -

-

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

-
-
-
- -
-

8.2.3 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');
-
-
- -
-[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
-
- -

-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 61. -

-
-
Hh_hinf = 1 - Hl_hinf;
-
-
-
-
- -
-

8.2.4 Obtained Complementary Filters

-
-

-The obtained complementary filters are shown on figure 61. -

- - -
-

hinf_filters_results.png -

-

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

-
-
-
-
- -
-

8.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. -

- -

-Thus, all the tools that has been developed for classical feedback control can be used for complementary filter design. -

-
-

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

- -
-
- -
-

8.3.1 Architecture

-
- -
-

complementary_filters_feedback_architecture.png -

-

Figure 62: Architecture used to generate the complementary filters

-
- -

-We have: -\[ y = \underbrace{\frac{L}{L + 1}}_{H_L} y_1 + \underbrace{\frac{1}{L + 1}}_{H_H} y_2 \] -with \(H_L + H_H = 1\). -

- -

-The only thing to design is \(L\) such that the complementary filters are stable with the wanted shape. -

- -

-A simple choice is: -\[ L = \left(\frac{\omega_c}{s}\right)^2 \frac{\frac{s}{\omega_c / \alpha} + 1}{\frac{s}{\omega_c} + \alpha} \] -

- -

-Which contains two integrator and a lead. \(\omega_c\) is used to tune the crossover frequency and \(\alpha\) the trade-off "bump" around blending frequency and filtering away from blending frequency. -

-
-
- -
-

8.3.2 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);
-
-
- - -
-

loop_gain_bode_plot.png -

-

Figure 63: Bode plot of the loop gain \(L\) (png, pdf)

-
-
-
- -
-

8.3.3 Complementary Filters Obtained

-
-

-We then compute the resulting low pass and high pass filters. -

-
-
Hl = L/(L + 1);
-Hh = 1/(L + 1);
-
-
- - -
-

low_pass_high_pass_filters.png -

-

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

-
-
-
-
- -
-

8.4 Analytical Formula found in the literature

-
-

- -

-
- -
-

8.4.1 Analytical Formula

-
-

-min15_compl_filter_desig_angle_estim -

-\begin{align*} - H_L(s) = \frac{K_p s + K_i}{s^2 + K_p s + K_i} \\ - H_H(s) = \frac{s^2}{s^2 + K_p s + K_i} -\end{align*} - -

-corke04_inert_visual_sensin_system_small_auton_helic -

-\begin{align*} - H_L(s) = \frac{1}{s/p + 1} \\ - H_H(s) = \frac{s/p}{s/p + 1} -\end{align*} - -

-jensen13_basic_uas -

-\begin{align*} - H_L(s) = \frac{2 \omega_0 s + \omega_0^2}{(s + \omega_0)^2} \\ - H_H(s) = \frac{s^2}{(s + \omega_0)^2} -\end{align*} - -\begin{align*} - H_L(s) = \frac{C(s)}{C(s) + s} \\ - H_H(s) = \frac{s}{C(s) + s} -\end{align*} - -

-shaw90_bandw_enhan_posit_measur_using_measur_accel -

-\begin{align*} - H_L(s) = \frac{3 \tau s + 1}{(\tau s + 1)^3} \\ - H_H(s) = \frac{\tau^3 s^3 + 3 \tau^2 s^2}{(\tau s + 1)^3} -\end{align*} - -

-baerveldt97_low_cost_low_weigh_attit -

-\begin{align*} - H_L(s) = \frac{2 \tau s + 1}{(\tau s + 1)^2} \\ - H_H(s) = \frac{\tau^2 s^2}{(\tau s + 1)^2} -\end{align*} -
-
- -
-

8.4.2 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;
-
-
- - -
-

comp_filters_literature.png -

-

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

-
-
-
- -
-

8.4.3 Discussion

-
-

-Analytical Formula found in the literature provides either no parameter for tuning the robustness / performance trade-off.

-
-
-
-
-

8.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: -

-
    -
  • the analytical formula provides a very simple way to generate the complementary filters (and thus the controller), they could even be used to tune the controller online using the parameters \(\alpha\) and \(\omega_0\). However, these formula have the property that \(|H_H|\) and \(|H_L|\) are symmetrical with the frequency \(\omega_0\) which may not be desirable.
    • -
  • while the \(\mathcal{H}_\infty\) synthesis of the complementary filters is not as straightforward as using the analytical formula, it provides a more optimized procedure to obtain the complementary filters
    • -
-
-
+

Bibliography

+
+
Barzilai, Aaron, Tom VanZandt, and Tom Kenny. 1998. “Technique for Measurement of the Noise of a Sensor in the Presence of Large Background Signals.” Review of Scientific Instruments 69 (7):2767–72. https://doi.org/10.1063/1.1149013.
+
Moore, Steven Ian, Andrew J. Fleming, and Yuen Kuan Yong. 2019. “Capacitive Instrumentation and Sensor Fusion for High-Bandwidth Nanopositioning.” IEEE Sensors Letters 3 (8):1–3. https://doi.org/10.1109/lsens.2019.2933065.
- -

- -

Bibliography

-
  • [barzilai98_techn_measur_noise_sensor_presen] Aaron Barzilai, Tom VanZandt & Tom Kenny, Technique for Measurement of the Noise of a Sensor in the Presence of Large Background Signals, Review of Scientific Instruments, 69(7), 2767-2772 (1998). link. doi.
    • -
  • [min15_compl_filter_desig_angle_estim] Min & Jeung, Complementary Filter Design for Angle Estimation Using Mems Accelerometer and Gyroscope, Department of Control and Instrumentation, Changwon National University, Changwon, Korea, 641-773 (2015).
    • -
  • [corke04_inert_visual_sensin_system_small_auton_helic] Peter Corke, An Inertial and Visual Sensing System for a Small Autonomous Helicopter, Journal of Robotic Systems, 21(2), 43-51 (2004). link. doi.
    • -
  • [jensen13_basic_uas] Austin Jensen, Cal Coopmans & YangQuan Chen, Basics and guidelines of complementary filters for small UAS navigation, nil, in in: 2013 International Conference on Unmanned Aircraft Systems - (ICUAS), edited by (2013)
    • -
  • [shaw90_bandw_enhan_posit_measur_using_measur_accel] Shaw & Srinivasan, Bandwidth Enhancement of Position Measurements Using Measured Acceleration, Mechanical Systems and Signal Processing, 4(1), 23-38 (1990). link. doi.
    • -
  • [baerveldt97_low_cost_low_weigh_attit] Baerveldt & Klang, A Low-Cost and Low-Weight Attitude Estimation System for an Autonomous Helicopter, nil, in in: Proceedings of IEEE International Conference on Intelligent Engineering Systems, edited by (1997)
    • -
-

Author: Thomas Dehaeze

-

Created: 2019-10-02 mer. 22:48

-

Validate

+

Created: 2020-09-23 mer. 15:37

diff --git a/matlab/index.org b/matlab/index.org index 3eb5ff7..4f3931f 100644 --- a/matlab/index.org +++ b/matlab/index.org @@ -29,15 +29,238 @@ In this document, the optimal and robust design of complementary filters is stud Two sensors are considered with both different noise characteristics and dynamical uncertainties represented by multiplicative input uncertainty. -- in section [[sec:optimal_comp_filters]]: the $\mathcal{H}_2$ synthesis is used to design complementary filters such that the RMS value of the super sensor's noise is minimized -- in section [[sec:comp_filter_robustness]]: the $\mathcal{H}_\infty$ synthesis is used to design complementary filters such that the super sensor's uncertainty is bonded to acceptable values -- in section [[sec:mixed_synthesis_sensor_fusion]]: the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis is used to both limit the super sensor's uncertainty and to lower the RMS value of the super sensor's noise -- in section [[sec:hinf_syn_perf_robust]]: the $\mathcal{H}_\infty$ synthesis is used for both limiting the noise and uncertainty of the super sensor -- in section [[sec:equi_super_sensor]]: we try to find the characteristics of the super sensor from the characteristics of the individual sensors and of the complementary filters -- in section [[sec:opti_robust_practice]]: a methodology is proposed to apply optimal and robust sensor fusion in practice -- in section [[sec:comp_filter_synthesis]]: methods of complementary filter synthesis are proposed +- Section [[sec:optimal_comp_filters]]: the $\mathcal{H}_2$ synthesis is used to design complementary filters such that the RMS value of the super sensor's noise is minimized +- Section [[sec:comp_filter_robustness]]: the $\mathcal{H}_\infty$ synthesis is used to design complementary filters such that the super sensor's uncertainty is bonded to acceptable values +- Section [[sec:mixed_synthesis_sensor_fusion]]: the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis is used to both limit the super sensor's uncertainty and to lower the RMS value of the super sensor's noise -* Optimal Sensor Fusion - Minimize the Super Sensor Noise +* Sensor Description +** Introduction :ignore: + +- [ ] Schematic of one sensor + +** Matlab Init :noexport:ignore: +#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) + <> +#+end_src + +#+begin_src matlab :exports none :results silent :noweb yes + <> +#+end_src + +#+begin_src matlab + freqs = logspace(0, 4, 1000); +#+end_src + +** Sensor Dynamics +Interferometer/Capacitive Sensor: +#+begin_src matlab + w_pos = 2*pi*2e3; % Measurement Banwdith [rad/s] + g_pos = 1e4; % Gain [V/m] + + G_pos = g_pos/s/(1 + s/w_pos); % Position Sensor Plant [V/(m/s)] +#+end_src + +Accelerometer: +#+begin_src matlab + m_acc = 0.01; % Inertial Mass [kg] + c_acc = 5; % Damping [N/(m/s)] + k_acc = 1e5; % Stiffness [N/m] + g_acc = 1e5; % Gain [V/m] + + G_acc = -g_acc*m_acc*s/(m_acc*s^2 + c_acc*s + k_acc); % Accelerometer Plant [V/(m/s)] +#+end_src + +#+begin_src matlab :exports none + figure; + % Magnitude + ax1 = subplot(2,1,1); + hold on; + plot(freqs, abs(squeeze(freqresp(G_pos, freqs, 'Hz'))), '-', 'DisplayName', '$G_{pos}$'); + plot(freqs, abs(squeeze(freqresp(G_acc, freqs, 'Hz'))), '-', 'DisplayName', '$G_{acc}$'); + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + ylabel('Magnitude'); set(gca, 'XTickLabel',[]); + legend('location', 'northeast'); + hold off; + + % Phase + ax2 = subplot(2,1,2); + hold on; + plot(freqs, 180/pi*angle(squeeze(freqresp(G_pos, freqs, 'Hz'))), '-'); + plot(freqs, 180/pi*angle(squeeze(freqresp(G_acc, 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)]); +#+end_src + +#+begin_src matlab :exports none + % w_pos_u = ureal('w_pos', w_pos, 'Percentage', 50); % Measurement Bandwidth [rad/s] + % g_pos_u = ureal('g_pos', g_pos, 'Percentage', 15); % Measurement Gain [V/m] + + % G_pos_u = g_pos_u/s/(1 + s/w_pos_u); % Position Sensor Plant Model [V/(m/s)] + + % m_acc_u = ureal('m_acc', m_acc, 'Percentage', 30); % Inertial Mass [kg] + % c_acc_u = ureal('c_acc', c_acc, 'Percentage', 50); % Damping [N/(m/s)] + % k_acc_u = ureal('k_acc', k_acc, 'Percentage', 20); % Stiffness [N/m] + % g_acc_u = ureal('g_acc', g_acc, 'Percentage', 20); % Gain + + % G_acc_u = -g_acc_u*m_acc_u*s/(m_acc_u*s^2 + c_acc_u*s + k_acc_u); % Accelerometer Model [V/(m/s)] + + % Gss_u = H_acc*inv(G_acc)*G_acc_u + H_pos*inv(G_pos)*G_pos_u; +#+end_src + +** Sensor Noise +Noise in $[m/s/\sqrt{Hz}]$. +#+begin_src matlab + omegac = 1000*2*pi; G0 = 1e-6; Ginf = 1e-3; + N_pos = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4); + + omegac = 0.05*2*pi; G0 = 1e-1; Ginf = 1e-6; + N_acc = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4); +#+end_src + +#+begin_src matlab :exports none + figure; + hold on; + plot(freqs, abs(squeeze(freqresp(N_pos, freqs, 'Hz'))), '-', 'DisplayName', '$N_{pos}$'); + plot(freqs, abs(squeeze(freqresp(N_acc, freqs, 'Hz'))), '-', 'DisplayName', '$N_{acc}$'); + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + xlabel('Frequency [Hz]'); ylabel('Amplitude Spectral Density $\left[ \frac{m/s}{\sqrt{Hz}} \right]$'); + hold off; + xlim([freqs(1), freqs(end)]); + legend('location', 'northeast'); +#+end_src + +** Sensor Model Uncertainty +The model uncertainty is described by multiplicative uncertainty. +#+begin_src matlab + n = 2; w0 = 2*pi*5e3; G0 = 0.05; G1 = 2; Gc = 1; + W_pos = (((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; + + G_acc_low = 2; G_acc_mid = 0.1; G_acc_hig = 4; + w_acc_low = 2*pi*3; w_acc_hig = 2*pi*400; + + n = 2; w0 = w_acc_low; G0 = G_acc_low; G1 = G_acc_mid; Gc = 1; + W_acc = (((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 = 2; w0 = w_acc_hig; G0 = 1; G1 = G_acc_hig/G_acc_mid; Gc = 1/G_acc_mid; + W_acc = W_acc*(((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n; +#+end_src + +#+begin_src matlab :exports none + Dphi_pos = 180/pi*asin(abs(squeeze(freqresp(W_pos, freqs, 'Hz')))); + Dphi_pos(abs(squeeze(freqresp(W_pos, freqs, 'Hz'))) > 1) = 360; + + Dphi_acc = 180/pi*asin(abs(squeeze(freqresp(W_acc, freqs, 'Hz')))); + Dphi_acc(abs(squeeze(freqresp(W_acc, freqs, 'Hz'))) > 1) = 360; + + figure; + % Magnitude + ax1 = subplot(2,1,1); + hold on; + p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W_acc, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W_acc, freqs, 'Hz'))), 1e-6))], 'w'); + p.FaceColor = [0.8500 0.3250 0.0980]; + p.EdgeColor = 'none'; + p.FaceAlpha = 0.3; + p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W_pos, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W_pos, freqs, 'Hz'))), 1e-6))], 'w'); + p.FaceColor = [0 0.4470 0.7410]; + p.EdgeColor = 'none'; + p.FaceAlpha = 0.3; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + set(gca, 'XTickLabel',[]); + ylabel('Magnitude'); + ylim([1e-2, 1e1]); + hold off; + legend('location', 'northeast'); + + % Phase + ax2 = subplot(2,1,2); + hold on; + p = patch([freqs flip(freqs)], [Dphi_acc; flip(-Dphi_acc)], 'w'); + p.FaceColor = [0.8500 0.3250 0.0980]; + p.EdgeColor = 'none'; + p.FaceAlpha = 0.3; + p = patch([freqs flip(freqs)], [Dphi_pos; flip(-Dphi_pos)], 'w'); + p.FaceColor = [0 0.4470 0.7410]; + p.EdgeColor = 'none'; + p.FaceAlpha = 0.3; + 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)]); +#+end_src + +#+begin_src matlab :exports none + Dphi_pos = 180/pi*asin(abs(squeeze(freqresp(W_pos, freqs, 'Hz')))); + Dphi_pos(abs(squeeze(freqresp(W_pos, freqs, 'Hz'))) > 1) = 360; + + G_pos_mag = abs(squeeze(freqresp(G_pos, freqs, 'Hz'))); + G_pos_ang = 180/pi*angle(squeeze(freqresp(G_pos, freqs, 'Hz'))); + + Dphi_acc = 180/pi*asin(abs(squeeze(freqresp(W_acc, freqs, 'Hz')))); + Dphi_acc(abs(squeeze(freqresp(W_acc, freqs, 'Hz'))) > 1) = 360; + + G_acc_mag = abs(squeeze(freqresp(G_acc, freqs, 'Hz'))); + G_acc_ang = 180/pi*angle(squeeze(freqresp(G_acc, freqs, 'Hz'))); + + figure; + % Magnitude + ax1 = subplot(2,1,1); + hold on; + p = patch([freqs flip(freqs)], ... + [G_pos_mag.*(1 + abs(squeeze(freqresp(W_pos, freqs, 'Hz')))); flip(G_pos_mag.*max(1 - abs(squeeze(freqresp(W_pos, freqs, 'Hz'))), 0.001))], ... + 'w', 'DisplayName', '$G_{pos}$'); + p.FaceColor = [0 0.4470 0.7410]; p.EdgeColor = 'none'; p.FaceAlpha = 0.3; + p = patch([freqs flip(freqs)], ... + [G_acc_mag.*(1 + abs(squeeze(freqresp(W_acc, freqs, 'Hz')))); flip(G_acc_mag.*max(1 - abs(squeeze(freqresp(W_acc, freqs, 'Hz'))), 1e-6))], ... + 'w', 'DisplayName', '$G_{acc}$'); + p.FaceColor = [0.8500 0.3250 0.0980]; + p.EdgeColor = 'none'; + p.FaceAlpha = 0.3; + plot(freqs, G_pos_mag, '-', 'color', [0 0.4470 0.7410], ... + 'DisplayName', '$\hat{G}_{pos}$'); + plot(freqs, G_acc_mag, '-', 'color', [0.8500 0.3250 0.0980], ... + 'DisplayName', '$\hat{G}_{acc}$'); + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + set(gca, 'XTickLabel',[]); + ylabel('Magnitude'); + ylim([1e-2, 1e3]); + hold off; + legend('location', 'northeast') + + % Phase + ax2 = subplot(2,1,2); + hold on; + p = patch([freqs flip(freqs)], [G_pos_ang+Dphi_pos; flip(G_pos_ang-Dphi_pos)], 'w'); + p.FaceColor = [0 0.4470 0.7410]; + p.EdgeColor = 'none'; + p.FaceAlpha = 0.3; + p = patch([freqs flip(freqs)], [G_acc_ang+Dphi_acc; flip(G_acc_ang-Dphi_acc)], 'w'); + p.FaceColor = [0.8500 0.3250 0.0980]; + p.EdgeColor = 'none'; + p.FaceAlpha = 0.3; + plot(freqs, G_pos_ang, '-', 'color', [0 0.4470 0.7410]); + plot(freqs, G_acc_ang, '-', 'color', [0.8500 0.3250 0.0980]); + 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)]); +#+end_src + +** Save Model +#+begin_src matlab + save('./mat/model.mat', 'freqs', 'G_acc', 'G_pos', 'N_pos', 'N_acc', 'W_pos', 'W_acc'); +#+end_src + +* Optimal Super Sensor Noise: $\mathcal{H}_2$ Synthesis with Acc and Pos :PROPERTIES: :header-args:matlab+: :tangle matlab/optimal_comp_filters.m :header-args:matlab+: :comments org :mkdirp yes @@ -51,11 +274,926 @@ Doing so, one "super sensor" is obtained that can have better noise characterist The complementary filters have to be designed in order to minimize the effect noise of each sensor on the super sensor noise. -** ZIP file containing the data and matlab files :ignore: +** ZIP file containing the data and matlab files :ignore: #+begin_note The Matlab scripts is accessible [[file:matlab/optimal_comp_filters.m][here]]. #+end_note +** Matlab Init :noexport:ignore: +#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) + <> +#+end_src + +#+begin_src matlab :exports none :results silent :noweb yes + <> +#+end_src + +#+begin_src matlab + load('./mat/model.mat', 'freqs', 'G_acc', 'G_pos', 'N_pos', 'N_acc', 'W_pos', 'W_acc'); +#+end_src + +** 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{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 +\end{pmatrix} +\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} \] + +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]]. +#+begin_src matlab + P = [N_acc -N_acc; + 0 N_pos; + 1 0]; +#+end_src + +And we do the $\mathcal{H}_2$ synthesis using the =h2syn= command. +#+begin_src matlab + [H_pos, ~, gamma] = h2syn(P, 1, 1); +#+end_src + +Finally, we define $H_2(s) = 1 - H_1(s)$. +#+begin_src matlab + H_acc = 1 - H_pos; +#+end_src + +#+begin_src matlab :exports none + % Filters are saved for further use + save('./mat/H2_filters.mat', 'H_pos', 'H_acc'); +#+end_src + +The complementary filters obtained are shown on figure [[fig:htwo_comp_filters]]. +#+begin_src matlab :exports none + figure; + hold on; + plot(freqs, abs(squeeze(freqresp(H_pos, freqs, 'Hz'))), '-', 'DisplayName', '$H_{pos}$'); + plot(freqs, abs(squeeze(freqresp(H_acc, freqs, 'Hz'))), '-', 'DisplayName', '$H_{acc}$'); + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + xlabel('Frequency [Hz]'); ylabel('Magnitude'); + hold off; + xlim([freqs(1), freqs(end)]); + legend('location', 'northeast'); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace + exportFig('figs/htwo_comp_filters.pdf', 'width', 'full', 'height', 'tall'); +#+end_src + +#+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]]) +#+RESULTS: +[[file:figs/htwo_comp_filters.png]] + +** Sensor Noise +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. + +#+begin_src matlab + PSD_S_pos = abs(squeeze(freqresp(N_pos, freqs, 'Hz'))).^2; + PSD_S_acc = abs(squeeze(freqresp(N_acc, freqs, 'Hz'))).^2; + PSD_H2 = abs(squeeze(freqresp(N_pos*H_pos, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N_acc*H_acc, freqs, 'Hz'))).^2; + + CPS_S_pos = cumtrapz(freqs, PSD_S_pos); + CPS_S_acc = cumtrapz(freqs, PSD_S_acc); + CPS_H2 = cumtrapz(freqs, PSD_H2); +#+end_src + +#+begin_src matlab :exports none + figure; + hold on; + plot(freqs, PSD_S_pos, '-', 'DisplayName', '$\Phi_{\hat{x}_{pos}}$'); + plot(freqs, PSD_S_acc, '-', 'DisplayName', '$\Phi_{\hat{x}_{acc}}$'); + 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 [$(m/s)^2/Hz$]'); + hold off; + xlim([freqs(1), freqs(end)]); + legend('location', 'northeast'); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace + exportFig('figs/psd_sensors_htwo_synthesis.pdf', 'width', 'full', 'height', 'tall'); +#+end_src + +#+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]]) +#+RESULTS: +[[file:figs/psd_sensors_htwo_synthesis.png]] + +#+begin_src matlab :exports none + figure; + hold on; + plot(freqs, CPS_S_pos, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{pos}} = %.1e$ [m/s rms]', sqrt(CPS_S_pos(end)))); + plot(freqs, CPS_S_acc, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{acc}} = %.1e$ [m/s rms]', sqrt(CPS_S_acc(end)))); + plot(freqs, CPS_H2, 'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_2}} = %.1e$ [m/s rms]', sqrt(CPS_H2(end)))); + set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); + xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum'); + hold off; + xlim([2*freqs(1), freqs(end)]); + % ylim([1e-10 1e-5]); + legend('location', 'southeast'); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace + exportFig('figs/cps_h2_synthesis.pdf', 'width', 'full', 'height', 'tall'); +#+end_src + +#+name: fig:cps_h2_synthesis +#+caption: Cumulative Power Spectrum of individual sensors and super sensor using the $\mathcal{H}_2$ synthesis ([[./figs/cps_h2_synthesis.png][png]], [[./figs/cps_h2_synthesis.pdf][pdf]]) +#+RESULTS: +[[file:figs/cps_h2_synthesis.png]] + +#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) + data2orgtable([sqrt(CPS_S_pos(end)), sqrt(CPS_S_acc(end)), sqrt(CPS_H2(end))]', {'Derived Position', 'Integrated Acceleration', 'Super Sensor - $\mathcal{H}_2$'}, {'RMS [m/s]'}, ' %.1e '); +#+end_src + +#+RESULTS: +| | RMS [m/s] | +|--------------------------------+-----------| +| Derived Position | 0.08 | +| Integrated Acceleration | 0.005 | +| Super Sensor - $\mathcal{H}_2$ | 0.0012 | + +** Time Domain Simulation +Parameters of the time domain simulation. +#+begin_src matlab + Fs = 1e4; % Sampling Frequency [Hz] + Ts = 1/Fs; % Sampling Time [s] + + t = 0:Ts:2; % Time Vector [s] +#+end_src + +Time domain velocity. +#+begin_src matlab + v = 0.1*sin((10*t).*t)'; +#+end_src + +Generate noises in velocity corresponding to sensor 1 and 2: +#+begin_src matlab + n_pos = lsim(N_pos, sqrt(Fs/2)*randn(length(t), 1), t); + n_acc = lsim(N_acc, sqrt(Fs/2)*randn(length(t), 1), t); +#+end_src + +#+begin_src matlab :exports none + figure; + hold on; + plot(t, n_pos, 'DisplayName', 'Differentiated Position'); + plot(t, n_acc, 'DisplayName', 'Integrated Acceleration'); + plot(t, (lsim(H_pos, n_pos, t)+lsim(H_acc, n_acc, t)), 'DisplayName', 'Super Sensor'); + hold off; + xlabel('Time [s]'); ylabel('Velocity [m/s]'); + legend(); +#+end_src + +#+begin_src matlab :exports none + figure; + hold on; + plot(t, v+n_pos, 'DisplayName', 'Differentiated Position'); + plot(t, v+n_acc, 'DisplayName', 'Integrated Acceleration'); + plot(t, v+(lsim(H_pos, n_pos, t)+lsim(H_acc, n_acc, t)), 'DisplayName', 'Super Sensor'); + plot(t, v, 'k--', 'DisplayName', 'True Velocity'); + hold off; + xlabel('Time [s]'); ylabel('Velocity [m/s]'); + legend(); + ylim([-0.3, 0.3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace + exportFig('figs/super_sensor_time_domain_h2.pdf', 'width', 'full', 'height', 'tall'); +#+end_src + +#+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]] + +** Discrepancy between sensor dynamics and model +Now suppose some difference between the sensor dynamics and the sensor model. +#+begin_src matlab + G_pos_u = G_pos*(1 + W_pos*ultidyn('Delta',[1 1])); + G_acc_u = G_acc*(1 + W_acc*ultidyn('Delta',[1 1])); + + Gss_u = H_acc*inv(G_acc)*G_acc_u + H_pos*inv(G_pos)*G_pos_u; +#+end_src + +#+begin_src matlab :exports none + Dphiss = 180/pi*asin(abs(squeeze(freqresp(W_pos*H_pos, freqs, 'Hz')))+abs(squeeze(freqresp(W_acc*H_acc, freqs, 'Hz')))); + Dphiss(abs(squeeze(freqresp(W_pos*H_pos, freqs, 'Hz')))+abs(squeeze(freqresp(W_acc*H_acc, freqs, 'Hz'))) > 1) = 190; +#+end_src + +#+begin_src matlab :exports none + Dphi_pos = 180/pi*asin(abs(squeeze(freqresp(W_pos, freqs, 'Hz')))); + Dphi_pos(abs(squeeze(freqresp(W_pos, freqs, 'Hz'))) > 1) = 360; + + Dphi_acc = 180/pi*asin(abs(squeeze(freqresp(W_acc, freqs, 'Hz')))); + Dphi_acc(abs(squeeze(freqresp(W_acc, freqs, 'Hz'))) > 1) = 360; + + Dphi_ss = 180/pi*asin(abs(squeeze(freqresp(W_pos*H_pos, freqs, 'Hz'))) + abs(squeeze(freqresp(W_acc*H_acc, freqs, 'Hz')))); + Dphi_ss(abs(squeeze(freqresp(W_pos*H_pos, freqs, 'Hz'))) + abs(squeeze(freqresp(W_acc*H_acc, freqs, 'Hz'))) > 1) = 360; + + figure; + % Magnitude + ax1 = subplot(2,1,1); + hold on; + p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W_acc, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W_acc, freqs, 'Hz'))), 1e-6))], 'w'); + p.FaceColor = [0.8500 0.3250 0.0980]; + p.EdgeColor = 'none'; + p.FaceAlpha = 0.3; + p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W_pos, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W_pos, freqs, 'Hz'))), 0.001))], 'w'); + p.FaceColor = [0 0.4470 0.7410]; + p.EdgeColor = 'none'; + p.FaceAlpha = 0.3; + p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W_pos*H_pos, freqs, 'Hz')))+abs(squeeze(freqresp(W_acc*H_acc, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W_pos*H_pos, freqs, 'Hz')))-abs(squeeze(freqresp(W_acc*H_acc, freqs, 'Hz'))), 0.001))], 'w'); + p.EdgeColor = 'black'; + p.FaceAlpha = 0; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + set(gca, 'XTickLabel',[]); + ylabel('Magnitude'); + ylim([1e-2, 1e3]); + hold off; + + % Phase + ax2 = subplot(2,1,2); + hold on; + p = patch([freqs flip(freqs)], [Dphi_acc; flip(-Dphi_acc)], 'w'); + p.FaceColor = [0.8500 0.3250 0.0980]; + p.EdgeColor = 'none'; + p.FaceAlpha = 0.3; + p = patch([freqs flip(freqs)], [Dphi_pos; flip(-Dphi_pos)], 'w'); + p.FaceColor = [0 0.4470 0.7410]; + p.EdgeColor = 'none'; + p.FaceAlpha = 0.3; + p = patch([freqs flip(freqs)], [Dphi_ss; flip(-Dphi_ss)], 'w'); + p.EdgeColor = 'black'; + p.FaceAlpha = 0; + 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)]); +#+end_src + + + + + + + + + + + + + + +#+begin_src matlab :exports none + G_acc_ms = usample(G_acc_m, 10); + + figure; + % Magnitude + ax1 = subplot(2,1,1); + hold on; + for i = 1:length(G_acc_ms) + plot(freqs, abs(squeeze(freqresp(G_acc_ms(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2]); + end + plot(freqs, abs(squeeze(freqresp(G_acc, freqs, 'Hz'))), 'k--'); + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + set(gca, 'XTickLabel',[]); + ylabel('Magnitude'); + hold off; + + % Phase + ax2 = subplot(2,1,2); + hold on; + for i = 1:length(G_acc_ms) + plot(freqs, 180/pi*angle(squeeze(freqresp(G_acc_ms(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2]); + end + plot(freqs, 180/pi*angle(squeeze(freqresp(G_acc, freqs, 'Hz'))), 'k--'); + set(gca,'xscale','log'); + yticks(-180:90:180); + ylim([-180 180]); + xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); + hold off; + linkaxes([ax1,ax2],'x'); +#+end_src + +#+begin_src matlab :exports none + G_pos_ms = usample(G_pos_m, 10); + + figure; + % Magnitude + ax1 = subplot(2,1,1); + hold on; + for i = 1:length(G_pos_ms) + plot(freqs, abs(squeeze(freqresp(G_pos_ms(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2]); + end + plot(freqs, abs(squeeze(freqresp(G_pos, freqs, 'Hz'))), 'k--'); + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + set(gca, 'XTickLabel',[]); + ylabel('Magnitude'); + hold off; + + % Phase + ax2 = subplot(2,1,2); + hold on; + for i = 1:length(G_pos_ms) + plot(freqs, 180/pi*angle(squeeze(freqresp(G_pos_ms(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2]); + end + plot(freqs, 180/pi*angle(squeeze(freqresp(G_pos, freqs, 'Hz'))), 'k--'); + set(gca,'xscale','log'); + yticks(-180:90:180); + ylim([-180 180]); + xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); + hold off; + linkaxes([ax1,ax2],'x'); +#+end_src + +#+begin_src matlab :exports none + G_ss_us = usample(Gss_u, 100); + + figure; + % Magnitude + ax1 = subplot(2,1,1); + hold on; + % for i = 1:length(G_ss_us) + % plot(freqs, abs(squeeze(freqresp(G_ss_us(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2]); + % end + p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W_pos*H_pos, freqs, 'Hz'))) + abs(squeeze(freqresp(W_acc*H_acc, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W_pos*H_pos, freqs, 'Hz'))) - abs(squeeze(freqresp(W_acc*H_acc, freqs, 'Hz'))), 0.001))], 'w'); + p.FaceColor = [0.8500 0.3250 0.0980]; + p.EdgeColor = 'none'; + p.FaceAlpha = 0.3; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + set(gca, 'XTickLabel',[]); + ylabel('Magnitude'); + ylim([0.1, 10]); + hold off; + + % Phase + ax2 = subplot(2,1,2); + hold on; + % for i = 1:length(G_ss_us) + % plot(freqs, 180/pi*angle(squeeze(freqresp(G_ss_us(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2]); + % end + % plot(freqs, Dphiss, 'k--'); + % plot(freqs, -Dphiss, 'k--'); + p = patch([freqs flip(freqs)], [Dphiss; flip(-Dphiss)], 'w'); + p.FaceColor = [0.8500 0.3250 0.0980]; + p.EdgeColor = 'none'; + p.FaceAlpha = 0.3; + 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)]); +#+end_src + +** 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). + +However, the synthesis does not take into account the robustness of the sensor fusion. + +* Robust Sensor Fusion: $\mathcal{H}_\infty$ Synthesis with Acc and Pos +:PROPERTIES: +:header-args:matlab+: :tangle matlab/comp_filter_robustness.m +:header-args:matlab+: :comments org :mkdirp yes +:END: +<> + +** Introduction :ignore: +We initially considered perfectly known sensor dynamics so that it can be perfectly inverted. + +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. [[fig:sensor_fusion_dynamic_uncertainty]]. + +#+name: fig:sensor_fusion_dynamic_uncertainty +#+caption: Sensor fusion architecture with sensor dynamics uncertainty +[[file:figs-tikz/sensor_fusion_dynamic_uncertainty.png]] + +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_note + The Matlab scripts is accessible [[file:matlab/comp_filter_robustness.m][here]]. +#+end_note + +** Matlab Init :noexport:ignore: +#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) + <> +#+end_src + +#+begin_src matlab :exports none :results silent :noweb yes + <> +#+end_src + +#+begin_src matlab + load('./mat/model.mat', 'freqs', 'G_acc', 'G_pos', 'N_pos', 'N_acc', 'W_pos', 'W_acc'); +#+end_src + +** Super Sensor Dynamical Uncertainty +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]]. + +#+name: fig:sensor_fusion_dynamic_uncertainty +#+caption: Fusion of two sensors with input multiplicative uncertainty +[[file:figs-tikz/sensor_fusion_dynamic_uncertainty.png]] + +The dynamics of the super sensor is represented by +\begin{align*} + \frac{\hat{x}}{x} &= (1 + W_1 \Delta_1) H_1 + (1 + W_2 \Delta_2) H_2 \\ + &= 1 + W_1 H_1 \Delta_1 + W_2 H_2 \Delta_2 +\end{align*} +with $\Delta_i$ is any transfer function satisfying $\| \Delta_i \|_\infty < 1$. + +We see that as soon as we have some uncertainty in the sensor dynamics, we have that the complementary filters have some effect on the transfer function from $x$ to $\hat{x}$. + +The uncertainty set of the transfer function from $\hat{x}$ to $x$ at frequency $\omega$ is bounded in the complex plane by a circle centered on 1 and with a radius equal to $|W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)|$ (figure [[fig:uncertainty_gain_phase_variation]]). + +We then have that the angle introduced by the super sensor is bounded by $\arcsin(\epsilon)$: +\[ \angle \frac{\hat{x}}{x}(j\omega) \le \arcsin \Big(|W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)|\Big) \] + +#+name: fig:uncertainty_gain_phase_variation +#+caption: Maximum phase variation +[[file:figs-tikz/uncertainty_gain_phase_variation.png]] + +** Synthesis objective +The uncertainty region of the super sensor dynamics is represented by a circle in the complex plane as shown in Fig. [[fig:uncertainty_gain_phase_variation]]. + +At each frequency $\omega$, the radius of the circle is $|W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)|$. + +Thus, the phase shift $\Delta\phi(\omega)$ due to the super sensor uncertainty is bounded by: +\[ |\Delta\phi(\omega)| \leq \arcsin\big( |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| \big) \] + +Let's define some allowed frequency depend phase shift $\Delta\phi_\text{max}(\omega) > 0$ such that: +\[ |\Delta\phi(\omega)| < \Delta\phi_\text{max}(\omega), \quad \forall\omega \] + + +If $H_1(s)$ and $H_2(s)$ are designed such that +\[ |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| < \sin\big( \Delta\phi_\text{max}(\omega) \big) \] + +The maximum phase shift due to dynamic uncertainty at frequency $\omega$ will be $\Delta\phi_\text{max}(\omega)$. + +** Requirements as an $\mathcal{H}_\infty$ norm +We now try to express this requirement in terms of an $\mathcal{H}_\infty$ norm. + +Let's define one weight $W_\phi(s)$ that represents the maximum wanted phase uncertainty: +\[ |W_{\phi}(j\omega)|^{-1} \approx \sin(\Delta\phi_{\text{max}}(\omega)), \quad \forall\omega \] + +Then: +\begin{align*} + & |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| < \sin\big( \Delta\phi_\text{max}(\omega) \big), \quad \forall\omega \\ + \Longleftrightarrow & |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| < |W_\phi(j\omega)|^{-1}, \quad \forall\omega \\ + \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 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 +\end{equation} + +One should not forget that at frequency where both sensors has unknown dynamics ($|W_1(j\omega)| > 1$ and $|W_2(j\omega)| > 1$), the super sensor dynamics will also be unknown and the phase uncertainty cannot be bounded. +Thus, at these frequencies, $|W_\phi|$ should be smaller than $1$. + +** 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]]. + +#+begin_src matlab + Dphi = 5; % [deg] + + n = 4; w0 = 2*pi*7e2; G0 = 1/sin(Dphi*pi/180); Ginf = 1/5; Gc = 1; + W_u = (((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; +#+end_src + +#+begin_src matlab :exports none + Dphi_pos = 180/pi*asin(abs(squeeze(freqresp(W_pos, freqs, 'Hz')))); + Dphi_pos(abs(squeeze(freqresp(W_pos, freqs, 'Hz'))) > 1) = 360; + + Dphi_acc = 180/pi*asin(abs(squeeze(freqresp(W_acc, freqs, 'Hz')))); + Dphi_acc(abs(squeeze(freqresp(W_acc, freqs, 'Hz'))) > 1) = 360; + + Dphi_max = 180/pi*asin(1./abs(squeeze(freqresp(W_u, freqs, 'Hz')))); + Dphi_max(1./abs(squeeze(freqresp(W_u, freqs, 'Hz'))) > 1) = 360; + + figure; + % Magnitude + ax1 = subplot(2,1,1); + hold on; + p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W_acc, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W_acc, freqs, 'Hz'))), 1e-6))], 'w'); + p.FaceColor = [0.8500 0.3250 0.0980]; + p.EdgeColor = 'none'; + p.FaceAlpha = 0.3; + p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W_pos, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W_pos, freqs, 'Hz'))), 1e-6))], 'w'); + p.FaceColor = [0 0.4470 0.7410]; + p.EdgeColor = 'none'; + p.FaceAlpha = 0.3; + p = patch([freqs flip(freqs)], [1 + 1./abs(squeeze(freqresp(W_u, freqs, 'Hz'))); flip(max(1 - 1./abs(squeeze(freqresp(W_u, freqs, 'Hz'))), 1e-6))], 'w'); + p.EdgeColor = 'black'; + p.FaceAlpha = 0; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + set(gca, 'XTickLabel',[]); + ylabel('Magnitude'); + ylim([1e-2, 1e1]); + hold off; + + % Phase + ax2 = subplot(2,1,2); + hold on; + p = patch([freqs flip(freqs)], [Dphi_acc; flip(-Dphi_acc)], 'w'); + p.FaceColor = [0.8500 0.3250 0.0980]; + p.EdgeColor = 'none'; + p.FaceAlpha = 0.3; + p = patch([freqs flip(freqs)], [Dphi_pos; flip(-Dphi_pos)], 'w'); + p.FaceColor = [0 0.4470 0.7410]; + p.EdgeColor = 'none'; + p.FaceAlpha = 0.3; + p = patch([freqs flip(freqs)], [Dphi_max; flip(-Dphi_max)], 'w'); + p.EdgeColor = 'black'; + p.FaceAlpha = 0; + 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)]); +#+end_src + +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]]. + +#+begin_src matlab :exports none + figure; + hold on; + plot(freqs, 1./abs(squeeze(freqresp(W_u*W_pos, freqs, 'Hz'))), '-', 'DisplayName', '$1/|W_1W_\phi|$'); + plot(freqs, 1./abs(squeeze(freqresp(W_u*W_acc, 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'); +#+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-normal" :post pdf2svg(file=*this*, ext="png") + <> +#+end_src + +#+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]] + +** $\mathcal{H}_\infty$ Synthesis +The $\mathcal{H}_\infty$ synthesis architecture used for the complementary filters is shown in Fig. [[fig:h_infinity_robust_fusion]]. + +#+name: fig:h_infinity_robust_fusion +#+caption: Architecture used for $\mathcal{H}_\infty$ synthesis of complementary filters +[[file:figs-tikz/h_infinity_robust_fusion.png]] + +The generalized plant is defined below. +#+begin_src matlab + P = [W_u*W_acc -W_u*W_acc; + 0 W_u*W_pos; + 1 0]; +#+end_src + +And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command. +#+begin_src matlab :results output replace :exports both + [H_pos, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on'); +#+end_src + +#+RESULTS: +#+begin_example +[H_pos, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on'); + + Test bounds: 0.5735 <= gamma <= 1.041 + + gamma X>=0 Y>=0 rho(XY)<1 p/f + 7.728e-01 0.0e+00 0.0e+00 1.208e-15 p + 6.658e-01 0.0e+00 0.0e+00 2.262e-15 p + 6.179e-01 0.0e+00 0.0e+00 5.324e-16 p + 5.953e-01 0.0e+00 0.0e+00 0.000e+00 p + 5.843e-01 0.0e+00 0.0e+00 4.055e-16 p + 5.789e-01 0.0e+00 0.0e+00 7.317e-16 p + 5.762e-01 0.0e+00 0.0e+00 0.000e+00 p + 5.748e-01 0.0e+00 0.0e+00 5.293e-15 p + 5.742e-01 0.0e+00 0.0e+00 2.379e-15 p + 5.738e-01 0.0e+00 0.0e+00 1.732e-15 p + Limiting gains... + 5.741e-01 4.0e-09 0.0e+00 7.347e-15 p + + Best performance (actual): 0.5738 +#+end_example + +And $H_1(s)$ is defined as the complementary of $H_2(s)$. +#+begin_src matlab + H_acc = 1 - H_pos; +#+end_src + +#+begin_src matlab :exports none + save('./mat/Hinf_filters.mat', 'H_pos', 'H_acc'); +#+end_src + +The obtained complementary filters are shown in Fig. [[fig:comp_filter_hinf_uncertainty]]. +#+begin_src matlab :exports none + figure; + + ax1 = subplot(2,1,1); + hold on; + set(gca,'ColorOrderIndex',1) + plot(freqs, 1./abs(squeeze(freqresp(W_u*W_pos, freqs, 'Hz'))), '--', 'DisplayName', '$W_{pos}$'); + set(gca,'ColorOrderIndex',2) + plot(freqs, 1./abs(squeeze(freqresp(W_u*W_acc, freqs, 'Hz'))), '--', 'DisplayName', '$W_{acc}$'); + + set(gca,'ColorOrderIndex',1) + plot(freqs, abs(squeeze(freqresp(H_pos, freqs, 'Hz'))), '-', 'DisplayName', '$H_{pos}$'); + set(gca,'ColorOrderIndex',2) + plot(freqs, abs(squeeze(freqresp(H_acc, freqs, 'Hz'))), '-', 'DisplayName', '$H_{acc}$'); + + 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(H_pos, freqs, 'Hz'))), '-'); + set(gca,'ColorOrderIndex',2) + plot(freqs, 180/pi*phase(squeeze(freqresp(H_acc, freqs, 'Hz'))), '-'); + hold off; + xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); + set(gca, 'XScale', 'log'); + yticks([-360:90:360]); + + linkaxes([ax1,ax2],'x'); + xlim([freqs(1), freqs(end)]); + xticks([0.1, 1, 10, 100, 1000]); +#+end_src + +#+HEADER: :tangle no :exports results :results none :noweb yes +#+begin_src matlab :var filepath="figs/comp_filter_hinf_uncertainty.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") + <> +#+end_src + +#+NAME: fig:comp_filter_hinf_uncertainty +#+CAPTION: Obtained complementary filters ([[./figs/comp_filter_hinf_uncertainty.png][png]], [[./figs/comp_filter_hinf_uncertainty.pdf][pdf]]) +[[file:figs/comp_filter_hinf_uncertainty.png]] + +** TODO 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]]. + +#+begin_src matlab :exports none + Gss = usample(H_pos*inv(G_pos)*G_pos_u + H_acc*inv(G_acc)*G_acc_u, 20); +#+end_src + +#+begin_src matlab :exports none + H2_filters = load('./mat/H2_filters.mat', 'H_pos', 'H_acc'); + Gss_H2 = usample(H2_filters.H_pos*inv(G_pos)*G_pos_u + H2_filters.H_acc*inv(G_acc)*G_acc_u, 20); +#+end_src + +#+begin_src matlab :exports none + figure; + + ax1 = subplot(2,1,1); + hold on; + set(gca,'ColorOrderIndex',1) + plot(freqs, abs(squeeze(freqresp(H_pos, freqs, 'Hz'))), '-', 'DisplayName', '$H_{pos}$'); + set(gca,'ColorOrderIndex',2) + plot(freqs, abs(squeeze(freqresp(H_acc, freqs, 'Hz'))), '-', 'DisplayName', '$H_{acc}$'); + set(gca,'ColorOrderIndex',1) + plot(freqs, abs(squeeze(freqresp(H2_filters.H_pos, freqs, 'Hz'))), '--', 'DisplayName', '$H_{pos,2}$'); + set(gca,'ColorOrderIndex',2) + plot(freqs, abs(squeeze(freqresp(H2_filters.H_acc, freqs, 'Hz'))), '--', 'DisplayName', '$H_{acc,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(H_pos, freqs, 'Hz'))), '-'); + set(gca,'ColorOrderIndex',2) + plot(freqs, 180/pi*phase(squeeze(freqresp(H_acc, freqs, 'Hz'))), '-'); + set(gca,'ColorOrderIndex',1) + plot(freqs, 180/pi*phase(squeeze(freqresp(H2_filters.H_pos, freqs, 'Hz'))), '--'); + set(gca,'ColorOrderIndex',2) + plot(freqs, 180/pi*phase(squeeze(freqresp(H2_filters.H_acc, freqs, 'Hz'))), '--'); + hold off; + xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); + set(gca, 'XScale', 'log'); + yticks([-360:90:360]); + + linkaxes([ax1,ax2],'x'); + xlim([freqs(1), freqs(end)]); + xticks([0.1, 1, 10, 100, 1000]); +#+end_src + +#+begin_src matlab :exports none + % We here compute the maximum and minimum phase of the super sensor + Dphiss = 180/pi*asin(abs(squeeze(freqresp(W_pos*H_pos, freqs, 'Hz')))+abs(squeeze(freqresp(W_acc*H_acc, freqs, 'Hz')))); + Dphiss(abs(squeeze(freqresp(W_pos*H_pos, freqs, 'Hz')))+abs(squeeze(freqresp(W_acc*H_acc, freqs, 'Hz'))) > 1) = 190; +#+end_src + +#+begin_src matlab :exports none + figure; + % Magnitude + ax1 = subplot(2,1,1); + hold on; + plot(freqs, abs(squeeze(freqresp(Gss(1, 1, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'DisplayName', 'SS Dynamics'); + for i = 2:length(Gss) + plot(freqs, abs(squeeze(freqresp(Gss(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'HandleVisibility', 'off'); + end + for i = 2:length(Gss_H2) + plot(freqs, abs(squeeze(freqresp(Gss_H2(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [1 0 0 0.2], 'HandleVisibility', 'off'); + end + % set(gca,'ColorOrderIndex',1); + % plot(freqs, 1 + abs(squeeze(freqresp(W_pos, freqs, 'Hz'))), '--'); + % set(gca,'ColorOrderIndex',1); + % plot(freqs, max(1 - abs(squeeze(freqresp(W_pos, freqs, 'Hz'))), 0), '--'); + % set(gca,'ColorOrderIndex',2); + % plot(freqs, 1 + abs(squeeze(freqresp(W_acc, freqs, 'Hz'))), '--'); + % set(gca,'ColorOrderIndex',2); + % plot(freqs, max(1 - abs(squeeze(freqresp(W_acc, freqs, 'Hz'))), 0), '--'); + plot(freqs, 1 + abs(squeeze(freqresp(inv(W_u), freqs, 'Hz'))), 'k--'); + plot(freqs, max(1 - abs(squeeze(freqresp(inv(W_u), freqs, 'Hz'))), 0), 'k--'); + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + set(gca, 'XTickLabel',[]); + legend('location', 'southwest'); + ylabel('Magnitude'); + ylim([5e-2, 10]); + hold off; + + % Phase + ax2 = subplot(2,1,2); + hold on; + for i = 1:length(Gss) + plot(freqs, 180/pi*angle(squeeze(freqresp(Gss(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2]); + end + for i = 1:length(Gss_H2) + plot(freqs, 180/pi*angle(squeeze(freqresp(Gss_H2(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2]); + end + % set(gca,'ColorOrderIndex',1); + % plot(freqs, Dphi_pos, '--'); + % set(gca,'ColorOrderIndex',1); + % plot(freqs, -Dphi_pos, '--'); + % set(gca,'ColorOrderIndex',2); + % plot(freqs, Dphi_acc, '--'); + % set(gca,'ColorOrderIndex',2); + % plot(freqs, -Dphi_acc, '--'); + % plot(freqs, Dphi_max, 'k--'); + % plot(freqs, -Dphi_max, 'k--'); + set(gca,'xscale','log'); + yticks(-180:90:180); + ylim([-180 180]); + xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); + hold off; + linkaxes([ax1,ax2],'x'); +#+end_src + +#+HEADER: :tangle no :exports results :results none :noweb yes +#+begin_src matlab :var filepath="figs/super_sensor_uncertainty_bode_plot.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") + <> +#+end_src + +#+NAME: fig:super_sensor_uncertainty_bode_plot +#+CAPTION: Uncertainty on the dynamics of the super sensor ([[./figs/super_sensor_uncertainty_bode_plot.png][png]], [[./figs/super_sensor_uncertainty_bode_plot.pdf][pdf]]) +[[file:figs/super_sensor_uncertainty_bode_plot.png]] + +The uncertainty of the super sensor cannot be made smaller than both the individual sensor. Ideally, it would follow the minimum uncertainty of both sensors. + +We here just used very wimple weights. +For instance, we could improve the dynamical uncertainty of the super sensor by making $|W_\phi(j\omega)|$ smaller bellow 2Hz where the dynamical uncertainty of the sensor 1 is small. + +** TODO 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. +#+begin_src matlab + omegac = 1000*2*pi; G0 = 1e-6; Ginf = 1e-3; + N_pos = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4); + + omegac = 0.05*2*pi; G0 = 1e-1; Ginf = 1e-6; + N_acc = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4); +#+end_src + +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]]. + +#+begin_src matlab + PSD_S_pos = abs(squeeze(freqresp(N_pos, freqs, 'Hz'))).^2; + PSD_S_acc = abs(squeeze(freqresp(N_acc, freqs, 'Hz'))).^2; + PSD_Hinf = abs(squeeze(freqresp(N_pos*H_pos, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N_acc*H_acc, freqs, 'Hz'))).^2; +#+end_src + +#+begin_src matlab :exports none + figure; + hold on; + plot(freqs, PSD_S_pos, '-', 'DisplayName', '$\Phi_{\hat{x}_{pos}}$'); + plot(freqs, PSD_S_acc, '-', 'DisplayName', '$\Phi_{\hat{x}_{acc}}$'); + plot(freqs, PSD_Hinf, 'k-', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_\infty}}$'); + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + xlabel('Frequency [Hz]'); ylabel('Power Spectral Density [$(m/s)^2/Hz$]'); + 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/psd_sensors_hinf_synthesis.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") + <> +#+end_src + +#+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]] + +#+begin_src matlab + CPS_S_pos = cumtrapz(freqs, PSD_S_pos); + CPS_S_acc = cumtrapz(freqs, PSD_S_acc); + CPS_Hinf = cumtrapz(freqs, PSD_Hinf); +#+end_src + +#+begin_src matlab :exports none + figure; + hold on; + plot(freqs, CPS_S_pos, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{pos}} = %.1e$ [m/s rms]', sqrt(CPS_S_pos(end)))); + plot(freqs, CPS_S_acc, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{acc}} = %.1e$ [m/s rms]', sqrt(CPS_S_acc(end)))); + plot(freqs, CPS_Hinf, 'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_\\infty}} = %.1e$ [m/s rms]', sqrt(CPS_Hinf(end)))); + set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); + xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum'); + hold off; + xlim([2*freqs(1), freqs(end)]); + % ylim([1e-10 1e-5]); + legend('location', 'southeast'); +#+end_src + +#+HEADER: :tangle no :exports results :results none :noweb yes +#+begin_src matlab :var filepath="figs/cps_sensors_hinf_synthesis.cps" :var figsize="full-tall" :post cps2svg(file=*this*, ext="png") + <> +#+end_src + +#+NAME: fig:cps_sensors_hinf_synthesis +#+CAPTION: Cumulative Power Spectrum of the obtained super sensor using the $\mathcal{H}_\infty$ synthesis ([[./figs/cps_sensors_hinf_synthesis.png][png]], [[./figs/cps_sensors_hinf_synthesis.cps][cps]]) +[[file:figs/cps_sensors_hinf_synthesis.png]] + +** Conclusion +Using the $\mathcal{H}_\infty$ synthesis, the dynamical uncertainty of the super sensor can be bounded to acceptable values. + +However, the RMS of the super sensor noise is not optimized as it was the case with the $\mathcal{H}_2$ synthesis + +* Optimal and Robust Sensor Fusion: Mixed $\mathcal{H}_2/\mathcal{H}_\infty$ Synthesis with Acc and Pos +:PROPERTIES: +:header-args:matlab+: :tangle matlab/mixed_synthesis_sensor_fusion.m +:header-args:matlab+: :comments org :mkdirp yes +:END: +<> +** ZIP file containing the data and matlab files :ignore: +#+begin_note + The Matlab scripts is accessible [[file:matlab/mixed_synthesis_sensor_fusion.m][here]]. +#+end_note + +** Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis - Introduction +The goal is to design complementary filters such that: +- the maximum uncertainty of the super sensor is bounded +- the RMS value of the super sensor noise is minimized + +To do so, we can use the Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis. + +The Matlab function for that is =h2hinfsyn= ([[https://fr.mathworks.com/help/robust/ref/h2hinfsyn.html][doc]]). + ** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) <> @@ -65,11 +1203,608 @@ The complementary filters have to be designed in order to minimize the effect no <> #+end_src +#+begin_src matlab + load('./mat/model.mat', 'freqs', 'G_acc', 'G_pos', 'N_pos', 'N_acc', 'W_pos', 'W_acc'); +#+end_src + +** Noise characteristics and Uncertainty of the individual sensors +Both dynamical uncertainty and noise characteristics of the individual sensors are shown in Fig. [[fig:mixed_synthesis_noise_uncertainty_sensors]]. + +#+begin_src matlab :exports none + figure; + ax1 = subplot(2, 1, 1); + hold on; + plot(freqs, abs(squeeze(freqresp(N_pos, freqs, 'Hz'))), '-', 'DisplayName', '$|N_{pos}(j\omega)|$'); + plot(freqs, abs(squeeze(freqresp(N_acc, freqs, 'Hz'))), '-', 'DisplayName', '$|N_{acc}(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(W_pos, freqs, 'Hz'))), '-', 'DisplayName', '$|W_{pos}(j\omega)|$'); + plot(freqs, abs(squeeze(freqresp(W_acc, freqs, 'Hz'))), '-', 'DisplayName', '$|W_{acc}(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)]); +#+end_src + +#+HEADER: :tangle no :exports results :results none :noweb yes +#+begin_src matlab :var filepath="figs/mixed_synthesis_noise_uncertainty_sensors.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") + <> +#+end_src + +#+NAME: fig:mixed_synthesis_noise_uncertainty_sensors +#+CAPTION: Noise characteristsics and Dynamical uncertainty of the individual sensors ([[./figs/mixed_synthesis_noise_uncertainty_sensors.png][png]], [[./figs/mixed_synthesis_noise_uncertainty_sensors.pdf][pdf]]) +[[file:figs/mixed_synthesis_noise_uncertainty_sensors.png]] + +** 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. + +#+begin_src matlab + Dphi = 5; % [deg] + + n = 4; w0 = 2*pi*1e3; G0 = 1/sin(Dphi*pi/180); Ginf = 1/5; Gc = 1; + W_u = (((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; +#+end_src + +#+begin_src matlab :exports none + figure; + hold on; + plot(freqs, abs(squeeze(freqresp(W_pos, freqs, 'Hz'))), '-', 'DisplayName', '$|W_{pos}(j\omega)|$'); + plot(freqs, abs(squeeze(freqresp(W_acc, freqs, 'Hz'))), '-', 'DisplayName', '$|W_{acc}(j\omega)|$'); + plot(freqs, 1./abs(squeeze(freqresp(W_u, 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)]); +#+end_src + +#+HEADER: :tangle no :exports results :results none :noweb yes +#+begin_src matlab :var filepath="figs/mixed_syn_hinf_weight.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") + <> +#+end_src + +#+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]]. + +#+begin_src matlab :exports none + 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'); +#+end_src + +#+HEADER: :tangle no :exports results :results none :noweb yes +#+begin_src matlab :var filepath="figs/mixed_syn_objective_hinf.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") + <> +#+end_src + +#+NAME: fig:mixed_syn_objective_hinf +#+CAPTION: $\mathcal{H}_\infty$ synthesis objective part of the mixed-synthesis ([[./figs/mixed_syn_objective_hinf.png][png]], [[./figs/mixed_syn_objective_hinf.pdf][pdf]]) +[[file:figs/mixed_syn_objective_hinf.png]] + +** Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis +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. +#+begin_src matlab + W1u = ss(W_pos*W_u); W2u = ss(W_acc*W_u); % Weight on the uncertainty + W1n = ss(N_pos); W2n = ss(N_acc); % Weight on the noise + + P = [W1u -W1u; + 0 W2u; + W1n -W1n; + 0 W2n; + 1 0]; +#+end_src + +The mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis is performed below. +#+begin_src matlab + Nmeas = 1; Ncon = 1; Nz2 = 2; + + [H_acc, ~, normz, ~] = h2hinfsyn(P, Nmeas, Ncon, Nz2, [0, 1], 'HINFMAX', 1, 'H2MAX', Inf, 'DKMAX', 100, 'TOL', 0.01, 'DISPLAY', 'on'); + + H_pos = 1 - H_acc; +#+end_src + +#+begin_src matlab :exports none + save('./mat/H2_Hinf_filters.mat', 'H_pos', 'H_acc'); +#+end_src + +The obtained complementary filters are shown in Fig. [[fig:comp_filters_mixed_synthesis]]. + +#+begin_src matlab :exports none + figure; + + ax1 = subplot(2,1,1); + hold on; + set(gca,'ColorOrderIndex',1) + plot(freqs, 1./abs(squeeze(freqresp(W_pos, freqs, 'Hz'))), '--', 'DisplayName', '$W_1$'); + set(gca,'ColorOrderIndex',2) + plot(freqs, 1./abs(squeeze(freqresp(W_acc, freqs, 'Hz'))), '--', 'DisplayName', '$W_2$'); + + set(gca,'ColorOrderIndex',1) + plot(freqs, abs(squeeze(freqresp(H_pos, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$'); + set(gca,'ColorOrderIndex',2) + plot(freqs, abs(squeeze(freqresp(H_acc, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$'); + + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + ylabel('Magnitude'); + set(gca, 'XTickLabel',[]); + ylim([1e-3, 2]); + legend('location', 'southwest'); + + ax2 = subplot(2,1,2); + hold on; + set(gca,'ColorOrderIndex',1) + plot(freqs, 180/pi*phase(squeeze(freqresp(H_pos, freqs, 'Hz'))), '-'); + set(gca,'ColorOrderIndex',2) + plot(freqs, 180/pi*phase(squeeze(freqresp(H_acc, freqs, 'Hz'))), '-'); + hold off; + xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); + set(gca, 'XScale', 'log'); + yticks([-360:90:360]); + + linkaxes([ax1,ax2],'x'); + xlim([freqs(1), freqs(end)]); + xticks([0.1, 1, 10, 100, 1000]); +#+end_src + +#+HEADER: :tangle no :exports results :results none :noweb yes +#+begin_src matlab :var filepath="figs/comp_filters_mixed_synthesis.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") + <> +#+end_src + +#+NAME: fig:comp_filters_mixed_synthesis +#+CAPTION: Obtained complementary filters after mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis ([[./figs/comp_filters_mixed_synthesis.png][png]], [[./figs/comp_filters_mixed_synthesis.pdf][pdf]]) +[[file:figs/comp_filters_mixed_synthesis.png]] + +** 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. + +#+begin_src matlab + PSD_S_pos = abs(squeeze(freqresp(N_pos, freqs, 'Hz'))).^2; + PSD_S_acc = abs(squeeze(freqresp(N_acc, freqs, 'Hz'))).^2; + PSD_H2Hinf = abs(squeeze(freqresp(N_pos*H_pos, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N_acc*H_acc, freqs, 'Hz'))).^2; +#+end_src + +#+begin_src matlab :exports none + figure; + hold on; + plot(freqs, PSD_S_pos, '-', 'DisplayName', '$\Phi_{\hat{x}_{pos}}$'); + plot(freqs, PSD_S_acc, '-', 'DisplayName', '$\Phi_{\hat{x}_{acc}}$'); + plot(freqs, PSD_H2Hinf, 'k-', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_2/\mathcal{H}_\infty}}$'); + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + xlabel('Frequency [Hz]'); ylabel('Power Spectral Density [$(m/s)^2/Hz$]'); + 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/psd_super_sensor_mixed_syn.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") + <> +#+end_src + +#+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]] + +#+begin_src matlab + CPS_S_pos = cumtrapz(freqs, PSD_S_pos); + CPS_S_acc = cumtrapz(freqs, PSD_S_acc); + CPS_H2Hinf = cumtrapz(freqs, PSD_H2Hinf); +#+end_src + +#+begin_src matlab :exports none + figure; + hold on; + plot(freqs, CPS_S_pos, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{pos}} = %.1e$ [m/s rms]', sqrt(CPS_S_pos(end)))); + plot(freqs, CPS_S_acc, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{acc}} = %.1e$ [m/s rms]', sqrt(CPS_S_acc(end)))); + plot(freqs, CPS_H2Hinf, 'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_\\infty/\\mathcal{H}_\\infty}} = %.1e$ [m/s rms]', sqrt(CPS_H2Hinf(end)))); + set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); + xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum'); + hold off; + xlim([2*freqs(1), freqs(end)]); + % ylim([1e-10 1e-5]); + legend('location', 'southeast'); +#+end_src + +#+HEADER: :tangle no :exports results :results none :noweb yes +#+begin_src matlab :var filepath="figs/cps_super_sensor_mixed_syn.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") + <> +#+end_src + +#+NAME: fig:cps_super_sensor_mixed_syn +#+CAPTION: Cumulative Power Spectrum of the Super Sensor obtained with the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis ([[./figs/cps_super_sensor_mixed_syn.png][png]], [[./figs/cps_super_sensor_mixed_syn.pdf][pdf]]) +[[file:figs/cps_super_sensor_mixed_syn.png]] + +** Obtained Super Sensor's Uncertainty +The uncertainty on the super sensor's dynamics is shown in Fig. [[fig:super_sensor_dyn_uncertainty_mixed_syn]]. + +#+begin_src matlab :exports none + Dphi_pos = 180/pi*asin(abs(squeeze(freqresp(W_pos, freqs, 'Hz')))); + Dphi_pos(abs(squeeze(freqresp(W_pos, freqs, 'Hz'))) > 1) = 360; + + Dphi_acc = 180/pi*asin(abs(squeeze(freqresp(W_acc, freqs, 'Hz')))); + Dphi_acc(abs(squeeze(freqresp(W_acc, freqs, 'Hz'))) > 1) = 360; + + Dphi_ss = 180/pi*asin(abs(squeeze(freqresp(W_pos*H_pos, freqs, 'Hz'))) + abs(squeeze(freqresp(W_acc*H_acc, freqs, 'Hz')))); + Dphi_ss(abs(squeeze(freqresp(W_pos*H_pos, freqs, 'Hz'))) + abs(squeeze(freqresp(W_acc*H_acc, freqs, 'Hz'))) > 1) = 360; + + figure; + % Magnitude + ax1 = subplot(2,1,1); + hold on; + p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W_acc, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W_acc, freqs, 'Hz'))), 1e-6))], 'w'); + p.FaceColor = [0.8500 0.3250 0.0980]; + p.EdgeColor = 'none'; + p.FaceAlpha = 0.3; + p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W_pos, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W_pos, freqs, 'Hz'))), 0.001))], 'w'); + p.FaceColor = [0 0.4470 0.7410]; + p.EdgeColor = 'none'; + p.FaceAlpha = 0.3; + p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W_pos*H_pos, freqs, 'Hz')))+abs(squeeze(freqresp(W_acc*H_acc, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W_pos*H_pos, freqs, 'Hz')))-abs(squeeze(freqresp(W_acc*H_acc, freqs, 'Hz'))), 0.001))], 'w'); + p.EdgeColor = 'black'; + p.FaceAlpha = 0; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + set(gca, 'XTickLabel',[]); + ylabel('Magnitude'); + ylim([1e-2, 1e3]); + hold off; + + % Phase + ax2 = subplot(2,1,2); + hold on; + p = patch([freqs flip(freqs)], [Dphi_acc; flip(-Dphi_acc)], 'w'); + p.FaceColor = [0.8500 0.3250 0.0980]; + p.EdgeColor = 'none'; + p.FaceAlpha = 0.3; + p = patch([freqs flip(freqs)], [Dphi_pos; flip(-Dphi_pos)], 'w'); + p.FaceColor = [0 0.4470 0.7410]; + p.EdgeColor = 'none'; + p.FaceAlpha = 0.3; + p = patch([freqs flip(freqs)], [Dphi_ss; flip(-Dphi_ss)], 'w'); + p.EdgeColor = 'black'; + p.FaceAlpha = 0; + 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)]); +#+end_src + + + + + + + + + + + + + + + + + + + + + + + +#+begin_src matlab :exports none + G1 = 1 + w1*ultidyn('Delta',[1 1]); + G2 = 1 + w2*ultidyn('Delta',[1 1]); + + 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; +#+end_src + +#+begin_src matlab + G_pos_u = G_pos*(1 + W_pos*ultidyn('Delta',[1 1])); + G_acc_u = G_acc*(1 + W_acc*ultidyn('Delta',[1 1])); +#+end_src + +#+begin_src matlab :exports none + Gss = usample(H_pos*inv(G_pos)*G_pos_u + H_acc*inv(G_acc)*G_acc_u, 20); +#+end_src + +#+begin_src matlab :exports none + Dphiss = 180/pi*asin(abs(squeeze(freqresp(W_pos*H_pos, freqs, 'Hz')))+abs(squeeze(freqresp(W_acc*H_acc, freqs, 'Hz')))); + Dphiss(abs(squeeze(freqresp(W_pos*H_pos, freqs, 'Hz')))+abs(squeeze(freqresp(W_acc*H_acc, freqs, 'Hz'))) > 1) = 190; +#+end_src + +#+begin_src matlab :exports none + figure; + % Magnitude + ax1 = subplot(2,1,1); + hold on; + plot(freqs, abs(squeeze(freqresp(Gss(:, :, 1, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'DisplayName', 'SS Dynamics'); + for i = 2:length(Gss) + plot(freqs, abs(squeeze(freqresp(Gss(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2], 'HandleVisibility', 'off'); + end + plot(freqs, 1 + abs(squeeze(freqresp(inv(W_u), freqs, 'Hz'))), 'k--'); + plot(freqs, max(1 - abs(squeeze(freqresp(inv(W_u), freqs, 'Hz'))), 0), 'k--'); + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + set(gca, 'XTickLabel',[]); + legend('location', 'southwest'); + ylabel('Magnitude'); + ylim([5e-2, 10]); + hold off; + + % Phase + ax2 = subplot(2,1,2); + hold on; + for i = 1:length(Gss) + plot(freqs, 180/pi*angle(squeeze(freqresp(Gss(:, :, i, 1), freqs, 'Hz'))), '-', 'color', [0 0 0 0.2]); + end + plot(freqs, Dphiss, 'k--'); + plot(freqs, -Dphiss, 'k--'); + set(gca,'xscale','log'); + yticks(-180:90:180); + ylim([-180 180]); + xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); + hold off; + linkaxes([ax1,ax2],'x'); +#+end_src + + + + + + + + + + + + + + + + +#+begin_src matlab :exports none + 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 + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + set(gca, 'XTickLabel',[]); + legend('location', 'southwest'); + ylabel('Magnitude'); + ylim([5e-2, 10]); + 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 + set(gca,'xscale','log'); + yticks(-180:90:180); + ylim([-180 180]); + xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); + hold off; + linkaxes([ax1,ax2],'x'); +#+end_src + +#+HEADER: :tangle no :exports results :results none :noweb yes +#+begin_src matlab :var filepath="figs/super_sensor_dyn_uncertainty_mixed_syn.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") + <> +#+end_src + +#+NAME: fig:super_sensor_dyn_uncertainty_mixed_syn +#+CAPTION: Super Sensor Dynamical Uncertainty obtained with the mixed synthesis ([[./figs/super_sensor_dyn_uncertainty_mixed_syn.png][png]], [[./figs/super_sensor_dyn_uncertainty_mixed_syn.pdf][pdf]]) +[[file:figs/super_sensor_dyn_uncertainty_mixed_syn.png]] + +** Comparison Hinf H2 H2/Hinf +#+begin_src matlab + H2_filters = load('./mat/H2_filters.mat', 'H_pos', 'H_acc'); + Hinf_filters = load('./mat/Hinf_filters.mat', 'H_pos', 'H_acc'); + H2_Hinf_filters = load('./mat/H2_Hinf_filters.mat', 'H_pos', 'H_acc'); +#+end_src + +#+begin_src matlab + PSD_H2 = abs(squeeze(freqresp(N_pos*H2_filters.H_pos, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N_acc*H2_filters.H_acc, freqs, 'Hz'))).^2; + CPS_H2 = sqrt(cumtrapz(freqs, PSD_H2)); + + PSD_Hinf = abs(squeeze(freqresp(N_pos*Hinf_filters.H_pos, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N_acc*Hinf_filters.H_acc, freqs, 'Hz'))).^2; + CPS_Hinf = sqrt(cumtrapz(freqs, PSD_Hinf)); + + PSD_H2Hinf = abs(squeeze(freqresp(N_pos*H2_Hinf_filters.H_pos, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N_acc*H2_Hinf_filters.H_acc, freqs, 'Hz'))).^2; + CPS_H2Hinf = sqrt(cumtrapz(freqs, PSD_H2Hinf)); +#+end_src + +#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) + data2orgtable([CPS_H2(end), CPS_Hinf(end), CPS_H2Hinf(end)]', {'Optimal: $\mathcal{H}_2$', 'Robust: $\mathcal{H}_\infty$', 'Mixed: $\mathcal{H}_2/\mathcal{H}_\infty$'}, {'RMS [m/s]'}, ' %.1e '); +#+end_src + +#+RESULTS: +| | RMS [m/s] | +|-------------------------------------------+-----------| +| Optimal: $\mathcal{H}_2$ | 0.0012 | +| Robust: $\mathcal{H}_\infty$ | 0.07 | +| Mixed: $\mathcal{H}_2/\mathcal{H}_\infty$ | 0.024 | + +** Conclusion +This synthesis methods allows both to: +- limit the dynamical uncertainty of the super sensor +- minimize the RMS value of the estimation + +* Old :noexport: +** Optimal Super Sensor Noise: $\mathcal{H}_2$ Synthesis +:PROPERTIES: +:header-args:matlab+: :tangle matlab/optimal_comp_filters.m +:header-args:matlab+: :comments org :mkdirp yes +:END: +<> + +*** Introduction :ignore: +The idea is to combine sensors that works in different frequency range using complementary filters. + +Doing so, one "super sensor" is obtained that can have better noise characteristics than the individual sensors over a large frequency range. + +the complementary filters have to be designed in order to minimize the effect noise of each sensor on the super sensor noise. + +*** zip file containing the data and matlab files :ignore: +#+begin_note + the matlab scripts is accessible [[file:matlab/optimal_comp_filters.m][here]]. +#+end_note + +*** matlab init :noexport:ignore: +#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) + <> +#+end_src + +#+begin_src matlab :exports none :results silent :noweb yes + <> +#+end_src + #+begin_src matlab freqs = logspace(-1, 3, 1000); #+end_src -** Architecture +*** Architecture Let's consider the sensor fusion architecture shown on figure [[fig:fusion_two_noisy_sensors_weights]] 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: @@ -111,7 +1846,7 @@ And the goal is the minimize the Root Mean Square (RMS) value of $\hat{x}$: \sigma_{\hat{x}} = \sqrt{\int_0^\infty \Phi_{\hat{x}}(\omega) d\omega} \end{equation} -** Noise of the sensors +*** 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) @@ -136,16 +1871,16 @@ Let's define the noise characteristics of the two sensors by choosing $N_1$ and legend('location', 'northeast'); #+end_src -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/noise_characteristics_sensors.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> +#+begin_src matlab :tangle no :exports results :results file replace + exportFig('figs/noise_characteristics_sensors.pdf', 'width', 'full', 'height', 'tall'); #+end_src -#+NAME: fig:noise_characteristics_sensors -#+CAPTION: Noise Characteristics of the two sensors ([[./figs/noise_characteristics_sensors.png][png]], [[./figs/noise_characteristics_sensors.pdf][pdf]]) +#+name: fig:noise_characteristics_sensors +#+caption: Noise Characteristics of the two sensors ([[./figs/noise_characteristics_sensors.png][png]], [[./figs/noise_characteristics_sensors.pdf][pdf]]) +#+RESULTS: [[file:figs/noise_characteristics_sensors.png]] -** H-Two Synthesis +*** 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. @@ -165,7 +1900,7 @@ We use the generalized plant architecture shown on figure [[fig:h_infinity_optim 0 & N_2 & 1 \\ N_1 & -N_2 & 0 \end{pmatrix} \begin{pmatrix} - w_1 \\ w_2 \\ u + W_1 \\ W_2 \\ u \end{pmatrix} \end{equation*} @@ -212,13 +1947,13 @@ The obtained RMS value of the super sensor is lower than the RMS value of the in legend('location', 'northeast'); #+end_src -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/htwo_comp_filters.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> +#+begin_src matlab :tangle no :exports results :results file replace + exportFig('figs/htwo_comp_filters.pdf', 'width', 'full', 'height', 'tall'); #+end_src -#+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]]) +#+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]]) +#+RESULTS: [[file:figs/htwo_comp_filters.png]] #+begin_src matlab @@ -240,19 +1975,19 @@ The obtained RMS value of the super sensor is lower than the RMS value of the in legend('location', 'northeast'); #+end_src -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/psd_sensors_htwo_synthesis.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> +#+begin_src matlab :tangle no :exports results :results file replace + exportFig('figs/psd_sensors_htwo_synthesis.pdf', 'width', 'full', 'height', 'tall'); #+end_src -#+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]]) +#+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]]) +#+RESULTS: [[file:figs/psd_sensors_htwo_synthesis.png]] #+begin_src matlab - CPS_S1 = 1/pi*cumtrapz(2*pi*freqs, PSD_S1); - CPS_S2 = 1/pi*cumtrapz(2*pi*freqs, PSD_S2); - CPS_H2 = 1/pi*cumtrapz(2*pi*freqs, PSD_H2); + CPS_S1 = cumtrapz(freqs, PSD_S1); + CPS_S2 = cumtrapz(freqs, PSD_S2); + CPS_H2 = cumtrapz(freqs, PSD_H2); #+end_src #+begin_src matlab :exports none @@ -269,484 +2004,93 @@ The obtained RMS value of the super sensor is lower than the RMS value of the in legend('location', 'southeast'); #+end_src -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/cps_h2_synthesis.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> +#+begin_src matlab :tangle no :exports results :results file replace + exportFig('figs/cps_h2_synthesis.pdf', 'width', 'full', 'height', 'tall'); #+end_src -#+NAME: fig:cps_h2_synthesis -#+CAPTION: Cumulative Power Spectrum of individual sensors and super sensor using the $\mathcal{H}_2$ synthesis ([[./figs/cps_h2_synthesis.png][png]], [[./figs/cps_h2_synthesis.pdf][pdf]]) +#+name: fig:cps_h2_synthesis +#+caption: Cumulative Power Spectrum of individual sensors and super sensor using the $\mathcal{H}_2$ synthesis ([[./figs/cps_h2_synthesis.png][png]], [[./figs/cps_h2_synthesis.pdf][pdf]]) +#+RESULTS: [[file:figs/cps_h2_synthesis.png]] -** Alternative H-Two Synthesis -An alternative Alternative formulation of the $\mathcal{H}_2$ synthesis is shown in Fig. [[fig:h_infinity_optimal_comp_filters_bis]]. - -#+name: fig:h_infinity_optimal_comp_filters_bis -#+caption: Alternative formulation of the $\mathcal{H}_2$ synthesis -[[file:figs-tikz/h_infinity_optimal_comp_filters_bis.png]] - -\begin{equation*} -\begin{pmatrix} - z_1 \\ z_2 \\ v -\end{pmatrix} = \begin{pmatrix} - N_1 & -N_1 \\ - 0 & N_2 \\ - 1 & 0 -\end{pmatrix} \begin{pmatrix} - w \\ u -\end{pmatrix} -\end{equation*} - - -** 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]]. - +*** Time Domain Simulation +Parameters of the time domain simulation. #+begin_src matlab - 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; + Fs = 1e3; % Sampling Frequency [Hz] + Ts = 1/Fs; % Sampling Time [s] - 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; + t = 0:Ts:5; % Time Vector [s] #+end_src +Generate noises in velocity corresponding to sensor 1 and 2: #+begin_src matlab - P = [W1a -W1a; - 0 W2a; - 1 0]; -#+end_src - -And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command. -#+begin_src matlab :results output replace :exports both - [H2a, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on'); -#+end_src - -#+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 - -#+begin_src matlab - H1a = 1 - H2a; + n1 = lsim(N1, sqrt(Fs/2)*randn(length(t), 1), t); + n2 = lsim(N2, sqrt(Fs/2)*randn(length(t), 1), t); #+end_src #+begin_src matlab :exports none figure; + hold on; + plot(t, n1, 'DisplayName', 'Differentiated Position'); + plot(t, n2, 'DisplayName', 'Integrated Acceleration'); + plot(t, lsim(H1, n1, t)+lsim(H2, n2, t), 'DisplayName', 'Super Sensor'); + hold off; + xlabel('Time [s]'); ylabel('Velocity [m/s]'); + legend(); +#+end_src +#+begin_src matlab :tangle no :exports results :results file replace + exportFig('figs/super_sensor_time_domain_h2.pdf', 'width', 'full', 'height', 'tall'); +#+end_src + +#+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]] + +*** Sensor Spurious Dynamics +#+begin_src matlab + G2 = tf(1); + + w1 = 2*pi*10; xi1 = 0.2; z1 = 2*pi*20; + G1 = (1 + 2*xi1*s/z1 + s^2/z1^2)/(1 + 2*xi1*s/w1 + s^2/w1^2); + + Gss = G1*H1 + G2*H2; +#+end_src + +#+begin_src matlab :exports none + figure; + % Magnitude 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; + plot(freqs, abs(squeeze(freqresp(G1*H1, freqs, 'Hz'))), '-', 'DisplayName', '$G_1$'); + plot(freqs, abs(squeeze(freqresp(G2*H2, freqs, 'Hz'))), '-', 'DisplayName', '$G_2$'); + plot(freqs, abs(squeeze(freqresp(Gss, freqs, 'Hz'))), 'k-', 'DisplayName', 'SS Dynamics'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - ylabel('Magnitude'); set(gca, 'XTickLabel',[]); - ylim([5e-4, 20]); - legend('location', 'northeast'); + legend('location', 'southwest'); + ylabel('Magnitude'); + hold off; + % Phase 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; + plot(freqs, 180/pi*angle(squeeze(freqresp(G1, freqs, 'Hz'))), '-'); + plot(freqs, 180/pi*angle(squeeze(freqresp(G2, freqs, 'Hz'))), '-'); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gss, freqs, 'Hz'))), 'k-'); + set(gca,'xscale','log'); + yticks(-180:90:180); + ylim([-180 180]); xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); - set(gca, 'XScale', 'log'); - yticks([-360:90:360]); - + hold off; linkaxes([ax1,ax2],'x'); - xlim([freqs(1), freqs(end)]); - xticks([0.1, 1, 10, 100, 1000]); #+end_src -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/weights_comp_filters_Hinfa.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> -#+end_src - -#+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. - -#+begin_src matlab - 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); -#+end_src - -** 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]]. - -#+begin_src matlab - 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 -#+end_src - -#+begin_src matlab - P = [W1b -W1b; - 0 W2b; - 1 0]; -#+end_src - -And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command. -#+begin_src matlab :results output replace :exports both - [H2b, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on'); -#+end_src - -#+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 - -#+begin_src matlab - H1b = 1 - H2b; -#+end_src - -#+begin_src matlab :exports none - figure; - - ax1 = subplot(2,1,1); - hold on; - set(gca,'ColorOrderIndex',1) - plot(freqs, 1./abs(squeeze(freqresp(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]); -#+end_src - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/weights_comp_filters_Hinfb.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> -#+end_src - -#+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]] - -#+begin_src matlab - 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); -#+end_src - -** H-Infinity Synthesis - method C -#+begin_src matlab - Wp = 0.56*(inv(N1)+inv(N2))/(1 + s/2/pi/1000); - - W1c = N1*Wp; - W2c = N2*Wp; -#+end_src - -#+begin_src matlab - P = [W1c -W1c; - 0 W2c; - 1 0]; -#+end_src - -And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command. -#+begin_src matlab :results output replace :exports both - [H2c, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on'); -#+end_src - -#+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 - -#+begin_src matlab - H1c = 1 - H2c; -#+end_src - -#+begin_src matlab :exports none - figure; - - ax1 = subplot(2,1,1); - hold on; - set(gca,'ColorOrderIndex',1) - plot(freqs, 1./abs(squeeze(freqresp(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]); -#+end_src - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/weights_comp_filters_Hinfc.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> -#+end_src - -#+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]] - -#+begin_src matlab - 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); -#+end_src - -** 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. [[fig:comparison_psd_noise]]. - -The Cumulative Power Spectrum for the same sensors are shown on Fig. [[fig:comparison_cps_noise]]. - -The RMS value of the obtained super sensors are shown on table [[tab:rms_results]]. - -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) - data2orgtable([norm([N1], 2) ; norm([N2], 2) ; norm([N1*H1, N2*H2], 2) ; norm([N1*H1a, N2*H2a], 2) ; norm([N1*H1b, N2*H2b], 2) ; norm([N1*H1c, N2*H2c], 2)], {'Sensor 1', 'Sensor 2', 'H2 Fusion', 'H-Infinity a', 'H-Infinity b', 'H-Infinity c'}, {'rms value'}, ' %.1e'); -#+end_src - -#+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 | - - -#+begin_src matlab :exports none - figure; - hold on; - plot(freqs, PSD_S1, '-', 'DisplayName', '$\Phi_{\hat{x}_1}$'); - plot(freqs, PSD_S2, '-', 'DisplayName', '$\Phi_{\hat{x}_2}$'); - plot(freqs, PSD_H2, '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'); -#+end_src - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/comparison_psd_noise.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> -#+end_src - -#+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]] - -#+begin_src matlab :exports none - figure; - hold on; - plot(freqs, CPS_S1, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_1} = %.1e$', sqrt(CPS_S1(end)))); - plot(freqs, CPS_S2, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_2} = %.1e$', sqrt(CPS_S2(end)))); - plot(freqs, CPS_H2, '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'); -#+end_src - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/comparison_cps_noise.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> -#+end_src - -#+NAME: fig:comparison_cps_noise -#+CAPTION: Comparison of the obtained Cumulative Power Spectrum using the three methods ([[./figs/comparison_cps_noise.png][png]], [[./figs/comparison_cps_noise.pdf][pdf]]) -[[file:figs/comparison_cps_noise.png]] - -** Obtained Super Sensor's noise uncertainty +*** 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. +Two weights $W_1(s)$ and $W_2(s)$ are used to described the amplitude of the dynamical uncertainty. #+begin_src matlab omegac = 100*2*pi; G0 = 0.1; Ginf = 10; @@ -849,19 +2193,19 @@ Right Half Plane zero might be introduced in the super sensor dynamics which wil #+CAPTION: Uncertianty regions of both individual sensors and of the super sensor when using the $\mathcal{H}_2$ synthesis ([[./figs/uncertainty_super_sensor_H2_syn.png][png]], [[./figs/uncertainty_super_sensor_H2_syn.pdf][pdf]]) [[file:figs/uncertainty_super_sensor_H2_syn.png]] -** Conclusion +*** 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). However, the synthesis does not take into account the robustness of the sensor fusion. -* Optimal Sensor Fusion - Minimize the Super Sensor Dynamical Uncertainty +** Robust Sensor Fusion: $\mathcal{H}_\infty$ Synthesis :PROPERTIES: :header-args:matlab+: :tangle matlab/comp_filter_robustness.m :header-args:matlab+: :comments org :mkdirp yes :END: <> -** Introduction :ignore: +*** Introduction :ignore: We initially considered perfectly known sensor dynamics so that it can be perfectly inverted. We now take into account the fact that the sensor dynamics is only partially known. @@ -873,12 +2217,12 @@ 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: +*** ZIP file containing the data and matlab files :ignore: #+begin_note The Matlab scripts is accessible [[file:matlab/comp_filter_robustness.m][here]]. #+end_note -** Matlab Init :noexport:ignore: +*** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) <> #+end_src @@ -887,7 +2231,7 @@ The objective here is to design complementary filters $H_1(s)$ and $H_2(s)$ in o <> #+end_src -** Super Sensor Dynamical Uncertainty +*** Super Sensor Dynamical Uncertainty 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]]. @@ -897,26 +2241,26 @@ Let's represent that with multiplicative input uncertainty as shown on figure [[ The dynamics of the super sensor is represented by \begin{align*} - \frac{\hat{x}}{x} &= (1 + w_1 \Delta_1) H_1 + (1 + w_2 \Delta_2) H_2 \\ - &= 1 + w_1 H_1 \Delta_1 + w_2 H_2 \Delta_2 + \frac{\hat{x}}{x} &= (1 + W_1 \Delta_1) H_1 + (1 + W_2 \Delta_2) H_2 \\ + &= 1 + W_1 H_1 \Delta_1 + W_2 H_2 \Delta_2 \end{align*} with $\Delta_i$ is any transfer function satisfying $\| \Delta_i \|_\infty < 1$. We see that as soon as we have some uncertainty in the sensor dynamics, we have that the complementary filters have some effect on the transfer function from $x$ to $\hat{x}$. -The uncertainty set of the transfer function from $\hat{x}$ to $x$ at frequency $\omega$ is bounded in the complex plane by a circle centered on 1 and with a radius equal to $|w_1(j\omega) H_1(j\omega)| + |w_2(j\omega) H_2(j\omega)|$ (figure [[fig:uncertainty_gain_phase_variation]]). +The uncertainty set of the transfer function from $\hat{x}$ to $x$ at frequency $\omega$ is bounded in the complex plane by a circle centered on 1 and with a radius equal to $|W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)|$ (figure [[fig:uncertainty_gain_phase_variation]]). We then have that the angle introduced by the super sensor is bounded by $\arcsin(\epsilon)$: -\[ \angle \frac{\hat{x}}{x}(j\omega) \le \arcsin \Big(|w_1(j\omega) H_1(j\omega)| + |w_2(j\omega) H_2(j\omega)|\Big) \] +\[ \angle \frac{\hat{x}}{x}(j\omega) \le \arcsin \Big(|W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)|\Big) \] #+name: fig:uncertainty_gain_phase_variation #+caption: Maximum phase variation [[file:figs-tikz/uncertainty_gain_phase_variation.png]] -** Dynamical uncertainty of the individual sensors +*** 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}$ +- 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}$ We define the weights that are used to characterize the dynamic uncertainty of the sensors. @@ -1009,48 +2353,48 @@ From the weights, we define the uncertain transfer functions of the sensors. Som #+CAPTION: Dynamic uncertainty of the two sensors ([[./figs/uncertainty_dynamics_sensors.png][png]], [[./figs/uncertainty_dynamics_sensors.pdf][pdf]]) [[file:figs/uncertainty_dynamics_sensors.png]] -** Synthesis objective +*** Synthesis objective The uncertainty region of the super sensor dynamics is represented by a circle in the complex plane as shown in Fig. [[fig:uncertainty_gain_phase_variation]]. -At each frequency $\omega$, the radius of the circle is $|w_1(j\omega) H_1(j\omega)| + |w_2(j\omega) H_2(j\omega)|$. +At each frequency $\omega$, the radius of the circle is $|W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)|$. Thus, the phase shift $\Delta\phi(\omega)$ due to the super sensor uncertainty is bounded by: -\[ |\Delta\phi(\omega)| \leq \arcsin\big( |w_1(j\omega) H_1(j\omega)| + |w_2(j\omega) H_2(j\omega)| \big) \] +\[ |\Delta\phi(\omega)| \leq \arcsin\big( |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| \big) \] Let's define some allowed frequency depend phase shift $\Delta\phi_\text{max}(\omega) > 0$ such that: \[ |\Delta\phi(\omega)| < \Delta\phi_\text{max}(\omega), \quad \forall\omega \] If $H_1(s)$ and $H_2(s)$ are designed such that -\[ |w_1(j\omega) H_1(j\omega)| + |w_2(j\omega) H_2(j\omega)| < \sin\big( \Delta\phi_\text{max}(\omega) \big) \] +\[ |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| < \sin\big( \Delta\phi_\text{max}(\omega) \big) \] The maximum phase shift due to dynamic uncertainty at frequency $\omega$ will be $\Delta\phi_\text{max}(\omega)$. -** Requirements as an $\mathcal{H}_\infty$ norm +*** Requirements as an $\mathcal{H}_\infty$ norm We now try to express this requirement in terms of an $\mathcal{H}_\infty$ norm. -Let's define one weight $w_\phi(s)$ that represents the maximum wanted phase uncertainty: -\[ |w_{\phi}(j\omega)|^{-1} \approx \sin(\Delta\phi_{\text{max}}(\omega)), \quad \forall\omega \] +Let's define one weight $W_\phi(s)$ that represents the maximum wanted phase uncertainty: +\[ |W_{\phi}(j\omega)|^{-1} \approx \sin(\Delta\phi_{\text{max}}(\omega)), \quad \forall\omega \] Then: \begin{align*} - & |w_1(j\omega) H_1(j\omega)| + |w_2(j\omega) H_2(j\omega)| < \sin\big( \Delta\phi_\text{max}(\omega) \big), \quad \forall\omega \\ - \Longleftrightarrow & |w_1(j\omega) H_1(j\omega)| + |w_2(j\omega) H_2(j\omega)| < |w_\phi(j\omega)|^{-1}, \quad \forall\omega \\ - \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 + & |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| < \sin\big( \Delta\phi_\text{max}(\omega) \big), \quad \forall\omega \\ + \Longleftrightarrow & |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| < |W_\phi(j\omega)|^{-1}, \quad \forall\omega \\ + \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 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 + \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} -One should not forget that at frequency where both sensors has unknown dynamics ($|w_1(j\omega)| > 1$ and $|w_2(j\omega)| > 1$), the super sensor dynamics will also be unknown and the phase uncertainty cannot be bounded. -Thus, at these frequencies, $|w_\phi|$ should be smaller than $1$. +One should not forget that at frequency where both sensors has unknown dynamics ($|W_1(j\omega)| > 1$ and $|W_2(j\omega)| > 1$), the super sensor dynamics will also be unknown and the phase uncertainty cannot be bounded. +Thus, at these frequencies, $|W_\phi|$ should be smaller than $1$. -** 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]]. +*** 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]]. #+begin_src matlab Dphi = 20; % [deg] @@ -1065,7 +2409,7 @@ The magnitude $|w_\phi(j\omega)|$ is shown in Fig. [[fig:magnitude_wphi]] and th #+begin_src matlab :exports none figure; hold on; - plot(freqs, abs(squeeze(freqresp(wphi, freqs, 'Hz'))), '-', 'DisplayName', '$w_\phi(s)$'); + 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; @@ -1079,7 +2423,7 @@ The magnitude $|w_\phi(j\omega)|$ is shown in Fig. [[fig:magnitude_wphi]] and th #+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]]) +#+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 @@ -1115,8 +2459,8 @@ The obtained upper bounds on the complementary filters in order to limit the pha #+begin_src matlab :exports none 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|$'); + 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; @@ -1133,7 +2477,7 @@ The obtained upper bounds on the complementary filters in order to limit the pha #+CAPTION: Upper bounds on the complementary filters set in order to limit the maximum phase uncertainty of the super sensor to 30 degrees until 500Hz ([[./figs/upper_bounds_comp_filter_max_phase_uncertainty.png][png]], [[./figs/upper_bounds_comp_filter_max_phase_uncertainty.pdf][pdf]]) [[file:figs/upper_bounds_comp_filter_max_phase_uncertainty.png]] -** $\mathcal{H}_\infty$ Synthesis +*** $\mathcal{H}_\infty$ Synthesis The $\mathcal{H}_\infty$ synthesis architecture used for the complementary filters is shown in Fig. [[fig:h_infinity_robust_fusion]]. #+name: fig:h_infinity_robust_fusion @@ -1228,7 +2572,7 @@ The obtained complementary filters are shown in Fig. [[fig:comp_filter_hinf_unce #+CAPTION: Obtained complementary filters ([[./figs/comp_filter_hinf_uncertainty.png][png]], [[./figs/comp_filter_hinf_uncertainty.pdf][pdf]]) [[file:figs/comp_filter_hinf_uncertainty.png]] -** Super sensor uncertainty +*** 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]]. #+begin_src matlab @@ -1307,9 +2651,9 @@ We can now compute the uncertainty of the super sensor. The result is shown in F The uncertainty of the super sensor cannot be made smaller than both the individual sensor. Ideally, it would follow the minimum uncertainty of both sensors. We here just used very wimple weights. -For instance, we could improve the dynamical uncertainty of the super sensor by making $|w_\phi(j\omega)|$ smaller bellow 2Hz where the dynamical uncertainty of the sensor 1 is small. +For instance, we could improve the dynamical uncertainty of the super sensor by making $|W_\phi(j\omega)|$ smaller bellow 2Hz where the dynamical uncertainty of the sensor 1 is small. -** Super sensor noise +*** 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. @@ -1381,12 +2725,12 @@ The CPS of both sensor and of the super sensor is shown in Fig. [[fig:cps_sensor #+CAPTION: Cumulative Power Spectrum of the obtained super sensor using the $\mathcal{H}_\infty$ synthesis ([[./figs/cps_sensors_hinf_synthesis.png][png]], [[./figs/cps_sensors_hinf_synthesis.cps][cps]]) [[file:figs/cps_sensors_hinf_synthesis.png]] -** Conclusion +*** Conclusion Using the $\mathcal{H}_\infty$ synthesis, the dynamical uncertainty of the super sensor can be bounded to acceptable values. However, the RMS of the super sensor noise is not optimized as it was the case with the $\mathcal{H}_2$ synthesis -** First Basic Example with gain mismatch :noexport: +*** 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 \\ @@ -1496,18 +2840,18 @@ We see that the blue complementary filters with a lower maximum norm permits to #+CAPTION: Comparison of the obtained super sensor transfer functions ([[./figs/tf_super_sensor_comp.png][png]], [[./figs/tf_super_sensor_comp.pdf][pdf]]) [[file:figs/tf_super_sensor_comp.png]] -* Optimal Sensor Fusion - Mixed Synthesis +** Optimal and Robust Sensor Fusion: Mixed $\mathcal{H}_2/\mathcal{H}_\infty$ Synthesis :PROPERTIES: :header-args:matlab+: :tangle matlab/mixed_synthesis_sensor_fusion.m :header-args:matlab+: :comments org :mkdirp yes :END: <> -** ZIP file containing the data and matlab files :ignore: +*** ZIP file containing the data and matlab files :ignore: #+begin_note The Matlab scripts is accessible [[file:matlab/mixed_synthesis_sensor_fusion.m][here]]. #+end_note -** Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis - Introduction +*** Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis - Introduction The goal is to design complementary filters such that: - the maximum uncertainty of the super sensor is bounded - the RMS value of the super sensor noise is minimized @@ -1516,7 +2860,7 @@ To do so, we can use the Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis. The Matlab function for that is =h2hinfsyn= ([[https://fr.mathworks.com/help/robust/ref/h2hinfsyn.html][doc]]). -** Matlab Init :noexport:ignore: +*** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) <> #+end_src @@ -1529,7 +2873,7 @@ The Matlab function for that is =h2hinfsyn= ([[https://fr.mathworks.com/help/rob freqs = logspace(-1, 3, 1000); #+end_src -** Noise characteristics and Uncertainty of the individual sensors +*** 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. #+begin_src matlab omegac = 100*2*pi; G0 = 0.1; Ginf = 10; @@ -1565,8 +2909,8 @@ Both dynamical uncertainty and noise characteristics of the individual sensors a 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)|$'); + 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; @@ -1585,7 +2929,7 @@ Both dynamical uncertainty and noise characteristics of the individual sensors a #+CAPTION: Noise characteristsics and Dynamical uncertainty of the individual sensors ([[./figs/mixed_synthesis_noise_uncertainty_sensors.png][png]], [[./figs/mixed_synthesis_noise_uncertainty_sensors.pdf][pdf]]) [[file:figs/mixed_synthesis_noise_uncertainty_sensors.png]] -** Weighting Functions on the uncertainty of the super sensor +*** 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. @@ -1598,9 +2942,9 @@ The maximum wanted multiplicative uncertainty is shown in Fig. [[fig:mixed_syn_h #+begin_src matlab :exports none 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}$'); + 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; @@ -1696,7 +3040,7 @@ The equivalent Magnitude and Phase uncertainties are shown in Fig. [[fig:mixed_s #+CAPTION: $\mathcal{H}_\infty$ synthesis objective part of the mixed-synthesis ([[./figs/mixed_syn_objective_hinf.png][png]], [[./figs/mixed_syn_objective_hinf.pdf][pdf]]) [[file:figs/mixed_syn_objective_hinf.png]] -** Mixed Synthesis Architecture +*** 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: @@ -1737,7 +3081,7 @@ We define the generalized plant that will be used for the mixed synthesis. 1 0]; #+end_src -** Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis +*** Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis The mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis is performed below. #+begin_src matlab Nmeas = 1; Ncon = 1; Nz2 = 2; @@ -1796,7 +3140,7 @@ The obtained complementary filters are shown in Fig. [[fig:comp_filters_mixed_sy #+CAPTION: Obtained complementary filters after mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis ([[./figs/comp_filters_mixed_synthesis.png][png]], [[./figs/comp_filters_mixed_synthesis.pdf][pdf]]) [[file:figs/comp_filters_mixed_synthesis.png]] -** Obtained Super Sensor's noise +*** 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. #+begin_src matlab :exports none @@ -1857,7 +3201,7 @@ The PSD and CPS of the super sensor's noise are shown in Fig. [[fig:psd_super_se #+CAPTION: Cumulative Power Spectrum of the Super Sensor obtained with the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis ([[./figs/cps_super_sensor_mixed_syn.png][png]], [[./figs/cps_super_sensor_mixed_syn.pdf][pdf]]) [[file:figs/cps_super_sensor_mixed_syn.png]] -** Obtained Super Sensor's Uncertainty +*** Obtained Super Sensor's Uncertainty The uncertainty on the super sensor's dynamics is shown in Fig. [[fig:super_sensor_dyn_uncertainty_mixed_syn]]. #+begin_src matlab :exports none @@ -1937,12 +3281,12 @@ The uncertainty on the super sensor's dynamics is shown in Fig. [[fig:super_sens #+CAPTION: Super Sensor Dynamical Uncertainty obtained with the mixed synthesis ([[./figs/super_sensor_dyn_uncertainty_mixed_syn.png][png]], [[./figs/super_sensor_dyn_uncertainty_mixed_syn.pdf][pdf]]) [[file:figs/super_sensor_dyn_uncertainty_mixed_syn.png]] -** Conclusion +*** Conclusion This synthesis methods allows both to: - limit the dynamical uncertainty of the super sensor - minimize the RMS value of the estimation -* Mixed Synthesis - LMI Optimization +* Mixed Synthesis - LMI Optimization :noexport: ** Introduction The following matlab scripts was written by Mohit. @@ -2397,1083 +3741,7 @@ The uncertainty on the super sensor's dynamics is shown in Fig. [[]]. #+CAPTION: Super Sensor Dynamical Uncertainty obtained with the mixed synthesis ([[./figs/super_sensor_uncertainty_compare_cvx_h2i.png][png]], [[./figs/super_sensor_uncertainty_compare_cvx_h2i.pdf][pdf]]) [[file:figs/super_sensor_uncertainty_compare_cvx_h2i.png]] -* H-Infinity synthesis to ensure both performance and robustness -:PROPERTIES: -:header-args:matlab+: :tangle matlab/hinf_syn_perf_robust.m -:header-args:matlab+: :comments org :mkdirp yes -:END: -<> - -** ZIP file containing the data and matlab files :ignore: -#+begin_note - The Matlab scripts is accessible [[file:matlab/hinf_syn_perf_robust.m][here]]. -#+end_note - -** Introduction -The idea is to use only the $\mathcal{H}_\infty$ norm to express both the maximum wanted super sensor uncertainty and the fact that we want to minimize the super sensor's noise. - -For *performance*, we may want to obtain a super sensor's noise that is close to the minimum of the individual sensor noises. - -The noise of the super sensor is: -\[ |N_{ss}(j\omega)|^2 = | H_1(j\omega) N_1(j\omega) |^2 + | H_2(j\omega) N_2(j\omega) |^2 \quad \forall\omega \] - -The minimum noise that we can obtain follows the minimum noise of the individual sensor: -\begin{align*} - & |N_{ss}(j\omega)| \approx |N_1(j\omega)| \quad \text{when} \quad |N_1(j\omega)| < |N_2(j\omega)| \\ - & |N_{ss}(j\omega)| \approx |N_2(j\omega)| \quad \text{when} \quad |N_2(j\omega)| < |N_1(j\omega)| -\end{align*} - -To do so, we want to design the complementary filters such that: -\begin{align*} - & |H_2(j\omega)| \ll 1 \quad \text{when} \quad |N_1(j\omega)| < |N_2(j\omega)| \\ - & |H_1(j\omega)| \ll 1 \quad \text{when} \quad |N_2(j\omega)| < |N_1(j\omega)| -\end{align*} - - - - -For the *uncertainty* of the super sensor. -The equivalent super sensor uncertainty is: -\[ |w_{ss}(j\omega)| = |H_1(j\omega) w_1(j\omega)| + |H_2(j\omega) w_2(j\omega)|, \quad \forall\omega \] - -The minimum uncertainty that we can obtain follows the minimum uncertainty of the individual sensor: -\begin{align*} - & |w_{ss}(j\omega)| \approx |w_1(j\omega)| \quad \text{when} \quad |w_1(j\omega)| < |w_2(j\omega)| \\ - & |w_{ss}(j\omega)| \approx |w_2(j\omega)| \quad \text{when} \quad |w_2(j\omega)| < |w_1(j\omega)| -\end{align*} - -To do so, we want to design the complementary filters such that: -\begin{align*} - & |H_2(j\omega)| \ll 1 \quad \text{when} \quad |w_1(j\omega)| < |w_2(j\omega)| \\ - & |H_1(j\omega)| \ll 1 \quad \text{when} \quad |w_2(j\omega)| < |w_1(j\omega)| -\end{align*} - - -Of course, the conditions for performance and uncertainty may not be compatible. - -We may not want to follow the minimum uncertainty. - -** Matlab Init :noexport:ignore: -#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) - <> -#+end_src - -#+begin_src matlab :exports none :results silent :noweb yes - <> -#+end_src - -#+begin_src matlab - freqs = logspace(-1, 3, 1000); -#+end_src - -** Dynamical uncertainty and Noise level of the individual sensors -Uncertainty on the individual sensors: -#+begin_src matlab - omegac = 100*2*pi; G0 = 0.1; Ginf = 10; - w1 = (Ginf*s/omegac + G0)/(s/omegac + 1); - - omegac = 0.2*2*pi; G0 = 5; Ginf = 0.1; - w2 = (Ginf*s/omegac + G0)/(s/omegac + 1); - omegac = 5000*2*pi; G0 = 1; Ginf = 50; - w2 = w2*(Ginf*s/omegac + G0)/(s/omegac + 1); -#+end_src - -Noise level of the individual sensors: -#+begin_src matlab - omegac = 100*2*pi; G0 = 1e-5; Ginf = 1e-4; - N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/100); - - omegac = 1*2*pi; G0 = 1e-3; Ginf = 1e-8; - N2 = ((sqrt(Ginf)*s/omegac + sqrt(G0))/(s/omegac + 1))^2/(1 + s/2/pi/4000)^2; -#+end_src - -#+begin_src matlab :exports none - 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)]); -#+end_src - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/noise_uncertainty_sensors_hinf.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> -#+end_src - -#+NAME: fig:noise_uncertainty_sensors_hinf -#+CAPTION: Noise and Uncertainty characteristics of the sensors ([[./figs/noise_uncertainty_sensors_hinf.png][png]], [[./figs/noise_uncertainty_sensors_hinf.pdf][pdf]]) -[[file:figs/noise_uncertainty_sensors_hinf.png]] - -** Weights for uncertainty and performance -We design weights that are used to describe the wanted upper bound on the super sensor's noise and super sensor's uncertainty. - -Weight on the uncertainty: -#+begin_src matlab - n = 4; w0 = 2*pi*500; G0 = 6; 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; - - Wu = 0.2*(s+3.142e04)/(s+628.3)/H; -#+end_src - -Weight on the performance: -#+begin_src matlab - n = 1; w0 = 2*pi*9; A = 6; - a = sqrt(2*A^(2/n) - 1 + 2*A^(1/n)*sqrt(A^(2/n) - 1)); - G = ((1 + s/(w0/a))*(1 + s/(w0*a))/(1 + s/w0)^2)^n; - - n = 2; w0 = 2*pi*9; G0 = 1e-2; G1 = 1; Gc = 5e-1; - G2 = (((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; - - Wp = inv(G2)*inv(G)*inv(N2); -#+end_src - -The noise and uncertainty weights of the individual sensors and the asked noise/uncertainty of the super sensor are displayed in Fig. [[fig:charac_sensors_weights]]. -#+begin_src matlab :exports none - 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)|$'); - plot(freqs, 1./abs(squeeze(freqresp(Wp, freqs, 'Hz'))), 'k--', 'DisplayName', '$|w_r(j\omega)|^{-1}$'); - 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)|$'); - plot(freqs, 1./abs(squeeze(freqresp(Wu, 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'); - - linkaxes([ax1,ax2],'x'); - xlim([freqs(1), freqs(end)]); -#+end_src - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/charac_sensors_weights.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> -#+end_src - -#+NAME: fig:charac_sensors_weights -#+CAPTION: Upper bounds on the super sensor's noise and super sensor's uncertainty ([[./figs/charac_sensors_weights.png][png]], [[./figs/charac_sensors_weights.pdf][pdf]]) -[[file:figs/charac_sensors_weights.png]] - - -The corresponding maximum norms of the filters to have the perf/robust asked are shown in Fig. [[fig:upper_bound_complementary_filters_perf_robust]]. -#+begin_src matlab :exports none - figure; - hold on; - set(gca,'ColorOrderIndex',1) - plot(freqs, 1./abs(squeeze(freqresp(N1*Wp, freqs, 'Hz'))), '-', 'DisplayName', '$|N_1| - perf$'); - set(gca,'ColorOrderIndex',2) - plot(freqs, 1./abs(squeeze(freqresp(N2*Wp, freqs, 'Hz'))), '-', 'DisplayName', '$|N_2| - perf$'); - set(gca,'ColorOrderIndex',1) - plot(freqs, 1./abs(squeeze(freqresp(w1*Wu, freqs, 'Hz'))), '--', 'DisplayName', '$|N_1| - robu$'); - set(gca,'ColorOrderIndex',2) - plot(freqs, 1./abs(squeeze(freqresp(w2*Wu, freqs, 'Hz'))), '--', 'DisplayName', '$|N_2| - robu$'); - set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - xlabel('Frequency [Hz]'); ylabel('Magnitude'); - hold off; - legend('location', 'northeast'); -#+end_src - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/upper_bound_complementary_filters_perf_robust.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> -#+end_src - -#+NAME: fig:upper_bound_complementary_filters_perf_robust -#+CAPTION: Upper bounds on the complementary filters ([[./figs/upper_bound_complementary_filters_perf_robust.png][png]], [[./figs/upper_bound_complementary_filters_perf_robust.pdf][pdf]]) -[[file:figs/upper_bound_complementary_filters_perf_robust.png]] - -** H-infinity synthesis with 4 outputs corresponding to the 4 weights -We do the $\mathcal{H}_\infty$ synthesis with 4 weights and 4 outputs. - -\begin{equation*} - \left\| \begin{matrix} - W_{1p}(s) (1 - N_2(s)) \\ - W_{2p}(s) N_2(s) \\ - W_{1u}(s) (1 - N_2(s)) \\ - W_{2u}(s) N_2(s) - \end{matrix} \right\|_\infty < 1 -\end{equation*} - - -#+begin_src matlab - W1p = N1*Wp/(1+s/2/pi/1000); % Used to render W1p proper - W2p = N2*Wp; - W1u = w1*Wu; - W2u = w2*Wu; -#+end_src - -#+begin_src matlab - P = [W1p -W1p; - 0 W2p; - W1u -W1u; - 0 W2u; - 1 0]; -#+end_src - -And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command. -#+begin_src matlab :results output replace :exports both - [H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on'); -#+end_src - -#+RESULTS: -#+begin_example -[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on'); -Resetting value of Gamma min based on D_11, D_12, D_21 terms - -Test bounds: 1.4139 < gamma <= 65.6899 - - gamma hamx_eig xinf_eig hamy_eig yinf_eig nrho_xy p/f - 65.690 1.3e+00 -6.7e-15 1.3e+00 -4.5e-13 0.0000 p - 33.552 1.3e+00 -9.4e-15 1.3e+00 -3.7e-14 0.0000 p - 17.483 1.3e+00 -5.6e-16 1.3e+00 -4.8e-13 0.0000 p - 9.448 1.3e+00 -3.2e-15 1.3e+00 -1.2e-13 0.0000 p - 5.431 1.3e+00 -2.3e-16 1.3e+00 -3.6e-13 0.0000 p - 3.422 1.3e+00 -7.3e-16 1.3e+00 -2.6e-15 0.0000 p - 2.418 1.3e+00 9.3e-17 1.3e+00 -3.0e-14 0.0000 p - 1.916 1.3e+00 2.4e-17 1.3e+00 -2.2e-14 0.0000 p - 1.665 1.3e+00 -2.5e-16 1.3e+00 -2.1e-14 0.0000 p - 1.539 1.3e+00 -6.9e-15 1.3e+00 -5.3e-14 0.0000 p - 1.477 1.3e+00 -2.1e-14 1.3e+00 -2.3e-13 0.0000 p - 1.445 1.3e+00 -1.3e-16 1.3e+00 -2.6e-15 0.0000 p - 1.430 1.3e+00 -4.9e-13 1.3e+00 -2.2e-13 0.0000 p - 1.422 1.3e+00 -1.2e+08# 1.3e+00 -2.6e-13 0.0000 f - 1.426 1.3e+00 -6.3e-13 1.3e+00 -3.3e-14 0.0000 p - 1.424 1.3e+00 -3.4e+08# 1.3e+00 -4.5e-14 0.0000 f - 1.425 1.3e+00 -1.7e+09# 1.3e+00 -5.2e-13 0.0000 f - - Gamma value achieved: 1.4256 -#+end_example - -#+begin_src matlab - H1 = 1 - H2; -#+end_src - -The obtained complementary filters with the upper bounds are shown in Fig. [[fig:hinf_result_comp_filters_4_outputs]]. -#+begin_src matlab :exports none - figure; - hold on; - set(gca,'ColorOrderIndex',1) - plot(freqs, 1./abs(squeeze(freqresp(W1p, freqs, 'Hz'))), '-', 'DisplayName', '$|N_1| - perf$'); - set(gca,'ColorOrderIndex',2) - plot(freqs, 1./abs(squeeze(freqresp(W2p, freqs, 'Hz'))), '-', 'DisplayName', '$|N_2| - perf$'); - set(gca,'ColorOrderIndex',1) - plot(freqs, 1./abs(squeeze(freqresp(W1u, freqs, 'Hz'))), '--', 'DisplayName', '$|N_1| - robu$'); - set(gca,'ColorOrderIndex',2) - plot(freqs, 1./abs(squeeze(freqresp(W2u, freqs, 'Hz'))), '--', 'DisplayName', '$|N_2| - robu$'); - plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), 'k--', 'DisplayName', '$|H_1|$'); - plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), 'k--', 'DisplayName', '$|H_2|$'); - set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - xlabel('Frequency [Hz]'); ylabel('Magnitude'); - hold off; - legend('location', 'northeast'); -#+end_src - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/hinf_result_comp_filters_4_outputs.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> -#+end_src - -#+NAME: fig:hinf_result_comp_filters_4_outputs -#+CAPTION: caption ([[./figs/hinf_result_comp_filters_4_outputs.png][png]], [[./figs/hinf_result_comp_filters_4_outputs.pdf][pdf]]) -[[file:figs/hinf_result_comp_filters_4_outputs.png]] - - - -#+begin_src matlab :exports none - 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)|$'); - plot(freqs, 1./abs(squeeze(freqresp(Wp, freqs, 'Hz'))), 'k--', 'DisplayName', '$|w_r(j\omega)|^{-1}$'); - plot(freqs, sqrt(abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2), 'k-', 'DisplayName', '$|N_{ss}(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)|$'); - plot(freqs, 1./abs(squeeze(freqresp(Wu, freqs, 'Hz'))), 'k--', 'DisplayName', '$|w_u(j\omega)|^{-1}$'); - plot(freqs, abs(squeeze(freqresp(w1*H1, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2, freqs, 'Hz'))), 'k-', 'DisplayName', '$|w_{ss}(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)]); -#+end_src - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/upper_bounds_perf_robust_result_4_outputs.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> -#+end_src - -#+NAME: fig:upper_bounds_perf_robust_result_4_outputs -#+CAPTION: Obtained PSD and uncertainty with the corresponding upper bounds ([[./figs/upper_bounds_perf_robust_result_4_outputs.png][png]], [[./figs/upper_bounds_perf_robust_result_4_outputs.pdf][pdf]]) -[[file:figs/upper_bounds_perf_robust_result_4_outputs.png]] - -#+begin_src matlab :exports none - 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; - - CPS_S1 = 1/pi*cumtrapz(2*pi*freqs, PSD_S1); - CPS_S2 = 1/pi*cumtrapz(2*pi*freqs, PSD_S2); - CPS_H2 = 1/pi*cumtrapz(2*pi*freqs, PSD_H2); -#+end_src - -#+begin_src matlab :exports none - figure; - - ax1 = subplot(2, 1, 1); - 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}}$'); - set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - xlabel('Frequency [Hz]'); ylabel('Power Spectral Density'); - hold off; - legend('location', 'northeast'); - - ax2 = subplot(2, 1, 2); - 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}} = %.1e$', sqrt(CPS_H2(end)))); - set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); - xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum'); - hold off; - ylim([1e-10 1e-5]); - legend('location', 'southeast'); - - linkaxes([ax1,ax2],'x'); - xlim([freqs(1), freqs(end)]); -#+end_src - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/4outputs_hinf_psd_cps2svg.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> -#+end_src - -#+NAME: fig:4outputs_hinf_psd_cps2svg -#+CAPTION: PSD and CPS ([[./figs/4outputs_hinf_psd_cps2svg.png][png]], [[./figs/4outputs_hinf_psd_cps2svg.pdf][pdf]]) -[[file:figs/4outputs_hinf_psd_cps2svg.png]] - - -#+begin_src matlab :exports none - G1 = 1 + w1*ultidyn('Delta',[1 1]); - G2 = 1 + w2*ultidyn('Delta',[1 1]); - - 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; -#+end_src - -#+begin_src matlab :exports none - 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 - set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - set(gca, 'XTickLabel',[]); - legend('location', 'southwest'); - ylabel('Magnitude'); - ylim([5e-2, 10]); - 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 - set(gca,'xscale','log'); - yticks(-180:90:180); - ylim([-180 180]); - xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); - hold off; - linkaxes([ax1,ax2],'x'); -#+end_src - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/4outputs_uncertainty.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> -#+end_src - -#+NAME: fig:4outputs_uncertainty -#+CAPTION: Dynamical uncertainty ([[./figs/4outputs_uncertainty.png][png]], [[./figs/4outputs_uncertainty.pdf][pdf]]) -[[file:figs/4outputs_uncertainty.png]] - -** TODO Weight for both :noexport: -:PROPERTIES: -:header-args:matlab+: :tangle no -:END: -We may want to weights that capture both requirements. -We then have one weight for H1 and one weight for H2 (2 weights in total instead of 1). - -#+begin_src matlab - W1 = w1*Wu*(1+s/2/pi/40)^2/(1 + s/2/pi/1000)^2; - W2 = N2*Wp; -#+end_src - -#+begin_src matlab :exports none - figure; - hold on; - set(gca,'ColorOrderIndex',1) - plot(freqs, 1./abs(squeeze(freqresp(N1*Wp, freqs, 'Hz'))), '-', 'DisplayName', '$|N_1| - perf$'); - set(gca,'ColorOrderIndex',2) - plot(freqs, 1./abs(squeeze(freqresp(N2*Wp, freqs, 'Hz'))), '-', 'DisplayName', '$|N_2| - perf$'); - set(gca,'ColorOrderIndex',1) - plot(freqs, 1./abs(squeeze(freqresp(w1*Wu, freqs, 'Hz'))), '--', 'DisplayName', '$|N_1| - robu$'); - set(gca,'ColorOrderIndex',2) - plot(freqs, 1./abs(squeeze(freqresp(w2*Wu, freqs, 'Hz'))), '--', 'DisplayName', '$|N_2| - robu$'); - plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), 'k--', 'DisplayName', '$|W_1| - robu$'); - plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), 'k--', 'DisplayName', '$|W_2| - robu$'); - set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - xlabel('Frequency [Hz]'); ylabel('Magnitude'); - hold off; - legend('location', 'northeast'); -#+end_src - -The generalized plant $P$ is then: -#+begin_src matlab - P = [W1 -W1; - 0 W2; - 1 0]; -#+end_src - -And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command. -#+begin_src matlab :results output replace :exports both - [H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on'); -#+end_src - -#+RESULTS: -#+begin_example -[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on'); -Resetting value of Gamma min based on D_11, D_12, D_21 terms - -Test bounds: 0.8000 < gamma <= 1312.5112 - - gamma hamx_eig xinf_eig hamy_eig yinf_eig nrho_xy p/f -1.313e+03 1.3e+01 -1.7e-16 6.3e+00 -1.4e-19 0.0000 p - 656.656 1.3e+01 -3.4e-17 6.3e+00 -1.9e-13 0.0000 p - 328.728 1.3e+01 7.7e-17 6.3e+00 -1.3e-24 0.0000 p - 164.764 1.3e+01 2.6e-17 6.3e+00 -1.1e-13 0.0000 p - 82.782 1.3e+01 -2.0e-16 6.3e+00 -1.1e-13 0.0000 p - 41.791 1.3e+01 1.0e-16 6.3e+00 -8.9e-16 0.0000 p - 21.295 1.3e+01 -8.4e-17 6.3e+00 -6.3e-15 0.0000 p - 11.048 1.3e+01 8.5e-17 6.3e+00 -8.6e-14 0.0000 p - 5.924 1.3e+01 -2.5e-16 6.3e+00 -7.5e-14 0.0000 p - 3.362 1.3e+01 -1.7e-17 6.3e+00 -1.2e-13 0.0000 p - 2.081 1.2e+01 -5.1e-17 6.3e+00 -1.3e-13 0.0000 p - 1.440 1.1e+01 -2.4e+09# 6.3e+00 -3.4e-13 0.0000 f - 1.761 1.2e+01 -7.9e-17 6.3e+00 -3.3e-13 0.0000 p - 1.601 1.1e+01 -1.0e+10# 6.3e+00 -1.4e-13 0.0000 f - 1.681 1.2e+01 -3.1e+10# 6.3e+00 -1.5e-13 0.0000 f - 1.721 1.2e+01 -1.5e+11# 6.3e+00 -3.2e-13 0.0000 f - 1.741 1.2e+01 -4.6e-17 6.3e+00 -1.3e-13 0.0000 p - 1.731 1.2e+01 -1.3e+12# 6.3e+00 -1.6e-13 0.0000 f - 1.736 1.2e+01 1.4e-16 6.3e+00 -1.0e-13 0.0000 p - 1.733 1.2e+01 -1.7e-09 6.3e+00 -1.3e-13 0.0000 p - 1.732 1.2e+01 -1.3e+13# 6.3e+00 -1.4e-13 0.0000 f - 1.733 1.2e+01 5.3e-18 6.3e+00 -1.3e-13 0.0000 p - - Gamma value achieved: 1.7326 -#+end_example - -#+begin_src matlab - H1 = 1 - H2; -#+end_src - -#+begin_src matlab :exports none - figure; - hold on; - set(gca,'ColorOrderIndex',1) - plot(freqs, 1./abs(squeeze(freqresp(N1*Wp, freqs, 'Hz'))), '-', 'DisplayName', '$|N_1| - perf$'); - set(gca,'ColorOrderIndex',2) - plot(freqs, 1./abs(squeeze(freqresp(N2*Wp, freqs, 'Hz'))), '-', 'DisplayName', '$|N_2| - perf$'); - set(gca,'ColorOrderIndex',1) - plot(freqs, 1./abs(squeeze(freqresp(w1*Wu, freqs, 'Hz'))), '--', 'DisplayName', '$|N_1| - robu$'); - set(gca,'ColorOrderIndex',2) - plot(freqs, 1./abs(squeeze(freqresp(w2*Wu, freqs, 'Hz'))), '--', 'DisplayName', '$|N_2| - robu$'); - plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), 'k--', 'DisplayName', '$|H_1|$'); - plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), 'k--', 'DisplayName', '$|H_2|$'); - set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - xlabel('Frequency [Hz]'); ylabel('Magnitude'); - hold off; - legend('location', 'northeast'); -#+end_src - -#+begin_src matlab :exports none - 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; - - CPS_S1 = 1/pi*cumtrapz(2*pi*freqs, PSD_S1); - CPS_S2 = 1/pi*cumtrapz(2*pi*freqs, PSD_S2); - CPS_H2 = 1/pi*cumtrapz(2*pi*freqs, PSD_H2); -#+end_src - -#+begin_src matlab :exports none - figure; - - ax1 = subplot(2, 1, 1); - 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}}$'); - set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - xlabel('Frequency [Hz]'); ylabel('Power Spectral Density'); - hold off; - legend('location', 'northeast'); - - ax2 = subplot(2, 1, 2); - 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}} = %.1e$', sqrt(CPS_H2(end)))); - set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); - xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum'); - hold off; - ylim([1e-10 1e-5]); - legend('location', 'southeast'); - - linkaxes([ax1,ax2],'x'); - xlim([freqs(1), freqs(end)]); -#+end_src - -#+begin_src matlab :exports none - G1 = 1 + w1*ultidyn('Delta',[1 1]); - G2 = 1 + w2*ultidyn('Delta',[1 1]); - - 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; -#+end_src - -#+begin_src matlab :exports none - 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 - set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - set(gca, 'XTickLabel',[]); - legend('location', 'southwest'); - ylabel('Magnitude'); - ylim([5e-2, 10]); - 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 - set(gca,'xscale','log'); - yticks(-180:90:180); - ylim([-180 180]); - xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); - hold off; - linkaxes([ax1,ax2],'x'); -#+end_src - -** TODO Try to obtain better weight for the dynamical uncertainty :noexport: -:PROPERTIES: -:header-args:matlab+: :tangle no -:END: -Maybe we are asking too much for the limiting of the uncertainty. In reality, we should only limit the uncertainty around the merging frequency so that no RHP zero is introduced, and around the wanted crossover frequency. - -Weights about the uncertainty of the sensors. -#+begin_src matlab - omegac = 100*2*pi; G0 = 0.1; Ginf = 10; - w1 = (Ginf*s/omegac + G0)/(s/omegac + 1); - - omegac = 0.2*2*pi; G0 = 5; Ginf = 0.1; - w2 = (Ginf*s/omegac + G0)/(s/omegac + 1); - omegac = 5000*2*pi; G0 = 1; Ginf = 50; - w2 = w2*(Ginf*s/omegac + G0)/(s/omegac + 1); -#+end_src - -We make one guess about a nice weight that is just above the minimum of both uncertainty weights -#+begin_src matlab - bodeFig({w1, w2, 0.5*inv(inv(w1)+inv(w2))}) -#+end_src - -#+begin_src matlab :exports none - % We here compute the maximum and minimum phase of both sensors - Dphi1 = 180/pi*asin(abs(squeeze(freqresp(w1, freqs, 'Hz')))); - Dphi2 = 180/pi*asin(abs(squeeze(freqresp(w2, freqs, 'Hz')))); - Dphi1(abs(squeeze(freqresp(w1, freqs, 'Hz'))) > 1) = 190; - Dphi2(abs(squeeze(freqresp(w2, freqs, 'Hz'))) > 1) = 190; -#+end_src - -Weight that is used to bound the uncertainty of the super sensor. -#+begin_src matlab - wu = inv(inv(w1)+inv(w2)); - - W1 = w1/wu; - W2 = w2/wu; -#+end_src - -#+begin_src matlab - bodeFig({1/W1, 1/W2}) -#+end_src - -The wanted shape of complementary filters: -#+begin_src matlab - H1w = 1/W1; - H2w = 1/W2; -#+end_src - -The maximum wanted uncertainty. -#+begin_src matlab - Dphiss = 180/pi*asin(abs(squeeze(freqresp(w1*H1w, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2w, freqs, 'Hz')))); - Dphiss(abs(squeeze(freqresp(w1*H1w, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2w, freqs, 'Hz'))) > 1) = 190; -#+end_src - -#+begin_src matlab :exports none - 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), '--'); - plot(freqs, 1 + (abs(squeeze(freqresp(w1*H1w, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2w, freqs, 'Hz')))), 'k--'); - plot(freqs, max(1 - (abs(squeeze(freqresp(w1*H1w, freqs, 'Hz')))+abs(squeeze(freqresp(w2*H2w, freqs, 'Hz')))), 0), 'k--'); - set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - set(gca, 'XTickLabel',[]); - ylabel('Magnitude'); - ylim([1e-1, 10]); - 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--'); - set(gca,'xscale','log'); - yticks(-180:90:180); - ylim([-180 180]); - xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); - hold off; - linkaxes([ax1,ax2],'x'); -#+end_src - - - -** TODO New idea about weighting function for robustness :noexport: -:PROPERTIES: -:header-args:matlab+: :tangle no -:END: -Trying to limit the phase is too complicated, it is much easier to limit the radius of the uncertainty circle. -This is of course linked to the gain and phase uncertainty, but it is easier to work with. - -Ideally, we want to have: -\begin{align*} - & |w_{ss}(j\omega)| \approx |w_1(j\omega)| \quad \text{when} \quad |w_1(j\omega)| < |w_2(j\omega)| \\ - & |w_{ss}(j\omega)| \approx |w_2(j\omega)| \quad \text{when} \quad |w_2(j\omega)| < |w_1(j\omega)| -\end{align*} - -It is thus very similar to what is done for limiting the super sensor noise. - -#+begin_src matlab - omegac = 100*2*pi; G0 = 0.1; Ginf = 10; - w1 = (Ginf*s/omegac + G0)/(s/omegac + 1); - - omegac = 0.2*2*pi; G0 = 5; Ginf = 0.1; - w2 = (Ginf*s/omegac + G0)/(s/omegac + 1); - omegac = 5000*2*pi; G0 = 1; Ginf = 50; - w2 = w2*(Ginf*s/omegac + G0)/(s/omegac + 1); -#+end_src - -Weights on the Robustness: -#+begin_src matlab - epsilon = 1; - - W1r = 1/epsilon*w1/w2; - W2r = 1/epsilon*w2/w1; -#+end_src - -#+begin_src matlab - omegac = 100*2*pi; G0 = 1e-5; Ginf = 1e-4; - N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/100); - - omegac = 1*2*pi; G0 = 1e-3; Ginf = 1e-8; - N2 = ((sqrt(Ginf)*s/omegac + sqrt(G0))/(s/omegac + 1))^2/(1 + s/2/pi/4000)^2; -#+end_src - -Weights on the Noise: -#+begin_src matlab - epsilon = 1; - - W1n = 1/epsilon*N1/N2; - W2n = 1/epsilon*N2/N1; - - W1n = W1n/(1 + s/2/pi/1000); % this is added so that it is proper -#+end_src - -#+begin_src matlab :exports none - figure; - hold on; - set(gca,'ColorOrderIndex',1) - plot(freqs, 1./abs(squeeze(freqresp(W1r, freqs, 'Hz'))), '-', 'DisplayName', 'W1 - Robust.'); - set(gca,'ColorOrderIndex',2) - plot(freqs, 1./abs(squeeze(freqresp(W2r, freqs, 'Hz'))), '-', 'DisplayName', 'W2 - Robust.'); - set(gca,'ColorOrderIndex',1) - plot(freqs, 1./abs(squeeze(freqresp(W1n, freqs, 'Hz'))), '--', 'DisplayName', 'W1 - Noise'); - set(gca,'ColorOrderIndex',2) - plot(freqs, 1./abs(squeeze(freqresp(W2n, freqs, 'Hz'))), '--', 'DisplayName', 'W2 - Noise'); - set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - xlabel('Frequency [Hz]'); ylabel('Magnitude'); - hold off; - xlim([freqs(1), freqs(end)]); - legend('location', 'northeast'); -#+end_src - -#+begin_src matlab - P = [W1n -W1n; - 0 W2r; - 1 0]; -#+end_src - -And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command. -#+begin_src matlab :results output replace :exports both - [H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on'); -#+end_src - -#+RESULTS: -#+begin_example -[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on'); -Resetting value of Gamma min based on D_11, D_12, D_21 terms - -Test bounds: 0.5000 < gamma <= 65.6270 - - gamma hamx_eig xinf_eig hamy_eig yinf_eig nrho_xy p/f - 65.627 1.4e+01 -4.7e-13 1.3e+00 -2.7e-12 0.0000 p - 33.063 1.4e+01 3.7e-13 1.3e+00 -1.1e-17 0.0000 p - 16.782 1.4e+01 -9.5e-13 1.3e+00 -6.9e-15 0.0000 p - 8.641 1.4e+01 5.6e-13 1.3e+00 -2.0e-13 0.0000 p - 4.570 1.4e+01 2.6e-13 1.3e+00 -4.3e-14 0.0000 p - 2.535 1.4e+01 4.6e-13 1.3e+00 -1.7e-13 0.0000 p - 1.518 1.4e+01 -5.7e-13 1.3e+00 -8.2e-14 0.0000 p - 1.009 1.3e+01 7.9e-14 1.3e+00 -2.5e-14 0.0000 p - 0.754 1.3e+01 -2.1e-12 1.3e+00 -4.9e-15 0.0000 p - 0.627 1.2e+01 -2.1e+04# 1.3e+00 -1.8e-14 0.0000 f - 0.691 1.3e+01 -1.1e+05# 1.3e+00 -3.5e-16 0.0000 f - 0.723 1.3e+01 -5.5e+05# 1.3e+00 -2.0e-14 0.0000 f - 0.738 1.3e+01 -8.4e-12 1.3e+00 -2.7e-14 0.0000 p - 0.731 1.3e+01 -2.3e+06# 1.3e+00 -3.3e-13 0.0000 f - 0.735 1.3e+01 -9.9e-11 1.3e+00 -2.1e-14 0.0000 p - 0.733 1.3e+01 -8.9e+06# 1.3e+00 -5.0e-13 0.0000 f - 0.734 1.3e+01 -2.2e-10 1.3e+00 -1.9e-14 0.0000 p - - Gamma value achieved: 0.7335 -#+end_example - -#+begin_src matlab - H1 = 1 - H2; -#+end_src - -#+begin_src matlab - 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; - 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; -#+end_src - -#+begin_src matlab :exports none - 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 - set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - set(gca, 'XTickLabel',[]); - legend('location', 'southwest'); - ylabel('Magnitude'); - ylim([5e-2, 10]); - 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 - set(gca,'xscale','log'); - yticks(-180:90:180); - ylim([-180 180]); - xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); - hold off; - linkaxes([ax1,ax2],'x'); -#+end_src - -#+begin_src matlab :exports none - 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'); -#+end_src - -#+begin_src matlab :exports none - 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'); -#+end_src - -** Conclusion -The $\mathcal{H}_\infty$ synthesis has been used to design complementary filters that permits to robustly merge sensors while ensuring a maximum noise level. -However, no guarantee is made that the RMS value of the super sensor's noise is minimized. - -* Equivalent Super Sensor -<> -** Introduction :ignore: -The goal here is to find the parameters of a single sensor that would best represent a super sensor. - -** Sensor Fusion Architecture -Let consider figure [[fig:sensor_fusion_full]] 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$. - -#+name: fig:sensor_fusion_full -#+caption: Sensor fusion architecture ([[./figs/sensor_fusion_full.png][png]], [[./figs/sensor_fusion_full.pdf][pdf]]). -#+RESULTS: -[[file:figs-tikz/sensor_fusion_full.png]] - - -\begin{align*} - \hat{x} &= H_1(s) N_1(s) \tilde{n}_1 + H_2(s) N_2(s) \tilde{n}_2 \\ - &\quad \quad + \Big(H_1(s) \big(1 + w_1(s) \Delta_1(s)\big) + H_2(s) \big(1 + w_2(s) \Delta_2(s)\big)\Big) x \\ - &= H_1(s) N_1(s) \tilde{n}_1 + H_2(s) N_2(s) \tilde{n}_2 \\ - &\quad \quad + \big(1 + H_1(s) w_1(s) \Delta_1(s) + H_2(s) w_2(s) \Delta_2(s)\big) x -\end{align*} - -To the dynamics of the super sensor is: -\begin{equation} - \frac{\hat{x}}{x} = 1 + H_1(s) w_1(s) \Delta_1(s) + H_2(s) w_2(s) \Delta_2(s) -\end{equation} - -And the noise of the super sensor is: -\begin{equation} - n_{ss} = H_1(s) N_1(s) \tilde{n}_1 + H_2(s) N_2(s) \tilde{n}_2 -\end{equation} - -** Equivalent Configuration -We try to determine $w_{ss}(s)$ and $N_{ss}(s)$ such that the sensor on figure [[fig:sensor_fusion_equivalent]] is equivalent to the super sensor of figure [[fig:sensor_fusion_full]]. - -#+name: fig:sensor_fusion_equivalent -#+caption: Equivalent Super Sensor ([[./figs/sensor_fusion_equivalent.png][png]], [[./figs/sensor_fusion_equivalent.pdf][pdf]]). -#+RESULTS: -[[file:figs-tikz/sensor_fusion_equivalent.png]] - -** Model the uncertainty of the super sensor -At each frequency $\omega$, the uncertainty set of the super sensor shown on figure [[fig:sensor_fusion_full]] 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 [[fig:sensor_fusion_equivalent]] is a circle centered on $1$ with a radius equal to $|w_{ss}(j\omega)|$ on the complex plane. - -Ideally, we want to find a weight $w_{ss}(s)$ so that: -#+begin_important -\[ |w_{ss}(j\omega)| = |H_1(j\omega) w_1(j\omega)| + |H_2(j\omega) w_2(j\omega)|, \quad \forall\omega \] -#+end_important - -** Model the noise of the super sensor -The PSD of the estimation $\hat{x}$ when $x = 0$ of the configuration shown on figure [[fig:sensor_fusion_full]] 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} \\ - &= | H_1(j\omega) N_1(j\omega) |^2 + | H_2(j\omega) N_2(j\omega) |^2 -\end{align*} - -The PSD of the estimation $\hat{x}$ when $x = 0$ of the configuration shown on figure [[fig:sensor_fusion_equivalent]] is: -\begin{align*} - \Phi_{\hat{x}}(\omega) &= | N_{ss}(j\omega) |^2 \Phi_{\tilde{n}} \\ - &= | N_{ss}(j\omega) |^2 -\end{align*} - -Ideally, we want to find a weight $N_{ss}(s)$ such that: -#+begin_important -\[ |N_{ss}(j\omega)|^2 = | H_1(j\omega) N_1(j\omega) |^2 + | H_2(j\omega) N_2(j\omega) |^2 \quad \forall\omega \] -#+end_important - -** First guess -We could choose -\begin{align*} - w_{ss}(s) &= H_1(s) w_1(s) + H_2(s) w_2(s) \\ - N_{ss}(s) &= H_1(s) N_1(s) + H_2(s) N_2(s) -\end{align*} - -But we would have: -\begin{align*} - |w_{ss}(j\omega)| &= |H_1(j\omega) w_1(j\omega) + H_2(j\omega) w_2(j\omega)|, \quad \forall\omega \\ - &\neq |H_1(j\omega) w_1(j\omega)| + |H_2(j\omega) w_2(j\omega)|, \quad \forall\omega -\end{align*} -and -\begin{align*} - |N_{ss}(j\omega)|^2 &= | H_1(j\omega) N_1(j\omega) + H_2(j\omega) N_2(j\omega) |^2 \quad \forall\omega \\ - &\neq | H_1(j\omega) N_1(j\omega)|^2 + |H_2(j\omega) N_2(j\omega) |^2 \quad \forall\omega \\ -\end{align*} - -* Optimal And Robust Sensor Fusion in Practice +* Optimal And Robust Sensor Fusion in Practice :noexport: <> ** Introduction :ignore: Here are the steps in order to apply optimal and robust sensor fusion: @@ -3574,7 +3842,7 @@ The goal is to accurately determine $w(s)$ for the sensors that have to be merge ** 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: -- $w_1(s)$ and $w_2(s)$ representing the dynamic uncertainty of both sensors +- $W_1(s)$ and $W_2(s)$ representing the dynamic uncertainty of both sensors - $N_1(s)$ and $N_2(s)$ representing the noise characteristics of both sensors The goal is to design complementary filters $H_1(s)$ and $H_2(s)$ shown in figure [[fig:sensor_fusion_full]] such that: @@ -3585,662 +3853,9 @@ The goal is to design complementary filters $H_1(s)$ and $H_2(s)$ shown in figur #+caption: Sensor fusion architecture with sensor dynamics uncertainty [[file:figs-tikz/sensor_fusion_full.png]] -* Methods of complementary filter synthesis -<> -** Complementary filters using analytical formula - :PROPERTIES: - :header-args:matlab+: :tangle matlab/comp_filters_analytical.m - :header-args:matlab+: :comments org :mkdirp yes - :END: - <> - -*** Introduction :ignore: -*** ZIP file containing the data and matlab files :ignore: -#+begin_src bash :exports none :results none - if [ matlab/comp_filters_analytical.m -nt data/comp_filters_analytical.zip ]; then - cp matlab/comp_filters_analytical.m comp_filters_analytical.m; - zip data/comp_filters_analytical \ - comp_filters_analytical.m - rm comp_filters_analytical.m; - fi -#+end_src - -#+begin_note - All the files (data and Matlab scripts) are accessible [[file:data/comp_filters_analytical.zip][here]]. -#+end_note - -*** Matlab Init :noexport:ignore: -#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) - <> -#+end_src - -#+begin_src matlab :exports none :results silent :noweb yes - <> -#+end_src - -#+begin_src matlab - freqs = logspace(-1, 3, 1000); -#+end_src - -*** 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]]. - -#+begin_src matlab - w0 = 2*pi; % [rad/s] - - Hh1 = (s/w0)/((s/w0)+1); - Hl1 = 1/((s/w0)+1); -#+end_src - -#+begin_src matlab :exports none - freqs = logspace(-2, 2, 1000); - - figure; - % Magnitude - ax1 = subplot(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 = subplot(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)]); -#+end_src - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/comp_filter_1st_order.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> -#+end_src - -#+NAME: fig:comp_filter_1st_order -#+CAPTION: Bode plot of first order complementary filter ([[./figs/comp_filter_1st_order.png][png]], [[./figs/comp_filter_1st_order.pdf][pdf]]) -[[file:figs/comp_filter_1st_order.png]] - -*** 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$. - -#+begin_src matlab :exports none - freqs_study = logspace(-2, 2, 10000); - alphas = [0.1, 1, 10]; - w0 = 2*pi*1; - - figure; - ax1 = subplot(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 = subplot(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)]); -#+end_src - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/comp_filters_param_alpha.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> -#+end_src - -#+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. -#+begin_src matlab :exports none - 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 -#+end_src - -#+begin_src matlab - figure; - plot(alphas, infnorms) - set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log'); - xlabel('$\alpha$'); ylabel('$\|H_1\|_\infty$'); -#+end_src - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/param_alpha_hinf_norm.pdf" :var figsize="wide-normal" :post pdf2svg(file=*this*, ext="png") - <> -#+end_src - -#+NAME: fig:param_alpha_hinf_norm -#+CAPTION: Evolution of the H-Infinity norm of the complementary filters with the parameter $\alpha$ ([[./figs/param_alpha_hinf_norm.png][png]], [[./figs/param_alpha_hinf_norm.pdf][pdf]]) -[[file:figs/param_alpha_hinf_norm.png]] - -*** 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]]. - -#+begin_src matlab - 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)); -#+end_src - -#+begin_src matlab :exports none - 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'); -#+end_src - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/complementary_filters_third_order.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> -#+end_src - -#+NAME: fig:complementary_filters_third_order -#+CAPTION: Third order complementary filters using the analytical formula ([[./figs/complementary_filters_third_order.png][png]], [[./figs/complementary_filters_third_order.pdf][pdf]]) -[[file:figs/complementary_filters_third_order.png]] - -** H-Infinity synthesis of complementary filters - :PROPERTIES: - :header-args:matlab+: :tangle matlab/h_inf_synthesis_complementary_filters.m - :header-args:matlab+: :comments org :mkdirp yes - :END: - <> - -*** Introduction :ignore: -*** ZIP file containing the data and matlab files :ignore: -#+begin_src bash :exports none :results none - if [ matlab/h_inf_synthesis_complementary_filters.m -nt data/h_inf_synthesis_complementary_filters.zip ]; then - cp matlab/h_inf_synthesis_complementary_filters.m h_inf_synthesis_complementary_filters.m; - zip data/h_inf_synthesis_complementary_filters \ - h_inf_synthesis_complementary_filters.m - rm h_inf_synthesis_complementary_filters.m; - fi -#+end_src - -#+begin_note - All the files (data and Matlab scripts) are accessible [[file:data/h_inf_synthesis_complementary_filters.zip][here]]. -#+end_note - -*** Matlab Init :noexport:ignore: -#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) - <> -#+end_src - -#+begin_src matlab :exports none :results silent :noweb yes - <> -#+end_src - -#+begin_src matlab - freqs = logspace(-1, 3, 1000); -#+end_src - -*** Synthesis Architecture -We here synthesize the complementary filters using the $\mathcal{H}_\infty$ synthesis. -The goal is to specify upper bounds on the norms of $H_L$ and $H_H$ while ensuring their complementary property ($H_L + H_H = 1$). - -In order to do so, we use the generalized plant shown on figure [[fig:sf_hinf_filters_plant_b]] where $w_L$ and $w_H$ weighting transfer functions that will be used to shape $H_L$ and $H_H$ respectively. - -#+name: fig:sf_hinf_filters_plant_b -#+caption: Generalized plant used for the $\mathcal{H}_\infty$ synthesis of the complementary filters -[[file:figs-tikz/sf_hinf_filters_plant_b.png]] - -The $\mathcal{H}_\infty$ synthesis applied on this generalized plant will give a transfer function $H_L$ (figure [[fig:sf_hinf_filters_b]]) 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 \] - -Thus, if the above condition is verified, we can define $H_H = 1 - H_L$ and we have that: -\[ \left\| \begin{array}{c} H_L w_L \\ H_H w_H \end{array} \right\|_\infty < 1 \] -Which is almost (with an maximum error of $\sqrt{2}$) equivalent to: -\begin{align*} - |H_L| &< \frac{1}{|w_L|}, \quad \forall \omega \\ - |H_H| &< \frac{1}{|w_H|}, \quad \forall \omega -\end{align*} - -We then see that $w_L$ and $w_H$ can be used to shape both $H_L$ and $H_H$ while ensuring (by definition of $H_H = 1 - H_L$) their complementary property. - -#+name: fig:sf_hinf_filters_b -#+caption: $\mathcal{H}_\infty$ synthesis of the complementary filters -[[file:figs-tikz/sf_hinf_filters_b.png]] - -*** Weights - -#+begin_src matlab - 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; -#+end_src - -#+begin_src matlab :exports none - 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'); -#+end_src - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/weights_wl_wh.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> -#+end_src - -#+NAME: fig:weights_wl_wh -#+CAPTION: Weights on the complementary filters $w_L$ and $w_H$ and the associated performance weights ([[./figs/weights_wl_wh.png][png]], [[./figs/weights_wl_wh.pdf][pdf]]) -[[file:figs/weights_wl_wh.png]] - -*** H-Infinity Synthesis -We define the generalized plant $P$ on matlab. -#+begin_src matlab - P = [0 wL; - wH -wH; - 1 0]; -#+end_src - -And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command. -#+begin_src matlab :results output replace :exports both - [Hl_hinf, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on'); -#+end_src - -#+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]]. -#+begin_src matlab - Hh_hinf = 1 - Hl_hinf; -#+end_src - -*** Obtained Complementary Filters - -The obtained complementary filters are shown on figure [[fig:hinf_filters_results]]. - -#+begin_src matlab :exports none - 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'); -#+end_src - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/hinf_filters_results.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> -#+end_src - -#+NAME: fig:hinf_filters_results -#+CAPTION: Obtained complementary filters using $\mathcal{H}_\infty$ synthesis ([[./figs/hinf_filters_results.png][png]], [[./figs/hinf_filters_results.pdf][pdf]]) -[[file:figs/hinf_filters_results.png]] - -** Feedback Control Architecture to generate Complementary Filters - :PROPERTIES: - :header-args:matlab+: :tangle matlab/feedback_generate_comp_filters.m - :header-args:matlab+: :comments org :mkdirp yes - :END: - <> - -*** Introduction :ignore: -The idea is here to use the fact that in a classical feedback architecture, $S + T = 1$, in order to design complementary filters. - -Thus, all the tools that has been developed for classical feedback control can be used for complementary filter design. - -*** ZIP file containing the data and matlab files :ignore: -#+begin_src bash :exports none :results none - if [ matlab/feedback_generate_comp_filters.m -nt data/feedback_generate_comp_filters.zip ]; then - cp matlab/feedback_generate_comp_filters.m feedback_generate_comp_filters.m; - zip data/feedback_generate_comp_filters \ - feedback_generate_comp_filters.m - rm feedback_generate_comp_filters.m; - fi -#+end_src - -#+begin_note - All the files (data and Matlab scripts) are accessible [[file:data/feedback_generate_comp_filters.zip][here]]. -#+end_note - -*** Matlab Init :noexport:ignore: -#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) - <> -#+end_src - -#+begin_src matlab :exports none :results silent :noweb yes - <> -#+end_src - -#+begin_src matlab - freqs = logspace(-2, 2, 1000); -#+end_src - -*** Architecture -#+name: fig:complementary_filters_feedback_architecture -#+caption: Architecture used to generate the complementary filters -[[file:figs-tikz/complementary_filters_feedback_architecture.png]] - -We have: -\[ y = \underbrace{\frac{L}{L + 1}}_{H_L} y_1 + \underbrace{\frac{1}{L + 1}}_{H_H} y_2 \] -with $H_L + H_H = 1$. - -The only thing to design is $L$ such that the complementary filters are stable with the wanted shape. - -A simple choice is: -\[ L = \left(\frac{\omega_c}{s}\right)^2 \frac{\frac{s}{\omega_c / \alpha} + 1}{\frac{s}{\omega_c} + \alpha} \] - -Which contains two integrator and a lead. $\omega_c$ is used to tune the crossover frequency and $\alpha$ the trade-off "bump" around blending frequency and filtering away from blending frequency. - -*** Loop Gain Design -Let's first define the loop gain $L$. -#+begin_src matlab - wc = 2*pi*1; - alpha = 2; - - L = (wc/s)^2 * (s/(wc/alpha) + 1)/(s/wc + alpha); -#+end_src - -#+begin_src matlab :exports none - 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]); -#+end_src - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/loop_gain_bode_plot.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> -#+end_src - -#+NAME: fig:loop_gain_bode_plot -#+CAPTION: Bode plot of the loop gain $L$ ([[./figs/loop_gain_bode_plot.png][png]], [[./figs/loop_gain_bode_plot.pdf][pdf]]) -[[file:figs/loop_gain_bode_plot.png]] - -*** Complementary Filters Obtained -We then compute the resulting low pass and high pass filters. -#+begin_src matlab - Hl = L/(L + 1); - Hh = 1/(L + 1); -#+end_src - -#+begin_src matlab :exports none - 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'); -#+end_src - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/low_pass_high_pass_filters.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> -#+end_src - -#+NAME: fig:low_pass_high_pass_filters -#+CAPTION: Low pass and High pass filters $H_L$ and $H_H$ for different values of $\alpha$ ([[./figs/low_pass_high_pass_filters.png][png]], [[./figs/low_pass_high_pass_filters.pdf][pdf]]) -[[file:figs/low_pass_high_pass_filters.png]] - -** Analytical Formula found in the literature - <> - -*** Analytical Formula -cite:min15_compl_filter_desig_angle_estim -\begin{align*} - H_L(s) = \frac{K_p s + K_i}{s^2 + K_p s + K_i} \\ - H_H(s) = \frac{s^2}{s^2 + K_p s + K_i} -\end{align*} - -cite:corke04_inert_visual_sensin_system_small_auton_helic -\begin{align*} - H_L(s) = \frac{1}{s/p + 1} \\ - H_H(s) = \frac{s/p}{s/p + 1} -\end{align*} - -cite:jensen13_basic_uas -\begin{align*} - H_L(s) = \frac{2 \omega_0 s + \omega_0^2}{(s + \omega_0)^2} \\ - H_H(s) = \frac{s^2}{(s + \omega_0)^2} -\end{align*} - -\begin{align*} - H_L(s) = \frac{C(s)}{C(s) + s} \\ - H_H(s) = \frac{s}{C(s) + s} -\end{align*} - -cite:shaw90_bandw_enhan_posit_measur_using_measur_accel -\begin{align*} - H_L(s) = \frac{3 \tau s + 1}{(\tau s + 1)^3} \\ - H_H(s) = \frac{\tau^3 s^3 + 3 \tau^2 s^2}{(\tau s + 1)^3} -\end{align*} - -cite:baerveldt97_low_cost_low_weigh_attit -\begin{align*} - H_L(s) = \frac{2 \tau s + 1}{(\tau s + 1)^2} \\ - H_H(s) = \frac{\tau^2 s^2}{(\tau s + 1)^2} -\end{align*} - -*** Matlab Init :noexport:ignore: -#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) - <> -#+end_src - -#+begin_src matlab :exports none :results silent :noweb yes - <> -#+end_src - -*** Matlab -#+begin_src 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; -#+end_src - -#+begin_src matlab :exports none - freqs = logspace(-1, 1, 1000); - - figure; - % Magnitude - ax1 = subplot(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 = subplot(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)]); -#+end_src - -#+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/comp_filters_literature.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") - <> -#+end_src - -#+NAME: fig:comp_filters_literature -#+CAPTION: Comparison of some complementary filters found in the literature ([[./figs/comp_filters_literature.png][png]], [[./figs/comp_filters_literature.pdf][pdf]]) -[[file:figs/comp_filters_literature.png]] - -*** Discussion -Analytical Formula found in the literature provides either no parameter for tuning the robustness / performance trade-off. - -** 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: -- the analytical formula provides a very simple way to generate the complementary filters (and thus the controller), they could even be used to tune the controller online using the parameters $\alpha$ and $\omega_0$. However, these formula have the property that $|H_H|$ and $|H_L|$ are symmetrical with the frequency $\omega_0$ which may not be desirable. -- while the $\mathcal{H}_\infty$ synthesis of the complementary filters is not as straightforward as using the analytical formula, it provides a more optimized procedure to obtain the complementary filters - -* Real World Example of optimal sensor fusion +* Real World Example of optimal sensor fusion :noexport: ** Introduction :ignore: -cite:moore19_capac_instr_sensor_fusion_high_bandw_nanop +cite:moore19_capac_instr_sensor_fusion_high_bandW_nanop ** Matlab Init :noexport:ignore: