Tracking of Moving Targets Through Asynchronous Measures

Abstract

Unmanned Aerial Vehicles (UAVs) have progressively gained interest in recent years due to the wide range of related applications, from aerial communications and autonomous flight to agriculture and logistics. However, accurate 3D localization is crucial for enabling these kinds of applications, and commonly used tracking algorithms are often performing unsatisfactorily in critical scenarios like urban canyons and environments, characterized by dense multipath and line of sight obstruction. In this work we derive a novel 3D tracking algorithm which, despite its mathematical simplicity, can efficiently track moving targets handling asynchronous arrival of the anchor measurements or obstructions of line-of-sight links and outperforming commonly used algorithms like the Extended Kalman Filter (EKF) and the Particle Filter (PF). The proposed algorithm tracks the 3D position, velocity, and acceleration of a moving target through the combination of range measurements, between the target and different anchors, which become available in numbers and time instants not necessarily ordered as usually assumed in these applications. We denote this condition as asynchronous measurements, meaning that the ranging measurements are not available from all the anchors and they refer to different positions of the UAV during the tracking. We also show that our estimator is optimal among the linear ones, meaning that within this class, it minimizes the estimation error variance. Finally, we explore the accuracy that can be achieved in simulated scenarios defined by realistic UAV altitudes, velocities, and trajectories, as well as typical ranging errors of wideband localization systems.

1. Introduction

Nowadays accurate 3D localization is crucial to enable many industrial applications, from robotics to agriculture [1,2], surveillance systems, and Unmanned Aerial Vehicles (UAVs). However, the requirements on positioning accuracy are getting more and more restrictive amidst varying and more challenging operational conditions, such as the lack of guaranteed periodic and reliable measurements from all anchors. Our motivation for deriving and exploring a novel 3D tracking algorithm in fact comes from the necessity of addressing some of the impairments the algorithms currently available are not able to effectively deal with. Particularly, in this article, we will focus on UAVs and explore scenarios in which the distance measurements from the anchors could reach the target asynchronously and without guarantee, due for instance to impairments like Non-Line-of-Sight (NLoS) or unreliable links. In the context of UAVs, a study [3], published in 2024, explores how carrier frequency offset, phase noise, and down-tilted base station antennas affect UAV positioning and tracking. Another article [4] addresses the problem of estimating the orientation, position, and linear velocity of a rigid body moving in 3D space with six degrees of freedom. This is achieved by combining inertial motion units and cameras using an Unscented Kalman Filter (UKF) in GPS-denied environments. Two additional studies have focused on the issue of mutual 3D localization between multiple UAVs, also deriving the Cramer–Rao Bound for relative localization accuracy [5,6].

The most widely employed tracking algorithms usually estimate the 3D position through multilateration, eventually combined with angular and power measurements, and rely on Extended Kalman Filters (EKFs) and Particle Filters (PFs) to track the target over time [7,8,9,10]. However, the multilateration process is commonly performed assuming that Time of Arrival (ToA) or other measurements are available at the processing unit in clusters associated with the same time instant or target position. Since a new 3D position update requires a set of 3 or 4 (if target and anchors are mutually asynchronous) ToA measurements, it is usually assumed that the target position does not change during the time required to collect these measurements. Here, we remove this assumption and consider a tracking system in which the measurements associated with different anchors can arrive asynchronously and without a specific order. Thus, each measurement is associated with a different target position, and as a result, the measurements cannot be organized into clusters, which would be necessary to generate a new 3D position update in the tracking process.

To the best of the authors’ knowledge, this scenario is not commonly considered in the literature, except for a few contributions. An article [11] published in 2001 provided an efficient strategy to localize and synchronize the nodes of a wireless sensor network, taking into account both clock offsets and clock drifts. However, this method is designed for static targets, which are not impacted by the issue of asynchronous ranging measures. A study published in 2014 [12] provided an algorithm to combine asynchronous state predictions and updates through EKF and PF. However, this article does not tackle the problem of the unavailability of direct state estimates, which occurs when the system has fewer than 3 reliable range measurements available to perform the update. Another study [13] published in 2021 proved that asynchronous GPS and IMU sensors can be effectively interpolated through a smoothing filter, before the application of a standard EKF. Nevertheless, this study assumes that the anchors (satellites in this case) and thus the corresponding range measurements are synchronous.

In this article, we propose a novel 3D tracking algorithm, denoted as the Asynchronous Tracking Algorithm, that relies on the multilateration of asynchronous anchors. In particular, we derive the best linear estimator for the update of a 3D position given a single new range measurement. This approach can fix many of the issues encountered by other algorithms and, in order to validate it, we will investigate the effect of asynchronous measurements, latency between the anchors, and line-of-sight unavailability for some of the anchors. We believe the proposed algorithm might represent a simple yet effective tool for improving ToA-based 3D positioning accuracy, which will be crucial in many promising fields such as UAV navigation and aerial communications.

The paper is structured as follows: in Section 2, we present our novel algorithm and its derivation, while in Section 3 we briefly discuss two widely employed tracking algorithms based on EKF and PF, which will be used as benchmarks to compare the results. Then, in Section 4, we show and discuss the numerical results, and, finally, Section 5 summarizes the work, its limitations and future steps.

2. Asynchronous Tracking Algorithm

In this paragraph, we derive the novel 3D tracking algorithm. Firstly we define the state vector $\mathbf{q}$ as

$$\mathbf{q}=\left(\begin{array}{c}\mathbf{r}\\ \dot{\mathbf{r}}\\ \ddot{\mathbf{r}}\end{array}\right),$$

(1)

where $\mathbf{r}$ denotes the 3D position of the target. Then, we denote $\widehat{\mathbf{q}}$ and $\mathbf{C}$ as the expectation and covariance matrix of $\mathbf{q}$, respectively, i.e.,

$$\widehat{\mathbf{q}}=\mathbb{E}\left\{\mathbf{q}\right\}$$

(2)

$$\mathbf{C}=\mathbb{E}\left\{\left(\mathbf{q}-\widehat{\mathbf{q}}\right)·{\left(\mathbf{q}-\widehat{\mathbf{q}}\right)}^T\right\}.$$

(3)

The objective of the estimator is to update $\widehat{\mathbf{q}}$ and $\mathbf{C}$ through a single range measurement between the target and an anchor whose position ${\mathbf{r}}_s$ is perfectly known. Multiple range measurements can simply be included by sequentially applying the algorithm for each new measurement.

Defining ${\widehat{\mathbf{q}}}_0$ and ${\mathbf{C}}_0$ as the values of $\widehat{\mathbf{q}}$ and $\mathbf{C}$ immediately before the update through a range measurement, the range $\rho$ between the anchor and the target is given by

$$\rho =|\mathbf{r}-{\mathbf{r}}_s|,$$

(4)

which can be approximated through Taylor’s expansion as

$$\begin{array}{cc}\hfill \rho \sim & |\widehat{\mathbf{r}}-{\mathbf{r}}_s|+{\displaystyle \frac{(\widehat{\mathbf{r}}-{\mathbf{r}}_s)·(\mathbf{r}-\widehat{\mathbf{r}})}{|\widehat{\mathbf{r}}-{\mathbf{r}}_s|}}\hfill \\ \hfill & =|\widehat{\mathbf{r}}-{\mathbf{r}}_s|+{(\mathbf{q}-{\widehat{\mathbf{q}}}_0)}^T·\mathbf{L},\hfill \end{array}$$

(5)

where the $9\times 1$ matrix $\mathbf{L}$ is defined as the Jacobian of $\rho$ with respect to $\mathbf{q}$ i.e.,

$$\mathbf{L}=\left(\begin{array}{c}{\displaystyle \frac{\widehat{\mathbf{r}}-{\mathbf{r}}_s}{|\widehat{\mathbf{r}}-{\mathbf{r}}_s|}}\\ \mathbf{0}\\ \mathbf{0}\end{array}\right).$$

(6)

Now, let us define $\widehat{\rho }$ as the outcome of the range measurement between the anchor and the target, and ${\sigma }_{\rho }$ as the corresponding Root Mean Square Error (RMSE).

Since the range measurement is not correlated with the a priori estimate of $\mathbf{q}$, the best linear estimator ${\widehat{\mathbf{q}}}_1$, after the update through the range measurement, can be found by minimizing the quadratic form $\mathrm{{\rm Y}}\left(\mathbf{q}\right)$ [14], defined as

$$\begin{array}{cc}\hfill \mathrm{{\rm Y}}\left(\mathbf{q}\right)& ={\left(\mathbf{q}-{\widehat{\mathbf{q}}}_0\right)}^T·{\mathbf{C}}_0^{-1}·\left(\mathbf{q}-{\widehat{\mathbf{q}}}_0\right)+{\left({\displaystyle \frac{\rho -\widehat{\rho }}{{\sigma }_{\rho }}}\right)}^2\hfill \\ \hfill & \sim {\left(\mathbf{q}-{\widehat{\mathbf{q}}}_0\right)}^T·{\mathbf{C}}_0^{-1}·\left(\mathbf{q}-{\widehat{\mathbf{q}}}_0\right)\hfill \\ \hfill & +{\left({\displaystyle \frac{|\widehat{\mathbf{r}}-{\mathbf{r}}_s|+{(\mathbf{q}-{\widehat{\mathbf{q}}}_0)}^T·\mathbf{L}-\widehat{\rho }}{{\sigma }_{\rho }}}\right)}^2,\hfill \end{array}$$

(7)

where we have replaced (5). Now, rearranging the terms, we obtain

$$\begin{array}{cc}\hfill \mathrm{{\rm Y}}\left(\mathbf{q}\right)& ={\left(\mathbf{q}-{\widehat{\mathbf{q}}}_0\right)}^T·\left({\mathbf{C}}_0^{-1}+{\displaystyle \frac{\mathbf{L}·{\mathbf{L}}^T}{{\sigma }_{\rho }^2}}\right)·\left(\mathbf{q}-{\widehat{\mathbf{q}}}_0\right)\hfill \\ \hfill & -2·{(\mathbf{q}-{\widehat{\mathbf{q}}}_0)}^T·\left({\displaystyle \frac{\widehat{\rho }-|\widehat{\mathbf{r}}-{\mathbf{r}}_s|}{{\sigma }_{\rho }^2}}\right)·\mathbf{L}\hfill \\ \hfill & +{\left({\displaystyle \frac{\widehat{\rho }-|\widehat{\mathbf{r}}-{\mathbf{r}}_s|}{{\sigma }_{\rho }}}\right)}^2\hfill \end{array}$$

(8)

The linear estimator ${\widehat{\mathbf{q}}}_1$, obtained by zeroing the derivative of the quadratic form $\mathrm{{\rm Y}}\left(\mathbf{q}\right)$ with respect to the vector $\mathbf{q}$ [14], is given by

$${\widehat{\mathbf{q}}}_1={\widehat{\mathbf{q}}}_0+\left({\displaystyle \frac{\widehat{\rho }-|\widehat{\mathbf{r}}-{\mathbf{r}}_s|}{{\sigma }_{\rho }^2}}\right)·{\left({\mathbf{C}}_0^{-1}+{\displaystyle \frac{\mathbf{L}·{\mathbf{L}}^T}{{\sigma }_{\rho }^2}}\right)}^{-1}·\mathbf{L}$$

(9)

and its corresponding covariance matrix ${\mathbf{C}}_1$ [14] is given by

$${\mathbf{C}}_1={\left({\mathbf{C}}_0^{-1}+{\displaystyle \frac{\mathbf{L}·{\mathbf{L}}^T}{{\sigma }_{\rho }^2}}\right)}^{-1}$$

(10)

We remark that, as the best linear estimator is obtained by the introduction of a single new measure, its application during tracking can be implemented without any limitation on the order and the timestamps of the ranging measurements coming from the set of the available anchors; the measurements are handled serially, exactly when they become available at the target or processing unit.

3. Benchmark Tracking Algorithms

In order evaluate and assess the performance of the algorithm illustrated in the previous paragraph, we have employed as benchmarks the Extended Kalman Filter and the Particle Filter.

3.1. Extended Kalman Filter

The EKF algorithm is implemented as described here below:

• First we estimate the non-tracked 3D position of the target as

  $$\widehat{\mathbf{x}}=arg\underset{\mathbf{x}}{min}\sum _{k=1}^{N_A}{\left({\displaystyle \frac{|\mathbf{x}-{\mathbf{x}}_k|-{\rho }_k}{{\sigma }_{{\rho }_k}}}\right)}^2$$

  (11)

  where ${\mathbf{x}}_k$ is the 3D position of the $k^{th}$ anchor, ${\rho }_k$ is the corresponding range and ${\sigma }_{{\rho }_k}$ is the RMSE of the range measurement.
• The covariance matrix of $\widehat{\mathbf{x}}$ is computed through local linearization of (11).
• Finally, the estimates of position, velocity and acceleration are tracked through the widely used EKF [8]. The update and prediction steps are performed when a complete set of ranging measures from all the anchors is available.

3.2. Particle Filter

The PF algorithm is implemented as described in the sequel:

• First, we generate 1000 particles, according to the initial 3D position estimate and covariance matrix.
• At each step, each particle is propagated according to the following motion model

  $$\left\{\begin{array}{c}{\mathbf{a}}^{\prime }=\mathbf{a}+{\sigma }_a\mathbf{n}\hfill \\ {\mathbf{v}}^{\prime }=\mathbf{v}+\mathbf{a}T\hfill \\ {\mathbf{x}}^{\prime }=\mathbf{x}+\mathbf{v}T+{\displaystyle \frac{1}{2}}\mathbf{a}{T}^2\hfill \end{array}\right.$$

  (12)

  where $\mathbf{a}$, $\mathbf{v}$ clearly denote the acceleration and velocity vectors, and $\mathbf{n}$ a 3D white Gaussian noise with unit variance and zero mean.
• After measuring the ranges towards the anchors, for each particle the corresponding weight is computed as

  $$w=exp\left[-{\displaystyle \frac{1}{2}}\sum _{k=1}^{N_A}{\left({\displaystyle \frac{|\mathbf{x}-{\mathbf{x}}_k|-{\rho }_k}{{\sigma }_{{\rho }_k}}}\right)}^2\right]$$

  (13)

  where ${\mathbf{x}}_k$ is the 3D position of the $k^{th}$ anchor, ${\rho }_k$ is the corresponding range and ${\sigma }_{{\rho }_k}$ is the RMSE of the range measurement.
• The distribution of particles is then resampled according to the computed weights.
• The procedure is iterated at each step, in order to track the target over time.

4. Numerical Results

We have simulated the scenario shown in Figure 1, which has been modeled as follows.

• A flying target moving on circular trajectories at fixed altitudes and tangential velocities.
• A set of 6 anchors located at the edges of a regular hexagon of side $d_A=2000 \mathrm{m}$.
• All the anchors are located at an altitude $h_A=25 \mathrm{m}$, similar to the common height of base stations.
• At each step, the target moves and obtains a single ranging measure from one of the anchors. In other words, the measures are arriving serially at the target from the anchors, so the UAV occupies different positions in the trajectory. The target performs its localization and tracking by processing the ranging measures as they become available.
• The ranging error is modeled as Gaussian with zero mean.
• Each anchor has a certain probability of being NLoS (Non-Line-of-Sight) at each step, which prevents the corresponding range from being measured correctly.
• The target is a priori aware of the LoS availability of each anchor. Therefore, it excludes the ranging measures that are affected by an NLoS link.

We compared the results obtained through the proposed algorithm and the benchmarks, i.e., the commonly used EKF and PF. The simulations were performed at different values of target altitude, velocity and Root Mean Square Error (RMSE) of the ranging measures. As depicted in Figures 4–6, we can observe that the proposed algorithm performs better than the EKF and the PF. We can also notice a remarkable difference in the accuracy on the vertical (V) and horizontal (H) components, due to the fact that the target flies relatively close to the horizontal plane spanned by the anchors and thus all the ranging anchors tend to be quasi-horizontal, making the estimate of the horizontal position much more accurate than the vertical one. The results are further commented on in the following subsections.

4.1. Time Evolution of Accuracy

In this section we will illustrate and discuss the evolution of the positioning error over time during the tracking process. The scenario is simulated according to the following assumptions:

• The altitude of the target is $h=100 \mathrm{m}$;
• The velocity of the target is $v=10 \mathrm{m}/\mathrm{s}$;
• The radius of the orbit is $r=1000 \mathrm{m}$;
• The RMSE of range measurements is ${\sigma }_{\rho }=0.1 \mathrm{m}$.
• For all the anchors the LoS probability is $p=100\%$

We can clearly notice in Figure 2 that the proposed algorithm outperforms both the EKF and the PF for both the horizontal and the vertical components. Furthermore, the accuracy achieved by the algorithm does not vary over time, differently from the PF and even more from the EKF, which are sensitive to the geometry of the scenario. Finally, we can see that on the horizontal plane, the PF and the EKF perform very similarly, while in the vertical direction, there is a significant advantage for the EKF, despite its greater simplicity from a computational point of view. For this reason, in the sequel, we will only illustrate and compare the results achieved through the proposed algorithm and the EKF since we have not found any advantages in the application of PF in the considered scenarios.

4.2. Line of Sight Probability

For this section, we have simulated a scenario according to the following assumptions:

• The altitude of the target is $h=100 \mathrm{m}$;
• The velocity of the target is $v=10 \mathrm{m}/\mathrm{s}$;
• The radius of the orbit is $r=1000 \mathrm{m}$;
• The RMSE of range measurements is ${\sigma }_{\rho }=0.1 \mathrm{m}$.
• For all the anchors the LoS probability is $p=70\%$

The LoS probability is introduced in order to take into account impairments like shadowing, channel variability and moving obstacles. At each motion step, for each anchor the LoS availability is randomly generated according to the LoS probability p.

The EKF needs at least 3 anchors to be visible simultaneously in order to compute a new 3D position and update the estimates, while the proposed algorithm employs all the visible anchors singularly and none of the range measurements gets wasted. Therefore, as shown in Figure 3, the performance gap between the algorithm and the benchmark (i.e., the EKF) is remarkable even when the line of sight probability is lowered.

Now, we will assume that all the anchors are visible in order to explore the effect of other parameters on the positioning accuracy.

4.3. Ranging Uncertainty

In this section, we have simulated the scenario at different levels of ranging performance, changing the root mean square ranging error ${\sigma }_{\rho }$, with the following assumptions:

• The altitude of the target is $h=100 \mathrm{m}$;
• The velocity of the target is $v=10 \mathrm{m}/\mathrm{s}$;
• The radius of the orbit is $r=1000 \mathrm{m}$;
• For all the anchors the LoS probability is $p=100\%$.

We can observe in Figure 4 that the proposed algorithm consistently outperforms the Kalman filter when the range measurements are very precise, while the performance gap gets smaller and smaller as ${\sigma }_{\rho }$ increases. This is due to the fact that the distance traveled by the target during the time required to collect all the anchors (which is constant), gets more and more negligible with respect to ${\sigma }_{\rho }$, as ${\sigma }_{\rho }$ increases.

4.4. Velocity

In this section, we have simulated the algorithms for different target velocities, with the following assumptions.

• The altitude of the target is $h=100 \mathrm{m}$;
• The radius of the orbit is $r=1000 \mathrm{m}$;
• The RMSE of range measurements is ${\sigma }_{\rho }=0.1 \mathrm{m}$;
• For all the anchors the LoS probability is $p=100\%$.

As expected, we can observe in Figure 5 that the proposed algorithm performs similarly or slightly better than the Kalman filter at low velocities, while the performance gap gets larger as the velocity increases. This is due to the fact that the proposed algorithm updates the estimate as soon as an anchor measure is received, while the Kalman filter must collect all the measures before updating the position estimate, and during the time required to receive all the anchors, the target moves significantly when the velocity is high enough.

4.5. Altitude

In this section, we have simulated the algorithms for a target at different altitudes, with the following assumptions.

• The velocity of the target is $v=10 \mathrm{m}/\mathrm{s}$;
• The radius of the orbit is $r=1000 \mathrm{m}$;
• The RMSE of range measurements is ${\sigma }_{\rho }=0.1 \mathrm{m}$;
• For all the anchors the LoS probability is $p=100\%$.

As depicted in Figure 6, the performance of the proposed algorithm does not vary significantly with the altitude and outperforms the EKF, which produces outliers, especially at low altitudes. This is due to the fact that all the anchors are located at the same altitude; thus, the positioning error rapidly increases as the target approaches the horizontal plane spanned by the anchors.

5. Conclusions

The proposed 3D tracking algorithm, developed for updating the position for every single ranging measure, can offer significant advantages in critical scenarios and use cases such as a lack of LoS anchors, serial measures, and fast-moving targets. However, accurate range measurements are required in order to observe significant improvements over commonly used algorithms. In standard non-critical scenarios, the proposed algorithm performs similarly to the standard ones; thus, we think it might represent a simple and effective option to tackle the problem of 3D tracking in challenging use cases, especially in the field of UAV applications.

It is worth highlighting that the proposed algorithm has been tested only through simulations, and the effects of some parameters have not been considered. Therefore, future work should include experimental tests to improve the validity and generality of our results, including the integration of estimates from other sensors such as GNSS and inertial navigation systems. Moreover, the algorithm is currently designed for the implementation with Two-Way Ranging systems, and would need to be updated in order to deal with Time Difference of Arrival (DToA) systems as well.