% Created 2020-09-22 mar. 11:10
% Intended LaTeX compiler: pdflatex
\documentclass[conference]{IEEEtran}
\usepackage[utf8]{inputenc}
\usepackage[T1]{fontenc}
\usepackage{graphicx}
\usepackage{grffile}
\usepackage{longtable}
\usepackage{wrapfig}
\usepackage{rotating}
\usepackage[normalem]{ulem}
\usepackage{amsmath}
\usepackage{textcomp}
\usepackage{amssymb}
\usepackage{capt-of}
\usepackage{hyperref}
\usepackage[most]{tcolorbox}
\usepackage{bm}
\usepackage{booktabs}
\usepackage{tabularx}
\usepackage{array}
\usepackage{siunitx}
\IEEEoverridecommandlockouts
\usepackage{cite}
\usepackage{amsmath,amssymb,amsfonts}
\usepackage{algorithmic}
\usepackage{graphicx}
\usepackage{textcomp}
\usepackage{xcolor}
\usepackage{cases}
\usepackage{tabularx,siunitx,booktabs}
\usepackage{algorithmic}
\usepackage{import, hyperref}
\renewcommand{\citedash}{--}
\def\BibTeX{{\rm B\kern-.05em{\sc i\kern-.025em b}\kern-.08em T\kern-.1667em\lower.7ex\hbox{E}\kern-.125emX}}
\usepackage{showframe}
\author{\IEEEauthorblockN{Dehaeze Thomas} \IEEEauthorblockA{\textit{European Synchrotron Radiation Facility} \\ Grenoble, France\\ \textit{Precision Mechatronics Laboratory} \\ \textit{University of Liege}, Belgium \\ thomas.dehaeze@esrf.fr }\and \IEEEauthorblockN{Collette Christophe} \IEEEauthorblockA{\textit{BEAMS Department}\\ \textit{Free University of Brussels}, Belgium\\ \textit{Precision Mechatronics Laboratory} \\ \textit{University of Liege}, Belgium \\ ccollett@ulb.ac.be }}
\date{2020-09-22}
\title{Optimal and Robust Sensor Fusion}
\begin{document}

\maketitle

\begin{abstract}
Abstract text to be done
\end{abstract}

\begin{IEEEkeywords}
Complementary Filters, Sensor Fusion, H-Infinity Synthesis
\end{IEEEkeywords}

\section{Introduction}
\label{sec:orgfaa194e}
\label{sec:introduction}

Section \ref{sec:optimal_fusion}
Section \ref{sec:robust_fusion}
Section \ref{sec:optimal_robust_fusion}
Section \ref{sec:experimental_validation}

\section{Optimal Super Sensor Noise: \(\mathcal{H}_2\) Synthesis}
\label{sec:org08f9f0e}
\label{sec:optimal_fusion}

\subsection{Sensor Model}
\label{sec:orgaa5ec56}

Let's consider a sensor measuring a physical quantity \(x\) (Figure \ref{fig:sensor_model}).
The sensor has an internal dynamics which is here modelled with a Linear Time Invariant (LTI) system transfer function \(G_i(s)\).

The noise of sensor can be described by the Power Spectral Density (PSD) \(\Phi_{n_i}(\omega)\).

This is approximated by shaping a white noise with unitary PSD \(\tilde{n}_i\) \eqref{eq:unitary_sensor_noise_psd} with a LTI transfer function \(N_i(s)\):
\begin{equation}
\begin{aligned}
  \Phi_{n_i}(\omega) &= \left| N_i(j\omega) \right|^2 \Phi_{\tilde{n}_i}(\omega) \\
                     &= \left| N_i(j\omega) \right|^2
\end{aligned}
\end{equation}

\begin{equation}
\label{eq:unitary_sensor_noise_psd}
  \Phi_{\tilde{n}_i}(\omega) = 1
\end{equation}

The output of the sensor \(v_i\):
\begin{equation}
  v_i = \left( G_i \right) x + \left( G_i N_i \right) \tilde{n}_i
\end{equation}

In order to obtain an estimate \(\hat{x}_i\) of \(x\), a model \(\hat{G}_i\) of the (true) sensor dynamics \(G_i\) is inverted and applied at the output (Figure \ref{fig:sensor_model}):
\begin{equation}
  \hat{x}_i = \left( \hat{G}_i^{-1} G_i \right) x + \left( \hat{G}_i^{-1} G_i N_i \right) \tilde{n}_i
\end{equation}

\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/sensor_model.pdf}
\caption{\label{fig:sensor_model}Sensor Model}
\end{figure}

\subsection{Sensor Fusion Architecture}
\label{sec:org17e7387}

Let's now consider two sensors measuring the same physical quantity \(x\) but with different dynamics \((G_1, G_2)\) and noise characteristics \((N_1, N_2)\) (Figure \ref{fig:sensor_fusion_noise_arch}).

The noise sources \(\tilde{n}_1\) and \(\tilde{n}_2\) are considered to be uncorrelated.

\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/sensor_fusion_noise_arch.pdf}
\caption{\label{fig:sensor_fusion_noise_arch}Sensor Fusion Architecture with sensor noise}
\end{figure}

The output of both sensors \((v1,v2)\) are then passed through the inverse of the sensor model to obtained two estimates \((\hat{x}_1, \hat{x}_2)\) of \(x\).
These two estimates are then filtered out by two filters \(H_1\) and \(H_2\) and summed to gives the super sensor estimate \(\hat{x}\).

\begin{equation}
  \begin{split}
    \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
  \end{split}
\end{equation}

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

We considered here complementary filters:
\begin{equation}
  H_1(s) + H_2(s) = 1
\end{equation}

In such case, the super sensor estimate \(\hat{x}\) is equal to \(x\) plus the noise of the individual sensors filtered out by the complementary filters:
\begin{equation}
  \hat{x} = x + \left( H_1 N_1 \right) \tilde{n}_1 + \left( H_2 N_2 \right) \tilde{n}_2
\end{equation}

\subsection{Super Sensor Noise}
\label{sec:orgb010f68}
Let's note \(n\) the super sensor noise.
\begin{equation}
  n = \left( H_1 N_1 \right) \tilde{n}_1 + \left( H_2 N_2 \right) \tilde{n}_2
\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
\end{equation}

It is clear that the PSD of the super sensor depends on the norm of the complementary filters.

\subsection{\(\mathcal{H}_2\) Synthesis of Complementary Filters}
\label{sec:orgf1d735c}

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\):
\begin{equation}
\label{eq:rms_value_estimation}
  \sigma_{n} = \sqrt{\int_0^\infty \Phi_{\hat{n}}(\omega) d\omega} = \left\| \begin{matrix} H_1 N_1 \\ H_2 N_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} H_1 N_1 \\ H_2 N_2 \end{matrix} \right\|_2\) is minimized.

\begin{equation}
\begin{pmatrix}
  z_1 \\ z_2 \\ v
\end{pmatrix} = \begin{bmatrix}
  N_1 & N_1 \\
  0   & 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.

\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/h_two_optimal_fusion.pdf}
\caption{\label{fig:h_two_optimal_fusion}Generalized plant \(P_{\mathcal{H}_2}\) used for the \(\mathcal{H}_2\) synthesis of complementary filters}
\end{figure}

\subsection{Example}
\label{sec:org074433c}

\subsection{Robustness Problem}
\label{sec:org21dc09f}

\section{Robust Sensor Fusion: \(\mathcal{H}_\infty\) Synthesis}
\label{sec:org2041184}
\label{sec:robust_fusion}

\subsection{Representation of Sensor Dynamical Uncertainty}
\label{sec:orgfd12a50}

Suppose that the sensor dynamics \(G_i(s)\) can be modelled by a nominal d
\begin{equation}
  G_i(s) = \hat{G}_i(s) \left( 1 + w_i(s) \Delta_i(s) \right); \quad |\Delta_i(j\omega)| < 1 \forall \omega
\end{equation}


\subsection{Sensor Fusion Architecture}
\label{sec:org11c9d00}
\begin{equation}
\begin{split}
  \hat{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) x
\end{split}
\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}

\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/sensor_fusion_arch_uncertainty.pdf}
\caption{\label{fig:sensor_fusion_arch_uncertainty}Sensor Fusion Architecture with sensor model uncertainty}
\end{figure}

\subsection{Super Sensor Dynamical Uncertainty}
\label{sec:org6673a25}

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

\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/uncertainty_set_super_sensor.pdf}
\caption{\label{fig:uncertainty_set_super_sensor}Super Sensor model uncertainty displayed in the complex plane}
\end{figure}

\subsection{\(\mathcal{H_\infty}\) Synthesis of Complementary Filters}
\label{sec:org41ccb1e}

In order to minimize the super sensor dynamical uncertainty

\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/h_infinity_robust_fusion.pdf}
\caption{\label{fig:h_infinity_robust_fusion}Generalized plant \(P_{\mathcal{H}_\infty}\) used for the \(\mathcal{H}_\infty\) synthesis of complementary filters}
\end{figure}

\subsection{Example}
\label{sec:orgba594da}

\section{Optimal and Robust Sensor Fusion: Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis}
\label{sec:orgc07eeab}
\label{sec:optimal_robust_fusion}

\subsection{Sensor Fusion Architecture}
\label{sec:orgddd6d33}

\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/sensor_fusion_arch_full.pdf}
\caption{\label{fig:sensor_fusion_arch_full}Super Sensor Fusion with both sensor noise and sensor model uncertainty}
\end{figure}

\subsection{Synthesis Objective}
\label{sec:org79824da}

\subsection{Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis}
\label{sec:org247ac1c}

\begin{figure}[htbp]
\centering
\includegraphics[scale=1]{figs/mixed_h2_hinf_synthesis.pdf}
\caption{\label{fig:mixed_h2_hinf_synthesis}Generalized plant \(P_{\mathcal{H}_2/\matlcal{H}_\infty}\) used for the mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) synthesis of complementary filters}
\end{figure}

\subsection{Example}
\label{sec:org7af2158}

\section{Experimental Validation}
\label{sec:orgb54c59b}
\label{sec:experimental_validation}

\subsection{Experimental Setup}
\label{sec:org40eadad}

\subsection{Sensor Noise and Dynamical Uncertainty}
\label{sec:orgb81743f}

\subsection{Mixed \(\mathcal{H}_2/\mathcal{H}_\infty\) Synthesis}
\label{sec:orgb2232ac}

\subsection{Super Sensor Noise and Dynamical Uncertainty}
\label{sec:orgd80a558}

\section{Conclusion}
\label{sec:org0da5eb6}
\label{sec:conclusion}

\section{Acknowledgment}
\label{sec:orge5b9b80}

\bibliography{ref}
\end{document}