Work on mixed synthesis
This commit is contained in:
parent
17c25e0ae2
commit
507de9f173
BIN
matlab/figs/htwo_hinf_comp_filters.pdf
Normal file
BIN
matlab/figs/htwo_hinf_comp_filters.pdf
Normal file
Binary file not shown.
BIN
matlab/figs/htwo_hinf_comp_filters.png
Normal file
BIN
matlab/figs/htwo_hinf_comp_filters.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 135 KiB |
BIN
matlab/figs/psd_sensors_htwo_hinf_synthesis.pdf
Normal file
BIN
matlab/figs/psd_sensors_htwo_hinf_synthesis.pdf
Normal file
Binary file not shown.
BIN
matlab/figs/psd_sensors_htwo_hinf_synthesis.png
Normal file
BIN
matlab/figs/psd_sensors_htwo_hinf_synthesis.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 76 KiB |
1874
matlab/figs/super_sensor_dynamical_uncertainty_Htwo_Hinf.pdf
Normal file
1874
matlab/figs/super_sensor_dynamical_uncertainty_Htwo_Hinf.pdf
Normal file
File diff suppressed because it is too large
Load Diff
BIN
matlab/figs/super_sensor_dynamical_uncertainty_Htwo_Hinf.png
Normal file
BIN
matlab/figs/super_sensor_dynamical_uncertainty_Htwo_Hinf.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 140 KiB |
BIN
matlab/figs/super_sensor_time_domain_h2_hinf.pdf
Normal file
BIN
matlab/figs/super_sensor_time_domain_h2_hinf.pdf
Normal file
Binary file not shown.
BIN
matlab/figs/super_sensor_time_domain_h2_hinf.png
Normal file
BIN
matlab/figs/super_sensor_time_domain_h2_hinf.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 60 KiB |
File diff suppressed because it is too large
Load Diff
276
matlab/index.org
276
matlab/index.org
@ -332,10 +332,6 @@ All the dynamical systems representing the sensors are saved for further use.
|
||||
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
|
||||
@ -603,20 +599,9 @@ The Power Spectral Density of the individual sensors' noise $\Phi_{n_1}, \Phi_{n
|
||||
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]$'}, ' %.3f ');
|
||||
data2orgtable([sqrt(trapz(freqs, PSD_S1)); sqrt(trapz(freqs, PSD_S2)); sqrt(trapz(freqs, PSD_H2))], {'$\sigma_{n_1}$', '$\sigma_{n_2}$', '$\sigma_{n_{\mathcal{H}_2}}$'}, {'RMS value $[m/s]$'}, ' %.3f ');
|
||||
#+end_src
|
||||
|
||||
#+name: tab:rms_noise_H2
|
||||
@ -637,7 +622,7 @@ The RMS value of the individual sensors and of the super sensor are listed in Ta
|
||||
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$]');
|
||||
xlabel('Frequency [Hz]'); ylabel('Power Spectral Density $\left[ \frac{(m/s)^2}{Hz} \right]$');
|
||||
hold off;
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
legend('location', 'northeast');
|
||||
@ -652,28 +637,6 @@ The RMS value of the individual sensors and of the super sensor are listed in Ta
|
||||
#+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]].
|
||||
@ -1109,13 +1072,6 @@ As expected, the super sensor obtained from the $\mathcal{H}_\infty$ synthesis i
|
||||
|
||||
PSD_H2 = abs(squeeze(freqresp(N1*H2_filters.H1, freqs, 'Hz'))).^2 + ...
|
||||
abs(squeeze(freqresp(N2*H2_filters.H2, freqs, 'Hz'))).^2;
|
||||
CPS_H2 = cumtrapz(freqs, PSD_H2);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
CPS_S2 = cumtrapz(freqs, PSD_S2);
|
||||
CPS_S1 = cumtrapz(freqs, PSD_S1);
|
||||
CPS_Hinf = cumtrapz(freqs, PSD_Hinf);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
@ -1126,7 +1082,7 @@ As expected, the super sensor obtained from the $\mathcal{H}_\infty$ synthesis i
|
||||
plot(freqs, PSD_H2, 'k-', 'DisplayName', '$\Phi_{n_{\mathcal{H}_2}}$');
|
||||
plot(freqs, PSD_Hinf, 'k--', 'DisplayName', '$\Phi_{n_{\mathcal{H}_\infty}}$');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Power Spectral Density [$(m/s)^2/Hz$]');
|
||||
xlabel('Frequency [Hz]'); ylabel('Power Spectral Density $\left[ \frac{(m/s)^2}{Hz} \right]$');
|
||||
hold off;
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
legend('location', 'northeast');
|
||||
@ -1142,7 +1098,7 @@ As expected, the super sensor obtained from the $\mathcal{H}_\infty$ synthesis i
|
||||
[[file:figs/psd_sensors_hinf_synthesis.png]]
|
||||
|
||||
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||||
data2orgtable([sqrt(CPS_H2(end)), sqrt(CPS_Hinf(end))]', {'Optimal: $\mathcal{H}_2$', 'Robust: $\mathcal{H}_\infty$'}, {'RMS [m/s]'}, ' %.1e ');
|
||||
data2orgtable([sqrt(trapz(freqs, PSD_H2)), sqrt(trapz(freqs, PSD_Hinf))]', {'Optimal: $\mathcal{H}_2$', 'Robust: $\mathcal{H}_\infty$'}, {'RMS [m/s]'}, ' %.1e ');
|
||||
#+end_src
|
||||
|
||||
#+name: tab:rms_noise_comp_H2_Hinf
|
||||
@ -1168,17 +1124,19 @@ However, the RMS of the super sensor noise is not optimized as it was the case w
|
||||
<<sec:mixed_synthesis_sensor_fusion>>
|
||||
** Introduction :ignore:
|
||||
|
||||
The (optima) $\mathcal{H}_2$ synthesis and the (robust) $\mathcal{H}_\infty$ synthesis are now combined to form an Optimal and Robust synthesis of complementary filters for sensor fusion.
|
||||
|
||||
The sensor fusion architecture is shown in Figure [[fig:sensor_fusion_arch_full]] ($\hat{G}_i$ are omitted for space reasons).
|
||||
|
||||
#+name: fig:sensor_fusion_arch_full
|
||||
#+caption: Sensor fusion architecture with sensor dynamics uncertainty
|
||||
[[file:figs-tikz/sensor_fusion_arch_full.png]]
|
||||
|
||||
The goal is to design complementary filters such that:
|
||||
- the maximum uncertainty of the super sensor is bounded
|
||||
- the maximum uncertainty of the super sensor is bounded to acceptable values (defined by $W_u(s)$)
|
||||
- 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]]).
|
||||
To do so, we can use the Mixed $\mathcal{H}_2/\mathcal{H}_\infty$ Synthesis presented in Section [[sec:H2_Hinf_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)
|
||||
@ -1195,53 +1153,44 @@ The Matlab function for that is =h2hinfsyn= ([[https://fr.mathworks.com/help/rob
|
||||
#+end_src
|
||||
|
||||
** Mixed $\mathcal{H}_2$ / $\mathcal{H}_\infty$ Synthesis
|
||||
<<sec:H2_Hinf_synthesis>>
|
||||
|
||||
The synthesis architecture that is used here is shown in Figure [[fig:mixed_h2_hinf_synthesis]].
|
||||
|
||||
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
|
||||
The filter $H_2(s)$ is synthesized such that it:
|
||||
- keeps the $\mathcal{H}_\infty$ norm of the transfer function from $w$ to $z_{\mathcal{H}_\infty}$ bellow some specified value
|
||||
- minimizes the $\mathcal{H}_2$ norm of the transfer function from $w$ to $z_{\mathcal{H}_2}$
|
||||
|
||||
#+name: fig:mixed_h2_hinf_synthesis
|
||||
#+caption: Mixed $\mathcal{H}_2/\mathcal{H}_\infty$ Synthesis
|
||||
[[file:figs-tikz/mixed_h2_hinf_synthesis.png]]
|
||||
|
||||
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*}
|
||||
Let's see that
|
||||
with $H_1(s)= 1 - H_2(s)$
|
||||
\begin{align}
|
||||
\left\| \frac{z_\infty}{w} \right\|_\infty &= \left\| \begin{matrix}H_1(s) W_1(s) W_u(s)\\ H_2(s) W_2(s) W_u(s)\end{matrix} \right\|_\infty \\
|
||||
\left\| \frac{z_2}{w} \right\|_2 &= \left\| \begin{matrix}H_1(s) N_1(s) \\ H_2(s) N_2(s)\end{matrix} \right\|_2 = \sigma_n
|
||||
\end{align}
|
||||
|
||||
Then:
|
||||
- we specify the maximum value for the $\mathcal{H}_\infty$ norm between $w$ and $z_\infty$ to be $1$
|
||||
- we don't specify any maximum value for the $\mathcal{H}_2$ norm between $w$ and $z_2$
|
||||
- we choose $W_1 = 0$ and $W_2 = 1$ such that the objective is to minimize the $\mathcal{H}_2$ norm between $w$ and $z_2$
|
||||
|
||||
The synthesis objective is to have:
|
||||
\[ \left\| \frac{z_\infty}{w} \right\|_\infty = \left\| \begin{matrix}W_1(s) H_1(s) \\ W_2(s) H_2(s)\end{matrix} \right\|_\infty < 1 \]
|
||||
and to minimize:
|
||||
\[ \left\| \frac{z_2}{w} \right\|_2 = \left\| \begin{matrix}N_1(s) H_1(s) \\ N_2(s) H_2(s)\end{matrix} \right\|_2 \]
|
||||
which is what we wanted.
|
||||
|
||||
We define the generalized plant that will be used for the mixed synthesis.
|
||||
The generalized plant $P_{\mathcal{H}_2/\mathcal{H}_\infty}$ is defined below
|
||||
#+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
|
||||
|
||||
P = [W1u -W1u;
|
||||
0 W2u;
|
||||
W1n -W1n;
|
||||
0 W2n;
|
||||
P = [Wu*W1 -Wu*W1;
|
||||
0 Wu*W2;
|
||||
N1 -N1;
|
||||
0 N2;
|
||||
1 0];
|
||||
#+end_src
|
||||
|
||||
The mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis is performed below.
|
||||
And the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis is performed.
|
||||
#+begin_src matlab
|
||||
Nmeas = 1; Ncon = 1; Nz2 = 2;
|
||||
[H2, ~] = h2hinfsyn(ss(P), 1, 1, 2, [0, 1], 'HINFMAX', 1, 'H2MAX', Inf, 'DKMAX', 100, 'TOL', 0.01, 'DISPLAY', 'on');
|
||||
#+end_src
|
||||
|
||||
[H1, ~, normz, ~] = h2hinfsyn(P, Nmeas, Ncon, Nz2, [0, 1], 'HINFMAX', 1, 'H2MAX', Inf, 'DKMAX', 100, 'TOL', 0.01, 'DISPLAY', 'on');
|
||||
|
||||
H2 = 1 - H1;
|
||||
#+begin_src matlab
|
||||
H1 = 1 - H2;
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
@ -1256,15 +1205,12 @@ The obtained complementary filters are shown in Figure [[fig:htwo_hinf_comp_filt
|
||||
|
||||
ax1 = subplot(2,1,1);
|
||||
hold on;
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$W_1$');
|
||||
set(gca,'ColorOrderIndex',2)
|
||||
plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$W_2$');
|
||||
plot(freqs, 1./abs(squeeze(freqresp(Wu*W1, freqs, 'Hz'))), '--', 'DisplayName', '$1/|W_uW_1|$');
|
||||
plot(freqs, 1./abs(squeeze(freqresp(Wu*W2, freqs, 'Hz'))), '--', 'DisplayName', '$1/|W_uW_2|$');
|
||||
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
|
||||
set(gca,'ColorOrderIndex',2)
|
||||
plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$');
|
||||
plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
|
||||
plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$');
|
||||
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
@ -1275,10 +1221,8 @@ The obtained complementary filters are shown in Figure [[fig:htwo_hinf_comp_filt
|
||||
|
||||
ax2 = subplot(2,1,2);
|
||||
hold on;
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, 180/pi*phase(squeeze(freqresp(H2, freqs, 'Hz'))), '-');
|
||||
set(gca,'ColorOrderIndex',2)
|
||||
plot(freqs, 180/pi*phase(squeeze(freqresp(H1, freqs, 'Hz'))), '-');
|
||||
plot(freqs, 180/pi*phase(squeeze(freqresp(H2, freqs, 'Hz'))), '-');
|
||||
hold off;
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
set(gca, 'XScale', 'log');
|
||||
@ -1286,7 +1230,6 @@ The obtained complementary filters are shown in Figure [[fig:htwo_hinf_comp_filt
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
xticks([0.1, 1, 10, 100, 1000]);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :tangle no :exports results :results file replace
|
||||
@ -1299,12 +1242,17 @@ The obtained complementary filters are shown in Figure [[fig:htwo_hinf_comp_filt
|
||||
[[file:figs/htwo_hinf_comp_filters.png]]
|
||||
|
||||
** Obtained Super Sensor's noise
|
||||
The PSD and CPS of the super sensor's noise are shown in Figure [[fig:psd_sensors_htwo_hinf_synthesis]] and Figure [[fig:cps_h2_hinf_synthesis]] respectively.
|
||||
The Power Spectral Density of the super sensor's noise is shown in Figure [[fig:psd_sensors_htwo_hinf_synthesis]].
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
% The filters are loaded
|
||||
H2_filters = load('./mat/H2_filters.mat', 'H2', 'H1');
|
||||
Hinf_filters = load('./mat/Hinf_filters.mat', 'H2', 'H1');
|
||||
A time domain simulation is shown in Figure [[fig:super_sensor_time_domain_h2_hinf]].
|
||||
|
||||
The RMS values of the super sensor noise for the presented three synthesis are listed in Table [[tab:rms_noise_comp]].
|
||||
|
||||
#+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;
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
@ -1312,7 +1260,6 @@ The PSD and CPS of the super sensor's noise are shown in Figure [[fig:psd_sensor
|
||||
|
||||
PSD_H2 = abs(squeeze(freqresp(N1*H2_filters.H1, freqs, 'Hz'))).^2 + ...
|
||||
abs(squeeze(freqresp(N2*H2_filters.H2, freqs, 'Hz'))).^2;
|
||||
CPS_H2 = cumtrapz(freqs, PSD_H2);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
@ -1320,17 +1267,6 @@ The PSD and CPS of the super sensor's noise are shown in Figure [[fig:psd_sensor
|
||||
|
||||
PSD_Hinf = abs(squeeze(freqresp(N1*Hinf_filters.H1, freqs, 'Hz'))).^2 + ...
|
||||
abs(squeeze(freqresp(N2*Hinf_filters.H2, freqs, 'Hz'))).^2;
|
||||
CPS_Hinf = cumtrapz(freqs, PSD_Hinf);
|
||||
#+end_src
|
||||
|
||||
#+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);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
@ -1357,32 +1293,63 @@ The PSD and CPS of the super sensor's noise are shown in Figure [[fig:psd_sensor
|
||||
#+RESULTS:
|
||||
[[file:figs/psd_sensors_htwo_hinf_synthesis.png]]
|
||||
|
||||
#+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;
|
||||
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}}$');
|
||||
plot(freqs, CPS_Hinf, 'k--', 'DisplayName', '$\Gamma_{n_{\mathcal{H}_\infty}}$');
|
||||
plot(freqs, CPS_H2Hinf, 'k-.', 'DisplayName', '$\Gamma_{n_{\mathcal{H}_2/\mathcal{H}_\infty}}$');
|
||||
set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Cumulative Power Spectrum');
|
||||
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;
|
||||
xlim([2*freqs(1), freqs(end)]);
|
||||
legend('location', 'southeast');
|
||||
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/cps_h2_hinf_synthesis.pdf', 'width', 'wide', 'height', 'normal');
|
||||
exportFig('figs/super_sensor_time_domain_h2_hinf.pdf', 'width', 'wide', 'height', 'normal');
|
||||
#+end_src
|
||||
|
||||
#+name: fig:cps_h2_hinf_synthesis
|
||||
#+CAPTION: Cumulative Power Spectrum of the Super Sensor obtained with the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis
|
||||
#+name: fig:super_sensor_time_domain_h2_hinf
|
||||
#+caption: Noise of individual sensors and noise of the super sensor
|
||||
#+RESULTS:
|
||||
[[file:figs/cps_h2_hinf_synthesis.png]]
|
||||
[[file:figs/super_sensor_time_domain_h2_hinf.png]]
|
||||
|
||||
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||||
data2orgtable([sqrt(trapz(freqs, PSD_H2)), sqrt(trapz(freqs, PSD_Hinf)), sqrt(trapz(freqs, PSD_H2Hinf))]', ...
|
||||
{'Optimal: $\mathcal{H}_2$', 'Robust: $\mathcal{H}_\infty$', 'Mixed: $\mathcal{H}_2/\mathcal{H}_\infty$'}, ...
|
||||
{'RMS [m/s]'}, ' %.1e ');
|
||||
#+end_src
|
||||
|
||||
#+name: tab:rms_noise_comp
|
||||
#+caption: Comparison of the obtained RMS noise of the super sensor
|
||||
#+attr_latex: :environment tabular :align cc
|
||||
#+attr_latex: :center t :booktabs t :float t
|
||||
#+RESULTS:
|
||||
| | RMS [m/s] |
|
||||
|-------------------------------------------+-----------|
|
||||
| Optimal: $\mathcal{H}_2$ | 0.0027 |
|
||||
| Robust: $\mathcal{H}_\infty$ | 0.041 |
|
||||
| Mixed: $\mathcal{H}_2/\mathcal{H}_\infty$ | 0.01 |
|
||||
|
||||
** Obtained Super Sensor's Uncertainty
|
||||
The uncertainty on the super sensor's dynamics is shown in Figure
|
||||
The uncertainty on the super sensor's dynamics is shown in Figure [[fig:super_sensor_dynamical_uncertainty_Htwo_Hinf]].
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
Dphi_Wu = 180/pi*asin(abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))));
|
||||
@ -1395,18 +1362,21 @@ The uncertainty on the super sensor's dynamics is shown in 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, 1 + abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'r--', ...
|
||||
'DisplayName', '$W_u$')
|
||||
plot(freqs, 1 - abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'r--', ...
|
||||
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');
|
||||
plot(freqs, 1 + abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), 'k--', ...
|
||||
'DisplayName', '$1 + W_u^{-1}\Delta$')
|
||||
plot(freqs, 1 - abs(squeeze(freqresp(inv(Wu), freqs, 'Hz'))), '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
|
||||
@ -1414,10 +1384,10 @@ The uncertainty on the super sensor's dynamics is shown in Figure
|
||||
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, Dphi_Wu, 'r--');
|
||||
plot(freqs, -Dphi_Wu, 'r--');
|
||||
plot(freqs, Dphi_ss, 'k-');
|
||||
plot(freqs, -Dphi_ss, 'k-');
|
||||
plot(freqs, Dphi_Wu, 'k--');
|
||||
plot(freqs, -Dphi_Wu, 'k--');
|
||||
set(gca,'xscale','log');
|
||||
yticks(-180:90:180);
|
||||
ylim([-180 180]);
|
||||
@ -1427,41 +1397,17 @@ The uncertainty on the super sensor's dynamics is shown in Figure
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
#+end_src
|
||||
|
||||
** 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');
|
||||
#+begin_src matlab :tangle no :exports results :results file replace
|
||||
exportFig('figs/super_sensor_dynamical_uncertainty_Htwo_Hinf.pdf', 'width', 'full', 'height', 'full');
|
||||
#+end_src
|
||||
|
||||
#+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);
|
||||
#+end_src
|
||||
|
||||
#+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
|
||||
|
||||
#+name: tab:rms_noise_comp
|
||||
#+caption: Comparison of the obtained RMS noise of the super sensor
|
||||
#+attr_latex: :environment tabular :align cc
|
||||
#+attr_latex: :center t :booktabs t :float t
|
||||
#+name: fig:super_sensor_dynamical_uncertainty_Htwo_Hinf
|
||||
#+caption: Super sensor dynamical uncertainty (solid curve) when using the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ Synthesis
|
||||
#+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 |
|
||||
[[file:figs/super_sensor_dynamical_uncertainty_Htwo_Hinf.png]]
|
||||
|
||||
** Conclusion
|
||||
This synthesis methods allows both to:
|
||||
The mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis of the complementary filters allows to:
|
||||
- limit the dynamical uncertainty of the super sensor
|
||||
- minimize the RMS value of the estimation
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user