1399 lines
58 KiB
HTML
1399 lines
58 KiB
HTML
<?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-11-12 jeu. 10:42 -->
|
|
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
|
|
<title>Robust and Optimal Sensor Fusion - Matlab Computation</title>
|
|
<meta name="generator" content="Org mode" />
|
|
<meta name="author" content="Thomas Dehaeze" />
|
|
<link rel="stylesheet" type="text/css" href="https://research.tdehaeze.xyz/css/style.css"/>
|
|
<script type="text/javascript" src="https://research.tdehaeze.xyz/js/script.js"></script>
|
|
<script>MathJax = {
|
|
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>
|
|
</head>
|
|
<body>
|
|
<div id="org-div-home-and-up">
|
|
<a accesskey="h" href="../index.html"> UP </a>
|
|
|
|
|
<a accesskey="H" href="../index.html"> HOME </a>
|
|
</div><div id="content">
|
|
<h1 class="title">Robust and Optimal Sensor Fusion - Matlab Computation</h1>
|
|
<div id="table-of-contents">
|
|
<h2>Table of Contents</h2>
|
|
<div id="text-table-of-contents">
|
|
<ul>
|
|
<li><a href="#orgb558a2f">1. Sensor Description</a>
|
|
<ul>
|
|
<li><a href="#org2e756d8">1.1. Sensor Dynamics</a></li>
|
|
<li><a href="#orgf266a4b">1.2. Sensor Model Uncertainty</a></li>
|
|
<li><a href="#orgb768f02">1.3. Sensor Noise</a></li>
|
|
<li><a href="#org426c21e">1.4. Save Model</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgce0ac83">2. Introduction to Sensor Fusion</a>
|
|
<ul>
|
|
<li><a href="#orgbb36114">2.1. Sensor Fusion Architecture</a></li>
|
|
<li><a href="#org3a4f4b8">2.2. Super Sensor Noise</a></li>
|
|
<li><a href="#org3f0594e">2.3. Super Sensor Dynamical Uncertainty</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org096192f">3. Optimal Super Sensor Noise: \(\mathcal{H}_2\) Synthesis</a>
|
|
<ul>
|
|
<li><a href="#org9188eae">3.1. \(\mathcal{H}_2\) Synthesis</a></li>
|
|
<li><a href="#org6d6d909">3.2. Super Sensor Noise</a></li>
|
|
<li><a href="#org850d99e">3.3. Discrepancy between sensor dynamics and model</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org6d1270a">4. Robust Sensor Fusion: \(\mathcal{H}_\infty\) Synthesis</a>
|
|
<ul>
|
|
<li><a href="#orgc5a0221">4.1. Weighting Function used to bound the super sensor uncertainty</a></li>
|
|
<li><a href="#org334f826">4.2. \(\mathcal{H}_\infty\) Synthesis</a></li>
|
|
<li><a href="#org211e432">4.3. Super sensor uncertainty</a></li>
|
|
<li><a href="#org7672e0e">4.4. Super sensor noise</a></li>
|
|
<li><a href="#orgdcb47cf">4.5. Conclusion</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org49a7cc2">5. Optimal and Robust Sensor Fusion: Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis</a>
|
|
<ul>
|
|
<li><a href="#org7ed8ed7">5.1. Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis</a></li>
|
|
<li><a href="#orgfb839cf">5.2. Obtained Super Sensor’s noise</a></li>
|
|
<li><a href="#orgb8596ac">5.3. Obtained Super Sensor’s Uncertainty</a></li>
|
|
<li><a href="#orgfcfa8a2">5.4. Conclusion</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgf267700">6. Matlab Functions</a>
|
|
<ul>
|
|
<li><a href="#org005db82">6.1. <code>createWeight</code></a></li>
|
|
<li><a href="#org816b32a">6.2. <code>plotMagUncertainty</code></a></li>
|
|
<li><a href="#orgddb3773">6.3. <code>plotPhaseUncertainty</code></a></li>
|
|
</ul>
|
|
</li>
|
|
</ul>
|
|
</div>
|
|
</div>
|
|
|
|
<p>
|
|
This document is arranged as follows:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>Section <a href="#org3cd5225">1</a>: the sensors are described (dynamics, uncertainty, noise)</li>
|
|
<li>Section <a href="#orge87db28">2</a>: the sensor fusion architecture is described and the super sensor noise and dynamical uncertainty are derived</li>
|
|
<li>Section <a href="#orgbaf748a">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="#orgea66128">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="#org2aeafb3">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="#org50a28ec">6</a>: Matlab functions used for the analysis are described</li>
|
|
</ul>
|
|
|
|
<div id="outline-container-orgb558a2f" class="outline-2">
|
|
<h2 id="orgb558a2f"><span class="section-number-2">1</span> Sensor Description</h2>
|
|
<div class="outline-text-2" id="text-1">
|
|
<p>
|
|
<a id="org3cd5225"></a>
|
|
</p>
|
|
<p>
|
|
In Figure <a href="#orgd79f98f">1</a> is shown a schematic of a sensor model that is used in the following study.
|
|
In this example, the measured quantity \(x\) is the velocity of an object.
|
|
</p>
|
|
|
|
<table id="org5aafbb9" 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="#orgd79f98f">1</a></caption>
|
|
|
|
<colgroup>
|
|
<col class="org-left" />
|
|
|
|
<col class="org-left" />
|
|
|
|
<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>
|
|
<th scope="col" class="org-left"><b>Unit</b></th>
|
|
</tr>
|
|
</thead>
|
|
<tbody>
|
|
<tr>
|
|
<td class="org-left">\(x\)</td>
|
|
<td class="org-left">Physical measured quantity</td>
|
|
<td class="org-left">\([m/s]\)</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left">\(\tilde{n}_i\)</td>
|
|
<td class="org-left">White noise with unitary PSD</td>
|
|
<td class="org-left"> </td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left">\(n_i\)</td>
|
|
<td class="org-left">Shaped noise</td>
|
|
<td class="org-left">\([m/s]\)</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left">\(v_i\)</td>
|
|
<td class="org-left">Sensor output measurement</td>
|
|
<td class="org-left">\([V]\)</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left">\(\hat{x}_i\)</td>
|
|
<td class="org-left">Estimate of \(x\) from the sensor</td>
|
|
<td class="org-left">\([m/s]\)</td>
|
|
</tr>
|
|
|
|
<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>
|
|
</tbody>
|
|
</table>
|
|
|
|
<table id="org3780b2b" 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="#orgd79f98f">1</a></caption>
|
|
|
|
<colgroup>
|
|
<col class="org-left" />
|
|
|
|
<col class="org-left" />
|
|
|
|
<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>
|
|
<th scope="col" class="org-left"><b>Unit</b></th>
|
|
</tr>
|
|
</thead>
|
|
<tbody>
|
|
<tr>
|
|
<td class="org-left">\(\hat{G}_i\)</td>
|
|
<td class="org-left">Nominal Sensor Dynamics</td>
|
|
<td class="org-left">\([\frac{V}{m/s}]\)</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left">\(W_i\)</td>
|
|
<td class="org-left">Weight representing the size of the uncertainty at each frequency</td>
|
|
<td class="org-left"> </td>
|
|
</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>
|
|
<td class="org-left"> </td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left">\(N_i\)</td>
|
|
<td class="org-left">Weight representing the sensor noise</td>
|
|
<td class="org-left">\([m/s]\)</td>
|
|
</tr>
|
|
</tbody>
|
|
</table>
|
|
|
|
|
|
<div id="orgd79f98f" class="figure">
|
|
<p><img src="figs-paper/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>
|
|
|
|
<div id="outline-container-org2e756d8" class="outline-3">
|
|
<h3 id="org2e756d8"><span class="section-number-3">1.1</span> Sensor Dynamics</h3>
|
|
<div class="outline-text-3" id="text-1-1">
|
|
<p>
|
|
<a id="org01c4832"></a>
|
|
Let’s consider two sensors measuring the velocity of an object.
|
|
</p>
|
|
|
|
<p>
|
|
The first sensor is an accelerometer.
|
|
Its nominal dynamics \(\hat{G}_1(s)\) is defined below.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<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>
|
|
|
|
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>
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The second sensor is a displacement sensor, its nominal dynamics \(\hat{G}_2(s)\) is defined below.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<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>
|
|
|
|
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>
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
These nominal dynamics are also taken as the model of the sensor dynamics.
|
|
The true sensor dynamics has some uncertainty associated to it and described in section <a href="#org3943238">1.2</a>.
|
|
</p>
|
|
|
|
<p>
|
|
Both sensor dynamics in \([\frac{V}{m/s}]\) are shown in Figure <a href="#org355b3fc">2</a>.
|
|
</p>
|
|
|
|
<div id="org355b3fc" class="figure">
|
|
<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>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgf266a4b" class="outline-3">
|
|
<h3 id="orgf266a4b"><span class="section-number-3">1.2</span> Sensor Model Uncertainty</h3>
|
|
<div class="outline-text-3" id="text-1-2">
|
|
<p>
|
|
<a id="org3943238"></a>
|
|
The uncertainty on the sensor dynamics is described by multiplicative uncertainty (Figure <a href="#orgd79f98f">1</a>).
|
|
</p>
|
|
|
|
<p>
|
|
The true sensor dynamics \(G_i(s)\) is then described by \eqref{eq:sensor_dynamics_uncertainty}.
|
|
</p>
|
|
|
|
\begin{equation}
|
|
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}
|
|
\end{equation}
|
|
|
|
<p>
|
|
The weights \(W_i(s)\) representing the dynamical uncertainty are defined below and their magnitude is shown in Figure <a href="#org2530a6e">3</a>.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<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);
|
|
|
|
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);
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The bode plot of the sensors nominal dynamics as well as their defined dynamical spread are shown in Figure <a href="#org415740c">4</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org2530a6e" class="figure">
|
|
<p><img src="figs/sensors_uncertainty_weights.png" alt="sensors_uncertainty_weights.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 3: </span>Magnitude of the multiplicative uncertainty weights \(|W_i(j\omega)|\)</p>
|
|
</div>
|
|
|
|
|
|
<div id="org415740c" class="figure">
|
|
<p><img src="figs/sensors_nominal_dynamics_and_uncertainty.png" alt="sensors_nominal_dynamics_and_uncertainty.png" />
|
|
</p>
|
|
<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>
|
|
</div>
|
|
|
|
<div id="outline-container-orgb768f02" class="outline-3">
|
|
<h3 id="orgb768f02"><span class="section-number-3">1.3</span> Sensor Noise</h3>
|
|
<div class="outline-text-3" id="text-1-3">
|
|
<p>
|
|
<a id="orgfd557c8"></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="#orgd79f98f">1</a>).
|
|
</p>
|
|
\begin{equation}
|
|
\Phi_{\tilde{n}_i}(\omega) = 1 \label{eq:unitary_noise_psd}
|
|
\end{equation}
|
|
|
|
<p>
|
|
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}]\).
|
|
</p>
|
|
\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}
|
|
|
|
<p>
|
|
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="#org5e2cd13">5</a>.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<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);
|
|
|
|
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);
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="org5e2cd13" class="figure">
|
|
<p><img src="figs/sensors_noise.png" alt="sensors_noise.png" />
|
|
</p>
|
|
<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>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org426c21e" class="outline-3">
|
|
<h3 id="org426c21e"><span class="section-number-3">1.4</span> Save Model</h3>
|
|
<div class="outline-text-3" id="text-1-4">
|
|
<p>
|
|
All the dynamical systems representing the sensors are saved for further use.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<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>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgce0ac83" class="outline-2">
|
|
<h2 id="orgce0ac83"><span class="section-number-2">2</span> Introduction to Sensor Fusion</h2>
|
|
<div class="outline-text-2" id="text-2">
|
|
<p>
|
|
<a id="orge87db28"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgbb36114" class="outline-3">
|
|
<h3 id="orgbb36114"><span class="section-number-3">2.1</span> Sensor Fusion Architecture</h3>
|
|
<div class="outline-text-3" id="text-2-1">
|
|
<p>
|
|
<a id="org80d2a51"></a>
|
|
</p>
|
|
|
|
<p>
|
|
The two sensors presented in Section <a href="#org3cd5225">1</a> are now merged together using complementary filters \(H_1(s)\) and \(H_2(s)\) to form a super sensor (Figure <a href="#orgffed4ec">6</a>).
|
|
</p>
|
|
|
|
|
|
<div id="orgffed4ec" class="figure">
|
|
<p><img src="figs-paper/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>
|
|
|
|
<p>
|
|
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}.
|
|
</p>
|
|
|
|
\begin{equation}
|
|
H_1(s) + H_2(s) = 1 \label{eq:complementary_property}
|
|
\end{equation}
|
|
|
|
<p>
|
|
The super sensor estimate \(\hat{x}\) is given by \eqref{eq:super_sensor_estimate}.
|
|
</p>
|
|
|
|
\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}
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org3a4f4b8" class="outline-3">
|
|
<h3 id="org3a4f4b8"><span class="section-number-3">2.2</span> Super Sensor Noise</h3>
|
|
<div class="outline-text-3" id="text-2-2">
|
|
<p>
|
|
<a id="org3e73362"></a>
|
|
</p>
|
|
|
|
<p>
|
|
If we first suppose that the models of the sensors \(\hat{G}_i\) are very close to the true sensor dynamics \(G_i\) \eqref{eq:good_dynamical_model}, we have that the super sensor estimate \(\hat{x}\) is equals to the measured quantity \(x\) plus the noise of the two sensors filtered out by the complementary filters \eqref{eq:estimate_perfect_models}.
|
|
</p>
|
|
|
|
\begin{equation}
|
|
\hat{G}_i^{-1}(s) G_i(s) \approx 1 \label{eq:good_dynamical_model}
|
|
\end{equation}
|
|
|
|
\begin{equation}
|
|
\hat{x} = x + \underbrace{\left( H_1 N_1 \right) \tilde{n}_1 + \left( H_2 N_2 \right) \tilde{n}_2}_{n} \label{eq:estimate_perfect_models}
|
|
\end{equation}
|
|
|
|
<p>
|
|
As the noise of both sensors are considered to be uncorrelated, the PSD of the super sensor noise is computed as follow:
|
|
</p>
|
|
\begin{equation}
|
|
\Phi_n(\omega) = \left| H_1(j\omega) N_1(j\omega) \right|^2 + \left| H_2(j\omega) N_2(j\omega) \right|^2 \label{eq:super_sensor_psd_noise}
|
|
\end{equation}
|
|
|
|
<p>
|
|
And the Root Mean Square (RMS) value of the super sensor noise \(\sigma_n\) is given by Equation \eqref{eq:super_sensor_rms_noise}.
|
|
</p>
|
|
\begin{equation}
|
|
\sigma_n = \sqrt{\int_0^\infty \Phi_n(\omega) d\omega} \label{eq:super_sensor_rms_noise}
|
|
\end{equation}
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org3f0594e" class="outline-3">
|
|
<h3 id="org3f0594e"><span class="section-number-3">2.3</span> Super Sensor Dynamical Uncertainty</h3>
|
|
<div class="outline-text-3" id="text-2-3">
|
|
<p>
|
|
<a id="orgd73f82f"></a>
|
|
</p>
|
|
|
|
<p>
|
|
If we consider some dynamical uncertainty (the true system dynamics \(G_i\) not being perfectly equal to our model \(\hat{G}_i\)) that we model by the use of multiplicative uncertainty (Figure <a href="#org05e7617">7</a>), the super sensor dynamics is then equals to:
|
|
</p>
|
|
|
|
\begin{equation}
|
|
\begin{aligned}
|
|
\frac{\hat{x}}{x} &= \Big( H_1 \hat{G}_1^{-1} \hat{G}_1 (1 + W_1 \Delta_1) + H_2 \hat{G}_2^{-1} \hat{G}_2 (1 + W_2 \Delta_2) \Big) \\
|
|
&= \Big( H_1 (1 + W_1 \Delta_1) + H_2 (1 + W_2 \Delta_2) \Big) \\
|
|
&= \left( 1 + H_1 W_1 \Delta_1 + H_2 W_2 \Delta_2 \right), \quad \|\Delta_i\|_\infty<1
|
|
\end{aligned}
|
|
\end{equation}
|
|
|
|
|
|
<div id="org05e7617" class="figure">
|
|
<p><img src="figs-paper/sensor_model_uncertainty.png" alt="sensor_model_uncertainty.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 7: </span>Sensor Model including Dynamical Uncertainty</p>
|
|
</div>
|
|
|
|
<p>
|
|
The uncertainty set of the transfer function from \(\hat{x}\) to \(x\) at frequency \(\omega\) is bounded in the complex plane by a circle centered on 1 and with a radius equal to \(|W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)|\) as shown in Figure <a href="#org1ff6ff8">8</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org1ff6ff8" class="figure">
|
|
<p><img src="figs-paper/uncertainty_set_super_sensor.png" alt="uncertainty_set_super_sensor.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 8: </span>Super Sensor model uncertainty displayed in the complex plane</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org096192f" class="outline-2">
|
|
<h2 id="org096192f"><span class="section-number-2">3</span> Optimal Super Sensor Noise: \(\mathcal{H}_2\) Synthesis</h2>
|
|
<div class="outline-text-2" id="text-3">
|
|
<p>
|
|
<a id="orgbaf748a"></a>
|
|
</p>
|
|
<p>
|
|
In this section, the complementary filters \(H_1(s)\) and \(H_2(s)\) are designed in order to minimize the RMS value of super sensor noise \(\sigma_n\).
|
|
</p>
|
|
|
|
<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}
|
|
|
|
<p>
|
|
The goal is to design \(H_1(s)\) and \(H_2(s)\) such that \(H_1(s) + H_2(s) = 1\) (complementary property) and such that \(\left\| \begin{matrix} H_1 N_1 \\ H_2 N_2 \end{matrix} \right\|_2\) is minimized (minimized RMS value of the super sensor noise).
|
|
This is done using the \(\mathcal{H}_2\) synthesis in Section <a href="#orgf1e20ae">3.1</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org9188eae" class="outline-3">
|
|
<h3 id="org9188eae"><span class="section-number-3">3.1</span> \(\mathcal{H}_2\) Synthesis</h3>
|
|
<div class="outline-text-3" id="text-3-1">
|
|
<p>
|
|
<a id="orgf1e20ae"></a>
|
|
</p>
|
|
|
|
<p>
|
|
Consider the generalized plant \(P_{\mathcal{H}_2}\) shown in Figure <a href="#org95769ad">9</a> and described by Equation \eqref{eq:H2_generalized_plant}.
|
|
</p>
|
|
|
|
|
|
<div id="org95769ad" class="figure">
|
|
<p><img src="figs-paper/h_two_optimal_fusion.png" alt="h_two_optimal_fusion.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 9: </span>Architecture used for \(\mathcal{H}_\infty\) synthesis of complementary filters</p>
|
|
</div>
|
|
|
|
\begin{equation} \label{eq:H2_generalized_plant}
|
|
\begin{pmatrix}
|
|
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
|
|
\end{pmatrix}
|
|
\end{equation}
|
|
|
|
<p>
|
|
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)\):
|
|
</p>
|
|
\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}
|
|
|
|
<p>
|
|
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.
|
|
</p>
|
|
|
|
<p>
|
|
The generalized plant \(P_{\mathcal{H}_2}\) is defined below
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">PH2 = [N1 <span class="org-type">-</span>N1;
|
|
0 N2;
|
|
1 0];
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The \(\mathcal{H}_2\) synthesis using the <code>h2syn</code> command
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">[H2, <span class="org-type">~</span>, gamma] = h2syn(PH2, 1, 1);
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Finally, \(H_1(s)\) is defined as follows
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">H1 = 1 <span class="org-type">-</span> H2;
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The obtained complementary filters are shown in Figure <a href="#org02514a9">10</a>.
|
|
</p>
|
|
|
|
<div id="org02514a9" class="figure">
|
|
<p><img src="figs/htwo_comp_filters.png" alt="htwo_comp_filters.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 10: </span>Obtained complementary filters using the \(\mathcal{H}_2\) Synthesis</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org6d6d909" class="outline-3">
|
|
<h3 id="org6d6d909"><span class="section-number-3">3.2</span> Super Sensor Noise</h3>
|
|
<div class="outline-text-3" id="text-3-2">
|
|
<p>
|
|
<a id="org497847a"></a>
|
|
</p>
|
|
|
|
<p>
|
|
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.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<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;
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The obtained ASD are shown in Figure <a href="#orgd3690df">11</a>.
|
|
</p>
|
|
|
|
<p>
|
|
The RMS value of the individual sensors and of the super sensor are listed in Table <a href="#org04c5321">3</a>.
|
|
</p>
|
|
<table id="org04c5321" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
|
|
<caption class="t-above"><span class="table-number">Table 3:</span> RMS value of the individual sensor noise and of the super sensor using the \(\mathcal{H}_2\) Synthesis</caption>
|
|
|
|
<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 value \([m/s]\)</th>
|
|
</tr>
|
|
</thead>
|
|
<tbody>
|
|
<tr>
|
|
<td class="org-left">\(\sigma_{n_1}\)</td>
|
|
<td class="org-right">0.015</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left">\(\sigma_{n_2}\)</td>
|
|
<td class="org-right">0.080</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left">\(\sigma_{n_{\mathcal{H}_2}}\)</td>
|
|
<td class="org-right">0.003</td>
|
|
</tr>
|
|
</tbody>
|
|
</table>
|
|
|
|
|
|
<div id="orgd3690df" class="figure">
|
|
<p><img src="figs/psd_sensors_htwo_synthesis.png" alt="psd_sensors_htwo_synthesis.png" />
|
|
</p>
|
|
<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>
|
|
</div>
|
|
|
|
<p>
|
|
A time domain simulation is now performed.
|
|
The measured velocity \(x\) is set to be a sweep sine with an amplitude of \(0.1\ [m/s]\).
|
|
The velocity estimates from the two sensors and from the super sensors are shown in Figure <a href="#orgf112864">12</a>.
|
|
The resulting noises are displayed in Figure <a href="#org1ab022b">13</a>.
|
|
</p>
|
|
|
|
|
|
<div id="orgf112864" class="figure">
|
|
<p><img src="figs/super_sensor_time_domain_h2.png" alt="super_sensor_time_domain_h2.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 12: </span>Noise of individual sensors and noise of the super sensor</p>
|
|
</div>
|
|
|
|
|
|
<div id="org1ab022b" class="figure">
|
|
<p><img src="figs/sensor_noise_H2_time_domain.png" alt="sensor_noise_H2_time_domain.png" />
|
|
</p>
|
|
<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>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org850d99e" class="outline-3">
|
|
<h3 id="org850d99e"><span class="section-number-3">3.3</span> Discrepancy between sensor dynamics and model</h3>
|
|
<div class="outline-text-3" id="text-3-3">
|
|
<p>
|
|
If we consider sensor dynamical uncertainty as explained in Section <a href="#org3943238">1.2</a>, we can compute what would be the super sensor dynamical uncertainty when using the complementary filters obtained using the \(\mathcal{H}_2\) Synthesis.
|
|
</p>
|
|
|
|
<p>
|
|
The super sensor dynamical uncertainty is shown in Figure <a href="#org35f4dc2">14</a>.
|
|
</p>
|
|
|
|
<p>
|
|
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.
|
|
</p>
|
|
|
|
<div id="org35f4dc2" class="figure">
|
|
<p><img src="figs/super_sensor_dynamical_uncertainty_H2.png" alt="super_sensor_dynamical_uncertainty_H2.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 14: </span>Super sensor dynamical uncertainty when using the \(\mathcal{H}_2\) Synthesis</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org6d1270a" class="outline-2">
|
|
<h2 id="org6d1270a"><span class="section-number-2">4</span> Robust Sensor Fusion: \(\mathcal{H}_\infty\) Synthesis</h2>
|
|
<div class="outline-text-2" id="text-4">
|
|
<p>
|
|
<a id="orgea66128"></a>
|
|
</p>
|
|
<p>
|
|
We initially considered perfectly known sensor dynamics so that it can be perfectly inverted.
|
|
</p>
|
|
|
|
<p>
|
|
We now take into account the fact that the sensor dynamics is only partially known.
|
|
To do so, we model the uncertainty that we have on the sensor dynamics by multiplicative input uncertainty as shown in Figure <a href="#org3a26ad3">15</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org3a26ad3" class="figure">
|
|
<p><img src="figs-paper/sensor_fusion_arch_uncertainty.png" alt="sensor_fusion_arch_uncertainty.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 15: </span>Sensor fusion architecture with sensor dynamics uncertainty</p>
|
|
</div>
|
|
|
|
<p>
|
|
As explained in Section <a href="#org3943238">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.
|
|
</p>
|
|
|
|
<p>
|
|
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}.
|
|
</p>
|
|
|
|
\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}
|
|
|
|
|
|
<p>
|
|
\(|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.
|
|
</p>
|
|
\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}
|
|
|
|
<p>
|
|
The choice of \(W_u\) is presented in Section <a href="#org5b00e7a">4.1</a>.
|
|
</p>
|
|
|
|
|
|
<p>
|
|
Condition \eqref{eq:upper_bound_uncertainty_bis} can almost be represented by \eqref{eq:hinf_norm_uncertainty} (within a factor \(\sqrt{2}\)).
|
|
</p>
|
|
\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}
|
|
|
|
|
|
|
|
<p>
|
|
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).
|
|
</p>
|
|
|
|
<p>
|
|
This is done using the \(\mathcal{H}_\infty\) synthesis in Section <a href="#org7bd80be">4.2</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgc5a0221" class="outline-3">
|
|
<h3 id="orgc5a0221"><span class="section-number-3">4.1</span> Weighting Function used to bound the super sensor uncertainty</h3>
|
|
<div class="outline-text-3" id="text-4-1">
|
|
<p>
|
|
<a id="org5b00e7a"></a>
|
|
</p>
|
|
|
|
<p>
|
|
\(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}.
|
|
</p>
|
|
|
|
\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}
|
|
|
|
<p>
|
|
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="#orge13d408">16</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Dphi = 10; <span class="org-comment">% [deg]</span>
|
|
|
|
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);
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="orge13d408" class="figure">
|
|
<p><img src="figs/weight_uncertainty_bounds_Wu.png" alt="weight_uncertainty_bounds_Wu.png" />
|
|
</p>
|
|
<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>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org334f826" class="outline-3">
|
|
<h3 id="org334f826"><span class="section-number-3">4.2</span> \(\mathcal{H}_\infty\) Synthesis</h3>
|
|
<div class="outline-text-3" id="text-4-2">
|
|
<p>
|
|
<a id="org7bd80be"></a>
|
|
</p>
|
|
|
|
<p>
|
|
The generalized plant \(P_{\mathcal{H}_\infty}\) used for the \(\mathcal{H}_\infty\) Synthesis of the complementary filters is shown in Figure <a href="#org461ce6a">17</a> and is described by Equation \eqref{eq:Hinf_generalized_plant}.
|
|
</p>
|
|
|
|
|
|
<div id="org461ce6a" class="figure">
|
|
<p><img src="figs-paper/h_infinity_robust_fusion.png" alt="h_infinity_robust_fusion.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 17: </span>Architecture used for \(\mathcal{H}_\infty\) synthesis of complementary filters</p>
|
|
</div>
|
|
|
|
\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}
|
|
|
|
<p>
|
|
The generalized plant is defined below.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<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;
|
|
1 0];
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
And the \(\mathcal{H}_\infty\) synthesis is performed using the <code>hinfsyn</code> command.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<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>);
|
|
</pre>
|
|
</div>
|
|
|
|
<pre class="example" id="orge32bb33">
|
|
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
|
|
</pre>
|
|
|
|
<p>
|
|
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)\).
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">H1 = 1 <span class="org-type">-</span> H2;
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The obtained complementary filters as well as the wanted upper bounds are shown in Figure <a href="#org938a541">18</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org938a541" class="figure">
|
|
<p><img src="figs/hinf_comp_filters.png" alt="hinf_comp_filters.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 18: </span>Obtained complementary filters using the \(\mathcal{H}_\infty\) Synthesis</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org211e432" class="outline-3">
|
|
<h3 id="org211e432"><span class="section-number-3">4.3</span> Super sensor uncertainty</h3>
|
|
<div class="outline-text-3" id="text-4-3">
|
|
<p>
|
|
The super sensor dynamical uncertainty is displayed in Figure <a href="#org52cdb78">19</a>.
|
|
It is confirmed that the super sensor dynamical uncertainty is less than the maximum allowed uncertainty defined by the norm of \(W_u(s)\).
|
|
</p>
|
|
|
|
<p>
|
|
The \(\mathcal{H}_\infty\) synthesis thus allows to design filters such that the super sensor has specified bounded uncertainty.
|
|
</p>
|
|
|
|
|
|
<div id="org52cdb78" class="figure">
|
|
<p><img src="figs/super_sensor_dynamical_uncertainty_Hinf.png" alt="super_sensor_dynamical_uncertainty_Hinf.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 19: </span>Super sensor dynamical uncertainty (solid curve) when using the \(\mathcal{H}_\infty\) Synthesis</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org7672e0e" class="outline-3">
|
|
<h3 id="org7672e0e"><span class="section-number-3">4.4</span> Super sensor noise</h3>
|
|
<div class="outline-text-3" id="text-4-4">
|
|
<p>
|
|
We now compute the obtain Power Spectral Density of the super sensor’s noise.
|
|
The Amplitude Spectral Densities are shown in Figure <a href="#org06b08df">20</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<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;
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The obtained RMS of the super sensor noise in the \(\mathcal{H}_2\) and \(\mathcal{H}_\infty\) case are shown in Table <a href="#org3cf92ce">4</a>.
|
|
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>
|
|
|
|
|
|
<div id="org06b08df" class="figure">
|
|
<p><img src="figs/psd_sensors_hinf_synthesis.png" alt="psd_sensors_hinf_synthesis.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 20: </span>Power Spectral Density of the estimated \(\hat{x}\) using the two sensors alone and using the \(\mathcal{H}_\infty\) synthesis</p>
|
|
</div>
|
|
|
|
<table id="org3cf92ce" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
|
|
<caption class="t-above"><span class="table-number">Table 4:</span> Comparison of the obtained RMS noise of the super sensor</caption>
|
|
|
|
<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>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgdcb47cf" class="outline-3">
|
|
<h3 id="orgdcb47cf"><span class="section-number-3">4.5</span> Conclusion</h3>
|
|
<div class="outline-text-3" id="text-4-5">
|
|
<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>
|
|
|
|
<div id="outline-container-org49a7cc2" class="outline-2">
|
|
<h2 id="org49a7cc2"><span class="section-number-2">5</span> Optimal and Robust Sensor Fusion: Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis</h2>
|
|
<div class="outline-text-2" id="text-5">
|
|
<p>
|
|
<a id="org2aeafb3"></a>
|
|
</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.
|
|
</p>
|
|
|
|
<p>
|
|
The sensor fusion architecture is shown in Figure <a href="#orgd633d12">21</a> (\(\hat{G}_i\) are omitted for space reasons).
|
|
</p>
|
|
|
|
|
|
<div id="orgd633d12" class="figure">
|
|
<p><img src="figs-paper/sensor_fusion_arch_full.png" alt="sensor_fusion_arch_full.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 21: </span>Sensor fusion architecture with sensor dynamics uncertainty</p>
|
|
</div>
|
|
|
|
<p>
|
|
The goal is to design complementary filters such that:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>the maximum uncertainty of the super sensor is bounded to acceptable values (defined by \(W_u(s)\))</li>
|
|
<li>the RMS value of the super sensor noise is minimized</li>
|
|
</ul>
|
|
|
|
<p>
|
|
To do so, we can use the Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis presented in Section <a href="#org669093b">5.1</a>.
|
|
</p>
|
|
</div>
|
|
<div id="outline-container-org7ed8ed7" class="outline-3">
|
|
<h3 id="org7ed8ed7"><span class="section-number-3">5.1</span> Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis</h3>
|
|
<div class="outline-text-3" id="text-5-1">
|
|
<p>
|
|
<a id="org669093b"></a>
|
|
</p>
|
|
|
|
<p>
|
|
The synthesis architecture that is used here is shown in Figure <a href="#orgc870005">22</a>.
|
|
</p>
|
|
|
|
<p>
|
|
The filter \(H_2(s)\) is synthesized such that it:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<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>
|
|
</ul>
|
|
|
|
|
|
<div id="orgc870005" class="figure">
|
|
<p><img src="figs-paper/mixed_h2_hinf_synthesis.png" alt="mixed_h2_hinf_synthesis.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 22: </span>Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis</p>
|
|
</div>
|
|
|
|
<p>
|
|
Let’s see that
|
|
with \(H_1(s)= 1 - H_2(s)\)
|
|
</p>
|
|
\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}
|
|
|
|
<p>
|
|
The generalized plant \(P_{\mathcal{H}_2/\mathcal{H}_\infty}\) is defined below
|
|
</p>
|
|
<div class="org-src-container">
|
|
<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>
|
|
|
|
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;
|
|
0 N2;
|
|
1 0];
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
And the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis is performed.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<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>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">H1 = 1 <span class="org-type">-</span> H2;
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The obtained complementary filters are shown in Figure <a href="#orgf37f308">23</a>.
|
|
</p>
|
|
|
|
|
|
<div id="orgf37f308" class="figure">
|
|
<p><img src="figs/htwo_hinf_comp_filters.png" alt="htwo_hinf_comp_filters.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 23: </span>Obtained complementary filters after mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgfb839cf" class="outline-3">
|
|
<h3 id="orgfb839cf"><span class="section-number-3">5.2</span> Obtained Super Sensor’s noise</h3>
|
|
<div class="outline-text-3" id="text-5-2">
|
|
<p>
|
|
The Amplitude Spectral Density of the super sensor’s noise is shown in Figure <a href="#orgb3c92e4">24</a>.
|
|
</p>
|
|
|
|
<p>
|
|
A time domain simulation is shown in Figure <a href="#org4a0db9b">25</a>.
|
|
</p>
|
|
|
|
<p>
|
|
The RMS values of the super sensor noise for the presented three synthesis are listed in Table <a href="#org3b10edf">5</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<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;
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="orgb3c92e4" class="figure">
|
|
<p><img src="figs/psd_sensors_htwo_hinf_synthesis.png" alt="psd_sensors_htwo_hinf_synthesis.png" />
|
|
</p>
|
|
<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>
|
|
</div>
|
|
|
|
|
|
<div id="org4a0db9b" class="figure">
|
|
<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 25: </span>Noise of individual sensors and noise of the super sensor</p>
|
|
</div>
|
|
|
|
<table id="org3b10edf" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
|
|
<caption class="t-above"><span class="table-number">Table 5:</span> Comparison of the obtained RMS noise of the super sensor</caption>
|
|
|
|
<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>
|
|
|
|
<tr>
|
|
<td class="org-left">Mixed: \(\mathcal{H}_2/\mathcal{H}_\infty\)</td>
|
|
<td class="org-right">0.0098</td>
|
|
</tr>
|
|
</tbody>
|
|
</table>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgb8596ac" class="outline-3">
|
|
<h3 id="orgb8596ac"><span class="section-number-3">5.3</span> Obtained Super Sensor’s Uncertainty</h3>
|
|
<div class="outline-text-3" id="text-5-3">
|
|
<p>
|
|
The uncertainty on the super sensor’s dynamics is shown in Figure <a href="#org11fe014">26</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org11fe014" class="figure">
|
|
<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 26: </span>Super sensor dynamical uncertainty (solid curve) when using the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgfcfa8a2" class="outline-3">
|
|
<h3 id="orgfcfa8a2"><span class="section-number-3">5.4</span> Conclusion</h3>
|
|
<div class="outline-text-3" id="text-5-4">
|
|
<p>
|
|
The mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis of the complementary filters allows to:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>limit the dynamical uncertainty of the super sensor</li>
|
|
<li>minimize the RMS value of the estimation</li>
|
|
</ul>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgf267700" class="outline-2">
|
|
<h2 id="orgf267700"><span class="section-number-2">6</span> Matlab Functions</h2>
|
|
<div class="outline-text-2" id="text-6">
|
|
<p>
|
|
<a id="org50a28ec"></a>
|
|
</p>
|
|
</div>
|
|
<div id="outline-container-org005db82" class="outline-3">
|
|
<h3 id="org005db82"><span class="section-number-3">6.1</span> <code>createWeight</code></h3>
|
|
<div class="outline-text-3" id="text-6-1">
|
|
<p>
|
|
<a id="orgb602691"></a>
|
|
</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"><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>
|
|
|
|
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
|
|
<span class="org-keyword">end</span>
|
|
|
|
mustBeBetween(args.G0, args.Gc, args.G1);
|
|
|
|
s = tf(<span class="org-string">'s'</span>);
|
|
|
|
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;
|
|
|
|
<span class="org-keyword">end</span>
|
|
|
|
<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>;
|
|
throwAsCaller(MException(eid,msg))
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org816b32a" class="outline-3">
|
|
<h3 id="org816b32a"><span class="section-number-3">6.2</span> <code>plotMagUncertainty</code></h3>
|
|
<div class="outline-text-3" id="text-6-2">
|
|
<p>
|
|
<a id="org2963b0d"></a>
|
|
</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"><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>
|
|
|
|
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, mustBeNonnegative} = 0.3
|
|
args.DisplayName char = <span class="org-string">''</span>
|
|
<span class="org-keyword">end</span>
|
|
|
|
<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>);
|
|
|
|
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);
|
|
|
|
p.FaceColor = colors(args.color_i, <span class="org-type">:</span>);
|
|
p.EdgeColor = <span class="org-string">'none'</span>;
|
|
p.FaceAlpha = args.opacity;
|
|
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgddb3773" class="outline-3">
|
|
<h3 id="orgddb3773"><span class="section-number-3">6.3</span> <code>plotPhaseUncertainty</code></h3>
|
|
<div class="outline-text-3" id="text-6-3">
|
|
<p>
|
|
<a id="org8d340da"></a>
|
|
</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"><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>
|
|
|
|
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 = <span class="org-string">''</span>
|
|
<span class="org-keyword">end</span>
|
|
|
|
<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>);
|
|
|
|
<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;
|
|
|
|
<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>)));
|
|
|
|
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);
|
|
|
|
p.FaceColor = colors(args.color_i, <span class="org-type">:</span>);
|
|
p.EdgeColor = <span class="org-string">'none'</span>;
|
|
p.FaceAlpha = args.opacity;
|
|
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<p>
|
|
|
|
<a href="ref.bib">ref.bib</a>
|
|
</p>
|
|
</div>
|
|
<div id="postamble" class="status">
|
|
<p class="author">Author: Thomas Dehaeze</p>
|
|
<p class="date">Created: 2020-11-12 jeu. 10:42</p>
|
|
</div>
|
|
</body>
|
|
</html>
|