Constrained 1D Localization for Downlink TDoA-Based UWB RTLS ^†

Abstract

The current development of ultra-wide band localization systems focuses on reducing the number of infrastructure nodes (anchors). In certain areas and applications the full three-dimensional position is not necessary; therefore, constraining the solution brings an opportunity to use fewer anchors. In this work, soft constraining of lateral and vertical position components for Time Difference of Arrival positioning in a corridor-like scenario is presented. Implementation in extended and unscented Kalman filter solvers is described. Tests in a real environment suggests that the constraints enable reliable along-track position estimation even with two or three anchors in sight, and the accuracy is better than 30 $\mathrm{c}$$\mathrm{m}$ (RMS). Moreover, the soft nature of constraints allows for uncertainty in the constraint definition.

1. Introduction

Localization and navigation in industrial applications still pose significant challenges. A variety of technologies is available nowadays, providing various levels of performance in terms of accuracy, reliability, and positioning fix frequency [1]. Every technology also brings its advantages and limitations. In particular, Ultra-wide Band (UWB) localization based on impulse radio (IR) is often reported to provide decimeter-level accuracy [2,3,4,5]. On the other hand, deployment and surveying and maintenance naturally come at a cost. Therefore, limiting the number of infrastructure nodes, i.e., anchors, is one of the current trends in real-time localization systems (RTLSs), see, for example, [6]. Currently, single-anchor positioning is pursued, see [7,8,9]; however, utilization of multi-antenna systems capable of angle-of-arrival measurement is assumed for such architecture. Alternatively, multipath propagation may be exploited [10].

In this article we focus on leveraging a few single-antenna anchors with unfavorable constellation geometry to provide useful positioning information to user terminals, i.e., tags. Certain scenarios require accurate localization in one dimension, e.g., position along a track, along a corridor, etc.; the lateral position component is not critical. In this particular case, a LiDAR-SLAM-enabled logistic unmanned ground vehicle (UGV) platform is considered. Typically, a common map is used in a fleet of robots and the robot is able to navigate itself once correctly initialized. This task may be particularly difficult in environments with limited feature availability or where the features are repetitive, and thus initialization based solely on the LiDAR pointcloud becomes unreliable, ambiguous or excessively slow. Unfortunately, such an environment is often found in warehouses, industrial buildings or office buildings.

Another risk to map-based positioning lies in the environment changing in time, e.g., relocating boxes, parking of vehicles, et cetera. Radio-based systems, such as UWB, do not rely on the environment map and thus are less sensitive to its changes.

In certain indoor areas, the UWB-based real-time localization system may already be deployed for other purposes (e.g., asset tracking, safety of workers); however, its coverage may not be sufficient for 2D or 3D localization in the areas where the robotic platforms are operating. For instance, signals from only two anchors may be available.

The constrained one-dimensional localization does not need to provide full (2D or 3D) position information; nonetheless, it should aid in the initialization of the LiDAR-map-based positioning algorithm and serve as a consistency check. In the typical corridor scenario, the lateral position is easily determined from the LiDAR data; however, the accurate along-corridor position is unavailable.

The presented Time Difference of Arrival (TDoA) positioning algorithm utilizes sparsely deployed anchors along the corridor that are synchronized in a wireless manner. The typical synchronization accuracy is around 300 $\mathrm{p}$$\mathrm{s}$ RMS [11]. Such anchors can be either part of an existing RTLS or deployed for a single purpose. Proprietary devices based on the Qorvo DW1000 UWB transceivers were used in this research.

More specifically, we focus on leveraging downlink localization, i.e., the anchors provide precisely timed messages and the tags compute their own position. Such an approach is used, for example, in [5,12,13]. As a consequence, the number of users is unlimited and the position estimate is readily available at the localized platform. Moreover, the tags do not have to transmit messages, which simplifies media access control in the UWB network.

2. Methodology

The motivation is to provide precise positions along an assumed trajectory, e.g., a corridor, with a reduced set of anchors. To counter the low number of available Time Difference of Arrival (TDoA) measurements, assumptions on vertical and lateral position should be leveraged.

Since the LiDAR-enabled robotic platforms utilize a map for the navigation, a truly one-dimensional (1D) localization is impractical. For ease of integration it is preferable to provide the position estimate in the map’s 2D or 3D coordinate system. The following sections describe the difficulties imposed by the limited anchor constellation, the positioning method itself and the application of the assumptions in the form of soft constraints.

2.1. Constellation

Since only a few anchors deployed along the path are utilized, there are certain consequences for the positioning performance. Firstly, there are not enough anchors for full 2D or 3D positioning. Secondly, the geometry of the constellation is extremely poor in terms of dilution of precision (DOP) [14].

The anchors are typically placed at a similar height above the surface, well above the ground and the height where the tags are located. As a consequence, high values of Vertical dilution of precision (VDOP) are encountered, and the vertical accuracy is compromised. Also, since the height of the tags and anchors are different, the vertical component cannot simply be ignored.

Additionally, the anchors are usually placed along the corridor; therefore, the lateral accuracy is poor. In the geometrically simplest case, the anchors would be placed directly above the path of the tags; however, such design is neither achievable nor practical in real scenarios. The placement is often a compromise of the needs of other infrastructure in place, conditions for UWB antenna placement and radio propagation requirements. Consequently, we may assume the tags move along the corridor, e.g., defined by a centerline; however, the anchor placement has both along-track and cross-track components.

Both of the problems are addressed by constraining the solution as described in Section 2.3.

2.2. Positioning Algorithm

The position estimation is performed through Kalman filtering. The state vector to be observed $\mathit{x}$ follows the definition in [5], and contains the three-dimensional position vector $\mathit{r}$, the clock drift $\dot{b}$, and its rate $\ddot{b}$; i.e., the state column vector holds

$$\mathit{x}={\left[\begin{array}{ccc}{\mathit{r}}^{\mathrm{T}}& \dot{b}& \ddot{b}\end{array}\right]}^{\mathrm{T}}.$$

(1)

It is worth noting that the drift is expressed as the time-derivative of clock bias, which is canceled out in the differentiation of the arrival times.

The Kalman filter state prediction is linear and straightforward; an identical position is assumed and the clock drift rate integrates towards the clock drift value. The questionable validity of the temporal state model is compensated by low confidence, expressed by process noise covariance matrix.

The measurement model features hyperbolic functions corresponding to the TDoA measurements; consequently, the measurement model is inherently nonlinear. The TDoA measurement model follows the definition in [5], and takes into account user terminal position $\mathit{r}$ and its clock drift $\dot{b}$. It holds

$$d_{i,j}=\parallel \mathit{r}-{\mathit{r}}_i\parallel -\parallel \mathit{r}-{\mathit{r}}_j\parallel +c_0\dot{b}\left(t_{Tx}^i-t_{Tx}^j\right),$$

(2)

where $d_{i,j}$ is the TDoA measurement from anchors i and j, respectively. Symbols ${\mathit{r}}_i$ and ${\mathit{r}}_i$ denote anchor position vectors and $\left(t_{Tx}^i-t_{Tx}^j\right)$ represent the difference in message transmission times from the synchronized anchors.

In this work we adopted two approaches: Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF), both implemented according to [15]. Both filters were tuned identically. In fact, the only difference is in the variance/covariance propagation in the measurement update step of the filters. While EKF relies on the linearization at the point of prediction, UKF utilizes unscented transformation with $2n$ sigma points of equal weights, where $n=5$ is the number of state vector elements. The sigma points are generated as

$${\mathit{x}}^{\left(i\right)}=\left\{\begin{array}{cc}\mathit{x}+{\left(\sqrt{n\mathbf{P}}\right)}_i,\hfill & i\le n\hfill \\ \mathit{x}-{\left(\sqrt{n\mathbf{P}}\right)}_{i-n},\hfill & n<i\le 2n\hfill \end{array}\right..$$

(3)

2.3. Constraints

Clearly, one or two TDoA measurements with poor geometry are not sufficient for accurate, unambiguous 2D or 3D positioning solutions. The deficiency in measurements is remedied by the introduction of constraints on position.

First, the known or limited height of the user device is leveraged by a soft constraint on the vertical coordinate. Consequently, the poor VDOP implied by the flatness of the anchor constellation is compensated. The constraint is linear and can thus be expressed in the matrix form as a pseudo-measurement, i.e.,

$${\mathbf{H}}_h=\left[\begin{array}{ccccc}0& 0& 1& 0& 0\end{array}\right],{\nu }_h=z_{\mathrm{sc}}-{\mathbf{H}}_h\mathit{x},$$

(4)

where ${\mathbf{H}}_h$ is part of the measurement model matrix, $z_{\mathrm{sc}}$ denotes the assumed height and ${\nu }_h$ is the corresponding innovation of the Kalman filter.

Secondly, when the constellation has poor lateral DOP (in a corridor-like scenario, for example), a constraint on horizontal position coordinates may be introduced. When both lateral and vertical constraints are in effect, the tag could be unambiguously localized with a single TDoA measurement, i.e., with only two anchors in range. The lateral component is constrained, effectively mitigating outliers in the sideways direction (e.g., positioning estimate wandering through the walls is not alleviated).

The constraint itself takes the form of the distance from a line, which should ideally be zero; however, the constraint is implemented in a soft manner and its weight is tuned by a corresponding covariance. In this case, the constraint line resembles the centerline of the corridor. Nonetheless, various constraint lines may be implemented, e.g., a line connecting two selected anchors.

The line is defined with a unit-length direction vector $\mathit{v}$ and a point of intersection $\mathit{p}$. The distance $\mathrm{dist}\left(\mathit{r}\right)$ from the line is [16]

$$\mathrm{dist}\left(\mathit{r}\right)=\sqrt{{\mathit{d}}^{\mathrm{T}}\left(\mathit{r}\right)·\mathit{d}\left(\mathit{r}\right)}.$$

(5)

The distance is computed as the size of difference vector $\mathit{d}$, connecting the position estimate with the closest point on the line

$$\mathit{d}\left(\mathit{r}\right)=(\mathit{p}-\mathit{r})-\left(\right(\mathit{p}-\mathit{r}\left)·\mathit{v}\right)\mathit{v}.$$

(6)

It is important to remark that the distance computation is in 2D space, i.e., in the horizontal plane. The vectors and their interpretation are illustrated in Figure 1.

For the sake of EKF implementation, the partial derivatives of function $\mathrm{dist}\left(\mathit{r}\right)$ w.r.t. the position vector $\mathit{r}$ must be obtained:

$${\displaystyle \frac{\partial \mathrm{dist}\left(\mathit{r}\right)}{\partial \mathit{r}}}={\displaystyle \frac{1}{\mathrm{dist}\left(\mathit{r}\right)}}{\left[\begin{array}{c}(v_x^2-1)d_x+v_x{v}_y{d}_y\\ v_x{v}_y{d}_x+(v_y^2-1)d_y\end{array}\right]}^{\mathrm{T}}.$$

(7)

Function $\mathrm{dist}\left(\mathit{r}\right)$ involves only x and y coordinates, and thus the model Jacobian is equal to

$${\mathbf{H}}_l=\mathrm{sign}({\mathit{d}}_x{\mathit{v}}_y-{\mathit{d}}_y{\mathit{v}}_x)\left[\begin{array}{cccc}{\displaystyle \frac{\partial \mathrm{dist}\left(\mathit{r}\right)}{\partial \mathit{r}}}& 0& 0& 0\end{array}\right],{\nu }_l=-\mathrm{sign}({\mathit{d}}_x{\mathit{v}}_y-{\mathit{d}}_y{\mathit{v}}_x)\mathrm{dist}\left(\mathit{r}\right).$$

(8)

Since the $\mathrm{dist}\left(\mathit{r}\right)$ function’s co-domain is non-negative, the information about the direction towards the line is lost for the UKF. Hence, the distance is made signed; the polarity corresponds to the third component of the cross product $\mathit{d}\times \mathit{v}$. The sign differentiates whether the point is right or left of the horizontal constraint line.

It is necessary to remark that both constraints are implemented as soft, i.e., the uncertainty factor is present and quantified by the respective variances. Consequently, the linear and vertical constraints are not enforced to the full extent, which would lead to projecting the position estimate onto the line at a certain height. The soft constraints pull the position estimate towards the expected line and height. Naturally, the intensity of such pull depends on the TDoA measurement covariance matrix, the a priori state covariance matrix and the constraint variances.

2.4. Evaluation and Ground Truth

The performance was evaluated by a dynamic test in a hallway approximately 45 $\mathrm{m}$ long and 2 $\mathrm{m}$ wide. The corridor is outlined by a thin black line in Figure 2. The UWB transceiver was mounted atop an autonomous ground vehicle platform, with the antenna placed above its coordinate reference point.

The vertical constraint is determined by the placement of the UWB antenna on the robotic platform, in particular its height above the floor level. The constraint height was set to $z_{\mathrm{sc}}=0.45 \mathrm{m}$, with high confidence expressed as ${\sigma }_z=0.08 \mathrm{m}$. The lateral constraint line was chosen to be the centerline of the corridor with confidence expressed as $\sigma =0.6 \mathrm{m}$.

The trajectory provided by the robotic platform was used as a reference. The positioning utilizes LiDAR, odometry and a map pre-surveyed by an identical platform. The position accuracy specifications are unavailable; however, it should be better than the specified stop-position repeatability, i.e., $\pm 7.5 \mathrm{c}\mathrm{m}$ according to [17].

During the test, the platform was stationary close to the right end of the corridor (according to Figure 2) until 25 $\mathrm{s}$. Then the platform rode back and forth through the corridor, with the velocity limited to $0.75$ $\mathrm{m}$/$\mathrm{s}$. The platform rotated 180° at times 80 $\mathrm{s}$ and 150 $\mathrm{s}$. After the 160 $\mathrm{s}$ time mark, the platform was stationary again.

3. Results

The trajectory estimate in the horizontal plane is available in Figure 2. The EKF and UKF provide rather similar, but not identical, results. In particular, the UKF provides slightly more accurate estimates in the middle part of the corridor. The reference position is indicated by the blue line.

Counterintuitively, the worst performance is observed in the vicinity of the anchor half-way through the corridor. Most likely, three factors contribute to the suboptimal performance for the x-axis position values between $-25$ $\mathrm{m}$ and $-18$ $\mathrm{m}$. In that particular area two TDoA measurements are available; however, their covariance is non-zero, since they are both using the message from the middle anchor. Also, a tiny change in either of the TDoA values results in a considerable difference in position estimate, due to the constant-path-difference hyperboloids being near their foci. The antenna pattern also corresponds to the effect, since there is lower gain and likely higher group-delay variation below the anchor. Since both EKF and UKF suffer from similar errors, error in positioning algorithm is less probable.

Quantitatively, the differences between EKF and UKF are marginal as well, as documented by

Table 1. Quantitative results of the test run: mean error (${\mu }_e$), standard deviation (${\sigma }_{ϵ}$) and RMS error; all values are in centimeters.

Algorithm	x-axis			y-axis			2D (Horizontal)			3D (Spatial)
	${\mathit{\mu }}_{\mathit{ϵ},\mathit{x}}$	${\mathit{\sigma }}_{\mathit{ϵ},\mathit{x}}$	${\mathbf{RMS}}_{\mathit{x}}$	${\mathit{\mu }}_{\mathit{ϵ},\mathit{y}}$	${\mathit{\sigma }}_{\mathit{ϵ},\mathit{y}}$	${\mathbf{RMS}}_{\mathit{y}}$	${\mathit{\mu }}_{\mathit{ϵ}\mathbf{,}\mathbf{2}\mathbf{D}}$	${\mathit{\sigma }}_{\mathit{ϵ},\mathbf{2}\mathbf{D}}$	${\mathbf{RMS}}_{\mathbf{2}\mathbf{D}}$	${\mathit{\mu }}_{\mathit{ϵ}\mathbf{,}\mathbf{3}\mathbf{D}}$	${\mathit{\sigma }}_{\mathit{ϵ}\mathbf{,}\mathbf{3}\mathbf{D}}$	${\mathbf{RMS}}_{\mathbf{3}\mathbf{D}}$
EKF	19.69	17.40	26.28	22.14	15.80	27.20	29.62	23.50	37.81	29.80	23.68	38.06
UKF	20.35	16.92	26.47	22.82	13.98	26.76	30.57	21.95	37.63	30.75	22.10	37.87

. Specifically, the RMS values differ by less than $\pm 2\%$. The UKF provides slightly better results in terms of standard deviation (${\sigma }_{ϵ}$); nonetheless, EKF brings marginally lower mean error (${\mu }_{ϵ}$). In both cases, the bias error (indicated by ${\mu }_{ϵ}$) contributes to the overall RMS. Since the moving platform does not follow the constraint line perfectly, such behavior is correct.

In Figure 3 and Figure 4, the time-series of the position error for the EKF and UKF solution is presented. The highest errors occur in time windows from 48 $\mathrm{s}$ to 73 $\mathrm{s}$ and from 105 $\mathrm{s}$ to 130 $\mathrm{s}$, corresponding to the passage of the area near the middle anchor. Nonetheless, the error surfaces mostly in the lateral direction (i.e., y-coordinate in this case), and to some extent in the vertical direction (i.e., z-coordinate) as well. Most importantly, the along-track component remains unaffected.

4. Conclusions

Constraints reduce the number of TDoA measurements required for successful positioning, and therefore the number of anchors. The soft-constraint implementation allows for uncertainty in the model that is quantified by the respective variance. Although the lateral and vertical errors are not fully mitigated by the soft constraints, the along-track position is not compromised. The two presented solutions, EKF and UKF, behave almost identically; the along-track RMS error is $26.28$ $\mathrm{c}$$\mathrm{m}$ and $26.47$ $\mathrm{c}$$\mathrm{m}$, respectively. Since the primary goal is to assist the initialization of LiDAR-based localization in featureless or ambiguous environments, the achieved accuracy is considered sufficient.