# AOA-Based Three-Dimensional Positioning and Tracking Using the Factor Graph Technique
## 3 Location Detector  
This section introduces the FG structure in detail for AOA-based 3D location detection, as shown in Figure 2. It is also known as the observer in EFK. Note that in the following discussion, the indexes of the sensor are ignored from the variables for an easier expression. First of all, the variable nodes (\(A_{\phi}\), \(A_{\theta}\)) are used, which contain the means and variances of the measured AOAs at each sensor. The outputs of (\(A_{\phi}\), \(A_{\theta}\)), i.e., (\(m_{\phi}\), \(\sigma_{\phi}^{2}\)) and (\(m_{\theta}\), \(\sigma_{\theta}^{2}\)), are then sent to the factor nodes (\(T_{A}\), \(T_{B}\), \(T_{C}\)) for trigonometric calculations, which triggers the FG iterations. As shown in Figure 2, \(T_{A}\) only takes the information from \(A_{\phi}\) and then feeds its outputs to the variable nodes \(\Delta x\) and \(\Delta y\), which forms a completed 2D location detection as in [16]. The calculations within the factor node \(T_{A}\) are referred to (3) and (5). In order to further fulfill 3D location detection, the information from \(A_{\theta}\) should also be utilized. Therefore, factor nodes \(T_{B}\) and \(T_{C}\) are introduced, which connect both \(A_{\phi}\) and \(A_{\theta}\) and feed to (\(\Delta x\), \(\Delta z\)) and (\(\Delta y\), \(\Delta z\)), respectively. The operations within \(T_{B}\) and \(T_{C}\) are based on (2), (4), (6), and (7). As a result, each of the relative distance variables is linked to two different trigonometric factor nodes, as shown in Figure 2.  
It should also be noted that in \(T_{A}\), the multiplication of two independent Gaussian variables is needed, while in \(T_{B}\) and \(T_{C}\), the multiplication of three independent Gaussian variables is required. Then, the means and variances of such multiplication results have to be calculated, assuming all messages are Gaussian. Specifically, given two independent variables \(a\sim\mathcal{N}(m_{a},\sigma_{a}^{2})\) and \(b\sim\mathcal{N}(m_{b},\sigma_{b}^{2})\), the product \(a\cdot b\) can be expressed by the mean and the variance as:  
Figure 2: 3D factor graph (FG) diagram for AOA-based location detection. \[m_{a\cdot b} = m_{a}\cdot m_{b}, \tag{13}\] \[\sigma_{a\cdot b}^{2} = m_{a}^{2}\cdot\sigma_{b}^{2}+m_{b}^{2}\cdot\sigma_{a}^{2}+\sigma_{ a}^{2}\cdot\sigma_{b}^{2}. \tag{14}\]  
If a third variable \(c\sim\mathcal{N}(m_{c},\sigma_{c}^{2})\) is introduced, the product of them \(a\cdot b\cdot c\) can be expressed in a similar way, as:  
\[m_{a\cdot b\cdot c}= m_{a}\cdot m_{b}\cdot m_{c}, \tag{15}\] \[\sigma_{a\cdot b\cdot c}^{2}= m_{a}^{2}\cdot m_{c}^{2}\cdot\sigma_{b}^{2}+m_{b}^{2}\cdot m_{c}^{2} \cdot\sigma_{a}^{2}+m_{a}^{2}\cdot m_{b}^{2}\cdot\sigma_{c}^{2}+\] (16) \[m_{a}^{2}\cdot\sigma_{b}^{2}\cdot\sigma_{c}^{2}+m_{b}^{2}\cdot \sigma_{a}^{2}\cdot\sigma_{c}^{2}+m_{c}^{2}\cdot\sigma_{a}^{2}\cdot\sigma_{b} ^{2}+\] \[\sigma_{a}^{2}\cdot\sigma_{b}^{2}\cdot\sigma_{c}^{2}.\]  
However, the original trigonometric calculations shown in (3)-(6) are not linear, which may break the Gaussianity of the output of the factor nodes. To deal with this problem, the first order TSexpansion is used to approximate the non-linear function \(f(\alpha)\) as:  
\[f\left(\alpha\right)\approx f\left(m_{\alpha}\right)+f^{\prime}\left(m_{\alpha }\right)\left(\alpha-m_{\alpha}\right), \tag{17}\]  
where the linear approximation is assumed to be around \(\alpha=m_{\alpha}\). As a result, three constant values \(f\left(m_{\alpha}\right)\), \(f^{\prime}\left(m_{\alpha}\right)\), and \(m_{\alpha}\) are obtained. Based on (17), the approximated mean and variance of \(f(\alpha)\) can be expressed by:  
Figure 2.0 Figure 2.1  
Based on (18) and (19), it is straightforward to calculate the means and the variances of particular trigonometric functions of (3)-(6), which are summarized in Figure 2.2  
Given the mathematical preliminaries above, the proposed FG detection will be explained in detail as follows.