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 . 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 . 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 , 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 .
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. 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 .
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 . 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)  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.
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-a Gaussian Process
A GP models a distribution over functions, which can be viewed as an infinite collection of random variables (RVs). 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
where the noise. A GP is completely defined by a mean and covariance function or kernel , which are explicitly stated as
Using these expressions, a GP can be written as
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.
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
where is the covariance matrix, and
is the collection of hyperparameters within the covariance function. The covariance matrix has the formand is sized according to the number of inputs that and have. For example, .
Training a GP is the act of optimizing
in order to find the hyperparameters that maximize the log maximum likelihood
, which is the probability that the training inputsproduce 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 . 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 . 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
where , and is white Gaussian process noise with covariance matrix . The linearized measurement model is of the form
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
The correction step requires the Kalman gain, that being
Using a left-invariant error definition, the correction step is
where is the innovation or the difference between the measurement and the expected measurement based on the measurement model and . The corrected covariance is
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
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
The exponential map for is given by
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 
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
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
The perturbation of the first term in (19) can be approximated with a first-order expansion as
This expansion can be included in the general perturbation of both sides of (19) as
The final variance on the normalized element is then
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
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
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
In order to achieve the linearized model (8), the left-invariant error definition,
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
Equation (27) can be linearized by approximating , and simplifying as
The final continuous-time linearized model is
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
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
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  and on matrix Lie group elements in . As such, linearization can be done directly on the measurement model by rewriting both and in terms of the left-invariant error as
where , allowing the simplification
This can be linearized by substituting , , , , and ignoring higher order terms to yield
The innovation in vector space can then be defined following the left error definition as
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.
In order to evaluate the proposed framework, three different estimators are considered.
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|
|No. Train Points||17,274||17,274||25,747|
|No. Test Points||15,737||3,937||71,333|
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.
|Dataset||Husky #1||Husky #2||Manual|
|Test Time (sec)||386.87||96.42||786.68|
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.
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.
-  (2017) The Invariant Extended Kalman Filter as a Stable Observer. IEEE Transactions on Automatic Control 62 (4), pp. 1797–1812. External Links: Cited by: §I, §II-C.
-  (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: Cited by: §I.
-  (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: Cited by: §I.
-  (2014-06) Automatic Model Construction with Gaussian Processes. Ph.D. Thesis, University of Cambridge, Cambridge, England, United Kingdom. Cited by: §IV-A.
-  (2008) Aided navigation: gps with high rate sensors. McGraw-Hill Education, New York, New York, USA. External Links: Cited by: §I, §V-A.
-  (2016) Fixed-wing UAV Attitude Estimation Using Single Antenna GPS Signal Strength Measurements. Aerospace 3 (2). External Links: Cited by: §I.
-  (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: Cited by: §I, §I.
-  (2018) Calibrating Away Inaccuracies in Ultra Wideband Range Measurements: A Maximum Likelihood Approach. IEEE Access 6, pp. 78719–7873078719–78730. External Links: Cited by: §I.
-  (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: Cited by: §I.
-  (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: Cited by: §I.
-  (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: Cited by: §V-B.
-  (2012) Invariant Momentum-tracking Kalman Filter for Attitude Estimation. In 2012 IEEE International Conference on Robotics and Automation, pp. 592–598. External Links: Cited by: §V-B.
-  (2006) Gaussian Processes for Machine Learning. The MIT Press, Cambridge, Massachusetts, USA. Cited by: §II-A, §IV-A.
-  (2008) Ultra-Wideband Positioning Systems: Theoretical Limits, Ranging Algorithms, and Protocols. Cambridge University Press, Cambridge, England, United Kingdom. External Links: Cited by: §I.
-  (2016) Kernel Methods for Accurate UWB-Based Ranging with Reduced Complexity. IEEE Transactions on Wireless Communications 15 (3), pp. 1783–1793. External Links: Cited by: §I.
-  (2020) A Micro Lie Theory for State Estimation in Robotics. arXiv:1812.01537v8. Cited by: §II-B.
Gaussian Process Regression with Multiple Response Variables. Chemometrics and Intelligent Laboratory Systems 142, pp. 159–165. External Links: Cited by: §II-A.
-  (2020) Learning-based Bias Correction for Ultra-wideband Localization of Resource-constrained Mobile Robots. External Links: Cited by: §I.
-  (2012-03) UWB Localization System for Indoor Applications: Concept, Realization and Analysis. Journal of Electrical and Computer Engineering 2012, pp. 1–11. External Links: Cited by: §I.