1390 lines
47 KiB
HTML
1390 lines
47 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. 18:25 -->
|
|
<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="#org42e0486">1. Sensor Description</a>
|
|
<ul>
|
|
<li><a href="#org647c690">1.1. Sensor Dynamics</a></li>
|
|
<li><a href="#orge2ef6d5">1.2. Sensor Model Uncertainty</a></li>
|
|
<li><a href="#org17664d8">1.3. Sensor Noise</a></li>
|
|
<li><a href="#org18c8385">1.4. Save Model</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org85fd6bc">2. Introduction to Sensor Fusion</a>
|
|
<ul>
|
|
<li><a href="#orgd9728f2">2.1. Sensor Fusion Architecture</a></li>
|
|
<li><a href="#org211028b">2.2. Super Sensor Noise</a></li>
|
|
<li><a href="#org1f329be">2.3. Super Sensor Dynamical Uncertainty</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org106ad63">3. Optimal Super Sensor Noise: \(\mathcal{H}_2\) Synthesis</a>
|
|
<ul>
|
|
<li><a href="#orgbbd13bd">3.1. \(\mathcal{H}_2\) Synthesis</a></li>
|
|
<li><a href="#org0d86fd6">3.2. Super Sensor Noise</a></li>
|
|
<li><a href="#org238d359">3.3. Discrepancy between sensor dynamics and model</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orge767c66">4. Robust Sensor Fusion: \(\mathcal{H}_\infty\) Synthesis</a>
|
|
<ul>
|
|
<li><a href="#orgdba13d6">4.1. Weighting Function used to bound the super sensor uncertainty</a></li>
|
|
<li><a href="#org440d265">4.2. \(\mathcal{H}_\infty\) Synthesis</a></li>
|
|
<li><a href="#org754b710">4.3. Super sensor uncertainty</a></li>
|
|
<li><a href="#org3c71ac3">4.4. Super sensor noise</a></li>
|
|
<li><a href="#org50d6031">4.5. Conclusion</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org6230c6a">5. Optimal and Robust Sensor Fusion: Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis</a>
|
|
<ul>
|
|
<li><a href="#org47226ff">5.1. Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis</a></li>
|
|
<li><a href="#org6ce5da3">5.2. Obtained Super Sensor’s noise</a></li>
|
|
<li><a href="#orgae2d47e">5.3. Obtained Super Sensor’s Uncertainty</a></li>
|
|
<li><a href="#orgd41a044">5.4. Conclusion</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgecfb74c">6. Matlab Functions</a>
|
|
<ul>
|
|
<li><a href="#orgbc9616b">6.1. <code>createWeight</code></a></li>
|
|
<li><a href="#org6933288">6.2. <code>plotMagUncertainty</code></a></li>
|
|
<li><a href="#org351f1ef">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="#orge8a8e6b">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="#org0e5d4db">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="#org310c590">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-org42e0486" class="outline-2">
|
|
<h2 id="org42e0486"><span class="section-number-2">1</span> Sensor Description</h2>
|
|
<div class="outline-text-2" id="text-1">
|
|
<p>
|
|
<a id="org7b36852"></a>
|
|
</p>
|
|
<p>
|
|
In Figure <a href="#org50592fc">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="orgbd57c1d" 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="#org50592fc">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="orgd5bc759" 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="#org50592fc">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="org50592fc" 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-org647c690" class="outline-3">
|
|
<h3 id="org647c690"><span class="section-number-3">1.1</span> Sensor Dynamics</h3>
|
|
<div class="outline-text-3" id="text-1-1">
|
|
<p>
|
|
<a id="org3d3853d"></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="#org3c9ef6e">1.2</a>.
|
|
</p>
|
|
|
|
<p>
|
|
Both sensor dynamics in \([\frac{V}{m/s}]\) are shown in Figure <a href="#orgdf43a57">2</a>.
|
|
</p>
|
|
|
|
<div id="orgdf43a57" 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-orge2ef6d5" class="outline-3">
|
|
<h3 id="orge2ef6d5"><span class="section-number-3">1.2</span> Sensor Model Uncertainty</h3>
|
|
<div class="outline-text-3" id="text-1-2">
|
|
<p>
|
|
<a id="org3c9ef6e"></a>
|
|
The uncertainty on the sensor dynamics is described by multiplicative uncertainty (Figure <a href="#org50592fc">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="#org5137ef5">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="#orgfec46ba">4</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org5137ef5" 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="orgfec46ba" 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-org17664d8" class="outline-3">
|
|
<h3 id="org17664d8"><span class="section-number-3">1.3</span> Sensor Noise</h3>
|
|
<div class="outline-text-3" id="text-1-3">
|
|
<p>
|
|
<a id="org9362cd8"></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="#org50592fc">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="#org953924d">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="org953924d" 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-org18c8385" class="outline-3">
|
|
<h3 id="org18c8385"><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-org85fd6bc" class="outline-2">
|
|
<h2 id="org85fd6bc"><span class="section-number-2">2</span> Introduction to Sensor Fusion</h2>
|
|
<div class="outline-text-2" id="text-2">
|
|
<p>
|
|
<a id="org39c4402"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgd9728f2" class="outline-3">
|
|
<h3 id="orgd9728f2"><span class="section-number-3">2.1</span> Sensor Fusion Architecture</h3>
|
|
<div class="outline-text-3" id="text-2-1">
|
|
<p>
|
|
<a id="orgc00e6f9"></a>
|
|
</p>
|
|
|
|
<p>
|
|
The two sensors presented in Section <a href="#org7b36852">1</a> are now merged together using complementary filters \(H_1(s)\) and \(H_2(s)\) to form a super sensor (Figure <a href="#org71ac5d2">6</a>).
|
|
</p>
|
|
|
|
|
|
<div id="org71ac5d2" 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-org211028b" class="outline-3">
|
|
<h3 id="org211028b"><span class="section-number-3">2.2</span> Super Sensor Noise</h3>
|
|
<div class="outline-text-3" id="text-2-2">
|
|
<p>
|
|
<a id="org2f8cec7"></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-org1f329be" class="outline-3">
|
|
<h3 id="org1f329be"><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="org0dafc5d"></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="#org3ef8213">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="org3ef8213" 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="#org27e67a3">8</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org27e67a3" 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-org106ad63" class="outline-2">
|
|
<h2 id="org106ad63"><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="orge8a8e6b"></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="org6587a06" 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="#org717549f">3.1</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgbbd13bd" class="outline-3">
|
|
<h3 id="orgbbd13bd"><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="org717549f"></a>
|
|
</p>
|
|
|
|
<p>
|
|
Consider the generalized plant \(P_{\mathcal{H}_2}\) shown in Figure <a href="#org6124aa7">10</a> and described by Equation \eqref{eq:H2_generalized_plant}.
|
|
</p>
|
|
|
|
|
|
<div id="org6124aa7" 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="#orgee5ee62">11</a>.
|
|
</p>
|
|
|
|
<div id="orgee5ee62" 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-org0d86fd6" class="outline-3">
|
|
<h3 id="org0d86fd6"><span class="section-number-3">3.2</span> Super Sensor Noise</h3>
|
|
<div class="outline-text-3" id="text-3-2">
|
|
<p>
|
|
<a id="org366e497"></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="#org98bc6c9">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 RMS value of the individual sensors and of the super sensor are listed in Table <a href="#org8db0d0e">3</a>.
|
|
</p>
|
|
<table id="org8db0d0e" 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="org98bc6c9" 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>
|
|
|
|
<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="#org1ecf54f">13</a>.
|
|
The resulting noises are displayed in Figure <a href="#org026504e">14</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org1ecf54f" 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 13: </span>Noise of individual sensors and noise of the super sensor</p>
|
|
</div>
|
|
|
|
|
|
<div id="org026504e" 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 14: </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-org238d359" class="outline-3">
|
|
<h3 id="org238d359"><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="#org3c9ef6e">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="#orga8596c4">15</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="orga8596c4" 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 15: </span>Super sensor dynamical uncertainty when using the \(\mathcal{H}_2\) Synthesis</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orge767c66" class="outline-2">
|
|
<h2 id="orge767c66"><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="org0e5d4db"></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="#org3abfec4">16</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org3abfec4" 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 16: </span>Sensor fusion architecture with sensor dynamics uncertainty</p>
|
|
</div>
|
|
|
|
<p>
|
|
As explained in Section <a href="#org3c9ef6e">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="#orgc44063d">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="#orga929244">4.2</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgdba13d6" class="outline-3">
|
|
<h3 id="orgdba13d6"><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="orgc44063d"></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="#org401a239">17</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="org401a239" class="figure">
|
|
<p><img src="figs/weight_uncertainty_bounds_Wu.png" alt="weight_uncertainty_bounds_Wu.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 17: </span>Uncertainty region of the two sensors as well as the wanted maximum uncertainty of the super sensor (dashed lines)</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org440d265" class="outline-3">
|
|
<h3 id="org440d265"><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="orga929244"></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="#org9810798">18</a> and is described by Equation \eqref{eq:Hinf_generalized_plant}.
|
|
</p>
|
|
|
|
|
|
<div id="org9810798" 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 18: </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="#orge56327e">19</a>.
|
|
</p>
|
|
|
|
|
|
<div id="orge56327e" class="figure">
|
|
<p><img src="figs/hinf_comp_filters.png" alt="hinf_comp_filters.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 19: </span>Obtained complementary filters using the \(\mathcal{H}_\infty\) Synthesis</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org754b710" class="outline-3">
|
|
<h3 id="org754b710"><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="#org86abf7c">20</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="org86abf7c" 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 20: </span>Super sensor dynamical uncertainty (solid curve) when using the \(\mathcal{H}_\infty\) Synthesis</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org3c71ac3" class="outline-3">
|
|
<h3 id="org3c71ac3"><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="#org0ee17aa">21</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="#org9031855">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="org0ee17aa" class="figure">
|
|
<p><img src="figs/psd_sensors_hinf_synthesis.png" alt="psd_sensors_hinf_synthesis.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 21: </span>Power Spectral Density of the estimated \(\hat{x}\) using the two sensors alone and using the</p>
|
|
</div>
|
|
|
|
<table id="org9031855" 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-org50d6031" class="outline-3">
|
|
<h3 id="org50d6031"><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-org6230c6a" class="outline-2">
|
|
<h2 id="org6230c6a"><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="org310c590"></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="#orgd6da96e">22</a> (\(\hat{G}_i\) are omitted for space reasons).
|
|
</p>
|
|
|
|
|
|
<div id="orgd6da96e" 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 22: </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="#orgac300ed">5.1</a>.
|
|
</p>
|
|
</div>
|
|
<div id="outline-container-org47226ff" class="outline-3">
|
|
<h3 id="org47226ff"><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="orgac300ed"></a>
|
|
</p>
|
|
|
|
<p>
|
|
The synthesis architecture that is used here is shown in Figure <a href="#orgd95faa8">23</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="orgd95faa8" 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 23: </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*Wu); W2u = ss(W1*Wu); % Weight on the uncertainty
|
|
W1n = ss(N2); W2n = ss(N1); % Weight on the noise
|
|
|
|
P = [Wu*W1 -Wu*W1;
|
|
0 Wu*W2;
|
|
N1 -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, ~] = h2hinfsyn(ss(P), 1, 1, 2, [0, 1], 'HINFMAX', 1, 'H2MAX', Inf, 'DKMAX', 100, 'TOL', 0.01, 'DISPLAY', 'on');
|
|
</pre>
|
|
</div>
|
|
|
|
<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="#org2f2ef8d">24</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org2f2ef8d" class="figure">
|
|
<p><img src="figs/htwo_hinf_comp_filters.png" alt="htwo_hinf_comp_filters.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 24: </span>Obtained complementary filters after mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org6ce5da3" class="outline-3">
|
|
<h3 id="org6ce5da3"><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 Power Spectral Density of the super sensor’s noise is shown in Figure <a href="#orgdef3c54">25</a>.
|
|
</p>
|
|
|
|
<p>
|
|
A time domain simulation is shown in Figure <a href="#org2b3cbc2">26</a>.
|
|
</p>
|
|
|
|
<p>
|
|
The RMS values of the super sensor noise for the presented three synthesis are listed in Table <a href="#orgbf21dd3">5</a>.
|
|
</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;
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="orgdef3c54" 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 25: </span>Power Spectral Density of the Super Sensor obtained with the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis</p>
|
|
</div>
|
|
|
|
|
|
<div id="org2b3cbc2" 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 26: </span>Noise of individual sensors and noise of the super sensor</p>
|
|
</div>
|
|
|
|
<table id="orgbf21dd3" 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.01</td>
|
|
</tr>
|
|
</tbody>
|
|
</table>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgae2d47e" class="outline-3">
|
|
<h3 id="orgae2d47e"><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="#org5d933bb">27</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org5d933bb" 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 27: </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-orgd41a044" class="outline-3">
|
|
<h3 id="orgd41a044"><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-orgecfb74c" class="outline-2">
|
|
<h2 id="orgecfb74c"><span class="section-number-2">6</span> Matlab Functions</h2>
|
|
<div class="outline-text-2" id="text-6">
|
|
<p>
|
|
<a id="orgd6c9208"></a>
|
|
</p>
|
|
</div>
|
|
<div id="outline-container-orgbc9616b" class="outline-3">
|
|
<h3 id="orgbc9616b"><span class="section-number-3">6.1</span> <code>createWeight</code></h3>
|
|
<div class="outline-text-3" id="text-6-1">
|
|
<p>
|
|
<a id="org81aea16"></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-org6933288" class="outline-3">
|
|
<h3 id="org6933288"><span class="section-number-3">6.2</span> <code>plotMagUncertainty</code></h3>
|
|
<div class="outline-text-3" id="text-6-2">
|
|
<p>
|
|
<a id="org82f6010"></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-org351f1ef" class="outline-3">
|
|
<h3 id="org351f1ef"><span class="section-number-3">6.3</span> <code>plotPhaseUncertainty</code></h3>
|
|
<div class="outline-text-3" id="text-6-3">
|
|
<p>
|
|
<a id="org07ed6ab"></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. 18:25</p>
|
|
</div>
|
|
</body>
|
|
</html>
|