1388 lines
48 KiB
HTML
1388 lines
48 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:35 -->
|
|
<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="#orgcc2172b">1. Sensor Description</a>
|
|
<ul>
|
|
<li><a href="#org23e78da">1.1. Sensor Dynamics</a></li>
|
|
<li><a href="#org94edbba">1.2. Sensor Model Uncertainty</a></li>
|
|
<li><a href="#org4cc404c">1.3. Sensor Noise</a></li>
|
|
<li><a href="#orgca9547c">1.4. Save Model</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org8fb094e">2. Introduction to Sensor Fusion</a>
|
|
<ul>
|
|
<li><a href="#org38e7087">2.1. Sensor Fusion Architecture</a></li>
|
|
<li><a href="#org3ba53c4">2.2. Super Sensor Noise</a></li>
|
|
<li><a href="#org0f97eb1">2.3. Super Sensor Dynamical Uncertainty</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgf2e7afd">3. Optimal Super Sensor Noise: \(\mathcal{H}_2\) Synthesis</a>
|
|
<ul>
|
|
<li><a href="#org3f9017b">3.1. \(\mathcal{H}_2\) Synthesis</a></li>
|
|
<li><a href="#orgb5bc159">3.2. Super Sensor Noise</a></li>
|
|
<li><a href="#orge0269cd">3.3. Discrepancy between sensor dynamics and model</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org869a9ab">4. Robust Sensor Fusion: \(\mathcal{H}_\infty\) Synthesis</a>
|
|
<ul>
|
|
<li><a href="#orgf20f681">4.1. Weighting Function used to bound the super sensor uncertainty</a></li>
|
|
<li><a href="#orge540fc3">4.2. \(\mathcal{H}_\infty\) Synthesis</a></li>
|
|
<li><a href="#org6d2d24e">4.3. Super sensor uncertainty</a></li>
|
|
<li><a href="#org8ce77ee">4.4. Super sensor noise</a></li>
|
|
<li><a href="#org8a94903">4.5. Conclusion</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org919d3da">5. Optimal and Robust Sensor Fusion: Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis</a>
|
|
<ul>
|
|
<li><a href="#org360cf70">5.1. Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis</a></li>
|
|
<li><a href="#org880e8a9">5.2. Obtained Super Sensor’s noise</a></li>
|
|
<li><a href="#orgb149f25">5.3. Obtained Super Sensor’s Uncertainty</a></li>
|
|
<li><a href="#orgfaf058b">5.4. Conclusion</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org2af7a78">6. Matlab Functions</a>
|
|
<ul>
|
|
<li><a href="#orgfe9741d">6.1. <code>createWeight</code></a></li>
|
|
<li><a href="#org6a88fe0">6.2. <code>plotMagUncertainty</code></a></li>
|
|
<li><a href="#orgbff6dca">6.3. <code>plotPhaseUncertainty</code></a></li>
|
|
</ul>
|
|
</li>
|
|
</ul>
|
|
</div>
|
|
</div>
|
|
|
|
<p>
|
|
This document is arranged as follows:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>Section <a href="#org2f5f470">1</a>: the sensors are described (dynamics, uncertainty, noise)</li>
|
|
<li>Section <a href="#org220320f">2</a>: the sensor fusion architecture is described and the super sensor noise and dynamical uncertainty are derived</li>
|
|
<li>Section <a href="#org0f63067">3</a>: the \(\mathcal{H}_2\) synthesis is used to design complementary filters such that the RMS value of the super sensor’s noise is minimized</li>
|
|
<li>Section <a href="#org25efa7e">4</a>: the \(\mathcal{H}_\infty\) synthesis is used to design complementary filters such that the super sensor’s uncertainty is bonded to acceptable values</li>
|
|
<li>Section <a href="#org5163fb1">5</a>: the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis is used to both limit the super sensor’s uncertainty and to lower the RMS value of the super sensor’s noise</li>
|
|
<li>Section <a href="#org71367a9">6</a>: Matlab functions used for the analysis are described</li>
|
|
</ul>
|
|
|
|
<div id="outline-container-orgcc2172b" class="outline-2">
|
|
<h2 id="orgcc2172b"><span class="section-number-2">1</span> Sensor Description</h2>
|
|
<div class="outline-text-2" id="text-1">
|
|
<p>
|
|
<a id="org2f5f470"></a>
|
|
</p>
|
|
<p>
|
|
In Figure <a href="#orgd19c6fc">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="orgc326093" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
|
|
<caption class="t-above"><span class="table-number">Table 1:</span> Description of signals in Figure <a href="#orgd19c6fc">1</a></caption>
|
|
|
|
<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="orgf877ead" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
|
|
<caption class="t-above"><span class="table-number">Table 2:</span> Description of Systems in Figure <a href="#orgd19c6fc">1</a></caption>
|
|
|
|
<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="orgd19c6fc" 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-org23e78da" class="outline-3">
|
|
<h3 id="org23e78da"><span class="section-number-3">1.1</span> Sensor Dynamics</h3>
|
|
<div class="outline-text-3" id="text-1-1">
|
|
<p>
|
|
<a id="org4813033"></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="#org992a983">1.2</a>.
|
|
</p>
|
|
|
|
<p>
|
|
Both sensor dynamics in \([\frac{V}{m/s}]\) are shown in Figure <a href="#org6409089">2</a>.
|
|
</p>
|
|
|
|
<div id="org6409089" 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-org94edbba" class="outline-3">
|
|
<h3 id="org94edbba"><span class="section-number-3">1.2</span> Sensor Model Uncertainty</h3>
|
|
<div class="outline-text-3" id="text-1-2">
|
|
<p>
|
|
<a id="org992a983"></a>
|
|
The uncertainty on the sensor dynamics is described by multiplicative uncertainty (Figure <a href="#orgd19c6fc">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="#org8beb676">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="#org751f977">4</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org8beb676" 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="org751f977" 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-org4cc404c" class="outline-3">
|
|
<h3 id="org4cc404c"><span class="section-number-3">1.3</span> Sensor Noise</h3>
|
|
<div class="outline-text-3" id="text-1-3">
|
|
<p>
|
|
<a id="org0cc1978"></a>
|
|
The noise of the sensors \(n_i\) are modelled by shaping a white noise with unitary PSD \(\tilde{n}_i\) \eqref{eq:unitary_noise_psd} with a LTI transfer function \(N_i(s)\) (Figure <a href="#orgd19c6fc">1</a>).
|
|
</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="#org2d39969">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="org2d39969" 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-orgca9547c" class="outline-3">
|
|
<h3 id="orgca9547c"><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-org8fb094e" class="outline-2">
|
|
<h2 id="org8fb094e"><span class="section-number-2">2</span> Introduction to Sensor Fusion</h2>
|
|
<div class="outline-text-2" id="text-2">
|
|
<p>
|
|
<a id="org220320f"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org38e7087" class="outline-3">
|
|
<h3 id="org38e7087"><span class="section-number-3">2.1</span> Sensor Fusion Architecture</h3>
|
|
<div class="outline-text-3" id="text-2-1">
|
|
<p>
|
|
<a id="org82e93f4"></a>
|
|
</p>
|
|
|
|
<p>
|
|
The two sensors presented in Section <a href="#org2f5f470">1</a> are now merged together using complementary filters \(H_1(s)\) and \(H_2(s)\) to form a super sensor (Figure <a href="#org8c9eb16">6</a>).
|
|
</p>
|
|
|
|
|
|
<div id="org8c9eb16" 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-org3ba53c4" class="outline-3">
|
|
<h3 id="org3ba53c4"><span class="section-number-3">2.2</span> Super Sensor Noise</h3>
|
|
<div class="outline-text-3" id="text-2-2">
|
|
<p>
|
|
<a id="orgd0b1104"></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-org0f97eb1" class="outline-3">
|
|
<h3 id="org0f97eb1"><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="orgb59d475"></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="#orgb46c073">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="orgb46c073" 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="#orgfd5d4b3">8</a>.
|
|
</p>
|
|
|
|
|
|
<div id="orgfd5d4b3" 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-orgf2e7afd" class="outline-2">
|
|
<h2 id="orgf2e7afd"><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="org0f63067"></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="orge6fcebb" 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="#orgc3c8ce2">3.1</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org3f9017b" class="outline-3">
|
|
<h3 id="org3f9017b"><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="orgc3c8ce2"></a>
|
|
</p>
|
|
|
|
<p>
|
|
Consider the generalized plant \(P_{\mathcal{H}_2}\) shown in Figure <a href="#orgfed5b00">10</a> and described by Equation \eqref{eq:H2_generalized_plant}.
|
|
</p>
|
|
|
|
|
|
<div id="orgfed5b00" 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="#org3561716">11</a>.
|
|
</p>
|
|
|
|
<div id="org3561716" 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-orgb5bc159" class="outline-3">
|
|
<h3 id="orgb5bc159"><span class="section-number-3">3.2</span> Super Sensor Noise</h3>
|
|
<div class="outline-text-3" id="text-3-2">
|
|
<p>
|
|
<a id="org22dd161"></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="#orgad2796b">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="#orgf505db2">3</a>.
|
|
</p>
|
|
<table id="orgf505db2" 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="orgad2796b" 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="#org50620bf">13</a>.
|
|
The resulting noises are displayed in Figure <a href="#orga4919f6">14</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org50620bf" 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="orga4919f6" 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-orge0269cd" class="outline-3">
|
|
<h3 id="orge0269cd"><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="#org992a983">1.2</a>, we can compute what would be the super sensor dynamical uncertainty when using the complementary filters obtained using the \(\mathcal{H}_2\) Synthesis.
|
|
</p>
|
|
|
|
<p>
|
|
The super sensor dynamical uncertainty is shown in Figure <a href="#org2fe77bd">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="org2fe77bd" 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-org869a9ab" class="outline-2">
|
|
<h2 id="org869a9ab"><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="org25efa7e"></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="#org9fe9ab2">16</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org9fe9ab2" 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="#org992a983">1.2</a>, at each frequency \(\omega\), the dynamical uncertainty of the super sensor can be represented in the complex plane by a circle with a radius equals to \(|H_1(j\omega) W_1(j\omega)| + |H_2(j\omega) W_2(j\omega)|\) and centered on 1.
|
|
</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="#org875b4b0">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="#org2b41098">4.2</a>.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-orgf20f681" class="outline-3">
|
|
<h3 id="orgf20f681"><span class="section-number-3">4.1</span> Weighting Function used to bound the super sensor uncertainty</h3>
|
|
<div class="outline-text-3" id="text-4-1">
|
|
<p>
|
|
<a id="org875b4b0"></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="#orgffb63ca">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="orgffb63ca" 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-orge540fc3" class="outline-3">
|
|
<h3 id="orge540fc3"><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="org2b41098"></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="#orgf4ccff4">18</a> and is described by Equation \eqref{eq:Hinf_generalized_plant}.
|
|
</p>
|
|
|
|
|
|
<div id="orgf4ccff4" 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="#org7a55110">19</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org7a55110" 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-org6d2d24e" class="outline-3">
|
|
<h3 id="org6d2d24e"><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="#org8ded02d">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="org8ded02d" 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-org8ce77ee" class="outline-3">
|
|
<h3 id="org8ce77ee"><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="#org3ff8b63">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="#org2b06274">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="org3ff8b63" 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="org2b06274" 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-org8a94903" class="outline-3">
|
|
<h3 id="org8a94903"><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-org919d3da" class="outline-2">
|
|
<h2 id="org919d3da"><span class="section-number-2">5</span> Optimal and Robust Sensor Fusion: Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis</h2>
|
|
<div class="outline-text-2" id="text-5">
|
|
<p>
|
|
<a id="org5163fb1"></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="#org427d030">22</a> (\(\hat{G}_i\) are omitted for space reasons).
|
|
</p>
|
|
|
|
|
|
<div id="org427d030" 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="#orgdf4b501">5.1</a>.
|
|
</p>
|
|
</div>
|
|
<div id="outline-container-org360cf70" class="outline-3">
|
|
<h3 id="org360cf70"><span class="section-number-3">5.1</span> Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis</h3>
|
|
<div class="outline-text-3" id="text-5-1">
|
|
<p>
|
|
<a id="orgdf4b501"></a>
|
|
</p>
|
|
|
|
<p>
|
|
The synthesis architecture that is used here is shown in Figure <a href="#org9fe5761">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="org9fe5761" 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', 1e-3, '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="#org9dd03be">24</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org9dd03be" 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-org880e8a9" class="outline-3">
|
|
<h3 id="org880e8a9"><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="#org41f2620">25</a>.
|
|
</p>
|
|
|
|
<p>
|
|
A time domain simulation is shown in Figure <a href="#orgd85ef6a">26</a>.
|
|
</p>
|
|
|
|
<p>
|
|
The RMS values of the super sensor noise for the presented three synthesis are listed in Table <a href="#org3b609de">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="org41f2620" 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="orgd85ef6a" 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="org3b609de" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
|
|
<caption class="t-above"><span class="table-number">Table 5:</span> Comparison of the obtained RMS noise of the super sensor</caption>
|
|
|
|
<colgroup>
|
|
<col class="org-left" />
|
|
|
|
<col class="org-right" />
|
|
</colgroup>
|
|
<thead>
|
|
<tr>
|
|
<th scope="col" class="org-left"> </th>
|
|
<th scope="col" class="org-right">RMS [m/s]</th>
|
|
</tr>
|
|
</thead>
|
|
<tbody>
|
|
<tr>
|
|
<td class="org-left">Optimal: \(\mathcal{H}_2\)</td>
|
|
<td class="org-right">0.0027</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left">Robust: \(\mathcal{H}_\infty\)</td>
|
|
<td class="org-right">0.041</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left">Mixed: \(\mathcal{H}_2/\mathcal{H}_\infty\)</td>
|
|
<td class="org-right">0.0098</td>
|
|
</tr>
|
|
</tbody>
|
|
</table>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgb149f25" class="outline-3">
|
|
<h3 id="orgb149f25"><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="#org37d0e3f">27</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org37d0e3f" 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-orgfaf058b" class="outline-3">
|
|
<h3 id="orgfaf058b"><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-org2af7a78" class="outline-2">
|
|
<h2 id="org2af7a78"><span class="section-number-2">6</span> Matlab Functions</h2>
|
|
<div class="outline-text-2" id="text-6">
|
|
<p>
|
|
<a id="org71367a9"></a>
|
|
</p>
|
|
</div>
|
|
<div id="outline-container-orgfe9741d" class="outline-3">
|
|
<h3 id="orgfe9741d"><span class="section-number-3">6.1</span> <code>createWeight</code></h3>
|
|
<div class="outline-text-3" id="text-6-1">
|
|
<p>
|
|
<a id="org38a3120"></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-org6a88fe0" class="outline-3">
|
|
<h3 id="org6a88fe0"><span class="section-number-3">6.2</span> <code>plotMagUncertainty</code></h3>
|
|
<div class="outline-text-3" id="text-6-2">
|
|
<p>
|
|
<a id="orgd3213c3"></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-orgbff6dca" class="outline-3">
|
|
<h3 id="orgbff6dca"><span class="section-number-3">6.3</span> <code>plotPhaseUncertainty</code></h3>
|
|
<div class="outline-text-3" id="text-6-3">
|
|
<p>
|
|
<a id="org5f660cc"></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:35</p>
|
|
</div>
|
|
</body>
|
|
</html>
|