Augmented UWB-ZUPT-SLAM Utilizing Multisensor Fusion

This article proposes a generalized UltraWideBand (UWB)-Zero-velocity-UPdaTe (ZUPT)-simultaneous localization and mapping (SLAM) algorithm, a SLAM approach, utilizing a combination of foot-mounted localization systems integrating inertial measurement units (IMUs), UWB modules, barometers, and dynamically-deployed beacons incorporating UWB, IMUs, and reference barometers. The proposed approach leverages a ZUPT-aided Inertial Navigation System augmented with self-contained sensor fusion techniques to map unknown UWB beacons dynamically deployed in an environment during navigation and then utilizes the localized beacons to bound position error propagation. An experimental testbed was developed, and we conducted two series of experiments to validate the performance of the proposed approach. The first experiment involved high-accuracy motion capture cameras in generating ground truth, and the results showed that the proposed approach estimated positions of UWB beacons with a maximum localization error of 0.36 m, when deployed during the first 15 and 20 s of the navigation. In the second experiment, a pedestrian traveled for around 3.5 km in 1 h in a large multifloor indoor environment and deployed seven beacons, during the first 63, 151, 290, 399, 517, 585, and 786 s of the experiment. The proposed generalized UWB-ZUPT-SLAM had a 3-D mean absolute error of 0.48 m in this experiment, equivalent to 0.013% traveling distance.


I. INTRODUCTION
D EVELOPING an accurate and reliable universal pedes- trian navigation framework can enable multiple different location-based services, including contact tracing [1], clinical gait analysis [2], and firefighter tracking [3].The universal navigation framework is required to maintain a high positioning accuracy for a long duration and cover various navigationchallenging scenarios, where global navigation satellite systems signals are too weak for acquisition and tracking, visibility is poor due to smoke, airborne particles, or low-illumination situations, and infrastructures dedicated to navigation are not accessible [4].In order to operate seamlessly through extreme conditions, a navigation system integrating positioning technologies with complementary localization features is considered a preferable solution [5].
Inertial navigation systems (INS) are a popular option to be integrated into a pedestrian navigation system as INS operates in a self-contained manner, requires no installation time, and utilizes Inertial Measurement Units (IMUs) that provide consistently available measurements [6].Nevertheless, errors of a standalone INS accumulate over time and grow unboundedly [7].In the case of pedestrian navigation, IMUs can be mounted on a person's foot, and this placement allows for using a Zero-velocity-UPdaTe (ZUPT) algorithm as an aiding technique for INS [8].The ZUPT algorithm periodically resets velocity errors during the stance phases of a human gait cycle, significantly reducing the rate of error growth of an INS [9].The ZUPT-aided INS has been analytically predicted and experimentally demonstrated to achieve an error of less than 1% traveling distance [10].However, this navigation approach is still a dead-reckoning mechanism with unbounded error growths along the positions and yaw angle [11], implying that positioning errors would eventually exceed an acceptable range of localization accuracy.Additional augmentation techniques for the ZUPT-aided INS are needed in long-term navigation tasks.
Sensor fusion solutions that opportunistically utilize ultrawideband (UWB) are often considered a promising aiding source to bound position error growths of INS [12], [13], [14], [15].In pedestrian navigation, previous research has shown that, when integrated with predeployed UWB beacons with pre-surveyed known locations, the position error growth of the ZUPT-aided INS becomes bounded, and the corresponding positioning accuracy could be effectively extended in long-term navigation [16], [17].It is, however, not always a valid assumption that the locations of beacons can be surveyed prior to the beginning of a navigation task [18].To overcome the need to presurvey UWB beacons while preserving the property that UWB range measurements bounds position error growth of footmounted INS, Jao et al. [19] developed the UWB-Foot-SLAM, a simultaneous localization and mapping (SLAM) framework based on both UWBs and ZUPT-aided INS using foot-mounted IMUs.The UWB-Foot-SLAM algorithm presented in [19] will be referred to as the original UWB-ZUPT-SLAM throughout this article.Leveraging the properties that a ZUPT-aided INS has high positioning accuracy in the first few minutes of a navigation task, the original UWB-ZUPT-SLAM algorithm maps unknown locations of UWB beacons deployed on the fly in a navigation environment during this period.After the beacons are localized, range measurements between a UWB node colocated with the foot-mounted IMU and an environment-deployed UWB node can provide position compensations for foot-mounted INS.It was experimentally demonstrated that the original UWB-ZUPT-SLAM framework has an order-of-magnitude improved navigation accuracy, as compared with the standalone ZUPT-aided INS, and achieved loop-closure errors (LCEs) of 1.49 m in a 25-min experiment with a 1.5-km-long trajectory.This article aims to enhance the original UWB-ZUPT-SLAM algorithm with self-contained multisensor fusion solutions.
This article proposes an augmented UWB-ZUPT-SLAM framework.The proposed approach, illustrated in Fig. 1, considers pedestrian navigation hardware consisting of two subsystems: a foot-mounted platform and a set of multiple beacons, each of which is integrated with IMUs, barometers, and UWB transceivers.The augmented UWB-ZUPT-SLAM algorithm inherits the original UWB-ZUPT-SLAM algorithm's characteristics of using UWB beacons to bound the position error propagation of foot-mounted INS without the need to presurveying the beacons and enhances the original algorithm with three additional mechanisms: foot-to-foot ranging measurements, differential altimeters, and beacon motion detection.The contributions of this article are as follows.
1) Developing the augmented UWB-ZUPT-SLAM that simultaneously localizes a pedestrian's positions and maps positions of unknown beacons with measurements collected from all sensors integrated into a foot-mounted localization system and multiple beacons.2) Developing a hardware system that realizes implementation of the proposed algorithm.

II. PROPOSED APPROACH
This section defines terminologies used throughout this article, introduces an overview of the proposed augmented UWB-ZUPT-SLAM algorithm, and presents an extended Kalman filter (EKF) used to implement the proposed approach.

A. Algorithm Overview
The proposed augmented UWB-ZUPT-SLAM algorithm simultaneously localizes a pedestrian's positions and maps positions of unknown beacons with measurements collected from all sensors integrated into a foot-mounted localization system and multiple beacons.Fig. 2 shows a block diagram illustrating the proposed algorithm.The algorithm is implemented with an EKF framework.A transceiver UWB integrated into a beacon is called a seeded UWB.We consider a beacon to have two statuses: unlocalized and localized.An unlocalized beacon has an estimated position uncertainty significantly larger than that of a foot-mounted localization system, while a localized UWB beacon has a position uncertainty similar to or smaller than that of the foot-mounted system.At the beginning of a navigation task, all the beacons are considered to be in the unlocalized status, and the EKF initializes the position of the beacons with very large uncertainties.Right after the initialization, the filter enters the mapping mode.In this mode, the position state uncertainties of the unlocalized beacons are reduced, and those associated with the foot-mounted localization system increase.Once the beacons are considered localized, which is the case when the position uncertainties of the beacons become smaller than that of the foot-mounted system, the position error growth of the foot-mounted localization system is bounded with UWB foot-to-beacon range measurements.
In our proposed augmented UWB-ZUPT-SLAM, localization of a pedestrian uses a combination of the ZUPT-aided INS and measurements of foot-mounted barometers, reference barometers, foot-to-foot ranges, and foot-to-beacon ranges.Reference barometers monitor baseline air pressure changes and local pressure fluctuations and are used in combination with footmounted barometers to achieve a differential altimeter mechanism, which was demonstrated to improve the reliability of altimeter measurements [20].The foot-to-foot ranging approach uses distance measurements between the two feet to enhance a dual foot-mounted INS configuration [21].
Beacon motion detection is included in the proposed augmented UWB-ZUPT-SLAM to avoid violating the assumption made by the original UWB-ZUPT-SLAM that a beacon is always stationary.When a motion is detected by an IMU embedded in a beacon, referred to as an event IMU throughout the text, the estimated beacon's position can no longer be trusted, and our proposed algorithm re-initializes the associated positions and uncertainties.
The following sections present the EKF used to realize the proposed augmented UWB-ZUPT-SLAM algorithm.

B. EKF States and Initialization
The proposed augmented UWB-ZUPT-SLAM algorithm is realized in an EKF framework, shown in Fig. 2. N denotes the number of deployed UWB beacons.The locations of these beacons are initially unknown.The pedestrian's navigation system, however, knows the unique media access control addresses of the beacons.Therefore, the augmented UWB-ZUPT-SLAM does not need a data associate step when measurements are taken from the beacons.
1) Filter States: The EKF uses states that include orientations, velocities, positions, biases of two foot-mounted IMUs, biases of the two barometers, and the positions of beacons.The states are expressed as follows: where q L,k , v L,k , and p L,k ∈ R 3×1 are the orientation, velocity, and position states of the left foot of an agent expressed in the navigation frame.2) Filter Initialization: At the beginning of a navigation task, we assume a pedestrian remains stationary for a short period of time.During this period, the initial roll and pitch angles are calculated based on accelerometer measurements, yaw angles are set to zero, and accelerometer and gyroscope biases are estimated using the knowledge of the stationary status.These initialization approaches are discussed in detail in [19].The initial barometric altitude bias, b b,L,k and b b,R,k , are both set to zeros.When a seeded UWB with an identification (ID) number j is first connected to the foot-mounted UWB at time k, the proposed augmented UWB-ZUPT-SLAM sets the initial beacon positions, denoted as p B j ,0 , and its corresponding initial uncertainties, σ p B j ,0 , as follows: where D is the dimension of a navigation environment, and the choice of the left foot's position is based on a pedestrian navigation testbed, discussed later in Section III-A.

C. EKF Prediction Step
In the prediction step of the EKF, the states corresponding to the foot-mounted IMUs, including

expressed as
where A INS,L,k and A INS,R,k are the propagation matrices of strapdown INS associated IMUs mounted on both feet.σ 2 b is the process noise variance associated with barometer bias states of the two feet and Q INS,L,k and Q INS,R,k contain process noise associated with the two IMUs' noise characteristics, including the angle random walk (ARW) of the gyroscopes, the velocity random walk (VRW) of the accelerometers, the rate random walk (RRW) of the gyroscopes, and the acceleration random walk (AcRW) of the accelerometers.Detailed formulations of [22].

D. EKF Update Step
The update step of the EKF uses five different types of measurements, including pseudomeasurements of zero velocity, barometric altimeters, barometric altitude biases, foot-to-foot ranging, and foot-to-beacon ranging.
1) Zero-Velocity Updates: When a stance phase is detected, pseudomeasurements of zero velocity along the three axes, denoted as v ZUPT = 0 3×1 are feedback to the EKF.This article adapts the Stance Hypothesis Optimal dEtection (SHOE) detector [23].The ZUPT measurement models, z ZUPT,L,k and z ZUPT,R,k , measurement matrices, H ZUPT,L,k and H ZUPT,R,k , and measurement covariance matrices, R ZUPT,L,k and R ZUPT,R,k , of the two feet are expressed as follows: where σ 2 ZUPT is the noise variance of the zero-velocity measurement v ZUPT , and I n×n is the n × n identity matrix.It should be noted that the variance could be different when mounting an IMU at different positions and performing different activities [10], [24], [25], [26], [27], [28], [29].
2) Barometric Altimeter: This article converts measurements of barometric altimeters mounted on the left and the right shoes at timestamp k to measurements of vertical displacements in the navigation frame, denoted as d ⊥,L,k and d ⊥,R,k , respectively [30], [31], [32].The altitude estimates derived from a single barometer were considered to include biases caused by variations in baseline air pressure and local pressure fluctuation due to opening doors/windows or air conditioning control in a building.The measurement models of the EKF corresponding to the altimeters on the left and right feet, z ALT,L,k and z ALT,R,k , the associated measurement matrices, H ALT,L,k and H ALT,R,k , and noise covariance matrices, R ALT,L,k and R ALT,R,k , are described as follows: where σ 2  ALT is the noise variance of the foot-mounted altimeter measurements.
3) Differential Barometer: Reference barometers integrated into beacons are used to measure barometric altitude biases for both the left and right feet.These measurements collected from the ith beacon become available when the foot-mounted UWBs are within the line-of-sight (LoS) connection with the seeded UWB.Altitude estimates computed based on the measurements are denoted as h ref i ,k .When beacons are stationary, variations in h ref i ,k are considered barometric altitude biases.The EKF measurement model corresponding to the barometric altitude biases is expressed as follows: where h ref i ,0 is the first attitude measurement collected by the reference barometer on the ith beacon.Corresponding measurement matrices, H b,L,k and H b,R,k , and noise covariance matrices, R b,L,k and R b,R,k , are expressed as ALT i is the measurement noise variance of a reference barometer.
4) Foot-to-Foot Range Augmentation: This article follows [21] to implement UWB-based foot-to-foot range augmentation in a dual foot-mounted IMU framework, which only uses measurements collected under LoS conditions that are detected using the probabilistic power metric approach described in [33].The corresponding measurement model, z F2F,k , measurement matrix, H F2F,k , and measurement noise covariance matrices, R F2F,k , are described as follows: Authorized licensed use limited to the terms of the applicable license agreement with IEEE.Restrictions apply.
where σ 2 F2F is the noise variance of the UWB foot-to-foot range measurements.
5) UWB Foot-to-Beacon Range Updates: We denoted the foot-to-beacon range measurements between a foot-mounted UWB and a seeded UWB on the ith beacon as r UWB i ,k .Similar to the foot-to-foot range measurements, foot-to-beacon range measurements were processed with bias compensation and used only when collected under LoS conditions.The corresponding footto-beacon range measurement model, z UWB i ,k , measurement matrix, H UWB i ,k , and measurement noise matrix, R UWB i ,k , are described as follows: where σ 2 UWB i is the noise variance of the foot-to-beacon range measurements between the foot-mounted UWB and the ith UWB beacon.Note that the left foot-mounted UWB was chosen to be paired with seeded UWBs.This is a convenient choice based on a pedestrian navigation testbed that will be discussed later in Section III-A.
When a beacon's onboard IMU detects that the beacon is experiencing a motion, the proposed augmented UWB-ZUPT-SLAM resets the beacon's estimated positions and uncertainties to the initial values presented in (1).The motion detection mechanism follows the acceleration-moving variance detector described in [23].
In this article, the EKF update step stacks all available aforementioned measurements in each iteration.Settings of the noise parameters are determined based on noise characteristics of sensors involved in the hardware implementation.Table I gives the EKF parameters used in this article.

III. EXPERIMENTAL VALIDATION
This article developed a customized experimental prototype to realize the proposed augmented UWB-ZUPT-SLAM algorithm and evaluated the algorithm with two experiments.The first experiment was conducted in a small indoor area on a single floor with ground truth (G.T.) measurements collected by high-accuracy motion capture cameras.The second experiment involved testing in a large multifloor building for a long duration of time.This section describes the experimental prototype and presents the results of the two experiments.

A. Experimental Prototype
Fig. 3 presents a developed experimental prototype, including a reconfigurable multisensor pedestrian navigation testbed, referred to as the Lab-On-Shoe platform, and multiple integrated UWB beacon units, referred to as PEdestrian ultrawideBand Beacon Localization Enhancement (PEBBLE) 2.0 systems.[19] documented firmware and hardware of the Lab-On-Shoe platform, which had a sampling rate of 1000 Hz.The PEBBLE 2.0 system was designed based on the PEBBLE system, described in [19], with upgraded features that included a dual-core microcontroller ESP32, an LSM9DS1 IMU, an MS5303-01BA barometer, and a DWM1000 UWB module.The Lab-On-Shoe platform and the PEBBLE 2.0 system were powered by 5-V lithium-ion batteries.
Authorized licensed use limited to the terms of the applicable license agreement with IEEE.Restrictions apply.

TABLE II PERFORMANCE OF THE ZUPT-AIDED INS AND THE ORIGINAL UWB-ZUPT-SLAM IN THE EXPERIMENTS DISCUSSED IN SECTION III-B
Fig. 4. Block diagram illustrating firmware implemented on each beacon in the PEBBLE 2.0 system shown in Fig. 3.
Fig. 4 shows the firmware implementation of the PEBBLE 2.0 system.An ESP32 microcontroller was programmed in the FreeRTOS framework with two cores, core0 and core1, each with a clock rate of 240 MHz.On core0, we implemented the SPI communication protocol to collect information on the connected node ID number and measurements of range, transmitter power, receiver first pulse power, and power metrics at a rate of 10 Hz.All the UWB modules in the developed PEBBLE 2.0 system were programmed to operate in the tag mode.A DWM1000 UWB operating in the tag mode can only be paired with a UWB operated in the anchor mode, and the range measurements between the two UWBs were obtained through a two-way ranging method.Therefore, when within a detectable range, all the UWB modules involved in the PEBBLE 2.0 system were connected only to the UWB mounted on the left shoe of the Lab-On-Shoe platform.On core1, we implemented the I 2 C communication protocol to collect data from the LSD9DS1 IMU and the ME5803-01BA barometer at a rate of 100 Hz and 25 Hz.All the collected data, including IMU, barometer, and UWB, were logged into an SD card at a rate of 100 Hz through the SPI protocol.

B. Scenarios #1: A Small Area With Reference Motion Capture Cameras 1) Experimental Scenario:
The first experiment was conducted to extensively evaluate the original UWB-ZUPT-SLAM discussed in [19] and investigated the repeatability of the navigation performance.A subject was equipped with the Lab-On-Shoe platform and carried two beacons.The experimental scenario was described in Section IV-A in [19], where OptiTrack motion capture cameras were used to obtain the G.T. positions of the two beacons and the subject's feet.In this experiment, among all the measurements produced by the Lab-On-Shoe platform and the PEBBLE 2.0 system, we only used the ones collected by left foot-mounted IMU, left foot-mounted UWB, and seeded UWBs, as these modules were used in [19] to evaluate the original UWB-ZUPT-SLAM algorithm.The subject performed three different trials consisting of closed-loop trajectories with distinct patterns.Table II summarizes details of the experimental processes.At the beginning of each trial, the subject stood still at the origin of the course for 10 s.Two beacons, denoted as beacon #1 and beacon #2, were deployed at the beginning of the experiment.The first LoS range measurements of beacon #1 were collected at the 15 s timestamp, and the first LoS measurements of beacon #2 were collected at the 20 s timestamp.When the two beacons were both within connection range, their range measurements were processed sequentially.As a result, each iteration of the EKF only processed one beacon at a time.
2) Experimental Results: We compared the navigation solutions computed by the original UWB-ZUPT-SLAM algorithm with a standalone ZUPT-aided INS.The initial yaw angle and positions used in the estimated solutions were aligned with the coordinate system of the motion capture cameras.G.T. positions provided by the motion capture cameras are used to evaluate the accuracy of the estimated navigation solutions.We down-sampled our solutions from 1000 Hz to 120 Hz to align with the sampling rate of the motion capture cameras.This experiment used 2-D root-mean-squared-errors (RMSEs), 2-D final displacement errors, and 2-D maximum displacement errors as performance metrics.
Fig. 5(a)-(c) presents the two navigation solutions with position uncertainties, expressed in terms of 3× the standard deviation (3σ), predicted by the EKF.The ZUPT-aided INS solution only estimated the positions of the agent, while the UWB-ZUPT-SLAM estimated both the agent's and beacons' positions.Table II summarizes the quantitative navigation performance of the ZUPT-aided INS and the original UWB-ZUPT-SLAM.In all three trials, the experimental results show that the original UWB-ZUPT-SLAM algorithm had a smaller navigation error than the standalone ZUPT-aided INS and could achieve a mapping error of less than 0.5 m.Fig. 5(d)-(i) presents propagation profiles of the covariances associated with the position states of the subject and the beacons when using the original UWB-ZUPT-SLAM algorithm.The experimental results are discussed in the next section.
3) Discussion: A few remarks can be made about this experiment as follows.
1) It could be observed in Fig. 5(a)-(c) that the 3σ uncertainties of the two beacons at the end of each trial had different values.The values are lower than 5 m in the case of the square and the Fig. 8 shapes and larger than 5 m in the case of walking a straight line.In our opinion, this phenomenon is similar to the position dilution of precision in other RF-signal-based positioning systems [34], and the differences are results of different observability of the EKF with respect to foot-to-beacon range measurements.In the case of a straight-line motion, using one-dimensional range measurements led to difficulties in uniquely determining the positions of the beacon along the directions perpendicular to the direction of the straight-line motion.2) We could see in Fig. 5(d)-(i) that the agent's position uncertainties follow an increasing trend over time while the beacons' position uncertainties decrease.In this experiment, the original UWB-ZUPT-SLAM was considered to operate in the mapping mode, as the agent's position uncertainties had not grown beyond the beacons' position uncertainties at the end of the experiment.Therefore, the UWB range measurements did not have significant numerical impacts on the agent's estimated positions.3) Table II gives that both the duration and trajectory length of trial #2 were longer than trial #3, but the RMSEs of the solution estimated either by the ZUPT-aided INS or the Authorized licensed use limited to the terms of the applicable license agreement with IEEE.Restrictions apply.

C. Scenarios #2: A Large Area Including Multiple Floors 1) Experimental Scenario:
In the second experiment, the subject walked inside the Engineering Gateway building at the University of California, Irvine, on three different floors covering terrains of flat planes, stairs, ramps, and an elevator.The experimental scenario had a physical dimension of approximately 90 m × 25 m × 10 m.A point cloud representation of the experimental scenario is shown in Fig. 6.The subject started the experiment on the first floor of the building by standing still for around 10 s and then walked continuously along a similar closed-loop path for four rounds.Each round lasted around 15 min, and the traveling distance was around 900 m.The duration of the entire experiment was around 1 h, and the total trajectory length was approximately 3.5 km.At the end of each round, the subject returned to the starting location to evaluate an LCE.
Seven beacons of the PEBBLE 2.0 were deployed during the first round at different locations on the three floors of the building.Pictures of the deployment locations can be found in Fig. 6.Beacons with ID #13, #9, #7, #11, #1, #17 and #15 were deployed at timestamps of 63, 151, 290, 399, 517, 585, and 786 s, respectively.Beacons with ID #9 and #13 were deployed on the first floor, beacons with ID #1, #7, and #11 were deployed on the second floor, and beacons with ID #15 and #17 were deployed on the third floor.Figs. 6 and 7 present the navigation solutions of the nine different approaches.It should be mentioned that beacons' locations in Figs. 6 and 7 were not presurveyed but estimated by the corresponding algorithms.Table III gives quantitative errors of the estimated navigation results.The LCEs of different approaches at the end of each round are shown in Fig. 8. Fig. 9 shows the position uncertainties of different states predicted by the EKF in the experiment.The horizontal position uncertainties   III.
Authorized licensed use limited to the terms of the applicable license agreement with IEEE.Restrictions apply. in Fig. 9 were computed by summing the 3σ along the x-and the y-axes.Comparing all the navigation approaches, the proposed augmented UWB-ZUPT-SLAM had the minimum 3-D MAE of 0.48 m in this experiment, equivalent to 0.013% traveling distance based on a 3.5-km-long trajectory.Using 3-D MAE as a comparison metric, the augmented UWB-ZUPT-SLAM improved navigation accuracy by 3.08×, 19.7×, and 52.9×, as compared with the original UWB-ZUPT-SLAM, Deterministic navigation solution, and traditional ZUPT-aided INS.Discussions of the experimental results are presented in the next section.
3) Discussion: The following lessons can be learned by inspecting the experimental results presented in Figs.7-9 and Table III.

1) Integrating barometers into the navigation solutions can
enhance both localization and mapping accuracy along the vertical direction of the UWB-ZUPT-SLAM algorithm.The rationale behind the improvements is that when operating in the mapping mode, the performance of the UWB-ZUPT-SLAM highly depends on the ZUPT-aided INS.The ZUPT-aided INS is identified to have position drifts faster along the vertical than the horizontal direction [35] and was pointed out to have difficulties in tracking vertical displacement when operating in a moving elevator [32].Augmenting a ZUPT-aided INS with a barometer could greatly mitigate these issues.2) Reference barometers integrated into beacons could significantly improve vertical displacement estimated by the foot-mounted barometer in long-term tasks.In our experiment, we could observe that a standalone foot-mounted barometer had a height displacement error exceeding 3 m in the last 17 min of the experiment.This error was reduced when the differential barometer mechanism was used, as given in Table III.However, we would like to point out that the differential barometer mechanism contains errors contributed by a combination of multiple factors, including sensor errors, such as scale factors and biases, and the fact that the baseline air pressure experienced by the reference barometer and the foot-mounted barometer may be different due to local pressure variations.
3) The INS-UWB-SLAM solution shows a bounded position error growth in Fig. 8, but has the largest 3-D MAE error among all the navigation solutions discussed in this section.The large error was because the first beacon was deployed at around 1 min into the experiment, where the standalone INS solution had accumulated positioning errors higher than multiple 10 s of meters during this period.It could also be observed that the INS-UWB-SLAM had a maximum position error larger than 1000 m, which is orders of magnitude larger than all the solutions involving the ZUPT algorithm.4) Observing the uncertainty propagation of beacons in Fig. 9, we could see that beacons involved at a later stage of the experiment converge to a larger value, as compared with the ones that were involved at the early stage.This was because, in the early stage of navigation, foot-mounted IMU had smaller position uncertainties, allowing for updating positions of the beacon with a higher confidence level.At the later stage, the position uncertainty of the IMU increased, and the confidence level when estimating the beacon's position was also reduced.5) As compared with the original UWB-ZUPT-SLAM, the augmented UWB-ZUPT-SLAM uses augmentation with reference barometers and foot-to-foot ranging measurements.In such a configuration, the solution achieves a smaller navigation error.This improvement is attributed to the UWB-ZUPT-SLAM framework, in which the localization performance of the foot-mounted localization system directly affects the accuracy of both localization and mapping performance.
The experimental results presented in Section III-B and Section III-C demonstrate that the proposed augmented UWB-ZUPT-SLAM inherits the advantages of the original UWB-ZUPT-SLAM, which could simultaneously localize unknown beacons' with sufficiently high accuracy, and uses the augmentation with sensor fusion solutions to further improve both localization and mapping accuracy.The proposed augmented UWB-ZUPT-SLAM algorithm significantly outperformed the traditional ZUPT-aided INS in long-term pedestrian navigation accuracy by 52×, achieving LCE of less than 1 m after navigating for 3.5 km in 1 h.

IV. CONCLUSION AND FUTURE WORK
This article proposed an augmented UWB-ZUPT-SLAM algorithm that simultaneously localizes positions of a pedestrian and maps locations of unknown UWB beacons that are dynamically deployed during a navigation task.The augmented UWB-ZUPT-SLAM algorithm preserved the characteristics of the original UWB-ZUPT-SLAM but is enhanced with multisensor fusion involving differential barometer and foot-to-foot ranging.In the worse case scenarios of the experiment involving a motion capture camera system, the UWB-ZUPT-SLAM mapped positions of the two UWB beacons with a maximum displacement error of 0.36 m and estimated a pedestrian's position with an RMSE of 0.19 m, an LCE of 0.3 m, and a maximum error of 0.5 m.In the experiment involving a subject walking for approximately 3.5 km in 1 h on three different floors with seven beacons, ZUPT-aided INS, the deterministic navigation, the original UWB-ZUPT-SLAM, and the proposed augmented UWB-ZUPT-SLAM had 3-D MAE of 25.86 m, 9.94 m, 1.96 m, and 0.48 m.It was demonstrated that the EKF covariances associated with pedestrian's positions in the case of the UWB-ZUPT-SLAM framework were bounded.The experimental results show that the original UWB-ZUPT-SLAM algorithm could significantly improve the long-term positioning accuracy of a pedestrian INS using foot-mounted IMUs, and the augmented UWB-ZUPT-SLAM further extends the high accuracy performance.
Future research might be aiming to improve the UWB-ZUPT-SLAM framework in the following directions.
1) Initialization of UWB position states can affect the accuracy of the beacon's estimated position.In the proposed approach, a beacon's position is initialized with an agent's current position.During the experiments presented in Sections III-B and III-C, the agent deployed a beacon within reach, matching the design of our approach.However, the deployment could be done in a more flexible manner, such as by throwing beacons to distant locations.In such cases, the initialization mechanism discussed in this article could lead to the estimated beacons' location being stuck in a statistical local minimum, degrading the navigation performance.One potential approach to address this issue is to use multiple initial guesses of a beacon's location, compute the likelihood of each guess, and select the one with the highest likelihood.
2) The ability to identify and compensate for nonline-ofsight (NLoS) UWB range measurements directly affects both the mapping and localization performance of our proposed UWB-ZUPT-SLAM.The experimental prototype discussed in Section III-A included foot-mounted UWB modules.This configuration was designed to avoid the need to estimate relative positions between a UWB and an IMU attached to a pedestrian.However, such a design had more difficulties receiving LoS measurements, as the modules were close to the ground and a pedestrian body part could block the direct signal path [21].To improve the UWB range measurement accuracy, advanced LoS/NLoS detection and bias compensation approaches could be advantageous [17].
3) It was demonstrated in Section III-C that position uncertainties of a beacon estimated by the UWB-ZUPT-SLAM framework are not fixed values but varying values due to the stochastic nature of the EKF used to realize the UWB-ZUPT-SLAM.The position uncertainties of beacons are affected by multiple factors, including settings of the process and measurement noise covariance matrix, timestamps of the first LoS foot-to-beacon UWB measurements, patterns of a pedestrian's trajectory, a pedestrian's activities, stance phase detection performance, and noise performance of inertial sensors and UWB modules.Future research is needed to determine the contributing factors, which may be achieved with an analytical expression of the uncertainty or a numerical simulation.4) The proposed augmented UWB-ZUPT-SLAM algorithm and the original UWB-ZUPT-SLAM discussed in [19] were realized in a centralized framework, where all the states were updated in every iteration of the EKF, even if some of the beacons were not connected.The proposed approach could be extended to a decentralized framework [33], which would be computationally less expensive and more friendly for real-time implementations.Investigating the tradeoffs between navigation performance and computational complexity of centralized and decentralized realizations would be beneficial.5) The power consumption performance of dynamically distributed beacons can be further improved.In our current settings, a beacon, after being deployed in a navigation environment, only provides range measurements to the foot-mounted system briefly and remains idle for the majority of the time.During the idleness, the beacon can be designed to go into "sleep" mode, where much less power is consumed, and "waken up" by signals with a frequency range lower than a UWB. 6) The performance of localization of pedestrians and mapping unknown beacons in the UWB-ZUPT-SLAM framework depends on the performance of the built-in ZUPTaided INS.We demonstrate in this article that using selfcontained enhancement for the ZUPT-aided INS could altogether augment the performance of the original UWB-ZUPT-SLAM.In the literature related to foot-mounted IMUs, many enhancement sources from multiple different aspects of the system have been investigated, Authorized licensed use limited to the terms of the applicable license agreement with IEEE.Restrictions apply.
including robust stance phase detection [24], [28], [37], additional sensor fusion solutions [38], [39], and IMU compensation [35], [40].We believe that combining some of the techniques with our UWB-ZUPT-SLAM framework could to improvement in navigation performance and reliability, but further research is needed to have experimental validation and quantitatively evaluate improvements brought by the additional sensing modalities.

Fig. 2 .
Fig. 2. Block diagram illustrating the proposed augmented UWB-ZUPT-SLAM algorithm.The algorithm involved two IMUs, two barometers, and two UWBs mounted on a foot-mounted localization system, as well as seeded UWBs, reference barometers, and event IMUs integrated into beacons to be deployed in an operating environment during a navigation task.Schematics of the two feet are identical, and that of each beacon is the same.
b g,L,k and b a,L,k ∈ R 3×1 are the gyroscope and accelerometer biases along the three axes of the body frame of the IMU mounted on the left foot.b b,L,k represents barometric altitude biases experienced by the left foot-mounted barometer.q R,k , v R,k , p R,k , b g,R,k , b a,R,k , and b b,R,k represent the orientation, velocity, position, gyroscope bias, accelerometer bias, and barometric altitude bias states associated with the right foot.p B 1 ,k , . . ., p B N ,k ∈ R 3×1 represent the position of the N beacons being deployed.
and b a,R,k , are propagated with the strapdown INS algorithm[6].The barometric altitude biases are assumed unchanged, and the position states of the beacons are assumed constant.The linearized continuous-time EKF propagation matrix, A k , and the process noise matrix, Q k , are Authorized licensed use limited to the terms of the applicable license agreement with IEEE.Restrictions apply.

Fig. 3 .
Fig. 3. Picture showing prototypes of a Lab-On-Shoe platform and three beacons of a PEBBLE 2.0 system.

Fig. 5 .
Fig. 5. (a)-(c) Estimated (Est.)navigation solutions computed with the original UWB-ZUPT-SLAM in the experiment discussed in Section III-B.Items colored in black correspond to the G.T. collected by motion capture cameras.It could be seen that the Est. and G.T. trajectories have small discrepancies.Quantitative evaluation of the estimated solutions is summarized in Table II.The radius of each dashed circle represents three times the position standard deviation predicted by the EKF at the end of the experiments.Positions of Beacon #1 (B1) and Beacon #2 (B2) are marked with star and diamond symbols, respectively.(d)-(i) Position estimates and its associated covariances of the original UWB-ZUPT-SLAM algorithm in the experiment discussed in Section III-B.It could be observed that the covariances of the agent's positions increased over time while the covariances of the beacons' positions were reduced.At the end of this experiment, the covariances of the agent's positions were still less than that of the beacons' locations.It could also be observed that the patterns of a subject's trajectories impact the mapping performance.

Fig. 6 .
Fig. 6. illustration of the experimental scenario and process.Seven beacons were deployed during the experiment discussed in Section III-C at different indoor locations.The locations of the beacons shown in this figure were not presurveyed but estimated by our proposed augmented UWB-ZUPT-SLAM algorithm.The point cloud representation of the experimental scenario is used as a visual reference.

2 )
Experimental Results: We compared the performance of nine different navigation solutions, including the traditional ZUPT-aided INS (ZUPT), ZUPT-aided INS augmented with a barometric altimeter (ALT), ZUPT-aided INS with a differential barometric altimeter (ALT+), ZUPT-aided INS augmented with barometric altimeters and foot-to-foot ranging (Deterministic), ZUPT-aided INS augmented with differential barometric altimeters and foot-to-foot ranging (Deterministic+), traditional INS-UWB-SLAM implemented in an EKF, the original UWB-ZUPT-SLAM, the original UWB-ZUPT-SLAM augmented with differential barometric altimeters (UWB-ZUPT-SLAM+), and the proposed augmented UWB-ZUPT-SLAM.Table III summarizes the description of each navigation solution.We used six performance metrics to evaluate the navigation solutions, including 3-D, 2-D, and vertical MAEs and 3-D, 2-D, and vertical LCEs.The MAEs were calculated based on the four estimated destinations at the end of each round of the close-loop path.

Fig. 7 .
Fig. 7. Navigation solutions using nine different algorithms in the experiment discussed in Section III-C.The solutions listed from top left to bottom right are traditional ZUPT-aided INS (ZUPT), ZUPT-aided INS augmented with a barometric altimeter (ZUPT/w ALT), ZUPT-aided INS with a differential barometric altimeter (ZUPT w/ALT+), ZUPT-aided INS augmented with barometric altimeters and foot-to-foot ranging (Deterministic), ZUPT-aided INS augmented with differential barometric altimeters and foot-to-foot ranging (Deterministic+), traditional INS-UWB-SLAM implemented in an EKF, the original UWB-ZUPT-SLAM, the original UWB-ZUPT-SLAM augmented with differential barometric altimeters (UWB-ZUPT-SLAM+), and the proposed augmented UWB-ZUPT-SLAM.The orange trajectories represent the estimated positions of the first loop of the experiment, and the blue trajectories represent the rest of the estimated positions.The duration of the experiment was around 1 h, and the trajectory length was around 3.5 km.The proposed augmented UWB-ZUPT-SLAM had the minimum 3-D MAE of 0.48 m, equivalent to 0.013% traveling distance based on a 3.5-km-long trajectory.Quantitative evaluation of all the navigation solutions is presented in TableIII.

Fig. 8 .
Fig. 8. Propagation of LCEs of different navigation solutions.We could observe that using self-contained aiding techniques for the ZUPT-aided INS could reduce the navigation error increase rate.The UWB-ZUPT-SLAM framework could effectively bound error growth, allowing for high positioning accuracy in long-term tasks.

TABLE III COMPARISON
OF NAVIGATION PERFORMANCE USING INS AIDED WITH DIFFERENT TECHNIQUES