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-02 18:35:13 +02:00
<!-- 2020 - 10 - 02 ven. 18:35 -->
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" >
2020-10-02 17:59:57 +02:00
< a accesskey = "h" href = "./index.html" > UP < / a >
2019-08-14 12:08:30 +02:00
|
2020-10-02 17:59:57 +02:00
< a accesskey = "H" href = "./index.html" > HOME < / a >
2019-08-14 12:08:30 +02:00
< / 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-02 18:35:13 +02:00
< li > < a href = "#orgcc2172b" > 1. Sensor Description< / a >
2019-08-14 12:08:30 +02:00
< ul >
2020-10-02 18:35:13 +02:00
< li > < a href = "#org23e78da" > 1.1. Sensor Dynamics< / a > < / li >
< li > < a href = "#org94edbba" > 1.2. Sensor Model Uncertainty< / a > < / li >
< li > < a href = "#org4cc404c" > 1.3. Sensor Noise< / a > < / li >
< li > < a href = "#orgca9547c" > 1.4. Save Model< / a > < / li >
2019-08-14 12:08:30 +02:00
< / ul >
< / li >
2020-10-02 18:35:13 +02:00
< li > < a href = "#org8fb094e" > 2. Introduction to Sensor Fusion< / a >
2019-08-14 12:08:30 +02:00
< ul >
2020-10-02 18:35:13 +02:00
< li > < a href = "#org38e7087" > 2.1. Sensor Fusion Architecture< / a > < / li >
< li > < a href = "#org3ba53c4" > 2.2. Super Sensor Noise< / a > < / li >
< li > < a href = "#org0f97eb1" > 2.3. Super Sensor Dynamical Uncertainty< / a > < / li >
2019-08-29 22:47:20 +02:00
< / ul >
< / li >
2020-10-02 18:35:13 +02:00
< li > < a href = "#orgf2e7afd" > 3. Optimal Super Sensor Noise: \(\mathcal{H}_2\) Synthesis< / a >
2019-08-30 14:39:01 +02:00
< ul >
2020-10-02 18:35:13 +02:00
< li > < a href = "#org3f9017b" > 3.1. \(\mathcal{H}_2\) Synthesis< / a > < / li >
< li > < a href = "#orgb5bc159" > 3.2. Super Sensor Noise< / a > < / li >
< li > < a href = "#orge0269cd" > 3.3. Discrepancy between sensor dynamics and model< / a > < / li >
2020-10-01 11:25:23 +02:00
< / ul >
< / li >
2020-10-02 18:35:13 +02:00
< li > < a href = "#org869a9ab" > 4. Robust Sensor Fusion: \(\mathcal{H}_\infty\) Synthesis< / a >
2020-10-01 11:25:23 +02:00
< ul >
2020-10-02 18:35:13 +02:00
< li > < a href = "#orgf20f681" > 4.1. Weighting Function used to bound the super sensor uncertainty< / a > < / li >
< li > < a href = "#orge540fc3" > 4.2. \(\mathcal{H}_\infty\) Synthesis< / a > < / li >
< li > < a href = "#org6d2d24e" > 4.3. Super sensor uncertainty< / a > < / li >
< li > < a href = "#org8ce77ee" > 4.4. Super sensor noise< / a > < / li >
< li > < a href = "#org8a94903" > 4.5. Conclusion< / a > < / li >
2020-10-01 11:25:23 +02:00
< / ul >
< / li >
2020-10-02 18:35:13 +02:00
< li > < a href = "#org919d3da" > 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-02 18:35:13 +02:00
< li > < a href = "#org360cf70" > 5.1. Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis< / a > < / li >
< li > < a href = "#org880e8a9" > 5.2. Obtained Super Sensor’ s noise< / a > < / li >
< li > < a href = "#orgb149f25" > 5.3. Obtained Super Sensor’ s Uncertainty< / a > < / li >
< li > < a href = "#orgfaf058b" > 5.4. Conclusion< / a > < / li >
2020-10-01 15:14:10 +02:00
< / ul >
< / li >
2020-10-02 18:35:13 +02:00
< li > < a href = "#org2af7a78" > 6. Matlab Functions< / a >
2020-10-01 15:14:10 +02:00
< ul >
2020-10-02 18:35:13 +02:00
< li > < a href = "#orgfe9741d" > 6.1. < code > createWeight< / code > < / a > < / li >
< li > < a href = "#org6a88fe0" > 6.2. < code > plotMagUncertainty< / code > < / a > < / li >
< li > < a href = "#orgbff6dca" > 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 >
2020-10-02 18:35:13 +02:00
This document is arranged as follows:
2019-08-14 12:08:30 +02:00
< / p >
< ul class = "org-ul" >
2020-10-02 18:35:13 +02:00
< li > Section < a href = "#org2f5f470" > 1< / a > : the sensors are described (dynamics, uncertainty, noise)< / li >
< li > Section < a href = "#org220320f" > 2< / a > : the sensor fusion architecture is described and the super sensor noise and dynamical uncertainty are derived< / li >
< li > Section < a href = "#org0f63067" > 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 = "#org25efa7e" > 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 = "#org5163fb1" > 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 >
< li > Section < a href = "#org71367a9" > 6< / a > : Matlab functions used for the analysis are described< / li >
2019-08-14 12:08:30 +02:00
< / ul >
2020-10-02 18:35:13 +02:00
< div id = "outline-container-orgcc2172b" class = "outline-2" >
< h2 id = "orgcc2172b" > < 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-02 18:35:13 +02:00
< a id = "org2f5f470" > < / a >
2020-10-01 15:14:10 +02:00
< / p >
< p >
2020-10-02 18:35:13 +02:00
In Figure < a href = "#orgd19c6fc" > 1< / a > is shown a schematic of a sensor model that is used in the following study.
2020-10-01 15:14:10 +02:00
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-02 18:35:13 +02:00
< table id = "orgc326093" 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 = "#orgd19c6fc" > 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-02 18:35:13 +02:00
< table id = "orgf877ead" 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 = "#orgd19c6fc" > 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-02 18:35:13 +02:00
< div id = "orgd19c6fc" 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-02 18:35:13 +02:00
< div id = "outline-container-org23e78da" class = "outline-3" >
< h3 id = "org23e78da" > < 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-02 18:35:13 +02:00
< a id = "org4813033" > < / 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-02 18:35:13 +02:00
The true sensor dynamics has some uncertainty associated to it and described in section < a href = "#org992a983" > 1.2< / a > .
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 11:25:23 +02:00
< p >
2020-10-02 18:35:13 +02:00
Both sensor dynamics in \([\frac{V}{m/s}]\) are shown in Figure < a href = "#org6409089" > 2< / a > .
2020-10-01 11:25:23 +02:00
< / p >
2020-10-02 18:35:13 +02:00
< div id = "org6409089" 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-02 18:35:13 +02:00
< div id = "outline-container-org94edbba" class = "outline-3" >
< h3 id = "org94edbba" > < 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-02 18:35:13 +02:00
< a id = "org992a983" > < / a >
The uncertainty on the sensor dynamics is described by multiplicative uncertainty (Figure < a href = "#orgd19c6fc" > 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-02 18:35:13 +02:00
The weights \(W_i(s)\) representing the dynamical uncertainty are defined below and their magnitude is shown in Figure < a href = "#org8beb676" > 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-02 18:35:13 +02:00
The bode plot of the sensors nominal dynamics as well as their defined dynamical spread are shown in Figure < a href = "#org751f977" > 4< / a > .
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 11:25:23 +02:00
2020-10-02 18:35:13 +02:00
< div id = "org8beb676" 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-02 18:35:13 +02:00
< div id = "org751f977" 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-02 18:35:13 +02:00
< div id = "outline-container-org4cc404c" class = "outline-3" >
< h3 id = "org4cc404c" > < 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-02 18:35:13 +02:00
< a id = "org0cc1978" > < / 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 = "#orgd19c6fc" > 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-02 18:35:13 +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 = "#org2d39969" > 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-02 18:35:13 +02:00
< div id = "org2d39969" 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-02 18:35:13 +02:00
< div id = "outline-container-orgca9547c" class = "outline-3" >
< h3 id = "orgca9547c" > < 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-02 18:35:13 +02:00
< div id = "outline-container-org8fb094e" class = "outline-2" >
< h2 id = "org8fb094e" > < 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-02 18:35:13 +02:00
< a id = "org220320f" > < / a >
2020-10-01 11:25:23 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< / div >
2020-10-02 18:35:13 +02:00
< div id = "outline-container-org38e7087" class = "outline-3" >
< h3 id = "org38e7087" > < span class = "section-number-3" > 2.1< / span > Sensor Fusion Architecture< / h3 >
2020-10-01 15:14:10 +02:00
< div class = "outline-text-3" id = "text-2-1" >
2020-10-01 11:25:23 +02:00
< p >
2020-10-02 18:35:13 +02:00
< a id = "org82e93f4" > < / 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-02 18:35:13 +02:00
The two sensors presented in Section < a href = "#org2f5f470" > 1< / a > are now merged together using complementary filters \(H_1(s)\) and \(H_2(s)\) to form a super sensor (Figure < a href = "#org8c9eb16" > 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
2020-10-02 18:35:13 +02:00
< div id = "org8c9eb16" class = "figure" >
2020-10-01 15:14:10 +02:00
< 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-02 18:35:13 +02:00
< div id = "outline-container-org3ba53c4" class = "outline-3" >
< h3 id = "org3ba53c4" > < span class = "section-number-3" > 2.2< / span > Super Sensor Noise< / h3 >
2020-10-01 15:14:10 +02:00
< div class = "outline-text-3" id = "text-2-2" >
< p >
2020-10-02 18:35:13 +02:00
< a id = "orgd0b1104" > < / a >
2020-10-01 15:14:10 +02:00
< / 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 >
2020-10-02 18:35:13 +02:00
< div id = "outline-container-org0f97eb1" class = "outline-3" >
< h3 id = "org0f97eb1" > < span class = "section-number-3" > 2.3< / span > Super Sensor Dynamical Uncertainty< / h3 >
2020-10-01 15:14:10 +02:00
< div class = "outline-text-3" id = "text-2-3" >
< p >
2020-10-02 18:35:13 +02:00
< a id = "orgb59d475" > < / a >
2020-10-01 15:14:10 +02:00
< / p >
< p >
2020-10-02 18:35:13 +02:00
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 = "#orgb46c073" > 7< / a > ), the super sensor dynamics is then equals to:
2020-10-01 15:14:10 +02:00
< / 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}
2020-10-02 18:35:13 +02:00
< div id = "orgb46c073" class = "figure" >
2020-10-01 15:14:10 +02:00
< 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 >
2020-10-02 18:35:13 +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)|\) as shown in Figure < a href = "#orgfd5d4b3" > 8< / a > .
2020-10-01 15:14:10 +02:00
< / p >
2020-10-02 18:35:13 +02:00
< div id = "orgfd5d4b3" class = "figure" >
2020-10-01 15:14:10 +02:00
< 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 >
2020-10-02 18:35:13 +02:00
< div id = "outline-container-orgf2e7afd" class = "outline-2" >
< h2 id = "orgf2e7afd" > < span class = "section-number-2" > 3< / span > Optimal Super Sensor Noise: \(\mathcal{H}_2\) Synthesis< / h2 >
2020-10-01 15:14:10 +02:00
< div class = "outline-text-2" id = "text-3" >
< p >
2020-10-02 18:35:13 +02:00
< a id = "org0f63067" > < / a >
2020-10-01 15:14:10 +02:00
< / 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 >
2020-10-02 18:35:13 +02:00
< div id = "orge6fcebb" class = "figure" >
2020-10-01 15:14:10 +02:00
< 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).
2020-10-02 18:35:13 +02:00
This is done using the \(\mathcal{H}_2\) synthesis in Section < a href = "#orgc3c8ce2" > 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-02 18:35:13 +02:00
< div id = "outline-container-org3f9017b" class = "outline-3" >
< h3 id = "org3f9017b" > < span class = "section-number-3" > 3.1< / span > \(\mathcal{H}_2\) Synthesis< / h3 >
2020-10-01 15:14:10 +02:00
< div class = "outline-text-3" id = "text-3-1" >
2019-08-29 14:55:03 +02:00
< p >
2020-10-02 18:35:13 +02:00
< a id = "orgc3c8ce2" > < / a >
2019-08-29 14:55:03 +02:00
< / p >
< p >
2020-10-02 18:35:13 +02:00
Consider the generalized plant \(P_{\mathcal{H}_2}\) shown in Figure < a href = "#orgfed5b00" > 10< / a > and described by Equation \eqref{eq:H2_generalized_plant}.
2019-08-14 12:08:30 +02:00
< / p >
2020-10-02 18:35:13 +02:00
< div id = "orgfed5b00" class = "figure" >
2020-10-01 15:14:10 +02:00
< 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-02 18:35:13 +02:00
The obtained complementary filters are shown in Figure < a href = "#org3561716" > 11< / a > .
2019-08-29 14:55:03 +02:00
< / p >
2020-10-02 18:35:13 +02:00
< div id = "org3561716" 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-02 18:35:13 +02:00
< div id = "outline-container-orgb5bc159" class = "outline-3" >
< h3 id = "orgb5bc159" > < span class = "section-number-3" > 3.2< / span > Super Sensor Noise< / h3 >
2020-10-01 15:14:10 +02:00
< div class = "outline-text-3" id = "text-3-2" >
2019-08-29 14:55:03 +02:00
< p >
2020-10-02 18:35:13 +02:00
< a id = "org22dd161" > < / a >
2019-08-29 14:55:03 +02:00
< / p >
< p >
2020-10-02 18:35:13 +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 = "#orgad2796b" > 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-02 18:35:13 +02:00
The RMS value of the individual sensors and of the super sensor are listed in Table < a href = "#orgf505db2" > 3< / a > .
2019-08-14 12:08:30 +02:00
< / p >
2020-10-02 18:35:13 +02:00
< table id = "orgf505db2" border = "2" cellspacing = "0" cellpadding = "6" rules = "groups" frame = "hsides" >
2020-10-01 15:14:10 +02:00
< 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-02 17:59:57 +02:00
< td class = "org-right" > 0.080< / 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_{\mathcal{H}_2}}\)< / td >
2020-10-02 17:59:57 +02:00
< td class = "org-right" > 0.003< / 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-02 18:35:13 +02:00
< div id = "orgad2796b" class = "figure" >
2020-10-01 15:14:10 +02:00
< 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
< 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]\).
2020-10-02 18:35:13 +02:00
The velocity estimates from the two sensors and from the super sensors are shown in Figure < a href = "#org50620bf" > 13< / a > .
The resulting noises are displayed in Figure < a href = "#orga4919f6" > 14< / a > .
2019-08-14 12:08:30 +02:00
< / p >
2020-10-02 18:35:13 +02:00
< div id = "org50620bf" 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-02 18:25:52 +02:00
< p > < span class = "figure-number" > Figure 13: < / 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
2020-10-02 18:35:13 +02:00
< div id = "orga4919f6" class = "figure" >
2020-10-01 15:14:10 +02:00
< p > < img src = "figs/sensor_noise_H2_time_domain.png" alt = "sensor_noise_H2_time_domain.png" / >
< / p >
2020-10-02 18:25:52 +02:00
< p > < span class = "figure-number" > Figure 14: < / span > Noise of the two sensors \(n_1, n_2\) and noise of the super sensor \(n\)< / 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
2020-10-02 18:35:13 +02:00
< div id = "outline-container-orge0269cd" class = "outline-3" >
< h3 id = "orge0269cd" > < span class = "section-number-3" > 3.3< / span > Discrepancy between sensor dynamics and model< / h3 >
2020-10-01 15:14:10 +02:00
< div class = "outline-text-3" id = "text-3-3" >
< p >
2020-10-02 18:35:13 +02:00
If we consider sensor dynamical uncertainty as explained in Section < a href = "#org992a983" > 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.
2020-10-01 15:14:10 +02:00
< / p >
2019-08-14 12:08:30 +02:00
< p >
2020-10-02 18:35:13 +02:00
The super sensor dynamical uncertainty is shown in Figure < a href = "#org2fe77bd" > 15< / 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
2020-10-02 18:35:13 +02:00
< div id = "org2fe77bd" class = "figure" >
2020-10-01 15:14:10 +02:00
< p > < img src = "figs/super_sensor_dynamical_uncertainty_H2.png" alt = "super_sensor_dynamical_uncertainty_H2.png" / >
< / p >
2020-10-02 18:25:52 +02:00
< p > < span class = "figure-number" > Figure 15: < / span > Super sensor dynamical uncertainty when using the \(\mathcal{H}_2\) Synthesis< / p >
2019-08-30 14:39:01 +02:00
< / div >
< / div >
< / div >
2020-10-01 15:14:10 +02:00
< / div >
2020-10-02 18:35:13 +02:00
< div id = "outline-container-org869a9ab" class = "outline-2" >
< h2 id = "org869a9ab" > < span class = "section-number-2" > 4< / span > Robust Sensor Fusion: \(\mathcal{H}_\infty\) Synthesis< / h2 >
2020-10-01 15:14:10 +02:00
< div class = "outline-text-2" id = "text-4" >
2019-08-14 12:08:30 +02:00
< p >
2020-10-02 18:35:13 +02:00
< a id = "org25efa7e" > < / 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-02 18:35:13 +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 = "#org9fe9ab2" > 16< / a > .
2019-08-14 12:08:30 +02:00
< / p >
2020-10-02 18:35:13 +02:00
< div id = "org9fe9ab2" class = "figure" >
2020-10-01 15:14:10 +02:00
< 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-02 18:25:52 +02:00
< p > < span class = "figure-number" > Figure 16: < / span > Sensor fusion architecture with sensor dynamics uncertainty< / p >
2019-08-14 12:08:30 +02:00
< / div >
< p >
2020-10-02 18:35:13 +02:00
As explained in Section < a href = "#org992a983" > 1.2< / a > , at each frequency \(\omega\), the dynamical uncertainty of the super sensor can be represented in the complex plane by a circle with a radius equals to \(|H_1(j\omega) W_1(j\omega)| + |H_2(j\omega) W_2(j\omega)|\) and centered on 1.
2019-08-14 12:08:30 +02:00
< / p >
< p >
2020-10-02 17:59:57 +02:00
In order to specify a wanted upper bound on the dynamical uncertainty, a weight \(W_u(s)\) is used where \(1/|W_u(j\omega)|\) represents the maximum allowed radius of the uncertainty circle corresponding to the super sensor dynamics at a frequency \(\omega\) \eqref{eq:upper_bound_uncertainty}.
2019-08-14 12:08:30 +02:00
< / p >
2020-10-02 17:59:57 +02:00
\begin{align}
& |H_1(j\omega) W_1(j\omega)| + |H_2(j\omega) W_2(j\omega)| < \frac{1}{|W_u(j\omega)|}, \quad \forall \omega \label{eq:upper_bound_uncertainty} \\
\Leftrightarrow & |H_1(j\omega) W_1(j\omega) W_u(j\omega)| + |H_2(j\omega) W_2(j\omega) W_u(j\omega)| < 1 , \ quad \ forall \ omega \ label { eq:upper_bound_uncertainty_bis }
\end{align}
2019-08-14 12:08:30 +02:00
< p >
2020-10-02 17:59:57 +02:00
\(|W_u(j\omega)|\) is also linked to the gain uncertainty \(\Delta G\) \eqref{eq:gain_uncertainty_bound} and phase uncertainty \(\Delta\phi\) \eqref{eq:phase_uncertainty_bound} of the super sensor.
2019-08-14 12:08:30 +02:00
< / p >
2020-10-02 17:59:57 +02:00
\begin{align}
\Delta G (\omega) & \le \frac{1}{|W_u(j\omega)|}, \quad \forall\omega \label{eq:gain_uncertainty_bound} \\
\Delta \phi (\omega) & \le \arcsin\left(\frac{1}{|W_u(j\omega)|}\right), \quad \forall\omega \label{eq:phase_uncertainty_bound}
\end{align}
2019-08-14 12:08:30 +02:00
< p >
2020-10-02 18:35:13 +02:00
The choice of \(W_u\) is presented in Section < a href = "#org875b4b0" > 4.1< / a > .
2019-08-14 12:08:30 +02:00
< / p >
2019-08-29 22:47:20 +02:00
< p >
2020-10-02 17:59:57 +02:00
Condition \eqref{eq:upper_bound_uncertainty_bis} can almost be represented by \eqref{eq:hinf_norm_uncertainty} (within a factor \(\sqrt{2}\)).
2019-08-29 22:47:20 +02:00
< / p >
2020-10-02 17:59:57 +02:00
\begin{equation}
\left\| \begin{matrix} H_1(s) W_1(s) W_u(s) \\ H_2(s) W_2(s) W_u(s) \end{matrix} \right\|_\infty < 1 \ label { eq:hinf_norm_uncertainty }
\end{equation}
2019-08-29 22:47:20 +02:00
< p >
2020-10-02 17:59:57 +02:00
The objective is to design \(H_1(s)\) and \(H_2(s)\) such that \(H_1(s) + H_2(s) = 1\) (complementary property) and such that \eqref{eq:hinf_norm_uncertainty} is verified (bounded dynamical uncertainty).
2019-08-29 22:47:20 +02:00
< / p >
< p >
2020-10-02 18:35:13 +02:00
This is done using the \(\mathcal{H}_\infty\) synthesis in Section < a href = "#org2b41098" > 4.2< / a > .
2019-08-29 22:47:20 +02:00
< / p >
< / div >
2020-10-02 18:35:13 +02:00
< div id = "outline-container-orgf20f681" class = "outline-3" >
< h3 id = "orgf20f681" > < span class = "section-number-3" > 4.1< / span > Weighting Function used to bound the super sensor uncertainty< / h3 >
2020-10-02 17:59:57 +02:00
< div class = "outline-text-3" id = "text-4-1" >
2019-08-29 22:47:20 +02:00
< p >
2020-10-02 18:35:13 +02:00
< a id = "org875b4b0" > < / a >
2019-08-29 22:47:20 +02:00
< / p >
< p >
2020-10-02 17:59:57 +02:00
\(W_u(s)\) is defined such that the super sensor phase uncertainty is less than 10 degrees below 100Hz \eqref{eq:phase_uncertainy_bound_low_freq} and is less than 180 degrees below 400Hz \eqref{eq:phase_uncertainty_max}.
2019-08-29 22:47:20 +02:00
< / p >
2020-10-02 17:59:57 +02:00
\begin{align}
\frac{1}{|W_u(j\omega)|} & < \sin\left(10 \frac{\pi}{180}\right), \quad \omega < 100 \ , \ text { Hz } \ label { eq:phase_uncertainy_bound_low_freq } \ \
\frac{1}{|W_u(j 2 \pi 400)|} & < 1 \ label { eq:phase_uncertainty_max }
\end{align}
2019-08-29 22:47:20 +02:00
< p >
2020-10-02 18:35:13 +02:00
The uncertainty bounds of the two individual sensor as well as the wanted maximum uncertainty bounds of the super sensor are shown in Figure < a href = "#orgffb63ca" > 17< / a > .
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-02 18:35:13 +02:00
< div id = "orgffb63ca" class = "figure" >
2020-10-02 17:59:57 +02:00
< p > < img src = "figs/weight_uncertainty_bounds_Wu.png" alt = "weight_uncertainty_bounds_Wu.png" / >
2019-08-29 22:47:20 +02:00
< / p >
2020-10-02 18:25:52 +02:00
< p > < span class = "figure-number" > Figure 17: < / span > Uncertainty region of the two sensors as well as the wanted maximum uncertainty of the super sensor (dashed lines)< / 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-02 18:35:13 +02:00
< div id = "outline-container-orge540fc3" class = "outline-3" >
< h3 id = "orge540fc3" > < span class = "section-number-3" > 4.2< / span > \(\mathcal{H}_\infty\) Synthesis< / h3 >
2020-10-02 17:59:57 +02:00
< div class = "outline-text-3" id = "text-4-2" >
2019-08-29 22:47:20 +02:00
< p >
2020-10-02 18:35:13 +02:00
< a id = "org2b41098" > < / a >
2019-08-30 14:39:01 +02:00
< / p >
2020-10-02 17:59:57 +02:00
< p >
2020-10-02 18:35:13 +02:00
The generalized plant \(P_{\mathcal{H}_\infty}\) used for the \(\mathcal{H}_\infty\) Synthesis of the complementary filters is shown in Figure < a href = "#orgf4ccff4" > 18< / a > and is described by Equation \eqref{eq:Hinf_generalized_plant}.
2020-10-02 17:59:57 +02:00
< / p >
2019-08-30 14:39:01 +02:00
2020-10-02 17:59:57 +02:00
2020-10-02 18:35:13 +02:00
< div id = "orgf4ccff4" 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-02 18:25:52 +02:00
< p > < span class = "figure-number" > Figure 18: < / 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
2020-10-02 17:59:57 +02:00
\begin{equation} \label{eq:Hinf_generalized_plant}
\begin{pmatrix}
z_1 \\ z_2 \\ v
\end{pmatrix} = \underbrace{\begin{bmatrix}
W_u W_1 & -W_u W_1 \\
0 & W_u W_2 \\
1 & 0
\end{bmatrix}}_{P_{\mathcal{H}_\infty}} \begin{pmatrix}
w \\ u
\end{pmatrix}
\end{equation}
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 >
2020-10-02 17:59:57 +02:00
And the \(\mathcal{H}_\infty\) synthesis is performed using the < code > hinfsyn< / code > command.
2019-08-29 22:47:20 +02:00
< / p >
< div class = "org-src-container" >
2020-10-02 17:59:57 +02:00
< pre class = "src src-matlab" > H2 = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'DISPLAY', 'on');
2019-08-29 22:47:20 +02:00
< / pre >
< / div >
< pre class = "example" >
2020-10-02 17:59:57 +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 >
2020-10-02 17:59:57 +02:00
The \(\mathcal{H}_\infty\) is successful as the \(\mathcal{H}_\infty\) norm of the “ closed loop” transfer function from \((w)\) to \((z_1,\ z_2)\) is less than one.
< / p >
< p >
\(H_1(s)\) is then defined as the complementary of \(H_2(s)\).
2019-08-30 14:39:01 +02:00
< / 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-02 18:35:13 +02:00
The obtained complementary filters as well as the wanted upper bounds are shown in Figure < a href = "#org7a55110" > 19< / a > .
2019-08-30 14:39:01 +02:00
< / p >
2019-08-29 22:47:20 +02:00
2020-10-02 17:59:57 +02:00
2020-10-02 18:35:13 +02:00
< div id = "org7a55110" class = "figure" >
2020-10-02 17:59:57 +02:00
< p > < img src = "figs/hinf_comp_filters.png" alt = "hinf_comp_filters.png" / >
2019-08-29 22:47:20 +02:00
< / p >
2020-10-02 18:25:52 +02:00
< p > < span class = "figure-number" > Figure 19: < / span > Obtained complementary filters using the \(\mathcal{H}_\infty\) Synthesis< / 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-02 18:35:13 +02:00
< div id = "outline-container-org6d2d24e" class = "outline-3" >
< h3 id = "org6d2d24e" > < span class = "section-number-3" > 4.3< / span > Super sensor uncertainty< / h3 >
2020-10-02 17:59:57 +02:00
< div class = "outline-text-3" id = "text-4-3" >
2019-08-29 22:47:20 +02:00
< p >
2020-10-02 18:35:13 +02:00
The super sensor dynamical uncertainty is displayed in Figure < a href = "#org8ded02d" > 20< / a > .
2020-10-02 17:59:57 +02:00
It is confirmed that the super sensor dynamical uncertainty is less than the maximum allowed uncertainty defined by the norm of \(W_u(s)\).
2019-08-30 14:39:01 +02:00
< / p >
< p >
2020-10-02 18:25:52 +02:00
The \(\mathcal{H}_\infty\) synthesis thus allows to design filters such that the super sensor has specified bounded uncertainty.
2020-10-02 17:59:57 +02:00
< / p >
2020-10-02 18:35:13 +02:00
< div id = "org8ded02d" class = "figure" >
2020-10-02 17:59:57 +02:00
< p > < img src = "figs/super_sensor_dynamical_uncertainty_Hinf.png" alt = "super_sensor_dynamical_uncertainty_Hinf.png" / >
2019-08-29 22:47:20 +02:00
< / p >
2020-10-02 18:25:52 +02:00
< p > < span class = "figure-number" > Figure 20: < / span > Super sensor dynamical uncertainty (solid curve) when using the \(\mathcal{H}_\infty\) Synthesis< / p >
2020-10-02 17:59:57 +02:00
< / div >
2019-08-29 22:47:20 +02:00
< / div >
< / div >
2019-08-30 14:39:01 +02:00
2020-10-02 18:35:13 +02:00
< div id = "outline-container-org8ce77ee" class = "outline-3" >
< h3 id = "org8ce77ee" > < span class = "section-number-3" > 4.4< / span > Super sensor noise< / h3 >
2020-10-02 17:59:57 +02:00
< div class = "outline-text-3" id = "text-4-4" >
2019-08-30 14:39:01 +02:00
< p >
2020-10-02 18:35:13 +02:00
We now compute the obtain Power Spectral Density of the super sensor’ s noise (Figure < a href = "#org3ff8b63" > 21< / a > ).
2019-08-30 14:39:01 +02:00
< / p >
2020-10-01 11:25:23 +02:00
< p >
2020-10-02 18:35:13 +02:00
The obtained RMS of the super sensor noise in the \(\mathcal{H}_2\) and \(\mathcal{H}_\infty\) case are shown in Table < a href = "#org2b06274" > 4< / a > .
2020-10-02 17:59:57 +02:00
As expected, the super sensor obtained from the \(\mathcal{H}_\infty\) synthesis is much noisier than the super sensor obtained from the \(\mathcal{H}_2\) synthesis.
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" >
2020-10-02 17:59:57 +02:00
< 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;
2019-08-30 14:39:01 +02:00
< / pre >
< / div >
2020-10-02 18:35:13 +02:00
< div id = "org3ff8b63" 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-02 18:25:52 +02:00
< p > < span class = "figure-number" > Figure 21: < / span > Power Spectral Density of the estimated \(\hat{x}\) using the two sensors alone and using the< / p >
2019-08-30 14:39:01 +02:00
< / div >
2020-10-02 18:35:13 +02:00
< table id = "org2b06274" border = "2" cellspacing = "0" cellpadding = "6" rules = "groups" frame = "hsides" >
2020-10-02 17:59:57 +02:00
< caption class = "t-above" > < span class = "table-number" > Table 4:< / span > Comparison of the obtained RMS noise of the super sensor< / caption >
2019-08-30 14:39:01 +02:00
2020-10-02 17:59:57 +02:00
< 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.0027< / td >
< / tr >
< tr >
< td class = "org-left" > Robust: \(\mathcal{H}_\infty\)< / td >
< td class = "org-right" > 0.041< / td >
< / tr >
< / tbody >
< / table >
2019-08-29 22:47:20 +02:00
< / div >
< / div >
2020-10-02 18:35:13 +02:00
< div id = "outline-container-org8a94903" class = "outline-3" >
< h3 id = "org8a94903" > < span class = "section-number-3" > 4.5< / span > Conclusion< / h3 >
2020-10-02 17:59:57 +02:00
< div class = "outline-text-3" id = "text-4-5" >
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-02 18:35:13 +02:00
< div id = "outline-container-org919d3da" class = "outline-2" >
< h2 id = "org919d3da" > < span class = "section-number-2" > 5< / span > Optimal and Robust Sensor Fusion: Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis< / h2 >
2020-10-01 15:14:10 +02:00
< div class = "outline-text-2" id = "text-5" >
2019-09-03 09:01:59 +02:00
< p >
2020-10-02 18:35:13 +02:00
< a id = "org5163fb1" > < / a >
2020-10-02 18:25:52 +02:00
< / p >
< p >
The (optima) \(\mathcal{H}_2\) synthesis and the (robust) \(\mathcal{H}_\infty\) synthesis are now combined to form an Optimal and Robust synthesis of complementary filters for sensor fusion.
2019-09-03 09:01:59 +02:00
< / p >
2020-10-02 18:25:52 +02:00
< p >
2020-10-02 18:35:13 +02:00
The sensor fusion architecture is shown in Figure < a href = "#org427d030" > 22< / a > (\(\hat{G}_i\) are omitted for space reasons).
2020-10-02 18:25:52 +02:00
< / p >
2020-10-02 18:35:13 +02:00
< div id = "org427d030" class = "figure" >
2020-10-01 15:14:10 +02:00
< p > < img src = "figs-tikz/sensor_fusion_arch_full.png" alt = "sensor_fusion_arch_full.png" / >
< / p >
2020-10-02 18:25:52 +02:00
< p > < span class = "figure-number" > Figure 22: < / span > Sensor fusion architecture with sensor dynamics uncertainty< / p >
2019-09-03 09:01:59 +02:00
< / div >
2020-10-02 17:59:57 +02:00
2019-08-30 14:39:01 +02:00
< p >
The goal is to design complementary filters such that:
< / p >
< ul class = "org-ul" >
2020-10-02 18:25:52 +02:00
< li > the maximum uncertainty of the super sensor is bounded to acceptable values (defined by \(W_u(s)\))< / li >
2019-08-30 14:39:01 +02:00
< li > the RMS value of the super sensor noise is minimized< / li >
< / ul >
< p >
2020-10-02 18:35:13 +02:00
To do so, we can use the Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis presented in Section < a href = "#orgdf4b501" > 5.1< / a > .
2019-08-30 14:39:01 +02:00
< / p >
< / div >
2020-10-02 18:35:13 +02:00
< div id = "outline-container-org360cf70" class = "outline-3" >
< h3 id = "org360cf70" > < span class = "section-number-3" > 5.1< / span > Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis< / h3 >
2020-10-02 17:59:57 +02:00
< div class = "outline-text-3" id = "text-5-1" >
2019-09-03 09:01:59 +02:00
< p >
2020-10-02 18:35:13 +02:00
< a id = "orgdf4b501" > < / a >
2020-10-02 18:25:52 +02:00
< / p >
< p >
2020-10-02 18:35:13 +02:00
The synthesis architecture that is used here is shown in Figure < a href = "#org9fe5761" > 23< / a > .
2019-09-03 09:01:59 +02:00
< / p >
< p >
2020-10-02 18:25:52 +02:00
The filter \(H_2(s)\) is synthesized such that it:
2019-09-03 09:01:59 +02:00
< / p >
< ul class = "org-ul" >
2020-10-02 18:25:52 +02:00
< li > keeps the \(\mathcal{H}_\infty\) norm of the transfer function from \(w\) to \(z_{\mathcal{H}_\infty}\) bellow some specified value< / li >
< li > minimizes the \(\mathcal{H}_2\) norm of the transfer function from \(w\) to \(z_{\mathcal{H}_2}\)< / li >
2019-09-03 09:01:59 +02:00
< / ul >
2020-10-02 18:35:13 +02:00
< div id = "org9fe5761" 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-02 18:25:52 +02:00
< p > < span class = "figure-number" > Figure 23: < / span > Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis< / p >
2019-09-03 09:01:59 +02:00
< / div >
< p >
2020-10-02 18:25:52 +02:00
Let’ s see that
with \(H_1(s)= 1 - H_2(s)\)
2019-09-03 09:01:59 +02:00
< / p >
2020-10-02 18:25:52 +02:00
\begin{align}
\left\| \frac{z_\infty}{w} \right\|_\infty & = \left\| \begin{matrix}H_1(s) W_1(s) W_u(s)\\ H_2(s) W_2(s) W_u(s)\end{matrix} \right\|_\infty \\
\left\| \frac{z_2}{w} \right\|_2 & = \left\| \begin{matrix}H_1(s) N_1(s) \\ H_2(s) N_2(s)\end{matrix} \right\|_2 = \sigma_n
\end{align}
2019-09-03 09:01:59 +02:00
2019-08-30 14:39:01 +02:00
< p >
2020-10-02 18:25:52 +02:00
The generalized plant \(P_{\mathcal{H}_2/\mathcal{H}_\infty}\) is defined below
2019-08-30 14:39:01 +02:00
< / 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
2020-10-02 18:25:52 +02:00
P = [Wu*W1 -Wu*W1;
0 Wu*W2;
N1 -N1;
0 N2;
1 0];
2019-08-30 14:39:01 +02:00
< / pre >
< / div >
2019-09-03 09:01:59 +02:00
< p >
2020-10-02 18:25:52 +02:00
And the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis is performed.
2019-09-03 09:01:59 +02:00
< / p >
2019-08-30 14:39:01 +02:00
< div class = "org-src-container" >
2020-10-02 18:35:13 +02:00
< pre class = "src src-matlab" > [H2, ~] = h2hinfsyn(ss(P), 1, 1, 2, [0, 1], 'HINFMAX', 1, 'H2MAX', Inf, 'DKMAX', 100, 'TOL', 1e-3, 'DISPLAY', 'on');
2020-10-02 18:25:52 +02:00
< / pre >
< / div >
2019-08-30 14:39:01 +02:00
2020-10-02 18:25:52 +02:00
< div class = "org-src-container" >
< pre class = "src src-matlab" > H1 = 1 - H2;
2019-08-30 14:39:01 +02:00
< / pre >
< / div >
2019-09-03 09:01:59 +02:00
< p >
2020-10-02 18:35:13 +02:00
The obtained complementary filters are shown in Figure < a href = "#org9dd03be" > 24< / a > .
2019-09-03 09:01:59 +02:00
< / p >
2020-10-02 18:35:13 +02:00
< div id = "org9dd03be" class = "figure" >
2020-10-02 17:59:57 +02:00
< p > < img src = "figs/htwo_hinf_comp_filters.png" alt = "htwo_hinf_comp_filters.png" / >
2019-09-03 09:01:59 +02:00
< / p >
2020-10-02 18:25:52 +02:00
< p > < span class = "figure-number" > Figure 24: < / 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-02 18:35:13 +02:00
< div id = "outline-container-org880e8a9" class = "outline-3" >
< h3 id = "org880e8a9" > < span class = "section-number-3" > 5.2< / span > Obtained Super Sensor’ s noise< / h3 >
2020-10-02 17:59:57 +02:00
< div class = "outline-text-3" id = "text-5-2" >
2019-09-03 09:01:59 +02:00
< p >
2020-10-02 18:35:13 +02:00
The Power Spectral Density of the super sensor’ s noise is shown in Figure < a href = "#org41f2620" > 25< / a > .
2019-09-03 09:01:59 +02:00
< / p >
2020-10-02 18:25:52 +02:00
< p >
2020-10-02 18:35:13 +02:00
A time domain simulation is shown in Figure < a href = "#orgd85ef6a" > 26< / a > .
2019-09-03 09:01:59 +02:00
< / p >
< p >
2020-10-02 18:35:13 +02:00
The RMS values of the super sensor noise for the presented three synthesis are listed in Table < a href = "#org3b609de" > 5< / a > .
2019-09-03 09:01:59 +02:00
< / p >
2020-10-01 11:25:23 +02:00
< div class = "org-src-container" >
2020-10-02 18:25:52 +02:00
< 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;
2020-10-01 11:25:23 +02:00
< / pre >
< / div >
2019-09-03 09:01:59 +02:00
2020-10-01 11:25:23 +02:00
2020-10-02 18:35:13 +02:00
< div id = "org41f2620" class = "figure" >
2020-10-02 18:25:52 +02:00
< p > < img src = "figs/psd_sensors_htwo_hinf_synthesis.png" alt = "psd_sensors_htwo_hinf_synthesis.png" / >
< / p >
< p > < span class = "figure-number" > Figure 25: < / span > Power Spectral Density of the Super Sensor obtained with the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis< / p >
< / div >
2020-10-01 11:25:23 +02:00
2020-10-02 18:35:13 +02:00
< div id = "orgd85ef6a" class = "figure" >
2020-10-02 18:25:52 +02:00
< p > < img src = "figs/super_sensor_time_domain_h2_hinf.png" alt = "super_sensor_time_domain_h2_hinf.png" / >
< / p >
< p > < span class = "figure-number" > Figure 26: < / span > Noise of individual sensors and noise of the super sensor< / p >
2019-09-03 09:01:59 +02:00
< / div >
2020-10-01 11:25:23 +02:00
2020-10-02 18:35:13 +02:00
< table id = "org3b609de" border = "2" cellspacing = "0" cellpadding = "6" rules = "groups" frame = "hsides" >
2020-10-02 17:59:57 +02:00
< caption class = "t-above" > < span class = "table-number" > Table 5:< / span > Comparison of the obtained RMS noise of the super sensor< / 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 >
< th scope = "col" class = "org-right" > RMS [m/s]< / th >
< / tr >
< / thead >
< tbody >
< tr >
< td class = "org-left" > Optimal: \(\mathcal{H}_2\)< / td >
2020-10-02 18:25:52 +02:00
< td class = "org-right" > 0.0027< / td >
2020-10-01 11:25:23 +02:00
< / 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 >
2020-10-02 18:35:13 +02:00
< td class = "org-right" > 0.0098< / td >
2020-10-01 11:25:23 +02:00
< / tr >
< / tbody >
< / table >
2019-09-03 09:01:59 +02:00
< / div >
< / div >
2020-10-02 18:35:13 +02:00
< div id = "outline-container-orgb149f25" class = "outline-3" >
< h3 id = "orgb149f25" > < span class = "section-number-3" > 5.3< / span > Obtained Super Sensor’ s Uncertainty< / h3 >
2020-10-02 18:25:52 +02:00
< div class = "outline-text-3" id = "text-5-3" >
< p >
2020-10-02 18:35:13 +02:00
The uncertainty on the super sensor’ s dynamics is shown in Figure < a href = "#org37d0e3f" > 27< / a > .
2020-10-02 18:25:52 +02:00
< / p >
2020-10-02 18:35:13 +02:00
< div id = "org37d0e3f" class = "figure" >
2020-10-02 18:25:52 +02:00
< p > < img src = "figs/super_sensor_dynamical_uncertainty_Htwo_Hinf.png" alt = "super_sensor_dynamical_uncertainty_Htwo_Hinf.png" / >
< / p >
< p > < span class = "figure-number" > Figure 27: < / span > Super sensor dynamical uncertainty (solid curve) when using the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis< / p >
< / div >
< / div >
< / div >
2020-10-02 18:35:13 +02:00
< div id = "outline-container-orgfaf058b" class = "outline-3" >
< h3 id = "orgfaf058b" > < span class = "section-number-3" > 5.4< / span > Conclusion< / h3 >
2020-10-02 18:25:52 +02:00
< div class = "outline-text-3" id = "text-5-4" >
2019-09-03 09:01:59 +02:00
< p >
2020-10-02 18:25:52 +02:00
The mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis of the complementary filters allows to:
2019-09-03 09:01:59 +02:00
< / 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-02 18:35:13 +02:00
< div id = "outline-container-org2af7a78" class = "outline-2" >
< h2 id = "org2af7a78" > < span class = "section-number-2" > 6< / span > Matlab Functions< / h2 >
2020-10-01 15:14:10 +02:00
< div class = "outline-text-2" id = "text-6" >
< p >
2020-10-02 18:35:13 +02:00
< a id = "org71367a9" > < / a >
2020-10-01 15:14:10 +02:00
< / p >
2020-10-01 11:25:23 +02:00
< / div >
2020-10-02 18:35:13 +02:00
< div id = "outline-container-orgfe9741d" class = "outline-3" >
< h3 id = "orgfe9741d" > < span class = "section-number-3" > 6.1< / span > < code > createWeight< / code > < / h3 >
2020-10-01 15:14:10 +02:00
< div class = "outline-text-3" id = "text-6-1" >
2020-10-01 11:25:23 +02:00
< p >
2020-10-02 18:35:13 +02:00
< a id = "org38a3120" > < / 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-02 18:35:13 +02:00
< div id = "outline-container-org6a88fe0" class = "outline-3" >
< h3 id = "org6a88fe0" > < span class = "section-number-3" > 6.2< / span > < code > plotMagUncertainty< / code > < / h3 >
2020-10-01 15:14:10 +02:00
< div class = "outline-text-3" id = "text-6-2" >
2020-10-01 11:25:23 +02:00
< p >
2020-10-02 18:35:13 +02:00
< a id = "orgd3213c3" > < / 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-02 18:35:13 +02:00
< div id = "outline-container-orgbff6dca" class = "outline-3" >
< h3 id = "orgbff6dca" > < span class = "section-number-3" > 6.3< / span > < code > plotPhaseUncertainty< / code > < / h3 >
2020-10-01 15:14:10 +02:00
< div class = "outline-text-3" id = "text-6-3" >
2020-10-01 11:25:23 +02:00
< p >
2020-10-02 18:35:13 +02:00
< a id = "org5f660cc" > < / 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-02 18:35:13 +02:00
< p class = "date" > Created: 2020-10-02 ven. 18:35< / p >
2019-08-14 12:08:30 +02:00
< / div >
< / body >
< / html >
