Resilient Simultaneous Localization and Mapping Fusing Ultra Wide Band Range Measurements and Visual Odometry

In this paper we consider a Simultaneous Localization and Mapping (SLAM) problem for a moving agent using Visual Odometry (VO) while measuring the range from a set of Ultra Wide Band (UWB) antennas, deployed in unknown position in the environment. The solution approach is based on a switching observer which, under standard working conditions, for each observed UWB, uses a two dimensional Extended Kalman Filter (EKF) providing an estimate of the range and bearing of the observed UWB with respect to the agent. This information is then used in a Robust EKF algorithm which solves the SLAM problem with performances that, even before closing the loop, are comparable to the ones that a VO algorithm (namely ORB-SLAM2) would obtain only after closing the loop. Moreover, a resilient module is added to the algorithm to evaluate the reliability of the position estimate of each observed UWB. When the Visual Odometry is not available, the switching observer uses an auxiliary EKF to provide an estimate of the agent position. This makes the proposed approach robust with respect to several kinds of unmodeled disturbances, like multipath effects, and automatically adapts to sensor failures with resilience (e.g. when Visual Odometry or UWB measurements are not available).


Introduction
Localization is a key ingredient in several robotics applications.Nowadays, technologies like RFID, NFC, Bluetooth, UWB, WiFi signals are more and more considered in GPS denied areas (indoor or even outdoor environments where the GPS signal is not reliable or not precise enough), in view of several features, like, among the others, the availability of the ID of the responding landmark, which provides a hardware solution to the data association problem afflicting other kinds of measurements, like, e.g., laser range finders.Moreover, these kinds of measurements can operate in harsh environments characterized by dust, smoke or bad light conditions.Localization based on these technologies is possible if the position of the landmarks is known.If this is not the case, a SLAM problem arises, as considered in this paper, where the position of the landmarks should be determined while simultaneously localizing the agent.
A common characteristic of the technologies listed before, and in particular of the UWB signals considered in this paper, is that they provide only range measurements: this sets us in the context of a Range Only SLAM problem, where the initialization of a new observed feature is not straightforward due to the lack of its bearing.Several different approaches have been proposed in the years to face this problem: trilateration approaches (e.g.[1]) provide an initial estimation of the landmark position using three or more available range measurements.Other approaches consider probabilistic grids as an accumulator voting scheme [2], polar coordinates to capture the Range Only nature of the measurements [3] or least square based methods [4].Recently, solutions relying on the cooperation among landmarks have been explored as an effective way to reduce uncertainty (e.g.[5] and [6]).UWB localization and SLAM have been widely compared in [7] and visual-UWB navigation in unknown environments has been addressed in [8], showing the effectiveness of the combination of UWB data with visual odometry, however, without focusing on the system capability to adapt to sensor faults (resilience), which is one of the contributions of this paper.Resilient Range-Only SLAM has been presented in [9], where the authors presented a method that has been applied to a unicycle like vehicle equipped with encoders on the actuated wheels.In that paper, the authors focused on the resilience in terms of ability to cope with unmodeled shift of UWB landmarks, but they did not take into account temporary unavailability of encoder readings, nor the possibility to use other dead reckoning sensors, rather than encoders.
The Range-Only SLAM approach proposed in this paper is based on an on-line range and bearing estimation of any responding UWB antenna performed through an Extended Kalman Filter (EKF), one for each UWB landmark, fed with the Visual Odometry and the range measurements (see also [10] and [11] in a RFID context).The use of Visual Odometry allows to apply the approach in more general contexts where encoder readings are not reliable enough or simply not available (this may occur, e.g., in agricultural applications).The range and the bearing estimate of a responding UWB antenna, provided by the corresponding EKF, are fused with the VO through an EKF-SLAM algorithm, particularly suited in this context due to the availability of the data association, which allows to realize a map with the position of the UWB antennas while simultaneously localizing the agent inside this map.As shown in the paper, the proposed algorithm achieves performances comparable to those of ORB-SLAM2 (see [12] for reference) with limited overhead, with the additive feature of being robust with respect to several disturbances.In particular, unlike ORB-SLAM2, it is capable of functioning even when the Visual Odometry is not available for a while: in this case the UWB map is frozen and the agent localization is achieved by considering in the main EKF algorithm only the UWB measurements; in this case, the computation times are shorter compared to the ORB-SLAM2 algorithm.
Thus, the main contribution of this paper is manifold: first we propose a method to reconstruct the bearing of a responding landmark in case of range only measurements, thus solving a Range Only SLAM problem.Then, the proposed RO-SLAM algorithm is endowed with a resilient module to improve robustness against disturbances (from now on, indicated with REKF-SLAM).More in detail, the resilient module is designed to properly exploit the range and the bearing estimate of the UWB antennas, based on their level of confidence and is also able to understand if it is necessary to reinitialize the estimate of a UWB landmark position (caused, e.g., by a diverging estimate or even by an unknown shift of the UWB antenna).This also allows to overcome the problem of a bad feature initialization, which may seri-ously afflict other RO-SLAM approaches, where the feature estimate initialization is often performed only once at the beginning.Finally, the proposed algorithm is supervised by a switching observer reported in Fig. 1, which modifies the filter structure when a VO failure is detected.Furthermore, the proposed algorithm is capable of working with any number of UWB antennas (at least one should be there if VO is not available) that may also vary during the run.
Resilience in SLAM is a topic of particular interest nowadays, such as sensor fault diagnosis as in [13].Noise and outliers resilient SLAM has been proposed in [14] and in [15].However the problem of sensor faults in SLAM has not been covered in the previous works.In the present paper, the resilience to sensor faults has been achieved through the introduction of a resilient engine for EKF-SLAM and a switching observer.As for switching state observers, they have already been used for vehicle state estimation [16].Fault detection for switched systems has been developed in [17] for systems with unknown switching law.In this paper, the switching law is simple, and the switching observer has been synthesized focusing on the resilience to both camera and UWB sensor faults.

Notation and Problem Formulation
The system setup considered in this paper is an indoor environment with L UWB antennas located on the ground.The agent carries an UWB antenna at the same height as the other antennas and a camera while moving freely in the environment.The agent pose at time t is denoted with p t = (x a,t , y a,t , θ t ), with (x a,t , y a,t ) the agent position and θ t its orientation.A camera equipped with a visual odometry algorithm (in this paper ORB-SLAM2 has been used) is carried by the agent providing an estimation of the camera pose at each time step k and denoted with Then, the discrete time dynamics of the agent can be approximated by: The agent also carries an UWB antenna collecting the range measurements from the L UWB antennas located on the ground in unknown positions (x u 1 , y u 1 ), . . ., (x u L , y u L ).The collected signals at time k from the L antennas are denoted with ρ k which is defined as follows: where ρ 1,k , . . ., ρ L,k are the range measurements from the UWB antennas at time k.Each range measurement ρ i,k is affected by a zero-mean Gaussian noise n ρ i ,k , i = 1, . . ., L, with standard deviation σ ρ .
Fig. 1 The switching observer schema with the auxiliary EKF, range and bearing estimation, and Resilient EKF-SLAM (REKF-SLAM) blocks.The range and bearing estimation block is responsible for the estimation of the UWB antenna range and bearing.The estimation of the previous block is then fed to the REKF-SLAM update block com-puting the estimated agent pose.REKF-SLAM prediction block is used to generate the predictions used in the REKF-SLAM phase.The Auxiliary EKF takes the UWB range measurements as inputs and generates an estimated pose that is selected by the switching observer when the VO is not available The range ρ i,k and bearing β i,k of the antenna i has been defined as follows: (2) where (x u i , y u i ) are the coordinates of the UWB antenna u i .A schema of the system (for L = 3) is depicted in Fig. 2, where the black circle represents the agent with its heading, and the three red circles the UWB antennas with their ranges and bearings.The problem considered in this paper is the resilient simultaneous localization and mapping of the agent, so that the agent pose and the antennas position could be estimated at the same time while resisting the effects of the outliers in the measurements, using UWB readings and Visual Odometry.

Range and Bearing Estimation
Each antenna position with respect to the agent is estimated with an EKF fusing range measurements with visual odom-etry readings.The discretized range and bearing dynamics are given by: Using these equations and the antenna range measurement i, a two dimensional EKF (initialized with the available range measurement and an arbitrary bearing, e.g.0, characterized by a large standard deviation, e.g.π/3) can be designed to produce an estimate ( ρi,k , βi,k ).This estimate will be regarded, from now on, as a sensor able to provide a range and bearing measurement with an uncertainty captured by the covariance matrix of the EKF associated with the antenna i.However, in the first steps of execution, the EKF reconstructs values far from the real antenna position, so a method to cope with this problem must be adopted.

Simultaneous Localization and Mapping
For each antenna in the environment, the EKF based algorithm in Section 3.1 is initialized and executed.At each time step k, the estimated ranges and bearings ( ρ1,k , β1,k ), • • • , ( ρL,k , βL,k ) are computed for the L antennas.These estima- tions are then available to perform the EKF SLAM algorithm.The state vector, including landmark positions to map the features in the environment, is The dynamics of the system is the following: and will be synthetically referred to in this paper by The range and bearing measurements between the agent with pose (x a,k , y a,k , θ k ) and the antenna in position (x u i ,k , y u i ,k ) can be defined as follows: When measurements from L antennas are available, Eq. 7 becomes a vector h(x k ) whose 2L elements contain the range and bearing from the L antennas.Here we want to emphasize that when a landmark (UWB antenna) moves away from the sensing range and no measurements arrive from that UWB anchor, the system automatically discards the landmark as part of the SLAM map.The previously maintained estimates are not stored and they are not used when the landmark becomes available again.Specifically, the EKF-SLAM algorithm is structured in such a way that when the same UWB anchor reappears in range, it is incorporated as a new landmark by the system.The EKF-based SLAM algorithm can be easily obtained in a way similar to the one reported in [9] for the case of encoder readings.However, the process model covariance matrix Q k in the case of Visual Odometry can be created based on information provided on the current pose, assuming that the global features in the environment are only a function of the current pose, as suggested in [18].Let consider the following measurement model: where p C f = [p x, f p y, f p z, f ] T represents the pose of an ORB-SLAM2 feature with respect to the camera frame, R G C the rotation from the camera to the global frame, p G f the feature pose with respect to global frame and p G the camera pose with respect to global frame.In order to compute Q k , the following summation can be performed over the n f features: where and f y being the focal lengths in the x and y directions, respectively) and: It can be observed that the Visual Odometry data are used both in the prediction step of the EKF-SLAM algorithm (when computing the a priori estimate of the SLAM state) and in the correction step of the filter (when range and bearing from the UWB antennas, estimated using VO, are considered).As a consequence, a refinement of the algorithm could be designed to consider the correlation between the noise in the prediction and in the correction step of the filter, ignored for simplicity in the implementation of this paper.

Countering the Impact of Outliers
Outliers that do not fit the Extended Kalman Filter's presumed stochastic model can have an impact on range and bearing reconstruction, which could pose a problem when estimating the parameters.A resilient EKF should be created to withstand the effects of the outliers.A module to identify the existence of one or more outliers in the measurements should be part of the innovative filter.If there are outliers, robust estimation is created based on the statistical test of the normalized residual using a modified version of the IGGIII technique described in [19].

Resilient Engine for EKF-SLAM
Detection of the global outliers.The observations are shaped as a Gaussian distribution N (ẑ − k , C obs,k ), according to the EKF-SLAM algorithm presented in Section 3.2.The presence of observation outliers leads to the possibility that the assumption could be violated; in order to understand if an outlier for the measurement is there or not, the system must be provided with a test to guarantee that the actual measurement is comparable to that of the alleged model.A test can be defined in order to check that no measurement z i , i ∈ {1, . . ., 2L} is influenced by outliers; this test is called the null hypothesis.Taking into account the measurement probability density function, the statistic term can be exploited in order to check for errors in the model.In particular, as provided in [20], the Mahalanobis distance, denoted with M k , between measurement z k and ẑ− k (being the corresponding mean) can be used as an index to determine possible errors in the model.Specifically, the square of this quantity can be taken into account: When the null hypothesis is true, M 2 k should follow a Chisquare distribution with a number of degrees of freedom equal to 2L.A new statistical test must be then defined and, with that, an α threshold below which the null hypothesis must be rejected.Then the Chi-square distribution α-quantile χ α is defined in such a way that: where P(M 2 k > χ α ) represents the probability that M 2 k is larger than χ α .This probability should be small as α.For the current measurement zk , a new index can be defined and denoted with M2 k ; when this index results being larger than the α-quantile, the null hypothesis shall be rejected and there is the possibility of having multiple outliers in the current measurement (see [21] for reference).
Scaling for Kalman gain.Although it does not identify which measurements contain outliers, the detection of the global outliers provides a way for determining if the model complies with the parameters.An alternative hypothesis to the null hypothesis can be defined, stating that the measurement z i is an outlier for multiple fixed i.The value of the normalized observation residual for the measurement i at time step k, is defined as: where n k,i is the i th element of the residual of the measurement and c n i n i is the i th element on the diagonal of C obs,k .The quantity defined in Eq. 12 can be used to decide whether an observation is an outlier or not.In the case of an outlier, the covariance must be inflated; that can be obtained shaping appropriately the Kalman gain K k .Hence, a new Kalman gain can be defined as a robust gain matrix factor K ji : where a 0 , a 1 are the robust Kalman gain constants and j is the j th element in the state vector; the time step k has been omitted in Eq. 13 and it should be noted that the factor a 1 −w i a 1 −a 0 is raised to the third power as presented in [22].The rationale for this decision is that when the normalized measurement residual falls inside the range of (a 0 , a 1 ], we wish to further reduce the robust Kalman gain factor.

VO Failures: the Auxiliary EKF
When the Visual Odometry is not available, the information on the robot motion can be obtained from the available UWB applying a two dimensional Extended Kalman Filter, denoted in the following by auxiliary EKF.Let k 0 be the time step when the VO becomes not available.When this occurs, the UWB position estimate is frozen at its current value and only the estimate of the agent position (x a,k , y a,k ) is updated according to the UWB range measurements.This is performed by applying an Extended Kalman Filter, estimating (x a,k , y a,k ) only using the available UWB range measurements ρ i,k .The prediction step of the filter, as the odometry is not available, is applied to the following approximate robot motion model: where the noise terms n x,k and n y,k model the (unknown) agent displacement at step k and are assumed 0 mean Gaussian random variables with standard deviation σ d depending on the maximum expected agent displacement over a sampling step.The correction step of the EKF accounts for the range measurements coming from any available UWB i, which position is given by the last estimate ( xu i ,k 0 , ŷu i ,k 0 ) provided by the EKF-SLAM algorithm before the VO failure (i.e. at the switching time k 0 ).
The initialization of the EKF (i.e. of ( xa,k , ŷa,k ) and of the corresponding 2 × 2 covariance matrix P xy,k ) is based on the agent position estimate provided by the EKF-SLAM algorithm at the switching time.Specifically, ( xa,k , ŷa,k ) is assigned from the latest agent position estimate from the EKF-SLAM, while the covariance matrix P xy,k is initialized with the 2 × 2 block of the EKF-SLAM covariance matrix related to the agent position estimate.
Then, the equations of the prediction step of the filter are given by: ŷ− a,k+1 = ŷa,k where being I 2 the 2 × 2 identity matrix.If no UWB measurement is available at step k +1, we simply confirm the estimate of the prediction step, by assigning xa,k+1 = x− a,k+1 , ŷa,k+1 = ŷ− a,k+1 and P xy,k+1 = P − xy,k+1 .In the case, on the contrary, at step k + 1 some UWB measurements ρ i,k+1 are available, the correction step of the EKF can be applied as follows.Let L v be the number of UWB measurements available at step k + 1 and let be the expected range measurement from UWB i, i = 1, 2, . . ., L v .Let ρk+1 be the L v × 1 vector comprising all the expected available UWB measurements at time step k +1 (similarly let ρ k+1 be the vector of the corresponding actual measurements).Then, the equations of the correction step of the filter are given by: where H k+1 is the L v ×2 Jacobian matrix of the measurement model with respect to the state, i.e., for each available UWB measurement i, the i-th raw of H k+1 is given by The Kalman Gain K k+1 is given by given by the variance σ 2 k+1,i associated with the noise in the measurement coming from the i-th observed UWB at time k+1.This noise depends on the error characterizing the UWB sensor and on the uncertainty associated with the estimate of the i-th UWB position.Hence: where: • σ ρ is the standard deviation of the measurement error characterizing the UWB sensor; • P u i is the 2 × 2 block of the covariance matrix of the EKF-SLAM related to the estimate xu i ,k 0 and ŷu i ,k 0 of the UWB coordinates at time k 0 (i.e. when the VO starts to become unavailable); • H k+1,u i is the derivative of the range measurement from the i-th UWB with respect to the UWB coordinates and this is equivalent, apart from the sign, to the derivative with respect to the agent coordinates, i.e.H k+1,u i = −H k+1,i , where H k+1,i is reported in Eq. 22.

Switching Observer
The switching observer is responsible for the fault detection in the camera sensor which, through the use of the ORB-SLAM2 algorithm, provides an agent pose estimation xvo .Failures of VO are detected when the VO algorithm is not able to provide a new sample for specific reasons (e.g.indoor/outdoor transitions, an insufficient number of features, hardware failure, etc.).Specifically, the proposed system detects a failure in two cases: when the VO algorithm communicates that it was not able to provide a new pose and/or when the new pose does not arrive within a certain amount of time.When the camera or the Visual Odometry algorithm have a fault, the pose estimation is not available and the Eqs. 4 and 6 cannot be used as the quantities δ vo x,k , δ vo y,k , δ vo θ,k are not available.Thus the system needs to switch to the auxiliary EKF presented in Section 3.5, as shown in Fig. 1.The switching law that regulates the way the system acts through the switching signal σ is available and straightforward: when there is a camera fault or the Visual Odometry is not available, the system uses the auxiliary EKF, otherwise the Visual Odometry is used and the system works regularly as described in Sections 3.1, 3.2, 3.3.When the VO becomes not available, the last agent position estimate from EKF-SLAM is used by the auxiliary EKF and its covariance matrix is initialized with the EKF-SLAM covariance matrix related to the agent position estimate as previously described in Section 3.5.On the other hand, when the VO becomes available, the EKF-SLAM uses the pose and covariance matrix coming from the VO (ORB-SLAM2) as described in Section 3.2.

Computational Complexity
The algorithm, in normal execution (i.e. when VO is available), comprises two components: the main EKF-SLAM algorithm and the set of the local EKFs estimating the range and the bearing of each UWB antenna.The cost associated with all the local EKFs increases linearly with the number L of UWB antennas.The computational complexity of the main EKF-SLAM algorithm is cubic with the number of UWB antennas, since it estimates 3+2L variables (i.e. the robot pose and the (x, y) coordinates of all the L UWB antennas).
For sufficiently small L the computational complexity of the local EKFs may prevail over the complexity of the EKF-SLAM algorithm, which will become the bottleneck of the approach for large values of L.

Experimental Setup
The experiments have been carried out in an indoor environment (6 × 8.3) m cluttered with furniture, boxes (both wooden and metallic) and desks, as shown in Fig. 3-a.
The agent is equipped with an Intel RealSense D435i camera, an UWB EVB1000 board and a Laptop with Intel Core i7-2640M CPU @ 2.80GHz, 16GB RAM as depicted in Fig. 3-b.The agent equipment is mounted on a four-wheeled platform trolley which can be moved freely on the floor.
At the boundary of the walkable perimeter, three UWB EVB1000 boards that are at the same height as the one on the agent have been placed in unidentified positions.On the aforementioned hardware, the ORB-SLAM2 method has been built in order to compute the camera position at a rate of 15 Hz while the UWB range measurements are delivered at a rate of 24 Hz.The UWB range measurements are filtered with a median filter and a moving average filter before being passed to the REKF-SLAM algorithm.Furthermore, the UWB range present bias in the measurements and this has been captured in separate experiments; the bias characteristics are then used in the experiments to adjust the measurements before feeding them to the EKF-SLAM algorithm.In that way, we don't have to take into account it in the measurement model for the expected range both in Eq. 7) and in Eq. 19.The camera trajectory before closing a VSLAM loop was employed in the tests, and the camera trajectory following loop closure was also recorded as a benchmark.With a standard variation of 0.09 mm, the UWB board placements and ground truth points have been measured with a laser distance meter.In order to assess the proposed solution in terms of comparison between the estimated, open-loop and closed-loop Visual Odometry trajectories and switching observer resilience capability in case of camera sensor fault(s), the experiments involved moving the agent around the room while recording the UWB and the camera data.

Parameter Tuning
The ORB-SLAM2 algorithm has been modified in order to increase its performances (with the introduction of both computational and GPU optimizations) and to add more parameters control (open source code is available in [23]).The ORB-SLAM2 has been used in IR-D mode, using the Infrared and Depth streams from the RealSense camera at 30 Hz with a resolution of 640 × 480 pixels and the num-   [24], a 0 ranges in [1.0, 2.5] and a 1 ranges in [3.0, 8.0].For the experiments a 0 = 1.5 and a 1 = 3.5 have been chosen in order to achieve a good trade-off between marking an observation as an outlier and integrating it as a correct measurement.The standard deviation σ d has been chosen as 0.05 m, as it directly depends on the maximum expected agent displacement over a sampling step.

Experiments
In the first experiment, the agent has been pulled around the room for 24.23 m while the camera and the UWB antenna mounted on the agent were acquiring data from the environment.The agent movement has been stopped in 4 points that have been marked and used as waypoints for ground truth later on.During the experiment the open-loop Visual Odometry (OLVO) and the UWB ranges have been recorded and used respectively as the inputs xvo and ρ for the switching observer.The closed-loop Visual Odometry (CLVO), avail- able as soon as the ORB-SLAM2 algorithm has closed a loop, has been recorded as well.
Table 1 shows the comparison between the CLVO, OLVO of ORB-SLAM2 and the REKF-SLAM estimate evaluated in the four waypoints used in the experiments.The table provides mean, median and standard deviation for each trajectory, showing the effectiveness of the proposed solution, which avoids trajectory drifts that may arise in VSLAM algorithms if there is no loop-closure for a long distance.Figure 4 shows the estimation errors for the UWB antenna locations that, after an initial phase, become very small.In the first steps, the errors are very high due to the unknown bearing and also on the particular path that the agent has traveled.
In the second experiment the agent has been pulled around the room for 22.41 m while the camera and the UWB antenna mounted on the agent were acquiring data from the environment.The agent movement has been stopped in four points that have been marked and used as waypoints as mentioned in the previous experiment.During the experiment we introduced conditions that caused the VO to fail.Specifically, rapid and permanent changes in the environment illumination were introduced, causing the ORB-SLAM2 algorithm to fail.This prevented the system to use the open-loop Visual Odometry and thus forcing the switching observer to adapt to the new perception configuration (without any visual odometry available).The first fault occurs at about 26 s after the beginning of the movement and it lasts for about 14 s, while the second fault occurred at about 70 s after the start and it lasts for about 8 s.
Table 2 shows the effectiveness of the proposed solution, which both avoids trajectory drifts and guarantees the system resilience against camera sensor faults (i.e. the system is still able to work if the camera sensor has a malfunction).Figure 5 depicts the closed-loop, open-loop Visual Odometry and the estimated trajectories both during the normal operation and when the system detects camera faults.Furthermore, the trajectory with the second camera sensor fault is affected by strong multipath effect, as visible in the blue dashed upper left trajectory in Fig. 5. Nonetheless, the system is still capable of filtering the disturbances and can give an acceptable estimate of the agent trajectory.Fig-ure 6 presents the covariance plots for the estimated states From the figure it can be noted that the system provides consistent state estimates and is not over confident.The blue ellipses in the plots are related to the estimated states [x a,k , y a,k ] where the magnitude of the spread, along the axes of the ellipses, is given by the eigenvalues of the covariance matrix.The same reasoning applies to the green ellipses for the UWB anchor estimated states.
A video of the second experiment is available https:// youtu.be/kZd_W-vbFS0.As for computation times, the proposed algorithm adds a 5% overhead to the ORB-SLAM2 algorithm when it is functioning without any fault, while its computation time is drastically reduced when a camera fault arises, performing 130 times faster than the ORB-SLAM2 algorithm (computation times comparison have been performed on the hardware platform specified in Section 4.1).

EuRoC Dataset Experiments
Finally, our algorithm has been validated using public datasets and real hardware experiments.In particular, the presented algorithm has been compared to VINS-Mono, VIR SLAM and ORB-SLAM2 computing the absolute trajectory error (ATE) on the EuRoC dataset [25] where ground truth is also available.The EuRoC dataset does not embed UWB ranging measurements, so a methodology to generate and integrate those data into the dataset is required.
In [26] the authors suggest adding a static anchor assumed in the origin of the frame created during the robot initialization; then they added a Gaussian white noise N (0, 0.05) to model the error of the UWB sensor.However, modeling the error in such a way does not take into account the components related to the bias and the multipath errors of the UWB sensor.So, the strategy adopted in our experiments, in order to aggregate UWB data, is to train a deep neural Fig. 6 Covariance plots for the estimated states at different time-steps k for the second experiment of Section 4.3.The covariance plot for the state θ k has not been reported for the sake of clarity.The blue ellipses represent the covariances for the state x a,k , y a,k , while the green ellipses represent the covariances for the UWB anchor position estimated states.The red line represents the CLVO, the black plus signs the agent ground truth at the significant time-steps and the black o signs the real UWB anchor positions  network ( [27] and [28]) having as inputs real UWB sensor measurements, real agent and UWB positions in order to also embed information about the real error profile (comprising the Gaussian white noise, bias and multipath errors).
Then we pass the ground truth data from the EuRoC datasets with the virtual UWB positions and we use the trained DNN network to generate synthetic data that have been used to feed our algorithm and to test the system with a realistic set of data (images from the EuRoC original dataset and UWB deep generated data).We would like to stress that the real UWB measurements were acquired in an environment basically different from that of EuRoC however, measurements acquired in an environment with dimensions similar to that of the EuRoC MH_01 to MH_05 and it has been populated with objects similar to those in the EuRoC environment.Table 3 shows how our REKF-SLAM algorithm outperform VINS-Mono, ORB-SLAM2 and VIR SLAM, where a single virtual UWB anchor, assumed to be in the origin of the frame created during the robot initialization and whose measurements are always available, is used.The metrics used for the comparison is ATE (Absolute Trajectory Error), defined as the root mean square error from error matrices (see [29] for further details).For the EuRoC MH_03 experiment we also provided in Fig. 7 the error plots in order to compare the different methodologies reported in Table 3.
Furthermore, we want to show the system capability to cope with sensors failures (both when VO and UWB measurements are not available) using run MH_03 from the EuRoC dataset.In the first experiment we placed three virtual UWB anchors in (3.0, 0.0), (6.0, 6.0), (12.0, 2.0) m and run our algorithm simulating two VO faults in different moments and with different duration (at minute 01 : 06 for 8 seconds and at minute 01 : 26 for 10 seconds).The results are showed in Fig. 8, where the ground truth is reported in red, the REKF-SLAM trajectory is black when the system is working without faults and it is blue dotted when the VO is not available.Figure 9 presents the covariance plots for the estimated states [x a,k , y a,k , x u 1 ,k , y u 1 ,k , x u 2 ,k , y u 2 ,k , x u 3 ,k , y u 3 ,k ] at different time-steps.From the figure it can be noted that the system provides consistent state estimates and is not over confident.Furthermore, the ATE has been computed for the presented experiment being 0.045 m; the ATE is slightly higher than that of ORB-SLAM2 in EuRoC MH_03 and this is something reasonable given that the system for 18 seconds in the experiments is running without the VO and uses the UWB measurements only that are affected by noise, as described in the previous paragraph.Still, the ATE is acceptable and comparable to the results presented in Table 3.The last experiment has been conducted using the EuRoC MH_03 dataset, with three virtual UWB anchors in (3.0, 0.0), (6.0, 6.0), (12.0, 2.0) m.Here the UWB maxi-   Fig. 11 Covariance plots for the estimated states [x a,k , y a,k , x u1,k , y u1,k , x u2,k , y u2,k , x u3,k , y u3,k ] at different timesteps k for the second experiment of Section 4.4.The covariance plot for the state θ k has not been reported for the sake of clarity.The blue ellipses represent the covariances for the state x a,k , y a,k , while the green ellipses represent the covariances for the UWB anchor position estimated states.The red line represents the ground truth, the black plus signs the agent ground truth at the significant time-steps and the black o signs the real UWB anchor positions mum reading range has been set to 7 m in order to simulate conditions where the UWB measurements are not available for a variable amount of time and where sometimes all the three UWB anchors are not available at all. Figure 10 shows the results of the experiment; the most challenging situation is in the upper left part of the figure, where the agent is further than 7 m from the three UWB anchors.
Here the REKF-SLAM trajectory and ground truth differs more than in the rest of the path since the UWB measurements are not available and the trajectory estimation slightly degrades.However, the presented algorithm is able to maintain a low ATE that, for this last experiment, is 0.025 m still lower than that of ORB-SLAM2 (0.028 m). Figure 11 presents the covariance plots for the estimated states [x a,k , y a,k , x u 1 ,k , y u 1 ,k , x u 2 ,k , y u 2 ,k , x u 3 ,k , y u 3 ,k ] at different time-steps.Even in this case, it can be noted that the system provides consistent state estimates and is not over confident.
Finally, a further analysis on the impact of the outlier detection mechanism and on the Kalman gain scaling has been done.In the first instance we have to verify the assumption of normal distribution of the observation noise.In order to do so, we analyzed the probability distribution of the residual for ρ and β for three UWBs in the EuRoC MH_03 experiment.In Fig. 12 we show the residual probability distribution functions for the residual of the measurements for three UWBs, showing that they present a normal distribution.Furthermore, we also performed the Chi-squared test with a significance level of 5% showing that in the case of ρ measurement, if we hypothesize a variance of 0.01 m, the null hypothesis is rejected.The test also suggests that the variance is 0.02 m, that is what is expected from UWB measurements and we can state that this result is consistent.
For the outlier detection, we generated a plot showing the outliers, inliers, false positives and false negatives as an output of the processing pipeline.In Fig. 13 we focused on the UWB anchor in position (6,6) m showing how the measurements are classified by the resilient engine.We also provide a Fig. 14 Classification of the measurements as inliers and outliers with details about the Kalman gain scaling.The outliers (black crosses) are scaled by the resilient engine and the corresponding Kalman gain components are set to 0. The inliers not scaled are presented as green crosses, whilst the yellow crosses represent the inliers scaled as reported in Eq. 13 metric to assess the goodness of the methodology, computing the percentage of false positives and false negatives within a range of 0.25 m (i.e. a measurement that falls within that range is considered as an inlier; an outlier otherwise).The percentage of false positives is 9.6%, while the percentage of false negatives is 6.2%.
Regarding the Kalman gain adjustment, we also provided a plot for the EuRoC MH_03 experiment where, besides inliers and outliers, we presented the outliers whose Kalman gain components have been scaled (zeroed) and the inliers whose Kalman gain components have been scaled, according to Eq. 13. Figure 14 shows the classification of inliers and outliers with the corresponding Kalman gain scaled for the measurements from the 3 UWB anchors.

Conclusions
A resilient solution to the SLAM problem in an UWB context has been presented in this paper.An EKF-SLAM algorithm, endowed with a resilience module, fuses visual odometry data of an agent with the ranges provided by a set of UWB antennas located in unknown positions at the same height as the UWB antenna carried by the agent.The Visual Odometry data is provided by ORB-SLAM2 algorithm from a camera stream based on depth and infrared frames.One Extended Kalman Filter is used in the solution for each UWB antenna, allowing for the dynamic estimation of the range and bearing of the various antennas.The method increases the efficiency of the Visual Odometry algorithm prior to loop closure and is robust against various types of disruptions.Additionally, the switching observer enables the system to function even when the Visual Odometry or UWB measurements are unavailable for an extended period of time throughout the agent movements, thereby delivering a trustworthy agent pose estimation.This feature, along with the relatively low computational complexity, makes it appealing and comparable to more well-known approaches using other types of sensors (such as cameras or laser range sensors), which directly provide range and bearing measurements but necessitate the solution of a data association problem.The overall algorithm can be sped up by parallelizing the execution of the local EKFs (which are independent blocks) Section 3.4, and possibly by exploiting the range and the bearing estimates in more efficient algorithms, like, e.g., the Sparse Extended Information Filter [22].

Fig. 2
Fig. 2 Representation of the system at time-step k

Fig. 3
Fig. 3 System layout in the indoor environment

Fig. 4 Fig. 5
Fig. 4 UWB antenna position estimation errors for the first experiment

Fig. 7 Fig. 8
Fig. 7 Error plots for the EuRoC MH_03 experiment over the time-steps for the VINS-Mono, ORB-SLAM2, VIR SLAM and the proposed REKF-SLAM method

Fig. 10
Fig. 10 Ground truth, REKF-SLAM trajectory and UWB anchors position for the EuRoC MH_03 experiment where the UWB maximum range has been set to 7 m

Fig. 12 Fig. 13
Fig. 12 Residual probability distribution function for the residual of ρ and β for three UWB anchors.In blue the UWB anchor in (3.0, 0.0), in red the UWB anchor in (6.0, 6.0) and in green the UWB anchor in (12.0, 2.0)

Table 1
Comparison of Euclidean distances between the four ground truth waypoints and CLVO, OLVO of ORB-SLAM2 and REKF-SLAM trajectory estimates evaluated at the waypoints (in m) for the first experiment (best values are in bold) per image has been set to 800.For the outlier detection, α-quantile χ α is chosen from the Chi-square distribution table for 6 degrees of freedom (range and bearing of 3 antennas are considered) and with the significance level being 1%, it is 16.81.The robust Kalman filter gain relies upon the constants a 0 and a 1 , appearing in Eq. 13.Based on the work in

Table 2
Comparison of Euclidean distances between the 4 ground truth waypoints and the CLVO, OLVO of ORB-SLAM2 and REKF-SLAM trajectory estimate evaluated in the waypoints (in m) for the second experiment (best values are in bold)