Add H-Infinity synthesis to minimize the super sensor noise RMS
This commit is contained in:
parent
ab1d026898
commit
665cc66e38
330
matlab/index.org
330
matlab/index.org
@ -250,6 +250,336 @@ The optimal sensor fusion has permitted to reduced the RMS value of the estimati
|
||||
| Sensor 2 | 1.3e-03 |
|
||||
| Optimal Sensor Fusion | 1.5e-04 |
|
||||
|
||||
** H-Infinity Synthesis
|
||||
*** First idea
|
||||
Another objective that we may have is that the noise of the super sensor $n_{SS}$ is following the minimum of the noise of the two sensors $n_1$ and $n_2$:
|
||||
\[ \Gamma_{n_{ss}}(\omega) = \min(\Gamma_{n_1}(\omega),\ \Gamma_{n_2}(\omega)) \]
|
||||
|
||||
In order to obtain that ideal case, we need that the complementary filters be designed such that:
|
||||
\begin{align*}
|
||||
& H_1(j\omega) = 1 \text{ and } H_2(j\omega) = 0 \text{ at frequencies where } \Gamma_{n_1}(\omega) < \Gamma_{n_2}(\omega) \\
|
||||
& H_1(j\omega) = 0 \text{ and } H_2(j\omega) = 1 \text{ at frequencies where } \Gamma_{n_1}(\omega) > \Gamma_{n_2}(\omega)
|
||||
\end{align*}
|
||||
|
||||
Which is indeed *impossible in practice*.
|
||||
|
||||
We could try to use filters with the highest possible order to approximate that.
|
||||
However, we may have robustness and implementation problems.
|
||||
|
||||
#+begin_src matlab
|
||||
omegac = 100*2*pi; G0 = 1e-5; Ginf = 1e-4;
|
||||
N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/4000);
|
||||
|
||||
omegac = 1*2*pi; G0 = 1e-3; Ginf = 1e-8;
|
||||
N2 = ((sqrt(Ginf)*s/omegac + sqrt(G0))/(s/omegac + 1))^2/(1 + s/2/pi/4000)^2;
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
n = 5; w0 = 2*pi*10; G0 = 1/10; G1 = 10000; Gc = 1/2;
|
||||
W1 = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n;
|
||||
|
||||
n = 5; w0 = 2*pi*8; G0 = 1000; G1 = 0.1; Gc = 1/2;
|
||||
W2 = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n;
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$W_1 = \frac{N_1}{N_2}$');
|
||||
plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$W_2 = \frac{N_2}{N_1}$');
|
||||
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
|
||||
P = [W1 -W1;
|
||||
0 W2;
|
||||
1 0];
|
||||
#+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');
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
#+begin_example
|
||||
[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
|
||||
Resetting value of Gamma min based on D_11, D_12, D_21 terms
|
||||
|
||||
Test bounds: 0.1000 < gamma <= 10500.0000
|
||||
|
||||
gamma hamx_eig xinf_eig hamy_eig yinf_eig nrho_xy p/f
|
||||
1.050e+04 2.1e+01 -3.0e-07 7.8e+00 -1.3e-15 0.0000 p
|
||||
5.250e+03 2.1e+01 -1.5e-08 7.8e+00 -5.8e-14 0.0000 p
|
||||
2.625e+03 2.1e+01 2.5e-10 7.8e+00 -3.7e-12 0.0000 p
|
||||
1.313e+03 2.1e+01 -3.2e-11 7.8e+00 -7.3e-14 0.0000 p
|
||||
656.344 2.1e+01 -2.2e-10 7.8e+00 -1.1e-15 0.0000 p
|
||||
328.222 2.1e+01 -1.1e-10 7.8e+00 -1.2e-15 0.0000 p
|
||||
164.161 2.1e+01 -2.4e-08 7.8e+00 -8.9e-16 0.0000 p
|
||||
82.130 2.1e+01 2.0e-10 7.8e+00 -9.1e-31 0.0000 p
|
||||
41.115 2.1e+01 -6.8e-09 7.8e+00 -4.1e-13 0.0000 p
|
||||
20.608 2.1e+01 3.3e-10 7.8e+00 -1.4e-12 0.0000 p
|
||||
10.354 2.1e+01 -9.8e-09 7.8e+00 -1.8e-15 0.0000 p
|
||||
5.227 2.1e+01 -4.1e-09 7.8e+00 -2.5e-12 0.0000 p
|
||||
2.663 2.1e+01 2.7e-10 7.8e+00 -4.0e-14 0.0000 p
|
||||
1.382 2.1e+01 -3.2e+05# 7.8e+00 -3.5e-14 0.0000 f
|
||||
2.023 2.1e+01 -5.0e-10 7.8e+00 0.0e+00 0.0000 p
|
||||
1.702 2.1e+01 -2.4e+07# 7.8e+00 -1.6e-13 0.0000 f
|
||||
1.862 2.1e+01 -6.0e+08# 7.8e+00 -1.0e-12 0.0000 f
|
||||
1.942 2.1e+01 -2.8e-09 7.8e+00 -8.1e-14 0.0000 p
|
||||
1.902 2.1e+01 -2.5e-09 7.8e+00 -1.1e-13 0.0000 p
|
||||
1.882 2.1e+01 -9.3e-09 7.8e+00 -2.0e-15 0.0001 p
|
||||
1.872 2.1e+01 -1.3e+09# 7.8e+00 -3.6e-22 0.0000 f
|
||||
1.877 2.1e+01 -2.6e+09# 7.8e+00 -1.2e-13 0.0000 f
|
||||
1.880 2.1e+01 -5.6e+09# 7.8e+00 -1.4e-13 0.0000 f
|
||||
1.881 2.1e+01 -1.2e+10# 7.8e+00 -3.3e-12 0.0000 f
|
||||
1.882 2.1e+01 -3.2e+10# 7.8e+00 -8.5e-14 0.0001 f
|
||||
|
||||
Gamma value achieved: 1.8824
|
||||
#+end_example
|
||||
|
||||
#+begin_src matlab
|
||||
H1 = 1 - H2;
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2,1,1);
|
||||
hold on;
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$w_1$');
|
||||
set(gca,'ColorOrderIndex',2)
|
||||
plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$w_2$');
|
||||
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
|
||||
set(gca,'ColorOrderIndex',2)
|
||||
plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$');
|
||||
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude');
|
||||
set(gca, 'XTickLabel',[]);
|
||||
ylim([5e-4, 20]);
|
||||
legend('location', 'northeast');
|
||||
|
||||
ax2 = subplot(2,1,2);
|
||||
hold on;
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, 180/pi*phase(squeeze(freqresp(H1, freqs, 'Hz'))), '-');
|
||||
set(gca,'ColorOrderIndex',2)
|
||||
plot(freqs, 180/pi*phase(squeeze(freqresp(H2, freqs, 'Hz'))), '-');
|
||||
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
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2, '-', 'DisplayName', '$|N_1|^2$');
|
||||
plot(freqs, abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2, '-', 'DisplayName', '$|N_2|^2$');
|
||||
plot(freqs, abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2, 'k-', 'DisplayName', '$|N_1H_1|^2+|N_2H_2|^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 :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||||
data2orgtable([norm([N1], 2);norm([N2], 2);norm([N1*H1 + N2*H2], 2)], {'Sensor 1', 'Sensor 2', 'Optimal Sensor Fusion'}, {'rms value'}, ' %.1e');
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
| | rms value |
|
||||
|-----------------------+-----------|
|
||||
| Sensor 1 | 1.1e-02 |
|
||||
| Sensor 2 | 1.3e-03 |
|
||||
| Optimal Sensor Fusion | 3.2e-04 |
|
||||
|
||||
*** Second idea
|
||||
We have that:
|
||||
\[ \Phi_{n_{ss}} = \left|H_1\right|^2 \Phi_{n_1} + \left|H_2\right|^2 \Phi_{n_2} \]
|
||||
|
||||
We model the Power Spectral Densities of the two sensors by weighting functions $N_1(s)$ and $N_2(s)$.
|
||||
\[ \Phi_{n_{ss}} = \left|H_1 N_1\right|^2 + \left|H_2\right N_2|^2 \]
|
||||
|
||||
Then, at frequencies where $|H_1| < |H_2|$ we would like that $|N_1| = 1$ and $|N_2| = 0$ as we discussed before.
|
||||
Then $|H_1 N_1|^2 + |H_2 N_2|^2 = |H_1 N_1|^2$.
|
||||
|
||||
However, if we design $H_2$ such that when $|N_1| < |N_2|$, we have that:
|
||||
\[ |H_2 N_2|^2 = \epsilon |H_1 N_1|^2 \]
|
||||
And we have:
|
||||
\begin{align*}
|
||||
\Phi_{n_{ss}} &= \left|H_1 N_1\right|^2 + |H_2 N_2|^2 \\
|
||||
&= (1 + \epsilon)\left|H_1 N_1\right|^2 \\
|
||||
&\approx \left|H_1 N_1\right|^2
|
||||
\end{align*}
|
||||
|
||||
We have then that:
|
||||
\[ |H_2 N_2| = \sqrt{\epsilon} |H_1 N_1| \]
|
||||
We suppose that $|H_1| = 1$:
|
||||
\[ |H_2| = \sqrt{\epsilon} \frac{|N_1|}{|N_2|} \]
|
||||
We can use that for the maximum magnitude of $|H_2|$ at frequencies where $|N_1| < |N_2|$
|
||||
|
||||
Similarly, we have that the maximum magnitude of $|H_1|$ at frequencies where $|N_1| > |N_2|$ is:
|
||||
\[ |H_1| = \sqrt{\epsilon} \frac{|N_2|}{|N_1|} \]
|
||||
|
||||
We could take $\epsilon = 1$, then we increase the noise of the super sensor just by a factor $\sqrt{2}$ over the all bandwidth from the idea case.
|
||||
|
||||
This could be used as weighting functions for the $\mathcal{H}_\infty$ synthesis of the complementary filters.
|
||||
|
||||
#+begin_src matlab
|
||||
omegac = 100*2*pi; G0 = 1e-5; Ginf = 1e-4;
|
||||
N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/4000);
|
||||
|
||||
omegac = 1*2*pi; G0 = 1e-3; Ginf = 1e-8;
|
||||
N2 = ((sqrt(Ginf)*s/omegac + sqrt(G0))/(s/omegac + 1))^2/(1 + s/2/pi/4000)^2;
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
W1 = N1/N2;
|
||||
W2 = N2/N1;
|
||||
|
||||
W1 = W1/(1 + s/2/pi/1000); % this is added so that it is proper
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$W_1 = \frac{N_1}{N_2}$');
|
||||
plot(freqs, abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$W_2 = \frac{N_2}{N_1}$');
|
||||
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
|
||||
P = [W1 -W1;
|
||||
0 W2;
|
||||
1 0];
|
||||
#+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');
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
#+begin_example
|
||||
[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
|
||||
Test bounds: 0.0000 < gamma <= 2625.0000
|
||||
|
||||
gamma hamx_eig xinf_eig hamy_eig yinf_eig nrho_xy p/f
|
||||
2.625e+03 1.8e+01 -2.3e-12 6.3e+00 -1.7e-14 0.0000 p
|
||||
1.313e+03 1.8e+01 -3.3e-12 6.3e+00 -5.9e-13 0.0000 p
|
||||
656.250 1.8e+01 -1.8e-12 6.3e+00 -1.2e-13 0.0000 p
|
||||
328.125 1.8e+01 -8.3e-13 6.3e+00 -7.5e-14 0.0000 p
|
||||
164.063 1.8e+01 -3.8e-13 6.3e+00 -6.1e-14 0.0000 p
|
||||
82.031 1.8e+01 -3.3e-22 6.3e+00 -5.4e-14 0.0000 p
|
||||
41.016 1.8e+01 -3.4e-12 6.3e+00 -1.4e-16 0.0000 p
|
||||
20.508 1.8e+01 -6.4e-14 6.3e+00 -4.4e-13 0.0000 p
|
||||
10.254 1.8e+01 -1.4e-13 6.3e+00 -4.9e-14 0.0000 p
|
||||
5.127 1.7e+01 -1.1e-12 6.3e+00 -1.6e-13 0.0000 p
|
||||
2.563 1.7e+01 -6.5e-13 6.3e+00 -7.3e-14 0.0000 p
|
||||
1.282 1.4e+01 -8.9e+04# 6.3e+00 -1.6e-14 0.0000 f
|
||||
1.923 1.6e+01 -4.6e+05# 6.3e+00 -2.1e-13 0.0000 f
|
||||
2.243 1.7e+01 -1.9e+06# 6.3e+00 -9.9e-14 0.0000 f
|
||||
2.403 1.7e+01 -3.7e-11 6.3e+00 -2.1e-12 0.0000 p
|
||||
2.323 1.7e+01 -4.6e+06# 6.3e+00 -5.8e-14 0.0000 f
|
||||
2.363 1.7e+01 -1.2e+07# 6.3e+00 -3.3e-14 0.0000 f
|
||||
2.383 1.7e+01 -6.8e+07# 6.3e+00 -1.2e-13 0.0000 f
|
||||
2.393 1.7e+01 -5.5e-11 6.3e+00 -1.8e-12 0.0000 p
|
||||
2.388 1.7e+01 1.3e-22 6.3e+00 -2.5e-15 0.0000 p
|
||||
2.386 1.7e+01 -1.5e+08# 6.3e+00 -1.4e-13 0.0000 f
|
||||
2.387 1.7e+01 -4.0e+08# 6.3e+00 -1.0e-13 0.0000 f
|
||||
2.388 1.7e+01 -2.1e+09# 6.3e+00 -4.2e-13 0.0000 f
|
||||
|
||||
Gamma value achieved: 2.3882
|
||||
#+end_example
|
||||
|
||||
#+begin_src matlab
|
||||
H1 = 1 - H2;
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2,1,1);
|
||||
hold on;
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, 1./abs(squeeze(freqresp(W1, freqs, 'Hz'))), '--', 'DisplayName', '$w_1$');
|
||||
set(gca,'ColorOrderIndex',2)
|
||||
plot(freqs, 1./abs(squeeze(freqresp(W2, freqs, 'Hz'))), '--', 'DisplayName', '$w_2$');
|
||||
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
|
||||
set(gca,'ColorOrderIndex',2)
|
||||
plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$');
|
||||
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude');
|
||||
set(gca, 'XTickLabel',[]);
|
||||
ylim([5e-4, 20]);
|
||||
legend('location', 'northeast');
|
||||
|
||||
ax2 = subplot(2,1,2);
|
||||
hold on;
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, 180/pi*phase(squeeze(freqresp(H1, freqs, 'Hz'))), '-');
|
||||
set(gca,'ColorOrderIndex',2)
|
||||
plot(freqs, 180/pi*phase(squeeze(freqresp(H2, freqs, 'Hz'))), '-');
|
||||
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
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2, '-', 'DisplayName', '$|N_1|^2$');
|
||||
plot(freqs, abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2, '-', 'DisplayName', '$|N_2|^2$');
|
||||
plot(freqs, abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2, 'k-', 'DisplayName', '$|N_1H_1|^2+|N_2H_2|^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 :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||||
data2orgtable([norm([N1], 2);norm([N2], 2);norm([N1*H1 + N2*H2], 2)], {'Sensor 1', 'Sensor 2', 'Optimal Sensor Fusion'}, {'rms value'}, ' %.1e');
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
| | rms value |
|
||||
|-----------------------+-----------|
|
||||
| Sensor 1 | 1.1e-02 |
|
||||
| Sensor 2 | 1.3e-03 |
|
||||
| Optimal Sensor Fusion | 1.9e-04 |
|
||||
|
||||
*** Conclusion
|
||||
From the above two experiments with $\mathcal{H}_\infty$ 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)
|
||||
|
||||
* Robustness to sensor dynamics uncertainty
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle matlab/comp_filter_robustness.m
|
||||
|
||||
Loading…
Reference in New Issue
Block a user