UP | HOME

Robust and Optimal Sensor Fusion - Matlab Computation

Table of Contents

In this document, the optimal and robust design of complementary filters is studied.

Two sensors are considered with both different noise characteristics and dynamical uncertainties represented by multiplicative input uncertainty.

1 Sensor Description

In Figure 1 is shown a schematic of a sensor model that is used in the following study. In this example, the measured quantity \(x\) is the velocity of an object.

Table 1: Description of signals in Figure 1
Notation Meaning Unit
\(x\) Physical measured quantity \([m/s]\)
\(\tilde{n}_i\) White noise with unitary PSD  
\(n_i\) Shaped noise \([m/s]\)
\(v_i\) Sensor output measurement \([V]\)
\(\hat{x}_i\) Estimate of \(x\) from the sensor \([m/s]\)
Table 2: Description of Systems in Figure 1
Notation Meaning Unit
\(\hat{G}_i\) Nominal Sensor Dynamics \([\frac{V}{m/s}]\)
\(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 \([m/s]\)

sensor_model_noise_uncertainty.png

Figure 1: Sensor Model

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.15*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 Introduction to Sensor Fusion

2.1 Sensor Fusion Architecture

The two sensors presented in Section 1 are now merged together using complementary filters \(H_1(s)\) and \(H_2(s)\) to form a super sensor (Figure 6).

sensor_fusion_noise_arch.png

Figure 6: Sensor Fusion Architecture

The complementary property of \(H_1(s)\) and \(H_2(s)\) means that the sum of their transfer function is equal to \(1\) \eqref{eq:complementary_property}.

\begin{equation} H_1(s) + H_2(s) = 1 \label{eq:complementary_property} \end{equation}

The super sensor estimate \(\hat{x}\) is given by \eqref{eq:super_sensor_estimate}.

\begin{equation} \hat{x} = \left( H_1 \hat{G}_1^{-1} G_1 + H_2 \hat{G}_2^{-1} G_2 \right) x + \left( H_1 \hat{G}_1^{-1} G_1 N_1 \right) \tilde{n}_1 + \left( H_2 \hat{G}_2^{-1} G_2 N_2 \right) \tilde{n}_2 \label{eq:super_sensor_estimate} \end{equation}

2.2 Super Sensor Noise

If we first suppose that the models of the sensors \(\hat{G}_i\) are very close to the true sensor dynamics \(G_i\) \eqref{eq:good_dynamical_model}, we have that the super sensor estimate \(\hat{x}\) is equals to the measured quantity \(x\) plus the noise of the two sensors filtered out by the complementary filters \eqref{eq:estimate_perfect_models}.

\begin{equation} \hat{G}_i^{-1}(s) G_i(s) \approx 1 \label{eq:good_dynamical_model} \end{equation} \begin{equation} \hat{x} = x + \underbrace{\left( H_1 N_1 \right) \tilde{n}_1 + \left( H_2 N_2 \right) \tilde{n}_2}_{n} \label{eq:estimate_perfect_models} \end{equation}

As the noise of both sensors are considered to be uncorrelated, the PSD of the super sensor noise is computed as follow:

\begin{equation} \Phi_n(\omega) = \left| H_1(j\omega) N_1(j\omega) \right|^2 + \left| H_2(j\omega) N_2(j\omega) \right|^2 \label{eq:super_sensor_psd_noise} \end{equation}

And the Root Mean Square (RMS) value of the super sensor noise \(\sigma_n\) is given by Equation \eqref{eq:super_sensor_rms_noise}.

\begin{equation} \sigma_n = \sqrt{\int_0^\infty \Phi_n(\omega) d\omega} \label{eq:super_sensor_rms_noise} \end{equation}

2.3 Super Sensor Dynamical Uncertainty

If we consider some dynamical uncertainty (the true system dynamics \(G_i\) not being perfectly equal to our model \(\hat{G}_i\)) that we model by the use of multiplicative uncertainty (Figure 7), the super sensor dynamics is then equals to:

\begin{equation} \begin{aligned} \frac{\hat{x}}{x} &= \Big( H_1 \hat{G}_1^{-1} \hat{G}_1 (1 + W_1 \Delta_1) + H_2 \hat{G}_2^{-1} \hat{G}_2 (1 + W_2 \Delta_2) \Big) \\ &= \Big( H_1 (1 + W_1 \Delta_1) + H_2 (1 + W_2 \Delta_2) \Big) \\ &= \left( 1 + H_1 W_1 \Delta_1 + H_2 W_2 \Delta_2 \right), \quad \|\Delta_i\|_\infty<1 \end{aligned} \end{equation}

sensor_model_uncertainty.png

Figure 7: Sensor Model including Dynamical Uncertainty

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)|\) as shown in Figure 8.

uncertainty_set_super_sensor.png

Figure 8: Super Sensor model uncertainty displayed in the complex plane

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

In this section, the complementary filters \(H_1(s)\) and \(H_2(s)\) are designed in order to minimize the RMS value of super sensor noise \(\sigma_n\).

sensor_fusion_noise_arch.png

Figure 9: Optimal Sensor Fusion Architecture

The RMS value of the super sensor noise is (neglecting the model uncertainty):

\begin{equation} \begin{aligned} \sigma_{n} &= \sqrt{\int_0^\infty |H_1(j\omega) N_1(j\omega)|^2 + |H_2(j\omega) N_2(j\omega)|^2 d\omega} \\ &= \left\| \begin{matrix} H_1 N_1 \\ H_2 N_2 \end{matrix} \right\|_2 \end{aligned} \end{equation}

The goal is to design \(H_1(s)\) and \(H_2(s)\) such that \(H_1(s) + H_2(s) = 1\) (complementary property) and such that \(\left\| \begin{matrix} H_1 N_1 \\ H_2 N_2 \end{matrix} \right\|_2\) is minimized (minimized RMS value of the super sensor noise). This is done using the \(\mathcal{H}_2\) synthesis in Section 3.1.

3.1 \(\mathcal{H}_2\) Synthesis

Consider the generalized plant \(P_{\mathcal{H}_2}\) shown in Figure 10 and described by Equation \eqref{eq:H2_generalized_plant}.

h_two_optimal_fusion.png

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

\begin{equation} \label{eq:H2_generalized_plant} \begin{pmatrix} z_1 \\ z_2 \\ v \end{pmatrix} = \underbrace{\begin{bmatrix} N_1 & -N_1 \\ 0 & N_2 \\ 1 & 0 \end{bmatrix}}_{P_{\mathcal{H}_2}} \begin{pmatrix} w \\ u \end{pmatrix} \end{equation}

Applying the \(\mathcal{H}_2\) synthesis on \(P_{\mathcal{H}_2}\) will generate a filter \(H_2(s)\) such that the \(\mathcal{H}_2\) norm from \(w\) to \((z_1,z_2)\) which is actually equals to \(\sigma_n\) by defining \(H_1(s) = 1 - H_2(s)\):

\begin{equation} \left\| \begin{matrix} z_1/w \\ z_2/w \end{matrix} \right\|_2 = \left\| \begin{matrix} N_1 (1 - H_2) \\ N_2 H_2 \end{matrix} \right\|_2 = \sigma_n \quad \text{with} \quad H_1(s) = 1 - H_2(s) \end{equation}

We then have that the \(\mathcal{H}_2\) synthesis applied on \(P_{\mathcal{H}_2}\) generates two complementary filters \(H_1(s)\) and \(H_2(s)\) such that the RMS value of super sensor noise is minimized.

The generalized plant \(P_{\mathcal{H}_2}\) is defined below 

PH2 = [N1 -N1;
       0   N2;
       1   0];

The \(\mathcal{H}_2\) synthesis using the h2syn command 

[H2, ~, gamma] = h2syn(PH2, 1, 1);

Finally, \(H_1(s)\) is defined as follows 

H1 = 1 - H2;

The obtained complementary filters are shown in Figure 11.

htwo_comp_filters.png

Figure 11: Obtained complementary filters using the \(\mathcal{H}_2\) Synthesis

3.2 Super Sensor Noise

The Power Spectral Density of the individual sensors’ noise \(\Phi_{n_1}, \Phi_{n_2}\) and of the super sensor noise \(\Phi_{n_{\mathcal{H}_2}}\) are computed below and shown in Figure 12. 

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;

The corresponding Cumulative Power Spectrum \(\Gamma_{n_1}\), \(\Gamma_{n_2}\) and \(\Gamma_{n_{\mathcal{H}_2}}\) (cumulative integration of the PSD \eqref{eq:CPS_definition}) are computed below and shown in Figure 13. 

CPS_S1 = cumtrapz(freqs, PSD_S1);
CPS_S2 = cumtrapz(freqs, PSD_S2);
CPS_H2 = cumtrapz(freqs, PSD_H2);
\begin{equation} \Gamma_n (\omega) = \int_0^\omega \Phi_n(\nu) d\nu \label{eq:CPS_definition} \end{equation}

The RMS value of the individual sensors and of the super sensor are listed in Table 3.

Table 3: RMS value of the individual sensor noise and of the super sensor using the \(\mathcal{H}_2\) Synthesis
  RMS value \([m/s]\)
\(\sigma_{n_1}\) 0.015
\(\sigma_{n_2}\) 0.08
\(\sigma_{n_{\mathcal{H}_2}}\) 0.0027

psd_sensors_htwo_synthesis.png

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

cps_h2_synthesis.png

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

A time domain simulation is now performed. The measured velocity \(x\) is set to be a sweep sine with an amplitude of \(0.1\ [m/s]\). The velocity estimates from the two sensors and from the super sensors are shown in Figure 14. The resulting noises are displayed in Figure 15.

super_sensor_time_domain_h2.png

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

sensor_noise_H2_time_domain.png

3.3 Discrepancy between sensor dynamics and model

If we consider sensor dynamical uncertainty as explained in Section 1.2, we can compute what would be the super sensor dynamical uncertainty when using the complementary filters obtained using the \(\mathcal{H}_2\) Synthesis.

The super sensor dynamical uncertainty is shown in Figure 16.

It is shown that the phase uncertainty is not bounded between 100Hz and 200Hz. As a result the super sensor signal can not be used for feedback applications about 100Hz.

super_sensor_dynamical_uncertainty_H2.png

Figure 16: Super sensor dynamical uncertainty when using the \(\mathcal{H}_2\) Synthesis

3.4 Conclusion

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

We initially considered perfectly known sensor dynamics so that it can be perfectly inverted.

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

sensor_fusion_arch_uncertainty.png

Figure 17: Sensor fusion architecture with sensor dynamics uncertainty

The objective here is to design complementary filters \(H_1(s)\) and \(H_2(s)\) in order to minimize the dynamical uncertainty of the super sensor.

4.1 Super Sensor Dynamical Uncertainty

In practical systems, the sensor dynamics has always some level of 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 \end{align*}

with \(\Delta_i\) is any transfer function satisfying \(\| \Delta_i \|_\infty < 1\).

We see that as soon as we have some uncertainty in the sensor dynamics, we have that the complementary filters have some effect on the transfer function from \(x\) to \(\hat{x}\).

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

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

uncertainty_gain_phase_variation.png

Figure 18: Maximum phase variation

4.2 Synthesis objective

The uncertainty region of the super sensor dynamics is represented by a circle in the complex plane as shown in Figure 18.

At each frequency \(\omega\), the radius of the circle is \(|W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)|\).

Thus, the phase shift \(\Delta\phi(\omega)\) due to the super sensor uncertainty is bounded by: \[ |\Delta\phi(\omega)| \leq \arcsin\big( |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| \big) \]

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

If \(H_1(s)\) and \(H_2(s)\) are designed such that \[ |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| < \sin\big( \Delta\phi_\text{max}(\omega) \big) \]

The maximum phase shift due to dynamic uncertainty at frequency \(\omega\) will be \(\Delta\phi_\text{max}(\omega)\).

4.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 \]

Then:

\begin{align*} & |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| < \sin\big( \Delta\phi_\text{max}(\omega) \big), \quad \forall\omega \\ \Longleftrightarrow & |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| < |W_\phi(j\omega)|^{-1}, \quad \forall\omega \\ \Longleftrightarrow & \left| W_1(j\omega) H_1(j\omega) W_\phi(j\omega) \right| + \left| W_2(j\omega) H_2(j\omega) W_\phi(j\omega) \right| < 1, \quad \forall\omega \end{align*}

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

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

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

Dphi = 10; % [deg]

Wu = createWeight('n', 2, 'w0', 2*pi*4e2, 'G0', 1/sin(Dphi*pi/180), 'G1', 1/4, 'Gc', 1);

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

upper_bounds_comp_filter_max_phase_uncertainty.png

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

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

The \(\mathcal{H}_\infty\) synthesis architecture used for the complementary filters is shown in Figure 20.

h_infinity_robust_fusion.png

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

The generalized plant is defined below. 

P = [Wu*W1 -Wu*W1;
     0      Wu*W2;
     1      0];

And we do the \(\mathcal{H}_\infty\) synthesis using the hinfsyn command. 

[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');

[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');

  Test bounds:  0.7071 <=  gamma  <=  1.291

    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

  Best performance (actual): 0.7071

And \(H_1(s)\) is defined as the complementary of \(H_2(s)\). 

H1 = 1 - H2;

The obtained complementary filters are shown in Figure 21.

comp_filter_hinf_uncertainty.png

Figure 21: Obtained complementary filters

4.6 Super sensor uncertainty

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

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.

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

The PSD of both sensor and of the super sensor is shown in Figure 22. The CPS of both sensor and of the super sensor is shown in Figure 23. 

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

psd_sensors_hinf_synthesis.png

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

cps_sensors_hinf_synthesis.png

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

4.8 Conclusion

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

However, the RMS of the super sensor noise is not optimized as it was the case with the \(\mathcal{H}_2\) synthesis

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

sensor_fusion_arch_full.png

Figure 24: Sensor fusion architecture with sensor dynamics uncertainty

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

The goal is to design complementary filters such that:

  • the maximum uncertainty of the super sensor is bounded
  • the RMS value of the super sensor noise is minimized

To do so, we can use the Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis.

The Matlab function for that is h2hinfsyn (doc).

5.2 Noise characteristics and Uncertainty of the individual sensors

Both dynamical uncertainty and noise characteristics of the individual sensors are shown in Figure 25.

mixed_synthesis_noise_uncertainty_sensors.png

Figure 25: Noise characteristsics and Dynamical uncertainty of the individual sensors

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

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

The synthesis architecture that is used here is shown in Figure 26.

The controller \(K\) is synthesized such that it:

  • Keeps the \(\mathcal{H}_\infty\) norm \(G\) of the transfer function from \(w\) to \(z_\infty\) bellow some specified value
  • Keeps the \(\mathcal{H}_2\) norm \(H\) of the transfer function from \(w\) to \(z_2\) bellow some specified value
  • Minimizes a trade-off criterion of the form \(W_1 G^2 + W_2 H^2\) where \(W_1\) and \(W_2\) are specified values

mixed_h2_hinf_synthesis.png

Figure 26: Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis

Here, we define \(P\) such that:

\begin{align*} \left\| \frac{z_\infty}{w} \right\|_\infty &= \left\| \begin{matrix}W_1(s) H_1(s) \\ W_2(s) H_2(s)\end{matrix} \right\|_\infty \\ \left\| \frac{z_2}{w} \right\|_2 &= \left\| \begin{matrix}N_1(s) H_1(s) \\ N_2(s) H_2(s)\end{matrix} \right\|_2 \end{align*}

Then:

  • we specify the maximum value for the \(\mathcal{H}_\infty\) norm between \(w\) and \(z_\infty\) to be \(1\)
  • we don’t specify any maximum value for the \(\mathcal{H}_2\) norm between \(w\) and \(z_2\)
  • we choose \(W_1 = 0\) and \(W_2 = 1\) such that the objective is to minimize the \(\mathcal{H}_2\) norm between \(w\) and \(z_2\)

The synthesis objective is to have: \[ \left\| \frac{z_\infty}{w} \right\|_\infty = \left\| \begin{matrix}W_1(s) H_1(s) \\ W_2(s) H_2(s)\end{matrix} \right\|_\infty < 1 \] and to minimize: \[ \left\| \frac{z_2}{w} \right\|_2 = \left\| \begin{matrix}N_1(s) H_1(s) \\ N_2(s) H_2(s)\end{matrix} \right\|_2 \] which is what we wanted.

We define the generalized plant that will be used for the mixed synthesis. 

W1u = ss(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;
     W1n -W1n;
     0    W2n;
     1    0];

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

Nmeas = 1; Ncon = 1; Nz2 = 2;

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

H2 = 1 - H1;

The obtained complementary filters are shown in Figure 27.

comp_filters_mixed_synthesis.png

Figure 27: Obtained complementary filters after mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis

5.5 Obtained Super Sensor’s noise

The PSD and CPS of the super sensor’s noise are shown in Figure 28 and Figure 29 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 28: Power Spectral Density of the Super Sensor obtained with the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis

cps_super_sensor_mixed_syn.png

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

5.6 Obtained Super Sensor’s Uncertainty

The uncertainty on the super sensor’s dynamics is shown in Figure

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

5.8 Conclusion

This synthesis methods allows both to:

  • limit the dynamical uncertainty of the super sensor
  • minimize the RMS value of the estimation

6 Matlab Functions

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

6.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, mustBeNonnegative} = 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

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

ref.bib

Author: Thomas Dehaeze

Created: 2020-10-01 jeu. 15:12