% Notation:
% Menge aller APs und deren Position
% Gemessene Distanz vs true Distanz vs Schätzung
% Selbe für Position

\section{Position Estimation}
\label{sec:position}

%Bei Indoor Lokalisierung geht es darum eine Position zu ermitteln. Hierfür nutzen wir unterschiedliche Verfahren. namley... usw. 

After measuring several distances to different anchor points one can calculate his current position.
%TODO Alles mit 2D, weil halt

\subsection{Multilateration}
%Ganz kurz erläutern was Multilateration eigentlich ist. in 2D min 3 aps und in 3D min. 4D. Aber grundsätzlich gilt: viel hilft viel.
%Es ist uns klar, dass Trilat nichts taugt aber ist halt der einfachste Schritt
%Typische Nachteile: Wenn Schnittpunkt nicht analytisch exakt bestimmt werden können
%FTM Nachteil: Häufig fallen die Messungen aus? Was tun? Alte Werte statisch halten? Keine est berechnen?
%Conceptually, multilateration determines the position by analytically intersecting at least $3$ circles for a 2-dimensional position, or at least $4$ spheres in case of a 3-dimensional coordinate system.
In two dimensions each distance measurement $d_i$ constrains the position estimate $\hat{\mPosVec}$ to a circle with radius equals $d_i$, where the center of the circle is the known position $\mPosVec_i = (x,y)^T$ of AP $i$.
Formally the distance is the euclidean distance between the known position and the estimate
\begin{equation}
d_i = \| \mPosVec_i - \hat{\mPosVec} \|   \text{.}
\end{equation}
Three ideal distances form a system of linear equations which can be uniquely solved to obtain the position.
Given more than three distances no solution can be found which satisfies all the constraints as the linear system is overdetermined.
Additionally, in the presence of noise and inaccurate measurements an exact analytical solution is not possible.
In this case an approximative solution $\mPosVec^*$ can be found by using a least squares approach which minimizes the quadratic error between the measured distance and the actual distance at a given point
\begin{equation}
  \mPosVec^* = \argmin_{\hat{\mPosVec}} \sum_{i}^{} \left(  \| \mPosVec_i - \hat{\mPosVec} \| - d_i \right)^2  \text{.}
\label{eq:leastSquare}
\end{equation}

This forms a classical non linear least squared optimization problem which can be solved with a numerical optimization method like \docGaussNetwon or \docLevenbergMarq.
The position $\hat{\mPosVec}$ which minimizes the error provides an approximative estimate $\mPosVec^*$.
In contrast to an analytical solution this results not in an ideal point intersection but an area where the error of \eqref{eq:leastSquare} is acceptable small.

A good starting value for the optimization can be obtained by computing the pseudo inverse of the matrix. % TODO formeln
Note that the linearization of \ref{?} is obtained by subtracting one equation from the remaining equations which introduces a potentially significant offset in the estimate.
This offset depends on the particular choice of equation to subtract.
%TODO local minima vs global?
%TODO was noch?

Another factor which influences the localization accuracy is the geometry of the setup where the measurements were taken.
The accuracy of multilateration estimate depends on the position of the APs and the position of the smartphone relatively to each other.
Therefore, it is important to consider the actual AP locations and the walkable area where the localization system is used to improve localization accuracy.
However, the best geometrical setup for localization is not necessarily the best setup for signal coverage.

Best localization results are archived when the distance circles of the APs intersect in a near orthogonal angle.
Localization performance degrades with wider intersection angles.
The optimal setup in a simple scenario given four APs is to place them at the corners of a square.
This yields best localization performance inside of the square.

Usually non-optimal AP locations need to be chosen due to environmental constrains like building structure or signal coverage.
These geometrical considerations can be founded on geometric dilution of precision (GDOP), which is a rating of the expected localization performance based on the sender-receiver geometry.
Lower GDOP values indicate better estimation precision due to smaller intersection angles.
GDOP allows to plan the deployment of APs for location estimation before actually installing the APs.

Given a setup with $i$ APs and their known positions $\mPosVec_i = (x,y)^T$, the GDOP value is calculated by forming a matrix 
%In order to compute the GDOP value in a setup with known AP positions $\mPosVec_i = (x,y)^T$ their correspondent unit vectors from a given position $(x,y)$ form the matrix
\begin{equation}
A = 
 \begin{pmatrix}
  \frac{(x_1 - x)}{R_1} & \frac{(y_1 - y)}{R_1}  \\[0.3em]
  \frac{(x_2 - x)}{R_2} & \frac{(y_2 - y)}{R_2}  \\[0.3em]
  \vdots                & \vdots                 \\[0.3em]
  \frac{(x_i - x)}{R_i} & \frac{(y_i - y)}{R_i}  \\
 \end{pmatrix}
\end{equation}
where $R_i=\sqrt{(x_i-x)^2+(y_i-y)^2}$. Each row of $A$ is a unit vector from a given position $(x,y)^T$ to the AP's position $\mPosVec_i$. Given the matrix $Q = (A^TA)^{-1}$ the GDOP value is the square root of the sum of the diagonal of $Q$:
\begin{equation}
\text{GDOP} = \sqrt{\text{trace}(Q)}
\end{equation}

While GDOP allows to rate the given scenario regarding position estimation precision it only takes the relative geometry of the senders and receiver into account.
Other factors like signal attenuation or absorption are not considered.
However, GDOP still gives a good first impression of the theoretical suitability of a geometrically arrangement of senders for position estimation and provides an indicator for further optimizations of it.
%DOP ganz nett aber signalstärken spielt auch eine Rolle

\subsection{Probabilistic}
\label{sec:prob}
%Dichte aus Messungen erzeugen.
%Distanzern werden mit Normalverteilung gewichtet
%Vorteil: Nicht ideale Schnittpunkte sind kein Problem, weil die Dichte sowas abbilden kann
%FTM Vorteil: Fehlen von Messungen kann probabilistisch erfasst werden indem Streuung der NV größer wird / Oder es enstehen einfach mehrere Hypthesen über die Position
Probabilistic frameworks allow to natively incorporate faulty or inaccurate measurements into the position model.
While multilateration produces a single point as position estimate the probabilistic framework allows to describe the estimated position as probability density function.
This approach has the advantage to model inaccurate measurments as variances and quantify them.

Assuming that FTM measurements follow a Gaussian distribution, the measured distance $d_i$ between the sender and AP $i$ is described by $\mathcal{N}(d^*_i, \sigma^2_i)$ where $d^*_i=\|\mPosVec_i - \mPosVec \|$ is the actual distance between the AP position $\mPosVec_i$ and a given position $\mPosVec$. $\sigma^2_i$ is the variance of the measurement.
Visualized on a two dimensional plane this produces a ring shaped density function where the cross section of the ring is the well known Gaussian curve.

Multiple distance measurements can be combined by a joint density function, if the individual distance measurements to the APs are statistical independent.
The probability to observe a distance measurement $d$ at position $\mPosVec$ is given as
\begin{equation}
  p(d | \mPosVec ) = \prod_i \frac{1}{\sqrt{2\pi\sigma^2_i}} \exp \left( - \frac{(d - d^*_i)^2}{2\sigma^2_i} \right)
\end{equation}

\subsection{Particle Filter}

%Particle Filter Introduction
As seen above, one can use a probability distribution like a Gaussian to describe the pedestrian’s most proper position and therefore the uncertainty of the system. 
However, drawing from a probability distribution and finding an analytical solution for densities is in more complex scenarios a difficult task. 
Especially when considering time sequential, non-linear and non-Gaussian models. 
Due to the strong variation in human movement and the complexity of different sensor modalities, positioning indoors is often seen as such.

Bayesian filters can solve this by updating the position estimation recursively with incoming measurement $\vec{o}_{1:t}$ until the current time $t$ using a set of probabilistic models describing the movement and likelihood.
A broad class to obtain numerical results for this approach are Monte Carlo (MC) methods.
By applying the time sequential hidden Markov process of Bayes filtering, one of the most important MC techniques results: particle filtering. 

In context of indoor positioning, a particle filter computes the posterior distribution $p(\vec{q}_t \mid \vec{o}_{1:t})$ describing the pedestrian’s possible whereabouts by using a sample set of $N$ independent random variables, $X^i_t \approx p(\vec{q}_t \mid \vec{o}_{1:t})$ where $i = 1, ..., N$ for approximation.
Due to importance sampling, a weight $W^i_t$ is assigned to each sample $\vec{X}^i_{t}$.
Thus, $\{W^i_{1:t}, \vec{X}^i_{1:t} \}_{i=1}^N$ is a weighted set of samples, also called particles.
Therefore a particle is a representation of one possible system state $\vec{q}$.
Compared to the method described in \ref{sec:prob}, this not only allows for a better description of the problem space, but also incorporates a priori knowledge by using a time recursive component. 

%Particle Filter Standard Filtering Equation
The filtering equation to calculate the posterior is given by the recursion

\begin{equation}
	\arraycolsep=1.2pt
	\begin{array}{ll}
		&p(\mStateVec_{t} \mid \mObsVec_{1:t}) \propto\\ 
			&\underbrace{p(\mObsVec_{t} \mid \mStateVec_{t})}_{\text{evaluation}}
			\int \underbrace{p(\mStateVec_{t} \mid \mStateVec_{t-1})}_{\text{transition}}
			\underbrace{p(\mStateVec_{t-1} \mid \mObsVec_{1:t-1})d\vec{q}_{t-1}}_{\text{recursion}}
	\end{array}
	\enspace ,
	\label{equ:bayesInt}
\end{equation}

\noindent where the hidden state $\mStateVec$ describes a position $\mPosVec = (x,y)^T$ given by

\begin{equation}
	\mStateVec = (\mPosVec),\enskip
	\mStateHeading \in \R \enspace.
\end{equation}

The corresponding observation vector covers all relevant sensor measurements.
In case of this work, we are only interested in the distance measure $d$ provided by FTM or in case of RSSI provided as described in \ref{subsec:rssi}. 
Thus $\mObsVec$ can be easily given by  
 
\begin{equation}
    \mObsVec = (\vec{d}_1, \dots ,\vec{d}_i) 
\end{equation}

containing a set of distance measurements $\vec{d}$ of all access-points $i$ currently visible to the phone. 
As realization of \eqref{equ:bayesInt} we use the well-known CONDENSATION particle filter \cite{Isard98:CCD}.
Here, new particles are propagated according to the transition, which models the dynamics of the system. 
Those particles are then weighted by the evaluation given the sensor measurements. 
A resampling step is deployed to prevent that only a small number of particles have a significant weight, i.e. handle the phenomenon of weight degeneracy. 
These steps are performed based on a predefined discrete update interval, e.g. every \SI{500}{\milli\second} or with every new measurement. 

%Transition
As described above, we deliberately do without a full stack IPS in order to clearly demonstrate the advantages and
disadvantages of FTM compared to RSSI. 
We have decided to utilize a very simple transition model, where the movement of particles from time step $t-1$ to $t$ is provided by drawing from a set of Gaussian distributions. 
New potential whereabouts $p(\mStateVec_{t} \mid \mStateVec_{t-1})$ are then

\begin{equation}
		\begin{aligned}
x_t &=&	\overbrace{x_{t-1}}^{\text{old pos.}}& & &+& \overbrace{\delta}^{\text{walked}} & & &\cdot& \overbrace{\cos(\mStateHeading)}^{\text{heading}}& &, \enskip \mStateHeading  &\sim \mathcal{U}(0, 2\pi)\\
y_t &=&	y_{t-1}\phantom{.}&	& &+& \delta \phantom{....}& & &\cdot& \sin(\mStateHeading)& &, \enskip \delta &\sim \mathcal{N}(\mu_{\text{walk}}, \sigma_{\text{walk}})	\\	
		\end{aligned}
		\label{eq:navMeshTrans}
\end{equation}

Note that the uniform distribution in \eqref{eq:navMeshTrans} is limited in the interval $[0; 2\pi)$ to avoid oversampling at the pole.
Further, the parameters for the Gaussian depend on the chosen update interval, as they describe the to-be-walked distance of the pedestrian.
Put simply, \eqref{eq:navMeshTrans} causes the particles to spread out in a (uniquely distributed) circle within a certain (Gaussian distributed) distance.

%Evaluation


