Abstract

Precise and seamless positioning is becoming a basic requirement for the Internet of Things (IoT). However, there is a gap for precise positioning in Global Navigation Satellite System- (GNSS-) denied indoor areas. Thus, a multisensor integration system based on ultrawide-band (UWB), inertial navigation system (INS), nonholonomic constraints (NHCs), and Rauch–Tung–Striebel (RTS) smoother is proposed. In this system, the UWB performs as the major precise positioning system, while the INS bridges the UWB-degraded and UWB-denied periods. Meanwhile, the NHC restrains the drifts of INS, while the RTS smoother further upgrades the navigation accuracy. The contributions of this article are as follows. First, it presents the robust least square- (RLS-) based UWB positioning. The proposed method is effective in mitigating the impact of the effect of non-line-of-sight (NLOS), which is one of the most significant error sources for UWB positioning. Second, it derives the mathematical model of the UWB/INS/NHC/RTS integration, which is new compared to the existing approaches. Results illustrate that the proposed system can provide centimeter-level positioning accuracy, millimeter-level velocimetry accuracy, and accuracy of better than 0.05 and 0.15 degrees for horizontal and vertical attitude angles, respectively. Even in the scenario with short-term UWB outages (30 s), simulation results show that the three-dimensional position still can be better than 20 cm. Such accuracy values reach the state-of-the-art for indoor positioning using UWB and INS.

1. Introduction

Location and connection are the two core contents in the Internet-of-Things (IoT) field [1]. Along with the rapid development of IoT applications, seamless positioning theories become more important than ever before. Continuous positioning service is widely needed in outdoor, indoor, and their intersection environments. Therefore, the focus of positioning and navigation is moving from the Global Navigation Satellite System- (GNSS-) based outdoor positioning techniques [2, 3] and the public wireless-based indoor navigation techniques [1, 4, 5] to the multisensor integration algorithms [6–10].

In the past decades, researchers performed many works on improving GNSS positioning performance outdoor. Currently, the multiconstellation GNSSs (mainly GPS, GLONASS, BDS, and Galileo [11] )-based Precise Point Positioning (PPP) and Real-Time Kinematic (RTK)) are adopted to provide users centimeter-lever positioning accuracy under the outdoor environments in both post- and real-time modes [12, 13]. However, because GNSS positioning is based on radio ranging, its precise-positioning performance would be degraded seriously and even destroyed while suffering from satellite signal losses partly or completely [9]. In order to bridge positioning solutions during GNSS signal-outages, researchers proposed to use the integration of GNSS and Inertial Navigation System (INS) [4, 14], in which the advantages of GNSS and INS are utilized and their drawbacks are overcome significantly [15, 16]. In such system, INS will provide users continuously and high-accuracy solutions in the short term during GNSS outage periods by processing only the specific forces and angle increments output from the accelerometers and gyroscopes in an inertial measurement unit (IMU) without using any other external measurements [7]. Therefore, the shortage of GNSS can be ameliorated. Previous works also show that the performance of GNSS can be improved obviously by INS aiding in both open sky environments and challenging conditions [8, 9, 17–19].

However, the INS-aided GNSS methods [4, 8, 18] still cannot provide acceptable positioning solutions for those who stay in indoor environments (i.e., office, underground parking, and market) because of IMU sensors’ drift. In addition, it is a fact that users spend much more time (about 80%) in the indoor environment than outdoors. Therefore, to fill the gap for indoor localization, positioning methods based on low-power wide-area network (LPWAN), wireless local area network (Wi-Fi), Bluetooth low energy (BLE), ZigBee, and radio frequency identification (RFID), especially the Wi-Fi-based methods, are proposed and are widely applied in domains such as IoT, smart wear, pedestrian navigation, and indoor navigation [20]. However, the positioning solutions from those techniques are typically low-accuracy and low-reliability ones. Because the reliability is more important than accuracy for most of the indoor mass-market applications, researchers proposed to integrate these wireless location technologies with other sensors, such as accelerometers, gyroscopes, BLE, and magnetometers [1, 5]. Under those integration systems, the dead recking (DR) algorithm is recommended to improve the reliability and continuity of wireless positioning solutions. Meanwhile, wireless localization methods are used to aid and compensate the drifts of DR. For example, the works from Li et al. [6] illustrate that the indoor navigation performance in terms of accuracy, reliability, and continuity is improved significantly while using the INS/Wi-Fi/magnetometer integration. However, these proposed methods only can provide meter-level to tens of meter-level positioning accuracy. Along with the development of intelligent robots and intelligent storage, precise-positioning solutions are required for indoor applications. Therefore, the ultrawide-band (UWB) positioning system [21, 22] is considered to be applied for the high-accuracy needed applications in indoor environments in recent years due to its high resolution, strong antijamming performance, and low transmit power.

The UWB-based positioning methods can offer decimeter even centimeter positioning solutions majorly because of its ability to precisely measure the time differences of arrival. However, their performances are strongly affected by the non-line-of-sight (NLOS) environments, which leads to large positioning errors [23]. It is also challenging to use standalone UWB to offer precise solutions with high reliability and continuity. Therefore, the INS is adopted to overcome UWB’s drawback. Currently, some works are done on the integration between UWB and INS [24–28]. Wherein Ferreira et al. [27] fuses the UWB positions with that of INS DR by using an extended Kalman filter. Meanwhile, an algorithm based on the identification of NLOS conditions is utilized to improve the positioning accuracy. The results show that better than 1.5 m positioning accuracy in horizontal can be obtained. Xu et al. [26] integrates the UWB and INS based on a robust unbiased finite impulse response filter bank algorithm by employing the Mahalanobis distance between the estimated and actual UWB measurements. To improve the performance of the INS/UWB system, Fan et al. [25] uses the antimagnetic ring and the double-state adaptive Kalman filter to eliminate the outliers from the UWB system in the NLOS environment. Li et al. [20] implement the UWB/INS integration based on the Sage–Husa fuzzy adaptive filter to solve the time-varying noise in complex indoor environments. However, most of these works are only using the horizontal positions of UWB. Both the vertical position component and the velocities are ignored.

In this contribution, the three-dimensional positions and velocities of UWB will be adopted. In addition, it considers the fact that indoor vehicle will keep contact with the ground so that no jump or sideslip happens; thus, almost no velocity exists along with lateral and vertical components of the vehicle. Under this condition, the nonholonomic constraints (NHC) [29], defined as that the velocities in cross-range and vertical directions of the vehicle are zeroes [9, 30], can be adopted to improve the accuracy of UWB/INS integration. Because, the NHC can restrain the drifts of navigation solutions caused by the biases and scale-factors of gyroscopes and accelerometers by estimating the corrections of velocity and attitude, which will make the solutions (i.e., position, velocity, and attitude) of UWB/INS integration more reliable with higher accuracy. Meanwhile, to make the solutions from UWB/INS integration reliable, a robust adaptive Kalman filter [31–35] is applied to reduce the influence of the time-varying measurement noises (caused by multipath effects and the NLOS) and the outliers in measurements. Besides, as is known, the estimation of Kalman filters [34] is calculated by using the measurements before the current epoch. Hence, the accuracy of parameters would become higher along with increasing epochs. To upgrade the accuracy of Kalman filter before the current epoch, a combination of the solutions of the forward Kalman filter and the backward Kalman filter is usually applied. However, such a smoothing process will spend much more time on calculation. To overcome the limitations of the Kalman filter (e.g., to upgrade the positioning accuracy and to reduce the calculation time), a fixed-interval smoother named Rauch–Tung–Striebel (RTS) smoother [31, 36, 37] is adopted in this research. Several contributions are provided in this research comparing with previous works: First, a robust least square (RLS) based on UWB ranging errors and residuals is applied to provide robust UWB position and velocity. The proposed method will be shown to be especially effective in mitigating the outliers and enhancing positioning under NLOS environments, which is one of the most significant error sources for UWB positioning. Second, the NHC is applied to upgrade the performance of UWB/INS integration, and we derive the mathematical models of INS/UWB/NHC integration. Three-dimensional UWB positions and velocities, instead of two-dimensional ones, are utilized. Finally, we propose to employ the RTS smoother to the UWB/INS/NHC integration system. To the best of our knowledge, this article is the first to apply the INS/UWB/NHC/RTS integration method to provide user three-dimensional solutions with higher accuracy in indoor environments, which provides the state-of-the-art performance currently.

Furthermore, to make the proposed method practical in use, we derive all the mathematical models based on the same spatiotemporal datum (i.e., the geodetical coordinate system and the GPS time system). Then, a set of GNSS observations, INS measurements, and UWB data is processed to evaluate the performance of the proposed system. In general, this paper is organized as follows. In Section 2, the theoretical models of UWB positioning and velocimetry based on the RLS estimator, UWB/INS loose integration, NHC aided UWB/INS integration, and UWB/INS/NHC/RTS integration are described in detail. Then, the “Experiment and evaluation” section is followed by the “Conclusion” part.

2. Mathematical Models

To make the organization of this paper clear, the algorithm structure of the proposed UWB/INS/NHC/RTS integration positioning system is depicted in Figure 1. In general, there are six major parts, named as system initialization, INS mechanization, UWB positioning and velocimetry, UWB/INS/NHC integration, feedback, and RTS smoother, respectively.

Figure 1 

Algorithm structure of proposed UWB/INS/NHC/RTS integration; RTS, NHC, and RLS denote, respectively, Rauch–Tung–Striebel smoother, nonholonomic constraint, and robust least square.

In this system, INS is treated as the core positioning method. After initialization, the compensated increments of velocity and angle calculated from accelerometers and gyroscopes are used to update the position, velocity, and attitude in the INS mechanization [38]. Then, the Kalman time update is employed to calculate the predicted variance-covariance of state parameters. Meanwhile, the INS-updated parameters and the predicted variance-covariance are saved, which will be used as input information in the RTS smoother. Afterwards, the time synchronization is operated between INS and UWB to match the high-rate IMU solutions with the low-rate UWB solutions. If no UWB data were available at the current IMU epoch, the algorithm goes back to the next IMU epoch and the INS solutions will be integrated with NHC. If there are UWB solutions, the Kalman measurement update of UWB/INS/NHC integration will be operated. Then, the estimated IMU sensors’ errors feedback to the next IMU epoch and the corresponding estimated parameters and the updated variance-covariance will be saved. Finally, after all of UWB and IMU data are processed completely, the RTS smoother is to run based on the saved information. The details for each part will be described below.

2.1. Attitude Initialization Algorithm

As an autonomous positioning technology, INS needs to be initialized before working. In general, initialization is to set the initial values for position, velocity, attitude, IMU sensor errors (e.g., biases and scale factors), and the a priori variance and a priori noise of the estimated parameters. In this paper, the initial values of position, velocity, and a priori variance will be provided by UWB positioning and velocimetry; the corresponding noise will be derived by accelerometer noises according to the relationship between position and acceleration in the INS mechanization [38]. The initial value of IMU biases, scale factors, and a priori variance and noise will be obtained from the IMU manufacturer. Here, a coarse alignment method is adopted to define the initial value of attitude [38].

The basic principle of coarse alignment is that the measurements of accelerometers and gyroscopes in static would be theoretically the noised gravity and rotation angular velocity of the earth in the body frame (b, forward-right-down, F-R-D). In the local navigation-frame (n, north-east-down, N-E-D), the gravity at a place can be obtained by using the international gravity field model [39] and that of rotation angular velocity can be calculated by using earth rotation angular velocity constant [40]. Then, the theoretical value of gravity and rotation angular velocity can be expressed in terms of the IMU measured gravity and rotation angular velocity after applying an attitude rotation matrix, which can be written as [38]where , N is the total number of IMU epoch), and denote the vector of the average values of the measured gravity and rotation velocity of the earth; the sub-/superscripts i and e denote the inertial-frame and the earth-centered-earth-fixed frame, respectively; and are the vector of the theoretical value of the rotation angular velocity and gravity in n frame; is the attitude rotation matrix that to transform value in b frame into n frame; and generated by using

The detail expressions for symbols in equation (1) can be listed aswhere , B, and are the earth rotation angular velocity constant, latitude, and gravity in the vertical direction, respectively. Then, according to equation (1), the can be written as

According to the relationship between and attitude in terms of the roll (), pitch (), and yaw (), the initial attitude can be obtained bywhere stands for the element of at the (=1, 2, 3) row and the (=1, 2, 3) column; tan and a tan 2 denote arctangent functions. The method for initialization of position and velocity is described following in detail.

2.2. UWB Positioning and Velocimetry

UWB can directly provide tag-base-station range values that can be used to calculate the tag’s position. Meanwhile, to obtain the velocity of tag, a differential operation between the ranges of the same UWB tag-base-station pair at the adjacent UWB epochs can be employed. Then, the positioning and velocimetry of UWB can be expressed bywhere and denote the range and range-rate between the tag and the s^th base station at k epoch; and indicate the position of tag and base station in e frame; and are the velocity of the UWB tag and base station in e frame; denotes the modulus operation. Since equations (6) and (7) are nonlinear, the Taylor expansion before the least square adjustment is used [27]:withwhere is UWB tag’s initial position used in Taylor expansion and is the theoretical distance between the tag and the s^th base station at k epoch calculated by using the initial position. Then, the least square adjustment is applied to estimate positions and velocities bywithwhere , , and are coefficient matrix, weight matrix, and measurement vector, respectively. To reduce the influence of NOLS on UWB positioning accuracy, the ranging noise and residual-based robust weight [41] is further adopted, which can be expressed aswhere () is the standard residual corresponding to residual and is the variance of residual; c is a constant that is usually chosen as c = 1.3–2.0 [41]; denotes the weight of k^th UWB measurement and is the ranging noise of k^th UWB measurement.

2.3. INS Mechanization

After finishing initialization, IMU measurements can be utilized to update position, velocity, and attitude in INS mechanization [42]. Usually, the successive expressions of INS mechanization in n frame can be expressed as [8]where , , and are the differential form of velocity, position, and attitude in terms of direction cosine matrix at IMU measuring center; () is the rotation angular rate of e frame (n frame) with respect to i frame (e frame) projected in n frame; and × denotes vector cross product.

However, one cannot apply the integration on equation (13) directly to obtain the corresponding discrete expressions. The reason for this fact is that the measurements ( and ) of IMU is based on b frame, and the direction of b-frame’s coordinate axes are different according to the definition of b frame. More importantly, the existence of errors in gravity, Coriolis force, rotational motion, and sculling motion makes it impossible to integrate angular rate and linear acceleration concurrently in a digital implementation. Therefore, after considering all of the errors, the discrete expressions for velocity update can be written as [38]withwhere denotes the rotation matrix indicating n-frame transforms from epoch k − 1 to k; is the unit matrix; , , and indicate the rotational motion error, sculling motion error, and error of gravity and Coriolis force; mid denotes the middle time between and ; is the IMU time interval; and are increments of velocity and attitude calculated by the sensor-error-compensated IMU data; and and denote the bias and scale factor of accelerometers (acc) and gyroscopes (gyro).

For the position update, one method is to treat the horizontal positions (latitude and longitude) as two angles and then adopt the quaternion to update them [38]. In this paper, we update the position by

For the attitude update, the attitude quaternion update algorithm is adopted [38]. The mathematical model can be described aswhere is the attitude quaternion, which can be used to calculate , and denotes the j^th (=1, 2, 3, 4) element of ; is the rotation vector corresponding to the n frame at epoch k with respect to the attitude of the n frame at epoch k-1 and is b frame rotation vector. Then, according to equation (5), attitude in terms of the roll, pitch, and yaw can be obtained respectively.

2.4. Kalman Filter Time Update

According to the description in Figure 1, Kalman filter time update works while INS mechanization is finished. The corresponding function can be described as [31]where is the parameter state noise with the corresponding variance of (the elements are determined by IMU performance); is the state transition matrix that is to describe the time-variation of parameters; is the noise-input depended matrix; is the predicted variance of parameters; is the parameter vector that can be given bywhere denotes error correction and is the attitude in terms of roll, pitch, and heading, and other symbols have been defined above.

As shown in equations (18) and (19), the key in Kalman filter time update is to define the state transition matrix (). In this contribution, 1^st Gauss–Markov process [31] is used to describe the behaviors of IMU biases and scale factors, and the classical PSI-angle error model [7, 8, 38] is adopted to present the behaviors of position, velocity, and attitude. Wherein the discrete forms of 1^st Gauss–Markov process for biases and scale factors of accelerometers and gyroscopes can be expressed aswhere and denote the IMU interval and biases correlation time and is the white noise with the a priori variance of that depends on the IMU performance. According to Shin [7, 8, 38], the PSI-angle error model can be defined aswhere all symbols are defined above.

2.5. Kalman Filter Measurement Update

After the Kalman filter time update, time synchronization between UWB solutions and INS is checked. According to different time synchronization checking results, two different integration methods can be used. If there are no UWB solutions, the INS/NHC integration model works; otherwise, the UWB/INS/NHC integration works. However, both integration methods are implemented based on the Kalman filter measurement update [10, 31]. The mathematical function can be written aswhere , , and denote the innovation vector, designed coefficient matrix, and observation noise with the corresponding variance of . Therefore, the key in Kalman filter measurement update is to calculate the innovation vector and the designed coefficient matrix.

2.6. NHC-Aided INS

While no UWB solutions available, the NHC [9, 29, 30] can be used to integrate with INS solutions. Under the assumption that velocity in b frame at right and down directions are zeros,

And, the INS updated velocity in b frame can be written as

Then, the innovation vector can be obtained by making difference between NHC velocities and that of INS updated ones, which can be written aswhere denotes the element of at (=1, 2, 3) line and (=1, 2, 3) column, and is the INS updated velocity in n frame. By making error perturbation on equation (26) around equation (20), we can getwhere denotes the zero vector with dimension and denotes all elements of at (=2, 3) line.

2.7. UWB/INS/NHC Integration

While UWB solutions are available, the position and velocity calculated by UWB and those updated by INS are used to form the innovation vector, which can be expressed aswhere and are the position and velocity calculated by UWB; and refer to the position and velocity at IMU measuring center predicted by INS mechanization [38]; and are the a priori covariance for the position innovation and velocity innovation, which are achieved from the UWB calculations directly; and represent the lever-arm corrections of position and velocity between the UWB tag and IMU center, with the expressions of [9]:where is the lever-arm vector measured accurately in the b frame before the system operating and is the matrix to transform position lever-arm correction in n frame to geodetic coordinate form [38]:where , , and H denote the meridian radius of curvature, the radius of curvature in the prime vertical, and height in the geodetic coordinate system.

Then, according to the information above, the innovation vector of UWB/INS/NHC integration can be described byand the corresponding designed coefficient matrix can be derived aswith

Then, by applying the innovation vector and coefficient matrix mentioned above, the parameter vector can be estimated by the Kalman filter [31]:where is the gain matrix of the Kalman filter.

2.8. Rauch–Tung–Striebel Smoother

The Kalman filter makes use of the information before the current epoch, which makes the estimation accuracy of Kalman filter at the current epoch higher than that before and shows a significant convergence phenomenon in the estimated solutions. Such phenomenon in real-time application is normal. However, for some postprocessing applications (i.e., high-accuracy indoor mapping), users need the solution of each epoch to have higher accuracy. To reduce such convergence fact in forward Kalman filter and upgrade the estimation accuracy before the current epoch, the Rauch–Tung–Striebel smoother [31, 36] is adopted. The algorithm is written aswhere k (=N − 1, N − 2, ..., 0) and N is the total number of IMU measurements.