1027 lines
43 KiB
Org Mode
1027 lines
43 KiB
Org Mode
|
#+TITLE: On the Design of Complementary Filters for Control - Computation with Matlab
|
||
|
|
: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*
|
||
|
|
#+PROPERTY: header-args:matlab+ :tangle matlab/comp_filters_design.m
|
||
|
|
#+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:
|
||
|
|
In this document, the design of complementary filters is studied.
|
||
|
|
|
||
|
|
One use of complementary filter is described below:
|
||
|
|
#+begin_quote
|
||
|
|
The basic idea of a complementary filter involves taking two or more sensors, filtering out unreliable frequencies for each sensor, and combining the filtered outputs to get a better estimate throughout the entire bandwidth of the system.
|
||
|
|
To achieve this, the sensors included in the filter should complement one another by performing better over specific parts of the system bandwidth.
|
||
|
|
#+end_quote
|
||
|
|
|
||
|
|
- in section [[sec:optimal_comp_filters]], the optimal design of the complementary filters in order to obtain the lowest resulting "super sensor" noise is studied
|
||
|
|
|
||
|
|
When blending two sensors using complementary filters with unknown dynamics, phase lag may be introduced that renders the close-loop system unstable.
|
||
|
|
- in section [[sec:comp_filter_robustness]], the blending robustness to sensor dynamic uncertainty is studied.
|
||
|
|
|
||
|
|
Then, three design methods for generating two complementary filters are proposed:
|
||
|
|
- in section [[sec:comp_filters_analytical]], analytical formulas are proposed
|
||
|
|
- in section [[sec:h_inf_synthesis_complementary_filters]], the $\mathcal{H}_\infty$ synthesis is used
|
||
|
|
- in section [[sec:feedback_generate_comp_filters]], the classical feedback architecture is used
|
||
|
|
|
||
|
|
* Optimal Sensor Fusion for noise characteristics
|
||
|
|
: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.
|
||
|
|
|
||
|
|
** ZIP file containing the data and matlab files :ignore:
|
||
|
|
#+begin_src bash :exports none :results none
|
||
|
|
if [ matlab/optimal_comp_filters.m -nt data/optimal_comp_filters.zip ]; then
|
||
|
|
cp matlab/optimal_comp_filters.m optimal_comp_filters.m;
|
||
|
|
zip data/optimal_comp_filters \
|
||
|
|
optimal_comp_filters.m
|
||
|
|
rm optimal_comp_filters.m;
|
||
|
|
fi
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_note
|
||
|
|
All the files (data and Matlab scripts) are accessible [[file:data/optimal_comp_filters.zip][here]].
|
||
|
|
#+end_note
|
||
|
|
|
||
|
|
** 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
|
||
|
|
freqs = logspace(-1, 3, 1000);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Architecture
|
||
|
|
Let's consider the sensor fusion architecture shown on figure [[fig:fusion_two_noisy_sensors_with_dyn]] where two sensors 1 and 2 are measuring the same quantity $x$ with different noise characteristics determined by $W_1$ and $W_2$.
|
||
|
|
|
||
|
|
$n_1$ and $n_2$ are white noise (constant power spectral density over all frequencies).
|
||
|
|
|
||
|
|
#+name: fig:fusion_two_noisy_sensors_with_dyn
|
||
|
|
#+caption: Fusion of two sensors
|
||
|
|
[[file:figs/fusion_two_noisy_sensors_with_dyn.png]]
|
||
|
|
|
||
|
|
We consider that the two sensor dynamics $G_1$ and $G_2$ are ideal ($G_1 = G_2 = 1$). We obtain the architecture of figure [[fig:fusion_two_noisy_sensors]].
|
||
|
|
|
||
|
|
#+name: fig:fusion_two_noisy_sensors
|
||
|
|
#+caption: Fusion of two sensors with ideal dynamics
|
||
|
|
[[file:figs/fusion_two_noisy_sensors.png]]
|
||
|
|
|
||
|
|
$H_1$ and $H_2$ are complementary filters ($H_1 + H_2 = 1$). The goal is to design $H_1$ and $H_2$ such that the effect of the noise sources $n_1$ and $n_2$ has the smallest possible effect on the estimation $\hat{x}$.
|
||
|
|
|
||
|
|
We have that the Power Spectral Density (PSD) of $\hat{x}$ is:
|
||
|
|
\[ \Gamma_{\hat{x}} = |H_1 W_1|^2 \Gamma_{n_1} + |H_2 W_2|^2 \Gamma_{n_2} \]
|
||
|
|
|
||
|
|
And the goal is the minimize the Root Mean Square (RMS) value of $\hat{x}$:
|
||
|
|
\[ \sigma_{\hat{x}} = \sqrt{\int_0^\infty \Gamma_{\hat{x}}(\omega) d\omega} \]
|
||
|
|
|
||
|
|
As $n_1$ and $n_2$ are white noise: $\Gamma_{n_1} = \Gamma_{n_2} = 1$ and we have:
|
||
|
|
\[ \sigma_{\hat{x}} = \sqrt{\int_0^\infty |H_1 W_1|^2(\omega) + |H_2 W_2|^2(\omega) d\omega} = \left\| \begin{matrix} H_1 W_1 \\ H_2 W_2 \end{matrix} \right\|_2 \]
|
||
|
|
|
||
|
|
Thus, the goal is to design $H_1$ and $H_2$ such that $H_1 + H_2 = 1$ and such that $\left\| \begin{matrix} H_1 W_1 \\ H_2 W_2 \end{matrix} \right\|_2$ is minimized.
|
||
|
|
|
||
|
|
For that, we will use the $\mathcal{H}_2$ Synthesis.
|
||
|
|
|
||
|
|
** Noise of the sensors
|
||
|
|
Let's define the noise characteristics of the two sensors by choosing $W_1$ and $W_2$:
|
||
|
|
- Sensor 1 characterized by $W_1$ has low noise at low frequency (for instance a geophone)
|
||
|
|
- Sensor 2 characterized by $W_2$ has low noise at high frequency (for instance an accelerometer)
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
omegac = 2*pi; G0 = 1e-2; Ginf = 1e-6;
|
||
|
|
W1 = ((sqrt(G0))/(s/omegac + 1))^2;
|
||
|
|
|
||
|
|
omegac = 100*2*pi; G0 = 1e-6; Ginf = 1e-2;
|
||
|
|
W2 = ((sqrt(Ginf)*s/omegac + sqrt(G0))/(s/omegac + 1))^2/(1 + s/2/pi/4000)^2;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
omegac = 100*2*pi; G0 = 1e-5; Ginf = 1e-4;
|
||
|
|
W1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/4000);
|
||
|
|
|
||
|
|
omegac = 1*2*pi; G0 = 1e-3; Ginf = 1e-8;
|
||
|
|
W2 = ((sqrt(Ginf)*s/omegac + sqrt(G0))/(s/omegac + 1))^2/(1 + s/2/pi/4000)^2;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(freqs, abs(squeeze(freqresp(W1, freqs, 'Hz'))), '-', 'DisplayName', '$W_1$');
|
||
|
|
plot(freqs, abs(squeeze(freqresp(W2, freqs, 'Hz'))), '-', 'DisplayName', '$W_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
|
||
|
|
|
||
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||
|
|
#+begin_src matlab :var filepath="figs/nosie_characteristics_sensors.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||
|
|
<<plt-matlab>>
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+NAME: fig:nosie_characteristics_sensors
|
||
|
|
#+CAPTION: Noise Characteristics of the two sensors ([[./figs/nosie_characteristics_sensors.png][png]], [[./figs/nosie_characteristics_sensors.pdf][pdf]])
|
||
|
|
[[file:figs/nosie_characteristics_sensors.png]]
|
||
|
|
|
||
|
|
** H-Two Synthesis
|
||
|
|
We use the generalized plant architecture shown on figure [[fig:h_infinity_optimal_comp_filters]].
|
||
|
|
|
||
|
|
#+name: fig:h_infinity_optimal_comp_filters
|
||
|
|
#+caption: $\mathcal{H}_2$ Synthesis - Generalized plant used for the optimal generation of complementary filters
|
||
|
|
[[file:figs/h_infinity_optimal_comp_filters.png]]
|
||
|
|
|
||
|
|
The transfer function from $[n_1, n_2]$ to $\hat{x}$ is:
|
||
|
|
\[ \begin{bmatrix} W_1 H_1 \\ W_2 (1 - H_1) \end{bmatrix} \]
|
||
|
|
If we define $H_2 = 1 - H_1$, we obtain:
|
||
|
|
\[ \begin{bmatrix} W_1 H_1 \\ W_2 H_2 \end{bmatrix} \]
|
||
|
|
|
||
|
|
Thus, if we minimize the $\mathcal{H}_2$ norm of this transfer function, we minimize the RMS value of $\hat{x}$.
|
||
|
|
|
||
|
|
We define the generalized plant $P$ on matlab as shown on figure [[fig:h_infinity_optimal_comp_filters]].
|
||
|
|
#+begin_src matlab
|
||
|
|
P = [0 W2 1;
|
||
|
|
W1 -W2 0];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
And we do the $\mathcal{H}_2$ synthesis using the =h2syn= command.
|
||
|
|
#+begin_src matlab
|
||
|
|
[H1, ~, gamma] = h2syn(P, 1, 1);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
What is minimized is =norm([W1*H1,W2*H2], 2)=.
|
||
|
|
|
||
|
|
Finally, we define $H_2 = 1 - H_1$.
|
||
|
|
#+begin_src matlab
|
||
|
|
H2 = 1 - H1;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Analysis
|
||
|
|
The complementary filters obtained are shown on figure [[fig:htwo_comp_filters]]. The PSD of the [[fig:psd_sensors_htwo_synthesis]].
|
||
|
|
Finally, the RMS value of $\hat{x}$ is shown on table [[tab:rms_results]].
|
||
|
|
The optimal sensor fusion has permitted to reduced the RMS value of the estimation error by a factor 8 compare to when using only one sensor.
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(freqs, abs(squeeze(freqresp(H1, freqs, 'Hz'))), '-', 'DisplayName', '$H_1$');
|
||
|
|
plot(freqs, abs(squeeze(freqresp(H2, freqs, 'Hz'))), '-', 'DisplayName', '$H_2$');
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Magnitude');
|
||
|
|
hold off;
|
||
|
|
xlim([freqs(1), freqs(end)]);
|
||
|
|
legend('location', 'northeast');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||
|
|
#+begin_src matlab :var filepath="figs/htwo_comp_filters.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||
|
|
<<plt-matlab>>
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+NAME: fig:htwo_comp_filters
|
||
|
|
#+CAPTION: Obtained complementary filters using the $\mathcal{H}_2$ Synthesis ([[./figs/htwo_comp_filters.png][png]], [[./figs/htwo_comp_filters.pdf][pdf]])
|
||
|
|
[[file:figs/htwo_comp_filters.png]]
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(freqs, abs(squeeze(freqresp(W1, freqs, 'Hz'))).^2, '-', 'DisplayName', '$|W_1|^2$');
|
||
|
|
plot(freqs, abs(squeeze(freqresp(W2, freqs, 'Hz'))).^2, '-', 'DisplayName', '$|W_2|^2$');
|
||
|
|
plot(freqs, abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))).^2, 'k-', 'DisplayName', '$|W_1H_1|^2+|W_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
|
||
|
|
|
||
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||
|
|
#+begin_src matlab :var filepath="figs/psd_sensors_htwo_synthesis.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||
|
|
<<plt-matlab>>
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+NAME: fig:psd_sensors_htwo_synthesis
|
||
|
|
#+CAPTION: Power Spectral Density of the estimated $\hat{x}$ using the two sensors alone and using the optimally fused signal ([[./figs/psd_sensors_htwo_synthesis.png][png]], [[./figs/psd_sensors_htwo_synthesis.pdf][pdf]])
|
||
|
|
[[file:figs/psd_sensors_htwo_synthesis.png]]
|
||
|
|
|
||
|
|
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||
|
|
data2orgtable([norm([W1], 2);norm([W2], 2);norm([W1*H1 + W2*H2], 2)], {'Sensor 1', 'Sensor 2', 'Optimal Sensor Fusion'}, {'rms value'}, ' %.1e');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: tab:rms_results
|
||
|
|
#+caption: RMS value of the estimation error when using the sensor individually and when using the two sensor merged using the optimal complementary filters
|
||
|
|
#+RESULTS:
|
||
|
|
| | rms value |
|
||
|
|
|-----------------------+-----------|
|
||
|
|
| Sensor 1 | 1.1e-02 |
|
||
|
|
| Sensor 2 | 1.3e-03 |
|
||
|
|
| Optimal Sensor Fusion | 1.5e-04 |
|
||
|
|
* Robustness to sensor dynamics uncertainty
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle matlab/comp_filter_robustness.m
|
||
|
|
:header-args:matlab+: :comments org :mkdirp yes
|
||
|
|
:END:
|
||
|
|
<<sec:comp_filter_robustness>>
|
||
|
|
|
||
|
|
** Introduction :ignore:
|
||
|
|
Let's first consider ideal sensors where $G_1 = 1$ and $G_2 = 1$ (figure [[fig:fusion_two_noisy_sensors_with_dyn_bis]]).
|
||
|
|
|
||
|
|
#+name: fig:fusion_two_noisy_sensors_with_dyn_bis
|
||
|
|
#+caption: Fusion of two sensors
|
||
|
|
[[file:figs/fusion_two_noisy_sensors_with_dyn_bis.png]]
|
||
|
|
|
||
|
|
# #+name: fig:fusion_two_signals
|
||
|
|
# #+caption: Fusion of two noisy measurements of $x$
|
||
|
|
# [[file:figs/fusion_two_signals.png]]
|
||
|
|
|
||
|
|
We then have:
|
||
|
|
\begin{align*}
|
||
|
|
\hat{x} &= (x + n_1) H_1 + (x + n_2) H_2 \\
|
||
|
|
&= x + n_1 H_1 + n_2 H_2
|
||
|
|
\end{align*}
|
||
|
|
|
||
|
|
So the estimation error is
|
||
|
|
\[ \delta_x = \hat{x} - x = n_1 H_1 + n_2 H_2 \]
|
||
|
|
|
||
|
|
And we see that the complementary filters are only shaping the noise and that they do not impact the transfer function from $x$ to $\hat{x}$ that is in the feedback path.
|
||
|
|
|
||
|
|
** ZIP file containing the data and matlab files :ignore:
|
||
|
|
#+begin_src bash :exports none :results none
|
||
|
|
if [ matlab/comp_filter_robustness.m -nt data/comp_filter_robustness.zip ]; then
|
||
|
|
cp matlab/comp_filter_robustness.m comp_filter_robustness.m;
|
||
|
|
zip data/comp_filter_robustness \
|
||
|
|
comp_filter_robustness.m
|
||
|
|
rm comp_filter_robustness.m;
|
||
|
|
fi
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_note
|
||
|
|
All the files (data and Matlab scripts) are accessible [[file:data/comp_filter_robustness.zip][here]].
|
||
|
|
#+end_note
|
||
|
|
|
||
|
|
** 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
|
||
|
|
|
||
|
|
** Static Gain Mismatch :noexport:
|
||
|
|
Even though we here still consider that the two sensors have perfect dynamics, we consider gain mismatch for the two sensors:
|
||
|
|
\begin{align*}
|
||
|
|
G_1(s) &= 1 + \delta_1(s) \\
|
||
|
|
G_2(s) &= 1 + \delta_2(s)
|
||
|
|
\end{align*}
|
||
|
|
|
||
|
|
Thus, we have:
|
||
|
|
\begin{align*}
|
||
|
|
\hat{x} &= (x + n_1) (1 + \delta_1) H_1 + (x + n_2) (1 + \delta_2) H_2 \\
|
||
|
|
&= x (1 + \delta_1 H_1 + \delta_2 H_2) + n_1 (1 + \delta_1) H_1 + n_2(1 + \delta_2) H_2
|
||
|
|
\end{align*}
|
||
|
|
|
||
|
|
So the transfer function from $x$ to $\hat{x}$ is:
|
||
|
|
\begin{align*}
|
||
|
|
\frac{\hat{x}}{x} &= 1 + \delta_1 H_1 + \delta_2 H_2 \\
|
||
|
|
&= 1 + \delta_1 H_1 + \delta_2 (1 - H_1) \\
|
||
|
|
&= 1 + (\delta_1 - \delta_2) H_1 + \delta_2 \\
|
||
|
|
\end{align*}
|
||
|
|
|
||
|
|
** Unknown sensor dynamics dynamics
|
||
|
|
In practical systems, the sensor dynamics has always some level of uncertainty.
|
||
|
|
Let's represent that with multiplicative input uncertainty as shown on figure [[fig:fusion_gain_mismatch]].
|
||
|
|
|
||
|
|
#+name: fig:fusion_gain_mismatch
|
||
|
|
#+caption: Fusion of two sensors with input multiplicative uncertainty
|
||
|
|
[[file:figs/fusion_gain_mismatch.png]]
|
||
|
|
|
||
|
|
We have:
|
||
|
|
\begin{align*}
|
||
|
|
\frac{\hat{x}}{x} &= (1 + W_1 \Delta_1) H_1 + (1 + W_2 \Delta_2) H_2 \\
|
||
|
|
&= 1 + W_1 H_1 \Delta_1 + W_2 H_2 \Delta_2
|
||
|
|
\end{align*}
|
||
|
|
With $\Delta_i$ is any transfer function satisfying $\| \Delta_i \|_\infty < 1$.
|
||
|
|
|
||
|
|
We see that as soon as we have some uncertainty in the sensor dynamics, we have that the complementary filters have some effect on the transfer function from $x$ to $\hat{x}$.
|
||
|
|
|
||
|
|
We want that the super sensor transfer function has a gain of 1 and no phase variation over all the frequencies:
|
||
|
|
\[ \frac{\hat{x}}{x} \approx 1 \]
|
||
|
|
|
||
|
|
Thus, we want that
|
||
|
|
\begin{align*}
|
||
|
|
& |W_1 H_1 \Delta_1 + W_2 H_2 \Delta_2| < \epsilon \quad \forall \omega, \forall \Delta_i, \|\Delta_i\|_\infty < 1 \\
|
||
|
|
\Longleftrightarrow & |W_1 H_1| + |W_2 H_2| < \epsilon \quad \forall \omega
|
||
|
|
\end{align*}
|
||
|
|
|
||
|
|
Which is approximately the same as requiring
|
||
|
|
\[ \left\| \begin{matrix} W_1 H_1 \\ W_2 H_2 \end{matrix} \right\|_\infty < \epsilon \]
|
||
|
|
|
||
|
|
|
||
|
|
*How small should we choose $\epsilon$?*
|
||
|
|
|
||
|
|
The uncertainty set of the transfer function from $\hat{x}$ to $x$ is bounded in the complex plane by a circle centered on 1 and with a radius equal to $\epsilon$ (figure [[fig:uncertainty_gain_phase_variation]]).
|
||
|
|
|
||
|
|
We then have that the angle introduced by the super sensor is bounded by $\arcsin(\epsilon)$:
|
||
|
|
\[ \angle \frac{\hat{x}}{x} \le \arcsin (\epsilon) \quad \forall \omega \]
|
||
|
|
|
||
|
|
#+name: fig:uncertainty_gain_phase_variation
|
||
|
|
#+caption: Maximum phase variation
|
||
|
|
[[file:figs/uncertainty_gain_phase_variation.png]]
|
||
|
|
|
||
|
|
Thus, we choose should choose $\epsilon$ so that the maximum phase uncertainty introduced by the sensors is of an acceptable value.
|
||
|
|
|
||
|
|
** Design the complementary filters in order to limit the phase and gain uncertainty of the super sensor
|
||
|
|
Let's say the two sensors dynamics $H_1$ and $H_2$ have been identified with the associated uncertainty weights $W_1$ and $W_2$.
|
||
|
|
|
||
|
|
If we want to have a maximum phase introduced by the sensors of 20 degrees, we have to design $H_1$ and $H_2$ such that:
|
||
|
|
\begin{align*}
|
||
|
|
& arcsin(|H_1 W_1| + |H_2 W_2|) < 20 \text{ deg} \\
|
||
|
|
\Longleftrightarrow & |H_1 W_1| + |H_2 W_2| < 0.34
|
||
|
|
\end{align*}
|
||
|
|
|
||
|
|
We can do that with the $\mathcal{H}_\infty$ synthesis by setting upper bounds on the complementary filters using weights that corresponds to the sensor dynamics uncertainty.
|
||
|
|
|
||
|
|
For simplicity, let's suppose $W_1(s) = W_2(s) = 0.1$ ($10\%$ uncertainty in the sensor gain).
|
||
|
|
\[ |H_1 W_1| + |H_2 W_2| < 3.4 \]
|
||
|
|
|
||
|
|
Thus, by limiting the norm of the complementary filters, we can limit the maximum unwanted phase introduced by the uncertainty on the sensors dynamics.
|
||
|
|
|
||
|
|
This is of primary importance in order to ensure the stability of the feedback loop using the super sensor signal.
|
||
|
|
|
||
|
|
** First Basic Example with gain mismatch
|
||
|
|
Let's consider two ideal sensors except one sensor has not an expected gain of one but a gain of $0.6$.
|
||
|
|
#+begin_src matlab
|
||
|
|
G1 = 1;
|
||
|
|
G2 = 0.6;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Let's design two complementary filters as shown on figure [[fig:comp_filters_robustness_test]].
|
||
|
|
The complementary filters shown in blue does not present a bump as the red ones but provides less sensor separation at high and low frequencies.
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
w0 = 2*pi;
|
||
|
|
alpha = 2;
|
||
|
|
|
||
|
|
H1a = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
|
||
|
|
H2a = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
|
||
|
|
|
||
|
|
w0 = 2*pi;
|
||
|
|
alpha = 0.1;
|
||
|
|
|
||
|
|
H1b = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
|
||
|
|
H2b = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
freqs = logspace(-1, 1, 1000);
|
||
|
|
|
||
|
|
figure;
|
||
|
|
% Magnitude
|
||
|
|
ax1 = subaxis(2,1,1);
|
||
|
|
hold on;
|
||
|
|
set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(H1a, freqs, 'Hz'))));
|
||
|
|
set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(H2a, freqs, 'Hz'))));
|
||
|
|
set(gca,'ColorOrderIndex',2); plot(freqs, abs(squeeze(freqresp(H1b, freqs, 'Hz'))));
|
||
|
|
set(gca,'ColorOrderIndex',2); plot(freqs, abs(squeeze(freqresp(H2b, freqs, 'Hz'))));
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]);
|
||
|
|
ylabel('Magnitude');
|
||
|
|
hold off;
|
||
|
|
% Phase
|
||
|
|
ax2 = subaxis(2,1,2);
|
||
|
|
hold on;
|
||
|
|
set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(H1a, freqs, 'Hz'))));
|
||
|
|
set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(H2a, freqs, 'Hz'))));
|
||
|
|
set(gca,'ColorOrderIndex',2); plot(freqs, 180/pi*angle(squeeze(freqresp(H1b, freqs, 'Hz'))));
|
||
|
|
set(gca,'ColorOrderIndex',2); plot(freqs, 180/pi*angle(squeeze(freqresp(H2b, freqs, 'Hz'))));
|
||
|
|
set(gca,'xscale','log');
|
||
|
|
yticks(-180:90:180);
|
||
|
|
ylim([-180 180]);
|
||
|
|
xlabel('Relative Frequency $\frac{\omega}{\omega_0}$'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
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/comp_filters_robustness_test.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||
|
|
<<plt-matlab>>
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+NAME: fig:comp_filters_robustness_test
|
||
|
|
#+CAPTION: The two complementary filters designed for the robustness test ([[./figs/comp_filters_robustness_test.png][png]], [[./figs/comp_filters_robustness_test.pdf][pdf]])
|
||
|
|
[[file:figs/comp_filters_robustness_test.png]]
|
||
|
|
|
||
|
|
We then compute the bode plot of the super sensor transfer function $H_1*G_1 + H_2*G_2$ for both complementary filters pair (figure [[fig:tf_super_sensor_comp]]).
|
||
|
|
|
||
|
|
We see that the blue complementary filters with a lower maximum norm permits to limit the phase lag introduced by the gain mismatch.
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
freqs = logspace(-1, 1, 1000);
|
||
|
|
|
||
|
|
figure;
|
||
|
|
% Magnitude
|
||
|
|
ax1 = subaxis(2,1,1);
|
||
|
|
hold on;
|
||
|
|
set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(H1a*G1 + H2a*G2, freqs, 'Hz'))));
|
||
|
|
set(gca,'ColorOrderIndex',2); plot(freqs, abs(squeeze(freqresp(H1b*G1 + H2b*G2, freqs, 'Hz'))));
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]);
|
||
|
|
ylabel('Magnitude');
|
||
|
|
ylim([1e-1, 1e1]);
|
||
|
|
hold off;
|
||
|
|
% Phase
|
||
|
|
ax2 = subaxis(2,1,2);
|
||
|
|
hold on;
|
||
|
|
set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(H1a*G1 + H2a*G2, freqs, 'Hz'))));
|
||
|
|
set(gca,'ColorOrderIndex',2); plot(freqs, 180/pi*angle(squeeze(freqresp(H1b*G1 + H2b*G2, freqs, 'Hz'))));
|
||
|
|
set(gca,'xscale','log');
|
||
|
|
yticks(-180:90:180);
|
||
|
|
ylim([-180 180]);
|
||
|
|
xlabel('Relative Frequency $\frac{\omega}{\omega_0}$'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
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/tf_super_sensor_comp.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||
|
|
<<plt-matlab>>
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+NAME: fig:tf_super_sensor_comp
|
||
|
|
#+CAPTION: Comparison of the obtained super sensor transfer functions ([[./figs/tf_super_sensor_comp.png][png]], [[./figs/tf_super_sensor_comp.pdf][pdf]])
|
||
|
|
[[file:figs/tf_super_sensor_comp.png]]
|
||
|
|
|
||
|
|
** TODO More Complete example with model uncertainty
|
||
|
|
* Complementary filters using analytical formula
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle matlab/comp_filters_analytical.m
|
||
|
|
:header-args:matlab+: :comments org :mkdirp yes
|
||
|
|
:END:
|
||
|
|
<<sec:comp_filters_analytical>>
|
||
|
|
|
||
|
|
** Introduction :ignore:
|
||
|
|
** ZIP file containing the data and matlab files :ignore:
|
||
|
|
#+begin_src bash :exports none :results none
|
||
|
|
if [ matlab/comp_filters_analytical.m -nt data/comp_filters_analytical.zip ]; then
|
||
|
|
cp matlab/comp_filters_analytical.m comp_filters_analytical.m;
|
||
|
|
zip data/comp_filters_analytical \
|
||
|
|
comp_filters_analytical.m
|
||
|
|
rm comp_filters_analytical.m;
|
||
|
|
fi
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_note
|
||
|
|
All the files (data and Matlab scripts) are accessible [[file:data/comp_filters_analytical.zip][here]].
|
||
|
|
#+end_note
|
||
|
|
|
||
|
|
** 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
|
||
|
|
freqs = logspace(-1, 3, 1000);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Analytical 1st order complementary filters
|
||
|
|
First order complementary filters are defined with following equations:
|
||
|
|
\begin{align}
|
||
|
|
H_L(s) = \frac{1}{1 + \frac{s}{\omega_0}}\\
|
||
|
|
H_H(s) = \frac{\frac{s}{\omega_0}}{1 + \frac{s}{\omega_0}}
|
||
|
|
\end{align}
|
||
|
|
|
||
|
|
Their bode plot is shown figure [[fig:comp_filter_1st_order]].
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
w0 = 2*pi; % [rad/s]
|
||
|
|
|
||
|
|
Hh1 = (s/w0)/((s/w0)+1);
|
||
|
|
Hl1 = 1/((s/w0)+1);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
freqs = logspace(-2, 2, 1000);
|
||
|
|
|
||
|
|
figure;
|
||
|
|
% Magnitude
|
||
|
|
ax1 = subaxis(2,1,1);
|
||
|
|
hold on;
|
||
|
|
set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(Hh1, freqs, 'Hz'))));
|
||
|
|
set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(Hl1, freqs, 'Hz'))));
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]);
|
||
|
|
ylabel('Magnitude');
|
||
|
|
hold off;
|
||
|
|
% Phase
|
||
|
|
ax2 = subaxis(2,1,2);
|
||
|
|
hold on;
|
||
|
|
set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(Hh1, freqs, 'Hz'))));
|
||
|
|
set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(Hl1, freqs, 'Hz'))));
|
||
|
|
set(gca,'xscale','log');
|
||
|
|
yticks(-180:90:180);
|
||
|
|
ylim([-180 180]);
|
||
|
|
xlabel('Relative Frequency $\frac{\omega}{\omega_0}$'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
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/comp_filter_1st_order.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||
|
|
<<plt-matlab>>
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+NAME: fig:comp_filter_1st_order
|
||
|
|
#+CAPTION: Bode plot of first order complementary filter ([[./figs/comp_filter_1st_order.png][png]], [[./figs/comp_filter_1st_order.pdf][pdf]])
|
||
|
|
[[file:figs/comp_filter_1st_order.png]]
|
||
|
|
|
||
|
|
** Second Order Complementary Filters
|
||
|
|
We here use analytical formula for the complementary filters $H_L$ and $H_H$.
|
||
|
|
|
||
|
|
The first two formulas that are used to generate complementary filters are:
|
||
|
|
\begin{align*}
|
||
|
|
H_L(s) &= \frac{(1+\alpha) (\frac{s}{\omega_0})+1}{\left((\frac{s}{\omega_0})+1\right) \left((\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1\right)}\\
|
||
|
|
H_H(s) &= \frac{(\frac{s}{\omega_0})^2 \left((\frac{s}{\omega_0})+1+\alpha\right)}{\left((\frac{s}{\omega_0})+1\right) \left((\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1\right)}
|
||
|
|
\end{align*}
|
||
|
|
where:
|
||
|
|
- $\omega_0$ is the blending frequency in rad/s.
|
||
|
|
- $\alpha$ is used to change the shape of the filters:
|
||
|
|
- Small values for $\alpha$ will produce high magnitude of the filters $|H_L(j\omega)|$ and $|H_H(j\omega)|$ near $\omega_0$ but smaller value for $|H_L(j\omega)|$ above $\approx 1.5 \omega_0$ and for $|H_H(j\omega)|$ below $\approx 0.7 \omega_0$
|
||
|
|
- A large $\alpha$ will do the opposite
|
||
|
|
|
||
|
|
This is illustrated on figure [[fig:comp_filters_param_alpha]].
|
||
|
|
The slope of those filters at high and low frequencies is $-2$ and $2$ respectively for $H_L$ and $H_H$.
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
freqs_study = logspace(-2, 2, 10000);
|
||
|
|
alphas = [0.1, 1, 10];
|
||
|
|
w0 = 2*pi*1;
|
||
|
|
|
||
|
|
figure;
|
||
|
|
ax1 = subaxis(2,1,1);
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(alphas)
|
||
|
|
alpha = alphas(i);
|
||
|
|
Hh2 = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
|
||
|
|
Hl2 = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
|
||
|
|
set(gca,'ColorOrderIndex',i);
|
||
|
|
plot(freqs_study, abs(squeeze(freqresp(Hh2, freqs_study, 'Hz'))));
|
||
|
|
set(gca,'ColorOrderIndex',i);
|
||
|
|
plot(freqs_study, abs(squeeze(freqresp(Hl2, freqs_study, 'Hz'))));
|
||
|
|
end
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]);
|
||
|
|
ylabel('Magnitude');
|
||
|
|
hold off;
|
||
|
|
ylim([1e-3, 20]);
|
||
|
|
% Phase
|
||
|
|
ax2 = subaxis(2,1,2);
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(alphas)
|
||
|
|
alpha = alphas(i);
|
||
|
|
Hh2 = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
|
||
|
|
Hl2 = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
|
||
|
|
set(gca,'ColorOrderIndex',i);
|
||
|
|
plot(freqs_study, 180/pi*angle(squeeze(freqresp(Hh2, freqs_study, 'Hz'))), 'DisplayName', sprintf('$\\alpha = %g$', alpha));
|
||
|
|
set(gca,'ColorOrderIndex',i);
|
||
|
|
plot(freqs_study, 180/pi*angle(squeeze(freqresp(Hl2, freqs_study, 'Hz'))), 'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
set(gca,'xscale','log');
|
||
|
|
yticks(-180:90:180);
|
||
|
|
ylim([-180 180]);
|
||
|
|
xlabel('Relative Frequency $\frac{\omega}{\omega_0}$'); ylabel('Phase [deg]');
|
||
|
|
legend('Location', 'northeast');
|
||
|
|
hold off;
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([freqs_study(1), freqs_study(end)]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||
|
|
#+begin_src matlab :var filepath="figs/comp_filters_param_alpha.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||
|
|
<<plt-matlab>>
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+NAME: fig:comp_filters_param_alpha
|
||
|
|
#+CAPTION: Effect of the parameter $\alpha$ on the shape of the generated second order complementary filters ([[./figs/comp_filters_param_alpha.png][png]], [[./figs/comp_filters_param_alpha.pdf][pdf]])
|
||
|
|
[[file:figs/comp_filters_param_alpha.png]]
|
||
|
|
|
||
|
|
We now study the maximum norm of the filters function of the parameter $\alpha$. As we saw that the maximum norm of the filters is important for the robust merging of filters.
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
alphas = logspace(-2, 2, 100);
|
||
|
|
w0 = 2*pi*1;
|
||
|
|
infnorms = zeros(size(alphas));
|
||
|
|
|
||
|
|
for i = 1:length(alphas)
|
||
|
|
alpha = alphas(i);
|
||
|
|
Hh2 = (s/w0)^2*((s/w0)+1+alpha)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
|
||
|
|
Hl2 = ((1+alpha)*(s/w0)+1)/(((s/w0)+1)*((s/w0)^2 + alpha*(s/w0) + 1));
|
||
|
|
infnorms(i) = norm(Hh2, 'inf');
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
figure;
|
||
|
|
plot(alphas, infnorms)
|
||
|
|
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
|
||
|
|
xlabel('$\alpha$'); ylabel('$\|H_1\|_\infty$');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||
|
|
#+begin_src matlab :var filepath="figs/param_alpha_hinf_norm.pdf" :var figsize="wide-normal" :post pdf2svg(file=*this*, ext="png")
|
||
|
|
<<plt-matlab>>
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+NAME: fig:param_alpha_hinf_norm
|
||
|
|
#+CAPTION: Evolution of the H-Infinity norm of the complementary filters with the parameter $\alpha$ ([[./figs/param_alpha_hinf_norm.png][png]], [[./figs/param_alpha_hinf_norm.pdf][pdf]])
|
||
|
|
[[file:figs/param_alpha_hinf_norm.png]]
|
||
|
|
|
||
|
|
** Third Order Complementary Filters
|
||
|
|
The following formula gives complementary filters with slopes of $-3$ and $3$:
|
||
|
|
\begin{align*}
|
||
|
|
H_L(s) &= \frac{\left(1+(\alpha+1)(\beta+1)\right) (\frac{s}{\omega_0})^2 + (1+\alpha+\beta)(\frac{s}{\omega_0}) + 1}{\left(\frac{s}{\omega_0} + 1\right) \left( (\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1 \right) \left( (\frac{s}{\omega_0})^2 + \beta (\frac{s}{\omega_0}) + 1 \right)}\\
|
||
|
|
H_H(s) &= \frac{(\frac{s}{\omega_0})^3 \left( (\frac{s}{\omega_0})^2 + (1+\alpha+\beta) (\frac{s}{\omega_0}) + (1+(\alpha+1)(\beta+1)) \right)}{\left(\frac{s}{\omega_0} + 1\right) \left( (\frac{s}{\omega_0})^2 + \alpha (\frac{s}{\omega_0}) + 1 \right) \left( (\frac{s}{\omega_0})^2 + \beta (\frac{s}{\omega_0}) + 1 \right)}
|
||
|
|
\end{align*}
|
||
|
|
|
||
|
|
The parameters are:
|
||
|
|
- $\omega_0$ is the blending frequency in rad/s
|
||
|
|
- $\alpha$ and $\beta$ that are used to change the shape of the filters similarly to the parameter $\alpha$ for the second order complementary filters
|
||
|
|
|
||
|
|
The filters are defined below and the result is shown on figure [[fig:complementary_filters_third_order]].
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
alpha = 1;
|
||
|
|
beta = 10;
|
||
|
|
w0 = 2*pi*14;
|
||
|
|
|
||
|
|
Hh3_ana = (s/w0)^3 * ((s/w0)^2 + (1+alpha+beta)*(s/w0) + (1+(alpha+1)*(beta+1)))/((s/w0 + 1)*((s/w0)^2+alpha*(s/w0)+1)*((s/w0)^2+beta*(s/w0)+1));
|
||
|
|
Hl3_ana = ((1+(alpha+1)*(beta+1))*(s/w0)^2 + (1+alpha+beta)*(s/w0) + 1)/((s/w0 + 1)*((s/w0)^2+alpha*(s/w0)+1)*((s/w0)^2+beta*(s/w0)+1));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
set(gca,'ColorOrderIndex',1)
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Hl3_ana, freqs, 'Hz'))), '-', 'DisplayName', '$H_L$ - Analytical');
|
||
|
|
set(gca,'ColorOrderIndex',2)
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Hh3_ana, freqs, 'Hz'))), '-', 'DisplayName', '$H_H$ - Analytical');
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Magnitude');
|
||
|
|
hold off;
|
||
|
|
xlim([freqs(1), freqs(end)]);
|
||
|
|
ylim([1e-3, 10]);
|
||
|
|
xticks([0.1, 1, 10, 100, 1000]);
|
||
|
|
legend('location', 'northeast');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||
|
|
#+begin_src matlab :var filepath="figs/complementary_filters_third_order.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||
|
|
<<plt-matlab>>
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+NAME: fig:complementary_filters_third_order
|
||
|
|
#+CAPTION: Third order complementary filters using the analytical formula ([[./figs/complementary_filters_third_order.png][png]], [[./figs/complementary_filters_third_order.pdf][pdf]])
|
||
|
|
[[file:figs/complementary_filters_third_order.png]]
|
||
|
|
|
||
|
|
* H-Infinity synthesis of complementary filters
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle matlab/h_inf_synthesis_complementary_filters.m
|
||
|
|
:header-args:matlab+: :comments org :mkdirp yes
|
||
|
|
:END:
|
||
|
|
<<sec:h_inf_synthesis_complementary_filters>>
|
||
|
|
|
||
|
|
** Introduction :ignore:
|
||
|
|
** ZIP file containing the data and matlab files :ignore:
|
||
|
|
#+begin_src bash :exports none :results none
|
||
|
|
if [ matlab/h_inf_synthesis_complementary_filters.m -nt data/h_inf_synthesis_complementary_filters.zip ]; then
|
||
|
|
cp matlab/h_inf_synthesis_complementary_filters.m h_inf_synthesis_complementary_filters.m;
|
||
|
|
zip data/h_inf_synthesis_complementary_filters \
|
||
|
|
h_inf_synthesis_complementary_filters.m
|
||
|
|
rm h_inf_synthesis_complementary_filters.m;
|
||
|
|
fi
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_note
|
||
|
|
All the files (data and Matlab scripts) are accessible [[file:data/h_inf_synthesis_complementary_filters.zip][here]].
|
||
|
|
#+end_note
|
||
|
|
|
||
|
|
** 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
|
||
|
|
freqs = logspace(-1, 3, 1000);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Synthesis Architecture
|
||
|
|
We here synthesize the complementary filters using the $\mathcal{H}_\infty$ synthesis.
|
||
|
|
The goal is to specify upper bounds on the norms of $H_L$ and $H_H$ while ensuring their complementary property ($H_L + H_H = 1$).
|
||
|
|
|
||
|
|
In order to do so, we use the generalized plant shown on figure [[fig:sf_hinf_filters_plant_b]] where $w_L$ and $w_H$ weighting transfer functions that will be used to shape $H_L$ and $H_H$ respectively.
|
||
|
|
|
||
|
|
#+name: fig:sf_hinf_filters_plant_b
|
||
|
|
#+caption: Generalized plant used for the $\mathcal{H}_\infty$ synthesis of the complementary filters
|
||
|
|
[[file:figs/sf_hinf_filters_plant_b.png]]
|
||
|
|
|
||
|
|
The $\mathcal{H}_\infty$ synthesis applied on this generalized plant will give a transfer function $H_L$ (figure [[fig:sf_hinf_filters_b]]) such that the $\mathcal{H}_\infty$ norm of the transfer function from $w$ to $[z_H,\ z_L]$ is less than one:
|
||
|
|
\[ \left\| \begin{array}{c} H_L w_L \\ (1 - H_L) w_H \end{array} \right\|_\infty < 1 \]
|
||
|
|
|
||
|
|
Thus, if the above condition is verified, we can define $H_H = 1 - H_L$ and we have that:
|
||
|
|
\[ \left\| \begin{array}{c} H_L w_L \\ H_H w_H \end{array} \right\|_\infty < 1 \]
|
||
|
|
Which is almost (with an maximum error of $\sqrt{2}$) equivalent to:
|
||
|
|
\begin{align*}
|
||
|
|
|H_L| &< \frac{1}{|w_L|}, \quad \forall \omega \\
|
||
|
|
|H_H| &< \frac{1}{|w_H|}, \quad \forall \omega
|
||
|
|
\end{align*}
|
||
|
|
|
||
|
|
We then see that $w_L$ and $w_H$ can be used to shape both $H_L$ and $H_H$ while ensuring (by definition of $H_H = 1 - H_L$) their complementary property.
|
||
|
|
|
||
|
|
#+name: fig:sf_hinf_filters_b
|
||
|
|
#+caption: $\mathcal{H}_\infty$ synthesis of the complementary filters
|
||
|
|
[[file:figs/sf_hinf_filters_b.png]]
|
||
|
|
|
||
|
|
** Weights
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
omegab = 2*pi*9;
|
||
|
|
wH = (omegab)^2/(s + omegab*sqrt(1e-5))^2;
|
||
|
|
omegab = 2*pi*28;
|
||
|
|
wL = (s + omegab/(4.5)^(1/3))^3/(s*(1e-4)^(1/3) + omegab)^3;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
set(gca,'ColorOrderIndex',1)
|
||
|
|
plot(freqs, 1./abs(squeeze(freqresp(wL, freqs, 'Hz'))), '-', 'DisplayName', '$w_L$');
|
||
|
|
set(gca,'ColorOrderIndex',2)
|
||
|
|
plot(freqs, 1./abs(squeeze(freqresp(wH, freqs, 'Hz'))), '-', 'DisplayName', '$w_H$');
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Magnitude');
|
||
|
|
hold off;
|
||
|
|
xlim([freqs(1), freqs(end)]);
|
||
|
|
ylim([1e-3, 10]);
|
||
|
|
xticks([0.1, 1, 10, 100, 1000]);
|
||
|
|
legend('location', 'northeast');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||
|
|
#+begin_src matlab :var filepath="figs/weights_wl_wh.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||
|
|
<<plt-matlab>>
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+NAME: fig:weights_wl_wh
|
||
|
|
#+CAPTION: Weights on the complementary filters $w_L$ and $w_H$ and the associated performance weights ([[./figs/weights_wl_wh.png][png]], [[./figs/weights_wl_wh.pdf][pdf]])
|
||
|
|
[[file:figs/weights_wl_wh.png]]
|
||
|
|
|
||
|
|
** H-Infinity Synthesis
|
||
|
|
We define the generalized plant $P$ on matlab.
|
||
|
|
#+begin_src matlab
|
||
|
|
P = [0 wL;
|
||
|
|
wH -wH;
|
||
|
|
1 0];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
And we do the $\mathcal{H}_\infty$ synthesis using the =hinfsyn= command.
|
||
|
|
#+begin_src matlab :results output replace :exports both
|
||
|
|
[Hl_hinf, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+RESULTS:
|
||
|
|
#+begin_example
|
||
|
|
[Hl_hinf, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
|
||
|
|
Test bounds: 0.0000 < gamma <= 1.7285
|
||
|
|
|
||
|
|
gamma hamx_eig xinf_eig hamy_eig yinf_eig nrho_xy p/f
|
||
|
|
1.729 4.1e+01 8.4e-12 1.8e-01 0.0e+00 0.0000 p
|
||
|
|
0.864 3.9e+01 -5.8e-02# 1.8e-01 0.0e+00 0.0000 f
|
||
|
|
1.296 4.0e+01 8.4e-12 1.8e-01 0.0e+00 0.0000 p
|
||
|
|
1.080 4.0e+01 8.5e-12 1.8e-01 0.0e+00 0.0000 p
|
||
|
|
0.972 3.9e+01 -4.2e-01# 1.8e-01 0.0e+00 0.0000 f
|
||
|
|
1.026 4.0e+01 8.5e-12 1.8e-01 0.0e+00 0.0000 p
|
||
|
|
0.999 3.9e+01 8.5e-12 1.8e-01 0.0e+00 0.0000 p
|
||
|
|
0.986 3.9e+01 -1.2e+00# 1.8e-01 0.0e+00 0.0000 f
|
||
|
|
0.993 3.9e+01 -8.2e+00# 1.8e-01 0.0e+00 0.0000 f
|
||
|
|
0.996 3.9e+01 8.5e-12 1.8e-01 0.0e+00 0.0000 p
|
||
|
|
0.994 3.9e+01 8.5e-12 1.8e-01 0.0e+00 0.0000 p
|
||
|
|
0.993 3.9e+01 -3.2e+01# 1.8e-01 0.0e+00 0.0000 f
|
||
|
|
|
||
|
|
Gamma value achieved: 0.9942
|
||
|
|
#+end_example
|
||
|
|
|
||
|
|
We then define the high pass filter $H_H = 1 - H_L$. The bode plot of both $H_L$ and $H_H$ is shown on figure [[fig:hinf_filters_results]].
|
||
|
|
#+begin_src matlab
|
||
|
|
Hh_hinf = 1 - Hl_hinf;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Obtained Complementary Filters
|
||
|
|
|
||
|
|
The obtained complementary filters are shown on figure [[fig:hinf_filters_results]].
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
set(gca,'ColorOrderIndex',1)
|
||
|
|
plot(freqs, 1./abs(squeeze(freqresp(wL, freqs, 'Hz'))), '--', 'DisplayName', '$w_L$');
|
||
|
|
set(gca,'ColorOrderIndex',2)
|
||
|
|
plot(freqs, 1./abs(squeeze(freqresp(wH, freqs, 'Hz'))), '--', 'DisplayName', '$w_H$');
|
||
|
|
|
||
|
|
set(gca,'ColorOrderIndex',1)
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Hl_hinf, freqs, 'Hz'))), '-', 'DisplayName', '$H_L$ - $\mathcal{H}_\infty$');
|
||
|
|
set(gca,'ColorOrderIndex',2)
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Hh_hinf, freqs, 'Hz'))), '-', 'DisplayName', '$H_H$ - $\mathcal{H}_\infty$');
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Magnitude');
|
||
|
|
hold off;
|
||
|
|
xlim([freqs(1), freqs(end)]);
|
||
|
|
ylim([1e-3, 10]);
|
||
|
|
xticks([0.1, 1, 10, 100, 1000]);
|
||
|
|
legend('location', 'northeast');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||
|
|
#+begin_src matlab :var filepath="figs/hinf_filters_results.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||
|
|
<<plt-matlab>>
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+NAME: fig:hinf_filters_results
|
||
|
|
#+CAPTION: Obtained complementary filters using $\mathcal{H}_\infty$ synthesis ([[./figs/hinf_filters_results.png][png]], [[./figs/hinf_filters_results.pdf][pdf]])
|
||
|
|
[[file:figs/hinf_filters_results.png]]
|
||
|
|
|
||
|
|
* Feedback Control Architecture to generate Complementary Filters
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle matlab/feedback_generate_comp_filters.m
|
||
|
|
:header-args:matlab+: :comments org :mkdirp yes
|
||
|
|
:END:
|
||
|
|
<<sec:feedback_generate_comp_filters>>
|
||
|
|
|
||
|
|
** Introduction :ignore:
|
||
|
|
The idea is here to use the fact that in a classical feedback architecture, $S + T = 1$, in order to design complementary filters.
|
||
|
|
|
||
|
|
Thus, all the tools that has been developed for classical feedback control can be used for complementary filter design.
|
||
|
|
|
||
|
|
** ZIP file containing the data and matlab files :ignore:
|
||
|
|
#+begin_src bash :exports none :results none
|
||
|
|
if [ matlab/feedback_generate_comp_filters.m -nt data/feedback_generate_comp_filters.zip ]; then
|
||
|
|
cp matlab/feedback_generate_comp_filters.m feedback_generate_comp_filters.m;
|
||
|
|
zip data/feedback_generate_comp_filters \
|
||
|
|
feedback_generate_comp_filters.m
|
||
|
|
rm feedback_generate_comp_filters.m;
|
||
|
|
fi
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_note
|
||
|
|
All the files (data and Matlab scripts) are accessible [[file:data/feedback_generate_comp_filters.zip][here]].
|
||
|
|
#+end_note
|
||
|
|
|
||
|
|
** 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
|
||
|
|
freqs = logspace(-2, 2, 1000);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Architecture
|
||
|
|
#+name: fig:complementary_filters_feedback_architecture
|
||
|
|
#+caption: Architecture used to generate the complementary filters
|
||
|
|
[[file:figs/complementary_filters_feedback_architecture.png]]
|
||
|
|
|
||
|
|
We have:
|
||
|
|
\[ y = \underbrace{\frac{L}{L + 1}}_{H_L} y_1 + \underbrace{\frac{1}{L + 1}}_{H_H} y_2 \]
|
||
|
|
with $H_L + H_H = 1$.
|
||
|
|
|
||
|
|
The only thing to design is $L$ such that the complementary filters are stable with the wanted shape.
|
||
|
|
|
||
|
|
A simple choice is:
|
||
|
|
\[ L = \left(\frac{\omega_c}{s}\right)^2 \frac{\frac{s}{\omega_c / \alpha} + 1}{\frac{s}{\omega_c} + \alpha} \]
|
||
|
|
|
||
|
|
Which contains two integrator and a lead. $\omega_c$ is used to tune the crossover frequency and $\alpha$ the trade-off "bump" around blending frequency and filtering away from blending frequency.
|
||
|
|
|
||
|
|
** Loop Gain Design
|
||
|
|
Let's first define the loop gain $L$.
|
||
|
|
#+begin_src matlab
|
||
|
|
wc = 2*pi*1;
|
||
|
|
alpha = 2;
|
||
|
|
|
||
|
|
L = (wc/s)^2 * (s/(wc/alpha) + 1)/(s/wc + alpha);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
figure;
|
||
|
|
|
||
|
|
ax1 = subplot(2,1,1);
|
||
|
|
plot(freqs, abs(squeeze(freqresp(L, freqs, 'Hz'))), '-');
|
||
|
|
ylabel('Magnitude');
|
||
|
|
set(gca, 'XScale', 'log');
|
||
|
|
set(gca, 'YScale', 'log');
|
||
|
|
|
||
|
|
ax2 = subplot(2,1,2);
|
||
|
|
plot(freqs, 180/pi*phase(squeeze(freqresp(L, freqs, 'Hz'))), '--');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
set(gca, 'XScale', 'log');
|
||
|
|
ylim([-180, 0]);
|
||
|
|
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
|
||
|
|
#+begin_src matlab :var filepath="figs/loop_gain_bode_plot.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||
|
|
<<plt-matlab>>
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+NAME: fig:loop_gain_bode_plot
|
||
|
|
#+CAPTION: Bode plot of the loop gain $L$ ([[./figs/loop_gain_bode_plot.png][png]], [[./figs/loop_gain_bode_plot.pdf][pdf]])
|
||
|
|
[[file:figs/loop_gain_bode_plot.png]]
|
||
|
|
|
||
|
|
** Complementary Filters Obtained
|
||
|
|
We then compute the resulting low pass and high pass filters.
|
||
|
|
#+begin_src matlab
|
||
|
|
Hl = L/(L + 1);
|
||
|
|
Hh = 1/(L + 1);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
alphas = [1, 2, 10];
|
||
|
|
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(alphas)
|
||
|
|
alpha = alphas(i);
|
||
|
|
L = (wc/s)^2 * (s/(wc/alpha) + 1)/(s/wc + alpha);
|
||
|
|
Hl = L/(L + 1);
|
||
|
|
Hh = 1/(L + 1);
|
||
|
|
set(gca,'ColorOrderIndex',i)
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Hl, freqs, 'Hz'))), 'DisplayName', sprintf('$\\alpha = %.0f$', alpha));
|
||
|
|
set(gca,'ColorOrderIndex',i)
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Hh, freqs, 'Hz'))), 'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude')
|
||
|
|
legend('location', 'northeast');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||
|
|
#+begin_src matlab :var filepath="figs/low_pass_high_pass_filters.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||
|
|
<<plt-matlab>>
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+NAME: fig:low_pass_high_pass_filters
|
||
|
|
#+CAPTION: Low pass and High pass filters $H_L$ and $H_H$ for different values of $\alpha$ ([[./figs/low_pass_high_pass_filters.png][png]], [[./figs/low_pass_high_pass_filters.pdf][pdf]])
|
||
|
|
[[file:figs/low_pass_high_pass_filters.png]]
|
||
|
|
|
||
|
|
* Bibliography :ignore:
|
||
|
|
bibliographystyle:unsrt
|
||
|
|
bibliography:ref.bib
|