2019-08-14 12:08:30 +02:00
<?xml version="1.0" encoding="utf-8"?>
< !DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN"
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
< html xmlns = "http://www.w3.org/1999/xhtml" lang = "en" xml:lang = "en" >
< head >
2020-10-01 15:14:10 +02:00
<!-- 2020 - 10 - 01 jeu. 15:12 -->
2019-08-14 12:08:30 +02:00
< meta http-equiv = "Content-Type" content = "text/html;charset=utf-8" / >
2019-08-29 11:28:51 +02:00
< title > Robust and Optimal Sensor Fusion - Matlab Computation< / title >
2019-08-14 12:08:30 +02:00
< meta name = "generator" content = "Org mode" / >
< meta name = "author" content = "Thomas Dehaeze" / >
< link rel = "stylesheet" type = "text/css" href = "../css/htmlize.css" / >
< link rel = "stylesheet" type = "text/css" href = "../css/readtheorg.css" / >
< script src = "../js/jquery.min.js" > < / script >
< script src = "../js/bootstrap.min.js" > < / script >
< script src = "../js/jquery.stickytableheaders.min.js" > < / script >
< script src = "../js/readtheorg.js" > < / script >
2020-10-01 15:14:10 +02:00
< link rel = "stylesheet" type = "text/css" href = "https://cdn.rawgit.com/dreampulse/computer-modern-web-font/master/fonts.css" >
2020-09-30 08:47:27 +02:00
< script > M a t h J a x = {
tex: {
tags: 'ams',
macros: {bm: ["\\boldsymbol{#1}",1],}
}
};
< / script >
< script type = "text/javascript" src = "https://cdn.jsdelivr.net/npm/mathjax@3/es5/tex-mml-chtml.js" > < / script >
2019-08-14 12:08:30 +02:00
< / head >
< body >
< div id = "org-div-home-and-up" >
< a accesskey = "h" href = "../index.html" > UP < / a >
|
< a accesskey = "H" href = "../index.html" > HOME < / a >
< / div > < div id = "content" >
2019-08-29 11:28:51 +02:00
< h1 class = "title" > Robust and Optimal Sensor Fusion - Matlab Computation< / h1 >
2019-08-14 12:08:30 +02:00
< div id = "table-of-contents" >
< h2 > Table of Contents< / h2 >
< div id = "text-table-of-contents" >
< ul >
2020-10-01 15:14:10 +02:00
< li > < a href = "#orgca76559" > 1. Sensor Description< / a >
2019-08-14 12:08:30 +02:00
< ul >
2020-10-01 15:14:10 +02:00
< li > < a href = "#org3c3d9de" > 1.1. Sensor Dynamics< / a > < / li >
< li > < a href = "#orgdad1407" > 1.2. Sensor Model Uncertainty< / a > < / li >
< li > < a href = "#orge7b9c26" > 1.3. Sensor Noise< / a > < / li >
< li > < a href = "#org93a8b3d" > 1.4. Save Model< / a > < / li >
2019-08-14 12:08:30 +02:00
< / ul >
< / li >
2020-10-01 15:14:10 +02:00
< li > < a href = "#org44fc9f2" > 2. Introduction to Sensor Fusion< / a >
2019-08-14 12:08:30 +02:00
< ul >
2020-10-01 15:14:10 +02:00
< li > < a href = "#org046f351" > 2.1. Sensor Fusion Architecture< / a > < / li >
< li > < a href = "#org21fff50" > 2.2. Super Sensor Noise< / a > < / li >
< li > < a href = "#org1c04704" > 2.3. Super Sensor Dynamical Uncertainty< / a > < / li >
2019-08-29 22:47:20 +02:00
< / ul >
< / li >
2020-10-01 15:14:10 +02:00
< li > < a href = "#org24786a9" > 3. Optimal Super Sensor Noise: \(\mathcal{H}_2\) Synthesis< / a >
2019-08-30 14:39:01 +02:00
< ul >
2020-10-01 15:14:10 +02:00
< li > < a href = "#orgb5cf4c2" > 3.1. \(\mathcal{H}_2\) Synthesis< / a > < / li >
< li > < a href = "#org5d3402d" > 3.2. Super Sensor Noise< / a > < / li >
< li > < a href = "#orgd724be3" > 3.3. Discrepancy between sensor dynamics and model< / a > < / li >
< li > < a href = "#org14ef526" > 3.4. Conclusion< / a > < / li >
2020-10-01 11:25:23 +02:00
< / ul >
< / li >
2020-10-01 15:14:10 +02:00
< li > < a href = "#orge170b7d" > 4. Robust Sensor Fusion: \(\mathcal{H}_\infty\) Synthesis< / a >
2020-10-01 11:25:23 +02:00
< ul >
2020-10-01 15:14:10 +02:00
< li > < a href = "#org58a2403" > 4.1. Super Sensor Dynamical Uncertainty< / a > < / li >
< li > < a href = "#org9563d0a" > 4.2. Synthesis objective< / a > < / li >
< li > < a href = "#orgd7f10bf" > 4.3. Requirements as an \(\mathcal{H}_\infty\) norm< / a > < / li >
< li > < a href = "#org2c9d72a" > 4.4. Weighting Function used to bound the super sensor uncertainty< / a > < / li >
< li > < a href = "#org2d1873b" > 4.5. \(\mathcal{H}_\infty\) Synthesis< / a > < / li >
< li > < a href = "#org30c3b86" > 4.6. Super sensor uncertainty< / a > < / li >
< li > < a href = "#org1a771db" > 4.7. Super sensor noise< / a > < / li >
< li > < a href = "#org27b50b1" > 4.8. Conclusion< / a > < / li >
2020-10-01 11:25:23 +02:00
< / ul >
< / li >
2020-10-01 15:14:10 +02:00
< li > < a href = "#org0e86f6d" > 5. Optimal and Robust Sensor Fusion: Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis< / a >
2020-10-01 11:25:23 +02:00
< ul >
2020-10-01 15:14:10 +02:00
< li > < a href = "#orgc034a5e" > 5.1. Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis - Introduction< / a > < / li >
< li > < a href = "#org69a4331" > 5.2. Noise characteristics and Uncertainty of the individual sensors< / a > < / li >
< li > < a href = "#org014bfe3" > 5.3. Weighting Functions on the uncertainty of the super sensor< / a > < / li >
< li > < a href = "#orgd80e164" > 5.4. Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis< / a > < / li >
< li > < a href = "#orgc600a0e" > 5.5. Obtained Super Sensor’ s noise< / a > < / li >
< li > < a href = "#org14a7871" > 5.6. Obtained Super Sensor’ s Uncertainty< / a > < / li >
< li > < a href = "#org6170476" > 5.7. Comparison Hinf H2 H2/Hinf< / a > < / li >
< li > < a href = "#org99a64d6" > 5.8. Conclusion< / a > < / li >
< / ul >
< / li >
< li > < a href = "#org6159413" > 6. Matlab Functions< / a >
< ul >
< li > < a href = "#org6ccaec5" > 6.1. < code > createWeight< / code > < / a > < / li >
< li > < a href = "#org66fd774" > 6.2. < code > plotMagUncertainty< / code > < / a > < / li >
< li > < a href = "#orge99a206" > 6.3. < code > plotPhaseUncertainty< / code > < / a > < / li >
2019-08-14 12:14:10 +02:00
< / ul >
< / li >
2019-08-14 12:08:30 +02:00
< / ul >
< / div >
< / div >
< p >
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
< / p >
< p >
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
< / p >
< ul class = "org-ul" >
2020-10-01 15:14:10 +02:00
< li > Section < a href = "#orgd3b3857" > 3< / a > : the \(\mathcal{H}_2\) synthesis is used to design complementary filters such that the RMS value of the super sensor’ s noise is minimized< / li >
< li > Section < a href = "#orgfcc39fd" > 4< / a > : the \(\mathcal{H}_\infty\) synthesis is used to design complementary filters such that the super sensor’ s uncertainty is bonded to acceptable values< / li >
< li > Section < a href = "#org760d4b9" > 5< / a > : 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< / li >
2019-08-14 12:08:30 +02:00
< / ul >
2020-10-01 15:14:10 +02:00
< div id = "outline-container-orgca76559" class = "outline-2" >
< h2 id = "orgca76559" > < span class = "section-number-2" > 1< / span > Sensor Description< / h2 >
2019-08-14 12:08:30 +02:00
< div class = "outline-text-2" id = "text-1" >
< p >
2020-10-01 15:14:10 +02:00
< a id = "orgc105b40" > < / a >
< / p >
< p >
In Figure < a href = "#org9ff61e6" > 1< / a > is shown a schematic of a sensor model that is used in the following study.
In this example, the measured quantity \(x\) is the velocity of an object.
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 11:25:23 +02:00
2020-10-01 15:14:10 +02:00
< table id = "orgbfba8e5" border = "2" cellspacing = "0" cellpadding = "6" rules = "groups" frame = "hsides" >
< caption class = "t-above" > < span class = "table-number" > Table 1:< / span > Description of signals in Figure < a href = "#org9ff61e6" > 1< / a > < / caption >
2020-10-01 11:25:23 +02:00
< colgroup >
< col class = "org-left" / >
2020-10-01 15:14:10 +02:00
< col class = "org-left" / >
2020-10-01 11:25:23 +02:00
< col class = "org-left" / >
< / colgroup >
< thead >
< tr >
< th scope = "col" class = "org-left" > < b > Notation< / b > < / th >
< th scope = "col" class = "org-left" > < b > Meaning< / b > < / th >
2020-10-01 15:14:10 +02:00
< th scope = "col" class = "org-left" > < b > Unit< / b > < / th >
2020-10-01 11:25:23 +02:00
< / tr >
< / thead >
< tbody >
< tr >
< td class = "org-left" > \(x\)< / td >
< td class = "org-left" > Physical measured quantity< / td >
2020-10-01 15:14:10 +02:00
< td class = "org-left" > \([m/s]\)< / td >
2020-10-01 11:25:23 +02:00
< / tr >
< tr >
< td class = "org-left" > \(\tilde{n}_i\)< / td >
< td class = "org-left" > White noise with unitary PSD< / td >
2020-10-01 15:14:10 +02:00
< td class = "org-left" >   < / td >
2020-10-01 11:25:23 +02:00
< / tr >
< tr >
< td class = "org-left" > \(n_i\)< / td >
< td class = "org-left" > Shaped noise< / td >
2020-10-01 15:14:10 +02:00
< td class = "org-left" > \([m/s]\)< / td >
2020-10-01 11:25:23 +02:00
< / tr >
< tr >
< td class = "org-left" > \(v_i\)< / td >
< td class = "org-left" > Sensor output measurement< / td >
2020-10-01 15:14:10 +02:00
< td class = "org-left" > \([V]\)< / td >
2020-10-01 11:25:23 +02:00
< / tr >
< tr >
< td class = "org-left" > \(\hat{x}_i\)< / td >
< td class = "org-left" > Estimate of \(x\) from the sensor< / td >
2020-10-01 15:14:10 +02:00
< td class = "org-left" > \([m/s]\)< / td >
2020-10-01 11:25:23 +02:00
< / tr >
< / tbody >
< / table >
2020-10-01 15:14:10 +02:00
< table id = "org58c8e91" border = "2" cellspacing = "0" cellpadding = "6" rules = "groups" frame = "hsides" >
< caption class = "t-above" > < span class = "table-number" > Table 2:< / span > Description of Systems in Figure < a href = "#org9ff61e6" > 1< / a > < / caption >
2020-10-01 11:25:23 +02:00
< colgroup >
< col class = "org-left" / >
2020-10-01 15:14:10 +02:00
< col class = "org-left" / >
2020-10-01 11:25:23 +02:00
< col class = "org-left" / >
< / colgroup >
< thead >
< tr >
< th scope = "col" class = "org-left" > < b > Notation< / b > < / th >
< th scope = "col" class = "org-left" > < b > Meaning< / b > < / th >
2020-10-01 15:14:10 +02:00
< th scope = "col" class = "org-left" > < b > Unit< / b > < / th >
2020-10-01 11:25:23 +02:00
< / tr >
< / thead >
< tbody >
< tr >
< td class = "org-left" > \(\hat{G}_i\)< / td >
< td class = "org-left" > Nominal Sensor Dynamics< / td >
2020-10-01 15:14:10 +02:00
< td class = "org-left" > \([\frac{V}{m/s}]\)< / td >
2020-10-01 11:25:23 +02:00
< / tr >
< tr >
< td class = "org-left" > \(W_i\)< / td >
< td class = "org-left" > Weight representing the size of the uncertainty at each frequency< / td >
2020-10-01 15:14:10 +02:00
< td class = "org-left" >   < / td >
2020-10-01 11:25:23 +02:00
< / tr >
< tr >
< td class = "org-left" > \(\Delta_i\)< / td >
< td class = "org-left" > Any complex perturbation such that \(\vert\vert\Delta_i\vert\vert_\infty < 1 \ ) < / td >
2020-10-01 15:14:10 +02:00
< td class = "org-left" >   < / td >
2020-10-01 11:25:23 +02:00
< / tr >
< tr >
< td class = "org-left" > \(N_i\)< / td >
< td class = "org-left" > Weight representing the sensor noise< / td >
2020-10-01 15:14:10 +02:00
< td class = "org-left" > \([m/s]\)< / td >
2020-10-01 11:25:23 +02:00
< / tr >
< / tbody >
< / table >
2020-10-01 15:14:10 +02:00
< div id = "org9ff61e6" class = "figure" >
2020-10-01 11:25:23 +02:00
< p > < img src = "figs-tikz/sensor_model_noise_uncertainty.png" alt = "sensor_model_noise_uncertainty.png" / >
< / p >
< p > < span class = "figure-number" > Figure 1: < / span > Sensor Model< / p >
< / div >
< / div >
2020-10-01 15:14:10 +02:00
< div id = "outline-container-org3c3d9de" class = "outline-3" >
< h3 id = "org3c3d9de" > < span class = "section-number-3" > 1.1< / span > Sensor Dynamics< / h3 >
2020-10-01 11:25:23 +02:00
< div class = "outline-text-3" id = "text-1-1" >
2019-08-14 12:08:30 +02:00
< p >
2020-10-01 15:14:10 +02:00
< a id = "orgeec3842" > < / a >
2020-10-01 11:25:23 +02:00
Let’ s consider two sensors measuring the velocity of an object.
2019-08-14 12:08:30 +02:00
< / p >
< p >
2020-10-01 11:25:23 +02:00
The first sensor is an accelerometer.
Its nominal dynamics \(\hat{G}_1(s)\) is defined below.
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 11:25:23 +02:00
< div class = "org-src-container" >
< pre class = "src 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]
2020-10-01 15:14:10 +02:00
G1 = g_acc*m_acc*s/(m_acc*s^2 + c_acc*s + k_acc); % Accelerometer Plant [V/(m/s)]
2020-10-01 11:25:23 +02:00
< / pre >
< / div >
2019-08-14 12:08:30 +02:00
< p >
2020-10-01 11:25:23 +02:00
The second sensor is a displacement sensor, its nominal dynamics \(\hat{G}_2(s)\) is defined below.
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 11:25:23 +02:00
< div class = "org-src-container" >
< pre class = "src 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)]
< / pre >
< / div >
2019-08-14 12:08:30 +02:00
< p >
2020-10-01 11:25:23 +02:00
These nominal dynamics are also taken as the model of the sensor dynamics.
2020-10-01 15:14:10 +02:00
The true sensor dynamics has some uncertainty associated to it and described in section < a href = "#org5a0d0e4" > 1.2< / a > .
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 11:25:23 +02:00
< p >
2020-10-01 15:14:10 +02:00
Both sensor dynamics in \([\frac{V}{m/s}]\) are shown in Figure < a href = "#orgc562240" > 2< / a > .
2020-10-01 11:25:23 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< div id = "orgc562240" class = "figure" >
2020-10-01 11:25:23 +02:00
< p > < img src = "figs/sensors_nominal_dynamics.png" alt = "sensors_nominal_dynamics.png" / >
< / p >
< p > < span class = "figure-number" > Figure 2: < / span > Sensor nominal dynamics from the velocity of the object to the output voltage< / p >
< / div >
2019-08-14 12:08:30 +02:00
< / div >
< / div >
2020-10-01 15:14:10 +02:00
< div id = "outline-container-orgdad1407" class = "outline-3" >
< h3 id = "orgdad1407" > < span class = "section-number-3" > 1.2< / span > Sensor Model Uncertainty< / h3 >
2020-10-01 11:25:23 +02:00
< div class = "outline-text-3" id = "text-1-2" >
2019-08-14 12:08:30 +02:00
< p >
2020-10-01 15:14:10 +02:00
< a id = "org5a0d0e4" > < / a >
The uncertainty on the sensor dynamics is described by multiplicative uncertainty (Figure < a href = "#org9ff61e6" > 1< / a > ).
2019-08-14 12:08:30 +02:00
< / p >
< p >
2020-10-01 11:25:23 +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
< / p >
2020-10-01 11:25:23 +02:00
2019-08-29 14:55:03 +02:00
\begin{equation}
2020-10-01 11:25:23 +02:00
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 }
2019-08-29 14:55:03 +02:00
\end{equation}
2019-08-14 12:08:30 +02:00
2020-10-01 11:25:23 +02:00
< p >
2020-10-01 15:14:10 +02:00
The weights \(W_i(s)\) representing the dynamical uncertainty are defined below and their magnitude is shown in Figure < a href = "#orgf6732e7" > 3< / a > .
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 11:25:23 +02:00
< div class = "org-src-container" >
< pre class = "src 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);
W2 = createWeight('n', 2, 'w0', 2*pi*1e2, 'G0', 0.05, 'G1', 4, 'Gc', 1);
< / pre >
2019-08-14 12:08:30 +02:00
< / div >
< p >
2020-10-01 15:14:10 +02:00
The bode plot of the sensors nominal dynamics as well as their defined dynamical spread are shown in Figure < a href = "#orgc569fc6" > 4< / a > .
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 11:25:23 +02:00
2020-10-01 15:14:10 +02:00
< div id = "orgf6732e7" class = "figure" >
2020-10-01 11:25:23 +02:00
< p > < img src = "figs/sensors_uncertainty_weights.png" alt = "sensors_uncertainty_weights.png" / >
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 11:25:23 +02:00
< p > < span class = "figure-number" > Figure 3: < / span > Magnitude of the multiplicative uncertainty weights \(|W_i(j\omega)|\)< / p >
< / div >
2019-08-14 12:08:30 +02:00
2019-08-29 14:55:03 +02:00
2020-10-01 15:14:10 +02:00
< div id = "orgc569fc6" class = "figure" >
2020-10-01 11:25:23 +02:00
< p > < img src = "figs/sensors_nominal_dynamics_and_uncertainty.png" alt = "sensors_nominal_dynamics_and_uncertainty.png" / >
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 11:25:23 +02:00
< p > < span class = "figure-number" > Figure 4: < / span > Nominal Sensor Dynamics \(\hat{G}_i\) (solid lines) as well as the spread of the dynamical uncertainty (background color)< / p >
< / div >
< / div >
2019-08-29 14:55:03 +02:00
< / div >
2019-08-14 12:08:30 +02:00
2020-10-01 15:14:10 +02:00
< div id = "outline-container-orge7b9c26" class = "outline-3" >
< h3 id = "orge7b9c26" > < span class = "section-number-3" > 1.3< / span > Sensor Noise< / h3 >
2020-10-01 11:25:23 +02:00
< div class = "outline-text-3" id = "text-1-3" >
2019-08-14 12:08:30 +02:00
< p >
2020-10-01 15:14:10 +02:00
< a id = "org5456884" > < / a >
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 < a href = "#org9ff61e6" > 1< / a > ).
2019-08-14 12:08:30 +02:00
< / p >
2019-08-29 14:55:03 +02:00
\begin{equation}
2020-10-01 11:25:23 +02:00
\Phi_{\tilde{n}_i}(\omega) = 1 \label{eq:unitary_noise_psd}
2019-08-29 14:55:03 +02:00
\end{equation}
2019-08-14 12:08:30 +02:00
< p >
2020-10-01 11:25:23 +02:00
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}]\).
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 11:25:23 +02:00
\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}
2019-08-14 12:08:30 +02:00
< p >
2020-10-01 15:14:10 +02:00
The weights \(N_1\) and \(N_2\) representing the amplitude spectral density of the sensor noises are defined below and shown in Figure < a href = "#orgd3ea86e" > 5< / a > .
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 11:25:23 +02:00
< div class = "org-src-container" >
2020-10-01 15:14:10 +02:00
< pre class = "src src-matlab" > omegac = 0.15*2*pi; G0 = 1e-1; Ginf = 1e-6;
2020-10-01 11:25:23 +02:00
N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4);
2019-08-14 12:08:30 +02:00
2020-10-01 11:25:23 +02:00
omegac = 1000*2*pi; G0 = 1e-6; Ginf = 1e-3;
N2 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4);
< / pre >
< / div >
2020-10-01 15:14:10 +02:00
< div id = "orgd3ea86e" class = "figure" >
2020-10-01 11:25:23 +02:00
< p > < img src = "figs/sensors_noise.png" alt = "sensors_noise.png" / >
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 11:25:23 +02:00
< p > < span class = "figure-number" > Figure 5: < / span > Amplitude spectral density of the sensors \(\sqrt{\Phi_{n_i}(\omega)} = |N_i(j\omega)|\)< / p >
< / div >
2019-08-14 12:08:30 +02:00
< / div >
< / div >
2020-10-01 15:14:10 +02:00
< div id = "outline-container-org93a8b3d" class = "outline-3" >
< h3 id = "org93a8b3d" > < span class = "section-number-3" > 1.4< / span > Save Model< / h3 >
2020-10-01 11:25:23 +02:00
< div class = "outline-text-3" id = "text-1-4" >
2019-08-14 12:08:30 +02:00
< p >
2020-10-01 11:25:23 +02:00
All the dynamical systems representing the sensors are saved for further use.
2019-08-14 12:08:30 +02:00
< / p >
< div class = "org-src-container" >
2020-10-01 11:25:23 +02:00
< pre class = "src src-matlab" > save('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
2019-08-14 12:08:30 +02:00
< / pre >
< / div >
2020-10-01 11:25:23 +02:00
< / div >
< / div >
< / div >
2019-08-14 12:08:30 +02:00
2020-10-01 15:14:10 +02:00
< div id = "outline-container-org44fc9f2" class = "outline-2" >
< h2 id = "org44fc9f2" > < span class = "section-number-2" > 2< / span > Introduction to Sensor Fusion< / h2 >
2020-10-01 11:25:23 +02:00
< div class = "outline-text-2" id = "text-2" >
< p >
2020-10-01 15:14:10 +02:00
< a id = "orga4cc3b8" > < / a >
2020-10-01 11:25:23 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< / div >
< div id = "outline-container-org046f351" class = "outline-3" >
< h3 id = "org046f351" > < span class = "section-number-3" > 2.1< / span > Sensor Fusion Architecture< / h3 >
< div class = "outline-text-3" id = "text-2-1" >
2020-10-01 11:25:23 +02:00
< p >
2020-10-01 15:14:10 +02:00
< a id = "orgd2cc63f" > < / a >
2020-10-01 11:25:23 +02:00
< / p >
2019-08-14 12:08:30 +02:00
2020-10-01 11:25:23 +02:00
< p >
2020-10-01 15:14:10 +02:00
The two sensors presented in Section < a href = "#orgc105b40" > 1< / a > are now merged together using complementary filters \(H_1(s)\) and \(H_2(s)\) to form a super sensor (Figure < a href = "#orga6bd433" > 6< / a > ).
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 11:25:23 +02:00
2020-10-01 15:14:10 +02:00
< div id = "orga6bd433" class = "figure" >
< p > < img src = "figs-tikz/sensor_fusion_noise_arch.png" alt = "sensor_fusion_noise_arch.png" / >
< / p >
< p > < span class = "figure-number" > Figure 6: < / span > Sensor Fusion Architecture< / p >
< / div >
2020-10-01 11:25:23 +02:00
< p >
2020-10-01 15:14:10 +02:00
The complementary property of \(H_1(s)\) and \(H_2(s)\) means that the sum of their transfer function is equal to \(1\) \eqref{eq:complementary_property}.
2020-10-01 11:25:23 +02:00
< / p >
2020-10-01 15:14:10 +02:00
\begin{equation}
H_1(s) + H_2(s) = 1 \label{eq:complementary_property}
\end{equation}
2020-10-01 11:25:23 +02:00
< p >
2020-10-01 15:14:10 +02:00
The super sensor estimate \(\hat{x}\) is given by \eqref{eq:super_sensor_estimate}.
2020-10-01 11:25:23 +02:00
< / p >
2020-10-01 15:14:10 +02:00
\begin{equation}
\hat{x} = \left( H_1 \hat{G}_1^{-1} G_1 + H_2 \hat{G}_2^{-1} G_2 \right) x + \left( H_1 \hat{G}_1^{-1} G_1 N_1 \right) \tilde{n}_1 + \left( H_2 \hat{G}_2^{-1} G_2 N_2 \right) \tilde{n}_2 \label{eq:super_sensor_estimate}
\end{equation}
2019-08-14 12:08:30 +02:00
< / div >
< / div >
2020-10-01 15:14:10 +02:00
< div id = "outline-container-org21fff50" class = "outline-3" >
< h3 id = "org21fff50" > < span class = "section-number-3" > 2.2< / span > Super Sensor Noise< / h3 >
< div class = "outline-text-3" id = "text-2-2" >
< p >
< a id = "orgce01316" > < / a >
< / p >
< p >
If we first suppose that the models of the sensors \(\hat{G}_i\) are very close to the true sensor dynamics \(G_i\) \eqref{eq:good_dynamical_model}, we have that the super sensor estimate \(\hat{x}\) is equals to the measured quantity \(x\) plus the noise of the two sensors filtered out by the complementary filters \eqref{eq:estimate_perfect_models}.
< / p >
\begin{equation}
\hat{G}_i^{-1}(s) G_i(s) \approx 1 \label{eq:good_dynamical_model}
\end{equation}
\begin{equation}
\hat{x} = x + \underbrace{\left( H_1 N_1 \right) \tilde{n}_1 + \left( H_2 N_2 \right) \tilde{n}_2}_{n} \label{eq:estimate_perfect_models}
\end{equation}
< p >
As the noise of both sensors are considered to be uncorrelated, the PSD of the super sensor noise is computed as follow:
< / p >
\begin{equation}
\Phi_n(\omega) = \left| H_1(j\omega) N_1(j\omega) \right|^2 + \left| H_2(j\omega) N_2(j\omega) \right|^2 \label{eq:super_sensor_psd_noise}
\end{equation}
< p >
And the Root Mean Square (RMS) value of the super sensor noise \(\sigma_n\) is given by Equation \eqref{eq:super_sensor_rms_noise}.
< / p >
\begin{equation}
\sigma_n = \sqrt{\int_0^\infty \Phi_n(\omega) d\omega} \label{eq:super_sensor_rms_noise}
\end{equation}
< / div >
< / div >
< div id = "outline-container-org1c04704" class = "outline-3" >
< h3 id = "org1c04704" > < span class = "section-number-3" > 2.3< / span > Super Sensor Dynamical Uncertainty< / h3 >
< div class = "outline-text-3" id = "text-2-3" >
< p >
< a id = "orgea09f91" > < / a >
< / p >
< p >
If we consider some dynamical uncertainty (the true system dynamics \(G_i\) not being perfectly equal to our model \(\hat{G}_i\)) that we model by the use of multiplicative uncertainty (Figure < a href = "#orgeccb26f" > 7< / a > ), the super sensor dynamics is then equals to:
< / p >
\begin{equation}
\begin{aligned}
\frac{\hat{x}}{x} & = \Big( H_1 \hat{G}_1^{-1} \hat{G}_1 (1 + W_1 \Delta_1) + H_2 \hat{G}_2^{-1} \hat{G}_2 (1 + W_2 \Delta_2) \Big) \\
& = \Big( H_1 (1 + W_1 \Delta_1) + H_2 (1 + W_2 \Delta_2) \Big) \\
& = \left( 1 + H_1 W_1 \Delta_1 + H_2 W_2 \Delta_2 \right), \quad \|\Delta_i\|_\infty< 1
\end{aligned}
\end{equation}
< div id = "orgeccb26f" class = "figure" >
< p > < img src = "figs-tikz/sensor_model_uncertainty.png" alt = "sensor_model_uncertainty.png" / >
< / p >
< p > < span class = "figure-number" > Figure 7: < / span > Sensor Model including Dynamical Uncertainty< / p >
< / div >
< p >
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)|\) as shown in Figure < a href = "#org15230a0" > 8< / a > .
< / p >
< div id = "org15230a0" class = "figure" >
< p > < img src = "figs-tikz/uncertainty_set_super_sensor.png" alt = "uncertainty_set_super_sensor.png" / >
< / p >
< p > < span class = "figure-number" > Figure 8: < / span > Super Sensor model uncertainty displayed in the complex plane< / p >
< / div >
< / div >
< / div >
< / div >
< div id = "outline-container-org24786a9" class = "outline-2" >
< h2 id = "org24786a9" > < span class = "section-number-2" > 3< / span > Optimal Super Sensor Noise: \(\mathcal{H}_2\) Synthesis< / h2 >
< div class = "outline-text-2" id = "text-3" >
< p >
< a id = "orgd3b3857" > < / a >
< / p >
< p >
In this section, the complementary filters \(H_1(s)\) and \(H_2(s)\) are designed in order to minimize the RMS value of super sensor noise \(\sigma_n\).
< / p >
< div id = "org0929d72" class = "figure" >
< p > < img src = "figs-tikz/sensor_fusion_noise_arch.png" alt = "sensor_fusion_noise_arch.png" / >
< / p >
< p > < span class = "figure-number" > Figure 9: < / span > Optimal Sensor Fusion Architecture< / p >
< / div >
< p >
The RMS value of the super sensor noise is (neglecting the model uncertainty):
< / p >
\begin{equation}
\begin{aligned}
\sigma_{n} & = \sqrt{\int_0^\infty |H_1(j\omega) N_1(j\omega)|^2 + |H_2(j\omega) N_2(j\omega)|^2 d\omega} \\
& = \left\| \begin{matrix} H_1 N_1 \\ H_2 N_2 \end{matrix} \right\|_2
\end{aligned}
\end{equation}
2019-08-14 12:08:30 +02:00
< p >
2020-10-01 15:14:10 +02:00
The goal is to design \(H_1(s)\) and \(H_2(s)\) such that \(H_1(s) + H_2(s) = 1\) (complementary property) and such that \(\left\| \begin{matrix} H_1 N_1 \\ H_2 N_2 \end{matrix} \right\|_2\) is minimized (minimized RMS value of the super sensor noise).
This is done using the \(\mathcal{H}_2\) synthesis in Section < a href = "#org5b1b6c8" > 3.1< / a > .
2019-08-29 14:55:03 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< / div >
2019-08-29 14:55:03 +02:00
2020-10-01 15:14:10 +02:00
< div id = "outline-container-orgb5cf4c2" class = "outline-3" >
< h3 id = "orgb5cf4c2" > < span class = "section-number-3" > 3.1< / span > \(\mathcal{H}_2\) Synthesis< / h3 >
< div class = "outline-text-3" id = "text-3-1" >
2019-08-29 14:55:03 +02:00
< p >
2020-10-01 15:14:10 +02:00
< a id = "org5b1b6c8" > < / a >
2019-08-29 14:55:03 +02:00
< / p >
< p >
2020-10-01 15:14:10 +02:00
Consider the generalized plant \(P_{\mathcal{H}_2}\) shown in Figure < a href = "#org3dd902a" > 10< / a > and described by Equation \eqref{eq:H2_generalized_plant}.
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< div id = "org3dd902a" class = "figure" >
< p > < img src = "figs-tikz/h_two_optimal_fusion.png" alt = "h_two_optimal_fusion.png" / >
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< p > < span class = "figure-number" > Figure 10: < / span > Architecture used for \(\mathcal{H}_\infty\) synthesis of complementary filters< / p >
2019-08-14 12:08:30 +02:00
< / div >
2020-10-01 15:14:10 +02:00
\begin{equation} \label{eq:H2_generalized_plant}
2019-08-30 14:39:01 +02:00
\begin{pmatrix}
2020-10-01 15:14:10 +02:00
z_1 \\ z_2 \\ v
\end{pmatrix} = \underbrace{\begin{bmatrix}
N_1 & -N_1 \\
0 & N_2 \\
1 & 0
\end{bmatrix}}_{P_{\mathcal{H}_2}} \begin{pmatrix}
w \\ u
2019-08-30 14:39:01 +02:00
\end{pmatrix}
2020-10-01 15:14:10 +02:00
\end{equation}
2019-08-30 14:39:01 +02:00
2019-08-14 12:08:30 +02:00
< p >
2020-10-01 15:14:10 +02:00
Applying the \(\mathcal{H}_2\) synthesis on \(P_{\mathcal{H}_2}\) will generate a filter \(H_2(s)\) such that the \(\mathcal{H}_2\) norm from \(w\) to \((z_1,z_2)\) which is actually equals to \(\sigma_n\) by defining \(H_1(s) = 1 - H_2(s)\):
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 15:14:10 +02:00
\begin{equation}
\left\| \begin{matrix} z_1/w \\ z_2/w \end{matrix} \right\|_2 = \left\| \begin{matrix} N_1 (1 - H_2) \\ N_2 H_2 \end{matrix} \right\|_2 = \sigma_n \quad \text{with} \quad H_1(s) = 1 - H_2(s)
\end{equation}
2019-08-14 12:08:30 +02:00
< p >
2020-10-01 15:14:10 +02:00
We then have that the \(\mathcal{H}_2\) synthesis applied on \(P_{\mathcal{H}_2}\) generates two complementary filters \(H_1(s)\) and \(H_2(s)\) such that the RMS value of super sensor noise is minimized.
2019-08-14 12:08:30 +02:00
< / p >
< p >
2020-10-01 15:14:10 +02:00
The generalized plant \(P_{\mathcal{H}_2}\) is defined below
2019-08-14 12:08:30 +02:00
< / p >
< div class = "org-src-container" >
2020-10-01 15:14:10 +02:00
< pre class = "src src-matlab" > PH2 = [N1 -N1;
0 N2;
1 0];
2019-08-14 12:08:30 +02:00
< / pre >
< / div >
< p >
2020-10-01 15:14:10 +02:00
The \(\mathcal{H}_2\) synthesis using the < code > h2syn< / code > command
2019-08-14 12:08:30 +02:00
< / p >
< div class = "org-src-container" >
2020-10-01 15:14:10 +02:00
< pre class = "src src-matlab" > [H2, ~, gamma] = h2syn(PH2, 1, 1);
2019-08-14 12:08:30 +02:00
< / pre >
< / div >
< p >
2020-10-01 15:14:10 +02:00
Finally, \(H_1(s)\) is defined as follows
2019-08-14 12:08:30 +02:00
< / p >
< div class = "org-src-container" >
2020-10-01 11:25:23 +02:00
< pre class = "src src-matlab" > H1 = 1 - H2;
2019-08-14 12:08:30 +02:00
< / pre >
< / div >
< p >
2020-10-01 15:14:10 +02:00
The obtained complementary filters are shown in Figure < a href = "#org320a0c5" > 11< / a > .
2019-08-29 14:55:03 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< div id = "org320a0c5" class = "figure" >
2020-10-01 11:25:23 +02:00
< p > < img src = "figs/htwo_comp_filters.png" alt = "htwo_comp_filters.png" / >
2019-08-29 14:55:03 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< p > < span class = "figure-number" > Figure 11: < / span > Obtained complementary filters using the \(\mathcal{H}_2\) Synthesis< / p >
2020-10-01 11:25:23 +02:00
< / div >
< / div >
< / div >
2019-08-29 14:55:03 +02:00
2020-10-01 15:14:10 +02:00
< div id = "outline-container-org5d3402d" class = "outline-3" >
< h3 id = "org5d3402d" > < span class = "section-number-3" > 3.2< / span > Super Sensor Noise< / h3 >
< div class = "outline-text-3" id = "text-3-2" >
2019-08-29 14:55:03 +02:00
< p >
2020-10-01 15:14:10 +02:00
< a id = "org22cf774" > < / a >
2019-08-29 14:55:03 +02:00
< / p >
< p >
2020-10-01 15:14:10 +02:00
The Power Spectral Density of the individual sensors’ noise \(\Phi_{n_1}, \Phi_{n_2}\) and of the super sensor noise \(\Phi_{n_{\mathcal{H}_2}}\) are computed below and shown in Figure < a href = "#orgd017917" > 12< / a > .
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< div class = "org-src-container" >
< pre class = "src 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;
< / pre >
< / div >
2019-08-14 12:08:30 +02:00
2020-10-01 11:25:23 +02:00
< p >
2020-10-01 15:14:10 +02:00
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 < a href = "#org069ec84" > 13< / a > .
2019-08-14 12:08:30 +02:00
< / p >
2019-08-29 14:55:03 +02:00
< div class = "org-src-container" >
2020-10-01 15:14:10 +02:00
< pre class = "src src-matlab" > CPS_S1 = cumtrapz(freqs, PSD_S1);
2020-10-01 11:25:23 +02:00
CPS_S2 = cumtrapz(freqs, PSD_S2);
CPS_H2 = cumtrapz(freqs, PSD_H2);
2019-08-29 14:55:03 +02:00
< / pre >
< / div >
2019-08-14 12:08:30 +02:00
2020-10-01 15:14:10 +02:00
\begin{equation}
\Gamma_n (\omega) = \int_0^\omega \Phi_n(\nu) d\nu \label{eq:CPS_definition}
\end{equation}
2019-08-29 14:55:03 +02:00
2020-10-01 15:14:10 +02:00
< p >
The RMS value of the individual sensors and of the super sensor are listed in Table < a href = "#org0aedb51" > 3< / a > .
2019-08-29 14:55:03 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< table id = "org0aedb51" border = "2" cellspacing = "0" cellpadding = "6" rules = "groups" frame = "hsides" >
< caption class = "t-above" > < span class = "table-number" > Table 3:< / span > RMS value of the individual sensor noise and of the super sensor using the \(\mathcal{H}_2\) Synthesis< / caption >
2020-10-01 11:25:23 +02:00
< colgroup >
< col class = "org-left" / >
< col class = "org-right" / >
< / colgroup >
< thead >
< tr >
< th scope = "col" class = "org-left" >   < / th >
2020-10-01 15:14:10 +02:00
< th scope = "col" class = "org-right" > RMS value \([m/s]\)< / th >
2020-10-01 11:25:23 +02:00
< / tr >
< / thead >
< tbody >
< tr >
2020-10-01 15:14:10 +02:00
< td class = "org-left" > \(\sigma_{n_1}\)< / td >
< td class = "org-right" > 0.015< / td >
2020-10-01 11:25:23 +02:00
< / tr >
< tr >
2020-10-01 15:14:10 +02:00
< td class = "org-left" > \(\sigma_{n_2}\)< / td >
2020-10-01 11:25:23 +02:00
< td class = "org-right" > 0.08< / td >
< / tr >
< tr >
2020-10-01 15:14:10 +02:00
< td class = "org-left" > \(\sigma_{n_{\mathcal{H}_2}}\)< / td >
< td class = "org-right" > 0.0027< / td >
2020-10-01 11:25:23 +02:00
< / tr >
< / tbody >
< / table >
2019-08-29 14:55:03 +02:00
2019-08-29 22:47:20 +02:00
2020-10-01 15:14:10 +02:00
< div id = "orgd017917" class = "figure" >
< p > < img src = "figs/psd_sensors_htwo_synthesis.png" alt = "psd_sensors_htwo_synthesis.png" / >
< / p >
< p > < span class = "figure-number" > Figure 12: < / span > Power Spectral Density of the estimated \(\hat{x}\) using the two sensors alone and using the optimally fused signal< / p >
2019-08-29 22:47:20 +02:00
< / div >
2019-08-14 12:08:30 +02:00
2020-10-01 15:14:10 +02:00
< div id = "org069ec84" class = "figure" >
< p > < img src = "figs/cps_h2_synthesis.png" alt = "cps_h2_synthesis.png" / >
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< p > < span class = "figure-number" > Figure 13: < / span > Cumulative Power Spectrum of individual sensors and super sensor using the \(\mathcal{H}_2\) synthesis< / p >
2019-08-14 12:08:30 +02:00
< / div >
< p >
2020-10-01 15:14:10 +02:00
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 < a href = "#org87c67f2" > 14< / a > .
The resulting noises are displayed in Figure < a href = "#orgeabe8e8" > 15< / a > .
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< div id = "org87c67f2" class = "figure" >
2020-10-01 11:25:23 +02:00
< p > < img src = "figs/super_sensor_time_domain_h2.png" alt = "super_sensor_time_domain_h2.png" / >
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< p > < span class = "figure-number" > Figure 14: < / span > Noise of individual sensors and noise of the super sensor< / p >
2019-08-30 14:39:01 +02:00
< / div >
2020-10-01 15:14:10 +02:00
< div id = "orgeabe8e8" class = "figure" >
< p > < img src = "figs/sensor_noise_H2_time_domain.png" alt = "sensor_noise_H2_time_domain.png" / >
< / p >
2019-08-30 14:39:01 +02:00
< / div >
< / div >
2020-10-01 11:25:23 +02:00
< / div >
2020-10-01 15:14:10 +02:00
< div id = "outline-container-orgd724be3" class = "outline-3" >
< h3 id = "orgd724be3" > < span class = "section-number-3" > 3.3< / span > Discrepancy between sensor dynamics and model< / h3 >
< div class = "outline-text-3" id = "text-3-3" >
< p >
If we consider sensor dynamical uncertainty as explained in Section < a href = "#org5a0d0e4" > 1.2< / a > , we can compute what would be the super sensor dynamical uncertainty when using the complementary filters obtained using the \(\mathcal{H}_2\) Synthesis.
< / p >
2019-08-14 12:08:30 +02:00
< p >
2020-10-01 15:14:10 +02:00
The super sensor dynamical uncertainty is shown in Figure < a href = "#org482651f" > 16< / a > .
2019-08-14 12:08:30 +02:00
< / p >
< p >
2020-10-01 15:14:10 +02:00
It is shown that the phase uncertainty is not bounded between 100Hz and 200Hz.
As a result the super sensor signal can not be used for feedback applications about 100Hz.
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< div id = "org482651f" class = "figure" >
< p > < img src = "figs/super_sensor_dynamical_uncertainty_H2.png" alt = "super_sensor_dynamical_uncertainty_H2.png" / >
< / p >
< p > < span class = "figure-number" > Figure 16: < / span > Super sensor dynamical uncertainty when using the \(\mathcal{H}_2\) Synthesis< / p >
2019-08-30 14:39:01 +02:00
< / div >
< / div >
< / div >
2019-08-14 12:08:30 +02:00
2020-10-01 15:14:10 +02:00
< div id = "outline-container-org14ef526" class = "outline-3" >
< h3 id = "org14ef526" > < span class = "section-number-3" > 3.4< / span > Conclusion< / h3 >
< / div >
< / div >
< div id = "outline-container-orge170b7d" class = "outline-2" >
< h2 id = "orge170b7d" > < span class = "section-number-2" > 4< / span > Robust Sensor Fusion: \(\mathcal{H}_\infty\) Synthesis< / h2 >
< div class = "outline-text-2" id = "text-4" >
2019-08-14 12:08:30 +02:00
< p >
2020-10-01 15:14:10 +02:00
< a id = "orgfcc39fd" > < / a >
2019-08-14 12:08:30 +02:00
< / p >
< p >
2019-08-30 14:39:01 +02:00
We initially considered perfectly known sensor dynamics so that it can be perfectly inverted.
2019-08-14 12:08:30 +02:00
< / p >
< p >
2019-08-30 14:39:01 +02:00
We now take into account the fact that the sensor dynamics is only partially known.
2020-10-01 15:14:10 +02:00
To do so, we model the uncertainty that we have on the sensor dynamics by multiplicative input uncertainty as shown in Figure < a href = "#org2ede630" > 17< / a > .
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< div id = "org2ede630" class = "figure" >
< p > < img src = "figs-tikz/sensor_fusion_arch_uncertainty.png" alt = "sensor_fusion_arch_uncertainty.png" / >
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< p > < span class = "figure-number" > Figure 17: < / span > Sensor fusion architecture with sensor dynamics uncertainty< / p >
2019-08-14 12:08:30 +02:00
< / div >
< p >
2019-08-30 14:39:01 +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.
2019-08-14 12:08:30 +02:00
< / p >
2019-08-30 14:39:01 +02:00
< / div >
2019-08-14 12:08:30 +02:00
2020-10-01 15:14:10 +02:00
< div id = "outline-container-org58a2403" class = "outline-3" >
< h3 id = "org58a2403" > < span class = "section-number-3" > 4.1< / span > Super Sensor Dynamical Uncertainty< / h3 >
< div class = "outline-text-3" id = "text-4-1" >
2019-08-14 12:08:30 +02:00
< p >
2019-08-30 14:39:01 +02:00
In practical systems, the sensor dynamics has always some level of uncertainty.
2019-08-14 12:08:30 +02:00
< / p >
< p >
2019-08-30 14:39:01 +02:00
The dynamics of the super sensor is represented by
2019-08-14 12:08:30 +02:00
< / p >
2019-08-29 22:47:20 +02:00
\begin{align*}
2020-10-01 11:25:23 +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
2019-08-29 22:47:20 +02:00
\end{align*}
2019-08-14 12:08:30 +02:00
< p >
2019-08-30 14:39:01 +02:00
with \(\Delta_i\) is any transfer function satisfying \(\| \Delta_i \|_\infty < 1 \ ) .
2019-08-14 12:08:30 +02:00
< / p >
2019-08-30 14:39:01 +02:00
< p >
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}\).
2019-08-14 12:08:30 +02:00
< / p >
< p >
2020-10-01 15:14:10 +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 < a href = "#orgd8516e1" > 18< / a > ).
2019-08-14 12:08:30 +02:00
< / p >
< p >
2019-08-30 14:39:01 +02:00
We then have that the angle introduced by the super sensor is bounded by \(\arcsin(\epsilon)\):
2020-10-01 11:25:23 +02:00
\[ \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) \]
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< div id = "orgd8516e1" class = "figure" >
2019-08-30 14:39:01 +02:00
< p > < img src = "figs-tikz/uncertainty_gain_phase_variation.png" alt = "uncertainty_gain_phase_variation.png" / >
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< p > < span class = "figure-number" > Figure 18: < / span > Maximum phase variation< / p >
2019-08-14 12:08:30 +02:00
< / div >
< / div >
< / div >
2020-10-01 15:14:10 +02:00
< div id = "outline-container-org9563d0a" class = "outline-3" >
< h3 id = "org9563d0a" > < span class = "section-number-3" > 4.2< / span > Synthesis objective< / h3 >
< div class = "outline-text-3" id = "text-4-2" >
2019-08-29 22:47:20 +02:00
< p >
2020-10-01 15:14:10 +02:00
The uncertainty region of the super sensor dynamics is represented by a circle in the complex plane as shown in Figure < a href = "#orgd8516e1" > 18< / a > .
2019-08-29 22:47:20 +02:00
< / p >
< p >
2020-10-01 11:25:23 +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)|\).
2019-08-29 22:47:20 +02:00
< / p >
< p >
Thus, the phase shift \(\Delta\phi(\omega)\) due to the super sensor uncertainty is bounded by:
2020-10-01 11:25:23 +02:00
\[ |\Delta\phi(\omega)| \leq \arcsin\big( |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| \big) \]
2019-08-29 22:47:20 +02:00
< / p >
< p >
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:
2019-08-29 22:47:20 +02:00
\[ |\Delta\phi(\omega)| < \Delta\phi_\text{max}(\omega), \quad \forall\omega \]
< / p >
< p >
If \(H_1(s)\) and \(H_2(s)\) are designed such that
2020-10-01 11:25:23 +02:00
\[ |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| < \sin\big( \Delta\phi_\text{max}(\omega) \big) \]
2019-08-29 22:47:20 +02:00
< / p >
< p >
The maximum phase shift due to dynamic uncertainty at frequency \(\omega\) will be \(\Delta\phi_\text{max}(\omega)\).
< / p >
< / div >
< / div >
2020-10-01 15:14:10 +02:00
< div id = "outline-container-orgd7f10bf" class = "outline-3" >
< h3 id = "orgd7f10bf" > < span class = "section-number-3" > 4.3< / span > Requirements as an \(\mathcal{H}_\infty\) norm< / h3 >
< div class = "outline-text-3" id = "text-4-3" >
2019-08-29 22:47:20 +02:00
< p >
2020-09-30 08:47:27 +02:00
We now try to express this requirement in terms of an \(\mathcal{H}_\infty\) norm.
2019-08-29 22:47:20 +02:00
< / p >
< p >
2020-10-01 11:25:23 +02:00
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 \]
2019-08-29 22:47:20 +02:00
< / p >
< p >
Then:
< / p >
\begin{align*}
2020-10-01 11:25:23 +02:00
& |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
2019-08-29 22:47:20 +02:00
\end{align*}
< p >
2019-08-30 09:17:18 +02:00
Which is approximately equivalent to (with an error of maximum \(\sqrt{2}\)):
2019-08-29 22:47:20 +02:00
< / p >
\begin{equation}
2020-10-01 15:14:10 +02:00
\label{org99994fc}
2020-10-01 11:25:23 +02:00
\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
2019-08-29 22:47:20 +02:00
\end{equation}
< p >
2020-10-01 11:25:23 +02:00
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\).
2019-08-29 22:47:20 +02:00
< / p >
< / div >
< / div >
2020-10-01 15:14:10 +02:00
< div id = "outline-container-org2c9d72a" class = "outline-3" >
< h3 id = "org2c9d72a" > < span class = "section-number-3" > 4.4< / span > Weighting Function used to bound the super sensor uncertainty< / h3 >
< div class = "outline-text-3" id = "text-4-4" >
2019-08-29 22:47:20 +02:00
< p >
2020-10-01 11:25:23 +02:00
Let’ s define \(W_\phi(s)\) in order to bound the maximum allowed phase uncertainty \(\Delta\phi_\text{max}\) of the super sensor dynamics.
2019-08-29 22:47:20 +02:00
< / p >
< div class = "org-src-container" >
2020-10-01 11:25:23 +02:00
< pre class = "src src-matlab" > Dphi = 10; % [deg]
2019-08-29 22:47:20 +02:00
2020-10-01 11:25:23 +02:00
Wu = createWeight('n', 2, 'w0', 2*pi*4e2, 'G0', 1/sin(Dphi*pi/180), 'G1', 1/4, 'Gc', 1);
2019-08-29 22:47:20 +02:00
< / pre >
< / div >
2020-10-01 11:25:23 +02:00
< div class = "org-src-container" >
< pre class = "src src-matlab" > save('./mat/Wu.mat', 'Wu');
< / pre >
2019-08-29 22:47:20 +02:00
< / div >
< p >
2020-10-01 15:14:10 +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 < a href = "#orgb53fcec" > 19< / a > .
2019-08-29 22:47:20 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< div id = "orgb53fcec" class = "figure" >
2019-08-29 22:47:20 +02:00
< p > < img src = "figs/upper_bounds_comp_filter_max_phase_uncertainty.png" alt = "upper_bounds_comp_filter_max_phase_uncertainty.png" / >
< / p >
2020-10-01 15:14:10 +02:00
< p > < span class = "figure-number" > Figure 19: < / span > Upper bounds on the complementary filters set in order to limit the maximum phase uncertainty of the super sensor to 30 degrees until 500Hz< / p >
2019-08-29 22:47:20 +02:00
< / div >
2019-08-30 14:39:01 +02:00
< / div >
< / div >
2019-08-29 22:47:20 +02:00
2020-10-01 15:14:10 +02:00
< div id = "outline-container-org2d1873b" class = "outline-3" >
< h3 id = "org2d1873b" > < span class = "section-number-3" > 4.5< / span > \(\mathcal{H}_\infty\) Synthesis< / h3 >
< div class = "outline-text-3" id = "text-4-5" >
2019-08-29 22:47:20 +02:00
< p >
2020-10-01 15:14:10 +02:00
The \(\mathcal{H}_\infty\) synthesis architecture used for the complementary filters is shown in Figure < a href = "#org009b5de" > 20< / a > .
2019-08-30 14:39:01 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< div id = "org009b5de" class = "figure" >
2019-08-30 14:39:01 +02:00
< p > < img src = "figs-tikz/h_infinity_robust_fusion.png" alt = "h_infinity_robust_fusion.png" / >
2019-08-29 22:47:20 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< p > < span class = "figure-number" > Figure 20: < / span > Architecture used for \(\mathcal{H}_\infty\) synthesis of complementary filters< / p >
2019-08-30 14:39:01 +02:00
< / div >
2019-08-29 22:47:20 +02:00
2019-08-30 14:39:01 +02:00
< p >
The generalized plant is defined below.
< / p >
2019-08-29 22:47:20 +02:00
< div class = "org-src-container" >
2020-10-01 11:25:23 +02:00
< pre class = "src src-matlab" > P = [Wu*W1 -Wu*W1;
2020-10-01 15:14:10 +02:00
0 Wu*W2;
1 0];
2019-08-29 22:47:20 +02:00
< / pre >
< / div >
< p >
And we do the \(\mathcal{H}_\infty\) synthesis using the < code > hinfsyn< / code > command.
< / p >
< div class = "org-src-container" >
2020-09-30 08:47:27 +02:00
< pre class = "src src-matlab" > [H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
2019-08-29 22:47:20 +02:00
< / pre >
< / div >
< pre class = "example" >
[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
2020-10-01 11:25:23 +02:00
Test bounds: 0.7071 < = gamma < = 1.291
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-08-29 22:47:20 +02:00
< / pre >
2019-08-30 14:39:01 +02:00
< p >
And \(H_1(s)\) is defined as the complementary of \(H_2(s)\).
< / p >
2019-08-29 22:47:20 +02:00
< div class = "org-src-container" >
2020-09-30 08:47:27 +02:00
< pre class = "src src-matlab" > H1 = 1 - H2;
2019-08-29 22:47:20 +02:00
< / pre >
< / div >
2019-08-30 14:39:01 +02:00
< p >
2020-10-01 15:14:10 +02:00
The obtained complementary filters are shown in Figure < a href = "#org8022713" > 21< / a > .
2019-08-30 14:39:01 +02:00
< / p >
2019-08-29 22:47:20 +02:00
2020-10-01 15:14:10 +02:00
< div id = "org8022713" class = "figure" >
2019-08-29 22:47:20 +02:00
< p > < img src = "figs/comp_filter_hinf_uncertainty.png" alt = "comp_filter_hinf_uncertainty.png" / >
< / p >
2020-10-01 15:14:10 +02:00
< p > < span class = "figure-number" > Figure 21: < / span > Obtained complementary filters< / p >
2019-08-29 22:47:20 +02:00
< / div >
2019-08-14 12:08:30 +02:00
< / div >
< / div >
2019-08-29 22:47:20 +02:00
2020-10-01 15:14:10 +02:00
< div id = "outline-container-org30c3b86" class = "outline-3" >
< h3 id = "org30c3b86" > < span class = "section-number-3" > 4.6< / span > Super sensor uncertainty< / h3 >
< div class = "outline-text-3" id = "text-4-6" >
2019-08-29 22:47:20 +02:00
< div class = "org-src-container" >
2020-10-01 11:25:23 +02:00
< pre class = "src src-matlab" > H2_filters = load('./mat/H2_filters.mat', 'H2', 'H1');
2019-08-29 22:47:20 +02:00
< / pre >
< / div >
< p >
2019-08-30 14:39:01 +02:00
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.
< / p >
< p >
We here just used very wimple weights.
2020-10-01 11:25:23 +02:00
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.
2019-08-29 22:47:20 +02:00
< / p >
< / div >
< / div >
2019-08-30 14:39:01 +02:00
2020-10-01 15:14:10 +02:00
< div id = "outline-container-org1a771db" class = "outline-3" >
< h3 id = "org1a771db" > < span class = "section-number-3" > 4.7< / span > Super sensor noise< / h3 >
< div class = "outline-text-3" id = "text-4-7" >
2019-08-30 14:39:01 +02:00
< p >
2020-09-30 08:47:27 +02:00
We now compute the obtain Power Spectral Density of the super sensor’ s noise.
2019-08-30 14:39:01 +02:00
The noise characteristics of both individual sensor are defined below.
< / p >
2020-10-01 11:25:23 +02:00
< p >
2020-10-01 15:14:10 +02:00
The PSD of both sensor and of the super sensor is shown in Figure < a href = "#org0f597e8" > 22< / a > .
The CPS of both sensor and of the super sensor is shown in Figure < a href = "#org91e95dc" > 23< / a > .
2020-10-01 11:25:23 +02:00
< / p >
2019-08-30 14:39:01 +02:00
2020-10-01 11:25:23 +02:00
< div class = "org-src-container" >
< pre class = "src 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);
2019-08-30 14:39:01 +02:00
< / pre >
< / div >
2020-10-01 15:14:10 +02:00
< div id = "org0f597e8" class = "figure" >
2019-08-30 14:39:01 +02:00
< p > < img src = "figs/psd_sensors_hinf_synthesis.png" alt = "psd_sensors_hinf_synthesis.png" / >
< / p >
2020-10-01 15:14:10 +02:00
< p > < span class = "figure-number" > Figure 22: < / span > Power Spectral Density of the obtained super sensor using the \(\mathcal{H}_\infty\) synthesis< / p >
2019-08-30 14:39:01 +02:00
< / div >
2020-10-01 15:14:10 +02:00
< div id = "org91e95dc" class = "figure" >
2019-08-30 14:39:01 +02:00
< p > < img src = "figs/cps_sensors_hinf_synthesis.png" alt = "cps_sensors_hinf_synthesis.png" / >
< / p >
2020-10-01 15:14:10 +02:00
< p > < span class = "figure-number" > Figure 23: < / span > Cumulative Power Spectrum of the obtained super sensor using the \(\mathcal{H}_\infty\) synthesis< / p >
2019-08-30 14:39:01 +02:00
< / div >
2019-08-29 22:47:20 +02:00
< / div >
< / div >
2020-10-01 15:14:10 +02:00
< div id = "outline-container-org27b50b1" class = "outline-3" >
< h3 id = "org27b50b1" > < span class = "section-number-3" > 4.8< / span > Conclusion< / h3 >
< div class = "outline-text-3" id = "text-4-8" >
2019-08-30 14:39:01 +02:00
< p >
Using the \(\mathcal{H}_\infty\) synthesis, the dynamical uncertainty of the super sensor can be bounded to acceptable values.
< / p >
< p >
However, the RMS of the super sensor noise is not optimized as it was the case with the \(\mathcal{H}_2\) synthesis
< / p >
< / div >
< / div >
< / div >
2020-10-01 15:14:10 +02:00
< div id = "outline-container-org0e86f6d" class = "outline-2" >
< h2 id = "org0e86f6d" > < span class = "section-number-2" > 5< / span > Optimal and Robust Sensor Fusion: Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis< / h2 >
< div class = "outline-text-2" id = "text-5" >
2019-09-03 09:01:59 +02:00
< p >
2020-10-01 15:14:10 +02:00
< a id = "org760d4b9" > < / a >
2019-09-03 09:01:59 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< div id = "org2752cd1" class = "figure" >
< p > < img src = "figs-tikz/sensor_fusion_arch_full.png" alt = "sensor_fusion_arch_full.png" / >
< / p >
< p > < span class = "figure-number" > Figure 24: < / span > Sensor fusion architecture with sensor dynamics uncertainty< / p >
2019-09-03 09:01:59 +02:00
< / div >
2019-08-30 14:39:01 +02:00
< / div >
2020-10-01 15:14:10 +02:00
< div id = "outline-container-orgc034a5e" class = "outline-3" >
< h3 id = "orgc034a5e" > < span class = "section-number-3" > 5.1< / span > Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis - Introduction< / h3 >
< div class = "outline-text-3" id = "text-5-1" >
2019-08-30 14:39:01 +02:00
< p >
The goal is to design complementary filters such that:
< / p >
< ul class = "org-ul" >
2019-09-03 09:01:59 +02:00
< li > the maximum uncertainty of the super sensor is bounded< / li >
2019-08-30 14:39:01 +02:00
< li > the RMS value of the super sensor noise is minimized< / li >
< / ul >
< p >
To do so, we can use the Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis.
< / p >
< p >
The Matlab function for that is < code > h2hinfsyn< / code > (< a href = "https://fr.mathworks.com/help/robust/ref/h2hinfsyn.html" > doc< / a > ).
< / p >
< / div >
< / div >
2020-10-01 15:14:10 +02:00
< div id = "outline-container-org69a4331" class = "outline-3" >
< h3 id = "org69a4331" > < span class = "section-number-3" > 5.2< / span > Noise characteristics and Uncertainty of the individual sensors< / h3 >
< div class = "outline-text-3" id = "text-5-2" >
2019-08-30 14:39:01 +02:00
< p >
2020-10-01 15:14:10 +02:00
Both dynamical uncertainty and noise characteristics of the individual sensors are shown in Figure < a href = "#orgeed2e30" > 25< / a > .
2019-08-30 14:39:01 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< div id = "orgeed2e30" class = "figure" >
2019-09-03 09:01:59 +02:00
< p > < img src = "figs/mixed_synthesis_noise_uncertainty_sensors.png" alt = "mixed_synthesis_noise_uncertainty_sensors.png" / >
< / p >
2020-10-01 15:14:10 +02:00
< p > < span class = "figure-number" > Figure 25: < / span > Noise characteristsics and Dynamical uncertainty of the individual sensors< / p >
2019-09-03 09:01:59 +02:00
< / div >
< / div >
< / div >
2020-10-01 15:14:10 +02:00
< div id = "outline-container-org014bfe3" class = "outline-3" >
< h3 id = "org014bfe3" > < span class = "section-number-3" > 5.3< / span > Weighting Functions on the uncertainty of the super sensor< / h3 >
< div class = "outline-text-3" id = "text-5-3" >
2019-09-03 09:01:59 +02:00
< p >
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 15:14:10 +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.
2019-09-03 09:01:59 +02:00
< / p >
< / div >
< / div >
2020-10-01 15:14:10 +02:00
< div id = "outline-container-orgd80e164" class = "outline-3" >
< h3 id = "orgd80e164" > < span class = "section-number-3" > 5.4< / span > Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis< / h3 >
< div class = "outline-text-3" id = "text-5-4" >
2019-09-03 09:01:59 +02:00
< p >
2020-10-01 15:14:10 +02:00
The synthesis architecture that is used here is shown in Figure < a href = "#org4520d6a" > 26< / a > .
2019-09-03 09:01:59 +02:00
< / p >
< p >
The controller \(K\) is synthesized such that it:
< / p >
< ul class = "org-ul" >
< li > Keeps the \(\mathcal{H}_\infty\) norm \(G\) of the transfer function from \(w\) to \(z_\infty\) bellow some specified value< / li >
< li > Keeps the \(\mathcal{H}_2\) norm \(H\) of the transfer function from \(w\) to \(z_2\) bellow some specified value< / li >
< li > 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< / li >
< / ul >
2020-10-01 15:14:10 +02:00
< div id = "org4520d6a" class = "figure" >
2019-09-03 09:01:59 +02:00
< p > < img src = "figs-tikz/mixed_h2_hinf_synthesis.png" alt = "mixed_h2_hinf_synthesis.png" / >
< / p >
2020-10-01 15:14:10 +02:00
< p > < span class = "figure-number" > Figure 26: < / span > Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis< / p >
2019-09-03 09:01:59 +02:00
< / div >
< p >
Here, we define \(P\) such that:
< / p >
\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*}
< p >
Then:
< / p >
< ul class = "org-ul" >
< li > we specify the maximum value for the \(\mathcal{H}_\infty\) norm between \(w\) and \(z_\infty\) to be \(1\)< / li >
2020-09-30 08:47:27 +02:00
< li > we don’ t specify any maximum value for the \(\mathcal{H}_2\) norm between \(w\) and \(z_2\)< / li >
2019-09-03 09:01:59 +02:00
< li > 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\)< / li >
< / ul >
< p >
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.
< / p >
2019-08-30 14:39:01 +02:00
< p >
We define the generalized plant that will be used for the mixed synthesis.
< / p >
< div class = "org-src-container" >
2020-10-01 11:25:23 +02:00
< pre class = "src src-matlab" > W1u = ss(W2*Wu); W2u = ss(W1*Wu); % Weight on the uncertainty
W1n = ss(N2); W2n = ss(N1); % Weight on the noise
2020-09-30 08:47:27 +02:00
P = [W1u -W1u;
0 W2u;
W1n -W1n;
0 W2n;
1 0];
2019-08-30 14:39:01 +02:00
< / pre >
< / div >
2019-09-03 09:01:59 +02:00
< p >
2020-09-30 08:47:27 +02:00
The mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis is performed below.
2019-09-03 09:01:59 +02:00
< / p >
2019-08-30 14:39:01 +02:00
< div class = "org-src-container" >
2020-09-30 08:47:27 +02:00
< pre class = "src src-matlab" > Nmeas = 1; Ncon = 1; Nz2 = 2;
2019-08-30 14:39:01 +02:00
2020-10-01 11:25:23 +02:00
[H1, ~, normz, ~] = h2hinfsyn(P, Nmeas, Ncon, Nz2, [0, 1], 'HINFMAX', 1, 'H2MAX', Inf, 'DKMAX', 100, 'TOL', 0.01, 'DISPLAY', 'on');
2019-08-30 14:39:01 +02:00
2020-10-01 11:25:23 +02:00
H2 = 1 - H1;
2019-08-30 14:39:01 +02:00
< / pre >
< / div >
2019-09-03 09:01:59 +02:00
< p >
2020-10-01 15:14:10 +02:00
The obtained complementary filters are shown in Figure < a href = "#org352d1dd" > 27< / a > .
2019-09-03 09:01:59 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< div id = "org352d1dd" class = "figure" >
2019-09-03 09:01:59 +02:00
< p > < img src = "figs/comp_filters_mixed_synthesis.png" alt = "comp_filters_mixed_synthesis.png" / >
< / p >
2020-10-01 15:14:10 +02:00
< p > < span class = "figure-number" > Figure 27: < / span > Obtained complementary filters after mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis< / p >
2019-09-03 09:01:59 +02:00
< / div >
< / div >
< / div >
2020-10-01 15:14:10 +02:00
< div id = "outline-container-orgc600a0e" class = "outline-3" >
< h3 id = "orgc600a0e" > < span class = "section-number-3" > 5.5< / span > Obtained Super Sensor’ s noise< / h3 >
< div class = "outline-text-3" id = "text-5-5" >
2019-09-03 09:01:59 +02:00
< p >
2020-10-01 15:14:10 +02:00
The PSD and CPS of the super sensor’ s noise are shown in Figure < a href = "#orge5c443e" > 28< / a > and Figure < a href = "#org3e57c81" > 29< / a > respectively.
2019-09-03 09:01:59 +02:00
< / p >
2020-10-01 11:25:23 +02:00
< div class = "org-src-container" >
< pre class = "src 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);
< / pre >
< / div >
2019-09-03 09:01:59 +02:00
2020-10-01 15:14:10 +02:00
< div id = "orge5c443e" class = "figure" >
2019-09-03 09:01:59 +02:00
< p > < img src = "figs/psd_super_sensor_mixed_syn.png" alt = "psd_super_sensor_mixed_syn.png" / >
< / p >
2020-10-01 15:14:10 +02:00
< p > < span class = "figure-number" > Figure 28: < / span > Power Spectral Density of the Super Sensor obtained with the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis< / p >
2019-09-03 09:01:59 +02:00
< / div >
2020-10-01 15:14:10 +02:00
< div id = "org3e57c81" class = "figure" >
2019-09-03 09:01:59 +02:00
< p > < img src = "figs/cps_super_sensor_mixed_syn.png" alt = "cps_super_sensor_mixed_syn.png" / >
< / p >
2020-10-01 15:14:10 +02:00
< p > < span class = "figure-number" > Figure 29: < / span > Cumulative Power Spectrum of the Super Sensor obtained with the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis< / p >
2019-09-03 09:01:59 +02:00
< / div >
< / div >
< / div >
2020-10-01 15:14:10 +02:00
< div id = "outline-container-org14a7871" class = "outline-3" >
< h3 id = "org14a7871" > < span class = "section-number-3" > 5.6< / span > Obtained Super Sensor’ s Uncertainty< / h3 >
< div class = "outline-text-3" id = "text-5-6" >
2019-09-03 09:01:59 +02:00
< p >
2020-10-01 15:14:10 +02:00
The uncertainty on the super sensor’ s dynamics is shown in Figure
2019-09-03 09:01:59 +02:00
< / p >
2020-10-01 11:25:23 +02:00
< / div >
< / div >
2019-09-03 09:01:59 +02:00
2020-10-01 15:14:10 +02:00
< div id = "outline-container-org6170476" class = "outline-3" >
< h3 id = "org6170476" > < span class = "section-number-3" > 5.7< / span > Comparison Hinf H2 H2/Hinf< / h3 >
< div class = "outline-text-3" id = "text-5-7" >
2020-10-01 11:25:23 +02:00
< div class = "org-src-container" >
< pre class = "src 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');
< / pre >
< / div >
2019-09-03 09:01:59 +02:00
2020-10-01 11:25:23 +02:00
< div class = "org-src-container" >
< pre class = "src 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);
< / pre >
2019-09-03 09:01:59 +02:00
< / div >
2020-10-01 11:25:23 +02:00
< table border = "2" cellspacing = "0" cellpadding = "6" rules = "groups" frame = "hsides" >
< colgroup >
< col class = "org-left" / >
< col class = "org-right" / >
< / colgroup >
< thead >
< tr >
< th scope = "col" class = "org-left" >   < / th >
< th scope = "col" class = "org-right" > RMS [m/s]< / th >
< / tr >
< / thead >
< tbody >
< tr >
< td class = "org-left" > Optimal: \(\mathcal{H}_2\)< / td >
< td class = "org-right" > 0.0012< / td >
< / tr >
< tr >
< td class = "org-left" > Robust: \(\mathcal{H}_\infty\)< / td >
< td class = "org-right" > 0.041< / td >
< / tr >
< tr >
< td class = "org-left" > Mixed: \(\mathcal{H}_2/\mathcal{H}_\infty\)< / td >
< td class = "org-right" > 0.011< / td >
< / tr >
< / tbody >
< / table >
2019-09-03 09:01:59 +02:00
< / div >
< / div >
2020-10-01 15:14:10 +02:00
< div id = "outline-container-org99a64d6" class = "outline-3" >
< h3 id = "org99a64d6" > < span class = "section-number-3" > 5.8< / span > Conclusion< / h3 >
< div class = "outline-text-3" id = "text-5-8" >
2019-09-03 09:01:59 +02:00
< p >
This synthesis methods allows both to:
< / p >
< ul class = "org-ul" >
< li > limit the dynamical uncertainty of the super sensor< / li >
< li > minimize the RMS value of the estimation< / li >
< / ul >
< / div >
< / div >
< / div >
2020-10-01 15:14:10 +02:00
< div id = "outline-container-org6159413" class = "outline-2" >
< h2 id = "org6159413" > < span class = "section-number-2" > 6< / span > Matlab Functions< / h2 >
< div class = "outline-text-2" id = "text-6" >
< p >
< a id = "org2667713" > < / a >
< / p >
2020-10-01 11:25:23 +02:00
< / div >
2020-10-01 15:14:10 +02:00
< div id = "outline-container-org6ccaec5" class = "outline-3" >
< h3 id = "org6ccaec5" > < span class = "section-number-3" > 6.1< / span > < code > createWeight< / code > < / h3 >
< div class = "outline-text-3" id = "text-6-1" >
2020-10-01 11:25:23 +02:00
< p >
2020-10-01 15:14:10 +02:00
< a id = "org72a8d27" > < / a >
2020-10-01 11:25:23 +02:00
< / p >
< p >
This Matlab function is accessible < a href = "src/createWeight.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src 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
< / pre >
< / div >
< / div >
< / div >
2020-10-01 15:14:10 +02:00
< div id = "outline-container-org66fd774" class = "outline-3" >
< h3 id = "org66fd774" > < span class = "section-number-3" > 6.2< / span > < code > plotMagUncertainty< / code > < / h3 >
< div class = "outline-text-3" id = "text-6-2" >
2020-10-01 11:25:23 +02:00
< p >
2020-10-01 15:14:10 +02:00
< a id = "org7384a61" > < / a >
2020-10-01 11:25:23 +02:00
< / p >
< p >
This Matlab function is accessible < a href = "src/plotMagUncertainty.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src 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
2020-10-01 15:14:10 +02:00
args.opacity (1,1) double {mustBeNumeric, mustBeNonnegative} = 0.3
2020-10-01 11:25:23 +02:00
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
< / pre >
< / div >
< / div >
< / div >
2020-10-01 15:14:10 +02:00
< div id = "outline-container-orge99a206" class = "outline-3" >
< h3 id = "orge99a206" > < span class = "section-number-3" > 6.3< / span > < code > plotPhaseUncertainty< / code > < / h3 >
< div class = "outline-text-3" id = "text-6-3" >
2020-10-01 11:25:23 +02:00
< p >
2020-10-01 15:14:10 +02:00
< a id = "orgcc7dac5" > < / a >
2020-10-01 11:25:23 +02:00
< / p >
< p >
This Matlab function is accessible < a href = "src/plotPhaseUncertainty.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src 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
< / pre >
< / div >
< / div >
< / div >
< / div >
2019-09-11 17:56:44 +02:00
< p >
2020-10-01 15:14:10 +02:00
< a href = "ref.bib" > ref.bib< / a >
2019-09-11 17:56:44 +02:00
< / p >
2019-08-14 12:08:30 +02:00
< / div >
< div id = "postamble" class = "status" >
< p class = "author" > Author: Thomas Dehaeze< / p >
2020-10-01 15:14:10 +02:00
< p class = "date" > Created: 2020-10-01 jeu. 15:12< / p >
2019-08-14 12:08:30 +02:00
< / div >
< / body >
< / html >
