tdehaeze/dehaeze20_optim_robus_compl_filte
1
0
Fork 0
Code Issues Pull Requests Releases Activity
6923496e4f
dehaeze20_optim_robus_compl.../matlab/index.html

1404 lines
58 KiB
HTML
Raw Blame History

<?xml version="1.0" encoding="utf-8"?>
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN"
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
<head>
<!-- 2020-10-05 lun. 15:08 -->
<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="#org0a492b7">1. Sensor Description</a>
<ul>
<li><a href="#org9575585">1.1. Sensor Dynamics</a></li>
<li><a href="#orgec8c81d">1.2. Sensor Model Uncertainty</a></li>
<li><a href="#org81d9a34">1.3. Sensor Noise</a></li>
<li><a href="#org4e23f6c">1.4. Save Model</a></li>
</ul>
</li>
<li><a href="#org2cab1a2">2. Introduction to Sensor Fusion</a>
<ul>
<li><a href="#org0cbc92d">2.1. Sensor Fusion Architecture</a></li>
<li><a href="#orge9e0bd4">2.2. Super Sensor Noise</a></li>
<li><a href="#orgefb4347">2.3. Super Sensor Dynamical Uncertainty</a></li>
</ul>
</li>
<li><a href="#org5896b60">3. Optimal Super Sensor Noise: \(\mathcal{H}_2\) Synthesis</a>
<ul>
<li><a href="#orgf5a8a84">3.1. \(\mathcal{H}_2\) Synthesis</a></li>
<li><a href="#org72159df">3.2. Super Sensor Noise</a></li>
<li><a href="#org8ffba19">3.3. Discrepancy between sensor dynamics and model</a></li>
</ul>
</li>
<li><a href="#org26ea7b1">4. Robust Sensor Fusion: \(\mathcal{H}_\infty\) Synthesis</a>
<ul>
<li><a href="#org8dfd9d2">4.1. Weighting Function used to bound the super sensor uncertainty</a></li>
<li><a href="#org7422ade">4.2. \(\mathcal{H}_\infty\) Synthesis</a></li>
<li><a href="#orga0267c6">4.3. Super sensor uncertainty</a></li>
<li><a href="#org979fede">4.4. Super sensor noise</a></li>
<li><a href="#orgda33992">4.5. Conclusion</a></li>
</ul>
</li>
<li><a href="#org15afe90">5. Optimal and Robust Sensor Fusion: Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis</a>
<ul>
<li><a href="#org0f81a91">5.1. Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis</a></li>
<li><a href="#org417aabd">5.2. Obtained Super Sensor&rsquo;s noise</a></li>
<li><a href="#org2dce888">5.3. Obtained Super Sensor&rsquo;s Uncertainty</a></li>
<li><a href="#org47da78c">5.4. Conclusion</a></li>
</ul>
</li>
<li><a href="#org0afe5ef">6. Matlab Functions</a>
<ul>
<li><a href="#orge81e522">6.1. <code>createWeight</code></a></li>
<li><a href="#org37ec2b4">6.2. <code>plotMagUncertainty</code></a></li>
<li><a href="#org9f73572">6.3. <code>plotPhaseUncertainty</code></a></li>
</ul>
</li>
</ul>
</div>
</div>
<p>
This document is arranged as follows:
</p>
<ul class="org-ul">
<li>Section <a href="#orgee25d07">1</a>: the sensors are described (dynamics, uncertainty, noise)</li>
<li>Section <a href="#orga64daad">2</a>: the sensor fusion architecture is described and the super sensor noise and dynamical uncertainty are derived</li>
<li>Section <a href="#orgdd6b9ce">3</a>: the \(\mathcal{H}_2\) synthesis is used to design complementary filters such that the RMS value of the super sensor&rsquo;s noise is minimized</li>
<li>Section <a href="#org5d93f37">4</a>: the \(\mathcal{H}_\infty\) synthesis is used to design complementary filters such that the super sensor&rsquo;s uncertainty is bonded to acceptable values</li>
<li>Section <a href="#org9f98c16">5</a>: the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis is used to both limit the super sensor&rsquo;s uncertainty and to lower the RMS value of the super sensor&rsquo;s noise</li>
<li>Section <a href="#orgf41dc8d">6</a>: Matlab functions used for the analysis are described</li>
</ul>
<div id="outline-container-org0a492b7" class="outline-2">
<h2 id="org0a492b7"><span class="section-number-2">1</span> Sensor Description</h2>
<div class="outline-text-2" id="text-1">
<p>
<a id="orgee25d07"></a>
</p>
<p>
In Figure <a href="#org35e2340">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="org2fe6194" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<caption class="t-above"><span class="table-number">Table 1:</span> Description of signals in Figure <a href="#org35e2340">1</a></caption>
<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">&#xa0;</td>
</tr>
<tr>
<td class="org-left">\(n_i\)</td>
<td class="org-left">Shaped noise</td>
<td class="org-left">\([m/s]\)</td>
</tr>
<tr>
<td class="org-left">\(v_i\)</td>
<td class="org-left">Sensor output measurement</td>
<td class="org-left">\([V]\)</td>
</tr>
<tr>
<td class="org-left">\(\hat{x}_i\)</td>
<td class="org-left">Estimate of \(x\) from the sensor</td>
<td class="org-left">\([m/s]\)</td>
</tr>
<tr>
<td class="org-left">\(\Phi_n(\omega)\)</td>
<td class="org-left">Power Spectral Density of \(n\)</td>
<td class="org-left">\([\frac{(m/s)^2}{Hz}]\)</td>
</tr>
<tr>
<td class="org-left">\(\phi_n(\omega)\)</td>
<td class="org-left">Amplitude Spectral Density of \(n\)</td>
<td class="org-left">\([\frac{m/s}{\sqrt{Hz}}]\)</td>
</tr>
<tr>
<td class="org-left">\(\sigma_n\)</td>
<td class="org-left">Root Mean Square Value of \(n\)</td>
<td class="org-left">\([m/s\ rms]\)</td>
</tr>
</tbody>
</table>
<table id="org281ecb3" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<caption class="t-above"><span class="table-number">Table 2:</span> Description of Systems in Figure <a href="#org35e2340">1</a></caption>
<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">&#xa0;</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">&#xa0;</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="org35e2340" class="figure">
<p><img src="figs-paper/sensor_model_noise_uncertainty.png" alt="sensor_model_noise_uncertainty.png" />
</p>
<p><span class="figure-number">Figure 1: </span>Sensor Model</p>
</div>
</div>
<div id="outline-container-org9575585" class="outline-3">
<h3 id="org9575585"><span class="section-number-3">1.1</span> Sensor Dynamics</h3>
<div class="outline-text-3" id="text-1-1">
<p>
<a id="org52415aa"></a>
Let&rsquo;s consider two sensors measuring the velocity of an object.
</p>
<p>
The first sensor is an accelerometer.
Its nominal dynamics \(\hat{G}_1(s)\) is defined below.
</p>
<div class="org-src-container">
<pre class="src src-matlab">m_acc = 0.01; <span class="org-comment">% Inertial Mass [kg]</span>
c_acc = 5; <span class="org-comment">% Damping [N/(m/s)]</span>
k_acc = 1e5; <span class="org-comment">% Stiffness [N/m]</span>
g_acc = 1e5; <span class="org-comment">% Gain [V/m]</span>
G1 = g_acc<span class="org-type">*</span>m_acc<span class="org-type">*</span>s<span class="org-type">/</span>(m_acc<span class="org-type">*</span>s<span class="org-type">^</span>2 <span class="org-type">+</span> c_acc<span class="org-type">*</span>s <span class="org-type">+</span> k_acc); <span class="org-comment">% Accelerometer Plant [V/(m/s)]</span>
</pre>
</div>
<p>
The second sensor is a displacement sensor, its nominal dynamics \(\hat{G}_2(s)\) is defined below.
</p>
<div class="org-src-container">
<pre class="src src-matlab">w_pos = 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>2e3; <span class="org-comment">% Measurement Banwdith [rad/s]</span>
g_pos = 1e4; <span class="org-comment">% Gain [V/m]</span>
G2 = g_pos<span class="org-type">/</span>s<span class="org-type">/</span>(1 <span class="org-type">+</span> s<span class="org-type">/</span>w_pos); <span class="org-comment">% Position Sensor Plant [V/(m/s)]</span>
</pre>
</div>
<p>
These nominal dynamics are also taken as the model of the sensor dynamics.
The true sensor dynamics has some uncertainty associated to it and described in section <a href="#orgf04439d">1.2</a>.
</p>
<p>
Both sensor dynamics in \([\frac{V}{m/s}]\) are shown in Figure <a href="#org2d1ef0b">2</a>.
</p>
<div id="org2d1ef0b" 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-orgec8c81d" class="outline-3">
<h3 id="orgec8c81d"><span class="section-number-3">1.2</span> Sensor Model Uncertainty</h3>
<div class="outline-text-3" id="text-1-2">
<p>
<a id="orgf04439d"></a>
The uncertainty on the sensor dynamics is described by multiplicative uncertainty (Figure <a href="#org35e2340">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="#org557e062">3</a>.
</p>
<div class="org-src-container">
<pre class="src src-matlab">W1 = createWeight(<span class="org-string">'n'</span>, 2, <span class="org-string">'w0'</span>, 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>3, <span class="org-string">'G0'</span>, 2, <span class="org-string">'G1'</span>, 0.1, <span class="org-string">'Gc'</span>, 1) <span class="org-type">*</span> ...
createWeight(<span class="org-string">'n'</span>, 2, <span class="org-string">'w0'</span>, 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>1e3, <span class="org-string">'G0'</span>, 1, <span class="org-string">'G1'</span>, 4<span class="org-type">/</span>0.1, <span class="org-string">'Gc'</span>, 1<span class="org-type">/</span>0.1);
W2 = createWeight(<span class="org-string">'n'</span>, 2, <span class="org-string">'w0'</span>, 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>1e2, <span class="org-string">'G0'</span>, 0.05, <span class="org-string">'G1'</span>, 4, <span class="org-string">'Gc'</span>, 1);
</pre>
</div>
<p>
The bode plot of the sensors nominal dynamics as well as their defined dynamical spread are shown in Figure <a href="#orgc218675">4</a>.
</p>
<div id="org557e062" 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="orgc218675" 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-org81d9a34" class="outline-3">
<h3 id="org81d9a34"><span class="section-number-3">1.3</span> Sensor Noise</h3>
<div class="outline-text-3" id="text-1-3">
<p>
<a id="org71b587e"></a>
The noise of the sensors \(n_i\) are modelled by shaping a white noise with unitary PSD \(\tilde{n}_i\) \eqref{eq:unitary_noise_psd} with a LTI transfer function \(N_i(s)\) (Figure <a href="#org35e2340">1</a>).
</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="#org51f9788">5</a>.
</p>
<div class="org-src-container">
<pre class="src src-matlab">omegac = 0.15<span class="org-type">*</span>2<span class="org-type">*</span><span class="org-constant">pi</span>; G0 = 1e<span class="org-type">-</span>1; Ginf = 1e<span class="org-type">-</span>6;
N1 = (Ginf<span class="org-type">*</span>s<span class="org-type">/</span>omegac <span class="org-type">+</span> G0)<span class="org-type">/</span>(s<span class="org-type">/</span>omegac <span class="org-type">+</span> 1)<span class="org-type">/</span>(1 <span class="org-type">+</span> s<span class="org-type">/</span>2<span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">/</span>1e4);
omegac = 1000<span class="org-type">*</span>2<span class="org-type">*</span><span class="org-constant">pi</span>; G0 = 1e<span class="org-type">-</span>6; Ginf = 1e<span class="org-type">-</span>3;
N2 = (Ginf<span class="org-type">*</span>s<span class="org-type">/</span>omegac <span class="org-type">+</span> G0)<span class="org-type">/</span>(s<span class="org-type">/</span>omegac <span class="org-type">+</span> 1)<span class="org-type">/</span>(1 <span class="org-type">+</span> s<span class="org-type">/</span>2<span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">/</span>1e4);
</pre>
</div>
<div id="org51f9788" 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-org4e23f6c" class="outline-3">
<h3 id="org4e23f6c"><span class="section-number-3">1.4</span> Save Model</h3>
<div class="outline-text-3" id="text-1-4">
<p>
All the dynamical systems representing the sensors are saved for further use.
</p>
<div class="org-src-container">
<pre class="src src-matlab">save(<span class="org-string">'./mat/model.mat'</span>, <span class="org-string">'freqs'</span>, <span class="org-string">'G1'</span>, <span class="org-string">'G2'</span>, <span class="org-string">'N2'</span>, <span class="org-string">'N1'</span>, <span class="org-string">'W2'</span>, <span class="org-string">'W1'</span>);
</pre>
</div>
</div>
</div>
</div>
<div id="outline-container-org2cab1a2" class="outline-2">
<h2 id="org2cab1a2"><span class="section-number-2">2</span> Introduction to Sensor Fusion</h2>
<div class="outline-text-2" id="text-2">
<p>
<a id="orga64daad"></a>
</p>
</div>
<div id="outline-container-org0cbc92d" class="outline-3">
<h3 id="org0cbc92d"><span class="section-number-3">2.1</span> Sensor Fusion Architecture</h3>
<div class="outline-text-3" id="text-2-1">
<p>
<a id="org31e00a0"></a>
</p>
<p>
The two sensors presented in Section <a href="#orgee25d07">1</a> are now merged together using complementary filters \(H_1(s)\) and \(H_2(s)\) to form a super sensor (Figure <a href="#org48a16fd">6</a>).
</p>
<div id="org48a16fd" class="figure">
<p><img src="figs-paper/sensor_fusion_noise_arch.png" alt="sensor_fusion_noise_arch.png" />
</p>
<p><span class="figure-number">Figure 6: </span>Sensor Fusion Architecture</p>
</div>
<p>
The complementary property of \(H_1(s)\) and \(H_2(s)\) means that the sum of their transfer function is equal to \(1\) \eqref{eq:complementary_property}.
</p>
\begin{equation}
H_1(s) + H_2(s) = 1 \label{eq:complementary_property}
\end{equation}
<p>
The super sensor estimate \(\hat{x}\) is given by \eqref{eq:super_sensor_estimate}.
</p>
\begin{equation}
\hat{x} = \left( H_1 \hat{G}_1^{-1} G_1 + H_2 \hat{G}_2^{-1} G_2 \right) x + \left( H_1 \hat{G}_1^{-1} G_1 N_1 \right) \tilde{n}_1 + \left( H_2 \hat{G}_2^{-1} G_2 N_2 \right) \tilde{n}_2 \label{eq:super_sensor_estimate}
\end{equation}
</div>
</div>
<div id="outline-container-orge9e0bd4" class="outline-3">
<h3 id="orge9e0bd4"><span class="section-number-3">2.2</span> Super Sensor Noise</h3>
<div class="outline-text-3" id="text-2-2">
<p>
<a id="orgff055a3"></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-orgefb4347" class="outline-3">
<h3 id="orgefb4347"><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="org859b213"></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="#org54538d7">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="org54538d7" class="figure">
<p><img src="figs-paper/sensor_model_uncertainty.png" alt="sensor_model_uncertainty.png" />
</p>
<p><span class="figure-number">Figure 7: </span>Sensor Model including Dynamical Uncertainty</p>
</div>
<p>
The uncertainty set of the transfer function from \(\hat{x}\) to \(x\) at frequency \(\omega\) is bounded in the complex plane by a circle centered on 1 and with a radius equal to \(|W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)|\) as shown in Figure <a href="#orgb11ef23">8</a>.
</p>
<div id="orgb11ef23" class="figure">
<p><img src="figs-paper/uncertainty_set_super_sensor.png" alt="uncertainty_set_super_sensor.png" />
</p>
<p><span class="figure-number">Figure 8: </span>Super Sensor model uncertainty displayed in the complex plane</p>
</div>
</div>
</div>
</div>
<div id="outline-container-org5896b60" class="outline-2">
<h2 id="org5896b60"><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="orgdd6b9ce"></a>
</p>
<p>
In this section, the complementary filters \(H_1(s)\) and \(H_2(s)\) are designed in order to minimize the RMS value of super sensor noise \(\sigma_n\).
</p>
<p>
The RMS value of the super sensor noise is (neglecting the model uncertainty):
</p>
\begin{equation}
\begin{aligned}
\sigma_{n} &= \sqrt{\int_0^\infty |H_1(j\omega) N_1(j\omega)|^2 + |H_2(j\omega) N_2(j\omega)|^2 d\omega} \\
&= \left\| \begin{matrix} H_1 N_1 \\ H_2 N_2 \end{matrix} \right\|_2
\end{aligned}
\end{equation}
<p>
The goal is to design \(H_1(s)\) and \(H_2(s)\) such that \(H_1(s) + H_2(s) = 1\) (complementary property) and such that \(\left\| \begin{matrix} H_1 N_1 \\ H_2 N_2 \end{matrix} \right\|_2\) is minimized (minimized RMS value of the super sensor noise).
This is done using the \(\mathcal{H}_2\) synthesis in Section <a href="#org5bc9386">3.1</a>.
</p>
</div>
<div id="outline-container-orgf5a8a84" class="outline-3">
<h3 id="orgf5a8a84"><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="org5bc9386"></a>
</p>
<p>
Consider the generalized plant \(P_{\mathcal{H}_2}\) shown in Figure <a href="#orgd218886">9</a> and described by Equation \eqref{eq:H2_generalized_plant}.
</p>
<div id="orgd218886" class="figure">
<p><img src="figs-paper/h_two_optimal_fusion.png" alt="h_two_optimal_fusion.png" />
</p>
<p><span class="figure-number">Figure 9: </span>Architecture used for \(\mathcal{H}_\infty\) synthesis of complementary filters</p>
</div>
\begin{equation} \label{eq:H2_generalized_plant}
\begin{pmatrix}
z_1 \\ z_2 \\ v
\end{pmatrix} = \underbrace{\begin{bmatrix}
N_1 & -N_1 \\
0 & N_2 \\
1 & 0
\end{bmatrix}}_{P_{\mathcal{H}_2}} \begin{pmatrix}
w \\ u
\end{pmatrix}
\end{equation}
<p>
Applying the \(\mathcal{H}_2\) synthesis on \(P_{\mathcal{H}_2}\) will generate a filter \(H_2(s)\) such that the \(\mathcal{H}_2\) norm from \(w\) to \((z_1,z_2)\) which is actually equals to \(\sigma_n\) by defining \(H_1(s) = 1 - H_2(s)\):
</p>
\begin{equation}
\left\| \begin{matrix} z_1/w \\ z_2/w \end{matrix} \right\|_2 = \left\| \begin{matrix} N_1 (1 - H_2) \\ N_2 H_2 \end{matrix} \right\|_2 = \sigma_n \quad \text{with} \quad H_1(s) = 1 - H_2(s)
\end{equation}
<p>
We then have that the \(\mathcal{H}_2\) synthesis applied on \(P_{\mathcal{H}_2}\) generates two complementary filters \(H_1(s)\) and \(H_2(s)\) such that the RMS value of super sensor noise is minimized.
</p>
<p>
The generalized plant \(P_{\mathcal{H}_2}\) is defined below
</p>
<div class="org-src-container">
<pre class="src src-matlab">PH2 = [N1 <span class="org-type">-</span>N1;
0 N2;
1 0];
</pre>
</div>
<p>
The \(\mathcal{H}_2\) synthesis using the <code>h2syn</code> command
</p>
<div class="org-src-container">
<pre class="src src-matlab">[H2, <span class="org-type">~</span>, gamma] = h2syn(PH2, 1, 1);
</pre>
</div>
<p>
Finally, \(H_1(s)\) is defined as follows
</p>
<div class="org-src-container">
<pre class="src src-matlab">H1 = 1 <span class="org-type">-</span> H2;
</pre>
</div>
<p>
The obtained complementary filters are shown in Figure <a href="#org8c7ba6b">10</a>.
</p>
<div id="org8c7ba6b" class="figure">
<p><img src="figs/htwo_comp_filters.png" alt="htwo_comp_filters.png" />
</p>
<p><span class="figure-number">Figure 10: </span>Obtained complementary filters using the \(\mathcal{H}_2\) Synthesis</p>
</div>
</div>
</div>
<div id="outline-container-org72159df" class="outline-3">
<h3 id="org72159df"><span class="section-number-3">3.2</span> Super Sensor Noise</h3>
<div class="outline-text-3" id="text-3-2">
<p>
<a id="orgc7cc0a8"></a>
</p>
<p>
The Power Spectral Density of the individual sensors&rsquo; noise \(\Phi_{n_1}, \Phi_{n_2}\) and of the super sensor noise \(\Phi_{n_{\mathcal{H}_2}}\) are computed below.
</p>
<div class="org-src-container">
<pre class="src src-matlab">PSD_S1 = abs(squeeze(freqresp(N1, freqs, <span class="org-string">'Hz'</span>)))<span class="org-type">.^</span>2;
PSD_S2 = abs(squeeze(freqresp(N2, freqs, <span class="org-string">'Hz'</span>)))<span class="org-type">.^</span>2;
PSD_H2 = abs(squeeze(freqresp(N1<span class="org-type">*</span>H1, freqs, <span class="org-string">'Hz'</span>)))<span class="org-type">.^</span>2 <span class="org-type">+</span> ...
abs(squeeze(freqresp(N2<span class="org-type">*</span>H2, freqs, <span class="org-string">'Hz'</span>)))<span class="org-type">.^</span>2;
</pre>
</div>
<p>
The obtained ASD are shown in Figure <a href="#orge481bcd">11</a>.
</p>
<p>
The RMS value of the individual sensors and of the super sensor are listed in Table <a href="#org3918d27">3</a>.
</p>
<table id="org3918d27" 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">&#xa0;</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="orge481bcd" class="figure">
<p><img src="figs/psd_sensors_htwo_synthesis.png" alt="psd_sensors_htwo_synthesis.png" />
</p>
<p><span class="figure-number">Figure 11: </span>Power Spectral Density of the estimated \(\hat{x}\) using the two sensors alone and using the optimally fused signal</p>
</div>
<p>
A time domain simulation is now performed.
The measured velocity \(x\) is set to be a sweep sine with an amplitude of \(0.1\ [m/s]\).
The velocity estimates from the two sensors and from the super sensors are shown in Figure <a href="#org9c2a7e4">12</a>.
The resulting noises are displayed in Figure <a href="#org4cc42bf">13</a>.
</p>
<div id="org9c2a7e4" class="figure">
<p><img src="figs/super_sensor_time_domain_h2.png" alt="super_sensor_time_domain_h2.png" />
</p>
<p><span class="figure-number">Figure 12: </span>Noise of individual sensors and noise of the super sensor</p>
</div>
<div id="org4cc42bf" class="figure">
<p><img src="figs/sensor_noise_H2_time_domain.png" alt="sensor_noise_H2_time_domain.png" />
</p>
<p><span class="figure-number">Figure 13: </span>Noise of the two sensors \(n_1, n_2\) and noise of the super sensor \(n\)</p>
</div>
</div>
</div>
<div id="outline-container-org8ffba19" class="outline-3">
<h3 id="org8ffba19"><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="#orgf04439d">1.2</a>, we can compute what would be the super sensor dynamical uncertainty when using the complementary filters obtained using the \(\mathcal{H}_2\) Synthesis.
</p>
<p>
The super sensor dynamical uncertainty is shown in Figure <a href="#org865879b">14</a>.
</p>
<p>
It is shown that the phase uncertainty is not bounded between 100Hz and 200Hz.
As a result the super sensor signal can not be used for feedback applications about 100Hz.
</p>
<div id="org865879b" class="figure">
<p><img src="figs/super_sensor_dynamical_uncertainty_H2.png" alt="super_sensor_dynamical_uncertainty_H2.png" />
</p>
<p><span class="figure-number">Figure 14: </span>Super sensor dynamical uncertainty when using the \(\mathcal{H}_2\) Synthesis</p>
</div>
</div>
</div>
</div>
<div id="outline-container-org26ea7b1" class="outline-2">
<h2 id="org26ea7b1"><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="org5d93f37"></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="#org2b71ca6">15</a>.
</p>
<div id="org2b71ca6" class="figure">
<p><img src="figs-paper/sensor_fusion_arch_uncertainty.png" alt="sensor_fusion_arch_uncertainty.png" />
</p>
<p><span class="figure-number">Figure 15: </span>Sensor fusion architecture with sensor dynamics uncertainty</p>
</div>
<p>
As explained in Section <a href="#orgf04439d">1.2</a>, at each frequency \(\omega\), the dynamical uncertainty of the super sensor can be represented in the complex plane by a circle with a radius equals to \(|H_1(j\omega) W_1(j\omega)| + |H_2(j\omega) W_2(j\omega)|\) and centered on 1.
</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="#orgefed264">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="#org2c990ce">4.2</a>.
</p>
</div>
<div id="outline-container-org8dfd9d2" class="outline-3">
<h3 id="org8dfd9d2"><span class="section-number-3">4.1</span> Weighting Function used to bound the super sensor uncertainty</h3>
<div class="outline-text-3" id="text-4-1">
<p>
<a id="orgefed264"></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="#orgdffee80">16</a>.
</p>
<div class="org-src-container">
<pre class="src src-matlab">Dphi = 10; <span class="org-comment">% [deg]</span>
Wu = createWeight(<span class="org-string">'n'</span>, 2, <span class="org-string">'w0'</span>, 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>4e2, <span class="org-string">'G0'</span>, 1<span class="org-type">/</span>sin(Dphi<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span>180), <span class="org-string">'G1'</span>, 1<span class="org-type">/</span>4, <span class="org-string">'Gc'</span>, 1);
</pre>
</div>
<div id="orgdffee80" class="figure">
<p><img src="figs/weight_uncertainty_bounds_Wu.png" alt="weight_uncertainty_bounds_Wu.png" />
</p>
<p><span class="figure-number">Figure 16: </span>Uncertainty region of the two sensors as well as the wanted maximum uncertainty of the super sensor (dashed lines)</p>
</div>
</div>
</div>
<div id="outline-container-org7422ade" class="outline-3">
<h3 id="org7422ade"><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="org2c990ce"></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="#org59777f5">17</a> and is described by Equation \eqref{eq:Hinf_generalized_plant}.
</p>
<div id="org59777f5" class="figure">
<p><img src="figs-paper/h_infinity_robust_fusion.png" alt="h_infinity_robust_fusion.png" />
</p>
<p><span class="figure-number">Figure 17: </span>Architecture used for \(\mathcal{H}_\infty\) synthesis of complementary filters</p>
</div>
\begin{equation} \label{eq:Hinf_generalized_plant}
\begin{pmatrix}
z_1 \\ z_2 \\ v
\end{pmatrix} = \underbrace{\begin{bmatrix}
W_u W_1 & -W_u W_1 \\
0 & W_u W_2 \\
1 & 0
\end{bmatrix}}_{P_{\mathcal{H}_\infty}} \begin{pmatrix}
w \\ u
\end{pmatrix}
\end{equation}
<p>
The generalized plant is defined below.
</p>
<div class="org-src-container">
<pre class="src src-matlab">P = [Wu<span class="org-type">*</span>W1 <span class="org-type">-</span>Wu<span class="org-type">*</span>W1;
0 Wu<span class="org-type">*</span>W2;
1 0];
</pre>
</div>
<p>
And the \(\mathcal{H}_\infty\) synthesis is performed using the <code>hinfsyn</code> command.
</p>
<div class="org-src-container">
<pre class="src src-matlab">H2 = hinfsyn(P, 1, 1,<span class="org-string">'TOLGAM'</span>, 0.001, <span class="org-string">'DISPLAY'</span>, <span class="org-string">'on'</span>);
</pre>
</div>
<pre class="example">
Test bounds: 0.7071 &lt;= gamma &lt;= 1.291
gamma X&gt;=0 Y&gt;=0 rho(XY)&lt;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 &ldquo;closed loop&rdquo; transfer function from \((w)\) to \((z_1,\ z_2)\) is less than one.
</p>
<p>
\(H_1(s)\) is then defined as the complementary of \(H_2(s)\).
</p>
<div class="org-src-container">
<pre class="src src-matlab">H1 = 1 <span class="org-type">-</span> H2;
</pre>
</div>
<p>
The obtained complementary filters as well as the wanted upper bounds are shown in Figure <a href="#orga1806e3">18</a>.
</p>
<div id="orga1806e3" class="figure">
<p><img src="figs/hinf_comp_filters.png" alt="hinf_comp_filters.png" />
</p>
<p><span class="figure-number">Figure 18: </span>Obtained complementary filters using the \(\mathcal{H}_\infty\) Synthesis</p>
</div>
</div>
</div>
<div id="outline-container-orga0267c6" class="outline-3">
<h3 id="orga0267c6"><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="#orge75f5ef">19</a>.
It is confirmed that the super sensor dynamical uncertainty is less than the maximum allowed uncertainty defined by the norm of \(W_u(s)\).
</p>
<p>
The \(\mathcal{H}_\infty\) synthesis thus allows to design filters such that the super sensor has specified bounded uncertainty.
</p>
<div id="orge75f5ef" class="figure">
<p><img src="figs/super_sensor_dynamical_uncertainty_Hinf.png" alt="super_sensor_dynamical_uncertainty_Hinf.png" />
</p>
<p><span class="figure-number">Figure 19: </span>Super sensor dynamical uncertainty (solid curve) when using the \(\mathcal{H}_\infty\) Synthesis</p>
</div>
</div>
</div>
<div id="outline-container-org979fede" class="outline-3">
<h3 id="org979fede"><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&rsquo;s noise.
The Amplitude Spectral Densities are shown in Figure <a href="#org5aac73f">20</a>.
</p>
<div class="org-src-container">
<pre class="src src-matlab">PSD_S2 = abs(squeeze(freqresp(N2, freqs, <span class="org-string">'Hz'</span>)))<span class="org-type">.^</span>2;
PSD_S1 = abs(squeeze(freqresp(N1, freqs, <span class="org-string">'Hz'</span>)))<span class="org-type">.^</span>2;
PSD_Hinf = abs(squeeze(freqresp(N1<span class="org-type">*</span>H1, freqs, <span class="org-string">'Hz'</span>)))<span class="org-type">.^</span>2 <span class="org-type">+</span> ...
abs(squeeze(freqresp(N2<span class="org-type">*</span>H2, freqs, <span class="org-string">'Hz'</span>)))<span class="org-type">.^</span>2;
</pre>
</div>
<p>
The obtained RMS of the super sensor noise in the \(\mathcal{H}_2\) and \(\mathcal{H}_\infty\) case are shown in Table <a href="#org3e08452">4</a>.
As expected, the super sensor obtained from the \(\mathcal{H}_\infty\) synthesis is much noisier than the super sensor obtained from the \(\mathcal{H}_2\) synthesis.
</p>
<div id="org5aac73f" class="figure">
<p><img src="figs/psd_sensors_hinf_synthesis.png" alt="psd_sensors_hinf_synthesis.png" />
</p>
<p><span class="figure-number">Figure 20: </span>Power Spectral Density of the estimated \(\hat{x}\) using the two sensors alone and using the</p>
</div>
<table id="org3e08452" 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">&#xa0;</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-orgda33992" class="outline-3">
<h3 id="orgda33992"><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-org15afe90" class="outline-2">
<h2 id="org15afe90"><span class="section-number-2">5</span> Optimal and Robust Sensor Fusion: Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis</h2>
<div class="outline-text-2" id="text-5">
<p>
<a id="org9f98c16"></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="#org3cc874e">21</a> (\(\hat{G}_i\) are omitted for space reasons).
</p>
<div id="org3cc874e" class="figure">
<p><img src="figs-paper/sensor_fusion_arch_full.png" alt="sensor_fusion_arch_full.png" />
</p>
<p><span class="figure-number">Figure 21: </span>Sensor fusion architecture with sensor dynamics uncertainty</p>
</div>
<p>
The goal is to design complementary filters such that:
</p>
<ul class="org-ul">
<li>the maximum uncertainty of the super sensor is bounded to acceptable values (defined by \(W_u(s)\))</li>
<li>the RMS value of the super sensor noise is minimized</li>
</ul>
<p>
To do so, we can use the Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis presented in Section <a href="#orgbbc8594">5.1</a>.
</p>
</div>
<div id="outline-container-org0f81a91" class="outline-3">
<h3 id="org0f81a91"><span class="section-number-3">5.1</span> Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis</h3>
<div class="outline-text-3" id="text-5-1">
<p>
<a id="orgbbc8594"></a>
</p>
<p>
The synthesis architecture that is used here is shown in Figure <a href="#orga971cdb">22</a>.
</p>
<p>
The filter \(H_2(s)\) is synthesized such that it:
</p>
<ul class="org-ul">
<li>keeps the \(\mathcal{H}_\infty\) norm of the transfer function from \(w\) to \(z_{\mathcal{H}_\infty}\) bellow some specified value</li>
<li>minimizes the \(\mathcal{H}_2\) norm of the transfer function from \(w\) to \(z_{\mathcal{H}_2}\)</li>
</ul>
<div id="orga971cdb" class="figure">
<p><img src="figs-paper/mixed_h2_hinf_synthesis.png" alt="mixed_h2_hinf_synthesis.png" />
</p>
<p><span class="figure-number">Figure 22: </span>Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis</p>
</div>
<p>
Let&rsquo;s see that
with \(H_1(s)= 1 - H_2(s)\)
</p>
\begin{align}
\left\| \frac{z_\infty}{w} \right\|_\infty &= \left\| \begin{matrix}H_1(s) W_1(s) W_u(s)\\ H_2(s) W_2(s) W_u(s)\end{matrix} \right\|_\infty \\
\left\| \frac{z_2}{w} \right\|_2 &= \left\| \begin{matrix}H_1(s) N_1(s) \\ H_2(s) N_2(s)\end{matrix} \right\|_2 = \sigma_n
\end{align}
<p>
The generalized plant \(P_{\mathcal{H}_2/\mathcal{H}_\infty}\) is defined below
</p>
<div class="org-src-container">
<pre class="src src-matlab">W1u = ss(W2<span class="org-type">*</span>Wu); W2u = ss(W1<span class="org-type">*</span>Wu); <span class="org-comment">% Weight on the uncertainty</span>
W1n = ss(N2); W2n = ss(N1); <span class="org-comment">% Weight on the noise</span>
P = [Wu<span class="org-type">*</span>W1 <span class="org-type">-</span>Wu<span class="org-type">*</span>W1;
0 Wu<span class="org-type">*</span>W2;
N1 <span class="org-type">-</span>N1;
0 N2;
1 0];
</pre>
</div>
<p>
And the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis is performed.
</p>
<div class="org-src-container">
<pre class="src src-matlab">[H2, <span class="org-type">~</span>] = h2hinfsyn(ss(P), 1, 1, 2, [0, 1], <span class="org-string">'HINFMAX'</span>, 1, <span class="org-string">'H2MAX'</span>, <span class="org-constant">Inf</span>, <span class="org-string">'DKMAX'</span>, 100, <span class="org-string">'TOL'</span>, 1e<span class="org-type">-</span>3, <span class="org-string">'DISPLAY'</span>, <span class="org-string">'on'</span>);
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab">H1 = 1 <span class="org-type">-</span> H2;
</pre>
</div>
<p>
The obtained complementary filters are shown in Figure <a href="#org30e8a3f">23</a>.
</p>
<div id="org30e8a3f" class="figure">
<p><img src="figs/htwo_hinf_comp_filters.png" alt="htwo_hinf_comp_filters.png" />
</p>
<p><span class="figure-number">Figure 23: </span>Obtained complementary filters after mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis</p>
</div>
</div>
</div>
<div id="outline-container-org417aabd" class="outline-3">
<h3 id="org417aabd"><span class="section-number-3">5.2</span> Obtained Super Sensor&rsquo;s noise</h3>
<div class="outline-text-3" id="text-5-2">
<p>
The Amplitude Spectral Density of the super sensor&rsquo;s noise is shown in Figure <a href="#orgfdfbf76">24</a>.
</p>
<p>
A time domain simulation is shown in Figure <a href="#orgdf5905f">25</a>.
</p>
<p>
The RMS values of the super sensor noise for the presented three synthesis are listed in Table <a href="#org3b7df55">5</a>.
</p>
<div class="org-src-container">
<pre class="src src-matlab">PSD_S2 = abs(squeeze(freqresp(N2, freqs, <span class="org-string">'Hz'</span>)))<span class="org-type">.^</span>2;
PSD_S1 = abs(squeeze(freqresp(N1, freqs, <span class="org-string">'Hz'</span>)))<span class="org-type">.^</span>2;
PSD_H2Hinf = abs(squeeze(freqresp(N1<span class="org-type">*</span>H1, freqs, <span class="org-string">'Hz'</span>)))<span class="org-type">.^</span>2 <span class="org-type">+</span> ...
abs(squeeze(freqresp(N2<span class="org-type">*</span>H2, freqs, <span class="org-string">'Hz'</span>)))<span class="org-type">.^</span>2;
</pre>
</div>
<div id="orgfdfbf76" class="figure">
<p><img src="figs/psd_sensors_htwo_hinf_synthesis.png" alt="psd_sensors_htwo_hinf_synthesis.png" />
</p>
<p><span class="figure-number">Figure 24: </span>Power Spectral Density of the Super Sensor obtained with the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis</p>
</div>
<div id="orgdf5905f" class="figure">
<p><img src="figs/super_sensor_time_domain_h2_hinf.png" alt="super_sensor_time_domain_h2_hinf.png" />
</p>
<p><span class="figure-number">Figure 25: </span>Noise of individual sensors and noise of the super sensor</p>
</div>
<table id="org3b7df55" 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">&#xa0;</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-org2dce888" class="outline-3">
<h3 id="org2dce888"><span class="section-number-3">5.3</span> Obtained Super Sensor&rsquo;s Uncertainty</h3>
<div class="outline-text-3" id="text-5-3">
<p>
The uncertainty on the super sensor&rsquo;s dynamics is shown in Figure <a href="#orgb2d28c5">26</a>.
</p>
<div id="orgb2d28c5" class="figure">
<p><img src="figs/super_sensor_dynamical_uncertainty_Htwo_Hinf.png" alt="super_sensor_dynamical_uncertainty_Htwo_Hinf.png" />
</p>
<p><span class="figure-number">Figure 26: </span>Super sensor dynamical uncertainty (solid curve) when using the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis</p>
</div>
</div>
</div>
<div id="outline-container-org47da78c" class="outline-3">
<h3 id="org47da78c"><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-org0afe5ef" class="outline-2">
<h2 id="org0afe5ef"><span class="section-number-2">6</span> Matlab Functions</h2>
<div class="outline-text-2" id="text-6">
<p>
<a id="orgf41dc8d"></a>
</p>
</div>
<div id="outline-container-orge81e522" class="outline-3">
<h3 id="orge81e522"><span class="section-number-3">6.1</span> <code>createWeight</code></h3>
<div class="outline-text-3" id="text-6-1">
<p>
<a id="org89bad6d"></a>
</p>
<p>
This Matlab function is accessible <a href="src/createWeight.m">here</a>.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[W]</span> = <span class="org-function-name">createWeight</span>(<span class="org-variable-name">args</span>)
<span class="org-comment">% createWeight -</span>
<span class="org-comment">%</span>
<span class="org-comment">% Syntax: [in_data] = createWeight(in_data)</span>
<span class="org-comment">%</span>
<span class="org-comment">% Inputs:</span>
<span class="org-comment">% - n - Weight Order</span>
<span class="org-comment">% - G0 - Low frequency Gain</span>
<span class="org-comment">% - G1 - High frequency Gain</span>
<span class="org-comment">% - Gc - Gain of W at frequency w0</span>
<span class="org-comment">% - w0 - Frequency at which |W(j w0)| = Gc</span>
<span class="org-comment">%</span>
<span class="org-comment">% Outputs:</span>
<span class="org-comment">% - W - Generated Weight</span>
arguments
args.n (1,1) double {mustBeInteger, mustBePositive} = 1
args.G0 (1,1) double {mustBeNumeric, mustBePositive} = 0.1
args.G1 (1,1) double {mustBeNumeric, mustBePositive} = 10
args.Gc (1,1) double {mustBeNumeric, mustBePositive} = 1
args.w0 (1,1) double {mustBeNumeric, mustBePositive} = 1
<span class="org-keyword">end</span>
mustBeBetween(args.G0, args.Gc, args.G1);
s = tf(<span class="org-string">'s'</span>);
W = (((1<span class="org-type">/</span>args.w0)<span class="org-type">*</span>sqrt((1<span class="org-type">-</span>(args.G0<span class="org-type">/</span>args.Gc)<span class="org-type">^</span>(2<span class="org-type">/</span>args.n))<span class="org-type">/</span>(1<span class="org-type">-</span>(args.Gc<span class="org-type">/</span>args.G1)<span class="org-type">^</span>(2<span class="org-type">/</span>args.n)))<span class="org-type">*</span>s <span class="org-type">+</span> (args.G0<span class="org-type">/</span>args.Gc)<span class="org-type">^</span>(1<span class="org-type">/</span>args.n))<span class="org-type">/</span>((1<span class="org-type">/</span>args.G1)<span class="org-type">^</span>(1<span class="org-type">/</span>args.n)<span class="org-type">*</span>(1<span class="org-type">/</span>args.w0)<span class="org-type">*</span>sqrt((1<span class="org-type">-</span>(args.G0<span class="org-type">/</span>args.Gc)<span class="org-type">^</span>(2<span class="org-type">/</span>args.n))<span class="org-type">/</span>(1<span class="org-type">-</span>(args.Gc<span class="org-type">/</span>args.G1)<span class="org-type">^</span>(2<span class="org-type">/</span>args.n)))<span class="org-type">*</span>s <span class="org-type">+</span> (1<span class="org-type">/</span>args.Gc)<span class="org-type">^</span>(1<span class="org-type">/</span>args.n)))<span class="org-type">^</span>args.n;
<span class="org-keyword">end</span>
<span class="org-comment">% Custom validation function</span>
<span class="org-keyword">function</span> <span class="org-function-name">mustBeBetween</span>(<span class="org-variable-name">a</span>,<span class="org-variable-name">b</span>,<span class="org-variable-name">c</span>)
<span class="org-keyword">if</span> <span class="org-type">~</span>((a <span class="org-type">&gt;</span> b <span class="org-type">&amp;&amp;</span> b <span class="org-type">&gt;</span> c) <span class="org-type">||</span> (c <span class="org-type">&gt;</span> b <span class="org-type">&amp;&amp;</span> b <span class="org-type">&gt;</span> a))
eid = <span class="org-string">'createWeight:inputError'</span>;
msg = <span class="org-string">'Gc should be between G0 and G1.'</span>;
throwAsCaller(MException(eid,msg))
<span class="org-keyword">end</span>
<span class="org-keyword">end</span>
</pre>
</div>
</div>
</div>
<div id="outline-container-org37ec2b4" class="outline-3">
<h3 id="org37ec2b4"><span class="section-number-3">6.2</span> <code>plotMagUncertainty</code></h3>
<div class="outline-text-3" id="text-6-2">
<p>
<a id="org8739875"></a>
</p>
<p>
This Matlab function is accessible <a href="src/plotMagUncertainty.m">here</a>.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[p]</span> = <span class="org-function-name">plotMagUncertainty</span>(<span class="org-variable-name">W</span>, <span class="org-variable-name">freqs</span>, <span class="org-variable-name">args</span>)
<span class="org-comment">% plotMagUncertainty -</span>
<span class="org-comment">%</span>
<span class="org-comment">% Syntax: [p] = plotMagUncertainty(W, freqs, args)</span>
<span class="org-comment">%</span>
<span class="org-comment">% Inputs:</span>
<span class="org-comment">% - W - Multiplicative Uncertainty Weight</span>
<span class="org-comment">% - freqs - Frequency Vector [Hz]</span>
<span class="org-comment">% - args - Optional Arguments:</span>
<span class="org-comment">% - G</span>
<span class="org-comment">% - color_i</span>
<span class="org-comment">% - opacity</span>
<span class="org-comment">%</span>
<span class="org-comment">% Outputs:</span>
<span class="org-comment">% - p - Plot Handle</span>
arguments
W
freqs double {mustBeNumeric, mustBeNonnegative}
args.G = tf(1)
args.color_i (1,1) double {mustBeInteger, mustBePositive} = 1
args.opacity (1,1) double {mustBeNumeric, mustBeNonnegative} = 0.3
args.DisplayName char = <span class="org-string">''</span>
<span class="org-keyword">end</span>
<span class="org-comment">% Get defaults colors</span>
colors = <span class="org-type">get</span>(<span class="org-variable-name">groot</span>, <span class="org-string">'defaultAxesColorOrder'</span>);
p = <span class="org-type">patch</span>([freqs flip(freqs)], ...
[abs(squeeze(freqresp(args.G, freqs, <span class="org-string">'Hz'</span>)))<span class="org-type">.*</span>(1 <span class="org-type">+</span> abs(squeeze(freqresp(W, freqs, <span class="org-string">'Hz'</span>)))); ...
flip(abs(squeeze(freqresp(args.G, freqs, <span class="org-string">'Hz'</span>)))<span class="org-type">.*</span>max(1 <span class="org-type">-</span> abs(squeeze(freqresp(W, freqs, <span class="org-string">'Hz'</span>))), 1e<span class="org-type">-</span>6))], <span class="org-string">'w'</span>, ...
<span class="org-string">'DisplayName'</span>, args.DisplayName);
p.FaceColor = colors(args.color_i, <span class="org-type">:</span>);
p.EdgeColor = <span class="org-string">'none'</span>;
p.FaceAlpha = args.opacity;
<span class="org-keyword">end</span>
</pre>
</div>
</div>
</div>
<div id="outline-container-org9f73572" class="outline-3">
<h3 id="org9f73572"><span class="section-number-3">6.3</span> <code>plotPhaseUncertainty</code></h3>
<div class="outline-text-3" id="text-6-3">
<p>
<a id="org49e21eb"></a>
</p>
<p>
This Matlab function is accessible <a href="src/plotPhaseUncertainty.m">here</a>.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[p]</span> = <span class="org-function-name">plotPhaseUncertainty</span>(<span class="org-variable-name">W</span>, <span class="org-variable-name">freqs</span>, <span class="org-variable-name">args</span>)
<span class="org-comment">% plotPhaseUncertainty -</span>
<span class="org-comment">%</span>
<span class="org-comment">% Syntax: [p] = plotPhaseUncertainty(W, freqs, args)</span>
<span class="org-comment">%</span>
<span class="org-comment">% Inputs:</span>
<span class="org-comment">% - W - Multiplicative Uncertainty Weight</span>
<span class="org-comment">% - freqs - Frequency Vector [Hz]</span>
<span class="org-comment">% - args - Optional Arguments:</span>
<span class="org-comment">% - G</span>
<span class="org-comment">% - color_i</span>
<span class="org-comment">% - opacity</span>
<span class="org-comment">%</span>
<span class="org-comment">% Outputs:</span>
<span class="org-comment">% - p - Plot Handle</span>
arguments
W
freqs double {mustBeNumeric, mustBeNonnegative}
args.G = tf(1)
args.color_i (1,1) double {mustBeInteger, mustBePositive} = 1
args.opacity (1,1) double {mustBeNumeric, mustBePositive} = 0.3
args.DisplayName char = <span class="org-string">''</span>
<span class="org-keyword">end</span>
<span class="org-comment">% Get defaults colors</span>
colors = <span class="org-type">get</span>(<span class="org-variable-name">groot</span>, <span class="org-string">'defaultAxesColorOrder'</span>);
<span class="org-comment">% Compute Phase Uncertainty</span>
Dphi = 180<span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">*</span>asin(abs(squeeze(freqresp(W, freqs, <span class="org-string">'Hz'</span>))));
Dphi(abs(squeeze(freqresp(W, freqs, <span class="org-string">'Hz'</span>))) <span class="org-type">&gt;</span> 1) = 360;
<span class="org-comment">% Compute Plant Phase</span>
G_ang = 180<span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">*</span>angle(squeeze(freqresp(args.G, freqs, <span class="org-string">'Hz'</span>)));
p = <span class="org-type">patch</span>([freqs flip(freqs)], [G_ang<span class="org-type">+</span>Dphi; flip(G_ang<span class="org-type">-</span>Dphi)], <span class="org-string">'w'</span>, ...
<span class="org-string">'DisplayName'</span>, args.DisplayName);
p.FaceColor = colors(args.color_i, <span class="org-type">:</span>);
p.EdgeColor = <span class="org-string">'none'</span>;
p.FaceAlpha = args.opacity;
<span class="org-keyword">end</span>
</pre>
</div>
</div>
</div>
</div>
<p>
<a href="ref.bib">ref.bib</a>
</p>
</div>
<div id="postamble" class="status">
<p class="author">Author: Thomas Dehaeze</p>
<p class="date">Created: 2020-10-05 lun. 15:08</p>
</div>
</body>
</html>
Reference in New Issue View Git Blame Copy Permalink