\section{Transition Model}
\label{sec:trans}
	%
	\newcommand{\spoint}{l}
	\newcommand{\gHead}{\theta_\text{walk}}
	\newcommand{\gDist}{d_\text{walk}}
	%
	To sample only transitions that are actually feasible
	within the environment, we utilize a \SI{20}{\centimeter}-gridded graph
	$G = (V,E)$ with vertices $v_i \in V$ and undirected edges $e_{i,j} \in E$
	%\commentByFrank{notation geaendert. so ok?}
	derived from the buildings floorplan as described in section \ref{sec:relatedWork}.
	However, we add improved $z$-transitions by also modelling realistic
	stairwells using nodes and edges, depicted in fig. \ref{fig:gridStairs}.
	%
	\begin{figure}
		\centering
		\input{gfx/grid/grid}
		\caption{
			Besides the nodes and edges defined by the distinct floors, we add realistic stairs to interconnect them.
			Stairs are given by three points $\vec{\spoint}_1, \vec{\spoint}_2, \vec{\spoint}_3$, defining the
			starting-edge $[\vec{\spoint}_1 \vec{\spoint}_2]$ and the direction $[\vec{\spoint}_2 \vec{\spoint}_3]$.
			}
		\label{fig:gridStairs}
	\end{figure}
	%	
	Stairs are defined using three points $\vec{\spoint}_1, \vec{\spoint}_2, \vec{\spoint}_3 \in \R^3$ whereby the segment
	$[ \vec{\spoint}_1 \vec{\spoint}_2 ]$ describes the starting-edge, and $[ \vec{\spoint}_2 \vec{\spoint}_3 ]$ the stair's
	direction (see fig. \ref{fig:gridStairs}). The grid-vertices corresponding to the starting-edge are determined using an
	intersection of the segment $[ \vec{\spoint}_1 \vec{\spoint}_2 ]$ with the \SI{20}{\centimeter} bounding-box around each
	node's centre $\fPos{v} = (x,y,z)^T$. 
	
	To reduce the system's memory footprint, we search for the largest connected region within the graph and
	remove all nodes and edges that are not connected to this region.
	
	New states $\mStateVec_{t}$ may now be sampled by starting at the vertex for
	position $\fPos{\mStateVec_{t-1}} = (x,y,z)^T$
	%\commentByFrank{eingefuehrt}
	and walking along adjacent nodes into a given walking-direction $\gHead$ until a distance $\gDist$ is
	reached \cite{Ebner-15}.
	Both, heading and distance, are supplied by the current sensor readings $\mObsVec_{t}$
	and thus reflect the pedestrian's real heading and walking speed including uncertainty.
	Working with relative sensor readings, the state's heading is updated during each transition:
	%
	\begin{align}
		\gHead	&= \mState_{t}^{\mStateHeading} = \mState_{t-1}^{\mStateHeading} +	\mObs_t^{\mObsHeading}						+ \mathcal{N}(0, \sigma_{\gHead}^2) \\
		\gDist	&= 									\mObs_t^{\mObsSteps} \cdot \mStepSize	+ \mathcal{N}(0, \sigma_{\gDist}^2) 
		.
	\end{align}
	%\commentByFrank{fixed. war das falsche makro in (2) und dem satz darunter. das delta musste weg. der state hat ein absolutes heading. step-size als variable}
	%
	During the random walk, each edge has its own probability $p(\mEdgeAB)$
	which e.g. depends on the edge's direction $\angle \mEdgeAB$ and the
	pedestrian's current heading $\gHead$. 
	Furthermore, section \ref{sec:nav} uses $p(\mEdgeAB)$ to incorporate prior path knowledge to
	favour edges leading towards the pedestrian's desired target $\mVertexDest$.
	%\commentByFrank{fixed}
	
	For each single movement on the graph, we calculate $p(\mEdgeAB)$ for all edges
	connected to a vertex $\mVertexA$, and, hereafter, randomly draw the to-be-walked edge
	depending on those probabilities. This step is repeated until the sum 
	of the length of all used edges exceeds $d$. The latter depends on the number of
	detected steps $\mObs_t^{\mObsSteps}$ and the pedestrian's step-size $\mStepSize$.
	%\commentByFrank{step-size als variable}
	
	To quantify the improvement prior knowledge is able to provide,
	we define a simple reference for $p(\mEdgeAB)$ that assigns a probability to each edge
	just based on its deviation from the currently estimated heading $\gHead$:
	%
	\begin{equation}
		p(\mEdgeAB) = p(\mEdgeAB \mid \gHead) = \mathcal{N} (\angle \mEdgeAB \mid \gHead, \sigma_\text{dev}^2).
		\label{eq:transSimple}
	\end{equation}
	%
	%	
	%	
	%	
	%	
\section{Navigational Knowledge}
	\label{sec:nav}
	%
	Considering navigation, a pedestrian wants to reach a well-known destination which represents additional
	prior knowledge. Most probably, the user will stick to the path presented by
	a navigation system. However, some deviations like chatting to someone or taking another route
	cannot be strictly ruled out. We will therefore describe a system that is able to deal with such
	variations as well as present an algorithm to calculate realistic routes based on the aforementioned grid.
	
	\subsection{Wall Avoidance}
	\label{sec:wallAvoidance}
		%
		%As discussed in section \ref{sec:relatedWork}, simply applying a shortest-path algorithm such as Dijkstra or
		%A* using the previously created graph would obviously lead to non-realistic paths sticking to walls and
		%walking many diagonals. Pedestrians however, will probably keep a small gap between themselves and
		%nearby walls. 
		%To calculate paths that resemble this	behaviour, an importance-factor is derived for
		%each vertex. Those will be used to modify the weight $\fDistance{v}{v'}$ between two vertices
		%$v,v'$, examined by the shortest-path algorithm.
		%
		Shortest-path algorithms such as Dijkstra use a scalar weight $\fDistance{v_1}{v_2}$ between two vertices
		to determine the path with the lowest overall weight.
		As discussed in section \ref{sec:relatedWork}, simply using the distance
		$\fDistance{\mVertexA}{\mVertexB}  = \| \mVertexA - \mVertexB \|$ as weighting function, would obviously lead to non-realistic paths 
		sticking to walls and walking many diagonals. Pedestrians however, will probably keep a small gap between 
		themselves and nearby walls. To calculate paths that resemble this	behaviour, an importance-factor is derived for
		each vertex. Those will be used to scale the distance between two nodes, just like navigation systems use
		the speed-limit as scaling-factor.
		%\commentByFrank{so besser? der ganze absatz.}
		To downvote vertices near walls, we need to determine the distance of each vertex from its nearest wall.
		We therefore derive an inverted version $G' = (V', E')$ of the graph $G$, just describing walls and 
		obstacles. A nearest-neighbour search $\fNN{\mVertexA}{V'}$ within $V'$ provides the vertex
		nearest to $\mVertexA$.
		%\begin{equation}
		%	v' = \fNN{v}{V'} \enskip .
		%\end{equation}
		To get a smooth gradient around walls, the avoidance-factor 
		is derived using a normal distribution with a deviation of \SI{0.5}{\meter}:
		%
		\begin{equation}
			\fWA{\mVertexA} = \mathcal{N}( \| \mVertexA - \fNN{\mVertexA}{V'} \| \mid 0.0, 0.5^2)
			\enskip .
			\label{eq:wallAvoidance}
		\end{equation}
		%\commentByFrank{fixed. WA war WallAvoidance. hatte statt ll immer $\|$ gelesen und deshalb nicht verstanden}
		%
		%The parameters of the normal distribution and the scaling-factors were chosen empirically.
		%While this approach provides good results for most areas, doors are downvoted by
		%\refeq{eq:wallAvoidance}, as they have only vertices that are close to walls.
		%Door detection and upvoting thus is the next conducted step. 
		While rendering wall-regions less likely, \refeq{eq:wallAvoidance} 
		will obviously have the same effect on doors as they are just a small gap between 
		consecutive walls. Therefore, a door-detection is necessary, to upvote them again.
		%
		%
		%
	\subsection{Door Detection}
	\label{sec:doorDetection}
		%	
		To automatically detect doors within the floorplan, we utilize the fact that doors are usually
		anchored between two straight walls and have a normed width. Examining the region directly
		around it, the door and its surrounding walls thus describe a flat ellipse with the door as its centre.
		%It is thus possible to detect doors within the floorplan using a PCA.
		
		To decide whether a vertex $v$ belongs to a door, we use \mbox{$k$-NN} to fetch its $k$ nearest neighbours
		$\{v'_1, v'_2, \dots, v'_k\} \in V'$ within the inverted grid $G'$, denoting nearby walls.
		%Hereafter we determine the centroid $\vec{c} \in \R^3$
		%and 2D covariance $(x,y)$ for those vertices.
		Hereafter we determine their centroid $\vec{c}$ and covariance $\mat{\Sigma}$:
		\begin{equation}
			\vec{c} = \frac{1}{k}\sum_{i=1}^{k} \fPos{v_i}
			,\smallskip
			\mat{\Sigma} = \frac{1}{k} \sum_{i=1}^{k} \varphi \cdot \varphi^T
			,\smallskip
			\varphi = \fPos{v_i}-\vec{c} \enskip.
		\end{equation}
		%
		For $\mat{\Sigma}$, the two largest eigenvalues $\lambda_1, \lambda_2$ with $\lambda_1 > \lambda_2$
		%\commentByFrank{fixed}
		are calculated. If their ratio $^{\lambda_1}/_{\lambda_2}$ is above a certain
		threshold, the neighbourhood describes a flat ellipse and thus either a door or a straight wall. 
		%
		To filter the latter, we enforce the euclidean distance \mbox{$\| \fPos{v} - \vec{c} \|$} between
		the centroid and the vertex to be very small. Hereafter, only vertices located directly within a
		door itself remain. 
		%
		\begin{figure}
			\includegraphics[width=\columnwidth]{door_pca}
			\caption{Detect doors within the floorplan using $k$-NN to get the centroid $\vec{c}$ and covariance $\mat{\Sigma}$
			of the wall-vertices (black) near $\mVertexA$. While the left version is fine, the $\mVertexA$ in the middle is too far 
			from $\vec{c}$ and the right one has an invalid eigenvalue-ratio.}
			\label{fig:doorPCA}
		\end{figure}
		%
		Fig. \ref{fig:doorPCA} depicts all three cases where
		(left) the vertex is part of a door,
		(middle) the distance between node and centroid is above the threshold and
		(right) the ratio between $\lambda_1$ and $\lambda_2$ is below the threshold.

		For smooth gradients around doors, we again use a distribution based on
		the distance of a vertex $\mVertexA$ from its nearest door and a deviation
		of \SI{1.0}{\meter}:
		%
		%%\commentByFrank{distanzrechnung: formel ok?}
		\begin{equation}
			\fDD{\mVertexA} = \mathcal{N}( \| \fPos{\mVertexA} - \vec{c} \| \mid 0.0, 1.0^2 )
			\label{eq:doorDetection}
		\end{equation}
		%
		The final importance combines \refeq{eq:wallAvoidance}
		and \refeq{eq:doorDetection}:
		%
		\begin{equation}
			\fImp{\mVertexA} = 1.0 - \fWA{\mVertexA} + \fDD{\mVertexA} \enspace .
			\label{eq:finalImp}
		\end{equation}	
		%
		While most nodes receive a neutral factor of $1.0$, wall-regions get lower and
		door-regions higher values, depicted in fig. \ref{fig:importance}.
		%
		\begin{figure}
			%\includegraphics[width=\columnwidth]{floorplan_importance}
			\input{gfx/floorplan_importance}
			\caption{The calculated importance-factor \refeq{eq:finalImp} for each vertex.
			While the dark wall-elements denote a small importance, the yellow areas around doors and narrow
			passages depict a high importance.}
			\label{fig:importance}
		\end{figure}
		%
	\subsection{Path Estimation}
	\label{sec:pathEstimation}
		%
		To route the pedestrian towards his desired target, a modified version
		of Dijkstra's algorithm is used. Instead of calculating the shortest path from the start to the end,
		the direction is inverted and the calculated terminates as soon as every single node was evaluated.
		Hereafter, every node in the grid knows the distance and shortest path to the pedestrian's target.
		
		To get realistic path suggestions, we use the importance-factors to adjust the edge-weight
		$\delta(\mEdgeAB)$ for the Dijkstra:
		%
		\begin{equation}
			\begin{split}
				\delta(\mEdgeAB) =
				\delta(\mVertexA, \mVertexB) = 
					\frac
						{ \| \mVertexA - \mVertexB \|  }
						{ \fImp{\mVertexB} }
					\enskip.
			\end{split}
			\label{eq:edgeWeight}
		\end{equation}
		%
		Eq. \eqref{eq:edgeWeight} artificially increases the euclidean distance between $\mVertexA, \mVertexB$ when
		$\mVertexB$ approaches a wall and decreases it when encountering a door.
		%
		%whereby $\text{stretch}(\cdots)$ is a scaling function (linear or non-linear) used to adjust
		%the impact of the previously calculated importance-factors.
		%
		Fig. \ref{fig:multiHeatMap} depicts the difference between the shortest path calculated without (dashed) and
		with importance-factors (solid), where the latter is clearly more realistic.
		%
		%\begin{figure}
		%	\includegraphics[angle=-90, width=\columnwidth, trim=20 19 17 9, clip]{floorplan_paths}
		%	\caption{Comparision of shortest-path calculation without (dotted) and with (solid) importance-factors
		%	use for edge-weight-adjustment.}
		%	\label{fig:shortestPath}
		%\end{figure}
		%
		%
	\subsection{Guidance}
		%
		Based on the previous considerations, we propose two approaches to utilize prior
		knowledge within the transition.
		
		\subsubsection{Shortest Path}
			%
			\newcommand{\pathCentroid}{{\vec{\overline{c}}_{t-1}}}
			\newcommand{\pathDev}{\sigma_{t-1}}
			\newcommand{\pathRef}{v_\text{ref}}
			%
			Before every transition, the centre-position $\pathCentroid = \fPos{\mStateVec_{t-1}^*}$ of the current sample-set, where 
			$\mStateVec_{t-1}^* = \argmax_{\mStateVec_{t-1}} \enspace p(\mStateVec_{t-1} \mid \mObsVec_{t-1})$
			represents the most proper state of the posterior distribution at time $t-1$, is calculated.
			%
			%
			%%\commentByFrank{avg-state vom sample-set. frank d. meinte ja hier muessen wir aufpassen. bin noch unschluessig wie.}
			%\commentByToni{Das ist gar nicht so einfach... wir haben nie ein Sample Set eingefuehrt. Nicht mal einen Sample. Wir haben immer nur diesen State... Man könnte natuerlich einfach sagen das $\Upsilon_t$ an set of random samples representing the posterior distribution ist oder einfach nur ein set von partikeln. habs mal eingefuegt wie ich denke}
			%
			This centre serves as the starting point for the shortest-path calculation.
			As it is not necessarily part of the grid, its nearest-grid-neighbour is determined and used instead.
			This vertex is located somewhere within the sample-set and already knows the way to the pedestrian's
			destination $\mVertexDest$.
			%
			As new states $\mStateVec_{t}$ should approach the pedestrian's destination
			we use a reference $\pathRef$ all states try to reach. This references must
			be both part of the shortest path and located somewhere outside of the sample-set.
			%			
			We thus calculate the standard deviation of the distance of all sample-positions
			$\fPos{\mStateVec_{t-1}}$ from aforementioned centre $\pathCentroid$.
			%\commentByFrank{so klarer? platz fuer groese Eq. fehlt und Notation zum ansprechen jedes einzelnen Particles vermeide ich lieber...}
			%
			%\begin{equation}
			%	d_\text{cen} = \| pos(q_{t-1}) - \pathCentroid \|
			%	\sigma_\text{cen} = stdDev(distance)
			%\end{equation}
			
			After advancing the starting-vertex by three times this deviation
			we get the new point $\pathRef$ that is: part of the shortest path, outside of the sample-set
			and closer (but not too close) to the desired destination.
			%
			Hereafter, the simple transition \refeq{eq:transSimple} is combined with a second probability,
			downvoting all grid-steps that depart from $\pathRef$.
			To still allow leaving the shortest path, the intensity of the downvoting is controlled via $\mUsePath$,
			with $0 < \mUsePath < 1$.
			Finally, \refeq{eq:transShortestPath} provides a metric tending towards the reference while
			still allowing the pedestrian to leave the shortest path:
			%
			\begin{equation}
				\begin{split}
					p(\mEdgeAB) &= 
					p(\mVertexB \mid \mVertexA) = 
						\mathcal{N} (\angle \mEdgeAB \mid \gHead, \sigma_\text{dev}^2) \cdot	\alpha \\
						\alpha &= 
						\begin{cases}
							\mUsePath		& \| \mVertexB - \pathRef \| < \| \mVertexA - \pathRef \|		\\
							(1-\mUsePath)	& \text{else}
						\end{cases}
				\end{split}
				\enskip .
				\label{eq:transShortestPath}
			\end{equation}
			%\commentByFrank{$\mUsePath$ als variable}
			%
			%
			%
		\subsubsection{Multipath}
			%
			The shortest-path algorithm mentioned in \ref{sec:pathEstimation} already calculated the distance
			$\fLength{\mVertexA}{\mVertexDest}$ % = \sum_{i=s}^{e-1} \| v_{i} - v_{i+1} \| $
			for the path from $\mVertexA$ to the pedestrian's destination $\mVertexDest$.
			We thus apply the same assumption as \refeq{eq:transShortestPath} and downvote edges
			not decreasing the distance to the destination:
			%
			\begin{equation}
				\begin{split}
					p(\mEdgeAB) &= 
					p(\mVertexB \mid \mVertexA) =
						\mathcal{N} (\angle \mEdgeAB \mid \gHead, \sigma_\text{dev}^2) \cdot	\alpha \\
						\alpha &= 
						\begin{cases}
							\mUsePath		& \fLength{\mVertexB}{\mVertexDest} < \fLength{\mVertexA}{\mVertexDest}		\\
							(1-\mUsePath)	& \text{else}
						\end{cases}
				\end{split}
				\enskip .
				\label{eq:transMultiPath}
			\end{equation}
			%
			Fig. \ref{fig:multiHeatMap} shows a heat-map of how often vertices were visited after several
			\SI{125}{\meter} walks. The colours from cold to hot indicate that both possible paths
			are covered and slight deviations from the shortest version are possible.
			%
			\begin{figure}
				%\includegraphics[width=\columnwidth, trim=4 8 4 4]{floorplan_dijkstra_heatmap}
				\input{gfx/floorplan_dijkstra_heatmap}
				\caption{Heat-Map showing the number of visits per vertex after $30.000$ walks using \refeq{eq:transMultiPath}.
				Both possible paths are covered and slight deviations are possible.
				Additionally shows the shortest-path calculation without (dashed) and with (solid) importance-factors
				used for edge-weight-adjustment.}
				%\commentByFrank{so besser?}
				\label{fig:multiHeatMap}
			\end{figure}
%
			It should be noted that the normal distribution in \eqref{eq:transShortestPath} and \eqref{eq:transMultiPath} does not integrate to \SI{1}{} due to circularity of angular data. 
This normally requires wrapped distributions like the von Mises distribution. 
However, in our case, the normal distribution can be assumed as sufficient for small enough $\sigma^2$.

