Add note about how to apply robust/optimal sensor fusion in practice

matlab/index.org

@@ -902,6 +902,117 @@ and
                       &\neq | H_1(j\omega) N_1(j\omega)|^2 + |H_2(j\omega) N_2(j\omega) |^2 \quad \forall\omega \\
 \end{align*}
 
+* Optimal And Robust Sensor Fusion in Practice
+
+Here are the steps in order to apply optimal and robust sensor fusion:
+
+- Measure the noise characteristics of the sensors to be merged (necessary for "optimal" part of the fusion)
+- Measure/Estimate the dynamic uncertainty of the sensors (necessary for "robust" part of the fusion)
+- Apply H2/H-infinity synthesis of the complementary filters
+
+** Measurement of the noise characteristics of the sensors
+*** Huddle Test
+The technique to estimate the sensor noise is taken from cite:barzilai98_techn_measur_noise_sensor_presen.
+
+Let's consider two sensors (sensor 1 and sensor 2) that are measuring the same quantity $x$ as shown in figure [[fig:huddle_test]].
+
+#+NAME: fig:huddle_test
+#+CAPTION: Huddle test block diagram
+[[file:figs-tikz/huddle_test.png]]
+
+Each sensor has uncorrelated noise $n_1$ and $n_2$ and internal dynamics $G_1(s)$ and $G_2(s)$ respectively.
+
+We here suppose that each sensor has the same magnitude of instrumental noise: $n_1 = n_2 = n$.
+We also assume that their dynamics is ideal: $G_1(s) = G_2(s) = 1$.
+
+We then have:
+#+NAME: eq:coh_bis
+\begin{equation}
+  \gamma_{\hat{x}_1\hat{x}_2}^2(\omega) = \frac{1}{1 + 2 \left( \frac{|\Phi_n(\omega)|}{|\Phi_{\hat{x}}(\omega)|} \right) + \left( \frac{|\Phi_n(\omega)|}{|\Phi_{\hat{x}}(\omega)|} \right)^2}
+\end{equation}
+
+Since the input signal $x$ and the instrumental noise $n$ are incoherent:
+#+NAME: eq:incoherent_noise
+\begin{equation}
+  |\Phi_{\hat{x}}(\omega)| = |\Phi_n(\omega)| + |\Phi_x(\omega)|
+\end{equation}
+
+From equations [[eq:coh_bis]] and [[eq:incoherent_noise]], we finally obtain
+#+begin_important
+#+NAME: eq:noise_psd
+\begin{equation}
+  |\Phi_n(\omega)| = |\Phi_{\hat{x}}(\omega)| \left( 1 - \sqrt{\gamma_{\hat{x}_1\hat{x}_2}^2(\omega)} \right)
+\end{equation}
+#+end_important
+
+*** Weights that represents the noises' PSD
+
+For further complementary filter synthesis, it is preferred to consider a normalized noise source $\tilde{n}$ that has a PSD equal to one ($\Phi_{\tilde{n}}(\omega) = 1$) and to use a weighting filter $N(s)$ in order to represent the frequency dependence of the noise.
+
+The weighting filter $N(s)$ should be designed such that:
+\begin{align*}
+                      & \Phi_n(\omega) \approx |N(j\omega)|^2 \Phi_{\tilde{n}}(\omega) \quad \forall \omega \\
+  \Longleftrightarrow & |N(j\omega)| \approx \sqrt{\Phi_n(\omega)} \quad \forall \omega
+\end{align*}
+
+These weighting filters can then be used to compare the noise level of sensors for the synthesis of complementary filters.
+
+The sensor with a normalized noise input is shown in figure [[fig:one_sensor_normalized_noise]].
+
+#+name: fig:one_sensor_normalized_noise
+#+caption: One sensor with normalized noise
+[[file:figs-tikz/one_sensor_normalized_noise.png]]
+
+*** Comparison of the noises' PSD
+Once the noise of the sensors to be merged have been characterized, the power spectral density of both sensors have to be compared.
+
+Ideally, the PSD of the noise are such that:
+\begin{align*}
+  \Phi_{n_1}(\omega) &< \Phi_{n_2}(\omega) \text{ for } \omega < \omega_m \\
+  \Phi_{n_1}(\omega) &> \Phi_{n_2}(\omega) \text{ for } \omega > \omega_m
+\end{align*}
+
+*** Computation of the coherence, power spectral density and cross spectral density of signals
+The coherence between signals $x$ and $y$ is defined as follow
+\[ \gamma^2_{xy}(\omega) = \frac{|\Phi_{xy}(\omega)|^2}{|\Phi_{x}(\omega)| |\Phi_{y}(\omega)|} \]
+where $|\Phi_x(\omega)|$ is the output Power Spectral Density (PSD) of signal $x$ and $|\Phi_{xy}(\omega)|$ is the Cross Spectral Density (CSD) of signal $x$ and $y$.
+
+The PSD and CSD are defined as follow:
+\begin{align}
+  |\Phi_x(\omega)|    &= \frac{2}{n_d T} \sum^{n_d}_{n=1} \left| X_k(\omega, T) \right|^2 \\
+  |\Phi_{xy}(\omega)| &= \frac{2}{n_d T} \sum^{n_d}_{n=1} [ X_k^*(\omega, T) ] [ Y_k(\omega, T) ]
+\end{align}
+where:
+- $n_d$ is the number for records averaged
+- $T$ is the length of each record
+- $X_k(\omega, T)$ is the finite Fourier transform of the $k^{\text{th}}$ record
+- $X_k^*(\omega, T)$ is its complex conjugate
+
+** Estimate the dynamic uncertainty of the sensors
+
+Let's consider one sensor represented on figure [[fig:one_sensor_dyn_uncertainty]].
+
+The dynamic uncertainty is represented by an input multiplicative uncertainty where $w(s)$ is a weight that represents the level of the uncertainty.
+
+The goal is to accurately determine $w(s)$ for the sensors that have to be merged.
+
+#+name: fig:one_sensor_dyn_uncertainty
+#+caption: Sensor with dynamic uncertainty
+[[file:figs-tikz/one_sensor_dyn_uncertainty.png]]
+
+** Optimal and Robust synthesis of the complementary filters
+Once the noise characteristics and dynamic uncertainty of both sensors have been determined and we have determined the following weighting functions:
+- $w_1(s)$ and $w_2(s)$ representing the dynamic uncertainty of both sensors
+- $N_1(s)$ and $N_2(s)$ representing the noise characteristics of both sensors
+
+The goal is to design complementary filters $H_1(s)$ and $H_2(s)$ shown in figure [[fig:sensor_fusion_full]] such that:
+- the uncertainty on the super sensor dynamics is minimized
+- the noise sources $\tilde{n}_1$ and $\tilde{n}_2$ has the lowest possible effect on the estimation $\hat{x}$
+
+#+name: fig:sensor_fusion_full
+#+caption: Sensor fusion architecture with sensor dynamics uncertainty
+[[file:figs-tikz/sensor_fusion_full.png]]
+
 * Methods of complementary filter synthesis
 ** Complementary filters using analytical formula
   :PROPERTIES: