diff --git a/matlab/figs/sensors_noise.pdf b/matlab/figs/sensors_noise.pdf new file mode 100644 index 0000000..03657f2 Binary files /dev/null and b/matlab/figs/sensors_noise.pdf differ diff --git a/matlab/figs/sensors_noise.png b/matlab/figs/sensors_noise.png new file mode 100644 index 0000000..27c2a50 Binary files /dev/null and b/matlab/figs/sensors_noise.png differ diff --git a/matlab/figs/sensors_nominal_dynamics.pdf b/matlab/figs/sensors_nominal_dynamics.pdf new file mode 100644 index 0000000..813f8ac Binary files /dev/null and b/matlab/figs/sensors_nominal_dynamics.pdf differ diff --git a/matlab/figs/sensors_nominal_dynamics.png b/matlab/figs/sensors_nominal_dynamics.png new file mode 100644 index 0000000..6ffc9cf Binary files /dev/null and b/matlab/figs/sensors_nominal_dynamics.png differ diff --git a/matlab/figs/sensors_nominal_dynamics_and_uncertainty.pdf b/matlab/figs/sensors_nominal_dynamics_and_uncertainty.pdf new file mode 100644 index 0000000..bea4815 Binary files /dev/null and b/matlab/figs/sensors_nominal_dynamics_and_uncertainty.pdf differ diff --git a/matlab/figs/sensors_nominal_dynamics_and_uncertainty.png b/matlab/figs/sensors_nominal_dynamics_and_uncertainty.png new file mode 100644 index 0000000..67fc366 Binary files /dev/null and b/matlab/figs/sensors_nominal_dynamics_and_uncertainty.png differ diff --git a/matlab/figs/sensors_uncertainty_weights.pdf b/matlab/figs/sensors_uncertainty_weights.pdf new file mode 100644 index 0000000..c6ef075 Binary files /dev/null and b/matlab/figs/sensors_uncertainty_weights.pdf differ diff --git a/matlab/figs/sensors_uncertainty_weights.png b/matlab/figs/sensors_uncertainty_weights.png new file mode 100644 index 0000000..aea155b Binary files /dev/null and b/matlab/figs/sensors_uncertainty_weights.png differ diff --git a/matlab/index.html b/matlab/index.html index ac3c157..a0fd56a 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 @@ -34,38 +34,52 @@

Table of Contents

@@ -81,16 +95,352 @@ Two sensors are considered with both different noise characteristics and dynamic

-
-

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

+
+

1 Sensor Description

- +In Figure 1 is shown a schematic of a sensor model that is used in the following study. +

+ + + + +++ ++ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Table 1: Description of signals in Figure 1
NotationMeaning
\(x\)Physical measured quantity
\(\tilde{n}_i\)White noise with unitary PSD
\(n_i\)Shaped noise
\(v_i\)Sensor output measurement
\(\hat{x}_i\)Estimate of \(x\) from the sensor
+ + + + +++ ++ + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Table 2: Description of Systems in Figure 1
NotationMeaning
\(\hat{G}_i\)Nominal Sensor Dynamics
\(W_i\)Weight representing the size of the uncertainty at each frequency
\(\Delta_i\)Any complex perturbation such that \(\vert\vert\Delta_i\vert\vert_\infty < 1\)
\(N_i\)Weight representing the sensor noise
+ + +
+

sensor_model_noise_uncertainty.png +

+

Figure 1: Sensor Model

+
+ +

+In this example, the measured quantity \(x\) is the velocity of an object. +The units of signals are listed in Table 3. +The units of systems are listed in Table 4. +

+ + + + +++ ++ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Table 3: Units of signals in Figure 1
NotationUnit
\(x\)\([m/s]\)
\(\tilde{n}_i\) 
\(n_i\)\([m/s]\)
\(v_i\)\([V]\)
\(\hat{x}_i\)\([m/s]\)
+ + + + + +++ ++ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Table 4: Units of Systems in Figure 1
NotationUnit
\(\hat{G}_i\)\([\frac{V}{m/s}]\)
\(\hat{G}_i^{-1}\)\([\frac{m/s}{V}]\)
\(W_i\) 
\(\Delta_i\) 
\(N_i\)\([m/s]\)
+
+ +
+

1.1 Sensor Dynamics

+
+

+ +Let’s consider two sensors measuring the velocity of an object. +

+ +

+The first sensor is an accelerometer. +Its nominal dynamics \(\hat{G}_1(s)\) is defined below. +

+
+
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]
+
+G1 = -g_acc*m_acc*s/(m_acc*s^2 + c_acc*s + k_acc); % Accelerometer Plant [V/(m/s)]
+
+
+ +

+The second sensor is a displacement sensor, its nominal dynamics \(\hat{G}_2(s)\) is defined below. +

+
+
w_pos = 2*pi*2e3; % Measurement Banwdith [rad/s]
+g_pos = 1e4; % Gain [V/m]
+
+G2 = g_pos/s/(1 + s/w_pos); % Position Sensor Plant [V/(m/s)]
+
+
+ +

+These nominal dynamics are also taken as the model of the sensor dynamics. +The true sensor dynamics has some uncertainty associated to it and described in section 1.2. +

+ +

+Both sensor dynamics in \([\frac{V}{m/s}]\) are shown in Figure 2. +

+ +
+

sensors_nominal_dynamics.png +

+

Figure 2: Sensor nominal dynamics from the velocity of the object to the output voltage

+
+
+
+ +
+

1.2 Sensor Model Uncertainty

+
+

+ +The uncertainty on the sensor dynamics is described by multiplicative uncertainty (Figure 1). +

+ +

+The true sensor dynamics \(G_i(s)\) is then described by \eqref{eq:sensor_dynamics_uncertainty}. +

+ +\begin{equation} + G_i(s) = \hat{G}_i(s) \left( 1 + W_i(s) \Delta_i(s) \right); \quad |\Delta_i(j\omega)| < 1 \forall \omega \label{eq:sensor_dynamics_uncertainty} +\end{equation} + +

+The weights \(W_i(s)\) representing the dynamical uncertainty are defined below and their magnitude is shown in Figure 3. +

+
+
W1 = createWeight('n', 2, 'w0', 2*pi*3,   'G0', 2, 'G1', 0.1,     'Gc', 1) * ...
+     createWeight('n', 2, 'w0', 2*pi*1e3, 'G0', 1, 'G1', 4/0.1, 'Gc', 1/0.1);
+
+W2 = createWeight('n', 2, 'w0', 2*pi*1e2, 'G0', 0.05, 'G1', 4, 'Gc', 1);
+
+
+ +

+The bode plot of the sensors nominal dynamics as well as their defined dynamical spread are shown in Figure 4. +

+ + +
+

sensors_uncertainty_weights.png +

+

Figure 3: Magnitude of the multiplicative uncertainty weights \(|W_i(j\omega)|\)

+
+ + +
+

sensors_nominal_dynamics_and_uncertainty.png +

+

Figure 4: Nominal Sensor Dynamics \(\hat{G}_i\) (solid lines) as well as the spread of the dynamical uncertainty (background color)

+
+
+
+ +
+

1.3 Sensor Noise

+
+

+ +The noise of the sensors \(n_i\) are modelled by shaping a white noise with unitary PSD \(\tilde{n}_i\) \eqref{eq:unitary_noise_psd} with a LTI transfer function \(N_i(s)\) (Figure 1). +

+\begin{equation} + \Phi_{\tilde{n}_i}(\omega) = 1 \label{eq:unitary_noise_psd} +\end{equation} + +

+The Power Spectral Density of the sensor noise \(\Phi_{n_i}(\omega)\) is then computed using \eqref{eq:sensor_noise_shaping} and expressed in \([\frac{(m/s)^2}{Hz}]\). +

+\begin{equation} + \Phi_{n_i}(\omega) = \left| N_i(j\omega) \right|^2 \Phi_{\tilde{n}_i}(\omega) \label{eq:sensor_noise_shaping} +\end{equation} + +

+The weights \(N_1\) and \(N_2\) representing the amplitude spectral density of the sensor noises are defined below and shown in Figure 5. +

+
+
omegac = 0.05*2*pi; G0 = 1e-1; Ginf = 1e-6;
+N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4);
+
+omegac = 1000*2*pi; G0 = 1e-6; Ginf = 1e-3;
+N2 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4);
+
+
+ + +
+

sensors_noise.png +

+

Figure 5: Amplitude spectral density of the sensors \(\sqrt{\Phi_{n_i}(\omega)} = |N_i(j\omega)|\)

+
+
+
+ +
+

1.4 Save Model

+
+

+All the dynamical systems representing the sensors are saved for further use. +

+ +
+
save('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
+
+
+
+
+
+ +
+

2 Optimal Super Sensor Noise: \(\mathcal{H}_2\) Synthesis with Acc and Pos

+
+

+

The idea is to combine sensors that works in different frequency range using complementary filters. @@ -111,106 +461,9 @@ The Matlab scripts is accessible here

-
-

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

- -

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

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

-
- -

-We consider that the two sensor dynamics \(G_1(s)\) and \(G_2(s)\) are ideal: -

-\begin{equation} -\label{org024c8f5} - G_1(s) = G_2(s) = 1 -\end{equation} - -

-We obtain the architecture of figure 2. -

- - -
-

sensor_fusion_noisy_perfect_dyn.png -

-

Figure 2: Fusion of two sensors with ideal dynamics

-
- -

-\(H_1(s)\) and \(H_2(s)\) are complementary filters: -

-\begin{equation} -\label{orga71be68} - 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: -\[ \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}\): -

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

1.2 Noise of the sensors

-
-

-Let’s define the noise characteristics of the two sensors by choosing \(N_1\) and \(N_2\): -

-
    -
  • Sensor 1 characterized by \(N_1(s)\) has low noise at low frequency (for instance a geophone)
    • -
  • Sensor 2 characterized by \(N_2(s)\) has low noise at high frequency (for instance an accelerometer)
    • -
- -
-
omegac = 100*2*pi; G0 = 1e-5; Ginf = 1e-4;
-N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/100);
-
-omegac = 1*2*pi; G0 = 1e-3; Ginf = 1e-8;
-N2 = ((sqrt(Ginf)*s/omegac + sqrt(G0))/(s/omegac + 1))^2/(1 + s/2/pi/4000)^2;
-
-
- - -
-

noise_characteristics_sensors.png -

-

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

-
-
-
- -
-

1.3 H-Two Synthesis

-
+
+

2.1 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 \] @@ -222,14 +475,14 @@ 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 6.

-
+

h_infinity_optimal_comp_filters.png

-

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

+

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

\begin{equation*} @@ -239,7 +492,7 @@ We use the generalized plant architecture shown on figure 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*} @@ -255,11 +508,12 @@ 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 6.

-
P = [0   N2  1;
-     N1 -N2  0];
+
P = [N1 -N1;
+     0   N2;
+     1   0];
@@ -267,7 +521,7 @@ We define the generalized plant \(P\) on matlab as shown on figure -
[H1, ~, gamma] = h2syn(P, 1, 1);
+
[H2, ~, gamma] = h2syn(P, 1, 1);
@@ -275,118 +529,142 @@ 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;
+
H1 = 1 - H2;

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

+ +
+

htwo_comp_filters.png +

+

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

+
+
+
+ +
+

2.2 Sensor Noise

+
+

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

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

The obtained RMS value of the super sensor is lower than the RMS value of the individual sensors.

- -
-

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;
+
+CPS_S1 = cumtrapz(freqs, PSD_S1);
+CPS_S2 = cumtrapz(freqs, PSD_S2);
+CPS_H2 = cumtrapz(freqs, PSD_H2);
-
+

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

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

-
+

cps_h2_synthesis.png

-

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

+

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

+ + + + +++ ++ + + + + + + + + + + + + + + + + + + + + + + +
 RMS [m/s]
Integrated Acceleration0.005
Derived Position0.08
Super Sensor - \(\mathcal{H}_2\)0.0012
-
-

1.4 Obtained Super Sensor’s noise uncertainty

-
+
+

2.3 Time Domain Simulation

+

-We would like to verify if the obtained sensor fusion architecture is robust to change in the sensor dynamics. +Parameters of the time domain simulation.

- -

-To study the dynamical uncertainty on the super sensor, we defined some multiplicative uncertainty on both sensor dynamics. -Two weights \(w_1(s)\) and \(w_2(s)\) are used to described the amplitude of the dynamical uncertainty. -

-
-
omegac = 100*2*pi; G0 = 0.1; Ginf = 10;
-w1 = (Ginf*s/omegac + G0)/(s/omegac + 1);
+
Fs = 1e4; % Sampling Frequency [Hz]
+Ts = 1/Fs; % Sampling Time [s]
 
-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);
+t = 0:Ts:2; % Time Vector [s]

-The sensor uncertain models are defined below. +Time domain velocity.

-
G1 = 1 + w1*ultidyn('Delta',[1 1]);
-G2 = 1 + w2*ultidyn('Delta',[1 1]);
+
v = 0.1*sin((10*t).*t)';

-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. 8. -Right Half Plane zero might be introduced in the super sensor dynamics which will render the feedback system unstable. +Generate noises in velocity corresponding to sensor 1 and 2:

-
-
Gss = G1*H1 + G2*H2;
+
n1 = lsim(N1, sqrt(Fs/2)*randn(length(t), 1), t);
+n2 = lsim(N2, sqrt(Fs/2)*randn(length(t), 1), t);
-
-

uncertainty_super_sensor_H2_syn.png +

+

super_sensor_time_domain_h2.png

-

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

+

Figure 10: Noise of individual sensors and noise of the super sensor

-
-

1.5 Conclusion

-
+
+

2.4 Discrepancy between sensor dynamics and model

+
+
+

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

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

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

-
+
+

3 Robust Sensor Fusion: \(\mathcal{H}_\infty\) Synthesis with Acc and Pos

+

- +

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

-
+

sensor_fusion_dynamic_uncertainty.png

-

Figure 9: Sensor fusion architecture with sensor dynamics uncertainty

+

Figure 11: Sensor fusion architecture with sensor dynamics uncertainty

@@ -431,27 +709,27 @@ The Matlab scripts is accessible here<

-
-

2.1 Super Sensor Dynamical Uncertainty

-
+
+

3.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 9. +Let’s represent that with multiplicative input uncertainty as shown on figure 11.

-
+

sensor_fusion_dynamic_uncertainty.png

-

Figure 10: Fusion of two sensors with input multiplicative uncertainty

+

Figure 12: Fusion of two sensors with input multiplicative uncertainty

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\). @@ -462,81 +740,37 @@ 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 11). +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 13).

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) \]

-
+

uncertainty_gain_phase_variation.png

-

Figure 11: Maximum phase variation

+

Figure 13: Maximum phase variation

-
-

2.2 Dynamical uncertainty of the individual sensors

-
+
+

3.2 Synthesis objective

+

-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}\)
    • -
- -

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

- -
-
omegac = 100*2*pi; G0 = 0.1; Ginf = 10;
-w1 = (Ginf*s/omegac + G0)/(s/omegac + 1);
-
-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. 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]);
-
-
- - -
-

uncertainty_dynamics_sensors.png -

-

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

-
-
-
- -
-

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. 11. +The uncertainty region of the super sensor dynamics is represented by a circle in the complex plane as shown in Fig. 13.

-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) \]

@@ -547,7 +781,7 @@ Let’s define some allowed frequency depend phase shift \(\Delta\phi_\text{

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) \]

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

-
-

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

-
+
+

3.3 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}\)):

\begin{equation} -\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 +\label{org829e35c} + \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\).

-
-

2.5 Weighting Function used to bound the super sensor uncertainty

-
+
+

3.4 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. 13 and the corresponding maximum allowed phase uncertainty of the super sensor dynamics of shown in Fig. 14. +Let’s define \(W_\phi(s)\) in order to bound the maximum allowed phase uncertainty \(\Delta\phi_\text{max}\) of the super sensor dynamics.

-
Dphi = 20; % [deg]
+
Dphi = 10; % [deg]
 
-n = 4; w0 = 2*pi*900; G0 = 1/sin(Dphi*pi/180); Ginf = 1/100; Gc = 1;
-wphi = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/Ginf)^(2/n)))*s + (G0/Gc)^(1/n))/((1/Ginf)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/Ginf)^(2/n)))*s + (1/Gc)^(1/n)))^n;
-
-W1 = w1*wphi;
-W2 = w2*wphi;
+Wu = createWeight('n', 2, 'w0', 2*pi*4e2, 'G0', 1/sin(Dphi*pi/180), 'G1', 1/4, 'Gc', 1);
- -
-

magnitude_wphi.png -

-

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

+
+
save('./mat/Wu.mat', 'Wu');
+

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

-
+

upper_bounds_comp_filter_max_phase_uncertainty.png

-

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)

+

Figure 14: 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

-
+
+

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

+

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

-
+

h_infinity_robust_fusion.png

-

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

+

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

The generalized plant is defined below.

-
P = [W1 -W1;
-     0   W2;
-     1   0];
+
P = [Wu*W1 -Wu*W1;
+     0       Wu*W2;
+     1       0];
@@ -672,25 +892,22 @@ And we do the \(\mathcal{H}_\infty\) synthesis using the hinfsyn co 
 [H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
-Resetting value of Gamma min based on D_11, D_12, D_21 terms
 
-Test bounds:      0.0447 <  gamma  <=      1.3318
+  Test bounds:  0.7071 <=  gamma  <=  1.291
 
-  gamma    hamx_eig  xinf_eig  hamy_eig   yinf_eig   nrho_xy   p/f
-    1.332   1.3e+01 -1.0e-14   1.3e+00   -2.6e-18    0.0000    p
-    0.688   1.3e-11# ********   1.3e+00   -6.7e-15  ********    f
-    1.010   1.1e+01 -1.5e-14   1.3e+00   -2.5e-14    0.0000    p
-    0.849   6.9e-11# ********   1.3e+00   -2.3e-14  ********    f
-    0.930   5.2e-12# ********   1.3e+00   -6.1e-18  ********    f
-    0.970   5.6e-11# ********   1.3e+00   -2.3e-14  ********    f
-    0.990   5.0e-11# ********   1.3e+00   -1.7e-17  ********    f
-    1.000   2.1e-10# ********   1.3e+00    0.0e+00  ********    f
-    1.005   1.9e-10# ********   1.3e+00   -3.7e-14  ********    f
-    1.008   1.1e+01 -9.1e-15   1.3e+00    0.0e+00    0.0000    p
-    1.006   1.2e-09# ********   1.3e+00   -6.9e-16  ********    f
-    1.007   1.1e+01 -4.6e-15   1.3e+00   -1.8e-16    0.0000    p
+    gamma        X>=0        Y>=0       rho(XY)<1    p/f
+  9.554e-01     0.0e+00     0.0e+00     3.529e-16     p
+  8.219e-01     0.0e+00     0.0e+00     5.204e-16     p
+  7.624e-01     3.8e-17     0.0e+00     1.955e-15     p
+  7.342e-01     0.0e+00     0.0e+00     5.612e-16     p
+  7.205e-01     0.0e+00     0.0e+00     7.184e-16     p
+  7.138e-01     0.0e+00     0.0e+00     0.000e+00     p
+  7.104e-01     4.1e-16     0.0e+00     6.749e-15     p
+  7.088e-01     0.0e+00     0.0e+00     2.794e-15     p
+  7.079e-01     0.0e+00     0.0e+00     6.503e-16     p
+  7.075e-01     0.0e+00     0.0e+00     4.302e-15     p
 
- Gamma value achieved:     1.0069
+  Best performance (actual): 0.7071

@@ -702,88 +919,81 @@ And \(H_1(s)\) is defined as the complementary of \(H_2(s)\).

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

-
+

comp_filter_hinf_uncertainty.png

-

Figure 17: Obtained complementary filters (png, pdf)

+

Figure 16: Obtained complementary filters (png, pdf)

-
-

2.7 Super sensor uncertainty

-
-

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

- +
+

3.6 Super sensor uncertainty

+
-
Gss = G1*H1 + G2*H2;
+
H2_filters = load('./mat/H2_filters.mat', 'H2', 'H1');
- -
-

super_sensor_uncertainty_bode_plot.png -

-

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

-
-

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.

-
-

2.8 Super sensor noise

-
+
+

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

-
-
omegac = 100*2*pi; G0 = 1e-5; Ginf = 1e-4;
-N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/100);
+

+The PSD of both sensor and of the super sensor is shown in Fig. 17.
+The CPS of both sensor and of the super sensor is shown in Fig. 18.
+

 
-omegac = 1*2*pi; G0 = 1e-3; Ginf = 1e-8;
-N2 = ((sqrt(Ginf)*s/omegac + sqrt(G0))/(s/omegac + 1))^2/(1 + s/2/pi/4000)^2;
+

+
PSD_S2   = abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2;
+PSD_S1   = abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2;
+PSD_Hinf = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
+PSD_H2   = abs(squeeze(freqresp(N1*H2_filters.H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2_filters.H2, freqs, 'Hz'))).^2;
+
+CPS_S2   = cumtrapz(freqs, PSD_S2);
+CPS_S1   = cumtrapz(freqs, PSD_S1);
+CPS_Hinf = cumtrapz(freqs, PSD_Hinf);
+CPS_H2   = cumtrapz(freqs, PSD_H2);
 

 

 
-

-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 19: Power Spectral Density of the obtained super sensor using the \(\mathcal{H}_\infty\) synthesis (png, pdf)

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

 

 
 
-

+

 
cps_sensors_hinf_synthesis.png
 

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

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

 

 

 

 
-

-
2.9 Conclusion

-

+

+
3.8 Conclusion

+

 

 Using the \(\mathcal{H}_\infty\) synthesis, the dynamical uncertainty of the super sensor can be bounded to acceptable values.
 

@@ -795,11 +1005,11 @@ However, the RMS of the super sensor noise is not optimized as it was the case w
 

 

 
-

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

-

+

+
4 Optimal and Robust Sensor Fusion: Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis with Acc and Pos

+

 

-
+
 

 

 

@@ -808,9 +1018,9 @@ The Matlab scripts is accessible 
-
3.1 Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis - Introduction

-

+

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

+

 

 The goal is to design complementary filters such that:
 

@@ -829,88 +1039,37 @@ The Matlab function for that is h2hinfsyn (
-
3.2 Noise characteristics and Uncertainty of the individual sensors

-

+

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

-

-
-

-Both dynamical uncertainty and noise characteristics of the individual sensors are shown in Fig. 21.
+Both dynamical uncertainty and noise characteristics of the individual sensors are shown in Fig. 19.
 

 
 
-

+

 
mixed_synthesis_noise_uncertainty_sensors.png
 

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

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

 

 

 

 
-

-
3.3 Weighting Functions on the uncertainty of the super sensor

-

+

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

-

-
-
-

-
mixed_syn_hinf_weight.png
-

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

+

 

 
+

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

+

 

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

-
-
-

-
mixed_syn_objective_hinf.png
-

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

-

-

-

-
-

-
3.4 Mixed Synthesis Architecture

-

-

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

 
 

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

+

 
mixed_h2_hinf_synthesis.png
 

-
Figure 24: Mixed H2/H-Infinity Synthesis

+
Figure 20: Mixed H2/H-Infinity Synthesis

 

 
 

@@ -958,8 +1117,8 @@ 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(W2*Wu); W2u = ss(W1*Wu); % Weight on the uncertainty
+W1n = ss(N2); W2n = ss(N1); % Weight on the noise
 
 P = [W1u -W1u;
      0    W2u;
@@ -968,80 +1127,134 @@ P = [W1u -W1u;
      1    0];
 

 

-

-

 
-

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

-

 

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

 

 
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');
+[H1, ~, normz, ~] = h2hinfsyn(P, Nmeas, Ncon, Nz2, [0, 1], 'HINFMAX', 1, 'H2MAX', Inf, 'DKMAX', 100, 'TOL', 0.01, 'DISPLAY', 'on');
 
-H1 = 1 - H2;
+H2 = 1 - H1;
 

 

 
 

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

 
 
-

+

 
comp_filters_mixed_synthesis.png
 

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

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

 

 

 

 
-

-
3.6 Obtained Super Sensor’s noise

-

+

+
4.5 Obtained Super Sensor’s noise

+

 

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

 
+

+
PSD_S2     = abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2;
+PSD_S1     = abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2;
+PSD_H2Hinf = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
 
-

+CPS_S2     = cumtrapz(freqs, PSD_S2);
+CPS_S1     = cumtrapz(freqs, PSD_S1);
+CPS_H2Hinf = cumtrapz(freqs, PSD_H2Hinf);
+

+

+
+
+

 
psd_super_sensor_mixed_syn.png
 

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

+
Figure 22: 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 27: Cumulative Power Spectrum of the Super Sensor obtained with the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis (png, pdf)

+
Figure 23: 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

-

+

+
4.6 Obtained Super Sensor’s Uncertainty

+

 

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

-
-
-

-
super_sensor_dyn_uncertainty_mixed_syn.png
-

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

-

 

 

 
-

-
3.8 Conclusion

-

+

+
4.7 Comparison Hinf H2 H2/Hinf

+

+

+
H2_filters      = load('./mat/H2_filters.mat',      'H2', 'H1');
+Hinf_filters    = load('./mat/Hinf_filters.mat',    'H2', 'H1');
+H2_Hinf_filters = load('./mat/H2_Hinf_filters.mat', 'H2', 'H1');
+

+

+
+

+
PSD_H2 = abs(squeeze(freqresp(N2*H2_filters.H2, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N1*H2_filters.H1, freqs, 'Hz'))).^2;
+CPS_H2 = cumtrapz(freqs, PSD_H2);
+
+PSD_Hinf = abs(squeeze(freqresp(N2*Hinf_filters.H2, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N1*Hinf_filters.H1, freqs, 'Hz'))).^2;
+CPS_Hinf = cumtrapz(freqs, PSD_Hinf);
+
+PSD_H2Hinf = abs(squeeze(freqresp(N2*H2_Hinf_filters.H2, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N1*H2_Hinf_filters.H1, freqs, 'Hz'))).^2;
+CPS_H2Hinf = cumtrapz(freqs, PSD_H2Hinf);
+

+

+
+
+
+
+++
++
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+




 RMS [m/s]
Optimal: \(\mathcal{H}_2\)0.0012
Robust: \(\mathcal{H}_\infty\)0.041
Mixed: \(\mathcal{H}_2/\mathcal{H}_\infty\)0.011

+

+

+
+

+
4.8 Conclusion

+

 

 This synthesis methods allows both to:
 

@@ -1053,6 +1266,182 @@ This synthesis methods allows both to:
 

 

 
+

+
5 Functions

+

+

+

+
5.1 createWeight

+

+

+
+

+
+

+This Matlab function is accessible here.
+

+
+

+
function [W] = createWeight(args)
+% createWeight -
+%
+% Syntax: [in_data] = createWeight(in_data)
+%
+% Inputs:
+%    - n  - Weight Order
+%    - G0 - Low frequency Gain
+%    - G1 - High frequency Gain
+%    - Gc - Gain of W at frequency w0
+%    - w0 - Frequency at which |W(j w0)| = Gc
+%
+% Outputs:
+%    - W - Generated Weight
+
+    arguments
+        args.n  (1,1) double {mustBeInteger, mustBePositive} = 1
+        args.G0 (1,1) double {mustBeNumeric, mustBePositive} = 0.1
+        args.G1 (1,1) double {mustBeNumeric, mustBePositive} = 10
+        args.Gc (1,1) double {mustBeNumeric, mustBePositive} = 1
+        args.w0 (1,1) double {mustBeNumeric, mustBePositive} = 1
+    end
+
+  mustBeBetween(args.G0, args.Gc, args.G1);
+
+  s = tf('s');
+
+  W = (((1/args.w0)*sqrt((1-(args.G0/args.Gc)^(2/args.n))/(1-(args.Gc/args.G1)^(2/args.n)))*s + (args.G0/args.Gc)^(1/args.n))/((1/args.G1)^(1/args.n)*(1/args.w0)*sqrt((1-(args.G0/args.Gc)^(2/args.n))/(1-(args.Gc/args.G1)^(2/args.n)))*s + (1/args.Gc)^(1/args.n)))^args.n;
+
+  end
+
+  % Custom validation function
+  function mustBeBetween(a,b,c)
+      if ~((a > b && b > c) || (c > b && b > a))
+          eid = 'createWeight:inputError';
+          msg = 'Gc should be between G0 and G1.';
+          throwAsCaller(MException(eid,msg))
+      end
+  end
+

+

+

+

+
+

+
5.2 plotMagUncertainty

+

+

+
+

+
+

+This Matlab function is accessible here.
+

+
+

+
function [p] = plotMagUncertainty(W, freqs, args)
+% plotMagUncertainty -
+%
+% Syntax: [p] = plotMagUncertainty(W, freqs, args)
+%
+% Inputs:
+%    - W     - Multiplicative Uncertainty Weight
+%    - freqs - Frequency Vector [Hz]
+%    - args  - Optional Arguments:
+%      - G
+%      - color_i
+%      - opacity
+%
+% Outputs:
+%    - p - Plot Handle
+
+arguments
+    W
+    freqs double {mustBeNumeric, mustBeNonnegative}
+    args.G = tf(1)
+    args.color_i (1,1) double {mustBeInteger, mustBePositive} = 1
+    args.opacity (1,1) double {mustBeNumeric, mustBePositive} = 0.3
+    args.DisplayName char = ''
+end
+
+% Get defaults colors
+colors = get(groot, 'defaultAxesColorOrder');
+
+p = patch([freqs flip(freqs)], ...
+          [abs(squeeze(freqresp(args.G, freqs, 'Hz'))).*(1 + abs(squeeze(freqresp(W, freqs, 'Hz')))); ...
+           flip(abs(squeeze(freqresp(args.G, freqs, 'Hz'))).*max(1 - abs(squeeze(freqresp(W, freqs, 'Hz'))), 1e-6))], 'w', ...
+          'DisplayName', args.DisplayName);
+
+p.FaceColor = colors(args.color_i, :);
+p.EdgeColor = 'none';
+p.FaceAlpha = args.opacity;
+
+end
+

+

+

+

+
+

+
5.3 plotPhaseUncertainty

+

+

+
+

+
+

+This Matlab function is accessible here.
+

+
+

+
function [p] = plotPhaseUncertainty(W, freqs, args)
+% plotPhaseUncertainty -
+%
+% Syntax: [p] = plotPhaseUncertainty(W, freqs, args)
+%
+% Inputs:
+%    - W     - Multiplicative Uncertainty Weight
+%    - freqs - Frequency Vector [Hz]
+%    - args  - Optional Arguments:
+%      - G
+%      - color_i
+%      - opacity
+%
+% Outputs:
+%    - p - Plot Handle
+
+arguments
+    W
+    freqs double {mustBeNumeric, mustBeNonnegative}
+    args.G = tf(1)
+    args.color_i (1,1) double {mustBeInteger, mustBePositive} = 1
+    args.opacity (1,1) double {mustBeNumeric, mustBePositive} = 0.3
+    args.DisplayName char = ''
+end
+
+% Get defaults colors
+colors = get(groot, 'defaultAxesColorOrder');
+
+% Compute Phase Uncertainty
+Dphi = 180/pi*asin(abs(squeeze(freqresp(W, freqs, 'Hz'))));
+Dphi(abs(squeeze(freqresp(W, freqs, 'Hz'))) > 1) = 360;
+
+% Compute Plant Phase
+G_ang = 180/pi*angle(squeeze(freqresp(args.G, freqs, 'Hz')));
+
+p = patch([freqs flip(freqs)], [G_ang+Dphi; flip(G_ang-Dphi)], 'w', ...
+          'DisplayName', args.DisplayName);
+
+p.FaceColor = colors(args.color_i, :);
+p.EdgeColor = 'none';
+p.FaceAlpha = args.opacity;
+
+end
+

+

+

+

+

+
 

 
 

@@ -1060,12 +1449,12 @@ This synthesis methods allows both to:
 
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.

+  
NO_ITEM_DATA:moore19_capac_instr_sensor_fusion_high_bandW_nanop

 

 

 

 
Author: Thomas Dehaeze

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

+
Created: 2020-10-01 jeu. 11:26

 

 
 
diff --git a/matlab/index.org b/matlab/index.org
index 7b192f5..0428afd 100644
--- a/matlab/index.org
+++ b/matlab/index.org
@@ -35,8 +35,56 @@ Two sensors are considered with both different noise characteristics and dynamic
 
 * Sensor Description
 ** Introduction                                                      :ignore:
+In Figure [[fig:sensor_model_noise_uncertainty]] is shown a schematic of a sensor model that is used in the following study.
 
-- [ ] Schematic of one sensor
+#+name: tab:sensor_signals
+#+caption: Description of signals in Figure [[fig:sensor_model_noise_uncertainty]]
+| *Notation*    | *Meaning*                       |
+|---------------+---------------------------------|
+| $x$           | Physical measured quantity      |
+| $\tilde{n}_i$ | White noise with unitary PSD    |
+| $n_i$         | Shaped noise                    |
+| $v_i$         | Sensor output measurement       |
+| $\hat{x}_i$   | Estimate of $x$ from the sensor |
+
+#+name: tab:sensor_dynamical_blocks
+#+caption: Description of Systems in Figure [[fig:sensor_model_noise_uncertainty]]
+| *Notation*  | *Meaning*                                                                    |
+|-------------+------------------------------------------------------------------------------|
+| $\hat{G}_i$ | Nominal Sensor Dynamics                                                      |
+| $W_i$       | Weight representing the size of the uncertainty at each frequency            |
+| $\Delta_i$  | Any complex perturbation such that $\vert\vert\Delta_i\vert\vert_\infty < 1$ |
+| $N_i$       | Weight representing the sensor noise                                         |
+
+#+name: fig:sensor_model_noise_uncertainty
+#+caption: Sensor Model
+#+RESULTS:
+[[file:figs-tikz/sensor_model_noise_uncertainty.png]]
+
+In this example, the measured quantity $x$ is the velocity of an object.
+The units of signals are listed in Table [[tab:signal_units]].
+The units of systems are listed in Table [[tab:dynamical_block_units]].
+
+#+name: tab:signal_units
+#+caption: Units of signals in Figure [[fig:sensor_model_noise_uncertainty]]
+| *Notation*    | *Unit*  |
+|---------------+---------|
+| $x$           | $[m/s]$ |
+| $\tilde{n}_i$ |         |
+| $n_i$         | $[m/s]$ |
+| $v_i$         | $[V]$   |
+| $\hat{x}_i$   | $[m/s]$ |
+
+
+#+name: tab:dynamical_block_units
+#+caption: Units of Systems in Figure [[fig:sensor_model_noise_uncertainty]]
+| *Notation*       | *Unit*            |
+|------------------+-------------------|
+| $\hat{G}_i$      | $[\frac{V}{m/s}]$ |
+| $\hat{G}_i^{-1}$ | $[\frac{m/s}{V}]$ |
+| $W_i$            |                   |
+| $\Delta_i$       |                   |
+| $N_i$            | $[m/s]$           |
 
 ** Matlab Init                                              :noexport:ignore:
 #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
@@ -53,7 +101,11 @@ Two sensors are considered with both different noise characteristics and dynamic
 #+end_src
 
 ** Sensor Dynamics
-Accelerometer:
+<>
+Let's consider two sensors measuring the velocity of an object.
+
+The first sensor is an accelerometer.
+Its nominal dynamics $\hat{G}_1(s)$ is defined below.
 #+begin_src matlab
   m_acc = 0.01; % Inertial Mass [kg]
   c_acc = 5;    % Damping [N/(m/s)]
@@ -63,7 +115,7 @@ Accelerometer:
   G1 = -g_acc*m_acc*s/(m_acc*s^2 + c_acc*s + k_acc); % Accelerometer Plant [V/(m/s)]
 #+end_src
 
-Interferometer/Capacitive Sensor:
+The second sensor is a displacement sensor, its nominal dynamics $\hat{G}_2(s)$ is defined below.
 #+begin_src matlab
   w_pos = 2*pi*2e3; % Measurement Banwdith [rad/s]
   g_pos = 1e4; % Gain [V/m]
@@ -71,15 +123,19 @@ Interferometer/Capacitive Sensor:
   G2 = g_pos/s/(1 + s/w_pos); % Position Sensor Plant [V/(m/s)]
 #+end_src
 
+These nominal dynamics are also taken as the model of the sensor dynamics.
+The true sensor dynamics has some uncertainty associated to it and described in section [[sec:sensor_uncertainty]].
+
+Both sensor dynamics in $[\frac{V}{m/s}]$ are shown in Figure [[fig:sensors_nominal_dynamics]].
 #+begin_src matlab :exports none
   figure;
   % Magnitude
   ax1 = subplot(2,1,1);
   hold on;
-  plot(freqs, abs(squeeze(freqresp(G1, freqs, 'Hz'))), '-', 'DisplayName', '$G_1$');
-  plot(freqs, abs(squeeze(freqresp(G2, freqs, 'Hz'))), '-', 'DisplayName', '$G_2$');
+  plot(freqs, abs(squeeze(freqresp(G1, freqs, 'Hz'))), '-', 'DisplayName', '$G_1(j\omega)$');
+  plot(freqs, abs(squeeze(freqresp(G2, freqs, 'Hz'))), '-', 'DisplayName', '$G_2(j\omega)$');
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  ylabel('Magnitude'); set(gca, 'XTickLabel',[]);
+  ylabel('Magnitude $[\frac{V}{m/s}]$'); set(gca, 'XTickLabel',[]);
   legend('location', 'northeast');
   hold off;
 
@@ -97,46 +153,26 @@ Interferometer/Capacitive Sensor:
   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]
-
-  % G2_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 + H2*inv(G2)*G2_u;
+#+begin_src matlab :tangle no :exports results :results file replace
+  exportFig('figs/sensors_nominal_dynamics.pdf', 'width', 'full', 'height', 'full');
 #+end_src
 
-** Sensor Noise
-Noise in $[m/s/\sqrt{Hz}]$.
-#+begin_src matlab
-  omegac = 0.05*2*pi; G0 = 1e-1; Ginf = 1e-6;
-  N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4);
-
-  omegac = 1000*2*pi; G0 = 1e-6; Ginf = 1e-3;
-  N2 = (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(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('Amplitude Spectral Density $\left[ \frac{m/s}{\sqrt{Hz}} \right]$');
-  hold off;
-  xlim([freqs(1), freqs(end)]);
-  legend('location', 'northeast');
-#+end_src
+#+name: fig:sensors_nominal_dynamics
+#+caption: Sensor nominal dynamics from the velocity of the object to the output voltage
+#+RESULTS:
+[[file:figs/sensors_nominal_dynamics.png]]
 
 ** Sensor Model Uncertainty
-The model uncertainty is described by multiplicative uncertainty.
+<>
+The uncertainty on the sensor dynamics is described by multiplicative uncertainty (Figure [[fig:sensor_model_noise_uncertainty]]).
+
+The true sensor dynamics $G_i(s)$ is then described by eqref:eq:sensor_dynamics_uncertainty.
+
+\begin{equation}
+  G_i(s) = \hat{G}_i(s) \left( 1 + W_i(s) \Delta_i(s) \right); \quad |\Delta_i(j\omega)| < 1 \forall \omega \label{eq:sensor_dynamics_uncertainty}
+\end{equation}
+
+The weights $W_i(s)$ representing the dynamical uncertainty are defined below and their magnitude is shown in Figure [[fig:sensors_uncertainty_weights]].
 #+begin_src matlab
   W1 = createWeight('n', 2, 'w0', 2*pi*3,   'G0', 2, 'G1', 0.1,     'Gc', 1) * ...
        createWeight('n', 2, 'w0', 2*pi*1e3, 'G0', 1, 'G1', 4/0.1, 'Gc', 1/0.1);
@@ -144,33 +180,30 @@ The model uncertainty is described by multiplicative uncertainty.
   W2 = createWeight('n', 2, 'w0', 2*pi*1e2, 'G0', 0.05, 'G1', 4, 'Gc', 1);
 #+end_src
 
+The bode plot of the sensors nominal dynamics as well as their defined dynamical spread are shown in Figure [[fig:sensors_nominal_dynamics_and_uncertainty]].
+
 #+begin_src matlab :exports none
   figure;
-  % Magnitude
-  ax1 = subplot(2,1,1);
   hold on;
-  plotMagUncertainty(W1, freqs, 'color_i', 1);
-  plotMagUncertainty(W2, freqs, 'color_i', 2);
-  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
-  set(gca, 'XTickLabel',[]);
-  ylabel('Magnitude');
-  ylim([1e-2, 1e1]);
+  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)|$');
   hold off;
-
-  % Phase
-  ax2 = subplot(2,1,2);
-  hold on;
-  plotPhaseUncertainty(W1, freqs, 'color_i', 1);
-  plotPhaseUncertainty(W2, freqs, 'color_i', 2);
-  set(gca,'xscale','log');
-  yticks(-180:90:180);
-  ylim([-180 180]);
-  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
-  hold off;
-  linkaxes([ax1,ax2],'x');
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  xlabel('Frequency [Hz]'); ylabel('Magnitude');
+  ylim([0, 5]);
   xlim([freqs(1), freqs(end)]);
+  legend('location', 'northwest');
 #+end_src
 
+#+begin_src matlab :tangle no :exports results :results file replace
+  exportFig('figs/sensors_uncertainty_weights.pdf', 'width', 'wide', 'height', 'normal');
+#+end_src
+
+#+name: fig:sensors_uncertainty_weights
+#+caption: Magnitude of the multiplicative uncertainty weights $|W_i(j\omega)|$
+#+RESULTS:
+[[file:figs/sensors_uncertainty_weights.png]]
+
 #+begin_src matlab :exports none
   figure;
   % Magnitude
@@ -184,8 +217,8 @@ The model uncertainty is described by multiplicative uncertainty.
   plot(freqs, abs(squeeze(freqresp(G2, freqs, 'Hz'))), 'DisplayName', '$\hat{G}_2$');
   set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
   set(gca, 'XTickLabel',[]);
-  ylabel('Magnitude');
-  ylim([1e-2, 1e3]);
+  ylabel('Magnitude $[\frac{V}{m/s}]$');
+  ylim([1e-2, 2e3]);
   legend('location', 'northeast');
   hold off;
 
@@ -207,12 +240,65 @@ The model uncertainty is described by multiplicative uncertainty.
   xlim([freqs(1), freqs(end)]);
 #+end_src
 
+#+begin_src matlab :tangle no :exports results :results file replace
+  exportFig('figs/sensors_nominal_dynamics_and_uncertainty.pdf', 'width', 'full', 'height', 'full');
+#+end_src
+
+#+name: fig:sensors_nominal_dynamics_and_uncertainty
+#+caption: Nominal Sensor Dynamics $\hat{G}_i$ (solid lines) as well as the spread of the dynamical uncertainty (background color)
+#+RESULTS:
+[[file:figs/sensors_nominal_dynamics_and_uncertainty.png]]
+
+** Sensor Noise
+<>
+The noise of the sensors $n_i$ are modelled by shaping a white noise with unitary PSD $\tilde{n}_i$ eqref:eq:unitary_noise_psd with a LTI transfer function $N_i(s)$ (Figure [[fig:sensor_model_noise_uncertainty]]).
+\begin{equation}
+  \Phi_{\tilde{n}_i}(\omega) = 1 \label{eq:unitary_noise_psd}
+\end{equation}
+
+The Power Spectral Density of the sensor noise $\Phi_{n_i}(\omega)$ is then computed using eqref:eq:sensor_noise_shaping and expressed in $[\frac{(m/s)^2}{Hz}]$.
+\begin{equation}
+  \Phi_{n_i}(\omega) = \left| N_i(j\omega) \right|^2 \Phi_{\tilde{n}_i}(\omega) \label{eq:sensor_noise_shaping}
+\end{equation}
+
+The weights $N_1$ and $N_2$ representing the amplitude spectral density of the sensor noises are defined below and shown in Figure [[fig:sensors_noise]].
+#+begin_src matlab
+  omegac = 0.05*2*pi; G0 = 1e-1; Ginf = 1e-6;
+  N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4);
+
+  omegac = 1000*2*pi; G0 = 1e-6; Ginf = 1e-3;
+  N2 = (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(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('Amplitude Spectral Density $\left[ \frac{m/s}{\sqrt{Hz}} \right]$');
+  hold off;
+  xlim([freqs(1), freqs(end)]);
+  legend('location', 'northeast');
+#+end_src
+
+#+begin_src matlab :tangle no :exports results :results file replace
+  exportFig('figs/sensors_noise.pdf', 'width', 'normal', 'height', 'normal');
+#+end_src
+
+#+name: fig:sensors_noise
+#+caption: Amplitude spectral density of the sensors $\sqrt{\Phi_{n_i}(\omega)} = |N_i(j\omega)|$
+#+RESULTS:
+[[file:figs/sensors_noise.png]]
+
 ** Save Model
+All the dynamical systems representing the sensors are saved for further use.
+
 #+begin_src matlab
   save('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
 #+end_src
 
-* First Order Complementary Filters
+* First Order Complementary Filters                                 :noexport:
 ** Matlab Init                                             :noexport:ignore:
 #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
   <>
@@ -683,7 +769,6 @@ 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 = 10; % [deg]
@@ -841,8 +926,6 @@ The obtained complementary filters are shown in Fig. [[fig:comp_filter_hinf_unce
 [[file:figs/comp_filter_hinf_uncertainty.png]]
 
 ** 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
   H2_filters = load('./mat/H2_filters.mat', 'H2', 'H1');
 #+end_src
@@ -1035,7 +1118,7 @@ Both dynamical uncertainty and noise characteristics of the individual sensors a
 
 ** 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.
+The maximum wanted multiplicative uncertainty is shown in Fig. .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.
 
 ** Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis
 The synthesis architecture that is used here is shown in Fig. [[fig:mixed_h2_hinf_synthesis]].
@@ -1199,7 +1282,7 @@ The PSD and CPS of the super sensor's noise are shown in Fig. [[fig:psd_super_se
 [[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]].
+The uncertainty on the super sensor's dynamics is shown in Fig.
 
 #+begin_src matlab :exports none
   Dphi_ss = 180/pi*asin(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))));
@@ -2700,7 +2783,7 @@ The PSD and CPS of the super sensor's noise are shown in Fig. [[fig:psd_super_se
 [[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]].
+The uncertainty on the super sensor's dynamics is shown in Fig.
 
 #+begin_src matlab :exports none
   G1 = 1 + w1*ultidyn('Delta',[1 1]);