Change G1 sign

matlab/index.org

@@ -24,7 +24,7 @@
 #+PROPERTY: header-args:matlab+ :output-dir figs
 :END:
 
-* Introduction                                                       :ignore:
+* 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.
@@ -118,7 +118,7 @@ Its nominal dynamics $\hat{G}_1(s)$ is defined below.
   k_acc = 1e5;  % Stiffness [N/m]
   g_acc = 1e5;  % Gain [V/m]
 
-  G1 = -g_acc*m_acc*s/(m_acc*s^2 + c_acc*s + k_acc); % Accelerometer Plant [V/(m/s)]
+  G1 = g_acc*m_acc*s/(m_acc*s^2 + c_acc*s + k_acc); % Accelerometer Plant [V/(m/s)]
 #+end_src
 
 The second sensor is a displacement sensor, its nominal dynamics $\hat{G}_2(s)$ is defined below.
@@ -305,7 +305,7 @@ All the dynamical systems representing the sensors are saved for further use.
 #+end_src
 
 * First Order Complementary Filters                                 :noexport:
-** Matlab Init                                             :noexport:ignore:
+** 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
@@ -392,7 +392,77 @@ All the dynamical systems representing the sensors are saved for further use.
 #+end_src
 
 
-* Optimal Super Sensor Noise: $\mathcal{H}_2$ Synthesis with Acc and Pos
+* Introduction to Sensor Fusion
+<<sec:introduction_sensor_fusion>>
+
+** Sensor Fusion Architecture
+<<sec:sensor_fusion_architecture>>
+
+The two sensors presented in Section [[sec:sensor_description]] are now merged together using complementary filters $H_1(s)$ and $H_2(s)$ to form a super sensor (Figure [[fig:sensor_fusion_noise_arch]]).
+
+#+name: fig:sensor_fusion_noise_arch
+#+caption: Sensor Fusion Architecture
+[[file:figs-tikz/sensor_fusion_noise_arch.png]]
+
+The complementary property of $H_1(s)$ and $H_2(s)$ means that the sum of their transfer function is equal to $1$ eqref:eq:complementary_property.
+
+\begin{equation}
+  H_1(s) + H_2(s) = 1 \label{eq:complementary_property}
+\end{equation}
+
+The super sensor estimate $\hat{x}$ is given by eqref:eq:super_sensor_estimate.
+
+\begin{equation}
+  \hat{x} = \left( H_1 \hat{G}_1^{-1} G_1 + H_2 \hat{G}_2^{-1} G_2 \right) x + \left( H_1 \hat{G}_1^{-1} G_1 N_1 \right) \tilde{n}_1 + \left( H_2 \hat{G}_2^{-1} G_2 N_2 \right) \tilde{n}_2 \label{eq:super_sensor_estimate}
+\end{equation}
+
+** Super Sensor Noise
+<<sec:super_sensor_noise>>
+
+If we first suppose that the models of the sensors $\hat{G}_i$ are very close to the true sensor dynamics $G_i$ eqref:eq:good_dynamical_model, we have that the super sensor estimate $\hat{x}$ is equals to the measured quantity $x$ plus the noise of the two sensors filtered out by the complementary filters eqref:eq:estimate_perfect_models.
+
+\begin{equation}
+  \hat{G}_i^{-1}(s) G_i(s) \approx 1 \label{eq:good_dynamical_model}
+\end{equation}
+
+\begin{equation}
+  \hat{x} = x + \underbrace{\left( H_1 N_1 \right) \tilde{n}_1 + \left( H_2 N_2 \right) \tilde{n}_2}_{n} \label{eq:estimate_perfect_models}
+\end{equation}
+
+As the noise of both sensors are considered to be uncorrelated, the PSD of the super sensor noise is computed as follow:
+\begin{equation}
+  \Phi_n(\omega) = \left| H_1 N_1 \right|^2 + \left| H_2 N_2 \right|^2 \label{eq:super_sensor_psd_noise}
+\end{equation}
+
+And the Root Mean Square (RMS) value of the super sensor noise $\sigma_n$ is given by eqref:eq:super_sensor_rms_noise.
+\begin{equation}
+  \sigma_n = \sqrt{\int_0^\infty \Phi_n(\omega) d\omega} \label{eq:super_sensor_rms_noise}
+\end{equation}
+
+** Super Sensor Dynamical Uncertainty
+<<sec:super_sensor_dynamical_uncertainty>>
+
+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 [[fig:sensor_model_uncertainty]]), the super sensor dynamics is then equals to:
+
+\begin{equation}
+\begin{aligned}
+  \frac{\hat{x}}{x} &= \Big(  H_1 \hat{G}_1^{-1} \hat{G}_1 (1 + W_1 \Delta_1) + H_2 \hat{G}_2^{-1} \hat{G}_2 (1 + W_2 \Delta_2) \Big) \\
+                    &= \Big( H_1 (1 + W_1 \Delta_1) + H_2 (1 + W_2 \Delta_2) \Big) \\
+                    &= \left( 1 + H_1 W_1 \Delta_1 + H_2 W_2 \Delta_2 \right), \quad \|\Delta_i\|_\infty<1
+\end{aligned}
+\end{equation}
+
+#+name: fig:sensor_model_uncertainty
+#+caption: Sensor Model including Dynamical Uncertainty
+[[file:figs-tikz/sensor_model_uncertainty.png]]
+
+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 [[fig:uncertainty_set_super_sensor]].
+
+#+name: fig:uncertainty_set_super_sensor
+#+caption: Super Sensor model uncertainty displayed in the complex plane
+[[file:figs-tikz/uncertainty_set_super_sensor.png]]
+
+* Optimal Super Sensor Noise: $\mathcal{H}_2$ Synthesis
 :PROPERTIES:
 :header-args:matlab+: :tangle matlab/optimal_comp_filters.m
 :header-args:matlab+: :comments org :mkdirp yes
@@ -400,10 +470,10 @@ All the dynamical systems representing the sensors are saved for further use.
 <<sec:optimal_comp_filters>>
 
 ** Introduction                                                      :ignore:
+In this section, the two sensors are merged together using complementary
+
 The idea is to combine sensors that works in different frequency range using complementary filters.
-
 Doing so, one "super sensor" is obtained that can have better noise characteristics than the individual sensors over a large frequency range.
-
 The complementary filters have to be designed in order to minimize the effect noise of each sensor on the super sensor noise.
 
 #+name: fig:sensor_fusion_noise_arch
@@ -455,7 +525,7 @@ If we define $H_2 = 1 - H_1$, we obtain:
 
 Thus, if we minimize the $\mathcal{H}_2$ norm of this transfer function, we minimize the RMS value of $\hat{x}$.
 
-We define the generalized plant $P$ on matlab as shown on figure [[fig:h_infinity_optimal_comp_filters]].
+We define the generalized plant $P$ on matlab as shown on Figure
 #+begin_src matlab
   P = [N1 -N1;
        0   N2;
@@ -477,7 +547,7 @@ Finally, we define $H_2(s) = 1 - H_1(s)$.
   save('./mat/H2_filters.mat', 'H2', 'H1');
 #+end_src
 
-The complementary filters obtained are shown on figure [[fig:htwo_comp_filters]].
+The complementary filters obtained are shown on Figure
 #+begin_src matlab :exports none
   figure;
   hold on;
@@ -670,7 +740,7 @@ From the above complementary filter design with the $\mathcal{H}_2$ and $\mathca
 
 However, the synthesis does not take into account the robustness of the sensor fusion.
 
-* Robust Sensor Fusion: $\mathcal{H}_\infty$ Synthesis with Acc and Pos
+* Robust Sensor Fusion: $\mathcal{H}_\infty$ Synthesis
 :PROPERTIES:
 :header-args:matlab+: :tangle matlab/comp_filter_robustness.m
 :header-args:matlab+: :comments org :mkdirp yes
@@ -1042,7 +1112,7 @@ Using the $\mathcal{H}_\infty$ synthesis, the dynamical uncertainty of the super
 
 However, the RMS of the super sensor noise is not optimized as it was the case with the $\mathcal{H}_2$ synthesis
 
-* Optimal and Robust Sensor Fusion: Mixed $\mathcal{H}_2/\mathcal{H}_\infty$ Synthesis with Acc and Pos
+* Optimal and Robust Sensor Fusion: Mixed $\mathcal{H}_2/\mathcal{H}_\infty$ Synthesis
 :PROPERTIES:
 :header-args:matlab+: :tangle matlab/mixed_synthesis_sensor_fusion.m
 :header-args:matlab+: :comments org :mkdirp yes