#+TITLE: Robust and Optimal Sensor Fusion
:DRAWER:
#+LATEX_CLASS: IEEEtran
#+LATEX_CLASS_OPTIONS: [conference]
#+OPTIONS: toc:nil todo:nil
#+STARTUP: overview

#+DATE: {{{time(%Y-%m-%d)}}}

#+AUTHOR: @@latex:\IEEEauthorblockN{Dehaeze Thomas}@@
#+AUTHOR: @@latex:\IEEEauthorblockA{\textit{European Synchrotron Radiation Facility} \\@@
#+AUTHOR: @@latex:Grenoble, France\\@@
#+AUTHOR: @@latex:\textit{Precision Mechatronics Laboratory} \\@@
#+AUTHOR: @@latex:\textit{University of Liege}, Belgium \\@@
#+AUTHOR: @@latex:thomas.dehaeze@esrf.fr@@
#+AUTHOR: @@latex:}\and@@
#+AUTHOR: @@latex:\IEEEauthorblockN{Collette Christophe}@@
#+AUTHOR: @@latex:\IEEEauthorblockA{\textit{BEAMS Department}\\@@
#+AUTHOR: @@latex:\textit{Free University of Brussels}, Belgium\\@@
#+AUTHOR: @@latex:\textit{Precision Mechatronics Laboratory} \\@@
#+AUTHOR: @@latex:\textit{University of Liege}, Belgium \\@@
#+AUTHOR: @@latex:ccollett@ulb.ac.be@@
#+AUTHOR: @@latex:}@@

#+LATEX_HEADER: \IEEEoverridecommandlockouts
#+LATEX_HEADER: \usepackage{cite}
#+LATEX_HEADER: \usepackage{amsmath,amssymb,amsfonts}
#+LATEX_HEADER: \usepackage{algorithmic}
#+LATEX_HEADER: \usepackage{graphicx}
#+LATEX_HEADER: \usepackage{textcomp}
#+LATEX_HEADER: \usepackage{xcolor}
#+LATEX_HEADER: \usepackage{cases}
#+LATEX_HEADER: \usepackage{tabularx,siunitx,booktabs}
#+LATEX_HEADER: \usepackage{algorithmic}
#+LATEX_HEADER: \usepackage{import, hyperref}
#+LATEX_HEADER: \renewcommand{\citedash}{--}

#+LATEX_HEADER_EXTRA: \usepackage{showframe}

#+LATEX_HEADER: \def\BibTeX{{\rm B\kern-.05em{\sc i\kern-.025em b}\kern-.08em T\kern-.1667em\lower.7ex\hbox{E}\kern-.125emX}}

\bibliographystyle{IEEEtran}
:END:

* LaTeX Config                                                     :noexport:
#+begin_src latex :tangle config.tex
#+end_src

* Build                                                            :noexport:
#+NAME: startblock
#+BEGIN_SRC emacs-lisp :results none
  (add-to-list 'org-latex-classes
               '("IEEEtran"
                 "\\documentclass{IEEEtran}"
                 ("\\section{%s}" . "\\section*{%s}")
                 ("\\subsection{%s}" . "\\subsection*{%s}")
                 ("\\subsubsection{%s}" . "\\subsubsection*{%s}")
                 ("\\paragraph{%s}" . "\\paragraph*{%s}")
                 ("\\subparagraph{%s}" . "\\subparagraph*{%s}"))
               )

  (defun delete-org-comments (backend)
    (loop for comment in (reverse (org-element-map (org-element-parse-buffer)
                                      'comment 'identity))
          do
          (setf (buffer-substring (org-element-property :begin comment)
                                  (org-element-property :end comment))
                "")))

  ;; add to export hook
  (add-hook 'org-export-before-processing-hook 'delete-org-comments)

  ;; Remove hypersetup
  (setq org-latex-with-hyperref nil)
#+END_SRC

* Abstract                                                           :ignore:
#+begin_abstract
  Abstract text to be done
#+end_abstract

* Keywords                                                            :ignore:
#+begin_IEEEkeywords
  Complementary Filters, Sensor Fusion, H-Infinity Synthesis
#+end_IEEEkeywords

* Introduction
<<sec:introduction>>

* Optimal Super Sensor Noise: $\mathcal{H}_2$ Synthesis
<<sec:optimal_fusion>>

** Sensor Model

** Sensor Fusion Architecture

#+name: fig:sensor_fusion_noise_arch
#+caption: Figure caption
#+attr_latex: :scale 1
[[file:figs/sensor_fusion_noise_arch.pdf]]

Let note $\Phi$ the PSD.
$\tilde{n}_1$ and $\tilde{n}_2$ are white noise with unitary power spectral density:
\begin{equation}
  \Phi_{\tilde{n}_i}(\omega) = 1
\end{equation}

\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} N_1 \right) \tilde{n}_1 + \left( H_2 \hat{G}_2^{-1} N_2 \right) \tilde{n}_2
\end{equation}

Suppose the sensor dynamical model $\hat{G}_i$ is perfect:
\begin{equation}
  \hat{G}_i = G_i
\end{equation}

Complementary Filters
\begin{equation}
  H_1(s) + H_2(s) = 1
\end{equation}


\begin{equation}
  \hat{x} = x + \left( H_1 \hat{G}_1^{-1} N_1 \right) \tilde{n}_1 + \left( H_2 \hat{G}_2^{-1} N_2 \right) \tilde{n}_2
\end{equation}

Perfect dynamics + filter noise


** Super Sensor Noise

Let's note $n$ the super sensor noise.

Its PSD is determined by:
\begin{equation}
  \Phi_n(\omega) = \left| H_1 \hat{G}_1^{-1} N_1 \right|^2 + \left| H_2 \hat{G}_2^{-1} N_2 \right|^2
\end{equation}

** $\mathcal{H}_2$ Synthesis of Complementary Filters

The goal is to design $H_1(s)$ and $H_2(s)$ such that the effect of the noise sources $\tilde{n}_1$ and $\tilde{n}_2$ has the smallest possible effect on the noise  $n$ of the estimation $\hat{x}$.

And the goal is the minimize the Root Mean Square (RMS) value of $n$:
#+name: eq:rms_value_estimation
\begin{equation}
  \sigma_{n} = \sqrt{\int_0^\infty \Phi_{\hat{n}}(\omega) d\omega} = \left\| \begin{matrix} \hat{G}_1^{-1} N_1 H_1 \\ \hat{G}_2^{-1} N_2 H_2 \end{matrix} \right\|_2
\end{equation}

Thus, the goal is to design $H_1(s)$ and $H_2(s)$ such that $H_1(s) + H_2(s) = 1$ and such that $\left\| \begin{matrix} \hat{G}_1^{-1} N_1 H_1 \\ \hat{G}_2^{-1} N_2 H_2 \end{matrix} \right\|_2$ is minimized.

\begin{equation}
\begin{pmatrix}
  z_1 \\ z_2 \\ v
\end{pmatrix} = \begin{bmatrix}
  \hat{G}_1^{-1} N_1 & -\hat{G}_1^{-1} N_1 \\
  0                  &  \hat{G}_2^{-1} N_2 \\
  1                  &  0
\end{bmatrix} \begin{pmatrix}
  w \\ u
\end{pmatrix}
\end{equation}

The $\mathcal{H}_2$ synthesis of the complementary filters thus minimized the RMS value of the super sensor noise.

#+name: fig:h_two_optimal_fusion
#+caption: Figure caption
#+attr_latex: :scale 1
[[file:figs/h_two_optimal_fusion.pdf]]

** Example

** Robustness Problem

* Robust Sensor Fusion: $\mathcal{H}_\infty$ Synthesis
<<sec:robust_fusion>>

** Representation of Sensor Dynamical Uncertainty

** Sensor Fusion Architecture
\begin{equation}
  \hat{x} = \left( H_1 \hat{G}_1^{-1} (1 + w_1 \Delta_1) G_1 + H_2 \hat{G}_2^{-1} (1 + w_2 \Delta_2) G_2 \right) x
\end{equation}
with $\Delta_i$ is any transfer function satisfying $\| \Delta_i \|_\infty < 1$.

Suppose the model inversion is equal to the nominal model:
\begin{equation}
  \hat{G}_i = G_i
\end{equation}

\begin{equation}
  \hat{x} = \left( 1 + H_1 w_1 \Delta_1 + H_2 w_2 \Delta_2 \right) x
\end{equation}

#+name: fig:sensor_fusion_arch_uncertainty
#+caption: Figure caption
#+attr_latex: :scale 1
[[file:figs/sensor_fusion_arch_uncertainty.pdf]]

** Super Sensor Dynamical Uncertainty

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)|$.

#+name: fig:uncertainty_set_super_sensor
#+caption: Figure caption
#+attr_latex: :scale 1
[[file:figs/uncertainty_set_super_sensor.pdf]]

** $\mathcal{H_\infty}$ Synthesis of Complementary Filters

In order to minimize the super sensor dynamical uncertainty

#+name: fig:h_infinity_robust_fusion
#+caption: Figure caption
#+attr_latex: :scale 1
[[file:figs/h_infinity_robust_fusion.pdf]]

** Example

* Optimal and Robust Sensor Fusion: Mixed $\mathcal{H}_2/\mathcal{H}_\infty$ Synthesis
<<sec:optimal_robust_fusion>>

** Sensor Fusion Architecture

#+name: fig:sensor_fusion_arch_full
#+caption: Figure caption
#+attr_latex: :scale 1
[[file:figs/sensor_fusion_arch_full.pdf]]

** Synthesis Objective

** Mixed $\mathcal{H}_2/\mathcal{H}_\infty$ Synthesis

#+name: fig:mixed_h2_hinf_synthesis
#+caption: Figure caption
#+attr_latex: :scale 1
[[file:figs/mixed_h2_hinf_synthesis.pdf]]

** Example

* Experimental Validation
<<sec:experimental_validation>>

** Experimental Setup

** Sensor Noise and Dynamical Uncertainty

** Mixed $\mathcal{H}_2/\mathcal{H}_\infty$ Synthesis

** Super Sensor Noise and Dynamical Uncertainty

* Conclusion
<<sec:conclusion>>

* Acknowledgment

* Bibliography                                                       :ignore:
\bibliography{ref}