Integrated Factor Graph Algorithm for DOA-Based Geolocation and Tracking

This paper proposes a new position tracking algorithm by integrating extended Kalman filter (EKF) and direction-of-arrival (DOA)-based geolocation into one factor graph (FG) framework. A distributed sensor network is assumed for detecting an anonymous target, where the process and observation equations in the state space model (SSM) are unknown. Importantly, the predicted state information can be utilized not only for filtering, but also for enhancing the observation process. To be specific, by taking the prediction into account as the a priori, a new FG scheme is proposed for GEolocation, denoted by FG-GE. The benefits are two-fold, compared with the conventional geolocation scheme which does not rely on the a priori information. First of all, significant performance improvement can be observed, in terms of the root mean square error (RMSE), when severe sensing errors are suddenly encountered. Furthermore, the proposed FG-GE can achieve dramatic reduction of computational complexity. In addition, this paper also proposes the use of a predicted Cramer-Rao lower bound (P-CRLB) to dynamically estimate the observation error variance, which demonstrates more robust tracking performance than that with only fixed average variance approximation.


I. INTRODUCTION
The roles to be played by wireless cellular networks are experiencing a paradigm shift from mobile communications to more dedicated infrastructure-supporting applications. Emerging use cases include smart transportation, factory automation, remote construction control, intelligent agriculture [1]- [4] and etc. In particular, position-based services are gaining increased attention through the rapid evolution of cellular networks [5], [6], where tracking of the user position is believed to be of great importance. Moreover, to cope with severe attenuation of millimeter-wave (mmWave) signals in fifth generation (5G) and beyond 5G (B5G) networks [7], accurate direction identification technologies are required. For example, beam tracking is applied in massive The associate editor coordinating the review of this manuscript and approving it for publication was Filbert Juwono . multiple-input multiple-output (mMIMO) systems [8], [9]. Challenged by the increased density and mobility of wireless devices, low-complexity but yet highly robust geolocation and tracking techniques are strongly demanded in the future networks.
To perform tracking, an accurate geolocation scheme is required, regarded as the observation process of the state space model (SSM). Conventional geolocation schemes rely on a series of range-related measurements in time, signal strength and angle domains. For example, applications using time-of-arrival (TOA) can be found in [10], [11], where time synchronization among targets, sensors and the fusion center is assumed. Time-difference-of-arrival (TDOA) is proposed in [12], [13], which eliminates the necessity of synchronization between sensors and the target. However, it can not outperform TOA as presented in [14]. More energy efficient geolocation techniques using received-signal-strength (RSS) VOLUME 8, 2020 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ can be found in [15], [16]. In such cases, off-line training with reference signals has to be performed beforehand, and furthermore, the transmit signal power from the target has to be known. Due to the constraints mentioned above, time and signal strength-based schemes are not applicable if the target is anonymous. Instead, this paper proposes an efficient use of direction-of-arrival (DOA) for the following reasons: (1) neither time synchronization nor off-line training is needed; (2) DOA measurement is possible even for silent target by using camera or other sensing devices; (3) multi-path effect on DOA measurements is supposed to be negligible in future networks, due to the very densely located sensing devices, applying mmWave techniques. DOA measuring techniques are well studied such as in [17]- [19]. However, discussion of practical techniques is out of the scope of this paper.
To avoid up-link transmission congestion in distributed sensor networks, factor graph (FG) algorithm [20] is applied, where only key parameters describing the probability density function (PDF) of DOA measurements are sent from sensors to the fusion center. FG is first used for GEolocation in [21], referred to as FG-GE in this paper. It is shown to achieve higher accuracy and lower complexity than the conventional schemes. The related work can be also found in [22]- [24]. In [25], a DOA-based two-dimensional (2D) FG-GE is proposed to detect a single anonymous target, with lower root mean square error (RMSE) than the least square (LS) technique. This work has been extended to three-dimensional (3D) in [26].
In addition, a sensor separation algorithm is proposed in [26] to solve the target-DOAs matching problem in the multi-target scenario. Since messages propagated over FG-GE are all Gaussian-approximated, only the mean and the variance are needed. Therefore, the required computational complexity is very light. The first order Taylor expansion is utilized in [25] to linearize the trigonometric functions, in order to keep the Gaussianity of messages' PDF. However, such approximation still incurs a certain level of computational cost, which may get severer if the FG structure becomes large and complex.
In this paper, a new DOA-based FG-GE is proposed to further reduce the computational cost of linear approximation. Compared with the conventional approaches in [25], [26], the proposed FG-GE always utilizes predicted state information, based on the tracker output at the previous timing. In this sense, geolocation and tracking jointly work in one framework, and form an integrated FG. In order to well express the real target behavior, extended Kalman filter (EKF) is used in this paper. The complexity of conventional EKF mainly lies in matrix operations based on the linearization result with the first order Taylor series expansion. Instead, an FG-based EKF, denoted by FG-EKF, is used in this paper, as in [27]. FG-EKF replaces matrix operations by scalar operations, and therefore significantly reduces the computational complexity. Note that the complexity analysis in this paper only considers FG-GE, while the analysis of FG-EKF can be found in [27].
Besides the complexity reduction, the predicted state information is also utilized for reducing the impact of sudden sensing errors. False alarm is one of the examples, where interfering signals are also detected at the sensor in additional to the desired ones. The principle is that, the current state of target should not vary significantly from that at the previous timing. Sudden sensing errors, which may not be realized by the sensor, can be identified at the center, and then eliminated in the FG-GE detection. The prediction made by EKF can be used as the a priori information, to evaluate the measurement data. In this paper, a simple false alarm scenario is studied. It has been shown that even though the sensor may suffer from false alarm, FG-GE can still take useful measurement data from such sensor.
Known as a tuning problem, estimating the variance of observation error plays an important role in EKF, as well discussed in [28], [29]. It should be noted that in the proposed tracking system, observation is not directly modeled with Gaussian errors. Instead, target positions are detected by distributed sensors, based on a series of Gaussian-distributed DOA measurements. The smallest variance of observation error is determined by the Cramer Rao lower bound (CRLB) [30]. Due to the fact that the proposed FG-GE can achieve a performance which is very close to the CRLB, it is reasonable to use CRLB to estimate the observation error variance in real time. However, calculating CRLB requires the real target position, which is practically not available. Hence, a predicted CRLB (P-CRLB) is calculated in this paper, where the predicted target position based on the result of previous timing is used. With this technique, the proposed FG-EKF is shown to achieve higher robustness than conventional schemes which assume only fixed estimation of the observation noise variance.
The main contributions of this work are summarized as follows.
1) A new DOA-based tracking system is proposed by integrating EKF and geolocation into one FG framework; 2) By utilizing the state prediction as the a priori information, the impact of sudden sensing errors can be eliminated; 3) The proposed FG-GE exhibits much lower computational complexity than the conventional scheme; 4) The robustness of tracking is further enhanced, by estimating the observation error variance for FG-EKF with the proposed P-CRLB in real time.
The organization of this paper is as follows. The tracking model to be investigated is described in Section II. Detailed steps of the proposed FG-EKF and FG-GE algorithms are then introduced in Section III. The predicted CRLB which is utilized in FG-EKF is derived in Section IV. Moreover, the complexity analysis of FG-GE is provided in Section V. The proposed tracking system is evaluated by a series of simulations, and the results are shown in Section VI. Finally, this work is concluded with several remarks in Section VII.

II. SYSTEM MODEL
A non-linear discrete SSM is focused on in this paper. The target state at timing k is represented by s k = [x k , y k ] T , k = {1, 2, . . . , K }, which defines the target position in a 2D plane. The process equation is given by where f (·) is a non-linear process function, and w k = w x,k , w y,k T represents a Gaussian-distributed noise vector, with each element following N 0, σ 2 w . According to the principle of EKF, the first order Taylor expansion is used to linearly approximate . However, f (·) is unknown by the tracker, and therefore it can not be directly applied to the position tracking problem. Instead, assuming that α exists which satisfies f (α) = s k−1 , (1) can be rewritten by where the gradient v k−1 = f (α) (s k−1 − α), which can be updated during the dynamic EKF process. Note that (2) is accurate only if the target does not move very fast between any two adjacent timings. Moreover, N distributed sensors locating at (X n , Y n ), with n = {1, 2, . . . , N }, are known to the fusion center. The measurement equation at sensor n and timing k can be given byθ with u n,k ∼ N 0, σ 2 θ being the measurement noise, andθ n,k denoting the measured DOA θ n,k , which is the output of the non-linear measurement function h(·), During each sampling timing, the N sensors are assumed to obtain L snapshots of DOA, from which the PDF parameters are extracted. Let mθ and σ 2 θ denote the mean and the variance ofθ , respectively, and the indexes n and k are omitted for the simplicity. The larger the value of L, the closer the values of mθ and σ 2 θ are to m θ and σ 2 θ , respectively. The transmission channels between sensors and the fusion center are assumed to be error-free, and therefore no specific transmission scheme is considered in this paper.
Given the real target state, the effective observation equation can be expressed by where e k denotes the observation noise vector. Since the target state is determined by the true DOAs, g (s k ) can be replaced byg (θ k ). Note that all messages in the proposed FG-GE algorithm are approximated to be Gaussian, and so is the observation noise, i.e., each element of e k follows N 0, σ 2 e . However, σ 2 e can not be directly measured. The smallest σ 2 e which can be theoretically achieved is determined by the variances ofθ k , according to the CRLB.

III. PROPOSED TRACKING ALGORITHM
The proposed DOA-based tracking algorithm is described in this section, including both FG-EKF and FG-GE.

A. FG-EKF
According to (2) and (5), the objective of this tracking problem is to find s k and v k which maximize the a posterior probability p (s k , v k |z 1:k ), where (·) 1:k denotes the data series from timing 1 to k. The marginal function of s k and v k , denoted byp (s k , v k ), is given bŷ where ∼ is the exclusion operator. The conditional PDF function in (6) can be further factorized by where (7) is derived based on Bayes' rule, and (8) is obtained due to that z k is only determined by s k . Further more, with the assumption that s k is only determined by s k−1 and v k−1 , and v k is only determined by v k−1 . By substituting (9) into (8), p (s 1:k , v 1:k |z 1:k ) can be re-written as where the denominator of (9), i.e. p (z k |z k−1 ), is ignored. Note that p (s 1:k−1 , v 1:k−1 |z 1:k−1 ) in (9) represents the filtering result of FG-EKF at the previous timing, and therefore is introduced in (10) with the timing index 1 to k. Based on (10), the proposed FG-EKF algorithm can be described in the following 3 steps.

1) STATE PREDICTION
First of all, the current state prediction is performed based on the FG-EKF outputs at the previous timing. As shown in Fig. 1, the message flow of the predicted state µ c (s k|k−1 ) is given by (11) where the message flows µ a (s k−1 ) and µ b (v k−1 ) are obtained as the FG-EKF outputs at timing k − 1, with the function

2) STATE REFINEMENT
Secondly, the predicted state s k|k−1 is further refined by the observation, yielding the FG-EKF results at the current timing k. As shown in Fig. 1, the message flow of the FG-EKF output µ e (s k ) is given by where µ d (z k ) denotes the message flow of the observation, obtained by the proposed FG-GE scheme.

3) GRADIENT UPDATE
After obtaining the refined state, the vector v k should also be updated. Since the process equation (1) is assumed to be unknown, v k is only updated by refining v k−1 with a correction termv k . According to Fig. 1, the message flow of thev k can be given by where µ f (s k ) is the message flow of the refined state, and f (v k |s k−1 , s k ) = s k − s k−1 . Finally, the message flow of the updated vector v k is given by B

. FG-GE
To obtain the state observation z k required by FG-EKF, the proposed FG-GE is applied, which is detailed in this subsection. Note that the sensor index n is omitted in the following derivations. With the first order Taylor series expansion, the true DOA, expressed by (4), can be linearly approximated, centered at the point β, as where β = [β x , β y ] T . Obviously, the approximation of (15) is accurate if β is close to s k . In this paper, we propose that β equals to the predicted state by FG-EKF from the previous timing, i.e., β = s k|k−1 . Therefore, (15) can be further expressed by where λ 1 , λ 2 and λ 3 are all constants, given by Therefore, the target position can be linearly expressed by Given the Gaussian-distributed DOA measurements as the input of FG-GE, all messages interchanged inside FG-GE are also Gaussian, where linear approximations at the function nodes follow (20) and (21). The proposed FG-GE structure is illustrated in Fig. 2, as a part of the integrated tracking system. Let η n denote the message flow of the measured DOA associated with the function node h n . In the upward process, ξ z x ,n and ξ z y ,n denote the messages forwarded from h n to z x and z y , respectively. Then, ρ z x ,n and ρ z y ,n are the downward messages arriving at h n from z x and z y , respectively. The detailed updates at the function node h n can be described by two steps as follows.
1) Update of ξ z x ,n : 2) Update of ρ z x ,n : m ρ zy,n = σ 2 ρ zy,n i∈{1:N },i =n The iteration is to be performed at the function node 1 to N , with the estimated position m z = m z x , m z y T obtained by and It should be noted that σ 2 z = σ 2 z x , σ 2 z y T denotes the variance vector of the FG-GE estimation. However, according to the observation equation defined in (5), g(s k ) is a constant, and therefore the observation variance of z k is equal to σ 2 e , which is different from the results in (32) and (33). The observation variance is calculated in the following section.

IV. P-CRLB DERIVATION
In this section, a P-CRLB is derived for the proposed EKF, as the estimate of the observation noise variance. First of all, by omitting the timing index k, the CRLB is given by [25] where F denotes the Fisher information matrix (FIM). Given the measured DOA variableθ with L samples, the FIM can be expressed by where the Gaussian PDF function ofθ is given by (36) Moreover, due to the fact that and according to (36), Then, the FIM can further be derived by where ∂θ ∂s is defined by the Jacobian matrix, and the Euclidean distance between sensor n and the target in the 2D plane is denoted by d n , with n = {1, 2, . . . , N }. Finally, the CRLB of the proposed geolocation system can be expressed by However, since the real target position is unknown in practice at the timing k, x k and y k in (41) and (42) are replaced by x k|k−1 and y k|k−1 , respectively, with which (40) can be rewritten as Finally, the P-CRLB can be obtained as

V. COMPLEXITY COMPARISON
The complexity comparison between the proposed FG-GE and the conventional FG-GE in [25] is provided in this section. Specifically, the required addition (ADD), multiplication (MUL) and trigonometric (TRI) operations for the both schemes within one iteration time are listed in Tables 1-2.   Note that we assume subtraction and division have the same complexity as addition and multiplication, respectively, for the sake of simplicity. Moreover, only the upward message flows as shown in Fig. 2 are considered, while the downward messages from the variable nodes z x and z y to the function nodes are not included, since they are identical for the two schemes compared. As shown in Table 1, λ 1 , λ 2 and λ 3 are all constants during the proposed FG-GE detection at each timing, and the required operations do not increase proportionally to the iteration time J . Moreover, only one trigonometric operation is needed in the proposed FG-GE. However, the complexities of the conventional FG-GE are found to be completely proportional to J , as shown in Table 2. In addition, more trigonometric operations are included as J increases. Therefore, our proposed FG-GE scheme is found less complicated compared to the conventional one.

VI. SIMULATIONS
In this section, an illustrative non-linear SSM is evaluated with the proposed tracking algorithm. With the timing where φ is set at π/60. The initial state s 0 = [x 0 , y 0 ] T = [0, 0] T . Simulations aiming at different investigation purposes are presented as follows.

A. FG-GE COMPARISON
In this sub-section, comparison between the proposed and the conventional counterpart schemes is provided under two scenarios. In Scenario 1, an indoor environment is considered, where three distributed sensors are deployed at (0,-1), (8,10) and (15, -2), respectively, with the unit being meter. At each timing, every sensor is assumed to measure 60 DOA samples, i.e., L = 60. σ 2 w = 0.05 and σ 2 θ = 3 • were also assumed. Scenario 2 focuses on the outdoor environment, with three sensors located at (0,-10), (80,100) and (150, -20), respectively, and the measurement snapshot number L = 70. Moreover, σ 2 w = 1 and σ 2 θ = 5 • were set. For fair comparison, the FG iteration time J = 10 was fixed, and the same random seed was used. Note that the conventional FG-GE applied in the simulation follows [25].
The detected target positions using FG-GE, as well as the tracking results are shown in Fig. 3. Clearly, in the both scenarios, the proposed FG-GE can achieve slightly lower root mean square errors (RMSEs) than that with the conventional scheme. The improvement is not significant because in such a stable sensing environment, even with the conventional scheme, close-CRLB performance can be achieved, and hence the room for further improvement is limited. The same performance trends can be observed when testing other random seeds in the simulation, but the results are omitted due to the space limitation. Note that the proposed scheme can achieve centimeter level RMSEs in indoor scenario, as shown in Fig. 3(a), which is supposed to satisfy the geolocation requirment in 6G systems [31].
Note that with the conventional FG-GE, the detection performances do not vary significantly at different timings. However, with the proposed FG-GE, a prediction of the current state is always needed. Therefore, large errors may happen at the initial stage, due to the lack of initial state prediction. In this sub-section, the impact of the initialization is neglected by assuming that the initial state prediction is known by the tracker. More detailed discussions are provided in the next sub-section.

B. CONVERGENCE
Due to the difficulty of theoretical analyses, the convergence behavior of the proposed FG-GE is investigated only by simulations in this sub-section. Specifically, it was evaluated in terms of two parameters, i.e., the timing index and the FG iterations.
First of all, the convergence behavior versus timing index is evaluated. In order to compare the average RMSE, 100 simulations were performed for the proposed and the conventional FG-GE schemes at each timing. The other parameters were set the same as in Scenario 2 defined in the previous subsection. Note that for the proposed FG-GE, prediction of the first stateŝ 1|0 was randomly chosen. In the first simulation, s 1|0 = [10, 15] T was used. It can be clearly seen from Fig. 4(a) that the average RMSE of the proposed FG-GE is around 9 meters at k = 1, compared to 1.8 meters with the conventional scheme. However, it quickly converges to almost the same level as that of the conventional scheme after the 3rd timing. According to Fig. 4(b), withŝ 1|0 = [15,25] T , the average RMSE with the proposed FG-GE is 17.5 meters at k = 1, which is larger than the previous case, becauseŝ 1|0 is farther from the real target position. It is found that, the initial RMSE of the proposed FG-GE depends on the state prediction at k = 1, but quickly converges after only 3 to 4 rounds of computations. Therefore, the initial convergence problem of the proposed FG-GE can be negligible in the tracking phase.
Then, the convergence behavior was evaluated versus the FG iterations. As mentioned in Section V, the proposed FG-GE requires less computations than that of the conventional scheme for each FG iteration. However, the real system complexity should also consider the FG iteration time required to achieve the performance convergence. Specifically, the average RMSEs of the two schemes are simulated versus J . The other parameters were set the same as that defined in Scenario 2 of Section VI-A.
According to Fig. 5, after the first round of FG iteration, the proposed FG-GE achieves an average RMSE at 30.5 meters, which is roughly 6 meters larger than that of the conventional scheme. However, the average RMSEs of the both schemes quickly converge after around 5 to 6 iterations. This observation clearly indicates that the proposed FG-GE does not require more FG iterations compared with the conventional scheme.

C. FALSE ALARM
In this sub-section, the robustness of the proposed FG-GE is presented when the DOA measurement suffers from false alarm. A simple scenario is evaluated in this simulation, i.e., false alarm happens at each timing with a probability p f for only one of the deployed sensors. If it happens, interfering signals with random DOAs will be observed at the sensor, besides the measurement of the real target. With the conventional FG-GE, the measurement data from the sensor in false alarm will not be used for detection at the fusion center, because separating the real and the interfering DOA measurements is difficult. However, with our proposed FG-GE scheme, false alarm signals can be identified, if measured VOLUME 8, 2020 DOAs are outside of the degree range threshold. The degree range threshold was set at ±20 • , compared with the predicted DOA based on the a priori information, and p f = 1/5. The rest of parameters followed the Scenario 2 of Section VI-A.
As shown in Fig. 6, the proposed FG-GE is more stable, with the average RMSE equal to 1.65 meters. However, the conventional scheme exhibits larger estimation variances caused by false alarm, with the average RMSE twice as large as that of the proposed scheme. Therefore, utilizing the a priori information at the FG-GE is proved to achieve higher stability, because the more the useful DOA measurements, the better the detection performance. Note that the average RMSE is calculated only considering the performances with the timing k > 5, in order to eliminate the influence of the initial convergence behavior.

D. P-CRLB
The utilization of P-CRLB is motivated by the fact that, the observation stability, expressed by the variance σ 2 e , may dynamically change in practical tracking environments. To keep the tracking performance stable, P-CRLB is used as the estimate of σ 2 e in real time. The estimation accuracy is evaluated by comparing three items, i.e., the P-CRLB, the real CRLB and the average mean square error (MSE) achieved by the proposed FG-GE. The average MSE mentioned above is equivalent to the average σ 2 e . Note that σ 2 e is practically determined by the measurement variance σ 2 θ , which is always kept the same for all sensors in order to observe the global effect trend. The other parameters are set the same as those defined in Scenario 2 of Section VI-A.
First of all, it is found from Fig. 7 that the gap of average MSE curves between the proposed FG-GE and the real CRLB is very minor, especially when σ 2 θ values are small. This observation implies an excellent geolocation performance of the proposed FG-GE. Moreover, the P-CRLB curve is found to be very close to that of the average MSE achieved by the simulations. Therefore, estimating the observation noise variance by P-CRLB is proved to be very accurate.
To further evaluate the robustness of FG-EKF using P-CRLB, simulations were conducted with dynamic DOA  measurement variances. Specifically, at each timing, σ 2 θ was randomly chosen from the set {2 • , 6 • , 10 • , 14 • , 18 • }, but still kept the same for all sensors. With the proposed technique, P-CRLB was dynamically calculated as the estimate of σ 2 e , required in the FG-EKF calculation. However, in the comparative scheme, only fixed σ 2 e is used, assuming that the observation error statistics can be obtained through the offline tests. In fact, evaluating the proposed FG-GE independently of FG-EKF is not feasible, as described in Section IV. Therefore, in this simulation, only approximated average σ 2 e was used, which was calculated as the MSE with the proposed FG-GE.
As shown in Fig. 8, the average RMSEs with dynamic P-CRLB are in general lower than that with the fixed σ 2 e . After further averaging the RMSEs over all timings, the final average RMSE achieved by the proposed technique is 1.64 meters, which is 1.24 meters smaller than that with the conventional scheme compared. Therefore, the proposed FG-EKF using P-CRLB exhibits higher robustness against dynamic environment changes than that with the case of offline average error estimation.

VII. CONCLUSION
This paper has proposed a DOA-based tracking algorithm, which integrates EKF and geolocation into one FG framework. The predicted state information obtained from EKF is used not only for filtering, but also for observation, i.e, as the a priori information of FG-GE. According to the simulation results, the proposed FG-GE can always achieve lower average RMSEs than that with conventional schemes. Although large errors may happen at the beginning of tracking due to the lack of accurate a priori, it has been shown that the detection performance quickly converges after only 3 or 4 timings. Moreover, the impact of sudden sensing errors, such as false alarm, can be effectively reduced by utilizing the a priori information. In addition, the P-CRLB is used in FG-EKF to estimate the variance of observation error. With this technique, tracking is made more robust in the presence of dynamic environment change, compared with the scheme using fixed estimation of the observation variance. The proposed tracking system can be easily implemented in practice due to its low complexity.