Range-only SLAM for robot-sensor network cooperation

This work is motivated by schemes of robot-sensor network cooperation where sensor nodes—beacons— are used as landmarks for range-only (RO) simultaneous localization and mapping (SLAM). In most existing RO-SLAM techniques beacons are considered as passive devices ignoring the capabilities they are actually endowed with. This paper proposes a RO-SLAM scheme that distributes the measurements gathering and integration between the beacons surrounding the robot. It naturally integrates inter-beacon measurements, signiﬁcantly improving map and robot estimations and speeding up beacon initialization. The proposed scheme is based on sparse extended information ﬁlter (SEIF) and it is proven that it preserves the constant time and sparsity properties of SEIF and thus, inherits its efﬁciency and scalability. As a result, our scheme has lower robot and map estimation errors, faster beacon initialization and lower computer requirements than existing methods. This paper This work


Introduction
This paper deals with range-only (RO) simultaneous localization and mapping (SLAM) in which the robot integrates range measurements to radio beacons deployed in the scenario in order to build a map of an unknown environment and to self-localize in that map.Our scheme uses the nodes of a sensor network as beacons-landmarks-for RO-SLAM.In short, sensor networks consist of spatially distributed autonomous devices-nodes-employed to gather measurements of an environment or a process and to cooperatively transmit them to a main location.Visual SLAM does not require any deployment in the environment, which is critical in many problems, and a very high number of very good visual SLAM solutions have been developed (Salas et al. 2015).However, RO-SLAM using radio beacons is more interesting than visual SLAM in many other cases.RO-SLAM is independent of lighting conditions and does not require line-of-sight operation, whereas visual SLAM is often sensitive to changing lighting conditions and is not suitable in presence of smoke, heavy dust (Brunner et al. 2013) or specular reflections.Besides, using radio beacons as landmarks naturally solves the data association problem, typical of visual SLAM.
Consider a GPS-denied scenario where a large number of sensor nodes have been deployed at unknown locations.For instance, they have been placed at random locations for real-time monitoring of an accident, or they are used for monitoring an industrial facility and their exact location was not registered during deployment.Each sensor node gathers measurements and transmits them to a Monitoring Station.We assume that each node can measure the distance to other nodes.This is not a hard requirement, in fact most Commercial Off-The-Shelf (COTS) sensor network nodes can measure the radio signal strength (RSSI) of incoming packets and estimate the range to the emitting node (Banatre et al. 2008).Accurate mapping and robot localization is critical for the navigation of the robot in complex GPS-denied environments.Also, the localization of sensor nodes-mapping-is necessary to geolocate the measurements collected.Besides, knowing the location of robots and nodes enable robot-sensor node cooperative missions of interest in these scenarios such as sensor node transportation and deployment (Corke et al. 2006;Maza et al. 2011) or collection of data from sensors (Martinez-de Dios et al. 2013, 2017).The potentialities of robot-sensor network collaboration has originated significant interest in RO-SLAM techniques that employ sensor nodes as beacons, see e.g.Challa et al. (2005), Menegatti et al. (2009), Nogueira et al. (2010).However, most reported techniques consider beacons as passive landmarks and do not exploit the capacities they are actually endowed with.
This paper proposes a RO-SLAM scheme that exploits the robot-sensor network collaboration by distributing measurement gathering and integration in SLAM between the beacons around the robot.It is based on Sparse Extended Information Filter (SEIF) (Thrun et al. 2004), and inherits its efficiency and scalability.Its distributed approach shares resource consumption, reducing robot CPU burden, and at the same time naturally integrates inter-beacon measurements (measurements between two beacons), which improves map and robot localization accuracies.It is proven in the paper that the proposed scheme accumulates higher amount of information than the conventional SEIF SLAM and at the same time, it preserves the sparsity of the information matrix and its constant time property.The paper presents the scheme, evaluates and compares its performance with existing methods in experiments performed with an octorotor aerial robot.
This paper is an extended version of our work (Torres-González et al. 2014a).The main improvements of this paper over (Torres-González et al. 2014a) are: -a new section that justifies the validity of the proposed approach analyzing its effect on the building of the information matrix.First, it theoretically proves the preservation of the main properties of SEIF using common assumptions.Then, it analyzes with simulations their validity when the assumptions are relaxed; -extension to 3D SLAM and integration of the scheme with an octorotor aerial robot; -a new section with the extensive validation of the proposed method in 3D SLAM using an octorotor.
-extension and more detailed related work.Also, the paper has been restructured and all sections have been completed and rewritten for clarity.
The paper is organized as follows.Section 2 summarizes the main existing RO-SLAM methods.Section 2.1 briefly summarizes SEIF SLAM as an introduction to our method, which is detailed in Sect.3. Section 4 analyzes the building of the information matrix in proposed scheme and proofs that it preserves the main properties of SEIF SLAM.Section 5 evaluates and compares with existing methods its performance and robustness in 3D SLAM in real experiments and simulations.The conclusions are in the last section.

Related work
A good number of RO-SLAM methods have been developed in the last years.Most of them employ approaches based on Extended Kalman Filter (EKF) SLAM or on Fast-SLAM.FastSLAM (Blanco et al. 2008) factorizes the state vector dividing it in the vehicle pose estimation and the map estimation.It is more flexible than EKF due to the use of a Particle Filter as the core of the algorithm, which allows having different noise distributions apart from only Gaussian as in the case of EKF.In FastSLAM each particle of the filter represents an hypothesis of the state (robot pose and map).EKF SLAM (Djugash et al. 2006;Menegatti et al. 2010) is a well-known method, obtains good results in most implementations but it lacks scalability.
Different strategies have been developed in the past years to improve efficiency and scalability in SLAM.The majority of them use the canonical form of the Gaussian distribution.Thin Junction Tree Filters (Paskin 2003) employ an special data structure-called junction tree-to represent the information matrix.Work (Paskin 2003) proposes approximations to keep these junction trees simple-thin-regardless of the map size.Treemap Filters (Frese 2006) hierarchically divide the map into local regions.
Dual to Kalman Filters, Information Filters (IFs) represent the state by its canonical form based on the information vector and the information matrix.In Extended Information Filter (EIF) SLAM the update stage is very efficient-additivebut the prediction stage requires operations with the whole information matrix, involving bad scalability with the map size.Besides, it is a full SLAM solution, i.e. it solves the SLAM problem after all information has been gathered, not being suitable for on-line applications.
Sparse Extended Information Filter (SEIF) (Thrun et al. 2004;Liu and Thrun 2003) solves on-line SLAM maintaining a sparse representation of the information matrix, which simplifies the matrix operations, improving efficiency and scalability.It has been demonstrated in Thrun et al. (2004), Frese (2005) that many of the off-diagonal elements of the information matrix are relatively close to zero.SEIF SLAM enforces the sparsity of the information matrix, enabling efficient algorithms for motion update and state recovery.As a result, it can be executed in constant time regardless of the map size.SEIF for feature-based SLAM has been researched in some works.In Eustice et al. (2005) a modification for ensuring the consistency of the global map estimate was proposed.SEIF SLAM for multi-robot systems was used in Thrun and Liu (2005).
Most RO-SLAM techniques employ only robot-beacon range measurements.Work (Djugash et al. 2006) was the first in using also measurements between beacons (inter-beacon).Integrating inter-beacon measurements improves map and robot estimation accuracies and speeds up beacon initialization.Despite its advantages few methods employ them since they involve significantly higher consumption of resources including the computational power required to integrate the higher number of measurements.In a previous work (Torres-González et al. 2014) we addressed the increment in resource consumption proposing a centralized EKF SLAM scheme that dynamically disconnects the integration of inter-beacon measurements in SLAM when the desired uncertainties have been met.In this paper we propose an efficient robot-beacons distributed SEIF scheme where beacons actively participate in gathering and integrating measurements in SLAM, sharing resource consumption and ensuring constant time execution.Work (Djugash et al. 2008) presented a decentralized scheme to self-localize a sensor network with the help of a mobile robot.It is based on Loopy Belief Propagation (LBP) (Frey and MacKay 1998), so it requires the sensor network to be sparse-not fully connected-which is an important limitation in many applications.In case of having loops in the network some information can be counted twice, making it more difficult to accurately recover the exact state representation.

Range only SEIF SLAM in a nutshell
This section briefly summarizes the SEIF SLAM algorithm as an introduction to the proposed scheme.Most expressions have been omitted.For notation simplicity, time subindex t has been also omitted.Refer to Thrun et al. (2005) for further details.The adopted state vector x is comprised of the robot position (x r ) and the location of all the beacons in the map (x 1 , x 2 , . . ., x N ).SEIF SLAM is based on Extended Information Filter (EIF).Dual to EKF, EIF represents the state vector by the information vector ξ = −1 μ and the information matrix = −1 , where μ is the mean of the state vector and is the covariance matrix., see (1), is symmetric and positive-semidefinite.Each off-diagonal entry of -called link (Thrun et al. 2004)-represents the relation between two elements in x.
At any time in the SLAM operation some of the offdiagonal elements of are zero meaning lack of information between the involved elements; some of them have high values-strong links-meaning high information; and a number of them have values close to zero-weak links.Weak links have much lower impact on the estimation than strong links but both involve similar computational burden.SEIF maintains a sparse representation of by keeping the number of active beacons-beacons with non-zero links to the robot-bounded by a threshold.Every time the number of active beacons is above the bound, the sparsification step is performed deactivating the beacons with the weakest links.
Measurement update in information filters modifies only the entries of corresponding to the elements involved in the measurement.Factorizing allows efficient update stage regardless of the map size.Also, the sparsity of significantly reduces the computational burden required in SEIF for the prediction stage.For linearizing the prediction and observation models it is required to recover the state estimate μ from the predicted ¯ and ξ .The whole state is not needed, only the states of the robot and active beacons are required.Of course, enforcing sparseness in involves an approximation error in the estimations obtained by SEIF.Work (Thrun et al. 2005) suggests using sparsification bounds in the range [4][5][6][7][8][9][10] in order to balance accuracy degradation and burden reductions.
We could not find any paper that used SEIF SLAM with range measurements.The observation model we adopted for the integration of robot-beacon range measurement z r, j taken by the robot to beacon b i is: where x , μ i y , μ i z ] are respectively the estimated positions of the robot and b i .h r,i is nonlinear.Its Jacobian is: All the entries of H r,i that are not shown in (3) are zero.All the entries are zero except for those corresponding to the robot and to b i .Our method also integrates inter-beacon measurements, such as z i, j gathered by beacon b i to b j .The model adopted h i, j is an extension of that in (2).Again, all the entries of Jacobian H i, j are zero except for those corresponding to the beacons involved in the measurement: Range measurements have the problem of partial observability.The SEIF SLAM filter should be combined with an auxiliary tool for beacon initialization such as are Particle Filters (Menegatti et al. 2010), Probability Grids (Djugash et al. 2006;Olson et al. 2004) and trilateration methods (Menegatti et al. 2009).Trilateration methods, although simple and efficient, are very sensitive to measurement noise and can lead to high initialization errors.Probability Grids provide better initialization but their accuracy depends on the size and resolution of the grid.Particle Filters (PFs) are the most widely applied method.They provide better accuracy and various mechanisms have been developed to reduce their computational burden.We adopted PFs in the experiments shown in the paper.When the robot takes the first measurement from beacon b i , the initialization of b i is started and then beacon b i enters at the "initialization phase".It keeps in this stage until the auxiliary tool converges and an initial estimation of the location of b i is computed.Then, it is added to the state vector.From now on we say that b i is at the "state vector phase".

Robot-sensor network distributed SEIF SLAM
The proposed work is strongly motivated by our envisioned application: aerial robot autonomous navigation for maintenance and repairing in industrial plants, which are environments with poor GPS reception.Figure 1 shows a picture of a typical environment in our problem.These scenarios are not well textured, they contain many pipes, which often have few features.Also, pipes frequently originate specular reflections and shadows causing drastic local lighting changes involving severe problems in visual and RGB-D SLAM techniques.Although very good visual and RGB-D SLAM solutions have been developed, in these environments they are not always robust enough for safe robot localization and navigation.In our problem RO-SLAM techniques that use sensor nodes-beacons-as landmarks are an interesting alternative.Each beacon is endowed with sensing, computing and communication capabilities and can measure the distance to the robot or to other beacons within its sensing region.These are not constraints in practice.A wide variety of Commercial Off-The-Shelf sensor nodes fulfill them.Our method performs satisfactorily with low densities of beacons deployed without special care in the scenario.Besides, In conventional RO-SLAM techniques the robot gathers range measurements to the beacons within its sensing region and integrates these measurements in the update stage.In the proposed scheme measurement gathering and integration in SLAM is distributed among the beacons.The beacons involved at time t can be classified into two sets: direct beacons and indirect beacons.Direct beacons are those that at time t are within the robot sensing region (S R r ).The set of direct beacons at time t is D B S t = {b i : b i ∈ S R r }.Indirect beacons are those that are outside S R r but within the sensing region of a direct beacon.The set of indirect beacons at t is: where S R i is the sensing region of beacon b i .
The operation of the presented scheme is as follows.The SLAM prediction stage is executed in the robot.The robot keeps L N B, the list with the direct beacons it has detected.Then, the robot broadcasts an UpdateReq message that includes L N B and the predicted state.Each direct beacon b i receives the message and extracts L N B. If b i finds itself in L N B, it gathers a range measurement to the robot (z i,r ) and to each of the beacons within its sensing region S R i .This measurement set is designed as Then, each direct beacon b i computes with the measurements in M S i its contribution to the update stage and transmits it to the robot in an UpdateResp message.The update stage in SLAM represented in the information form is additive.Thus, the robot reconstructs the SLAM updated state by adding the contributions it received.Figure 2-top summarizes the main tasks and message interchanged in the proposed scheme.
Measurement gathering is illustrated in Fig. 2-bottom.The red and gray circumferences represent the sensing regions of the robot and beacons, respectively.Direct beacons are represented in black color and, indirect in gray: tional RO-SLAM the robot gathers and integrates measurements to direct beacons, {z r,1 , z r,5 , z r,6 } in Fig. 2. In our scheme each direct beacon b i gathers and integrates the measurements in its M S i , e.g.b 5 gathers M S 5 = {z 5,r , z 5,1 , z 5,6 }.Thus, when the robot completes the SLAM update stage it has integrated measurements {z 1,r , z 1,2 , z 1,3 , z 1,5 , z 5,r , z 5,1 , z 5,6 , z 6,r , z 6,5 }.
Our scheme integrates all robot-beacon and inter-beacon measurements that involve one or more direct beacons, naturally avoiding repeated measurements.It gathers one measurement for all these distances except for those between two direct beacons, e.g.b 1 and b 5 .In these cases two different measurements are gathered, e.g.z 1,5 and z 5,1 .The method is also robust to message loss, as is shown in Sect. 5.

Operation of the robot
The operation of the robot in one prediction-update cycle is summarized in Algorithm 1. First, the robot computes the SLAM prediction stage.We assume static beacons and a robot with nonlinear kinematic model.Thus, its Jacobian should be computed at each time, which requires recovering μ.Our scheme uses the efficient algorithm described in Thrun et al. (2005) for motion update and state recovery (step 1 in Algorithm 1).This algorithm computes the predicted information vector ξ , the information matrix ¯ and also the recovered predicted estimate μ.

Algorithm 1 Summary of the operation of the robot
Require: ξ t−1 , t−1 1: SEIF SLAM motion update and state recovery 2: Update L N B and create UpdateReq message 3: Broadcast UpdateReq message 4: Receive UpdateResp messages from beacons 5: Sum SLAM update contributions to ξ and ¯ as in ( 7) and ( 8) 6: SEIF SLAM Sparsification 7: return ξ t , t Next, the robot broadcasts an UpdateReq message that includes the predicted estimate μ.Transmitting the whole μ is not suitable in cases with large numbers of beacons: it would require broadcasting large-or several-messages, increasing message losses.Besides, the greater part of μ is not of interest for direct beacons.Only the elements in μ required for each direct beacon in L N B are transmitted.Each direct beacon b i gathers range measurements to the robot and to the beacons b j ∈ S R i .For integrating them it needs μ i , μ r and μ j ∀b j ∈ S R i .ev i is the vector with the estimates required for direct beacon b i to perform its SLAM update: At the beginning L N B is assumed empty.If a direct beacon that received the UpdateReq message does not find itself in L N B, it sends a Beacon Discover y message to the robot with its ID and the IDs of the beacons that are within its sensing region.Then, the robot will add it to L N B. When the robot does not receive update contributions from beacon b i within L N B in a number of consecutive times, it is interpreted that b i is currently outside S R r and is deleted from UpdateResp message from b i contains its contribution to the SLAM update (ξ i and i ).The robot receives the UpdateResp messages and when a timeout expires, it reconstructs the updated state (ξ and ) adding the predicted ξ and ¯ to the contributions it received: where F i is the projection matrix that implements operations that allocate ξ i and i at the correct entries for ξ and .
Finally, the robot checks if the updated satisfies the sparsification bound θ x .If not, the active beacons with the strongest links are selected and the weakest links are deactivated.Note that measurements from both active and non-active beacons are integrated in the update stage.Integrating measurements from a non-active beacon gives this beacon the possibility of being selected in the sparsification step.

Operation of beacons
The operation of the beacons is summarized in Algorithm 2. Each beacon b i that received an UpdateReq message first measures the range to the beacons in its sensing range-its measurement set is M S i .The operation of b i is different if it is at the "initialization phase" or at the "state vector phase".If b i is at the "state vector phase", it computes its SLAM update contribution integrating the measurements in M S i as follows: where h i,k (ev i ) and H i,k are the predictions and Jacobians for each measurement in M S i either if it is a robot-beacon measurement or an inter-beacon measurement.R is the covariance matrix of the measurement noise.
Algorithm 2 Summary of the operation of beacon b i ) then 3: Take measurements M S i 4: if (b i is at "state vector phase") then 5: Compute ξ i and i with M S i as in ( 9) and (10) 6: Send ξ i and i to robot in an UpdateResp message 7: else 8: Send M S i to robot in an UpdateResp message 9: end if 10: else 11: Send to robot a BeaconDiscovery message 12: end if Next, each beacon b i transmits the resulting ξ i and i to the robot in an UpdateResp message, as in step 6 in Algorithm 2.
If b i is the "initialization phase", the measurements in M S i cannot be used for the update of the SLAM state vector: they are used for the initialization of beacon b i .Two versions of the proposed scheme were developed: -Method1: The initialization of each beacon b i is computed by the robot.In this case b i transmits to the robot an UpdateResp message containing M S i .The robot will use M S i for the initialization of b i .When beacon initial-ization is finished, the robot computes the estimation of the location of b i and adds it to the SLAM state vector x.-Method2: Each beacon computes its own initialization: both SLAM update and beacon initialization are decentralized.b i integrates M S i in its own initialization tool.
When the beacon initialization is finished, b i sends the robot its estimation in an UpdateResp message so that it adds the initialized beacon to x.
The selection between Method1 or Method2 depends on the beacon computational capabilities and on the initialization tool.For instance, with beacons implemented using Wireless Sensor Networks (WSN) technology, Method2 is suitable in case of using trilateration initialization but would require Method1 if using PFs.Beacons implemented with embedded PCs (RaspberryPi as in the experiments), smatphones or PDAs can execute Method2 using PFs as initialization tools.

Analysis of the proposed scheme
If we analyze the building of the information matrix in the proposed scheme we can obtain the following conclusions: -In the proposed scheme the integration of inter-beacon measurements is responsible for obtaining higher values of , and hence lower robot localization and map uncertainties, than schemes that integrate only robot-beacon measurements.We believe that this conclusion is clear and for brevity it is not proven.-The proposed method preserves the sparsity of the information matrix of the conventional SEIF SLAM.Under common assumptions, both create the same links of the information matrix.This will be proven in Sect.4.2.-The proposed scheme preserves the constant time property of SEIF SLAM, which integrates only robot-beacon measurements.It is clear that the prediction stage and the distributed update stage of the proposed scheme are constant time.In Sect.4.3 it will be proven that state recovery in our scheme is also constant time.
Finally, in Sect.4.4 the assumptions of the demonstrations used in Sects.4.2 and 4.3 are relaxed and the above conclusions are confirmed using simulations.Next, the building of the information matrix and links is described.

Building of the information matrix
The information matrix and its structure was briefly presented in Sect.2.1.Each off-diagonal entry of the information matrix constraints two elements in the state vector-each ondiagonal entry constraints one.The off-diagonal entry r,i Two types of links can be distinguished: robot-beacon links and inter-beacon links.A robot-beacon link relates the robot with a beacon.They are created or reinforced (the value of the link is incremented) when a robot-beacon measurement is integrated in the SLAM update stage.Integrating z r, j affects r,r , j, j and the robot-beacon links between the robot and b j , i.e. r, j and its symmetric j,r .Inter-beacon links relate two beacons.They are created or reinforced when an inter-beacon measurement is integrated.Integrating z i, j affects i,i , j, j and inter-beacon links i, j and j,i .Interbeacon links are created/reinforced also when integrating the robot motion in the SLAM prediction stage.
Figure 3 illustrates how the information matrix is built in the conventional SEIF SLAM (a-c) and in the proposed scheme (d-f) in a simple example.We assume that b 1 and b 2 are currently direct beacons (within S R r ) and that b 1 and b 2 are within the sensing region one another.b 3 is within the sensing region of b 2 .We assume that initially r,1 = r,2 = 1,2 = 0. First, we describe the operation of the conventional SEIF SLAM.The SEIF SLAM update stage integrates measurements z r,1 and z r,2 , which creates robotbeacon links r,1 and r,2 -and their symmetric links, see Fig. 3a.Next, the robot motion in the SEIF SLAM prediction stage transfers some low amount of information from the robot-beacon linksr,1 and r,2 in Fig. 3b-to the interbeacon links between the involved beacons-1,2 .Link 1,2 is created.At that time, r,1 and r,2 are strong (have high value) whereas 1,2 is weak.Next, the sparsification step in this example deactivates b 1 and hence, removes link r,1 , see Fig. 3c.
Our scheme creates exactly the same robot-beacon links that SEIF SLAM and exactly with the same value-strength.The main difference between both is how they treat interbeacon links.In SEIF SLAM the only way to create/reinforce inter-beacon links is through the robot motion in the prediction stage.Besides, in our method they are created/reinforced also when integrating inter-beacon measurements.As a result inter-beacon links accumulate significantly more information than in SEIF SLAM.

Preservation of the sparsity of the information matrix
The conventional SEIF SLAM imposes sparsification bounds both in the number of robot-beacon links, θ x , and in the number of inter-beacon links, θ y (Thrun et al. 2004).In Thrun et al. (2004) it was proven that keeping the number of robotbeacon links below θ x automatically constrains the number of inter-beacon links below θ y .One could think that in our scheme integrating inter-beacon measurements may increase the number of inter-beacon links, violating the sparsification bound.In this section we proof that this is not the case: we demonstrate that in many scenarios it creates exactly the same inter-beacon links that the conventional SEIF SLAM.Our scheme accumulates higher amount of information than the conventional SEIF SLAM, but this increment is not used to create new links but to increase the strength of existing links, enabling lower map and robot uncertainty with a compact description.
Without loss of generality we assume that: (1) every beacon is within the robot sensing range at some time in the robot path and; (2) every pair of neighbor beacons in the environment (beacons which are within the sensing region one another) fall within S R r at some time in its path.These assumptions do not involve practical constraints.They are met in cases where the robot follows densenavigation paths, which are typically adopted in SLAM in mapping or surveillance applications.Besides, in Sect.4.4 it is shown that sparsity preservation is confirmed also when these assumptions are relaxed.
In the proposed scheme inter-beacon links are created through the robot motion and also by the integration of inter-beacon measurements.In the following the sets of inter-beacon links created in the full experiment through both mechanisms are analyzed.The integration of z i, j creates/ reinforces inter-beacon link i, j .Assuming densenavigation robot paths, the robot will integrate inter-beacon measurements of every pair of neighbor beacons ∀ b i , b j : b i ∈ S R j .Thus, in the full robot path the set of inter-beacon links created by inter-beacon measurements are: Besides, recall that the robot motion creates an interbeacon link between any pair of beacons in which both beacons fall simultaneously within S R r .The robot path is assumed dense enough such that every pair of neighbor beacons b i and b j in the environment-∀b i , b j : b i ∈ S R j -fall simultaneously within S R r at some time in the robot path.Thus, the set of inter-beacon links between neighbor beacons created through the robot motion in the full robot path is: The robot motion creates inter-beacon links between neighbor beacons but, also between beacons that are not neighbors one another but both are simultaneously within S R r at a certain time t.The set of inter-beacon links between non-neighbor beacons will be called L 2 R .In the general case L 2 R = ∅ and the set of inter-beacon links created through the robot motion is 11) and ( 12) it is easy to notice that L I B ⊆ L R .
In the conventional SEIF SLAM inter-beacon links are created only via the robot motion: the set of inter-beacon links is L SE I F = L R .In our scheme they are created through both mechanisms: the set of inter-beacon links is L prop = L I B ∪ L R = L R .Thus, our scheme creates exactly the same inter-beacon links as SEIF SLAM, preserving the sparsity of the information matrix.From Sect.4.1 it is clear that both create the same robot-beacon links.Thus, it is concluded that under the above assumptions both create the same robotbeacon and also inter-beacon links.
If the dense-navigation robot path assumption is not met, inter-beacon measurements could create some links in L I B that are not present in L R .Section 4.4 evaluates through simulation the effects of relaxing this assumption.

Preservation of constant time property
Constant time execution is a well known property of SEIF SLAM.The proposed scheme distributes measurement gathering and update computation making them independent of the map size.This section analyzes if the state/map recovery in the proposed scheme preserves constant time execution.
A critical key for SEIF SLAM being constant time is that it only recovers the state of the robot and of all the active direct beacons.As described in Sect.3.1 our scheme recovers ev i for each direct beacon b i currently within S R r .Recalling Eq. ( 6), state/map recovery for direct beacon b i involves computing: μ r , μ i and the state of the beacons neighbors of b i .This should be done for each direct beacon at time t.Thus, considering all direct beacons it is easy to notice that at time t state/map recovery should recover: the robot state; the states of currently active direct beacons and; the state of active indirect beacons at time t-recall the definition in Sect.3. In the proposed scheme recovering the states of indirect beacons involves an increment in the computational cost over that of SEIF SLAM.However, as proven below state/map recovery is kept constant time.
Let n D B and n I B be the number of direct and indirect beacons in the surrounding of the robot at time t.SEIF SLAM recovers the states of the robot and of direct beacons (in black in Fig. 2).The proposed scheme recovers also the state of indirect beacons (in gray in Fig. 2).Our scheme uses the state recovery algorithm presented in Thrun et al. (2005), which complexity is linear with the number of beacons states recovered: O(n D B ) for SEIF SLAM and O(n D B + n I B ) for the proposed scheme.
The complexity of state/map recovery for the proposed method is as bounded as it is for SEIF SLAM.The computational burden of state/map recovery in our method-and also in SEIF SLAM-depends on beacon density, and not on the total number of beacons present in the environment.Highly inhomogeneous local beacon densities are not very useful for RO-SLAM.It is more interesting if beacons are deployed in densities with some homogeneity.In these cases n D B + n I B will have similar values along the robot path.

Simulation relaxing assumptions
This section analyzes using simulations the conclusions in Sects.4.2 and 4.3 when the assumptions are relaxed.A 70 × 70 m scenario with 50 randomly deployed beacons was considered.The range measurement noise is assumed Gaussian with zero mean and standard deviation of σ m = 0.8 m.The robot trajectories were different in every experiment and did not meet the dense-navigation assumption.The sparsifi- cation bound was θ x = 10, in the range suggested in Thrun et al. (2005).The proposed method was compared with EKF SLAM and SEIF SLAM.All the methods used PFs with 500 particles for beacon initialization and of course, all were set with exactly the same parameters.
It is easy to notice that the proposed scheme integrates more information than EKF SLAM and SEIF SLAM.Table 1 compares their performance in a set of 50 simulations with different beacon settings and robot paths.Two main metrics are analyzed: the average amount of information integrated in and the average number of inter-beacon links created.Our scheme creates the same robot-beacon links than SEIF SLAM and are not compared.In average, the proposed method integrated 66% more information than EKF SLAM and 67% more than the conventional SEIF SLAM.Besides, in average EKF SLAM created 1042 inter-beacon links, SEIF SLAM created 631 and the proposed scheme, 645.These scenarios do not meet the dense-navigation robot path assumption and the proposed scheme created only 2.2% more inter-beacon links but integrated 67% more information than the conventional SEIF SLAM.
The proposed method creates the same inter-beacon links as the conventional SEIF SLAM in case of having densenavigation robot paths.If not, it creates links involving beacons that are out of the sensing range of the robot in its path.These links could not be created by SEIF SLAM and are useful to improve map estimation completeness.For instance, Fig. 4-top shows the inter-beacon links created by SEIF SLAM and the proposed method in one example.In the proposed scheme all the beacons created links with other beacons.That is not the case with SEIF SLAM, which cannot create links with some beacons originating incomplete maps.
Our method creates a relatively low number of strong inter-beacon links between nearby beacons which provide high amount of information of the map.As an example, Fig. 4-center shows the values of the inter-beacon links created in each case in the simulation in Fig. 4 and n I B along the simulation be noticed that n I B is higher when n D B is higher, and lower when n D B is lower, exhibiting some proportionality.n D B took values between 4 and 8 during most of the time, and n I B , between 5 and 13.The lowest values took place when the robot was located at the borders of the scenario, where few beacons were within S R r .The highest occurred the robot was at places with high local beacon densities.These results depend on beacon density and not on map size.

Validation in 3D SLAM experiments
The proposed scheme was integrated and experimented in AMUSE, an octorotor aerial vehicle developed in the ARCAS project for aerial manipulation and industry main- Maintenance of industrial facilities is currently performed using sensor nodes that gather measurements for process monitoring and anomaly detection.In these complex scenarios GPS is often unavailable or has bad quality-intermittent signals, reception from few satellites, GPS shadows.UAS are suitable tools for confirming the detected anomalies (Martinez-de Dios and Ollero 2006) but require accurate sensor and robot localization.
A total of 24 beacons were deployed at random locations and different heights in a 25 × 25 m environment.Each beacon was comprised of a RaspberryPi running Linux connected through USB to a Nanotron nanoPAN 5375 Timeof-Flight (ToF) range sensor and to a WiFi USB adapter, see Fig. 5-bottom.The performance of Nanotron sensors in outdoors was characterized experimentally.Their error can be modeled as a Gaussian PDF with zero mean and a standard deviation of σ m = 0.6 m.Each beacon executed an independent ROS (Robots Operating System) node that implemented the operation shown in Algorithm 2. The PFs were set with 500 particles distributed on a sphere.The spar-sification bound taken was θ x = 10, in the range suggested in Thrun et al. (2005).A Nanotron nanoPAN 5375 sensor was mounted on the octorotor landing skid.AMUSE is equipped with a Novatel OEM6 GPS with centimeter accuracy, which was taken as ground truth.UAS odometry obtained from Inertial Measurement Units is often too noisy to be used in SLAM.Like most works in 3D SLAM, e.g.Fabresse et al. (2014), we opted for employing 5 beacons as anchors for correcting UAS odometry.

Performance analysis
Figure 6 shows the result of the proposed scheme in one experiment in XY view (left) and 3D view (right).The resulting average map and robot errors were 0.34 and 0.49 m, respectively.The errors in XY were 0.19 and 0.35 m.In Z , they were 0.29 and 0.28 m.Table 2 compares the proposed Method2 with EKF SLAM and SEIF SLAM.Of course, all the methods were set with exactly the same parameters.This table shows the resulting average map and robot localization RMS errors and PF convergence times.The mean robot and map RMS errors for Method2 were 21 and 33% lower than for SEIF SLAM, and 20 and 30% lower than for EKF SLAM.The mean PF convergence time for Method2 was 81% lower than for SEIF SLAM.Besides, the robot CPU time for Method2 was 35.3% lower than for SEIF SLAM.EKF SLAM devoted the lowest robot CPU time: it is efficient with low number of beacons but scales badly with the map size as described in Sect.5.2.
If we compare these results in 3D experiments with those in 2D described in Torres-González et al. (2014a), we notice that in the 3D experiments the robot location error was 49-35 cm in XY-and, in the 2D experiments, it was 41 cm.There are two main differences between both sets of experiments.First, the range sensors were the same in both cases but the 3D experiments were performed in outdoors and the 2D experiments, in indoors.The range measurements from Nanotron sensors have higher noise level in indoors (σ m = 0.8 m) than in outdoors (σ m = 0.6 m).Second, the odometry of the aerial robot is based essentially on measurements from Inertial Measurement Units (IMU) and is significantly more noisy than that of the ground robot, which is based on encoders.The robot odometry is used in the prediction stage and range measurements, in the update stage.Thus, the prediction stage in these 3D experiments is less accurate than that in Torres-González et al. (2014a) but the update stage is more accurate.If we consider both effects, in these sets of experiments the improvement due to lower measurement noise was stronger than the worsening due to poorer odometry.
Figures 7-top and -center show the evolution of the location error for each beacon in Method2 and SEIF SLAM-EKF SLAM performs similarly to SEIF SLAM and is not shown.The drawing for each beacon starts when its aux-  In these experiments, the proposed method created exactly the same inter-beacon links-143-as SEIF SLAM.The sensing range of Nanotron sensors was higher than 100 m and hence, the robot dense-navigation assumption was met. Figure 7-bottom analyzes the evolution of the values of each inter-beacon link along the experiment in the proposed scheme (red) and in SEIF SLAM (green).For better illustrating the tendency, the envelope that groups the 90% central curves is shown for each case.Links start taking non-zero values in the proposed scheme sooner than in SEIF SLAM, which is originated by the shorter beacon PF convergence times.The values of inter-beacon links in the proposed scheme grow at higher constant rate-and are higher at any time-than in SEIF SLAM.At any time along the experiment the proposed method has accumulated more information in , involving lower mapping uncertainty.

Discussion
This section analyzes four aspects of the proposed scheme: (1) robustness against message errors, (2) beacon density, (3) consumption of resources of beacons and (4) scalability.
The impact of message loss was evaluated using the measurements collected in the real experiments and simulating transmission errors with Packet Reception Rates (PRRs) in the range [60-99%].Figure 8-top shows the map and robot localization errors obtained with EKF SLAM, SEIF SLAM and the proposed Method2.The first two are not influenced by transmission errors.The update stage of Method2 is additive, which makes it rather robust to message loss.Map RMS error for Method2 is lower than for EKF SLAM and SEIF SLAM for any PRR level.Method2 had the lowest robot error for PRR levels higher than 70%.Although the proposed method makes extensive use of transmissions, it is significantly robust to transmission errors.
Beacon density has also influence in the proposed method.Rather than physical density, in our problem density refers to measurement density, i.e. in average the number of beacons that are within the measurement range of other beacons.The measurement range of the beacons used in these experiments is 100 m, i.e. each sensor could measure the range to the rest of the deployed sensors.Hence, reducing the number of beacons considered in the experimental area reduces the beacon density.In the following the influence of beacon density in real experiments is analyzed.We executed SEIF and the presented method with the logs of the experiments shown in Sect.5.1 but with lower numbers of sensors N D = 1, 2, . . ., 18, i.e. we did not considered the measurements from some of the nodes.For each N D, both techniques were executed in a total of 15 different scenarios, each with different combinations of beacons (randomly selected).very evident: the errors decrease at lower rate since the main component in the error is originated by the robot odometry.SEIF behaves similarly but its improvement is lower due to the lack of inter-beacon measurements.
In these experiments beacon density of N D = 6 was a good trade-off between robot location error and beacon deployment effort.In a real environment, to be conservative we would select N D = 8-11.Since the measurement range of the sensors is 100 m, it involves deploying 8-11 beacons per 100 × 100 m 2 , which is low effort-demanding and feasible in many industrial plants.In the experiments performed the sensor locations were not carefully selected: they were  selected in a random way.In a real scenario no special care would be required either apart from trying to avoid large direct obstacles between beacons and the robot.The proposed scheme exploits beacon capabilities involving higher consumption of beacon resources than conventional methods.Table 3 shows the average energy consumed by the beacons in the experiments.It was estimated with the number of measurements gathered and the number of messages interchanged in the experiments, using the power consumption characteristics in the manufacturers data-sheet.The beacons energy consumption using Method2 is more than 3 times higher than with EFK SLAM and SEIF SLAM.It seems interesting to develop strategies to reduce the number of inter-beacon measurements without losing their advantages: map and robot accuracy and fast map initialization.
In order to analyze scalability we performed series of simulations, with the same parameters of the real experiments.25 experiments were performed for each map size with between 20 and 200 beacons-the beacon density was kept steady.Figure 9a shows the average robot computational burden of the EKF SLAM, SEIF SLAM and the proposed Method1 and Method2.Method2 is the most efficient for maps with more Inter-beacon measurements enforce map consistency and, in absence of inter-beacon measurements, the estimations are more influenced by the robot odometry noise.
Figure 9c shows the average number of inter-beacon links created at the end of each simulation by each method.Figure 9d shows the sparsity of the information matrix expressed as the ratio between the inter-beacon links created in the experiment and the total number of possible inter-beacon links.The sparsity of the information matrix increases with the map size.They both perform similarly in terms of the absolute number of links and sparsity of the information matrix.Notice that although with higher map sizes the number of links created in the proposed scheme is a bit higher than in SEIF SLAM, its information matrix is sparser.Integrating inter-beacon measurements allows our scheme to detect more beacons and add them to the state vector, improving map completeness, but these new beacons create very few links, increasing the sparsity of the information matrix.For instance, with maps of 190 beacons, the state vector in our scheme contained 185 beacons, created 7048 inter-beacon links out of the total of 17020 possible links, whereas in SEIF SLAM the state vector contained 167 beacons and created 6349 inter-beacon links out of 13861 possible links.Our scheme builds more complete maps involving larger state vectors and information matrices than SEIF SLAM but also the information matrices are sparser and the robot CPU burden is lower as shown in Fig. 9a.

Conclusions
This paper proposed a distributed SEIF SLAM scheme for robot-sensor network cooperation applications.This cooperation allows the distribution between the robot and the sensor network nodes (beacons) of tasks like measurement gathering and integration in SLAM.Beacons take range measurements to the robot and to their neighbor beacons, estimate their contribution to the SLAM update and send it to the robot.Besides robot-beacon measurements, the proposed scheme naturally integrates inter-beacon measurements, resulting in more accurate map and robot estimations and faster convergence of the beacon initialization PFs.Distributing measurement integration in SLAM reduces the robot burden.
The impact of the proposed scheme on the building of the information matrix was analyzed in detail.It was proven that: (1) it accumulates higher amount of information than SEIF SLAM; (2) it creates a very similar number of links than SEIF SLAM, preserving the sparsity of the information matrix and; (3) it preserves the constant time property of SEIF SLAM.The proposed scheme integrates more information than SEIF SLAM preserving its compact description, efficiency and scalability.The proposed scheme was validated and compared to existing methods in 3D real experiments.
Integrating inter-beacon measurements involves significant advantages but also requires the consumption of resources such as energy and bandwidth.It would be interesting to develop tools that decide which measurements are more interesting to integrate in SLAM and which do not provide enough information and are not worth gathering them.This issue is object of current research.

Fig. 1
Fig. 1 Pictures of the objective scenario of the proposed method

Fig. 2 (
Fig. 2 (Top) main tasks and messages interchanged between the robot and beacon b i in the proposed scheme.(Bottom) measurement gathering in a conventional RO-SLAM and in the proposed scheme.The red triangle represents the robot.Direct beacons are represented with black circles and indirect, with gray circles.Circumferences represent the sensing regions of the robot (red) and of beacons (gray) (Color figure online)

Fig. 3 (
Fig. 3 (Top) effect of the steps in the conventional SEIF SLAM on the information matrix: a measurement integration, b prediction stage and c sparsification.(Bottom) effect of the steps in the proposed scheme on the information matrix: d measurement integration, e prediction stage and f sparsification links together the estimations of the robot pose and of the location of beacon b i .Entry i, j ∀i = j links together the localization estimations of b i and b j .Two types of links can be distinguished: robot-beacon links and inter-beacon links.A robot-beacon link relates the robot with a beacon.They are created or reinforced (the value of the link is incremented) when a robot-beacon measurement is integrated in the SLAM update stage.Integrating z r, j affects r,r , j, j and the robot-beacon links between the robot and b j , i.e. r, j and its symmetric j,r .Inter-beacon links relate two beacons.They are created or reinforced when an inter-beacon measurement is integrated.Integrating z i, j affects i,i , j, j and inter-beacon links i, j and j,i .Interbeacon links are created/reinforced also when integrating the robot motion in the SLAM prediction stage.Figure3illustrates how the information matrix is built in the conventional SEIF SLAM (a-c) and in the proposed scheme (d-f) in a simple example.We assume that b 1 and b 2 are currently direct beacons (within S R r ) and that b 1 and b 2 are within the sensing region one another.b 3 is within the sensing region of b 2 .We assume that initially r,1 =

Fig. 4 (
Fig. 4 (Top) inter-beacon links created in one simulation by the proposed method (left) and SEIF SLAM (right).Only the 25% strongest links are shown for visibility.(Center) values of the inter-beacon links created by the proposed method, SEIF SLAM and EKF SLAM, ordered-stronger links first-for visualization.(Bottom) values of n D B and n I B along the simulation

Fig. 5 (
Fig. 5 (Top) AMUSE octorotor flying during one experiment.(Bottom) Components of the beacons employed: a RaspberryPi connected to a Nanotron sensor, a WiFi USB adapter and a battery

Fig. 6
Fig. 6 Result of 3D SLAM experiment: (left) XY view, (right) 3D view.The mean map and robot errors were 0.34 and 0.49 m, respectively.The mean errors in XY were 0.19 and 0.28 m

Fig. 7
Fig. 7Evolution of the location error for each beacon in SEIF SLAM (top) and in the proposed Method2 (center).(Bottom) Evolution of the values of inter-beacon links along the experiment in SEIF SLAM (green) and in our scheme (red).For better illustrating the tendency, the envelope that groups the 90% central curves is shown (Color figure online)

Fig. 8 (
Fig. 8 (Top) impact of PRR on map-full lines-and robot-dashederrors for the proposed Method2, EKF SLAM and SEIF SLAM.(Bottom) analysis of the influence of beacon density on the proposed Method2

Fig. 9
Fig.9Performance analysis of the proposed Method1, Method2, EKF SLAM and conventional SEIF SLAM with different map sizes: a robot computational burden measured using MATLAB profiler; b maprepresented with full lines-and robot-dashed lines-errors; c number

Table 1
of the average amount of information accumulated and the number of inter-beacon links created by EKF SLAM, SEIF SLAM and the proposed scheme in a set of experiments

Table 3
Average beacon energy consumption[J]