Rework H2 synthesis section

This commit is contained in:
Thomas Dehaeze 2020-10-01 15:14:10 +02:00
parent 27db1ace65
commit 607595c0cb
17 changed files with 2482 additions and 689 deletions

Binary file not shown.

Binary file not shown.

Before

Width:  |  Height:  |  Size: 125 KiB

After

Width:  |  Height:  |  Size: 64 KiB

Binary file not shown.

Binary file not shown.

Before

Width:  |  Height:  |  Size: 102 KiB

After

Width:  |  Height:  |  Size: 107 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 166 KiB

After

Width:  |  Height:  |  Size: 69 KiB

Binary file not shown.

Binary file not shown.

After

Width:  |  Height:  |  Size: 46 KiB

Binary file not shown.

Binary file not shown.

Before

Width:  |  Height:  |  Size: 84 KiB

After

Width:  |  Height:  |  Size: 84 KiB

File diff suppressed because it is too large Load Diff

Binary file not shown.

After

Width:  |  Height:  |  Size: 139 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 84 KiB

After

Width:  |  Height:  |  Size: 61 KiB

File diff suppressed because it is too large Load Diff

View File

@ -12,6 +12,7 @@
#+HTML_HEAD: <script src="../js/bootstrap.min.js"></script>
#+HTML_HEAD: <script src="../js/jquery.stickytableheaders.min.js"></script>
#+HTML_HEAD: <script src="../js/readtheorg.js"></script>
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="https://cdn.rawgit.com/dreampulse/computer-modern-web-font/master/fonts.css">
#+PROPERTY: header-args:matlab :session *MATLAB*
#+PROPERTY: header-args:matlab+ :tangle no
@ -42,56 +43,32 @@ Two sensors are considered with both different noise characteristics and dynamic
** Introduction :ignore:
In Figure [[fig:sensor_model_noise_uncertainty]] 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.
#+name: tab:sensor_signals
#+caption: Description of signals in Figure [[fig:sensor_model_noise_uncertainty]]
| *Notation* | *Meaning* |
|---------------+---------------------------------|
| $x$ | Physical measured quantity |
| $\tilde{n}_i$ | White noise with unitary PSD |
| $n_i$ | Shaped noise |
| $v_i$ | Sensor output measurement |
| $\hat{x}_i$ | Estimate of $x$ from the sensor |
| *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]$ |
#+name: tab:sensor_dynamical_blocks
#+caption: Description of Systems in Figure [[fig:sensor_model_noise_uncertainty]]
| *Notation* | *Meaning* |
|-------------+------------------------------------------------------------------------------|
| $\hat{G}_i$ | Nominal Sensor Dynamics |
| $W_i$ | Weight representing the size of the uncertainty at each frequency |
| $\Delta_i$ | Any complex perturbation such that $\vert\vert\Delta_i\vert\vert_\infty < 1$ |
| $N_i$ | Weight representing the sensor noise |
| *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]$ |
#+name: fig:sensor_model_noise_uncertainty
#+caption: Sensor Model
#+RESULTS:
[[file:figs-tikz/sensor_model_noise_uncertainty.png]]
In this example, the measured quantity $x$ is the velocity of an object.
The units of signals are listed in Table [[tab:signal_units]].
The units of systems are listed in Table [[tab:dynamical_block_units]].
#+name: tab:signal_units
#+caption: Units of signals in Figure [[fig:sensor_model_noise_uncertainty]]
| *Notation* | *Unit* |
|---------------+---------|
| $x$ | $[m/s]$ |
| $\tilde{n}_i$ | |
| $n_i$ | $[m/s]$ |
| $v_i$ | $[V]$ |
| $\hat{x}_i$ | $[m/s]$ |
#+name: tab:dynamical_block_units
#+caption: Units of Systems in Figure [[fig:sensor_model_noise_uncertainty]]
| *Notation* | *Unit* |
|------------------+-------------------|
| $\hat{G}_i$ | $[\frac{V}{m/s}]$ |
| $\hat{G}_i^{-1}$ | $[\frac{m/s}{V}]$ |
| $W_i$ | |
| $\Delta_i$ | |
| $N_i$ | $[m/s]$ |
** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
@ -269,7 +246,7 @@ The Power Spectral Density of the sensor noise $\Phi_{n_i}(\omega)$ is then comp
The weights $N_1$ and $N_2$ representing the amplitude spectral density of the sensor noises are defined below and shown in Figure [[fig:sensors_noise]].
#+begin_src matlab
omegac = 0.05*2*pi; G0 = 1e-1; Ginf = 1e-6;
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;
@ -431,10 +408,10 @@ If we first suppose that the models of the sensors $\hat{G}_i$ are very close to
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 N_1 \right|^2 + \left| H_2 N_2 \right|^2 \label{eq:super_sensor_psd_noise}
\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 eqref:eq:super_sensor_rms_noise.
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}
@ -470,16 +447,23 @@ The uncertainty set of the transfer function from $\hat{x}$ to $x$ at frequency
<<sec:optimal_comp_filters>>
** Introduction :ignore:
In this section, the two sensors are merged together using complementary
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.
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$.
#+name: fig:sensor_fusion_noise_arch
#+caption: Optimal Sensor Fusion Architecture
[[file:figs-tikz/sensor_fusion_noise_arch.png]]
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 [[sec:H2_synthesis]].
** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
@ -490,54 +474,51 @@ The complementary filters have to be designed in order to minimize the effect no
#+end_src
#+begin_src matlab
addpath('src');
load('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
#+end_src
** H-Two Synthesis
As $\tilde{n}_1$ and $\tilde{n}_2$ are normalized white noise: $\Phi_{\tilde{n}_1}(\omega) = \Phi_{\tilde{n}_2}(\omega) = 1$ and we have:
\[ \sigma_{\hat{x}} = \sqrt{\int_0^\infty |H_1 N_1|^2(\omega) + |H_2 N_2|^2(\omega) d\omega} = \left\| \begin{matrix} H_1 N_1 \\ H_2 N_2 \end{matrix} \right\|_2 \]
Thus, the goal is to design $H_1(s)$ and $H_2(s)$ such that $H_1(s) + H_2(s) = 1$ and such that $\left\| \begin{matrix} H_1 N_1 \\ H_2 N_2 \end{matrix} \right\|_2$ is minimized.
** $\mathcal{H}_2$ Synthesis
<<sec:H2_synthesis>>
For that, we use the $\mathcal{H}_2$ Synthesis.
We use the generalized plant architecture shown on Figure [[fig:h_two_optimal_fusion]].
Consider the generalized plant $P_{\mathcal{H}_2}$ shown in Figure [[fig:h_two_optimal_fusion]] and described by Equation eqref:eq:H2_generalized_plant.
#+name: fig:h_two_optimal_fusion
#+caption: Architecture used for $\mathcal{H}_\infty$ synthesis of complementary filters
[[file:figs-tikz/h_two_optimal_fusion.png]]
\begin{equation*}
\begin{equation} \label{eq:H2_generalized_plant}
\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
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*}
\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} \]
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}
Thus, if we minimize the $\mathcal{H}_2$ norm of this transfer function, we minimize the RMS value of $\hat{x}$.
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.
We define the generalized plant $P$ on matlab as shown on Figure
The generalized plant $P_{\mathcal{H}_2}$ is defined below
#+begin_src matlab
P = [N1 -N1;
0 N2;
1 0];
PH2 = [N1 -N1;
0 N2;
1 0];
#+end_src
And we do the $\mathcal{H}_2$ synthesis using the =h2syn= command.
The $\mathcal{H}_2$ synthesis using the =h2syn= command
#+begin_src matlab
[H2, ~, gamma] = h2syn(P, 1, 1);
[H2, ~, gamma] = h2syn(PH2, 1, 1);
#+end_src
Finally, we define $H_2(s) = 1 - H_1(s)$.
Finally, $H_1(s)$ is defined as follows
#+begin_src matlab
H1 = 1 - H2;
#+end_src
@ -547,185 +528,26 @@ Finally, we define $H_2(s) = 1 - H_1(s)$.
save('./mat/H2_filters.mat', 'H2', 'H1');
#+end_src
The complementary filters obtained are shown on Figure
The obtained complementary filters are shown in Figure [[fig:htwo_comp_filters]].
#+begin_src matlab :exports none
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
xlim([freqs(1), freqs(end)]);
legend('location', 'northeast');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/htwo_comp_filters.pdf', 'width', 'full', 'height', 'tall');
#+end_src
#+name: fig:htwo_comp_filters
#+caption: Obtained complementary filters using the $\mathcal{H}_2$ Synthesis ([[./figs/htwo_comp_filters.png][png]], [[./figs/htwo_comp_filters.pdf][pdf]])
#+RESULTS:
[[file:figs/htwo_comp_filters.png]]
** Sensor Noise
The PSD of the noise of the individual sensor and of the super sensor are shown in Figure [[fig:psd_sensors_htwo_synthesis]].
The Cumulative Power Spectrum (CPS) is shown on Figure [[fig:cps_h2_synthesis]].
The obtained RMS value of the super sensor is lower than the RMS value of the individual sensors.
#+begin_src matlab
PSD_S1 = abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2;
PSD_S2 = abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2;
PSD_H2 = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
CPS_S1 = cumtrapz(freqs, PSD_S1);
CPS_S2 = cumtrapz(freqs, PSD_S2);
CPS_H2 = cumtrapz(freqs, PSD_H2);
#+end_src
#+begin_src matlab :exports none
figure;
hold on;
plot(freqs, PSD_S1, '-', 'DisplayName', '$\Phi_{\hat{x}_1}$');
plot(freqs, PSD_S2, '-', 'DisplayName', '$\Phi_{\hat{x}_2}$');
plot(freqs, PSD_H2, 'k-', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_2}}$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Power Spectral Density [$(m/s)^2/Hz$]');
hold off;
xlim([freqs(1), freqs(end)]);
legend('location', 'northeast');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/psd_sensors_htwo_synthesis.pdf', 'width', 'full', 'height', 'tall');
#+end_src
#+name: fig:psd_sensors_htwo_synthesis
#+caption: Power Spectral Density of the estimated $\hat{x}$ using the two sensors alone and using the optimally fused signal ([[./figs/psd_sensors_htwo_synthesis.png][png]], [[./figs/psd_sensors_htwo_synthesis.pdf][pdf]])
#+RESULTS:
[[file:figs/psd_sensors_htwo_synthesis.png]]
#+begin_src matlab :exports none
figure;
hold on;
plot(freqs, CPS_S1, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_1} = %.1e$ [m/s rms]', sqrt(CPS_S1(end))));
plot(freqs, CPS_S2, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_2} = %.1e$ [m/s rms]', sqrt(CPS_S2(end))));
plot(freqs, CPS_H2, 'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_2}} = %.1e$ [m/s rms]', sqrt(CPS_H2(end))));
set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum');
hold off;
xlim([2*freqs(1), freqs(end)]);
legend('location', 'southeast');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/cps_h2_synthesis.pdf', 'width', 'full', 'height', 'tall');
#+end_src
#+name: fig:cps_h2_synthesis
#+caption: Cumulative Power Spectrum of individual sensors and super sensor using the $\mathcal{H}_2$ synthesis ([[./figs/cps_h2_synthesis.png][png]], [[./figs/cps_h2_synthesis.pdf][pdf]])
#+RESULTS:
[[file:figs/cps_h2_synthesis.png]]
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([sqrt(CPS_S1(end)), sqrt(CPS_S2(end)), sqrt(CPS_H2(end))]', {'Integrated Acceleration', 'Derived Position', 'Super Sensor - $\mathcal{H}_2$'}, {'RMS [m/s]'}, ' %.1e ');
#+end_src
#+RESULTS:
| | RMS [m/s] |
|--------------------------------+-----------|
| Integrated Acceleration | 0.005 |
| Derived Position | 0.08 |
| Super Sensor - $\mathcal{H}_2$ | 0.0012 |
** Time Domain Simulation
Parameters of the time domain simulation.
#+begin_src matlab
Fs = 1e4; % Sampling Frequency [Hz]
Ts = 1/Fs; % Sampling Time [s]
t = 0:Ts:2; % Time Vector [s]
#+end_src
Time domain velocity.
#+begin_src matlab
v = 0.1*sin((10*t).*t)';
#+end_src
Generate noises in velocity corresponding to sensor 1 and 2:
#+begin_src matlab
n1 = lsim(N1, sqrt(Fs/2)*randn(length(t), 1), t);
n2 = lsim(N2, sqrt(Fs/2)*randn(length(t), 1), t);
#+end_src
#+begin_src matlab :exports none
figure;
hold on;
set(gca,'ColorOrderIndex',2)
plot(t, n2, 'DisplayName', 'Differentiated Position');
set(gca,'ColorOrderIndex',1)
plot(t, n1, 'DisplayName', 'Integrated Acceleration');
set(gca,'ColorOrderIndex',3)
plot(t, (lsim(H1, n1, t)+lsim(H2, n2, t)), 'k-', 'DisplayName', 'Super Sensor');
hold off;
xlabel('Time [s]'); ylabel('Velocity [m/s]');
legend();
#+end_src
#+begin_src matlab :exports none
figure;
hold on;
set(gca,'ColorOrderIndex',2)
plot(t, v+n2, 'DisplayName', 'Differentiated Position');
set(gca,'ColorOrderIndex',1)
plot(t, v+n1, 'DisplayName', 'Integrated Acceleration');
set(gca,'ColorOrderIndex',3)
plot(t, v+(lsim(H1, n1, t)+lsim(H2, n2, t)), 'DisplayName', 'Super Sensor');
plot(t, v, 'k--', 'DisplayName', 'True Velocity');
hold off;
xlabel('Time [s]'); ylabel('Velocity [m/s]');
legend();
ylim([-0.3, 0.3]);
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/super_sensor_time_domain_h2.pdf', 'width', 'full', 'height', 'tall');
#+end_src
#+name: fig:super_sensor_time_domain_h2
#+caption: Noise of individual sensors and noise of the super sensor
#+RESULTS:
[[file:figs/super_sensor_time_domain_h2.png]]
** Discrepancy between sensor dynamics and model
#+begin_src matlab :exports none
Dphi_ss = 180/pi*asin(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))));
Dphi_ss(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))) > 1) = 360;
figure;
% Magnitude
ax1 = subplot(2,1,1);
hold on;
plotMagUncertainty(W1, freqs, 'color_i', 1);
plotMagUncertainty(W2, freqs, 'color_i', 2);
p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))+abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))-abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 0.001))], 'w');
p.EdgeColor = 'black'; p.FaceAlpha = 0;
plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), 'DisplayName', '$H_1$');
plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), 'DisplayName', '$H_2$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');
ylim([1e-2, 1e1]);
hold off;
legend('location', 'northeast');
% Phase
ax2 = subplot(2,1,2);
hold on;
plotPhaseUncertainty(W1, freqs, 'color_i', 1);
plotPhaseUncertainty(W2, freqs, 'color_i', 2);
p = patch([freqs flip(freqs)], [Dphi_ss; flip(-Dphi_ss)], 'w');
p.EdgeColor = 'black'; p.FaceAlpha = 0;
plot(freqs, 180/pi*angle(squeeze(freqresp(H1, freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(H2, freqs, 'Hz'))));
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
@ -735,10 +557,214 @@ Generate noises in velocity corresponding to sensor 1 and 2:
xlim([freqs(1), freqs(end)]);
#+end_src
** Conclusion
From the above complementary filter design with the $\mathcal{H}_2$ and $\mathcal{H}_\infty$ synthesis, it still seems that the $\mathcal{H}_2$ synthesis gives the complementary filters that permits to obtain the minimal super sensor noise (when measuring with the $\mathcal{H}_2$ norm).
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/htwo_comp_filters.pdf', 'width', 'full', 'height', 'tall');
#+end_src
However, the synthesis does not take into account the robustness of the sensor fusion.
#+name: fig:htwo_comp_filters
#+caption: Obtained complementary filters using the $\mathcal{H}_2$ Synthesis
#+RESULTS:
[[file:figs/htwo_comp_filters.png]]
** Super Sensor Noise
<<sec:H2_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 [[fig:psd_sensors_htwo_synthesis]].
#+begin_src matlab
PSD_S1 = abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2;
PSD_S2 = abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2;
PSD_H2 = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2 + ...
abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
#+end_src
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 [[fig:cps_h2_synthesis]].
#+begin_src matlab
CPS_S1 = cumtrapz(freqs, PSD_S1);
CPS_S2 = cumtrapz(freqs, PSD_S2);
CPS_H2 = cumtrapz(freqs, PSD_H2);
#+end_src
\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 [[tab:rms_noise_H2]].
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([sqrt(CPS_S1(end)); sqrt(CPS_S2(end)); sqrt(CPS_H2(end))], {'$\sigma_{n_1}$', '$\sigma_{n_2}$', '$\sigma_{n_{\mathcal{H}_2}}$'}, {'RMS value $[m/s]$'}, ' %.1e ');
#+end_src
#+name: tab:rms_noise_H2
#+caption: RMS value of the individual sensor noise and of the super sensor using the $\mathcal{H}_2$ Synthesis
#+RESULTS:
| | RMS value $[m/s]$ |
|------------------------------+-------------------|
| $\sigma_{n_1}$ | 0.015 |
| $\sigma_{n_2}$ | 0.08 |
| $\sigma_{n_{\mathcal{H}_2}}$ | 0.0027 |
#+begin_src matlab :exports none
figure;
hold on;
plot(freqs, PSD_S1, '-', 'DisplayName', '$\Phi_{n_1}$');
plot(freqs, PSD_S2, '-', 'DisplayName', '$\Phi_{n_2}$');
plot(freqs, PSD_H2, 'k-', 'DisplayName', '$\Phi_{n_{\mathcal{H}_2}}$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Power Spectral Density [$(m/s)^2/Hz$]');
hold off;
xlim([freqs(1), freqs(end)]);
legend('location', 'northeast');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/psd_sensors_htwo_synthesis.pdf', 'width', 'wide', 'height', 'normal');
#+end_src
#+name: fig:psd_sensors_htwo_synthesis
#+caption: Power Spectral Density of the estimated $\hat{x}$ using the two sensors alone and using the optimally fused signal
#+RESULTS:
[[file:figs/psd_sensors_htwo_synthesis.png]]
#+begin_src matlab :exports none
figure;
hold on;
plot(freqs, CPS_S1, '-', 'DisplayName', '$\Gamma_{n_1}$');
plot(freqs, CPS_S2, '-', 'DisplayName', '$\Gamma_{n_2}$');
plot(freqs, CPS_H2, 'k-', 'DisplayName', '$\Gamma_{n_{\mathcal{H}_2}}$');
set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum $[(m/s)^2]$');
hold off;
xlim([2*freqs(1), freqs(end)]);
legend('location', 'southeast');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/cps_h2_synthesis.pdf', 'width', 'wide', 'height', 'normal');
#+end_src
#+name: fig:cps_h2_synthesis
#+caption: Cumulative Power Spectrum of individual sensors and super sensor using the $\mathcal{H}_2$ synthesis
#+RESULTS:
[[file:figs/cps_h2_synthesis.png]]
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 [[fig:super_sensor_time_domain_h2]].
The resulting noises are displayed in Figure [[fig:sensor_noise_H2_time_domain]].
#+begin_src matlab :exports none
Fs = 1e4; % Sampling Frequency [Hz]
Ts = 1/Fs; % Sampling Time [s]
t = 0:Ts:2; % Time Vector [s]
v = 0.1*sin((10*t).*t)'; % Velocity measured [m/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);
#+end_src
#+begin_src matlab :exports none
figure;
hold on;
set(gca,'ColorOrderIndex',2)
plot(t, v + n2, 'DisplayName', '$\hat{x}_2$');
set(gca,'ColorOrderIndex',1)
plot(t, v + n1, 'DisplayName', '$\hat{x}_1$');
set(gca,'ColorOrderIndex',3)
plot(t, v + (lsim(H1, n1, t) + lsim(H2, n2, t)), 'DisplayName', '$\hat{x}$');
plot(t, v, 'k--', 'DisplayName', '$x$');
hold off;
xlabel('Time [s]'); ylabel('Velocity [m/s]');
legend('location', 'southwest');
ylim([-0.3, 0.3]);
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/super_sensor_time_domain_h2.pdf', 'width', 'wide', 'height', 'normal');
#+end_src
#+name: fig:super_sensor_time_domain_h2
#+caption: Noise of individual sensors and noise of the super sensor
#+RESULTS:
[[file:figs/super_sensor_time_domain_h2.png]]
#+begin_src matlab :exports none
figure;
hold on;
set(gca,'ColorOrderIndex',2)
plot(t, n2, 'DisplayName', '$n_2$');
set(gca,'ColorOrderIndex',1)
plot(t, n1, 'DisplayName', '$n_1$');
set(gca,'ColorOrderIndex',3)
plot(t, (lsim(H1, n1, t)+lsim(H2, n2, t)), '-', 'DisplayName', '$n$');
hold off;
xlabel('Time [s]'); ylabel('Sensor Noise [m/s]');
legend();
ylim([-0.2, 0.2]);
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/sensor_noise_H2_time_domain.pdf', 'width', 'wide', 'height', 'normal');
#+end_src
#+name: fig:sensor_noise_H2_time_domain
#+caption:
#+RESULTS:
[[file:figs/sensor_noise_H2_time_domain.png]]
** Discrepancy between sensor dynamics and model
If we consider sensor dynamical uncertainty as explained in Section [[sec:sensor_uncertainty]], 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 [[fig:super_sensor_dynamical_uncertainty_H2]].
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.
#+begin_src matlab :exports none
Dphi_ss = 180/pi*asin(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))));
Dphi_ss(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))) > 1) = 360;
figure;
% Magnitude
ax1 = subplot(2,1,1);
hold on;
plotMagUncertainty(W1, freqs, 'color_i', 1, 'DisplayName', '$1 + W_1 \Delta_1$');
plotMagUncertainty(W2, freqs, 'color_i', 2, 'DisplayName', '$1 + W_2 \Delta_2$');
plot(freqs, 1 + abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))+abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 'k-', ...
'DisplayName', '$1 + W_1 \Delta_1 + W_2 \Delta_2$')
plot(freqs, max(1 - abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))-abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 0.001), 'k-', ...
'HandleVisibility', 'off');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');
ylim([1e-2, 1e1]);
legend('location', 'southeast');
hold off;
% Phase
ax2 = subplot(2,1,2);
hold on;
plotPhaseUncertainty(W1, freqs, 'color_i', 1);
plotPhaseUncertainty(W2, freqs, 'color_i', 2);
plot(freqs, Dphi_ss, 'k-');
plot(freqs, -Dphi_ss, 'k-');
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/super_sensor_dynamical_uncertainty_H2.pdf', 'width', 'full', 'height', 'full');
#+end_src
#+name: fig:super_sensor_dynamical_uncertainty_H2
#+caption: Super sensor dynamical uncertainty when using the $\mathcal{H}_2$ Synthesis
#+RESULTS:
[[file:figs/super_sensor_dynamical_uncertainty_H2.png]]
* Robust Sensor Fusion: $\mathcal{H}_\infty$ Synthesis
:PROPERTIES:
@ -757,7 +783,7 @@ To do so, we model the uncertainty that we have on the sensor dynamics by multip
#+caption: Sensor fusion architecture with sensor dynamics uncertainty
[[file:figs-tikz/sensor_fusion_arch_uncertainty.png]]
The objective here is to design complementary filters $H_1(s)$ and $H_2(s)$ in order to minimize the dynamical uncertainty of the super sensor.
The objective here is to design complementary filters $H_1(s)$ and $H_2(s)$ in order to bound the dynamical uncertainty of the super sensor to acceptable values.
** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
@ -897,7 +923,7 @@ The obtained upper bounds on the complementary filters in order to limit the pha
#+end_src
#+NAME: fig:upper_bounds_comp_filter_max_phase_uncertainty
#+CAPTION: Upper bounds on the complementary filters set in order to limit the maximum phase uncertainty of the super sensor to 30 degrees until 500Hz ([[./figs/upper_bounds_comp_filter_max_phase_uncertainty.png][png]], [[./figs/upper_bounds_comp_filter_max_phase_uncertainty.pdf][pdf]])
#+CAPTION: Upper bounds on the complementary filters set in order to limit the maximum phase uncertainty of the super sensor to 30 degrees until 500Hz
[[file:figs/upper_bounds_comp_filter_max_phase_uncertainty.png]]
** $\mathcal{H}_\infty$ Synthesis
@ -910,8 +936,8 @@ The $\mathcal{H}_\infty$ synthesis architecture used for the complementary filte
The generalized plant is defined below.
#+begin_src matlab
P = [Wu*W1 -Wu*W1;
0 Wu*W2;
1 0];
0 Wu*W2;
1 0];
#+end_src
And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command.
@ -988,7 +1014,7 @@ The obtained complementary filters are shown in Figure [[fig:comp_filter_hinf_un
#+end_src
#+NAME: fig:comp_filter_hinf_uncertainty
#+CAPTION: Obtained complementary filters ([[./figs/comp_filter_hinf_uncertainty.png][png]], [[./figs/comp_filter_hinf_uncertainty.pdf][pdf]])
#+CAPTION: Obtained complementary filters
[[file:figs/comp_filter_hinf_uncertainty.png]]
** Super sensor uncertainty
@ -1081,7 +1107,7 @@ The CPS of both sensor and of the super sensor is shown in Figure [[fig:cps_sens
#+end_src
#+NAME: fig:psd_sensors_hinf_synthesis
#+CAPTION: Power Spectral Density of the obtained super sensor using the $\mathcal{H}_\infty$ synthesis ([[./figs/psd_sensors_hinf_synthesis.png][png]], [[./figs/psd_sensors_hinf_synthesis.pdf][pdf]])
#+CAPTION: Power Spectral Density of the obtained super sensor using the $\mathcal{H}_\infty$ synthesis
[[file:figs/psd_sensors_hinf_synthesis.png]]
#+begin_src matlab :exports none
@ -1104,7 +1130,7 @@ The CPS of both sensor and of the super sensor is shown in Figure [[fig:cps_sens
#+end_src
#+NAME: fig:cps_sensors_hinf_synthesis
#+CAPTION: Cumulative Power Spectrum of the obtained super sensor using the $\mathcal{H}_\infty$ synthesis ([[./figs/cps_sensors_hinf_synthesis.png][png]], [[./figs/cps_sensors_hinf_synthesis.cps][cps]])
#+CAPTION: Cumulative Power Spectrum of the obtained super sensor using the $\mathcal{H}_\infty$ synthesis
[[file:figs/cps_sensors_hinf_synthesis.png]]
** Conclusion
@ -1180,7 +1206,7 @@ Both dynamical uncertainty and noise characteristics of the individual sensors a
#+end_src
#+NAME: fig:mixed_synthesis_noise_uncertainty_sensors
#+CAPTION: Noise characteristsics and Dynamical uncertainty of the individual sensors ([[./figs/mixed_synthesis_noise_uncertainty_sensors.png][png]], [[./figs/mixed_synthesis_noise_uncertainty_sensors.pdf][pdf]])
#+CAPTION: Noise characteristsics and Dynamical uncertainty of the individual sensors
[[file:figs/mixed_synthesis_noise_uncertainty_sensors.png]]
** Weighting Functions on the uncertainty of the super sensor
@ -1287,7 +1313,7 @@ The obtained complementary filters are shown in Figure [[fig:comp_filters_mixed_
#+end_src
#+NAME: fig:comp_filters_mixed_synthesis
#+CAPTION: Obtained complementary filters after mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis ([[./figs/comp_filters_mixed_synthesis.png][png]], [[./figs/comp_filters_mixed_synthesis.pdf][pdf]])
#+CAPTION: Obtained complementary filters after mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis
[[file:figs/comp_filters_mixed_synthesis.png]]
** Obtained Super Sensor's noise
@ -1322,7 +1348,7 @@ The PSD and CPS of the super sensor's noise are shown in Figure [[fig:psd_super_
#+end_src
#+NAME: fig:psd_super_sensor_mixed_syn
#+CAPTION: Power Spectral Density of the Super Sensor obtained with the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis ([[./figs/psd_super_sensor_mixed_syn.png][png]], [[./figs/psd_super_sensor_mixed_syn.pdf][pdf]])
#+CAPTION: Power Spectral Density of the Super Sensor obtained with the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis
[[file:figs/psd_super_sensor_mixed_syn.png]]
#+begin_src matlab :exports none
@ -1345,7 +1371,7 @@ The PSD and CPS of the super sensor's noise are shown in Figure [[fig:psd_super_
#+end_src
#+NAME: fig:cps_super_sensor_mixed_syn
#+CAPTION: Cumulative Power Spectrum of the Super Sensor obtained with the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis ([[./figs/cps_super_sensor_mixed_syn.png][png]], [[./figs/cps_super_sensor_mixed_syn.pdf][pdf]])
#+CAPTION: Cumulative Power Spectrum of the Super Sensor obtained with the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis
[[file:figs/cps_super_sensor_mixed_syn.png]]
** Obtained Super Sensor's Uncertainty
@ -1503,7 +1529,7 @@ This Matlab function is accessible [[file:src/plotMagUncertainty.m][here]].
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.opacity (1,1) double {mustBeNumeric, mustBeNonnegative} = 0.3
args.DisplayName char = ''
end

View File

@ -19,7 +19,7 @@ arguments
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.opacity (1,1) double {mustBeNumeric, mustBeNonnegative} = 0.3
args.DisplayName char = ''
end