Add Introduction
This commit is contained in:
parent
53619b5db3
commit
81074516d5
@ -3,7 +3,7 @@
|
||||
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
|
||||
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
|
||||
<head>
|
||||
<!-- 2020-10-02 ven. 18:25 -->
|
||||
<!-- 2020-10-02 ven. 18:35 -->
|
||||
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
|
||||
<title>Robust and Optimal Sensor Fusion - Matlab Computation</title>
|
||||
<meta name="generator" content="Org mode" />
|
||||
@ -35,50 +35,50 @@
|
||||
<h2>Table of Contents</h2>
|
||||
<div id="text-table-of-contents">
|
||||
<ul>
|
||||
<li><a href="#org42e0486">1. Sensor Description</a>
|
||||
<li><a href="#orgcc2172b">1. Sensor Description</a>
|
||||
<ul>
|
||||
<li><a href="#org647c690">1.1. Sensor Dynamics</a></li>
|
||||
<li><a href="#orge2ef6d5">1.2. Sensor Model Uncertainty</a></li>
|
||||
<li><a href="#org17664d8">1.3. Sensor Noise</a></li>
|
||||
<li><a href="#org18c8385">1.4. Save Model</a></li>
|
||||
<li><a href="#org23e78da">1.1. Sensor Dynamics</a></li>
|
||||
<li><a href="#org94edbba">1.2. Sensor Model Uncertainty</a></li>
|
||||
<li><a href="#org4cc404c">1.3. Sensor Noise</a></li>
|
||||
<li><a href="#orgca9547c">1.4. Save Model</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#org85fd6bc">2. Introduction to Sensor Fusion</a>
|
||||
<li><a href="#org8fb094e">2. Introduction to Sensor Fusion</a>
|
||||
<ul>
|
||||
<li><a href="#orgd9728f2">2.1. Sensor Fusion Architecture</a></li>
|
||||
<li><a href="#org211028b">2.2. Super Sensor Noise</a></li>
|
||||
<li><a href="#org1f329be">2.3. Super Sensor Dynamical Uncertainty</a></li>
|
||||
<li><a href="#org38e7087">2.1. Sensor Fusion Architecture</a></li>
|
||||
<li><a href="#org3ba53c4">2.2. Super Sensor Noise</a></li>
|
||||
<li><a href="#org0f97eb1">2.3. Super Sensor Dynamical Uncertainty</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#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>
|
||||
<li><a href="#orgbbd13bd">3.1. \(\mathcal{H}_2\) Synthesis</a></li>
|
||||
<li><a href="#org0d86fd6">3.2. Super Sensor Noise</a></li>
|
||||
<li><a href="#org238d359">3.3. Discrepancy between sensor dynamics and model</a></li>
|
||||
<li><a href="#org3f9017b">3.1. \(\mathcal{H}_2\) Synthesis</a></li>
|
||||
<li><a href="#orgb5bc159">3.2. Super Sensor Noise</a></li>
|
||||
<li><a href="#orge0269cd">3.3. Discrepancy between sensor dynamics and model</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#orge767c66">4. Robust Sensor Fusion: \(\mathcal{H}_\infty\) Synthesis</a>
|
||||
<li><a href="#org869a9ab">4. Robust Sensor Fusion: \(\mathcal{H}_\infty\) Synthesis</a>
|
||||
<ul>
|
||||
<li><a href="#orgdba13d6">4.1. Weighting Function used to bound the super sensor uncertainty</a></li>
|
||||
<li><a href="#org440d265">4.2. \(\mathcal{H}_\infty\) Synthesis</a></li>
|
||||
<li><a href="#org754b710">4.3. Super sensor uncertainty</a></li>
|
||||
<li><a href="#org3c71ac3">4.4. Super sensor noise</a></li>
|
||||
<li><a href="#org50d6031">4.5. Conclusion</a></li>
|
||||
<li><a href="#orgf20f681">4.1. Weighting Function used to bound the super sensor uncertainty</a></li>
|
||||
<li><a href="#orge540fc3">4.2. \(\mathcal{H}_\infty\) Synthesis</a></li>
|
||||
<li><a href="#org6d2d24e">4.3. Super sensor uncertainty</a></li>
|
||||
<li><a href="#org8ce77ee">4.4. Super sensor noise</a></li>
|
||||
<li><a href="#org8a94903">4.5. Conclusion</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#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>
|
||||
<li><a href="#org47226ff">5.1. Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis</a></li>
|
||||
<li><a href="#org6ce5da3">5.2. Obtained Super Sensor’s noise</a></li>
|
||||
<li><a href="#orgae2d47e">5.3. Obtained Super Sensor’s Uncertainty</a></li>
|
||||
<li><a href="#orgd41a044">5.4. Conclusion</a></li>
|
||||
<li><a href="#org360cf70">5.1. Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis</a></li>
|
||||
<li><a href="#org880e8a9">5.2. Obtained Super Sensor’s noise</a></li>
|
||||
<li><a href="#orgb149f25">5.3. Obtained Super Sensor’s Uncertainty</a></li>
|
||||
<li><a href="#orgfaf058b">5.4. Conclusion</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#orgecfb74c">6. Matlab Functions</a>
|
||||
<li><a href="#org2af7a78">6. Matlab Functions</a>
|
||||
<ul>
|
||||
<li><a href="#orgbc9616b">6.1. <code>createWeight</code></a></li>
|
||||
<li><a href="#org6933288">6.2. <code>plotMagUncertainty</code></a></li>
|
||||
<li><a href="#org351f1ef">6.3. <code>plotPhaseUncertainty</code></a></li>
|
||||
<li><a href="#orgfe9741d">6.1. <code>createWeight</code></a></li>
|
||||
<li><a href="#org6a88fe0">6.2. <code>plotMagUncertainty</code></a></li>
|
||||
<li><a href="#orgbff6dca">6.3. <code>plotPhaseUncertainty</code></a></li>
|
||||
</ul>
|
||||
</li>
|
||||
</ul>
|
||||
@ -86,32 +86,30 @@
|
||||
</div>
|
||||
|
||||
<p>
|
||||
In this document, the optimal and robust design of complementary filters is studied.
|
||||
This document is arranged as follows:
|
||||
</p>
|
||||
|
||||
<p>
|
||||
Two sensors are considered with both different noise characteristics and dynamical uncertainties represented by multiplicative input uncertainty.
|
||||
</p>
|
||||
|
||||
<ul class="org-ul">
|
||||
<li>Section <a href="#orge8a8e6b">3</a>: the \(\mathcal{H}_2\) synthesis is used to design complementary filters such that the RMS value of the super sensor’s noise is minimized</li>
|
||||
<li>Section <a href="#org0e5d4db">4</a>: the \(\mathcal{H}_\infty\) synthesis is used to design complementary filters such that the super sensor’s uncertainty is bonded to acceptable values</li>
|
||||
<li>Section <a href="#org310c590">5</a>: the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis is used to both limit the super sensor’s uncertainty and to lower the RMS value of the super sensor’s noise</li>
|
||||
<li>Section <a href="#org2f5f470">1</a>: the sensors are described (dynamics, uncertainty, noise)</li>
|
||||
<li>Section <a href="#org220320f">2</a>: the sensor fusion architecture is described and the super sensor noise and dynamical uncertainty are derived</li>
|
||||
<li>Section <a href="#org0f63067">3</a>: the \(\mathcal{H}_2\) synthesis is used to design complementary filters such that the RMS value of the super sensor’s noise is minimized</li>
|
||||
<li>Section <a href="#org25efa7e">4</a>: the \(\mathcal{H}_\infty\) synthesis is used to design complementary filters such that the super sensor’s uncertainty is bonded to acceptable values</li>
|
||||
<li>Section <a href="#org5163fb1">5</a>: the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis is used to both limit the super sensor’s uncertainty and to lower the RMS value of the super sensor’s noise</li>
|
||||
<li>Section <a href="#org71367a9">6</a>: Matlab functions used for the analysis are described</li>
|
||||
</ul>
|
||||
|
||||
<div id="outline-container-org42e0486" class="outline-2">
|
||||
<h2 id="org42e0486"><span class="section-number-2">1</span> Sensor Description</h2>
|
||||
<div id="outline-container-orgcc2172b" class="outline-2">
|
||||
<h2 id="orgcc2172b"><span class="section-number-2">1</span> Sensor Description</h2>
|
||||
<div class="outline-text-2" id="text-1">
|
||||
<p>
|
||||
<a id="org7b36852"></a>
|
||||
<a id="org2f5f470"></a>
|
||||
</p>
|
||||
<p>
|
||||
In Figure <a href="#org50592fc">1</a> is shown a schematic of a sensor model that is used in the following study.
|
||||
In Figure <a href="#orgd19c6fc">1</a> is shown a schematic of a sensor model that is used in the following study.
|
||||
In this example, the measured quantity \(x\) is the velocity of an object.
|
||||
</p>
|
||||
|
||||
<table id="orgbd57c1d" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
|
||||
<caption class="t-above"><span class="table-number">Table 1:</span> Description of signals in Figure <a href="#org50592fc">1</a></caption>
|
||||
<table id="orgc326093" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
|
||||
<caption class="t-above"><span class="table-number">Table 1:</span> Description of signals in Figure <a href="#orgd19c6fc">1</a></caption>
|
||||
|
||||
<colgroup>
|
||||
<col class="org-left" />
|
||||
@ -160,8 +158,8 @@ In this example, the measured quantity \(x\) is the velocity of an object.
|
||||
</tbody>
|
||||
</table>
|
||||
|
||||
<table id="orgd5bc759" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
|
||||
<caption class="t-above"><span class="table-number">Table 2:</span> Description of Systems in Figure <a href="#org50592fc">1</a></caption>
|
||||
<table id="orgf877ead" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
|
||||
<caption class="t-above"><span class="table-number">Table 2:</span> Description of Systems in Figure <a href="#orgd19c6fc">1</a></caption>
|
||||
|
||||
<colgroup>
|
||||
<col class="org-left" />
|
||||
@ -205,18 +203,18 @@ In this example, the measured quantity \(x\) is the velocity of an object.
|
||||
</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>
|
||||
<p><span class="figure-number">Figure 1: </span>Sensor Model</p>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org647c690" class="outline-3">
|
||||
<h3 id="org647c690"><span class="section-number-3">1.1</span> Sensor Dynamics</h3>
|
||||
<div id="outline-container-org23e78da" class="outline-3">
|
||||
<h3 id="org23e78da"><span class="section-number-3">1.1</span> Sensor Dynamics</h3>
|
||||
<div class="outline-text-3" id="text-1-1">
|
||||
<p>
|
||||
<a id="org3d3853d"></a>
|
||||
<a id="org4813033"></a>
|
||||
Let’s consider two sensors measuring the velocity of an object.
|
||||
</p>
|
||||
|
||||
@ -247,14 +245,14 @@ G2 = g_pos/s/(1 + s/w_pos); % Position Sensor Plant [V/(m/s)]
|
||||
|
||||
<p>
|
||||
These nominal dynamics are also taken as the model of the sensor dynamics.
|
||||
The true sensor dynamics has some uncertainty associated to it and described in section <a href="#org3c9ef6e">1.2</a>.
|
||||
The true sensor dynamics has some uncertainty associated to it and described in section <a href="#org992a983">1.2</a>.
|
||||
</p>
|
||||
|
||||
<p>
|
||||
Both sensor dynamics in \([\frac{V}{m/s}]\) are shown in Figure <a href="#orgdf43a57">2</a>.
|
||||
Both sensor dynamics in \([\frac{V}{m/s}]\) are shown in Figure <a href="#org6409089">2</a>.
|
||||
</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>
|
||||
<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 id="outline-container-orge2ef6d5" class="outline-3">
|
||||
<h3 id="orge2ef6d5"><span class="section-number-3">1.2</span> Sensor Model Uncertainty</h3>
|
||||
<div id="outline-container-org94edbba" class="outline-3">
|
||||
<h3 id="org94edbba"><span class="section-number-3">1.2</span> Sensor Model Uncertainty</h3>
|
||||
<div class="outline-text-3" id="text-1-2">
|
||||
<p>
|
||||
<a id="org3c9ef6e"></a>
|
||||
The uncertainty on the sensor dynamics is described by multiplicative uncertainty (Figure <a href="#org50592fc">1</a>).
|
||||
<a id="org992a983"></a>
|
||||
The uncertainty on the sensor dynamics is described by multiplicative uncertainty (Figure <a href="#orgd19c6fc">1</a>).
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -279,7 +277,7 @@ The true sensor dynamics \(G_i(s)\) is then described by \eqref{eq:sensor_dynami
|
||||
\end{equation}
|
||||
|
||||
<p>
|
||||
The weights \(W_i(s)\) representing the dynamical uncertainty are defined below and their magnitude is shown in Figure <a href="#org5137ef5">3</a>.
|
||||
The weights \(W_i(s)\) representing the dynamical uncertainty are defined below and their magnitude is shown in Figure <a href="#org8beb676">3</a>.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">W1 = createWeight('n', 2, 'w0', 2*pi*3, 'G0', 2, 'G1', 0.1, 'Gc', 1) * ...
|
||||
@ -290,18 +288,18 @@ W2 = createWeight('n', 2, 'w0', 2*pi*1e2, 'G0', 0.05, 'G1', 4, 'Gc', 1);
|
||||
</div>
|
||||
|
||||
<p>
|
||||
The bode plot of the sensors nominal dynamics as well as their defined dynamical spread are shown in Figure <a href="#orgfec46ba">4</a>.
|
||||
The bode plot of the sensors nominal dynamics as well as their defined dynamical spread are shown in Figure <a href="#org751f977">4</a>.
|
||||
</p>
|
||||
|
||||
|
||||
<div id="org5137ef5" class="figure">
|
||||
<div id="org8beb676" class="figure">
|
||||
<p><img src="figs/sensors_uncertainty_weights.png" alt="sensors_uncertainty_weights.png" />
|
||||
</p>
|
||||
<p><span class="figure-number">Figure 3: </span>Magnitude of the multiplicative uncertainty weights \(|W_i(j\omega)|\)</p>
|
||||
</div>
|
||||
|
||||
|
||||
<div id="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>
|
||||
<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 id="outline-container-org17664d8" class="outline-3">
|
||||
<h3 id="org17664d8"><span class="section-number-3">1.3</span> Sensor Noise</h3>
|
||||
<div id="outline-container-org4cc404c" class="outline-3">
|
||||
<h3 id="org4cc404c"><span class="section-number-3">1.3</span> Sensor Noise</h3>
|
||||
<div class="outline-text-3" id="text-1-3">
|
||||
<p>
|
||||
<a id="org9362cd8"></a>
|
||||
The noise of the sensors \(n_i\) are modelled by shaping a white noise with unitary PSD \(\tilde{n}_i\) \eqref{eq:unitary_noise_psd} with a LTI transfer function \(N_i(s)\) (Figure <a href="#org50592fc">1</a>).
|
||||
<a id="org0cc1978"></a>
|
||||
The noise of the sensors \(n_i\) are modelled by shaping a white noise with unitary PSD \(\tilde{n}_i\) \eqref{eq:unitary_noise_psd} with a LTI transfer function \(N_i(s)\) (Figure <a href="#orgd19c6fc">1</a>).
|
||||
</p>
|
||||
\begin{equation}
|
||||
\Phi_{\tilde{n}_i}(\omega) = 1 \label{eq:unitary_noise_psd}
|
||||
@ -328,7 +326,7 @@ The Power Spectral Density of the sensor noise \(\Phi_{n_i}(\omega)\) is then co
|
||||
\end{equation}
|
||||
|
||||
<p>
|
||||
The weights \(N_1\) and \(N_2\) representing the amplitude spectral density of the sensor noises are defined below and shown in Figure <a href="#org953924d">5</a>.
|
||||
The weights \(N_1\) and \(N_2\) representing the amplitude spectral density of the sensor noises are defined below and shown in Figure <a href="#org2d39969">5</a>.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">omegac = 0.15*2*pi; G0 = 1e-1; Ginf = 1e-6;
|
||||
@ -340,7 +338,7 @@ N2 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4);
|
||||
</div>
|
||||
|
||||
|
||||
<div id="org953924d" class="figure">
|
||||
<div id="org2d39969" class="figure">
|
||||
<p><img src="figs/sensors_noise.png" alt="sensors_noise.png" />
|
||||
</p>
|
||||
<p><span class="figure-number">Figure 5: </span>Amplitude spectral density of the sensors \(\sqrt{\Phi_{n_i}(\omega)} = |N_i(j\omega)|\)</p>
|
||||
@ -348,8 +346,8 @@ N2 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4);
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org18c8385" class="outline-3">
|
||||
<h3 id="org18c8385"><span class="section-number-3">1.4</span> Save Model</h3>
|
||||
<div id="outline-container-orgca9547c" class="outline-3">
|
||||
<h3 id="orgca9547c"><span class="section-number-3">1.4</span> Save Model</h3>
|
||||
<div class="outline-text-3" id="text-1-4">
|
||||
<p>
|
||||
All the dynamical systems representing the sensors are saved for further use.
|
||||
@ -363,27 +361,27 @@ All the dynamical systems representing the sensors are saved for further use.
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org85fd6bc" class="outline-2">
|
||||
<h2 id="org85fd6bc"><span class="section-number-2">2</span> Introduction to Sensor Fusion</h2>
|
||||
<div id="outline-container-org8fb094e" class="outline-2">
|
||||
<h2 id="org8fb094e"><span class="section-number-2">2</span> Introduction to Sensor Fusion</h2>
|
||||
<div class="outline-text-2" id="text-2">
|
||||
<p>
|
||||
<a id="org39c4402"></a>
|
||||
<a id="org220320f"></a>
|
||||
</p>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgd9728f2" class="outline-3">
|
||||
<h3 id="orgd9728f2"><span class="section-number-3">2.1</span> Sensor Fusion Architecture</h3>
|
||||
<div id="outline-container-org38e7087" class="outline-3">
|
||||
<h3 id="org38e7087"><span class="section-number-3">2.1</span> Sensor Fusion Architecture</h3>
|
||||
<div class="outline-text-3" id="text-2-1">
|
||||
<p>
|
||||
<a id="orgc00e6f9"></a>
|
||||
<a id="org82e93f4"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
The two sensors presented in Section <a href="#org7b36852">1</a> are now merged together using complementary filters \(H_1(s)\) and \(H_2(s)\) to form a super sensor (Figure <a href="#org71ac5d2">6</a>).
|
||||
The two sensors presented in Section <a href="#org2f5f470">1</a> are now merged together using complementary filters \(H_1(s)\) and \(H_2(s)\) to form a super sensor (Figure <a href="#org8c9eb16">6</a>).
|
||||
</p>
|
||||
|
||||
|
||||
<div id="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>
|
||||
<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 id="outline-container-org211028b" class="outline-3">
|
||||
<h3 id="org211028b"><span class="section-number-3">2.2</span> Super Sensor Noise</h3>
|
||||
<div id="outline-container-org3ba53c4" class="outline-3">
|
||||
<h3 id="org3ba53c4"><span class="section-number-3">2.2</span> Super Sensor Noise</h3>
|
||||
<div class="outline-text-3" id="text-2-2">
|
||||
<p>
|
||||
<a id="org2f8cec7"></a>
|
||||
<a id="orgd0b1104"></a>
|
||||
</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 id="outline-container-org1f329be" class="outline-3">
|
||||
<h3 id="org1f329be"><span class="section-number-3">2.3</span> Super Sensor Dynamical Uncertainty</h3>
|
||||
<div id="outline-container-org0f97eb1" class="outline-3">
|
||||
<h3 id="org0f97eb1"><span class="section-number-3">2.3</span> Super Sensor Dynamical Uncertainty</h3>
|
||||
<div class="outline-text-3" id="text-2-3">
|
||||
<p>
|
||||
<a id="org0dafc5d"></a>
|
||||
<a id="orgb59d475"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
If we consider some dynamical uncertainty (the true system dynamics \(G_i\) not being perfectly equal to our model \(\hat{G}_i\)) that we model by the use of multiplicative uncertainty (Figure <a href="#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>
|
||||
|
||||
\begin{equation}
|
||||
@ -462,18 +460,18 @@ If we consider some dynamical uncertainty (the true system dynamics \(G_i\) not
|
||||
\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>
|
||||
<p><span class="figure-number">Figure 7: </span>Sensor Model including Dynamical Uncertainty</p>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
The uncertainty set of the transfer function from \(\hat{x}\) to \(x\) at frequency \(\omega\) is bounded in the complex plane by a circle centered on 1 and with a radius equal to \(|W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)|\) as shown in Figure <a href="#org27e67a3">8</a>.
|
||||
The uncertainty set of the transfer function from \(\hat{x}\) to \(x\) at frequency \(\omega\) is bounded in the complex plane by a circle centered on 1 and with a radius equal to \(|W_1(j\omega) H_1(j\omega)| + |W_2(j\omega) H_2(j\omega)|\) as shown in Figure <a href="#orgfd5d4b3">8</a>.
|
||||
</p>
|
||||
|
||||
|
||||
<div id="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>
|
||||
<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 id="outline-container-org106ad63" class="outline-2">
|
||||
<h2 id="org106ad63"><span class="section-number-2">3</span> Optimal Super Sensor Noise: \(\mathcal{H}_2\) Synthesis</h2>
|
||||
<div id="outline-container-orgf2e7afd" class="outline-2">
|
||||
<h2 id="orgf2e7afd"><span class="section-number-2">3</span> Optimal Super Sensor Noise: \(\mathcal{H}_2\) Synthesis</h2>
|
||||
<div class="outline-text-2" id="text-3">
|
||||
<p>
|
||||
<a id="orge8a8e6b"></a>
|
||||
<a id="org0f63067"></a>
|
||||
</p>
|
||||
<p>
|
||||
In this section, the complementary filters \(H_1(s)\) and \(H_2(s)\) are designed in order to minimize the RMS value of super sensor noise \(\sigma_n\).
|
||||
</p>
|
||||
|
||||
|
||||
<div id="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>
|
||||
<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>
|
||||
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>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgbbd13bd" class="outline-3">
|
||||
<h3 id="orgbbd13bd"><span class="section-number-3">3.1</span> \(\mathcal{H}_2\) Synthesis</h3>
|
||||
<div id="outline-container-org3f9017b" class="outline-3">
|
||||
<h3 id="org3f9017b"><span class="section-number-3">3.1</span> \(\mathcal{H}_2\) Synthesis</h3>
|
||||
<div class="outline-text-3" id="text-3-1">
|
||||
<p>
|
||||
<a id="org717549f"></a>
|
||||
<a id="orgc3c8ce2"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
Consider the generalized plant \(P_{\mathcal{H}_2}\) shown in Figure <a href="#org6124aa7">10</a> and described by Equation \eqref{eq:H2_generalized_plant}.
|
||||
Consider the generalized plant \(P_{\mathcal{H}_2}\) shown in Figure <a href="#orgfed5b00">10</a> and described by Equation \eqref{eq:H2_generalized_plant}.
|
||||
</p>
|
||||
|
||||
|
||||
<div id="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>
|
||||
<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>
|
||||
|
||||
<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>
|
||||
|
||||
<div id="orgee5ee62" class="figure">
|
||||
<div id="org3561716" class="figure">
|
||||
<p><img src="figs/htwo_comp_filters.png" alt="htwo_comp_filters.png" />
|
||||
</p>
|
||||
<p><span class="figure-number">Figure 11: </span>Obtained complementary filters using the \(\mathcal{H}_2\) Synthesis</p>
|
||||
@ -594,15 +592,15 @@ The obtained complementary filters are shown in Figure <a href="#orgee5ee62">11<
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org0d86fd6" class="outline-3">
|
||||
<h3 id="org0d86fd6"><span class="section-number-3">3.2</span> Super Sensor Noise</h3>
|
||||
<div id="outline-container-orgb5bc159" class="outline-3">
|
||||
<h3 id="orgb5bc159"><span class="section-number-3">3.2</span> Super Sensor Noise</h3>
|
||||
<div class="outline-text-3" id="text-3-2">
|
||||
<p>
|
||||
<a id="org366e497"></a>
|
||||
<a id="org22dd161"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
The Power Spectral Density of the individual sensors’ noise \(\Phi_{n_1}, \Phi_{n_2}\) and of the super sensor noise \(\Phi_{n_{\mathcal{H}_2}}\) are computed below and shown in Figure <a href="#org98bc6c9">12</a>.
|
||||
The Power Spectral Density of the individual sensors’ noise \(\Phi_{n_1}, \Phi_{n_2}\) and of the super sensor noise \(\Phi_{n_{\mathcal{H}_2}}\) are computed below and shown in Figure <a href="#orgad2796b">12</a>.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">PSD_S1 = abs(squeeze(freqresp(N1, freqs, 'Hz'))).^2;
|
||||
@ -613,9 +611,9 @@ PSD_H2 = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2 + ...
|
||||
</div>
|
||||
|
||||
<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>
|
||||
<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>
|
||||
|
||||
<colgroup>
|
||||
@ -648,7 +646,7 @@ The RMS value of the individual sensors and of the super sensor are listed in Ta
|
||||
</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>
|
||||
<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>
|
||||
A time domain simulation is now performed.
|
||||
The measured velocity \(x\) is set to be a sweep sine with an amplitude of \(0.1\ [m/s]\).
|
||||
The velocity estimates from the two sensors and from the super sensors are shown in Figure <a href="#org1ecf54f">13</a>.
|
||||
The resulting noises are displayed in Figure <a href="#org026504e">14</a>.
|
||||
The velocity estimates from the two sensors and from the super sensors are shown in Figure <a href="#org50620bf">13</a>.
|
||||
The resulting noises are displayed in Figure <a href="#orga4919f6">14</a>.
|
||||
</p>
|
||||
|
||||
|
||||
<div id="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>
|
||||
<p><span class="figure-number">Figure 13: </span>Noise of individual sensors and noise of the super sensor</p>
|
||||
</div>
|
||||
|
||||
|
||||
<div id="org026504e" class="figure">
|
||||
<div id="orga4919f6" class="figure">
|
||||
<p><img src="figs/sensor_noise_H2_time_domain.png" alt="sensor_noise_H2_time_domain.png" />
|
||||
</p>
|
||||
<p><span class="figure-number">Figure 14: </span>Noise of the two sensors \(n_1, n_2\) and noise of the super sensor \(n\)</p>
|
||||
@ -677,15 +675,15 @@ The resulting noises are displayed in Figure <a href="#org026504e">14</a>.
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org238d359" class="outline-3">
|
||||
<h3 id="org238d359"><span class="section-number-3">3.3</span> Discrepancy between sensor dynamics and model</h3>
|
||||
<div id="outline-container-orge0269cd" class="outline-3">
|
||||
<h3 id="orge0269cd"><span class="section-number-3">3.3</span> Discrepancy between sensor dynamics and model</h3>
|
||||
<div class="outline-text-3" id="text-3-3">
|
||||
<p>
|
||||
If we consider sensor dynamical uncertainty as explained in Section <a href="#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>
|
||||
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>
|
||||
@ -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.
|
||||
</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>
|
||||
<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 id="outline-container-orge767c66" class="outline-2">
|
||||
<h2 id="orge767c66"><span class="section-number-2">4</span> Robust Sensor Fusion: \(\mathcal{H}_\infty\) Synthesis</h2>
|
||||
<div id="outline-container-org869a9ab" class="outline-2">
|
||||
<h2 id="org869a9ab"><span class="section-number-2">4</span> Robust Sensor Fusion: \(\mathcal{H}_\infty\) Synthesis</h2>
|
||||
<div class="outline-text-2" id="text-4">
|
||||
<p>
|
||||
<a id="org0e5d4db"></a>
|
||||
<a id="org25efa7e"></a>
|
||||
</p>
|
||||
<p>
|
||||
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>
|
||||
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>
|
||||
|
||||
|
||||
<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>
|
||||
<p><span class="figure-number">Figure 16: </span>Sensor fusion architecture with sensor dynamics uncertainty</p>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
As explained in Section <a href="#org3c9ef6e">1.2</a>, at each frequency \(\omega\), the dynamical uncertainty of the super sensor can be represented in the complex plane by a circle with a radius equals to \(|H_1(j\omega) W_1(j\omega)| + |H_2(j\omega) W_2(j\omega)|\) and centered on 1.
|
||||
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>
|
||||
@ -747,7 +745,7 @@ In order to specify a wanted upper bound on the dynamical uncertainty, a weight
|
||||
\end{align}
|
||||
|
||||
<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>
|
||||
|
||||
|
||||
@ -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>
|
||||
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>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgdba13d6" class="outline-3">
|
||||
<h3 id="orgdba13d6"><span class="section-number-3">4.1</span> Weighting Function used to bound the super sensor uncertainty</h3>
|
||||
<div id="outline-container-orgf20f681" class="outline-3">
|
||||
<h3 id="orgf20f681"><span class="section-number-3">4.1</span> Weighting Function used to bound the super sensor uncertainty</h3>
|
||||
<div class="outline-text-3" id="text-4-1">
|
||||
<p>
|
||||
<a id="orgc44063d"></a>
|
||||
<a id="org875b4b0"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -786,7 +784,7 @@ This is done using the \(\mathcal{H}_\infty\) synthesis in Section <a href="#org
|
||||
\end{align}
|
||||
|
||||
<p>
|
||||
The uncertainty bounds of the two individual sensor as well as the wanted maximum uncertainty bounds of the super sensor are shown in Figure <a href="#org401a239">17</a>.
|
||||
The uncertainty bounds of the two individual sensor as well as the wanted maximum uncertainty bounds of the super sensor are shown in Figure <a href="#orgffb63ca">17</a>.
|
||||
</p>
|
||||
|
||||
<div class="org-src-container">
|
||||
@ -797,7 +795,7 @@ Wu = createWeight('n', 2, 'w0', 2*pi*4e2, 'G0', 1/sin(Dphi*pi/180), 'G1', 1/4, '
|
||||
</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>
|
||||
<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 id="outline-container-org440d265" class="outline-3">
|
||||
<h3 id="org440d265"><span class="section-number-3">4.2</span> \(\mathcal{H}_\infty\) Synthesis</h3>
|
||||
<div id="outline-container-orge540fc3" class="outline-3">
|
||||
<h3 id="orge540fc3"><span class="section-number-3">4.2</span> \(\mathcal{H}_\infty\) Synthesis</h3>
|
||||
<div class="outline-text-3" id="text-4-2">
|
||||
<p>
|
||||
<a id="orga929244"></a>
|
||||
<a id="org2b41098"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
The generalized plant \(P_{\mathcal{H}_\infty}\) used for the \(\mathcal{H}_\infty\) Synthesis of the complementary filters is shown in Figure <a href="#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>
|
||||
|
||||
|
||||
<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>
|
||||
<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>
|
||||
|
||||
<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>
|
||||
|
||||
|
||||
<div id="orge56327e" class="figure">
|
||||
<div id="org7a55110" class="figure">
|
||||
<p><img src="figs/hinf_comp_filters.png" alt="hinf_comp_filters.png" />
|
||||
</p>
|
||||
<p><span class="figure-number">Figure 19: </span>Obtained complementary filters using the \(\mathcal{H}_\infty\) Synthesis</p>
|
||||
@ -896,11 +894,11 @@ The obtained complementary filters as well as the wanted upper bounds are shown
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org754b710" class="outline-3">
|
||||
<h3 id="org754b710"><span class="section-number-3">4.3</span> Super sensor uncertainty</h3>
|
||||
<div id="outline-container-org6d2d24e" class="outline-3">
|
||||
<h3 id="org6d2d24e"><span class="section-number-3">4.3</span> Super sensor uncertainty</h3>
|
||||
<div class="outline-text-3" id="text-4-3">
|
||||
<p>
|
||||
The super sensor dynamical uncertainty is displayed in Figure <a href="#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)\).
|
||||
</p>
|
||||
|
||||
@ -909,7 +907,7 @@ The \(\mathcal{H}_\infty\) synthesis thus allows to design filters such that the
|
||||
</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>
|
||||
<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 id="outline-container-org3c71ac3" class="outline-3">
|
||||
<h3 id="org3c71ac3"><span class="section-number-3">4.4</span> Super sensor noise</h3>
|
||||
<div id="outline-container-org8ce77ee" class="outline-3">
|
||||
<h3 id="org8ce77ee"><span class="section-number-3">4.4</span> Super sensor noise</h3>
|
||||
<div class="outline-text-3" id="text-4-4">
|
||||
<p>
|
||||
We now compute the obtain Power Spectral Density of the super sensor’s noise (Figure <a href="#org0ee17aa">21</a>).
|
||||
We now compute the obtain Power Spectral Density of the super sensor’s noise (Figure <a href="#org3ff8b63">21</a>).
|
||||
</p>
|
||||
|
||||
<p>
|
||||
The obtained RMS of the super sensor noise in the \(\mathcal{H}_2\) and \(\mathcal{H}_\infty\) case are shown in Table <a href="#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.
|
||||
</p>
|
||||
|
||||
@ -938,13 +936,13 @@ PSD_Hinf = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2 + ...
|
||||
</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>
|
||||
<p><span class="figure-number">Figure 21: </span>Power Spectral Density of the estimated \(\hat{x}\) using the two sensors alone and using the</p>
|
||||
</div>
|
||||
|
||||
<table id="org9031855" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
|
||||
<table id="org2b06274" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
|
||||
<caption class="t-above"><span class="table-number">Table 4:</span> Comparison of the obtained RMS noise of the super sensor</caption>
|
||||
|
||||
<colgroup>
|
||||
@ -973,8 +971,8 @@ PSD_Hinf = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2 + ...
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org50d6031" class="outline-3">
|
||||
<h3 id="org50d6031"><span class="section-number-3">4.5</span> Conclusion</h3>
|
||||
<div id="outline-container-org8a94903" class="outline-3">
|
||||
<h3 id="org8a94903"><span class="section-number-3">4.5</span> Conclusion</h3>
|
||||
<div class="outline-text-3" id="text-4-5">
|
||||
<p>
|
||||
Using the \(\mathcal{H}_\infty\) synthesis, the dynamical uncertainty of the super sensor can be bounded to acceptable values.
|
||||
@ -987,22 +985,22 @@ However, the RMS of the super sensor noise is not optimized as it was the case w
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org6230c6a" class="outline-2">
|
||||
<h2 id="org6230c6a"><span class="section-number-2">5</span> Optimal and Robust Sensor Fusion: Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis</h2>
|
||||
<div id="outline-container-org919d3da" class="outline-2">
|
||||
<h2 id="org919d3da"><span class="section-number-2">5</span> Optimal and Robust Sensor Fusion: Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis</h2>
|
||||
<div class="outline-text-2" id="text-5">
|
||||
<p>
|
||||
<a id="org310c590"></a>
|
||||
<a id="org5163fb1"></a>
|
||||
</p>
|
||||
<p>
|
||||
The (optima) \(\mathcal{H}_2\) synthesis and the (robust) \(\mathcal{H}_\infty\) synthesis are now combined to form an Optimal and Robust synthesis of complementary filters for sensor fusion.
|
||||
</p>
|
||||
|
||||
<p>
|
||||
The sensor fusion architecture is shown in Figure <a href="#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>
|
||||
|
||||
|
||||
<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>
|
||||
<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>
|
||||
|
||||
<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>
|
||||
</div>
|
||||
<div id="outline-container-org47226ff" class="outline-3">
|
||||
<h3 id="org47226ff"><span class="section-number-3">5.1</span> Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis</h3>
|
||||
<div id="outline-container-org360cf70" class="outline-3">
|
||||
<h3 id="org360cf70"><span class="section-number-3">5.1</span> Mixed \(\mathcal{H}_2\) / \(\mathcal{H}_\infty\) Synthesis</h3>
|
||||
<div class="outline-text-3" id="text-5-1">
|
||||
<p>
|
||||
<a id="orgac300ed"></a>
|
||||
<a id="orgdf4b501"></a>
|
||||
</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>
|
||||
@ -1040,7 +1038,7 @@ The filter \(H_2(s)\) is synthesized such that it:
|
||||
</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>
|
||||
<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.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">[H2, ~] = h2hinfsyn(ss(P), 1, 1, 2, [0, 1], 'HINFMAX', 1, 'H2MAX', Inf, 'DKMAX', 100, 'TOL', 0.01, 'DISPLAY', 'on');
|
||||
<pre class="src src-matlab">[H2, ~] = h2hinfsyn(ss(P), 1, 1, 2, [0, 1], 'HINFMAX', 1, 'H2MAX', Inf, 'DKMAX', 100, 'TOL', 1e-3, 'DISPLAY', 'on');
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
@ -1084,11 +1082,11 @@ And the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis is performed.
|
||||
</div>
|
||||
|
||||
<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>
|
||||
|
||||
|
||||
<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>
|
||||
<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 id="outline-container-org6ce5da3" class="outline-3">
|
||||
<h3 id="org6ce5da3"><span class="section-number-3">5.2</span> Obtained Super Sensor’s noise</h3>
|
||||
<div id="outline-container-org880e8a9" class="outline-3">
|
||||
<h3 id="org880e8a9"><span class="section-number-3">5.2</span> Obtained Super Sensor’s noise</h3>
|
||||
<div class="outline-text-3" id="text-5-2">
|
||||
<p>
|
||||
The Power Spectral Density of the super sensor’s noise is shown in Figure <a href="#orgdef3c54">25</a>.
|
||||
The Power Spectral Density of the super sensor’s noise is shown in Figure <a href="#org41f2620">25</a>.
|
||||
</p>
|
||||
|
||||
<p>
|
||||
A time domain simulation is shown in Figure <a href="#org2b3cbc2">26</a>.
|
||||
A time domain simulation is shown in Figure <a href="#orgd85ef6a">26</a>.
|
||||
</p>
|
||||
|
||||
<p>
|
||||
The RMS values of the super sensor noise for the presented three synthesis are listed in Table <a href="#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>
|
||||
|
||||
<div class="org-src-container">
|
||||
@ -1120,20 +1118,20 @@ PSD_H2Hinf = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2 + ...
|
||||
</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>
|
||||
<p><span class="figure-number">Figure 25: </span>Power Spectral Density of the Super Sensor obtained with the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis</p>
|
||||
</div>
|
||||
|
||||
|
||||
<div id="org2b3cbc2" class="figure">
|
||||
<div id="orgd85ef6a" class="figure">
|
||||
<p><img src="figs/super_sensor_time_domain_h2_hinf.png" alt="super_sensor_time_domain_h2_hinf.png" />
|
||||
</p>
|
||||
<p><span class="figure-number">Figure 26: </span>Noise of individual sensors and noise of the super sensor</p>
|
||||
</div>
|
||||
|
||||
<table id="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>
|
||||
|
||||
<colgroup>
|
||||
@ -1160,22 +1158,22 @@ PSD_H2Hinf = abs(squeeze(freqresp(N1*H1, freqs, 'Hz'))).^2 + ...
|
||||
|
||||
<tr>
|
||||
<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>
|
||||
</tbody>
|
||||
</table>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgae2d47e" class="outline-3">
|
||||
<h3 id="orgae2d47e"><span class="section-number-3">5.3</span> Obtained Super Sensor’s Uncertainty</h3>
|
||||
<div id="outline-container-orgb149f25" class="outline-3">
|
||||
<h3 id="orgb149f25"><span class="section-number-3">5.3</span> Obtained Super Sensor’s Uncertainty</h3>
|
||||
<div class="outline-text-3" id="text-5-3">
|
||||
<p>
|
||||
The uncertainty on the super sensor’s dynamics is shown in Figure <a href="#org5d933bb">27</a>.
|
||||
The uncertainty on the super sensor’s dynamics is shown in Figure <a href="#org37d0e3f">27</a>.
|
||||
</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>
|
||||
<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’s dynamics is shown in Figure <a href=
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgd41a044" class="outline-3">
|
||||
<h3 id="orgd41a044"><span class="section-number-3">5.4</span> Conclusion</h3>
|
||||
<div id="outline-container-orgfaf058b" class="outline-3">
|
||||
<h3 id="orgfaf058b"><span class="section-number-3">5.4</span> Conclusion</h3>
|
||||
<div class="outline-text-3" id="text-5-4">
|
||||
<p>
|
||||
The mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis of the complementary filters allows to:
|
||||
@ -1197,18 +1195,18 @@ The mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis of the complementary fi
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgecfb74c" class="outline-2">
|
||||
<h2 id="orgecfb74c"><span class="section-number-2">6</span> Matlab Functions</h2>
|
||||
<div id="outline-container-org2af7a78" class="outline-2">
|
||||
<h2 id="org2af7a78"><span class="section-number-2">6</span> Matlab Functions</h2>
|
||||
<div class="outline-text-2" id="text-6">
|
||||
<p>
|
||||
<a id="orgd6c9208"></a>
|
||||
<a id="org71367a9"></a>
|
||||
</p>
|
||||
</div>
|
||||
<div id="outline-container-orgbc9616b" class="outline-3">
|
||||
<h3 id="orgbc9616b"><span class="section-number-3">6.1</span> <code>createWeight</code></h3>
|
||||
<div id="outline-container-orgfe9741d" class="outline-3">
|
||||
<h3 id="orgfe9741d"><span class="section-number-3">6.1</span> <code>createWeight</code></h3>
|
||||
<div class="outline-text-3" id="text-6-1">
|
||||
<p>
|
||||
<a id="org81aea16"></a>
|
||||
<a id="org38a3120"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -1260,11 +1258,11 @@ This Matlab function is accessible <a href="src/createWeight.m">here</a>.
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org6933288" class="outline-3">
|
||||
<h3 id="org6933288"><span class="section-number-3">6.2</span> <code>plotMagUncertainty</code></h3>
|
||||
<div id="outline-container-org6a88fe0" class="outline-3">
|
||||
<h3 id="org6a88fe0"><span class="section-number-3">6.2</span> <code>plotMagUncertainty</code></h3>
|
||||
<div class="outline-text-3" id="text-6-2">
|
||||
<p>
|
||||
<a id="org82f6010"></a>
|
||||
<a id="orgd3213c3"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -1315,11 +1313,11 @@ end
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org351f1ef" class="outline-3">
|
||||
<h3 id="org351f1ef"><span class="section-number-3">6.3</span> <code>plotPhaseUncertainty</code></h3>
|
||||
<div id="outline-container-orgbff6dca" class="outline-3">
|
||||
<h3 id="orgbff6dca"><span class="section-number-3">6.3</span> <code>plotPhaseUncertainty</code></h3>
|
||||
<div class="outline-text-3" id="text-6-3">
|
||||
<p>
|
||||
<a id="org07ed6ab"></a>
|
||||
<a id="org5f660cc"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -1383,7 +1381,7 @@ end
|
||||
</div>
|
||||
<div id="postamble" class="status">
|
||||
<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>
|
||||
</body>
|
||||
</html>
|
||||
|
||||
@ -48,13 +48,13 @@
|
||||
:END:
|
||||
|
||||
* Introduction :ignore:
|
||||
In this document, the optimal and robust design of complementary filters is studied.
|
||||
|
||||
Two sensors are considered with both different noise characteristics and dynamical uncertainties represented by multiplicative input uncertainty.
|
||||
|
||||
This document is arranged as follows:
|
||||
- Section [[sec:sensor_description]]: the sensors are described (dynamics, uncertainty, noise)
|
||||
- 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: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:matlab_functions]]: Matlab functions used for the analysis are described
|
||||
|
||||
* Sensor Description
|
||||
: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');
|
||||
#+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
|
||||
<<sec:introduction_sensor_fusion>>
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -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))
|
||||
#+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:
|
||||
bibliographystyle:unsrt
|
||||
bibliography:ref.bib
|
||||
|
||||
Loading…
Reference in New Issue
Block a user