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 Comparison with Bibliographic example

1.1 Bendat, J., Optimum filters for independent measurements of two related perturbed messages (1957)

(Bendat 1957)

freqs = logspace(-1, 2, 1000);

Weights to shape the noise of both sensors:

K1 = 100;
K2 = 1;
b = 10;
b1 = b;
b2 = b;

N1 = sqrt(K1)*b1/(b1+s)/(s + 1e-2);
N2 = sqrt(K2)*b2/(b2+s);

bendat57_noise_weights.png

Figure 1: Weights

\(\mathcal{H}_2\) synthesis:

P = [0   N2  1;
     N1 -N2  0];
[H1, ~, gamma] = h2syn(P, 1, 1);
H2 = 1 - H1;

The optimal obtained filter (from the paper) is:

a = sqrt(K2/K1);
G = (a*s + 1 + a*b)/(1 + a*b)/(a*s + 1);

bendat57_optimal_filters.png

Figure 2: Obtain Filters

bendat57_psd_estimation.png

Figure 3: PSD of the individual sensors + super sensor

  RMS
Sensor 1 22.93
Sensor 2 2.37
H2 Synthesis 1.74
Paper 1.74

1.2 Plummer, A. R., Optimal complementary filters and their application in motion measurement (2006)

(Plummer 2006)

Weights

N1 = 24.3e-6*(s + 2*pi*0.1)*(s + 1220)/1220*(1/(1 + s/2/pi/1e4)/(1 + s/2/pi/1e4));
N2 = 0.363/(s + 0.01)*(s + 12.2)/(s + 0.01);

plummer06_noise_weights.png

Figure 4: Weights

\(\mathcal{H}_2\) synthesis:

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

[H1, ~, gamma] = h2syn(P, 1, 1);

H2 = 1 - H1;

The optimal obtained filter (from the paper) is:

G = (0.0908*s + 1)/(5.51e-7*s^3 + 7.47e-4*s^2 + 0.0908*s + 1);

plummer06_optimal_filters.png

Figure 5: Obtain Filters

plummer06_psd_estimation.png

Figure 6: PSD of the individual sensors + super sensor

  RMS
Sensor 1 130.0
Sensor 2 0.00753
H2 Synthesis 0.00091
Paper 0.0107

Parameters of the time domain simulation.

Fs = 2.5e3; % Sampling Frequency [Hz]
Ts = 1/Fs; % Sampling Time [s]

t = 0:Ts:2; % Time Vector [s]

Generate noises in velocity corresponding to sensor 1 and 2:

n1 = lsim(N1, sqrt(Fs/2)*randn(length(t), 1), t);
n2 = lsim(N2, sqrt(Fs/2)*randn(length(t), 1), t);

plummer06_time_domain_signals.png

Figure 7: Time domain signals

1.3 Robert Grover Brown, P. Y. C. H., Introduction to random signals and applied kalman filtering with matlab exercises (2012)

(Robert Grover Brown 2012) Section 8.6

w0 = 1; % [rad/s]
wc = 20*w0; % [rad/s]

k1 = sqrt(200*sqrt(2)*w0^3); % [m]
k2 = sqrt(100*pi/wc); % [m]

N1 = k1/(s^2 + sqrt(2)*s + 1);
N2 = k2/(1 + s/(wc*(2/pi)));

robert12_noise_weights.png

Figure 8: Weights

And we do the \(\mathcal{H}_2\) synthesis using the h2syn command.

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

[H1, ~, gamma] = h2syn(P, 1, 1);

H2 = 1 - H1;

robert12_optimal_filters.png

Figure 9: Obtain Filters

robert12_psd_estimation.png

Figure 10: PSD of the individual sensors + super sensor

We can see that the optimal \(\mathcal{H}_2\) control gives similar results as Kalman filtering.

Method Mean Square Error
Kalman Filter 21.47
Euristic 35.32
Optimal H2 20.96
Method Mean Square Error
Kalman Filter 21.47
Euristic 35.32
Optimal H2 21.40

2 Optimal Sensor Fusion - Minimize the Super Sensor Noise

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

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

The complementary filters have to be designed in order to minimize the effect noise of each sensor on the super sensor noise.

The Matlab scripts is accessible here.

2.1 Architecture

Let’s consider the sensor fusion architecture shown on figure 11 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{org800b631} \Phi_{\tilde{n}_1}(\omega) = \Phi_{\tilde{n}_2}(\omega) = 1 \end{equation}

fusion_two_noisy_sensors_weights.png

Figure 11: Fusion of two sensors

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

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

We obtain the architecture of figure 12.

sensor_fusion_noisy_perfect_dyn.png

Figure 12: Fusion of two sensors with ideal dynamics

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

\begin{equation} \label{org8e22dff} 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{orgeef7d07} \sigma_{\hat{x}} = \sqrt{\int_0^\infty \Phi_{\hat{x}}(\omega) d\omega} \end{equation}

2.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 13: Noise Characteristics of the two sensors (png, pdf)

2.3 H-Two Synthesis

As \(\tilde{n}_1\) and \(\tilde{n}_2\) are normalized white noise: \(\Phi_{\tilde{n}_1}(\omega) = \Phi_{\tilde{n}_2}(\omega) = 1\) and we have: \[ \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 14.

h_infinity_optimal_comp_filters.png

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

\begin{equation*} \begin{pmatrix} z \\ v \end{pmatrix} = \begin{pmatrix} 0 & N_2 & 1 \\ N_1 & -N_2 & 0 \end{pmatrix} \begin{pmatrix} w_1 \\ w_2 \\ u \end{pmatrix} \end{equation*}

The transfer function from \([n_1, n_2]\) to \(\hat{x}\) is: \[ \begin{bmatrix} N_1 H_1 \\ N_2 (1 - H_1) \end{bmatrix} \] If we define \(H_2 = 1 - H_1\), we obtain: \[ \begin{bmatrix} N_1 H_1 \\ N_2 H_2 \end{bmatrix} \]

Thus, if we minimize the \(\mathcal{H}_2\) norm of this transfer function, we minimize the RMS value of \(\hat{x}\).

We define the generalized plant \(P\) on matlab as shown on figure 14.

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

And we do the \(\mathcal{H}_2\) synthesis using the h2syn command.

[H1, ~, gamma] = h2syn(P, 1, 1);

Finally, we define \(H_2(s) = 1 - H_1(s)\).

H2 = 1 - H1;

The complementary filters obtained are shown on figure 15.

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

The Cumulative Power Spectrum (CPS) is shown on Fig. 17.

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

htwo_comp_filters.png

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

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

psd_sensors_htwo_synthesis.png

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

CPS_S1 = 1/pi*cumtrapz(2*pi*freqs, PSD_S1);
CPS_S2 = 1/pi*cumtrapz(2*pi*freqs, PSD_S2);
CPS_H2 = 1/pi*cumtrapz(2*pi*freqs, PSD_H2);

cps_h2_synthesis.png

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

2.4 Alternative H-Two Synthesis

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

h_infinity_optimal_comp_filters_bis.png

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

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

2.5 H-Infinity Synthesis - method A

Another objective that we may have is that the noise of the super sensor \(n_{SS}\) is following the minimum of the noise of the two sensors \(n_1\) and \(n_2\): \[ \Gamma_{n_{ss}}(\omega) = \min(\Gamma_{n_1}(\omega),\ \Gamma_{n_2}(\omega)) \]

In order to obtain that ideal case, we need that the complementary filters be designed such that:

\begin{align*} & |H_1(j\omega)| = 1 \text{ and } |H_2(j\omega)| = 0 \text{ at frequencies where } \Gamma_{n_1}(\omega) < \Gamma_{n_2}(\omega) \\ & |H_1(j\omega)| = 0 \text{ and } |H_2(j\omega)| = 1 \text{ at frequencies where } \Gamma_{n_1}(\omega) > \Gamma_{n_2}(\omega) \end{align*}

Which is indeed impossible in practice.

We could try to approach that with the \(\mathcal{H}_\infty\) synthesis by using high order filters.

As shown on Fig. 13, 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. 19.

n = 5; w0 = 2*pi*10; G0 = 1/10; G1 = 10000; Gc = 1/2;
W1a = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n;

n = 5; w0 = 2*pi*8; G0 = 1000; G1 = 0.1; Gc = 1/2;
W2a = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n;
P = [W1a -W1a;
     0    W2a;
     1    0];

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

[H2a, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
[H2a, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
Resetting value of Gamma min based on D_11, D_12, D_21 terms

Test bounds:      0.1000 <  gamma  <=  10500.0000

  gamma    hamx_eig  xinf_eig  hamy_eig   yinf_eig   nrho_xy   p/f
1.050e+04   2.1e+01 -3.0e-07   7.8e+00   -1.3e-15    0.0000    p
5.250e+03   2.1e+01 -1.5e-08   7.8e+00   -5.8e-14    0.0000    p
2.625e+03   2.1e+01   2.5e-10   7.8e+00   -3.7e-12    0.0000    p
1.313e+03   2.1e+01 -3.2e-11   7.8e+00   -7.3e-14    0.0000    p
  656.344   2.1e+01 -2.2e-10   7.8e+00   -1.1e-15    0.0000    p
  328.222   2.1e+01 -1.1e-10   7.8e+00   -1.2e-15    0.0000    p
  164.161   2.1e+01 -2.4e-08   7.8e+00   -8.9e-16    0.0000    p
   82.130   2.1e+01   2.0e-10   7.8e+00   -9.1e-31    0.0000    p
   41.115   2.1e+01 -6.8e-09   7.8e+00   -4.1e-13    0.0000    p
   20.608   2.1e+01   3.3e-10   7.8e+00   -1.4e-12    0.0000    p
   10.354   2.1e+01 -9.8e-09   7.8e+00   -1.8e-15    0.0000    p
    5.227   2.1e+01 -4.1e-09   7.8e+00   -2.5e-12    0.0000    p
    2.663   2.1e+01   2.7e-10   7.8e+00   -4.0e-14    0.0000    p
    1.382   2.1e+01 -3.2e+05#  7.8e+00   -3.5e-14    0.0000    f
    2.023   2.1e+01 -5.0e-10   7.8e+00    0.0e+00    0.0000    p
    1.702   2.1e+01 -2.4e+07#  7.8e+00   -1.6e-13    0.0000    f
    1.862   2.1e+01 -6.0e+08#  7.8e+00   -1.0e-12    0.0000    f
    1.942   2.1e+01 -2.8e-09   7.8e+00   -8.1e-14    0.0000    p
    1.902   2.1e+01 -2.5e-09   7.8e+00   -1.1e-13    0.0000    p
    1.882   2.1e+01 -9.3e-09   7.8e+00   -2.0e-15    0.0001    p
    1.872   2.1e+01 -1.3e+09#  7.8e+00   -3.6e-22    0.0000    f
    1.877   2.1e+01 -2.6e+09#  7.8e+00   -1.2e-13    0.0000    f
    1.880   2.1e+01 -5.6e+09#  7.8e+00   -1.4e-13    0.0000    f
    1.881   2.1e+01 -1.2e+10#  7.8e+00   -3.3e-12    0.0000    f
    1.882   2.1e+01 -3.2e+10#  7.8e+00   -8.5e-14    0.0001    f

 Gamma value achieved:     1.8824
H1a = 1 - H2a;

weights_comp_filters_Hinfa.png

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

We then compute the Power Spectral Density as well as the Cumulative Power Spectrum.

PSD_Ha = abs(squeeze(freqresp(N1*H1a, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2a, freqs, 'Hz'))).^2;
CPS_Ha = 1/pi*cumtrapz(2*pi*freqs, PSD_Ha);

2.6 H-Infinity Synthesis - method B

We have that: \[ \Phi_{\hat{x}}(\omega) = \left|H_1(j\omega) N_1(j\omega)\right|^2 + \left|H_2(j\omega) N_2(j\omega)\right|^2 \]

Then, at frequencies where \(|H_1(j\omega)| < |H_2(j\omega)|\) we would like that \(|N_1(j\omega)| = 1\) and \(|N_2(j\omega)| = 0\) as we discussed before. Then \(|H_1 N_1|^2 + |H_2 N_2|^2 = |N_1|^2\).

We know that this is impossible in practice. A more realistic choice is to design \(H_2(s)\) such that when \(|N_2(j\omega)| > |N_1(j\omega)|\), we have that: \[ |H_2 N_2|^2 = \epsilon |H_1 N_1|^2 \]

Which is equivalent to have (by supposing \(|H_1| \approx 1\)): \[ |H_2| = \sqrt{\epsilon} \frac{|N_1|}{|N_2|} \]

And we have:

\begin{align*} \Phi_{\hat{x}} &= \left|H_1 N_1\right|^2 + |H_2 N_2|^2 \\ &= (1 + \epsilon) \left| H_1 N_1 \right|^2 \\ &\approx \left|N_1\right|^2 \end{align*}

Similarly, we design \(H_1(s)\) such that at frequencies where \(|N_1| > |N_2|\): \[ |H_1| = \sqrt{\epsilon} \frac{|N_2|}{|N_1|} \]

For instance, is we take \(\epsilon = 1\), then the PSD of \(\hat{x}\) is increased by just by a factor \(\sqrt{2}\) over the all frequencies from the idea case.

We use this as the weighting functions for the \(\mathcal{H}_\infty\) synthesis of the complementary filters.

The weighting function and the obtained complementary filters are shown in Fig. 20.

epsilon = 2;

W1b = 1/epsilon*N1/N2;
W2b = 1/epsilon*N2/N1;

W1b = W1b/(1 + s/2/pi/1000); % this is added so that it is proper
P = [W1b -W1b;
     0    W2b;
     1    0];

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

[H2b, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
[H2b, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
Test bounds:      0.0000 <  gamma  <=     32.8125

  gamma    hamx_eig  xinf_eig  hamy_eig   yinf_eig   nrho_xy   p/f
   32.812   1.8e+01   3.4e-10   6.3e+00   -2.9e-13    0.0000    p
   16.406   1.8e+01   3.4e-10   6.3e+00   -1.2e-15    0.0000    p
    8.203   1.8e+01   3.3e-10   6.3e+00   -2.6e-13    0.0000    p
    4.102   1.8e+01   3.3e-10   6.3e+00   -2.1e-13    0.0000    p
    2.051   1.7e+01   3.4e-10   6.3e+00   -7.2e-16    0.0000    p
    1.025   1.6e+01 -1.3e+06#  6.3e+00   -8.3e-14    0.0000    f
    1.538   1.7e+01   3.4e-10   6.3e+00   -2.0e-13    0.0000    p
    1.282   1.7e+01   3.4e-10   6.3e+00   -7.9e-17    0.0000    p
    1.154   1.7e+01   3.6e-10   6.3e+00   -1.8e-13    0.0000    p
    1.089   1.7e+01 -3.4e+06#  6.3e+00   -1.7e-13    0.0000    f
    1.122   1.7e+01 -1.0e+07#  6.3e+00   -3.2e-13    0.0000    f
    1.138   1.7e+01 -1.3e+08#  6.3e+00   -1.8e-13    0.0000    f
    1.146   1.7e+01   3.2e-10   6.3e+00   -3.0e-13    0.0000    p
    1.142   1.7e+01   5.5e-10   6.3e+00   -2.8e-13    0.0000    p
    1.140   1.7e+01 -1.5e-10   6.3e+00   -2.3e-13    0.0000    p
    1.139   1.7e+01 -4.8e+08#  6.3e+00   -6.2e-14    0.0000    f
    1.139   1.7e+01   1.3e-09   6.3e+00   -8.9e-17    0.0000    p

 Gamma value achieved:     1.1390
H1b = 1 - H2b;

weights_comp_filters_Hinfb.png

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

PSD_Hb = abs(squeeze(freqresp(N1*H1b, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2b, freqs, 'Hz'))).^2;
CPS_Hb = 1/pi*cumtrapz(2*pi*freqs, PSD_Hb);

2.7 H-Infinity Synthesis - method C

Wp = 0.56*(inv(N1)+inv(N2))/(1 + s/2/pi/1000);

W1c = N1*Wp;
W2c = N2*Wp;
P = [W1c -W1c;
     0    W2c;
     1    0];

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

[H2c, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
[H2c, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
Test bounds:      0.0000 <  gamma  <=     36.7543

  gamma    hamx_eig  xinf_eig  hamy_eig   yinf_eig   nrho_xy   p/f
   36.754   5.7e+00 -1.0e-13   6.3e+00   -6.2e-25    0.0000    p
   18.377   5.7e+00 -1.4e-12   6.3e+00   -1.8e-13    0.0000    p
    9.189   5.7e+00 -4.3e-13   6.3e+00   -4.7e-15    0.0000    p
    4.594   5.7e+00 -9.4e-13   6.3e+00   -4.7e-15    0.0000    p
    2.297   5.7e+00 -1.3e-16   6.3e+00   -6.8e-14    0.0000    p
    1.149   5.7e+00 -1.6e-17   6.3e+00   -1.5e-15    0.0000    p
    0.574   5.7e+00 -5.2e+02#  6.3e+00   -5.9e-14    0.0000    f
    0.861   5.7e+00 -3.1e+04#  6.3e+00   -3.8e-14    0.0000    f
    1.005   5.7e+00 -1.6e-12   6.3e+00   -1.1e-14    0.0000    p
    0.933   5.7e+00 -1.1e+05#  6.3e+00   -7.2e-14    0.0000    f
    0.969   5.7e+00 -3.3e+05#  6.3e+00   -5.6e-14    0.0000    f
    0.987   5.7e+00 -1.2e+06#  6.3e+00   -4.5e-15    0.0000    f
    0.996   5.7e+00 -6.5e-16   6.3e+00   -1.7e-15    0.0000    p
    0.992   5.7e+00 -2.9e+06#  6.3e+00   -6.1e-14    0.0000    f
    0.994   5.7e+00 -9.7e+06#  6.3e+00   -3.0e-16    0.0000    f
    0.995   5.7e+00 -8.0e-10   6.3e+00   -1.9e-13    0.0000    p
    0.994   5.7e+00 -2.3e+07#  6.3e+00   -4.3e-14    0.0000    f

 Gamma value achieved:     0.9949
H1c = 1 - H2c;

weights_comp_filters_Hinfc.png

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

PSD_Hc = abs(squeeze(freqresp(N1*H1c, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2c, freqs, 'Hz'))).^2;
CPS_Hc = 1/pi*cumtrapz(2*pi*freqs, PSD_Hc);

2.8 Comparison of the methods

The three methods are now compared.

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

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

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

Table 1: RMS value of the estimation error when using the sensor individually and when using the two sensor merged using the optimal complementary filters
  rms value
Sensor 1 1.3e-03
Sensor 2 1.3e-03
H2 Fusion 1.2e-04
H-Infinity a 2.4e-04
H-Infinity b 1.4e-04
H-Infinity c 2.2e-04

comparison_psd_noise.png

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

comparison_cps_noise.png

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

2.9 Obtained Super Sensor’s noise uncertainty

We would like to verify if the obtained sensor fusion architecture is robust to change in the sensor dynamics.

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

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

The sensor uncertain models are defined below.

G1 = 1 + w1*ultidyn('Delta',[1 1]);
G2 = 1 + w2*ultidyn('Delta',[1 1]);

The super sensor uncertain model is defined below using the complementary filters obtained with the \(\mathcal{H}_2\) synthesis. The dynamical uncertainty bounds of the super sensor is shown in Fig. 24. Right Half Plane zero might be introduced in the super sensor dynamics which will render the feedback system unstable.

Gss = G1*H1 + G2*H2;

uncertainty_super_sensor_H2_syn.png

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

2.10 Conclusion

From the above complementary filter design with the \(\mathcal{H}_2\) and \(\mathcal{H}_\infty\) synthesis, it still seems that the \(\mathcal{H}_2\) synthesis gives the complementary filters that permits to obtain the minimal super sensor noise (when measuring with the \(\mathcal{H}_2\) norm).

However, the synthesis does not take into account the robustness of the sensor fusion.

3 Optimal Sensor Fusion - Minimize the Super Sensor Dynamical Uncertainty

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

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

sensor_fusion_dynamic_uncertainty.png

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

The Matlab scripts is accessible here.

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

sensor_fusion_dynamic_uncertainty.png

Figure 26: 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 \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 27).

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 27: Maximum phase variation

3.2 Dynamical uncertainty of the individual sensors

Let say we want to merge two sensors:

  • sensor 1 that has unknown dynamics above 10Hz: \(|w_1(j\omega)| > 1\) for \(\omega > 10\text{ Hz}\)
  • sensor 2 that has unknown dynamics below 1Hz and above 1kHz \(|w_2(j\omega)| > 1\) for \(\omega < 1\text{ Hz}\) and \(\omega > 1\text{ kHz}\)

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. 28 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 28: Dynamic uncertainty of the two sensors (png, pdf)

3.3 Synthesis objective

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

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

3.4 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{org6b95a9d} \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\).

3.5 Weighting Function used to bound the super sensor uncertainty

Let’s define \(w_\phi(s)\) in order to bound the maximum allowed phase uncertainty \(\Delta\phi_\text{max}\) of the super sensor dynamics. The magnitude \(|w_\phi(j\omega)|\) is shown in Fig. 29 and the corresponding maximum allowed phase uncertainty of the super sensor dynamics of shown in Fig. 30.

Dphi = 20; % [deg]

n = 4; w0 = 2*pi*900; G0 = 1/sin(Dphi*pi/180); Ginf = 1/100; Gc = 1;
wphi = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/Ginf)^(2/n)))*s + (G0/Gc)^(1/n))/((1/Ginf)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/Ginf)^(2/n)))*s + (1/Gc)^(1/n)))^n;

W1 = w1*wphi;
W2 = w2*wphi;

magnitude_wphi.png

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

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

upper_bounds_comp_filter_max_phase_uncertainty.png

Figure 31: 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)

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

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

h_infinity_robust_fusion.png

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

The generalized plant is defined below.

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

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

[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
Resetting value of Gamma min based on D_11, D_12, D_21 terms

Test bounds:      0.0447 <  gamma  <=      1.3318

  gamma    hamx_eig  xinf_eig  hamy_eig   yinf_eig   nrho_xy   p/f
    1.332   1.3e+01 -1.0e-14   1.3e+00   -2.6e-18    0.0000    p
    0.688   1.3e-11# ********   1.3e+00   -6.7e-15  ********    f
    1.010   1.1e+01 -1.5e-14   1.3e+00   -2.5e-14    0.0000    p
    0.849   6.9e-11# ********   1.3e+00   -2.3e-14  ********    f
    0.930   5.2e-12# ********   1.3e+00   -6.1e-18  ********    f
    0.970   5.6e-11# ********   1.3e+00   -2.3e-14  ********    f
    0.990   5.0e-11# ********   1.3e+00   -1.7e-17  ********    f
    1.000   2.1e-10# ********   1.3e+00    0.0e+00  ********    f
    1.005   1.9e-10# ********   1.3e+00   -3.7e-14  ********    f
    1.008   1.1e+01 -9.1e-15   1.3e+00    0.0e+00    0.0000    p
    1.006   1.2e-09# ********   1.3e+00   -6.9e-16  ********    f
    1.007   1.1e+01 -4.6e-15   1.3e+00   -1.8e-16    0.0000    p

 Gamma value achieved:     1.0069

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

H1 = 1 - H2;

The obtained complementary filters are shown in Fig. 33.

comp_filter_hinf_uncertainty.png

Figure 33: Obtained complementary filters (png, pdf)

3.7 Super sensor uncertainty

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

Gss = G1*H1 + G2*H2;

super_sensor_uncertainty_bode_plot.png

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

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

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

The PSD of both sensor and of the super sensor is shown in Fig. 35. The CPS of both sensor and of the super sensor is shown in Fig. 36.

psd_sensors_hinf_synthesis.png

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

cps_sensors_hinf_synthesis.png

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

3.9 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

4 Optimal Sensor Fusion - Mixed Synthesis

The Matlab scripts is accessible here.

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

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

mixed_synthesis_noise_uncertainty_sensors.png

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

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. 38. 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 38: Wanted maximum module uncertainty of the super sensor (png, pdf)

The equivalent Magnitude and Phase uncertainties are shown in Fig. 39.

mixed_syn_objective_hinf.png

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

4.4 Mixed Synthesis Architecture

The synthesis architecture that is used here is shown in Fig. 40.

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 40: Mixed H2/H-Infinity 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(w1*wphi); W2u = ss(w2*wphi); % Weight on the uncertainty
W1n = ss(N1); W2n = ss(N2); % Weight on the noise

P = [W1u -W1u;
     0    W2u;
     W1n -W1n;
     0    W2n;
     1    0];

4.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 = 1 - H2;

The obtained complementary filters are shown in Fig. 41.

comp_filters_mixed_synthesis.png

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

4.6 Obtained Super Sensor’s noise

The PSD and CPS of the super sensor’s noise are shown in Fig. 42 and Fig. 43 respectively.

psd_super_sensor_mixed_syn.png

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

4.7 Obtained Super Sensor’s Uncertainty

The uncertainty on the super sensor’s dynamics is shown in Fig. 44.

super_sensor_dyn_uncertainty_mixed_syn.png

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

4.8 Conclusion

This synthesis methods allows both to:

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

5 Mixed Synthesis - LMI Optimization

5.1 Introduction

The following matlab scripts was written by Mohit.

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

5.3 Weights

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

n = 4; w0 = 2*pi*900; G0 = 9; G1 = 1; Gc = 1.1;
H = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n;
wphi = 0.2*(s+3.142e04)/(s+628.3)/H;

W1u = ss(w1*wphi); W2u = ss(w2*wphi); % Weight on the uncertainty
W1n = ss(N1); W2n = ss(N2); % Weight on the noise
P = [W1u -W1u;
     0    W2u;
     W1n -W1n;
     0    W2n;
     1    0];

5.4 LMI Optimization

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

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

A = P.A;
Bw = P.B(:,1);
Bu = P.B(:,2);
Cz1 = P.C(1:2,:); Dz1w = P.D(1:2,1); Dz1u = P.D(1:2,2); % Hinf
Cz2 = P.C(3:4,:); Dz2w = P.D(1:2,1); Dz2u = P.D(1:2,2); % H2
Cy = P.C(5,:); Dyw = P.D(5,1); Dyu = P.D(5,2);

n = size(P.A,1);
ny = 1; % number of measurements
nu = 1; % number of control inputs
nz = 2;
nw = 1;

Wtinf = 0;
Wt2 = 1;

We Define all the variables.

cvx_startup;

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

We define the minimization objective.

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

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

gam<=1; % Keep the Hinf norm less than 1

[ X, eye(n,n) ;
  eye(n,n), Y ] >= 0 ;

[ A*X + Bu*Ch + X*A' + Ch'*Bu', A+Bu*Dh*Cy+Ah', Bw+Bu*Dh*Dyw, X*Cz1' + Ch'*Dz1u' ;
  (A+Bu*Dh*Cy+Ah')', Y*A + A'*Y + Bh*Cy + Cy'*Bh', Y*Bw + Bh*Dyw, (Cz1+Dz1u*Dh*Cy)' ;
  (Bw+Bu*Dh*Dyw)', Bw'*Y + Dyw'*Bh', -eye(nw,nw), (Dz1w+Dz1u*Dh*Dyw)' ;
  Cz1*X + Dz1u*Ch, Cz1+Dz1u*Dh*Cy, Dz1w+Dz1u*Dh*Dyw, -gam*eye(nz,nz)] <= 0 ;

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

trace(W) <= eta ;

[ W, Cz2*X+Dz2u*Ch, Cz2*X+Dz2u*Ch;
  X*Cz2'+Ch'*Dz2u', X, eye(n,n) ;
  (Cz2*X+Dz2u*Ch)', eye(n,n), Y ] >= 0 ;

[ A*X + Bu*Ch + X*A' + Ch'*Bu', A+Bu*Dh*Cy+Ah', Bw+Bu*Dh*Dyw ;
  (A+Bu*Dh*Cy+Ah')', Y*A + A'*Y + Bh*Cy + Cy'*Bh', Y*Bw + Bh*Dyw ;
  (Bw+Bu*Dh*Dyw)', Bw'*Y + Dyw'*Bh', -eye(nw,nw)] <= 0 ;

And we run the optimization.

cvx_end
cvx_status

Finally, we can compute the obtained complementary filters.

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

H2 = tf(ss(Ak,Bk,Ck,Dk));
H1 = 1 - H2;

5.5 Result

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

LMI_obtained_comp_filters.png

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

5.6 Comparison with the matlab Mixed Synthesis

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

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

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

H1m = 1 - H2m;

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

compare_cvx_h2hinf_comp_filters.png

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

5.7 H-Infinity Objective

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

comp_cvx_h2i_hinf_norm.png

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

5.8 Obtained Super Sensor’s noise

The PSD and CPS of the super sensor’s noise obtained with the CVX toolbox and h2hinfsyn command are compared in Fig. 48 and 49.

psd_compare_cvx_h2i.png

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

cps_compare_cvx_h2i.png

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

5.9 Obtained Super Sensor’s Uncertainty

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

super_sensor_uncertainty_compare_cvx_h2i.png

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

6 H-Infinity synthesis to ensure both performance and robustness

The Matlab scripts is accessible here.

6.1 Introduction

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

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

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

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

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

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

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

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

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

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

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

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

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

We may not want to follow the minimum uncertainty.

6.2 Dynamical uncertainty and Noise level of the individual sensors

Uncertainty on the individual sensors:

omegac = 100*2*pi; G0 = 0.1; Ginf = 10;
w1 = (Ginf*s/omegac + G0)/(s/omegac + 1);

omegac = 0.2*2*pi; G0 = 5; Ginf = 0.1;
w2 = (Ginf*s/omegac + G0)/(s/omegac + 1);
omegac = 5000*2*pi; G0 = 1; Ginf = 50;
w2 = w2*(Ginf*s/omegac + G0)/(s/omegac + 1);

Noise level of the individual sensors:

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

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

noise_uncertainty_sensors_hinf.png

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

6.3 Weights for uncertainty and performance

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

Weight on the uncertainty:

n = 4; w0 = 2*pi*500; G0 = 6; G1 = 1; Gc = 1.1;
H = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n;

Wu = 0.2*(s+3.142e04)/(s+628.3)/H;

Weight on the performance:

n = 1; w0 = 2*pi*9; A = 6;
a = sqrt(2*A^(2/n) - 1 + 2*A^(1/n)*sqrt(A^(2/n) - 1));
G = ((1 + s/(w0/a))*(1 + s/(w0*a))/(1 + s/w0)^2)^n;

n = 2; w0 = 2*pi*9; G0 = 1e-2; G1 = 1; Gc = 5e-1;
G2 = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n;

Wp = inv(G2)*inv(G)*inv(N2);

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

charac_sensors_weights.png

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

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

upper_bound_complementary_filters_perf_robust.png

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

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

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

\begin{equation*} \left\| \begin{matrix} W_{1p}(s) (1 - N_2(s)) \\ W_{2p}(s) N_2(s) \\ W_{1u}(s) (1 - N_2(s)) \\ W_{2u}(s) N_2(s) \end{matrix} \right\|_\infty < 1 \end{equation*}
W1p = N1*Wp/(1+s/2/pi/1000); % Used to render W1p proper
W2p = N2*Wp;
W1u = w1*Wu;
W2u = w2*Wu;
P = [W1p -W1p;
     0    W2p;
     W1u -W1u;
     0    W2u;
     1    0];

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

[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
Resetting value of Gamma min based on D_11, D_12, D_21 terms

Test bounds:      1.4139 <  gamma  <=     65.6899

  gamma    hamx_eig  xinf_eig  hamy_eig   yinf_eig   nrho_xy   p/f
   65.690   1.3e+00 -6.7e-15   1.3e+00   -4.5e-13    0.0000    p
   33.552   1.3e+00 -9.4e-15   1.3e+00   -3.7e-14    0.0000    p
   17.483   1.3e+00 -5.6e-16   1.3e+00   -4.8e-13    0.0000    p
    9.448   1.3e+00 -3.2e-15   1.3e+00   -1.2e-13    0.0000    p
    5.431   1.3e+00 -2.3e-16   1.3e+00   -3.6e-13    0.0000    p
    3.422   1.3e+00 -7.3e-16   1.3e+00   -2.6e-15    0.0000    p
    2.418   1.3e+00   9.3e-17   1.3e+00   -3.0e-14    0.0000    p
    1.916   1.3e+00   2.4e-17   1.3e+00   -2.2e-14    0.0000    p
    1.665   1.3e+00 -2.5e-16   1.3e+00   -2.1e-14    0.0000    p
    1.539   1.3e+00 -6.9e-15   1.3e+00   -5.3e-14    0.0000    p
    1.477   1.3e+00 -2.1e-14   1.3e+00   -2.3e-13    0.0000    p
    1.445   1.3e+00 -1.3e-16   1.3e+00   -2.6e-15    0.0000    p
    1.430   1.3e+00 -4.9e-13   1.3e+00   -2.2e-13    0.0000    p
    1.422   1.3e+00 -1.2e+08#  1.3e+00   -2.6e-13    0.0000    f
    1.426   1.3e+00 -6.3e-13   1.3e+00   -3.3e-14    0.0000    p
    1.424   1.3e+00 -3.4e+08#  1.3e+00   -4.5e-14    0.0000    f
    1.425   1.3e+00 -1.7e+09#  1.3e+00   -5.2e-13    0.0000    f

 Gamma value achieved:     1.4256
H1 = 1 - H2;

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

hinf_result_comp_filters_4_outputs.png

Figure 54: caption (png, pdf)

upper_bounds_perf_robust_result_4_outputs.png

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

4outputs_hinf_psd_cps2svg.png

Figure 56: PSD and CPS (png, pdf)

4outputs_uncertainty.png

Figure 57: Dynamical uncertainty (png, pdf)

6.5 Conclusion

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

7 Equivalent Super Sensor

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

7.1 Sensor Fusion Architecture

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

sensor_fusion_full.png

Figure 58: Sensor fusion architecture (png, pdf).

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

To the dynamics of the super sensor is:

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

And the noise of the super sensor is:

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

7.2 Equivalent Configuration

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

sensor_fusion_equivalent.png

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

7.3 Model the uncertainty of the super sensor

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

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

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

7.4 Model the noise of the super sensor

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

\begin{align*} \Phi_{\hat{x}}(\omega) &= | H_1(j\omega) N_1(j\omega) |^2 \Phi_{\tilde{n}_1} + | H_2(j\omega) N_2(j\omega) |^2 \Phi_{\tilde{n}_2} \\ &= | H_1(j\omega) N_1(j\omega) |^2 + | H_2(j\omega) N_2(j\omega) |^2 \end{align*}

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

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

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

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

7.5 First guess

We could choose

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

But we would have:

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

and

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

8 Optimal And Robust Sensor Fusion in Practice

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

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

8.1 Measurement of the noise characteristics of the sensors

8.1.1 Huddle Test

The technique to estimate the sensor noise is taken from (Barzilai, VanZandt, and Kenny 1998).

Let’s consider two sensors (sensor 1 and sensor 2) that are measuring the same quantity \(x\) as shown in figure 60.

huddle_test.png

Figure 60: Huddle test block diagram

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

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

We then have:

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

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

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

From equations \eqref{eq:coh_bis} and \eqref{eq:incoherent_noise}, we finally obtain

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

8.1.2 Weights that represents the noises’ PSD

For further complementary filter synthesis, it is preferred to consider a normalized noise source \(\tilde{n}\) that has a PSD equal to one (\(\Phi_{\tilde{n}}(\omega) = 1\)) and to use a weighting filter \(N(s)\) in order to represent the frequency dependence of the noise.

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

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

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

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

one_sensor_normalized_noise.png

Figure 61: One sensor with normalized noise

8.1.3 Comparison of the noises’ PSD

Once the noise of the sensors to be merged have been characterized, the power spectral density of both sensors have to be compared.

Ideally, the PSD of the noise are such that:

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

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

The coherence between signals \(x\) and \(y\) is defined as follow \[ \gamma^2_{xy}(\omega) = \frac{|\Phi_{xy}(\omega)|^2}{|\Phi_{x}(\omega)| |\Phi_{y}(\omega)|} \] where \(|\Phi_x(\omega)|\) is the output Power Spectral Density (PSD) of signal \(x\) and \(|\Phi_{xy}(\omega)|\) is the Cross Spectral Density (CSD) of signal \(x\) and \(y\).

The PSD and CSD are defined as follow:

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

where:

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

8.2 Estimate the dynamic uncertainty of the sensors

Let’s consider one sensor represented on figure 62.

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

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

one_sensor_dyn_uncertainty.png

Figure 62: Sensor with dynamic uncertainty

8.3 Optimal and Robust synthesis of the complementary filters

Once the noise characteristics and dynamic uncertainty of both sensors have been determined and we have determined the following weighting functions:

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

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

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

sensor_fusion_full.png

Figure 63: Sensor fusion architecture with sensor dynamics uncertainty

9 Methods of complementary filter synthesis

9.1 Complementary filters using analytical formula

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

9.1.1 Analytical 1st order complementary filters

First order complementary filters are defined with following equations:

\begin{align} H_L(s) = \frac{1}{1 + \frac{s}{\omega_0}}\\ H_H(s) = \frac{\frac{s}{\omega_0}}{1 + \frac{s}{\omega_0}} \end{align}

Their bode plot is shown figure 64.

w0 = 2*pi; % [rad/s]

Hh1 = (s/w0)/((s/w0)+1);
Hl1 = 1/((s/w0)+1);

comp_filter_1st_order.png

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

9.1.2 Second Order Complementary Filters

We here use analytical formula for the complementary filters \(H_L\) and \(H_H\).

The first two formulas that are used to generate complementary filters are:

\begin{align*} H_L(s) &= \frac{(1+\alpha) (\frac{s}{\omega_0})+1}{\left((\frac{s}{\omega_0})+1\right) \left((\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1\right)}\\ H_H(s) &= \frac{(\frac{s}{\omega_0})^2 \left((\frac{s}{\omega_0})+1+\alpha\right)}{\left((\frac{s}{\omega_0})+1\right) \left((\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1\right)} \end{align*}

where:

  • \(\omega_0\) is the blending frequency in rad/s.
  • \(\alpha\) is used to change the shape of the filters:
    • Small values for \(\alpha\) will produce high magnitude of the filters \(|H_L(j\omega)|\) and \(|H_H(j\omega)|\) near \(\omega_0\) but smaller value for \(|H_L(j\omega)|\) above \(\approx 1.5 \omega_0\) and for \(|H_H(j\omega)|\) below \(\approx 0.7 \omega_0\)
    • A large \(\alpha\) will do the opposite

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

We now study the maximum norm of the filters function of the parameter \(\alpha\). As we saw that the maximum norm of the filters is important for the robust merging of filters.

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

param_alpha_hinf_norm.png

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

9.1.3 Third Order Complementary Filters

The following formula gives complementary filters with slopes of \(-3\) and \(3\):

\begin{align*} H_L(s) &= \frac{\left(1+(\alpha+1)(\beta+1)\right) (\frac{s}{\omega_0})^2 + (1+\alpha+\beta)(\frac{s}{\omega_0}) + 1}{\left(\frac{s}{\omega_0} + 1\right) \left( (\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1 \right) \left( (\frac{s}{\omega_0})^2 + \beta (\frac{s}{\omega_0}) + 1 \right)}\\ H_H(s) &= \frac{(\frac{s}{\omega_0})^3 \left( (\frac{s}{\omega_0})^2 + (1+\alpha+\beta) (\frac{s}{\omega_0}) + (1+(\alpha+1)(\beta+1)) \right)}{\left(\frac{s}{\omega_0} + 1\right) \left( (\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1 \right) \left( (\frac{s}{\omega_0})^2 + \beta (\frac{s}{\omega_0}) + 1 \right)} \end{align*}

The parameters are:

  • \(\omega_0\) is the blending frequency in rad/s
  • \(\alpha\) and \(\beta\) that are used to change the shape of the filters similarly to the parameter \(\alpha\) for the second order complementary filters

The filters are defined below and the result is shown on figure 67.

alpha = 1;
beta = 10;
w0 = 2*pi*14;

Hh3_ana = (s/w0)^3 * ((s/w0)^2 + (1+alpha+beta)*(s/w0) + (1+(alpha+1)*(beta+1)))/((s/w0 + 1)*((s/w0)^2+alpha*(s/w0)+1)*((s/w0)^2+beta*(s/w0)+1));
Hl3_ana = ((1+(alpha+1)*(beta+1))*(s/w0)^2 + (1+alpha+beta)*(s/w0) + 1)/((s/w0 + 1)*((s/w0)^2+alpha*(s/w0)+1)*((s/w0)^2+beta*(s/w0)+1));

complementary_filters_third_order.png

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

9.2 H-Infinity synthesis of complementary filters

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

9.2.1 Synthesis Architecture

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

In order to do so, we use the generalized plant shown on figure 68 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 68: 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 69) such that the \(\mathcal{H}_\infty\) norm of the transfer function from \(w\) to \([z_H,\ z_L]\) is less than one: \[ \left\| \begin{array}{c} H_L w_L \\ (1 - H_L) w_H \end{array} \right\|_\infty < 1 \]

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

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

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

sf_hinf_filters_b.png

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

9.2.2 Weights

omegab = 2*pi*9;
wH = (omegab)^2/(s + omegab*sqrt(1e-5))^2;
omegab = 2*pi*28;
wL = (s + omegab/(4.5)^(1/3))^3/(s*(1e-4)^(1/3) + omegab)^3;

weights_wl_wh.png

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

9.2.3 H-Infinity Synthesis

We define the generalized plant \(P\) on matlab.

P = [0   wL;
     wH -wH;
     1   0];

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

[Hl_hinf, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
[Hl_hinf, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
Test bounds:      0.0000 <  gamma  <=      1.7285

  gamma    hamx_eig  xinf_eig  hamy_eig   yinf_eig   nrho_xy   p/f
    1.729   4.1e+01   8.4e-12   1.8e-01    0.0e+00    0.0000    p
    0.864   3.9e+01 -5.8e-02#  1.8e-01    0.0e+00    0.0000    f
    1.296   4.0e+01   8.4e-12   1.8e-01    0.0e+00    0.0000    p
    1.080   4.0e+01   8.5e-12   1.8e-01    0.0e+00    0.0000    p
    0.972   3.9e+01 -4.2e-01#  1.8e-01    0.0e+00    0.0000    f
    1.026   4.0e+01   8.5e-12   1.8e-01    0.0e+00    0.0000    p
    0.999   3.9e+01   8.5e-12   1.8e-01    0.0e+00    0.0000    p
    0.986   3.9e+01 -1.2e+00#  1.8e-01    0.0e+00    0.0000    f
    0.993   3.9e+01 -8.2e+00#  1.8e-01    0.0e+00    0.0000    f
    0.996   3.9e+01   8.5e-12   1.8e-01    0.0e+00    0.0000    p
    0.994   3.9e+01   8.5e-12   1.8e-01    0.0e+00    0.0000    p
    0.993   3.9e+01 -3.2e+01#  1.8e-01    0.0e+00    0.0000    f

 Gamma value achieved:     0.9942

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

Hh_hinf = 1 - Hl_hinf;

9.2.4 Obtained Complementary Filters

The obtained complementary filters are shown on figure 71.

hinf_filters_results.png

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

9.3 Feedback Control Architecture to generate Complementary Filters

The idea is here to use the fact that in a classical feedback architecture, \(S + T = 1\), in order to design complementary filters.

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

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

9.3.1 Architecture

complementary_filters_feedback_architecture.png

Figure 72: Architecture used to generate the complementary filters

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

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

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

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

9.3.2 Loop Gain Design

Let’s first define the loop gain \(L\).

wc = 2*pi*1;
alpha = 2;

L = (wc/s)^2 * (s/(wc/alpha) + 1)/(s/wc + alpha);

loop_gain_bode_plot.png

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

9.3.3 Complementary Filters Obtained

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

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

low_pass_high_pass_filters.png

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

9.4 Analytical Formula found in the literature

9.4.1 Analytical Formula

(Min and Jeung 2015)

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

(Corke 2004)

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

(Jensen, Coopmans, and Chen 2013)

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

(Shaw and Srinivasan 1990)

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

(Baerveldt and Klang 1997)

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

9.4.2 Matlab

omega0 = 1*2*pi; % [rad/s]
tau = 1/omega0; % [s]

% From cite:corke04_inert_visual_sensin_system_small_auton_helic
HL1 = 1/(s/omega0 + 1); HH1 = s/omega0/(s/omega0 + 1);

% From cite:jensen13_basic_uas
HL2 = (2*omega0*s + omega0^2)/(s+omega0)^2; HH2 = s^2/(s+omega0)^2;

% From cite:shaw90_bandw_enhan_posit_measur_using_measur_accel
HL3 = (3*tau*s + 1)/(tau*s + 1)^3; HH3 = (tau^3*s^3 + 3*tau^2*s^2)/(tau*s + 1)^3;

comp_filters_literature.png

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

9.4.3 Discussion

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

9.5 Comparison of the different methods of synthesis

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

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

10 Real World Example of optimal sensor fusion

10.1 Matlab Code

Take an Accelerometer and a Geophone both measuring the absolute motion of a structure.

Parameters of the inertial sensors.

m_acc = 0.01;
k_acc = 1e6;
c_acc = 20;

m_geo = 1;
k_geo = 1e3;
c_geo = 10;

Transfer function from motion to measurement

For the accelerometer. The measurement is the relative motion structure/inertial mass: \[ \frac{d}{\ddot{w}} = \frac{-m}{ms^2 + cs + k} \]

For the geophone. The measurement is the relative velocity structure/inertial mass: \[ \frac{\dot{d}}{\dot{w}} = \frac{-ms^2}{ms^2 + cs + k} \]

G_acc = -m_acc/(m_acc*s^2 + c_acc*s + k_acc); % [m/(m/s^2)]
G_geo = -m_geo*s^2/(m_geo*s^2 + c_geo*s + k_geo); % [m/s/m/s]

Suppose the measure of the relative motion for the accelerometer (capacitive sensor for instance) has a white noise characteristic: Suppose the measure of the relative velocity (current flowing through the coil) has a white noise characteristic:

Define the noise characteristics

n = 1; w0 = 2*pi*5e3; G0 = 5e-12; G1 = 1e-15; Gc = G0/2;
L_acc = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n;

n = 1; w0 = 2*pi*5e3; G0 = 1e-6; G1 = 1e-8; Gc = G0/2;
L_geo = (((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;

Transfer function of the conversion to obtain the velocity:

C_acc = (-k_acc/m_acc/(2*pi + s));
C_geo = tf(-1);

Let’s plot the noise of both sensors: Dynamics of both sensors

10.2 Time domain signals

Fs = 1e4; % Sampling Frequency [Hz]
Ts = 1/Fs; % Sampling Time [s]

t = 0:Ts:10; % Time Vector [s]
n_acc = lsim(L_acc*C_acc, sqrt(Fs/2)*randn(length(t), 1), t); % [m/s]
n_geo = lsim(L_geo*C_geo, sqrt(Fs/2)*randn(length(t), 1), t); % [m/s]
figure;
hold on;
plot(t, n_geo)
plot(t, n_acc)
hold off;

10.3 H2 Synthesis

N1 = L_acc*C_acc;
N2 = L_geo*C_geo;
bodeFig({N1, N2}, logspace(-1, 5, 1000))
P = [0   N2  1;
     N1 -N2  0];

And we do the \(\mathcal{H}_2\) synthesis using the h2syn command.

[H1, ~, gamma] = h2syn(P, 1, 1);

Finally, we define \(H_2(s) = 1 - H_1(s)\).

H2 = 1 - H1;
bodeFig({H1, H2}, struct('phase', true))
n_acc_filt = lsim(H1, n_acc, t);
n_geo_filt = lsim(H2, n_geo, t);
  RMS
Accelerometer 9.7e-05
Geophone 5.9e-05
Super Sensor 1.5e-05
figure;
hold on;
plot(t, n_geo)
plot(t, n_acc)
plot(t, n_acc_filt + n_geo_filt)
hold off;

10.4 Signal and Noise

Velocity Signal:

v = lsim(1/(1 + s/2/pi/2), 1e-4*sqrt(Fs/2)*randn(length(t), 1), t);
v = 1e-4 * sin(2*pi*100*t);
v_acc = lsim(s*G_acc*C_acc, v, t) + n_acc;
v_geo = lsim(G_geo*C_geo,   v, t) + n_geo;
v_ss = lsim(H1, v_acc, t) + lsim(H2, v_geo, t);
figure;
hold on;
plot(t, v_geo)
plot(t, v_acc)
plot(t, v_ss)
plot(t, v, 'k--')
hold off;
xlim([1, 1+0.1])

10.5 PSD and CPS

nx = length(n_acc);
na = 16;
win = hanning(floor(nx/na));

[p_acc, f] = pwelch(n_acc, win, 0, [], Fs);
[p_geo, ~] = pwelch(n_geo, win, 0, [], Fs);
[p_ss,  ~] = pwelch(n_acc_filt + n_geo_filt, win, 0, [], Fs);

10.6 Transfer function of the super sensor

bodeFig({s*C_acc*G_acc, C_geo*G_geo, s*C_acc*G_acc*H1+C_geo*G_geo*H2}, struct('phase', true))

Bibliography

Baerveldt, A.-J., and R. Klang. 1997. “A Low-Cost and Low-Weight Attitude Estimation System for an Autonomous Helicopter.” In Proceedings of IEEE International Conference on Intelligent Engineering Systems, nil. https://doi.org/10.1109/ines.1997.632450.
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.
Bendat, J. 1957. “Optimum Filters for Independent Measurements of Two Related Perturbed Messages.” IRE Transactions on Circuit Theory, --. https://doi.org/10.1109/tct.1957.1086345.
Corke, Peter. 2004. “An Inertial and Visual Sensing System for a Small Autonomous Helicopter.” Journal of Robotic Systems 21 (2):43–51. https://doi.org/10.1002/rob.10127.
Jensen, Austin, Cal Coopmans, and YangQuan Chen. 2013. “Basics and Guidelines of Complementary Filters for Small UAS Navigation.” In 2013 International Conference on Unmanned Aircraft Systems (ICUAS), nil. https://doi.org/10.1109/icuas.2013.6564726.
Min, Hyung Gi, and Eun Tae Jeung. 2015. “Complementary Filter Design for Angle Estimation Using Mems Accelerometer and Gyroscope.” Department of Control and Instrumentation, Changwon National University, Changwon, Korea, 641–773.
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.
Plummer, A. R. 2006. “Optimal Complementary Filters and Their Application in Motion Measurement.” Proceedings of the Institution of Mechanical Engineers, Part I: Journal of Systems and Control Engineering 220 (6):489–507. https://doi.org/10.1243/09596518JSCE229.
Robert Grover Brown, Patrick Y. C. Hwang. 2012. Introduction to Random Signals and Applied Kalman Filtering with Matlab Exercises. 4th ed. Wiley.
Shaw, F.R., and K. Srinivasan. 1990. “Bandwidth Enhancement of Position Measurements Using Measured Acceleration.” Mechanical Systems and Signal Processing 4 (1):23–38. https://doi.org/10.1016/0888-3270(90)90038-m.

Author: Thomas Dehaeze

Created: 2020-09-28 lun. 17:27