Particle Filtering for Three-Dimensional TDoA-Based Positioning Using Four Anchor Nodes

In this article, the four-anchor time difference of arrival (TDoA)-based three-dimensional (3D) positioning by particle filtering is addressed. The implemented particle filter uses 1000 particles to represent the probability density function (pdf) of interest, i.e., the posterior pdf of the target node’s state (position). A resampling procedure is used to generate particles in the prediction step, and TDoA measurements are used to determine the importance, i.e., weight, of each particle to enable updating the posterior pdf and estimating the position of the target node. The simulation results show the feasibility of this approach and the possibility to employ it in indoor positioning applications under the assumed working conditions using, e.g., the ultra-wideband (UWB) wireless technology. Therefore, it is possible to enable unmanned air vehicle (UAV) positioning applications, e.g., inventory management in large warehouses, without the need for an excessive number of anchor nodes.


Introduction
The localization of a target node using time difference of arrival (TDoA) measurements received and is still receiving considerable attention. TDoA-based positioning algorithms [1][2][3][4][5][6][7] are widely applied in sensor networks [1,8], wireless communication [2,3], target tracking [9,10], navigation [11], underwater [12], tactile interfacing for human-computer interaction [13], and seismic exploration [14]. The determination of a unique target node's location using TDoA measurements in closed form requires at least four anchor nodes, i.e., three TDoA measurements, in the two-dimensional (2D) space and five anchor nodes, i.e., four TDoA measurements, in the three-dimensional (3D) space [15]. However, an object's location can also be determined with one fewer anchor node, i.e., three and four anchor nodes for the 2D and 3D spaces, respectively, if further information is available to resolve any location ambiguities [16]. Solving the TDoA-based 2D/3D positioning problem with an iterative algorithm using three/four anchor nodes requires a good initial position estimate to guarantee convergence to the true position.
Unmanned air vehicles (UAVs), also known as drones, are an emerging technology with a surge in many different fields of application [17]. Both expressions (UAV and drone) are used interchangeably throughout the article. Recent technological advancements increased the functionalities and features of drones, while unit prices are constantly decreasing. Drones can be equipped with various sensors to achieve certain functions. Sensors for guidance and control functions include positioning sensors, e.g., global navigation satellite system (GNSS) receivers, inertial sensors, e.g., accelerometers and gyroscopes, barometers, digital compasses, and ultrasound sensors. Communication functions are accomplished using radio wave antennas of different frequency, such as WLAN, Wi-Fi, RFID, ultra-wideband (UWB), and 5G. Environmental monitoring tasks are performed using video cameras, to increase the efficiency of cost and time, as well as the safety of human workers. A low-cost sensing system was developed with an EKF-based multi-sensor fusion framework for autonomous navigation of UAVs in warehouse indoor environments. The used low-cost sensors included three cameras (forward, upward, and downward), a 2D laser scanner, a 1D range sensor, and an IMU, in addition to a map containing pose information of attached tags. The use of UAVs in warehouse inventory and similar applications was also reported in References [47,[49][50][51][52][53].

Particle Filtering for TDoA-Based Positioning
Bayesian inference approaches provide well-studied tools for calculating the distribution of a target node's location given TDoA measurements. Thus, the required probability density function (pdf) is the probability of the target node's location given the available TDoA measurements. Bayes' rule defines a method to calculate this pdf using prior information, a system or dynamic model, i.e., knowledge of target node's motion, to describe the evolution of the target node's location with time, and a measurement model relating noisy TDoA measurements to the location of the target node. Bayes' rule is mathematically expressed as where x is the state vector, i.e., the 3D location of the target node, which is a random variable; z is the noisy TDoA measurement vector; p(x) is the prior pdf of the state; p(z|x) is the measurement model, i.e., the pdf of the TDoA measurements conditioned on the state; p(x|z) is the posterior pdf of the state (pdf of interest), which describes the distribution of the target node's location taking into account the TDoA measurements.
If the system and measurement models are both linear and Gaussian, the state can be optimally estimated in closed form using the Kalman filter (KF). If either the system or measurement model is nonlinear or non-Gaussian, the KF can only provide a suboptimal estimate to the non-Gaussian posterior pdf. Therefore, approximate computational methods are needed, which are based on the state space approach to time series modeling [54]. The approach adopted in this study proceeds in two steps (or stages): prediction and update. The prediction step propagates the state pdf at one time to the next time by using the dynamic model. Since the state is subject to unknown disturbances, modeled as random noise, the propagated state pdf is broadened and deformed. The last available measurements are used in the update step to modify, i.e., to tighten, the predicted state pdf. This is accomplished by using Equation (1), which mathematically describes the mechanism for updating knowledge about the state pdf in the light of new measurements.

Problem Statement and Concept of Solution
In nonlinear filtering problems, the state vector is denoted x(t) ∈ R 3 , where t is continuous-valued time. The state evolution is described using a continuous-time stochastic differential equation, also referred to as an Itô differential equation [55]. However, the state is usually sampled at discrete time instants and, thus, the state at the k-th discrete sample time is denoted x k ≡ x(t k ). The state evolves according to a continuous-time stochastic model, i.e., system model.
where f(·) is a known (possibly nonlinear) function of the state, and v(t) is a process noise sequence, which accounts for random disturbances in the target node's motion. TDoA measurements are a nonlinear function of the state, i.e., the target node's location. They occur at times t k , where k = 1, 2, . . . K. The k-th measurement is denoted z(t) ∈ R m , where m is the dimension of the measurement vector, i.e., the number of TDoA measurements available at any time instant t k .
If four anchor nodes are used, m = 3. The measurements are related to the state via the equation, i.e., measurement model.
where h k (·) is a known (possibly nonlinear) function, and w k is a measurement noise sequence. Defining the measurement history Z k {z 1 , . . . z k }, we seek estimates of x k based on the sequence of all available measurements up to the time t k , i.e., the problem, from a Bayesian perspective, is to recursively calculate the posterior pdf p(x k |Z k ) in two stages: prediction and update. The prediction step moves the pdf p(x k−1 |Z k−1 ) forward in time to the pdf p(x k |Z k−1 ) using the system model and without incorporating the new TDoA measurements z k . The update step incorporates the new TDoA measurements z k , using the measurement model, into the predicted pdf p(x k |Z k−1 ) to obtain the updated, i.e., posterior, pdf p(x k |Z k ).
The prediction step uses the system model in Equation (2) to obtain the predicted pdf p(x k |Z k−1 ) via the Chapman-Kolmogorov equation.
Equation (4) is a statement of the law of total probability. Since Equation (2) describes a first-order Markov process, the first term in the integral of Equation (4) simplifies to p(x k |x k−1 ), which is the probabilistic model of the state evolution. Thus, Equation (4) is rewritten as The prediction, i.e., prior, pdf is updated by incorporating the available TDoA measurements z k to obtain the posterior pdf of the state via Bayes' rule.
The pdf on the denominator is the normalizing constant. The first term on the nominator, i.e., the (measurement) likelihood function, which is defined by the measurement model in Equation (3), simplifies to p(z k |x k ) due to conditional independence. Thus, Equation (6) is rewritten as The optimal Bayesian solution based on Equations (5) and (7) is conceptual because the analytic solution of both equations cannot be determined in most practical situations. Therefore, numerical approximations have to be used.

The Particle Filter
In general, it is not possible to write a closed-form expression for the posterior pdf p(x k |Z k ) in the nonlinear non-Gaussian case. Therefore, an approximate solution is required. The solution used in this study is referred to as the particle filter, which is a numerical approximation based on random sampling. Particle filters are applications of Monte Carlo methods to Bayesian estimation [56]. Background information on particle filtering can be found in, e.g., References [57][58][59][60]. The fundamental concept in the particle filter is to approximate the posterior pdf p(x k |Z k ) as a weighted combination of sample points, also known as particles. where δ(x) is the Dirac delta function, w p k refers to weights and sums to unity, and x p k refers to particles. The convergence properties of the approximation were well studied [61,62]. The expectation of any nonlinear function of the state given the approximate posterior pdf can be evaluated as follows: The approximation of Equation (9) is referred to as Monte Carlo integration and can be sequentially applied to both Equations (5) and (7), i.e., the Chapman-Kolmogorov prediction and Bayesian update, respectively. Therefore, the particle filter, and Bayesian filtering in general, is also referred to as sequential Monte Carlo (SMC).
The particle filter algorithm, thus, provides a mechanism to recursively generate a set of weighted particles approximating p(x k |Z k ) starting from a previous set of weighted particles approximating p(x k−1 |Z k−1 ) in two steps (or phases): prediction and update. In the first phase, the particles are moved to new locations using a tractable pdf referred to as the proposal distribution, approximating the pdf of interest. In the second phase, new particle weights are determined to correct for the difference between the proposal distribution and the true pdf. This procedure is referred to as importance sampling [57].
The proposal distribution is a function selected by the system designer to cover the whole state space where the true distribution is non-zero. A poor selection of the proposal distribution lowers the efficiency of particle filters since many particles will be assigned very low weights and, therefore, a larger number of particles will be required to satisfy performance requirements. The sample-importance-resample (SIR), also denoted sequential importance resampling, particle filter is a common and popular implementation that uses the system dynamics as a proposal distribution, which is relatively straightforward and simplifies weight updating. In this study, a form of SIR particle filter was implemented, in which, for each particle x p k−1 , a new particle x p k is drawn from the transition pdf p(x k |x p k−1 ) , and then weights are updated by scaling the previous weights by the current measurement likelihood and renormalizing.
where W k = P p=1 p z k |x p k w p k−1 is the normalizing factor.

Particle Filter Implementation
Particle filtering was also implemented for positioning problems in, e.g., References [58,60,[63][64][65]. In the SIR version of the particle filter, also known as a bootstrap filter or condensation algorithm, the particles are randomly generated from the motion (or dynamics) model. Sampling from the dynamics can be a very diffuse distribution. Thus, the proportion of particles that sample a trajectory close to the measurements may be very small. Therefore, a large number of particles may be required to represent the high-probability regions of the state space. Resampling is a strategy to improve the number of particles following trajectories with high likelihood. At each time step, copies of highly likely particles replace unlikely particles via a random sampling process. Some of the resampling strategies include multinomial resampling, stratified resampling, systematic resampling, and residual resampling [66,67].
The implemented particle filter is listed in Algorithm 1. Initially, there is no information about the position of the target node. Therefore, the particle filter generates P particles uniformly distributed in the whole working space. When the set of the three TDoA measurements, made by four anchor nodes, is available at the first time index, i.e., k = 1, the weight w p k of each particle x p k is then computed as follows: where dd k,i is the i-th TDoA measurement, i.e., range or distance difference measurement, at time t k , i = 1, . . . , 3, and dd p i is the i-th distance difference of the particle x p k , which is the measurement model. The weight of each particle is inversely proportional to the similarity metric used, which is the sum of squared distances between dd k,i and dd p i . The distance difference measurement dd k,i is expressed as where a i+1 is the known 3D position of the non-master anchor nodes in a 3D Cartesian coordinate system, x k is the estimated 3D position of the target node at time t k , a 1 is the known 3D position of the reference or master anchor node, and w i+1,1 is the measurement noise. The i-th distance difference of the particle x p k , i.e., the measurement model is computed as follows: A number L of the best-weighted particles, where L < P , is selected, and the weights of these selected particles are normalized. The position x k of the target node is then estimated using these L selected particles and their normalized weights as follows: which is the weighted trimmed average estimate (WTAE). The initialization phase is completed after computing the first estimate of the target node's position, i.e., x 1 .
In the prediction step, the particles are placed in new locations to account for the dynamics of the target node. A resampling procedure is used to generate uniformly distributed particles.
where R = R x R y R z is called the resampling space, where the resampling ranges R x , R y , and R z , in the x-, y-, and z-directions, respectively, are assumed to be equal, i.e., R x = R y = R z = R. Thus, the proposal distribution approximating the posterior pdf generates new particles, which are uniformly distributed within a cube of 2R side length and center at the previous state estimate x k−1 . When the value of R is properly determined, i.e., to consider the maximum expected displacement in the x-, y-, and z-directions during the measurement sampling time, the cloud of particles is continuously updated to ensure a good representation of the posterior pdf and acceptable accuracy of the particle filter while avoiding degeneracy over time. Sudden or unexpected movements can cause incorrect predictions that lower the diversity of the particle cloud and fail the particle filter. Therefore, the value of R should be selected to also account for such extreme situations to quickly converge to the true location or to recover from false locations. If convergence or recovery fails, the particle filter becomes stranded and has to be restarted using the last reliable measurements [68].
When new TDoA measurements are available at the next time index, the weights of the new particles are calculated using Equation (11), and then a new state estimate, i.e., new target node's position, is computed using Equation (14). The iteration of the particle filter is further continued as indicated in Algorithm 1.

Algorithm 1
The particle filter algorithm.

Initialization:
Generate P particles uniformly distributed in the whole working space. Compute the weight of each particle x p k according to Equation (11), when TDoA measurements are available at time t k , k = 1. Estimate the position of the target node, x k , according to Equation (14).

Prediction:
Generate new particles according to Equation (15).

Update:
Compute the weight of each particle x p k according to Equation (11), when TDoA measurements are available at time t k , k > 1.

State Estimation:
Estimate the position of the target node, x k , according to Equation (14). Set k = k + 1 and repeat from step 1.

Simulation Results
The performance of the proposed particle filter was evaluated by computer simulations using MATLAB. The root-mean-square error (RMSE) of the positioning solutions was investigated upon varying resampling range, R, values, and SNR levels. The TDoA, i.e., distance difference, measurements were simulated as true value plus a zero-mean Gaussian noise. The SNR of any measured TDoA signal is defined as The variance of the Gaussian noise, σ 2 i , is then computed as Two simulation environments were considered: (1) a small-size indoor environment to resemble applications such as an UAV indoor light show, which is a key performance at many ceremonies [45]; (2) a large-size indoor environment to resemble a UAV-based inventory management application. In the small-size environment, four simulation scenarios were considered with identical anchor node arrangement. The RMSE results were obtained via 100 simulation runs and are presented in Sections 4.1-4.4. The RMSE results, in the large-size environment, were obtained via 50 simulation runs and are presented in Section 4.5. The implemented particle filter used a constant number of 1000 particles in all experiments. The position of the target node (UAV or drone) was estimated by Equation (14) using the 10% best-weighted particles, i.e., L = 0.1 × P.

Three-Dimensional Linear Path
Four anchor nodes were placed at (0,0,0), (10,0,10), (10,10,0), and (0,10,10), and a drone moved along a linear path from position (0.5,0.5,0.5) to position (9.5,9.5,9.5) (see Figure 1) at constant velocities of 0.1 m/s in the x-, y-, and z-directions. Table 1 lists the total number of TDoA measurements obtained along the path and the traveled distances in the x-, y-, and z-directions between any two measurement times at all investigated measurement update rates. The maximum true distance difference, TDoA, value along the path was 12.58 m. Thus, the corresponding maximum standard deviation of the measurement errors according to Equation (17)  measurements obtained along the path and the traveled distances in the x-, y-, and z-directions between any two measurement times at all investigated measurement update rates. The maximum true distance difference, TDoA, value along the path was 12.58 m. Thus, the corresponding maximum standard deviation of the measurement errors according to Equation (17)    The RMSE results at the investigated measurement update rates are depicted in Figures 2-6. The particle filter did not converge at an update rate of 1 Hz with a resampling range, R, of 0.1 m, over the investigated SNR levels, even if the initial position was known because the search volume was too small to allow the efficient working of the filtering mechanism. Therefore, the results with 0.1 m resampling range were omitted in Figure 2. The resampling range should be at least slightly greater than the traveled distance in the time between any two successive measurements, which was 0.1 m in the x-, y-, and z-directions at the measurement update rate of 1 Hz, to allow the particle filter to find and consider the correct volume containing the most likely drone positions, where the particle cloud should be placed. It can be seen, from Figures 2-6, that the positioning accuracy, in the RMSE sense, increased with increasing measurement update rate and SNR levels. Best accuracies can generally be obtained with a smaller resampling range if the measurement update rate and the SNR level are high enough. Thus, the search volume would be sufficient to account for measurement errors and the drone movements in the x-, y-, and z-directions. The obtained accuracy at the SNR level of 40 dB was almost constant over the resampling ranges 0.1 to 0.5 m at all investigated measurement update rates, since the traveled distance by the drone in the x-, y-, and z-directions was correctly contained within the search volumes suggested by the resampling ranges, and the measurement errors were small. It can be seen, from Figures 2-6, that the positioning accuracy, in the RMSE sense, increased with increasing measurement update rate and SNR levels. Best accuracies can generally be obtained with a smaller resampling range if the measurement update rate and the SNR level are high enough. Thus, the search volume would be sufficient to account for measurement errors and the drone movements in the x-, y-, and z-directions. The obtained accuracy at the SNR level of 40 dB was almost constant over the resampling ranges 0.1 to 0.5 m at all investigated measurement update rates, since the traveled distance by the drone in the x-, y-, and z-directions was correctly contained within the search volumes suggested by the resampling ranges, and the measurement errors were small.  The standard deviations of the measurement errors at the SNR levels of 25 and 30 dB, i.e., 71 and 40 cm, respectively, seem to be feasible for real-world scenarios with dimensions similar to the simulated scenario. The horizontal and vertical RMSEs at the SNR level of 25 dB were about 14 and 10 cm, and those at the SNR level of 30 dB were about 12 and 8 cm, respectively, at a measurement update rate of 16 Hz. The best accuracies (from Figures 2-6), rounded to two significant digits, obtained at all investigated SNR levels and measurement update rates are summarized in Table 2. The accuracies increased from left to right as the SNR level increased, and from top to bottom as the measurement update rate increased. Since the SNR levels of 25 and 30 dB, corresponding to measurement errors of 71 and 40 cm, respectively, can be achieved in a similar real-world scenario, a horizontal accuracy of 20 cm or less and a vertical accuracy of 15 cm or less could be obtained if at least a measurement update rate of 4 Hz is implemented, as can be seen from Table 2. The standard deviations of the measurement errors at the SNR levels of 25 and 30 dB, i.e., 71 and 40 cm, respectively, seem to be feasible for real-world scenarios with dimensions similar to the simulated scenario. The horizontal and vertical RMSEs at the SNR level of 25 dB were about 14 and 10 cm, and those at the SNR level of 30 dB were about 12 and 8 cm, respectively, at a measurement update rate of 16 Hz. The best accuracies (from Figures 2-6), rounded to two significant digits, obtained at all investigated SNR levels and measurement update rates are summarized in Table 2. The accuracies increased from left to right as the SNR level increased, and from top to bottom as the measurement update rate increased. Since the SNR levels of 25 and 30 dB, corresponding to measurement errors of 71 and 40 cm, respectively, can be achieved in a similar real-world scenario, a horizontal accuracy of 20 cm or less and a vertical accuracy of 15 cm or less could be obtained if at least a measurement update rate of 4 Hz is implemented, as can be seen from Table 2.

Horizontal Linear Path
The drone moved along a horizontal linear path from position (0.5,0.5,2.5) to the position (9.5,9.5,2.5) (Figure 7), at a constant height of 2.5 m and constant velocities of 0.1 m/s in the xand y-directions. The total number of measurements and traveled distances, in the xand y-directions only, along the path at the investigated measurement update rates were identical to the values listed in Table 1. The maximum true distance difference along the path was 11.07 m. Thus, the corresponding maximum standard deviation of the measurement errors according to Equation (17)

Horizontal Linear Path
The drone moved along a horizontal linear path from position (0.5,0.5,2.5) to the position (9.5,9.5,2.5) (Figure 7), at a constant height of 2.5 m and constant velocities of 0.1 m/s in the x-and ydirections. The total number of measurements and traveled distances, in the x-and y-directions only, along the path at the investigated measurement update rates were identical to the values listed in Table 1. The maximum true distance difference along the path was 11.07 m. Thus, the corresponding maximum standard deviation of the measurement errors according to Equation (17)   The positioning accuracy behavior in the RMSE sense, at all SNR levels, measurement update rates, and resampling ranges, was similar to the 3D linear path case investigated in the previous subsection. Therefore, the plots of the results are skipped for brevity, and only the summary of these results is listed in Table 3. A horizontal accuracy of 22 cm or less and a vertical accuracy of 15 cm or The positioning accuracy behavior in the RMSE sense, at all SNR levels, measurement update rates, and resampling ranges, was similar to the 3D linear path case investigated in the previous subsection. Therefore, the plots of the results are skipped for brevity, and only the summary of these  Table 3. A horizontal accuracy of 22 cm or less and a vertical accuracy of 15 cm or less could be obtained at an SNR of 25 dB or higher if at least a measurement update rate of 4 Hz is achieved, as can be seen from Table 3.

Horizontal Circular Path
The drone moved on a horizontal circular path, with a radius of 4 m and center at (5,5,7.5) (Figure 8  At a measurement update rate of 1 Hz, the particle filter did not converge with the resampling ranges of 0.1 and 0.2 m, regardless of the SNR level, since both ranges were less than the maximum distance of 0.25 m that could be traveled in the xand y-directions between any two measurement times. Thus, the particle filter would not find the correct volume of likely drone positions. Therefore, the results with the resampling ranges of 0.1 and 0.2 m are omitted in Figure 9. The particle filter did not also converge at a measurement update rate of 2 Hz with the resampling range of 0.1 m since the maximum distance that could be traveled in the xand y-directions between any two measurement times was equal to 0.125 m. Therefore, the results with a resampling range of 0.1 m are also omitted in Figure 10. RMSE results at the measurement update rates of 4, 8, and 16 Hz are shown in Figures 11-13, respectively. Figure 8. Anchor node arrangement and the horizontal circular path.         The best accuracies, rounded to two significant digits, obtained at all investigated SNR levels and measurement update rates are summarized in Table 4. A horizontal accuracy of 16 cm or less and a vertical accuracy of 11 cm or less could be obtained at an SNR of 25 dB or higher if at least a measurement update rate of 4 Hz is achieved, as can be seen from Table 4.

Helical Path
The drone moved on a helical path, with a radius of 4 m (Figure 14), at a constant horizontal angular velocity of π 20 rad/s and a constant vertical velocity of 7.85 cm/s from position (8,4,0) to position (8,4,6.28). The corresponding velocities in the xand y-directions varied between a minimum of 0.05 m/s and a maximum of 0.63 m/s.

Helical Path
The drone moved on a helical path, with a radius of 4 m (Figure 14), at a constant horizontal angular velocity of rad/s and a constant vertical velocity of 7.85 cm/s from position (8,4,0) to position (8,4,6.28). The corresponding velocities in the x-and y-directions varied between a minimum of 0.05 m/s and a maximum of 0.63 m/s.  Positioning accuracy results at measurement update rates of 4, 8, and 16 Hz are illustrated in Figures 15-17, respectively, and summarized in Table 5. In this higher-dynamics scenario, an SNR level of at least 30 dB was required to obtain a horizontal accuracy below 20 cm at the 4 and 8 Hz measurement update rates. The obtained vertical accuracy was 15 cm or less. At a measurement update rate of 16 Hz, horizontal and vertical accuracies of 20 and 14 cm, respectively, could already be obtained at an SNR level of 25 dB.  Figures 15-17, respectively, and summarized in Table 5. In this higher-dynamics scenario, an SNR level of at least 30 dB was required to obtain a horizontal accuracy below 20 cm at the 4 and 8 Hz measurement update rates. The obtained vertical accuracy was 15 cm or less. At a measurement update rate of 16 Hz, horizontal and vertical accuracies of 20 and 14 cm, respectively, could already be obtained at an SNR level of 25 dB.

An Inventory Management Scenario
In Reference [69], a four-anchor UWB system ranging up to 125 m at an update rate of 40 Hz was deployed. UWB two-way ranging at 80 Hz was reported in Reference [45]. Six anchors, placed on the ground and the ceiling, in addition to IMU measurements, were used for 3D localization. A range

An Inventory Management Scenario
In Reference [69], a four-anchor UWB system ranging up to 125 m at an update rate of 40 Hz was deployed. UWB two-way ranging at 80 Hz was reported in Reference [45]. Six anchors, placed on the ground and the ceiling, in addition to IMU measurements, were used for 3D localization. A range

An Inventory Management Scenario
In Reference [69], a four-anchor UWB system ranging up to 125 m at an update rate of 40 Hz was deployed. UWB two-way ranging at 80 Hz was reported in Reference [45]. Six anchors, placed on the ground and the ceiling, in addition to IMU measurements, were used for 3D localization. A range measurement accuracy of about 10 cm was achieved over a working range of a few hundred meters. The ranging accuracy had the same level irrespective of the target-anchor distance. UWB two-way ranging schemes were designed in Reference [47] and reached minimum and maximum practical measurement update rates of 62 and 372 Hz, respectively. The positioning system was applied to a 400-m circular track covered by 20 anchors, where reliable ranging up to 80 m and occasionally up to 220 m was obtained.
The settings of this simulation resemble a realistic UAV-based inventory management scenario. Four anchor nodes were placed at (0,0,0), (100,0,15), (100,100,0), and (0,100,15), and a drone moved along several linear paths from position (5,5,0) to position (60,5,0) (Figure 18), at constant velocities of 0.5 m/s in the corresponding direction. TDoA measurements were obtained at a rate of 20 Hz with a total number of 22,658 measurements along the paths. Thus, the traveled distance between any two measurements was 2.5 cm in the corresponding direction. The maximum true distance difference value along the paths was 127.3 m. Therefore, the SNR level was set to 50 dB to yield a maximum standard deviation of the measurement errors of about 40 cm according to Equation (17). measurement accuracy of about 10 cm was achieved over a working range of a few hundred meters. The ranging accuracy had the same level irrespective of the target-anchor distance. UWB two-way ranging schemes were designed in Reference [47] and reached minimum and maximum practical measurement update rates of 62 and 372 Hz, respectively. The positioning system was applied to a 400-m circular track covered by 20 anchors, where reliable ranging up to 80 m and occasionally up to 220 m was obtained. The settings of this simulation resemble a realistic UAV-based inventory management scenario. Four anchor nodes were placed at (0,0,0), (100,0,15), (100,100,0), and (0,100,15), and a drone moved along several linear paths from position (5,5,0) to position (60,5,0) ( Figure 18), at constant velocities of 0.5 m/s in the corresponding direction. TDoA measurements were obtained at a rate of 20 Hz with a total number of 22,658 measurements along the paths. Thus, the traveled distance between any two measurements was 2.5 cm in the corresponding direction. The maximum true distance difference value along the paths was 127.3 m. Therefore, the SNR level was set to 50 dB to yield a maximum standard deviation of the measurement errors of about 40 cm according to Equation (17). The positioning accuracy was investigated with varying resampling ranges from 0.1 to 0.5 m. The RMSE results are plotted with error bars, representing the standard deviation, in Figure 19, and are listed in Table 6. It can be seen from Table 6 that a resampling range, , of 0.3 m delivered the best combination of horizontal and vertical accuracies of 11 and 24 cm, respectively. However, the horizontal accuracy was similar (11-12 cm) over the investigated resampling ranges, and the vertical accuracy was also similar (24-25 cm) over resampling ranges greater than 0.1 m. The error bars in Figure 19 (their values are listed in Table 6) show that the vertical errors fluctuated higher than the horizontal errors. Figure 20 shows the particle filter position estimates of a single simulation run with a resampling range, , of 0.3 m. It can be seen that the particle filter required more time to converge to the correct vertical position, as illustrated in the initial phase. The particle filter also needed time to recover after sudden direction changes. It can also be noted that the vertical position estimates were more accurate and smoother during vertical movements and fluctuated during the horizontal movements. This fluctuation was due to the applied resampling range that dictated considering vertical position candidates above and below the true horizontal path. Therefore, fusing height information from, e.g., a barometer, can refine the value of the resampling range in the vertical dimension and, thus, The positioning accuracy was investigated with varying resampling ranges from 0.1 to 0.5 m. The RMSE results are plotted with error bars, representing the standard deviation, in Figure 19, and are listed in Table 6. It can be seen from Table 6 that a resampling range, R, of 0.3 m delivered the best combination of horizontal and vertical accuracies of 11 and 24 cm, respectively. However, the horizontal accuracy was similar (11-12 cm) over the investigated resampling ranges, and the vertical accuracy was also similar (24-25 cm) over resampling ranges greater than 0.1 m. The error bars in Figure 19 (their values are listed in Table 6) show that the vertical errors fluctuated higher than the horizontal errors. Figure 20 shows the particle filter position estimates of a single simulation run with a resampling range, R, of 0.3 m. It can be seen that the particle filter required more time to converge to the correct vertical position, as illustrated in the initial phase. The particle filter also needed time to recover after sudden direction changes. It can also be noted that the vertical position estimates were more Sensors 2020, 20, 4516 20 of 26 accurate and smoother during vertical movements and fluctuated during the horizontal movements. This fluctuation was due to the applied resampling range that dictated considering vertical position candidates above and below the true horizontal path. Therefore, fusing height information from, e.g., a barometer, can refine the value of the resampling range in the vertical dimension and, thus, smoother vertical position estimates can be obtained during horizontal movements. The 3D, horizontal, and vertical RMSEs, rounded to two significant digits, of the simulation run depicted in Figure 20 were 26, 11, and 24 cm, respectively, i.e., identical to the results over 50 simulation runs listed in Table 6 (column with resampling range of 0.3 m).
To avoid the bias in the RMSE results of Figure 19, it is common to discard 2% of the position estimates with the highest errors due to the convergence and recovery phases [70,71]. The 3D, horizontal, and vertical RMSEs after discarding 1% and 2% of the position estimates with the highest errors were, respectively, 20, 10, and 17 cm, and 19, 10, and 16 cm. Theses accuracies are acceptable for inventory management applications and indicate that the proposed particle filter works well under the assumed measurement update rate and error conditions. smoother vertical position estimates can be obtained during horizontal movements. The 3D, horizontal, and vertical RMSEs, rounded to two significant digits, of the simulation run depicted in Figure 20 were 26, 11, and 24 cm, respectively, i.e., identical to the results over 50 simulation runs listed in Table 6 (column with resampling range of 0.3 m).
To avoid the bias in the RMSE results of Figure 19, it is common to discard 2% of the position estimates with the highest errors due to the convergence and recovery phases [70,71]. The 3D, horizontal, and vertical RMSEs after discarding 1% and 2% of the position estimates with the highest errors were, respectively, 20, 10, and 17 cm, and 19, 10, and 16 cm. Theses accuracies are acceptable for inventory management applications and indicate that the proposed particle filter works well under the assumed measurement update rate and error conditions.    The mean computation time required for a single run of the filter with 1000 particles, T 1000 mean , was estimated on two platforms: (1) an Intel Core i5 central processing unit (CPU) at 2.67 GHz with 8 GB random-access memory (RAM) running MATLAB R2018b, and (2) an Intel Core i7 CPU at 2.2 GHz with 16 GB RAM running MATLAB R2019b. The estimated T 1000 mean values on both platforms were about 20 and 14 ms, i.e., allowing the proposed particle filter to work at about 50 and 70 Hz, respectively, within the MATLAB environment.

Conclusions
The TDoA measurement technique is widely used for position estimation. TDoA-based positioning is a nonlinear estimation problem. A particle filter algorithm was proposed to solve the four-anchor TDoA-based 3D positioning problem. The proposed particle filter was demonstrated via simulation studies for enabling UAV indoor positioning applications. It works with the minimum number of anchor nodes, from the mathematical point of view, required for 3D positioning and, thus, increases the availability of positioning systems, usually employs more than four anchors, in the case of anchor node failures and bad geometric dilution of precision (GDOP) conditions, and reduces the overall system hardware costs. The assumed working conditions can be achieved by UWB wireless technology. Therefore, it is possible to enable UAV positioning in, e.g., large warehouses for inventory management applications without the need for an excessive number of anchor nodes.
The correct arrangement and location calibration of the anchor nodes in the 3D space are important design criteria to get accurate 3D position estimates. Therefore, the arrangement of the anchor nodes has to be optimized to meet the accuracy requirements of the target application.
The simple implementation with any kind of model and noise assumption is the main reason for the wide application of particle filters. If the proposed resampling range, used in the prediction step of the particle filter, is correctly set, it can help to obtain better state predictions than directly integrating acceleration information from an IMU [45]. The particle filter also enables easy incorporation of further useful information, e.g., map data, and plain integration of further sensors, e.g., IMU, barometer, cameras, etc.
UAV indoor applications are increasing rapidly due to the variety of sensors that can be available onboard. Therefore, hybrid and complementary positioning systems should be used with sufficient computing power to tackle difficulties such as velocity estimation, signal multipath effects, and non-line-of-sight (NLoS) propagation errors. Candidate systems include IMUs, cameras,

Conclusions
The TDoA measurement technique is widely used for position estimation. TDoA-based positioning is a nonlinear estimation problem. A particle filter algorithm was proposed to solve the four-anchor TDoA-based 3D positioning problem. The proposed particle filter was demonstrated via simulation studies for enabling UAV indoor positioning applications. It works with the minimum number of anchor nodes, from the mathematical point of view, required for 3D positioning and, thus, increases the availability of positioning systems, usually employs more than four anchors, in the case of anchor node failures and bad geometric dilution of precision (GDOP) conditions, and reduces the overall system hardware costs. The assumed working conditions can be achieved by UWB wireless technology. Therefore, it is possible to enable UAV positioning in, e.g., large warehouses for inventory management applications without the need for an excessive number of anchor nodes.
The correct arrangement and location calibration of the anchor nodes in the 3D space are important design criteria to get accurate 3D position estimates. Therefore, the arrangement of the anchor nodes has to be optimized to meet the accuracy requirements of the target application.
The simple implementation with any kind of model and noise assumption is the main reason for the wide application of particle filters. If the proposed resampling range, used in the prediction step of the particle filter, is correctly set, it can help to obtain better state predictions than directly integrating acceleration information from an IMU [45]. The particle filter also enables easy incorporation of further useful information, e.g., map data, and plain integration of further sensors, e.g., IMU, barometer, cameras, etc.
UAV indoor applications are increasing rapidly due to the variety of sensors that can be available onboard. Therefore, hybrid and complementary positioning systems should be used with sufficient computing power to tackle difficulties such as velocity estimation, signal multipath effects, and non-line-of-sight (NLoS) propagation errors. Candidate systems include IMUs, cameras, ultrasound devices, and signals of opportunity. Thus, with a proper regulatory framework for UAV usage [19] and more advanced UAV technologies, an endless number of more complex applications and sophisticated use cases can be developed and implemented on top of the proposed approach. This implies future works toward integrating IMU and visual information from cameras and considering other practical sources of error such as multipath, NLoS propagation, anchor node clock synchronization, and optimal anchor node placement. Other interesting future work topics include (1) auto-calibration of the anchor nodes to determine their relative positions and reduce installation time in rapid deployment ad hoc applications, (2) formation flight of multi-UAV in a variety of indoor and outdoor environments, and (3) enabling cooperative positioning among UAVs.
Funding: This research received no external funding.