# Evaluation on Nonholonomic Constraints and Rauch–Tung–Striebel Filter-Enhanced UWB/INS Integration

Authors: Zhouzheng Gao; Lin Chen; Yu Min; Jie Lv; You Li
Journal: Mathematical Problems in Engineering
 (2020)

---

## 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.


---

## Body

## 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 Section2, 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 Figure1. 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](1)ω˜ibbf˜bκ˜b=Cnbωiengnκn,where ω˜ibb=∑k=0Nωib,kb/N, N is the total number of IMU epoch), and f˜b=∑k=0Nfkb/N 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; ωien and gn are the vector of the theoretical value of the rotation angular velocity and gravity in n frame; Cnb is the attitude rotation matrix that to transform value in b frame into n frame; κn and κ˜b generated by using(2)κnκ˜b=gn×ωienf˜b×ω˜ibb.The detail expressions for symbols in equation (1) can be listed as(3)ωiengnω˜ibbf˜b=ωecosB0−ωesinB00−gω˜ib,Fbω˜ib,Rbω˜ib,Dbf˜Fbf˜Rbf˜Db,where ωe, B, and g are the earth rotation angular velocity constant, latitude, and gravity in the vertical direction, respectively. Then, according to equation (1), the Cbn can be written as(4)Cbn=CnbT=gnTωienTκnT−1Cbnf˜bTω˜ibbTκ˜bT.According to the relationship betweenCbn and attitude in terms of the roll (ϑ), pitch (ϕ), and yaw (ψ), the initial attitude can be obtained by(5)ϑϕψ=−tan−1Cb3,1n1−Cb3,1natan2Cb3,2n,Cb3,3natan2Cb2,1n,Cb1,1n,where Cb,j,kn stands for the element of Cbn at the jth (=1, 2, 3) row and the kth (=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 by(6)dk,s=pUWB,ke−pk,se=xUWB,ke−xk,se2+yUWB,ke−yk,se2+zUWB,ke−zk,se2,(7)d˙k,s=dk−1,s−dk,s=vUWB,ke−vk,se,where dk,s and d˙k,s denote the range and range-rate between the tag and the sth base station at k epoch; pUWBe=xUWBe,yUWBe,zUWBeT and pke=xse,yse,zseT indicate the position of tag and base station in e frame; vUWBe and vke 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]:(8)dk,s−dk,s,0=lk⋅δxUWB,ke+mk⋅δyUWB,ke+hk⋅δzUWB,ke,d˙k,s=lk⋅vUWB,k,0e+mk⋅vy,UWB,ke+hk⋅vz,UWB,ke,with(9)lk=xUWB,k,0e−xk,sedk,s,0,mk=yUWB,k,0e−yk,sedk,s,0,hk=zUWB,k,0e−zk,sedk,s,0,where xUWB,k,0e,yUWB,k,0e,zUWB,k,0eT is UWB tag’s initial position used in Taylor expansion and dk,s,0 is the theoretical distance between the tag and the sth base station at k epoch calculated by using the initial position. Then, the least square adjustment is applied to estimate positions and velocities by(10)X=ATPA−1ATPL,with(11)A=AdAd˙,L=LdLd˙,P=Pd00Pd˙.where A, P, and L 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 as(12)Pd,k,k=Pd,k,k,Vk′≤c,Pd,k,k,cVk′>c,where (Vk′=Vk/σk) is the standard residual corresponding to residual Vk and σk is the variance of residual; c is a constant that is usually chosen as c = 1.3–2.0 [41]; Pd,k,k=1.0/νk2 denotes the weight of kth UWB measurement and νk is the ranging noise of kth 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](13)v˙INSnp˙INSnC˙bn=Cbnfb−2×ωien+ωenn×vINSn+gnvINSnCbnωibb×−ωien+ωenn×Cbn,where v˙INSn, p˙INSn, and C˙bn are the differential form of velocity, position, and attitude in terms of direction cosine matrix at IMU measuring center; ωien (ωenn) 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 (ωibb and fb) 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](14)vINS,kn=vINS,k−1n+0.5⋅Cnk−1nk+I⋅Cbk−1nk−1⋅Δvf,kb+ΔvRotational+Δvsculling+Δvgrav_cor,with(15)ΔvRotationalΔvscullingΔvgrav_cor=Δθω,k×Δvf,kb2Δθω,k−1×Δvf,kb+Δvf,k−1b×Δθω,k12gn−2ωien+ωenn×vINS,k−1nmid⋅Δt,ΔvfbΔθωb=∫tk−1tkfbdt−bacc⋅ΔtI−sacc∫tk−1tkωibbdt−bgyro⋅ΔtI−sgyro,where Cnk−1nk denotes the rotation matrix indicating n-frame transforms from epoch k − 1 to k; I is the unit matrix; ΔvRotational, Δvsculling, and Δvgrav_cor indicate the rotational motion error, sculling motion error, and error of gravity and Coriolis force; mid denotes the middle time between tk and tk−1; Δt is the IMU time interval; Δvfb and Δθωb are increments of velocity and attitude calculated by the sensor-error-compensated IMU data; and b and s 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(16)pINS,kn=pINS,k−1n+vINS,midn⋅Δt.For the attitude update, the attitude quaternion update algorithm is adopted [38]. The mathematical model can be described as(17)qb,kn,k=cos0.5ζk−sin0.5ζk0.5ζk0.5ζk⊗qb,k−1n,k−1⊗cos0.5ςksin0.5ςk0.5ςk0.5ςk,Cb,kn,k=q12−q22−q32+q422⋅q1q2−q3q42⋅q1⋅q3−q2⋅q42⋅q1⋅q2+q3⋅q4q12−q22−q32+q422q2⋅q3+q1⋅q42⋅q1⋅q3−q2⋅q42⋅q2⋅q3+q1⋅q4q12−q22−q32+q42,where qbn is the attitude quaternion, which can be used to calculate Cbn, and qj denotes the jth (=1, 2, 3, 4) element of qbn; ζk=ωien+ωennmid⋅Δt 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 ςk=Δθω,k−1+Δθω,k−1×Δθω,k/12 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 Figure1, Kalman filter time update works while INS mechanization is finished. The corresponding function can be described as [31](18)xk/k−1=Φk/k−1xk−1+μ,μ∼N0,Q,(19)Px,k/k−1=Φk/k−1Px,k−1Φk/k−1T+Γk,k−1Qk−1Γk,k−1T,where μ is the parameter state noise with the corresponding variance of Q (the elements are determined by IMU performance); Φk/k−1 is the state transition matrix that is to describe the time-variation of parameters; Γ is the noise-input depended matrix; Px,k/k−1 is the predicted variance of parameters; xk is the parameter vector that can be given by(20)x=δpINSn,δvINSn,δΨ,δbacc,δbgyro,δsacc,δsgyroT,where δ 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 (Φk/k−1). In this contribution, 1st 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 1st Gauss–Markov process for biases and scale factors of accelerometers and gyroscopes can be expressed as(21)bacc,k=e−τ/Tb,acc⋅bacc,k−1+ηb,acc,k−1,ηb,acc,k−1∼0,2⋅σb,acc2τTb,acc,bgyro,k=e−τ/Tb,gyro⋅bgyro,k−1+ηb,gyro,k−1,ηb,gyro,k−1∼0,2⋅σb,gyro2τTb,gyro,sacc,k=e−τ/Ts,acc⋅sacc,k−1+ηs,acc,k−1,ηs,acc,k−1∼0,2⋅σs,acc2τTs,acc,sgyro,k=e−τ/Ts,gyro⋅sgyro,k−1+ηs,gyro,k−1,ηs,gyro,k−1∼0,2⋅σs,gyro2τTs,gyro,where τ and T 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 as(22)δpINS,knδvINS,knδΨk=I−ωenn×⋅ΔtδpINS,k−1n+δvINS,k−1n⋅ΔtI−ωien+ωinn⋅Δt×δvINS,k−1n+fn×δΨk−1+δgn+Cbnδfb⋅ΔtI−ωinn×⋅ΔtδΨk−1−Cbnδωibb⋅Δt,where 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 as(23)Zk=Hkxk+ηk,ηk∼N0,R,where Z, H, and η denote the innovation vector, designed coefficient matrix, and observation noise with the corresponding variance of R. 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,(24)vNHCb=vRbvDb≈00.And, the INS updated velocity inb frame can be written as(25)vINSb=CbnTvINSn.Then, the innovation vector can be obtained by making difference between NHC velocities and that of INS updated ones, which can be written as(26)ZNHC,k=vNHCb−CnbvINSn=00−Cn,2,1bvNn+Cn,2,2bvEn+Cn,2,3bvDnCn,3,1bvNn+Cn,3,2bvEn+Cn,3,3bvDn,where Cn,j,kb denotes the element of Cnb at jth (=1, 2, 3) line and kth (=1, 2, 3) column, and vINSn=vNn,vEn,vDnT is the INS updated velocity in n frame. By making error perturbation on equation (26) around equation (20), we can get(27)δvNHCb=CnbvINSn−CnbvINSn×δΨ,HNHC,k=01×3Cn,2,1∼3bCn,2,1∼3bvINSn×01×301×301×301×301×3Cn,3,1∼3bCn,3,1∼3bvINSn×01×301×301×301×3,where 01×3 denotes the zero vector with 1×3 dimension and Cn,s,1∼3b denotes all elements of Cnb at sth (=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 as(28)ZUWB,k=pUWBnvUWBn−pINSnvINSn−ΔPιΔvι+ηpηv,ηpηv∼N0,σp20,σv2,where pUWBn and vUWBn are the position and velocity calculated by UWB; pINSn and vINSn refer to the position and velocity at IMU measuring center predicted by INS mechanization [38]; σp2 and σv2 are the a priori covariance for the position innovation and velocity innovation, which are achieved from the UWB calculations directly; Δpι and Δvι represent the lever-arm corrections of position and velocity between the UWB tag and IMU center, with the expressions of [9]:(29)ΔPιΔvι=ϒCbnιbωinn×Cbnιb−Cbnιb×ωibb,where ιb 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]:(30)ϒ=1RM+H0001RN+HcosB000−1,where RM, RN, 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 by(31)ZUWB/NHC,k=pUWBnvUWBn00−pINSn+ϒCbnιbvINSn+ωinn×Cbnιb−Cbnιb×ωibbCn,2,1∼3bvINSnCn,3,1∼3bvINSn,and the corresponding designed coefficient matrix can be derived as(32)HUWB/NHC,k=I3×303×3Cbnιb×03×303×303×303×303×3I3×3Hv,Ψ03×3Cbnιb×03×3Cbnιb×ωibb01×3Cn,2,1∼3bCn,2,1∼3bvINSn×01×301×301×301×301×3Cn,3,1∼3bCn,3,1∼3bvINSn×01×301×301×301×3,with(33)Hv,Ψ=−ωinn×ιb×−ιb×ωibb.Then, by applying the innovation vector and coefficient matrix mentioned above, the parameter vector can be estimated by the Kalman filter [31]:(34)xkPx,k=xk/k−1+KkZk−Hkxk/k−1I−KkHkPx,k/k−1I−KkHkT+KkRkKkT,where Kk 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 as(35)xk/N=xk/k+Px,k/kΦk+1/kTPx,k+1/k−1xk+1/N−xk+1/k,Px,k/N=Px,k/k+Px,k/kΦk+1/kTPk+1/k−1Px,k+1/N−Px,k+1/k−1Px,k/kΦk+1/kTPk+1/k−1T,where k (=N − 1, N − 2, ..., 0) and N is the total number of IMU measurements.


## 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](1)ω˜ibbf˜bκ˜b=Cnbωiengnκn,where ω˜ibb=∑k=0Nωib,kb/N, N is the total number of IMU epoch), and f˜b=∑k=0Nfkb/N 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; ωien and gn are the vector of the theoretical value of the rotation angular velocity and gravity in n frame; Cnb is the attitude rotation matrix that to transform value in b frame into n frame; κn and κ˜b generated by using(2)κnκ˜b=gn×ωienf˜b×ω˜ibb.The detail expressions for symbols in equation (1) can be listed as(3)ωiengnω˜ibbf˜b=ωecosB0−ωesinB00−gω˜ib,Fbω˜ib,Rbω˜ib,Dbf˜Fbf˜Rbf˜Db,where ωe, B, and g are the earth rotation angular velocity constant, latitude, and gravity in the vertical direction, respectively. Then, according to equation (1), the Cbn can be written as(4)Cbn=CnbT=gnTωienTκnT−1Cbnf˜bTω˜ibbTκ˜bT.According to the relationship betweenCbn and attitude in terms of the roll (ϑ), pitch (ϕ), and yaw (ψ), the initial attitude can be obtained by(5)ϑϕψ=−tan−1Cb3,1n1−Cb3,1natan2Cb3,2n,Cb3,3natan2Cb2,1n,Cb1,1n,where Cb,j,kn stands for the element of Cbn at the jth (=1, 2, 3) row and the kth (=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 by(6)dk,s=pUWB,ke−pk,se=xUWB,ke−xk,se2+yUWB,ke−yk,se2+zUWB,ke−zk,se2,(7)d˙k,s=dk−1,s−dk,s=vUWB,ke−vk,se,where dk,s and d˙k,s denote the range and range-rate between the tag and the sth base station at k epoch; pUWBe=xUWBe,yUWBe,zUWBeT and pke=xse,yse,zseT indicate the position of tag and base station in e frame; vUWBe and vke 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]:(8)dk,s−dk,s,0=lk⋅δxUWB,ke+mk⋅δyUWB,ke+hk⋅δzUWB,ke,d˙k,s=lk⋅vUWB,k,0e+mk⋅vy,UWB,ke+hk⋅vz,UWB,ke,with(9)lk=xUWB,k,0e−xk,sedk,s,0,mk=yUWB,k,0e−yk,sedk,s,0,hk=zUWB,k,0e−zk,sedk,s,0,where xUWB,k,0e,yUWB,k,0e,zUWB,k,0eT is UWB tag’s initial position used in Taylor expansion and dk,s,0 is the theoretical distance between the tag and the sth base station at k epoch calculated by using the initial position. Then, the least square adjustment is applied to estimate positions and velocities by(10)X=ATPA−1ATPL,with(11)A=AdAd˙,L=LdLd˙,P=Pd00Pd˙.where A, P, and L 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 as(12)Pd,k,k=Pd,k,k,Vk′≤c,Pd,k,k,cVk′>c,where (Vk′=Vk/σk) is the standard residual corresponding to residual Vk and σk is the variance of residual; c is a constant that is usually chosen as c = 1.3–2.0 [41]; Pd,k,k=1.0/νk2 denotes the weight of kth UWB measurement and νk is the ranging noise of kth 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](13)v˙INSnp˙INSnC˙bn=Cbnfb−2×ωien+ωenn×vINSn+gnvINSnCbnωibb×−ωien+ωenn×Cbn,where v˙INSn, p˙INSn, and C˙bn are the differential form of velocity, position, and attitude in terms of direction cosine matrix at IMU measuring center; ωien (ωenn) 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 (ωibb and fb) 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](14)vINS,kn=vINS,k−1n+0.5⋅Cnk−1nk+I⋅Cbk−1nk−1⋅Δvf,kb+ΔvRotational+Δvsculling+Δvgrav_cor,with(15)ΔvRotationalΔvscullingΔvgrav_cor=Δθω,k×Δvf,kb2Δθω,k−1×Δvf,kb+Δvf,k−1b×Δθω,k12gn−2ωien+ωenn×vINS,k−1nmid⋅Δt,ΔvfbΔθωb=∫tk−1tkfbdt−bacc⋅ΔtI−sacc∫tk−1tkωibbdt−bgyro⋅ΔtI−sgyro,where Cnk−1nk denotes the rotation matrix indicating n-frame transforms from epoch k − 1 to k; I is the unit matrix; ΔvRotational, Δvsculling, and Δvgrav_cor indicate the rotational motion error, sculling motion error, and error of gravity and Coriolis force; mid denotes the middle time between tk and tk−1; Δt is the IMU time interval; Δvfb and Δθωb are increments of velocity and attitude calculated by the sensor-error-compensated IMU data; and b and s 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(16)pINS,kn=pINS,k−1n+vINS,midn⋅Δt.For the attitude update, the attitude quaternion update algorithm is adopted [38]. The mathematical model can be described as(17)qb,kn,k=cos0.5ζk−sin0.5ζk0.5ζk0.5ζk⊗qb,k−1n,k−1⊗cos0.5ςksin0.5ςk0.5ςk0.5ςk,Cb,kn,k=q12−q22−q32+q422⋅q1q2−q3q42⋅q1⋅q3−q2⋅q42⋅q1⋅q2+q3⋅q4q12−q22−q32+q422q2⋅q3+q1⋅q42⋅q1⋅q3−q2⋅q42⋅q2⋅q3+q1⋅q4q12−q22−q32+q42,where qbn is the attitude quaternion, which can be used to calculate Cbn, and qj denotes the jth (=1, 2, 3, 4) element of qbn; ζk=ωien+ωennmid⋅Δt 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 ςk=Δθω,k−1+Δθω,k−1×Δθω,k/12 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 Figure1, Kalman filter time update works while INS mechanization is finished. The corresponding function can be described as [31](18)xk/k−1=Φk/k−1xk−1+μ,μ∼N0,Q,(19)Px,k/k−1=Φk/k−1Px,k−1Φk/k−1T+Γk,k−1Qk−1Γk,k−1T,where μ is the parameter state noise with the corresponding variance of Q (the elements are determined by IMU performance); Φk/k−1 is the state transition matrix that is to describe the time-variation of parameters; Γ is the noise-input depended matrix; Px,k/k−1 is the predicted variance of parameters; xk is the parameter vector that can be given by(20)x=δpINSn,δvINSn,δΨ,δbacc,δbgyro,δsacc,δsgyroT,where δ 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 (Φk/k−1). In this contribution, 1st 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 1st Gauss–Markov process for biases and scale factors of accelerometers and gyroscopes can be expressed as(21)bacc,k=e−τ/Tb,acc⋅bacc,k−1+ηb,acc,k−1,ηb,acc,k−1∼0,2⋅σb,acc2τTb,acc,bgyro,k=e−τ/Tb,gyro⋅bgyro,k−1+ηb,gyro,k−1,ηb,gyro,k−1∼0,2⋅σb,gyro2τTb,gyro,sacc,k=e−τ/Ts,acc⋅sacc,k−1+ηs,acc,k−1,ηs,acc,k−1∼0,2⋅σs,acc2τTs,acc,sgyro,k=e−τ/Ts,gyro⋅sgyro,k−1+ηs,gyro,k−1,ηs,gyro,k−1∼0,2⋅σs,gyro2τTs,gyro,where τ and T 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 as(22)δpINS,knδvINS,knδΨk=I−ωenn×⋅ΔtδpINS,k−1n+δvINS,k−1n⋅ΔtI−ωien+ωinn⋅Δt×δvINS,k−1n+fn×δΨk−1+δgn+Cbnδfb⋅ΔtI−ωinn×⋅ΔtδΨk−1−Cbnδωibb⋅Δt,where 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 as(23)Zk=Hkxk+ηk,ηk∼N0,R,where Z, H, and η denote the innovation vector, designed coefficient matrix, and observation noise with the corresponding variance of R. 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,(24)vNHCb=vRbvDb≈00.And, the INS updated velocity inb frame can be written as(25)vINSb=CbnTvINSn.Then, the innovation vector can be obtained by making difference between NHC velocities and that of INS updated ones, which can be written as(26)ZNHC,k=vNHCb−CnbvINSn=00−Cn,2,1bvNn+Cn,2,2bvEn+Cn,2,3bvDnCn,3,1bvNn+Cn,3,2bvEn+Cn,3,3bvDn,where Cn,j,kb denotes the element of Cnb at jth (=1, 2, 3) line and kth (=1, 2, 3) column, and vINSn=vNn,vEn,vDnT is the INS updated velocity in n frame. By making error perturbation on equation (26) around equation (20), we can get(27)δvNHCb=CnbvINSn−CnbvINSn×δΨ,HNHC,k=01×3Cn,2,1∼3bCn,2,1∼3bvINSn×01×301×301×301×301×3Cn,3,1∼3bCn,3,1∼3bvINSn×01×301×301×301×3,where 01×3 denotes the zero vector with 1×3 dimension and Cn,s,1∼3b denotes all elements of Cnb at sth (=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 as(28)ZUWB,k=pUWBnvUWBn−pINSnvINSn−ΔPιΔvι+ηpηv,ηpηv∼N0,σp20,σv2,where pUWBn and vUWBn are the position and velocity calculated by UWB; pINSn and vINSn refer to the position and velocity at IMU measuring center predicted by INS mechanization [38]; σp2 and σv2 are the a priori covariance for the position innovation and velocity innovation, which are achieved from the UWB calculations directly; Δpι and Δvι represent the lever-arm corrections of position and velocity between the UWB tag and IMU center, with the expressions of [9]:(29)ΔPιΔvι=ϒCbnιbωinn×Cbnιb−Cbnιb×ωibb,where ιb 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]:(30)ϒ=1RM+H0001RN+HcosB000−1,where RM, RN, 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 by(31)ZUWB/NHC,k=pUWBnvUWBn00−pINSn+ϒCbnιbvINSn+ωinn×Cbnιb−Cbnιb×ωibbCn,2,1∼3bvINSnCn,3,1∼3bvINSn,and the corresponding designed coefficient matrix can be derived as(32)HUWB/NHC,k=I3×303×3Cbnιb×03×303×303×303×303×3I3×3Hv,Ψ03×3Cbnιb×03×3Cbnιb×ωibb01×3Cn,2,1∼3bCn,2,1∼3bvINSn×01×301×301×301×301×3Cn,3,1∼3bCn,3,1∼3bvINSn×01×301×301×301×3,with(33)Hv,Ψ=−ωinn×ιb×−ιb×ωibb.Then, by applying the innovation vector and coefficient matrix mentioned above, the parameter vector can be estimated by the Kalman filter [31]:(34)xkPx,k=xk/k−1+KkZk−Hkxk/k−1I−KkHkPx,k/k−1I−KkHkT+KkRkKkT,where Kk 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 as(35)xk/N=xk/k+Px,k/kΦk+1/kTPx,k+1/k−1xk+1/N−xk+1/k,Px,k/N=Px,k/k+Px,k/kΦk+1/kTPk+1/k−1Px,k+1/N−Px,k+1/k−1Px,k/kΦk+1/kTPk+1/k−1T,where k (=N − 1, N − 2, ..., 0) and N is the total number of IMU measurements.


## 3. Experiment, Evaluation, and Discussion

In order to validate the performance of the proposed system, a field test is arranged and the data are analyzed, which will be described in this section in detail.


### 3.1. Data Collection and Processing Methods

A set of GNSS observations, INS measurements, and UWB data collected around a square in a residential area is processed and analyzed. In this test, a multiconstellation GNSS receiver, a tactical-grade IMU, and a UWB tag are fixed together on a handcart. Wherein, the data rate of GNSS, IMU, and UWB are 1 Hz, 200 Hz, and 1 Hz, respectively. The instability of biases and scale factors of gyroscopes and accelerometers are 0.5°/h and 250 mGal. The random walk noises of angle and velocity are 0.03°/√h and 0.05 m/s/√h, respectively.The scene of this test is shown in Figure2. As is shown, it is almost an open sky environment. Under such a condition, the GNSS can be tracked continuously, which makes it possible that the solutions calculated by GNSS RTK/INS integration can be utilized as the reference values for evaluating the performance of UWB/INS/NHC integration. To make the accuracy of reference values high enough, the GNSS base station is just hundreds of meters away. Besides, the level-arms of UWB tag and the receiver antenna of GNSS rover compared to IMU center in IMU body frame are measured precisely before the test operating, which can be used to transform the solutions from different modes to a reference point (i.e., IMU center). Then, the performance of the proposed mode can be evaluated reasonably. Meanwhile, a total station (top-left corner in Figure 2) is used to determine the coordinate of each UWB base station under the Gauss coordinate system (north-east-vertical). Then, these base station coordinates are transformed into the WGS-84 coordinate system which is to match the coordinate system adopted by the proposed algorithm.Figure 2
Experiment scene of the GNSS/INS/UWB integration test.During the whole test (lasting about 2200 seconds), the handcart (shown in Figure2) is pushed along an approximate ‘#‘-shaped route with a velocity of within ± 0.8 m/s, which can be seen from Figures 3 and 4. The attitude angles of the handcart that changed along the motion are depicted in Figure 4(b). To simulate UWB’s application situation indoor, we only move the handcart in the area made up of the six UWB base stations (marked as 1, 2, 3, 4, 6, and 7 as shown Figure 3(a)). Meanwhile, the number 5 UWB station is placed in the middle of this area to make sure more UWB measurements can be collected while handcart moving. To evaluate the NLOS effect on UWB, several testers walk around base stations randomly. According to the tracked UWB base stations plotted in Figure 3(b), the average number of available UWB stations is 5.8, and the corresponding dilution of precision in horizontal (HDOP) and vertical (VDOP) is 1.1 and 8.7, respectively, in terms of root-mean-squared (RMS) value. Such a condition will make the positioning accuracy in horizontal higher than those in vertical. According to the velocity solutions, it can be known that the handcart stays static for about 15 minutes, which is to provide enough static data for the attitude initialization. Then, the collected data were processed majorly by five different modes, namely, UWB positioning, INS/NHC integration, INS/NHC/RTS integration, UWB/INS/RTS integration, and UWB/INS/NHC/RTS integration. The differences between the results from these different modes and the reference values are used to show the performance of the proposed method.Figure 3
Three-dimensional test trajectory (a) and the number/HDOP/VDOP of visible UWB base stations during the test (b).
(a)(b)Figure 4
Velocity (a) and attitude (b) during the test calculated by UWB/INS/NHC/RTS integration mode.
(a)(b)


### 3.2. Robust LS-Enhanced UWB Positioning

Firstly, the positioning accuracy of UWB with and without a robust algorithm is analyzed. As is known, the quality of UWB measurements will be influenced by the NLOS effect. Accordingly, the UWB positioning accuracy will be degraded while there are NLOS effects. Shown in Figure5 are the range measurements from the UWB tag to each base station and the corresponding measuring error of each range measurement. According to Figure 5(a), it can be seen that UWB ranging would be low quality and even fail when suffering from a strong NLOS effect. Besides, measuring errors in Figure 5(b) also provide similar conclusions. Therefore, the robust least square adjustment is employed in UWB positioning to reduce the impact of NLOS on the positioning accuracy.Figure 5
Measurements (a) and corresponding noise (b)) of each UWB tag-base-station pair (1, 2, …, 7 denote UWB base station mark).
(a)(b)Figure6(a) shows the UWB positioning offsets (in n frame) using robust least square adjustment by comparing with the RTK/INS integration solutions. Significantly, the positioning accuracy in horizontal is much higher (several times) than that in the vertical component. According to statistics, the RMS in the north, east, and down components are 7.6 cm, 7.0 cm, and 52.6 cm, respectively, and the minimum/maximum values for the three directions are −54/+23 cm, −51/+27 cm, and −1161/+277 cm. When comparing Figures 6(a) and 3(b), the change of positioning accuracy also presents a strong relationship along with the change of available UWB number, HDOP, and VDOP. It is because the geometry structure between the UWB tag and base stations in horizontal is much stronger than that in vertical. It is also the main reason why previous works were hard to provide precise position accuracy in vertical by UWB.Figure 6
UWB positioning errors of robust least square (a) and the position improvements caused the robust algorithm (b).
(a)(b)The improvements benefitting from robust estimation are shown in Figure6(b). Improvements of 18 cm, 11 cm, and 202 cm can be obtained after applying the RLS algorithm in UWB positioning with the percentages of 70.3%, 611%, and 79.2% in the north, east, and down components. This outcome means that applying the proposed RLS algorithm is effective to upgrade UWB positioning accuracy. The reason for this fact is that the RLS method can set smaller weight for the measurements with lower quality and raise the weight for the measurements with higher quality by equation (12). The works below are all based on the RLS-based UWB solutions.


### 3.3. INS-, NHC-, and RTS-Enhanced UWB

Although the robust algorithm can improve UWB positioning accuracy visibly, its positioning performance may still be degraded by some error sources inherent to wireless techniques. To further enhance the performance, other types of positioning sensors are used. This part will present the impacts of INS, NHC, and RTS on upgrading UWB positioning accuracy further.As is known, the INS-based DR is usually adopted as an effective method for GNSS signal blocked areas or indoor environments. Therefore, the positioning accuracy of INS, INS/NHC integration, and INS/NHC/RTS integration are analyzed firstly, and the corresponding position offsets compared to the reference values are plotted in the Figure7. Significantly, the solutions of INS drift rapidly along with time due to the uncompensated time-cumulative errors (biases and scale factors). While using the NHC to aid INS, such drifts are restrained obviously, especially in the vertical direction (the red line). It is because the NHC provides external measurements for velocity in the lateral and vertical directions in the body frame, which will improve the estimation accuracy of velocity and attitude according to equation (26). Then, the corrected velocity and attitude will further improve the accuracy of positioning by equations (14) and (16). However, compared with Figure 4(a), it can be seen that NHC performs much more visible on the platform in kinematic than that in static because the observability of gyroscope errors in the vertical direction is weaker when the platform is static. Moreover, as listed in Table 1, positioning accuracy is improved from hundreds of meters to tens of meters while RTS smoother and NHC aided INS together. The improvement percentages in the three directions are up to 88.7%, 96.5%, and 46.2%. The main reason is that information before the current epoch is utilized to estimate parameters in the Kalman filter, while the RTS smoother can use all of the measurements to estimate parameters at each epoch.Figure 7
Positioning errors calculated by INS, INS/NHC integration, INS/NHC/RTS integration, UWB, INS/UWB/RTS integration, and UWB/INS/NHC/RTS integration models, respectively.
(a)(b)Table 1
RMS position, velocity, and attitude of different data processing methods.
Positioning methodsPosition (m)Velocity (m/s)Attitude (deg)NorthEastDownNorthEastDownRollPitchYawINS/NHC382.93126.241.312.1370.7060.0120.0180.0522.829INS/NHC/RTS43.1864.3690.6980.1150.0490.0060.0140.0122.465UWB0.0760.0700.5260.3810.4580.263———UWB/INS/RTS0.0490.0500.1400.0040.0040.0020.0040.0030.313UWB/INS/NHC/RTS0.0460.0490.0930.0040.0040.0020.0030.0030.152Shown in Figure7(b) are the results from UWB, UWB/INS/RTS integration, and UWB/INS/NHC/RTS integration. As a comparison, the position RMSs of UWB/INS/RTS integration of the three components have been upgraded to 4.9 cm, 5.0 cm, and 14.0 cm, with about 99.9%, 99.9%, and 89.3% improvements compared to the solutions of INS aided NHC, about 99.8%, 98.8%, and 79.9% enhancements compared to that of INS/NHC/RTS integration, and approximately 35.5%, 28.6%, and 73.4% improvements compared to that of UWB. These results mean that the RTS smoother-based UWB/INS integration almost can provide centimeter-level positioning accuracy for users, which is much higher than that of UWB only or INS-based DR, compared to that of INS/NHC/RTS integration, UWB provides three-dimensional position and velocity external measurements to correct the solutions of INS and makes the estimation more accuracy by equation (32). In addition, by applying NHC to UWB/INS/RTS integration, about 6.1%, 2.0%, and 33.6% more improvements can be obtained and the position RMSs are upgraded to 4.6 cm, 4.9 cm, and 9.3 cm (listed in Table 1).In general, the proposed method makes use of the advantages of UWB, INS, NHC, and RTS (i.e., the long-term high accuracy of UWB in environments without NLOS, short-term high accuracy and continuity of INS, external constraints of NHC on platform motion, and optimal estimation of RTS) and provides users more reliable, precise, and continued positioning solutions than previous works.Besides positioning, the proposed method can also provide velocity and attitude solutions for users (shown in Figure8). According to the results, similar conclusions can be obtained. For velocimetry, the velocity RMSs are 38.1 cm/s, 45.8 cm/s, and 26.3 cm/s for UWB, 11.5 cm/s, 4.9 cm/s, and 0.6 cm/s for INS/NHC/RTS integration, and 0.4 cm/s, 0.4 cm/s, and 0.2 cm/s for UWB/INS/NHC/RTS integration. Significantly, with the enhancements of NHC and RTS, velocity accuracy can be up to higher than centimeter level or even millimeter level. However, the impacts of NHC on improving the velocity accuracy of UWB/INS/RTS integration are slight and almost can be ignored. That is because the accuracy of velocity is mainly dependent on the performance of IMU while there are enough external measurements [8].Figure 8
Errors of velocity and attitude calculated by the integration models of INS/NHC, INS/NHC/RTS, UWB, UWB/INS/RTS, and UWB/INS/NHC/RTS.
(a)(b)As is known, UWB can also provide pitch and yaw angles in dynamic conditions by using the UWB velocity information [38]. However, the accuracy of such two attitude angles is influenced significantly by the modulus of velocity (usually ≥ 3 m/s). Considering the velocity of our test is lower than 0.8 m/s, no attitude solutions are calculated from UWB. Shown in Figure 8(b) are attitude offsets from NHC-aided INS, NHC- and RTS-aided INS, UWB/INS/RTS integration, and UWB/INS/NHC/RTS integration. As listed in Table 1, the accuracy of roll and pitch angles is better than 0.05 degrees for all the four data-processing methods, which is much higher than those of yaw angles. As mentioned in Gao et al. [10], it is caused by weak observability of the gyroscope errors along the vertical axis.


### 3.4. UWB NLOS Analysis

The reason for applying INS, NHC, and RTS to UWB is to further make the positioning solutions more robust and reliable, especially in NLOS environments. In this test, we generate six UWB signal-blocked periods from the field test data, with each lasting for 30 seconds, to further evaluate the proposed approach in the challenging UWB-denied environments. We processed these data under UWB/INS integration, UWB/INS/NHC integration, and UWB/INS/NHC/RTS integration modes, and the corresponding position errors are shown in Figure9. Significantly, the position accuracy is influenced while comparing it with the results in Figure 7(b). According to the statistics in Figure 10, the RMSs of UWB/INS/NHC/RTS integration are 4.8 cm, 5.1 cm, and 15.1 cm in the north, east, and vertical components, with about 4.2%, 3.9%, and 38.4% accuracy loss compared to the solutions of UWB/INS/NHC/RTS integration without UWB outages. However, the proposed method still can provide users centimeter-level positioning accuracy. Besides, it is also visible that the enhancements from NHC and RTS on UWB/INS integration (the RMSs are 11.8 cm, 17.1 cm, and 47.3 cm) are obvious. Statistics illustrate that about 1.7%, 8.8%, and 65.5% improvements can be obtained from nonholonomic constraints (the RMSs are 11.6 cm, 15.6 cm, and 16.3 cm), and about 58.6%, 67.3%, and 7.4% enhancements can be provided by RTS smoother. Meanwhile, in this UWB outage simulation, it seems that RTS smoother mainly improves the positioning accuracy in horizontal and NHC upgrades the vertical accuracy. Besides, the position drifts during different UWB-outage time scales are also provided in Figure 10, from which the position accuracy is better than 30 cm and 5 cm during outages for within 30 seconds and 15 seconds, respectively. This outcome is promising, which indicates that the proposed method can provide robust and continuous position solutions in short-term NLOS or even UWB-denied scenarios.Figure 9
Position offsets of UWB/INS integration, UWB/INS/NHC integration, and UWB/INS/NHC/RTS integration while applying six UWB outage simulations.Figure 10
Position offsets of INS in different time-scales (a), and position RMS (b) of UWB/INS integration, UWB/INS/NHC integration, and UWB/INS/NHC/RTS integration while applying six UWB outage simulations.
(a)(b)


## 3.1. Data Collection and Processing Methods

A set of GNSS observations, INS measurements, and UWB data collected around a square in a residential area is processed and analyzed. In this test, a multiconstellation GNSS receiver, a tactical-grade IMU, and a UWB tag are fixed together on a handcart. Wherein, the data rate of GNSS, IMU, and UWB are 1 Hz, 200 Hz, and 1 Hz, respectively. The instability of biases and scale factors of gyroscopes and accelerometers are 0.5°/h and 250 mGal. The random walk noises of angle and velocity are 0.03°/√h and 0.05 m/s/√h, respectively.The scene of this test is shown in Figure2. As is shown, it is almost an open sky environment. Under such a condition, the GNSS can be tracked continuously, which makes it possible that the solutions calculated by GNSS RTK/INS integration can be utilized as the reference values for evaluating the performance of UWB/INS/NHC integration. To make the accuracy of reference values high enough, the GNSS base station is just hundreds of meters away. Besides, the level-arms of UWB tag and the receiver antenna of GNSS rover compared to IMU center in IMU body frame are measured precisely before the test operating, which can be used to transform the solutions from different modes to a reference point (i.e., IMU center). Then, the performance of the proposed mode can be evaluated reasonably. Meanwhile, a total station (top-left corner in Figure 2) is used to determine the coordinate of each UWB base station under the Gauss coordinate system (north-east-vertical). Then, these base station coordinates are transformed into the WGS-84 coordinate system which is to match the coordinate system adopted by the proposed algorithm.Figure 2
Experiment scene of the GNSS/INS/UWB integration test.During the whole test (lasting about 2200 seconds), the handcart (shown in Figure2) is pushed along an approximate ‘#‘-shaped route with a velocity of within ± 0.8 m/s, which can be seen from Figures 3 and 4. The attitude angles of the handcart that changed along the motion are depicted in Figure 4(b). To simulate UWB’s application situation indoor, we only move the handcart in the area made up of the six UWB base stations (marked as 1, 2, 3, 4, 6, and 7 as shown Figure 3(a)). Meanwhile, the number 5 UWB station is placed in the middle of this area to make sure more UWB measurements can be collected while handcart moving. To evaluate the NLOS effect on UWB, several testers walk around base stations randomly. According to the tracked UWB base stations plotted in Figure 3(b), the average number of available UWB stations is 5.8, and the corresponding dilution of precision in horizontal (HDOP) and vertical (VDOP) is 1.1 and 8.7, respectively, in terms of root-mean-squared (RMS) value. Such a condition will make the positioning accuracy in horizontal higher than those in vertical. According to the velocity solutions, it can be known that the handcart stays static for about 15 minutes, which is to provide enough static data for the attitude initialization. Then, the collected data were processed majorly by five different modes, namely, UWB positioning, INS/NHC integration, INS/NHC/RTS integration, UWB/INS/RTS integration, and UWB/INS/NHC/RTS integration. The differences between the results from these different modes and the reference values are used to show the performance of the proposed method.Figure 3
Three-dimensional test trajectory (a) and the number/HDOP/VDOP of visible UWB base stations during the test (b).
(a)(b)Figure 4
Velocity (a) and attitude (b) during the test calculated by UWB/INS/NHC/RTS integration mode.
(a)(b)


## 3.2. Robust LS-Enhanced UWB Positioning

Firstly, the positioning accuracy of UWB with and without a robust algorithm is analyzed. As is known, the quality of UWB measurements will be influenced by the NLOS effect. Accordingly, the UWB positioning accuracy will be degraded while there are NLOS effects. Shown in Figure5 are the range measurements from the UWB tag to each base station and the corresponding measuring error of each range measurement. According to Figure 5(a), it can be seen that UWB ranging would be low quality and even fail when suffering from a strong NLOS effect. Besides, measuring errors in Figure 5(b) also provide similar conclusions. Therefore, the robust least square adjustment is employed in UWB positioning to reduce the impact of NLOS on the positioning accuracy.Figure 5
Measurements (a) and corresponding noise (b)) of each UWB tag-base-station pair (1, 2, …, 7 denote UWB base station mark).
(a)(b)Figure6(a) shows the UWB positioning offsets (in n frame) using robust least square adjustment by comparing with the RTK/INS integration solutions. Significantly, the positioning accuracy in horizontal is much higher (several times) than that in the vertical component. According to statistics, the RMS in the north, east, and down components are 7.6 cm, 7.0 cm, and 52.6 cm, respectively, and the minimum/maximum values for the three directions are −54/+23 cm, −51/+27 cm, and −1161/+277 cm. When comparing Figures 6(a) and 3(b), the change of positioning accuracy also presents a strong relationship along with the change of available UWB number, HDOP, and VDOP. It is because the geometry structure between the UWB tag and base stations in horizontal is much stronger than that in vertical. It is also the main reason why previous works were hard to provide precise position accuracy in vertical by UWB.Figure 6
UWB positioning errors of robust least square (a) and the position improvements caused the robust algorithm (b).
(a)(b)The improvements benefitting from robust estimation are shown in Figure6(b). Improvements of 18 cm, 11 cm, and 202 cm can be obtained after applying the RLS algorithm in UWB positioning with the percentages of 70.3%, 611%, and 79.2% in the north, east, and down components. This outcome means that applying the proposed RLS algorithm is effective to upgrade UWB positioning accuracy. The reason for this fact is that the RLS method can set smaller weight for the measurements with lower quality and raise the weight for the measurements with higher quality by equation (12). The works below are all based on the RLS-based UWB solutions.


## 3.3. INS-, NHC-, and RTS-Enhanced UWB

Although the robust algorithm can improve UWB positioning accuracy visibly, its positioning performance may still be degraded by some error sources inherent to wireless techniques. To further enhance the performance, other types of positioning sensors are used. This part will present the impacts of INS, NHC, and RTS on upgrading UWB positioning accuracy further.As is known, the INS-based DR is usually adopted as an effective method for GNSS signal blocked areas or indoor environments. Therefore, the positioning accuracy of INS, INS/NHC integration, and INS/NHC/RTS integration are analyzed firstly, and the corresponding position offsets compared to the reference values are plotted in the Figure7. Significantly, the solutions of INS drift rapidly along with time due to the uncompensated time-cumulative errors (biases and scale factors). While using the NHC to aid INS, such drifts are restrained obviously, especially in the vertical direction (the red line). It is because the NHC provides external measurements for velocity in the lateral and vertical directions in the body frame, which will improve the estimation accuracy of velocity and attitude according to equation (26). Then, the corrected velocity and attitude will further improve the accuracy of positioning by equations (14) and (16). However, compared with Figure 4(a), it can be seen that NHC performs much more visible on the platform in kinematic than that in static because the observability of gyroscope errors in the vertical direction is weaker when the platform is static. Moreover, as listed in Table 1, positioning accuracy is improved from hundreds of meters to tens of meters while RTS smoother and NHC aided INS together. The improvement percentages in the three directions are up to 88.7%, 96.5%, and 46.2%. The main reason is that information before the current epoch is utilized to estimate parameters in the Kalman filter, while the RTS smoother can use all of the measurements to estimate parameters at each epoch.Figure 7
Positioning errors calculated by INS, INS/NHC integration, INS/NHC/RTS integration, UWB, INS/UWB/RTS integration, and UWB/INS/NHC/RTS integration models, respectively.
(a)(b)Table 1
RMS position, velocity, and attitude of different data processing methods.
Positioning methodsPosition (m)Velocity (m/s)Attitude (deg)NorthEastDownNorthEastDownRollPitchYawINS/NHC382.93126.241.312.1370.7060.0120.0180.0522.829INS/NHC/RTS43.1864.3690.6980.1150.0490.0060.0140.0122.465UWB0.0760.0700.5260.3810.4580.263———UWB/INS/RTS0.0490.0500.1400.0040.0040.0020.0040.0030.313UWB/INS/NHC/RTS0.0460.0490.0930.0040.0040.0020.0030.0030.152Shown in Figure7(b) are the results from UWB, UWB/INS/RTS integration, and UWB/INS/NHC/RTS integration. As a comparison, the position RMSs of UWB/INS/RTS integration of the three components have been upgraded to 4.9 cm, 5.0 cm, and 14.0 cm, with about 99.9%, 99.9%, and 89.3% improvements compared to the solutions of INS aided NHC, about 99.8%, 98.8%, and 79.9% enhancements compared to that of INS/NHC/RTS integration, and approximately 35.5%, 28.6%, and 73.4% improvements compared to that of UWB. These results mean that the RTS smoother-based UWB/INS integration almost can provide centimeter-level positioning accuracy for users, which is much higher than that of UWB only or INS-based DR, compared to that of INS/NHC/RTS integration, UWB provides three-dimensional position and velocity external measurements to correct the solutions of INS and makes the estimation more accuracy by equation (32). In addition, by applying NHC to UWB/INS/RTS integration, about 6.1%, 2.0%, and 33.6% more improvements can be obtained and the position RMSs are upgraded to 4.6 cm, 4.9 cm, and 9.3 cm (listed in Table 1).In general, the proposed method makes use of the advantages of UWB, INS, NHC, and RTS (i.e., the long-term high accuracy of UWB in environments without NLOS, short-term high accuracy and continuity of INS, external constraints of NHC on platform motion, and optimal estimation of RTS) and provides users more reliable, precise, and continued positioning solutions than previous works.Besides positioning, the proposed method can also provide velocity and attitude solutions for users (shown in Figure8). According to the results, similar conclusions can be obtained. For velocimetry, the velocity RMSs are 38.1 cm/s, 45.8 cm/s, and 26.3 cm/s for UWB, 11.5 cm/s, 4.9 cm/s, and 0.6 cm/s for INS/NHC/RTS integration, and 0.4 cm/s, 0.4 cm/s, and 0.2 cm/s for UWB/INS/NHC/RTS integration. Significantly, with the enhancements of NHC and RTS, velocity accuracy can be up to higher than centimeter level or even millimeter level. However, the impacts of NHC on improving the velocity accuracy of UWB/INS/RTS integration are slight and almost can be ignored. That is because the accuracy of velocity is mainly dependent on the performance of IMU while there are enough external measurements [8].Figure 8
Errors of velocity and attitude calculated by the integration models of INS/NHC, INS/NHC/RTS, UWB, UWB/INS/RTS, and UWB/INS/NHC/RTS.
(a)(b)As is known, UWB can also provide pitch and yaw angles in dynamic conditions by using the UWB velocity information [38]. However, the accuracy of such two attitude angles is influenced significantly by the modulus of velocity (usually ≥ 3 m/s). Considering the velocity of our test is lower than 0.8 m/s, no attitude solutions are calculated from UWB. Shown in Figure 8(b) are attitude offsets from NHC-aided INS, NHC- and RTS-aided INS, UWB/INS/RTS integration, and UWB/INS/NHC/RTS integration. As listed in Table 1, the accuracy of roll and pitch angles is better than 0.05 degrees for all the four data-processing methods, which is much higher than those of yaw angles. As mentioned in Gao et al. [10], it is caused by weak observability of the gyroscope errors along the vertical axis.


## 3.4. UWB NLOS Analysis

The reason for applying INS, NHC, and RTS to UWB is to further make the positioning solutions more robust and reliable, especially in NLOS environments. In this test, we generate six UWB signal-blocked periods from the field test data, with each lasting for 30 seconds, to further evaluate the proposed approach in the challenging UWB-denied environments. We processed these data under UWB/INS integration, UWB/INS/NHC integration, and UWB/INS/NHC/RTS integration modes, and the corresponding position errors are shown in Figure9. Significantly, the position accuracy is influenced while comparing it with the results in Figure 7(b). According to the statistics in Figure 10, the RMSs of UWB/INS/NHC/RTS integration are 4.8 cm, 5.1 cm, and 15.1 cm in the north, east, and vertical components, with about 4.2%, 3.9%, and 38.4% accuracy loss compared to the solutions of UWB/INS/NHC/RTS integration without UWB outages. However, the proposed method still can provide users centimeter-level positioning accuracy. Besides, it is also visible that the enhancements from NHC and RTS on UWB/INS integration (the RMSs are 11.8 cm, 17.1 cm, and 47.3 cm) are obvious. Statistics illustrate that about 1.7%, 8.8%, and 65.5% improvements can be obtained from nonholonomic constraints (the RMSs are 11.6 cm, 15.6 cm, and 16.3 cm), and about 58.6%, 67.3%, and 7.4% enhancements can be provided by RTS smoother. Meanwhile, in this UWB outage simulation, it seems that RTS smoother mainly improves the positioning accuracy in horizontal and NHC upgrades the vertical accuracy. Besides, the position drifts during different UWB-outage time scales are also provided in Figure 10, from which the position accuracy is better than 30 cm and 5 cm during outages for within 30 seconds and 15 seconds, respectively. This outcome is promising, which indicates that the proposed method can provide robust and continuous position solutions in short-term NLOS or even UWB-denied scenarios.Figure 9
Position offsets of UWB/INS integration, UWB/INS/NHC integration, and UWB/INS/NHC/RTS integration while applying six UWB outage simulations.Figure 10
Position offsets of INS in different time-scales (a), and position RMS (b) of UWB/INS integration, UWB/INS/NHC integration, and UWB/INS/NHC/RTS integration while applying six UWB outage simulations.
(a)(b)


## 4. Conclusions

This contribution proposed to adopt the Rauch–Tung–Striebel smoother and nonholonomic constraints enhanced UWB/INS integration algorithm to provide precise solutions for indoor positioning and navigation. The principle and the mathematical models of the method are described in detail. Field experiment results show that (1) the proposed method can provide centimeter-level positioning accuracy, millimeter-level velocimetry accuracy, and the accuracy of higher than 0.05 degrees for the roll and pitch angles and 0.15 degrees for the yaw angle, which is more accurate than other integration strategies; (2) both Rauch–Tung–Striebel smoother and non-holonomic constraints present significant improvements on the accuracy of position, velocity, and attitude; (3) during the UWB non-line-of-sight environments, the NHC and RTS smoother can provide about 59%, 70%, and 68% position improvements when they work together. Furthermore, the NHC presents more impacts on vertical direction, while the RTS smoother shows more in horizontal components.Although the proposed method can provide high-accuracy solutions, its performance may be degraded significantly when the NLOS or UWB-denied environments last for a long time. Therefore, we are considering to integrate other sensors, such as the visual camera and Lidar, with the current system to further upgrade the performance of current UWB/INS/NHC integration in the more complicated indoor conditions.



---

*Source: 7274181-2020-10-13.xml*