Heading Estimation Using Ultra-Wideband Received Signal Strength and Gaussian Processes

09/10/2021 ∙ by Daniil Lisus, et al. ∙ McGill University 0

It is essential that a robot has the ability to determine its position and orientation to execute tasks autonomously. Heading estimation is especially challenging in indoor environments where magnetic distortions make magnetometer-based heading estimation difficult. Ultra-wideband (UWB) transceivers are common in indoor localization problems. This letter experimentally demonstrates how to use UWB range and received signal strength (RSS) measurements to estimate robot heading. The RSS of a UWB antenna varies with its orientation. As such, a Gaussian process (GP) is used to learn a data-driven relationship from UWB range and RSS inputs to orientation outputs. Combined with a gyroscope in an invariant extended Kalman filter, this realizes a heading estimation method that uses only UWB and gyroscope measurements.

READ FULL TEXT VIEW PDF
POST COMMENT

Comments

There are no comments yet.

Authors

page 6

This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

Execution of robotic tasks relies heavily on the ability to estimate the position and orientation of the robot. In outdoor environments, GPS is a reliable positioning solution. In indoor environments, ultra-wideband (UWB) transceivers are frequently used for positioning [14]. However, the task of attitude estimation, particularly in indoor applications, is still an ongoing problem. Traditional methods use a magnetometer to predict a robot’s orientation based on the local magnetic field. However, this field can be greatly distorted in indoor settings by metallic objects. Another approach, referred to position-aided inertial navigation, involves the use of inertial measurement unit (IMU) and position measurements to solve for the robot’s orientation [5, Chapter 11]. However, heading becomes unobservable and starts to drift during motion with constant linear velocity, including when the robot is stationary or rotating in a fixed position [10]. In this letter, it is experimentally shown that UWB transceivers, commonly used to estimate position in indoor applications, can also be used to provide heading estimates.

UWB transceivers are attractive due to their low power, small size, low cost, decimeter-level ranging accuracy, and ability to act as a communication medium as well as a positioning device [7], when a system of static transceivers, called anchors, is present. The large bandwidth of UWB allows the transceivers to send nanosecond-duration radio impulses, resulting in an increased time resolution and resistance to noise caused by reflected signals [19].

Fig. 1: A sample room setup of a robot equipped with a UWB tag and gyroscope and 5 stationary UWB anchors. The local frame and robot body frame are shown.
Fig. 2: A simulated radiation pattern for an ideal dipole antenna (left) and an ideal helical antenna (right). The directivity value, proportional to the RSS, in dBi is plotted as a function of the angle around the antenna.

Recently, several researchers have aimed to improve the quality of UWB range measurements by utilizing Gaussian Process (GP) models. These models have been used to correct for discrepancies caused by nonuniform radiation patterns of the UWB antenna and by non-line-of-sight communications [7, 15, 8, 18]. Since range measurements are used to estimate position, improved range measurements result in improved positioning accuracy. Existing literature focuses on the single-tag, multi-anchor approach where a single UWB module, called a tag, is mounted on a moving robot and measures the range to multiple stationary UWB modules, called anchors, with known positions. A depiction of this approach is shown in Figure 1. Previous papers, such as [9, 3]

, have investigated predicting the angle of arrival of a received UWB signal. An approach for 2D pose estimation is shown in

[9]. However, the method relies on extracting the channel impulse response, which is not possible with some off-the-shelf UWB modules. Attitude estimation based on GPS received signal strength (RSS) is shown in [6].

Compared to previous papers that use GP models to improve the accuracy of UWB range measurements, this letter explores how to use a GP model, trained using range and RSS inputs and ground truth heading outputs, to provide direct heading estimation of an indoor planar robot. The GP model’s ability to “learn” a relationship between range, RSS, and heading is due to the heading-dependent RSS pattern associated with a UWB antenna [2]. This dependency is visualized in Figure 2 for simulated ideal dipole and helical antennas. The pattern is unique for a given antenna and independent of the transceiver.

The GP model is capable of providing both a heading estimate and a corresponding uncertainty. However, to further enhance heading estimation, the GP model is used in the correction step of an invariant extended Kalman filter (IEKF) [1] that uses a gyroscope, another common sensor, within a prediction step. The proposed approach is capable of heading estimation using only a UWB module and a gyroscope, both of which are typically present in indoor setups that localize using an IMU and range measurements. A visualization of the proposed approach is shown in Figure 3.

Fig. 3: A high-level diagram of the proposed approach. The Gaussian Process (GP) block is pre-trained for a given anchor setup.

The rest of the letter is structured as follows. Section II provides a general overview of GP models and the IEKF. Section III formulates the state to be estimated. Section IV describes the GP implementation used to produce a heading prediction and Section V derives the IEKF filter that couples gyroscope data and the GP model. Finally, Section VI presents the performance of the proposed method on real data and Section VII provides concluding remarks.

Ii Preliminaries

Ii-a Gaussian Process

A GP models a distribution over functions, which can be viewed as an infinite collection of random variables (RVs)

[13]. Specifically, GPs attempt to capture the distribution over possible functions , provided with inputs and outputs . In reality, the measured outputs are noisy and assumed to take the form

(1)

where the noise

is normally distributed with variance

. A GP is completely defined by a mean and covariance function or kernel , which are explicitly stated as

(2)
(3)

Using these expressions, a GP can be written as

(4)

where the distribution of the output of the function is estimated for a given input

. Typically, this value is a scalar and multiple GPs are built to model a vector of output values. The construction of multiple-output GPs greatly increases the complexity and is an active area of research

[17].

A GP model produces predictions by conditioning the query RV corresponding to the test input , on all of the available training RVs, which are the set of measurements corresponding to inputs . The mean and covariance functions of the conditional distribution of are given by

(5)
(6)

where is the covariance matrix, and

is the collection of hyperparameters within the covariance function. The covariance matrix has the form

and is sized according to the number of inputs that and have. For example, .

Training a GP is the act of optimizing

(7)

in order to find the hyperparameters that maximize the log maximum likelihood

, which is the probability that the training inputs

produce the measured training outputs through the modelled functional distribution.

Ii-B Matrix Lie Groups

A matrix Lie group is a set of invertible square matrices that form a group under the operation of matrix multiplication [16]. The matrix Lie algebra associated with a matrix Lie group is denoted as and is the tangent space at the identity element on the matrix Lie group manifold. The exponential map and the logarithm map provide a way to transition between these spaces. The “vee” operator and the “wedge” operator map the matrix Lie algebra to and from the vector space , for the degrees of freedom of the matrix Lie group elements.

Using these operators, it is possible to express an error on a matrix Lie group element as either a left or right multiplication. A left-invariant error, which is used throughout this paper, is of the form , where is a nominal matrix Lie group element and is associated with the error . An error of this form is said to be left-invariant, since and can can be left multiplied by another element without any change in the overall error expression.

Ii-C Invariant Extended Kalman Filter

The IEKF is a variation of the extended Kalman filter (EKF) that can be used to directly estimate elements of matrix Lie groups [1]. The IEKF uses a particular error definition, which often leads to Jacobians that are state-estimate independent. Additionally, the IEKF frequently has enhanced transient performance. In the case of attitude estimation, the IEKF enables estimation of the or matrix Lie group elements directly.

The IEKF is constructed by linearizing the process and measurement models. The linearized process model is of the form

(8)

where , and is white Gaussian process noise with covariance matrix . The linearized measurement model is of the form

(9)

where is a small change in the measurement , and is white Gaussian measurement noise with covariance matrix .

The prediction step of the IEKF is , where is the interoceptive measurement, is the corrected state estimate at , is the nonlinear process model, and is the predicted state at . The predicted covariance is

(10)

The correction step requires the Kalman gain, that being

(11)

Using a left-invariant error definition, the correction step is

(12)

where is the innovation or the difference between the measurement and the expected measurement based on the measurement model and . The corrected covariance is

(13)

The IEKF formulation is presented in a general matrix form, indicated by the bolded letters. However, when applied to an problem, as is done in Section III of this paper, many elements are scalar, and are not bolded.

Iii Problem Definition

The goal is to estimate the discrete-time matrix Lie group state

(14)

where is the 2D attitude of the body frame relative to the local frame . The two frames are visualized in Figure 1. The inverse of the state is simply the transpose of the element, and is written as .

The state has one degree of freedom, the “yaw” angle . The matrix Lie algebra associated with is

(15)

The exponential map for is given by

(16)

Iv Gaussian Process Implementation

In order to produce an estimate for the full element, two separate GPs are trained, one to predict and another to predict . Estimating the element directly avoids issues arising from ambiguities and angle wrap-around. However, one cannot guarantee that the GP predictions will be restricted to , nor that their outputs together satisfy . As such, these “pseudo” sine and cosine predictions of the true and are denoted simply as and , respectively. The range and RSS measurements from each of the 5 anchors in the room are used as inputs for both GPs. A motion capture system provides ground truth heading that is used for the training outputs and for test validation.

Iv-a Kernel Selection and Training

Each of the GPs use the standard squared exponential (SE) kernel for the covariance function, which can be written for two arbitrary inputs and as [4]

(17)

where and are the hyperparameters to be optimized and are known as the characteristic lengthscale and signal variance, respectively [13, Chapter 2.3]. The hyperparameters are optimized using the MATLAB fitrgp library, which selects hyperparameters that minimize the five-fold cross-validation loss using the training data [13, Chapter 5.3]. The SE kernel is chosen after experimentation due to its simplicity and comparable-or-better performance compared to other standard kernels, such as the Matern or ARD kernels.

Iv-B Gaussian Process Output Normalization

The full element can be assembled from the individual and components through the relationship (16). However, since there is no guarantee that the produced element would satisfy the constraint , the outputs of the two GPs are first normalized and then assembled into a proper element using

(18)

where and are GP predictions at timestep .

The normalized prediction is a nonlinear function of the independently predicted sine and cosine terms, each with their respective uncertainties. As such, the uncertainty of the resulting normalized element is approximated through a standard linearization procedure. The GP outputs and are random variables with mean with variance , and mean with variance , respectively. For brevity, the subscript is omitted. Equation (18) can be written as

(19)

The perturbation of the first term in (19) can be approximated with a first-order expansion as

(20)

This expansion can be included in the general perturbation of both sides of (19) as

(21)

The final variance on the normalized element is then

(22)

where . This uncertainty is used in Section V-B.

V Filter Formulation

V-a Process Model

The continuous-time attitude kinematics are given by Poisson’s equation

(23)

where is the angular velocity of the body frame relative to the local frame, resolved in the body frame, and time dependence is dropped for notational convenience.

The interoceptive sensor in this problem, the gyroscope, measures angular velocity subject to noise as

(24)

where is the zero-mean additive noise, is the power spectral density, and is the Dirac delta function. Equation (23) can thus be rewritten as

(25)

In order to achieve the linearized model (8), the left-invariant error definition,

(26)

is used. A left-invariant error is chosen in order to be consistent with the left-invariant measurement model. The rate of change of the error (26) is

(27)

Equation (27) can be linearized by approximating , and simplifying as

(28)

The final continuous-time linearized model is

(29)

where the subscript implies continuous time. The discrete time and matrices needed for the discrete time form (8) can be computed from , , and based on [5, Chapter 3.5.5]. The discrete-time linearized process model, of the form (8), is

(30)

where .

V-B Measurement Model

The GP runs separately from the filter and produces individual independent components of an element. These outputs are then normalized as per (18) to form a full element of the group, which is then used as a heading measurement in the correction step of the filter. The GP also produces a variance on this element as per (22). The discrete-time measurement is then defined as

(31)

where and which is left-invariant according to the definition in Section II-B. This is similar to the error definition used on quaternions in [12] and on matrix Lie group elements in [11]. As such, linearization can be done directly on the measurement model by rewriting both and in terms of the left-invariant error as

(32)

where , allowing the simplification

(33)

This can be linearized by substituting , , , , and ignoring higher order terms to yield

(34)

The innovation in vector space can then be defined following the left error definition as

(35)

where .

Fig. 4: The experimental setup of a Pozyx UWB Developer Tag mounted on a Clearpath Husky mobile robot.
Fig. 5: The training path used for datasets Husky #1 and #2 (blue) and the test path of dataset Husky #2 (red). Green arrows show the starting poses of the robot for both paths.
Fig. 6: The ground truth and (black), overlayed with the GP estimates (dark blue) and their bounds (light blue). Results are produced from the manually collected dataset, with the test set down-sampled and shortened for visual clarity.

Vi Results

The GP and IEKF are tested on real data collected using 5 Pozyx UWB Creator Anchor modules with a Pozyx UWB Developer Tag mounted on a Clearpath Husky mobile robot, as shown in Figure 6. The Pozyx modules use the DW1000 transceiver and a linearly polarized UWB chip antenna. The radiation pattern of the antenna is not provided by the supplier, further motivating the use of a GP. The training data is collected by moving and rotating the robot in an area of approximately for roughly data points. Two test sets are collected in the same area, with care taken to follow a path different from the training data and each other. Ground truth is recorded using a motion capture system to evaluate performance. The two test sets are referred to as the “Husky #1” and “Husky #2” datasets. The training path and one of the test paths is shown in Figure 6.

The results for a “Manual” dataset are also provided for completeness. This dataset is collected by manually moving a Pozyx UWB Developer Tag in an area of approximately for roughly data points. The setup for this dataset is otherwise identical to the Husky ones.

Vi-a Gaussian Process

The numerical results of the raw GP predictions of the components and are shown in Table I. A visualization is shown in Figure 6. Although it is clear that a relationship is learned, the prediction quality is poor. A possible reason is that the Pozyx Developer Tags only provide RSS measurements to the nearest integer value in dBi. In these experiments, the RSS measurements vary by around dBi, meaning that only a few RSS values are fed into the GP. Thus, it is likely that the accuracy could be improved with higher-resolution RSS measurements.

Vi-B Filter

In order to evaluate the proposed framework, three different estimators are considered.

Vi-B1 Dead-reckoning

In order to validate the benefit of adding the GP model as a measurement step in an IEKF with a gyroscope-based prediction, a purely gyroscope-based dead reckoning estimation approach is considered.

Dataset Husky #1 Husky #2 Manual
RMSE 0.52 0.55 0.49
Mean 1.35 1.35 0.79
RMSE 0.55 0.56 0.45
Mean 1.28 1.28 0.89
No. Train Points 17,274 17,274 25,747
No. Test Points 15,737 3,937 71,333
TABLE I: Gaussian Process prediction results.

Vi-B2 Magnetometer Correction

To provide a comparison to more classic indoor heading estimation methods, a magnetometer-based IEKF (mag IEKF) is also run. This filter is identical to the filter outlined in Section V, except that the measurement in the correction step is produced from magnetometer data, as opposed to from a GP estimate. The magnetometer data is calibrated for bias and distortion from each respective training set. The local magnetic field vector is also computed based on each training set independently. The mag IEKF is tested using 100 Monte Carlo runs, where the filter is run on experimental data with a random initial prediction sampled from . The numerical results are presented in Table II

. The poor performance on the Manual dataset is attributed to the significantly closer proximity of the UWB tag, which includes the magnetometer, to the floor and the pipes and other metallic objects below it. This likely impacted the magnetometer measurements by both causing more outliers during the test run and also decreasing the quality of the calibration procedure that was run on the training data.

Vi-B3 Gaussian Process Correction

An IEKF with a GP correction step, as outlined in Section V, is tested on experimental data using 100 Monte Carlo runs with a random initial prediction sampled from . The numerical results for all datasets are presented in Table II.

For the Husky #1 dataset, the filter produces an average RMSE of degrees, with an average bound of degrees. The full Monte Carlo results are visualized in Figure 7. The filter is consistent and recovers from any initial prediction error. The only exception to the filter consistency occurs at approximately s, where the error and Mahalanobis distance both go outside of their respective bounds. At this time, the robot drastically changes its heading and the predictions generated using the gyroscope and the GP are both poor. Similar spikes in the error of the dead reckoning predictions can be observed in the same time period. Although the filter temporarily deviates, consistency is quickly recovered.

Fig. 7: Top: The errors and average bounds of the GP IEKF estimate from 100 Monte Carlo runs. Bottom: The average Mahalanobis distance of the GP IEKF error from 100 Monte Carlo runs plotted with the 1DOF bound (red), and the expected mean distance (grey).
Dataset Husky #1 Husky #2 Manual
GP RMSE (deg) 9.74 9.16 11.45
Steady (deg) 23.49 23.49 22.92
Mag RMSE (deg) 7.90 9.14 27.61
Steady (deg) 20.93 20.91 38.37
Test Time (sec) 386.87 96.42 786.68
TABLE II: Invariant extended Kalman filter results.

A comparison between the estimates produced on the Husky #1 dataset by the various estimators is presented in Figure 8, with Figure 9 showing the absolute value of the error over time. As expected, the dead reckoning prediction suffers from an inability to correct initial errors and slowly drifts even in perfectly initialized scenarios. The mag IEKF slightly outperforms the GP IEKF on the Husky datasets, although the performance is comparable. On the Manual dataset, magnetic field disturbances caused by proximity to the floor likely result in the poor performance of the mag IEKF.

Fig. 8: The error and the bounds produced by dead reckoning (green), the mag IEKF filter (red), and the GP IEKF filter (black) initialized with radian of error.

Vii Conclusion

This letter shows the feasibility of estimating the heading of a vehicle using UWB range and RSS measurements to 5 stationary anchors in a GP model. When combined with a gyroscope in an IEKF framework, the GP estimates are shown to realize convergence from arbitrary initial conditions. The filter produces a heading estimate with an RMSE of approximately degrees and a bound of approximately degrees across three trials. It is hypothesized that with access to higher resolution RSS measurements, the GP estimate and overall heading prediction could be improved.

Fig. 9: The absolute value of the error for perfectly (orange) and imperfectly (green) initialized dead reckoning, and for 100 Monte Carlo averaged mag IEKF (red) and GP IEKF (black).

References

  • [1] A. Barrau and S. Bonnabel (2017) The Invariant Extended Kalman Filter as a Stable Observer. IEEE Transactions on Automatic Control 62 (4), pp. 1797–1812. External Links: Document Cited by: §I, §II-C.
  • [2] J. Chen, D. Raye, W. Khawaja, P. Sinha, and I. Guvenc (2018) Impact of 3D UWB Antenna Radiation Pattern on Air-to-Ground Drone Connectivity. In 2018 IEEE 88th Vehicular Technology Conference, pp. 1–5. External Links: Document Cited by: §I.
  • [3] I. Dotlic, A. Cornell, H. Ma, J. Clancy, and M. McLaughlin (2017) Angle of Arrival Estimation Using Decawave DW1000 Integrated Circuits. In 2017 14th Workshop on Positioning, Navigation and Communications (WPNC), pp. 1–6. External Links: Document Cited by: §I.
  • [4] D. K. Duvenaud (2014-06) Automatic Model Construction with Gaussian Processes. Ph.D. Thesis, University of Cambridge, Cambridge, England, United Kingdom. Cited by: §IV-A.
  • [5] J.A. Farrell (2008) Aided navigation: gps with high rate sensors. McGraw-Hill Education, New York, New York, USA. External Links: ISBN 9780071642668 Cited by: §I, §V-A.
  • [6] J. Gross, Y. Gu, and M. Rhudy (2016) Fixed-wing UAV Attitude Estimation Using Single Antenna GPS Signal Strength Measurements. Aerospace 3 (2). External Links: Document Cited by: §I.
  • [7] A. Ledergerber and R. D’Andrea (2017-08) Ultra-wideband Range Measurement Model with Gaussian Processes. In 2017 IEEE Conference on Control Technology and Applications (CCTA), pp. 1929–1934. External Links: Document Cited by: §I, §I.
  • [8] A. Ledergerber and R. D’Andrea (2018) Calibrating Away Inaccuracies in Ultra Wideband Range Measurements: A Maximum Likelihood Approach. IEEE Access 6, pp. 78719–7873078719–78730. External Links: Document Cited by: §I.
  • [9] A. Ledergerber, M. Hamer, and R. D’Andrea (2019) Angle of Arrival Estimation based on Channel Impulse Response Measurements. In 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 6686–6692. External Links: Document Cited by: §I.
  • [10] M. H. Lee, J. H. Lee, Y. H. Koh, H. G. Park, J. H. Moon, and S. P. Hong (2010) Observability and Estimability Analysis of the GPS and INS in the Vehicle. Journal of Mechanical Systems for Transportation and Logistics 3 (3), pp. 537–551. External Links: Document Cited by: §I.
  • [11] K. Lenac, J. Ćesić, I. Marković, and I. Petrović (2018) Exactly Sparse Delayed State Filter on Lie Groups for Long-term Pose Graph SLAM. The International Journal of Robotics Research 37 (6), pp. 585–610. External Links: Document Cited by: §V-B.
  • [12] S. M. Persson and I. Sharf (2012) Invariant Momentum-tracking Kalman Filter for Attitude Estimation. In 2012 IEEE International Conference on Robotics and Automation, pp. 592–598. External Links: Document Cited by: §V-B.
  • [13] C. E. Rasmussen and C. K. I. Williams (2006) Gaussian Processes for Machine Learning. The MIT Press, Cambridge, Massachusetts, USA. Cited by: §II-A, §IV-A.
  • [14] Z. Sahinoglu, S. Gezici, and I. Guvenc (2008) Ultra-Wideband Positioning Systems: Theoretical Limits, Ranging Algorithms, and Protocols. Cambridge University Press, Cambridge, England, United Kingdom. External Links: Document Cited by: §I.
  • [15] V. Savic, E. G. Larsson, J. Ferrer-Coll, and P. Stenumgaard (2016) Kernel Methods for Accurate UWB-Based Ranging with Reduced Complexity. IEEE Transactions on Wireless Communications 15 (3), pp. 1783–1793. External Links: Document Cited by: §I.
  • [16] J. Sol, J. Deray, and D. Atchuthan (2020) A Micro Lie Theory for State Estimation in Robotics. arXiv:1812.01537v8. Cited by: §II-B.
  • [17] B. Wang and T. Chen (2015)

    Gaussian Process Regression with Multiple Response Variables

    .
    Chemometrics and Intelligent Laboratory Systems 142, pp. 159–165. External Links: Document Cited by: §II-A.
  • [18] W. Zhao, A. Goudar, J. Panerati, and A. P. Schoellig (2020) Learning-based Bias Correction for Ultra-wideband Localization of Resource-constrained Mobile Robots. External Links: 2003.09371 Cited by: §I.
  • [19] L. Zwirello, T. Schipper, M. Harter, and T. Zwick (2012-03) UWB Localization System for Indoor Applications: Concept, Realization and Analysis. Journal of Electrical and Computer Engineering 2012, pp. 1–11. External Links: Document Cited by: §I.