dehaeze20_optim_robus_compl.../matlab/index.org

1513 lines
60 KiB
Org Mode
Raw Normal View History

2019-08-21 16:34:47 +02:00
#+TITLE: Robust and Optimal Sensor Fusion - Matlab Computation
2019-08-14 12:08:30 +02:00
:DRAWER:
#+HTML_LINK_HOME: ../index.html
#+HTML_LINK_UP: ../index.html
#+LATEX_CLASS: cleanreport
#+LATEX_CLASS_OPTIONS: [tocnp, secbreak, minted]
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
#+HTML_HEAD: <script src="../js/jquery.min.js"></script>
#+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>
#+PROPERTY: header-args:matlab :session *MATLAB*
2019-09-03 09:01:59 +02:00
#+PROPERTY: header-args:matlab+ :tangle no
2019-08-14 12:08:30 +02:00
#+PROPERTY: header-args:matlab+ :comments org
#+PROPERTY: header-args:matlab+ :exports both
#+PROPERTY: header-args:matlab+ :results none
#+PROPERTY: header-args:matlab+ :eval no-export
#+PROPERTY: header-args:matlab+ :noweb yes
#+PROPERTY: header-args:matlab+ :mkdirp yes
#+PROPERTY: header-args:matlab+ :output-dir figs
:END:
* Introduction :ignore:
2019-09-03 09:01:59 +02:00
In this document, the optimal and robust design of complementary filters is studied.
2019-08-14 12:08:30 +02:00
2019-09-03 09:01:59 +02:00
Two sensors are considered with both different noise characteristics and dynamical uncertainties represented by multiplicative input uncertainty.
2019-08-14 12:08:30 +02:00
2020-09-30 08:47:27 +02:00
- Section [[sec:optimal_comp_filters]]: the $\mathcal{H}_2$ synthesis is used to design complementary filters such that the RMS value of the super sensor's noise is minimized
- Section [[sec:comp_filter_robustness]]: the $\mathcal{H}_\infty$ synthesis is used to design complementary filters such that the super sensor's uncertainty is bonded to acceptable values
- Section [[sec:mixed_synthesis_sensor_fusion]]: the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis is used to both limit the super sensor's uncertainty and to lower the RMS value of the super sensor's noise
2019-08-14 12:08:30 +02:00
2020-09-30 08:47:27 +02:00
* Sensor Description
2020-10-01 12:39:05 +02:00
:PROPERTIES:
:header-args:matlab+: :tangle matlab/sensor_description.m
:header-args:matlab+: :comments org :mkdirp yes
:END:
<<sec:sensor_description>>
2019-08-14 12:08:30 +02:00
** Introduction :ignore:
In Figure [[fig:sensor_model_noise_uncertainty]] is shown a schematic of a sensor model that is used in the following study.
#+name: tab:sensor_signals
#+caption: Description of signals in Figure [[fig:sensor_model_noise_uncertainty]]
| *Notation* | *Meaning* |
|---------------+---------------------------------|
| $x$ | Physical measured quantity |
| $\tilde{n}_i$ | White noise with unitary PSD |
| $n_i$ | Shaped noise |
| $v_i$ | Sensor output measurement |
| $\hat{x}_i$ | Estimate of $x$ from the sensor |
#+name: tab:sensor_dynamical_blocks
#+caption: Description of Systems in Figure [[fig:sensor_model_noise_uncertainty]]
| *Notation* | *Meaning* |
|-------------+------------------------------------------------------------------------------|
| $\hat{G}_i$ | Nominal Sensor Dynamics |
| $W_i$ | Weight representing the size of the uncertainty at each frequency |
| $\Delta_i$ | Any complex perturbation such that $\vert\vert\Delta_i\vert\vert_\infty < 1$ |
| $N_i$ | Weight representing the sensor noise |
#+name: fig:sensor_model_noise_uncertainty
#+caption: Sensor Model
#+RESULTS:
[[file:figs-tikz/sensor_model_noise_uncertainty.png]]
In this example, the measured quantity $x$ is the velocity of an object.
The units of signals are listed in Table [[tab:signal_units]].
The units of systems are listed in Table [[tab:dynamical_block_units]].
#+name: tab:signal_units
#+caption: Units of signals in Figure [[fig:sensor_model_noise_uncertainty]]
| *Notation* | *Unit* |
|---------------+---------|
| $x$ | $[m/s]$ |
| $\tilde{n}_i$ | |
| $n_i$ | $[m/s]$ |
| $v_i$ | $[V]$ |
| $\hat{x}_i$ | $[m/s]$ |
#+name: tab:dynamical_block_units
#+caption: Units of Systems in Figure [[fig:sensor_model_noise_uncertainty]]
| *Notation* | *Unit* |
|------------------+-------------------|
| $\hat{G}_i$ | $[\frac{V}{m/s}]$ |
| $\hat{G}_i^{-1}$ | $[\frac{m/s}{V}]$ |
| $W_i$ | |
| $\Delta_i$ | |
| $N_i$ | $[m/s]$ |
2019-08-14 12:08:30 +02:00
** Matlab Init :noexport:ignore:
2019-08-14 12:08:30 +02:00
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
#+end_src
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src
#+begin_src matlab
addpath('src');
2020-09-30 08:47:27 +02:00
freqs = logspace(0, 4, 1000);
2019-08-14 12:08:30 +02:00
#+end_src
2020-09-30 08:47:27 +02:00
** Sensor Dynamics
<<sec:sensor_dynamics>>
Let's consider two sensors measuring the velocity of an object.
The first sensor is an accelerometer.
Its nominal dynamics $\hat{G}_1(s)$ is defined below.
2020-09-30 08:47:27 +02:00
#+begin_src matlab
m_acc = 0.01; % Inertial Mass [kg]
c_acc = 5; % Damping [N/(m/s)]
k_acc = 1e5; % Stiffness [N/m]
g_acc = 1e5; % Gain [V/m]
2019-08-14 12:08:30 +02:00
G1 = -g_acc*m_acc*s/(m_acc*s^2 + c_acc*s + k_acc); % Accelerometer Plant [V/(m/s)]
#+end_src
The second sensor is a displacement sensor, its nominal dynamics $\hat{G}_2(s)$ is defined below.
#+begin_src matlab
w_pos = 2*pi*2e3; % Measurement Banwdith [rad/s]
g_pos = 1e4; % Gain [V/m]
G2 = g_pos/s/(1 + s/w_pos); % Position Sensor Plant [V/(m/s)]
2020-09-30 08:47:27 +02:00
#+end_src
2019-08-14 12:08:30 +02:00
These nominal dynamics are also taken as the model of the sensor dynamics.
The true sensor dynamics has some uncertainty associated to it and described in section [[sec:sensor_uncertainty]].
Both sensor dynamics in $[\frac{V}{m/s}]$ are shown in Figure [[fig:sensors_nominal_dynamics]].
2020-09-30 08:47:27 +02:00
#+begin_src matlab :exports none
figure;
% Magnitude
ax1 = subplot(2,1,1);
hold on;
plot(freqs, abs(squeeze(freqresp(G1, freqs, 'Hz'))), '-', 'DisplayName', '$G_1(j\omega)$');
plot(freqs, abs(squeeze(freqresp(G2, freqs, 'Hz'))), '-', 'DisplayName', '$G_2(j\omega)$');
2020-09-30 08:47:27 +02:00
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude $[\frac{V}{m/s}]$'); set(gca, 'XTickLabel',[]);
2020-09-30 08:47:27 +02:00
legend('location', 'northeast');
hold off;
2020-09-30 08:47:27 +02:00
% Phase
ax2 = subplot(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G1, freqs, 'Hz'))), '-');
plot(freqs, 180/pi*angle(squeeze(freqresp(G2, freqs, 'Hz'))), '-');
2020-09-30 08:47:27 +02:00
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/sensors_nominal_dynamics.pdf', 'width', 'full', 'height', 'full');
2020-09-30 08:47:27 +02:00
#+end_src
2019-08-14 12:08:30 +02:00
#+name: fig:sensors_nominal_dynamics
#+caption: Sensor nominal dynamics from the velocity of the object to the output voltage
#+RESULTS:
[[file:figs/sensors_nominal_dynamics.png]]
** Sensor Model Uncertainty
<<sec:sensor_uncertainty>>
The uncertainty on the sensor dynamics is described by multiplicative uncertainty (Figure [[fig:sensor_model_noise_uncertainty]]).
2019-08-14 12:08:30 +02:00
The true sensor dynamics $G_i(s)$ is then described by eqref:eq:sensor_dynamics_uncertainty.
2019-08-14 12:08:30 +02:00
\begin{equation}
G_i(s) = \hat{G}_i(s) \left( 1 + W_i(s) \Delta_i(s) \right); \quad |\Delta_i(j\omega)| < 1 \forall \omega \label{eq:sensor_dynamics_uncertainty}
\end{equation}
The weights $W_i(s)$ representing the dynamical uncertainty are defined below and their magnitude is shown in Figure [[fig:sensors_uncertainty_weights]].
2020-09-30 08:47:27 +02:00
#+begin_src matlab
W1 = createWeight('n', 2, 'w0', 2*pi*3, 'G0', 2, 'G1', 0.1, 'Gc', 1) * ...
createWeight('n', 2, 'w0', 2*pi*1e3, 'G0', 1, 'G1', 4/0.1, 'Gc', 1/0.1);
2020-09-30 08:47:27 +02:00
W2 = createWeight('n', 2, 'w0', 2*pi*1e2, 'G0', 0.05, 'G1', 4, 'Gc', 1);
2019-08-14 12:08:30 +02:00
#+end_src
The bode plot of the sensors nominal dynamics as well as their defined dynamical spread are shown in Figure [[fig:sensors_nominal_dynamics_and_uncertainty]].
2020-09-30 08:47:27 +02:00
#+begin_src matlab :exports none
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(W1, freqs, 'Hz'))), 'DisplayName', '$|W_1(j\omega)|$');
plot(freqs, abs(squeeze(freqresp(W2, freqs, 'Hz'))), 'DisplayName', '$|W_2(j\omega)|$');
2020-09-30 08:47:27 +02:00
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
ylim([0, 5]);
2020-09-30 08:47:27 +02:00
xlim([freqs(1), freqs(end)]);
legend('location', 'northwest');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/sensors_uncertainty_weights.pdf', 'width', 'wide', 'height', 'normal');
2020-09-30 08:47:27 +02:00
#+end_src
#+name: fig:sensors_uncertainty_weights
#+caption: Magnitude of the multiplicative uncertainty weights $|W_i(j\omega)|$
#+RESULTS:
[[file:figs/sensors_uncertainty_weights.png]]
2020-09-30 08:47:27 +02:00
#+begin_src matlab :exports none
figure;
% Magnitude
ax1 = subplot(2,1,1);
hold on;
plotMagUncertainty(W1, freqs, 'G', G1, 'color_i', 1, 'DisplayName', '$G_1$');
plotMagUncertainty(W2, freqs, 'G', G2, 'color_i', 2, 'DisplayName', '$G_2$');
set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(G1, freqs, 'Hz'))), 'DisplayName', '$\hat{G}_1$');
plot(freqs, abs(squeeze(freqresp(G2, freqs, 'Hz'))), 'DisplayName', '$\hat{G}_2$');
2020-09-30 08:47:27 +02:00
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude $[\frac{V}{m/s}]$');
ylim([1e-2, 2e3]);
legend('location', 'northeast');
2020-09-30 08:47:27 +02:00
hold off;
% Phase
ax2 = subplot(2,1,2);
hold on;
plotPhaseUncertainty(W1, freqs, 'G', G1, 'color_i', 1);
plotPhaseUncertainty(W2, freqs, 'G', G2, 'color_i', 2);
set(gca,'ColorOrderIndex',1)
plot(freqs, 180/pi*angle(squeeze(freqresp(G1, freqs, 'Hz'))), 'DisplayName', '$\hat{G}_1$');
plot(freqs, 180/pi*angle(squeeze(freqresp(G2, freqs, 'Hz'))), 'DisplayName', '$\hat{G}_2$');
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/sensors_nominal_dynamics_and_uncertainty.pdf', 'width', 'full', 'height', 'full');
#+end_src
#+name: fig:sensors_nominal_dynamics_and_uncertainty
#+caption: Nominal Sensor Dynamics $\hat{G}_i$ (solid lines) as well as the spread of the dynamical uncertainty (background color)
#+RESULTS:
[[file:figs/sensors_nominal_dynamics_and_uncertainty.png]]
** Sensor Noise
<<sec:sensor_noise>>
The noise of the sensors $n_i$ are modelled by shaping a white noise with unitary PSD $\tilde{n}_i$ eqref:eq:unitary_noise_psd with a LTI transfer function $N_i(s)$ (Figure [[fig:sensor_model_noise_uncertainty]]).
\begin{equation}
\Phi_{\tilde{n}_i}(\omega) = 1 \label{eq:unitary_noise_psd}
\end{equation}
The Power Spectral Density of the sensor noise $\Phi_{n_i}(\omega)$ is then computed using eqref:eq:sensor_noise_shaping and expressed in $[\frac{(m/s)^2}{Hz}]$.
\begin{equation}
\Phi_{n_i}(\omega) = \left| N_i(j\omega) \right|^2 \Phi_{\tilde{n}_i}(\omega) \label{eq:sensor_noise_shaping}
\end{equation}
The weights $N_1$ and $N_2$ representing the amplitude spectral density of the sensor noises are defined below and shown in Figure [[fig:sensors_noise]].
#+begin_src matlab
omegac = 0.05*2*pi; G0 = 1e-1; Ginf = 1e-6;
N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4);
omegac = 1000*2*pi; G0 = 1e-6; Ginf = 1e-3;
N2 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4);
#+end_src
#+begin_src matlab :exports none
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(N1, freqs, 'Hz'))), '-', 'DisplayName', '$|N_1(j\omega)|$');
plot(freqs, abs(squeeze(freqresp(N2, freqs, 'Hz'))), '-', 'DisplayName', '$|N_2(j\omega)|$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude Spectral Density $\left[ \frac{m/s}{\sqrt{Hz}} \right]$');
hold off;
xlim([freqs(1), freqs(end)]);
legend('location', 'northeast');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/sensors_noise.pdf', 'width', 'normal', 'height', 'normal');
#+end_src
#+name: fig:sensors_noise
#+caption: Amplitude spectral density of the sensors $\sqrt{\Phi_{n_i}(\omega)} = |N_i(j\omega)|$
#+RESULTS:
[[file:figs/sensors_noise.png]]
** Save Model
All the dynamical systems representing the sensors are saved for further use.
#+begin_src matlab
save('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
#+end_src
* First Order Complementary Filters :noexport:
** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
#+end_src
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src
#+begin_src matlab
load('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
#+end_src
** Complementary Filters
#+begin_src matlab
wc = 2*pi*400;
H1 = s/wc/(1 + s/wc);
H2 = 1/(1 + s/wc);
#+end_src
#+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
G2_u = G2*(1 + W2*ultidyn('Delta',[1 1]));
G1_u = G1*(1 + W1*ultidyn('Delta',[1 1]));
Gss_u = H1*inv(G1)*G1_u + H2*inv(G2)*G2_u;
#+end_src
#+begin_src matlab :exports none
Dphi1 = 180/pi*asin(abs(squeeze(freqresp(W1, freqs, 'Hz'))));
Dphi1(abs(squeeze(freqresp(W1, freqs, 'Hz'))) > 1) = 360;
Dphi2 = 180/pi*asin(abs(squeeze(freqresp(W2, freqs, 'Hz'))));
Dphi2(abs(squeeze(freqresp(W2, freqs, 'Hz'))) > 1) = 360;
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;
p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W1, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W1, freqs, 'Hz'))), 1e-6))], 'w');
2020-09-30 08:47:27 +02:00
p.FaceColor = [0 0.4470 0.7410];
p.EdgeColor = 'none';
p.FaceAlpha = 0.3;
p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W2, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W2, freqs, 'Hz'))), 0.001))], 'w');
2020-09-30 08:47:27 +02:00
p.FaceColor = [0.8500 0.3250 0.0980];
p.EdgeColor = 'none';
p.FaceAlpha = 0.3;
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;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');
ylim([1e-2, 1e1]);
hold off;
% Phase
ax2 = subplot(2,1,2);
hold on;
p = patch([freqs flip(freqs)], [Dphi1; flip(-Dphi1)], 'w');
p.FaceColor = [0 0.4470 0.7410]; p.EdgeColor = 'none'; p.FaceAlpha = 0.3;
p = patch([freqs flip(freqs)], [Dphi2; flip(-Dphi2)], 'w');
p.FaceColor = [0.8500 0.3250 0.0980]; p.EdgeColor = 'none'; p.FaceAlpha = 0.3;
p = patch([freqs flip(freqs)], [Dphi_ss; flip(-Dphi_ss)], 'w');
p.EdgeColor = 'black'; p.FaceAlpha = 0;
2020-09-30 08:47:27 +02:00
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
* Optimal Super Sensor Noise: $\mathcal{H}_2$ Synthesis with Acc and Pos
:PROPERTIES:
:header-args:matlab+: :tangle matlab/optimal_comp_filters.m
:header-args:matlab+: :comments org :mkdirp yes
:END:
<<sec:optimal_comp_filters>>
** Introduction :ignore:
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.
2020-10-01 12:39:05 +02:00
#+name: fig:sensor_fusion_noise_arch
#+caption: Optimal Sensor Fusion Architecture
[[file:figs-tikz/sensor_fusion_noise_arch.png]]
2020-09-30 08:47:27 +02:00
** Matlab Init :noexport:ignore:
2020-09-30 08:47:27 +02:00
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
#+end_src
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src
#+begin_src matlab
load('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
2020-09-30 08:47:27 +02:00
#+end_src
2019-08-14 12:08:30 +02:00
** 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.
2020-10-01 12:36:25 +02:00
We use the generalized plant architecture shown on Figure [[fig:h_two_optimal_fusion]].
#+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]]
2019-08-14 12:08:30 +02:00
\begin{equation*}
\begin{pmatrix}
z \\ v
\end{pmatrix} = \begin{pmatrix}
0 & N_2 & 1 \\
N_1 & -N_2 & 0
\end{pmatrix} \begin{pmatrix}
2020-09-30 08:47:27 +02:00
W_1 \\ W_2 \\ u
\end{pmatrix}
\end{equation*}
2019-08-14 12:08:30 +02:00
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} \]
2019-08-14 12:08:30 +02:00
If we define $H_2 = 1 - H_1$, we obtain:
\[ \begin{bmatrix} N_1 H_1 \\ N_2 H_2 \end{bmatrix} \]
2019-08-14 12:08:30 +02:00
Thus, if we minimize the $\mathcal{H}_2$ norm of this transfer function, we minimize the RMS value of $\hat{x}$.
We define the generalized plant $P$ on matlab as shown on figure [[fig:h_infinity_optimal_comp_filters]].
#+begin_src matlab
P = [N1 -N1;
0 N2;
1 0];
2019-08-14 12:08:30 +02:00
#+end_src
And we do the $\mathcal{H}_2$ synthesis using the =h2syn= command.
#+begin_src matlab
[H2, ~, gamma] = h2syn(P, 1, 1);
2019-08-14 12:08:30 +02:00
#+end_src
Finally, we define $H_2(s) = 1 - H_1(s)$.
2019-08-14 12:08:30 +02:00
#+begin_src matlab
H1 = 1 - H2;
2019-08-14 12:08:30 +02:00
#+end_src
2020-09-30 08:47:27 +02:00
#+begin_src matlab :exports none
% Filters are saved for further use
save('./mat/H2_filters.mat', 'H2', 'H1');
2020-09-30 08:47:27 +02:00
#+end_src
2019-08-14 12:08:30 +02:00
2020-09-30 08:47:27 +02:00
The complementary filters obtained are shown on figure [[fig:htwo_comp_filters]].
2019-08-14 12:08:30 +02:00
#+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$');
2019-08-14 12:08:30 +02:00
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
2020-09-30 08:47:27 +02:00
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/htwo_comp_filters.pdf', 'width', 'full', 'height', 'tall');
2019-08-14 12:08:30 +02:00
#+end_src
2020-09-30 08:47:27 +02:00
#+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:
2019-08-14 12:08:30 +02:00
[[file:figs/htwo_comp_filters.png]]
2020-09-30 08:47:27 +02:00
** Sensor Noise
2020-10-01 12:36:25 +02:00
The PSD of the noise of the individual sensor and of the super sensor are shown in Figure [[fig:psd_sensors_htwo_synthesis]].
2020-09-30 08:47:27 +02:00
2020-10-01 12:36:25 +02:00
The Cumulative Power Spectrum (CPS) is shown on Figure [[fig:cps_h2_synthesis]].
2020-09-30 08:47:27 +02:00
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;
2020-09-30 08:47:27 +02:00
CPS_S1 = cumtrapz(freqs, PSD_S1);
CPS_S2 = cumtrapz(freqs, PSD_S2);
CPS_H2 = cumtrapz(freqs, PSD_H2);
#+end_src
2019-08-14 12:08:30 +02:00
#+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}}$');
2019-08-14 12:08:30 +02:00
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
2020-09-30 08:47:27 +02:00
xlabel('Frequency [Hz]'); ylabel('Power Spectral Density [$(m/s)^2/Hz$]');
2019-08-14 12:08:30 +02:00
hold off;
xlim([freqs(1), freqs(end)]);
legend('location', 'northeast');
#+end_src
2020-09-30 08:47:27 +02:00
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/psd_sensors_htwo_synthesis.pdf', 'width', 'full', 'height', 'tall');
2019-08-14 12:08:30 +02:00
#+end_src
2020-09-30 08:47:27 +02:00
#+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:
2019-08-14 12:08:30 +02:00
[[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))));
2020-09-30 08:47:27 +02:00
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;
2020-09-30 08:47:27 +02:00
xlim([2*freqs(1), freqs(end)]);
legend('location', 'southeast');
#+end_src
2020-09-30 08:47:27 +02:00
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/cps_h2_synthesis.pdf', 'width', 'full', 'height', 'tall');
#+end_src
2020-09-30 08:47:27 +02:00
#+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]]
2020-09-30 08:47:27 +02:00
#+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 ');
2020-09-30 08:47:27 +02:00
#+end_src
2020-09-30 08:47:27 +02:00
#+RESULTS:
| | RMS [m/s] |
|--------------------------------+-----------|
| Integrated Acceleration | 0.005 |
| Derived Position | 0.08 |
2020-09-30 08:47:27 +02:00
| Super Sensor - $\mathcal{H}_2$ | 0.0012 |
2020-09-30 08:47:27 +02:00
** Time Domain Simulation
Parameters of the time domain simulation.
#+begin_src matlab
Fs = 1e4; % Sampling Frequency [Hz]
Ts = 1/Fs; % Sampling Time [s]
2020-09-30 08:47:27 +02:00
t = 0:Ts:2; % Time Vector [s]
#+end_src
2020-09-30 08:47:27 +02:00
Time domain velocity.
#+begin_src matlab
v = 0.1*sin((10*t).*t)';
#+end_src
2020-09-30 08:47:27 +02:00
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);
2020-09-30 08:47:27 +02:00
#+end_src
2020-09-30 08:47:27 +02:00
#+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');
2020-09-30 08:47:27 +02:00
hold off;
xlabel('Time [s]'); ylabel('Velocity [m/s]');
legend();
#+end_src
2020-09-30 08:47:27 +02:00
#+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');
2020-09-30 08:47:27 +02:00
plot(t, v, 'k--', 'DisplayName', 'True Velocity');
hold off;
xlabel('Time [s]'); ylabel('Velocity [m/s]');
legend();
ylim([-0.3, 0.3]);
#+end_src
2020-09-30 08:47:27 +02:00
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/super_sensor_time_domain_h2.pdf', 'width', 'full', 'height', 'tall');
#+end_src
2020-09-30 08:47:27 +02:00
#+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]]
2020-09-30 08:47:27 +02:00
** 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;
2020-09-30 08:47:27 +02:00
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;
2020-09-30 08:47:27 +02:00
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');
ylim([1e-2, 1e1]);
2020-09-30 08:47:27 +02:00
hold off;
2020-09-30 08:47:27 +02:00
% Phase
ax2 = subplot(2,1,2);
hold on;
plotPhaseUncertainty(W1, freqs, 'color_i', 1);
plotPhaseUncertainty(W2, freqs, 'color_i', 2);
2020-09-30 08:47:27 +02:00
p = patch([freqs flip(freqs)], [Dphi_ss; flip(-Dphi_ss)], 'w');
p.EdgeColor = 'black'; p.FaceAlpha = 0;
2020-09-30 08:47:27 +02:00
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
2020-09-30 08:47:27 +02:00
** 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.
2020-09-30 08:47:27 +02:00
* Robust Sensor Fusion: $\mathcal{H}_\infty$ Synthesis with Acc and Pos
:PROPERTIES:
:header-args:matlab+: :tangle matlab/comp_filter_robustness.m
:header-args:matlab+: :comments org :mkdirp yes
:END:
<<sec:comp_filter_robustness>>
** Introduction :ignore:
2020-09-30 08:47:27 +02:00
We initially considered perfectly known sensor dynamics so that it can be perfectly inverted.
2020-09-30 08:47:27 +02:00
We now take into account the fact that the sensor dynamics is only partially known.
2020-10-01 12:36:25 +02:00
To do so, we model the uncertainty that we have on the sensor dynamics by multiplicative input uncertainty as shown in Figure [[fig:sensor_fusion_arch_uncertainty]].
2020-09-30 08:47:27 +02:00
2020-10-01 12:36:25 +02:00
#+name: fig:sensor_fusion_arch_uncertainty
2020-09-30 08:47:27 +02:00
#+caption: Sensor fusion architecture with sensor dynamics uncertainty
2020-10-01 12:36:25 +02:00
[[file:figs-tikz/sensor_fusion_arch_uncertainty.png]]
2020-09-30 08:47:27 +02:00
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.
** Matlab Init :noexport:ignore:
2020-09-30 08:47:27 +02:00
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
#+end_src
2020-09-30 08:47:27 +02:00
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src
2020-09-30 08:47:27 +02:00
#+begin_src matlab
addpath('src');
load('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
2020-09-30 08:47:27 +02:00
#+end_src
2020-09-30 08:47:27 +02:00
** Super Sensor Dynamical Uncertainty
In practical systems, the sensor dynamics has always some level of uncertainty.
2020-09-30 08:47:27 +02:00
The dynamics of the super sensor is represented by
\begin{align*}
2020-09-30 08:47:27 +02:00
\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*}
2020-09-30 08:47:27 +02:00
with $\Delta_i$ is any transfer function satisfying $\| \Delta_i \|_\infty < 1$.
2020-09-30 08:47:27 +02:00
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}$.
2020-09-30 08:47:27 +02:00
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 [[fig:uncertainty_gain_phase_variation]]).
2020-09-30 08:47:27 +02:00
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) \]
2020-09-30 08:47:27 +02:00
#+name: fig:uncertainty_gain_phase_variation
#+caption: Maximum phase variation
[[file:figs-tikz/uncertainty_gain_phase_variation.png]]
2020-09-30 08:47:27 +02:00
** Synthesis objective
2020-10-01 12:36:25 +02:00
The uncertainty region of the super sensor dynamics is represented by a circle in the complex plane as shown in Figure [[fig:uncertainty_gain_phase_variation]].
2020-09-30 08:47:27 +02:00
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)|$.
2020-09-30 08:47:27 +02:00
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) \]
2020-09-30 08:47:27 +02:00
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 \]
2020-09-30 08:47:27 +02:00
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) \]
2020-09-30 08:47:27 +02:00
The maximum phase shift due to dynamic uncertainty at frequency $\omega$ will be $\Delta\phi_\text{max}(\omega)$.
** 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}$):
#+name: eq:hinf_conf_phase_uncertainty
\begin{equation}
\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$.
** 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.
#+begin_src matlab
Dphi = 10; % [deg]
2020-09-30 08:47:27 +02:00
Wu = createWeight('n', 2, 'w0', 2*pi*4e2, 'G0', 1/sin(Dphi*pi/180), 'G1', 1/4, 'Gc', 1);
#+end_src
#+begin_src matlab
save('./mat/Wu.mat', 'Wu');
#+end_src
#+begin_src matlab :exports none
2020-09-30 08:47:27 +02:00
figure;
% Magnitude
ax1 = subplot(2,1,1);
hold on;
plotMagUncertainty(W1, freqs, 'color_i', 1);
plotMagUncertainty(W2, freqs, 'color_i', 2);
p = plotMagUncertainty(inv(Wu), freqs, 'color_i', 3);
p.EdgeColor = 'black'; p.FaceAlpha = 0;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
2020-09-30 08:47:27 +02:00
ylabel('Magnitude');
ylim([1e-2, 1e1]);
hold off;
2020-09-30 08:47:27 +02:00
% Phase
ax2 = subplot(2,1,2);
hold on;
plotPhaseUncertainty(W1, freqs, 'color_i', 1);
plotPhaseUncertainty(W2, freqs, 'color_i', 2);
p = plotPhaseUncertainty(inv(Wu), freqs);
p.EdgeColor = 'black'; p.FaceAlpha = 0;
2020-09-30 08:47:27 +02:00
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
2020-09-30 08:47:27 +02:00
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
2020-09-30 08:47:27 +02:00
#+end_src
2020-10-01 12:36:25 +02:00
The obtained upper bounds on the complementary filters in order to limit the phase uncertainty of the super sensor are represented in Figure [[fig:upper_bounds_comp_filter_max_phase_uncertainty]].
2020-09-30 08:47:27 +02:00
#+begin_src matlab :exports none
figure;
hold on;
plot(freqs, 1./abs(squeeze(freqresp(Wu*W1, freqs, 'Hz'))), '-', 'DisplayName', '$1/|W_1W_\phi|$');
plot(freqs, 1./abs(squeeze(freqresp(Wu*W2, freqs, 'Hz'))), '-', 'DisplayName', '$1/|W_2W_\phi|$');
2020-09-30 08:47:27 +02:00
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
#+HEADER: :tangle no :exports results :results none :noweb yes
2020-09-30 08:47:27 +02:00
#+begin_src matlab :var filepath="figs/upper_bounds_comp_filter_max_phase_uncertainty.pdf" :var figsize="full-normal" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
2020-09-30 08:47:27 +02:00
#+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]])
[[file:figs/upper_bounds_comp_filter_max_phase_uncertainty.png]]
2020-09-30 08:47:27 +02:00
** $\mathcal{H}_\infty$ Synthesis
2020-10-01 12:36:25 +02:00
The $\mathcal{H}_\infty$ synthesis architecture used for the complementary filters is shown in Figure [[fig:h_infinity_robust_fusion]].
2019-09-03 09:01:59 +02:00
2020-09-30 08:47:27 +02:00
#+name: fig:h_infinity_robust_fusion
#+caption: Architecture used for $\mathcal{H}_\infty$ synthesis of complementary filters
[[file:figs-tikz/h_infinity_robust_fusion.png]]
2019-09-03 09:01:59 +02:00
2020-09-30 08:47:27 +02:00
The generalized plant is defined below.
2019-09-03 09:01:59 +02:00
#+begin_src matlab
P = [Wu*W1 -Wu*W1;
0 Wu*W2;
1 0];
2019-09-03 09:01:59 +02:00
#+end_src
And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command.
#+begin_src matlab :results output replace :exports both
[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
2019-09-03 09:01:59 +02:00
#+end_src
#+RESULTS:
#+begin_example
[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
2020-09-30 08:47:27 +02:00
Test bounds: 0.7071 <= gamma <= 1.291
2020-09-30 08:47:27 +02:00
gamma X>=0 Y>=0 rho(XY)<1 p/f
9.554e-01 0.0e+00 0.0e+00 3.529e-16 p
8.219e-01 0.0e+00 0.0e+00 5.204e-16 p
7.624e-01 3.8e-17 0.0e+00 1.955e-15 p
7.342e-01 0.0e+00 0.0e+00 5.612e-16 p
7.205e-01 0.0e+00 0.0e+00 7.184e-16 p
7.138e-01 0.0e+00 0.0e+00 0.000e+00 p
7.104e-01 4.1e-16 0.0e+00 6.749e-15 p
7.088e-01 0.0e+00 0.0e+00 2.794e-15 p
7.079e-01 0.0e+00 0.0e+00 6.503e-16 p
7.075e-01 0.0e+00 0.0e+00 4.302e-15 p
Best performance (actual): 0.7071
2019-09-03 09:01:59 +02:00
#+end_example
2020-09-30 08:47:27 +02:00
And $H_1(s)$ is defined as the complementary of $H_2(s)$.
2019-09-03 09:01:59 +02:00
#+begin_src matlab
H1 = 1 - H2;
2020-09-30 08:47:27 +02:00
#+end_src
#+begin_src matlab :exports none
save('./mat/Hinf_filters.mat', 'H2', 'H1');
2019-09-03 09:01:59 +02:00
#+end_src
2020-10-01 12:36:25 +02:00
The obtained complementary filters are shown in Figure [[fig:comp_filter_hinf_uncertainty]].
2019-09-03 09:01:59 +02:00
#+begin_src matlab :exports none
figure;
ax1 = subplot(2,1,1);
hold on;
plot(freqs, 1./abs(squeeze(freqresp(Wu*W1, freqs, 'Hz'))), '--', 'DisplayName', '$|WuW_1|$');
plot(freqs, 1./abs(squeeze(freqresp(Wu*W2, freqs, 'Hz'))), '--', 'DisplayName', '$|WuW_2|$');
2019-09-03 09:01:59 +02:00
set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$');
2019-09-03 09:01:59 +02:00
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude');
set(gca, 'XTickLabel',[]);
legend('location', 'northeast');
ax2 = subplot(2,1,2);
hold on;
plot(freqs, 180/pi*phase(squeeze(freqresp(H1, freqs, 'Hz'))), '-');
plot(freqs, 180/pi*phase(squeeze(freqresp(H2, freqs, 'Hz'))), '-');
2019-09-03 09:01:59 +02:00
hold off;
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
set(gca, 'XScale', 'log');
yticks([-360:90:360]);
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
xticks([0.1, 1, 10, 100, 1000]);
#+end_src
#+HEADER: :tangle no :exports results :results none :noweb yes
2020-09-30 08:47:27 +02:00
#+begin_src matlab :var filepath="figs/comp_filter_hinf_uncertainty.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
2019-09-03 09:01:59 +02:00
<<plt-matlab>>
#+end_src
2020-09-30 08:47:27 +02:00
#+NAME: fig:comp_filter_hinf_uncertainty
#+CAPTION: Obtained complementary filters ([[./figs/comp_filter_hinf_uncertainty.png][png]], [[./figs/comp_filter_hinf_uncertainty.pdf][pdf]])
[[file:figs/comp_filter_hinf_uncertainty.png]]
** Super sensor uncertainty
#+begin_src matlab
H2_filters = load('./mat/H2_filters.mat', 'H2', 'H1');
2020-09-30 08:47:27 +02:00
#+end_src
2020-09-30 08:47:27 +02:00
#+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;
Dphi_ss_H2 = 180/pi*asin(abs(squeeze(freqresp(W2*H2_filters.H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H2_filters.H1, freqs, 'Hz'))));
Dphi_ss_H2(abs(squeeze(freqresp(W2*H2_filters.H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H2_filters.H1, freqs, 'Hz'))) > 1) = 360;
2020-09-30 08:47:27 +02:00
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;
p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W2*H2_filters.H2, freqs, 'Hz')))+abs(squeeze(freqresp(W1*H2_filters.H1, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W2*H2_filters.H2, freqs, 'Hz')))-abs(squeeze(freqresp(W1*H2_filters.H1, freqs, 'Hz'))), 0.001))], 'w');
p.EdgeColor = 'black'; p.FaceAlpha = 0; p.LineStyle = '--';
2020-09-30 08:47:27 +02:00
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');
ylim([1e-2, 1e1]);
2020-09-30 08:47:27 +02:00
hold off;
% 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;
p = patch([freqs flip(freqs)], [Dphi_ss_H2; flip(-Dphi_ss_H2)], 'w');
p.EdgeColor = 'black'; p.FaceAlpha = 0; p.LineStyle = '--';
2020-09-30 08:47:27 +02:00
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)]);
2020-09-30 08:47:27 +02:00
#+end_src
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.
2020-09-30 08:47:27 +02:00
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.
** Super sensor noise
2020-09-30 08:47:27 +02:00
We now compute the obtain Power Spectral Density of the super sensor's noise.
The noise characteristics of both individual sensor are defined below.
2020-10-01 12:36:25 +02:00
The PSD of both sensor and of the super sensor is shown in Figure [[fig:psd_sensors_hinf_synthesis]].
The CPS of both sensor and of the super sensor is shown in Figure [[fig:cps_sensors_hinf_synthesis]].
2020-09-30 08:47:27 +02:00
#+begin_src matlab
PSD_S2 = abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2;
PSD_S1 = abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2;
PSD_Hinf = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
PSD_H2 = abs(squeeze(freqresp(N1*H2_filters.H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2_filters.H2, freqs, 'Hz'))).^2;
CPS_S2 = cumtrapz(freqs, PSD_S2);
CPS_S1 = cumtrapz(freqs, PSD_S1);
CPS_Hinf = cumtrapz(freqs, PSD_Hinf);
CPS_H2 = cumtrapz(freqs, PSD_H2);
2020-09-30 08:47:27 +02:00
#+end_src
#+begin_src matlab :exports none
figure;
hold on;
plot(freqs, PSD_S2, '-', 'DisplayName', '$\Phi_{\hat{x}_{pos}}$');
plot(freqs, PSD_S1, '-', 'DisplayName', '$\Phi_{\hat{x}_{acc}}$');
plot(freqs, PSD_Hinf, 'k-', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_\infty}}$');
plot(freqs, PSD_H2, 'k--', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_2}}$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
2020-09-30 08:47:27 +02:00
xlabel('Frequency [Hz]'); ylabel('Power Spectral Density [$(m/s)^2/Hz$]');
hold off;
xlim([freqs(1), freqs(end)]);
legend('location', 'northeast');
#+end_src
#+HEADER: :tangle no :exports results :results none :noweb yes
2020-09-30 08:47:27 +02:00
#+begin_src matlab :var filepath="figs/psd_sensors_hinf_synthesis.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
2020-09-30 08:47:27 +02:00
#+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]])
[[file:figs/psd_sensors_hinf_synthesis.png]]
#+begin_src matlab :exports none
figure;
hold on;
plot(freqs, CPS_S2, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{pos}} = %.1e$ [m/s rms]', sqrt(CPS_S2(end))));
plot(freqs, CPS_S1, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{acc}} = %.1e$ [m/s rms]', sqrt(CPS_S1(end))));
2020-09-30 08:47:27 +02:00
plot(freqs, CPS_Hinf, 'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_\\infty}} = %.1e$ [m/s rms]', sqrt(CPS_Hinf(end))));
set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum');
hold off;
2020-09-30 08:47:27 +02:00
xlim([2*freqs(1), freqs(end)]);
% ylim([1e-10 1e-5]);
legend('location', 'southeast');
#+end_src
#+HEADER: :tangle no :exports results :results none :noweb yes
2020-09-30 08:47:27 +02:00
#+begin_src matlab :var filepath="figs/cps_sensors_hinf_synthesis.cps" :var figsize="full-tall" :post cps2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
2020-09-30 08:47:27 +02:00
#+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]])
[[file:figs/cps_sensors_hinf_synthesis.png]]
2020-09-30 08:47:27 +02:00
** Conclusion
Using the $\mathcal{H}_\infty$ synthesis, the dynamical uncertainty of the super sensor can be bounded to acceptable values.
2020-09-30 08:47:27 +02:00
However, the RMS of the super sensor noise is not optimized as it was the case with the $\mathcal{H}_2$ synthesis
2020-09-30 08:47:27 +02:00
* Optimal and Robust Sensor Fusion: Mixed $\mathcal{H}_2/\mathcal{H}_\infty$ Synthesis with Acc and Pos
:PROPERTIES:
:header-args:matlab+: :tangle matlab/mixed_synthesis_sensor_fusion.m
:header-args:matlab+: :comments org :mkdirp yes
:END:
<<sec:mixed_synthesis_sensor_fusion>>
2020-10-01 12:36:25 +02:00
** Introduction :ignore:
#+name: fig:sensor_fusion_arch_full
#+caption: Sensor fusion architecture with sensor dynamics uncertainty
[[file:figs-tikz/sensor_fusion_arch_full.png]]
2020-09-30 08:47:27 +02:00
** 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= ([[https://fr.mathworks.com/help/robust/ref/h2hinfsyn.html][doc]]).
** 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>>
#+end_src
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src
#+begin_src matlab
load('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
load('./mat/Wu.mat', 'Wu');
#+end_src
2020-09-30 08:47:27 +02:00
** Noise characteristics and Uncertainty of the individual sensors
2020-10-01 12:36:25 +02:00
Both dynamical uncertainty and noise characteristics of the individual sensors are shown in Figure [[fig:mixed_synthesis_noise_uncertainty_sensors]].
2020-09-30 08:47:27 +02:00
#+begin_src matlab :exports none
2020-09-30 08:47:27 +02:00
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(N2, freqs, 'Hz'))), '-', 'DisplayName', '$|N_{pos}(j\omega)|$');
plot(freqs, abs(squeeze(freqresp(N1, freqs, 'Hz'))), '-', 'DisplayName', '$|N_{acc}(j\omega)|$');
2020-09-30 08:47:27 +02:00
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
legend('location', 'northeast');
ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, abs(squeeze(freqresp(W2, freqs, 'Hz'))), '-', 'DisplayName', '$|W_{pos}(j\omega)|$');
plot(freqs, abs(squeeze(freqresp(W1, freqs, 'Hz'))), '-', 'DisplayName', '$|W_{acc}(j\omega)|$');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
hold off;
legend('location', 'northeast');
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
#+end_src
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/mixed_synthesis_noise_uncertainty_sensors.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+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]])
[[file:figs/mixed_synthesis_noise_uncertainty_sensors.png]]
** 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.
2020-10-01 12:36:25 +02:00
The maximum wanted multiplicative uncertainty is shown in Figure .The idea here is that we don't really need low uncertainty at low frequency but only near the crossover frequency that is suppose to be around 300Hz here.
2020-09-30 08:47:27 +02:00
** Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis
2020-10-01 12:36:25 +02:00
The synthesis architecture that is used here is shown in Figure [[fig:mixed_h2_hinf_synthesis]].
2020-09-30 08:47:27 +02:00
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
2020-09-30 08:47:27 +02:00
#+name: fig:mixed_h2_hinf_synthesis
2020-10-01 12:36:25 +02:00
#+caption: Mixed $\mathcal{H}_2/\mathcal{H}_\infty$ Synthesis
2020-09-30 08:47:27 +02:00
[[file:figs-tikz/mixed_h2_hinf_synthesis.png]]
2019-08-14 12:08:30 +02:00
2020-09-30 08:47:27 +02:00
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*}
2019-08-14 12:08:30 +02:00
2020-09-30 08:47:27 +02:00
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$
2019-08-14 12:08:30 +02:00
2020-09-30 08:47:27 +02:00
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.
2019-08-14 12:08:30 +02:00
2020-09-30 08:47:27 +02:00
We define the generalized plant that will be used for the mixed synthesis.
#+begin_src matlab
W1u = ss(W2*Wu); W2u = ss(W1*Wu); % Weight on the uncertainty
W1n = ss(N2); W2n = ss(N1); % Weight on the noise
2019-08-14 12:08:30 +02:00
2020-09-30 08:47:27 +02:00
P = [W1u -W1u;
0 W2u;
W1n -W1n;
0 W2n;
1 0];
#+end_src
2019-08-14 12:08:30 +02:00
2020-09-30 08:47:27 +02:00
The mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis is performed below.
#+begin_src matlab
Nmeas = 1; Ncon = 1; Nz2 = 2;
[H1, ~, normz, ~] = h2hinfsyn(P, Nmeas, Ncon, Nz2, [0, 1], 'HINFMAX', 1, 'H2MAX', Inf, 'DKMAX', 100, 'TOL', 0.01, 'DISPLAY', 'on');
2020-09-30 08:47:27 +02:00
H2 = 1 - H1;
2019-08-14 12:08:30 +02:00
#+end_src
2020-09-30 08:47:27 +02:00
#+begin_src matlab :exports none
save('./mat/H2_Hinf_filters.mat', 'H2', 'H1');
2019-08-14 12:08:30 +02:00
#+end_src
2020-10-01 12:36:25 +02:00
The obtained complementary filters are shown in Figure [[fig:comp_filters_mixed_synthesis]].
2019-08-14 12:08:30 +02:00
2020-09-30 08:47:27 +02:00
#+begin_src matlab :exports none
figure;
2019-08-14 12:08:30 +02:00
2020-09-30 08:47:27 +02:00
ax1 = subplot(2,1,1);
hold on;
set(gca,'ColorOrderIndex',1)
plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$W_1$');
2020-09-30 08:47:27 +02:00
set(gca,'ColorOrderIndex',2)
plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$W_2$');
2019-08-14 12:08:30 +02:00
2020-09-30 08:47:27 +02:00
set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
2020-09-30 08:47:27 +02:00
set(gca,'ColorOrderIndex',2)
plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$');
2019-08-14 12:08:30 +02:00
2020-09-30 08:47:27 +02:00
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude');
set(gca, 'XTickLabel',[]);
ylim([1e-3, 2]);
legend('location', 'southwest');
2019-08-14 12:08:30 +02:00
2020-09-30 08:47:27 +02:00
ax2 = subplot(2,1,2);
hold on;
set(gca,'ColorOrderIndex',1)
plot(freqs, 180/pi*phase(squeeze(freqresp(H2, freqs, 'Hz'))), '-');
2020-09-30 08:47:27 +02:00
set(gca,'ColorOrderIndex',2)
plot(freqs, 180/pi*phase(squeeze(freqresp(H1, freqs, 'Hz'))), '-');
2020-09-30 08:47:27 +02:00
hold off;
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
set(gca, 'XScale', 'log');
yticks([-360:90:360]);
2019-08-14 12:08:30 +02:00
2020-09-30 08:47:27 +02:00
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
xticks([0.1, 1, 10, 100, 1000]);
#+end_src
2019-08-14 12:08:30 +02:00
2020-09-30 08:47:27 +02:00
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/comp_filters_mixed_synthesis.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
2020-09-30 08:47:27 +02:00
#+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]])
[[file:figs/comp_filters_mixed_synthesis.png]]
2020-09-30 08:47:27 +02:00
** Obtained Super Sensor's noise
2020-10-01 12:36:25 +02:00
The PSD and CPS of the super sensor's noise are shown in Figure [[fig:psd_super_sensor_mixed_syn]] and Figure [[fig:cps_super_sensor_mixed_syn]] respectively.
#+begin_src matlab
PSD_S2 = abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2;
PSD_S1 = abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2;
PSD_H2Hinf = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
CPS_S2 = cumtrapz(freqs, PSD_S2);
CPS_S1 = cumtrapz(freqs, PSD_S1);
CPS_H2Hinf = cumtrapz(freqs, PSD_H2Hinf);
2020-09-30 08:47:27 +02:00
#+end_src
2020-09-30 08:47:27 +02:00
#+begin_src matlab :exports none
figure;
hold on;
plot(freqs, PSD_S2, '-', 'DisplayName', '$\Phi_{\hat{x}_{pos}}$');
plot(freqs, PSD_S1, '-', 'DisplayName', '$\Phi_{\hat{x}_{acc}}$');
2020-09-30 08:47:27 +02:00
plot(freqs, PSD_H2Hinf, 'k-', 'DisplayName', '$\Phi_{\hat{x}_{\mathcal{H}_2/\mathcal{H}_\infty}}$');
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
2020-09-30 08:47:27 +02:00
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/psd_super_sensor_mixed_syn.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+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]])
[[file:figs/psd_super_sensor_mixed_syn.png]]
#+begin_src matlab :exports none
2020-09-30 08:47:27 +02:00
figure;
hold on;
plot(freqs, CPS_S2, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{pos}} = %.1e$ [m/s rms]', sqrt(CPS_S2(end))));
plot(freqs, CPS_S1, '-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{acc}} = %.1e$ [m/s rms]', sqrt(CPS_S1(end))));
2020-09-30 08:47:27 +02:00
plot(freqs, CPS_H2Hinf, 'k-', 'DisplayName', sprintf('$\\sigma_{\\hat{x}_{\\mathcal{H}_\\infty/\\mathcal{H}_\\infty}} = %.1e$ [m/s rms]', sqrt(CPS_H2Hinf(end))));
set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum');
hold off;
xlim([2*freqs(1), freqs(end)]);
% ylim([1e-10 1e-5]);
legend('location', 'southeast');
#+end_src
2020-09-30 08:47:27 +02:00
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/cps_super_sensor_mixed_syn.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
2020-09-30 08:47:27 +02:00
#+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]])
[[file:figs/cps_super_sensor_mixed_syn.png]]
** Obtained Super Sensor's Uncertainty
2020-10-01 12:36:25 +02:00
The uncertainty on the super sensor's dynamics is shown in Figure
2020-09-30 08:47:27 +02:00
#+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;
2020-09-30 08:47:27 +02:00
figure;
% Magnitude
2019-09-03 09:13:16 +02:00
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;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');
ylim([1e-2, 1e1]);
hold off;
% Phase
2019-09-03 09:13:16 +02:00
ax2 = subplot(2,1,2);
hold on;
plotPhaseUncertainty(W1, freqs, 'color_i', 1);
plotPhaseUncertainty(W2, freqs, 'color_i', 2);
2020-09-30 08:47:27 +02:00
p = patch([freqs flip(freqs)], [Dphi_ss; flip(-Dphi_ss)], 'w');
p.EdgeColor = 'black'; p.FaceAlpha = 0;
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
2020-09-30 08:47:27 +02:00
xlim([freqs(1), freqs(end)]);
#+end_src
2020-09-30 08:47:27 +02:00
** Comparison Hinf H2 H2/Hinf
#+begin_src matlab
H2_filters = load('./mat/H2_filters.mat', 'H2', 'H1');
Hinf_filters = load('./mat/Hinf_filters.mat', 'H2', 'H1');
H2_Hinf_filters = load('./mat/H2_Hinf_filters.mat', 'H2', 'H1');
2020-09-30 08:47:27 +02:00
#+end_src
2020-09-30 08:47:27 +02:00
#+begin_src matlab
PSD_H2 = abs(squeeze(freqresp(N2*H2_filters.H2, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N1*H2_filters.H1, freqs, 'Hz'))).^2;
CPS_H2 = cumtrapz(freqs, PSD_H2);
PSD_Hinf = abs(squeeze(freqresp(N2*Hinf_filters.H2, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N1*Hinf_filters.H1, freqs, 'Hz'))).^2;
CPS_Hinf = cumtrapz(freqs, PSD_Hinf);
PSD_H2Hinf = abs(squeeze(freqresp(N2*H2_Hinf_filters.H2, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N1*H2_Hinf_filters.H1, freqs, 'Hz'))).^2;
CPS_H2Hinf = cumtrapz(freqs, PSD_H2Hinf);
2020-09-30 08:47:27 +02:00
#+end_src
2020-09-30 08:47:27 +02:00
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([sqrt(CPS_H2(end)), sqrt(CPS_Hinf(end)), sqrt(CPS_H2Hinf(end))]', {'Optimal: $\mathcal{H}_2$', 'Robust: $\mathcal{H}_\infty$', 'Mixed: $\mathcal{H}_2/\mathcal{H}_\infty$'}, {'RMS [m/s]'}, ' %.1e ');
#+end_src
2020-09-30 08:47:27 +02:00
#+RESULTS:
| | RMS [m/s] |
|-------------------------------------------+-----------|
| Optimal: $\mathcal{H}_2$ | 0.0012 |
| Robust: $\mathcal{H}_\infty$ | 0.041 |
| Mixed: $\mathcal{H}_2/\mathcal{H}_\infty$ | 0.011 |
2020-09-30 08:47:27 +02:00
** Conclusion
This synthesis methods allows both to:
- limit the dynamical uncertainty of the super sensor
- minimize the RMS value of the estimation
2020-10-01 12:39:05 +02:00
* Matlab Functions
<<sec:matlab_functions>>
** =createWeight=
:PROPERTIES:
:header-args:matlab+: :tangle src/createWeight.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:createWeight>>
This Matlab function is accessible [[file:src/createWeight.m][here]].
#+begin_src matlab
function [W] = createWeight(args)
% createWeight -
%
% Syntax: [in_data] = createWeight(in_data)
%
% Inputs:
% - n - Weight Order
% - G0 - Low frequency Gain
% - G1 - High frequency Gain
% - Gc - Gain of W at frequency w0
% - w0 - Frequency at which |W(j w0)| = Gc
%
% Outputs:
% - W - Generated Weight
arguments
args.n (1,1) double {mustBeInteger, mustBePositive} = 1
args.G0 (1,1) double {mustBeNumeric, mustBePositive} = 0.1
args.G1 (1,1) double {mustBeNumeric, mustBePositive} = 10
args.Gc (1,1) double {mustBeNumeric, mustBePositive} = 1
args.w0 (1,1) double {mustBeNumeric, mustBePositive} = 1
end
mustBeBetween(args.G0, args.Gc, args.G1);
s = tf('s');
W = (((1/args.w0)*sqrt((1-(args.G0/args.Gc)^(2/args.n))/(1-(args.Gc/args.G1)^(2/args.n)))*s + (args.G0/args.Gc)^(1/args.n))/((1/args.G1)^(1/args.n)*(1/args.w0)*sqrt((1-(args.G0/args.Gc)^(2/args.n))/(1-(args.Gc/args.G1)^(2/args.n)))*s + (1/args.Gc)^(1/args.n)))^args.n;
end
% Custom validation function
function mustBeBetween(a,b,c)
if ~((a > b && b > c) || (c > b && b > a))
eid = 'createWeight:inputError';
msg = 'Gc should be between G0 and G1.';
throwAsCaller(MException(eid,msg))
end
end
#+end_src
** =plotMagUncertainty=
:PROPERTIES:
:header-args:matlab+: :tangle src/plotMagUncertainty.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:plotMagUncertainty>>
This Matlab function is accessible [[file:src/plotMagUncertainty.m][here]].
#+begin_src matlab
function [p] = plotMagUncertainty(W, freqs, args)
% plotMagUncertainty -
%
% Syntax: [p] = plotMagUncertainty(W, freqs, args)
%
% Inputs:
% - W - Multiplicative Uncertainty Weight
% - freqs - Frequency Vector [Hz]
% - args - Optional Arguments:
% - G
% - color_i
% - opacity
%
% Outputs:
% - p - Plot Handle
arguments
W
freqs double {mustBeNumeric, mustBeNonnegative}
args.G = tf(1)
args.color_i (1,1) double {mustBeInteger, mustBePositive} = 1
args.opacity (1,1) double {mustBeNumeric, mustBePositive} = 0.3
args.DisplayName char = ''
end
% Get defaults colors
colors = get(groot, 'defaultAxesColorOrder');
p = patch([freqs flip(freqs)], ...
[abs(squeeze(freqresp(args.G, freqs, 'Hz'))).*(1 + abs(squeeze(freqresp(W, freqs, 'Hz')))); ...
flip(abs(squeeze(freqresp(args.G, freqs, 'Hz'))).*max(1 - abs(squeeze(freqresp(W, freqs, 'Hz'))), 1e-6))], 'w', ...
'DisplayName', args.DisplayName);
p.FaceColor = colors(args.color_i, :);
p.EdgeColor = 'none';
p.FaceAlpha = args.opacity;
end
#+end_src
** =plotPhaseUncertainty=
:PROPERTIES:
:header-args:matlab+: :tangle src/plotPhaseUncertainty.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:plotPhaseUncertainty>>
This Matlab function is accessible [[file:src/plotPhaseUncertainty.m][here]].
#+begin_src matlab
function [p] = plotPhaseUncertainty(W, freqs, args)
% plotPhaseUncertainty -
%
% Syntax: [p] = plotPhaseUncertainty(W, freqs, args)
%
% Inputs:
% - W - Multiplicative Uncertainty Weight
% - freqs - Frequency Vector [Hz]
% - args - Optional Arguments:
% - G
% - color_i
% - opacity
%
% Outputs:
% - p - Plot Handle
arguments
W
freqs double {mustBeNumeric, mustBeNonnegative}
args.G = tf(1)
args.color_i (1,1) double {mustBeInteger, mustBePositive} = 1
args.opacity (1,1) double {mustBeNumeric, mustBePositive} = 0.3
args.DisplayName char = ''
end
% Get defaults colors
colors = get(groot, 'defaultAxesColorOrder');
% Compute Phase Uncertainty
Dphi = 180/pi*asin(abs(squeeze(freqresp(W, freqs, 'Hz'))));
Dphi(abs(squeeze(freqresp(W, freqs, 'Hz'))) > 1) = 360;
% Compute Plant Phase
G_ang = 180/pi*angle(squeeze(freqresp(args.G, freqs, 'Hz')));
p = patch([freqs flip(freqs)], [G_ang+Dphi; flip(G_ang-Dphi)], 'w', ...
'DisplayName', args.DisplayName);
p.FaceColor = colors(args.color_i, :);
p.EdgeColor = 'none';
p.FaceAlpha = args.opacity;
end
#+end_src
2020-09-22 09:51:26 +02:00
* Bibliography :ignore:
2019-08-14 12:08:30 +02:00
bibliographystyle:unsrt
bibliography:ref.bib