# AOA-Based Three-Dimensional Positioning and Tracking Using the Factor Graph Technique
## 2 System Model  
In this paper, a discrete and non-linear state space model (SSM) is considered. A single target is to be detected, and its state at time index \(k\) can be denoted by the 3D location \(\mathbf{s}_{k}=[x_{k},y_{k},z_{k}]^{T}\), with \(k=\{1,2,...,K\}\). Moreover, there are \(N\) distributed sensors deployed in this system, and the fusion center is assumed to know their position \((X_{n},Y_{n},Z_{n})\), with \(n=\{1,2,...,N\}\). The azimuth and elevation angles, denoted by \(\phi\) and \(\theta\), respectively, are shown in Figure 1. For the sake of performing location detection, the relative distance \(\Delta d_{n}\) from the \(n\)-th sensor to the target is computed by:  
\[\Delta d_{n}= \begin{bmatrix}\Delta x_{n}\\ \Delta y_{n}\\ \Delta z_{n}\end{bmatrix} = \begin{bmatrix}X_{n}\\ Y_{n}\\ Z_{n}\end{bmatrix} - \begin{bmatrix}x\\ y\\ z\end{bmatrix} . \tag{1}\]  
The measured AOA at each sensor is comprised of both azimuth \(\phi\) and elevation \(\theta\) angles. The definition of \(\phi\) and \(\theta\) in the spherical coordinate exactly follows the standard way shown in Figure 1. The relative distances calculated above can be connected by the true AOAs as follows, where the subscripts are omitted for simplicity. Figure 1: 3D coordinate with azimuth (\(\phi\)) and elevation (\(\theta\)) angles. \[\Delta x =\Delta z\cdot\tan\left(\theta\right)\cdot\sin\left(\phi\right) \tag{2}\] \[=\Delta y\cdot\tan\left(\phi\right),\] (3) \[\Delta y =\Delta z\cdot\tan\left(\theta\right)\cdot\cos\left(\phi\right)\] (4) \[=\Delta x\cdot\cot\left(\phi\right),\] (5) \[\Delta z =\Delta y\cdot\cot\left(\theta\right)\cdot\csc\left(\phi\right)\] (6) \[=\Delta x\cdot\cot\left(\theta\right)\cdot\sec\left(\phi\right). \tag{7}\]  
As shown in Equations (3)-(6), the calculations of relative distances are self-contained. In order to obtain a converged result, iterative detection is performed based on an FG framework, which is known as the message passing algorithm. Note that there are always two ways to calculate the relative distance, which indicates that there will be two factor nodes included in the proposed FG structure, connecting to the variable node of the relative distance. A detailed discussion of the FG structure will be given in the next section. Moreover, it is assumed that for every time index, sensors are able to measure \(L\) samples of AOA from the target, and the variable of measured AOA can be described as a Gaussian model as:  
\[\phi_{l} =\phi+n_{\phi,l}, \tag{8}\] \[\theta_{l} =\theta+n_{\theta,l}, \tag{9}\]  
where \(l=\left\{1,2,...,L\right\}\), and the noise components \(n_{\phi}\) and \(n_{\theta}\) follow \(\mathcal{N}\left(0,\sigma_{\phi}^{2}\right)\) and \(\mathcal{N}\left(0,\sigma_{\theta}^{2}\right)\), respectively. Even though the quantization errors may break the Gaussianity of the measured AOAs, they are assumed to be very small in this paper, such that the input of the FG detector can be still regarded as Gaussian distributed variables. Moreover, it is assumed that the measured AOAs can be sent from sensors to the fusion center with error-free transmissions. Due to the Gaussian assumption of the measured AOAs at each sensor, they can be described by only parameters of the Gaussian probability density function (PDF), instead of \(L\) snapshots. By omitting the indexes \(n\) and \(k\) for simplicity, \(m_{\hat{\phi}}\) and \(\sigma_{\hat{\phi}}^{2}\) are used to represent the mean and the variance of \(\hat{\phi}\), respectively. Similarly, \(m_{\hat{\theta}}\) and \(\sigma_{\hat{\theta}}^{2}\) are used to represent the mean and the variance of \(\hat{\theta}\), respectively. Clearly, when \(L\  
where the observation noise vector is represented by \(\mathbf{e}_{k}\). Since \(\mathbf{s}_{k}\) is fully determined by the true AOAs \(\phi\) and \(\theta\), \(g\left(\mathbf{s}_{k}\right)\) can be also expressed by \(\tilde{g}\left(\boldsymbol{\phi}_{k},\theta_{k}\right)\).