Robust Simultaneous Localization and Mapping Using Range and Bearing Estimation of Radio Ultra High Frequency Identification Tags

In this article, we consider an indoor simultaneous localization and mapping (SLAM) problem for a mobile robot measuring the phase of the signal backscattered by a set of passive radio ultra high frequency identification (ID) tags, deployed in unknown position on the ceiling of the environment. The solution approach is based on the introduction, for each radio frequency identification (RFID) tag observed, of a multihypothesis extended Kalman filter (MHEKF) which, based on the measured phases and on the wheel encoder readings, provides an estimate of the range and of the bearing of the observed tag with respect to the robot. This information is then used in an extended Kalman filter (EKF) solving the SLAM problem. Since an effective range and bearing estimate is available only after some steps, a resilient module is added to the algorithm to evaluate the reliability of the position estimate of each observed tag. As shown through numerical and experimental results, this makes the proposed approach robust with respect to several kinds of unmodeled disturbances, like multipath effects or even the unexpected change of the position of a tag.


I. INTRODUCTION
I N THE last years, the radio frequency identification (RFID) technology is becoming more and more popular as a promising framework for robot localization applications. This is due to many reasons: among the others the fact that the RFID technology can be successfully used when other kinds of sensors may fail, due to harsh or bad light conditions, and especially when considering passive tags as in this article, these are cheap and almost maintenance free. Moreover, the identification (ID) of the RFID tags is known and this provides a straightforward hardware solution to the data association problem, which may afflict other kinds of sensors. Finally, the Manuscript  use of the RFID technology, as pointed out in [1], is already widespread in industrial environments for inventory purposes (see, e.g., [2]), so the localization algorithms based on this technology could potentially come with an almost negligible additional equipment cost.
There is a vast literature dealing with the use of the RFID technology for the localization of a mobile vehicle. Several different approaches have been proposed in the years which rely on various kinds of RFID measurements available, like, among the others, the received signal strength indication (RSSI), the time of arrival (TOA), the phase shift in the backscattered RFID signal or simply the information associated with the tag detection. Also, the use of the RFID technology has been sometimes considered in combination with other kinds of proprioceptive or exteroceptive sensors. A comprehensive survey on this subject has been recently presented in [1], where it is observed that the most investigated and relevant approach in this framework is the one where phase measurements coming from passive ultra high frequency -radio frequency identification (UHF-RFID) systems are combined with proprioceptive sensors. This is actually also the context considered in this article, where a reader, installed on board a mobile robot, measures the phase shift coming from a set of passive UHF-RFID tags deployed in unknown position on the ceiling of the environment.
There are several reasons for considering as RFID measurements the phase shift in the backscattered signal: phases are in fact very sensitive to the tag-reader distance and provide, as a consequence, a very precise measurement of this quantity. They are usually available in off-the-shelf systems based on passive tags and provide a more reliable and precise sensing of the distance variation with respect to other kinds of RFID measurements, even in the case of signals affected by undesired phenomena, like multipath.
The problem with the phases is their periodicity, since a given phase shift corresponds to several possible tag-reader distances. To deal with this ambiguity, several approaches have been proposed in the literature, some of them based on the simultaneous use of different signal frequencies, like, e.g., [3]. This kind of approaches is effective even if has to fight with the limited allowed range of RFID frequency dictated by Federal Communications Commission (FCC) regulations, which motivates the interesting approach proposed in [4]. Another possible solution is the one based on measurements taken from different positions, whose relative displacement must be known [5], [6], [7]. The ambiguity in phase measurements can This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ also be solved in an indirect way by using multihypotheses filters where wrong hypotheses (e.g., on the robot pose, like in [8]) are pruned according to the phase measurements which become available during the reader (or the tag) motion.
In this vein, several approaches have been presented in the last years to localize a robot with an RFID reader installed on board, which measures the phase shift in the RFID signal backscattered by a given set of passive RFID tags located in the environment. In many cases, the position of these tags is assumed known, like, e.g., in [8], [9], [10] and in [11], which also performs an observability analysis of the problem. In [12] a similar problem is considered even if the situation is in some sense reversed with respect to the previous works since a tagged object is moving and there is a set of reader antennas in known position to estimate the tagged object motion.
The assumption that the position of the reference tags is known may be too optimistic for many reasons. First of all, this knowledge may require a long and laborious preparation of the environment. Moreover, an exact knowledge of the RFID tag position is actually not possible, both for practical reasons and also for technological aspects, like the one due to the uncertainty in the antenna phase center [13]. Notice, at this regard, that, in view of the high sensitivity of the phase measurements, a precise information regarding the position of the tags is usually needed to make the localization process effective. In addition, phase measurements are characterized by at least one more unknown parameter, namely the phase offset, which depends, e.g., on the hardware of the system. It could be quite inconvenient, if not impossible, to evaluate this parameter for each tag, especially in the case of a very large number of tags. At this regard, it could be important to mention the analysis presented in [14], where it is shown how the offset can not be considered a constant parameter but may depend on the tag-reader relative position. This observation may invalidate a common practice adopted in several approaches, consisting in subtracting from phase measurements the phase measured, e.g., in the starting position of the robot, practice characterized also by other problems, like a general increase in the noise, especially if the reference measurement is strongly perturbed.
For all these reasons, it appears important, if not mandatory, to develop localization approaches where the tag positions, together possibly with other unknown parameters (like the offset), are included in the estimation algorithm. This can be required also in view of possible disturbances which could affect the environment and perturb the tag locations [15].
The tag positions can be refined starting from an initial rough information on their value, like, e.g., in [16] and [17], which also deal with the problem associated with the unknown offsets. But they can be also completely unknown at the beginning, with the consequence that they should be estimated together with the robot pose: a simultaneous localization and mapping (SLAM) problem then arises, where also the effect of the unknown offset must be taken into account.
A solution to a SLAM problem in this context is presented in [18], which extends the approach of the range only SLAM technique proposed in [19] to consider the phase periodicity. The SLAM algorithm presented in [18] is based on a Rao-Blackwellized particle filter, where the robot pose is estimated through a set of particles, each one containing, in addition to the robot pose guess, also, for every tag detected, a set of extended Kalman filter (EKF) instances associated with all the candidate positions of that tag with respect to the robot. The problem considered in [18] is roughly 2-D (tags are placed on the ceiling which has an approximately known height). So, when a tag is detected for the first time, a set of EKF instances is initialized along a number of concentric circles around the robot pose guess at that moment associated with the considered particle. When new phase measurements arrive, these EKF instances are weighed according to the agreement between actual and expected measurements and are possibly pruned if their weight falls below a given threshold. The approach provides a satisfactory solution to the SLAM problem since also the offset is handled as part of the estimation of the ceiling height, which is assumed only roughly known at the beginning. It presents however two major limits. The first one is the computational complexity, large at the beginning of the estimation process, due to the large number of candidate EKF instances associated with each new observed tag in each particle. The second problem is that pruning is not reversible: once an EKF instance disappears, it cannot be restored anymore. This may concern also good EKF instances which may be wrongly pruned due to measurement noise and to other kinds of disturbance.
A more efficient and robust solution is presented in this article. Preliminary results on this approach have been presented in [20]. It exploits the multihypothesis EFK (MHEKF) developed in [21] which, based on the wheel encoder readings and on the phase measurements coming from a given tag, is able to provide an estimate of the range and of the bearing of that tag with respect to the robot. Once the range and the bearing of the tags is available, standard SLAM approaches can be considered. Since the ID of the tags is available, an EKF-SLAM-based algorithm is adopted in this article as a valid solution to the problem. This approach presents several interesting features, as described below.
First of all the time complexity is much lower with respect to the approach proposed in [18]. In fact, even if there is a MHEKF running for each tag, every MHEKF contains a limited number of EKF instances (typically between 10 or 20, depending on the maximum detection range of the reader) and each instance is a 3-D EKF (it estimates the range, the bearing, and the phase offset of the tag).
Another interesting feature is the intrinsic robustness of the approach against disturbances, since, in this case, hypotheses are not pruned and the algorithm is able to restore a good behavior after possible unreliable periods, where strong perturbations could have compromised the effectiveness of the approach, or of part of the approach, for a while. This has been obtained by providing the algorithm with a resilience module, formulated on a robust residual-based adaptive estimation EKF (as described in [22] and [23]), which may decide to switch off the measurements coming from a given tag if the position estimate of this tag appears not reliable enough, according to certain metrics defined in the article. When the position Fig. 1. Indoor environment with the robot in its initial position and six tags placed on the ceiling. The triplet (x, y, z) defines the absolute reference frame. The robot reference frame (x (r) , y (r) , z (r) ) is also a static frame which is defined by considering the initial pose of the robot. estimate of the switched off tag returns reliable, this tag may be restored and included in the estimation process. As shown through numerical results, the algorithm may also handle situations where a tag position is changed at some unknown time during the experiment.
It is interesting to observe that, even under multipath effects or other perturbations, a wrong but stable estimate of a tag position could provide a virtual landmark which helps in any case the robot pose estimation process. Clearly, in that case, the tag position estimate will be wrong but it can be corrected if the robot reaches an area where the disturbance, causing the wrong tag estimate, decreases.
Finally, being the phase offset included in the estimation process, also the problem of an unknown and possibly nonconstant offset as mentioned in [14], may be handled to some extent by the proposed algorithm.

II. NOTATION AND PROBLEM FORMULATION
The system setup considered in this article is an indoor environment with a certain number L of RFID tags located on the ceiling, as depicted in Fig. 1. The (x, y, z) absolute reference frame is defined by assuming that the floor is the z = 0 plane, with the z-axis pointing toward the ceiling. Unless otherwise specified, everything will be described with respect to this frame. However, in the SLAM algorithm, the robot will reconstruct its path and the tag coordinates with respect to another frame defined with the origin in the initial robot position and with the x-axis oriented along the initial motion direction. The robot frame is also a static frame related to the global frame through a roto-translation in the x y plane.
The robot is a unicycle-like vehicle with a differential drive kinematics. If p t = (x r,t , y r,t , θ t ) denotes the robot pose at time t, with (x r,t , y r,t ) the robot position and θ t its orientation, the discrete time dynamics of the robot is as follow: with u R,k and u L ,k representing the distance covered in the interval (kδ t , (k + 1)δ t ) (i.e., at time step k, with discretization step δ t ) by the right and left wheels, respectively, and d is the distance between the two wheels. The distance u R,k covered at time step k by the right wheel is related to a noisy encoder reading u e R,k by the relation u e R,k = u R,k + n R,k , where the noise term n R,k is assumed a 0-mean Gaussian random variable with variance given by K R |u e R,k |, being K R a positive constant. A similar argument can be applied to the left wheel. In order to simplify the notation, the following definitions can be adopted: A reader installed onboard the robot collects the phases of the RFID signals backscattered by L tags located in unknown The collected signal at time k from the j th tag is as follow: where A k, j (on/off) and φ k, j (on/off) are the amplitude and phase of the signal modulated by the tag's binary (on/off) data sequence. In ideal conditions, the phase φ k, j accounts for the round trip of wave propagation between the vehicle and the tag. In practical conditions, a commercial RFID reader extracts the differential received signal between the two modulating states of the tag as follow: that includes bias hardware noise and the interference of the signal with the environment. In indoor environments and in presence of scattering objects as furniture, benches, appliances, etc., the signal scattered by the tag feels the effects of the electromagnetic coupling of antennas with the environment, so its intensity and phase differ from that of ideal conditions. In particular, there are positions where the signal is so weak that it is not received (i.e., it is below the hardware noise threshold) and positions where the intensity is higher than the noise threshold but the phase is perturbed, i.e., the measured phase θ k, j differs from the value calculated considering only the round trip path. This effect is particularly noticeable in case of standard tags as inlay tags based on dipole-like antennas and a matching network. They are sensitive to multipath (i.e., the bouncing of the signals from the surfaces of the environment) and to the material of the surface where they are placed on, since they suffer from impedance mismatch and loss of efficiency that affect, moreover, the phase offset. For these reasons, the phase measurement at time k collected by the reader onboard the robot for each tag can be defined as follow: where K = 2π/λ (with λ the wavelength of the electromagnetic signal), D k is the (unknown) tag-reader distance, φ o is an unknown offset depending on the hardware and n φ,k is, at each time k, a 0-mean Gaussian noise and φ m,k is a disturb of the phase that accounts for effects (i.e., multipath) of the environment. We define the range ρ i,k and the bearing β i,k of a tag i as follows: where (x T i , y T i ) are the unknown coordinates of tag T i . According to this definition, the distance D k in (5) when the i th tag is observed, is given by (ρ 2 i,k +h 2 ) 1/2 , where h is the ceiling height, approximately known. When the robot moves (the tag is assumed fixed in its unknown position (x T i , y T i )), the range ρ i,k and the bearing β i,k change over time. Knowing the wheel displacements u R,k and u L ,k , it is possible to derive the equations which describe the dynamics of the variables (ρ i,k , β i,k ). We obtain, after discretization: where u k and ω k have been defined in (2). Each tag position with respect to the robot is estimated with a MHEKF that fuses the phase measurements with the odometry readings, as proposed in [21]. The objective of this article is to solve a SLAM problem so that both the robot pose and the tags position (which represent the unknowns of the problem) could be estimated at the same time while resisting the effects of outliers in the measurements. The known quantities in the problem are the encoder readings, the phases of the signal backscattered by the RFID tags on the ceiling and an approximate value of the ceiling height h with respect to the reader on the robot.

A. Schematic of the Overall Algorithm
The SLAM problem formulated in Section II is solved through an EKF SLAM algorithm which uses the range and the bearing estimation of each detected tag provided by a set of MHEKF, one for each tag. The main steps of the approach are summarized as follows.
1) Initialization: Initialize for each tag a MHEKF as described in Section III-B and initialize the EKF SLAM as described in the initialization step of Algorithm 1.

3) Step k (Instance Instability Detection and Update):
Perform the instability detection step described in Section III-D1 and update state and covariance according to the state and covariance update step reported in the same section.

5)
Step k (SLAM Prediction): Perform the prediction step of Algorithm 1.

6) Step k (SLAM Correction With Robust Kalman Gain):
Perform the correction step of Algorithm 1 with the Kalman gain modified as in (27). 7) Set k = k + 1 and return to item 2.

B. Range and Bearing Estimation
Based on the work in [21], each tag position with respect to the robot is estimated through a MHEKF that fuses the phase measurements with the odometry readings. The proposed algorithm, for each tag i , initializes n M EKF instances = 1, 2, . . . , n M , each one with a different value of the range, since for the periodicity of the RFID phases, several ranges correspond to the same value of a given measured phase. Then, in each EKF instance , the a priori estimate for range and bearing and the corresponding covariance matrix is computed. The correction step of the EKF algorithm is applied independently in each instance and finally for each instance two metrics are created and then used to compute its weight. Instances with a too small weight are moved to cycles corresponding to the current phase measurement not covered by other instances. Finally, the estimatesρ i,k andβ i,k are taken from the best EKF instance (i.e., by the EKF instance with larger weight). The algorithm is described here only considering the range and bearing estimation, while the phase offset φ o estimation can be easily included in the MHEKF, as described in [21]. Moreover, the offset estimation compensates possible errors in the knowledge of the ceiling height h. The best EKF instance can be seen as a sensor able to measure the range and the bearing of a certain tag with a certain noise and fault rate associated with it, throughout this article.
Using the best EKF instance however leads to problems related to the choices of that can change from one time step to another, given that the weight of the instances change over time. One problem is that these switches between instances strongly affect range and bearing estimations as shown in Fig. 2. This can introduce big differences between consecutive measurements and a method to cope with this problem, and with the problem related to the frequent switches between instances with similar weight, must be designed. This will be considered in Section III-D.

C. Simultaneous Localization and Mapping
For each tag i in the environment, the algorithm described in Section III-B is initialized and executed. As an output at each time step k, the estimated ranges and bearings (ρ 1,k ,β 1,k ), . . . , (ρ L ,k ,β L ,k ) are computed for the L features (i.e., tags). These estimations are then available to perform a SLAM algorithm with a note: each estimated range and bearing couple can come uniquely from its corresponding tag. This leads to a main simplification for the data association as (ρ i,k ,β i,k ) (i = 1, 2, . . . , L), regarded to as a measurement of feature i , is uncorrelated to any other feature j ( j = i ). The SLAM algorithm adopted in this work uses an EFK. The robot pose at time k is (x r,k , y r,k , θ k ), so the state vector, also including tag positions to map the features in the environment, is as follow: The dynamics of the system is as follow: and will be synthetically referred to in this article by where u e k and w e k come from (2) and n R,k and n L ,k have been already defined before (2). The (3 + 2L) × (3 + 2L) Jacobian matrix F k of the state dynamics with respect to the state is defined as follows: The (3 + 2L) × 2 Jacobian matrix with respect to the encoder noises n R,k and n L ,k is as follow: When a new range and bearing measurement with the robot in state (x r,k , y r,k , θ k ) and the tag in position (x T i,k , y T i,k ) is available, it can be expressed as follows: When measurements from L tags are available, (14) becomes a vector whose 2L elements contain the range and bearing from the L tags. The Jacobian H k of this measurement funct-ion with respect to the state is a 2L × (3 + 2L) matrix: The EKF-based SLAM algorithm is summarized in Algorithm 1.

D. Dealing With MHEKF Instance Changes
As already mentioned in Section III-B, the MHEKF works with a certain number of EKF instances (10 or 20 are enough to cover the set of all possible cycles), each one initialized on a different cycle corresponding to the initial phase measurement. The instance with larger weight will be chosen to provide the estimatesρ i,k andβ i,k (i.e., the i th tag measurements used by the EKF-SLAM algorithm). At each timestep, the choice of the instance could change. When this happens, the range and bearing estimates may be affected by a sudden change. These instance changes are usually caused by an instance stabilization process intrinsically operated by the multiple EKF while selecting the hypothesis which best fits with the measurements. From a SLAM point of view, this behavior can be seen as a condition to initialize a new feature (i.e., another possible tag position in the map) and should be managed with a proper algorithm for partial reinitialization. In order to do so, a methodology to determine if an instance can be considered as stable is proposed in this article. For a single MHEKF, given the = 1, 2, . . . , n M instances, let be the instance with the largest weight at time k, is stable at time k if whereî j is the instance with the largest weight at time j and s t is a positive integer to be properly selected.
The instance is considered unstable if (22) is not satisfied for it. This method is applied for all the L MHEKFs.
1) EKF-SLAM With Selective Tag State Estimation Reset: a) Instability detection step: Perform a check on the stability over the L MHEKF instances with the largest weight according to (22). If one or more unstable instances are there, then proceed to the next step, otherwise cycle over the instability detection step.
b) State and covariance update step: If the instance i is marked as unstable at time step k, the state vector and the covariance matrix of the EKF-SLAM must be updated according to the following: The other elements of the state vector are left unchanged as no reset is needed neither in the robot state nor in the tag states which are not affected by instance instability. For the covariance matrix the assumption that ρ T i,k and β T i,k are considered as real measurements has been made. These measurements have their own covariance matrix taken from the multiple EKF and we can assume that they are independent both from the robot pose and the other tags coordinates. Given the previous assumptions, we can definez = [ρ T i,k , β T i,k ] T as the range and bearing estimation vector for the T i tag that has to be reinitialized. Furthermore, ifx k is the EKF-SLAM vector without the coordinates of tag T i andỹ = [x T i,k ,ŷ T i,k ] T is the vector of the estimate of the tag coordinates, we can define the covariance matrix of the extended state vector [x,z,ỹ] T as follows: where Fx and Fz are the Jacobian matrices of the relations (23) with respect tox andz, respectively, The EKF-SLAM covariance matrix P, after the reinitialization of tag T i position estimate, is as follow: where the order of rows and columns could be different from the previous formula according to the T i tag coordinates positions in the EKF-SLAM state vector. The state and covariance update step shall be run for all the instances marked as unstable.

E. Resisting the Effects of Outliers With Resilient EKF-SLAM Through Hypothesis Test and Robust Estimation
The range and bearing estimation through MHEKF is affected by outliers that do not fulfill the assumed stochastic model of EFK, so this can be a potential problem for parameters estimation as also mentioned in [25]. In order to resist the effects of the outliers, a resilient EFK has been designed. The novel filter includes a module to detect the presence of one or more outliers in the measurements. If outliers are there, the robust estimation is designed using a modified version of scheme III of the Institute of Geodesy and Geophysics of China (IGGIII) scheme based on statistic test of the normalized residual. Furthermore, a fault detection algorithm has been implemented in order to exclude potentially faulty sensors (i.e., wrong MHEKF estimates) from the SLAM algorithm.
Global outliers detection. According to Section III-C, the observation is modeled by a Gaussian distribution with mean and covarianceẑ − k and C obs,k , respectively. So the probability density function can be written as follow: where |C obs,k | is the determinant of C obs,k . If some outliers in the observation are there or if the Gaussian distribution of the observation noise is contaminated with some other distributions, (24) will no longer hold and this means that some violations to the assumption or modeling errors could exist. Against a potential measurement outlier, a check if the actual observation is compatible with the assumed model has to be done. The null hypothesis H 0 test is as follow: Equation (24) is used as the relevant null distribution which holds under the assumed model and twice the minus exponent in (24) represents the relevant test statistic. The test statistic term is the judging index to detect the modeling errors and this is the square of the Mahalanobis distance from observation z k to its meanẑ − k as deduced in [26] as follow: where M k is the Mahalanobis distance at time step k. The distribution of the test statistic under the null hypothesis is decided so, assuming the null hypothesis is true, γ k should be Chi-square distributed with 2L degrees of freedom. In order to design the statistical test a probability threshold α is needed, below which the null hypothesis will be rejected. The α-quantile χ α of the Chi-square distribution is predetermined so that where P(γ k > χ α ) is the probability of γ k being larger than χ α and this should be small as α. Substituting the actual observationz k , we obtain the actual judging indexγ k , and if this is larger than the α-quantile, the null hypothesis can be rejected and we are confident that one or more outliers occur in the observation as also described in [27]. The null hypothesis H 0 test, being a statistic test, is always accompanied by the probability errors (Type I and Type II errors) with respect to the significance level and the power of a test as in Table I as suggested in [28]. These kinds of errors cannot be minimized at the same time: studying how to balance these two types of errors is a good tuning parameter for the filter resiliency.

1) Kalman Gain Scaling:
The global outliers detection gives a methodology to understand if the model does not conform with the specifications, although it does not find which are the outliers in the measurements. The alternative hypothesis to H 0 is that there is at least one outlier in one known observation [29]: A The observation z i for some fixed i is an outlier. The decision can be based on the value of the normalized measurement residual for the observation i at the time step k: where n k,i is the i th element of the measurement residual as in (17), c n i n i denotes the i th diagonal element of C obs,k given in (19). If H 0 holds true, then w k,i ∼ N(0, 1) as derived in [29]. If an observation i at timestep k is affected by an outlier, the covariance should be inflated and this can be done acting on the Kalman gain K k . The robust gain matrix factor of Kalman filterK ji (where the time step k has been omitted to make clearer notations from now on) is as follow: (27) where j is the j th element in the state vector and a 0 , a 1 are the robust constants of the Kalman gain, usually determined based on the objective requirements. In (27) the factor (a 1 −w i )/(a 1 −a 0 ) is raised to third power, which differs from the implementation of the IGGIII scheme presented in [25] where the same factor is raised to the second power. The reason for this choice is that we want to further decrease the robust Kalman gain factor when the normalized measurement residual is in the range (a 0 , a 1 ]. Moreover, the measurement vector is composed of range and bearing per each tag, estimated by one MHEKF as reported in Section III-C. This leads to the assumption that if the measurement residual related to the range or bearing measurement of the same tag (same multiple EKF) is such that w i > a 1 , we set K ji to 0.

2) Sensor Fault Detection:
The robust gain matrix factor of Kalman adjustments allows a further consideration regarding the measurement reliability associated with a certain sensor providing the couple (ρ i,k ,β i,k ). From this point of view (27) can give a further information regarding the number of outliers detected and one can relate its frequency to the reliability of the sensor itself. In particular for the tth tag sensor, one can define the number of times an outlier is detected subsequentially, weighing the outlier whose normalized measurement residual w i falls within the range (a 0 , a 1 ) with a smaller weight compared to the one falling in the range [a 1 , ∞): (28) withk being the first timestep when the outlier is detected (with k >k) and whereḡ is a proper weight withḡ > 1. The expression in (28) can be used in order to assess how much a sensor is faulty. Based on this fault factor and on the reliability factor defined as follows: when no faults are there ∀k ∈ [k, k] withk the first timestep when the sensor presents no faults, a policy can be designed in order to shutdown the sensor or to restart it when its faults stop increasing. When a tag sensor presents a first fault, the weighted number of faults is computed according to (28). When η f,t (k) becomes greater than the threshold T f , that tag sensor t is shutdown. Similarly, after a shutdown when a tag sensor becomes reliable for a proper amount of time T r (i.e., η r,t (k), computed as in (30), becomes greater than the threshold T r ), that tag sensor t is restarted.

IV. NUMERICAL INVESTIGATION AND EXAMPLES
This section comprises two sets of simulations, one with synthetic UHF-RFID data corrupted only by a Gaussian noise and the other with UHF-RFID data generated by means of a numerical calculation based on suitable beam-tracing algorithms [31], [32], [33]. The scenario considered in the first case is a 2 × 2 m 2 indoor environment without multipath effects whilst, in the second case, a 6 × 6 m 2 room containing some furniture is considered, as detailed below. The robot performs random paths of the type reported in Fig. 3. These paths are generated by randomly selecting the initial position and orientation of the robot and with the odometry readings numerically generated with a Gaussian error. When going straight, the robot proceeds at constant speed covering about 1 cm in each time step. When performing a turn, the robot covers about 5 • per time step. The duration T of the simulations considered in this section is 2000 steps, corresponding to an average of about 15 m of traveled distance (depending on the number of turns performed). The parameters of the robot are as follows: d = 26 cm, K R = K L = 0.01 cm. In the two simulation sets the UHF-RFID tags are located at a height of 2.5 m with respect to the robot antenna and the estimated height is perturbed over each simulation with a random error of ±3 cm (this error is due to the fact that the ceiling height is known with a certain approximation). Moreover, the unknown offset φ o on the phase measurements depending on the hardware, as stated in Section II, is also considered in the simulations as a random value from 0 • to 360 • and different in each simulation.
After discussing in Section IV-A some issues related to the choice of the main parameters appearing in the algorithms, a set of simulations is reported in Section IV-B to illustrate the behavior of the proposed approach in a noisy environment, where phase measurements are characterized by a Gaussian noise with standard deviation σ φ = 10 • . Furthermore, in Section IV-C a set of simulations is reported where the phase measurements are generated by numerical calculation based on beam-tracing algorithms, adding the Gaussian noise, considering a 6 × 6 m 2 indoor environment with some furniture to also consider the effect of multipath on the system, shown in Fig. 4.
The numerical model we have used takes into account the materials of the environment and the propagation effects.
In particular, the model has been developed for the central frequency of the allowed European band for RFID (i.e., 867 MHz), walls are considered made of concrete having relative permittivity 6 and loss tangent 0.25, while furniture are supposed made of wood with relative permittivity 1.7 and loss tangent 0.001. The wave propagation algorithms account for multiple reflections from walls and furniture and wedge diffraction. Since the interactions (i.e., reflections and diffractions) with the environment may be mitigated by particular radiation properties of the reader's and tag's antenna (e.g., directive antennas may mitigate multipath effects), we use a half-space isotropic model of antennas in order to test the SLAM algorithms in the most possible general conditions. Considering the reader's antenna is near the floor and points toward the ceiling, while the tag's antenna is on the ceiling and points toward the floor, both reader's and tag's antenna are modeled as isotropic sources in the half-space they are pointing. A half-space isotropic source accounts for multipath coming from all directions in that half-space consequently it Indoor scenario with four tags located in position (1.5, 1.5), (1.5, 2.5), (2.5, 1.5), (2.5, 2.5). The robot moves in the dashed red 2 × 2 square area, close to wood furniture, which introduce multipath. The 4 insets show the phase of the four tags measured in the 2 × 2 considered area: multipath effects, which perturb phases and create voids (white spots on the diagrams where the signal is not received), are clearly visible (in ideal conditions we would obtain concentric circles). The vertical bar indicates the phase value in degrees. models the most general multipath condition. Throughout in Sections IV-A-IV-C, the results of the presented methodology have been compared to those of the methodology proposed in [18], showing the overall performance improvements both in terms of SLAM estimates and computational complexity.

A. Parameter Tuning
The proposed algorithm relies on the following parameters to be properly tuned as follow.
1) s t , which is a constant used to decide if the current instance for a tag estimation in the MHEKF is stable or not, as number of timesteps. 2) α-quantile χ α in (26). 3) a 0 and a 1 (with a 0 < a 1 ) constants, appearing in (27). 4)ḡ constant, weighing the outliers whose normalized measurement residual is greater than a 1 . 5) T f is the threshold beyond which the sensor is considered faulty. 6) T r is the threshold beyond which the sensor is considered reliable. As for s t , a too small value of this parameter (e.g., s t less than 5 steps) makes the instance stable even if it is switching quite frequently and this should be avoided. On the other hand a high value of s t (e.g., s t greater than 40 steps) implies that the instance is seen as stable after a considerable amount of time, and going beyond this value could lead to evaluate the instance as unstable continuously. Based on numerical results, a good tradeoff has been obtained by taking s t = 20 steps.
For the outlier detection, α-quantile χ α in (26) is chosen from the Chi-square distribution table for 8 degrees of freedom, as four tags are considered in the scenario and each tag has 2 (for range and bearing measurements). For the 8 degrees of freedom Chi-square distribution with the significance level being 1%, it is 20.09.
The robust Kalman filter gain relies upon the constants a 0 and a 1 (with a 0 < a 1 ), appearing in (27). Based on the work in [30], a 0 ranges in [1.0, 2.5] and a 1 ranges in [3.0, 8.0]. For the simulations, a 0 = 1.5 and a 1 = 3.5 have been chosen in order to achieve a good tradeoff between marking an observation as an outlier and integrating it as a correct measurement (eventually adjusting it).
The sensor fault detection method in Algorithm III-E relies upon the constantḡ weighing the outliers whose normalized measurement residual is greater than a 1 , as previously fixed. The valueḡ = 2 has been chosen as it allows the algorithm to weigh twice the outliers that zero the Kalman gain elements K ji with respect to the outliers that reduces the Kalman gain elementsK ji according to (27). Furthermore, T f and T r are two important parameters for the fault/reliability detection, T f being the threshold beyond which the sensor is considered faulty. If a too small value is chosen for this threshold (e.g., T f < 5), then the system will shutdown the sensors even if they are still working with reduced quality. On the other hand if its value is too high (e.g., T f > 20), the system will only catch few faulty situations. Given that, a good trade off has been obtained by taking T f = 10, which means that the system is capable of shutting down the malfunctioning sensor in the worst case after 10 timesteps. A similar argument applies to parameter T r which has been set to 8.

B. Numerical Examples
In this section multipath effects are not considered and only a Gaussian noise has been added to the ideal phase measurements. The scenario is the one described in Fig. 3. The execution of the robust EKF-SLAM algorithm is depicted in Fig. 5. The path traveled by the wheeled robot is reported in Fig. 6, where the ground truth trajectory (in blue) is shown together with the trajectory estimated by the robust EKF-SLAM (in red). The same set of simulations has been run with the SLAM algorithm presented in [18], in order to compare their performances with the indexes proposed in the same article.
As for the robot position estimation, we first compute the difference between the true and the estimated distance of the robot from the position of the four tags in the various steps k of the simulation, i.e., Then, an average robot position estimation error is computed by averaging the previous quantity over the four tags and considering the last 1000 steps of the simulation as follow:   As for the tag position estimation, we similarly define the difference between the true and the estimated distance among the four tags in the various steps k of the simulation, i.e.,  where six is the total number of distance errors e tij,T , i = 1, 2, 3 and j = i + 1, . . . , 4. The results of the simulations for the two algorithms are depicted in Fig. 7 where the errors e r and e t , previously defined, for 100 simulations for the method presented in this article (top) and the results for the algorithm proposed in [18] (bottom) are showed. This figure shows how both the errors for the robot and tags position estimation with the presented EKF-SLAM are smaller compared to the SLAM approach in [18]. Table II shows the estimation errors average over all the simulations for the two methods. A further simulation has been carried out in order to assess the performance of the resilience module; in particular, the tag (1.5, 1.5) has been shifted instantly from its original position to (0, 1.5) from time-step 1000. Over the 100 simulations, the average error e r is 0.039 m for the presented method and 0.097 m for the approach in [18]; e t is 0.026 m for the presented method and 0.209 m for the approach in [18]. The tag perturbation heavily affects the SLAM approach in [18] as its average error for tag position estimate for the shifted tag is 0.945 m while is 0.034 m with the presented approach.

C. Numerical Analysis With Multipath Affected Signals
This section refers to the case described in Fig. 4, where phase measurements are generated using the ray tracing software mentioned above and are corrupted with a Gaussian noise with standard deviation 10 • . 100 simulations have been run with both the approach described in this article and the one presented in [18]. Fig. 8 shows the results of the simulations for both methodologies. From Fig. 8 and from Table III it is clear how the presented EKF-SLAM methodology outperforms the approach presented in [18] both in Fig. 8. Error trends for both robot (e r ) and tags (e t ) position estimation for the EKF-SLAM (top) and for the particle filter approach (bottom) in the case of multipath affected measurements. terms of performance and, also, from a computational point of view. In fact, the simulations ran on a AMD Ryzen 7 3800x 8-core processor 3.9 GHz with 32 GB RAM on Linux Ubuntu 20.04 with MATLAB R2021a, showed an average computation time per simulation of 12.82 s for the presented method and 91.59 s for the method in [18]. The difference of the computation times would have been even more pronounced with more tags, as the approach in [18] needs about 1000 initial instances for any new tag, while the presented method needs a few dozen. For the presented method, the computation time for each time step (taking into account that each simulation consists of 2000 time steps) is 0.0064 s: this low computation time allows an implementation on a real robot even with small computational power with a real-time fashion.

V. EXPERIMENTAL RESULTS
A set of experimental tests has been performed in the office room depicted in Fig. 9. The robot used in the experiments is a custom unicycle-like vehicle with a differential drive kinematics where the distance between the left and right wheel is 38.2 cm. The robot mounts a Raspberry Pi 4 with a Linux Ubuntu 20.04 OS where a motion planner has been developed for the high-level control. An Arduino Mini is also installed in order to control the low-level references to the motors and the encoder readings. The reader antenna is placed on board the robot at a height of 32 cm from the floor, it is a right-hand circularly polarized microstrip patch antenna with 7 dBi of gain. The reader (M6e ThingMagic), wireless controlled by means of a remote PC and a Raspberry Pi 4 on board the robot, supplies the antenna with a power of 25 dBm and collects measured data with a rate of 15 Hz while the robot moves with a speed of 0.2 m/s. Measurements have been performed Fig. 9. Experimental setup, with three tags on the ceiling, the robot during its mission and the (x, y, z) frame adopted by the robot to solve the SLAM problem, selected according to the initial pose of the robot.
with UHF-RFID wave at the frequency of 868 MHz. Tags have right-hand circular polarized antennas similar to that of the reader, they have been realized with a stacked annular ring microstrip antenna as described in [8]. Their low profile (the thickness is less than 1 cm) is well suited to be mounted on the ceiling by means of a small metallic ground plane as shown in Fig. 9. The distance between the plane of placement of the tags and the plane of placement of the reader antenna is about 2.5 m.
The experiments have been conducted with the robot moving autonomously along a path while collecting data from the encoders and from the UHF-RFID reader. The data collected during the runs have been used offline to feed the developed algorithms and to assess their performances. A map of the environment is reported in Fig. 10 together with the estimated robot trajectory for the experiment described in this section. Nominally the robot is requested to cover a rectangular path performing two turns in the area under the tags. The requested trajectory is followed open loop by the robot. Due to several kinds of disturbance, the actual trajectory was quite different from the nominal rectangular one, as witnessed by the trajectory reconstructed by the SLAM algorithm. The ground truth of the overall trajectory is not available in this experiment: we only measured the final position of the robot at the end of its mission, which allowed to compute the performance indexes considered in this article. In particular, we report in Tables IV and V the true and the estimated distances d i j among tags (i, j ∈ {1, 2, 3}) and, respectively, the true and the estimated distance d i of the final robot position from the projection on the floor of the three tags (i = 1, 2, 3). Fig. 11 reports the map reconstructed by the algorithm. Fig. 10. Map of the considered environment, with the robot in its initial position and orientation (defining the global frame adopted) and the (x, y) position of the three tags considered in the experiment. The path estimated by applying the proposed SLAM algorithm is also reported.  Map realized by the proposed algorithm: black and red stars represent, respectively, the true and the estimated position of the three tags. The black square and the red circle represent the true and, respectively, the estimated final position of the robot.
The proposed methodology appears effective also in this experimental case even if the estimation errors increase with respect to the simulation scenario. This mainly depends on various unmodeled disturbances acting on the system, like, e.g., the effect of the relative tag-reader position on the phase measurements, which appears to produce the most relevant perturbation. According to this effect, as also observed in the literature [14], the offset in the phase measurements cannot be considered a constant quantity, as assumed in the model. Nevertheless, since this quantity is subject to estimation, the proposed algorithm is in part able to adapt to this change and to produce an acceptable behavior. Other unmodeled phenomena include multipath effects, which produce quite wrong or even missing measurements, and systematic errors like the approximate knowledge of system parameters, including the wheel and the robot dimension. Due to these unmodeled phenomena and to other systematic sources of error, the approach in [18] was not able to produce effective estimation results in this experimental context, with the filter diverging after some steps.

VI. CONCLUSION
A robust solution to the SLAM problem in a RFID context has been proposed in this article. An EKF-SLAM algorithm, endowed with a resilience module, fuses odometry data of a mobile robot with the phase of the signal backscattered by a set of UHF-RFID passive tags deployed in unknown position on the ceiling of the environment. The reader is installed onboard the robot. The solution is based on a set of MHEKFs, one for each tag, which allow to dynamically estimate the range and the bearing of the different tags. The approach is robust against several kinds of disturbances and improves, also from this point of view, the performance of other methods presented in the literature in this context. The approach does not require the knowledge of the phase offset characterizing the RFID measurements and, for this reason, does not need a preliminary calibration procedure. This characteristic, together with the relative limited computational complexity, makes it appealing and comparable to more popular approaches where other kinds of sensors, directly providing range and bearing measurements but requiring the solution of a data association problem (like cameras or laser range sensors), are considered.