A Loose-Coupled Fusion of Inertial and UWB Assisted by a Decision-Making Algorithm for Localization of Emergency Responders

Abstract

Combining different technologies is gaining significant popularity among researchers and industry for the development of indoor positioning systems (IPSs). These hybrid IPSs emerge as a robust solution for indoor localization as the drawbacks of each technology can be mitigated or even eliminated by using complementary technologies. However, fusing position estimates from different technologies is still very challenging and, therefore, a hot research topic. In this work, we pose fusing the ultrawideband (UWB) position estimates with the estimates provided by a pedestrian dead reckoning (PDR) by using a Kalman filter. To improve the IPS accuracy, a decision-making algorithm was developed that aims to assess the usability of UWB measurements based on the identification of non-line-of-sight (NLOS) conditions. Three different data fusion algorithms are tested, based on three different time-of-arrival positioning algorithms, and experimental results show a localization accuracy of below 1.5 m for a 99th percentile.

1. Introduction

Indoor localization is a fast-growing field of research that is attracting both academia and industry worldwide. With the recent advances in this field, new application areas demanding high accurate indoor positioning systems (IPSs) are emerging. Examples of such applications are positioning of employees in industrial environments and emergency responders’ operations, 3D motion detection, patient monitoring, and logistics. Among these, emergency responders’ positioning is regarded as one of the most difficult and challenging application scenarios. The surrounding environment is harsh, highly dynamic, covers vast operative areas, and is unstructured [1,2,3,4,5,6,7].

Traditionally, two leading technologies are used for indoor localization, radio transceivers (e.g., Wi-Fi, Bluetooth, ultrawideband—UWB) and inertial measurement units (IMUs). However, both technologies have some limitations that severely impact the IPS performance. On the one hand, the low availability and stability (i.e., received signal strength—RSS—fluctuation and the non-line-of-sight—NLOS—impact on time-of-arrival—ToA—methods) of radio frequency (RF) signals in indoor environments are the main limitations of the radio signal-based IPSs. On the other hand, the intrinsic drift associated with the sensors that make up an IMU degrades the accuracy of the position estimate over time. To overcome these limitations, several researchers proposed IPSs that combine both approaches in a single solution. In a hybrid IPS, the two subsystems (radio signal- and IMU-based) calculate the first responder’s position independently, and then a data fusion algorithm fuses both position data to provide a single position estimate. Data fusion has proven to be an essential tool to enhance the performance of localization systems in both indoor and outdoor scenarios. With the right combination of positioning technologies, data fusion approaches revealed to be effective in providing both short- and long-term position accuracy. This is an essential criterion for the emergency responders as their missions cover vast operational areas and for long periods [1].

In the literature, it is common to find different data fusion algorithms to fuse position estimates computed by IMU- and radio-based technologies. The data fusion on localization starts with the fusion of GPS and inertial sensors. The works in [8,9] rely on a Kalman filter (KF) to fuse the data obtained from a GPS receiver and inertial sensors. Since GPS signals are not available in indoor environments, other radio-based technologies have been used to perform data fusion. Deng et al. proposed an extended Kalman filter (EKF) to fuse Wi-Fi RSS with the smartphone inertial sensors [10]. In the work of Masiero et al., the Wi-Fi RSS is also fused with the smartphone inertial sensors, but they rely on a particle filter for the data fusion [11]. The geometrical information of the building is also used in the data fusion process. In the work of Waqar et al., a Bayes filter is used to fuse the motion information acquired from a smartphone and the Wi-Fi fingerprinting [12]. Fischer et al. proposed an EKF to fuse the position estimates from a PDR system and ultrasonic beacons [13]. An EKF is also used in the works [14,15] to fuse inertial data with UWB measurements. Yang et al. proposed an EKF to fuse the smartphone inertial sensors with the data of an acoustic localization system [16]. A finite impulse response (FIR) filter is proposed by Zhao et al. to fuse inertial and UWB measurements [17].

From the above works on data fusion for localization, the KF stands out as the most used method by researchers on the field. The popularity of the KF relies on the fact that it requires low computational resources, provides real-time localization and accurate position estimation. For these reasons, in this work, a KF is used to combine the position information obtained from the UWB and PDR systems. Like the approach in [16], data fusion is performed in two stages. In the first stage, the UWB measurements are analyzed to determine the usability of the UWB position estimate. The usability of the UWB measurements is assessed based on the NLOS identification that relies on channel impulse response (CIR) parameters, and the identification is performed based on a random forest model. In the second stage, the position estimates of both systems are fused using a KF. The main contributions of this work are the following:

• Development and integration of a hybrid IPS into a fully-functional personal protective equipment (PPE) that besides the position estimation is capable of monitoring physiological and environmental parameter and provides bi-directional communication between the emergency responder and the incident commander;
• A loose-coupled IMU/UWB position fusion based on a KF with different ToA-based positioning algorithms, which can compensate the ToA NLOS effect and IMU error accumulation problem;
• Assess the performance of the data fusion algorithm by using three different ToA-based positioning algorithms for the UWB position estimate;
• Development of a decision-making algorithm to assess the usability of the UWB data for the data fusion process.

The paper is organized as follows. Section 2 presents the proposed framework. This includes the architecture of the PROTACTICAL cyber–physical system (CPS), the pedestrian dead reckoning (PDR) method, the UWB positioning algorithms, the decision-making algorithm used to assess the usability of UWB position data, and the algorithm developed to fuse both position data. The experimental analysis and related discussion are reported in Section 3. Finally, Section 4 concludes the paper.

2. The PROTACTICAL Positioning Framework

In this section, the different components that make up the IPS of the PROTACTICAL CPS are described. Firstly, an overview of the PROTACTICAL CPS is provided, followed by a brief description of all system components. Then, the different subsystems of the hybrid IPS designed for the PROTACTICAL CPS are introduced, namely: the pedestrian dead reckoning (PDR), the UWB positioning, the algorithm for the usability analysis of the UWB data, and the algorithm that fuses both positioning estimates.

2.1. The PROTACTICAL Cyber–Physical System

The PROTACTICAL CPS is a smart personal protective equipment that was designed to enhance firefighters’ occupational health, safety, situational awareness, and comfort. It is capable of monitoring physiological and environmental parameters, locating and tracking firefighters, and detecting, in real-time, life-threating scenarios [18,19,20,21].

The PROTACTICAL CPS is based on a three-tier architecture (Figure 1). Tier one consists of a wireless body sensor network (WBSN)—WBSN-PROTACTICAL—that comprises several nodes: PROTACTICAL, designed to locate the firefighter and monitor physiological and environmental parameters, and a network coordinator (Gateway-PROTACTICAL). The WBSN-PROTACTICAL is based on ZigBee technology. The complete list of nodes that compose the WBSN-PROTACTICAL, as well as the corresponding monitored parameters is shown in

Table 1. Nodes and monitored parameters within the wireless body sensor network (WBSN)-PROTACTICAL.

Node	Garment	Monitored Parameters
1	Shirt	Heart rate
2	Shirt	Sweat detection and inner temperature (back)
3	Shirt	Breathing rate
4	Coat	Heat flux across the coat, inner temperature (front), and inactivity/fall detection
5	Coat	CO, CO2, environmental temperature, and relativity humidity
6	Boot	inactivity and relative position
7	Coat	inactivity, panic event, and user feedback
Gateway	Coat	absolute position (GPS), indoor location (UWB+PDR), environmental temperature

.

Tier two is the CPS network—ad hoc-PROTACTICAL—and is responsible for the bidirectional communication between the emergency responders and the incident commander. The CPS network is based on an ad-hoc routing principle, and the coverage of this network is expanded through the use of forward-PROTACTICAL as the emergency responders enter a building. This network is based on Wi-Fi technology.

Finally, tier three is the base station PROTACTICAL, which is responsible for storing and displaying the acquired information, managing the network, and triggering distress messages towards the emergency responders if a life-threatening event is detected.

2.2. The Pedestrian Dead Reckoning Subsystem

The PDR subsystem is an inertial navigation system (INS) that continuously estimates the position of the emergency responder based on the accelerations and angular velocities measured from a triad of accelerometers and gyroscopes. To minimize the position drift incurred because of the inertial sensors’ measurements drift over time, an EKF is fed with pseudo-zero-velocity updates (ZUPT) every time the emergency responder’s foot is in contact with the ground. Figure 2 shows the basic principle of operation of the ZUPT-aided EKF algorithm implemented for the PROTACTICAL CPS, and the pseudocode of the implemented algorithm is shown in Figure 3. A detailed description of the ZUPT-aided EKF algorithm, as well as the hardware specification for the node-PROTACTICAL 6, can be found in [21].

In the PROTACTICAL CPS, the node-PROTACTICAL 6 is responsible for computing the position of the emergency responder based on the PDR method. This node is placed on the instep of the foot, near the ankle, and is fastened to the shoe by the shoe’s laces.

2.3. UWB Positioning Subsystem

The DW1000 chip developed by DecaWave (Dublin, Ireland) is a UWB transceiver compliant with the IEEE802.15.4-2011 standard that revolutionized the UWB market as they provide very accurate ranging measurements at low transceiver cost (approx. €25 per unit) [22]. These UWB transceivers perform ranging measurements based on the two-way ranging (TWR) technique, which is a variant of the time-of-arrival technique. In the proposed system, the tag node is responsible for starting the ranging procedure and the anchor for computing the respective distance. The DW1000 UWB transceivers are configured to operate on channel four (500-MHz bandwidth with a center frequency of 3993.6 MHz), preamble length of 1024, and a data rate of 110 kb/s.

Based on the ranging measurements provided by the DW1000 UWB transceiver, the position of the emergency responder is calculated by three ToA-based positioning algorithms, namely: the analytical method, the nonlinear least-squares method based on a first-order Taylor expansion (for abbreviation we will refer to this algorithm as “Taylor series”), and the EKF. These algorithms were selected as they have different complexity and are designed to address different issues on localization. The analytical method is the simplest localization method with less computational requirements. However, the number of possible ranging measurement combinations has to be known beforehand since one equation has to be defined for each tag-anchor pair. On the other hand, the Taylor series and the EKF methods aim to deal with nonlinearity issues aroused by the localization problem and the covariance of the ranging measurements. Although the complexity of these algorithms is higher, it is expected to observe an improvement in performance when compared with the first two algorithms. While the Taylor series is an extension to the trilateration-based localization algorithms, the EKF is a predictive algorithm that aims to predict the next state based on a system model and the ranging measurements. A brief description of the three ToA-based position algorithms implemented in this work is provided in the following subsections. For a detailed description of the algorithms, the readers are referred to [19].

2.3.1. Analytical Method

In this work, the analytical method follows the linear implementation proposed by Caffery [23]. So, considering the challenging scenario where only three anchor nodes are available, the 2D localization problem can be expressed as a set of three equations with two unknowns:

$$\{\begin{array}{c}{\left(x-x_1\right)}^2+{\left(y-y_1\right)}^2=d_1^2\\ {\left(x-x_2\right)}^2+{\left(y-y_2\right)}^2=d_2^2\\ {\left(x-x_3\right)}^2+{\left(y-y_3\right)}^2=d_3^2\end{array}$$

(1)

where $x_{\mathrm{i}}$ and $y_{\mathrm{i}}$ are the known coordinates of the three anchor nodes, $x$ and $y$ are the unknown user’s coordinates, and the $d_{\mathrm{i}}^2$ are the distances measured by the DW1000 UWB transceiver.

By differencing the previous equations with a selected anchor node and rearranging the resulting equations, the tag’s position can be calculated as:

$$x=\frac{\left(y_2-y_1\right)C_3-\left(y_3-y_2\right)C_1}{\left[\left(x_3-x_2\right)\left(y_2-y_1\right)-\left(x_2-x_1\right)\left(y_3-y_2\right)\right]}$$

(2)

where:

$$C_1=\frac{1}{2}({\Vert A_2\Vert }^2-{\Vert A_1\Vert }^2+d_1^2-d_2^2)$$

(3)

$$C_3=\frac{1}{2}({\Vert A_3\Vert }^2-{\Vert A_2\Vert }^2+d_2^2-d_3^2)$$

(4)

where $\Vert A_i\Vert$ is the norm of the position of anchor $i$.

By substituting (11) into either (9) or (10) and solving in terms of $y$, gives:

$$y=\frac{\left(x_2-x_1\right)C_3-\left(x_3-x_2\right)C_1}{\left[\left(y_3-y_2\right)\left(x_2-x_1\right)-\left(y_2-y_1\right)\left(x_3-x_2\right)\right]}$$

(5)

2.3.2. Taylor Series Method

Since the 2D positioning is a nonlinear problem, the Taylor series method proposes a linearization strategy to correct the error of an initial position estimate $X_0$. Therefore, this linearization problem can be represented as the following:

$$d\left(X\right)\approx d\left(X_0\right)+{\mathrm{H}}_0\left(X-X_0\right)$$

(6)

where $X_0$ is the vector of the initial estimation, $X$ is the vector of the anchor nodes’ coordinates, and ${\mathrm{H}}_0$ represents the Jacobian matrix of $d\left(X\right)$ around $X_0$, which can be represented as:

$${\mathrm{H}}_0={\left[\begin{array}{c}\frac{\partial d_1}{\partial x} \frac{\partial d_1}{\partial y}\\ \frac{\partial d_2}{\partial x} \frac{\partial d_2}{\partial y}\\ \hfill \dots \hfill \\ \frac{\partial d_n}{\partial x} \frac{\partial d_n}{\partial y}\end{array}\right]}_{X=X_0}=\left[\begin{array}{c}\frac{x_0-x_1}{r_1} \frac{y_0-y_1}{r_1}\\ \frac{x_0-x_2}{r_2} \frac{y_0-y_2}{r_2}\\ \hfill \dots \hfill \\ \frac{x_0-x_n}{r_n} \frac{y_0-y_n}{r_n}\end{array}\right]$$

(7)

where $r_i=\sqrt{{\left(x_0-x_i\right)}^2+{\left(y_0-y_i\right)}^2}$.

It is worthy of mentioning that this linearization is only valid if the initial estimate is sufficiently closed to the actual location of the tag. In this work, the initial estimation is obtained from the analytical method described above. Assuming that the ranging measurements are independent and its error follows a Gaussian distribution, a weighted least squares solution for this problem can be as follows:

$$\mathsf{\delta }={\left({\mathrm{H}}_0{}^{\mathrm{T}}{\mathrm{R}}^{-1}{\mathrm{H}}_0\right)}^{-1}{\mathrm{H}}_0{}^{\mathrm{T}}{\mathrm{R}}^{-1}\mathrm{M}$$

(8)

For details about the different elements of the previous equation, the readers are referred to [19].

Based on the initial position estimation $\left(x_0,y_0\right)$ and the computed $\delta$, the position estimation can be updated as follows:

$$\{\begin{array}{c}x=x_0+{\delta }_x\\ y=y_0+{\delta }_y\end{array}$$

(9)

The position estimate is refined based on an iterative process.

2.3.3. Extended Kalman Filter Method

The performance of the EKF method is closely linked to the correct definition of a motion model for the system. Since human motion is highly unpredictable, the random motion model (i.e., model the motion as Gaussian noise) was selected in this work. Considering this design decision, the state transition model of the system can be defined as:

$${\mathrm{X}}_{k+1}={\mathrm{AX}}_k+{\mathrm{w}}_k$$

(10)

where ${\mathrm{X}}_{k+1}$ and ${\mathrm{X}}_k={\left[\begin{array}{cc}{x}_{tag}& y_{tag}\end{array}\right]}^{\mathrm{T}}$ represent, respectively, the current and the previous position state vectors. ${\mathrm{w}}_k$ is the process noise that allows changes in position and orientation with covariance matrix ${\mathrm{Q}}_k={\left[\begin{array}{cc}{ϵ}_x& {ϵ}_y\end{array}\right]}^{\mathrm{T}}$. The values of the covariance matrix ${\mathrm{Q}}_k$ were determined empirically. The matrix $\mathrm{A}$ represents the state transition matrix and is modeled as an identity matrix:

$$\mathrm{A}={\mathrm{I}}_2=\left[\begin{array}{c}\begin{array}{cc}1& 0\end{array}\\ \begin{array}{cc}0& 1\end{array}\end{array}\right]$$

(11)

The measurement model can be represented by:

$${\mathrm{Z}}_k=\mathrm{h}\left({\mathrm{X}}_k\right)+{\mathrm{v}}_x$$

(12)

where ${\mathrm{Z}}_k$ is the current ranging measurements vector, $\mathrm{h}\left({\mathrm{X}}_k\right)$ is the observation matrix, and ${\mathrm{v}}_x$ is the measurement noise whose covariance is ${\mathrm{R}}_k$. The index $k$ indicates that the parameters can change over time. The observation matrix $\mathrm{h}\left({\mathrm{X}}_k\right)$ and the corresponding Jacobian ${\mathrm{H}}_k$ are derived from (1) and are given as follows:

$$\mathrm{h}\left({\mathrm{X}}_k\right)=\sqrt{{\left(x-x_k\right)}^2+{\left(y-y_k\right)}^2}$$

(13)

$${\mathrm{H}}_k=\left[\begin{array}{cc}\frac{x-x_k}{\sqrt{{\left(x-x_k\right)}^2+{\left(y-y_k\right)}^2}}& \frac{y-y_k}{\sqrt{{\left(x-x_k\right)}^2+{\left(y-y_k\right)}^2}}\end{array}\right]$$

(14)

Based on the models described above, the EKF estimates the tag position based on two different stages: prediction and update. These two stages are described in detail on [19].

2.4. Usability Analysis of UWB Position Data

Despite the attractive features of UWB technology for indoor localization in harsh environments (e.g., robust signaling, through-wall propagation, and broad bandwidth that allows high-resolution ranging) [24,25,26,27,28,29], the NLOS propagation severely affects the performance of the UWB positioning system by adding positive bias in distance estimation [24,30]. As the number of anchor nodes in the NLOS condition increases, the UWB positioning performance significantly decreases. As demonstrated on the dynamic tests of [19], when the two anchor nodes are in NLOS condition, the positioning performance is unsatisfactory, even when a NLOS error mitigation algorithm is applied. So, to avoid fusing incorrect/inaccurate UWB position estimates, a decision-making algorithm has been developed to analyze the usability of the UWB data for position estimation.

A key part of the decision-making algorithm is a NLOS identification algorithm that allows distinguishing ranging measurements that were taken in LOS and NLOS. To build the NLOS identification algorithm, a measurement campaign was performed and allowed the collection of more than 18,000 samples in both LOS and NLOS propagation conditions (the occlusion of the human body generates the NLOS condition). In the defined setup, the distance between the anchor and the tag ranges from 1 up to 44 m. Since temporal and CIR data distribution statistics require an additional processing that can add a delay of 4–5 s per measurement [25], which can compromise the real-time requirements of IPS for emergency responders [1], to build this algorithm only CIR amplitude-based features that can be immediately, or with little processing, available from the DW1000 UWB transceivers built-in diagnostic capability were used. Examples of such features are: the first three points of the first path amplitude; standard deviation of the noise in the CIR accumulator; max growth of CIR power; max noise value; estimated received signal strength (RSS); estimated received power of the first path impulse; ratio of power of maximum noise and power in the first path; and power difference between the estimated RSS and the estimated power of the first impulse. These features were fed into different machine learning algorithms (e.g., k-nearest neighbor, naïve Bayes, support vector machine, random forest, and decision trees), and their performance was assessed based on a 10-fold cross-validation strategy. All features combinations were tested, from models with one feature to models with all 13 features. The best performing model was the random forest classifier with an accuracy of 98.4% on the test dataset, obtained for a model with three features (power difference, estimated received power of the first path impulse, and estimated power of the first impulse). Although models with more features exhibited higher accuracy (maximum accuracy of 98.8% for a model with 10 features), this model showed the best tradeoff between number of features/algorithm complexity versus accuracy. The detailed description of the measurement campaign, feature selection, and algorithm comparison/evaluation are being prepared to be published as a separate paper.

Unlike the decision-making algorithm proposed in [16], the decision-making algorithm here proposed does not make assumptions on the user mobility. The proposed algorithm is suitable for all movements, runs in real-time, and has low computational requirements, making it a viable solution for IPSs for emergency responders. At each time step, the decision-making algorithm checks if UWB measurements are available. If new UWB measurements are received, the NLOS identification algorithm is run to check if there are UWB measurements that were acquired in the NLOS condition. If two or more ranging measurements are acquired under NLOS influence, these measurements are discarded, the PDR system gives the user’s position. In this work, it is assumed that only three UWB ranging measurements are available. If more ranging measurements are available, the system has to have at least two ranging measurements in LOS to perform the data fusion with the UWB position estimate. Then, the UWB position is computed, and the data fusion algorithm is executed. Unlike the work in [16], the proposed method is not necessary to perform any additional validation on the UWB data. When the number of anchor nodes in the NLOS condition is less than two, the confidence on the estimated position is very high. Even if the previous measurements were rejected (based on the selected criterion) or lost. Figure 4 illustrates the flowchart of the decision-making algorithm proposed.

2.5. Fusing UWB and PDR Position Estimates

For the data fusion process, two sources of measurements are available, the UWB and the PDR. The PDR measurements are used in the prediction stage and the UWB measurements are used in the update stage. Since the target application is to track pedestrians, the process model is defined based on the PDR estimates, and the state model is given as follows:

$${\mathrm{X}}_k={\mathrm{FX}}_{k-1}+{\mathrm{Gd}}_k+{\mathrm{w}}_k,$$

(15)

where $\mathrm{F}$ and $\mathrm{G}$ are identity matrices, ${\mathrm{w}}_k$ is the process noise with covariance noise ${\mathrm{Q}}_k={\left[\begin{array}{cc}{\epsilon }_x& {\epsilon }_y\end{array}\right]}^T$, which are determined empirically. ${\mathrm{X}}_k={\left[\begin{array}{cc}{x}_k& y_k\end{array}\right]}^T$ is the current 2D coordinates of the pedestrian and ${\mathrm{X}}_{k-1}$ is the previous position of the pedestrian. ${\mathrm{d}}_k$ is the process model of the system and is given as:

$${\mathrm{d}}_k=\left[\begin{array}{c}d\cos \left({\psi }_k\right)\\ d\sin \left({\psi }_k\right)\end{array}\right],$$

(16)

where $d$ and ${\psi }_k$ are the step length and walking direction at the time step $k$, respectively. These estimates are obtained based on the PDR system described in Section 2.2.

When the UWB measurements are available, the position of the user is computed based on one of the methods proposed in Section 2.3, and the measurement model is given as:

$${\mathrm{Z}}_k={\mathrm{HX}}_k+{\mathrm{v}}_k,$$

(17)

where $\mathrm{H}$ is an identity matrix and ${\mathrm{v}}_k$ is the measurement noise with covariance matrix ${\mathrm{R}}_k={\left[\begin{array}{cc}{\epsilon }_x& {\epsilon }_y\end{array}\right]}^T$.

When a stance phase is detected, the prediction equations are applied:

$${\mathrm{X}}_k^{-}={\mathrm{FX}}_{k-1}+{\mathrm{Gd}}_k,$$

(18)

$${\mathrm{P}}_k^{-}={\mathrm{FP}}_{k-1}{\mathrm{F}}^T+\mathrm{Q},$$

(19)

Similarly, when the UWB estimates are available, the update equations can be applied:

$${\mathrm{K}}_k={\mathrm{P}}_k^{-}{\mathrm{H}}^T{\left({\mathrm{HP}}_k^{-}{\mathrm{H}}^T+{\mathrm{R}}_k\right)}^{-1},$$

(20)

$${\mathrm{X}}_k={\mathrm{X}}_k^{-}+{\mathrm{K}}_k\left({\mathrm{Z}}_k-{\mathrm{HX}}_k^{-}\right),$$

(21)

$${\mathrm{P}}_k=\left(\mathrm{I}-{\mathrm{K}}_k\mathrm{H}\right){\mathrm{P}}_k^{-},$$

(22)

where $\mathrm{I}$ in an identity matrix with appropriate dimensions.

The structure of the data fusion algorithm is given in Figure 5.

3. Results

In this section, we present some experiments we carried out to evaluate the effectiveness of the proposed hybrid localization approach. We designed two different experimental setups that impose different indoor propagation conditions and different tracks to challenge both the UWB- and the PDR-based positioning systems.

In the following subsections, we first describe the experimental setups considered for the IPS evaluation. Then, the performance metrics used to assess the system performance are described, followed by the results obtained in both scenarios. Finally, the performance of the decision-making algorithm is discussed.

3.1. Experimental Setup

In this section, we describe the deployment scenarios used to evaluate the performance of the data fusion algorithm proposed in this paper. In these experiments, DW1000 UWB transceivers and custom made IMU [21] are used to collect, respectively, the ToA-based ranging measurements and the inertial data needed to run the UWB positioning and the ZUPT-aided EKF algorithms, which are then used by the data fusion algorithm.

For the UWB positioning subsystem, two types of nodes were considered, anchor, and tag nodes. Both nodes were identical in terms of hardware. The anchor nodes were placed on a tripod at an antenna height of 1.33 m, and their position was known. The tag was responsible for starting the ranging message with an anchor node, computing the corresponding distance between the nodes, acquiring the channel propagation parameters necessary for NLOS identification, and logging this data to a computer through a USB connection. The tag node repeated this process continuously for all anchor nodes available, starting from anchor one to anchor n, where n was the number of anchor nodes available. This cycle was repeated until the user completes the predefined path.

For the PDR subsystem, the IMU raw data was collected at a frequency of 100 Hz and sent to a laptop via ZigBee for later processing. The full scale of the triaxial accelerometers and gyroscopes was set to 16 g and 2000 °/s, respectively. These ranges were chosen to guarantee that the sensor output does not saturate during the experiments. To guarantee that the system complied with the IPS requirements of emergency responders [1], only a minimalist calibration was performed. i.e., at the beginning of the experiment, the user was standing still for 30 s to calculate the gyroscope bias (by averaging the gyroscope’s outputs) and the sensor was powered on for several minutes before the data collection to handle the temperature effect.

The synchronization of the two subsystems (UWB and PDR) was an essential factor for data fusion performance. Since in the current version of the system, the two subsystems do not communicate, two MATLAB scripts were created to log, and timestamp the data. These scripts ran in parallel, and emulate a serial port. However, it was challenging to start both scripts at the same time. Since the sampling frequency of the IMU was known and was constant throughout the experiments, its script was the first to run and the system clock was logged. Then, the UWB script was run, and the system clock was also logged. Since the UWB measurements had a lower sampling rate (1–2 Hz), the MATLAB tic and toc functions were used to register the elapsed time between measurements. For the offline experiments, the start delay between the two experiments was computed based on the system clock logged. Then, this delay was added to the timestamps of the UWB measurements. A timestamp button was added to the UWB MATLAB script to help with the algorithms performance evaluation. Every time the user performed a turn, he/she pressed this button, and the system clock was logged in a file. This time was then used as a reference to calculate the accuracy of the system. By applying this method, the accuracy of the different methods was estimated in seven and nine points for the atrium and lab experiments, respectively. Figure 6 illustrates the two testbeds considered to evaluate the localization algorithms. The two scenarios were, respectively, an atrium with 9.4 × 7 m free space area (Figure 6a), and a lab with an area of 10.7 m × 7 m, desks, metallic cabinets, and textile machines (Figure 6b).

The gray squares represent the location of anchor nodes, and the red line represents the path performed. For each scenario, the experiment was run five times. During the experiments, no other people were allowed to stay or walk through the scenarios. All the localization algorithms were implemented in MATLAB, run offline, and use the same data set. In this way, we guaranteed that all the algorithms were evaluated under the same conditions and, therefore, a fair comparison could be performed.

3.2. Performance Metrics

The performance assessment of the proposed data fusion algorithm was based on the following performance metrics: accuracy and total distance travelled.

3.2.1. Accuracy

In this work, the accuracy is defined as the Euclidean distance between the estimated position and the coordinates of the reference points. The reference points corresponded to the turns that the user had to perform in each scenario. The accuracy ($E_{acc}$) is computed as follows:

$$E_{acc}=\sqrt{{\left(x_{Est}-x_{Actual}\right)}^2+{\left(y_{Est}-y_{Actual}\right)}^2},$$

(23)

where $\left(x_{Est},y_{Est}\right)$ are the Cartesian coordinates estimated by the localization algorithm, and $\left(x_{Actual},y_{Actual}\right)$ are the correct Cartesian coordinates of the reference points. The accuracy metric is presented as a cumulative distribution function (CDF) to give a better representation of the performance achieved by the different algorithms. For each CDF plot, we consider the accuracy computed for each reference point and all the track repetitions.

3.2.2. Total Distance Error

The total distance error ($E_{DT}$) is computed as follows:

$$E_{DT}=\frac{1}{n}{{\displaystyle \sum }}_{i=1}^n\left|D_{e,i}-D_T\right|,$$

(24)

where $n$ is the number of experiments performed, $D_{e,i}$ is the total distance travelled estimated at trial i, and $D_T$ is the exact total distance. For total distance for the atrium and lab scenarios is 29.04 and 22.64 m, respectively.

3.3. Experimental Results

In this section, the proposed data fusion algorithm is evaluated and compared with the UWB and PDR systems. Three different ToA-based positioning algorithms are tested for the calculation of the UWB position in the data fusion process.

3.3.1. Atrium Experiments

Figure 7 shows the CDF plot for the accuracy achieved by each algorithm in the atrium scenario. The dashed lines represent the position error obtained for the data fusion methods, the dash-dotted lines the position error of the positioning methods with only UWB measurements and the dotted line the position error obtained for the PDR method. The lines with a cross, triangle, and solid circle represent the positioning methods that rely on the analytical method, Taylor series, and EKF, respectively. As can be seen from Figure 7, the data fusion method with the analytical method and the PDR method achieved very similar performance with a position error below 1.5 m for a 99th percentile (1.38 and 1.47 m for data fusion method with the analytical method and the PDR, respectively). The performance of the data fusion method with the Taylor series was very similar to the former methods until percentile 95 but sufferrf a severe degradation for higher percentiles. This method haf the highest absolute position estimation error (4.75 m). As expected, ToA-based positioning methods haf the worst performance. i.e., for a 90th percentile, all methods had a position error above 2 m. This low performance can be explained by the impact that NLOS error has on the position estimate.

Figure 8 shows the paths estimated by the different positioning methods implemented in this work. Although the raw measurements (inertial data and UWB distance) were the same for all plots, we split the plot in three (one for each ToA-based positioning method) for easier comparison between the methods. As can be seen in the figure, all the data fusion-based methods showed better tracking capabilities than the ToA-based positioning methods and did not suffer from the drift like the PDR method. Except for the data fusion version that relies on an EKF to compute the UWB position, the other methods showed an estimated path very close to the travelled path. Additionally, all the data fusion versions successfully rejected the biased UWB position estimates, which validates the decision-making algorithm proposed.

3.3.2. Lab Experiments

Figure 9 shows the CDF plot for the accuracy achieved by each algorithm in the lab scenario. Like in the previous case, the dashed lines represent the position error obtained for the data fusion methods, the dash-dotted lines the position error of the positioning methods with only UWB measurements, and the dotted line the position error obtained for the PDR method. The lines with a cross, triangle, and solid circle represent the positioning methods that rely on the analytical method, Taylor series, and EKF, respectively. Based on the results of Figure 9, we can see that the data fusion methods outperformed the other methods for any percentile. Like in the atrium scenario, the performance of the data fusion method with the Taylor series suffered a degradation for percentiles higher than 90. However, it was still higher than the non-data fusion methods. For a 99th percentile, the position error reported by the data fusion methods was below 2 m (1.37, 1.81, and 1.59 for the data fusion methods with the analytical method, Taylor series, and EKF, respectively). Like in the atrium scenario, the ToA-based methods showed a position error around 3m. However, the worst performance was reported by the PDR method with a position error of 3.35 m for the 99th percentile. This error is explained by the drift accumulated after performing four consecutive turns. Figure 10 illustrates clearly this phenomenon.

The paths estimated by the different positioning methods are shown in Figure 10. Like in the previous scenario, the plot is split into three for a better comparison between the different methods implemented. Although the estimated path is not as smooth as the ones in the atrium scenario, it can be observed that the data fusion algorithm performs better than the other methods. The orientation drift of the PDR system has been corrected, and the UWB measurements in the NLOS condition have been successfully rejected. In this scenario, the data fusion method with the analytical method had the best performance. Like in the atrium, the EKF version had the worst performance in terms of the estimated path.

3.3.3. Decision-Making Algorithm Performance Evaluation

Table 2. Confusion matrix for the LOS/NLOS identification in the atrium scenario.

	LOS	NLOS
LOS	294	5
NLOS	59	200

and

Table 3. Confusion matrix for the LOS/NLOS identification in the lab scenario.

	LOS	NLOS
LOS	324	55
NLOS	52	245

show the confusion matrix for the LOS/NLOS identification in the atrium and lab scenarios, respectively. As can be seen from the tables, the accuracy of the identification on the LOS/NLOS scenarios was quite high for both scenarios (above 80% accuracy in both scenarios). However, in the atrium scenario, the accuracy was higher than in the lab scenario (88.53% and 84.17%, respectively). This difference in the performance can be explained by the influence of the surrounding environment on the algorithm performance. A clear example of the surrounding interference occurred on the atrium. As can be seen in Table 1, only 5 LOS samples were classified as NLOS, but 59 NLOS samples were classified as LOS. This misclassification mainly occurred when the pedestrian was walking near the walls, confirming that the multipath signals degrade the algorithm performance.

4. Discussion

For both scenarios, the superiority of the data fusion method proposed is quite evident. Overall, the path estimated by the data fusion algorithm is closer to the exact trajectory and successfully bound the errors of both methods. Typically, due to the superiority of UWB technology for ToA-based distance calculation, the ToA-based positioning algorithms have a superior and consistent performance throughout all the paths. i.e., its performance does not worsen with time like the IMU-based methods. However, UWB measurements can suffer severe degradation if the signal is blocked by the human body (the NLOS scenario). So, the proposed data fusion algorithm benefits from this knowledge in the sense that when the UWB position is reliable (two or more nodes under LOS condition), the data fusion algorithm gives more weight to those measurements, and the user’s position is computed with higher accuracy. When the UWB position is corrupted (NLOS scenario), the algorithm will use the relative path estimate obtained from the PDR algorithm. In this way, the errors of both systems can be bonded, resulting in a system with superior performance. The effectiveness of the proposed method is evident in the lab experiment. During the first half of the path performed in this scenario, two anchor nodes were in NLOS, leading to a low-quality ToA-based position estimate. On the other hand, the PDR system provided a reasonable position estimate at the beginning of the test, but it deteriorated after four consecutive turns in an area of one square meter in the middle of the path. However, with the proposed data fusion method, these performance limitations were attenuated, and the system was capable of providing a reasonable position estimate throughout the whole experiment.

Three variants of the data fusion method were evaluated during the experiments. These variants differ in the positioning method used to compute the UWB position estimate (analytical method, Taylor series, and EKF). Based on the results obtained from the experiments, the analytical method variant exhibited a higher performance in both scenarios. The EKF variant exhibited the worst performance among the three variants evaluated. This low performance can be explained by the low availability of measurements to perform the localization, due to the low sampling frequency of the UWB system, and the smoothing nature of this method. Because of the tracking nature of the EKF method, for a good performance, the EKF should be fed with new measurements at a constant sampling rate and relatively close to each other. However, during real-life scenarios, it is very likely the loss of UWB measurements during long periods, which will compromise the performance of the system. For this reason, the EKF method for ToA-based positioning is not recommended.

By comparing the results of the total travelled distance error (Figure 11), it can be observed that the data fusion methods performed significantly better than the UWB-based methods. Whereas, the PDR method had the best performance in the atrium scenario (0.76 m), but its performance significantly worsened in the lab scenario (3.78 m). This difference can be explained by the methods used to estimate the total distance travelled. For the PDR system, the travelled distance was calculated based on the sum of displacement between two sampling periods. Whereas, the UWB and data fusion methods calculated the displacement based on the Euclidean distance between two consecutive points. This method was also used to refine the step displacement required to feed the movement model of the Kalman filter used for data fusion. Additionally, by removing the UWB position estimates with two or more anchor nodes in NLOS, the error in the total distance travelled was significantly reduced. For the analytical variant, the data fusion approach in the atrium scenario achieved an error reduction in the total distance travelled of 42% for the UWB-based method and increased 276% compared to the PDR method. However, in the lab scenario, the error reduction was 89% and 50% compared to the UWB-based and PDR methods, respectively. Unlike the other methods, the error in the estimation of the total distance travelled of the EKF-based methods was due to the underestimation of the distance travelled. This phenomenon occurs because of the smoothing nature of the EKF method. Among the data fusion methods, the variant with the analytical method had the best performance in both scenarios.

Finally, the usability analysis of UWB data also proved to be an essential tool to improve the performance of the data fusion algorithm. As shown in Figure 7 and Figure 9, when two or more anchor nodes are in NLOS, the estimated position of all positioning algorithms became very unpredictable. The NLOS identification algorithm has revealed to be very useful in the identification of these measurements. The elimination of those estimations improved the accuracy of the IPS.

5. Conclusions

In this paper, several data fusion methods have been proposed and evaluated to fuse the inertial data and UWB ranging measurements in a loose-coupled way. A decision-making algorithm was developed to improve the accuracy of data fusion process. This algorithm assessed the usability of the UWB measurements based on CIR data and a random forest algorithm and fused both information if the UWB measurements are in LOS.

The different positioning strategies were evaluated in two scenarios with different propagation conditions (an open space environment—atrium—and a lab). Based on the results obtained, the data fusion method with the analytical method achieved higher position accuracy in both scenarios (a position error below 1.5 m) and provided the lower distance travelled error.

As far as future work is concerned, we plan to assess the performance of the proposed data fusion algorithm with different movement patterns (e.g., walking hunched, knee-dragging motion, running, and crawling). Furthermore, we plan to develop an algorithm for assisted deployment of waypoints as this is an indispensable asset for localization in unstructured environments as it is the case of the emergency responders [1]. With the placement of the waypoints on the fly, new geometric configurations of the anchor nodes will arise, and some will likely degrade the performance of the data fusion algorithm. A new decision-making algorithm may be necessary to deal with these situations.