1432 lines
49 KiB
HTML
1432 lines
49 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-10-02 ven. 17:57 -->
|
|
<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="../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>
|
|
<link rel="stylesheet" type="text/css" href="https://cdn.rawgit.com/dreampulse/computer-modern-web-font/master/fonts.css">
|
|
<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="#org0018cea">1. Sensor Description</a>
|
|
<ul>
|
|
<li><a href="#org0f599d3">1.1. Sensor Dynamics</a></li>
|
|
<li><a href="#org232e4d7">1.2. Sensor Model Uncertainty</a></li>
|
|
<li><a href="#orgbab1fa3">1.3. Sensor Noise</a></li>
|
|
<li><a href="#org3f66361">1.4. Save Model</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org0c3e200">2. Introduction to Sensor Fusion</a>
|
|
<ul>
|
|
<li><a href="#orgfef0d8a">2.1. Sensor Fusion Architecture</a></li>
|
|
<li><a href="#orgdafc2e2">2.2. Super Sensor Noise</a></li>
|
|
<li><a href="#org6971700">2.3. Super Sensor Dynamical Uncertainty</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org5d7f543">3. Optimal Super Sensor Noise: \(\mathcal{H}_2\) Synthesis</a>
|
|
<ul>
|
|
<li><a href="#orgec52a35">3.1. \(\mathcal{H}_2\) Synthesis</a></li>
|
|
<li><a href="#orgfb11c20">3.2. Super Sensor Noise</a></li>
|
|
<li><a href="#org0546999">3.3. Discrepancy between sensor dynamics and model</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org7a1a0b1">4. Robust Sensor Fusion: \(\mathcal{H}_\infty\) Synthesis</a>
|
|
<ul>
|
|
<li><a href="#org7e385ec">4.1. Weighting Function used to bound the super sensor uncertainty</a></li>
|
|
<li><a href="#org7512abd">4.2. \(\mathcal{H}_\infty\) Synthesis</a></li>
|
|
<li><a href="#orgb63a727">4.3. Super sensor uncertainty</a></li>
|
|
<li><a href="#org086db01">4.4. Super sensor noise</a></li>
|
|
<li><a href="#orgecc766f">4.5. Conclusion</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org9d3e5e6">5. Optimal and Robust Sensor Fusion: Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis</a>
|
|
<ul>
|
|
<li><a href="#orgd8a7f3d">5.1. Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis</a></li>
|
|
<li><a href="#orgad1f6d9">5.2. Obtained Super Sensor’s noise</a></li>
|
|
<li><a href="#org657b4af">5.3. Obtained Super Sensor’s Uncertainty</a></li>
|
|
<li><a href="#org64d866e">5.4. Comparison Hinf H2 H2/Hinf</a></li>
|
|
<li><a href="#org7631be4">5.5. Conclusion</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org8a6e087">6. Matlab Functions</a>
|
|
<ul>
|
|
<li><a href="#org81b52dd">6.1. <code>createWeight</code></a></li>
|
|
<li><a href="#org47da4db">6.2. <code>plotMagUncertainty</code></a></li>
|
|
<li><a href="#org4fba382">6.3. <code>plotPhaseUncertainty</code></a></li>
|
|
</ul>
|
|
</li>
|
|
</ul>
|
|
</div>
|
|
</div>
|
|
|
|
<p>
|
|
In this document, the optimal and robust design of complementary filters is studied.
|
|
</p>
|
|
|
|
<p>
|
|
Two sensors are considered with both different noise characteristics and dynamical uncertainties represented by multiplicative input uncertainty.
|
|
</p>
|
|
|
|
<ul class="org-ul">
|
|
<li>Section <a href="#orgd084707">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="#org47542ab">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="#org2ea99bd">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>
|
|
</ul>
|
|
|
|
<div id="outline-container-org0018cea" class="outline-2">
|
|
<h2 id="org0018cea"><span class="section-number-2">1</span> Sensor Description</h2>
|
|
<div class="outline-text-2" id="text-1">
|
|
<p>
|
|
<a id="org3f8bf11"></a>
|
|
</p>
|
|
<p>
|
|
In Figure <a href="#org8a977ea">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="orgae27e5d" 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="#org8a977ea">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>
|
|
</tbody>
|
|
</table>
|
|
|
|
<table id="org0a0caaa" 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="#org8a977ea">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="org8a977ea" class="figure">
|
|
<p><img src="figs-tikz/sensor_model_noise_uncertainty.png" alt="sensor_model_noise_uncertainty.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 1: </span>Sensor Model</p>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org0f599d3" class="outline-3">
|
|
<h3 id="org0f599d3"><span class="section-number-3">1.1</span> Sensor Dynamics</h3>
|
|
<div class="outline-text-3" id="text-1-1">
|
|
<p>
|
|
<a id="org19c1a42"></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; % Inertial Mass [kg]
|
|
c_acc = 5; % Damping [N/(m/s)]
|
|
k_acc = 1e5; % Stiffness [N/m]
|
|
g_acc = 1e5; % Gain [V/m]
|
|
|
|
G1 = g_acc*m_acc*s/(m_acc*s^2 + c_acc*s + k_acc); % Accelerometer Plant [V/(m/s)]
|
|
</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*pi*2e3; % Measurement Banwdith [rad/s]
|
|
g_pos = 1e4; % Gain [V/m]
|
|
|
|
G2 = g_pos/s/(1 + s/w_pos); % Position Sensor Plant [V/(m/s)]
|
|
</pre>
|
|
</div>
|
|
|
|
<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="#org6fbba77">1.2</a>.
|
|
</p>
|
|
|
|
<p>
|
|
Both sensor dynamics in \([\frac{V}{m/s}]\) are shown in Figure <a href="#org37e16e8">2</a>.
|
|
</p>
|
|
|
|
<div id="org37e16e8" 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-org232e4d7" class="outline-3">
|
|
<h3 id="org232e4d7"><span class="section-number-3">1.2</span> Sensor Model Uncertainty</h3>
|
|
<div class="outline-text-3" id="text-1-2">
|
|
<p>
|
|
<a id="org6fbba77"></a>
|
|
The uncertainty on the sensor dynamics is described by multiplicative uncertainty (Figure <a href="#org8a977ea">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="#org6cad6b3">3</a>.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">W1 = createWeight('n', 2, 'w0', 2*pi*3, 'G0', 2, 'G1', 0.1, 'Gc', 1) * ...
|
|
createWeight('n', 2, 'w0', 2*pi*1e3, 'G0', 1, 'G1', 4/0.1, 'Gc', 1/0.1);
|
|
|
|
W2 = createWeight('n', 2, 'w0', 2*pi*1e2, 'G0', 0.05, 'G1', 4, 'Gc', 1);
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The bode plot of the sensors nominal dynamics as well as their defined dynamical spread are shown in Figure <a href="#org09d22d8">4</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org6cad6b3" 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="org09d22d8" 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-orgbab1fa3" class="outline-3">
|
|
<h3 id="orgbab1fa3"><span class="section-number-3">1.3</span> Sensor Noise</h3>
|
|
<div class="outline-text-3" id="text-1-3">
|
|
<p>
|
|
<a id="orge523137"></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="#org8a977ea">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="#orgde91635">5</a>.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">omegac = 0.15*2*pi; G0 = 1e-1; Ginf = 1e-6;
|
|
N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4);
|
|
|
|
omegac = 1000*2*pi; G0 = 1e-6; Ginf = 1e-3;
|
|
N2 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4);
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="orgde91635" 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-org3f66361" class="outline-3">
|
|
<h3 id="org3f66361"><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('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org0c3e200" class="outline-2">
|
|
<h2 id="org0c3e200"><span class="section-number-2">2</span> Introduction to Sensor Fusion</h2>
|
|
<div class="outline-text-2" id="text-2">
|
|
<p>
|
|
<a id="orgb1af975"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgfef0d8a" class="outline-3">
|
|
<h3 id="orgfef0d8a"><span class="section-number-3">2.1</span> Sensor Fusion Architecture</h3>
|
|
<div class="outline-text-3" id="text-2-1">
|
|
<p>
|
|
<a id="orga489cea"></a>
|
|
</p>
|
|
|
|
<p>
|
|
The two sensors presented in Section <a href="#org3f8bf11">1</a> are now merged together using complementary filters \(H_1(s)\) and \(H_2(s)\) to form a super sensor (Figure <a href="#orga2e23af">6</a>).
|
|
</p>
|
|
|
|
|
|
<div id="orga2e23af" class="figure">
|
|
<p><img src="figs-tikz/sensor_fusion_noise_arch.png" alt="sensor_fusion_noise_arch.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 6: </span>Sensor Fusion Architecture</p>
|
|
</div>
|
|
|
|
<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-orgdafc2e2" class="outline-3">
|
|
<h3 id="orgdafc2e2"><span class="section-number-3">2.2</span> Super Sensor Noise</h3>
|
|
<div class="outline-text-3" id="text-2-2">
|
|
<p>
|
|
<a id="org1021e35"></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-org6971700" class="outline-3">
|
|
<h3 id="org6971700"><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="org99d8da1"></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="#orge09e9bb">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="orge09e9bb" class="figure">
|
|
<p><img src="figs-tikz/sensor_model_uncertainty.png" alt="sensor_model_uncertainty.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 7: </span>Sensor Model including Dynamical Uncertainty</p>
|
|
</div>
|
|
|
|
<p>
|
|
The uncertainty set of the transfer function from \(\hat{x}\) to \(x\) at frequency \(\omega\) is bounded in the complex plane by a circle centered on 1 and with a radius equal to \(|W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)|\) as shown in Figure <a href="#orgdf656f6">8</a>.
|
|
</p>
|
|
|
|
|
|
<div id="orgdf656f6" class="figure">
|
|
<p><img src="figs-tikz/uncertainty_set_super_sensor.png" alt="uncertainty_set_super_sensor.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 8: </span>Super Sensor model uncertainty displayed in the complex plane</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org5d7f543" class="outline-2">
|
|
<h2 id="org5d7f543"><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="orgd084707"></a>
|
|
</p>
|
|
<p>
|
|
In this section, the complementary filters \(H_1(s)\) and \(H_2(s)\) are designed in order to minimize the RMS value of super sensor noise \(\sigma_n\).
|
|
</p>
|
|
|
|
|
|
<div id="org5f7aea0" class="figure">
|
|
<p><img src="figs-tikz/sensor_fusion_noise_arch.png" alt="sensor_fusion_noise_arch.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 9: </span>Optimal Sensor Fusion Architecture</p>
|
|
</div>
|
|
|
|
<p>
|
|
The RMS value of the super sensor noise is (neglecting the model uncertainty):
|
|
</p>
|
|
\begin{equation}
|
|
\begin{aligned}
|
|
\sigma_{n} &= \sqrt{\int_0^\infty |H_1(j\omega) N_1(j\omega)|^2 + |H_2(j\omega) N_2(j\omega)|^2 d\omega} \\
|
|
&= \left\| \begin{matrix} H_1 N_1 \\ H_2 N_2 \end{matrix} \right\|_2
|
|
\end{aligned}
|
|
\end{equation}
|
|
|
|
<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="#orgb4f3263">3.1</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgec52a35" class="outline-3">
|
|
<h3 id="orgec52a35"><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="orgb4f3263"></a>
|
|
</p>
|
|
|
|
<p>
|
|
Consider the generalized plant \(P_{\mathcal{H}_2}\) shown in Figure <a href="#org56e5415">10</a> and described by Equation \eqref{eq:H2_generalized_plant}.
|
|
</p>
|
|
|
|
|
|
<div id="org56e5415" class="figure">
|
|
<p><img src="figs-tikz/h_two_optimal_fusion.png" alt="h_two_optimal_fusion.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 10: </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 -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, ~, 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 - H2;
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The obtained complementary filters are shown in Figure <a href="#org376b50b">11</a>.
|
|
</p>
|
|
|
|
<div id="org376b50b" class="figure">
|
|
<p><img src="figs/htwo_comp_filters.png" alt="htwo_comp_filters.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 11: </span>Obtained complementary filters using the \(\mathcal{H}_2\) Synthesis</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgfb11c20" class="outline-3">
|
|
<h3 id="orgfb11c20"><span class="section-number-3">3.2</span> Super Sensor Noise</h3>
|
|
<div class="outline-text-3" id="text-3-2">
|
|
<p>
|
|
<a id="orgcb8437f"></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 and shown in Figure <a href="#orgd23ad96">12</a>.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">PSD_S1 = abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2;
|
|
PSD_S2 = abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2;
|
|
PSD_H2 = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2 + ...
|
|
abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The corresponding Cumulative Power Spectrum \(\Gamma_{n_1}\), \(\Gamma_{n_2}\) and \(\Gamma_{n_{\mathcal{H}_2}}\) (cumulative integration of the PSD \eqref{eq:CPS_definition}) are computed below and shown in Figure <a href="#org0741aa0">13</a>.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">CPS_S1 = cumtrapz(freqs, PSD_S1);
|
|
CPS_S2 = cumtrapz(freqs, PSD_S2);
|
|
CPS_H2 = cumtrapz(freqs, PSD_H2);
|
|
</pre>
|
|
</div>
|
|
|
|
\begin{equation}
|
|
\Gamma_n (\omega) = \int_0^\omega \Phi_n(\nu) d\nu \label{eq:CPS_definition}
|
|
\end{equation}
|
|
|
|
<p>
|
|
The RMS value of the individual sensors and of the super sensor are listed in Table <a href="#org978281b">3</a>.
|
|
</p>
|
|
<table id="org978281b" 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="orgd23ad96" class="figure">
|
|
<p><img src="figs/psd_sensors_htwo_synthesis.png" alt="psd_sensors_htwo_synthesis.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 12: </span>Power Spectral Density of the estimated \(\hat{x}\) using the two sensors alone and using the optimally fused signal</p>
|
|
</div>
|
|
|
|
|
|
<div id="org0741aa0" class="figure">
|
|
<p><img src="figs/cps_h2_synthesis.png" alt="cps_h2_synthesis.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 13: </span>Cumulative Power Spectrum of individual sensors and super sensor using the \(\mathcal{H}_2\) synthesis</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="#org4f621fa">14</a>.
|
|
The resulting noises are displayed in Figure <a href="#orgf97c6e6">15</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org4f621fa" 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 14: </span>Noise of individual sensors and noise of the super sensor</p>
|
|
</div>
|
|
|
|
|
|
<div id="orgf97c6e6" 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 15: </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-org0546999" class="outline-3">
|
|
<h3 id="org0546999"><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="#org6fbba77">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="#org106a7e8">16</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="org106a7e8" class="figure">
|
|
<p><img src="figs/super_sensor_dynamical_uncertainty_H2.png" alt="super_sensor_dynamical_uncertainty_H2.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 16: </span>Super sensor dynamical uncertainty when using the \(\mathcal{H}_2\) Synthesis</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org7a1a0b1" class="outline-2">
|
|
<h2 id="org7a1a0b1"><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="org47542ab"></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="#org022607b">17</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org022607b" class="figure">
|
|
<p><img src="figs-tikz/sensor_fusion_arch_uncertainty.png" alt="sensor_fusion_arch_uncertainty.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 17: </span>Sensor fusion architecture with sensor dynamics uncertainty</p>
|
|
</div>
|
|
|
|
<p>
|
|
As explained in Section <a href="#org6fbba77">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="#org7a4e1fb">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="#orgd943222">4.2</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org7e385ec" class="outline-3">
|
|
<h3 id="org7e385ec"><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="org7a4e1fb"></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="#orgd4a62b9">18</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Dphi = 10; % [deg]
|
|
|
|
Wu = createWeight('n', 2, 'w0', 2*pi*4e2, 'G0', 1/sin(Dphi*pi/180), 'G1', 1/4, 'Gc', 1);
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="orgd4a62b9" class="figure">
|
|
<p><img src="figs/weight_uncertainty_bounds_Wu.png" alt="weight_uncertainty_bounds_Wu.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 18: </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-org7512abd" class="outline-3">
|
|
<h3 id="org7512abd"><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="orgd943222"></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="#org91113a2">19</a> and is described by Equation \eqref{eq:Hinf_generalized_plant}.
|
|
</p>
|
|
|
|
|
|
<div id="org91113a2" class="figure">
|
|
<p><img src="figs-tikz/h_infinity_robust_fusion.png" alt="h_infinity_robust_fusion.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 19: </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*W1 -Wu*W1;
|
|
0 Wu*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,'TOLGAM', 0.001, 'DISPLAY', 'on');
|
|
</pre>
|
|
</div>
|
|
|
|
<pre class="example">
|
|
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 - H2;
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The obtained complementary filters as well as the wanted upper bounds are shown in Figure <a href="#org9193e39">20</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org9193e39" class="figure">
|
|
<p><img src="figs/hinf_comp_filters.png" alt="hinf_comp_filters.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 20: </span>Obtained complementary filters using the \(\mathcal{H}_\infty\) Synthesis</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgb63a727" class="outline-3">
|
|
<h3 id="orgb63a727"><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="#org2e16d57">21</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 frequency band where the super sensor has small dynamical uncertainty is larger than for the individual sensor.
|
|
</p>
|
|
|
|
|
|
<div id="org2e16d57" 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 21: </span>Super sensor dynamical uncertainty (solid curve) when using the \(\mathcal{H}_\infty\) Synthesis</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org086db01" class="outline-3">
|
|
<h3 id="org086db01"><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 (Figure <a href="#org80c5802">22</a>).
|
|
</p>
|
|
|
|
<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="#org2672302">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 class="org-src-container">
|
|
<pre class="src src-matlab">PSD_S2 = abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2;
|
|
PSD_S1 = abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2;
|
|
PSD_Hinf = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2 + ...
|
|
abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="org80c5802" class="figure">
|
|
<p><img src="figs/psd_sensors_hinf_synthesis.png" alt="psd_sensors_hinf_synthesis.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 22: </span>Power Spectral Density of the estimated \(\hat{x}\) using the two sensors alone and using the</p>
|
|
</div>
|
|
|
|
<table id="org2672302" 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-orgecc766f" class="outline-3">
|
|
<h3 id="orgecc766f"><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-org9d3e5e6" class="outline-2">
|
|
<h2 id="org9d3e5e6"><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="org2ea99bd"></a>
|
|
</p>
|
|
|
|
<div id="org16dea35" class="figure">
|
|
<p><img src="figs-tikz/sensor_fusion_arch_full.png" alt="sensor_fusion_arch_full.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 23: </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</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.
|
|
</p>
|
|
|
|
<p>
|
|
The Matlab function for that is <code>h2hinfsyn</code> (<a href="https://fr.mathworks.com/help/robust/ref/h2hinfsyn.html">doc</a>).
|
|
</p>
|
|
</div>
|
|
<div id="outline-container-orgd8a7f3d" class="outline-3">
|
|
<h3 id="orgd8a7f3d"><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>
|
|
The synthesis architecture that is used here is shown in Figure <a href="#orgeb632fd">24</a>.
|
|
</p>
|
|
|
|
<p>
|
|
The controller \(K\) is synthesized such that it:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>Keeps the \(\mathcal{H}_\infty\) norm \(G\) of the transfer function from \(w\) to \(z_\infty\) bellow some specified value</li>
|
|
<li>Keeps the \(\mathcal{H}_2\) norm \(H\) of the transfer function from \(w\) to \(z_2\) bellow some specified value</li>
|
|
<li>Minimizes a trade-off criterion of the form \(W_1 G^2 + W_2 H^2\) where \(W_1\) and \(W_2\) are specified values</li>
|
|
</ul>
|
|
|
|
|
|
<div id="orgeb632fd" class="figure">
|
|
<p><img src="figs-tikz/mixed_h2_hinf_synthesis.png" alt="mixed_h2_hinf_synthesis.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 24: </span>Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis</p>
|
|
</div>
|
|
|
|
<p>
|
|
Here, we define \(P\) such that:
|
|
</p>
|
|
\begin{align*}
|
|
\left\| \frac{z_\infty}{w} \right\|_\infty &= \left\| \begin{matrix}W_1(s) H_1(s) \\ W_2(s) H_2(s)\end{matrix} \right\|_\infty \\
|
|
\left\| \frac{z_2}{w} \right\|_2 &= \left\| \begin{matrix}N_1(s) H_1(s) \\ N_2(s) H_2(s)\end{matrix} \right\|_2
|
|
\end{align*}
|
|
|
|
<p>
|
|
Then:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>we specify the maximum value for the \(\mathcal{H}_\infty\) norm between \(w\) and \(z_\infty\) to be \(1\)</li>
|
|
<li>we don’t specify any maximum value for the \(\mathcal{H}_2\) norm between \(w\) and \(z_2\)</li>
|
|
<li>we choose \(W_1 = 0\) and \(W_2 = 1\) such that the objective is to minimize the \(\mathcal{H}_2\) norm between \(w\) and \(z_2\)</li>
|
|
</ul>
|
|
|
|
<p>
|
|
The synthesis objective is to have:
|
|
\[ \left\| \frac{z_\infty}{w} \right\|_\infty = \left\| \begin{matrix}W_1(s) H_1(s) \\ W_2(s) H_2(s)\end{matrix} \right\|_\infty < 1 \]
|
|
and to minimize:
|
|
\[ \left\| \frac{z_2}{w} \right\|_2 = \left\| \begin{matrix}N_1(s) H_1(s) \\ N_2(s) H_2(s)\end{matrix} \right\|_2 \]
|
|
which is what we wanted.
|
|
</p>
|
|
|
|
<p>
|
|
We define the generalized plant that will be used for the mixed synthesis.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">W1u = ss(W2*Wu); W2u = ss(W1*Wu); % Weight on the uncertainty
|
|
W1n = ss(N2); W2n = ss(N1); % Weight on the noise
|
|
|
|
P = [W1u -W1u;
|
|
0 W2u;
|
|
W1n -W1n;
|
|
0 W2n;
|
|
1 0];
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis is performed below.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Nmeas = 1; Ncon = 1; Nz2 = 2;
|
|
|
|
[H1, ~, normz, ~] = h2hinfsyn(P, Nmeas, Ncon, Nz2, [0, 1], 'HINFMAX', 1, 'H2MAX', Inf, 'DKMAX', 100, 'TOL', 0.01, 'DISPLAY', 'on');
|
|
|
|
H2 = 1 - H1;
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The obtained complementary filters are shown in Figure <a href="#org8aa7261">25</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org8aa7261" class="figure">
|
|
<p><img src="figs/htwo_hinf_comp_filters.png" alt="htwo_hinf_comp_filters.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 25: </span>Obtained complementary filters after mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgad1f6d9" class="outline-3">
|
|
<h3 id="orgad1f6d9"><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 PSD and CPS of the super sensor’s noise are shown in Figure <a href="#org59049a9">26</a> and Figure <a href="#org2b132e1">27</a> respectively.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">PSD_S2 = abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2;
|
|
PSD_S1 = abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2;
|
|
PSD_H2Hinf = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
|
|
|
|
CPS_S2 = cumtrapz(freqs, PSD_S2);
|
|
CPS_S1 = cumtrapz(freqs, PSD_S1);
|
|
CPS_H2Hinf = cumtrapz(freqs, PSD_H2Hinf);
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="org59049a9" 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 26: </span>Power Spectral Density of the Super Sensor obtained with the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis</p>
|
|
</div>
|
|
|
|
|
|
<div id="org2b132e1" class="figure">
|
|
<p><img src="figs/cps_h2_hinf_synthesis.png" alt="cps_h2_hinf_synthesis.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 27: </span>Cumulative Power Spectrum of the Super Sensor obtained with the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org657b4af" class="outline-3">
|
|
<h3 id="org657b4af"><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
|
|
</p>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org64d866e" class="outline-3">
|
|
<h3 id="org64d866e"><span class="section-number-3">5.4</span> Comparison Hinf H2 H2/Hinf</h3>
|
|
<div class="outline-text-3" id="text-5-4">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">H2_filters = load('./mat/H2_filters.mat', 'H2', 'H1');
|
|
Hinf_filters = load('./mat/Hinf_filters.mat', 'H2', 'H1');
|
|
H2_Hinf_filters = load('./mat/H2_Hinf_filters.mat', 'H2', 'H1');
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">PSD_H2 = abs(squeeze(freqresp(N2*H2_filters.H2, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N1*H2_filters.H1, freqs, 'Hz'))).^2;
|
|
CPS_H2 = cumtrapz(freqs, PSD_H2);
|
|
|
|
PSD_Hinf = abs(squeeze(freqresp(N2*Hinf_filters.H2, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N1*Hinf_filters.H1, freqs, 'Hz'))).^2;
|
|
CPS_Hinf = cumtrapz(freqs, PSD_Hinf);
|
|
|
|
PSD_H2Hinf = abs(squeeze(freqresp(N2*H2_Hinf_filters.H2, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N1*H2_Hinf_filters.H1, freqs, 'Hz'))).^2;
|
|
CPS_H2Hinf = cumtrapz(freqs, PSD_H2Hinf);
|
|
</pre>
|
|
</div>
|
|
|
|
<table id="org0fadcb1" 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.0012</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left">Robust: \(\mathcal{H}_\infty\)</td>
|
|
<td class="org-right">0.041</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left">Mixed: \(\mathcal{H}_2/\mathcal{H}_\infty\)</td>
|
|
<td class="org-right">0.011</td>
|
|
</tr>
|
|
</tbody>
|
|
</table>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org7631be4" class="outline-3">
|
|
<h3 id="org7631be4"><span class="section-number-3">5.5</span> Conclusion</h3>
|
|
<div class="outline-text-3" id="text-5-5">
|
|
<p>
|
|
This synthesis methods allows both to:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>limit the dynamical uncertainty of the super sensor</li>
|
|
<li>minimize the RMS value of the estimation</li>
|
|
</ul>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org8a6e087" class="outline-2">
|
|
<h2 id="org8a6e087"><span class="section-number-2">6</span> Matlab Functions</h2>
|
|
<div class="outline-text-2" id="text-6">
|
|
<p>
|
|
<a id="orgde452bf"></a>
|
|
</p>
|
|
</div>
|
|
<div id="outline-container-org81b52dd" class="outline-3">
|
|
<h3 id="org81b52dd"><span class="section-number-3">6.1</span> <code>createWeight</code></h3>
|
|
<div class="outline-text-3" id="text-6-1">
|
|
<p>
|
|
<a id="org99b44b3"></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">function [W] = createWeight(args)
|
|
% createWeight -
|
|
%
|
|
% Syntax: [in_data] = createWeight(in_data)
|
|
%
|
|
% Inputs:
|
|
% - n - Weight Order
|
|
% - G0 - Low frequency Gain
|
|
% - G1 - High frequency Gain
|
|
% - Gc - Gain of W at frequency w0
|
|
% - w0 - Frequency at which |W(j w0)| = Gc
|
|
%
|
|
% Outputs:
|
|
% - W - Generated Weight
|
|
|
|
arguments
|
|
args.n (1,1) double {mustBeInteger, mustBePositive} = 1
|
|
args.G0 (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
|
args.G1 (1,1) double {mustBeNumeric, mustBePositive} = 10
|
|
args.Gc (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
args.w0 (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
end
|
|
|
|
mustBeBetween(args.G0, args.Gc, args.G1);
|
|
|
|
s = tf('s');
|
|
|
|
W = (((1/args.w0)*sqrt((1-(args.G0/args.Gc)^(2/args.n))/(1-(args.Gc/args.G1)^(2/args.n)))*s + (args.G0/args.Gc)^(1/args.n))/((1/args.G1)^(1/args.n)*(1/args.w0)*sqrt((1-(args.G0/args.Gc)^(2/args.n))/(1-(args.Gc/args.G1)^(2/args.n)))*s + (1/args.Gc)^(1/args.n)))^args.n;
|
|
|
|
end
|
|
|
|
% Custom validation function
|
|
function mustBeBetween(a,b,c)
|
|
if ~((a > b && b > c) || (c > b && b > a))
|
|
eid = 'createWeight:inputError';
|
|
msg = 'Gc should be between G0 and G1.';
|
|
throwAsCaller(MException(eid,msg))
|
|
end
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org47da4db" class="outline-3">
|
|
<h3 id="org47da4db"><span class="section-number-3">6.2</span> <code>plotMagUncertainty</code></h3>
|
|
<div class="outline-text-3" id="text-6-2">
|
|
<p>
|
|
<a id="org61a7308"></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">function [p] = plotMagUncertainty(W, freqs, args)
|
|
% plotMagUncertainty -
|
|
%
|
|
% Syntax: [p] = plotMagUncertainty(W, freqs, args)
|
|
%
|
|
% Inputs:
|
|
% - W - Multiplicative Uncertainty Weight
|
|
% - freqs - Frequency Vector [Hz]
|
|
% - args - Optional Arguments:
|
|
% - G
|
|
% - color_i
|
|
% - opacity
|
|
%
|
|
% Outputs:
|
|
% - p - Plot Handle
|
|
|
|
arguments
|
|
W
|
|
freqs double {mustBeNumeric, mustBeNonnegative}
|
|
args.G = tf(1)
|
|
args.color_i (1,1) double {mustBeInteger, mustBePositive} = 1
|
|
args.opacity (1,1) double {mustBeNumeric, mustBeNonnegative} = 0.3
|
|
args.DisplayName char = ''
|
|
end
|
|
|
|
% Get defaults colors
|
|
colors = get(groot, 'defaultAxesColorOrder');
|
|
|
|
p = patch([freqs flip(freqs)], ...
|
|
[abs(squeeze(freqresp(args.G, freqs, 'Hz'))).*(1 + abs(squeeze(freqresp(W, freqs, 'Hz')))); ...
|
|
flip(abs(squeeze(freqresp(args.G, freqs, 'Hz'))).*max(1 - abs(squeeze(freqresp(W, freqs, 'Hz'))), 1e-6))], 'w', ...
|
|
'DisplayName', args.DisplayName);
|
|
|
|
p.FaceColor = colors(args.color_i, :);
|
|
p.EdgeColor = 'none';
|
|
p.FaceAlpha = args.opacity;
|
|
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org4fba382" class="outline-3">
|
|
<h3 id="org4fba382"><span class="section-number-3">6.3</span> <code>plotPhaseUncertainty</code></h3>
|
|
<div class="outline-text-3" id="text-6-3">
|
|
<p>
|
|
<a id="orgce5e283"></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">function [p] = plotPhaseUncertainty(W, freqs, args)
|
|
% plotPhaseUncertainty -
|
|
%
|
|
% Syntax: [p] = plotPhaseUncertainty(W, freqs, args)
|
|
%
|
|
% Inputs:
|
|
% - W - Multiplicative Uncertainty Weight
|
|
% - freqs - Frequency Vector [Hz]
|
|
% - args - Optional Arguments:
|
|
% - G
|
|
% - color_i
|
|
% - opacity
|
|
%
|
|
% Outputs:
|
|
% - p - Plot Handle
|
|
|
|
arguments
|
|
W
|
|
freqs double {mustBeNumeric, mustBeNonnegative}
|
|
args.G = tf(1)
|
|
args.color_i (1,1) double {mustBeInteger, mustBePositive} = 1
|
|
args.opacity (1,1) double {mustBeNumeric, mustBePositive} = 0.3
|
|
args.DisplayName char = ''
|
|
end
|
|
|
|
% Get defaults colors
|
|
colors = get(groot, 'defaultAxesColorOrder');
|
|
|
|
% Compute Phase Uncertainty
|
|
Dphi = 180/pi*asin(abs(squeeze(freqresp(W, freqs, 'Hz'))));
|
|
Dphi(abs(squeeze(freqresp(W, freqs, 'Hz'))) > 1) = 360;
|
|
|
|
% Compute Plant Phase
|
|
G_ang = 180/pi*angle(squeeze(freqresp(args.G, freqs, 'Hz')));
|
|
|
|
p = patch([freqs flip(freqs)], [G_ang+Dphi; flip(G_ang-Dphi)], 'w', ...
|
|
'DisplayName', args.DisplayName);
|
|
|
|
p.FaceColor = colors(args.color_i, :);
|
|
p.EdgeColor = 'none';
|
|
p.FaceAlpha = args.opacity;
|
|
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<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-10-02 ven. 17:57</p>
|
|
</div>
|
|
</body>
|
|
</html>
|