diff --git a/matlab/figs/comparison_cps_noise.pdf b/matlab/figs/comparison_cps_noise.pdf new file mode 100644 index 0000000..56e85ff Binary files /dev/null and b/matlab/figs/comparison_cps_noise.pdf differ diff --git a/matlab/figs/comparison_cps_noise.png b/matlab/figs/comparison_cps_noise.png new file mode 100644 index 0000000..d77546b Binary files /dev/null and b/matlab/figs/comparison_cps_noise.png differ diff --git a/matlab/figs/comparison_psd_noise.pdf b/matlab/figs/comparison_psd_noise.pdf new file mode 100644 index 0000000..c1036de Binary files /dev/null and b/matlab/figs/comparison_psd_noise.pdf differ diff --git a/matlab/figs/comparison_psd_noise.png b/matlab/figs/comparison_psd_noise.png new file mode 100644 index 0000000..c8684d6 Binary files /dev/null and b/matlab/figs/comparison_psd_noise.png differ diff --git a/matlab/figs/cps_h2_synthesis.pdf b/matlab/figs/cps_h2_synthesis.pdf new file mode 100644 index 0000000..5a63b75 Binary files /dev/null and b/matlab/figs/cps_h2_synthesis.pdf differ diff --git a/matlab/figs/cps_h2_synthesis.png b/matlab/figs/cps_h2_synthesis.png new file mode 100644 index 0000000..0692196 Binary files /dev/null and b/matlab/figs/cps_h2_synthesis.png differ diff --git a/matlab/figs/htwo_comp_filters.pdf b/matlab/figs/htwo_comp_filters.pdf index 8479d11..3fe328f 100644 Binary files a/matlab/figs/htwo_comp_filters.pdf and b/matlab/figs/htwo_comp_filters.pdf differ diff --git a/matlab/figs/htwo_comp_filters.png b/matlab/figs/htwo_comp_filters.png index 743e6bd..78ce5fe 100644 Binary files a/matlab/figs/htwo_comp_filters.png and b/matlab/figs/htwo_comp_filters.png differ diff --git a/matlab/figs/noise_characteristics_sensors.pdf b/matlab/figs/noise_characteristics_sensors.pdf new file mode 100644 index 0000000..fb41a13 Binary files /dev/null and b/matlab/figs/noise_characteristics_sensors.pdf differ diff --git a/matlab/figs/noise_characteristics_sensors.png b/matlab/figs/noise_characteristics_sensors.png new file mode 100644 index 0000000..5f1303f Binary files /dev/null and b/matlab/figs/noise_characteristics_sensors.png differ diff --git a/matlab/figs/nosie_characteristics_sensors.pdf b/matlab/figs/nosie_characteristics_sensors.pdf index 088180f..b7c43fe 100644 Binary files a/matlab/figs/nosie_characteristics_sensors.pdf and b/matlab/figs/nosie_characteristics_sensors.pdf differ diff --git a/matlab/figs/nosie_characteristics_sensors.png b/matlab/figs/nosie_characteristics_sensors.png index d4460b8..5f1303f 100644 Binary files a/matlab/figs/nosie_characteristics_sensors.png and b/matlab/figs/nosie_characteristics_sensors.png differ diff --git a/matlab/figs/psd_sensors_htwo_synthesis.pdf b/matlab/figs/psd_sensors_htwo_synthesis.pdf index 20bf7b2..0f2a10f 100644 Binary files a/matlab/figs/psd_sensors_htwo_synthesis.pdf and b/matlab/figs/psd_sensors_htwo_synthesis.pdf differ diff --git a/matlab/figs/psd_sensors_htwo_synthesis.png b/matlab/figs/psd_sensors_htwo_synthesis.png index b0cf286..757dae5 100644 Binary files a/matlab/figs/psd_sensors_htwo_synthesis.png and b/matlab/figs/psd_sensors_htwo_synthesis.png differ diff --git a/matlab/figs/weights_comp_filters_Hinfa.pdf b/matlab/figs/weights_comp_filters_Hinfa.pdf new file mode 100644 index 0000000..298a034 Binary files /dev/null and b/matlab/figs/weights_comp_filters_Hinfa.pdf differ diff --git a/matlab/figs/weights_comp_filters_Hinfa.png b/matlab/figs/weights_comp_filters_Hinfa.png new file mode 100644 index 0000000..64b7b7c Binary files /dev/null and b/matlab/figs/weights_comp_filters_Hinfa.png differ diff --git a/matlab/figs/weights_comp_filters_Hinfb.pdf b/matlab/figs/weights_comp_filters_Hinfb.pdf new file mode 100644 index 0000000..18985cc Binary files /dev/null and b/matlab/figs/weights_comp_filters_Hinfb.pdf differ diff --git a/matlab/figs/weights_comp_filters_Hinfb.png b/matlab/figs/weights_comp_filters_Hinfb.png new file mode 100644 index 0000000..39e7779 Binary files /dev/null and b/matlab/figs/weights_comp_filters_Hinfb.png differ diff --git a/matlab/index.html b/matlab/index.html index 03921da..fd961af 100644 --- a/matlab/index.html +++ b/matlab/index.html @@ -3,7 +3,7 @@ "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> - + Robust and Optimal Sensor Fusion - Matlab Computation @@ -279,84 +279,80 @@ for the JavaScript code in this tag.

Table of Contents

-
-

4.1 Measurement of the noise characteristics of the sensors

+
+

4.1 Measurement of the noise characteristics of the sensors

-
-

4.1.1 Huddle Test

+
+

4.1.1 Huddle Test

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

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

-
+

huddle_test.png

-

Figure 14: Huddle test block diagram

+

Figure 19: Huddle test block diagram

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

\begin{equation} -\label{orgdaace50} +\label{org456a8db} \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} @@ -1320,16 +1350,16 @@ We then have: Since the input signal \(x\) and the instrumental noise \(n\) are incoherent:

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

-From equations \eqref{orgdaace50} and \eqref{org4188c50}, we finally obtain +From equations \eqref{org456a8db} and \eqref{org0b69a39}, we finally obtain

\begin{equation} -\label{orgf2631e0} +\label{org47f030a} |\Phi_n(\omega)| = |\Phi_{\hat{x}}(\omega)| \left( 1 - \sqrt{\gamma_{\hat{x}_1\hat{x}_2}^2(\omega)} \right) \end{equation} @@ -1337,8 +1367,8 @@ From equations \eqref{orgdaace50} and \eqref{org4188c50}, we finally obtain
-
-

4.1.2 Weights that represents the noises' PSD

+
+

4.1.2 Weights that represents the noises' PSD

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

-The sensor with a normalized noise input is shown in figure 15. +The sensor with a normalized noise input is shown in figure 20.

-
+

one_sensor_normalized_noise.png

-

Figure 15: One sensor with normalized noise

+

Figure 20: One sensor with normalized noise

-
-

4.1.3 Comparison of the noises' PSD

+
+

4.1.3 Comparison of the noises' PSD

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

-
-

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

+
+

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

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

-
-

4.2 Estimate the dynamic uncertainty of the sensors

+
+

4.2 Estimate the dynamic uncertainty of the sensors

-Let's consider one sensor represented on figure 16. +Let's consider one sensor represented on figure 21.

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

-
+

one_sensor_dyn_uncertainty.png

-

Figure 16: Sensor with dynamic uncertainty

+

Figure 21: Sensor with dynamic uncertainty

-
-

4.3 Optimal and Robust synthesis of the complementary filters

+
+

4.3 Optimal and Robust synthesis of the complementary filters

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

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

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

sensor_fusion_full.png

-

Figure 17: Sensor fusion architecture with sensor dynamics uncertainty

+

Figure 22: Sensor fusion architecture with sensor dynamics uncertainty

-
-

5 Methods of complementary filter synthesis

+
+

5 Methods of complementary filter synthesis

-
-

5.1 Complementary filters using analytical formula

+
+

5.1 Complementary filters using analytical formula

- +

@@ -1486,8 +1516,8 @@ All the files (data and Matlab scripts) are accessible -

5.1.1 Analytical 1st order complementary filters

+
+

5.1.1 Analytical 1st order complementary filters

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

-Their bode plot is shown figure 18. +Their bode plot is shown figure 23.

@@ -1510,16 +1540,16 @@ Hl1 = 1
-
+

comp_filter_1st_order.png

-

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

+

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

-
-

5.1.2 Second Order Complementary Filters

+
+

5.1.2 Second Order Complementary Filters

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

-This is illustrated on figure 19. +This is illustrated on figure 24. 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 19: Effect of the parameter \(\alpha\) on the shape of the generated second order complementary filters (png, pdf)

+

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

@@ -1568,16 +1598,16 @@ xlabel('$ +

param_alpha_hinf_norm.png

-

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

+

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

-
-

5.1.3 Third Order Complementary Filters

+
+

5.1.3 Third Order Complementary Filters

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

-The filters are defined below and the result is shown on figure 21. +The filters are defined below and the result is shown on figure 26.

@@ -1610,20 +1640,20 @@ Hl3_ana = ( +

complementary_filters_third_order.png

-

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

+

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

-
-

5.2 H-Infinity synthesis of complementary filters

+
+

5.2 H-Infinity synthesis of complementary filters

- +

@@ -1633,8 +1663,8 @@ All the files (data and Matlab scripts) are accessible -

5.2.1 Synthesis Architecture

+
+

5.2.1 Synthesis Architecture

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

-In order to do so, we use the generalized plant shown on figure 22 where \(w_L\) and \(w_H\) weighting transfer functions that will be used to shape \(H_L\) and \(H_H\) respectively. +In order to do so, we use the generalized plant shown on figure 27 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 22: Generalized plant used for the \(\mathcal{H}_\infty\) synthesis of the complementary filters

+

Figure 27: 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 23) such that the \(\mathcal{H}_\infty\) norm of the transfer function from \(w\) to \([z_H,\ z_L]\) is less than one: +The \(\mathcal{H}_\infty\) synthesis applied on this generalized plant will give a transfer function \(H_L\) (figure 28) 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 \]

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

-
+

sf_hinf_filters_b.png

-

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

+

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

-
-

5.2.2 Weights

+
+

5.2.2 Weights

omegab = 2*pi*9;
@@ -1692,16 +1722,16 @@ wL = (s 
+

weights_wl_wh.png

-

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

+

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

-
-

5.2.3 H-Infinity Synthesis

+
+

5.2.3 H-Infinity Synthesis

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

-We then define the high pass filter \(H_H = 1 - H_L\). The bode plot of both \(H_L\) and \(H_H\) is shown on figure 25. +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 30.

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

5.2.4 Obtained Complementary Filters

+
+

5.2.4 Obtained Complementary Filters

-The obtained complementary filters are shown on figure 25. +The obtained complementary filters are shown on figure 30.

-
+

hinf_filters_results.png

-

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

+

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

-
-

5.3 Feedback Control Architecture to generate Complementary Filters

+
+

5.3 Feedback Control Architecture to generate Complementary Filters

- +

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

5.3.1 Architecture

+
+

5.3.1 Architecture

-
+

complementary_filters_feedback_architecture.png

-

Figure 26: Architecture used to generate the complementary filters

+

Figure 31: Architecture used to generate the complementary filters

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

-
-

5.3.2 Loop Gain Design

+
-
-

5.3.3 Complementary Filters Obtained

+
+

5.3.3 Complementary Filters Obtained

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

-
+

low_pass_high_pass_filters.png

-

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

+

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

-
-

5.4 Analytical Formula found in the literature

+
+

5.4 Analytical Formula found in the literature

- +

-
-

5.4.1 Analytical Formula

+
+

5.4.1 Analytical Formula

min15_compl_filter_desig_angle_estim @@ -1924,8 +1954,8 @@ Hh = 1/

-
-

5.4.2 Matlab

+
+

5.4.2 Matlab

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

comp_filters_literature.png

-

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

+

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

-
-

5.4.3 Discussion

+
+

5.4.3 Discussion

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

-
-

5.5 Comparison of the different methods of synthesis

+
+

5.5 Comparison of the different methods of synthesis

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

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

Author: Thomas Dehaeze

-

Created: 2019-08-29 jeu. 11:26

+

Created: 2019-08-29 jeu. 14:53

Validate

diff --git a/matlab/index.org b/matlab/index.org index 130f37f..3ec04ab 100644 --- a/matlab/index.org +++ b/matlab/index.org @@ -86,61 +86,65 @@ The complementary filters have to be designed in order to minimize the effect no #+end_src ** Architecture -Let's consider the sensor fusion architecture shown on figure [[fig:fusion_two_noisy_sensors_with_dyn]] where two sensors 1 and 2 are measuring the same quantity $x$ with different noise characteristics determined by $W_1$ and $W_2$. +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)$. -$n_1$ and $n_2$ are white noise (constant power spectral density over all frequencies). +$\tilde{n}_1$ and $\tilde{n}_2$ are normalized white noise: +#+name: eq:normalized_noise +\begin{equation} + \Phi_{\tilde{n}_1}(\omega) = \Phi_{\tilde{n}_1}(\omega) = 1 +\end{equation} -#+name: fig:fusion_two_noisy_sensors_with_dyn +#+name: fig:fusion_two_noisy_sensors_weights #+caption: Fusion of two sensors -[[file:figs-tikz/fusion_two_noisy_sensors_with_dyn.png]] +[[file:figs-tikz/fusion_two_noisy_sensors_weights.png]] -We consider that the two sensor dynamics $G_1$ and $G_2$ are ideal ($G_1 = G_2 = 1$). We obtain the architecture of figure [[fig:fusion_two_noisy_sensors]]. +We consider that the two sensor dynamics $G_1(s)$ and $G_2(s)$ are ideal: +#+name: eq:idea_dynamics +\begin{equation} + G_1(s) = G_2(s) = 1 +\end{equation} -#+name: fig:fusion_two_noisy_sensors +We obtain the architecture of figure [[fig:sensor_fusion_noisy_perfect_dyn]]. + +#+name: fig:sensor_fusion_noisy_perfect_dyn #+caption: Fusion of two sensors with ideal dynamics -[[file:figs-tikz/fusion_two_noisy_sensors.png]] +[[file:figs-tikz/sensor_fusion_noisy_perfect_dyn.png]] -$H_1$ and $H_2$ are complementary filters ($H_1 + H_2 = 1$). The goal is to design $H_1$ and $H_2$ such that the effect of the noise sources $n_1$ and $n_2$ has the smallest possible effect on the estimation $\hat{x}$. +$H_1(s)$ and $H_2(s)$ are complementary filters: +#+name: eq:comp_filters_property +\begin{equation} + H_1(s) + H_2(s) = 1 +\end{equation} + +The goal is to design $H_1(s)$ and $H_2(s)$ such that the effect of the noise sources $\tilde{n}_1$ and $\tilde{n}_2$ has the smallest possible effect on the estimation $\hat{x}$. We have that the Power Spectral Density (PSD) of $\hat{x}$ is: -\[ \Gamma_{\hat{x}} = |H_1 W_1|^2 \Gamma_{n_1} + |H_2 W_2|^2 \Gamma_{n_2} \] +\[ \Phi_{\hat{x}}(\omega) = |H_1(j\omega) N_1(j\omega)|^2 \Phi_{\tilde{n}_1}(\omega) + |H_2(j\omega) N_2(j\omega)|^2 \Phi_{\tilde{n}_2}(\omega), \quad \forall \omega \] And the goal is the minimize the Root Mean Square (RMS) value of $\hat{x}$: -\[ \sigma_{\hat{x}} = \sqrt{\int_0^\infty \Gamma_{\hat{x}}(\omega) d\omega} \] - -As $n_1$ and $n_2$ are white noise: $\Gamma_{n_1} = \Gamma_{n_2} = 1$ and we have: -\[ \sigma_{\hat{x}} = \sqrt{\int_0^\infty |H_1 W_1|^2(\omega) + |H_2 W_2|^2(\omega) d\omega} = \left\| \begin{matrix} H_1 W_1 \\ H_2 W_2 \end{matrix} \right\|_2 \] - -Thus, the goal is to design $H_1$ and $H_2$ such that $H_1 + H_2 = 1$ and such that $\left\| \begin{matrix} H_1 W_1 \\ H_2 W_2 \end{matrix} \right\|_2$ is minimized. - -For that, we will use the $\mathcal{H}_2$ Synthesis. +#+name: eq:rms_value_estimation +\begin{equation} + \sigma_{\hat{x}} = \sqrt{\int_0^\infty \Phi_{\hat{x}}(\omega) d\omega} +\end{equation} ** Noise of the sensors -Let's define the noise characteristics of the two sensors by choosing $W_1$ and $W_2$: -- Sensor 1 characterized by $W_1$ has low noise at low frequency (for instance a geophone) -- Sensor 2 characterized by $W_2$ has low noise at high frequency (for instance an accelerometer) - -#+begin_src matlab :exports none - omegac = 2*pi; G0 = 1e-2; Ginf = 1e-6; - W1 = ((sqrt(G0))/(s/omegac + 1))^2; - - omegac = 100*2*pi; G0 = 1e-6; Ginf = 1e-2; - W2 = ((sqrt(Ginf)*s/omegac + sqrt(G0))/(s/omegac + 1))^2/(1 + s/2/pi/4000)^2; -#+end_src +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) #+begin_src matlab omegac = 100*2*pi; G0 = 1e-5; Ginf = 1e-4; - W1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/4000); + N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/100); omegac = 1*2*pi; G0 = 1e-3; Ginf = 1e-8; - W2 = ((sqrt(Ginf)*s/omegac + sqrt(G0))/(s/omegac + 1))^2/(1 + s/2/pi/4000)^2; + 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; hold on; - plot(freqs, abs(squeeze(freqresp(W1, freqs, 'Hz'))), '-', 'DisplayName', '$W_1$'); - plot(freqs, abs(squeeze(freqresp(W2, freqs, 'Hz'))), '-', 'DisplayName', '$W_2$'); + plot(freqs, abs(squeeze(freqresp(N1, freqs, 'Hz'))), '-', 'DisplayName', '$N_1$'); + plot(freqs, abs(squeeze(freqresp(N2, freqs, 'Hz'))), '-', 'DisplayName', '$N_2$'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); xlabel('Frequency [Hz]'); ylabel('Magnitude'); hold off; @@ -149,15 +153,21 @@ Let's define the noise characteristics of the two sensors by choosing $W_1$ and #+end_src #+HEADER: :tangle no :exports results :results none :noweb yes -#+begin_src matlab :var filepath="figs/nosie_characteristics_sensors.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") +#+begin_src matlab :var filepath="figs/noise_characteristics_sensors.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") <> #+end_src -#+NAME: fig:nosie_characteristics_sensors -#+CAPTION: Noise Characteristics of the two sensors ([[./figs/nosie_characteristics_sensors.png][png]], [[./figs/nosie_characteristics_sensors.pdf][pdf]]) -[[file:figs/nosie_characteristics_sensors.png]] +#+NAME: fig:noise_characteristics_sensors +#+CAPTION: Noise Characteristics of the two sensors ([[./figs/noise_characteristics_sensors.png][png]], [[./figs/noise_characteristics_sensors.pdf][pdf]]) +[[file:figs/noise_characteristics_sensors.png]] ** 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 @@ -165,16 +175,16 @@ We use the generalized plant architecture shown on figure [[fig:h_infinity_optim [[file:figs-tikz/h_infinity_optimal_comp_filters.png]] The transfer function from $[n_1, n_2]$ to $\hat{x}$ is: -\[ \begin{bmatrix} W_1 H_1 \\ W_2 (1 - H_1) \end{bmatrix} \] +\[ \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} W_1 H_1 \\ W_2 H_2 \end{bmatrix} \] +\[ \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 = [0 W2 1; - W1 -W2 0]; + P = [0 N2 1; + N1 -N2 0]; #+end_src And we do the $\mathcal{H}_2$ synthesis using the =h2syn= command. @@ -182,17 +192,18 @@ And we do the $\mathcal{H}_2$ synthesis using the =h2syn= command. [H1, ~, gamma] = h2syn(P, 1, 1); #+end_src -What is minimized is =norm([W1*H1,W2*H2], 2)=. - -Finally, we define $H_2 = 1 - H_1$. +Finally, we define $H_2(s) = 1 - H_1(s)$. #+begin_src matlab H2 = 1 - H1; #+end_src -** Analysis -The complementary filters obtained are shown on figure [[fig:htwo_comp_filters]]. The PSD of the [[fig:psd_sensors_htwo_synthesis]]. -Finally, the RMS value of $\hat{x}$ is shown on table [[tab:rms_results]]. -The optimal sensor fusion has permitted to reduced the RMS value of the estimation error by a factor 8 compare to when using only one sensor. +The complementary filters obtained are shown on figure [[fig:htwo_comp_filters]]. + +The PSD of the noise of the individual sensor and of the super sensor are shown in Fig. [[fig:psd_sensors_htwo_synthesis]]. + +The Cumulative Power Spectrum (CPS) is shown on Fig. [[fig:cps_h2_synthesis]]. + +The obtained RMS value of the super sensor is lower than the RMS value of the individual sensors. #+begin_src matlab :exports none figure; @@ -215,14 +226,20 @@ The optimal sensor fusion has permitted to reduced the RMS value of the estimati #+CAPTION: Obtained complementary filters using the $\mathcal{H}_2$ Synthesis ([[./figs/htwo_comp_filters.png][png]], [[./figs/htwo_comp_filters.pdf][pdf]]) [[file:figs/htwo_comp_filters.png]] +#+begin_src matlab + PSD_S1 = abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2; + PSD_S2 = abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2; + PSD_H2 = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2; +#+end_src + #+begin_src matlab :exports none figure; hold on; - plot(freqs, abs(squeeze(freqresp(W1, freqs, 'Hz'))).^2, '-', 'DisplayName', '$|W_1|^2$'); - plot(freqs, abs(squeeze(freqresp(W2, freqs, 'Hz'))).^2, '-', 'DisplayName', '$|W_2|^2$'); - plot(freqs, abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))).^2, 'k-', 'DisplayName', '$|W_1H_1|^2+|W_2H_2|^2$'); + 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('Magnitude'); + xlabel('Frequency [Hz]'); ylabel('Power Spectral Density'); hold off; xlim([freqs(1), freqs(end)]); legend('location', 'northeast'); @@ -237,77 +254,76 @@ The optimal sensor fusion has permitted to reduced the RMS value of the estimati #+CAPTION: Power Spectral Density of the estimated $\hat{x}$ using the two sensors alone and using the optimally fused signal ([[./figs/psd_sensors_htwo_synthesis.png][png]], [[./figs/psd_sensors_htwo_synthesis.pdf][pdf]]) [[file:figs/psd_sensors_htwo_synthesis.png]] -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) - data2orgtable([norm([W1], 2);norm([W2], 2);norm([W1*H1 + W2*H2], 2)], {'Sensor 1', 'Sensor 2', 'Optimal Sensor Fusion'}, {'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.1e-02 | -| Sensor 2 | 1.3e-03 | -| Optimal Sensor Fusion | 1.5e-04 | - -** H-Infinity Synthesis -*** First idea -Another objective that we may have is that the noise of the super sensor $n_{SS}$ is following the minimum of the noise of the two sensors $n_1$ and $n_2$: -\[ \Gamma_{n_{ss}}(\omega) = \min(\Gamma_{n_1}(\omega),\ \Gamma_{n_2}(\omega)) \] - -In order to obtain that ideal case, we need that the complementary filters be designed such that: -\begin{align*} - & H_1(j\omega) = 1 \text{ and } H_2(j\omega) = 0 \text{ at frequencies where } \Gamma_{n_1}(\omega) < \Gamma_{n_2}(\omega) \\ - & H_1(j\omega) = 0 \text{ and } H_2(j\omega) = 1 \text{ at frequencies where } \Gamma_{n_1}(\omega) > \Gamma_{n_2}(\omega) -\end{align*} - -Which is indeed *impossible in practice*. - -We could try to use filters with the highest possible order to approximate that. -However, we may have robustness and implementation problems. - #+begin_src matlab - omegac = 100*2*pi; G0 = 1e-5; Ginf = 1e-4; - N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/4000); - - omegac = 1*2*pi; G0 = 1e-3; Ginf = 1e-8; - N2 = ((sqrt(Ginf)*s/omegac + sqrt(G0))/(s/omegac + 1))^2/(1 + s/2/pi/4000)^2; -#+end_src - -#+begin_src matlab - n = 5; w0 = 2*pi*10; G0 = 1/10; G1 = 10000; Gc = 1/2; - W1 = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n; - - n = 5; w0 = 2*pi*8; G0 = 1000; G1 = 0.1; Gc = 1/2; - W2 = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n; + CPS_S1 = 1/pi*cumtrapz(2*pi*freqs, PSD_S1); + CPS_S2 = 1/pi*cumtrapz(2*pi*freqs, PSD_S2); + CPS_H2 = 1/pi*cumtrapz(2*pi*freqs, PSD_H2); #+end_src #+begin_src matlab :exports none figure; hold on; - plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$W_1 = \frac{N_1}{N_2}$'); - plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$W_2 = \frac{N_2}{N_1}$'); - set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - xlabel('Frequency [Hz]'); ylabel('Magnitude'); + 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([freqs(1), freqs(end)]); - legend('location', 'northeast'); + 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/cps_h2_synthesis.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") + <> +#+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]]) +[[file:figs/cps_h2_synthesis.png]] + +** 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]]. + +#+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; + + 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; #+end_src #+begin_src matlab - P = [W1 -W1; - 0 W2; - 1 0]; + 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 - [H2, ~, 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'); #+end_src #+RESULTS: #+begin_example -[H2, ~, 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 @@ -343,7 +359,7 @@ Test bounds: 0.1000 < gamma <= 10500.0000 #+end_example #+begin_src matlab - H1 = 1 - H2; + H1a = 1 - H2a; #+end_src #+begin_src matlab :exports none @@ -352,14 +368,14 @@ Test bounds: 0.1000 < gamma <= 10500.0000 ax1 = subplot(2,1,1); hold on; set(gca,'ColorOrderIndex',1) - plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$w_1$'); + plot(freqs, 1./abs(squeeze(freqresp(W1a, freqs, 'Hz'))), '--', 'DisplayName', '$w_1$'); set(gca,'ColorOrderIndex',2) - plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$w_2$'); + plot(freqs, 1./abs(squeeze(freqresp(W2a, freqs, 'Hz'))), '--', 'DisplayName', '$w_2$'); set(gca,'ColorOrderIndex',1) - plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$'); + plot(freqs, abs(squeeze(freqresp(H1a, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$'); set(gca,'ColorOrderIndex',2) - plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$'); + plot(freqs, abs(squeeze(freqresp(H2a, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); @@ -371,9 +387,9 @@ Test bounds: 0.1000 < gamma <= 10500.0000 ax2 = subplot(2,1,2); hold on; set(gca,'ColorOrderIndex',1) - plot(freqs, 180/pi*phase(squeeze(freqresp(H1, freqs, 'Hz'))), '-'); + plot(freqs, 180/pi*phase(squeeze(freqresp(H1a, freqs, 'Hz'))), '-'); set(gca,'ColorOrderIndex',2) - plot(freqs, 180/pi*phase(squeeze(freqresp(H2, freqs, 'Hz'))), '-'); + plot(freqs, 180/pi*phase(squeeze(freqresp(H2a, freqs, 'Hz'))), '-'); hold off; xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); set(gca, 'XScale', 'log'); @@ -384,135 +400,100 @@ Test bounds: 0.1000 < gamma <= 10500.0000 xticks([0.1, 1, 10, 100, 1000]); #+end_src -#+begin_src matlab :exports none - figure; - hold on; - plot(freqs, abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2, '-', 'DisplayName', '$|N_1|^2$'); - plot(freqs, abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2, '-', 'DisplayName', '$|N_2|^2$'); - plot(freqs, abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2, 'k-', 'DisplayName', '$|N_1H_1|^2+|N_2H_2|^2$'); - set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - xlabel('Frequency [Hz]'); ylabel('Magnitude'); - hold off; - xlim([freqs(1), freqs(end)]); - legend('location', 'northeast'); +#+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 -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) - data2orgtable([norm([N1], 2);norm([N2], 2);norm([N1*H1 + N2*H2], 2)], {'Sensor 1', 'Sensor 2', 'Optimal Sensor Fusion'}, {'rms value'}, ' %.1e'); +#+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 -#+RESULTS: -| | rms value | -|-----------------------+-----------| -| Sensor 1 | 1.1e-02 | -| Sensor 2 | 1.3e-03 | -| Optimal Sensor Fusion | 3.2e-04 | - -*** Second idea +** H-Infinity Synthesis - method B We have that: -\[ \Phi_{n_{ss}} = \left|H_1\right|^2 \Phi_{n_1} + \left|H_2\right|^2 \Phi_{n_2} \] +\[ \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 \] -We model the Power Spectral Densities of the two sensors by weighting functions $N_1(s)$ and $N_2(s)$. -\[ \Phi_{n_{ss}} = \left|H_1 N_1\right|^2 + \left|H_2\right N_2|^2 \] +Then, at frequencies where $|H_1(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$. -Then, at frequencies where $|H_1| < |H_2|$ we would like that $|N_1| = 1$ and $|N_2| = 0$ as we discussed before. -Then $|H_1 N_1|^2 + |H_2 N_2|^2 = |H_1 N_1|^2$. - -However, if we design $H_2$ such that when $|N_1| < |N_2|$, we have that: +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_{n_{ss}} &= \left|H_1 N_1\right|^2 + |H_2 N_2|^2 \\ - &= (1 + \epsilon)\left|H_1 N_1\right|^2 \\ - &\approx \left|H_1 N_1\right|^2 + \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*} -We have then that: -\[ |H_2 N_2| = \sqrt{\epsilon} |H_1 N_1| \] -We suppose that $|H_1| = 1$: -\[ |H_2| = \sqrt{\epsilon} \frac{|N_1|}{|N_2|} \] -We can use that for the maximum magnitude of $|H_2|$ at frequencies where $|N_1| < |N_2|$ - -Similarly, we have that the maximum magnitude of $|H_1|$ at frequencies where $|N_1| > |N_2|$ is: +Similarly, we design $H_1(s)$ such that at frequencies where $|N_1| > |N_2|$: \[ |H_1| = \sqrt{\epsilon} \frac{|N_2|}{|N_1|} \] -We could take $\epsilon = 1$, then we increase the noise of the super sensor just by a factor $\sqrt{2}$ over the all bandwidth from the idea case. +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. -This could be used as weighting functions for the $\mathcal{H}_\infty$ synthesis of the complementary filters. +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 - omegac = 100*2*pi; G0 = 1e-5; Ginf = 1e-4; - N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/4000); + epsilon = 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; + 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 - W1 = N1/N2; - W2 = N2/N1; - - W1 = W1/(1 + s/2/pi/1000); % this is added so that it is proper -#+end_src - -#+begin_src matlab :exports none - figure; - hold on; - plot(freqs, abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$W_1 = \frac{N_1}{N_2}$'); - plot(freqs, abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$W_2 = \frac{N_2}{N_1}$'); - set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - xlabel('Frequency [Hz]'); ylabel('Magnitude'); - hold off; - xlim([freqs(1), freqs(end)]); - legend('location', 'northeast'); -#+end_src - -#+begin_src matlab - P = [W1 -W1; - 0 W2; - 1 0]; + 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 - [H2, ~, 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'); #+end_src #+RESULTS: #+begin_example -[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on'); -Test bounds: 0.0000 < gamma <= 2625.0000 +[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 -2.625e+03 1.8e+01 -2.3e-12 6.3e+00 -1.7e-14 0.0000 p -1.313e+03 1.8e+01 -3.3e-12 6.3e+00 -5.9e-13 0.0000 p - 656.250 1.8e+01 -1.8e-12 6.3e+00 -1.2e-13 0.0000 p - 328.125 1.8e+01 -8.3e-13 6.3e+00 -7.5e-14 0.0000 p - 164.063 1.8e+01 -3.8e-13 6.3e+00 -6.1e-14 0.0000 p - 82.031 1.8e+01 -3.3e-22 6.3e+00 -5.4e-14 0.0000 p - 41.016 1.8e+01 -3.4e-12 6.3e+00 -1.4e-16 0.0000 p - 20.508 1.8e+01 -6.4e-14 6.3e+00 -4.4e-13 0.0000 p - 10.254 1.8e+01 -1.4e-13 6.3e+00 -4.9e-14 0.0000 p - 5.127 1.7e+01 -1.1e-12 6.3e+00 -1.6e-13 0.0000 p - 2.563 1.7e+01 -6.5e-13 6.3e+00 -7.3e-14 0.0000 p - 1.282 1.4e+01 -8.9e+04# 6.3e+00 -1.6e-14 0.0000 f - 1.923 1.6e+01 -4.6e+05# 6.3e+00 -2.1e-13 0.0000 f - 2.243 1.7e+01 -1.9e+06# 6.3e+00 -9.9e-14 0.0000 f - 2.403 1.7e+01 -3.7e-11 6.3e+00 -2.1e-12 0.0000 p - 2.323 1.7e+01 -4.6e+06# 6.3e+00 -5.8e-14 0.0000 f - 2.363 1.7e+01 -1.2e+07# 6.3e+00 -3.3e-14 0.0000 f - 2.383 1.7e+01 -6.8e+07# 6.3e+00 -1.2e-13 0.0000 f - 2.393 1.7e+01 -5.5e-11 6.3e+00 -1.8e-12 0.0000 p - 2.388 1.7e+01 1.3e-22 6.3e+00 -2.5e-15 0.0000 p - 2.386 1.7e+01 -1.5e+08# 6.3e+00 -1.4e-13 0.0000 f - 2.387 1.7e+01 -4.0e+08# 6.3e+00 -1.0e-13 0.0000 f - 2.388 1.7e+01 -2.1e+09# 6.3e+00 -4.2e-13 0.0000 f + 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: 2.3882 + Gamma value achieved: 1.1390 #+end_example #+begin_src matlab - H1 = 1 - H2; + H1b = 1 - H2b; #+end_src #+begin_src matlab :exports none @@ -521,14 +502,14 @@ Test bounds: 0.0000 < gamma <= 2625.0000 ax1 = subplot(2,1,1); hold on; set(gca,'ColorOrderIndex',1) - plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$w_1$'); + plot(freqs, 1./abs(squeeze(freqresp(W1b, freqs, 'Hz'))), '--', 'DisplayName', '$w_1$'); set(gca,'ColorOrderIndex',2) - plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$w_2$'); + plot(freqs, 1./abs(squeeze(freqresp(W2b, freqs, 'Hz'))), '--', 'DisplayName', '$w_2$'); set(gca,'ColorOrderIndex',1) - plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$'); + plot(freqs, abs(squeeze(freqresp(H1b, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$'); set(gca,'ColorOrderIndex',2) - plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$'); + plot(freqs, abs(squeeze(freqresp(H2b, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); @@ -540,9 +521,9 @@ Test bounds: 0.0000 < gamma <= 2625.0000 ax2 = subplot(2,1,2); hold on; set(gca,'ColorOrderIndex',1) - plot(freqs, 180/pi*phase(squeeze(freqresp(H1, freqs, 'Hz'))), '-'); + plot(freqs, 180/pi*phase(squeeze(freqresp(H1b, freqs, 'Hz'))), '-'); set(gca,'ColorOrderIndex',2) - plot(freqs, 180/pi*phase(squeeze(freqresp(H2, freqs, 'Hz'))), '-'); + plot(freqs, 180/pi*phase(squeeze(freqresp(H2b, freqs, 'Hz'))), '-'); hold off; xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); set(gca, 'XScale', 'log'); @@ -553,32 +534,96 @@ Test bounds: 0.0000 < gamma <= 2625.0000 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 + +** 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)], {'Sensor 1', 'Sensor 2', 'H2 Fusion', 'H-Infinity a', 'H-Infinity b'}, {'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 | + + #+begin_src matlab :exports none figure; hold on; - plot(freqs, abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2, '-', 'DisplayName', '$|N_1|^2$'); - plot(freqs, abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2, '-', 'DisplayName', '$|N_2|^2$'); - plot(freqs, abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2, 'k-', 'DisplayName', '$|N_1H_1|^2+|N_2H_2|^2$'); + plot(freqs, PSD_S1, '-', 'DisplayName', '$\Phi_{\hat{x}_1}$'); + plot(freqs, PSD_S2, '-', 'DisplayName', '$\Phi_{\hat{x}_2}$'); + plot(freqs, PSD_H2, 'k-', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_2}}$'); + plot(freqs, PSD_Ha, 'k--', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_\infty},a}$'); + plot(freqs, PSD_Hb, 'k-.', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_\infty},b}$'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - xlabel('Frequency [Hz]'); ylabel('Magnitude'); + xlabel('Frequency [Hz]'); ylabel('Power Spectral Density'); hold off; xlim([freqs(1), freqs(end)]); legend('location', 'northeast'); #+end_src -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) - data2orgtable([norm([N1], 2);norm([N2], 2);norm([N1*H1 + N2*H2], 2)], {'Sensor 1', 'Sensor 2', 'Optimal Sensor Fusion'}, {'rms value'}, ' %.1e'); +#+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 -#+RESULTS: -| | rms value | -|-----------------------+-----------| -| Sensor 1 | 1.1e-02 | -| Sensor 2 | 1.3e-03 | -| Optimal Sensor Fusion | 1.9e-04 | +#+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]] -*** Conclusion -From the above two experiments with $\mathcal{H}_\infty$ it still seems that the $\mathcal{H}_2$ synthesis gives the complementary filters that permits to obtain the minimal super sensor noise (when measuring with the $\mathcal{H}_2$ norm) +#+begin_src matlab :exports none + figure; + hold on; + plot(freqs, CPS_S1, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_1} = %.1e$', sqrt(CPS_S1(end)))); + plot(freqs, CPS_S2, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_2} = %.1e$', sqrt(CPS_S2(end)))); + plot(freqs, CPS_H2, 'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_2}} = %.1e$', sqrt(CPS_H2(end)))); + plot(freqs, CPS_Ha, 'k--', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_\\infty, a}} = %.1e$', sqrt(CPS_Ha(end)))); + plot(freqs, CPS_Hb, 'k-.', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_\\infty, b}} = %.1e$', sqrt(CPS_Hb(end)))); + set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); + xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum'); + hold off; + xlim([2e-1, freqs(end)]); + ylim([1e-10 1e-5]); + legend('location', 'southeast'); +#+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]] + +** 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). * Robustness to sensor dynamics uncertainty :PROPERTIES: diff --git a/tikz/figs/fusion_two_noisy_sensors.pdf b/tikz/figs/fusion_two_noisy_sensors.pdf index 7dc988c..4f69610 100644 Binary files a/tikz/figs/fusion_two_noisy_sensors.pdf and b/tikz/figs/fusion_two_noisy_sensors.pdf differ diff --git a/tikz/figs/fusion_two_noisy_sensors.png b/tikz/figs/fusion_two_noisy_sensors.png index 0a3b91e..eeaaa7f 100644 Binary files a/tikz/figs/fusion_two_noisy_sensors.png and b/tikz/figs/fusion_two_noisy_sensors.png differ diff --git a/tikz/figs/fusion_two_noisy_sensors.svg b/tikz/figs/fusion_two_noisy_sensors.svg index e3f848b..adf2852 100644 --- a/tikz/figs/fusion_two_noisy_sensors.svg +++ b/tikz/figs/fusion_two_noisy_sensors.svg @@ -1,165 +1,264 @@ - + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + - + - + - + + + + - + - + - + - + - + - - - - - - - - - - + - - + - + + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + - + - + - + - - - - - + - - - - - + - + + + + + + + + + + + + + + + + + + + + + + + + - - + - + + + + + + + + + + + + + + - + - + + - + - - + - - - - + + + + + + + + + + - - - - - - - - - - - - + - - - - - - + - - - - - - - - - - - - - - - - - - - - - - - + diff --git a/tikz/figs/fusion_two_noisy_sensors_weights.pdf b/tikz/figs/fusion_two_noisy_sensors_weights.pdf new file mode 100644 index 0000000..520839a Binary files /dev/null and b/tikz/figs/fusion_two_noisy_sensors_weights.pdf differ diff --git a/tikz/figs/fusion_two_noisy_sensors_weights.png b/tikz/figs/fusion_two_noisy_sensors_weights.png new file mode 100644 index 0000000..b252545 Binary files /dev/null and b/tikz/figs/fusion_two_noisy_sensors_weights.png differ diff --git a/tikz/figs/fusion_two_noisy_sensors_weights.svg b/tikz/figs/fusion_two_noisy_sensors_weights.svg new file mode 100644 index 0000000..6c0a30d --- /dev/null +++ b/tikz/figs/fusion_two_noisy_sensors_weights.svg @@ -0,0 +1,330 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tikz/figs/h_infinity_optimal_comp_filters.pdf b/tikz/figs/h_infinity_optimal_comp_filters.pdf index 73416f4..dc65707 100644 Binary files a/tikz/figs/h_infinity_optimal_comp_filters.pdf and b/tikz/figs/h_infinity_optimal_comp_filters.pdf differ diff --git a/tikz/figs/h_infinity_optimal_comp_filters.png b/tikz/figs/h_infinity_optimal_comp_filters.png index 6e4776e..867e9d0 100644 Binary files a/tikz/figs/h_infinity_optimal_comp_filters.png and b/tikz/figs/h_infinity_optimal_comp_filters.png differ diff --git a/tikz/figs/h_infinity_optimal_comp_filters.svg b/tikz/figs/h_infinity_optimal_comp_filters.svg index 898b184..7ed35e9 100644 --- a/tikz/figs/h_infinity_optimal_comp_filters.svg +++ b/tikz/figs/h_infinity_optimal_comp_filters.svg @@ -1,5 +1,5 @@ - + @@ -9,7 +9,7 @@ - + @@ -45,99 +45,108 @@ + + + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - - + + - + - - - - - - + - + - - - - - - - - - + + + + + + + + + + + + + + + + + + + + - + - - - - + + + + - + - + - + diff --git a/tikz/figs/sensor_fusion_noisy_perfect_dyn.pdf b/tikz/figs/sensor_fusion_noisy_perfect_dyn.pdf new file mode 100644 index 0000000..835c00b Binary files /dev/null and b/tikz/figs/sensor_fusion_noisy_perfect_dyn.pdf differ diff --git a/tikz/figs/sensor_fusion_noisy_perfect_dyn.png b/tikz/figs/sensor_fusion_noisy_perfect_dyn.png new file mode 100644 index 0000000..a0444f4 Binary files /dev/null and b/tikz/figs/sensor_fusion_noisy_perfect_dyn.png differ diff --git a/tikz/figs/sensor_fusion_noisy_perfect_dyn.svg b/tikz/figs/sensor_fusion_noisy_perfect_dyn.svg new file mode 100644 index 0000000..d1f35b3 --- /dev/null +++ b/tikz/figs/sensor_fusion_noisy_perfect_dyn.svg @@ -0,0 +1,303 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tikz/index.org b/tikz/index.org index f74ef20..47d703c 100644 --- a/tikz/index.org +++ b/tikz/index.org @@ -108,7 +108,7 @@ Configuration file is accessible [[file:config.org][here]]. #+begin_src latex :file h_infinity_optimal_comp_filters.pdf :tangle figs/h_infinity_optimal_comp_filters.tex <> \begin{tikzpicture} - \node[block={5.0cm}{3.0cm}, dashed] (P) {}; + \node[block={5.0cm}{3.0cm}, fill=black!20!white, dashed] (P) {}; \node[above] at (P.north) {$P$}; \coordinate[] (inputn1) at ($(P.south west)!0.8!(P.north west) + (-\cdist, 0)$); @@ -118,18 +118,18 @@ Configuration file is accessible [[file:config.org][here]]. \coordinate[] (outputx) at ($(P.south east)!0.5!(P.north east) + ( \cdist, 0)$); \coordinate[] (outputv) at ($(P.south east)!0.1!(P.north east) + ( \cdist, 0)$); - \node[block, right=1.5 of inputn1] (W1){$W_1$}; - \node[block, right=1.5 of inputn2] (W2){$W_2$}; - \node[addb={+}{}{}{}{-}, right=of W1] (sub) {}; - \node[addb, right=2 of W2] (add) {}; + \node[block, right=1.5 of inputn1] (N1){$N_1$}; + \node[block, right=1.5 of inputn2] (N2){$N_2$}; + \node[addb={+}{}{}{}{-}, right=of N1] (sub) {}; + \node[addb, right=2 of N2] (add) {}; \node[block, below=of P] (H1) {$H_1$}; - \draw[->] (inputn1)node[above right]{$n_1$} -- (W1.west); - \draw[->] (inputn2)node[above right]{$n_2$} -- (W2.west); - \draw[->] (W1) -- (sub.west); - \draw[->] (W2) -| (sub.south); - \draw[->] (W2-|sub.south) node[branch]{} -- (add.west); + \draw[->] (inputn1)node[above right]{$\tilde{n}_1$} -- (N1.west); + \draw[->] (inputn2)node[above right]{$\tilde{n}_2$} -- (N2.west); + \draw[->] (N1) -- (sub.west); + \draw[->] (N2) -| (sub.south); + \draw[->] (N2-|sub.south) node[branch]{} -- (add.west); \draw[->] (sub.east) -- ++(0.5, 0) |- ($(outputv) + (-0.3, 0)$) |- (H1.east); \draw[->] (H1.west) -| ($(inputu) + (0.3, 0)$) -| (add.south); \draw[->] (add.east) -- (outputx) node[above left]{$\hat{x}$}; @@ -142,28 +142,40 @@ Configuration file is accessible [[file:config.org][here]]. [[file:figs/h_infinity_optimal_comp_filters.png]] * Fusion of two noisy sensors #+begin_src latex :file fusion_two_noisy_sensors.pdf :tangle figs/fusion_two_noisy_sensors.tex + <> + \begin{tikzpicture} \node[branch] (x) at (0, 0); - \node[addb, above right=1.5 and 1 of x](add1){}; - \node[addb, below right=1.5 and 1 of x](add2){}; - \node[block, above=0.5 of add1](W1){$W_1$}; - \node[block, above=0.5 of add2](W2){$W_2$}; - \node[block, right=1 of add1](H1){$H_1$}; - \node[block, right=1 of add2](H2){$H_2$}; - \node[addb, right=4 of x](add){}; + \node[block, above right=0.5 and 0.5 of x](G1){$G_1(s)$}; + \node[block, below right=0.5 and 0.5 of x](G2){$G_2(s)$}; + \node[addb, right=0.8 of G1](add1){}; + \node[addb, right=0.8 of G2](add2){}; + \node[block, right=0.8 of add1](H1){$H_1(s)$}; + \node[block, right=0.8 of add2](H2){$H_2(s)$}; + \node[addb, right=5 of x](add){}; - \draw[] ($(x)+(-1, 0)$) node[above right]{$x$} -- (x); - \draw[->] (x) |- (add1.west); - \draw[->] (x) |- (add2.west); - \draw[->] (W1.south) -- (add1.north); - \draw[->] (W2.south) -- (add2.north); - \draw[<-] (W1.north) -- ++(0, 0.8)node[below right]{$n_1$}; - \draw[<-] (W2.north) -- ++(0, 0.8)node[below right]{$n_2$}; + \draw[] ($(x)+(-0.7, 0)$) node[above right]{$x$} -- (x.center); + \draw[->] (x.center) |- (G1.west); + \draw[->] (x.center) |- (G2.west); + \draw[->] (G1.east) -- (add1.west); + \draw[->] (G2.east) -- (add2.west); + \draw[<-] (add1.north) -- ++(0, 0.8)node[below right](n1){$n_1$}; + \draw[<-] (add2.north) -- ++(0, 0.8)node[below right](n2){$n_2$}; \draw[->] (add1.east) -- (H1.west); \draw[->] (add2.east) -- (H2.west); \draw[->] (H1) -| (add.north); \draw[->] (H2) -| (add.south); - \draw[->] (add.east) -- ++(1, 0) node[above left]{$\hat{x}$}; + \draw[->] (add.east) -- ++(0.7, 0) node[above left]{$\hat{x}$}; + + \begin{scope}[on background layer] + \node[fit={($(G2.south-|x)+(-0.2, -0.3)$) ($(n1.north east-|add.east)+(0.2, 0.3)$)}, fill=black!10!white, draw, dashed, inner sep=0pt] (supersensor) {}; + \node[below left] at (supersensor.north east) {Super Sensor}; + + \node[fit={($(G1.south west)+(-0.3, -0.1)$) ($(n1.north east)+(0.0, 0.1)$)}, fill=black!20!white, draw, dashed, inner sep=0pt] (sensor1) {}; + \node[below right] at (sensor1.north west) {Sensor 1}; + \node[fit={($(G2.south west)+(-0.3, -0.1)$) ($(n2.north east)+(0.0, 0.1)$)}, fill=black!20!white, draw, dashed, inner sep=0pt] (sensor2) {}; + \node[below right] at (sensor2.north west) {Sensor 2}; + \end{scope} \end{tikzpicture} #+end_src @@ -171,6 +183,97 @@ Configuration file is accessible [[file:config.org][here]]. #+caption: Fusion of two noisy sensors ([[./figs/fusion_two_noisy_sensors.png][png]], [[./figs/fusion_two_noisy_sensors.pdf][pdf]], [[./figs/fusion_two_noisy_sensors.tex][tex]]). #+RESULTS: [[file:figs/fusion_two_noisy_sensors.png]] + +* Fusion of two noisy sensors with perfect dynamics +#+begin_src latex :file sensor_fusion_noisy_perfect_dyn.pdf :tangle figs/sensor_fusion_noisy_perfect_dyn.tex + <> + \begin{tikzpicture} + \node[branch] (x) at (0, 0); + \node[addb, above right=1.2 and 1.8 of x](addn1){}; + \node[addb, below right=1.2 and 1.8 of x](addn2){}; + \node[block, above=0.5 of addn1](N1) {$N_1(s)$}; + \node[block, above=0.5 of addn2](N2) {$N_2(s)$}; + \node[block, right=1.2 of addn1](H1){$H_1(s)$}; + \node[block, right=1.2 of addn2](H2){$H_2(s)$}; + \node[addb, right=4.5 of x](add){}; + + \draw[] ($(x)+(-0.7, 0)$) node[above right]{$x$} -- (x.center); + \draw[->] (x.center) |- (addn1.west); + \draw[->] (x.center) |- (addn2.west); + \draw[->] (addn1.east) -- (H1.west)node[above left]{$\hat{x}_1$}; + \draw[->] (addn2.east) -- (H2.west)node[above left]{$\hat{x}_2$}; + \draw[->] ($(N1.north)+(0,0.7)$) node[below right](n1){$\tilde{n}_1$} -- (N1.north); + \draw[->] ($(N2.north)+(0,0.7)$) node[below right](n2){$\tilde{n}_2$} -- (N2.north); + \draw[->] (N1.south) -- (addn1.north)node[above right]{$n_1$}; + \draw[->] (N2.south) -- (addn2.north)node[above right]{$n_2$}; + \draw[->] (H1.east) -| (add.north); + \draw[->] (H2.east) -| (add.south); + \draw[->] (add.east) -- ++(0.7, 0) node[above left]{$\hat{x}$}; + + \begin{scope}[on background layer] + \node[fit={($(addn2.south-|x)+(-0.2, -0.4)$) ($(n1.north-|add.east)+(0.2, 0.3)$)}, fill=black!10!white, draw, dashed, inner sep=0pt] (supersensor) {}; + \node[below left] at (supersensor.north east) {Super Sensor}; + \node[block, fit={($(x|-addn1.south)+(0.2, -0.2)$) ($(n1.north-|N1.east)+(0.2, 0.1)$)}, fill=black!20!white, dashed, inner sep=0pt] (sensor1) {}; + \node[below right] at (sensor1.north west) {Sensor 1}; + \node[block, fit={($(x|-addn2.south)+(0.2, -0.2)$) ($(n2.north-|N2.east)+(0.2, 0.1)$)}, fill=black!20!white, dashed, inner sep=0pt] (sensor2) {}; + \node[below right] at (sensor2.north west) {Sensor 2}; + \end{scope} + \end{tikzpicture} +#+end_src + +#+name: fig:sensor_fusion_noisy_perfect_dyn +#+caption: Sensor fusion architecture with sensor dynamics uncertainty ([[./figs/sensor_fusion_noisy_perfect_dyn.png][png]], [[./figs/sensor_fusion_noisy_perfect_dyn.pdf][pdf]], [[./figs/sensor_fusion_noisy_perfect_dyn.tex][tex]]). +#+RESULTS: +[[file:figs/sensor_fusion_noisy_perfect_dyn.png]] + +* Fusion of two noisy sensors with sensors weights +#+begin_src latex :file fusion_two_noisy_sensors_weights.pdf :tangle figs/fusion_two_noisy_sensors_weights.tex + <> + + \begin{tikzpicture} + \node[branch] (x) at (0, 0); + \node[block, above right=1 and 0.5 of x](G1){$G_1(s)$}; + \node[block, below right=1 and 0.5 of x](G2){$G_2(s)$}; + \node[addb, right=0.8 of G1](add1){}; + \node[addb, right=0.8 of G2](add2){}; + \node[block, above=0.5 of add1](N1){$N_1(s)$}; + \node[block, above=0.5 of add2](N2){$N_2(s)$}; + \node[block, right=0.9 of add1](H1){$H_1(s)$}; + \node[block, right=0.9 of add2](H2){$H_2(s)$}; + \node[addb, right=5 of x](add){}; + + \draw[] ($(x)+(-0.7, 0)$) node[above right]{$x$} -- (x.center); + \draw[->] (x.center) |- (G1.west); + \draw[->] (x.center) |- (G2.west); + \draw[->] (G1.east) -- (add1.west); + \draw[->] (G2.east) -- (add2.west); + \draw[<-] (N1.north) -- ++(0, 0.6)node[below right](n1){$\tilde{n}_1$}; + \draw[<-] (N2.north) -- ++(0, 0.6)node[below right](n2){$\tilde{n}_2$}; + \draw[->] (N1.south) -- (add1.north)node[above right]{$n_1$}; + \draw[->] (N2.south) -- (add2.north)node[above right]{$n_2$}; + \draw[->] (add1.east) -- (H1.west); + \draw[->] (add2.east) -- (H2.west); + \draw[->] (H1) -| (add.north); + \draw[->] (H2) -| (add.south); + \draw[->] (add.east) -- ++(0.7, 0) node[above left]{$\hat{x}$}; + + \begin{scope}[on background layer] + \node[fit={($(G2.south-|x)+(-0.2, -0.3)$) ($(n1.north east-|add.east)+(0.2, 0.3)$)}, fill=black!10!white, draw, dashed, inner sep=0pt] (supersensor) {}; + \node[below left] at (supersensor.north east) {Super Sensor}; + + \node[fit={($(G1.south west)+(-0.3, -0.1)$) ($(n1.north-|N1.east)+(0.2, 0.1)$)}, fill=black!20!white, draw, dashed, inner sep=0pt] (sensor1) {}; + \node[below right] at (sensor1.north west) {Sensor 1}; + \node[fit={($(G2.south west)+(-0.3, -0.1)$) ($(n2.north-|N1.east)+(0.2, 0.1)$)}, fill=black!20!white, draw, dashed, inner sep=0pt] (sensor2) {}; + \node[below right] at (sensor2.north west) {Sensor 2}; + \end{scope} + \end{tikzpicture} +#+end_src + +#+name: fig:fusion_two_noisy_sensors_weights +#+caption: Fusion of two noisy sensors ([[./figs/fusion_two_noisy_sensors_weights.png][png]], [[./figs/fusion_two_noisy_sensors_weights.pdf][pdf]], [[./figs/fusion_two_noisy_sensors_weights.tex][tex]]). +#+RESULTS: +[[file:figs/fusion_two_noisy_sensors_weights.png]] + * Fusion of two noisy signals #+begin_src latex :file fusion_two_signals.pdf :tangle figs/fusion_two_signals.tex \begin{tikzpicture}