\documentclass[a4paper,11pt]{article}
\usepackage{graphicx}
\usepackage{times}

\renewcommand{\baselinestretch}{1.0}

\sloppy


\textwidth 182mm   	% <------ These are the adjustments we made 10/18/2005   178
\textheight 243mm	% You may or may not need to adjust these numbes againv  240
\oddsidemargin -7mm
\evensidemargin -7mm
\topmargin -3mm 
\columnsep 5mm


\begin{document}


\title{Crossing Point Estimation in Human/Robot Navigation - \newline Statistical Linearization  versus Sigma-Point-Transformation}

\author{Rainer Palm and Achim J. Lilienthal,
\\AASS, Dept. of Technology, \"Orebro University, Sweden\\
Technical University Munich (TUM), Germany\\
email:rub.palm@t-online.de, achim.j.lilienthal@tum.de
}
\date{}

\maketitle

-----------------------------------------------------------------------------------------------------------------------------------------
\newline
{\Large{\bf Abstract}}
\newline
%\begin{abstract}
Interactions between mobile robots and human operators in common areas require a high safety especially in terms of trajectory planning, obstacle avoidance and 
mutual cooperation. In this connection the crossings of planned trajectories, their uncertainty based on model fluctuations, system noise and sensor  noise,
play an outstanding role. This paper discusses the calculation of expected areas of interactions during human-robot navigation with respect to fuzzy and noisy 
information. Expected crossing points of possible trajectories are nonlinearily associated with positions and orientations of robot and human. 
The nonlinear transformation of a noisy system input, such as directions of motion of human and robot, to a system output, the expected area of intersection of 
their trajectories,  is done by two methods: statistical linearization and the sigma-point-transformation. For both approaches fuzzy approximations are  presented 
and the inverse problem is  discussed where the input distribution parameters are computed from given output distribution parameters.\newline
{\bf Keywords}: {Human-Robot interaction, Gaussian noise, sigma-point transformation, Unscented Kalman Filter} \newline
---------------------------------------------------------------------------------------------------------------------------------------------
%\end{abstract}
%\onecolumn \maketitle \normalsize\vfill
%---------------------------------------------------------------------------------------------------------
\section{{Introduction}}\label{Introduction}
\noindent Planning and performing of mobile robot tasks in the presence of human operators while sharing the same workspace requires a high level of stability and safety.
Research activities regarding navigation, obstacle avoidance, adaptation and collaboration between robots and human agents have been widely reported 
\cite{Khatib85,skoglund2010,firl2014}.  Multiple target tracking for robots using higher control
levels in a control hierarchy are discussed in  \cite{luo2014,chen2018}. A human-friendly interaction between robots and humans can be obtained by human-like sensor 
systems \cite{robertsson2007,palm2006,kassner2014}.  
A prominent role in robot navigation is the trajectory-crossing problem of robots and humans \cite{bruce2015,palm2016} and corresponding fuzzy solutions 
\cite{palm2018}.
Motivations for a fuzzy solution of the intersection problem are manifold. One point is an uncertain measurement of the position and
orientation of the human agent, because of which the use of a fuzzy signal and an adequate fuzzy processing seems natural \cite{palm1994,foulloy2003}. 
Another aspect is  the need for decreasing  the computing effort in the case of  complex  calculations during a very small time interval. 
System uncertainties and observation noise lead to uncertainties of the intersection estimations. 
This paper deals with the one-robot one-human trajectory crossing problem  whereas small uncertainties in position and orientation
may lead to high uncertainties  at the intersection points. Position and orientation of human and robot are nonlinearly coupled but can be linearized. In the following
the linear part of the nonlinear system  is considered in the  analysis reported  for small variations at the input {\cite{palm1993,banelli2013}. In the following 
the  "direct task" is described, meaning that the  parameters of the input distribution are transformed to the output distribution  parameters. The "inverse task"
is also solved, meaning that for defined output distribution parameters the input parameters are calculated.
In this paper two methods are outlined:\newline
1. The {\it statistical linearization}, that linearizes the nonlinearity around the operating area ate the intersection. Means and standard deviations on the 
input parameters positions,  orientations) are transformed through the linearized nonlinear system to obtain means and standard deviations of the output parameters 
position of intersection). \newline 
2. The {\it Sigma-Point Transformation}, that calculates the so-called sigma-points  of the input distribution including mean and covariance of the input.
The sigma-points are directly propagated through the nonlinear system \cite{julier2004, merwe2004, terenjanu2011, asl2020} to obtain means and covariance  of the output and, with this, 
the standard deviations of the output (position of intersection). The advantage of sigma-point transformation is that it captures the 1st and 2nd order 
statistics of a random variable, whereas the statistical linearization approximates a random variable only by its 1st order.
%In the context of larger  standard deviations at the input, we address the case of mixed Gaussian distributions.

The paper is organized as follows. In section \ref{intersect_1a}  the general intersection problem  and its analytical approach is described. 
Section \ref{intersect_1b} deals with the transformation/conversion of Gaussian distributions for a 2-input - 2-output system and for 6-input - 2-output system
plus corresponding inverse and fuzzy solutions. 
In section \ref{sigma_trans}  the sigma-point approach plus inverse and fuzzy solutions is addressed.
%In Section \ref{mixed} mixed Gaussian distributions and their contribution to the intersection problem are presented. 
Section \ref{simulation} presents simulations of the {\it statistical sinearization} and the {\it sigma-point Transformation}, to show the quality of the  
input-output conversion of the distributions, and the impact of different resolutions of fuzzy approximations  on the accuracy of the  
random variable intersection. 
Finally, Section \ref{conclusion} concludes the paper with a discussion of the two different approaches and a comparison of the methods.


\section {{Computation of intersections}}\label{intersect_1a}
The problem can be stated as follows: \newline
Robot and human agent move in a common area according to their tasks or intentions.
To avoid collisions, possible intersections of the paths of the agents
should be predicted for both trajectory planning and on-line interactions. To accomplish
this, positions, orientations and intended movements of robot and human should be estimated
as accurately as needed. \newline


\begin{figure}[thp]
   %\epsfysize=4.5cm % 10.5
	\centering	
  % \hspace{1.0cm}	
   \includegraphics[width=0.45\textwidth]{randomvar.eps}
   \vspace{-0.0cm}
	\caption{Intersection principle}
	\label{fig:randomvar}
\end{figure}

In this connection, uncertainties and noise on the random variables position/orientation ${\bf x}_R$,  ${\bf x}_H$, $\phi_R$ and $\phi_H$
of robot and human have a great impact on the calculation of the expected intersection position ${\bf x}_c$. The random variable ${\bf x}_c$ is 
calculated as the crossing point of the extension of the  orientation or velocity vectors of robot and human which may 
change during motion depending on the task and current interaction. The task is to calculate the intersection and its 
uncertainty in the presence of known uncertainties of the acting agents robot and human. \newline
System noise ${\bf w}_R$ and ${\bf w}_H$ for robot and human can be obtained from experiments. The noise ${\bf w}_c$ of the
'virtual' intersection is  composed by the nonlinear transformed noise  ${\bf w}_R$ and ${\bf w}_H$ and some additional noise
${\bf v}_c$ that may come from uncertainties of the nonlinear computation of the intersection position ${\bf x}_c$ (see Fig. \ref{fig:randomvar}). 
In the following, the geometrical relations are described as well as fuzzy approximations and nonlinear transformations of the random
variables ${\bf x}_R$,  ${\bf x}_H$, $\phi_R$ and $\phi_H$.
\newpage
\subsection {Geometrical relations}
Let the intersection $(x_c, y_c)$ of two linear trajectories ${\bf x}_R(t)$ and  ${\bf x}_H(t)$ in a plane 
be described by the following relations (see Fig.\ref{fig:dist_human_rob_2}) 

\begin{eqnarray}\label{inter_1}
x_H=x_R + d_{RH}\cos (\phi_R + \delta_R)\nonumber\\
y_H=y_R + d_{RH}\sin (\phi_R + \delta_R)\\
x_R=x_H + d_{RH}\cos (\phi_H + \delta_H)\nonumber\\
y_R=y_H + d_{RH}\sin (\phi_H + \delta_H)\nonumber
\end{eqnarray},

where  ${\bf x}_H=(x_H,y_H)$ and ${\bf x}_R=(x_R,y_R)$ are the positions of human and robot and 
$\phi_H$ and $\phi_R$ are their orientation angles,  $\delta_H$ and $\delta_R$ are positive angles measured from the $y$ coordinates counterclockwise.
The angle at the intersection is $\tilde \beta= \pi - \delta_R - \delta_H$.
The variables ${\bf x}_H$,  ${\bf x}_R$, $\phi_R$,$\phi_H$ $\delta_H$, $\phi_H$ $\delta_R$, distance $d_{RH}$ and  angle $\gamma$ are assumed to be measurable.
If  $\phi_H$ is not directly measurable then it can be computed by
\begin{equation}\label{inter_2}
\phi_H=\arcsin ((y_H - y_R)/d_{RH}) - \delta_H + \pi
\end{equation}



\begin{figure}[thp]
   %\epsfysize=4.5cm % 10.5
	\centering	
  % \hspace{1.0cm}	
   \includegraphics[width=0.45\textwidth]{dist_human_rob_2a.eps}
   \vspace{-0.0cm}
	\caption{Human-robot scenario: geometry}
	\label{fig:dist_human_rob_2}
\end{figure}

	
The coordinates $x_c$ and $y_c$ of the intersection are computed straight forward by \cite{palm2018}

{\small
\begin{eqnarray}\label{inter_3}
x_c &=& \frac{A-B}{\tan \phi_R -\tan \phi_H} \nonumber\\
y_c &=& \frac{A\tan \phi_H-B \tan \phi_R}{\tan \phi_R -\tan \phi_H} \\
A&=&x_R \tan \phi_R - y_R \nonumber\\
B&=&x_H \tan \phi_H - y_H \nonumber
\end{eqnarray}
}



Rewriting  (\ref{inter_3}) leads to

{\small
\begin{eqnarray}\label{inter_4}
x_c &=& \left( x_R \frac{\tan \phi_R}{G} - y_R\frac{1}{G} \right) -  \left( x_H \frac{\tan \phi_H}{G} - y_H \frac{1}{G} \right)\nonumber\\
y_c &=& \left( x_R \frac{\tan \phi_R \tan \phi_H}{G} - y_R\frac{ \tan \phi_H}{G} \right) \nonumber \\
&-& \left( x_H \frac{\tan \phi_H \tan \phi_R}{G} - y_H \frac{ \tan \phi_R}{G} \right)  \\
G&=&\tan \phi_R - \tan \phi_H \nonumber
\end{eqnarray}
}


After rearranging (\ref{inter_4}) we observe that ${\bf x}_c = (x_c, y_c)^T$ is linear in ${\bf x}_{RH}=(x_R, y_R, x_H, y_H)^T$


 \begin{eqnarray} \label{inter_fuzzy_1}
{\bf x}_c = A_{RH} \cdot {\bf x}_{RH}
\end{eqnarray}
where
\begin{small}


\begin{eqnarray}\label{inter_fuzzy_2}
A_{RH} = f(\phi_R,\phi_H) = \qquad \qquad \qquad \qquad \qquad \nonumber\\
\frac{1}{G}\left( \begin{array}{cccc}
         \tan \phi_R & -1 & -\tan \phi_H & 1   \nonumber\\
         \tan \phi_R \tan \phi_H & - \tan \phi_H & -\tan \phi_R \tan \phi_H & \tan \phi_R  \nonumber
       \end{array} \right)  \nonumber
\end{eqnarray}
\end{small}

This notation is of advantage for further computations such as fuzzification of the intersection problem and the transformation of 
error distributions.


\subsection{Computation of intersections - fuzzy approach}\label{intersect_fuzzy}
 The fuzzy solution presented in the following is a combination of classical analytical (crisp) methods and rule based methods in the sense of a Takagi-Sugeno 
fuzzy rule base.

In the following we introduce a fuzzy rule-based approximation of (\ref{inter_fuzzy_1})
with $n \times n$ fuzzy rules $R_{i,j}$ 
\begin{eqnarray} \label{inter_fuzzy_4} 
R_{i,j}: \quad  IF \quad \phi_R &=& {\Phi_R}_i \quad  AND \quad  \phi_H = {\Phi_H}_j \quad \\
 THEN \quad  {\bf x}_c &=& {A_{RH}}_{i,j} \cdot {\bf x}_{RH} \nonumber
\end{eqnarray}

$n$ - number of fuzzy terms  $\Phi_{R_i}$, and $\Phi_{H_j}$ for  $\phi_R$ and $\phi_H$

with the result

\begin{eqnarray} \label{inter_fuzzy_3} 
{\bf x}_c = \sum_{i,j} w_i(\phi_R)  w_j(\phi_H) \cdot {A_{RH}}_{i,j} \cdot {\bf x}_{RH} 
\end{eqnarray}
$i, j= 1 ... n$,  
$w_i(\phi_R), w_j(\phi_H) \in [0,1]$ are normalized membership functions with $\sum_i w_i(\phi_R)=1$ and $\sum_j w_j(\phi_H)=1$.


\begin{figure}[thp]
   \centering
   \hspace{0.0cm}
   \includegraphics[width=0.35\textwidth]{mfs_60.eps}
   %\hspace{2.0cm}
	\caption{Membership functions for $\Delta \phi_R, \Delta \phi_H=0-360 ^\circ$}	
	\label{fig:mfs_60}
	\end{figure}


Let the universes of discourse for $\phi_R$ and $\phi_H$ be $\phi_R, \phi_H \in [0, 360°]$. Furthermore, let  these universes of discourse be divided 
into $n$ partitions (for example 6) of $60°$ which leads to $6 \times 6$ fuzzy rules. Corresponding membership functions are shown in fig. \ref{fig:mfs_60}.
It turns out that this resolution leads to a poor  fuzzy approximation. The approximation quality can be improved by increasing the number of fuzzy sets which
however results in a quadratic increase of the number of fuzzy rules.
To avoid an "explosion" of the number of fuzzy rules being computed in one  time step  a set of sub-areas covering a small 
number of rules for each sub-area  is defined.  Based on the measurements of $\phi_R$ and $\phi_H$, the appropriate sub-area is selected  
together with a corresponding set of rules (see Fig. \ref{fig:fuzzy_sectors_1}, sub-area $A_R, A_H$). With this, the number of rules to be activated at 
one time step of calculation  is low, although the total number of rules can be high.
At the borderlines between  sub-areas  abrupt changes may occur which can be avoided by overlapping of sub-areas.


\begin{figure}[thp]  
   \hspace{0.0cm}
   \centering
   \includegraphics[width=0.40\textwidth]{fuzzy_sectors_1.eps}
   %\hspace{2.0cm}
	\caption{Fuzzy sectors}	
	\label{fig:fuzzy_sectors_1}
	\end{figure}



\subsection{Differential approach}
Robots and human agents usually change their positions, orientations, and velocities which requires a differential approach apart from the exact solution
(\ref{inter_4}). In addition, the analysis of uncertainty and noise at ${\bf x}_c$ and the existence of noise at $\phi_R$, $\phi_H$, and 
${\bf x}_{RH}=(x_R, y_R, x_H, y_H)^T$ requires a differential strategy.




Differentiating  (\ref{inter_4})  with ${\bf x}_{RH}=const.$ yields

{\small
\begin{eqnarray} \label{inter_diff_1}
{\bf \dot x}_c = {\tilde J} \cdot {\bf \dot \phi} \qquad \qquad \qquad \nonumber\\
{\bf \dot \phi}=(\dot\phi_R  \quad  \dot\phi_H)^T; \quad
%\end{eqnarray}
%with
%{\small
%\begin{eqnarray}\label{inter_fuzzy_2a}
      {\tilde J}=\left( \begin{array}{cc}
             {\tilde J}_{11}& {\tilde J}_{12} \cr
             {\tilde J}_{21}& {\tilde J}_{22} \cr
             \end{array} \right)
\end{eqnarray}
}

 where
{\small
\begin{eqnarray}
{\tilde J}_{11}&=&\left( \begin{array}{cccc}
                     -\tan \phi_H & 1 & \tan \phi_H & -1 \cr
                     \end{array} \right)\frac{{\bf x}_{RH}}{G^2 \cdot \cos^2 \phi_R}  \nonumber\\
{\tilde J}_{12}&=&\left( \begin{array}{cccc}
                     \tan \phi_R & -1 & -\tan \phi_R & 1 \cr
                \end{array} \right)\frac{{\bf x}_{RH}}{G^2 \cdot \cos^2 \phi_H}     \nonumber\\
{\tilde J}_{21}&=& {\tilde J}_{11} \cdot \tan \phi_H \nonumber\\
{\tilde J}_{22}&=& {\tilde J}_{12} \cdot \tan \phi_R \nonumber
\end{eqnarray}
}
The following sections deal with the accuracy of the computed intersection in the case of noisy orientation information (see Fig. \ref{fig:noise_hum_rob}).

%\newpage

\section{Transformation of Gaussian distributions}\label{intersect_1b}
\subsection{General assumptions}
Consider a nonlinear system

\begin{equation}\label{gauss_0}
{\bf z}= F(\bf x)
\end{equation}

where the random variables ${\bf x}=(x_1, x_2)^T$ denote the input, ${\bf z}=(z_1, z_2)^T$ the output and  %(see Fig. \ref{fig:block_direct})
$F$ denotes  a nonlinear transformation. The distribution of the uncorrelated Gaussian distributed components $x_1$ and $x_2$ is described by 

\begin{equation}\label{gauss_1}
f_{x_1,x_2}=\frac{1}{2 \pi \sigma_{x_1} \sigma_{x_2}}exp(-\frac{1}{2}(\frac{e_{x_1}^2}{\sigma_{x_1}^2} + \frac{e_{x_2}^2}{\sigma_{x_2}^2}))
\end{equation}

where $e_{x_1}={x_1}-\bar {x_1}$, $\bar {x_1}$ - mean$({x_1})$, $\sigma_{x_1}$ - standard deviation $x_1$
and $e_{x_2}={x_2}-\bar {x_2}$, $\bar {x_2}$ - mean$({x_2})$, $\sigma_{x_2}$ - standard deviation $x_2$.

The goal is: Given the nonlinear transformation (\ref{gauss_0}) and the distribution (\ref{gauss_0}).
Compute the output signals $z_1$ and $z_2$   and their distributions together with their standard deviations and the correlation coefficient. Linear systems 
transform Gaussian distributions linearly such that the output signals are also Gaussian distributed. This does not apply for  nonlinear systems, but  if the 
input standard deviation is small enough then a local linear transfer function can be built for which the outputs are Gaussian distributed. 
Suppose the the input standard devivations are small with respect to the nonlinear function then the output distribution can be written as follows
{\small
\begin{eqnarray}\label{gauss_2}
f_{z_1,z_2}=\frac{1}{2 \pi \sigma_{z_1} \sigma_{z_2} \sqrt{1 - \rho_{z_{12}} ^2}} \cdot  \qquad \qquad \qquad \qquad \\
exp(-\frac{1}{2(1 - \rho_{z_{12}} ^2)} (\frac{e_{z_1}^2}{\sigma_{z_1}^2} + \frac{e_{z_2}^2}{\sigma_{z_2}^2} - \frac{2 \rho_{z_{12}} e_{z_1} e_{z_2}}{\sigma_{z_1} \sigma_{z_2}}))\nonumber
\end{eqnarray}
}

$\rho_{z_{12}}$ - correlation coefficient.

\begin{figure}[bhp]
    %\epsfysize=4.5cm % 10.5
 	\centering	
    \hspace{0.0cm}	
    \includegraphics[width=0.25\textwidth]{noise_hum_rob.eps}
    \vspace{-0.0cm}
 	\caption{Intersection with noisy orientations}
 	\label{fig:noise_hum_rob}
 	\end{figure}


\subsection{{Statistical linearization, two inputs-two outputs }}\label{differential}
Let the nonlinear transformation ${\bf F}$  be described by two smooth  transfer functions
(see block scheme \ref{fig:in_out_fuzzy}) 
{\small
\begin{eqnarray} \label{gauss_3}
z_1=f_1({x_1},{x_2})\nonumber\\
z_2=f_2({x_1},{x_2})
\end{eqnarray}
}
where $({x_1},{x_2})=({\phi_R},{\phi_H})$ and $({z_1},{z_2})=({x_c},{y_c})$

Linearization   of (\ref{gauss_3}) yields
{\small
\begin{eqnarray}\label{gauss_4}
{\bf dz} = {\tilde J} \cdot {\bf dx} \quad or \quad {\bf e_z} = {\tilde J} \cdot {\bf e_x}
\end{eqnarray}
}

with
{\small
\begin{eqnarray}\label{gauss_5}
{\bf e_z} = (e_{z_1}, e_{z_2})^T \quad and \quad {\bf e_x} = (e_{x_1}, e_{x_2})^T \\
{\bf dz} = (dz_1, dz_2)^T \quad and \quad {\bf dx} = (dx_1, dx_2)^T \nonumber
\end{eqnarray}
}


\begin{figure}[thp]
   %\epsfysize=4.5cm % 10.5
	\centering	
   \hspace{-0.0cm}	
   \includegraphics[width=0.48\textwidth]{in_out.eps}
   \vspace{-0.0cm}
	\caption{Differential transformation}
	\label{fig:in_out_fuzzy}
	\end{figure}


{\small
\begin{eqnarray}\label{gauss_7} 				
{\tilde J} = { \left( \begin{array}{cc}
               \partial f_1/\partial x_1,\partial f_1/\partial x_2 \\
               \partial f_2/\partial x_1,\partial f_2/\partial x_2
                                    \end{array} \right) }
\end{eqnarray}
}


%}

\paragraph{Output distribution}\label{intersect_diff_a}

To obtain the density $f_{z_1,z_2}$ (\ref{gauss_2}) of the output signal, we  invert (\ref{gauss_5})
and substitute the entries of ${\bf e_x}$ into (\ref{gauss_1})

\begin{eqnarray}\label{gauss_8}
{\bf e_x} = J \cdot {\bf e_z}
\end{eqnarray}
with
$J={\tilde J}^{-1}$ and

{\small
\begin{eqnarray}\label{gauss_9} 				
{J} = { { \left( \begin{array}{cc}
               J_{11} & J_{12} \\
               J_{21} & J_{22}
         \end{array} \right)= \left( \begin{array}{c}
               {\bf j_{xz}} \\
               {\bf j_{yz}}
         \end{array} \right) } }
\end{eqnarray}
}

where ${\bf j_{xz}}= (J_{11},J_{12})$ and ${\bf j_{yz}}= (J_{21},J_{22})$.
Entries $J_{ij}$ are the result of the inversion of ${\tilde J}$.
From this substitution which we get


{\small
\begin{eqnarray}\label{gauss_10} 				
f_{x_1, x_2}=K_{x_1,x_2} \cdot  \qquad \qquad \qquad \qquad \nonumber \\
exp(-\frac{1}{2} \cdot {\bf e_z}^T \cdot ({\bf j_{x_1,z}}^T,{\bf j_{x_2,z}}^T)\cdot S_x^{-1} \cdot { \left( \begin{array}{c}
               {\bf j_{x_1,z}} \\
               {\bf j_{x_2,z}}
         \end{array} \right) \cdot {\bf e_z}})
\end{eqnarray}
}

      where $K_{x_1,x_2}=\frac{1}{2 \pi \sigma_{x_1} \sigma_{x_2}}$ and

{\small
\begin{eqnarray}\label{gauss_11}
S_x^{-1}= {\left( \begin{array}{cc}
               \frac{1}{\sigma_{x_1}^2},0 \\
                        0,\frac{1}{\sigma_{x_2}^2}
\end{array} \right) }
\end{eqnarray}
}

The exponent of (\ref{gauss_10}) is rewritten into

\begin{eqnarray}\label{gauss_12} 				
xpo= -\frac{1}{2} \cdot (\frac{1}{\sigma_{x_1}^2}(e_{z_1}J_{11} + e_{z_2}J_{12})^2 \nonumber \\
 +  \frac{1}{\sigma_{x_2}^2}(e_{z_1}J_{21} + e_{z_2}J_{22})^2)
\end{eqnarray}


and furthermore

{\small
\begin{eqnarray}\label{gauss_13} 				
xpo = -\frac{1}{2} \cdot [e_{z_1}^2(\frac{J_{11}^2}{\sigma_{x_1}^2} + \frac{J_{21}^2}{\sigma_{x_2}^2}) +  e_{z_2}^2(\frac{J_{12}^2}{\sigma_{x_1}^2}
+ \frac{J_{22}^2}{\sigma_{x_2}^2})  + \nonumber\\
2 \cdot e_{z_1} e_{z_2}(\frac{J_{11}J_{12}}{\sigma_{x_1}^2} + \frac{J_{21}J_{22}}{\sigma_{x_2}^2})] \qquad \qquad
\end{eqnarray}
}

%Now, we compare  xpo  in (\ref{gauss_13}) with the exponent in (\ref{gauss_2}) of the output density  (\ref{gauss_2})


Let
{\small
\begin{eqnarray}\label{gauss_15}
A=(\frac{J_{11}^2}{\sigma_{x_1}^2} + \frac{J_{21}^2}{\sigma_{x_2}^2}); \qquad
B=(\frac{J_{12}^2}{\sigma_{x_1}^2} + \frac{J_{22}^2}{\sigma_{x_2}^2})  \nonumber \\
C=(\frac{J_{11}J_{12}}{\sigma_{x_1}^2} + \frac{J_{21}J_{22}}{\sigma_{x_2}^2}) \qquad \qquad %\qquad \qquad
\end{eqnarray}
}
then a comparison of xpo in (\ref{gauss_13}) and the exponent in (\ref{gauss_2}) yields
{\small
\begin{eqnarray}\label{gauss_16}
\frac{1}{(1 - \rho_{z_{12}} ^2)} \frac{1}{\sigma_{z_1}^2}=A; \qquad
\frac{1}{(1 - \rho_{z_{12}} ^2)} \frac{1}{\sigma_{z_2}^2}=B \nonumber \\
\frac{-2 \rho_{z_{12}}} {(1 - \rho_{z_{12}} ^2)} \frac{1}{\sigma_{z_1} \sigma_{z_2}}=2 C \qquad \qquad %\qquad  \qquad \qquad
\end{eqnarray}
 }
 Standard deviations $\sigma_{z_1}$, $\sigma_{z_2}$ and correlation coefficient $\rho_{z_{12}}$ yield

{\small
\begin{eqnarray}\label{gauss_17}
\rho_{z_{12}}= - \frac{C}{\sqrt{AB}}  \qquad \qquad \qquad \qquad \nonumber \\
\frac{1}{\sigma_{z_1}^2}=A - \frac{C^2}{B}; \qquad
\frac{1}{\sigma_{z_2}^2}=B - \frac{C^2}{A}
\end{eqnarray}
}


The result is: if the parameter of the input distribution and the transfer function $F(x,y)$ are known,then the output  distribution parameters can be  
computed straight forward.


\paragraph{{Fuzzy solution}}\label{fuzzy}

To save computing costs in real time we create a TS fuzzy model that is represented by the rules $R_{ij}$


\begin{eqnarray}\label{gauss_18}
R_{ij}: \qquad \qquad \qquad \qquad \\
IF \quad {x_1}={X_1}_i \quad AND \quad {x_2}={X_2}_i  \nonumber\\
THEN  \quad \rho_{z_{12}}= - \frac{C_{ij}}{\sqrt{A_{ij}B_{ij}}}\nonumber \\
                                                              AND \qquad \qquad \qquad \qquad \qquad\nonumber \\
                                                              \frac{1}{\sigma_{z_1}^2}=A_{ij} - \frac{C_{ij}^2}{B_{ij}}; \nonumber \\
                                                              AND \qquad \qquad \qquad \qquad \qquad \nonumber\\
                                                              \frac{1}{\sigma_{z_2}^2}=B_{ij} - \frac{C_{ij}^2}{A_{ij}}\nonumber
\end{eqnarray}
where ${X_1}_i,{X_2}_i$ are fuzzy terms for $x_1, x_2$,  $A_{ij}, B_{ij}, C_{ij} $ are  functions of predefined variables $x_1={x_1}_{i}$ and $x_2={x_2}_{i}$

From (\ref{gauss_18}) we derive


\begin{eqnarray}\label{gauss_19}
\rho_{z_{12}}= - \sum_{ij} w_i(x_1) w_j(x_2) \frac{C_{ij}}{\sqrt{A_{ij}B_{ij}}}\nonumber \\
\frac{1}{\sigma_{z_1}^2}= \sum_{ij} w_i(x_1) w_j(x_2) (A_{ij} - \frac{C_{ij}^2}{B_{ij}}) \\
\frac{1}{\sigma_{z_2}^2}= \sum_{ij} w_i(x_1) w_j(x_2) (B_{ij} - \frac{C_{ij}^2}{A_{ij}})\nonumber
\end{eqnarray}

$w_i({x_1})\in [0,1]$ and $w_j({x_2}) \in [0,1]$ are weighting functions with $\sum_i w_i({x_1})=1$, $\sum_j w_j({x_2})=1$


\paragraph{Inverse Solution}\label{inverse}
The previous paragraph  discussed the direct transformation task: Let the distribution parameters of the input variable be defined, find the corresponding 
output parameters. However it might also be useful to solve the inverse task: Given the output parameters (standard deviation, correlation coefficient),
find the corresponding input parameters.
This solution of inverse task is similar to those discussed in section  \ref{differential}. The starting point are equations (\ref{gauss_1}) and (\ref{gauss_2})  
which describe the distributions of the inputs and outputs, respectively. Then we substitute (\ref{gauss_4}) into (\ref{gauss_1}) and rename  the resulting 
exponent $xpo_z$ into $xpo_x$ and discuss the exponent $xpo_x$ 

\begin{eqnarray}\label{gauss_20}
xpo_x = \frac{-1}{2(1 - \rho_{z_{12}} ^2)}({\bf e_x}^T {\tilde J}^T S_z^{-1} {\tilde J} {\bf e_x}  - \frac{2 \rho_{z_{12}} e_{z_1} e_{z_2}}{\sigma_{z_1} \sigma_{z_2}})
\end{eqnarray}
with
{\small
\begin{eqnarray}\label{gauss_20a}
S_x^{-1}= {\left( \begin{array}{cc}
               \frac{1}{\sigma_{z_1}^2},0 \nonumber\\
                        0,\frac{1}{\sigma_{z_2}^2}\nonumber
\end{array} \right) }
\end{eqnarray}
}


Now, comparing (\ref{gauss_20}) with the exponent of (\ref{gauss_1}) of the input density we find that the mixed term in (\ref{gauss_20}) must be zero
from which we obtain  the correlation coefficient $\rho_{z_{12}}$ and with this  the standard deviations of the inputs  

\begin{eqnarray}\label{gauss_25}
\rho_{z_{12}}= (\frac{{\tilde J_{11}}{\tilde J_{12}}}{\sigma_{z_1}^2} +  \frac{{\tilde J_{21}}{\tilde J_{22}}}{\sigma_{z_2}^2}) \frac{\sigma_{z_1}\sigma_{z_2}}{({\tilde J_{11}} {\tilde J_{22}} + {\tilde J_{12}} {\tilde J_{21}})}\nonumber\\
\frac{1}{\sigma_{x}^2}=( \frac{{\tilde J_{11}}^2}{\sigma_{z_1}^2} +  \frac{{\tilde J_{21}}^2}{\sigma_{z_2}^2}
 - \frac{2 \rho_{z_{12}}}{\sigma_{z_1} \sigma_{z_2}}{\tilde J_{11}}{\tilde J_{21}} )/ (1 - \rho_{z_{12}} ^2)\\
 \frac{1}{\sigma_{y}^2}=( \frac{{\tilde J_{12}}^2}{\sigma_{z_1}^2} +  \frac{{\tilde J_{22}}^2}{\sigma_{z_2}^2} - \frac{2 \rho_{z_{12}}}{\sigma_{z_1} \sigma_{z_2}}{\tilde J_{12}}{\tilde J_{22}}  )/(1 - \rho_{z_{12}}^2) \nonumber
\end{eqnarray}
The detailed development can be found in \cite{palm2019b}.



\subsection{{Six inputs - two outputs}}\label{six_inputs}

Consider again the  nonlinear system


\begin{equation}\label{gauss_0a}
{\bf x_c}= F({\bf x})
\end{equation}

In the previous subsections we assumed the positions ${\bf x}_R$ and ${\bf x}_H$ not to be currupted with noise.
However taking into account the positions to be random variables, the number of inputs are 6 so that the input vector yields ${\bf x}=(x_1, x_2, x_3, x_4, x_5, x_6)^T$ or  
${\bf x}=(\phi_R, \phi_H, x_{R}, y_{R},  x_{H}, y_{H})$ with the output vector ${\bf x_c}=(x_c, y_c)^T$ .

Furthermore let the uncorrelated Gaussian distributed inputs $x_1$ ... $x_6$ be described by the 6-dim density

\begin{equation}\label{gauss_1a}
f_{x_i}=\frac{1}{(2 \pi)^{6/2}|S_x|^{1/2}} exp(-\frac{1}{2}({{\bf e_x}^T}{S_x}^{-1} {\bf e_x}))
\end{equation}
where ${\bf e_x}=({e_x}_1, {e_x}_2, ..., {e_x}_6)^T$; ${\bf e_x}={\bf x}-{\bar {\bf x}}$, ${\bar {\bf x}}$ - mean(${\bf x}$), $S_x$ - covariance matrix.

\begin{displaymath}\label{gauss_3a}
S_x= {\left( \begin{array}{cccc}
                        \sigma_{x_1}^2 & 0&...&0 \cr
                        0&\sigma_{x_2}^2& ...& 0\cr
                                          ...&...&...&...\cr
                         0& ... &0&\sigma_{x_6}^2
                                             \end{array} \right) }
\end{displaymath}

According to (\ref{gauss_2}) the output density is described by



\begin{eqnarray}\label{gauss_2a}
f_{x_c,y_c}=\frac{1}{2 \pi \sigma_{x_c} \sigma_{y_c} \sqrt{1 - \rho ^2}} \cdot \\
exp(-\frac{1}{2(1 - \rho ^2)} ({\bf e_{x_c}^T} {S_c}^{-1}{\bf e_{x_c}}  - \frac{2 \rho e_{x_c} e_{y_c}}{\sigma_{x_c} \sigma_{y_c}}))\nonumber
\end{eqnarray}



$\rho$ - correlation coefficient, ${\bf e}_{x_c}=(e_{x_c},e_{y_c})^T$.



After some calculations \cite{palm2021} we find for $\rho$, $\frac{1}{\sigma_{x_c}^2}$ and $\frac{1}{\sigma_{y_c}^2}$

\begin{eqnarray}\label{gauss_2ac}
\rho &=& -\frac{C}{\sqrt{AD}} \nonumber\\
 \frac{1}{\sigma_{x_c}^2}&=&A - \frac{C^2}{D}; \quad \frac{1}{\sigma_{y_c}^2}=D - \frac{C^2}{A}
\end{eqnarray}
with

\begin{eqnarray}\label{gauss_2ab}
A=\sum_{i=1}^6 \frac{1}{\sigma_{x_i}^2} J_{i1}^2; \quad
B=\sum_{i=1}^6 \frac{1}{\sigma_{x_i}^2} J_{i1}J_{i2} \\
C=\sum_{i=1}^6 \frac{1}{\sigma_{x_i}^2} J_{i1}J_{i2}; \quad
D=\sum_{i=1}^6 \frac{1}{\sigma_{x_i}^2} J_{i2}^2\nonumber
\end{eqnarray}


which is the counterpart to  the 2-dim input case (\ref{gauss_17}).


\paragraph{Inverse solution}
An inverse solution cannot be uniquely computed due to the underdetermined character of the 6-input -- 2-output system.
Therefore, from  required variances at the intersection position (output) corresponding variances for positions and orientations
of robot-human or robot-robot  (input) cannot be concluded.


\paragraph{{Fuzzy approach}}\label{six_fuzz}

The steps to the fuzzy approach is very similar to those of the 2-input case:\newline
- define operation points ${\bf x}_i=(x_1, x_2, x_3, x_4, x_5, x_6)_i^T$ \newline
- compute $A_{i}$, $B_{i}$, $C_{i}$ at ${\bf x}_i=(x_1, x_2, x_3, x_4, x_5, x_6)_i^T$ from (\ref{gauss_2ab})\newline
- formulate  fuzzy rules $R_i$ according to (\ref{gauss_18}) and (\ref{gauss_19}), $i=1...n$ \newline

The number $n$ of rules is computed as follows: \newline
With $l=6$ - number of fuzzy terms, $k=6$ - number of inputs, we obtain $n=l^k=6^6$ - number of rules.\newline
This number of rules is unacceptable high. To limit $n$ to an adequate number, one has to limit the number of inputs and/or
fuzzy terms  to look for the most influential variables either on heuristic or systematic way  \cite{schaefer2005}. This however is not
the issue to be discussed in this paper.

%\newpage

\section{{Sigma-Point-Transformation}}\label{sigma_trans}
In the following the estimation/identification of the standard deviations of possible intersection coordinates of trajectories for both robot/robot and 
human/robot - combinations by means of the sigma-point technique is discussed. The following method is based on the  Unscented Kalman Filter technique where 
the intersections cannot be directly measured but predicted/computed only. Nevertheless it is possible to compute the variance of the predicted events, 
such as possible collisions or planned rendezvous situations,  by a direct  propagation of statistical parameters - the sigma-points - through the nonlinear 
geometrical relation which is a result of the crossing of two trajectories.
Let \newline
${\bf x}=(x_1, x_2)^T$ - input vector, ${\bf x_c}=(x_{c1}, x_{c2})^T$ - output vector 
where for the special case $(x_1, x_2)^T = (\phi_R, \phi_H)^T$ and $(x_{c1}, x_{c2})^T=(x_{c}, y_{c})^T$
The nonlinear relation between ${\bf x}$ and ${\bf x_c}$ is given by (\ref{sigma_1})

\begin{equation}\label{sigma_1}
{\bf x_c}={\bf F(x)}
\end{equation}

For the discrete case we obtain for the state ${\bf x_c}$
\begin{equation}\label{sigma_2}
{\bf x_c}(k)={\bf F}({\bf x}(k-1) + {\bf w}(k-1)) 
\end{equation}

and for the measured output ${\bf z_c}(k)$
\begin{equation}\label{sigma_3}
{\bf z_c}(k)={{\bf h(x_c)}(k) + {\bf v}(k))} 
\end{equation}
where ${\bf w}$ and ${\bf v}$ are the system noise and measurement noise, respectively.
${\bf h(x_c)}$ is the output nonlinearity. Let furthermore \newline
$\bar{{\bf x}}(k)$ - mean at time $t_k$ \newline
${\bf P}(k)$ - covariance matrix \newline
${\bf x}_0$ - initial state with known mean $\mu_0 = E({\bf x}_0)$ \newline
${\bf P}_0(k) = E[({\bf x}_0 - \mu_0)({\bf x}_0 - \mu_0)^T ]$ 


\paragraph{Selection of sigma-points}

   \begin{figure}[hbp]
   \centering
    \hspace{0.0cm}
  \includegraphics[width=0.40\textwidth]{sigma_points_2D.eps}
   \hspace{1.0cm}
	\caption{Sigma-points for a 2-dim Gaussian random variable}	
	\label{fig:sigma_points 1}
	\end{figure}
	
Sigma-points are selected parameters of a given error distribution of a random variable. Sigma-points lie along the major eigen-axes 
of the covariance matrix of the random variable. The height of each sigma-point (see Fig. \ref{fig:sigma_points 1}) represents its relative weight 
$W^j$ used in the following selection procedure. 

Let ${\bf X}(k-1)$ be a  set of $2n+1$ sigma-points where $n$ is the dimension of the state space (in our example $n=2$).

\begin{equation}\label{sigma_4}
{\bf X}(k-1)=\{({\bf x}^j(k-1),W^j)|j=0...2n\} 
\end{equation}

Consider the following selection of sigma-points


\begin{eqnarray}\label{sigma_5}
{\bf x}^0(k-1)&=& {\bar{\bf x}}(k-1)\nonumber\\
& &-1 < W^0 < 1 \\
W^0&=&\frac{\lambda}{n + \lambda};\quad \lambda= \alpha^2(n + \kappa) - n \nonumber\\
{\bf x}^i(k-1)&=& {\bar{\bf x}}(k-1) + \sqrt {(\frac{n}{1-W^0}{\bf P}(k-1))}; \quad i=1 ... n\nonumber\\
 {\bf x}^i(k-1)&=& {\bar{\bf x}}(k-1) - \sqrt {(\frac{n}{1-W^0}{\bf P}(k-1))}; \quad i=(n+1) ... 2n\nonumber\\
 W^j&=&\frac{1-W^0}{2n}\nonumber\\
% \forall i&=&1...n \nonumber
\end{eqnarray}

under the following condition 
\begin{equation}\label{sigma_6}
\sum_{j=0}^{2n}W^j=1
\end{equation}
$\alpha$ and $\kappa$ are scaling factors. A usual choice is $\alpha=10^{-2}$ and $\kappa =0$. \newline
$\sqrt{\frac{n}{1-W^0}{\bf P}(k-1)}$ is the row/column of the matrix square root of ${\frac{n}{1-W^0}{\bf P}}$. The square root
of a matrix ${\bf P}$ is the solution ${\bf S}$ for ${\bf P} = {\bf S} \cdot {\bf S}$ which is obtained by  Cholesky factorization.

\paragraph{Model forecast step}
To go on with the UKF, the following step is devoted to the model forecast.
In this way the sigma-points ${\bf x}^j(k)$ are propagated through the nonlinear process model

\begin{equation}\label{sigma_7}
{\bf x}_c^{f,j}(k)={\bf F}({\bf x}^j(k-1))
\end{equation}

where the superscript $f$ means "forecast".
From these transformed and forecasted sigma-points the mean and covariance for the forecast value of ${\bf x}_c(k)$

\begin{eqnarray}\label{sigma_8}
&{\bf x}_c^f(k)&=\sum_{j=0}^{2n}W^j {\bf x}_c^{f,j}(k)\nonumber\\
&{\bf P}^f(k)&=\sum_{j=0}^{2n}W^j ({\bf x}_c^{f,j}(k) - {\bf x}_c^{f}(k))({\bf x}_c^{f,j}(k) - {\bf x}_c^{f}(k))^T
% &+& {\bf Q}(k-1) \nonumber
\end{eqnarray}

\paragraph {Measurement update step}
In this step the sigma-points are propagated through the nonlinear observation model 

\begin{equation}\label{sigma_9}
{\bf z}_c^{f,j}(k)={\bf h}({\bf x}_c^j(k-1))
\end{equation}
from which we obtain mean and covariance (innovation covariance)

\begin{eqnarray}\label{sigma_10}
{\bf z}_c^f(k-1)=\sum_{j=0}^{2n}W^j {\bf z}_c^{f,j}(k-1)\nonumber\\
Cov(\tilde{\bf z}_c^f(k-1))=\\
\sum_{j=0}^{2n}W^j ({\bf z}_c^{f,j}(k-1) - {\bf z}_c^{f}(k-1))\times \nonumber\\  
({\bf z}_c^{f,j}(k-1) - {\bf z}_c^{f}(k-1))^T + {\bf R}(k)\nonumber
\end{eqnarray}

and the cross-covariance
\begin{eqnarray}\label{sigma_11}
Cov(\tilde{\bf x}_c^f(k),\tilde{\bf z}_c^f(k-1))=\nonumber\\
\sum_{j=0}^{2n}W^j ({\bf x}_c^{f,j}(k) - {\bf x}_c^{f}(k)) ({\bf z}_c^{f,j}(k-1) - {\bf z}_c^{f}(k-1))^T
\end{eqnarray}


\paragraph{Data assimilation step}

In this step, called  the forecast information is combined with the new information from the output
${\bf z}(k)$ from which we obtain with the Kalman filter gain ${\bf K}$
\begin{equation}\label{sigma_12}
\hat{\bf x}_c(k)={\bf x}_c^f(k) + {\bf K}(k)({\bf z}_c(k) - {\bf z}_c^f(k-1))
\end{equation}
The gain $\bf K$ is given by

\begin{equation}\label{sigma_13}
{\bf K}(k)= Cov(\tilde{\bf x}_c^f(k),\tilde{\bf z}_c^f(k-1)) \cdot Cov^{-1}(\tilde{\bf z}_c^f(k-1))
\end{equation}

and the posterior covariance is updated by
\begin{equation}\label{sigma_14}
{\bf P}(k)= {\bf P}^f(k) - {\bf K}(k)\cdot Cov(\tilde{\bf z}_c^f(k-1)){\bf K}^T(k)
\end{equation}

Usually it is sufficient to compute mean and variance for the output/state ${\bf x}_c$ of the nonlinear static system ${\bf F(x)}$.
In this case it is possible to stop a further computing at eq. (\ref{sigma_8}) meaning rather to calculate the transformed sigma-points 
${\bf x}_c^{f,j}$ and develop the regarding output means and variances from (\ref{sigma_7}) and (\ref{sigma_8}). In this connection it is enough
to substitute the covariance matrix $\bf Q$ into (\ref{sigma_5}) instead of $\bf P$.
One advantage of the sigma-point approach prior to statistical linearization is the easy scalability to multi-dimensional random variables.

For the intersection problem there are 2 cases: \newline
1. 2 inputs, 2 outputs (2 orientation angles, 2 crossing coordinates) \newline
2. 6 inputs, 2 outputs (2 orientation angles and 4 position coordinates,  2 crossing coordinates) \newline
For the statistical  linearization (method 1) the step from the 2 inputs - 2 outputs case to the (6,2)-case is computationally more
costly than that for the sigma-point approach (method 2), (see eqs. (\ref{gauss_12}) ... (\ref{gauss_17}) versus eqs. (\ref{sigma_4}),  (\ref{sigma_6})... (\ref{sigma_8}).

\paragraph{{Sigma-points - Fuzzy solutions}}\label{sigma_trans_fuzz}
In order to lower the computing effort the application of TS-fuzzy interpolation may be a solution
which will be shown in the following.
Having a look at the 2-dimensional  problem  we can see a nonlinear propagation of the input sigma-points 
through a nonlinear function ${\bf F}$. 
Let ${\bf x}^j$ be the 2-dimensional "input" sigma-points 

\begin{equation}\label{sigma_15}
{\bf x}^j= (x_1^j,x_2^j)^T
\end{equation}

or for the special case "intersection"

\begin{equation}\label{sigma_16}
{\bf x}^j= (\phi_R^j,\phi_H^j)^T
\end{equation}

The propagation through ${\bf F}$ leads to "output" sigma-points

\begin{equation}\label{sigma_17}
{\bf x}_c^{f,j}(k)= {\bf F}({\bf x}^j(k-1))
\end{equation}

or for the special case

\begin{eqnarray}\label{sigma_18}
{\bf x}_c^{f,j}(k)&=& {\bf F}(x_1^j(k-1),x_2^j(k-1))=\nonumber\\
& &{\bf F}(\phi_R^j(k-1),\phi_H^j(k-1))
\end{eqnarray}

The special nonlinear function ${\bf F}$ is described by (see (\ref{inter_fuzzy_1}))
\begin{eqnarray}\label{sigma_19}
{\bf x}_c = {\bf A}_{RH}(\phi_R,\phi_H) \cdot {\bf x}_{RH}
\end{eqnarray}

where ${\bf A}_{RH}$ is a nonlinear matrix (\ref{inter_fuzzy_2}) linearly combined with the position vector
${\bf x}_{RH}=(x_R, y_R,x_H,y_H)^T$. 




A fuzzification aims at ${\bf A}_{RH}$:

\begin{eqnarray}\label{sigma_20}
{\bf F}^{fuzz}(\phi_R,\phi_H)={\bf A}_{RH}^{fuzz}\cdot{\bf x}_{RH}=\nonumber\\
 \sum_{l_1,l_2}^m w^{l_1}(\phi_R) w^{l_2}(\phi_H)\cdot {\bf A}_{RH}(\phi_R^{l_1},\phi_H^{l_2}) \cdot {\bf x}_{RH}
\end{eqnarray}


Applied to the sigma-points $(\phi_R^j,\phi_H^j)$ we get a TS fuzzy model described by the following rules $R_{l_1,l_2}$

\begin{eqnarray}\label{sigma_20a}
R_{l_1,l_2}: \qquad \qquad \qquad \qquad \\
IF \quad {\phi_R^j}={\Phi_R^j}_{l_1} \quad AND \quad {\phi_H^j}={\Phi_H^j}_{l_2}  \nonumber\\
THEN  \quad {\bf x}_c^{f,j}={\bf A}_{RH}(\phi_R^{l_1,j},\phi_H^{l_2,j}) \cdot {\bf x}_{RH}\nonumber
\end{eqnarray}
where ${\Phi_R^j}_{l_1},{\Phi_H^j}_{l_2}$ are fuzzy terms for ${\phi_R^j}, {\phi_H^j}$;  the matrices ${\bf A}_{RH}$ are  
functions of predefined variables ${\phi_R^j}$ and ${\phi_H^j}$.
This set of rules leads to the  result

\begin{eqnarray}\label{sigma_21}
{\bf x}_c^{f,j}={\bf F}^{fuzz}(\phi_R^j,\phi_H^j)=\nonumber\\
\sum_{l_1,l_2}^m w^{l_1}(\phi_R^j) w^{l_2}(\phi_H^j)\cdot {\bf A}_{RH}(\phi_R^{l_1,j},\phi_H^{l_2,j}) \cdot {\bf x}_{RH}
\end{eqnarray}
$w^{l_1}(\phi_R^j)\in [0,1]$ and $w^{l_2}(\phi_H^j) \in [0,1]$ are weighting functions with $\sum_{l_1} w^{l_1}=1$,  $\sum_{l_2} w^{l_2}=1$.
The advantage of this approach is that the $l_1 \times l_2$ matrices ${\bf A}_{RH}^{l_1, l_2,j}={\bf A}_{RH}(\phi_R^{l_1,j},\phi_H^{l_2,j})$
can be computed off line.
Then, the calculation of mean and covariance matrix is obtained by
\begin{eqnarray}\label{sigma_22}
{\bf x}_c^f(k)=\sum_{j=0}^{2n}{\bf W}^j {\bf x}_c^{f,j}(k)\nonumber\\
{\bf P}^f(k)=\sum_{j=0}^{2n}{\bf W}^j {\tilde{\bf x}}_c^{f,j}(k) ({\tilde{\bf x}}_c^{f,j}(k))^T\\
{\tilde{\bf x}}_c^{f,j}={\bf x}_c^{f,j} - {\bf x}_c^{f}\nonumber
\end{eqnarray}

From the covariance ${\bf P}^f$ the variances ${\sigma_c}_{xx},{\sigma_c}_{yy},{\sigma_c}_{xy}$ can be obtained

\begin{eqnarray}\label{sigma_23}
{\sigma_c}_{xx}=E((x_c^f- {\bar x}_c^f)^2))\nonumber\\
{\sigma_c}_{yy}=E((y_c^f- {\bar y}_c^f)^2))\\
{\sigma_c}_{xy}={\sigma_c}_{yx}=E((x_c^f- {\bar x}_c^f) \cdot (y_c^f- {\bar y}_c^f))\nonumber
\end{eqnarray}
\paragraph{Inverse solution}
The inverse solution for the Sigma-Point approach is much easier to get than that for the statistical linearization method.
Starting from eq. (\ref{sigma_1}) we build the inverse function
\begin{equation}\label{sigma_24}
{\bf x}={\bf F}^{-1}({\bf x}_c)
\end{equation}
on the condition that ${\bf F}^{-1}$ exists. Then the covariance matrix $\bf P$ is defined in correspondence to the 
required variances ${\sigma_c}_{xx},{\sigma_c}_{yy}$, and ${\sigma_c}_{xy}$. The following next steps correspond to equations
(\ref{sigma_1})-(\ref{sigma_8}). The position vector ${\bf x}_{RH}$ is assumed to be  known. The inversion of ${\bf F}$ requirers
a linearization of ${\bf x}_{RH}$ and a starting point to obtain a stable convergence to the inverse ${\bf F}^{-1}$. The result is
the mean $\bf x$ and the covariance $\bf Q$ at the input. A reliable inversion is only possible for the 2-input 2-output case.

\paragraph{6-inputs 2-outputs}
This case works exactly as the 2-input 2-output case along eqs. (\ref{sigma_1})-(\ref{sigma_8}) due to the fact
that the computation of the Sigma-Points (\ref{sigma_5})-(\ref{sigma_6}) and the propagation through the nonlinearity $\bf F$
includes  automatically the input and output dimensions.


\section{{Simulation results}}\label{simulation}
The following simulations show results of uncertainties of predicted intersections based on statistical linearization and sigma-point-Transformation.
For both methods identical parameters are employed for comparisson reasons (see fig. \ref{fig:dist_human_rob_2})
Position/orientation of robot and human are given by \newline
${\bf x}_R=(x_R,y_R)^T=(2,0)^T$m \newline 
${\bf x}_H=(x_H,y_H)^T=(4,10)^T$m  \newline
$\phi_R= 1.78                                                                                                                                                                                                                                                                                                                                                                                                                                           rad = 102^\circ$, \newline
$\phi_H=3.69rad = 212^\circ$.\newline
$\phi_R$ and  $\phi_H$ are corrupted by Gaussian noise with standard deviations (std) of $\sigma_{\phi_R}=\sigma_{x_1}=0.02$ rad, 
$(=1.1^\circ)$, $\sigma_{\phi_H}=\sigma_{x_2}=0.02$ rad, $(=1.1^\circ)$ .
\subsubsection*{Statistical linearization}

Table \ref{tab1} shows a comparison of the  non-fuzzy method  with the fuzzy approach  using  sectors  of $60^\circ, 30^\circ, 15^\circ, 7.5^\circ$  
of the unit circle for the orientations of robot and human.  %and figures \ref{fig:noise_60_deg}-\ref{fig:noise_7k5_deg}. 
Notations in table \ref{tab2} are: ${\sigma_{x}}_c$ - std-computed, ${\sigma_{x}}_m$ - std-measured etc. As expected, we see that
higher  resolutions lead to a better match between fuzzy and analytical approach.  
Furthermore, the match between measured and calculated values depends on the form of membership functions (MFS). 
For example, low input standard deviations (0.02 rad) show a better match for Gaussian membership functions, higher input standard deviations 
(0.05 rad = $2.9^\circ$) require Gaussian bell shape membership functions which comes from different smoothing effects 
(see columns 4 and 5 in table \ref{tab2}).\newline
A comparison of control surfaces and corresponding measurements $x_{cm}, y_{cm}$ (black and red dots)
is depicted in figures \ref{fig:analyt_mesh} - \ref{fig:7k5_fuzz_mesh}. Figure \ref{fig:analyt_mesh} shows the control surface of 
$x_c$ and $y_c$ for the non-fuzzy case (\ref{inter_4}). Control surfaces of the fuzzy approximations (\ref{inter_fuzzy_3}) for $30^\circ$ and $7.5^\circ$ 
sectors  \cite{palm2019b} are shown in figures \ref{fig:30_fuzz_mesh} - \ref{fig:7k5_fuzz_mesh}.  Resolution 
$30^\circ$ (fig. \ref{fig:30_fuzz_mesh}) shows a very high deviation compared to the non-fuzzy approach (fig. \ref{fig:analyt_mesh}) 
which decreases further down to resolution $7.5^\circ$ (fig. \ref{fig:7k5_fuzz_mesh}). This explains the high differences  
between measured and computed standard deviations and correlation coefficients, in particular for sector sizes of $30^\circ$  and higher.



\begin{table}[thb]
 \caption{Standard deviations, fuzzy and non-fuzzy results}
 \hspace{4.0cm}
 \small{
 \centering
\begin{tabular}{|c|c|c|c|c|c|c|}\hline \label{tab1}
input std&\multicolumn{4}{c|}{0.02 Gauss, bell shaped (GB)} & {Gauss} &{0.05 GB}\cr \hline
    sector size/ $^\circ$         &     $60^\circ$              &        $30^\circ$           &        $15^\circ$   &    ${\bf 7.5^\circ }$ &    ${\bf 7.5^\circ }$ & ${\bf 7.5^\circ }$         \\ \hline \hline
	non-fuzz ${\sigma_{x}}_c$   & 0.143 & 0.140 &  0.138 &     {\bf 0.125}  &  {\bf 0.144}  &    {\bf 0.366}             \\ \hline
	fuzz   ${\sigma_{x}}_c$   & 0.220 & 0.184 & 0.140  &     {\bf 0.126}  &  {\bf 0.144}  &   {\bf 0.367}               \\ \hline
	%fuzz M2  ${\sigma_{z_1}}_c$   & 0.177 & 0.190 & 0.141  &     {\bf 0.141}  &  {\bf 0.142}  &   {\bf 0.368}           \\ \hline
    non-fuzz ${\sigma_{x}}_m$   & 0.160 & 0.144 & 0.138  &     {\bf 0.126}  &  {\bf 0.142}  &   {\bf 0.368}         \\ \hline
	fuzz   ${\sigma_{x}}_m$   & 0.555 & 0.224 & 0.061  &     {\bf 0.225}    &  {\bf 0.164}  &   {\bf 0.381}          \\ \hline  \hline
	%fuzzy M2  ${\sigma_{z_1}}_m$& $J^\dagger_{3,1}$ & $J^\dagger_{3,2}$ & $J^\dagger_{3,3}$  &                    \\ \hline \hline 	
    non-fuzz ${\sigma_{y}}_c$   & 0.128 & 0.132 & 0.123  &     {\bf 0.114}  &  {\bf 0.124}  &   {\bf 0.303}              \\ \hline
	fuzz  ${\sigma_{y}}_c$   & 0.092 & 0.087 & 0.120  &     {\bf 0.112}  &  {\bf 0.122}  &   {\bf 0.299}             \\ \hline
	%fuzz M2  ${\sigma_{z_2}}_c$   & 0.160 & 0.150 & 0.120  &     {\bf 0.119}  &  {\bf 0.119}  &   {\bf 0.306}             \\ \hline
	non-fuzz ${\sigma_{y}}_m$   & 0.134 & 0.120 & 0.123  &     {\bf 0.113}  &  {\bf 0.129}  &   {\bf 0.310}                \\ \hline
	fuzz   ${\sigma_{y}}_m$   & 0.599 & 0.171 & 0.034  &       {\bf 0.154}  &  {\bf 0.139}  &   {\bf 0.325}              \\ \hline \hline
	%fuzzy M2  ${\sigma_{z_2}}_m$& $J^\dagger_{3,1}$ & $J^\dagger_{3,2}$ & $J^\dagger_{3,3}$  &                    \\ \hline \hline
	non-fuzz ${\rho_{xy}}_c$  & 0.576 & 0.541 & 0.588  &     {\bf 0.561}  &  {\bf 0.623}  &   {\bf 0.669}            \\ \hline
	fuzz   ${\rho_{xy}}_c$  & -0.263& 0.272 & 0.478  &     {\bf 0.506}  &  {\bf 0.592}  &   {\bf 0.592}              \\ \hline
	%fuzz M2  ${\rho_{z_{12}}}_c$  & -0.461& 0.177 & 0.481  &     {\bf 0.524}  &  {\bf 0.538}  &   {\bf 0.535}             \\ \hline
	non-fuzz ${\rho_{xy}}_m$  & 0.572 & 0.459 & 0.586  &     {\bf 0.549}  &  {\bf 0.660}  &   {\bf 0.667}            \\ \hline
	fuzz   ${\rho_{xy}}_m$  & 0.380 & 0.575 & 0.990  &       {\bf 0.711}  &  {\bf 0.635}  &   {\bf 0.592}           \\ \hline  \hline
	%fuzzy  M2 ${\rho_{z_{12}}}_m$& $J^\dagger_{3,1}$ & $J^\dagger_{3,2}$ & $J^\dagger_{3,3}$  &                     \\ \hline \hline
\end{tabular}
}
 \end{table}




 	\begin{figure}[htb]
 	 \hspace{-2.0cm}
     \begin{minipage}{0.70\textwidth}
        \begin{center}
         \includegraphics[width=0.65\textwidth]{analyt_mesh_new.eps} 
    \hspace{-2.0cm}
 	\caption{Control surface non-fuzzy}
 	\label{fig:analyt_mesh}
 	\end{center}
      \end{minipage}\hfill
      \hspace{-3.5cm}
    %  \end{figure}
      \begin{minipage}{0.70\textwidth}
      \begin{center}
      \vspace{-0.0cm}
    \includegraphics[width=0.65\textwidth]{30_fuzz_mesh_new.eps}   
     \hspace{-0.0cm}
 	\caption{Control surface fuzzy, $30^\circ$}
 	\hspace{-0.0cm}
 	\label{fig:30_fuzz_mesh}
 	\end{center}
     \end{minipage}\hfill
    \end{figure}	

  
  \begin{figure}[hpt]
      \centering
      %\hspace{-0.8cm}
       \includegraphics[width=0.55\textwidth]{7k5_fuzz_mesh_new.eps}     
   \vspace{-0.0cm}
	\caption{Control surface fuzzy, $7.5^\circ$}
	\label{fig:7k5_fuzz_mesh}
	 %\hspace{-1.8cm}
  \end{figure}

\newpage

\subsubsection*{Sigma-Point method}

{\bf 2-inputs - 2-outputs}: \newline

The simulation of the sigma-point method is based on a Matlab implementation of an unscented Kalman filter by \cite{cao2021}.
The first example deals with the 2-inputs - 2-outputs case in which  only the orientations are taken into account, but the disturbances of the positions 
of robot and human are not part of the sigma-point calculation. A comparison between the computed and measured covariance show a very good match.
The same holds for the standard deviations ${\sigma_{x}}_c, {\sigma_{y}}_c$. A comparison with the statistical linearization shows a good match as well
(see table \ref{tab2}, rows 1 and 2).

A view at the sigma-points presents the following results: Figure \ref{fig:sigmapoints_in} shows the two-dimensional distribution of the orientation
angles $(\phi_R$, $\phi_H)$ and the corresponding sigma-points $s_1, ..., s_5$ where $s_1$ denotes the mean value. 
Figure \ref{fig:sigmapoints_out} shows the two-dimensional distribution of the intersection coordinates $(x_c, y_c)$ with the sigma-points $S_1, ..., S_5$.
$S_1$ denotes the mean value and $S_1, ..., S_5$ are distributed in such a way that the $s_i$ are transformed  into $S_i$, $i=1...5$.
From both figures an optimal selection of both  $s_1, ..., s_5$ and $S_1, ..., S_5$ can be observed which results in a good match of the computed and measured
standard deviations $\sigma_{{\bf x}_c}$.



\begin{figure}[htb]
 	 \hspace{-2.0cm}
     \begin{minipage}{0.70\textwidth}
        \begin{center}
         \includegraphics[width=0.65\textwidth]{s_orig.eps} 
    \hspace{-2.0cm}
 	\caption{$Sigma-Points$, input}
 	\label{fig:sigmapoints_in}
 	\end{center}
      \end{minipage}\hfill
      \hspace{-3.5cm}
    %  \end{figure}
      \begin{minipage}{0.70\textwidth}
      \begin{center}
      \vspace{0.5cm}
    \includegraphics[width=0.65\textwidth]{s_trans.eps}   
     \hspace{-0.0cm}
 	\caption{$Sigma-Points$, output}
 	\hspace{-0.0cm}
 	\label{fig:sigmapoints_out}
 	\end{center}
     \end{minipage}\hfill
    \end{figure}

\newpage
	
 {\bf 6-inputs - 2-outputs}: \newline
 The 6-inputs 2-outputs example shows that the additional consideration of 4 input position coordinates with $\sigma_{{\bf x}_R}=0.02$ leads to similar 
results both for computed and measured covariances and between sigma-point method and statistical linearization
(see ${ P}(7,7)={\sigma_x}_c^2$, ${ P}(8,8)={\sigma_y}_c^2$, and ${ covar}(7,7)={\sigma_x}_m^2$, ${ covar}(8,8)={\sigma_y}_m^2$, ${\sigma_x}_c^2$ - 
computed, ${\sigma_x}_m^2$ - measured variation). Table \ref{tab2} shows the covariance submatrix considering the output positions only. 
\newline

Computed covariance: 
{\small
\begin{eqnarray}
P&=&10^{-1} \times 
{\hspace{0.0cm}\left( \begin{array}{cccccccc}
             0.004&   -0.000&  -0.000&    0.000&   -0.000&   -0.000&   -0.030&   -0.018 \\
 	        -0.000&    0.004&    0.000&   -0.000&   -0.000&   -0.000&    0.003&   -0.017\\
 	        -0.000&    0.000&    0.004&    0.000&   -0.000&   -0.000&    0.004&    0.002\\
 	        0.000&   -0.000&    0.000&    0.004&   -0.000&   -0.000&    0.001&    0.000\\
  	        -0.000&   -0.000&   -0.000&   -0.000&    0.004&    0.000&    0.000&   -0.002\\
 	        -0.000&   -0.000&   -0.000&   -0.000&    0.000&    0.004&   -0.001&    0.004\\
 	        -0.030&    0.003&    0.004&    0.001&    0.000&   -0.001&   {\bf 0.235}&  {\bf  0.127}\\
 	        -0.018&   -0.017&    0.002&    0.000&   -0.002&    0.004&   {\bf 0.127}&  {\bf 0.165}
\end{array} \right)}\\
{\sigma_{x}}_c&=&0.153, \qquad {\sigma_{y}}_c=0.122\nonumber
\end{eqnarray}}

 Measured Covariance: \newline

{\small
\begin{eqnarray}
covar&=&10^{-1} \times 
\hspace{0.0cm}{\left( \begin{array}{cccccccc}
         0.004&   0.000&   0.000&  0.000&   0.000&  -0.000&  -0.028&  -0.020 \\
	     0.000&   0.004&   0.000&   0.001&   0.000&  -0.000&   0.000&  -0.020 \\
	     0.000&   0.000&    0.004&   -0.000&   0.001&  -0.001&  0.003&   0.001 \\
  	     0.000&   0.001&  -0.000&   0.004&   -0.000&   -0.000& -0.000&  -0.003 \\
  	     0.000&  0.000&   0.001&  -0.000&   0.005&  -0.000& -0.001& -0.006 \\
  	    -0.000&  -0.000&  -0.001&  -0.000&  -0.000&   0.005&  -0.000&   0.005 \\
 	    -0.028&  0.000&   0.003&  -0.000&  -0.001&  -0.000&  {\bf 0.213}&  {\bf 0.131} \\
 	    -0.020&  -0.020&   0.001&  -0.003&  -0.006&   0.005&  {\bf 0.131}&  {\bf 0.182} 
\end{array} \right)}\\
{\sigma_{x}}_c&=&0.145, \qquad  {\sigma_{y}}_c=0.134 \nonumber
\end{eqnarray}
}


  

\newpage



{\bf 2-inputs 2-outputs, direct and inverse solution}  \newline
The next example shows the computation of the direct and inverse case. In the direct case we obtain again  similar values
between computed and measured covariances and, with this,  standard deviations. The results of the inverse solution leads to 
similar values of the original inputs (orientations $x_1=\phi_R, x_2=\phi_H$), (see table \ref{tab2}). Simulations of fuzzy versions showed the same
similarities and can therefore be left out here.
\newline
\newline


  
  \begin{table}[thb]
 \caption{Covariances, standard deviations - computed and measured}
 \hspace{0.0cm}
 \footnotesize{
 \centering
\begin{tabular}{|c|c|c|c|c|}\hline \label{tab2}
Outputs& {Covariance, computed} & {Covariance, measured} & {${\sigma_{x}}_c$, comp/meas} &{${\sigma_{y}}_c$, comp/meas}  \cr \hline
    2 inputs       &     {\tiny
$P=\hspace{-0.0cm}{\left( \begin{array}{cc}
     0.0213 &   0.0114\\
     0.0114 &   0.0159
\end{array} \right)}$}              &        {\tiny
   $covar=\hspace{-0.0cm}{\left( \begin{array}{cc}
         0.0264 &   0.0146\\
          0.0146 &   0.0166
  \end{array} \right)}$}           &       $0.145/0.144$   &    $0.126/0.134$        \\ \hline \hline
    2 inputs, stat. lin.       &     
       -      &        -          &       $0.144/0.142$   &    $0.124/0.129$        \\ \hline \hline    
      6 inputs       &     {\tiny
$P=\hspace{-0.0cm}{\left( \begin{array}{cc}
     0.0235  &  0.0127\\
     0.0127  &  0.0165
\end{array} \right)}$}              &        {\tiny
   $covar=\hspace{-0.0cm}{\left( \begin{array}{cc}
         { 0.0213}&  { 0.0131}\\
         { 0.0131}&  { 0.0182}
  \end{array} \right)}$}           &       $0.135/0.145$   &    $0.122/0.134$        \\ \hline \hline
	Direct solution   & {\tiny
$P=\hspace{-0.0cm}{\left( \begin{array}{cc}
                0.0234  &  0.0133\\
                0.0133  &  0.0151
  \end{array} \right)}$} & {\tiny
   $covar=\hspace{-0.0cm}{\left( \begin{array}{cc}
         0.0264 &   0.0146\\
          0.0146 &   0.0166
  \end{array} \right)}$} &  $0.152/0.162$ &     $0.128/128$              \\ \hline \hline
	Inverse solution      & {\tiny
$P=10^{-3} \times \hspace{-0.0cm}{\left( \begin{array}{cc}
                           0.4666&    0.0522 \\
                           0.0522&    0.4744
\end{array} \right)}$} & {\tiny
$covar=10^{-3} \times \hspace{-0.0cm}{\left( \begin{array}{cc}
               0.4841&   -0.0190\\
               -0.0190&    0.396
\end{array} \right)}$} & $0.0215/0.0220$  &     $0.0217/0.0190$                 \\ \hline
\end{tabular}
}
\end{table}


{\bf 2-inputs 2-outputs, Moving robot/human}  \newline

The next example deals with robot and human in motion. Figure \ref{fig:sigma_1} shows positions and orientations of robot and human at selected 
time steps $t_1 ... t_5$ and the development of the corresponding intersections ${\bf x}_c$.  

\begin{figure}[ht]
      \centering
      %\hspace{-0.5cm}
       \includegraphics[width=0.55\textwidth]{sigma_2.eps}     
   \vspace{-0.0cm}
	\caption{Moving robot and human}
	\label{fig:sigma_1}
	 %\hspace{-1.8cm}
  \end{figure}


Figure \ref{fig:timeline_1} shows the corresponding time plot.
The time steps  $t_1 ... t_5$  are taken at $0.58s, ..., 4.58s$ with a  time distance of $1s$ which are 25 time steps of 0.04s each. 
Robot and human start at \newline
${\bf x}_R=(x_R,y_R)^T=(2,0)^T$m \newline 
${\bf x}_H=(x_H,y_H)^T=(4,10)^T$m  \newline
with the velocities \newline ${\dot x}_R(k)=-0.21 m/s$,\newline ${\dot y}_R(1)=+0.24 m/s$,\newline ${\dot x}_H(k)=-0.26 m/s$,\newline 
${\dot y}_H(1)= -0.24 m/s$\newline
$k$ - time step \newline
The $x$ components of the velocities ${\dot x}_R(k)$ and  ${\dot x}_H(k)$ stay constant during the whole simulation. \newline
The $y$ components change their  velocities with  constant factors
\begin{eqnarray}
{\dot y}_R(k+1)&=&K_R\cdot {\dot y}_R(k)\nonumber\\
{\dot y}_H(k+1)&=&K_H\cdot {\dot y}_H(k)\nonumber
\end{eqnarray}
where $K_R=1.2$ and $K_H=0.9$.
The orientation angles start with \newline
$\phi_R=1.78 rad$ \newline                                   
$\phi_H=3.69 rad$. \newline
and change their values every second according to the direction of motion.

From both plots one observes  an expected decrease of the output standard deviations for a mutual
decrease of their distances to the regarding intersection and a good match between computed and measured values  ${\bf x}_c$ (see table \ref{tab3}).
With the information about the distance of the robot and the standard deviation from and at the expected intersection, respectively,
it becomes possible to plan either an avoidance strategy  or a mutual cooperation between robot and human.

\begin{figure}[ht]
      \centering
      %\hspace{-0.5cm}
       \includegraphics[width=0.65\textwidth]{time_move.eps}     
   \vspace{-0.0cm}
	\caption{Time plot, robot and human}
	\label{fig:timeline_1}
	 %\hspace{-1.8cm}
  \end{figure}


\begin{table}[ht]
 \caption{Covariances, standard deviations - computed and measured, moving robot/human}
 \hspace{1.5cm}
 \footnotesize{
 \centering
\begin{tabular}{|c|c|c|c|c|}\hline \label{tab3}
Outputs& {Covariance, computed} & {Covariance, measured} & {${\sigma_{x}}_c$, comp/meas} &{${\sigma_{y}}_c$, comp/meas}  \cr \hline
    $t_1$      &     {\tiny
$P=\hspace{-0.0cm}{\left( \begin{array}{cc}
     0.0220  &  0.0017\\
     0.0017  &  0.0163
\end{array} \right)}$}              &        {\tiny
   $covar=\hspace{-0.0cm}{\left( \begin{array}{cc}
         0.0246 &   -0.0002\\
          -0.0002 &   0.0202
  \end{array} \right)}$}           &       $0.148/0.156$   &    $0.127/0.142$        \\ \hline \hline
     $t_2$      &     {\tiny
$P=\hspace{-0.0cm}{\left( \begin{array}{cc}
     0.0198  &  0.0023\\
     0.0023  &  0.0138
\end{array} \right)}$}              &        {\tiny
   $covar=\hspace{-0.0cm}{\left( \begin{array}{cc}
         0.0222 &   0.0018\\
         0.0018 &   0.0153
  \end{array} \right)}$}           &       $0.140/0.148$   &    $0.117/0.123$        \\ \hline \hline
	$t_3$   & {\tiny
$P=\hspace{-0.0cm}{\left( \begin{array}{cc}
                0.0168  &  0.0030\\
                0.0030  &  0.0107
  \end{array} \right)}$} & {\tiny
   $covar=\hspace{-0.0cm}{\left( \begin{array}{cc}
         0.0140 &   0.0040\\
          0.0040 &   0.0088
  \end{array} \right)}$} &  $0.129/0.118$ &     $0.103/0.093$              \\ \hline \hline
	$t_4$     & {\tiny
$P= \hspace{-0.0cm}{\left( \begin{array}{cc}
                0.0151  &  0.0029\\
                0.0029 &  0.0083
\end{array} \right)}$} & {\tiny
$covar=\hspace{-0.0cm}{\left( \begin{array}{cc}
          0.0127 &   0.0014\\
          0.0014 &   0.0073
\end{array} \right)}$} & $0.122/0.112$  &     $0.091/0.085$                 \\ \hline
	$t_5$     & {\tiny
$P=\hspace{-0.0cm}{\left( \begin{array}{cc}
                0.0125   &  0.0023\\
                0.0023 &  0.0061
\end{array} \right)}$} & {\tiny
$covar=\hspace{-0.0cm}{\left( \begin{array}{cc}
          0.0102 &   0.0030\\
          0.0030 &   0.0056
\end{array} \right)}$} & $0.112/0.101$  &     $0.078/0.074$                 \\ \hline
\end{tabular}
}
\end{table}

\newpage

\section{{Summary and conclusions}}\label{conclusion}
The content of this work is the prediction of encounter
situations of mobile robots and human agents in shared areas by analyzing planned/intended trajectories in 
the presence of uncertainties and system and observation noise. In this context the problem of intersections 
of trajectories with respect to system uncertainties and Gaussian noise of position and orientation
of the agents involved are discussed.  The problem is adressed by two methods: the statistical linearization of distributions 
and the sigma-point-Transformation of distribution parameters. Positions and orientations of robot and human are corrupted with 
Gaussian noise represented by the parameters mean and standard deviation.  The goal is to calculate mean and standard deviation/variation 
at the intersection via the nonlinear relation between positions/ orientations of robot and human on the one hand and the position of the 
intersection of their intended trajectories on the other hand. 

This analysis is realized by statistical linearization of the nonlinear relation between the statistics of robot and human (input) 
and the statistics of the intersection (output). The output results are mean and standard deviation of the intersection as functions 
of the input parameters mean and standard deviation of positions and orientations of robot and human. 
This work is first carried out for 2 inputs - 2 outputs relations (2 orientations of robot/human - 2 intersection coordinates) and then 
for 6 inputs - 2 outputs (2 orientations and 4 position coordinates of robot/human - 2 intersection coordinates). These 
cases were extended to their fuzzy versions by different Takagi-Sugeno (TS) fuzzy approximations and compared with the non-fuzzy case. 
Up to a certain resolution the approximation works as accurate as the original non-fuzzy version. For the 2-input - 2-output case an 
inverse solution is derived, except the 6-input - 2-output case because of the underdetermined nature of the differential 
input-output relation. 

The sigma-point transformation aims at transforming/propagating distribution parameters - the sigma-points - directly 
through nonlinearities. The transformed sigma-points are then converted  into distribution parameters  mean and covariance matrix. 
The sigma-point transformation is closely connected to the unscented Kalman filter which is used in the example of robot and human in motion.
%The advantage of sigma-point transformation is that it captures the 1st and 2nd order statistics of the random variable, whereas the statistical linearization   
%approximates the nonlinearity only by its 1st order.  
The specialty of the example is a computed virtual system  output ("observation") - the intersection of two intended trajectories - 
where the corresponding output uncertainty is a sum of the transformed position/orientation noise and the 
computational uncertainty from the fuzzy approximation. In total the comparison between the computed and measured covariances show very good match 
and the comparison with the statistical linearization shows good coincidences as well. Both the sigma-point transformation and the differential statistical 
linearization scales for more than 2 variables linearly. However the computing costs for the differential approach are still higher than that for the 
sigma-point approach. 
%A problem arises for the fuzzy approach in connection with the sigma-point transformation because of the high number of combinations 
%of input fuzzy sets which is  too far beyond any practical implementation.
In summary, a prediction of the accuracy of human-robot trajectories using the methods presented in this work increases the performance of 
human robot collaboration and human safety. In future work this method can be used for robot-human scenarios in factory workshops and for 
robots working in complicated environments like rescue operations in cooperation with human operators.

\section{Abbreviations}
Not applicable

\section{Competing interests}
The authors declare that they have no competing interests

\section{Consent for publication}
Not applicable

\section{Ethics approval and consent to participate}
Not applicable

\section{Funding}
This work has received funding from the European Union’s Horizon 2020 research and innovation programme 
under grant agreement No 101017274 (DARKO)

\section{Availability of data and materials}
Not applicable

\section{Authors' contributions}
R.P. developed the methods and implemented the simulation programs. A.L. conceived the project and gave advise and support. 

%\section{Acknowledgment}{This work has received funding from the European Union’s Horizon 2020 research and innovation programme under grant agreement No 101017274 (DARKO).}
%\newpage
%\bibliographystyle{apalike}
%{\small
\bibliography{icrabib}%}

\bibliographystyle{unsrt}
\end{document}


