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-05 15:41:57 +02:00
<!-- 2020 - 10 - 05 lun. 15:08 -->
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-05 15:41:57 +02:00
< li > < a href = "#org0a492b7" > 1. Sensor Description< / a >
2019-08-14 12:08:30 +02:00
< ul >
2020-10-05 15:41:57 +02:00
< li > < a href = "#org9575585" > 1.1. Sensor Dynamics< / a > < / li >
< li > < a href = "#orgec8c81d" > 1.2. Sensor Model Uncertainty< / a > < / li >
< li > < a href = "#org81d9a34" > 1.3. Sensor Noise< / a > < / li >
< li > < a href = "#org4e23f6c" > 1.4. Save Model< / a > < / li >
2019-08-14 12:08:30 +02:00
< / ul >
< / li >
2020-10-05 15:41:57 +02:00
< li > < a href = "#org2cab1a2" > 2. Introduction to Sensor Fusion< / a >
2019-08-14 12:08:30 +02:00
< ul >
2020-10-05 15:41:57 +02:00
< li > < a href = "#org0cbc92d" > 2.1. Sensor Fusion Architecture< / a > < / li >
< li > < a href = "#orge9e0bd4" > 2.2. Super Sensor Noise< / a > < / li >
< li > < a href = "#orgefb4347" > 2.3. Super Sensor Dynamical Uncertainty< / a > < / li >
2019-08-29 22:47:20 +02:00
< / ul >
< / li >
2020-10-05 15:41:57 +02:00
< li > < a href = "#org5896b60" > 3. Optimal Super Sensor Noise: \(\mathcal{H}_2\) Synthesis< / a >
2019-08-30 14:39:01 +02:00
< ul >
2020-10-05 15:41:57 +02:00
< li > < a href = "#orgf5a8a84" > 3.1. \(\mathcal{H}_2\) Synthesis< / a > < / li >
< li > < a href = "#org72159df" > 3.2. Super Sensor Noise< / a > < / li >
< li > < a href = "#org8ffba19" > 3.3. Discrepancy between sensor dynamics and model< / a > < / li >
2020-10-01 11:25:23 +02:00
< / ul >
< / li >
2020-10-05 15:41:57 +02:00
< li > < a href = "#org26ea7b1" > 4. Robust Sensor Fusion: \(\mathcal{H}_\infty\) Synthesis< / a >
2020-10-01 11:25:23 +02:00
< ul >
2020-10-05 15:41:57 +02:00
< li > < a href = "#org8dfd9d2" > 4.1. Weighting Function used to bound the super sensor uncertainty< / a > < / li >
< li > < a href = "#org7422ade" > 4.2. \(\mathcal{H}_\infty\) Synthesis< / a > < / li >
< li > < a href = "#orga0267c6" > 4.3. Super sensor uncertainty< / a > < / li >
< li > < a href = "#org979fede" > 4.4. Super sensor noise< / a > < / li >
< li > < a href = "#orgda33992" > 4.5. Conclusion< / a > < / li >
2020-10-01 11:25:23 +02:00
< / ul >
< / li >
2020-10-05 15:41:57 +02:00
< li > < a href = "#org15afe90" > 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-05 15:41:57 +02:00
< li > < a href = "#org0f81a91" > 5.1. Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis< / a > < / li >
< li > < a href = "#org417aabd" > 5.2. Obtained Super Sensor’ s noise< / a > < / li >
< li > < a href = "#org2dce888" > 5.3. Obtained Super Sensor’ s Uncertainty< / a > < / li >
< li > < a href = "#org47da78c" > 5.4. Conclusion< / a > < / li >
2020-10-01 15:14:10 +02:00
< / ul >
< / li >
2020-10-05 15:41:57 +02:00
< li > < a href = "#org0afe5ef" > 6. Matlab Functions< / a >
2020-10-01 15:14:10 +02:00
< ul >
2020-10-05 15:41:57 +02:00
< li > < a href = "#orge81e522" > 6.1. < code > createWeight< / code > < / a > < / li >
< li > < a href = "#org37ec2b4" > 6.2. < code > plotMagUncertainty< / code > < / a > < / li >
< li > < a href = "#org9f73572" > 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-05 15:41:57 +02:00
< li > Section < a href = "#orgee25d07" > 1< / a > : the sensors are described (dynamics, uncertainty, noise)< / li >
< li > Section < a href = "#orga64daad" > 2< / a > : the sensor fusion architecture is described and the super sensor noise and dynamical uncertainty are derived< / li >
< li > Section < a href = "#orgdd6b9ce" > 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 = "#org5d93f37" > 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 = "#org9f98c16" > 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 = "#orgf41dc8d" > 6< / a > : Matlab functions used for the analysis are described< / li >
2019-08-14 12:08:30 +02:00
< / ul >
2020-10-05 15:41:57 +02:00
< div id = "outline-container-org0a492b7" class = "outline-2" >
< h2 id = "org0a492b7" > < 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-05 15:41:57 +02:00
< a id = "orgee25d07" > < / a >
2020-10-01 15:14:10 +02:00
< / p >
< p >
2020-10-05 15:41:57 +02:00
In Figure < a href = "#org35e2340" > 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-05 15:41:57 +02:00
< table id = "org2fe6194" 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 = "#org35e2340" > 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 >
2020-10-05 11:46:14 +02:00
< tr >
< td class = "org-left" > \(\Phi_n(\omega)\)< / td >
< td class = "org-left" > Power Spectral Density of \(n\)< / td >
< td class = "org-left" > \([\frac{(m/s)^2}{Hz}]\)< / td >
< / tr >
< tr >
< td class = "org-left" > \(\phi_n(\omega)\)< / td >
< td class = "org-left" > Amplitude Spectral Density of \(n\)< / td >
< td class = "org-left" > \([\frac{m/s}{\sqrt{Hz}}]\)< / td >
< / tr >
< tr >
< td class = "org-left" > \(\sigma_n\)< / td >
< td class = "org-left" > Root Mean Square Value of \(n\)< / td >
< td class = "org-left" > \([m/s\ rms]\)< / td >
< / tr >
2020-10-01 11:25:23 +02:00
< / tbody >
< / table >
2020-10-05 15:41:57 +02:00
< table id = "org281ecb3" 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 = "#org35e2340" > 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-05 15:41:57 +02:00
< div id = "org35e2340" class = "figure" >
< p > < img src = "figs-paper/sensor_model_noise_uncertainty.png" alt = "sensor_model_noise_uncertainty.png" / >
2020-10-01 11:25:23 +02:00
< / p >
< p > < span class = "figure-number" > Figure 1: < / span > Sensor Model< / p >
< / div >
< / div >
2020-10-05 15:41:57 +02:00
< div id = "outline-container-org9575585" class = "outline-3" >
< h3 id = "org9575585" > < 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-05 15:41:57 +02:00
< a id = "org52415aa" > < / 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" >
2020-10-05 11:46:14 +02:00
< pre class = "src src-matlab" > m_acc = 0.01; < span class = "org-comment" > % Inertial Mass [kg]< / span >
c_acc = 5; < span class = "org-comment" > % Damping [N/(m/s)]< / span >
k_acc = 1e5; < span class = "org-comment" > % Stiffness [N/m]< / span >
g_acc = 1e5; < span class = "org-comment" > % Gain [V/m]< / span >
2020-10-01 11:25:23 +02:00
2020-10-05 11:46:14 +02:00
G1 = g_acc< span class = "org-type" > *< / span > m_acc< span class = "org-type" > *< / span > s< span class = "org-type" > /< / span > (m_acc< span class = "org-type" > *< / span > s< span class = "org-type" > ^< / span > 2 < span class = "org-type" > +< / span > c_acc< span class = "org-type" > *< / span > s < span class = "org-type" > +< / span > k_acc); < span class = "org-comment" > % Accelerometer Plant [V/(m/s)]< / span >
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" >
2020-10-05 11:46:14 +02:00
< pre class = "src src-matlab" > w_pos = 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 2e3; < span class = "org-comment" > % Measurement Banwdith [rad/s]< / span >
g_pos = 1e4; < span class = "org-comment" > % Gain [V/m]< / span >
2020-10-01 11:25:23 +02:00
2020-10-05 11:46:14 +02:00
G2 = g_pos< span class = "org-type" > /< / span > s< span class = "org-type" > /< / span > (1 < span class = "org-type" > +< / span > s< span class = "org-type" > /< / span > w_pos); < span class = "org-comment" > % Position Sensor Plant [V/(m/s)]< / span >
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
These nominal dynamics are also taken as the model of the sensor dynamics.
2020-10-05 15:41:57 +02:00
The true sensor dynamics has some uncertainty associated to it and described in section < a href = "#orgf04439d" > 1.2< / a > .
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 11:25:23 +02:00
< p >
2020-10-05 15:41:57 +02:00
Both sensor dynamics in \([\frac{V}{m/s}]\) are shown in Figure < a href = "#org2d1ef0b" > 2< / a > .
2020-10-01 11:25:23 +02:00
< / p >
2020-10-05 15:41:57 +02:00
< div id = "org2d1ef0b" 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-05 15:41:57 +02:00
< div id = "outline-container-orgec8c81d" class = "outline-3" >
< h3 id = "orgec8c81d" > < 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-05 15:41:57 +02:00
< a id = "orgf04439d" > < / a >
The uncertainty on the sensor dynamics is described by multiplicative uncertainty (Figure < a href = "#org35e2340" > 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-05 15:41:57 +02:00
The weights \(W_i(s)\) representing the dynamical uncertainty are defined below and their magnitude is shown in Figure < a href = "#org557e062" > 3< / 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-05 11:46:14 +02:00
< pre class = "src src-matlab" > W1 = createWeight(< span class = "org-string" > 'n'< / span > , 2, < span class = "org-string" > 'w0'< / span > , 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 3, < span class = "org-string" > 'G0'< / span > , 2, < span class = "org-string" > 'G1'< / span > , 0.1, < span class = "org-string" > 'Gc'< / span > , 1) < span class = "org-type" > *< / span > ...
createWeight(< span class = "org-string" > 'n'< / span > , 2, < span class = "org-string" > 'w0'< / span > , 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 1e3, < span class = "org-string" > 'G0'< / span > , 1, < span class = "org-string" > 'G1'< / span > , 4< span class = "org-type" > /< / span > 0.1, < span class = "org-string" > 'Gc'< / span > , 1< span class = "org-type" > /< / span > 0.1);
2020-10-01 11:25:23 +02:00
2020-10-05 11:46:14 +02:00
W2 = createWeight(< span class = "org-string" > 'n'< / span > , 2, < span class = "org-string" > 'w0'< / span > , 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 1e2, < span class = "org-string" > 'G0'< / span > , 0.05, < span class = "org-string" > 'G1'< / span > , 4, < span class = "org-string" > 'Gc'< / span > , 1);
2020-10-01 11:25:23 +02:00
< / pre >
2019-08-14 12:08:30 +02:00
< / div >
< p >
2020-10-05 15:41:57 +02:00
The bode plot of the sensors nominal dynamics as well as their defined dynamical spread are shown in Figure < a href = "#orgc218675" > 4< / a > .
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 11:25:23 +02:00
2020-10-05 15:41:57 +02:00
< div id = "org557e062" 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-05 15:41:57 +02:00
< div id = "orgc218675" 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-05 15:41:57 +02:00
< div id = "outline-container-org81d9a34" class = "outline-3" >
< h3 id = "org81d9a34" > < 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-05 15:41:57 +02:00
< a id = "org71b587e" > < / 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 = "#org35e2340" > 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-05 15:41:57 +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 = "#org51f9788" > 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-05 11:46:14 +02:00
< pre class = "src src-matlab" > omegac = 0.15< span class = "org-type" > *< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > ; G0 = 1e< span class = "org-type" > -< / span > 1; Ginf = 1e< span class = "org-type" > -< / span > 6;
N1 = (Ginf< span class = "org-type" > *< / span > s< span class = "org-type" > /< / span > omegac < span class = "org-type" > +< / span > G0)< span class = "org-type" > /< / span > (s< span class = "org-type" > /< / span > omegac < span class = "org-type" > +< / span > 1)< span class = "org-type" > /< / span > (1 < span class = "org-type" > +< / span > s< span class = "org-type" > /< / span > 2< span class = "org-type" > /< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > 1e4);
2019-08-14 12:08:30 +02:00
2020-10-05 11:46:14 +02:00
omegac = 1000< span class = "org-type" > *< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > ; G0 = 1e< span class = "org-type" > -< / span > 6; Ginf = 1e< span class = "org-type" > -< / span > 3;
N2 = (Ginf< span class = "org-type" > *< / span > s< span class = "org-type" > /< / span > omegac < span class = "org-type" > +< / span > G0)< span class = "org-type" > /< / span > (s< span class = "org-type" > /< / span > omegac < span class = "org-type" > +< / span > 1)< span class = "org-type" > /< / span > (1 < span class = "org-type" > +< / span > s< span class = "org-type" > /< / span > 2< span class = "org-type" > /< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > 1e4);
2020-10-01 11:25:23 +02:00
< / pre >
< / div >
2020-10-05 15:41:57 +02:00
< div id = "org51f9788" 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-05 15:41:57 +02:00
< div id = "outline-container-org4e23f6c" class = "outline-3" >
< h3 id = "org4e23f6c" > < 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-05 11:46:14 +02:00
< pre class = "src src-matlab" > save(< span class = "org-string" > './mat/model.mat'< / span > , < span class = "org-string" > 'freqs'< / span > , < span class = "org-string" > 'G1'< / span > , < span class = "org-string" > 'G2'< / span > , < span class = "org-string" > 'N2'< / span > , < span class = "org-string" > 'N1'< / span > , < span class = "org-string" > 'W2'< / span > , < span class = "org-string" > 'W1'< / span > );
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-05 15:41:57 +02:00
< div id = "outline-container-org2cab1a2" class = "outline-2" >
< h2 id = "org2cab1a2" > < 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-05 15:41:57 +02:00
< a id = "orga64daad" > < / a >
2020-10-01 11:25:23 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< / div >
2020-10-05 15:41:57 +02:00
< div id = "outline-container-org0cbc92d" class = "outline-3" >
< h3 id = "org0cbc92d" > < 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-05 15:41:57 +02:00
< a id = "org31e00a0" > < / 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-05 15:41:57 +02:00
The two sensors presented in Section < a href = "#orgee25d07" > 1< / a > are now merged together using complementary filters \(H_1(s)\) and \(H_2(s)\) to form a super sensor (Figure < a href = "#org48a16fd" > 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-05 15:41:57 +02:00
< div id = "org48a16fd" class = "figure" >
< p > < img src = "figs-paper/sensor_fusion_noise_arch.png" alt = "sensor_fusion_noise_arch.png" / >
2020-10-01 15:14:10 +02:00
< / 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-05 15:41:57 +02:00
< div id = "outline-container-orge9e0bd4" class = "outline-3" >
< h3 id = "orge9e0bd4" > < 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-05 15:41:57 +02:00
< a id = "orgff055a3" > < / 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-05 15:41:57 +02:00
< div id = "outline-container-orgefb4347" class = "outline-3" >
< h3 id = "orgefb4347" > < 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-05 15:41:57 +02:00
< a id = "org859b213" > < / a >
2020-10-01 15:14:10 +02:00
< / p >
< p >
2020-10-05 15:41:57 +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 = "#org54538d7" > 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-05 15:41:57 +02:00
< div id = "org54538d7" class = "figure" >
< p > < img src = "figs-paper/sensor_model_uncertainty.png" alt = "sensor_model_uncertainty.png" / >
2020-10-01 15:14:10 +02:00
< / p >
< p > < span class = "figure-number" > Figure 7: < / span > Sensor Model including Dynamical Uncertainty< / p >
< / div >
< p >
2020-10-05 15:41:57 +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 = "#orgb11ef23" > 8< / a > .
2020-10-01 15:14:10 +02:00
< / p >
2020-10-05 15:41:57 +02:00
< div id = "orgb11ef23" class = "figure" >
< p > < img src = "figs-paper/uncertainty_set_super_sensor.png" alt = "uncertainty_set_super_sensor.png" / >
2020-10-01 15:14:10 +02:00
< / 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-05 15:41:57 +02:00
< div id = "outline-container-org5896b60" class = "outline-2" >
< h2 id = "org5896b60" > < 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-05 15:41:57 +02:00
< a id = "orgdd6b9ce" > < / 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 >
< 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-05 15:41:57 +02:00
This is done using the \(\mathcal{H}_2\) synthesis in Section < a href = "#org5bc9386" > 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-05 15:41:57 +02:00
< div id = "outline-container-orgf5a8a84" class = "outline-3" >
< h3 id = "orgf5a8a84" > < 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-05 15:41:57 +02:00
< a id = "org5bc9386" > < / a >
2019-08-29 14:55:03 +02:00
< / p >
< p >
2020-10-05 15:41:57 +02:00
Consider the generalized plant \(P_{\mathcal{H}_2}\) shown in Figure < a href = "#orgd218886" > 9< / a > and described by Equation \eqref{eq:H2_generalized_plant}.
2019-08-14 12:08:30 +02:00
< / p >
2020-10-05 15:41:57 +02:00
< div id = "orgd218886" class = "figure" >
< p > < img src = "figs-paper/h_two_optimal_fusion.png" alt = "h_two_optimal_fusion.png" / >
2019-08-14 12:08:30 +02:00
< / p >
2020-10-05 11:46:14 +02:00
< p > < span class = "figure-number" > Figure 9: < / 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-05 11:46:14 +02:00
< pre class = "src src-matlab" > PH2 = [N1 < span class = "org-type" > -< / span > N1;
2020-10-01 15:14:10 +02:00
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-05 11:46:14 +02:00
< pre class = "src src-matlab" > [H2, < span class = "org-type" > ~< / span > , 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-05 11:46:14 +02:00
< pre class = "src src-matlab" > H1 = 1 < span class = "org-type" > -< / span > H2;
2019-08-14 12:08:30 +02:00
< / pre >
< / div >
< p >
2020-10-05 15:41:57 +02:00
The obtained complementary filters are shown in Figure < a href = "#org8c7ba6b" > 10< / a > .
2019-08-29 14:55:03 +02:00
< / p >
2020-10-05 15:41:57 +02:00
< div id = "org8c7ba6b" 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-05 11:46:14 +02:00
< p > < span class = "figure-number" > Figure 10: < / 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-05 15:41:57 +02:00
< div id = "outline-container-org72159df" class = "outline-3" >
< h3 id = "org72159df" > < 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-05 15:41:57 +02:00
< a id = "orgc7cc0a8" > < / a >
2019-08-29 14:55:03 +02:00
< / p >
< p >
2020-10-05 11:46:14 +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.
2019-08-14 12:08:30 +02:00
< / p >
2020-10-01 15:14:10 +02:00
< div class = "org-src-container" >
2020-10-05 11:46:14 +02:00
< pre class = "src src-matlab" > PSD_S1 = abs(squeeze(freqresp(N1, freqs, < span class = "org-string" > 'Hz'< / span > )))< span class = "org-type" > .^< / span > 2;
PSD_S2 = abs(squeeze(freqresp(N2, freqs, < span class = "org-string" > 'Hz'< / span > )))< span class = "org-type" > .^< / span > 2;
PSD_H2 = abs(squeeze(freqresp(N1< span class = "org-type" > *< / span > H1, freqs, < span class = "org-string" > 'Hz'< / span > )))< span class = "org-type" > .^< / span > 2 < span class = "org-type" > +< / span > ...
abs(squeeze(freqresp(N2< span class = "org-type" > *< / span > H2, freqs, < span class = "org-string" > 'Hz'< / span > )))< span class = "org-type" > .^< / span > 2;
2020-10-01 15:14:10 +02:00
< / pre >
< / div >
2019-08-14 12:08:30 +02:00
2020-10-01 11:25:23 +02:00
< p >
2020-10-05 15:41:57 +02:00
The obtained ASD are shown in Figure < a href = "#orge481bcd" > 11< / a > .
2020-10-05 11:46:14 +02:00
< / p >
< p >
2020-10-05 15:41:57 +02:00
The RMS value of the individual sensors and of the super sensor are listed in Table < a href = "#org3918d27" > 3< / a > .
2019-08-14 12:08:30 +02:00
< / p >
2020-10-05 15:41:57 +02:00
< table id = "org3918d27" 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-05 15:41:57 +02:00
< div id = "orge481bcd" 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 >
2020-10-05 11:46:14 +02:00
< p > < span class = "figure-number" > Figure 11: < / 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-05 15:41:57 +02:00
The velocity estimates from the two sensors and from the super sensors are shown in Figure < a href = "#org9c2a7e4" > 12< / a > .
The resulting noises are displayed in Figure < a href = "#org4cc42bf" > 13< / a > .
2019-08-14 12:08:30 +02:00
< / p >
2020-10-05 15:41:57 +02:00
< div id = "org9c2a7e4" 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-05 11:46:14 +02:00
< p > < span class = "figure-number" > Figure 12: < / 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-05 15:41:57 +02:00
< div id = "org4cc42bf" 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-05 11:46:14 +02:00
< p > < span class = "figure-number" > Figure 13: < / 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-05 15:41:57 +02:00
< div id = "outline-container-org8ffba19" class = "outline-3" >
< h3 id = "org8ffba19" > < 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-05 15:41:57 +02:00
If we consider sensor dynamical uncertainty as explained in Section < a href = "#orgf04439d" > 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-05 15:41:57 +02:00
The super sensor dynamical uncertainty is shown in Figure < a href = "#org865879b" > 14< / 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-05 15:41:57 +02:00
< div id = "org865879b" 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-05 11:46:14 +02:00
< p > < span class = "figure-number" > Figure 14: < / 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-05 15:41:57 +02:00
< div id = "outline-container-org26ea7b1" class = "outline-2" >
< h2 id = "org26ea7b1" > < 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-05 15:41:57 +02:00
< a id = "org5d93f37" > < / 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-05 15:41:57 +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 = "#org2b71ca6" > 15< / a > .
2019-08-14 12:08:30 +02:00
< / p >
2020-10-05 15:41:57 +02:00
< div id = "org2b71ca6" class = "figure" >
< p > < img src = "figs-paper/sensor_fusion_arch_uncertainty.png" alt = "sensor_fusion_arch_uncertainty.png" / >
2019-08-14 12:08:30 +02:00
< / p >
2020-10-05 11:46:14 +02:00
< p > < span class = "figure-number" > Figure 15: < / span > Sensor fusion architecture with sensor dynamics uncertainty< / p >
2019-08-14 12:08:30 +02:00
< / div >
< p >
2020-10-05 15:41:57 +02:00
As explained in Section < a href = "#orgf04439d" > 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-05 15:41:57 +02:00
The choice of \(W_u\) is presented in Section < a href = "#orgefed264" > 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-05 15:41:57 +02:00
This is done using the \(\mathcal{H}_\infty\) synthesis in Section < a href = "#org2c990ce" > 4.2< / a > .
2019-08-29 22:47:20 +02:00
< / p >
< / div >
2020-10-05 15:41:57 +02:00
< div id = "outline-container-org8dfd9d2" class = "outline-3" >
< h3 id = "org8dfd9d2" > < 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-05 15:41:57 +02:00
< a id = "orgefed264" > < / 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-05 15:41:57 +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 = "#orgdffee80" > 16< / a > .
2019-08-29 22:47:20 +02:00
< / p >
< div class = "org-src-container" >
2020-10-05 11:46:14 +02:00
< pre class = "src src-matlab" > Dphi = 10; < span class = "org-comment" > % [deg]< / span >
2019-08-29 22:47:20 +02:00
2020-10-05 11:46:14 +02:00
Wu = createWeight(< span class = "org-string" > 'n'< / span > , 2, < span class = "org-string" > 'w0'< / span > , 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 4e2, < span class = "org-string" > 'G0'< / span > , 1< span class = "org-type" > /< / span > sin(Dphi< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > 180), < span class = "org-string" > 'G1'< / span > , 1< span class = "org-type" > /< / span > 4, < span class = "org-string" > 'Gc'< / span > , 1);
2019-08-29 22:47:20 +02:00
< / pre >
< / div >
2020-10-05 15:41:57 +02:00
< div id = "orgdffee80" 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-05 11:46:14 +02:00
< p > < span class = "figure-number" > Figure 16: < / 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-05 15:41:57 +02:00
< div id = "outline-container-org7422ade" class = "outline-3" >
< h3 id = "org7422ade" > < 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-05 15:41:57 +02:00
< a id = "org2c990ce" > < / a >
2019-08-30 14:39:01 +02:00
< / p >
2020-10-02 17:59:57 +02:00
< p >
2020-10-05 15:41:57 +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 = "#org59777f5" > 17< / 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-05 15:41:57 +02:00
< div id = "org59777f5" class = "figure" >
< p > < img src = "figs-paper/h_infinity_robust_fusion.png" alt = "h_infinity_robust_fusion.png" / >
2019-08-29 22:47:20 +02:00
< / p >
2020-10-05 11:46:14 +02:00
< p > < span class = "figure-number" > Figure 17: < / 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-05 11:46:14 +02:00
< pre class = "src src-matlab" > P = [Wu< span class = "org-type" > *< / span > W1 < span class = "org-type" > -< / span > Wu< span class = "org-type" > *< / span > W1;
0 Wu< span class = "org-type" > *< / span > W2;
2020-10-01 15:14:10 +02:00
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-05 11:46:14 +02:00
< pre class = "src src-matlab" > H2 = hinfsyn(P, 1, 1,< span class = "org-string" > 'TOLGAM'< / span > , 0.001, < span class = "org-string" > 'DISPLAY'< / span > , < span class = "org-string" > 'on'< / span > );
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-10-05 11:46:14 +02:00
< pre class = "src src-matlab" > H1 = 1 < span class = "org-type" > -< / span > H2;
2019-08-29 22:47:20 +02:00
< / pre >
< / div >
2019-08-30 14:39:01 +02:00
< p >
2020-10-05 15:41:57 +02:00
The obtained complementary filters as well as the wanted upper bounds are shown in Figure < a href = "#orga1806e3" > 18< / 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-05 15:41:57 +02:00
< div id = "orga1806e3" 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-05 11:46:14 +02:00
< p > < span class = "figure-number" > Figure 18: < / 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-05 15:41:57 +02:00
< div id = "outline-container-orga0267c6" class = "outline-3" >
< h3 id = "orga0267c6" > < 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-05 15:41:57 +02:00
The super sensor dynamical uncertainty is displayed in Figure < a href = "#orge75f5ef" > 19< / 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-05 15:41:57 +02:00
< div id = "orge75f5ef" 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-05 11:46:14 +02:00
< p > < span class = "figure-number" > Figure 19: < / 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-05 15:41:57 +02:00
< div id = "outline-container-org979fede" class = "outline-3" >
< h3 id = "org979fede" > < 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-05 11:46:14 +02:00
We now compute the obtain Power Spectral Density of the super sensor’ s noise.
2020-10-05 15:41:57 +02:00
The Amplitude Spectral Densities are shown in Figure < a href = "#org5aac73f" > 20< / 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" >
2020-10-05 11:46:14 +02:00
< pre class = "src src-matlab" > PSD_S2 = abs(squeeze(freqresp(N2, freqs, < span class = "org-string" > 'Hz'< / span > )))< span class = "org-type" > .^< / span > 2;
PSD_S1 = abs(squeeze(freqresp(N1, freqs, < span class = "org-string" > 'Hz'< / span > )))< span class = "org-type" > .^< / span > 2;
PSD_Hinf = abs(squeeze(freqresp(N1< span class = "org-type" > *< / span > H1, freqs, < span class = "org-string" > 'Hz'< / span > )))< span class = "org-type" > .^< / span > 2 < span class = "org-type" > +< / span > ...
abs(squeeze(freqresp(N2< span class = "org-type" > *< / span > H2, freqs, < span class = "org-string" > 'Hz'< / span > )))< span class = "org-type" > .^< / span > 2;
2019-08-30 14:39:01 +02:00
< / pre >
< / div >
2020-10-05 11:46:14 +02:00
< p >
2020-10-05 15:41:57 +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 = "#org3e08452" > 4< / a > .
2020-10-05 11:46:14 +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.
< / p >
2019-08-30 14:39:01 +02:00
2020-10-05 15:41:57 +02:00
< div id = "org5aac73f" 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-05 11:46:14 +02:00
< p > < span class = "figure-number" > Figure 20: < / 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-05 15:41:57 +02:00
< table id = "org3e08452" 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-05 15:41:57 +02:00
< div id = "outline-container-orgda33992" class = "outline-3" >
< h3 id = "orgda33992" > < 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-05 15:41:57 +02:00
< div id = "outline-container-org15afe90" class = "outline-2" >
< h2 id = "org15afe90" > < 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-05 15:41:57 +02:00
< a id = "org9f98c16" > < / 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-05 15:41:57 +02:00
The sensor fusion architecture is shown in Figure < a href = "#org3cc874e" > 21< / a > (\(\hat{G}_i\) are omitted for space reasons).
2020-10-02 18:25:52 +02:00
< / p >
2020-10-05 15:41:57 +02:00
< div id = "org3cc874e" class = "figure" >
< p > < img src = "figs-paper/sensor_fusion_arch_full.png" alt = "sensor_fusion_arch_full.png" / >
2020-10-01 15:14:10 +02:00
< / p >
2020-10-05 11:46:14 +02:00
< p > < span class = "figure-number" > Figure 21: < / 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-05 15:41:57 +02:00
To do so, we can use the Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis presented in Section < a href = "#orgbbc8594" > 5.1< / a > .
2019-08-30 14:39:01 +02:00
< / p >
< / div >
2020-10-05 15:41:57 +02:00
< div id = "outline-container-org0f81a91" class = "outline-3" >
< h3 id = "org0f81a91" > < 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-05 15:41:57 +02:00
< a id = "orgbbc8594" > < / a >
2020-10-02 18:25:52 +02:00
< / p >
< p >
2020-10-05 15:41:57 +02:00
The synthesis architecture that is used here is shown in Figure < a href = "#orga971cdb" > 22< / 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-05 15:41:57 +02:00
< div id = "orga971cdb" class = "figure" >
< p > < img src = "figs-paper/mixed_h2_hinf_synthesis.png" alt = "mixed_h2_hinf_synthesis.png" / >
2019-09-03 09:01:59 +02:00
< / p >
2020-10-05 11:46:14 +02:00
< p > < span class = "figure-number" > Figure 22: < / 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-05 11:46:14 +02:00
< pre class = "src src-matlab" > W1u = ss(W2< span class = "org-type" > *< / span > Wu); W2u = ss(W1< span class = "org-type" > *< / span > Wu); < span class = "org-comment" > % Weight on the uncertainty< / span >
W1n = ss(N2); W2n = ss(N1); < span class = "org-comment" > % Weight on the noise< / span >
2020-09-30 08:47:27 +02:00
2020-10-05 11:46:14 +02:00
P = [Wu< span class = "org-type" > *< / span > W1 < span class = "org-type" > -< / span > Wu< span class = "org-type" > *< / span > W1;
0 Wu< span class = "org-type" > *< / span > W2;
N1 < span class = "org-type" > -< / span > N1;
2020-10-02 18:25:52 +02:00
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-05 11:46:14 +02:00
< pre class = "src src-matlab" > [H2, < span class = "org-type" > ~< / span > ] = h2hinfsyn(ss(P), 1, 1, 2, [0, 1], < span class = "org-string" > 'HINFMAX'< / span > , 1, < span class = "org-string" > 'H2MAX'< / span > , < span class = "org-constant" > Inf< / span > , < span class = "org-string" > 'DKMAX'< / span > , 100, < span class = "org-string" > 'TOL'< / span > , 1e< span class = "org-type" > -< / span > 3, < span class = "org-string" > 'DISPLAY'< / span > , < span class = "org-string" > 'on'< / span > );
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" >
2020-10-05 11:46:14 +02:00
< pre class = "src src-matlab" > H1 = 1 < span class = "org-type" > -< / span > H2;
2019-08-30 14:39:01 +02:00
< / pre >
< / div >
2019-09-03 09:01:59 +02:00
< p >
2020-10-05 15:41:57 +02:00
The obtained complementary filters are shown in Figure < a href = "#org30e8a3f" > 23< / a > .
2019-09-03 09:01:59 +02:00
< / p >
2020-10-05 15:41:57 +02:00
< div id = "org30e8a3f" 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-05 11:46:14 +02:00
< p > < span class = "figure-number" > Figure 23: < / 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-05 15:41:57 +02:00
< div id = "outline-container-org417aabd" class = "outline-3" >
< h3 id = "org417aabd" > < 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-05 15:41:57 +02:00
The Amplitude Spectral Density of the super sensor’ s noise is shown in Figure < a href = "#orgfdfbf76" > 24< / a > .
2019-09-03 09:01:59 +02:00
< / p >
2020-10-02 18:25:52 +02:00
< p >
2020-10-05 15:41:57 +02:00
A time domain simulation is shown in Figure < a href = "#orgdf5905f" > 25< / a > .
2019-09-03 09:01:59 +02:00
< / p >
< p >
2020-10-05 15:41:57 +02:00
The RMS values of the super sensor noise for the presented three synthesis are listed in Table < a href = "#org3b7df55" > 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-05 11:46:14 +02:00
< pre class = "src src-matlab" > PSD_S2 = abs(squeeze(freqresp(N2, freqs, < span class = "org-string" > 'Hz'< / span > )))< span class = "org-type" > .^< / span > 2;
PSD_S1 = abs(squeeze(freqresp(N1, freqs, < span class = "org-string" > 'Hz'< / span > )))< span class = "org-type" > .^< / span > 2;
PSD_H2Hinf = abs(squeeze(freqresp(N1< span class = "org-type" > *< / span > H1, freqs, < span class = "org-string" > 'Hz'< / span > )))< span class = "org-type" > .^< / span > 2 < span class = "org-type" > +< / span > ...
abs(squeeze(freqresp(N2< span class = "org-type" > *< / span > H2, freqs, < span class = "org-string" > 'Hz'< / span > )))< span class = "org-type" > .^< / span > 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-05 15:41:57 +02:00
< div id = "orgfdfbf76" 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 >
2020-10-05 11:46:14 +02:00
< p > < span class = "figure-number" > Figure 24: < / span > Power Spectral Density of the Super Sensor obtained with the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis< / p >
2020-10-02 18:25:52 +02:00
< / div >
2020-10-01 11:25:23 +02:00
2020-10-05 15:41:57 +02:00
< div id = "orgdf5905f" 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 >
2020-10-05 11:46:14 +02:00
< p > < span class = "figure-number" > Figure 25: < / 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-05 15:41:57 +02:00
< table id = "org3b7df55" 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-05 15:41:57 +02:00
< div id = "outline-container-org2dce888" class = "outline-3" >
< h3 id = "org2dce888" > < 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-05 15:41:57 +02:00
The uncertainty on the super sensor’ s dynamics is shown in Figure < a href = "#orgb2d28c5" > 26< / a > .
2020-10-02 18:25:52 +02:00
< / p >
2020-10-05 15:41:57 +02:00
< div id = "orgb2d28c5" 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 >
2020-10-05 11:46:14 +02:00
< p > < span class = "figure-number" > Figure 26: < / span > Super sensor dynamical uncertainty (solid curve) when using the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis< / p >
2020-10-02 18:25:52 +02:00
< / div >
< / div >
< / div >
2020-10-05 15:41:57 +02:00
< div id = "outline-container-org47da78c" class = "outline-3" >
< h3 id = "org47da78c" > < 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-05 15:41:57 +02:00
< div id = "outline-container-org0afe5ef" class = "outline-2" >
< h2 id = "org0afe5ef" > < 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-05 15:41:57 +02:00
< a id = "orgf41dc8d" > < / a >
2020-10-01 15:14:10 +02:00
< / p >
2020-10-01 11:25:23 +02:00
< / div >
2020-10-05 15:41:57 +02:00
< div id = "outline-container-orge81e522" class = "outline-3" >
< h3 id = "orge81e522" > < 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-05 15:41:57 +02:00
< a id = "org89bad6d" > < / 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" >
2020-10-05 11:46:14 +02:00
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > [W]< / span > = < span class = "org-function-name" > createWeight< / span > (< span class = "org-variable-name" > args< / span > )
< span class = "org-comment" > % createWeight -< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % Syntax: [in_data] = createWeight(in_data)< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % Inputs:< / span >
< span class = "org-comment" > % - n - Weight Order< / span >
< span class = "org-comment" > % - G0 - Low frequency Gain< / span >
< span class = "org-comment" > % - G1 - High frequency Gain< / span >
< span class = "org-comment" > % - Gc - Gain of W at frequency w0< / span >
< span class = "org-comment" > % - w0 - Frequency at which |W(j w0)| = Gc< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % Outputs:< / span >
< span class = "org-comment" > % - W - Generated Weight< / span >
2020-10-01 11:25:23 +02:00
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
2020-10-05 11:46:14 +02:00
< span class = "org-keyword" > end< / span >
2020-10-01 11:25:23 +02:00
mustBeBetween(args.G0, args.Gc, args.G1);
2020-10-05 11:46:14 +02:00
s = tf(< span class = "org-string" > 's'< / span > );
2020-10-01 11:25:23 +02:00
2020-10-05 11:46:14 +02:00
W = (((1< span class = "org-type" > /< / span > args.w0)< span class = "org-type" > *< / span > sqrt((1< span class = "org-type" > -< / span > (args.G0< span class = "org-type" > /< / span > args.Gc)< span class = "org-type" > ^< / span > (2< span class = "org-type" > /< / span > args.n))< span class = "org-type" > /< / span > (1< span class = "org-type" > -< / span > (args.Gc< span class = "org-type" > /< / span > args.G1)< span class = "org-type" > ^< / span > (2< span class = "org-type" > /< / span > args.n)))< span class = "org-type" > *< / span > s < span class = "org-type" > +< / span > (args.G0< span class = "org-type" > /< / span > args.Gc)< span class = "org-type" > ^< / span > (1< span class = "org-type" > /< / span > args.n))< span class = "org-type" > /< / span > ((1< span class = "org-type" > /< / span > args.G1)< span class = "org-type" > ^< / span > (1< span class = "org-type" > /< / span > args.n)< span class = "org-type" > *< / span > (1< span class = "org-type" > /< / span > args.w0)< span class = "org-type" > *< / span > sqrt((1< span class = "org-type" > -< / span > (args.G0< span class = "org-type" > /< / span > args.Gc)< span class = "org-type" > ^< / span > (2< span class = "org-type" > /< / span > args.n))< span class = "org-type" > /< / span > (1< span class = "org-type" > -< / span > (args.Gc< span class = "org-type" > /< / span > args.G1)< span class = "org-type" > ^< / span > (2< span class = "org-type" > /< / span > args.n)))< span class = "org-type" > *< / span > s < span class = "org-type" > +< / span > (1< span class = "org-type" > /< / span > args.Gc)< span class = "org-type" > ^< / span > (1< span class = "org-type" > /< / span > args.n)))< span class = "org-type" > ^< / span > args.n;
2020-10-01 11:25:23 +02:00
2020-10-05 11:46:14 +02:00
< span class = "org-keyword" > end< / span >
2020-10-01 11:25:23 +02:00
2020-10-05 11:46:14 +02:00
< span class = "org-comment" > % Custom validation function< / span >
< span class = "org-keyword" > function< / span > < span class = "org-function-name" > mustBeBetween< / span > (< span class = "org-variable-name" > a< / span > ,< span class = "org-variable-name" > b< / span > ,< span class = "org-variable-name" > c< / span > )
< span class = "org-keyword" > if< / span > < span class = "org-type" > ~< / span > ((a < span class = "org-type" > > < / span > b < span class = "org-type" > & & < / span > b < span class = "org-type" > > < / span > c) < span class = "org-type" > ||< / span > (c < span class = "org-type" > > < / span > b < span class = "org-type" > & & < / span > b < span class = "org-type" > > < / span > a))
eid = < span class = "org-string" > 'createWeight:inputError'< / span > ;
msg = < span class = "org-string" > 'Gc should be between G0 and G1.'< / span > ;
2020-10-01 11:25:23 +02:00
throwAsCaller(MException(eid,msg))
2020-10-05 11:46:14 +02:00
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
2020-10-01 11:25:23 +02:00
< / pre >
< / div >
< / div >
< / div >
2020-10-05 15:41:57 +02:00
< div id = "outline-container-org37ec2b4" class = "outline-3" >
< h3 id = "org37ec2b4" > < 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-05 15:41:57 +02:00
< a id = "org8739875" > < / 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" >
2020-10-05 11:46:14 +02:00
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > [p]< / span > = < span class = "org-function-name" > plotMagUncertainty< / span > (< span class = "org-variable-name" > W< / span > , < span class = "org-variable-name" > freqs< / span > , < span class = "org-variable-name" > args< / span > )
< span class = "org-comment" > % plotMagUncertainty -< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % Syntax: [p] = plotMagUncertainty(W, freqs, args)< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % Inputs:< / span >
< span class = "org-comment" > % - W - Multiplicative Uncertainty Weight< / span >
< span class = "org-comment" > % - freqs - Frequency Vector [Hz]< / span >
< span class = "org-comment" > % - args - Optional Arguments:< / span >
< span class = "org-comment" > % - G< / span >
< span class = "org-comment" > % - color_i< / span >
< span class = "org-comment" > % - opacity< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % Outputs:< / span >
< span class = "org-comment" > % - p - Plot Handle< / span >
2020-10-01 11:25:23 +02:00
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-05 11:46:14 +02:00
args.DisplayName char = < span class = "org-string" > ''< / span >
< span class = "org-keyword" > end< / span >
2020-10-01 11:25:23 +02:00
2020-10-05 11:46:14 +02:00
< span class = "org-comment" > % Get defaults colors< / span >
colors = < span class = "org-type" > get< / span > (< span class = "org-variable-name" > groot< / span > , < span class = "org-string" > 'defaultAxesColorOrder'< / span > );
2020-10-01 11:25:23 +02:00
2020-10-05 11:46:14 +02:00
p = < span class = "org-type" > patch< / span > ([freqs flip(freqs)], ...
[abs(squeeze(freqresp(args.G, freqs, < span class = "org-string" > 'Hz'< / span > )))< span class = "org-type" > .*< / span > (1 < span class = "org-type" > +< / span > abs(squeeze(freqresp(W, freqs, < span class = "org-string" > 'Hz'< / span > )))); ...
flip(abs(squeeze(freqresp(args.G, freqs, < span class = "org-string" > 'Hz'< / span > )))< span class = "org-type" > .*< / span > max(1 < span class = "org-type" > -< / span > abs(squeeze(freqresp(W, freqs, < span class = "org-string" > 'Hz'< / span > ))), 1e< span class = "org-type" > -< / span > 6))], < span class = "org-string" > 'w'< / span > , ...
< span class = "org-string" > 'DisplayName'< / span > , args.DisplayName);
2020-10-01 11:25:23 +02:00
2020-10-05 11:46:14 +02:00
p.FaceColor = colors(args.color_i, < span class = "org-type" > :< / span > );
p.EdgeColor = < span class = "org-string" > 'none'< / span > ;
2020-10-01 11:25:23 +02:00
p.FaceAlpha = args.opacity;
2020-10-05 11:46:14 +02:00
< span class = "org-keyword" > end< / span >
2020-10-01 11:25:23 +02:00
< / pre >
< / div >
< / div >
< / div >
2020-10-05 15:41:57 +02:00
< div id = "outline-container-org9f73572" class = "outline-3" >
< h3 id = "org9f73572" > < 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-05 15:41:57 +02:00
< a id = "org49e21eb" > < / 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" >
2020-10-05 11:46:14 +02:00
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > [p]< / span > = < span class = "org-function-name" > plotPhaseUncertainty< / span > (< span class = "org-variable-name" > W< / span > , < span class = "org-variable-name" > freqs< / span > , < span class = "org-variable-name" > args< / span > )
< span class = "org-comment" > % plotPhaseUncertainty -< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % Syntax: [p] = plotPhaseUncertainty(W, freqs, args)< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % Inputs:< / span >
< span class = "org-comment" > % - W - Multiplicative Uncertainty Weight< / span >
< span class = "org-comment" > % - freqs - Frequency Vector [Hz]< / span >
< span class = "org-comment" > % - args - Optional Arguments:< / span >
< span class = "org-comment" > % - G< / span >
< span class = "org-comment" > % - color_i< / span >
< span class = "org-comment" > % - opacity< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % Outputs:< / span >
< span class = "org-comment" > % - p - Plot Handle< / span >
2020-10-01 11:25:23 +02:00
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
2020-10-05 11:46:14 +02:00
args.DisplayName char = < span class = "org-string" > ''< / span >
< span class = "org-keyword" > end< / span >
2020-10-01 11:25:23 +02:00
2020-10-05 11:46:14 +02:00
< span class = "org-comment" > % Get defaults colors< / span >
colors = < span class = "org-type" > get< / span > (< span class = "org-variable-name" > groot< / span > , < span class = "org-string" > 'defaultAxesColorOrder'< / span > );
2020-10-01 11:25:23 +02:00
2020-10-05 11:46:14 +02:00
< span class = "org-comment" > % Compute Phase Uncertainty< / span >
Dphi = 180< span class = "org-type" > /< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > asin(abs(squeeze(freqresp(W, freqs, < span class = "org-string" > 'Hz'< / span > ))));
Dphi(abs(squeeze(freqresp(W, freqs, < span class = "org-string" > 'Hz'< / span > ))) < span class = "org-type" > > < / span > 1) = 360;
2020-10-01 11:25:23 +02:00
2020-10-05 11:46:14 +02:00
< span class = "org-comment" > % Compute Plant Phase< / span >
G_ang = 180< span class = "org-type" > /< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > angle(squeeze(freqresp(args.G, freqs, < span class = "org-string" > 'Hz'< / span > )));
2020-10-01 11:25:23 +02:00
2020-10-05 11:46:14 +02:00
p = < span class = "org-type" > patch< / span > ([freqs flip(freqs)], [G_ang< span class = "org-type" > +< / span > Dphi; flip(G_ang< span class = "org-type" > -< / span > Dphi)], < span class = "org-string" > 'w'< / span > , ...
< span class = "org-string" > 'DisplayName'< / span > , args.DisplayName);
2020-10-01 11:25:23 +02:00
2020-10-05 11:46:14 +02:00
p.FaceColor = colors(args.color_i, < span class = "org-type" > :< / span > );
p.EdgeColor = < span class = "org-string" > 'none'< / span > ;
2020-10-01 11:25:23 +02:00
p.FaceAlpha = args.opacity;
2020-10-05 11:46:14 +02:00
< span class = "org-keyword" > end< / span >
2020-10-01 11:25:23 +02:00
< / 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-05 15:41:57 +02:00
< p class = "date" > Created: 2020-10-05 lun. 15:08< / p >
2019-08-14 12:08:30 +02:00
< / div >
< / body >
< / html >
