# UWB/PDR Tightly Coupled Navigation with Robust Extended Kalman Filter for NLOS Environments

Authors: Xin Li; Yan Wang; Kourosh Khoshelham
Journal: Mobile Information Systems
 (2018)

---

## Abstract

The fusion of ultra-wideband (UWB) and inertial measurement unit (IMU) is an effective solution to overcome the challenges of UWB in nonline-of-sight (NLOS) conditions and error accumulation of inertial positioning in indoor environments. However, existing systems are based on foot-mounted or body-worn IMUs, which limit the application of the system to specific practical scenarios. In this paper, we propose the fusion of UWB and pedestrian dead reckoning (PDR) using smartphone IMU, which has the potential to provide a universal solution to indoor positioning. The PDR algorithm is based on low-pass filtering of acceleration data and time thresholding to estimate the step length. According to different movement patterns of pedestrians, such as walking and running, several step models are comparatively analyzed to determine the appropriate model and related parameters of the step length. For the PDR direction calculation, the Madgwick algorithm is adopted to improve the calculation accuracy of the heading algorithm. The proposed UWB/PDR fusion algorithm is based on the extended Kalman filter (EKF), in which the Mahalanobis distance from the observation to the prior distribution is used to suppress the influence of abnormal UWB data on the positioning results. Experimental results show that the algorithm is robust to the intermittent noise, continuous noise, signal interruption, and other abnormalities of the UWB data.


---

## Body

## 1. Introduction

Indoor positioning technology has a wide range of applications from supermarket shopping assistance to drone positioning and patient tracking in hospitals [1–3]. Among various approaches, the ultra-wideband (UWB) positioning technology is particularly attractive because of its decimeter-level positioning accuracy. However, in many practical scenarios, such as warehouse robot positioning and emergency response in crowded scenes, UWB signals may be blocked by people, cargos, or other obstacles, which will cause signal problems such as multipath effect, strength attenuation, and even signal loss, resulting in a sharp drop in the UWB positioning accuracy [4, 5]. The fusion of inertial measurement unit (IMU) in a pedestrian dead reckoning (PDR) method and UWB is an effective way to achieve high-precision positioning in nonline-of-sight (NLOS) environments. While UWB provides accurate absolute positioning in line-of-sight conditions, the PDR ensures a continuous and smooth trajectory in periods of UWB signal loss.There are a few works in the literature on combining UWB and inertial sensors. Fan et al. [6, 7] developed a loosely coupled fusion method based on the EKF to track the pedestrian movement. However, at least three effective UWB range measurements are required for this method, which may not be available under the NLOS conditions. Some scholars have presented a tightly coupled method which combines the UWB range measurement with the IMU measurements [8–10]. Although the positioning precision and stability are indeed improved by these methods, the signal noise and interruption of the beacons are not considered. Benini et al. [11] proposed an IMU/UWB/vision fusion method for the drone positioning, with two-dimensional positioning accuracy of 10 cm. Wang et al. [12] designed a tightly coupled GPS/UWB/IMU integrated system based on the adaptive robust Kalman filter. González et al. [13] used particle filter algorithm to fuse the data of UWB, IMU, and odometer, achieving good positioning stability under the NLOS conditions. However, the particle filter algorithm is difficult to be realized in an embedded system due to its high computational complexity. The fusion of UWB and PDR has been proposed in a few other works [14–17]. However, existing UWB/PDR fusion methods are based on foot-mounted or body-worn IMUs. While the positioning accuracy of PDR with foot-mounted or body-worn IMU is higher than that of smartphone-based PDR, due to the use of zero-velocity update, these systems are generally not scalable and are limited to a few specific application scenarios.There are many methods for outliers detection. The most direct method is to reject the observations with large residual directly. However, this method may lead to a lack of continuity in the estimates of the covariance [18]. Median-based filters can also be used to realize robust EKF, and this kind of filter may be highly robust, but not efficient. This filter is based on a block-based batch-reprocessing manner and is difficult for real-time applications [19]. By minimizing the estimation error of the worst case, H-infinity filter can control the influence of some uncertain observations on the system state, but it fails when large outliers exist [20]. M-estimation-based robust filters have been widely studied in recent years, such as the Huber-based unscented EKF is generalized to cope with the case that outliers in both the prediction and observation coexist simultaneously [21]. But M-estimation-based robust filters achieve robustness at the cost of reducing the accuracy of the nonlinear transformation itself. Essentially the M-estimation-based filters may be equivalent in some sense to the method of observation-noise inflating or observation-trimming, but with relatively computational complexities [22].To address the above challenges, in this paper, we propose a tightly coupled UWB/PDR fusion method based on smartphone IMU for positioning in indoor environments. To the best of our knowledge, the proposed method is the first to address the UWB/PDR fusion using smartphone IMU. We also adopt the Madgwick algorithm for heading estimation in smartphone-based PDR and show that a significant improvement in heading estimation accuracy can be achieved. Finally, we propose the use of Mahalanobis distance from the observation to the prior distribution to suppress the influence of abnormal UWB data on the positioning results.The remainder of the paper is organized as follows: In Section2, the PDR algorithm based on smartphone inertial sensor is discussed and the theoretical analysis and experimental results of step detection, step length calculation, and heading calculation are given. In Section 3, the UWB/PDR fusion algorithm based on the EKF is studied, emphasizing the principle of the robust algorithm based on the Mahalanobis distance. In Section 4, several experiments are presented and the results are analyzed. In Section 5, the paper is summarized.


## 2. Overview of the Proposed Method

An overview of the proposed method for UWB/PDR fusion is shown in Figure1. Using the IMU data, steps are detected based on a combination of low-pass filtering, acceleration, and time threshold. Considering two different movement patterns, walking and running, the estimation of step length by using various models is compared and analyzed, and the optimal method for step length estimation and the corresponding parameters for different movement patterns are identified. Real-time heading estimation is performed by adopting the Madgwick algorithm [23], which improves the accuracy of heading estimation and enhances the positioning accuracy of the PDR algorithm.Figure 1
Framework of the research.The UWB/PDR fusion is done based on the EKF algorithm. To make the fusion robust against abnormal and outlying sensor readings, the use of the Mahalanobis distance from the observation to the prior distribution is proposed. As shown by experimental evaluation, the proposed method is robust to intermittent noise, continuous noise, signal interruption, and other abnormalities of the UWB data.


## 3. PDR Algorithm Based on Smartphone Inertial Sensors

The PDR algorithm consists of step detection, step length estimation, and heading estimation. Based on the movement characteristics of the pedestrian, the accelerometer data are used to measure the number of steps and estimate the step length. Then, in combination with the heading information obtained from the IMU, the current position of the pedestrian is calculated by using equation (1).(1)Ni+1=Ni+stcosψt,Et+1=Et+stsinψt,where N,E are the position coordinates of the pedestrian, s is the step length, and ψ is the heading angle.


### 3.1. Step Detection

Walking is characterized by a periodic acceleration pattern. With the foot from lifting to landing, that is, with the gravity center from rising to falling, the acceleration data in the vertical direction follow a curve from peak to trough. Therefore, the detection of a step can be done based on acceleration measurements. Smooth region detection [24], zero-crossing detection [25], peak detection [26, 27], and autocorrelation method [28] are commonly used methods.In this work, the method proposed in [29] is adopted, where the acquired vertical acceleration is first processed by low-pass filtering, so that spurious peaks are removed from the waveform of the acceleration data. Since the filtering order, the pass-band frequency, and the stop-band frequency are all set empirically, sporadic pseudopeaks and pseudotroughs might still exist in the waveform. Therefore, using the filtered data, the amplitude difference and the time difference between consecutive filtered values are used to identify the correct maxima and minima, thereby performing step detection, as described in [30]. The detection result is shown in Figure 2.Figure 2
Identification of maxima and minima in the step detection.


### 3.2. Step Length Calculation

The commonly used step length calculation models can be roughly divided into three types: constant model, linear model, and nonlinear model. Among them, the constant model is the simplest one; however, it has the drawback that it cannot adapt to the real-time changes of the pedestrian velocity. A statistical table of step length at different levels is presented in [31], and the fixed step length can be selected from the table according to the movement pattern characteristics and the step frequency.For the linear model, Levi and Judd [32] proposed the linear relationship between step length and step frequency as follows:(2)s=A+B×f+C×var+w,where f is the step frequency of each step, that is, the reciprocal of the step cycle; var is the acceleration variance of each step; and w is the Gaussian noise. A and C are the regression coefficients obtained through training. A similar linear model is proposed in [33]:(3)s=A+B×p+C×a¯max,where p indicates the step cycle of each step, a¯max is the peak value of the acceleration after smoothing, and A and C are the regression coefficients obtained through training. The calculation model for the step length from a nonlinear perspective has also been analyzed, and a nonlinear step length calculation formula with only one parameter is used in [34]:(4)s=K×amax−amin4,where amax and amin represent the maximum and minimum acceleration of each step, respectively, and K is the regression coefficient.


### 3.3. Heading Calculation by the Madgwick Algorithm

When the pedestrian walks with the smartphone held horizontally, the azimuth of the smartphone can be regarded as the heading angle of the pedestrian. The azimuth can be calculated by the built-in algorithm based on the gyroscope or the magnetometer in the smartphone. However, this heading calculation is largely influenced by the smartphone pose and the user’s body movement. Figure3 shows an example where a slight sway of the body causes a deviation of the calculated heading angle (the green line) from the actual heading angle (the blue line) reaching nearly 40 degrees.Figure 3
Comparison of the heading angles.In order to improve the accuracy of the heading calculation, the state-of-art Attitude and Heading Reference Systems (AHRS), i.e., the Madgwick algorithm, is introduced into the PDR algorithm in this paper. In this algorithm, in order to estimate the direction, the measurements of accelerometer, gyroscope, and magnetometer are combined with two absolute fields, the geomagnetic field and the gravity field, whose directions and intensities are known. The advantage of the AHRS algorithm is that the attitude error can be compensated continuously, improving the accuracy of the direction. As can be seen from Figure3, the calculation result (the pink line) is basically consistent with the reference heading. The Madgwick algorithm consists of two steps as described below.


#### 3.3.1. Initial Direction Calculation

Initially, it is assumed that the sensor is either in a stationary state or moves at a constant velocity, so that only the gravity vector is measured by the accelerometer. In addition, it is assumed that the magnetic field is nondisturbed, and therefore, only the geomagnetic field is measured by the magnetometer. The pitch (θ) and roll (ϕ) can be derived from fxb,fyb,fzb, the component of the gravity vector on the three axes.(5)ϕ=tanfybfzb−1,θ=−sinfxbg−1.The value of yaw (ψ) cannot be obtained by the gravity vector, but can be obtained by the magnetometer. The measurement of the magnetometer is recorded as mb=mxb,myb,mzb, representing the projection of the geomagnetic field on the three axes of the body frame (b-frame). Since pitch (θ) and roll (ϕ) are known, mb can be converted onto the XOY plane of the navigation frame (n-frame), with its orthogonal component recorded as mn=mxn,myn,mzn. The conversion relationship between the two sets of magnetic data is(6)mxnmynmzn=cosθ0−sinθ010inθ0cosθ−11000cosϕ000cosϕ−1mxbmybmzb.In the geographic frame, thebx points to the north, and the by pointing to the east is equal to 0, so the geomagnetic field data are b=bx,0,bz. Based on the angular transformation obtained by the accelerometer, the geomagnetic data in the n-frame and geographic frame are transformed onto the same XOY plane, and the angle difference between the two transformed data is the heading angle ψ, as shown in Figure 4. According to the trigonometric relationship, Formulas (7) and (8) are obtained.(7)bx=sqrtmxb2+myb2,(8)bz=mzn.Figure 4
Navigation frame and geographic frame.The relationship of the geomagnetic data between b-frame and geographic frame is(9)b=Cbnmb.The heading angle can be calculated by Formula (9):(10)ψ=arctanmxbcosϕ+mzbsinϕmxbsinϕ−mzbcosϕsinθ−mybcosθ.


#### 3.3.2. Attitude Calculation

For a specific vector, its magnitude and direction are the same when it is represented by different coordinate systems. This is the principle of the attitude calculation. However, error exists in the transformation matrix between the two coordinate systems. When a vector is transformed by a rotation matrix with error, a deviation between the transformed result and the original vector will appear. This deviation can be used to correct the rotation matrix, and the attitude is thus corrected. In the Madgwick algorithm, this rotation matrix is represented by a quaternion, and the attitude is calculated by modifying the quaternion. The accelerometer and the magnetometer are the main measured objects in the attitude calculation process. In the Madgwick algorithm, the current attitude is updated by the gyroscope:(11)qt+1=qt+0.5qtωtdt,where q is the attitude of the sensor. The attitude error is corrected by calculating the difference between the accelerometer’s measurement and the magnetometer’s measurement under n-frame and b-frame. Assume that e=0,ex,ey,ez and s=0,sx,sy,sz are the vector coordinates under n-frame and b-frame, respectively. Then for the accelerometer, e=0,0,0,1 and s=0,fxb,fyb,fzb; for the magnetometer e=0,bx,0,bz and s=0,mxb,myb,mzb. The transformation relationship is(12)s=q−1eq.The error equation is defined as(13)Eq=q−1eq−s.The above formula is solved by the Gauss–Newton method:(14)qt+1=qt−μt∇E∇E,where μt is the step length and ∇ is the differential operator. Equations (11) and (14) are combined, and(15)qt+1=qt+0.5qtωtdt−β∇E∇Edt,where β in equation (15) represents the weight, which is used to measure the error of the result calculated by the angular velocity.


## 3.1. Step Detection

Walking is characterized by a periodic acceleration pattern. With the foot from lifting to landing, that is, with the gravity center from rising to falling, the acceleration data in the vertical direction follow a curve from peak to trough. Therefore, the detection of a step can be done based on acceleration measurements. Smooth region detection [24], zero-crossing detection [25], peak detection [26, 27], and autocorrelation method [28] are commonly used methods.In this work, the method proposed in [29] is adopted, where the acquired vertical acceleration is first processed by low-pass filtering, so that spurious peaks are removed from the waveform of the acceleration data. Since the filtering order, the pass-band frequency, and the stop-band frequency are all set empirically, sporadic pseudopeaks and pseudotroughs might still exist in the waveform. Therefore, using the filtered data, the amplitude difference and the time difference between consecutive filtered values are used to identify the correct maxima and minima, thereby performing step detection, as described in [30]. The detection result is shown in Figure 2.Figure 2
Identification of maxima and minima in the step detection.


## 3.2. Step Length Calculation

The commonly used step length calculation models can be roughly divided into three types: constant model, linear model, and nonlinear model. Among them, the constant model is the simplest one; however, it has the drawback that it cannot adapt to the real-time changes of the pedestrian velocity. A statistical table of step length at different levels is presented in [31], and the fixed step length can be selected from the table according to the movement pattern characteristics and the step frequency.For the linear model, Levi and Judd [32] proposed the linear relationship between step length and step frequency as follows:(2)s=A+B×f+C×var+w,where f is the step frequency of each step, that is, the reciprocal of the step cycle; var is the acceleration variance of each step; and w is the Gaussian noise. A and C are the regression coefficients obtained through training. A similar linear model is proposed in [33]:(3)s=A+B×p+C×a¯max,where p indicates the step cycle of each step, a¯max is the peak value of the acceleration after smoothing, and A and C are the regression coefficients obtained through training. The calculation model for the step length from a nonlinear perspective has also been analyzed, and a nonlinear step length calculation formula with only one parameter is used in [34]:(4)s=K×amax−amin4,where amax and amin represent the maximum and minimum acceleration of each step, respectively, and K is the regression coefficient.


## 3.3. Heading Calculation by the Madgwick Algorithm

When the pedestrian walks with the smartphone held horizontally, the azimuth of the smartphone can be regarded as the heading angle of the pedestrian. The azimuth can be calculated by the built-in algorithm based on the gyroscope or the magnetometer in the smartphone. However, this heading calculation is largely influenced by the smartphone pose and the user’s body movement. Figure3 shows an example where a slight sway of the body causes a deviation of the calculated heading angle (the green line) from the actual heading angle (the blue line) reaching nearly 40 degrees.Figure 3
Comparison of the heading angles.In order to improve the accuracy of the heading calculation, the state-of-art Attitude and Heading Reference Systems (AHRS), i.e., the Madgwick algorithm, is introduced into the PDR algorithm in this paper. In this algorithm, in order to estimate the direction, the measurements of accelerometer, gyroscope, and magnetometer are combined with two absolute fields, the geomagnetic field and the gravity field, whose directions and intensities are known. The advantage of the AHRS algorithm is that the attitude error can be compensated continuously, improving the accuracy of the direction. As can be seen from Figure3, the calculation result (the pink line) is basically consistent with the reference heading. The Madgwick algorithm consists of two steps as described below.


### 3.3.1. Initial Direction Calculation

Initially, it is assumed that the sensor is either in a stationary state or moves at a constant velocity, so that only the gravity vector is measured by the accelerometer. In addition, it is assumed that the magnetic field is nondisturbed, and therefore, only the geomagnetic field is measured by the magnetometer. The pitch (θ) and roll (ϕ) can be derived from fxb,fyb,fzb, the component of the gravity vector on the three axes.(5)ϕ=tanfybfzb−1,θ=−sinfxbg−1.The value of yaw (ψ) cannot be obtained by the gravity vector, but can be obtained by the magnetometer. The measurement of the magnetometer is recorded as mb=mxb,myb,mzb, representing the projection of the geomagnetic field on the three axes of the body frame (b-frame). Since pitch (θ) and roll (ϕ) are known, mb can be converted onto the XOY plane of the navigation frame (n-frame), with its orthogonal component recorded as mn=mxn,myn,mzn. The conversion relationship between the two sets of magnetic data is(6)mxnmynmzn=cosθ0−sinθ010inθ0cosθ−11000cosϕ000cosϕ−1mxbmybmzb.In the geographic frame, thebx points to the north, and the by pointing to the east is equal to 0, so the geomagnetic field data are b=bx,0,bz. Based on the angular transformation obtained by the accelerometer, the geomagnetic data in the n-frame and geographic frame are transformed onto the same XOY plane, and the angle difference between the two transformed data is the heading angle ψ, as shown in Figure 4. According to the trigonometric relationship, Formulas (7) and (8) are obtained.(7)bx=sqrtmxb2+myb2,(8)bz=mzn.Figure 4
Navigation frame and geographic frame.The relationship of the geomagnetic data between b-frame and geographic frame is(9)b=Cbnmb.The heading angle can be calculated by Formula (9):(10)ψ=arctanmxbcosϕ+mzbsinϕmxbsinϕ−mzbcosϕsinθ−mybcosθ.


### 3.3.2. Attitude Calculation

For a specific vector, its magnitude and direction are the same when it is represented by different coordinate systems. This is the principle of the attitude calculation. However, error exists in the transformation matrix between the two coordinate systems. When a vector is transformed by a rotation matrix with error, a deviation between the transformed result and the original vector will appear. This deviation can be used to correct the rotation matrix, and the attitude is thus corrected. In the Madgwick algorithm, this rotation matrix is represented by a quaternion, and the attitude is calculated by modifying the quaternion. The accelerometer and the magnetometer are the main measured objects in the attitude calculation process. In the Madgwick algorithm, the current attitude is updated by the gyroscope:(11)qt+1=qt+0.5qtωtdt,where q is the attitude of the sensor. The attitude error is corrected by calculating the difference between the accelerometer’s measurement and the magnetometer’s measurement under n-frame and b-frame. Assume that e=0,ex,ey,ez and s=0,sx,sy,sz are the vector coordinates under n-frame and b-frame, respectively. Then for the accelerometer, e=0,0,0,1 and s=0,fxb,fyb,fzb; for the magnetometer e=0,bx,0,bz and s=0,mxb,myb,mzb. The transformation relationship is(12)s=q−1eq.The error equation is defined as(13)Eq=q−1eq−s.The above formula is solved by the Gauss–Newton method:(14)qt+1=qt−μt∇E∇E,where μt is the step length and ∇ is the differential operator. Equations (11) and (14) are combined, and(15)qt+1=qt+0.5qtωtdt−β∇E∇Edt,where β in equation (15) represents the weight, which is used to measure the error of the result calculated by the angular velocity.


## 3.3.1. Initial Direction Calculation

Initially, it is assumed that the sensor is either in a stationary state or moves at a constant velocity, so that only the gravity vector is measured by the accelerometer. In addition, it is assumed that the magnetic field is nondisturbed, and therefore, only the geomagnetic field is measured by the magnetometer. The pitch (θ) and roll (ϕ) can be derived from fxb,fyb,fzb, the component of the gravity vector on the three axes.(5)ϕ=tanfybfzb−1,θ=−sinfxbg−1.The value of yaw (ψ) cannot be obtained by the gravity vector, but can be obtained by the magnetometer. The measurement of the magnetometer is recorded as mb=mxb,myb,mzb, representing the projection of the geomagnetic field on the three axes of the body frame (b-frame). Since pitch (θ) and roll (ϕ) are known, mb can be converted onto the XOY plane of the navigation frame (n-frame), with its orthogonal component recorded as mn=mxn,myn,mzn. The conversion relationship between the two sets of magnetic data is(6)mxnmynmzn=cosθ0−sinθ010inθ0cosθ−11000cosϕ000cosϕ−1mxbmybmzb.In the geographic frame, thebx points to the north, and the by pointing to the east is equal to 0, so the geomagnetic field data are b=bx,0,bz. Based on the angular transformation obtained by the accelerometer, the geomagnetic data in the n-frame and geographic frame are transformed onto the same XOY plane, and the angle difference between the two transformed data is the heading angle ψ, as shown in Figure 4. According to the trigonometric relationship, Formulas (7) and (8) are obtained.(7)bx=sqrtmxb2+myb2,(8)bz=mzn.Figure 4
Navigation frame and geographic frame.The relationship of the geomagnetic data between b-frame and geographic frame is(9)b=Cbnmb.The heading angle can be calculated by Formula (9):(10)ψ=arctanmxbcosϕ+mzbsinϕmxbsinϕ−mzbcosϕsinθ−mybcosθ.


## 3.3.2. Attitude Calculation

For a specific vector, its magnitude and direction are the same when it is represented by different coordinate systems. This is the principle of the attitude calculation. However, error exists in the transformation matrix between the two coordinate systems. When a vector is transformed by a rotation matrix with error, a deviation between the transformed result and the original vector will appear. This deviation can be used to correct the rotation matrix, and the attitude is thus corrected. In the Madgwick algorithm, this rotation matrix is represented by a quaternion, and the attitude is calculated by modifying the quaternion. The accelerometer and the magnetometer are the main measured objects in the attitude calculation process. In the Madgwick algorithm, the current attitude is updated by the gyroscope:(11)qt+1=qt+0.5qtωtdt,where q is the attitude of the sensor. The attitude error is corrected by calculating the difference between the accelerometer’s measurement and the magnetometer’s measurement under n-frame and b-frame. Assume that e=0,ex,ey,ez and s=0,sx,sy,sz are the vector coordinates under n-frame and b-frame, respectively. Then for the accelerometer, e=0,0,0,1 and s=0,fxb,fyb,fzb; for the magnetometer e=0,bx,0,bz and s=0,mxb,myb,mzb. The transformation relationship is(12)s=q−1eq.The error equation is defined as(13)Eq=q−1eq−s.The above formula is solved by the Gauss–Newton method:(14)qt+1=qt−μt∇E∇E,where μt is the step length and ∇ is the differential operator. Equations (11) and (14) are combined, and(15)qt+1=qt+0.5qtωtdt−β∇E∇Edt,where β in equation (15) represents the weight, which is used to measure the error of the result calculated by the angular velocity.


## 4. Tightly Coupled UWB/PDR Fusion Algorithm

The UWB provides absolute positioning, but its performance is affected by the NLOS conditions. In contrast, the PDR method based on smartphone IMU data provides relative positioning characterized by error accumulation, but it is independent of the environmental conditions. Therefore, a UWB/PDR fusion algorithm is expected to outperform the individual techniques. In this section, a UWB/PDR fusion algorithm based on the robust EKF is presented.


### 4.1. UWB/PDR Fusion Algorithm

The acceleration data of the IMU are firstly used to determine whether the state is stationary or moving. At the beginning of the positioning, the pedestrian stands still for a few seconds. Gross error of the UWB positioning result in the stationary state is eliminated, and the average value is calculated as the starting position of the PDR algorithm. The initial orientation angle is obtained by using the Madgwick algorithm, and then, the PDR calculation is performed according to Formula (1). In the EKF fusion algorithm, the calculation position and the heading of the PDR algorithm are the state variables of the filter system, namely,(16)Xt=Nt,Et,ψtT.The state equation of the EKF algorithm is(17)Nt+1=Nt+stcosψt,Et+1=Et+stsinψt,ψt+1=ψt+ωtdt,where Nt and Et are the north direction value and the east direction value of the terminal coordinates, respectively; st is the step length of each step obtained by step detection; ψt is the angle in the horizontal movement direction; and ωt is the average angular velocity of the whole step. Derivative of Formula (17) is taken, and the state transition matrix is obtained:(18)Ft=10−stsinψt01stcosψt001.It is assumed that the position coordinate and the dynamic noise of the heading obey a Gaussian distribution with the mean of 0 and the covariance ofσN2,σE2,σψ2. In actual applications, σψ2 is larger when turning and is smaller when going straight. Accordingly, by determining the pedestrian’s movement pattern, such as going straight or turning, the dynamic noise of the variables related to the heading is adaptively determined. The dynamic noise matrix is(19)Qt=σN2000σE2000σψ2,where σN2=σE2=2. When the pedestrian is going straight, σψ2=2°2; when the pedestrian is turning, σψ2=15°2.The UWB ranging measurementrt,i and the heading angle ψ˜t calculated by the Madgwick algorithm are observations. Given n beacons with 2D coordinates denoted as Bi=Bx,i,By,i, i∈1,n, the observation function is defined as(20)hXt=B1−pt⋮Bn−ptψt+ωtdt,where pt=xt,yt is the position coordinates calculated by the PDR and . represents the Euclidean distance. The Jacobian matrix of the observation equation is defined as(21)Ht=Bx,1−xtB1−ptBy,1−ytB1−pt0⋮⋮⋮Bx,n−xtBn−ptBy,n−ytBn−pt0001.The measurement noise matrix is(22)R=σr120…00⋱…0⋮…σrn200…0σψ2,σri2=0.42.


### 4.2. Robustness to Abnormal UWB Range Measurements

Suppose the UWB observation obeys Gaussian distribution, that is,rt,i obeys a Gaussian distribution with the mean of hiXt and the variance of Ht,iPt−Ht,iT−Rt,i, where Pt− is the a priori covariance matrix of the state variables. Therefore, γt,i, the square of the Mahalanobis distance between rt,i and hiXt obeys the χ2 distribution [35], i.e.,(23)γt,i=mt,i−hiXtTHt,iPt−Ht,iT+Rt,i−1mt,i−hiXt∼χ12,where χ12 represents a χ2 distribution with the freedom degree of 1. That is to say, given the significance level α, then(24)Prγt,i<χ1,α2=1−α,where Pr represents the probability of a random event and χ1,α2 is the α-quantile of the χ2 distribution with the freedom degree of 1. In this paper, the significance level α is set to 0.001, and χ1,α2 is determined to be 6.2 according to the chi-square distribution table. Observations that do not satisfy the condition are considered as outliers. The effect of the abnormal observations on the posterior estimation can be attenuated by increasing the covariance of the abnormal values. Algorithm 1 is the robust EKF algorithm.Algorithm 1: REKF algorithm Pseudocode.
Filtering: fort = 1, 2,…
(1)
State prediction
(2)
For each measurement
(3)
while γt,i≥χ1,α2
(4)
Rt,i=γt,i/χ1,α2×Rt,i
(5)
Recalculateγt,i based on Formula (23)
(6)
end
(7)
end
(8)
State updateFor observation with larger Mahalanobis distance, its covariance is required to be increased to reduce its influence on the posterior estimation. The covariance matrix for the new observation can be updated according to the following formula:(25)Rt,i′=Rt,i,γt,i<χ1,α2,γt,iχ1,α2×Rt,i,γt,i≥χ1,α2,where γt,i/χ1,α2 is the ratio of the Mahalanobis distance to the threshold of the observation at the current operating point and covariance. In each iteration process, the Mahalanobis distance between the observation and the current operating point is calculated according to the system state of the current operating point, and the observation covariance is updated accordingly. Before solving the posterior state, the observation covariance is firstly determined by the prior state, which is different from the standard Kalman filter.For computational complexity, REKF is just added lines 2–7 for robust processing with respect to EKF. The problem here is to calculate the number of while loops. From the fourth line of the algorithm, we can see that each cycleRt,i is getting larger, and from Formula (23), the value of denominators is increasing, the value of numerator is unchanged, so each step γt,i is getting smaller. That is, while loops can be finished within a limited number of loops (usually no more than 10 times in experiments), so they should be equivalent to EKF in terms of computational complexity.


## 4.1. UWB/PDR Fusion Algorithm

The acceleration data of the IMU are firstly used to determine whether the state is stationary or moving. At the beginning of the positioning, the pedestrian stands still for a few seconds. Gross error of the UWB positioning result in the stationary state is eliminated, and the average value is calculated as the starting position of the PDR algorithm. The initial orientation angle is obtained by using the Madgwick algorithm, and then, the PDR calculation is performed according to Formula (1). In the EKF fusion algorithm, the calculation position and the heading of the PDR algorithm are the state variables of the filter system, namely,(16)Xt=Nt,Et,ψtT.The state equation of the EKF algorithm is(17)Nt+1=Nt+stcosψt,Et+1=Et+stsinψt,ψt+1=ψt+ωtdt,where Nt and Et are the north direction value and the east direction value of the terminal coordinates, respectively; st is the step length of each step obtained by step detection; ψt is the angle in the horizontal movement direction; and ωt is the average angular velocity of the whole step. Derivative of Formula (17) is taken, and the state transition matrix is obtained:(18)Ft=10−stsinψt01stcosψt001.It is assumed that the position coordinate and the dynamic noise of the heading obey a Gaussian distribution with the mean of 0 and the covariance ofσN2,σE2,σψ2. In actual applications, σψ2 is larger when turning and is smaller when going straight. Accordingly, by determining the pedestrian’s movement pattern, such as going straight or turning, the dynamic noise of the variables related to the heading is adaptively determined. The dynamic noise matrix is(19)Qt=σN2000σE2000σψ2,where σN2=σE2=2. When the pedestrian is going straight, σψ2=2°2; when the pedestrian is turning, σψ2=15°2.The UWB ranging measurementrt,i and the heading angle ψ˜t calculated by the Madgwick algorithm are observations. Given n beacons with 2D coordinates denoted as Bi=Bx,i,By,i, i∈1,n, the observation function is defined as(20)hXt=B1−pt⋮Bn−ptψt+ωtdt,where pt=xt,yt is the position coordinates calculated by the PDR and . represents the Euclidean distance. The Jacobian matrix of the observation equation is defined as(21)Ht=Bx,1−xtB1−ptBy,1−ytB1−pt0⋮⋮⋮Bx,n−xtBn−ptBy,n−ytBn−pt0001.The measurement noise matrix is(22)R=σr120…00⋱…0⋮…σrn200…0σψ2,σri2=0.42.


## 4.2. Robustness to Abnormal UWB Range Measurements

Suppose the UWB observation obeys Gaussian distribution, that is,rt,i obeys a Gaussian distribution with the mean of hiXt and the variance of Ht,iPt−Ht,iT−Rt,i, where Pt− is the a priori covariance matrix of the state variables. Therefore, γt,i, the square of the Mahalanobis distance between rt,i and hiXt obeys the χ2 distribution [35], i.e.,(23)γt,i=mt,i−hiXtTHt,iPt−Ht,iT+Rt,i−1mt,i−hiXt∼χ12,where χ12 represents a χ2 distribution with the freedom degree of 1. That is to say, given the significance level α, then(24)Prγt,i<χ1,α2=1−α,where Pr represents the probability of a random event and χ1,α2 is the α-quantile of the χ2 distribution with the freedom degree of 1. In this paper, the significance level α is set to 0.001, and χ1,α2 is determined to be 6.2 according to the chi-square distribution table. Observations that do not satisfy the condition are considered as outliers. The effect of the abnormal observations on the posterior estimation can be attenuated by increasing the covariance of the abnormal values. Algorithm 1 is the robust EKF algorithm.Algorithm 1: REKF algorithm Pseudocode.
Filtering: fort = 1, 2,…
(1)
State prediction
(2)
For each measurement
(3)
while γt,i≥χ1,α2
(4)
Rt,i=γt,i/χ1,α2×Rt,i
(5)
Recalculateγt,i based on Formula (23)
(6)
end
(7)
end
(8)
State updateFor observation with larger Mahalanobis distance, its covariance is required to be increased to reduce its influence on the posterior estimation. The covariance matrix for the new observation can be updated according to the following formula:(25)Rt,i′=Rt,i,γt,i<χ1,α2,γt,iχ1,α2×Rt,i,γt,i≥χ1,α2,where γt,i/χ1,α2 is the ratio of the Mahalanobis distance to the threshold of the observation at the current operating point and covariance. In each iteration process, the Mahalanobis distance between the observation and the current operating point is calculated according to the system state of the current operating point, and the observation covariance is updated accordingly. Before solving the posterior state, the observation covariance is firstly determined by the prior state, which is different from the standard Kalman filter.For computational complexity, REKF is just added lines 2–7 for robust processing with respect to EKF. The problem here is to calculate the number of while loops. From the fourth line of the algorithm, we can see that each cycleRt,i is getting larger, and from Formula (23), the value of denominators is increasing, the value of numerator is unchanged, so each step γt,i is getting smaller. That is, while loops can be finished within a limited number of loops (usually no more than 10 times in experiments), so they should be equivalent to EKF in terms of computational complexity.


## 5. Experiments

### 5.1. Experimental Setup

The test site is the underground garage of the University of Melbourne. As shown in Figure5(a), 4 UWB beacons were placed at the four corners of the rectangular area. MATE9 of Huawei, China, is selected as the experimental mobile smartphone, as shown in Figure 5(b). The chip of BeSpoon, France, is used as the core chip of the UWB tag/beacon. The UWB tag and the smartphone are connected via a serial port. In order to keep the data of the IMU and the UWB synchronized, the ranging data from both the IMU and the UWB are simultaneously received by the smartphone. During the walking process, the smartphone is steady held in the hand of the pedestrian, basically keeping a uniform motion state. The azimuth of the smartphone is taken as the heading angle of the human body.Figure 5
Experimental setup and equipment. (a) The underground garage and (b) UWB tag and smartphone.
(a)
(b)Two routes are set in the experiment: one is a rectangular route with fewer turnings and the other is an 8-shaped route with more turnings, as shown in Figure6. In the experiment, two laps are taken along each route in a clockwise direction. The start point and the end point are marked in the figure. The rectangular points in the figure denote the location of the four beacons, and the four red circles represent the four columns of the underground garage.Figure 6
Experimental routes. (a) Route 1 and (b) Route 2.
(a)
(b)


### 5.2. Step Detection and Step Length Calculation

#### 5.2.1. Step Detection

The PDR experiment was conducted by two boys and two girls. The height and weight of the two boys are 1.8 meters and 70 kilograms and 1.78 meters and 72 kilograms, respectively. The height and weight of the two girls are 1.65 meters and 55 kilograms and 1.64 meters and 50 kilograms, respectively. The step detection result is shown in Figure7.Figure 7
Step detection result.
(a)
(b)In the table, “Walk” represents the normal walking pattern and “Run” represents the running pattern, including split-step running, stride running, and normal running. It can be seen from the accuracy rate that when the user is walking forward at normal velocity, the accuracy rate of step detection is up to 98% or more, with an average of 99.2%, and the number of wrongly detected step is within 2; when the user is running forward at different velocities, the average accuracy rate is 97.2%, with the minimum accuracy rate of 94.2%, and the maximum wrongly detected step number is 5. The results show that this method can be well applied to the normal walking pattern, while for the abnormal movement patterns, the parameters of step detection are required to be further studied and optimized.


#### 5.2.2. Step Length Calculation

Formulas (2) and (3) of the linear model and Formula (4) of the nonlinear model based on acceleration statistics are verified in this paper. Four persons of different heights and different body sizes are selected to move in different patterns, including normal walking pattern and running pattern, in order to study the applicability and reliability of the three models in different movement patterns.Seven sets of data for the normal walking pattern, including walking along the straight line, walking along the fold lines, etc., are firstly selected. All the step lengths are accumulated to calculate the pedestrians’ movement distance, and the calculated value is compared to the actual distance. The least square method is used to solve the regression coefficients of the linear model and the coefficient of the nonlinear model. The regression coefficients of the linear model areA1=0.678, B1=−0.036, C1=0.031, A2=0.409, B2=0.011, C2=0.068. The coefficient of the nonlinear model is K=0.425. Then the step length is calculated, as shown in the following table.Table1 shows that the average absolute distance differences of the three models are 1.555 m, 0.931 m, and 2.032 m, respectively, with the maximum error no more than 5 meters. The variances of the absolute distance differences of the three models are 1.50, 0.61, and 2.04, respectively, indicating that the linear model is the most stable one with the smallest error. Similarly, four persons of different heights and different body sizes are selected to move in running patterns, including running along the straight line and running along the fold lines. Seven sets of running data are collected, and the corresponding movement distances are calculated, as shown in Table 2.Table 1
Normal gait step length calculation (unit: m).
Actual distance
40.5
71.4
43.2
39.5
81.6
211.68
179
Constant model
38.608
73.766
43.348
40.421
81.526
213.771
182.391
Linear model
41.411
71.372
44.799
38.894
82.381
209.381
178.707
Nonlinear model
38.427
73.065
46.227
39.895
81.520
215.612
175.950Table 2
Abnormal gait step length calculation (unit: m).
Actual distance
55.2
52.5
40.5
40.8
31.8
39.5
81.6
Constant model
46.183
52.621
36.437
40.631
35.907
51.297
73.788
Linear model
46.865
54. 992
36.072
42.554
38.021
46.854
76.173
Nonlinear model
46.377
54.656
37.393
40.131
37.717
46.490
76.270The average absolute distance differences obtained by the three models are 5.298 m, 4.973 m, and 4.713 m, respectively, and the one calculated by Formula (4) is 89% and 95% of that calculated by the other two models, respectively. The variances of the absolute distance differences of the three models are 19.74, 9.24, and 8.23, respectively, indicating that the nonlinear model is the most stable one with the highest accuracy.In summary, when a pedestrian moves in a normal gait, the linear model corresponding to Formula (3) is preferred for calculating the step length; when the pedestrian moves in an abnormal gait, i.e., in the running pattern, the nonlinear model corresponding to Formula (4) is preferred. In practical applications, the movement pattern of the pedestrian should be firstly identified, and then the appropriate model is selected according to the identification result.


### 5.3. Fusion Positioning Using Original UWB Data

#### 5.3.1. Analysis of Original UWB Data

As can be seen from Figure8, some UWB ranging data of Route 1 and Route 2 are blocked by the column, resulting in discontinuous and abrupt range measurement. More abnormal data appear on Route 2 due to the complexity of the route.Figure 8
Range measurement of (a) Route 1 and (b) Route 2.
(a)
(b)


#### 5.3.2. Analysis of the Estimated Trajectory

Figure9 shows the positioning results of Route 1 and Route 2. In the figure, UWB represents the positioning result of the raw UWB data; PDR represents the positioning result of the smartphone IMU; EKF represents the UWB/PDR fusion positioning result of the EKF algorithm; REKF represents the UWB/PDR fusion positioning result of the EKF algorithm based on the Mahalanobis distance.Figure 9
Positioning results of (a) Route 1 and (b) Route 2.
(a)
(b)In subgraph (a), for the UWB, most of the positioning points are consistent with the reference trajectory, but due to the blocking of the column and the pedestrian during walking, some abnormal points appear in the positioning result. For the PDR, the trajectory is relatively smooth, but due to the cumulative error of step length and direction, the overall positioning trajectory deviates from the reference one. For the EKF, advantages of UWB and PDR algorithms are used and most of the positioning result is consistent with the reference trajectory, but several large jumps appear since the abnormal UWB cannot be processed by this algorithm. For the REKF, the positioning result almost coincides with the reference trajectory, presenting strong ability to process abnormal data.Route 2 is more complex than Route 1. Therefore, in subgraph (b), for the UWB, more data are blocked by the pedestrian and the column, resulting in more abnormal points on the positioning result. For the PDR, the accumulation error of the direction grows with the increase of the number of turnings, resulting in a large shift in the overall trajectory. For the EKF, there are many jumps on the entire trajectory since the algorithm is unavoidably disturbed by the abnormal data. For the REKF, it still shows strong antinoise ability, with its positioning result basically consistent with the reference trajectory.Table3 shows the positioning results of different algorithms. From the perspective of the root mean square error (RMSE), the REKF algorithm presents the optimal performance. Subgraph (a) and subgraph (b) of Figure 10 show the values of γ corresponding to the four beacons of Route 1 and Route 2, respectively. Among them, γ is greater than the threshold of 6.2 for several times, indicating that abnormal UWB range measurement exist. The observation covariance is adjusted by γ, and the larger the abnormal ranging value, the larger the corresponding γ. For example, the maximum abnormal ranging value of Beacon 2 of Route 2 is 110m, and the corresponding γ is close to 5000. The effect of abnormal observations on the positioning result is inhibited by the dynamically changing γ.Table 3
Positioning error analysis of Route 1 and Route 2.
UWB
PDR
EKF
REKF
Route 1 (RMSE/m)
0.92
2.34
0.78
0.35
Route 2 (RMSE/m)
1.44
3.26
1.04
0.45Figure 10
γ values of (a) Route 1 and (b) Route 2.
(a)
(b)


### 5.4. Fusion Positioning Using UWB Data Contaminated with Noise

To further analyze the performance of the REKF algorithm, three kinds of noise are injected into the UWB data of Route 2. The first one is uniformly increasing random noise of different ratios; the second one is continuous noise randomly injected; the third one is noise caused by the randomly blocked UWB beacons over a continuous time period.


#### 5.4.1. Uniformly Increasing Random Noise of Different Ratios

Gaussian white noise with an intensity of 30 dBW is used. The ratios of the injected noise account for 20%, 10%, and 7% of the total number of measurements, respectively. Figure11 is the result of the four beacons with 20% Gaussian white noise randomly and uniformly injected.Figure 11
4 beacons with 20% Gaussian white noise randomly and uniformly injected.Figure12 shows the positioning results of the REKF algorithm with 20%, 10%, and 7% noise injected, respectively. It can be seen that the REKF algorithm will be affected to a certain extent with the increase of the noise ratio, but compared to the continuous noise which will be explained in the next section, this uniformly injected noise results in a relatively small effect on the system state. The correct system state is used by the REKF algorithm to determine the quality of the current ranging value, thereby determining whether to rely more on the results of the PDR algorithm or not. In general, the trajectory obtained by using the REKF algorithm is basically consistent with the reference trajectory. The several jumps in the figure are caused by the adjacent relationship between the injected noise and the noise of the raw data, since such continuous error will cause instability of the system state.Figure 12
Positioning results of Route 2 with UWB noise of different ratios injected.


#### 5.4.2. Continuous Noise Randomly Injected

Figure13 shows the results of Beacons 2–4 with 30 dBW Gaussian white noise continuously injected. The data segments with noise injected are 90∼120 and 200∼230.Figure 13
3 beacons with continuous noise injected.Figure14 shows the positioning results when different numbers of beacons are affected by such noise. It can be seen that when such noise is simultaneously injected into three beacons, the positioning result is seriously affected, as shown by the blue trajectory. This is because continuous abnormal data will cause large errors in the system state, and when the system state is inaccurate, the determination of the UWB observation’s reliability will also be inaccurate, resulting in a wrong estimation of the observation covariance, and a system state which is difficult to be corrected. In other words, when the system state deviates from the real trajectory, the following correct observations will be defined as outliers. When such noise is injected into one or two beacons, the positioning result is less affected. In the robust EKF algorithm, 2 to 3 reliable UWB observations are used to maintain the positioning accuracy of the system.Figure 14
Positioning results of Route 2 with continuous noise injected into Beacons 1–3.


#### 5.4.3. Noise Caused by the Blocked UWB Beacons over Continuous Time Period

Figure15 shows the results of the four beacons being blocked on data segments of 90∼120 and 200∼230. Figure 16 shows the positioning results when different numbers of beacons are affected by such noise. It can be seen that when four beacons are simultaneously blocked, the positioning result is completely dependent on the PDR algorithm, and the positioning trajectory over the blocking time period is basically consistent with that of the PDR algorithm. When three or two beacons are blocked, the positioning result is less affected. In other words, even if only one beacon’s data are received by the smartphone, the positioning will also be assisted.Figure 15
4 beacons intermittently blocked.Figure 16
Positioning results of Route 2 with 2–4 beacons’ data intermittently blocked.


## 5.1. Experimental Setup

The test site is the underground garage of the University of Melbourne. As shown in Figure5(a), 4 UWB beacons were placed at the four corners of the rectangular area. MATE9 of Huawei, China, is selected as the experimental mobile smartphone, as shown in Figure 5(b). The chip of BeSpoon, France, is used as the core chip of the UWB tag/beacon. The UWB tag and the smartphone are connected via a serial port. In order to keep the data of the IMU and the UWB synchronized, the ranging data from both the IMU and the UWB are simultaneously received by the smartphone. During the walking process, the smartphone is steady held in the hand of the pedestrian, basically keeping a uniform motion state. The azimuth of the smartphone is taken as the heading angle of the human body.Figure 5
Experimental setup and equipment. (a) The underground garage and (b) UWB tag and smartphone.
(a)
(b)Two routes are set in the experiment: one is a rectangular route with fewer turnings and the other is an 8-shaped route with more turnings, as shown in Figure6. In the experiment, two laps are taken along each route in a clockwise direction. The start point and the end point are marked in the figure. The rectangular points in the figure denote the location of the four beacons, and the four red circles represent the four columns of the underground garage.Figure 6
Experimental routes. (a) Route 1 and (b) Route 2.
(a)
(b)


## 5.2. Step Detection and Step Length Calculation

### 5.2.1. Step Detection

The PDR experiment was conducted by two boys and two girls. The height and weight of the two boys are 1.8 meters and 70 kilograms and 1.78 meters and 72 kilograms, respectively. The height and weight of the two girls are 1.65 meters and 55 kilograms and 1.64 meters and 50 kilograms, respectively. The step detection result is shown in Figure7.Figure 7
Step detection result.
(a)
(b)In the table, “Walk” represents the normal walking pattern and “Run” represents the running pattern, including split-step running, stride running, and normal running. It can be seen from the accuracy rate that when the user is walking forward at normal velocity, the accuracy rate of step detection is up to 98% or more, with an average of 99.2%, and the number of wrongly detected step is within 2; when the user is running forward at different velocities, the average accuracy rate is 97.2%, with the minimum accuracy rate of 94.2%, and the maximum wrongly detected step number is 5. The results show that this method can be well applied to the normal walking pattern, while for the abnormal movement patterns, the parameters of step detection are required to be further studied and optimized.


### 5.2.2. Step Length Calculation

Formulas (2) and (3) of the linear model and Formula (4) of the nonlinear model based on acceleration statistics are verified in this paper. Four persons of different heights and different body sizes are selected to move in different patterns, including normal walking pattern and running pattern, in order to study the applicability and reliability of the three models in different movement patterns.Seven sets of data for the normal walking pattern, including walking along the straight line, walking along the fold lines, etc., are firstly selected. All the step lengths are accumulated to calculate the pedestrians’ movement distance, and the calculated value is compared to the actual distance. The least square method is used to solve the regression coefficients of the linear model and the coefficient of the nonlinear model. The regression coefficients of the linear model areA1=0.678, B1=−0.036, C1=0.031, A2=0.409, B2=0.011, C2=0.068. The coefficient of the nonlinear model is K=0.425. Then the step length is calculated, as shown in the following table.Table1 shows that the average absolute distance differences of the three models are 1.555 m, 0.931 m, and 2.032 m, respectively, with the maximum error no more than 5 meters. The variances of the absolute distance differences of the three models are 1.50, 0.61, and 2.04, respectively, indicating that the linear model is the most stable one with the smallest error. Similarly, four persons of different heights and different body sizes are selected to move in running patterns, including running along the straight line and running along the fold lines. Seven sets of running data are collected, and the corresponding movement distances are calculated, as shown in Table 2.Table 1
Normal gait step length calculation (unit: m).
Actual distance
40.5
71.4
43.2
39.5
81.6
211.68
179
Constant model
38.608
73.766
43.348
40.421
81.526
213.771
182.391
Linear model
41.411
71.372
44.799
38.894
82.381
209.381
178.707
Nonlinear model
38.427
73.065
46.227
39.895
81.520
215.612
175.950Table 2
Abnormal gait step length calculation (unit: m).
Actual distance
55.2
52.5
40.5
40.8
31.8
39.5
81.6
Constant model
46.183
52.621
36.437
40.631
35.907
51.297
73.788
Linear model
46.865
54. 992
36.072
42.554
38.021
46.854
76.173
Nonlinear model
46.377
54.656
37.393
40.131
37.717
46.490
76.270The average absolute distance differences obtained by the three models are 5.298 m, 4.973 m, and 4.713 m, respectively, and the one calculated by Formula (4) is 89% and 95% of that calculated by the other two models, respectively. The variances of the absolute distance differences of the three models are 19.74, 9.24, and 8.23, respectively, indicating that the nonlinear model is the most stable one with the highest accuracy.In summary, when a pedestrian moves in a normal gait, the linear model corresponding to Formula (3) is preferred for calculating the step length; when the pedestrian moves in an abnormal gait, i.e., in the running pattern, the nonlinear model corresponding to Formula (4) is preferred. In practical applications, the movement pattern of the pedestrian should be firstly identified, and then the appropriate model is selected according to the identification result.


## 5.2.1. Step Detection

The PDR experiment was conducted by two boys and two girls. The height and weight of the two boys are 1.8 meters and 70 kilograms and 1.78 meters and 72 kilograms, respectively. The height and weight of the two girls are 1.65 meters and 55 kilograms and 1.64 meters and 50 kilograms, respectively. The step detection result is shown in Figure7.Figure 7
Step detection result.
(a)
(b)In the table, “Walk” represents the normal walking pattern and “Run” represents the running pattern, including split-step running, stride running, and normal running. It can be seen from the accuracy rate that when the user is walking forward at normal velocity, the accuracy rate of step detection is up to 98% or more, with an average of 99.2%, and the number of wrongly detected step is within 2; when the user is running forward at different velocities, the average accuracy rate is 97.2%, with the minimum accuracy rate of 94.2%, and the maximum wrongly detected step number is 5. The results show that this method can be well applied to the normal walking pattern, while for the abnormal movement patterns, the parameters of step detection are required to be further studied and optimized.


## 5.2.2. Step Length Calculation

Formulas (2) and (3) of the linear model and Formula (4) of the nonlinear model based on acceleration statistics are verified in this paper. Four persons of different heights and different body sizes are selected to move in different patterns, including normal walking pattern and running pattern, in order to study the applicability and reliability of the three models in different movement patterns.Seven sets of data for the normal walking pattern, including walking along the straight line, walking along the fold lines, etc., are firstly selected. All the step lengths are accumulated to calculate the pedestrians’ movement distance, and the calculated value is compared to the actual distance. The least square method is used to solve the regression coefficients of the linear model and the coefficient of the nonlinear model. The regression coefficients of the linear model areA1=0.678, B1=−0.036, C1=0.031, A2=0.409, B2=0.011, C2=0.068. The coefficient of the nonlinear model is K=0.425. Then the step length is calculated, as shown in the following table.Table1 shows that the average absolute distance differences of the three models are 1.555 m, 0.931 m, and 2.032 m, respectively, with the maximum error no more than 5 meters. The variances of the absolute distance differences of the three models are 1.50, 0.61, and 2.04, respectively, indicating that the linear model is the most stable one with the smallest error. Similarly, four persons of different heights and different body sizes are selected to move in running patterns, including running along the straight line and running along the fold lines. Seven sets of running data are collected, and the corresponding movement distances are calculated, as shown in Table 2.Table 1
Normal gait step length calculation (unit: m).
Actual distance
40.5
71.4
43.2
39.5
81.6
211.68
179
Constant model
38.608
73.766
43.348
40.421
81.526
213.771
182.391
Linear model
41.411
71.372
44.799
38.894
82.381
209.381
178.707
Nonlinear model
38.427
73.065
46.227
39.895
81.520
215.612
175.950Table 2
Abnormal gait step length calculation (unit: m).
Actual distance
55.2
52.5
40.5
40.8
31.8
39.5
81.6
Constant model
46.183
52.621
36.437
40.631
35.907
51.297
73.788
Linear model
46.865
54. 992
36.072
42.554
38.021
46.854
76.173
Nonlinear model
46.377
54.656
37.393
40.131
37.717
46.490
76.270The average absolute distance differences obtained by the three models are 5.298 m, 4.973 m, and 4.713 m, respectively, and the one calculated by Formula (4) is 89% and 95% of that calculated by the other two models, respectively. The variances of the absolute distance differences of the three models are 19.74, 9.24, and 8.23, respectively, indicating that the nonlinear model is the most stable one with the highest accuracy.In summary, when a pedestrian moves in a normal gait, the linear model corresponding to Formula (3) is preferred for calculating the step length; when the pedestrian moves in an abnormal gait, i.e., in the running pattern, the nonlinear model corresponding to Formula (4) is preferred. In practical applications, the movement pattern of the pedestrian should be firstly identified, and then the appropriate model is selected according to the identification result.


## 5.3. Fusion Positioning Using Original UWB Data

### 5.3.1. Analysis of Original UWB Data

As can be seen from Figure8, some UWB ranging data of Route 1 and Route 2 are blocked by the column, resulting in discontinuous and abrupt range measurement. More abnormal data appear on Route 2 due to the complexity of the route.Figure 8
Range measurement of (a) Route 1 and (b) Route 2.
(a)
(b)


### 5.3.2. Analysis of the Estimated Trajectory

Figure9 shows the positioning results of Route 1 and Route 2. In the figure, UWB represents the positioning result of the raw UWB data; PDR represents the positioning result of the smartphone IMU; EKF represents the UWB/PDR fusion positioning result of the EKF algorithm; REKF represents the UWB/PDR fusion positioning result of the EKF algorithm based on the Mahalanobis distance.Figure 9
Positioning results of (a) Route 1 and (b) Route 2.
(a)
(b)In subgraph (a), for the UWB, most of the positioning points are consistent with the reference trajectory, but due to the blocking of the column and the pedestrian during walking, some abnormal points appear in the positioning result. For the PDR, the trajectory is relatively smooth, but due to the cumulative error of step length and direction, the overall positioning trajectory deviates from the reference one. For the EKF, advantages of UWB and PDR algorithms are used and most of the positioning result is consistent with the reference trajectory, but several large jumps appear since the abnormal UWB cannot be processed by this algorithm. For the REKF, the positioning result almost coincides with the reference trajectory, presenting strong ability to process abnormal data.Route 2 is more complex than Route 1. Therefore, in subgraph (b), for the UWB, more data are blocked by the pedestrian and the column, resulting in more abnormal points on the positioning result. For the PDR, the accumulation error of the direction grows with the increase of the number of turnings, resulting in a large shift in the overall trajectory. For the EKF, there are many jumps on the entire trajectory since the algorithm is unavoidably disturbed by the abnormal data. For the REKF, it still shows strong antinoise ability, with its positioning result basically consistent with the reference trajectory.Table3 shows the positioning results of different algorithms. From the perspective of the root mean square error (RMSE), the REKF algorithm presents the optimal performance. Subgraph (a) and subgraph (b) of Figure 10 show the values of γ corresponding to the four beacons of Route 1 and Route 2, respectively. Among them, γ is greater than the threshold of 6.2 for several times, indicating that abnormal UWB range measurement exist. The observation covariance is adjusted by γ, and the larger the abnormal ranging value, the larger the corresponding γ. For example, the maximum abnormal ranging value of Beacon 2 of Route 2 is 110m, and the corresponding γ is close to 5000. The effect of abnormal observations on the positioning result is inhibited by the dynamically changing γ.Table 3
Positioning error analysis of Route 1 and Route 2.
UWB
PDR
EKF
REKF
Route 1 (RMSE/m)
0.92
2.34
0.78
0.35
Route 2 (RMSE/m)
1.44
3.26
1.04
0.45Figure 10
γ values of (a) Route 1 and (b) Route 2.
(a)
(b)


## 5.3.1. Analysis of Original UWB Data

As can be seen from Figure8, some UWB ranging data of Route 1 and Route 2 are blocked by the column, resulting in discontinuous and abrupt range measurement. More abnormal data appear on Route 2 due to the complexity of the route.Figure 8
Range measurement of (a) Route 1 and (b) Route 2.
(a)
(b)


## 5.3.2. Analysis of the Estimated Trajectory

Figure9 shows the positioning results of Route 1 and Route 2. In the figure, UWB represents the positioning result of the raw UWB data; PDR represents the positioning result of the smartphone IMU; EKF represents the UWB/PDR fusion positioning result of the EKF algorithm; REKF represents the UWB/PDR fusion positioning result of the EKF algorithm based on the Mahalanobis distance.Figure 9
Positioning results of (a) Route 1 and (b) Route 2.
(a)
(b)In subgraph (a), for the UWB, most of the positioning points are consistent with the reference trajectory, but due to the blocking of the column and the pedestrian during walking, some abnormal points appear in the positioning result. For the PDR, the trajectory is relatively smooth, but due to the cumulative error of step length and direction, the overall positioning trajectory deviates from the reference one. For the EKF, advantages of UWB and PDR algorithms are used and most of the positioning result is consistent with the reference trajectory, but several large jumps appear since the abnormal UWB cannot be processed by this algorithm. For the REKF, the positioning result almost coincides with the reference trajectory, presenting strong ability to process abnormal data.Route 2 is more complex than Route 1. Therefore, in subgraph (b), for the UWB, more data are blocked by the pedestrian and the column, resulting in more abnormal points on the positioning result. For the PDR, the accumulation error of the direction grows with the increase of the number of turnings, resulting in a large shift in the overall trajectory. For the EKF, there are many jumps on the entire trajectory since the algorithm is unavoidably disturbed by the abnormal data. For the REKF, it still shows strong antinoise ability, with its positioning result basically consistent with the reference trajectory.Table3 shows the positioning results of different algorithms. From the perspective of the root mean square error (RMSE), the REKF algorithm presents the optimal performance. Subgraph (a) and subgraph (b) of Figure 10 show the values of γ corresponding to the four beacons of Route 1 and Route 2, respectively. Among them, γ is greater than the threshold of 6.2 for several times, indicating that abnormal UWB range measurement exist. The observation covariance is adjusted by γ, and the larger the abnormal ranging value, the larger the corresponding γ. For example, the maximum abnormal ranging value of Beacon 2 of Route 2 is 110m, and the corresponding γ is close to 5000. The effect of abnormal observations on the positioning result is inhibited by the dynamically changing γ.Table 3
Positioning error analysis of Route 1 and Route 2.
UWB
PDR
EKF
REKF
Route 1 (RMSE/m)
0.92
2.34
0.78
0.35
Route 2 (RMSE/m)
1.44
3.26
1.04
0.45Figure 10
γ values of (a) Route 1 and (b) Route 2.
(a)
(b)


## 5.4. Fusion Positioning Using UWB Data Contaminated with Noise

To further analyze the performance of the REKF algorithm, three kinds of noise are injected into the UWB data of Route 2. The first one is uniformly increasing random noise of different ratios; the second one is continuous noise randomly injected; the third one is noise caused by the randomly blocked UWB beacons over a continuous time period.


### 5.4.1. Uniformly Increasing Random Noise of Different Ratios

Gaussian white noise with an intensity of 30 dBW is used. The ratios of the injected noise account for 20%, 10%, and 7% of the total number of measurements, respectively. Figure11 is the result of the four beacons with 20% Gaussian white noise randomly and uniformly injected.Figure 11
4 beacons with 20% Gaussian white noise randomly and uniformly injected.Figure12 shows the positioning results of the REKF algorithm with 20%, 10%, and 7% noise injected, respectively. It can be seen that the REKF algorithm will be affected to a certain extent with the increase of the noise ratio, but compared to the continuous noise which will be explained in the next section, this uniformly injected noise results in a relatively small effect on the system state. The correct system state is used by the REKF algorithm to determine the quality of the current ranging value, thereby determining whether to rely more on the results of the PDR algorithm or not. In general, the trajectory obtained by using the REKF algorithm is basically consistent with the reference trajectory. The several jumps in the figure are caused by the adjacent relationship between the injected noise and the noise of the raw data, since such continuous error will cause instability of the system state.Figure 12
Positioning results of Route 2 with UWB noise of different ratios injected.


### 5.4.2. Continuous Noise Randomly Injected

Figure13 shows the results of Beacons 2–4 with 30 dBW Gaussian white noise continuously injected. The data segments with noise injected are 90∼120 and 200∼230.Figure 13
3 beacons with continuous noise injected.Figure14 shows the positioning results when different numbers of beacons are affected by such noise. It can be seen that when such noise is simultaneously injected into three beacons, the positioning result is seriously affected, as shown by the blue trajectory. This is because continuous abnormal data will cause large errors in the system state, and when the system state is inaccurate, the determination of the UWB observation’s reliability will also be inaccurate, resulting in a wrong estimation of the observation covariance, and a system state which is difficult to be corrected. In other words, when the system state deviates from the real trajectory, the following correct observations will be defined as outliers. When such noise is injected into one or two beacons, the positioning result is less affected. In the robust EKF algorithm, 2 to 3 reliable UWB observations are used to maintain the positioning accuracy of the system.Figure 14
Positioning results of Route 2 with continuous noise injected into Beacons 1–3.


### 5.4.3. Noise Caused by the Blocked UWB Beacons over Continuous Time Period

Figure15 shows the results of the four beacons being blocked on data segments of 90∼120 and 200∼230. Figure 16 shows the positioning results when different numbers of beacons are affected by such noise. It can be seen that when four beacons are simultaneously blocked, the positioning result is completely dependent on the PDR algorithm, and the positioning trajectory over the blocking time period is basically consistent with that of the PDR algorithm. When three or two beacons are blocked, the positioning result is less affected. In other words, even if only one beacon’s data are received by the smartphone, the positioning will also be assisted.Figure 15
4 beacons intermittently blocked.Figure 16
Positioning results of Route 2 with 2–4 beacons’ data intermittently blocked.


## 5.4.1. Uniformly Increasing Random Noise of Different Ratios

Gaussian white noise with an intensity of 30 dBW is used. The ratios of the injected noise account for 20%, 10%, and 7% of the total number of measurements, respectively. Figure11 is the result of the four beacons with 20% Gaussian white noise randomly and uniformly injected.Figure 11
4 beacons with 20% Gaussian white noise randomly and uniformly injected.Figure12 shows the positioning results of the REKF algorithm with 20%, 10%, and 7% noise injected, respectively. It can be seen that the REKF algorithm will be affected to a certain extent with the increase of the noise ratio, but compared to the continuous noise which will be explained in the next section, this uniformly injected noise results in a relatively small effect on the system state. The correct system state is used by the REKF algorithm to determine the quality of the current ranging value, thereby determining whether to rely more on the results of the PDR algorithm or not. In general, the trajectory obtained by using the REKF algorithm is basically consistent with the reference trajectory. The several jumps in the figure are caused by the adjacent relationship between the injected noise and the noise of the raw data, since such continuous error will cause instability of the system state.Figure 12
Positioning results of Route 2 with UWB noise of different ratios injected.


## 5.4.2. Continuous Noise Randomly Injected

Figure13 shows the results of Beacons 2–4 with 30 dBW Gaussian white noise continuously injected. The data segments with noise injected are 90∼120 and 200∼230.Figure 13
3 beacons with continuous noise injected.Figure14 shows the positioning results when different numbers of beacons are affected by such noise. It can be seen that when such noise is simultaneously injected into three beacons, the positioning result is seriously affected, as shown by the blue trajectory. This is because continuous abnormal data will cause large errors in the system state, and when the system state is inaccurate, the determination of the UWB observation’s reliability will also be inaccurate, resulting in a wrong estimation of the observation covariance, and a system state which is difficult to be corrected. In other words, when the system state deviates from the real trajectory, the following correct observations will be defined as outliers. When such noise is injected into one or two beacons, the positioning result is less affected. In the robust EKF algorithm, 2 to 3 reliable UWB observations are used to maintain the positioning accuracy of the system.Figure 14
Positioning results of Route 2 with continuous noise injected into Beacons 1–3.


## 5.4.3. Noise Caused by the Blocked UWB Beacons over Continuous Time Period

Figure15 shows the results of the four beacons being blocked on data segments of 90∼120 and 200∼230. Figure 16 shows the positioning results when different numbers of beacons are affected by such noise. It can be seen that when four beacons are simultaneously blocked, the positioning result is completely dependent on the PDR algorithm, and the positioning trajectory over the blocking time period is basically consistent with that of the PDR algorithm. When three or two beacons are blocked, the positioning result is less affected. In other words, even if only one beacon’s data are received by the smartphone, the positioning will also be assisted.Figure 15
4 beacons intermittently blocked.Figure 16
Positioning results of Route 2 with 2–4 beacons’ data intermittently blocked.


## 6. Conclusions

A UWB/PDR fusion positioning method based on the robust EKF is proposed in this paper. In this algorithm, the Mahalanobis distance between the observation and the system state is calculated to update the observation covariance, inhibiting the effect of abnormal observations on the positioning results. In addition, the Madgwick algorithm is introduced into the heading calculation of the PDR algorithm, effectively suppressing the cumulative error of the heading calculation. The experimental results show that in the case of intermittent or continuous UWB ranging noise and signal interruption, the proposed method exhibits strong robustness, with positioning accuracy higher than that of the EKF algorithm. However, how to improve the performance of the algorithm in the presence of stronger UWB ranging noise and much more signal interruption is required to be further tested.



---

*Source: 8019581-2018-12-05.xml*