Multi-Epoch Multi-Agent CollaborativeLocalization Using Grid-based 3DMA GNSS and

Inter-Agent RangingSiddharth Tanwar and Grace Xingxin Gao

Stanford University

BIOGRAPHY

Siddharth Tanwar is a Ph.D. student in the Department of Electrical Engineering, Stanford Univesity. He received his B.Tech.degree in Electrical Engineering from the Indian Institute of Technology, Kanpur, India in 2017 and M.S. degree inElectrical and Computer Engineering from the University of Illinois, Urbana Champaign in 2019. His current researchinterests include autonomous vehicle navigation and control, robotic networks and collaborative robot navigation.

Grace Xingxin Gao is an assistant professor in the Department of Aeronautics and Astronautics at Stanford University. Beforejoining Stanford University, she was an assistant professor at University of Illinois at Urbana-Champaign. She obtainedher Ph.D. degree at Stanford University. Her research is on robust and secure positioning, navigation and timing withapplications to manned and unmanned aerial vehicles, robotics and power systems.

ABSTRACT

GPS navigation in urban environments is prone to error sources such as multipath and signal blockage. In our previous work,we demonstrated a snap-shot multi-agent collaborative 3D-mapping aided (3DMA) GNSS localization algorithm to reduce theimpact of these error sources on the position solution. However, a source of error in snap-shot algorithms is jump discontinuitiesin the position solution due to fluke noise measurements. Therefore, an approach which takes into account temporal correlationbetween agent positions will further increase the robustness of the system through jump discontinuity error mitigation.

In this paper, we present a multi-epoch decentralized collaborative localization algorithm for urban navigation using 3DMAGNSS and inter-agent ranging. The algorithm is a multi-epoch variant of our previous snap-shot multi-agent collaborative3DMA localization algorithm and uses Intelligent Urban Positioning (IUP) and ambiguity mitigation by constraining an agentsprobability distribution using its neighbors. The key additions that extend our prior algorithm to a multi-epoch frameworkare the inclusion of a velocity estimation framework using banks of Extended Kalman filters and a discretized Bayes filterprediction step to propagate a position probability distribution in time. Furthermore, the methodology is applicable to sparselyconnected asynchronous networks, and has limited information exchange. The proposed method is validated on simulateddatasets in an urban area of Champaign, Illinois with multiple agents in a variety of scenarios. We demonstrate improvedperformance in terms of positioning accuracy. We also analyze the impact of network connectivity on positioning accuracy.

I. INTRODUCTION

GPS navigation in urban environments suffers from error sources such as multipath and signal blockage. However, ifwe consider several agents in a network, the localization capabilities may differ among the agents due to reasons such asheterogeneity of sensors, different view of the sky, the structure of buildings around agents, etc. This can be leveraged tolocalize all the agents better if they cooperate with each other. Recent work on multi-epoch 3D-mapping-aided (3DMA) GridFilter algorithm [1] has shown high potential in 3DMA GNSS navigation for a single agent in urban environments. Alongwith reducing ambiguity error in positioning compared to snapshot 3DMA methods such as [2], this methodology provides anopportunity to incorporate additional external sensing (albeit not trivially) in a multi-epoch approach to further increase therobustness of the system in terms of accuracy, ambiguity mitigation and jump discontinuity error mitigation.

A. Collaborative Localization (CL)

In early 1990s, Kurazume et al. showed that we can achieve a lower positioning error using CL when compared to deadreckoning. Over the years, several developments in the field have led to a classification of CL approaches based on the networkarchitecture as centralized [3],[4],[5],[6] and decentralized [7],[8],[9],[10] algorithms. The centralized algorithms rely on theuse of one or more fusion centers [11] to host a bulk of the computation resources. This makes these algorithms vulnerableto points of failure and impose higher communication requirements on the agents and the fusion center. This further limitsscalability of such approaches.

Decentralization, on the other hand, promotes scalability and provides security against single points of failure. While anoptimal decentralized approach as proposed by Roumeliotis et al. in [12] showed that the equations of a centralized Kalman

Fig. 1: An overview of the proposed methodology for an example network of three agents (Agents i, j and k). Only the agents that obtaininter-agent ranges are involved in the information exchange (agents i and j in this example). This keeps communication to the minimumsince we do not require information to be shared to the agents not performing the relative update step (agent k in this example). Each agentperforms a propagation step using a predefined motion model and a velocity distribution. This is followed by the update step which comprisesof private update (using 3DMA GNSS techniques) and relative update (using a Bayesian method for inter-agent ranging measurement update).Finally, a velocity distribution is calculated using a bank of Kalman filters over the grid of candidate points.

estimator can be distributed into reduced dimension filters, such an approach relies on a synchronous, fully connected networkof agents. This limits the usability of the methodology to small systems in a controlled environment. To tackle these issues,methods which approximate the optimal filter have been devised. These operate within constraints such as limited inter-agentconnectivity in a network [13],[10],[14], lossy and asynchronous communication [15], as well as lower communication andprocessing overhead per agent [7],[10],[16].

B. 3DMA GNSS

Techniques which rely on the use of 3D map data to aid in GNSS localization fall under the category of 3DMA GNSS algo-rithms. These methods are suited to urban environments since 3D maps are readily available for the same. These methods broadlyinclude terrain height aiding [17], mapping-aided ranging [18][19][20][21], and shadow matching techniques [22][23][24][25].Terrain height aiding utilizes digital elevation maps to restrict an agent’s motion on the ground or at a constant offset to it.This, effectively, removes a dimension from the position solution and improves accuracy along the remaining dimensions bya factor of two in urban environments[21]. Methods have also been proposed in [18][19] which predict satellite non line-of-sight (N-LOS) path delays using the 3D city map. However, this is is computationally expensive and path delays cannot beprecomputed efficiently.

We can also use the 3D city map to predict satellite as line-of-sight (LOS) or N-LOS and remove the predicted N-LOSsatellites from the solution computation [26][27][28]. However, this leads to poor satellite geometry in urban environmentsand typically leads to high localization errors in across-street directions. Shadow matching proposed by Groves in [22] is aseminal work in 3DMA GNSS and tackles the problem of error in across-street direction. In Shadow matching, we classifysatellites as LOS or N-LOS based on the received signal-to-noise ration (SNR) value. We further predict over a grid ofcandidate points the probability of each being the likely position solution using known satellite positions and the 3D city map.This predicted probability is then combined with the satellite classification probabilities to obtain the final position probabilitydistribution over the grid. Shadow Matching naturally compliments ranging-based methods [29]. In [30], Suzuki proposed a3DMA GPS integration using a particle filter. While particle filters allow better modeling of the non-linearity in estimation,they are susceptible to erroneous convergence giving overly optimistic incorrect estimates. Intelligent Urban Positioning (IUP)proposed in [2] combined pseudorange and SNR values by using Shadow Matching (SM) [29], Likelihood-based 3DMARanging (LB-3DMAr) [20], and Least Squares 3DMA Ranging (LS-3DMAr) [21] collectively. This method is, however, proneto error sources such as ambiguity [29]. In our previous work [31], we tackled this issue of ambiguity by introducing anadditional sensing modality in the form of inter-agent ranging in a CL framework. However, the method is prone to jumpdiscontinuities due to fluke noise in measurements since it is a snap-shot algorithm. More recently, work on multi-epoch3D-mapping-aided (3DMA) Grid Filter algorithm [1] by Groves and Adjrad has shown higher accuracy in 3DMA GNSSnavigation for a single agent in urban environments.

In this work, we propose a multi-epoch decentralized collaborative localization framework for navigation of multiple agentsin an urban environment by coupling 3DMA GNSS measurements with inter-agent ranging sensor modality. The proposedframework is applicable to sparsely communicating networks and information exchange is limited to only those agents whichobtain relative range measurements. Furthermore, we allow the agents to carry out the updates asynchronously. We buildour work upon a collaborative shadow matching algorithm [31], and multi-epoch 3DMA Grid Filter algorithm [1]. Unlike[31], which is a snapshot algorithm, this work allows for a mechanism to account for temporal correlations between posesfor each agent. We do this by the introduction of a discretized Bayes filter prediction step to propagate agent positionprobability distribution in time. Additionally, we introduce a velocity distribution calculation step which uses pseudo-range ratemeasurements in a bank of Kalman filters to provide velocity estimates for the prediction step. The proposed methodology allowsus to work with multiple agents in a grid filter framework which is not directly extensible from [1]. This enables the proposedmethodology to further improve ambiguity error mitigation and localization accuracy by preventing jump discontinuities atsubsequent time steps and introducing an additional sensor measurement. Fig. 1 presents an overview of the proposed methodwith three agents (Agents i,j and k). We validate the proposed decentralized collaborative localization algorithm throughsimulation in urban settings.

II. PROBLEM FORMULATION

In this work, we consider a network (denoted by O) of N agents in an urban area with a known 3D Map (denoted by C).S denotes the set of all operating satellites in the GPS constellation. For each agent i ∈ O, St

i denotes the set of satellites’information available to agent i at time t. For each satellite s ∈ St

i , agent i stores the satellite position and velocity (obtainedfrom online public access ephemerides data and orbital physics), pseudorange, associated C/N0 value, and pseudorange rate(if obtained by measurement). Each agent has a set of neighboring agents (denoted by Mt

i) to which it can communicateinformation. An agent maintains its state denoted by Y t

i consisting of the agent position in terms of latitude and longitude. Weconstrain the agent to be at ground level (such as pedestrians, autonomous vehicles etc.) and obtain the altitude informationusing digital elevation maps [32]. The state Y t

i is obtained from a probability distribution Λti discretized over a grid of candidate

positions (denoted by Gi). Note that Λti can be multi-modal. Additionally, we also maintain a velocity estimate (denoted by

vti,cell) and corresponding covariance (denoted by Qti,cell) for each cell over the grid of candidate points. The set of these

estimates over the grid gives us the velocity distribution and is denoted by V ti .

Each agent i ∈ O performs the following sensor measurements:(i) Satellite pseudorange measurement: The satellite pseudorange measurement model is described in two parts here. First,

for a line-of-sight (LOS) signal reception, for each satellite s ∈ Sti , a pseudorange measurement ρtsi at time t is obtained.This is modeled as

ρtsi = ‖pti − pts‖+ bti + εsi (1)

where ‖·‖ denotes euclidean distance, pti, pts are the 3-D coordinates of the agent and satellite s respectively in ECEF

frame, bti is the receiver clock bias (in m) and εsi is noise. Further, this noise is assumed independent and Gaussiandistributed as

εtsi ∼ N(

0, εtsi2)

(2)

Here, εtsi2 can be obtained as a function of satellite signal to noise ratio, elevation etc. [33]. In our algorithms, we used

εtsi2

= α+ β × 10−cnotsi

10 (3)

where {cnotsi} is the received signal to noise ratio from satellite s at time t, α and β are empirically determined constants.Second, for a non-line-of-sight (NLOS) reception, this pseudorange is modeled as

ρtsi = ‖pti − pts‖+ bti + κsi (4)

where ‖·‖ denotes Euclidean distance, pti, pts and bti have the same meaning as above, and κsi is noise assumed to be

skew-normal distributed with the probability distribution function given in [34]. Note that predicting satellite as LOS orN-LOS is also part of the problem.

(ii) Satellite pseudorange rate measurement: For the multi-epoch algorithm, velocity calculation requires pseudorange ratemeasurements. This measurement model is described as

ρtsi = (vts − vt) · 1tsi + bt+ ∝si (5)

where vts is the satellite velocity vector obtained from the navigation message broadcast by the satellite, v is agent velocityvector. Both vts and v are expressed in ECEF coordinate frame. 1tsi is the user-to-satellite line-of-sight unit vector and

bt is the rate of change in receiver clock (m/s). ∝si is the noise and is modeled as white Gaussian with zero mean andcovariance ∝t

si2 which is empirically determined.

(iii) Inter-agent ranging measurement: Agents possess ranging sensors to estimate distance to neighboring agents with knowncorrespondences. This is modeled as

rtij = ‖pti − ptj‖+ ϕij (6)

where ϕij is white Gaussian noise and is modeled as

ϕij ∼ N(

0, ψtij

2)

(7)

The term ψtij

2 can be empirically estimated. Note that the clock bias term is eliminated using methods such as round triptime of arrival.

The problem, then, is to obtain a consistent estimate of state Y ti for each agent i ∈ O given the pseudoranges from all LOS

and N-LOS satellites ρtsi, pseudorange rates ρtsi, inter-agent ranging measurements rtij , satellite positions pts and velocities vts,and 3D map C. Additionally, agents are allowed to communicate some information to a finite set of neighbors Mt

i.

III. METHODOLOGY

Multi-Epoch Collaborative Shadow Matching (ME-CSM) algorithm follows a Bayesian filter [35] structure comprising ofthe prediction and update steps in a discrete grid of position candidate points. The algorithm is a multi-epoch variant of thesnapshot Collaborative Shadow Matching (CSM) algorithm described in [31]. The key additions to the snapshot algorithm from[31] are two fold. First is the inclusion of a velocity estimation framework using extended Kalman filtering with pseudorangerate measurements. Second is the Bayesian prediction step which propagates the position probability distribution using avelocity distribution over the grid of candidate points. The algorithm adopts elements from Intelligent Urban Positioning (IUP)algorithm [2] and CSM [31] in the Bayesian framework. Unlike the snapshot algorithm, this allows for a mechanism to accountfor temporal correlations between poses for each agent. This enables the proposed methodology to further improve ambiguityerror mitigation and localization accuracy by preventing jump discontinuities at subsequent time steps and introducing anadditional sensor measurement.

A. ME-CSM Algorithm Details

The proposed algorithm, ME-CSM, is described in algorithm 1. Each agent’s state is initialized as a guess and this is usedto generate a grid of candidate positions in step 2. We initialize this grid to a size large enough such that the agent stayswithin the grid area for certain time steps. If the agent reaches close to the edges of the grid area, we re-initialize the gridand associated probabilities with the center of the grid as the agent’s position. The associated position probability distributionΛti is initially assumed uniform over the grid. For each grid cell, we keep track of a velocity estimate assuming a Gaussian

probability distribution. Thus, for each grid cell r ∈ Gi, we store the mean velocity vti,r and associated covariance Qti,r. The

set of these velocity estimates over the grid is denoted by V ti . At initialization, each of these mean velocities is set to zero

and the covariances are set to a high value.The function predictStep in step 4 of algorithm 1 uses the velocity distribution V t

i and a motion model to propagate theposition probability distribution. This function is described by algorithm 2 and details are given in Section III-C. If satellites arevisible to agent i, then it executes a private update step given by steps 5-8 in algorithm 1. The function storeSats processesraw GPS data to apply satellite clock and atmospheric corrections to pseudoranges, and pseudorange rates and computessatellite position and velocities from ephemerides. Hence, for each satellite s ∈ S, the set St

i stores corrected pseudorangevalue, pseudorange rate value, measurement noise covariance (using received signal-to-noise ratio value), satellite position andsatellite velocity. The function IUP is given by algorithm 3 and described in detail in [2]. A brief summary of the step is alsogiven in Section III-B. The output of this step is a single probability distribution over the grid of candidate positions Λt

i,com.In step 8, this is combined with the predicted probability distribution Λt∗

i using hypothesis integration. At the time of relativemeasurement, the agent receives a set of position probability distributions belstj from the agents being ranged to. This set ofprobabilities is characterized by their mean and covariance assuming a Gaussian distribution for simplicity. The remainder ofthe steps in the relative update step, i.e. steps 12-15 in algorithm 1, are identical to CSM and are described in detail in [31].The function kalmamBank in line 17 is used to generate the velocity distribution and is detailed in Section III-D. Finally,functions findPos and findVel are used to get a state estimate Y t+1

i and velocity estimate vt+1i with covariance Qt+1

i . Thesefunctions are detailed in Section III-E.

B. IUP framework

The IUP algorithm is a single agent snap-shot localization algorithm and combines Shadow Matching (SM) and Likelihoodbased 3DMA Ranging (LB-3DMAr). A high-level flow of the algorithm is shown in algorithm 3. Steps 1- 3 in algorithm3 correspond to SM whereas steps 1, 2, 4 - 6 correspond to LB-3DMAr. The function predictVisi predicts, for eachgrid candidate, satellites’ visibility (LOS or N-LOS) using the 3D city map and generates a probability distribution Pi,prob

for all satellites s ∈ Si. The function classifySat classifies satellites as LOS or N-LOS based on received C/N0 value.Function scoreSM finally uses this predicted and observed visibility to generate a probability distribution Λi,SM using ShadowMatching in step 3 in algorithm 3. Function computeInnovation computes for each grid candidate, measurement innovation(difference between observed and expected pseudorange) for all satellites s ∈ Si. Function remapNLOS remaps the predictedNLOS satellites for each grid candidate to a skew normal distribution. Function scoreLBr finally computes a probabilitydistribution Λi,LBr using LB-3DMAr approach. Finally the probability distributions Λi,LBr and Λi,SM are combined into asingle distribution Λi,com in step 7 in algorithm 3.

Algorithm 1 ME-CSM procedure for each agent i

Require: position probability distribution Λti, velocity distribution V t

i , Map C1: Initialization: at t = 0, choose Y (0)

i as an initial guess, Λti as uniform, V t

i with zero means and high covariances foreach cell

2: Gi ← genGrid(Y(0)i )

3: for each time step t do

4: Λt∗

i ← predictStep(Λti, V

ti , Gi)

5: if obtain satellite measurements then6: St

i ← storeSats(all s ∈ S)7: Λt

i,com ← IUP(Sti , Gi, C)

8: Λt∗

i ← hypoIntegration(Λti,com,Λ

t∗

i )9: end if

10: if obtain relative measurement rtij to agent j then11: receive from agent j: belstj12: belsti ← obtainModes(Λt∗

i )13: {Λn

i ,Λmj } ← findPair(belsti, bels

tj , r

tij)

14: Ktj ← genRotProb(Λm

j , rtij , Gi)

15: Λt∗

i ← hypoIntegration(Ktj ,Λ

t∗

i )16: end if

17: V t+1i ← kalmanBank(V t

i , Si, Gi)18: Λt+1

i ← Λt∗

i

19: Y t+1i ← findPos(Λt+1

i , Gi)20: vt+1

i , Qt+1i ← findVel(V t+1

i ,Λt+1i )

21: end for

Algorithm 2 predictStep function for agent i

Require: Λti, V

ti , Gi

1: for each grid cell r ∈ Gi do2: pti(r)← f(Gi(r), V

ti (r))

3: Λt∗

i (r)←∑

s∈GiprobCalc(Gi(s), p

ti(r)) · Λt

i(s)4: end for5: return Λt∗

i

C. Prediction Step

The prediction step in algorithm 1 is detailed in algorithm 2. This step is derived from the discretized version of the Bayesfilter [35]. In a general discrete Bayes filter with state at time t given by xt, a probability of propagation associated with themotion model P , and belief (probability of being in a certain state) bel(xt), we have the prediction step as

Algorithm 3 IUP Algorithm (adapted from [2])

Require: set of satellites’ info. Si, Map C, Grid Gi

1: Pi,pred ← predictVisi(Gi, Si, C)2: Pi,SM ← classifySat(Si)3: Λi,SM ← scoreSM(Pi,pred, Pi,SM , Gi)

4: {zi} ← computeInnovation(Si, Gi)5: {zi} ← remapNLOS(Pi,pred, {zi})6: Λi,LBr ← scoreLbr({zi}, Si, Gi)7: Λi,com ← hypoIntegration(Λi,SM ,Λi,LBr)8: return Λi,com

bel(xt) =∑xt−1

P (xt|xt−1) · bel(xt−1). (8)

Here, bel(xt) is the predicted belief of being in state xt. The summation is over all previous states xt−1 and theircorresponding beliefs bel(xt−1) as shown in algorithm 2. The function f in step 2 of algorithm 2 is dependent on themotion model of the agent such as vehicle, pedestrian etc. The function probCalc in step 3 of algorithm 2 is described using amulti-variate Gaussian distribution and is identical to the function pdf(·, ·) given below in equation (9) where Σ is the systemmodel noise and k is the dimension of the state (in this case k = 3).

pdf(x, µ) =1√

(2π)k|Σ|exp

(−1

2(x− µ)T Σ−1(x− µ)

)(9)

The Gaussian distribution above is a property of the motion model that we use for simplicity. Since the prior probabilitydistribution Λt

i is not Gaussian, the output of the prediction step Λt∗

i is not necessarily a Gaussian distribution.

D. Velocity Distribution Calculation

For each grid cell r ∈ Gi, we run an extended Kalman filter (EKF) which updates the velocity mean vti,r and covarianceQt

i,r using a pre-determined motion model (e.g. vehicle, pedestrian etc.) and the observed pseudorange rate measurementsusing the sensor model from equation (5). These EKFs together generate a set of velocity distributions over the entire gridcalled V t

i . This is done in step 17 of algorithm 1.

E. Calculating Position and Velocity Solution

At each time step, the steps 19 and 20 of algorithm 1 compute a position and velocity solution for the agent. The positionsolution is simply a weighted sum of all grid cell coordinates using the position probability distribution and is given by thefollowing equation:

Y t+1i =

∑r∈Gi

Λt+1i (r)Gi(r). (10)

Here, Gi(r) is the coordinate of a grid cell r and Λt+1i (r) is the associated position probability distribution.

The velocity solution is the weighted sum of the Gaussian velocity probabilities at each grid cell and is given by the followingequations

vt+1i =

∑r∈Gi

Λt+1i (r)vt+1

i,r (11)

Qt+1i =

∑r∈Gi

(Λt+1i (r))2Qt+1

i,r (12)

where vt+1i,r and Qt+1

i,r are the mean and covariance associated with the velocity Kalman filter corresponding to the grid cellr ∈ Gi.

(a) Skyplot of satellites visible to an agent in an open sky. (b) Area of interest for experimentation. All agents are spawned in the shadedregion.

Fig. 2: Simulation setup

(a) Open Street Map building footprint (b) LiDAR point cloud of the area of interest (c) Resultant LOD1 Map superimposed onGoogle Earth for reference.

Fig. 3: 3D Map generation

IV. SIMULATION SETUP AND RESULTS

A. Setup

We present simulation results in an urban area of Champaign, Illinois. We only simulate the received sensor data on differentagents and use a real world location as well as satellite geometry. To generate the 3D map of the area, we consider two sources:Illinois Geospatial Data Clearinghouse LiDAR data [32] and OpenStreetMap (OSM) [36]. OSM provides building footprintsand the LiDAR data provides building heights. We combine the two sources to obtain a level-of-detail 1 (LOD1) map shown inFig. 3c. We spawn agents in a 100 m × 100 m area shown in Fig. 2b at ground level. Ten agents are generated with four of themusing a vehicle motion model with constant velocity and six following randomized trajectories at low velocities. Fig. 4 showsthe position of the agents at various time instances. The network is sparsely connected and need not be a single connectednetwork at all times as shown in Fig. 5. The average connectivity of the agents is 1.69 over the time of the simulation. Agent6 is located in the alleyway (shown in Fig. 4) and observes a skewed satellite geometry. Additionally, this agent is isolatedfrom most other agents and has at most 2 neighbors at any time instant. Agents 3 and 4 move in the alleyways running N-Sand E-W respectively effectively blocking majority of the sky for these agents. Agents 1 and 2 move N-S and E-W on themain roads respectively.

In our simulation, we simulate the agent position and use real satellite positions and velocities from a time of day. Therefore,we know with certainty if an agent is LOS using the 3D city map. We use this ground truth and distort it with measurementmodels to simulate real sensor data. We obtain satellite pseudoranges using models described in equations (1), (2), and (4),

(a) Agent positions at t = 0 s (b) Agent positions at t = 2.5 s

(c) Agent positions at t = 5 s (d) Agent positions at t = 10 s

Fig. 4: Simulation setup shows the motion of agents. The blue dots indicate the agent’s latest position. The numbers in thebubbles indicate agent number. Agents 1 - 4 are moving in straight lines whereas the other agents are executing random motionlocally.

and inter-agent ranging using equation (6). The noise for LOS satellites and inter-agent ranging is modeled as an additiveGaussian with zero mean. The standard deviation in the noise for LOS satellites is assumed to be a constant value of 10 m.The inter-agent ranging measurements are assumed to be more accurate with a standard deviation of 1 m. For NLOS satellites,the noise is modeled as a skew normal distribution with the choice of parameters same as in [2]. To obtain the C/N0 values,we assign values above a parameter C/N0high to LOS satellites and between C/N0low and C/N0high for N-LOS satellites.The parameters for the noise terms, C/N0high, and C/N0low are empirically determined to reflect a user-grade receiver. Forexample, to reflect a UBlox LEA-6T receiver with a patch antenna, we set C/N0high = 45 dB-Hz and C/N0low = 20 dB-Hz.Additionally, to obtain the pseudorange rate measurements, we use the model described in 5. The noise for the pseudorangerate measurements is modeled as an additive Gaussian with zero mean and standard deviation 0.5 m/s .

We run the simulation for a total time of 10 s with 0.1 s time steps. Agents are allowed to communicate and range to onlytheir line-of-sight neighbors that are at-least 7 m away and within 50 m distance of them.

We compare the following approaches on our dataset:• Localization using Shadow Matching (SM) [29]• Localization using Intelligent Urban Positioning (IUP) [2]• Snapshot Collaborative Shadow Matching (CSM) [31]• Multi-epoch Collaborative Shadow Matching (ME-CSM)

And we report the results on horizontal error (in ENU frame) in localization from their true positions by the various methods.Note that SM and IUP methods do not take into account the inter-agent ranging measurements and rely solely on GPSmeasurements. In these methods, we assume each agent is independent of the others.

Fig. 5: The blue points are the agent positions and the links indicate a ranging and communication link between the agents.We can see that the network is sparsely connected and is not connected as a single graph.

TABLE I: RMS Error in Horizontal Positioning for Agents

RMS error in estimation (in m)ME-CSM CSM IUP SM

Agent 1 3.1 4.7 5.5 19.7Agent 2 3.0 5.5 7.9 24.0Agent 3 6.6 9.7 12.6 20.9Agent 4 2.9 4.3 7.2 17.3Agent 5 1.3 2.9 3.3 17.2Agent 6 6.6 12.1 9.4 25.8Agent 7 2.0 4.6 7.2 18.4Agent 8 3.7 8.2 9.0 14.8Agent 9 1.5 6.0 5.4 9.5

Agent 10 1.8 3.2 18.4 41.7

B. Results

Fig. 6 shows the plot of error in estimation from true position over time for various methods. Table I lists the RMS errorin position averaged over time for the agents using the aforementioned methods. We can see from Fig. 6 that both CSM andME-CSM have a lower positioning error than SM and IUP. This is expected since an additional ranging measurement shouldimprove localization capabilities. Fig. 7 shows the estimated velocity for agent 1 in ECEF x-direction with the 3− σ bounds.Although the velocity tracking is not accurate, the true velocity is always within the 3− σ bounds. This is sufficient to helpus obtain lower RMS error in position than the snapshot CSM algorithm. These results validate that the multi-epoch algorithmoutperforms the other state-of-the-art 3DMA GNSS localization algorithms in providing more accurate position estimate forall agents. Fig. 8 shows a qualitative comparison of the probability distribution for the various methods. In figure 9, we seethe plot of neighbor connectivity with time and localization error with time for the agents. In particular, if we look at agent3, then we see that the localization error decreases at the 7 second mark when the connectivity increases. This occurs becauseagent 3 is located in a challenging environment as it passes through the N-S alleyway (see fig. 4c) and additional sensing helpsit here. On the other hand, if we look at agent 6 in fig. 9, then we observe that at the 5 second mark, when the connectivityincreases, the error also increases. This is due to the error in the localization solution of the new neighbor (agent 3) in thechallenging environment. This indicates the possibility of contamination of localization accuracy by faulty neighbors in ouralgorithm.

V. CONCLUSION

In this paper, we built upon the snapshot CSM algorithm and developed a multi-epoch variant. We formulated the methodologybased on Bayesian filtering to associate temporal correlations between agent states as well as incorporated pseudorange ratemeasurements in our algorithm. The algorithm outperformed other 3DMA GNSS approaches and effectively reduced error inthe solution. We demonstrated this algorithm on a simulated dataset based on a real world location in Champaign, Illinois,with an asynchronous, sparsely connected network.

Fig. 6: Comparison of horizontal estimation error from true position between IUP, CSM and ME-CSM over time for eachagent. ME-CSM has lower errors on average across agents.

Fig. 7: Plot of ECEF x-axis component of estimated velocity with 3σ bounds and the true velocity.

(a) IUP probability distribution (b) CSM probability distribution (c) ME-CSM probability distribution

Fig. 8: Probability distributions obtained at an interim time for agent 8 for qualitative analysis. The inverted yellow triangle isthe true agent position. We can see that the spread of the distribution is least for the proposed ME-CSM algorithm, followedby CSM and IUP.

ACKNOWLEDGEMENTS

This material is based upon work supported by the National Science Foundation under award number 1750864. The authorswould like to sincerely thank Mr. Matt Peretic, Mr. Shubhendra Chauhan, Ms. Tara Mina, and Ms. Sriramya Bhamidipati fortheir assistance in data collection.

Fig. 9: Plot of connectivity (in blue) with time and error (in red) with time for the agents.

REFERENCES

[1] P. Groves and M. Adjrad, “Multi-epoch 3D mapping aided GNSS using a grid filter.” Institute of Navigation (ION), 2018.[2] M. Adjrad and P. D. Groves, “Intelligent urban positioning using shadow matching and GNSS ranging aided by 3D mapping,” in Proceedings of the

ION GNSS, 2016, pp. 534–553.[3] A. Howard, M. J. Matark, and G. S. Sukhatme, “Localization for mobile robot teams using maximum likelihood estimation,” in Intelligent Robots and

Systems, 2002. IEEE/RSJ International Conference on, vol. 1. IEEE, 2002, pp. 434–439.[4] B. E. Nemsick, A. D. Buchan, A. Nagabandi, R. S. Fearing, and A. Zakhor, “Cooperative inchworm localization with a low cost team,” in Robotics and

Automation (ICRA), 2017 IEEE International Conference on. IEEE, 2017, pp. 6323–6330.[5] A. I. Mourikis and S. I. Roumeliotis, “Optimal sensor scheduling for resource-constrained localization of mobile robot formations,” IEEE Transactions

on Robotics, vol. 22, no. 5, pp. 917–931, 2006.[6] P. Schmuck, “Multi-UAV collaborative monocular SLAM,” in Robotics and Automation (ICRA), 2017 IEEE International Conference on. IEEE, 2017,

pp. 3863–3870.[7] A. Howard, M. J. Mataric, and G. S. Sukhatme, “Putting the’I’in’team’: An ego-centric approach to cooperative localization,” in Robotics and Automation,

2003. Proceedings. ICRA’03. IEEE International Conference on, vol. 1. IEEE, 2003, pp. 868–874.[8] A. Martinelli, F. Pont, and R. Siegwart, “Multi-robot localization using relative observations,” in Robotics and Automation, 2005. ICRA 2005. Proceedings

of the 2005 IEEE International Conference on. IEEE, 2005, pp. 2797–2802.[9] F. R. Fabresse, F. Caballero, and A. Ollero, “Decentralized simultaneous localization and mapping for multiple aerial vehicles using range-only sensors,”

in Robotics and Automation (ICRA), 2015 IEEE International Conference on. IEEE, 2015, pp. 6408–6414.[10] E. D. Nerurkar, S. I. Roumeliotis, and A. Martinelli, “Distributed maximum a posteriori estimation for multi-robot cooperative localization,” in Robotics

and Automation, 2009. ICRA’09. IEEE International Conference on. IEEE, 2009, pp. 1402–1409.[11] T. R. Wanasinghe, G. K. Mann, and R. G. Gosine, “Distributed leader-assistive localization method for a heterogeneous multirobotic system,” IEEE

Transactions on Automation Science and Engineering, vol. 12, no. 3, pp. 795–809, 2015.[12] S. I. Roumeliotis and G. A. Bekey, “Distributed multirobot localization,” IEEE Transactions on Robotics and Automation, vol. 18, no. 5, pp. 781–795,

2002.[13] L. Luft, T. Schubert, S. I. Roumeliotis, and W. Burgard, “Recursive decentralized collaborative localization for sparsely communicating robots.” in

Robotics: Science and Systems, 2016.[14] S. Tanwar and G. X. Gao, “Decentralized collaborative localization with deep GPS coupling for UAVs,” in Position, Location and Navigation Symposium

(PLANS), 2018 IEEE/ION. IEEE, 2018, pp. 767–774.[15] A. Prorok, A. Bahr, and A. Martinoli, “Low-cost collaborative localization for large-scale multi-robot systems,” in Robotics and Automation (ICRA),

2012 IEEE International Conference on. IEEE, 2012, pp. 4236–4241.[16] B. Ramirez, H. Chung, H. Derhamy, J. Eliasson, and J. C. Barca, “Relative localization with computer vision and UWB range for flying robot formation

control,” in Control, Automation, Robotics and Vision (ICARCV), 2016 14th International Conference on. IEEE, 2016, pp. 1–6.[17] J. Amt and J. Raquet, “Positioning for range-based land navigation systems using surface topography,” in Proceedings of ION GNSS, 2006, pp. 1494–1505.[18] T. Suzuki and N. Kubo, “Correcting GNSS multipath errors using a 3D surface model and particle filter,” Proc. ION GNSS+ 2013, 2013.[19] R. Kumar and M. G. Petovello, “A novel GNSS positioning technique for improved accuracy in urban canyon scenarios using 3D city model,” in

Proceedings of the 27th International Technical Meeting of the Satellite Division of the Institute of Navigation (ION GNSS+ 2014), Tampa, FL, USA,vol. 812, 2014, p. 21392148.

[20] P. D. Groves and M. Adjrad, “Likelihood-based GNSS positioning using LOS/NLOS predictions from 3D mapping and pseudoranges,” GPS Solutions,vol. 21, no. 4, pp. 1805–1816, 2017.

[21] M. Adjrad and P. D. Groves, “Enhancing least squares GNSS positioning with 3D mapping without accurate prior knowledge,” Navigation: Journal ofThe Institute of Navigation, vol. 64, no. 1, pp. 75–91, 2017.

[22] P. D. Groves, “Shadow matching: A new GNSS positioning technique for urban canyons,” The Journal of Navigation, vol. 64, no. 3, pp. 417–430, 2011.[23] L. Wang, “Investigation of shadow matching for GNSS positioning in urban canyons,” Ph.D. dissertation, UCL (University College London), 2015.[24] L.-T. Hsu, Y. Gu, and S. Kamijo, “3D building model-based pedestrian positioning method using GPS/GLONASS/QZSS and its reliability calculation,”

GPS solutions, vol. 20, no. 3, pp. 413–428, 2016.[25] R. Yozevitch and B. B. Moshe, “A robust shadow matching algorithm for gnss positioning,” Navigation: Journal of The Institute of Navigation, vol. 62,

no. 2, pp. 95–109, 2015.[26] A. Bourdeau, M. Sahmoudi, and J. Tourneret, “Tight integration of GNSS and a 3D city model for robust positioning in urban canyons,” in ION GNSS,

2012, pp. 1263–1269.[27] M. Obst, S. Bauer, and G. Wanielik, “Urban multipath detection and mitigation with dynamic 3D maps for reliable land vehicle localization,” in Position

Location and Navigation Symposium (PLANS), 2012 IEEE/ION. IEEE, 2012, pp. 685–691.[28] S. Peyraud, D. Betaille, S. Renault, M. Ortiz, F. Mougel, D. Meizel, and F. Peyret, “About non-line-of-sight satellite detection and exclusion in a 3D

map-aided localization algorithm,” Sensors, vol. 13, no. 1, pp. 829–847, 2013.[29] P. D. Groves, L. Wang, M. Adjrad, and C. Ellul, “GNSS shadow matching: The challenges ahead,” in The Institute of Navigation, 2015.[30] T. Suzuki, “Integration of GNSS positioning and 3D map using particle filter,” Proceedings of the ION GNSS+ Sep 12-16, 2016.[31] S. Tanwar and G. X. Gao, “Decentralized collaborative localization in urban environments using 3D-Mapping-Aided (3DMA) GNSS and inter-agent

ranging,” Proc. ION GNSS+ 2018, 2018.[32] Illinois Height Modernization Program, Illinois State Geological Survey, and Illinois Department of Transportation. (2002-2013)

Illinois LiDAR county database: Illinois State Geological Survey. [Online]. Available: https://clearinghouse.isgs.illinois.edu/data/elevation/illinois-height-modernization-ilhmp-lidar-data

[33] H. Kuusniemi, A. Wieser, G. Lachapelle, and J. Takala, “User-level reliability monitoring in urban personal satellite-navigation,” IEEE Transactions onAerospace and Electronic Systems, vol. 43, no. 4, 2007.

[34] A. Azzalini and A. D. Valle, “The multivariate skew-normal distribution,” Biometrika, vol. 83, no. 4, pp. 715–726, 1996.[35] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics. MIT Press, 2005.[36] M. Haklay and P. Weber, “Openstreetmap: User-generated street maps,” IEEE Pervas Comput, vol. 7, no. 4, pp. 12–18, 2008.