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

1461 lines
50 KiB
HTML
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

<?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-01 jeu. 11:26 -->
<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>
<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="#org0562f5c">1. Sensor Description</a>
<ul>
<li><a href="#org3010d2c">1.1. Sensor Dynamics</a></li>
<li><a href="#orgdb953b8">1.2. Sensor Model Uncertainty</a></li>
<li><a href="#org10b2aca">1.3. Sensor Noise</a></li>
<li><a href="#org44ed4c1">1.4. Save Model</a></li>
</ul>
</li>
<li><a href="#org464859d">2. Optimal Super Sensor Noise: \(\mathcal{H}_2\) Synthesis with Acc and Pos</a>
<ul>
<li><a href="#org6401a01">2.1. H-Two Synthesis</a></li>
<li><a href="#org77a4cc8">2.2. Sensor Noise</a></li>
<li><a href="#orgb0eae43">2.3. Time Domain Simulation</a></li>
<li><a href="#org994795d">2.4. Discrepancy between sensor dynamics and model</a></li>
<li><a href="#orgdafc9ac">2.5. Conclusion</a></li>
</ul>
</li>
<li><a href="#org791e210">3. Robust Sensor Fusion: \(\mathcal{H}_\infty\) Synthesis with Acc and Pos</a>
<ul>
<li><a href="#orgf7a11f2">3.1. Super Sensor Dynamical Uncertainty</a></li>
<li><a href="#org1f6ea00">3.2. Synthesis objective</a></li>
<li><a href="#org7df844d">3.3. Requirements as an \(\mathcal{H}_\infty\) norm</a></li>
<li><a href="#org21cd96f">3.4. Weighting Function used to bound the super sensor uncertainty</a></li>
<li><a href="#org32acc0d">3.5. \(\mathcal{H}_\infty\) Synthesis</a></li>
<li><a href="#orgbf92cbe">3.6. Super sensor uncertainty</a></li>
<li><a href="#org23b62b8">3.7. Super sensor noise</a></li>
<li><a href="#org0cb0b10">3.8. Conclusion</a></li>
</ul>
</li>
<li><a href="#orge22cf08">4. Optimal and Robust Sensor Fusion: Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis with Acc and Pos</a>
<ul>
<li><a href="#org7981c46">4.1. Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis - Introduction</a></li>
<li><a href="#orga0b5528">4.2. Noise characteristics and Uncertainty of the individual sensors</a></li>
<li><a href="#org100bf37">4.3. Weighting Functions on the uncertainty of the super sensor</a></li>
<li><a href="#orgbe26d6f">4.4. Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis</a></li>
<li><a href="#org062c26e">4.5. Obtained Super Sensor&rsquo;s noise</a></li>
<li><a href="#org8a0bef2">4.6. Obtained Super Sensor&rsquo;s Uncertainty</a></li>
<li><a href="#orga1c1d8f">4.7. Comparison Hinf H2 H2/Hinf</a></li>
<li><a href="#orgc59f1bc">4.8. Conclusion</a></li>
</ul>
</li>
<li><a href="#org2e08794">5. Functions</a>
<ul>
<li><a href="#orge1c196d">5.1. <code>createWeight</code></a></li>
<li><a href="#org61ce738">5.2. <code>plotMagUncertainty</code></a></li>
<li><a href="#org6d139f2">5.3. <code>plotPhaseUncertainty</code></a></li>
</ul>
</li>
</ul>
</div>
</div>
<p>
In this document, the optimal and robust design of complementary filters is studied.
</p>
<p>
Two sensors are considered with both different noise characteristics and dynamical uncertainties represented by multiplicative input uncertainty.
</p>
<ul class="org-ul">
<li>Section <a href="#org8f89a2c">2</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="#org01199f2">3</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="#org8c8e334">4</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>
</ul>
<div id="outline-container-org0562f5c" class="outline-2">
<h2 id="org0562f5c"><span class="section-number-2">1</span> Sensor Description</h2>
<div class="outline-text-2" id="text-1">
<p>
In Figure <a href="#org5fea978">1</a> is shown a schematic of a sensor model that is used in the following study.
</p>
<table id="org9eb8245" 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="#org5fea978">1</a></caption>
<colgroup>
<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>
</tr>
</thead>
<tbody>
<tr>
<td class="org-left">\(x\)</td>
<td class="org-left">Physical measured quantity</td>
</tr>
<tr>
<td class="org-left">\(\tilde{n}_i\)</td>
<td class="org-left">White noise with unitary PSD</td>
</tr>
<tr>
<td class="org-left">\(n_i\)</td>
<td class="org-left">Shaped noise</td>
</tr>
<tr>
<td class="org-left">\(v_i\)</td>
<td class="org-left">Sensor output measurement</td>
</tr>
<tr>
<td class="org-left">\(\hat{x}_i\)</td>
<td class="org-left">Estimate of \(x\) from the sensor</td>
</tr>
</tbody>
</table>
<table id="orgc5cb667" 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="#org5fea978">1</a></caption>
<colgroup>
<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>
</tr>
</thead>
<tbody>
<tr>
<td class="org-left">\(\hat{G}_i\)</td>
<td class="org-left">Nominal Sensor Dynamics</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>
</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>
</tr>
<tr>
<td class="org-left">\(N_i\)</td>
<td class="org-left">Weight representing the sensor noise</td>
</tr>
</tbody>
</table>
<div id="org5fea978" 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>
<p>
In this example, the measured quantity \(x\) is the velocity of an object.
The units of signals are listed in Table <a href="#orge6f6a36">3</a>.
The units of systems are listed in Table <a href="#org93b2c85">4</a>.
</p>
<table id="orge6f6a36" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<caption class="t-above"><span class="table-number">Table 3:</span> Units of signals in Figure <a href="#org5fea978">1</a></caption>
<colgroup>
<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>Unit</b></th>
</tr>
</thead>
<tbody>
<tr>
<td class="org-left">\(x\)</td>
<td class="org-left">\([m/s]\)</td>
</tr>
<tr>
<td class="org-left">\(\tilde{n}_i\)</td>
<td class="org-left">&#xa0;</td>
</tr>
<tr>
<td class="org-left">\(n_i\)</td>
<td class="org-left">\([m/s]\)</td>
</tr>
<tr>
<td class="org-left">\(v_i\)</td>
<td class="org-left">\([V]\)</td>
</tr>
<tr>
<td class="org-left">\(\hat{x}_i\)</td>
<td class="org-left">\([m/s]\)</td>
</tr>
</tbody>
</table>
<table id="org93b2c85" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<caption class="t-above"><span class="table-number">Table 4:</span> Units of Systems in Figure <a href="#org5fea978">1</a></caption>
<colgroup>
<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>Unit</b></th>
</tr>
</thead>
<tbody>
<tr>
<td class="org-left">\(\hat{G}_i\)</td>
<td class="org-left">\([\frac{V}{m/s}]\)</td>
</tr>
<tr>
<td class="org-left">\(\hat{G}_i^{-1}\)</td>
<td class="org-left">\([\frac{m/s}{V}]\)</td>
</tr>
<tr>
<td class="org-left">\(W_i\)</td>
<td class="org-left">&#xa0;</td>
</tr>
<tr>
<td class="org-left">\(\Delta_i\)</td>
<td class="org-left">&#xa0;</td>
</tr>
<tr>
<td class="org-left">\(N_i\)</td>
<td class="org-left">\([m/s]\)</td>
</tr>
</tbody>
</table>
</div>
<div id="outline-container-org3010d2c" class="outline-3">
<h3 id="org3010d2c"><span class="section-number-3">1.1</span> Sensor Dynamics</h3>
<div class="outline-text-3" id="text-1-1">
<p>
<a id="orgf8cc485"></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; % 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="#orgc7ffc28">1.2</a>.
</p>
<p>
Both sensor dynamics in \([\frac{V}{m/s}]\) are shown in Figure <a href="#org118bb8c">2</a>.
</p>
<div id="org118bb8c" 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-orgdb953b8" class="outline-3">
<h3 id="orgdb953b8"><span class="section-number-3">1.2</span> Sensor Model Uncertainty</h3>
<div class="outline-text-3" id="text-1-2">
<p>
<a id="orgc7ffc28"></a>
The uncertainty on the sensor dynamics is described by multiplicative uncertainty (Figure <a href="#org5fea978">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="#org08e9455">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="#orgfcb1b0b">4</a>.
</p>
<div id="org08e9455" 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="orgfcb1b0b" 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-org10b2aca" class="outline-3">
<h3 id="org10b2aca"><span class="section-number-3">1.3</span> Sensor Noise</h3>
<div class="outline-text-3" id="text-1-3">
<p>
<a id="org4d9d0db"></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="#org5fea978">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="#orgdcd8034">5</a>.
</p>
<div class="org-src-container">
<pre class="src src-matlab">omegac = 0.05*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="orgdcd8034" 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-org44ed4c1" class="outline-3">
<h3 id="org44ed4c1"><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-org464859d" class="outline-2">
<h2 id="org464859d"><span class="section-number-2">2</span> Optimal Super Sensor Noise: \(\mathcal{H}_2\) Synthesis with Acc and Pos</h2>
<div class="outline-text-2" id="text-2">
<p>
<a id="org8f89a2c"></a>
</p>
<p>
The idea is to combine sensors that works in different frequency range using complementary filters.
</p>
<p>
Doing so, one &ldquo;super sensor&rdquo; is obtained that can have better noise characteristics than the individual sensors over a large frequency range.
</p>
<p>
The complementary filters have to be designed in order to minimize the effect noise of each sensor on the super sensor noise.
</p>
<div class="note">
<p>
The Matlab scripts is accessible <a href="matlab/optimal_comp_filters.m">here</a>.
</p>
</div>
</div>
<div id="outline-container-org6401a01" class="outline-3">
<h3 id="org6401a01"><span class="section-number-3">2.1</span> H-Two Synthesis</h3>
<div class="outline-text-3" id="text-2-1">
<p>
As \(\tilde{n}_1\) and \(\tilde{n}_2\) are normalized white noise: \(\Phi_{\tilde{n}_1}(\omega) = \Phi_{\tilde{n}_2}(\omega) = 1\) and we have:
\[ \sigma_{\hat{x}} = \sqrt{\int_0^\infty |H_1 N_1|^2(\omega) + |H_2 N_2|^2(\omega) d\omega} = \left\| \begin{matrix} H_1 N_1 \\ H_2 N_2 \end{matrix} \right\|_2 \]
Thus, the goal is to design \(H_1(s)\) and \(H_2(s)\) such that \(H_1(s) + H_2(s) = 1\) and such that \(\left\| \begin{matrix} H_1 N_1 \\ H_2 N_2 \end{matrix} \right\|_2\) is minimized.
</p>
<p>
For that, we use the \(\mathcal{H}_2\) Synthesis.
</p>
<p>
We use the generalized plant architecture shown on figure <a href="#org3651394">6</a>.
</p>
<div id="org3651394" class="figure">
<p><img src="figs-tikz/h_infinity_optimal_comp_filters.png" alt="h_infinity_optimal_comp_filters.png" />
</p>
<p><span class="figure-number">Figure 6: </span>\(\mathcal{H}_2\) Synthesis - Generalized plant used for the optimal generation of complementary filters</p>
</div>
\begin{equation*}
\begin{pmatrix}
z \\ v
\end{pmatrix} = \begin{pmatrix}
0 & N_2 & 1 \\
N_1 & -N_2 & 0
\end{pmatrix} \begin{pmatrix}
W_1 \\ W_2 \\ u
\end{pmatrix}
\end{equation*}
<p>
The transfer function from \([n_1, n_2]\) to \(\hat{x}\) is:
\[ \begin{bmatrix} N_1 H_1 \\ N_2 (1 - H_1) \end{bmatrix} \]
If we define \(H_2 = 1 - H_1\), we obtain:
\[ \begin{bmatrix} N_1 H_1 \\ N_2 H_2 \end{bmatrix} \]
</p>
<p>
Thus, if we minimize the \(\mathcal{H}_2\) norm of this transfer function, we minimize the RMS value of \(\hat{x}\).
</p>
<p>
We define the generalized plant \(P\) on matlab as shown on figure <a href="#org3651394">6</a>.
</p>
<div class="org-src-container">
<pre class="src src-matlab">P = [N1 -N1;
0 N2;
1 0];
</pre>
</div>
<p>
And we do 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(P, 1, 1);
</pre>
</div>
<p>
Finally, we define \(H_2(s) = 1 - H_1(s)\).
</p>
<div class="org-src-container">
<pre class="src src-matlab">H1 = 1 - H2;
</pre>
</div>
<p>
The complementary filters obtained are shown on figure <a href="#orga1435ed">7</a>.
</p>
<div id="orga1435ed" class="figure">
<p><img src="figs/htwo_comp_filters.png" alt="htwo_comp_filters.png" />
</p>
<p><span class="figure-number">Figure 7: </span>Obtained complementary filters using the \(\mathcal{H}_2\) Synthesis (<a href="./figs/htwo_comp_filters.png">png</a>, <a href="./figs/htwo_comp_filters.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-org77a4cc8" class="outline-3">
<h3 id="org77a4cc8"><span class="section-number-3">2.2</span> Sensor Noise</h3>
<div class="outline-text-3" id="text-2-2">
<p>
The PSD of the noise of the individual sensor and of the super sensor are shown in Fig. <a href="#org942deb9">8</a>.
</p>
<p>
The Cumulative Power Spectrum (CPS) is shown on Fig. <a href="#org5e74fc0">9</a>.
</p>
<p>
The obtained RMS value of the super sensor is lower than the RMS value of the individual sensors.
</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;
CPS_S1 = cumtrapz(freqs, PSD_S1);
CPS_S2 = cumtrapz(freqs, PSD_S2);
CPS_H2 = cumtrapz(freqs, PSD_H2);
</pre>
</div>
<div id="org942deb9" class="figure">
<p><img src="figs/psd_sensors_htwo_synthesis.png" alt="psd_sensors_htwo_synthesis.png" />
</p>
<p><span class="figure-number">Figure 8: </span>Power Spectral Density of the estimated \(\hat{x}\) using the two sensors alone and using the optimally fused signal (<a href="./figs/psd_sensors_htwo_synthesis.png">png</a>, <a href="./figs/psd_sensors_htwo_synthesis.pdf">pdf</a>)</p>
</div>
<div id="org5e74fc0" class="figure">
<p><img src="figs/cps_h2_synthesis.png" alt="cps_h2_synthesis.png" />
</p>
<p><span class="figure-number">Figure 9: </span>Cumulative Power Spectrum of individual sensors and super sensor using the \(\mathcal{H}_2\) synthesis (<a href="./figs/cps_h2_synthesis.png">png</a>, <a href="./figs/cps_h2_synthesis.pdf">pdf</a>)</p>
</div>
<table border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<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">Integrated Acceleration</td>
<td class="org-right">0.005</td>
</tr>
<tr>
<td class="org-left">Derived Position</td>
<td class="org-right">0.08</td>
</tr>
<tr>
<td class="org-left">Super Sensor - \(\mathcal{H}_2\)</td>
<td class="org-right">0.0012</td>
</tr>
</tbody>
</table>
</div>
</div>
<div id="outline-container-orgb0eae43" class="outline-3">
<h3 id="orgb0eae43"><span class="section-number-3">2.3</span> Time Domain Simulation</h3>
<div class="outline-text-3" id="text-2-3">
<p>
Parameters of the time domain simulation.
</p>
<div class="org-src-container">
<pre class="src src-matlab">Fs = 1e4; % Sampling Frequency [Hz]
Ts = 1/Fs; % Sampling Time [s]
t = 0:Ts:2; % Time Vector [s]
</pre>
</div>
<p>
Time domain velocity.
</p>
<div class="org-src-container">
<pre class="src src-matlab">v = 0.1*sin((10*t).*t)';
</pre>
</div>
<p>
Generate noises in velocity corresponding to sensor 1 and 2:
</p>
<div class="org-src-container">
<pre class="src src-matlab">n1 = lsim(N1, sqrt(Fs/2)*randn(length(t), 1), t);
n2 = lsim(N2, sqrt(Fs/2)*randn(length(t), 1), t);
</pre>
</div>
<div id="orgcb48597" 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 10: </span>Noise of individual sensors and noise of the super sensor</p>
</div>
</div>
</div>
<div id="outline-container-org994795d" class="outline-3">
<h3 id="org994795d"><span class="section-number-3">2.4</span> Discrepancy between sensor dynamics and model</h3>
</div>
<div id="outline-container-orgdafc9ac" class="outline-3">
<h3 id="orgdafc9ac"><span class="section-number-3">2.5</span> Conclusion</h3>
<div class="outline-text-3" id="text-2-5">
<p>
From the above complementary filter design with the \(\mathcal{H}_2\) and \(\mathcal{H}_\infty\) synthesis, it still seems that the \(\mathcal{H}_2\) synthesis gives the complementary filters that permits to obtain the minimal super sensor noise (when measuring with the \(\mathcal{H}_2\) norm).
</p>
<p>
However, the synthesis does not take into account the robustness of the sensor fusion.
</p>
</div>
</div>
</div>
<div id="outline-container-org791e210" class="outline-2">
<h2 id="org791e210"><span class="section-number-2">3</span> Robust Sensor Fusion: \(\mathcal{H}_\infty\) Synthesis with Acc and Pos</h2>
<div class="outline-text-2" id="text-3">
<p>
<a id="org01199f2"></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 Fig. <a href="#orgd93c41e">11</a>.
</p>
<div id="orgd93c41e" class="figure">
<p><img src="figs-tikz/sensor_fusion_dynamic_uncertainty.png" alt="sensor_fusion_dynamic_uncertainty.png" />
</p>
<p><span class="figure-number">Figure 11: </span>Sensor fusion architecture with sensor dynamics uncertainty</p>
</div>
<p>
The objective here is to design complementary filters \(H_1(s)\) and \(H_2(s)\) in order to minimize the dynamical uncertainty of the super sensor.
</p>
<div class="note">
<p>
The Matlab scripts is accessible <a href="matlab/comp_filter_robustness.m">here</a>.
</p>
</div>
</div>
<div id="outline-container-orgf7a11f2" class="outline-3">
<h3 id="orgf7a11f2"><span class="section-number-3">3.1</span> Super Sensor Dynamical Uncertainty</h3>
<div class="outline-text-3" id="text-3-1">
<p>
In practical systems, the sensor dynamics has always some level of uncertainty.
Let&rsquo;s represent that with multiplicative input uncertainty as shown on figure <a href="#orgd93c41e">11</a>.
</p>
<div id="org2e8c1c1" class="figure">
<p><img src="figs-tikz/sensor_fusion_dynamic_uncertainty.png" alt="sensor_fusion_dynamic_uncertainty.png" />
</p>
<p><span class="figure-number">Figure 12: </span>Fusion of two sensors with input multiplicative uncertainty</p>
</div>
<p>
The dynamics of the super sensor is represented by
</p>
\begin{align*}
\frac{\hat{x}}{x} &= (1 + W_1 \Delta_1) H_1 + (1 + W_2 \Delta_2) H_2 \\
&= 1 + W_1 H_1 \Delta_1 + W_2 H_2 \Delta_2
\end{align*}
<p>
with \(\Delta_i\) is any transfer function satisfying \(\| \Delta_i \|_\infty < 1\).
</p>
<p>
We see that as soon as we have some uncertainty in the sensor dynamics, we have that the complementary filters have some effect on the transfer function from \(x\) to \(\hat{x}\).
</p>
<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)|\) (figure <a href="#org5992e00">13</a>).
</p>
<p>
We then have that the angle introduced by the super sensor is bounded by \(\arcsin(\epsilon)\):
\[ \angle \frac{\hat{x}}{x}(j\omega) \le \arcsin \Big(|W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)|\Big) \]
</p>
<div id="org5992e00" class="figure">
<p><img src="figs-tikz/uncertainty_gain_phase_variation.png" alt="uncertainty_gain_phase_variation.png" />
</p>
<p><span class="figure-number">Figure 13: </span>Maximum phase variation</p>
</div>
</div>
</div>
<div id="outline-container-org1f6ea00" class="outline-3">
<h3 id="org1f6ea00"><span class="section-number-3">3.2</span> Synthesis objective</h3>
<div class="outline-text-3" id="text-3-2">
<p>
The uncertainty region of the super sensor dynamics is represented by a circle in the complex plane as shown in Fig. <a href="#org5992e00">13</a>.
</p>
<p>
At each frequency \(\omega\), the radius of the circle is \(|W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)|\).
</p>
<p>
Thus, the phase shift \(\Delta\phi(\omega)\) due to the super sensor uncertainty is bounded by:
\[ |\Delta\phi(\omega)| \leq \arcsin\big( |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| \big) \]
</p>
<p>
Let&rsquo;s define some allowed frequency depend phase shift \(\Delta\phi_\text{max}(\omega) > 0\) such that:
\[ |\Delta\phi(\omega)| < \Delta\phi_\text{max}(\omega), \quad \forall\omega \]
</p>
<p>
If \(H_1(s)\) and \(H_2(s)\) are designed such that
\[ |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| < \sin\big( \Delta\phi_\text{max}(\omega) \big) \]
</p>
<p>
The maximum phase shift due to dynamic uncertainty at frequency \(\omega\) will be \(\Delta\phi_\text{max}(\omega)\).
</p>
</div>
</div>
<div id="outline-container-org7df844d" class="outline-3">
<h3 id="org7df844d"><span class="section-number-3">3.3</span> Requirements as an \(\mathcal{H}_\infty\) norm</h3>
<div class="outline-text-3" id="text-3-3">
<p>
We now try to express this requirement in terms of an \(\mathcal{H}_\infty\) norm.
</p>
<p>
Let&rsquo;s define one weight \(W_\phi(s)\) that represents the maximum wanted phase uncertainty:
\[ |W_{\phi}(j\omega)|^{-1} \approx \sin(\Delta\phi_{\text{max}}(\omega)), \quad \forall\omega \]
</p>
<p>
Then:
</p>
\begin{align*}
& |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| < \sin\big( \Delta\phi_\text{max}(\omega) \big), \quad \forall\omega \\
\Longleftrightarrow & |W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)| < |W_\phi(j\omega)|^{-1}, \quad \forall\omega \\
\Longleftrightarrow & \left| W_1(j\omega) H_1(j\omega) W_\phi(j\omega) \right| + \left| W_2(j\omega) H_2(j\omega) W_\phi(j\omega) \right| < 1, \quad \forall\omega
\end{align*}
<p>
Which is approximately equivalent to (with an error of maximum \(\sqrt{2}\)):
</p>
\begin{equation}
\label{org829e35c}
\left\| \begin{matrix} W_1(s) W_\phi(s) H_1(s) \\ W_2(s) W_\phi(s) H_2(s) \end{matrix} \right\|_\infty < 1
\end{equation}
<p>
One should not forget that at frequency where both sensors has unknown dynamics (\(|W_1(j\omega)| > 1\) and \(|W_2(j\omega)| > 1\)), the super sensor dynamics will also be unknown and the phase uncertainty cannot be bounded.
Thus, at these frequencies, \(|W_\phi|\) should be smaller than \(1\).
</p>
</div>
</div>
<div id="outline-container-org21cd96f" class="outline-3">
<h3 id="org21cd96f"><span class="section-number-3">3.4</span> Weighting Function used to bound the super sensor uncertainty</h3>
<div class="outline-text-3" id="text-3-4">
<p>
Let&rsquo;s define \(W_\phi(s)\) in order to bound the maximum allowed phase uncertainty \(\Delta\phi_\text{max}\) of the super sensor dynamics.
</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 class="org-src-container">
<pre class="src src-matlab">save('./mat/Wu.mat', 'Wu');
</pre>
</div>
<p>
The obtained upper bounds on the complementary filters in order to limit the phase uncertainty of the super sensor are represented in Fig. <a href="#org665493b">14</a>.
</p>
<div id="org665493b" class="figure">
<p><img src="figs/upper_bounds_comp_filter_max_phase_uncertainty.png" alt="upper_bounds_comp_filter_max_phase_uncertainty.png" />
</p>
<p><span class="figure-number">Figure 14: </span>Upper bounds on the complementary filters set in order to limit the maximum phase uncertainty of the super sensor to 30 degrees until 500Hz (<a href="./figs/upper_bounds_comp_filter_max_phase_uncertainty.png">png</a>, <a href="./figs/upper_bounds_comp_filter_max_phase_uncertainty.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-org32acc0d" class="outline-3">
<h3 id="org32acc0d"><span class="section-number-3">3.5</span> \(\mathcal{H}_\infty\) Synthesis</h3>
<div class="outline-text-3" id="text-3-5">
<p>
The \(\mathcal{H}_\infty\) synthesis architecture used for the complementary filters is shown in Fig. <a href="#org13165b0">15</a>.
</p>
<div id="org13165b0" 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 15: </span>Architecture used for \(\mathcal{H}_\infty\) synthesis of complementary filters</p>
</div>
<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 we do the \(\mathcal{H}_\infty\) synthesis using the <code>hinfsyn</code> command.
</p>
<div class="org-src-container">
<pre class="src src-matlab">[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
</pre>
</div>
<pre class="example">
[H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
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>
And \(H_1(s)\) is 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 are shown in Fig. <a href="#orgb81c06d">16</a>.
</p>
<div id="orgb81c06d" class="figure">
<p><img src="figs/comp_filter_hinf_uncertainty.png" alt="comp_filter_hinf_uncertainty.png" />
</p>
<p><span class="figure-number">Figure 16: </span>Obtained complementary filters (<a href="./figs/comp_filter_hinf_uncertainty.png">png</a>, <a href="./figs/comp_filter_hinf_uncertainty.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-orgbf92cbe" class="outline-3">
<h3 id="orgbf92cbe"><span class="section-number-3">3.6</span> Super sensor uncertainty</h3>
<div class="outline-text-3" id="text-3-6">
<div class="org-src-container">
<pre class="src src-matlab">H2_filters = load('./mat/H2_filters.mat', 'H2', 'H1');
</pre>
</div>
<p>
The uncertainty of the super sensor cannot be made smaller than both the individual sensor. Ideally, it would follow the minimum uncertainty of both sensors.
</p>
<p>
We here just used very wimple weights.
For instance, we could improve the dynamical uncertainty of the super sensor by making \(|W_\phi(j\omega)|\) smaller bellow 2Hz where the dynamical uncertainty of the sensor 1 is small.
</p>
</div>
</div>
<div id="outline-container-org23b62b8" class="outline-3">
<h3 id="org23b62b8"><span class="section-number-3">3.7</span> Super sensor noise</h3>
<div class="outline-text-3" id="text-3-7">
<p>
We now compute the obtain Power Spectral Density of the super sensor&rsquo;s noise.
The noise characteristics of both individual sensor are defined below.
</p>
<p>
The PSD of both sensor and of the super sensor is shown in Fig. <a href="#orgc5696a6">17</a>.
The CPS of both sensor and of the super sensor is shown in Fig. <a href="#orgdbe004f">18</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_Hinf = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
PSD_H2 = abs(squeeze(freqresp(N1*H2_filters.H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2_filters.H2, freqs, 'Hz'))).^2;
CPS_S2 = cumtrapz(freqs, PSD_S2);
CPS_S1 = cumtrapz(freqs, PSD_S1);
CPS_Hinf = cumtrapz(freqs, PSD_Hinf);
CPS_H2 = cumtrapz(freqs, PSD_H2);
</pre>
</div>
<div id="orgc5696a6" class="figure">
<p><img src="figs/psd_sensors_hinf_synthesis.png" alt="psd_sensors_hinf_synthesis.png" />
</p>
<p><span class="figure-number">Figure 17: </span>Power Spectral Density of the obtained super sensor using the \(\mathcal{H}_\infty\) synthesis (<a href="./figs/psd_sensors_hinf_synthesis.png">png</a>, <a href="./figs/psd_sensors_hinf_synthesis.pdf">pdf</a>)</p>
</div>
<div id="orgdbe004f" class="figure">
<p><img src="figs/cps_sensors_hinf_synthesis.png" alt="cps_sensors_hinf_synthesis.png" />
</p>
<p><span class="figure-number">Figure 18: </span>Cumulative Power Spectrum of the obtained super sensor using the \(\mathcal{H}_\infty\) synthesis (<a href="./figs/cps_sensors_hinf_synthesis.png">png</a>, <a href="./figs/cps_sensors_hinf_synthesis.cps">cps</a>)</p>
</div>
</div>
</div>
<div id="outline-container-org0cb0b10" class="outline-3">
<h3 id="org0cb0b10"><span class="section-number-3">3.8</span> Conclusion</h3>
<div class="outline-text-3" id="text-3-8">
<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-orge22cf08" class="outline-2">
<h2 id="orge22cf08"><span class="section-number-2">4</span> Optimal and Robust Sensor Fusion: Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis with Acc and Pos</h2>
<div class="outline-text-2" id="text-4">
<p>
<a id="org8c8e334"></a>
</p>
<div class="note">
<p>
The Matlab scripts is accessible <a href="matlab/mixed_synthesis_sensor_fusion.m">here</a>.
</p>
</div>
</div>
<div id="outline-container-org7981c46" class="outline-3">
<h3 id="org7981c46"><span class="section-number-3">4.1</span> Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis - Introduction</h3>
<div class="outline-text-3" id="text-4-1">
<p>
The goal is to design complementary filters such that:
</p>
<ul class="org-ul">
<li>the maximum uncertainty of the super sensor is bounded</li>
<li>the RMS value of the super sensor noise is minimized</li>
</ul>
<p>
To do so, we can use the Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis.
</p>
<p>
The Matlab function for that is <code>h2hinfsyn</code> (<a href="https://fr.mathworks.com/help/robust/ref/h2hinfsyn.html">doc</a>).
</p>
</div>
</div>
<div id="outline-container-orga0b5528" class="outline-3">
<h3 id="orga0b5528"><span class="section-number-3">4.2</span> Noise characteristics and Uncertainty of the individual sensors</h3>
<div class="outline-text-3" id="text-4-2">
<p>
Both dynamical uncertainty and noise characteristics of the individual sensors are shown in Fig. <a href="#orgf88d833">19</a>.
</p>
<div id="orgf88d833" class="figure">
<p><img src="figs/mixed_synthesis_noise_uncertainty_sensors.png" alt="mixed_synthesis_noise_uncertainty_sensors.png" />
</p>
<p><span class="figure-number">Figure 19: </span>Noise characteristsics and Dynamical uncertainty of the individual sensors (<a href="./figs/mixed_synthesis_noise_uncertainty_sensors.png">png</a>, <a href="./figs/mixed_synthesis_noise_uncertainty_sensors.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-org100bf37" class="outline-3">
<h3 id="org100bf37"><span class="section-number-3">4.3</span> Weighting Functions on the uncertainty of the super sensor</h3>
<div class="outline-text-3" id="text-4-3">
<p>
We design weights for the \(\mathcal{H}_\infty\) part of the synthesis in order to limit the dynamical uncertainty of the super sensor.
The maximum wanted multiplicative uncertainty is shown in Fig. .The idea here is that we don&rsquo;t really need low uncertainty at low frequency but only near the crossover frequency that is suppose to be around 300Hz here.
</p>
</div>
</div>
<div id="outline-container-orgbe26d6f" class="outline-3">
<h3 id="orgbe26d6f"><span class="section-number-3">4.4</span> Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis</h3>
<div class="outline-text-3" id="text-4-4">
<p>
The synthesis architecture that is used here is shown in Fig. <a href="#orgd1a9c36">20</a>.
</p>
<p>
The controller \(K\) is synthesized such that it:
</p>
<ul class="org-ul">
<li>Keeps the \(\mathcal{H}_\infty\) norm \(G\) of the transfer function from \(w\) to \(z_\infty\) bellow some specified value</li>
<li>Keeps the \(\mathcal{H}_2\) norm \(H\) of the transfer function from \(w\) to \(z_2\) bellow some specified value</li>
<li>Minimizes a trade-off criterion of the form \(W_1 G^2 + W_2 H^2\) where \(W_1\) and \(W_2\) are specified values</li>
</ul>
<div id="orgd1a9c36" 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 20: </span>Mixed H2/H-Infinity Synthesis</p>
</div>
<p>
Here, we define \(P\) such that:
</p>
\begin{align*}
\left\| \frac{z_\infty}{w} \right\|_\infty &= \left\| \begin{matrix}W_1(s) H_1(s) \\ W_2(s) H_2(s)\end{matrix} \right\|_\infty \\
\left\| \frac{z_2}{w} \right\|_2 &= \left\| \begin{matrix}N_1(s) H_1(s) \\ N_2(s) H_2(s)\end{matrix} \right\|_2
\end{align*}
<p>
Then:
</p>
<ul class="org-ul">
<li>we specify the maximum value for the \(\mathcal{H}_\infty\) norm between \(w\) and \(z_\infty\) to be \(1\)</li>
<li>we don&rsquo;t specify any maximum value for the \(\mathcal{H}_2\) norm between \(w\) and \(z_2\)</li>
<li>we choose \(W_1 = 0\) and \(W_2 = 1\) such that the objective is to minimize the \(\mathcal{H}_2\) norm between \(w\) and \(z_2\)</li>
</ul>
<p>
The synthesis objective is to have:
\[ \left\| \frac{z_\infty}{w} \right\|_\infty = \left\| \begin{matrix}W_1(s) H_1(s) \\ W_2(s) H_2(s)\end{matrix} \right\|_\infty < 1 \]
and to minimize:
\[ \left\| \frac{z_2}{w} \right\|_2 = \left\| \begin{matrix}N_1(s) H_1(s) \\ N_2(s) H_2(s)\end{matrix} \right\|_2 \]
which is what we wanted.
</p>
<p>
We define the generalized plant that will be used for the mixed synthesis.
</p>
<div class="org-src-container">
<pre class="src src-matlab">W1u = ss(W2*Wu); W2u = ss(W1*Wu); % Weight on the uncertainty
W1n = ss(N2); W2n = ss(N1); % Weight on the noise
P = [W1u -W1u;
0 W2u;
W1n -W1n;
0 W2n;
1 0];
</pre>
</div>
<p>
The mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis is performed below.
</p>
<div class="org-src-container">
<pre class="src src-matlab">Nmeas = 1; Ncon = 1; Nz2 = 2;
[H1, ~, normz, ~] = h2hinfsyn(P, Nmeas, Ncon, Nz2, [0, 1], 'HINFMAX', 1, 'H2MAX', Inf, 'DKMAX', 100, 'TOL', 0.01, 'DISPLAY', 'on');
H2 = 1 - H1;
</pre>
</div>
<p>
The obtained complementary filters are shown in Fig. <a href="#orgac7eb0d">21</a>.
</p>
<div id="orgac7eb0d" class="figure">
<p><img src="figs/comp_filters_mixed_synthesis.png" alt="comp_filters_mixed_synthesis.png" />
</p>
<p><span class="figure-number">Figure 21: </span>Obtained complementary filters after mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis (<a href="./figs/comp_filters_mixed_synthesis.png">png</a>, <a href="./figs/comp_filters_mixed_synthesis.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-org062c26e" class="outline-3">
<h3 id="org062c26e"><span class="section-number-3">4.5</span> Obtained Super Sensor&rsquo;s noise</h3>
<div class="outline-text-3" id="text-4-5">
<p>
The PSD and CPS of the super sensor&rsquo;s noise are shown in Fig. <a href="#org419d7cc">22</a> and Fig. <a href="#org0f5a69a">23</a> respectively.
</p>
<div class="org-src-container">
<pre class="src src-matlab">PSD_S2 = abs(squeeze(freqresp(N2, freqs, 'Hz'))).^2;
PSD_S1 = abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2;
PSD_H2Hinf = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N2*H2, freqs, 'Hz'))).^2;
CPS_S2 = cumtrapz(freqs, PSD_S2);
CPS_S1 = cumtrapz(freqs, PSD_S1);
CPS_H2Hinf = cumtrapz(freqs, PSD_H2Hinf);
</pre>
</div>
<div id="org419d7cc" class="figure">
<p><img src="figs/psd_super_sensor_mixed_syn.png" alt="psd_super_sensor_mixed_syn.png" />
</p>
<p><span class="figure-number">Figure 22: </span>Power Spectral Density of the Super Sensor obtained with the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis (<a href="./figs/psd_super_sensor_mixed_syn.png">png</a>, <a href="./figs/psd_super_sensor_mixed_syn.pdf">pdf</a>)</p>
</div>
<div id="org0f5a69a" class="figure">
<p><img src="figs/cps_super_sensor_mixed_syn.png" alt="cps_super_sensor_mixed_syn.png" />
</p>
<p><span class="figure-number">Figure 23: </span>Cumulative Power Spectrum of the Super Sensor obtained with the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis (<a href="./figs/cps_super_sensor_mixed_syn.png">png</a>, <a href="./figs/cps_super_sensor_mixed_syn.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-org8a0bef2" class="outline-3">
<h3 id="org8a0bef2"><span class="section-number-3">4.6</span> Obtained Super Sensor&rsquo;s Uncertainty</h3>
<div class="outline-text-3" id="text-4-6">
<p>
The uncertainty on the super sensor&rsquo;s dynamics is shown in Fig.
</p>
</div>
</div>
<div id="outline-container-orga1c1d8f" class="outline-3">
<h3 id="orga1c1d8f"><span class="section-number-3">4.7</span> Comparison Hinf H2 H2/Hinf</h3>
<div class="outline-text-3" id="text-4-7">
<div class="org-src-container">
<pre class="src src-matlab">H2_filters = load('./mat/H2_filters.mat', 'H2', 'H1');
Hinf_filters = load('./mat/Hinf_filters.mat', 'H2', 'H1');
H2_Hinf_filters = load('./mat/H2_Hinf_filters.mat', 'H2', 'H1');
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab">PSD_H2 = abs(squeeze(freqresp(N2*H2_filters.H2, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N1*H2_filters.H1, freqs, 'Hz'))).^2;
CPS_H2 = cumtrapz(freqs, PSD_H2);
PSD_Hinf = abs(squeeze(freqresp(N2*Hinf_filters.H2, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N1*Hinf_filters.H1, freqs, 'Hz'))).^2;
CPS_Hinf = cumtrapz(freqs, PSD_Hinf);
PSD_H2Hinf = abs(squeeze(freqresp(N2*H2_Hinf_filters.H2, freqs, 'Hz'))).^2+abs(squeeze(freqresp(N1*H2_Hinf_filters.H1, freqs, 'Hz'))).^2;
CPS_H2Hinf = cumtrapz(freqs, PSD_H2Hinf);
</pre>
</div>
<table border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<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.0012</td>
</tr>
<tr>
<td class="org-left">Robust: \(\mathcal{H}_\infty\)</td>
<td class="org-right">0.041</td>
</tr>
<tr>
<td class="org-left">Mixed: \(\mathcal{H}_2/\mathcal{H}_\infty\)</td>
<td class="org-right">0.011</td>
</tr>
</tbody>
</table>
</div>
</div>
<div id="outline-container-orgc59f1bc" class="outline-3">
<h3 id="orgc59f1bc"><span class="section-number-3">4.8</span> Conclusion</h3>
<div class="outline-text-3" id="text-4-8">
<p>
This synthesis methods allows both to:
</p>
<ul class="org-ul">
<li>limit the dynamical uncertainty of the super sensor</li>
<li>minimize the RMS value of the estimation</li>
</ul>
</div>
</div>
</div>
<div id="outline-container-org2e08794" class="outline-2">
<h2 id="org2e08794"><span class="section-number-2">5</span> Functions</h2>
<div class="outline-text-2" id="text-5">
</div>
<div id="outline-container-orge1c196d" class="outline-3">
<h3 id="orge1c196d"><span class="section-number-3">5.1</span> <code>createWeight</code></h3>
<div class="outline-text-3" id="text-5-1">
<p>
<a id="org5e935d3"></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 &gt; b &amp;&amp; b &gt; c) || (c &gt; b &amp;&amp; b &gt; 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-org61ce738" class="outline-3">
<h3 id="org61ce738"><span class="section-number-3">5.2</span> <code>plotMagUncertainty</code></h3>
<div class="outline-text-3" id="text-5-2">
<p>
<a id="orgc983abf"></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, mustBePositive} = 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-org6d139f2" class="outline-3">
<h3 id="org6d139f2"><span class="section-number-3">5.3</span> <code>plotPhaseUncertainty</code></h3>
<div class="outline-text-3" id="text-5-3">
<p>
<a id="org51e7987"></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'))) &gt; 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>
</p>
<style>.csl-entry{text-indent: -1.5em; margin-left: 1.5em;}</style><h2 class='citeproc-org-bib-h2'>Bibliography</h2>
<div class="csl-bib-body">
<div class="csl-entry"><a name="citeproc_bib_item_1"></a>Barzilai, Aaron, Tom VanZandt, and Tom Kenny. 1998. “Technique for Measurement of the Noise of a Sensor in the Presence of Large Background Signals.” <i>Review of Scientific Instruments</i> 69 (7):2767–72. <a href="https://doi.org/10.1063/1.1149013">https://doi.org/10.1063/1.1149013</a>.</div>
<div class="csl-entry">NO_ITEM_DATA:moore19_capac_instr_sensor_fusion_high_bandW_nanop</div>
</div>
</div>
<div id="postamble" class="status">
<p class="author">Author: Thomas Dehaeze</p>
<p class="date">Created: 2020-10-01 jeu. 11:26</p>
</div>
</body>
</html>
Reference in New Issue View Git Blame Copy Permalink