tdehaeze/dehaeze20_optim_robus_compl_filte
1
0
Fork 0
Code Issues Pull Requests Releases Activity

Add Introduction

Browse Source
This commit is contained in:
Thomas Dehaeze 2020-10-02 18:35:13 +02:00
parent 53619b5db3
commit 81074516d5
4 changed files with 275 additions and 3963 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

376
matlab/index.html
View File

@ -3,7 +3,7 @@
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en"> <html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
<head> <head>
<!-- 2020-10-02 ven. 18:25 --> <!-- 2020-10-02 ven. 18:35 -->
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" /> <meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
<title>Robust and Optimal Sensor Fusion - Matlab Computation</title> <title>Robust and Optimal Sensor Fusion - Matlab Computation</title>
<meta name="generator" content="Org mode" /> <meta name="generator" content="Org mode" />
@ -35,50 +35,50 @@
<h2>Table of Contents</h2> <h2>Table of Contents</h2>
<div id="text-table-of-contents"> <div id="text-table-of-contents">
<ul> <ul>
<li><a href="#org42e0486">1. Sensor Description</a> <li><a href="#orgcc2172b">1. Sensor Description</a>
<ul> <ul>
<li><a href="#org647c690">1.1. Sensor Dynamics</a></li> <li><a href="#org23e78da">1.1. Sensor Dynamics</a></li>
<li><a href="#orge2ef6d5">1.2. Sensor Model Uncertainty</a></li> <li><a href="#org94edbba">1.2. Sensor Model Uncertainty</a></li>
<li><a href="#org17664d8">1.3. Sensor Noise</a></li> <li><a href="#org4cc404c">1.3. Sensor Noise</a></li>
<li><a href="#org18c8385">1.4. Save Model</a></li> <li><a href="#orgca9547c">1.4. Save Model</a></li>
</ul> </ul>
</li> </li>
<li><a href="#org85fd6bc">2. Introduction to Sensor Fusion</a> <li><a href="#org8fb094e">2. Introduction to Sensor Fusion</a>
<ul> <ul>
<li><a href="#orgd9728f2">2.1. Sensor Fusion Architecture</a></li> <li><a href="#org38e7087">2.1. Sensor Fusion Architecture</a></li>
<li><a href="#org211028b">2.2. Super Sensor Noise</a></li> <li><a href="#org3ba53c4">2.2. Super Sensor Noise</a></li>
<li><a href="#org1f329be">2.3. Super Sensor Dynamical Uncertainty</a></li> <li><a href="#org0f97eb1">2.3. Super Sensor Dynamical Uncertainty</a></li>
</ul> </ul>
</li> </li>
<li><a href="#org106ad63">3. Optimal Super Sensor Noise: \(\mathcal{H}_2\) Synthesis</a> <li><a href="#orgf2e7afd">3. Optimal Super Sensor Noise: \(\mathcal{H}_2\) Synthesis</a>
<ul> <ul>
<li><a href="#orgbbd13bd">3.1. \(\mathcal{H}_2\) Synthesis</a></li> <li><a href="#org3f9017b">3.1. \(\mathcal{H}_2\) Synthesis</a></li>
<li><a href="#org0d86fd6">3.2. Super Sensor Noise</a></li> <li><a href="#orgb5bc159">3.2. Super Sensor Noise</a></li>
<li><a href="#org238d359">3.3. Discrepancy between sensor dynamics and model</a></li> <li><a href="#orge0269cd">3.3. Discrepancy between sensor dynamics and model</a></li>
</ul> </ul>
</li> </li>
<li><a href="#orge767c66">4. Robust Sensor Fusion: \(\mathcal{H}_\infty\) Synthesis</a> <li><a href="#org869a9ab">4. Robust Sensor Fusion: \(\mathcal{H}_\infty\) Synthesis</a>
<ul> <ul>
<li><a href="#orgdba13d6">4.1. Weighting Function used to bound the super sensor uncertainty</a></li> <li><a href="#orgf20f681">4.1. Weighting Function used to bound the super sensor uncertainty</a></li>
<li><a href="#org440d265">4.2. \(\mathcal{H}_\infty\) Synthesis</a></li> <li><a href="#orge540fc3">4.2. \(\mathcal{H}_\infty\) Synthesis</a></li>
<li><a href="#org754b710">4.3. Super sensor uncertainty</a></li> <li><a href="#org6d2d24e">4.3. Super sensor uncertainty</a></li>
<li><a href="#org3c71ac3">4.4. Super sensor noise</a></li> <li><a href="#org8ce77ee">4.4. Super sensor noise</a></li>
<li><a href="#org50d6031">4.5. Conclusion</a></li> <li><a href="#org8a94903">4.5. Conclusion</a></li>
</ul> </ul>
</li> </li>
<li><a href="#org6230c6a">5. Optimal and Robust Sensor Fusion: Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis</a> <li><a href="#org919d3da">5. Optimal and Robust Sensor Fusion: Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis</a>
<ul> <ul>
<li><a href="#org47226ff">5.1. Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis</a></li> <li><a href="#org360cf70">5.1. Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis</a></li>
<li><a href="#org6ce5da3">5.2. Obtained Super Sensor&rsquo;s noise</a></li> <li><a href="#org880e8a9">5.2. Obtained Super Sensor&rsquo;s noise</a></li>
<li><a href="#orgae2d47e">5.3. Obtained Super Sensor&rsquo;s Uncertainty</a></li> <li><a href="#orgb149f25">5.3. Obtained Super Sensor&rsquo;s Uncertainty</a></li>
<li><a href="#orgd41a044">5.4. Conclusion</a></li> <li><a href="#orgfaf058b">5.4. Conclusion</a></li>
</ul> </ul>
</li> </li>
<li><a href="#orgecfb74c">6. Matlab Functions</a> <li><a href="#org2af7a78">6. Matlab Functions</a>
<ul> <ul>
<li><a href="#orgbc9616b">6.1. <code>createWeight</code></a></li> <li><a href="#orgfe9741d">6.1. <code>createWeight</code></a></li>
<li><a href="#org6933288">6.2. <code>plotMagUncertainty</code></a></li> <li><a href="#org6a88fe0">6.2. <code>plotMagUncertainty</code></a></li>
<li><a href="#org351f1ef">6.3. <code>plotPhaseUncertainty</code></a></li> <li><a href="#orgbff6dca">6.3. <code>plotPhaseUncertainty</code></a></li>
</ul> </ul>
</li> </li>
</ul> </ul>
@ -86,32 +86,30 @@
</div> </div>
<p> <p>
In this document, the optimal and robust design of complementary filters is studied. This document is arranged as follows:
</p> </p>
<p>
Two sensors are considered with both different noise characteristics and dynamical uncertainties represented by multiplicative input uncertainty.
</p>
<ul class="org-ul"> <ul class="org-ul">
<li>Section <a href="#orge8a8e6b">3</a>: the \(\mathcal{H}_2\) synthesis is used to design complementary filters such that the RMS value of the super sensor&rsquo;s noise is minimized</li> <li>Section <a href="#org2f5f470">1</a>: the sensors are described (dynamics, uncertainty, noise)</li>
<li>Section <a href="#org0e5d4db">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="#org220320f">2</a>: the sensor fusion architecture is described and the super sensor noise and dynamical uncertainty are derived</li>
<li>Section <a href="#org310c590">5</a>: the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis is used to both limit the super sensor&rsquo;s uncertainty and to lower the RMS value of the super sensor&rsquo;s noise</li> <li>Section <a href="#org0f63067">3</a>: the \(\mathcal{H}_2\) synthesis is used to design complementary filters such that the RMS value of the super sensor&rsquo;s noise is minimized</li>
<li>Section <a href="#org25efa7e">4</a>: the \(\mathcal{H}_\infty\) synthesis is used to design complementary filters such that the super sensor&rsquo;s uncertainty is bonded to acceptable values</li>
<li>Section <a href="#org5163fb1">5</a>: the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis is used to both limit the super sensor&rsquo;s uncertainty and to lower the RMS value of the super sensor&rsquo;s noise</li>
<li>Section <a href="#org71367a9">6</a>: Matlab functions used for the analysis are described</li>
</ul> </ul>
<div id="outline-container-org42e0486" class="outline-2"> <div id="outline-container-orgcc2172b" class="outline-2">
<h2 id="org42e0486"><span class="section-number-2">1</span> Sensor Description</h2> <h2 id="orgcc2172b"><span class="section-number-2">1</span> Sensor Description</h2>
<div class="outline-text-2" id="text-1"> <div class="outline-text-2" id="text-1">
<p> <p>
<a id="org7b36852"></a> <a id="org2f5f470"></a>
</p> </p>
<p> <p>
In Figure <a href="#org50592fc">1</a> is shown a schematic of a sensor model that is used in the following study. In Figure <a href="#orgd19c6fc">1</a> is shown a schematic of a sensor model that is used in the following study.
In this example, the measured quantity \(x\) is the velocity of an object. In this example, the measured quantity \(x\) is the velocity of an object.
</p> </p>
<table id="orgbd57c1d" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides"> <table id="orgc326093" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<caption class="t-above"><span class="table-number">Table 1:</span> Description of signals in Figure <a href="#org50592fc">1</a></caption> <caption class="t-above"><span class="table-number">Table 1:</span> Description of signals in Figure <a href="#orgd19c6fc">1</a></caption>
<colgroup> <colgroup>
<col class="org-left" /> <col class="org-left" />
@ -160,8 +158,8 @@ In this example, the measured quantity \(x\) is the velocity of an object.
</tbody> </tbody>
</table> </table>
<table id="orgd5bc759" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides"> <table id="orgf877ead" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<caption class="t-above"><span class="table-number">Table 2:</span> Description of Systems in Figure <a href="#org50592fc">1</a></caption> <caption class="t-above"><span class="table-number">Table 2:</span> Description of Systems in Figure <a href="#orgd19c6fc">1</a></caption>
<colgroup> <colgroup>
<col class="org-left" /> <col class="org-left" />
@ -205,18 +203,18 @@ In this example, the measured quantity \(x\) is the velocity of an object.
</table> </table>
<div id="org50592fc" class="figure"> <div id="orgd19c6fc" class="figure">
<p><img src="figs-tikz/sensor_model_noise_uncertainty.png" alt="sensor_model_noise_uncertainty.png" /> <p><img src="figs-tikz/sensor_model_noise_uncertainty.png" alt="sensor_model_noise_uncertainty.png" />
</p> </p>
<p><span class="figure-number">Figure 1: </span>Sensor Model</p> <p><span class="figure-number">Figure 1: </span>Sensor Model</p>
</div> </div>
</div> </div>
<div id="outline-container-org647c690" class="outline-3"> <div id="outline-container-org23e78da" class="outline-3">
<h3 id="org647c690"><span class="section-number-3">1.1</span> Sensor Dynamics</h3> <h3 id="org23e78da"><span class="section-number-3">1.1</span> Sensor Dynamics</h3>
<div class="outline-text-3" id="text-1-1"> <div class="outline-text-3" id="text-1-1">
<p> <p>
<a id="org3d3853d"></a> <a id="org4813033"></a>
Let&rsquo;s consider two sensors measuring the velocity of an object. Let&rsquo;s consider two sensors measuring the velocity of an object.
</p> </p>
@ -247,14 +245,14 @@ G2 = g_pos/s/(1 + s/w_pos); % Position Sensor Plant [V/(m/s)]
<p> <p>
These nominal dynamics are also taken as the model of the sensor dynamics. These nominal dynamics are also taken as the model of the sensor dynamics.
The true sensor dynamics has some uncertainty associated to it and described in section <a href="#org3c9ef6e">1.2</a>. The true sensor dynamics has some uncertainty associated to it and described in section <a href="#org992a983">1.2</a>.
</p> </p>
<p> <p>
Both sensor dynamics in \([\frac{V}{m/s}]\) are shown in Figure <a href="#orgdf43a57">2</a>. Both sensor dynamics in \([\frac{V}{m/s}]\) are shown in Figure <a href="#org6409089">2</a>.
</p> </p>
<div id="orgdf43a57" class="figure"> <div id="org6409089" class="figure">
<p><img src="figs/sensors_nominal_dynamics.png" alt="sensors_nominal_dynamics.png" /> <p><img src="figs/sensors_nominal_dynamics.png" alt="sensors_nominal_dynamics.png" />
</p> </p>
<p><span class="figure-number">Figure 2: </span>Sensor nominal dynamics from the velocity of the object to the output voltage</p> <p><span class="figure-number">Figure 2: </span>Sensor nominal dynamics from the velocity of the object to the output voltage</p>
@ -262,12 +260,12 @@ Both sensor dynamics in \([\frac{V}{m/s}]\) are shown in Figure <a href="#orgdf4
</div> </div>
</div> </div>
<div id="outline-container-orge2ef6d5" class="outline-3"> <div id="outline-container-org94edbba" class="outline-3">
<h3 id="orge2ef6d5"><span class="section-number-3">1.2</span> Sensor Model Uncertainty</h3> <h3 id="org94edbba"><span class="section-number-3">1.2</span> Sensor Model Uncertainty</h3>
<div class="outline-text-3" id="text-1-2"> <div class="outline-text-3" id="text-1-2">
<p> <p>
<a id="org3c9ef6e"></a> <a id="org992a983"></a>
The uncertainty on the sensor dynamics is described by multiplicative uncertainty (Figure <a href="#org50592fc">1</a>). The uncertainty on the sensor dynamics is described by multiplicative uncertainty (Figure <a href="#orgd19c6fc">1</a>).
</p> </p>
<p> <p>
@ -279,7 +277,7 @@ The true sensor dynamics \(G_i(s)\) is then described by \eqref{eq:sensor_dynami
\end{equation} \end{equation}
<p> <p>
The weights \(W_i(s)\) representing the dynamical uncertainty are defined below and their magnitude is shown in Figure <a href="#org5137ef5">3</a>. The weights \(W_i(s)\) representing the dynamical uncertainty are defined below and their magnitude is shown in Figure <a href="#org8beb676">3</a>.
</p> </p>
<div class="org-src-container"> <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) * ... <pre class="src src-matlab">W1 = createWeight('n', 2, 'w0', 2*pi*3, 'G0', 2, 'G1', 0.1, 'Gc', 1) * ...
@ -290,18 +288,18 @@ W2 = createWeight('n', 2, 'w0', 2*pi*1e2, 'G0', 0.05, 'G1', 4, 'Gc', 1);
</div> </div>
<p> <p>
The bode plot of the sensors nominal dynamics as well as their defined dynamical spread are shown in Figure <a href="#orgfec46ba">4</a>. The bode plot of the sensors nominal dynamics as well as their defined dynamical spread are shown in Figure <a href="#org751f977">4</a>.
</p> </p>
<div id="org5137ef5" class="figure"> <div id="org8beb676" class="figure">
<p><img src="figs/sensors_uncertainty_weights.png" alt="sensors_uncertainty_weights.png" /> <p><img src="figs/sensors_uncertainty_weights.png" alt="sensors_uncertainty_weights.png" />
</p> </p>
<p><span class="figure-number">Figure 3: </span>Magnitude of the multiplicative uncertainty weights \(|W_i(j\omega)|\)</p> <p><span class="figure-number">Figure 3: </span>Magnitude of the multiplicative uncertainty weights \(|W_i(j\omega)|\)</p>
</div> </div>
<div id="orgfec46ba" class="figure"> <div id="org751f977" class="figure">
<p><img src="figs/sensors_nominal_dynamics_and_uncertainty.png" alt="sensors_nominal_dynamics_and_uncertainty.png" /> <p><img src="figs/sensors_nominal_dynamics_and_uncertainty.png" alt="sensors_nominal_dynamics_and_uncertainty.png" />
</p> </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> <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>
@ -309,12 +307,12 @@ The bode plot of the sensors nominal dynamics as well as their defined dynamical
</div> </div>
</div> </div>
<div id="outline-container-org17664d8" class="outline-3"> <div id="outline-container-org4cc404c" class="outline-3">
<h3 id="org17664d8"><span class="section-number-3">1.3</span> Sensor Noise</h3> <h3 id="org4cc404c"><span class="section-number-3">1.3</span> Sensor Noise</h3>
<div class="outline-text-3" id="text-1-3"> <div class="outline-text-3" id="text-1-3">
<p> <p>
<a id="org9362cd8"></a> <a id="org0cc1978"></a>
The noise of the sensors \(n_i\) are modelled by shaping a white noise with unitary PSD \(\tilde{n}_i\) \eqref{eq:unitary_noise_psd} with a LTI transfer function \(N_i(s)\) (Figure <a href="#org50592fc">1</a>). The noise of the sensors \(n_i\) are modelled by shaping a white noise with unitary PSD \(\tilde{n}_i\) \eqref{eq:unitary_noise_psd} with a LTI transfer function \(N_i(s)\) (Figure <a href="#orgd19c6fc">1</a>).
</p> </p>
\begin{equation} \begin{equation}
\Phi_{\tilde{n}_i}(\omega) = 1 \label{eq:unitary_noise_psd} \Phi_{\tilde{n}_i}(\omega) = 1 \label{eq:unitary_noise_psd}
@ -328,7 +326,7 @@ The Power Spectral Density of the sensor noise \(\Phi_{n_i}(\omega)\) is then co
\end{equation} \end{equation}
<p> <p>
The weights \(N_1\) and \(N_2\) representing the amplitude spectral density of the sensor noises are defined below and shown in Figure <a href="#org953924d">5</a>. The weights \(N_1\) and \(N_2\) representing the amplitude spectral density of the sensor noises are defined below and shown in Figure <a href="#org2d39969">5</a>.
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">omegac = 0.15*2*pi; G0 = 1e-1; Ginf = 1e-6; <pre class="src src-matlab">omegac = 0.15*2*pi; G0 = 1e-1; Ginf = 1e-6;
@ -340,7 +338,7 @@ N2 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4);
</div> </div>
<div id="org953924d" class="figure"> <div id="org2d39969" class="figure">
<p><img src="figs/sensors_noise.png" alt="sensors_noise.png" /> <p><img src="figs/sensors_noise.png" alt="sensors_noise.png" />
</p> </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> <p><span class="figure-number">Figure 5: </span>Amplitude spectral density of the sensors \(\sqrt{\Phi_{n_i}(\omega)} = |N_i(j\omega)|\)</p>
@ -348,8 +346,8 @@ N2 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4);
</div> </div>
</div> </div>
<div id="outline-container-org18c8385" class="outline-3"> <div id="outline-container-orgca9547c" class="outline-3">
<h3 id="org18c8385"><span class="section-number-3">1.4</span> Save Model</h3> <h3 id="orgca9547c"><span class="section-number-3">1.4</span> Save Model</h3>
<div class="outline-text-3" id="text-1-4"> <div class="outline-text-3" id="text-1-4">
<p> <p>
All the dynamical systems representing the sensors are saved for further use. All the dynamical systems representing the sensors are saved for further use.
@ -363,27 +361,27 @@ All the dynamical systems representing the sensors are saved for further use.
</div> </div>
</div> </div>
<div id="outline-container-org85fd6bc" class="outline-2"> <div id="outline-container-org8fb094e" class="outline-2">
<h2 id="org85fd6bc"><span class="section-number-2">2</span> Introduction to Sensor Fusion</h2> <h2 id="org8fb094e"><span class="section-number-2">2</span> Introduction to Sensor Fusion</h2>
<div class="outline-text-2" id="text-2"> <div class="outline-text-2" id="text-2">
<p> <p>
<a id="org39c4402"></a> <a id="org220320f"></a>
</p> </p>
</div> </div>
<div id="outline-container-orgd9728f2" class="outline-3"> <div id="outline-container-org38e7087" class="outline-3">
<h3 id="orgd9728f2"><span class="section-number-3">2.1</span> Sensor Fusion Architecture</h3> <h3 id="org38e7087"><span class="section-number-3">2.1</span> Sensor Fusion Architecture</h3>
<div class="outline-text-3" id="text-2-1"> <div class="outline-text-3" id="text-2-1">
<p> <p>
<a id="orgc00e6f9"></a> <a id="org82e93f4"></a>
</p> </p>
<p> <p>
The two sensors presented in Section <a href="#org7b36852">1</a> are now merged together using complementary filters \(H_1(s)\) and \(H_2(s)\) to form a super sensor (Figure <a href="#org71ac5d2">6</a>). The two sensors presented in Section <a href="#org2f5f470">1</a> are now merged together using complementary filters \(H_1(s)\) and \(H_2(s)\) to form a super sensor (Figure <a href="#org8c9eb16">6</a>).
</p> </p>
<div id="org71ac5d2" class="figure"> <div id="org8c9eb16" class="figure">
<p><img src="figs-tikz/sensor_fusion_noise_arch.png" alt="sensor_fusion_noise_arch.png" /> <p><img src="figs-tikz/sensor_fusion_noise_arch.png" alt="sensor_fusion_noise_arch.png" />
</p> </p>
<p><span class="figure-number">Figure 6: </span>Sensor Fusion Architecture</p> <p><span class="figure-number">Figure 6: </span>Sensor Fusion Architecture</p>
@ -407,11 +405,11 @@ The super sensor estimate \(\hat{x}\) is given by \eqref{eq:super_sensor_estimat
</div> </div>
</div> </div>
<div id="outline-container-org211028b" class="outline-3"> <div id="outline-container-org3ba53c4" class="outline-3">
<h3 id="org211028b"><span class="section-number-3">2.2</span> Super Sensor Noise</h3> <h3 id="org3ba53c4"><span class="section-number-3">2.2</span> Super Sensor Noise</h3>
<div class="outline-text-3" id="text-2-2"> <div class="outline-text-3" id="text-2-2">
<p> <p>
<a id="org2f8cec7"></a> <a id="orgd0b1104"></a>
</p> </p>
<p> <p>
@ -442,15 +440,15 @@ And the Root Mean Square (RMS) value of the super sensor noise \(\sigma_n\) is g
</div> </div>
</div> </div>
<div id="outline-container-org1f329be" class="outline-3"> <div id="outline-container-org0f97eb1" class="outline-3">
<h3 id="org1f329be"><span class="section-number-3">2.3</span> Super Sensor Dynamical Uncertainty</h3> <h3 id="org0f97eb1"><span class="section-number-3">2.3</span> Super Sensor Dynamical Uncertainty</h3>
<div class="outline-text-3" id="text-2-3"> <div class="outline-text-3" id="text-2-3">
<p> <p>
<a id="org0dafc5d"></a> <a id="orgb59d475"></a>
</p> </p>
<p> <p>
If we consider some dynamical uncertainty (the true system dynamics \(G_i\) not being perfectly equal to our model \(\hat{G}_i\)) that we model by the use of multiplicative uncertainty (Figure <a href="#org3ef8213">7</a>), the super sensor dynamics is then equals to: If we consider some dynamical uncertainty (the true system dynamics \(G_i\) not being perfectly equal to our model \(\hat{G}_i\)) that we model by the use of multiplicative uncertainty (Figure <a href="#orgb46c073">7</a>), the super sensor dynamics is then equals to:
</p> </p>
\begin{equation} \begin{equation}
@ -462,18 +460,18 @@ If we consider some dynamical uncertainty (the true system dynamics \(G_i\) not
\end{equation} \end{equation}
<div id="org3ef8213" class="figure"> <div id="orgb46c073" class="figure">
<p><img src="figs-tikz/sensor_model_uncertainty.png" alt="sensor_model_uncertainty.png" /> <p><img src="figs-tikz/sensor_model_uncertainty.png" alt="sensor_model_uncertainty.png" />
</p> </p>
<p><span class="figure-number">Figure 7: </span>Sensor Model including Dynamical Uncertainty</p> <p><span class="figure-number">Figure 7: </span>Sensor Model including Dynamical Uncertainty</p>
</div> </div>
<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)|\) as shown in Figure <a href="#org27e67a3">8</a>. The uncertainty set of the transfer function from \(\hat{x}\) to \(x\) at frequency \(\omega\) is bounded in the complex plane by a circle centered on 1 and with a radius equal to \(|W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)|\) as shown in Figure <a href="#orgfd5d4b3">8</a>.
</p> </p>
<div id="org27e67a3" class="figure"> <div id="orgfd5d4b3" class="figure">
<p><img src="figs-tikz/uncertainty_set_super_sensor.png" alt="uncertainty_set_super_sensor.png" /> <p><img src="figs-tikz/uncertainty_set_super_sensor.png" alt="uncertainty_set_super_sensor.png" />
</p> </p>
<p><span class="figure-number">Figure 8: </span>Super Sensor model uncertainty displayed in the complex plane</p> <p><span class="figure-number">Figure 8: </span>Super Sensor model uncertainty displayed in the complex plane</p>
@ -482,18 +480,18 @@ The uncertainty set of the transfer function from \(\hat{x}\) to \(x\) at freque
</div> </div>
</div> </div>
<div id="outline-container-org106ad63" class="outline-2"> <div id="outline-container-orgf2e7afd" class="outline-2">
<h2 id="org106ad63"><span class="section-number-2">3</span> Optimal Super Sensor Noise: \(\mathcal{H}_2\) Synthesis</h2> <h2 id="orgf2e7afd"><span class="section-number-2">3</span> Optimal Super Sensor Noise: \(\mathcal{H}_2\) Synthesis</h2>
<div class="outline-text-2" id="text-3"> <div class="outline-text-2" id="text-3">
<p> <p>
<a id="orge8a8e6b"></a> <a id="org0f63067"></a>
</p> </p>
<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\). 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>
<div id="org6587a06" class="figure"> <div id="orge6fcebb" class="figure">
<p><img src="figs-tikz/sensor_fusion_noise_arch.png" alt="sensor_fusion_noise_arch.png" /> <p><img src="figs-tikz/sensor_fusion_noise_arch.png" alt="sensor_fusion_noise_arch.png" />
</p> </p>
<p><span class="figure-number">Figure 9: </span>Optimal Sensor Fusion Architecture</p> <p><span class="figure-number">Figure 9: </span>Optimal Sensor Fusion Architecture</p>
@ -511,23 +509,23 @@ The RMS value of the super sensor noise is (neglecting the model uncertainty):
<p> <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). The goal is to design \(H_1(s)\) and \(H_2(s)\) such that \(H_1(s) + H_2(s) = 1\) (complementary property) and such that \(\left\| \begin{matrix} H_1 N_1 \\ H_2 N_2 \end{matrix} \right\|_2\) is minimized (minimized RMS value of the super sensor noise).
This is done using the \(\mathcal{H}_2\) synthesis in Section <a href="#org717549f">3.1</a>. This is done using the \(\mathcal{H}_2\) synthesis in Section <a href="#orgc3c8ce2">3.1</a>.
</p> </p>
</div> </div>
<div id="outline-container-orgbbd13bd" class="outline-3"> <div id="outline-container-org3f9017b" class="outline-3">
<h3 id="orgbbd13bd"><span class="section-number-3">3.1</span> \(\mathcal{H}_2\) Synthesis</h3> <h3 id="org3f9017b"><span class="section-number-3">3.1</span> \(\mathcal{H}_2\) Synthesis</h3>
<div class="outline-text-3" id="text-3-1"> <div class="outline-text-3" id="text-3-1">
<p> <p>
<a id="org717549f"></a> <a id="orgc3c8ce2"></a>
</p> </p>
<p> <p>
Consider the generalized plant \(P_{\mathcal{H}_2}\) shown in Figure <a href="#org6124aa7">10</a> and described by Equation \eqref{eq:H2_generalized_plant}. Consider the generalized plant \(P_{\mathcal{H}_2}\) shown in Figure <a href="#orgfed5b00">10</a> and described by Equation \eqref{eq:H2_generalized_plant}.
</p> </p>
<div id="org6124aa7" class="figure"> <div id="orgfed5b00" class="figure">
<p><img src="figs-tikz/h_two_optimal_fusion.png" alt="h_two_optimal_fusion.png" /> <p><img src="figs-tikz/h_two_optimal_fusion.png" alt="h_two_optimal_fusion.png" />
</p> </p>
<p><span class="figure-number">Figure 10: </span>Architecture used for \(\mathcal{H}_\infty\) synthesis of complementary filters</p> <p><span class="figure-number">Figure 10: </span>Architecture used for \(\mathcal{H}_\infty\) synthesis of complementary filters</p>
@ -583,10 +581,10 @@ Finally, \(H_1(s)\) is defined as follows
</div> </div>
<p> <p>
The obtained complementary filters are shown in Figure <a href="#orgee5ee62">11</a>. The obtained complementary filters are shown in Figure <a href="#org3561716">11</a>.
</p> </p>
<div id="orgee5ee62" class="figure"> <div id="org3561716" class="figure">
<p><img src="figs/htwo_comp_filters.png" alt="htwo_comp_filters.png" /> <p><img src="figs/htwo_comp_filters.png" alt="htwo_comp_filters.png" />
</p> </p>
<p><span class="figure-number">Figure 11: </span>Obtained complementary filters using the \(\mathcal{H}_2\) Synthesis</p> <p><span class="figure-number">Figure 11: </span>Obtained complementary filters using the \(\mathcal{H}_2\) Synthesis</p>
@ -594,15 +592,15 @@ The obtained complementary filters are shown in Figure <a href="#orgee5ee62">11<
</div> </div>
</div> </div>
<div id="outline-container-org0d86fd6" class="outline-3"> <div id="outline-container-orgb5bc159" class="outline-3">
<h3 id="org0d86fd6"><span class="section-number-3">3.2</span> Super Sensor Noise</h3> <h3 id="orgb5bc159"><span class="section-number-3">3.2</span> Super Sensor Noise</h3>
<div class="outline-text-3" id="text-3-2"> <div class="outline-text-3" id="text-3-2">
<p> <p>
<a id="org366e497"></a> <a id="org22dd161"></a>
</p> </p>
<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 and shown in Figure <a href="#org98bc6c9">12</a>. 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 and shown in Figure <a href="#orgad2796b">12</a>.
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">PSD_S1 = abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2; <pre class="src src-matlab">PSD_S1 = abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2;
@ -613,9 +611,9 @@ PSD_H2 = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2 + ...
</div> </div>
<p> <p>
The RMS value of the individual sensors and of the super sensor are listed in Table <a href="#org8db0d0e">3</a>. The RMS value of the individual sensors and of the super sensor are listed in Table <a href="#orgf505db2">3</a>.
</p> </p>
<table id="org8db0d0e" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides"> <table id="orgf505db2" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<caption class="t-above"><span class="table-number">Table 3:</span> RMS value of the individual sensor noise and of the super sensor using the \(\mathcal{H}_2\) Synthesis</caption> <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> <colgroup>
@ -648,7 +646,7 @@ The RMS value of the individual sensors and of the super sensor are listed in Ta
</table> </table>
<div id="org98bc6c9" class="figure"> <div id="orgad2796b" class="figure">
<p><img src="figs/psd_sensors_htwo_synthesis.png" alt="psd_sensors_htwo_synthesis.png" /> <p><img src="figs/psd_sensors_htwo_synthesis.png" alt="psd_sensors_htwo_synthesis.png" />
</p> </p>
<p><span class="figure-number">Figure 12: </span>Power Spectral Density of the estimated \(\hat{x}\) using the two sensors alone and using the optimally fused signal</p> <p><span class="figure-number">Figure 12: </span>Power Spectral Density of the estimated \(\hat{x}\) using the two sensors alone and using the optimally fused signal</p>
@ -657,19 +655,19 @@ The RMS value of the individual sensors and of the super sensor are listed in Ta
<p> <p>
A time domain simulation is now performed. 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 measured velocity \(x\) is set to be a sweep sine with an amplitude of \(0.1\ [m/s]\).
The velocity estimates from the two sensors and from the super sensors are shown in Figure <a href="#org1ecf54f">13</a>. The velocity estimates from the two sensors and from the super sensors are shown in Figure <a href="#org50620bf">13</a>.
The resulting noises are displayed in Figure <a href="#org026504e">14</a>. The resulting noises are displayed in Figure <a href="#orga4919f6">14</a>.
</p> </p>
<div id="org1ecf54f" class="figure"> <div id="org50620bf" class="figure">
<p><img src="figs/super_sensor_time_domain_h2.png" alt="super_sensor_time_domain_h2.png" /> <p><img src="figs/super_sensor_time_domain_h2.png" alt="super_sensor_time_domain_h2.png" />
</p> </p>
<p><span class="figure-number">Figure 13: </span>Noise of individual sensors and noise of the super sensor</p> <p><span class="figure-number">Figure 13: </span>Noise of individual sensors and noise of the super sensor</p>
</div> </div>
<div id="org026504e" class="figure"> <div id="orga4919f6" class="figure">
<p><img src="figs/sensor_noise_H2_time_domain.png" alt="sensor_noise_H2_time_domain.png" /> <p><img src="figs/sensor_noise_H2_time_domain.png" alt="sensor_noise_H2_time_domain.png" />
</p> </p>
<p><span class="figure-number">Figure 14: </span>Noise of the two sensors \(n_1, n_2\) and noise of the super sensor \(n\)</p> <p><span class="figure-number">Figure 14: </span>Noise of the two sensors \(n_1, n_2\) and noise of the super sensor \(n\)</p>
@ -677,15 +675,15 @@ The resulting noises are displayed in Figure <a href="#org026504e">14</a>.
</div> </div>
</div> </div>
<div id="outline-container-org238d359" class="outline-3"> <div id="outline-container-orge0269cd" class="outline-3">
<h3 id="org238d359"><span class="section-number-3">3.3</span> Discrepancy between sensor dynamics and model</h3> <h3 id="orge0269cd"><span class="section-number-3">3.3</span> Discrepancy between sensor dynamics and model</h3>
<div class="outline-text-3" id="text-3-3"> <div class="outline-text-3" id="text-3-3">
<p> <p>
If we consider sensor dynamical uncertainty as explained in Section <a href="#org3c9ef6e">1.2</a>, we can compute what would be the super sensor dynamical uncertainty when using the complementary filters obtained using the \(\mathcal{H}_2\) Synthesis. If we consider sensor dynamical uncertainty as explained in Section <a href="#org992a983">1.2</a>, we can compute what would be the super sensor dynamical uncertainty when using the complementary filters obtained using the \(\mathcal{H}_2\) Synthesis.
</p> </p>
<p> <p>
The super sensor dynamical uncertainty is shown in Figure <a href="#orga8596c4">15</a>. The super sensor dynamical uncertainty is shown in Figure <a href="#org2fe77bd">15</a>.
</p> </p>
<p> <p>
@ -693,7 +691,7 @@ 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. As a result the super sensor signal can not be used for feedback applications about 100Hz.
</p> </p>
<div id="orga8596c4" class="figure"> <div id="org2fe77bd" class="figure">
<p><img src="figs/super_sensor_dynamical_uncertainty_H2.png" alt="super_sensor_dynamical_uncertainty_H2.png" /> <p><img src="figs/super_sensor_dynamical_uncertainty_H2.png" alt="super_sensor_dynamical_uncertainty_H2.png" />
</p> </p>
<p><span class="figure-number">Figure 15: </span>Super sensor dynamical uncertainty when using the \(\mathcal{H}_2\) Synthesis</p> <p><span class="figure-number">Figure 15: </span>Super sensor dynamical uncertainty when using the \(\mathcal{H}_2\) Synthesis</p>
@ -702,11 +700,11 @@ As a result the super sensor signal can not be used for feedback applications ab
</div> </div>
</div> </div>
<div id="outline-container-orge767c66" class="outline-2"> <div id="outline-container-org869a9ab" class="outline-2">
<h2 id="orge767c66"><span class="section-number-2">4</span> Robust Sensor Fusion: \(\mathcal{H}_\infty\) Synthesis</h2> <h2 id="org869a9ab"><span class="section-number-2">4</span> Robust Sensor Fusion: \(\mathcal{H}_\infty\) Synthesis</h2>
<div class="outline-text-2" id="text-4"> <div class="outline-text-2" id="text-4">
<p> <p>
<a id="org0e5d4db"></a> <a id="org25efa7e"></a>
</p> </p>
<p> <p>
We initially considered perfectly known sensor dynamics so that it can be perfectly inverted. We initially considered perfectly known sensor dynamics so that it can be perfectly inverted.
@ -714,18 +712,18 @@ We initially considered perfectly known sensor dynamics so that it can be perfec
<p> <p>
We now take into account the fact that the sensor dynamics is only partially known. We now take into account the fact that the sensor dynamics is only partially known.
To do so, we model the uncertainty that we have on the sensor dynamics by multiplicative input uncertainty as shown in Figure <a href="#org3abfec4">16</a>. To do so, we model the uncertainty that we have on the sensor dynamics by multiplicative input uncertainty as shown in Figure <a href="#org9fe9ab2">16</a>.
</p> </p>
<div id="org3abfec4" class="figure"> <div id="org9fe9ab2" class="figure">
<p><img src="figs-tikz/sensor_fusion_arch_uncertainty.png" alt="sensor_fusion_arch_uncertainty.png" /> <p><img src="figs-tikz/sensor_fusion_arch_uncertainty.png" alt="sensor_fusion_arch_uncertainty.png" />
</p> </p>
<p><span class="figure-number">Figure 16: </span>Sensor fusion architecture with sensor dynamics uncertainty</p> <p><span class="figure-number">Figure 16: </span>Sensor fusion architecture with sensor dynamics uncertainty</p>
</div> </div>
<p> <p>
As explained in Section <a href="#org3c9ef6e">1.2</a>, at each frequency \(\omega\), the dynamical uncertainty of the super sensor can be represented in the complex plane by a circle with a radius equals to \(|H_1(j\omega) W_1(j\omega)| + |H_2(j\omega) W_2(j\omega)|\) and centered on 1. As explained in Section <a href="#org992a983">1.2</a>, at each frequency \(\omega\), the dynamical uncertainty of the super sensor can be represented in the complex plane by a circle with a radius equals to \(|H_1(j\omega) W_1(j\omega)| + |H_2(j\omega) W_2(j\omega)|\) and centered on 1.
</p> </p>
<p> <p>
@ -747,7 +745,7 @@ In order to specify a wanted upper bound on the dynamical uncertainty, a weight
\end{align} \end{align}
<p> <p>
The choice of \(W_u\) is presented in Section <a href="#orgc44063d">4.1</a>. The choice of \(W_u\) is presented in Section <a href="#org875b4b0">4.1</a>.
</p> </p>
@ -765,15 +763,15 @@ The objective is to design \(H_1(s)\) and \(H_2(s)\) such that \(H_1(s) + H_2(s)
</p> </p>
<p> <p>
This is done using the \(\mathcal{H}_\infty\) synthesis in Section <a href="#orga929244">4.2</a>. This is done using the \(\mathcal{H}_\infty\) synthesis in Section <a href="#org2b41098">4.2</a>.
</p> </p>
</div> </div>
<div id="outline-container-orgdba13d6" class="outline-3"> <div id="outline-container-orgf20f681" class="outline-3">
<h3 id="orgdba13d6"><span class="section-number-3">4.1</span> Weighting Function used to bound the super sensor uncertainty</h3> <h3 id="orgf20f681"><span class="section-number-3">4.1</span> Weighting Function used to bound the super sensor uncertainty</h3>
<div class="outline-text-3" id="text-4-1"> <div class="outline-text-3" id="text-4-1">
<p> <p>
<a id="orgc44063d"></a> <a id="org875b4b0"></a>
</p> </p>
<p> <p>
@ -786,7 +784,7 @@ This is done using the \(\mathcal{H}_\infty\) synthesis in Section <a href="#org
\end{align} \end{align}
<p> <p>
The uncertainty bounds of the two individual sensor as well as the wanted maximum uncertainty bounds of the super sensor are shown in Figure <a href="#org401a239">17</a>. The uncertainty bounds of the two individual sensor as well as the wanted maximum uncertainty bounds of the super sensor are shown in Figure <a href="#orgffb63ca">17</a>.
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
@ -797,7 +795,7 @@ Wu = createWeight('n', 2, 'w0', 2*pi*4e2, 'G0', 1/sin(Dphi*pi/180), 'G1', 1/4, '
</div> </div>
<div id="org401a239" class="figure"> <div id="orgffb63ca" class="figure">
<p><img src="figs/weight_uncertainty_bounds_Wu.png" alt="weight_uncertainty_bounds_Wu.png" /> <p><img src="figs/weight_uncertainty_bounds_Wu.png" alt="weight_uncertainty_bounds_Wu.png" />
</p> </p>
<p><span class="figure-number">Figure 17: </span>Uncertainty region of the two sensors as well as the wanted maximum uncertainty of the super sensor (dashed lines)</p> <p><span class="figure-number">Figure 17: </span>Uncertainty region of the two sensors as well as the wanted maximum uncertainty of the super sensor (dashed lines)</p>
@ -805,19 +803,19 @@ Wu = createWeight('n', 2, 'w0', 2*pi*4e2, 'G0', 1/sin(Dphi*pi/180), 'G1', 1/4, '
</div> </div>
</div> </div>
<div id="outline-container-org440d265" class="outline-3"> <div id="outline-container-orge540fc3" class="outline-3">
<h3 id="org440d265"><span class="section-number-3">4.2</span> \(\mathcal{H}_\infty\) Synthesis</h3> <h3 id="orge540fc3"><span class="section-number-3">4.2</span> \(\mathcal{H}_\infty\) Synthesis</h3>
<div class="outline-text-3" id="text-4-2"> <div class="outline-text-3" id="text-4-2">
<p> <p>
<a id="orga929244"></a> <a id="org2b41098"></a>
</p> </p>
<p> <p>
The generalized plant \(P_{\mathcal{H}_\infty}\) used for the \(\mathcal{H}_\infty\) Synthesis of the complementary filters is shown in Figure <a href="#org9810798">18</a> and is described by Equation \eqref{eq:Hinf_generalized_plant}. The generalized plant \(P_{\mathcal{H}_\infty}\) used for the \(\mathcal{H}_\infty\) Synthesis of the complementary filters is shown in Figure <a href="#orgf4ccff4">18</a> and is described by Equation \eqref{eq:Hinf_generalized_plant}.
</p> </p>
<div id="org9810798" class="figure"> <div id="orgf4ccff4" class="figure">
<p><img src="figs-tikz/h_infinity_robust_fusion.png" alt="h_infinity_robust_fusion.png" /> <p><img src="figs-tikz/h_infinity_robust_fusion.png" alt="h_infinity_robust_fusion.png" />
</p> </p>
<p><span class="figure-number">Figure 18: </span>Architecture used for \(\mathcal{H}_\infty\) synthesis of complementary filters</p> <p><span class="figure-number">Figure 18: </span>Architecture used for \(\mathcal{H}_\infty\) synthesis of complementary filters</p>
@ -884,11 +882,11 @@ The \(\mathcal{H}_\infty\) is successful as the \(\mathcal{H}_\infty\) norm of t
</div> </div>
<p> <p>
The obtained complementary filters as well as the wanted upper bounds are shown in Figure <a href="#orge56327e">19</a>. The obtained complementary filters as well as the wanted upper bounds are shown in Figure <a href="#org7a55110">19</a>.
</p> </p>
<div id="orge56327e" class="figure"> <div id="org7a55110" class="figure">
<p><img src="figs/hinf_comp_filters.png" alt="hinf_comp_filters.png" /> <p><img src="figs/hinf_comp_filters.png" alt="hinf_comp_filters.png" />
</p> </p>
<p><span class="figure-number">Figure 19: </span>Obtained complementary filters using the \(\mathcal{H}_\infty\) Synthesis</p> <p><span class="figure-number">Figure 19: </span>Obtained complementary filters using the \(\mathcal{H}_\infty\) Synthesis</p>
@ -896,11 +894,11 @@ The obtained complementary filters as well as the wanted upper bounds are shown
</div> </div>
</div> </div>
<div id="outline-container-org754b710" class="outline-3"> <div id="outline-container-org6d2d24e" class="outline-3">
<h3 id="org754b710"><span class="section-number-3">4.3</span> Super sensor uncertainty</h3> <h3 id="org6d2d24e"><span class="section-number-3">4.3</span> Super sensor uncertainty</h3>
<div class="outline-text-3" id="text-4-3"> <div class="outline-text-3" id="text-4-3">
<p> <p>
The super sensor dynamical uncertainty is displayed in Figure <a href="#org86abf7c">20</a>. The super sensor dynamical uncertainty is displayed in Figure <a href="#org8ded02d">20</a>.
It is confirmed that the super sensor dynamical uncertainty is less than the maximum allowed uncertainty defined by the norm of \(W_u(s)\). 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>
@ -909,7 +907,7 @@ The \(\mathcal{H}_\infty\) synthesis thus allows to design filters such that the
</p> </p>
<div id="org86abf7c" class="figure"> <div id="org8ded02d" class="figure">
<p><img src="figs/super_sensor_dynamical_uncertainty_Hinf.png" alt="super_sensor_dynamical_uncertainty_Hinf.png" /> <p><img src="figs/super_sensor_dynamical_uncertainty_Hinf.png" alt="super_sensor_dynamical_uncertainty_Hinf.png" />
</p> </p>
<p><span class="figure-number">Figure 20: </span>Super sensor dynamical uncertainty (solid curve) when using the \(\mathcal{H}_\infty\) Synthesis</p> <p><span class="figure-number">Figure 20: </span>Super sensor dynamical uncertainty (solid curve) when using the \(\mathcal{H}_\infty\) Synthesis</p>
@ -917,15 +915,15 @@ The \(\mathcal{H}_\infty\) synthesis thus allows to design filters such that the
</div> </div>
</div> </div>
<div id="outline-container-org3c71ac3" class="outline-3"> <div id="outline-container-org8ce77ee" class="outline-3">
<h3 id="org3c71ac3"><span class="section-number-3">4.4</span> Super sensor noise</h3> <h3 id="org8ce77ee"><span class="section-number-3">4.4</span> Super sensor noise</h3>
<div class="outline-text-3" id="text-4-4"> <div class="outline-text-3" id="text-4-4">
<p> <p>
We now compute the obtain Power Spectral Density of the super sensor&rsquo;s noise (Figure <a href="#org0ee17aa">21</a>). We now compute the obtain Power Spectral Density of the super sensor&rsquo;s noise (Figure <a href="#org3ff8b63">21</a>).
</p> </p>
<p> <p>
The obtained RMS of the super sensor noise in the \(\mathcal{H}_2\) and \(\mathcal{H}_\infty\) case are shown in Table <a href="#org9031855">4</a>. The obtained RMS of the super sensor noise in the \(\mathcal{H}_2\) and \(\mathcal{H}_\infty\) case are shown in Table <a href="#org2b06274">4</a>.
As expected, the super sensor obtained from the \(\mathcal{H}_\infty\) synthesis is much noisier than the super sensor obtained from the \(\mathcal{H}_2\) synthesis. 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> </p>
@ -938,13 +936,13 @@ PSD_Hinf = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2 + ...
</div> </div>
<div id="org0ee17aa" class="figure"> <div id="org3ff8b63" class="figure">
<p><img src="figs/psd_sensors_hinf_synthesis.png" alt="psd_sensors_hinf_synthesis.png" /> <p><img src="figs/psd_sensors_hinf_synthesis.png" alt="psd_sensors_hinf_synthesis.png" />
</p> </p>
<p><span class="figure-number">Figure 21: </span>Power Spectral Density of the estimated \(\hat{x}\) using the two sensors alone and using the</p> <p><span class="figure-number">Figure 21: </span>Power Spectral Density of the estimated \(\hat{x}\) using the two sensors alone and using the</p>
</div> </div>
<table id="org9031855" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides"> <table id="org2b06274" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<caption class="t-above"><span class="table-number">Table 4:</span> Comparison of the obtained RMS noise of the super sensor</caption> <caption class="t-above"><span class="table-number">Table 4:</span> Comparison of the obtained RMS noise of the super sensor</caption>
<colgroup> <colgroup>
@ -973,8 +971,8 @@ PSD_Hinf = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2 + ...
</div> </div>
</div> </div>
<div id="outline-container-org50d6031" class="outline-3"> <div id="outline-container-org8a94903" class="outline-3">
<h3 id="org50d6031"><span class="section-number-3">4.5</span> Conclusion</h3> <h3 id="org8a94903"><span class="section-number-3">4.5</span> Conclusion</h3>
<div class="outline-text-3" id="text-4-5"> <div class="outline-text-3" id="text-4-5">
<p> <p>
Using the \(\mathcal{H}_\infty\) synthesis, the dynamical uncertainty of the super sensor can be bounded to acceptable values. Using the \(\mathcal{H}_\infty\) synthesis, the dynamical uncertainty of the super sensor can be bounded to acceptable values.
@ -987,22 +985,22 @@ However, the RMS of the super sensor noise is not optimized as it was the case w
</div> </div>
</div> </div>
<div id="outline-container-org6230c6a" class="outline-2"> <div id="outline-container-org919d3da" class="outline-2">
<h2 id="org6230c6a"><span class="section-number-2">5</span> Optimal and Robust Sensor Fusion: Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis</h2> <h2 id="org919d3da"><span class="section-number-2">5</span> Optimal and Robust Sensor Fusion: Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis</h2>
<div class="outline-text-2" id="text-5"> <div class="outline-text-2" id="text-5">
<p> <p>
<a id="org310c590"></a> <a id="org5163fb1"></a>
</p> </p>
<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. 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>
<p> <p>
The sensor fusion architecture is shown in Figure <a href="#orgd6da96e">22</a> (\(\hat{G}_i\) are omitted for space reasons). The sensor fusion architecture is shown in Figure <a href="#org427d030">22</a> (\(\hat{G}_i\) are omitted for space reasons).
</p> </p>
<div id="orgd6da96e" class="figure"> <div id="org427d030" class="figure">
<p><img src="figs-tikz/sensor_fusion_arch_full.png" alt="sensor_fusion_arch_full.png" /> <p><img src="figs-tikz/sensor_fusion_arch_full.png" alt="sensor_fusion_arch_full.png" />
</p> </p>
<p><span class="figure-number">Figure 22: </span>Sensor fusion architecture with sensor dynamics uncertainty</p> <p><span class="figure-number">Figure 22: </span>Sensor fusion architecture with sensor dynamics uncertainty</p>
@ -1017,18 +1015,18 @@ The goal is to design complementary filters such that:
</ul> </ul>
<p> <p>
To do so, we can use the Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis presented in Section <a href="#orgac300ed">5.1</a>. To do so, we can use the Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis presented in Section <a href="#orgdf4b501">5.1</a>.
</p> </p>
</div> </div>
<div id="outline-container-org47226ff" class="outline-3"> <div id="outline-container-org360cf70" class="outline-3">
<h3 id="org47226ff"><span class="section-number-3">5.1</span> Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis</h3> <h3 id="org360cf70"><span class="section-number-3">5.1</span> Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis</h3>
<div class="outline-text-3" id="text-5-1"> <div class="outline-text-3" id="text-5-1">
<p> <p>
<a id="orgac300ed"></a> <a id="orgdf4b501"></a>
</p> </p>
<p> <p>
The synthesis architecture that is used here is shown in Figure <a href="#orgd95faa8">23</a>. The synthesis architecture that is used here is shown in Figure <a href="#org9fe5761">23</a>.
</p> </p>
<p> <p>
@ -1040,7 +1038,7 @@ The filter \(H_2(s)\) is synthesized such that it:
</ul> </ul>
<div id="orgd95faa8" class="figure"> <div id="org9fe5761" class="figure">
<p><img src="figs-tikz/mixed_h2_hinf_synthesis.png" alt="mixed_h2_hinf_synthesis.png" /> <p><img src="figs-tikz/mixed_h2_hinf_synthesis.png" alt="mixed_h2_hinf_synthesis.png" />
</p> </p>
<p><span class="figure-number">Figure 23: </span>Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis</p> <p><span class="figure-number">Figure 23: </span>Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis</p>
@ -1074,7 +1072,7 @@ P = [Wu*W1 -Wu*W1;
And the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis is performed. And the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis is performed.
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">[H2, ~] = h2hinfsyn(ss(P), 1, 1, 2, [0, 1], 'HINFMAX', 1, 'H2MAX', Inf, 'DKMAX', 100, 'TOL', 0.01, 'DISPLAY', 'on'); <pre class="src src-matlab">[H2, ~] = h2hinfsyn(ss(P), 1, 1, 2, [0, 1], 'HINFMAX', 1, 'H2MAX', Inf, 'DKMAX', 100, 'TOL', 1e-3, 'DISPLAY', 'on');
</pre> </pre>
</div> </div>
@ -1084,11 +1082,11 @@ And the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis is performed.
</div> </div>
<p> <p>
The obtained complementary filters are shown in Figure <a href="#org2f2ef8d">24</a>. The obtained complementary filters are shown in Figure <a href="#org9dd03be">24</a>.
</p> </p>
<div id="org2f2ef8d" class="figure"> <div id="org9dd03be" class="figure">
<p><img src="figs/htwo_hinf_comp_filters.png" alt="htwo_hinf_comp_filters.png" /> <p><img src="figs/htwo_hinf_comp_filters.png" alt="htwo_hinf_comp_filters.png" />
</p> </p>
<p><span class="figure-number">Figure 24: </span>Obtained complementary filters after mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis</p> <p><span class="figure-number">Figure 24: </span>Obtained complementary filters after mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis</p>
@ -1096,19 +1094,19 @@ The obtained complementary filters are shown in Figure <a href="#org2f2ef8d">24<
</div> </div>
</div> </div>
<div id="outline-container-org6ce5da3" class="outline-3"> <div id="outline-container-org880e8a9" class="outline-3">
<h3 id="org6ce5da3"><span class="section-number-3">5.2</span> Obtained Super Sensor&rsquo;s noise</h3> <h3 id="org880e8a9"><span class="section-number-3">5.2</span> Obtained Super Sensor&rsquo;s noise</h3>
<div class="outline-text-3" id="text-5-2"> <div class="outline-text-3" id="text-5-2">
<p> <p>
The Power Spectral Density of the super sensor&rsquo;s noise is shown in Figure <a href="#orgdef3c54">25</a>. The Power Spectral Density of the super sensor&rsquo;s noise is shown in Figure <a href="#org41f2620">25</a>.
</p> </p>
<p> <p>
A time domain simulation is shown in Figure <a href="#org2b3cbc2">26</a>. A time domain simulation is shown in Figure <a href="#orgd85ef6a">26</a>.
</p> </p>
<p> <p>
The RMS values of the super sensor noise for the presented three synthesis are listed in Table <a href="#orgbf21dd3">5</a>. The RMS values of the super sensor noise for the presented three synthesis are listed in Table <a href="#org3b609de">5</a>.
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
@ -1120,20 +1118,20 @@ PSD_H2Hinf = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2 + ...
</div> </div>
<div id="orgdef3c54" class="figure"> <div id="org41f2620" class="figure">
<p><img src="figs/psd_sensors_htwo_hinf_synthesis.png" alt="psd_sensors_htwo_hinf_synthesis.png" /> <p><img src="figs/psd_sensors_htwo_hinf_synthesis.png" alt="psd_sensors_htwo_hinf_synthesis.png" />
</p> </p>
<p><span class="figure-number">Figure 25: </span>Power Spectral Density of the Super Sensor obtained with the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis</p> <p><span class="figure-number">Figure 25: </span>Power Spectral Density of the Super Sensor obtained with the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis</p>
</div> </div>
<div id="org2b3cbc2" class="figure"> <div id="orgd85ef6a" class="figure">
<p><img src="figs/super_sensor_time_domain_h2_hinf.png" alt="super_sensor_time_domain_h2_hinf.png" /> <p><img src="figs/super_sensor_time_domain_h2_hinf.png" alt="super_sensor_time_domain_h2_hinf.png" />
</p> </p>
<p><span class="figure-number">Figure 26: </span>Noise of individual sensors and noise of the super sensor</p> <p><span class="figure-number">Figure 26: </span>Noise of individual sensors and noise of the super sensor</p>
</div> </div>
<table id="orgbf21dd3" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides"> <table id="org3b609de" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<caption class="t-above"><span class="table-number">Table 5:</span> Comparison of the obtained RMS noise of the super sensor</caption> <caption class="t-above"><span class="table-number">Table 5:</span> Comparison of the obtained RMS noise of the super sensor</caption>
<colgroup> <colgroup>
@ -1160,22 +1158,22 @@ PSD_H2Hinf = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2 + ...
<tr> <tr>
<td class="org-left">Mixed: \(\mathcal{H}_2/\mathcal{H}_\infty\)</td> <td class="org-left">Mixed: \(\mathcal{H}_2/\mathcal{H}_\infty\)</td>
<td class="org-right">0.01</td> <td class="org-right">0.0098</td>
</tr> </tr>
</tbody> </tbody>
</table> </table>
</div> </div>
</div> </div>
<div id="outline-container-orgae2d47e" class="outline-3"> <div id="outline-container-orgb149f25" class="outline-3">
<h3 id="orgae2d47e"><span class="section-number-3">5.3</span> Obtained Super Sensor&rsquo;s Uncertainty</h3> <h3 id="orgb149f25"><span class="section-number-3">5.3</span> Obtained Super Sensor&rsquo;s Uncertainty</h3>
<div class="outline-text-3" id="text-5-3"> <div class="outline-text-3" id="text-5-3">
<p> <p>
The uncertainty on the super sensor&rsquo;s dynamics is shown in Figure <a href="#org5d933bb">27</a>. The uncertainty on the super sensor&rsquo;s dynamics is shown in Figure <a href="#org37d0e3f">27</a>.
</p> </p>
<div id="org5d933bb" class="figure"> <div id="org37d0e3f" class="figure">
<p><img src="figs/super_sensor_dynamical_uncertainty_Htwo_Hinf.png" alt="super_sensor_dynamical_uncertainty_Htwo_Hinf.png" /> <p><img src="figs/super_sensor_dynamical_uncertainty_Htwo_Hinf.png" alt="super_sensor_dynamical_uncertainty_Htwo_Hinf.png" />
</p> </p>
<p><span class="figure-number">Figure 27: </span>Super sensor dynamical uncertainty (solid curve) when using the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis</p> <p><span class="figure-number">Figure 27: </span>Super sensor dynamical uncertainty (solid curve) when using the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis</p>
@ -1183,8 +1181,8 @@ The uncertainty on the super sensor&rsquo;s dynamics is shown in Figure <a href=
</div> </div>
</div> </div>
<div id="outline-container-orgd41a044" class="outline-3"> <div id="outline-container-orgfaf058b" class="outline-3">
<h3 id="orgd41a044"><span class="section-number-3">5.4</span> Conclusion</h3> <h3 id="orgfaf058b"><span class="section-number-3">5.4</span> Conclusion</h3>
<div class="outline-text-3" id="text-5-4"> <div class="outline-text-3" id="text-5-4">
<p> <p>
The mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis of the complementary filters allows to: The mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis of the complementary filters allows to:
@ -1197,18 +1195,18 @@ The mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis of the complementary fi
</div> </div>
</div> </div>
<div id="outline-container-orgecfb74c" class="outline-2"> <div id="outline-container-org2af7a78" class="outline-2">
<h2 id="orgecfb74c"><span class="section-number-2">6</span> Matlab Functions</h2> <h2 id="org2af7a78"><span class="section-number-2">6</span> Matlab Functions</h2>
<div class="outline-text-2" id="text-6"> <div class="outline-text-2" id="text-6">
<p> <p>
<a id="orgd6c9208"></a> <a id="org71367a9"></a>
</p> </p>
</div> </div>
<div id="outline-container-orgbc9616b" class="outline-3"> <div id="outline-container-orgfe9741d" class="outline-3">
<h3 id="orgbc9616b"><span class="section-number-3">6.1</span> <code>createWeight</code></h3> <h3 id="orgfe9741d"><span class="section-number-3">6.1</span> <code>createWeight</code></h3>
<div class="outline-text-3" id="text-6-1"> <div class="outline-text-3" id="text-6-1">
<p> <p>
<a id="org81aea16"></a> <a id="org38a3120"></a>
</p> </p>
<p> <p>
@ -1260,11 +1258,11 @@ This Matlab function is accessible <a href="src/createWeight.m">here</a>.
</div> </div>
</div> </div>
<div id="outline-container-org6933288" class="outline-3"> <div id="outline-container-org6a88fe0" class="outline-3">
<h3 id="org6933288"><span class="section-number-3">6.2</span> <code>plotMagUncertainty</code></h3> <h3 id="org6a88fe0"><span class="section-number-3">6.2</span> <code>plotMagUncertainty</code></h3>
<div class="outline-text-3" id="text-6-2"> <div class="outline-text-3" id="text-6-2">
<p> <p>
<a id="org82f6010"></a> <a id="orgd3213c3"></a>
</p> </p>
<p> <p>
@ -1315,11 +1313,11 @@ end
</div> </div>
</div> </div>
<div id="outline-container-org351f1ef" class="outline-3"> <div id="outline-container-orgbff6dca" class="outline-3">
<h3 id="org351f1ef"><span class="section-number-3">6.3</span> <code>plotPhaseUncertainty</code></h3> <h3 id="orgbff6dca"><span class="section-number-3">6.3</span> <code>plotPhaseUncertainty</code></h3>
<div class="outline-text-3" id="text-6-3"> <div class="outline-text-3" id="text-6-3">
<p> <p>
<a id="org07ed6ab"></a> <a id="org5f660cc"></a>
</p> </p>
<p> <p>
@ -1383,7 +1381,7 @@ end
</div> </div>
<div id="postamble" class="status"> <div id="postamble" class="status">
<p class="author">Author: Thomas Dehaeze</p> <p class="author">Author: Thomas Dehaeze</p>
<p class="date">Created: 2020-10-02 ven. 18:25</p> <p class="date">Created: 2020-10-02 ven. 18:35</p>
</div> </div>
</body> </body>
</html> </html>

92
matlab/index.org
View File

@ -48,13 +48,13 @@
:END: :END:
* Introduction :ignore: * Introduction :ignore:
In this document, the optimal and robust design of complementary filters is studied. This document is arranged as follows:
- Section [[sec:sensor_description]]: the sensors are described (dynamics, uncertainty, noise)
Two sensors are considered with both different noise characteristics and dynamical uncertainties represented by multiplicative input uncertainty. - Section [[sec:introduction_sensor_fusion]]: the sensor fusion architecture is described and the super sensor noise and dynamical uncertainty are derived
- Section [[sec:optimal_comp_filters]]: the $\mathcal{H}_2$ synthesis is used to design complementary filters such that the RMS value of the super sensor's noise is minimized - Section [[sec:optimal_comp_filters]]: the $\mathcal{H}_2$ synthesis is used to design complementary filters such that the RMS value of the super sensor's noise is minimized
- Section [[sec:comp_filter_robustness]]: the $\mathcal{H}_\infty$ synthesis is used to design complementary filters such that the super sensor's uncertainty is bonded to acceptable values - Section [[sec:comp_filter_robustness]]: the $\mathcal{H}_\infty$ synthesis is used to design complementary filters such that the super sensor's uncertainty is bonded to acceptable values
- Section [[sec:mixed_synthesis_sensor_fusion]]: the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis is used to both limit the super sensor's uncertainty and to lower the RMS value of the super sensor's noise - Section [[sec:mixed_synthesis_sensor_fusion]]: the mixed $\mathcal{H}_2/\mathcal{H}_\infty$ synthesis is used to both limit the super sensor's uncertainty and to lower the RMS value of the super sensor's noise
- Section [[sec:matlab_functions]]: Matlab functions used for the analysis are described
* Sensor Description * Sensor Description
:PROPERTIES: :PROPERTIES:
@ -307,90 +307,6 @@ All the dynamical systems representing the sensors are saved for further use.
save('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1'); save('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
#+end_src #+end_src
* First Order Complementary Filters :noexport:
** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
#+end_src
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src
#+begin_src matlab
load('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
#+end_src
** Complementary Filters
#+begin_src matlab
wc = 2*pi*400;
H1 = s/wc/(1 + s/wc);
H2 = 1/(1 + s/wc);
#+end_src
#+begin_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;
#+end_src
#+begin_src matlab
G2_u = G2*(1 + W2*ultidyn('Delta',[1 1]));
G1_u = G1*(1 + W1*ultidyn('Delta',[1 1]));
Gss_u = H1*inv(G1)*G1_u + H2*inv(G2)*G2_u;
#+end_src
#+begin_src matlab :exports none
Dphi1 = 180/pi*asin(abs(squeeze(freqresp(W1, freqs, 'Hz'))));
Dphi1(abs(squeeze(freqresp(W1, freqs, 'Hz'))) > 1) = 360;
Dphi2 = 180/pi*asin(abs(squeeze(freqresp(W2, freqs, 'Hz'))));
Dphi2(abs(squeeze(freqresp(W2, freqs, 'Hz'))) > 1) = 360;
Dphi_ss = 180/pi*asin(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))));
Dphi_ss(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))) > 1) = 360;
figure;
% Magnitude
ax1 = subplot(2,1,1);
hold on;
p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W1, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W1, freqs, 'Hz'))), 1e-6))], 'w');
p.FaceColor = [0 0.4470 0.7410];
p.EdgeColor = 'none';
p.FaceAlpha = 0.3;
p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W2, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W2, freqs, 'Hz'))), 0.001))], 'w');
p.FaceColor = [0.8500 0.3250 0.0980];
p.EdgeColor = 'none';
p.FaceAlpha = 0.3;
p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))+abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))-abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 0.001))], 'w');
p.EdgeColor = 'black';
p.FaceAlpha = 0;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');
ylim([1e-2, 1e1]);
hold off;
% Phase
ax2 = subplot(2,1,2);
hold on;
p = patch([freqs flip(freqs)], [Dphi1; flip(-Dphi1)], 'w');
p.FaceColor = [0 0.4470 0.7410]; p.EdgeColor = 'none'; p.FaceAlpha = 0.3;
p = patch([freqs flip(freqs)], [Dphi2; flip(-Dphi2)], 'w');
p.FaceColor = [0.8500 0.3250 0.0980]; p.EdgeColor = 'none'; p.FaceAlpha = 0.3;
p = patch([freqs flip(freqs)], [Dphi_ss; flip(-Dphi_ss)], 'w');
p.EdgeColor = 'black'; p.FaceAlpha = 0;
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
#+end_src
* Introduction to Sensor Fusion * Introduction to Sensor Fusion
<<sec:introduction_sensor_fusion>> <<sec:introduction_sensor_fusion>>

3686
matlab/index_old.html
View File

File diff suppressed because it is too large Load Diff

84
matlab/index_old.org
View File

@ -4909,6 +4909,90 @@ Velocity Signal:
bodeFig({s*C_acc*G_acc, C_geo*G_geo, s*C_acc*G_acc*H1+C_geo*G_geo*H2}, struct('phase', true)) bodeFig({s*C_acc*G_acc, C_geo*G_geo, s*C_acc*G_acc*H1+C_geo*G_geo*H2}, struct('phase', true))
#+end_src #+end_src
* First Order Complementary Filters :noexport:
** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
#+end_src
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src
#+begin_src matlab
load('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
#+end_src
** Complementary Filters
#+begin_src matlab
wc = 2*pi*400;
H1 = s/wc/(1 + s/wc);
H2 = 1/(1 + s/wc);
#+end_src
#+begin_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;
#+end_src
#+begin_src matlab
G2_u = G2*(1 + W2*ultidyn('Delta',[1 1]));
G1_u = G1*(1 + W1*ultidyn('Delta',[1 1]));
Gss_u = H1*inv(G1)*G1_u + H2*inv(G2)*G2_u;
#+end_src
#+begin_src matlab :exports none
Dphi1 = 180/pi*asin(abs(squeeze(freqresp(W1, freqs, 'Hz'))));
Dphi1(abs(squeeze(freqresp(W1, freqs, 'Hz'))) > 1) = 360;
Dphi2 = 180/pi*asin(abs(squeeze(freqresp(W2, freqs, 'Hz'))));
Dphi2(abs(squeeze(freqresp(W2, freqs, 'Hz'))) > 1) = 360;
Dphi_ss = 180/pi*asin(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))));
Dphi_ss(abs(squeeze(freqresp(W2*H2, freqs, 'Hz'))) + abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))) > 1) = 360;
figure;
% Magnitude
ax1 = subplot(2,1,1);
hold on;
p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W1, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W1, freqs, 'Hz'))), 1e-6))], 'w');
p.FaceColor = [0 0.4470 0.7410];
p.EdgeColor = 'none';
p.FaceAlpha = 0.3;
p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W2, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W2, freqs, 'Hz'))), 0.001))], 'w');
p.FaceColor = [0.8500 0.3250 0.0980];
p.EdgeColor = 'none';
p.FaceAlpha = 0.3;
p = patch([freqs flip(freqs)], [1 + abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))+abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))); flip(max(1 - abs(squeeze(freqresp(W2*H2, freqs, 'Hz')))-abs(squeeze(freqresp(W1*H1, freqs, 'Hz'))), 0.001))], 'w');
p.EdgeColor = 'black';
p.FaceAlpha = 0;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]);
ylabel('Magnitude');
ylim([1e-2, 1e1]);
hold off;
% Phase
ax2 = subplot(2,1,2);
hold on;
p = patch([freqs flip(freqs)], [Dphi1; flip(-Dphi1)], 'w');
p.FaceColor = [0 0.4470 0.7410]; p.EdgeColor = 'none'; p.FaceAlpha = 0.3;
p = patch([freqs flip(freqs)], [Dphi2; flip(-Dphi2)], 'w');
p.FaceColor = [0.8500 0.3250 0.0980]; p.EdgeColor = 'none'; p.FaceAlpha = 0.3;
p = patch([freqs flip(freqs)], [Dphi_ss; flip(-Dphi_ss)], 'w');
p.EdgeColor = 'black'; p.FaceAlpha = 0;
set(gca,'xscale','log');
yticks(-180:90:180);
ylim([-180 180]);
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
#+end_src
* Bibliography :ignore: * Bibliography :ignore:
bibliographystyle:unsrt bibliographystyle:unsrt
bibliography:ref.bib bibliography:ref.bib