Relax and Recover: Guaranteed Range-Only Continuous Localization

Range-only localization has applications as diverse as underwater navigation, drone tracking and indoor localization. While the theoretical foundations of lateration—range-only localization for static points—are well understood, there is a lack of understanding when it comes to localizing a moving device. As most interesting applications in robotics involve moving objects, we study the theory of trajectory recovery. This problem has received a lot of attention; however, state-of-the-art methods are of a probabilistic or heuristic nature and not well suited for guaranteeing trajectory recovery. In this letter, we pose trajectory recovery as a quadratic problem and show that we can relax it to a linear form, which admits a closed-form solution. We provide necessary and sufficient recovery conditions and in particular show that trajectory recovery can be guaranteed when the number of measurements is proportional to the trajectory complexity. Finally, we apply our reconstruction algorithm to simulated and real-world data.


I. INTRODUCTION
A robot's ability to localize itself accurately is essential for applications such as exploration, rescue and delivery.In many environments, visual (camera-based) simultaneous localization and mapping (SLAM) is the go-to solution for reliable localization [1].However, various settings exist in which visual SLAM is not practical, for example when scanning the environment is impossible (e.g.passive indoor localization) or when the environment does not exhibit enough reliable features (for example under water [2], [3], at high altitudes [4], or in large exhibition-style rooms [5]).In such situations, it is sometimes more feasible to install a few fixed anchors which can provide the robot with distance measurements.Given sparse range measurements from multiple anchors, the robot can calculate its position through multilateration.
While position recovery guarantees exist for traditional lateration in static setups (see Figure 1 (a)), the problem is less understood when the robot is moving.To date, practical systems predominately recover trajectories by coupling partial lateration with filtering techniques.While these approaches lead to good performance, they offer little hope of providing fundamental guarantees for the recovery of the robot's continuous trajectory.In these cases, under which conditions is it possible to uniquely recover the trajectory?Switzerland firstname.lastname@epfl.ch1.Two different approaches for recovering a trajectory r(t) (solid line) from distance measurements (dashed lines) to anchors am.In conventional lateration (a), we recover single points at which we have at least D + 1 distance measurements.The proposed method (b) recovers the continuous representation of the trajectory r(t) from non-synchronized measurements.
We answer this question in the particular setting in which a moving robot obtains range measurements from static and known anchors.In particular, we do not require the measurements to be perfectly synchronized, nor to be uniformly distributed in time.We make the realistic assumption that the robot can only measure one range at a time and limit ourselves to smooth trajectories-in particular, we focus on bandlimited and polynomial trajectories.An example setup is sketched in Figure 1 (b).It is straight-forward to see that traditional lateration cannot provide us with recovery algorithms, and even less with uniqueness guarantees.One can instead resort to trajectory estimation algorithms, which provide either a probabilistic or deterministic description of the continuous trajectory, but no guarantees for perfect recovery exist.
In this paper, we obtain a novel closed-form solution to the trajectory estimation problem, by relaxing the quadratic constraints.In addition, by studying the obtained linear system, we deduce necessary and sufficient conditions for trajectory recovery of the relaxed problem.This also provides a sufficient condition for trajectory recovery of the original (non-relaxed) problem.

II. PROBLEM FORMULATION
To enable us to more accurately compare our contribution with existing techniques, we define our problem setup before discussing related work in the next section.Throughout the paper, we use regular lower case letters for variables (t), regular upper case letters for constants (K), bold lower case letters for column vectors (c) and bold upper case for matrices (C).By D we denote the robot's embedding dimension, which is fixed but in principle could be arbitrary.In this paper, we usually use D = 2.
Our aim is to recover the position, r(t) ∈ R D , of a moving device (e.g. a robot), for t in some given interval, t ∈ I ⊂ R. At a set of time instances {t n : n = 0, . . ., N − 1}, t n ∈ I, we measure the distance from the robot's (unknown) position r n := r(t n ) to one of M fixed anchors.We denote the anchor positions by a m ∈ R D , m = 0, . . ., M − 1, and assume that their locations are known.The measured distances are thus d n = ||r n − a mn ||, where || • || is the Euclidean norm and m n is the index of the anchor used at time n.In practice, we assume that we can measure distances dn corrupted by additive zero-mean Gaussian noise: dn = d n + n , where n ∼ N (0, σ 2 ).
For ease of analysis, we assume that each t n is different; in fact, this is a strength of our formulation as it means measurements from different anchors are not assumed to be synchronized and, since the t n are real numbers, two consecutive t n 's can be arbitrarily close together.
For noisy measurements, the maximum likelihood estimator (MLE) of the device's position, at one time instant t n , is given by the solution to the following optimization problem: (1) In the noiseless case, the optimal rn are solutions to the following system of equations: for n = 0, . . ., N − 1.These problems are non convex and difficult to solve.In the following section, we will review some of the numerous techniques that have been devised to simplify these problems.
III. RELATED WORK Note that although the focus of the paper is localization, we include the SLAM literature, as localization is a core part of SLAM and thus many methods can be transferred from one problem to the other.

A. Basic concepts for range-only localization
A core concept of many range-based localization algorithms is lateration, or how to estimate an object's location from distances to anchor points of known position.
This problem, formalized in (1), is not convex and to date, no algorithm is guaranteed to find the optimal solution.Therefore, (1) is commonly tackled with a standard nonlinear least squares solver, which in general leads to a local minimum.Alternatively, by squaring the two terms inside the brackets, the relaxed problem (dubbed Squared Range Least Squares, SRLS), can be optimally solved as shown in [6].However, the result is no longer the MLE.
An important variation of the classical lateration problem is when the anchor locations are also unknown.Historically, this problem has often been analyzed through Euclidean Distance Matrices (EDMs), which contain the squared distances between points, and exhibit characteristic properties which can be exploited for denoising, completion and point recovery.[7].Although EDMs by definition are more closely related to the SLAM problem, knowledge of anchor locations can be incorporated through the Procrustes transform, or in a semidefinite program, as proposed in [8].

B. Non-parametric trajectory recovery
Using the above methods, a moving object can only be localized at discrete time instances.This imposes a strong requirement on the number of measurements available at each such time instance, and does not ensure consistency between the subsequent position estimates.
Numerous algorithms solve these two issues by combining range measurements with movement estimates from inertial measurement units (IMUs) in standard filtering methods such as particle or Kalman filters [9]- [13].Note that the obtained accuracy depends strongly on the sampling rate at which position updates can be computed, and problems can arise when IMU measurements are delivered at a much higher frequency than other modalities [14].
Sampling rate issues can be solved by continuous-time non-parametric models.A widely used approach [15]- [17] is to impose time consistency using Gaussian processes.Despite the continuous-time paradigm this method is considered similar to the filter based methods, because the objects position is predicted directly from the measurements.This means that no explicit parameter estimation is required.Numerous research efforts have been invested to make these computationally expensive methods more efficient, using for example Bayes trees for incremental reordering and just-intime relinearization [18].

C. Parametric trajectory recovery
In this paper, we aim to recover a parametric model of the robot's position.A number of other works have proposed recovery of parametric trajectory models, predominantly using splines.A comprehensive review of this field is given in [14].
Li et al. [19] solve the classical SLAM problem, replacing the position update of the usual state-space equations with a continuous, parametric trajectory x(t) = F (C k , t).The authors consider polynomial basis functions identical to ours, and update the coefficients C k for sliding time windows at time index k.They use a standard iterative solver which in general converges to a local minimum.Other methods [14], [20], [21] solve the same trajectory estimation problem, parameterizing the trajectory with B-spline basis functions.As B-splines have a local support, they automatically offer more flexibility in fitting complex trajectories without recursively updating the coefficients.
As opposed to these methods solving the more general SLAM framework with arbitrary measurement models, we show that by focusing on the modality of range measurements, a closed-form solution and recovery guarantees can be deduced.
Recently, trajectory estimation has been integrated in the traditional EDM framework with so-called Kinetic EDMs [22], where all points are considered to move on trajectories.In a similar spirit, in this paper we extend the traditional lateration framework to a device moving on a trajectory, however we incorporate knowledge of anchor points and we provide recovery guarantees.

IV. METHOD
In this section, we give an outline of our recovery algorithm.We first introduce the trajectory model and reformulate (2) to include it.Then, we relax the problem by reformulating it into a linear system of equations that can be solved with any linear solver.
We assume that the robot trajectory coordinates belong to some K-dimensional linear space of functions F: where {f k : k = 0, . . ., K − 1} is a basis for F, and the vectors c k ∈ R D are the multidimensional basis coefficients.
In this work, we focus on bandlimited functions and polynomials.Both these models can approximate naturally occurring trajectories well.For example, bandlimited trajectories describe the oscillatory motion of a body around a stationary point.Polynomials cover constant speed motion (K = 1), constant acceleration (e.g.free fall, K = 2) and linearly changing acceleration (K = 3).For more complex trajectories, polynomials are the essential ingredient to the commonly used spline approximation.
For the space of bandlimited functions, we define the basis functions for odd degree K as where τ is the fixed period of the trajectory.For the space of polynomials we simply use the monomial basis f k (t) = t k .We can now reformulate (2) in terms of the coefficients c k .By setting we can express the sampled positions in matrix form: r n = Cf n .The distances thus become d n = ||Cf n − a mn ||.In the noiseless case would like to solve (2), which can be rephrased as the following system of equations: where L is introduced to separate the terms linear in C from the quadratic, non-convex terms in C. A common approach is to make the problem convex, for example via semidefinite relaxation [23].We propose to simply discard the second constraint entirely, which at first sight introduces an additional K(K + 1) independent variables.However, as we will show in Section V, the effective increase is in fact linear in K. Since we drop constraints, we know that even if there exists a unique solution to (4), the solution to the relaxed equation might not be unique.However, if the solution to the relaxed problem is unique, it is also the unique solution to (4).
In order to devise a recovery algorithm, we write the remaining equality constraint of (4) in linear form.To this end, we introduce the vectorized forms vect (C) and Since a mn Cf n is a scalar, it is equal to its trace and thus where the first equality comes from the commutativity of the trace, tr(A B) = tr(AB ), and the second from the fact that tr(A B) = vect (A) vect (B).Similarly, Therefore, the first equality constraint of (4) can be rewritten as where we have introduced Since we only need to recover vect (C), we apply a Singular Value Decomposition (SVD) to T F , which reduces the dimension of the system.As we show in the next section, this does not affect the solution of Ĉ and the dimensionality of T F is reduced to O(K) dimensions.The reduced system becomes which we solve with the right inverse.Here, U ΣV = T F is the SVD decomposition of T F and Σ is an r × r diagonal matrix, with r the rank of T F .This system in (6) can be solved with any linear solver.For clarity, the straight-forward recovery algorithm is summarized in Algorithm 1.Note that we check invertibility before solving the linear system, which is the topic of the next section,.
Based on the known complexities of the most costly components of our algorithm (that is SVD, matrix multiplications and solving the linear system) we calculate that the complexity of our algorithm is O(N 2 K + K 4 ).

V. GUARANTEES
In this section, we present necessary and sufficient conditions for trajectory recovery from the relaxed linear system.Since the unique solution to the relaxed problem is also the solution to the original problem, we obtain a sufficient condition for the original trajectory recovery from (4).
The crucial operation in Algorithm 1 is solving the linear system.This step is equivalent to finding the right inverse of T A U Σ .Note that, since we assume we know the anchor positions perfectly, this matrix is not affected by noise.Thus, the whole analysis of this section is noiseagnostic.We do not, however, consider numerical errors.
First, let us observe that since U Σ is full rank, for T A U Σ to be full rank, we need a) T A to be full column rank, and b) columns of U to be independent of the columns of T A .When T A U Σ is invertible we can recover vect (C) (and V vect (L), which we discard).This means that the smallest number of measurements for which the recovery of C could be possible is DK + r, where r is the rank of T F .Now, maintaining the assumptions of Section IV and adding two additional ones, we can precisely characterize the sets of measurements sufficient for recovery of C. First, we additionally assume that the times t n are generated by some continuous time process.In practice, any non-adversarial times, such as uniform or uniformly random times, can be used.Second, we also assume that no D anchors lie on the same (affine) subspace.This assumption is only slightly stronger than the very common requirement that the anchors span the (extended) space.Randomly placed anchors will satisfy these conditions almost surely.Under these weak assumptions, we can now introduce the main theoretical contribution of this paper.
Theorem 1.Given at least K(D + 2) − 1 measurements (at different times), the matrix C can be uniquely recovered with probability one if where k m is the number of measurements in which the m-th anchor is used.Moreover, if (7) is not satisfied, C cannot be uniquely reconstructed using Algorithm 1.We observe that the probability of recovering C grows with the number of anchors.This is because, the more anchors we use the higher the chance that the measurements spread uniformly and satisfy (7).Lower plot: the number of anchors is set to M = 4 and the trajectory degree varies.For Algorithm 1, (7) states that the minimum number of required measurements is 11, 19 and 27 for trajectory complexities of 3, 5 and 7, respectively.
Intuitively, (7) means that measurements cannot be arbitrarily distributed between anchors.In particular, if an anchor provides more than K measurements, only the first K have an effect on uniqueness.Moreover, unique recovery is not possible with measurements from less than D + 1 anchors.To give a better understanding of this condition, we depict three examples in Figure 2.
Theorem 1 also implies that, in principle, we need only O(DK) measurements to localize, even though we use a relaxation that would normally lead to a quadratic increase in the required data.Even without the relaxation, one would expect to need (D+1)K measurements for recovery.Indeed, to recover a trajectory of complexity K, one can independently localize K points along it, and to localize a single point, we need D + 1 distance measurements.
In the interest of clarity and space, a sketch of the proof of Theorem 1 can be found in the Appendix.The full proof will appear in a more abstract, extended publication that is currently under preparation.
A natural question to ask is, how likely it is to obtain a measurement set sufficient to recover C. Unfortunately, the probability of the set of measurements satisfying Theorem 1 does not seem to have a closed form formula. Fortunately, it depends only on the partition of measurements between the M anchors, so it can be easily calculated numerically by counting partitions that satisfy (7).
Clearly, the probability of recovering C is non decreasing, because adding a new measurement can only increase the rank of T A T F .In practice, we observe that the probability is already large for (D + 2)K − 1 measurements, and grows with the number of measurements to 1.The probability also depends on the degree of the trajectory K and number of anchors (see Figure 3).
Finally, note that, in practice, the matrix might be ill conditioned when the anchors are almost co-linear, or many measurements to the same anchor are taken at almost the same time.

VI. RESULTS
In the previous section, we have established conditions under which unique recovery of C is possible.In this section, we assume that those conditions are satisfied and investigate the trajectory recovery under more realistic, noisy conditions.We test the robustness of our algorithm to noise on simulated and real data.

A. Noise model
As mentioned before, we assume additive Gaussian noise on distance measurements dn = d n + n , where n are i.i.d.random variables, n ∼ N (0, σ).
An immediate problem comes from the fact that we use squared distances for recovery.This means that the additive noise becomes multiplicative and additive: while our recovery method implicitly assumes only additive noise.Indeed, the pseudoinverse, in Algorithm 1, leads to an Ordinary Least Squares (OLS) optimisation, which is the MLE in the presence of additive Gaussian noise.To alleviate this problem, we propose a Weighted Least Squares (WLS) approach.
More precisely, the noisy version of the n-row of ( 5) is If the noise is small, the 2 n /2 term is negligible.The term d n n can be normalized to follow an i.i.d.distribution more closely by dividing both sides of the equation by the measured distances dn : Usually, d n / dn will be closer to one than d n , and thus the weighed linear system is more suited to our model.In practice, to avoid dividing by extremely small numbers, we can add some small regularisation γ to the distance and divide by dn + γ.

B. Simulations
In this section, we report the root squared error E of reconstructing C: where || • || F is the Frobenius norm.For the chosen basis of bandlimited functions, this norm is equivalent to the power of the signal: using power as opposed to energy makes errors comparable between trajectories with different periods.For more intuition, see Figure 4. .Reconstruction from noisy measurements using the weighted system of equations.For clarity, slopes (dark lines) were fitted to the averages over 1000 simulations (light lines).The simulated distances were between 0.1 m and 10 m.Upper plot: the trajectory degree was set to K = 5 and M = 4 anchors were used; the magnitude of the noise is changing.In this setup, the minimum number of measurements required is 19.We can see that the algorithm is robust to noise starting from about 3× oversampling.Lower plot: the trajectory degree was set to K = 3 and noise magnitude was set to σ = 1 m; the number of anchors is changing.The reconstruction error does not depend heavily on the number of anchors, but it has much higher variance for D + 1 = 3 anchors.
In the simulations, we take samples t n uniformly in the interval [0, τ ], and at each time we choose anchors uniformly at random.If we do not use all available measurements, we discard some of them uniformly at random.We fix τ = 2.
In Figure 5, we can see the coefficients reconstruction error obtained using the weighted reconstruction.We can see that the error decreases with oversampling, and the fitted slope is roughly −0.6.This means that for 10× oversampling we get more than a 5× reconstruction improvement.The regular (non-weighted) solver performed similarly, with a smaller improvement from oversampling.

C. Real-world experiments
We test our trajectory estimation algorithm on a dataset provided by Djugash et al. [24].It consists of an autonomous lawnmower moving on a grass field, using ultra-wideband (UWB) signals to 4 stationary anchors for range measurements, and densely sampled kinematic GPS for ground truth.The distance measurements have an average standard deviation of ca.0.5 m, with a tendency to overestimate [24].Fig. 6.Reconstruction accuracy of lawnmower trajectory using period τ = 54 s.In dotted black is the ground truth trajectory GPS), in blue is our reconstruction, and the orange dots show the SRLS estimates.The top row shows different complexities K using all 500 available distance measurements.In the bottom row, we fix K to 5 and drop measurements uniformly at random.The different colors show the reconstructions for different sets of uniformly chosen measurements.
The trajectory completed by the robot does not perfectly fit our models, but we will see that it can be approximated by the bandlimited model.We can estimate its period τ by visual inspection 1 .Using all range 500 measurements, we use Algorithm 1 to estimate the coefficients for different complexities K, and report the obtained trajectories in the first row of Figure 6.We see that the trajectory is well approximated with model degrees of K = 5 or higher.
Next, we fix K to the smallest sensible value for the given trajectory (K = 5) and test the performance of our algorithm when dropping measurements down to the minimum number required to satisfy Theorem 1 (19 measurements).The second row in Figure 6 shows that the obtained reconstruction quality remains satisfactory down to 30 measurements only, and is not so sensitive to the specific distance measurements selected.The estimates start to break down for smaller N ; this is in accordance with our simulations, where we also observed a very noisy regime for few measurements.
For comparison, we also plot localization results with the point-wise SRLS method [6], using the latest distance measurements from D +1 anchors.The trajectory only starts to be recognizable from N = 200 measurements onwards, and the individual estimates are more noisy than ours.

VII. CONCLUSION AND FUTURE WORK
In this work we have proposed a closed-form trajectory estimation method.Even though the method is based on a relaxation, the number of required measurements stays modest.Furthermore, our theory provides recovery guarantees and the framework eliminates the impractical assumption of perfectly synchronized measurements and dense deployment of anchors.We have demonstrated the performance of our method both on simulated and real data, showing in particular the advantage over point-wise lateration.
To the best of our knowledge, this paper presents the first recovery guarantees for trajectory recovery from range measurements.As such, we have focused on the theoretical aspects.In the future, we believe that the model can be extended to build more sophisticated and practically relevant localization algorithms.One natural extension would be to allow for a dynamically changing trajectory model, for example by using the proposed theory in spline approximations, or by dynamically updating the period τ .Finally, it would be interesting to investigate if the reconstruction could be improved with standard linear regression tricks, for example incorporating different regularizations or random projections.

A. Sketch of the proof of Theorem 1
In order to prove Theorem 1, we need to prove that, given the right measurements (as stated in the theorem), the following three conditions are satisfied: 1) T A is full rank, 2) 2K − 1 (independent) columns of T F are independent of the columns of T A , 3) T F has rank 2K − 1.
It is equivalent to prove these conditions for the modified matrices T A and T F created by moving the first K columns of T F to T A .We use the three following facts to prove the above conditions: 1) A polynomial either has a discrete set of zeros or is zero everywhere.In the former case, the polynomial is non zero at a random point with probability 1.
2) The product of two polynomials of degree < K is a polynomial of degree < 2K − 1, and the same holds for bandlimited functions.3) Matrix rank is sub-additive and, if f and g are independent, the matrices f f and gg are independent too.
The first condition is proved from the properties of the determinant of T A , which is a polynomial in its entries.After expanding it in f k , we can apply the first fact giving us a set of algebraic equations that a m must not satisfy.
Due to the second fact, the columns of T F contain polynomials of order up to 2K − 1.Then, the first fact can be applied inductively over (some) columns of T F to obtain the second condition.
Finally, to prove the third condition, we use the second fact to bound the rank from above, and the third fact to bound the rank from below.
Since the first two conditions require the first fact, they are probabilistic in nature, and only the last condition is a purely deterministic result.
Fig.1.Two different approaches for recovering a trajectory r(t) (solid line) from distance measurements (dashed lines) to anchors am.In conventional lateration (a), we recover single points at which we have at least D + 1 distance measurements.The proposed method (b) recovers the continuous representation of the trajectory r(t) from non-synchronized measurements.

Algorithm 1 :
Trajectory Reconstruction Data: Anchor coordinates a m , distance measurements d m , times and anchor indices t n , m n Result: Trajectory coefficients Ĉ, empty if not unique

1 2
vect (L) , where we introduce b n := 1 2 ||a mn || 2 − d 2 n for simplicity.By concatenating the above equations, we obtain b = T A T F vect (

Fig. 2 .
Examples of sufficient and insufficient measurements, with model degree K = 3.(a) Condition (7) is satisfied, but we do not have (D + 2)K − 1 = 11 measurements.(b) We have 11 measurements, but condition(7) is not satisfied because too many of the measurements involve the same anchor.(c) Both conditions are satisfied and the recovery is possible.

Fig. 3 .
Fig.3.Probability of recovering C, with dimension D = 2. Upper plot: the trajectory degree is set to K = 5 and the number of anchors vary.We observe that the probability of recovering C grows with the number of anchors.This is because, the more anchors we use the higher the chance that the measurements spread uniformly and satisfy(7).Lower plot: the number of anchors is set to M = 4 and the trajectory degree varies.For Algorithm 1,(7) states that the minimum number of required measurements is 11, 19 and 27 for trajectory complexities of 3, 5 and 7, respectively.

Fig. 4 .Fig. 5
Fig.4.Visualisation of the distance between trajectories.Original bandlimited trajectory of order K = 7 (dashed blue) and a randomly perturbed trajectory (solid orange).The Frobenius distance between the trajectory coefficients is displayed above each subplot.