Set-theoretic Localization for Mobile Robots with Infrastructure-basedSensing

Xiao Li, Yutong Li, Anouck Girard, Ilya Kolmanovsky

Abstract— In this paper, we introduce a set-theoretic ap-proach for mobile robot localization with infrastructure-basedsensing. The proposed method computes sets that over-boundthe robot body and orientation under an assumption of knownnoise bounds on the sensor and robot motion model. Weestablish theoretical properties and computational approachesfor this set-theoretic localization approach and illustrate itsapplication to an automated valet parking example in simu-lations and to omnidirectional robot localization problems inreal-world experiments. We demonstrate that the set-theoreticlocalization method can perform robustly against uncertaintyset initialization and sensor noises compared to the FastSLAM.

I. INTRODUCTION

One of the major challenges for navigating mobile robotssafely is in their accurate and reliable localization [1]. Apromising approach is to leverage the infrastructure-basedsensing and wireless communications/V2X [2]. With theadvances in computing power, real-time simultaneous local-ization and mapping (SLAM) has become more widespreadfor robot localization tasks in unmapped environments [3].In particular, with prior knowledge of the surroundings, theinfrastructure-based SLAM is an appealing centralized local-ization approach as it reduces computational burden by treat-ing individual agent’s localization tasks independently [4].However, quantification of the localization uncertainties isgenerally handled by estimations of confidence intervalsor ellipsoids within those based on probabilistic methods,e.g. Bayes filters, particle filters [1]. Explicit uncertaintybounds, which are crucial in safety-critical systems, arerarely considered in probabilistic methods.

Consider a centralized closed-circuit television (CCTV)system which collects measurements associated with therobot in Fig. 1. Suppose we use the FastSLAM to estimatethe 2D robot body area. The FastSLAM is based on particlefiltering. As shown in Fig. 1, localization results with fewerparticles tend to underestimate the robot body. If we furtheruse the estimated robot body in a planning module [5],an estimated area which fails to contain the entire robotbody may eventually cause a collision. Though the estimatedareas with 500 and 1000 particles over-bound the robotbody, a larger number of particles increases sampling andcomputation burden and impedes online deployment. In fact,the estimated area is guaranteed to contain the robot body ifand only if we sample an infinite number of particles.

The authors are with the University of Michigan, Ann Arbor, MI 48109,USA. hsiaoli, yutli, anouck, ilya@umich.edu. The last author acknowledgessupport by the National Science Foundation under Award ECCS-1931738.

Fig. 1: FastSLAM estimated robot body.

This motivates us to adopt the set-theoretic approach toperform mobile robot localization, which is inspired by [6].Such an approach relies on the assumption of bounded uncer-tainties and guarantees that the nominal state is necessarilywithin the estimated uncertainty set. The proposed methodfirst embeds the uncertainty bounds on estimated states andmeasurements into representations of intervals and convexpolytopes. Then, the set-valued motion model propagationand measurements are combined to update set-valued esti-mates of 2D robot body area and orientation. We use theautomated valet parking in which the use of infrastructure-based sensing is often assumed [7], [8], [9] as an exampleto validate the effectiveness of the proposed method insimulations and to compare the results with FastSLAM. Wealso report the results from real-world experiments whichdeployed an omnidirectional robot and a Lidar-based sensingsystem.

The main contributions of this paper are as follows: (1) Weextend the theoretical results of the set-theoretic localizationmethod to an infrastructure-based sensing setting. (2) Weshow that the proposed method is less sensitive to uncertaintyset initialization and sensor noises compared to FastSLAM.(3) We use polytopes to approximate the uncertainty setswhich reduces conservatism as compared to boxes and isstill computationally efficient due to low dimensional char-acteristics of the problem.

II. RELATED WORK

Estimation problems [10] associated with localization inrobotics have been extensively studied. Probabilistic methodssuch as the classical Kalman filter, particle filter, Bayesianfilter, and unscented Kalman filter have been considered forthe robotics localization problems [1], [10], [11]. Barrauet al. [12] design an invariant Kalman filter based on theestimation error being invariant under actions of a matrix

arX

iv:2

110.

0174

9v1

[cs

.RO

] 4

Oct

202

1

Lie group. For SLAM problems, new methods, for example,extended Kalman filter (EKF) SLAM [13], [14] and Fast-SLAM [15] have been developed. With the advances in com-puting power, matrix and graph optimization algorithms [16]such as iSAM [17] and GTSAM [18] have become feasiblefor implementation. SLAM algorithms which rely on visualsensors e.g. monocular, stereo, RGB-D cameras have beenproposed in [19], [20], [21], [22]. With the development ofmachine learning, significant progress in outdoor visual placerecognition and localization has been achieved [23], [24],[25], [26].

In contrast to the above probabilistic and optimizationmethods, set-theoretic methods enable a deterministic estima-tion of uncertainty bounds for robotics localization, mappingand system state estimation problems [27], [28], [29], [6],[30], [31], [32]. Inspired by [6], we propose a set-theoreticlocalization algorithm which use infrastructure-based sensorsfor mobile robots localization. We also consider applicationsof this algorithm to an autonomous valet parking example.We use polytopes to over-bound the uncertainty sets whichis different from [6] where boxes are used and results in lessconservative estimates.

III. PROBLEM FORMULATION

Fig. 2: Schematic diagram of an infrastructure-based local-ization system.

As shown in Fig. 2, suppose the localization systemconsists of m infrastructure-based sensors in X − Y planeΩ ⊂ R2 that are assumed to be cameras. The ith camera’snominal state is li = [li,x li,y li,θ]

T and i = 1, . . . ,m wherethe nominal position li,xy = [li,x li,y]T ∈ Ω and the nominalorientation li,θ ∈ [−π, π]. A mobile robot with orientationpθ ∈ [−π, π], covered by a rectangle Pxy ⊂ Ω of width cwand length cl, travels in the X − Y plane. We assume thatthere are n detectable markers attached to the robot (in Fig. 2,n = 4) such that the robot body Pxy is in the convex hull ofthe markers. We denote the ith marker’s nominal position bypi = [pi,x pi,y]T ∈ Ω. Note that in our problem setting, aslong as the infrastructure-based sensor can measure the rangeor bearing of markers, there is no restriction on the sensor’stype, i.e. it can be a monocular camera, Lidar, et al. We alsonote that the marker can be virtual, such as ORB [33] orSIFT [34] features with object detection [35] such as vehiclewheel detection [36], [37] to refine the region of interest.

We use the following model to represent the markerposition dynamics

pi(k + 1) = pi(k) + w(k) (1)

where the disturbance w(k) ∈ R2 is unknown-but-boundedas ‖w(k)‖∞ ≤ εw and εw > 0 is a known bound. For eachtime step k, we are able to collect measurements Mli(k) =αi,j(k)

αi,j(k) = atan2(pj,y− li,y, pj,x− li,x)− li,θ+va(k)i,j (2)

where αi,j(k) represents the angle measurement of jth

marker by the ith camera with bounded additive noiseva(k)i,j i.e. |va(k)i,j | ≤ εva where εva > 0 is a knownbound. Additionally, we assume each camera has a max-imum detection range rmax such that marker satisfying∥∥∥pj − li,xy∥∥∥

2≤ rmax is detectable by ith camera.

Fig. 3: Illustrations of the set-theoretic localization problem.

At previous time step k, we assume the nominal statesli(k)i=1,...,m, pi(k)i=1,...,n are within the correspond-ing known and compact uncertainty sets Li(k)i=1,...,m,Pi(k)i=1,...,n where Li(k) ⊂ Ω × [−π, π], Pi(k) ⊂ Ω.As shown in Fig. 3, the ith uncertainty set Pi(k) bounds allthe possible positions of the ith marker in the X − Y planesuch that pi(k) ∈ Pi(k). Suppose we collect measurementsMli(k + 1)i=1,...,m at current time step k + 1. The goalsof the set-theoretic localization method are as follows:

1) Propagate and update Li(k + 1)i=1,...,m, Pi(k +1)i=1,...,n based on (1) and (2) such that li(k + 1) ∈Li(k+ 1) for i = 1, . . . ,m and pi(k+ 1) ∈ Pi(k+ 1)for i = 1, . . . , n.

2) Estimate two sets Pxy(k + 1) ⊂ Ω and Pθ(k + 1) ⊂[−π, π] based on uncertainty sets Li(k+1)i=1,...,m,Pi(k + 1)i=1,...,n while guarantees the robot bodyPxy(k+ 1) ⊂ Pxy(k+ 1) and orientation pθ(k+ 1) ∈Pθ(k + 1).

IV. SET-THEORETIC LOCALIZATION

In this section, we first use the robot motion model topropagate the uncertainty sets in Sec. IV-A, then updatethe propagated sets with measurements derived from theinfrastructure-based sensors in Sec. IV-B. By incorporatingthe geometrical constraints between individual markers, wecan improve the accuracy in estimation of the robot bodyand orientation in IV-C. An extension of the proposedmethod to the case with stereo camera-based infrastructuresensing systems is introduced in Sec. IV-D. We conclude thesection with an over-approximation strategy to simplify theset operations in Sec. IV-E.

A. Motion Propagation

Based on (1) and prior to incorporating measurementinformation, the uncertainty sets Pi of the ith marker andLi of the ith camera are updated as

Pi(k + 1|k) = Pi(k)⊕ B∞(εw), (3)

Li(k + 1|k) = Li(k), (4)

where ⊕ denotes the Minkowski sum and B∞(εw) is an∞-norm ball of radius εw. For camera uncertainty set prop-agation, to simplify the computations, we decompose Li intotwo bounded sets Li,xy ⊂ Ω and Li,θ ⊂ [−π, π] such thatLi ⊂ Li,xy ×Li,θ. This decomposition simplifies the updateprocedure in Sec. IV-B. This way, (4) is rewritten as

Li,xy(k + 1|k) = Li,xy(k),

Li,θ(k + 1|k) = Li,θ(k).(5)

B. Measurement Update

Given measurements Mli(k+ 1) from the ith camera, wefirst update the uncertainty set Li,θ, and then we sequentiallyupdate Li,xy and Pj . For each individual measurement αi,j ∈Mli(k+1), since Li,xy(k+1|k) and Pj(k+1|k) are bounded,we can compute an interval bound

atan2(pj,y − li,y, pj,x − li,x) ∈ [βmin, βmax], (6)

where

βmin = minli,xy∈Li,xy(k+1|k),pj∈Pj(k+1|k)

(atan2(pj,y − li,y, pj,x − li,x)) ,

βmax = maxli,xy∈Li,xy(k+1|k),pj∈Pj(k+1|k)

(atan2(pj,y − li,y, pj,x − li,x)) .

From (2) and (6), the camera nominal orientation li,θ ∈[ψi,j , φi,j ] where

ψi,j = βmin − αi,j − εva ,φi,j = βmax − αi,j + εva .

Given a measurement set Mli(k + 1), the camera nominalorientation li,θ must be confined to the set

Li,θ(k+ 1) = Li,θ(k+ 1|k)⋂ ⋂

αi,j∈Mli

[ψi,j , φi,j ]

. (7)

With this updated Li,θ(k + 1), we are able to derive atighter bound on Li,xy and Pj . We first define a set

LM (pj , αi,j) :=

l ∈ R2| |αi,j − atan2(−ly,−lx) + θc| ≤ εva + δθc(8)

where li,θ is in the bounded interval Li,θ(k + 1) ⊂ [θc −δθc, θc + δθc]. In the coordinate frame with pj as the origin,the set LM (pj , αi,j) denotes a feasible region for li for whichmeasurement αi,j is plausible. Back to the global frame, the

ith camera nominal position is in Pj(k+1|k)⊕LM (pj , αi,j).Given measurements Mli , li,xy is necessarily within

Li,xy(k + 1) = Li,xy(k + 1|k)⋂ ⋂

αi,j∈Mli

(Pj(k + 1|k)⊕ LM (pj , αi,j))

.(9)

Analogously to (8), in a reference frame centered at theith camera, the jth marker position is inferred to belong theset

PM (li, αi,j) :=

p ∈ R2| |αi,j − atan2(py, px) + θc| ≤ εva + δθc.(10)

Considering all measurements that are corresponding to thejth marker, pj must belong to the set,

Pj(k + 1) = Pj(k + 1|k)⋂

⋂i=1,...,m,

αi,j′∈Mli, j′=j

((Li,xy(k + 1)⊕ PM (li, αi,j′))) (11)

C. Robot Body and Orientation Estimation

Before estimating the robot body and orientation, we canexploit a rigid body constraint for two arbitrary markerspi, pj ∈ pii=1,...,n of the form ‖pi − pj‖2 = rij and refinethe uncertainty sets of the ith and jth marker positions via

Pi(k + 1) = Pi(k + 1) ∩ (Pj ⊕ B2(rij)) ,

Pj(k + 1) = Pj(k + 1) ∩ (Pi ⊕ B2(rij)) ,(12)

where B2(rij) represents a `2-norm ball of radius rij .

Fig. 4: Robot body estimation using convex envelope ofmarker uncertainty sets.

Eventually, after the set propagation by (3), (5), update viameasurements by (7), (9), (11) and set refinement by rigidbody constraint above, the entire robot body is over-boundedby a convex envelope Pxy(k + 1) as shown in Fig. 4 whichis the convex hull of all vertices of Pi(k + 1), i = 1, . . . , ni.e.

Pxy(k + 1) := convHull(Pi(k + 1)i=1,...,n), (13)

where convHull() is the command in CORA [38], [39],[40] that calculates convex envelope of the input sets.

Meanwhile, the vector from pi to pj forms an offset angle∆θij with the robot nominal orientation pθ. Consequently,the robot nominal orientation is within

Pθ =⋂

i,j=1,...,n, i 6=j

[βij−∆θij , βij −∆θij ] (14)

where atan2(pj,y − pi,y, pj,x − pi,x) ∈ [βij, βij ].

D. Extension to the Stereo Camera Setting

If stereo cameras are installed in place of the monocularones, each angle measurement in (2) is coupled with a rangemeasurement,

ri,j(k) =

√(pj,y − li,y)2 + (pj,x − li,x)2 + vr(k)i,j . (15)

where the unknown additive noise satisfies |vr(k)i,j | ≤ εvr

and εvr > 0 is a known bound.The measurement update process of Li,xy and Pj follows

Sec. IV-B where LM in (8) and PM in (10) are redefined as

LM (pj , αi,j , ri,j) :=l ∈ R2

∣∣∣∣ |αi,j − atan2(−ly,−lx) + θc| ≤ εva + δθc∣∣∣ri,j −√l2x + l2y

∣∣∣ ≤ εvr

,

PM (li, αi,j , ri,j) :=p ∈ R2

∣∣∣∣ |αi,j − atan2(py, px) + θc| ≤ εva + δθc,∣∣∣ri,j −√p2x + p2y

∣∣∣ ≤ εvr

.

The proposed method guarantees the following property (theproof is available in Appendix):

Proposition 1. Assume li,xy(0) ∈ Li,xy(0), li,θ(0) ∈Li,θ(0), pj(0) ∈ Pj(0) and i = 1, . . . ,m, j = 1, . . . , n, i.e.the initial nominal states are within the initial uncertaintysets. Then, based on the set-theoretic method in (3), (5), (7),(9), (11), (12), (13) and (14), the robot body Pxy(k) andorientation pθ(k) are confined to the estimated uncertaintysets Pxy(k), Pθ(k) for all k > 0, i.e. ∀k > 0, Pxy(k) ⊂Pxy(k), pθ(k) ∈ Pθ(k).

E. Uncertainty Set Approximation

With the problem formulation in Sec. III, the orientationand position uncertainty sets are initialized as intervals andrectangles respectively, which can be arbitrarily approxi-mated by convex polytopes. This way, set operations in theproposed localization method can be simplified. As shownin Fig. 5, LM in (8) and PM in (10) can be represented bytwo circular sectors in the monocular camera and two annularsectors in the stereo camera. We use convex polygons to over-bound LM and PM where θ1,2 = (αi,j+θc)±(εva +δθc) andr1,2 = ri,j ± εvr . The benefit of this over-approximation isthat the set of convex polytopes is closed under Minkowskisum and intersection.

Fig. 5: Illustrations of set approximations by convex poly-gons.

V. SIMULATION AND EXPERIMENTAL RESULTS

In this section, we first apply the proposed set-theoreticlocalization method to an automated valet parking example(Fig. 6). The simulation results of the proposed methodversus the FastSLAM in [15] are compared. The real-worldexperimental results with an omnidirectional robot and Lidar-based infrastructure sensing system are presented in Sec. V-B. The code and demonstration videos are available inSetThmSLAM GitHub.

Fig. 6: Simulation results of the vehicle tracking a referencetrajectory in a parking space.

A. Simulation Results

As illustrated in Fig. 6, a simulated parking space equippedwith 21 cameras is built based on the Automated ParkingValet toolbox in MATLAB. Each camera has 70 field ofview and 20 m measurement range. A vehicle of length cl =4 m and width cw = 1.8 m is navigating within the parkingspace to track a reference trajectory. The proposed methodis applied to localize the vehicle body and orientation andthe results are compared with the ones using the FastSLAMin both monocular and stereo camera settings.

We use CORA [38], [39], [40] in MATLAB to conductset operations between polytopes, e.g. Minkowski sum, in-tersections of polytopes, etc. The FastSLAM [15] is initial-ized with 100 particles. Each particles stores camera andmarker states (i.e. li,xy, li,θi=1,...,m, pii=1,...,n) whichare sampled from their corresponding uncertainty sets (i.e.Li,xy, Li,θi=1,...,m, Pii=1,...,n). In the FastSLAM, weestimate Pxy using the command enclosePoints() inCORA to compute a convex polygon that encloses all markerpositions stored in the particles. We set the disturbance boundεw = 2.5 m and the sampling time Ts = 0.5 s.

From the snapshot in Fig. 6, the estimated uncertaintyset using the FastSLAM (green solid line) sometime failsto contain the entire vehicle body. In contrast, the proposedmethod guarantees that the vehicle body is always containedwithin the estimated set (blue solid line), which validatesProposition 1.

The results using the proposed method and the FastSLAMare compared in Fig. 7 and 8. As shown in Figs. 7a-band Figs. 8a-b, compared with the results using FastSLAM(purple line and shaded area), the volume ratio V (Pxy)

cw·cl of the

(a) (b)

(c) (d)

(e) (f)

(g) (h)

(i)

Fig. 7: Comparisons of vehicle body estimations betweenthe proposed method and the FastSLAM in terms of therobustness against uncertainty set initialization and sensornoises.

estimated uncertainty set Pxy to the vehicle body does notgrow with the increase of V (Pi(k = 0)) which is the initialvolume of Pi. Compared with the FastSLAM, the proposedmethod performs robustly against changes of V (Pi(k = 0)).The reason is that with limited numbers of particles, higheruncertainty in the marker’s initial position disadvantages theFastSLAM in estimating the probabilistic distributions ofthe markers’ positions, thus deteriorating its performancesignificantly. In contrast, within the proposed method, thevolume of Pxy largely depends on PM through (11). PM iscomputed based on the sensor noise bounds εva , εvr and theestimated uncertainty bound δθc which makes the proposedmethod more robust against the changes of V (Pi(k = 0))compared to the FastSLAM.

As shown in Figs. 7c-f and Figs. 8c-f, the proposed methodis more sensitive to the changes of V (Li,θ(k = 0)) andV (Li,xy(k = 0)), which are the initial volumes of Li,θ andLi,xy , compared to the FastSLAM. It is reasonable as againin (10) and (11), the estimated uncertainty bound δθc, whichlargely depends on V (Li,θ(k = 0)) and V (Li,xy(k = 0)),directly affects the volume of Pxy and leads to degradedperformance. The investigation into the method for furtherimproving the robustness against V (Li,θ(k = 0)) andV (Li,xy(k = 0)) is left to future work.

Finally, the comparisons of the proposed method and theFastSLAM in terms of the robustness against sensor noises

(a) (b)

(c) (d)

(e) (f)

(g) (h)

(i)

Fig. 8: Comparisons of vehicle orientation estimations be-tween the proposed method and the FastSLAM in termsof the robustness against uncertainty set initialization andsensor noises.

are presented in Figs. 7g-i and 8g-i. Although the meanvalues using the proposed method and the FastSLAM areclose, smaller standard deviations are achieved by deployingour proposed method. This is due to the fact that ourmethod conducts the set estimation in a deterministic way. Incontrast, at every time step in the FastSLAM, the re-samplingof particles increases the randomness of the estimated setvolume, which leads to higher standard deviations.

B. Experimental Results

To further validate the proposed method, we conduct real-world experiments. We use Lidar as the infrastructure-basedsensor to acquire both range and bearing measurementsfor the localization of an omnidirectional robot. As shownin Fig. 9, the localization system consists of three Lidars(RPLidar A1). We attach a vertical bar to the center of therobot and use it as the Lidar detection marker. Meanwhile,we attach three visual detection markers to the robot so thatwe can obtain the ground-true position of the robot centerfrom the OptiTrack motion capture system.

We first calibrate the range and bearing measurement noisebounds as εvr = 0.073 m and εva = 8.05, respectively.With a robot trajectory that covers the majority area of thetest field, we calibrate the noise bounds as the maximumerrors between measurements from OptiTrack and the onesfrom Lidars. The robot navigates in the test field with a

Fig. 9: Omnidirectional robot localization using lidar mea-surements: (Left) Photo of the test field with three RPLidars(infrastructure based sensors), and an omnidirectional robotattached with detection markers. (Right) Test results usingthe proposed method.

maximum speed of 0.10 m/s. As shown in Fig. 9, theproposed set theoretic localization method guarantees theestimated uncertainty set (green line) always contains therobot body (circle with a radius of 0.12 m), the test videocan be found in here. This result demonstrates the possibilityfor real-world implementation of the proposed method.

VI. CONCLUSION

In this paper, a set-theoretic localization algorithm whichexploits infrastructure-based sensing has been proposed. Thetheoretical properties and computational approaches for thisset-theoretic localization method have been established. Ro-bustness of the proposed method to uncertainty set initializa-tion and sensor noise has been demonstrated. Future workwill focus on addressing the latency introduced by matchingmeasurements and detected markers, and integration withdownstream planning and control modules to enforce safety-related constraints.

APPENDIX

Proof of Proposition 1:

We assume the following condition

li,xy(k) ∈ Li,xy(k), li,θ(k) ∈ Li,θ(k), pj(k) ∈ Pj(k),

i = 1, . . . ,m, j = 1, . . . , n,(16)

holds for k = 0 during initialization. Through the propaga-tion and update of uncertainty sets, if the condition in (16)being true for k+1 can be induced from same condition beingsatisfied at time step k, the nominal states li,xy, li,θ, pjstay in the uncertainty sets Li,xy, Li,θ, Pj respectively byprinciple of induction. Afterward, it’s convenient to verifythat the robot body and orientation are contained in Pxy, Pθ.For simplification, we provide the proof for the monocularcamera that the stereo case resembles.

A. Motion Propagation

First, we examine the propagation process in (3), (5). Con-sidering pj(k) ∈ Pj(k) and w(k) ∈ B∞(εw), the nominalstate pj(k+ 1|k) ∈ Pj(k+ 1|k) is guaranteed by Minkowskisum in (3). Obviously, li,xy(k + 1|k) ∈ Li,xy(k + 1|k) andli,θ(k + 1|k) ∈ Li,θ(k + 1|k) follow from (16) given (5).

B. Measurement Update

1) Update of Li,θ: By (2) and (6), li,θ(k+1) ∈ [ψi,j , φi,j ]for each measurement αi,j . Indeed, considering all the mea-surements in Mli , the camera nominal orientation is confinedto the set intersection over all possible intervals as in (7).

2) Update of Li,xy: Given measurement αi,j and in areference frame centered at pj , the coordinates of the ith

camera [l′x l′y]T = li,xy(k + 1|k)− pj(k + 1|k) satisfy

atan2(−l′y,−l′x) = αi,j + li,θ(k + 1|k)− va(k + 1)i,j

by (2). Since the noise |va(k + 1)i,j | ≤ εva and li,θ(k +1|k) ∈ Li,θ(k+ 1|k) ⊂ [θc − δθc, θc + δθc], it can be shownthat

li,xy(k + 1|k)− pj(k + 1|k) ∈ LM (pj , αi,j)

from (8). Given pj(k+1|k) ∈ Pj(k+1|k) from propagationand equation above, the camera nominal position satisfies

li(k + 1|k) = pj(k + 1|k) +(li,xy(k + 1|k)− pj(k + 1|k)

)∈ Pj(k + 1|k)⊕ LM (pj , αi,j).

Considering all the measurements, the camera nominal po-sition is within the set intersection over all possible Pj(k +1|k)⊕ LM (pj , αi,j) as in (9).

3) Update of Pj: It’s likely that only a subset of camerashave corresponding measurements of the jth marker. Asabove, note that

pj(k + 1)− li,xy(k + 1) ∈ PM (li, αi,j).

As li,xy(k + 1) ∈ Li,xy(k + 1) from B.2, we have

pj(k + 1) ∈ Li,xy(k + 1)⊕ PM (li, αi,j).

Considering all Mli that contain measurement of the jth

marker and pj(k + 1) ∈ Pj(k + 1|k) from A, the markernominal position pj(k + 1) ∈ Pj(k + 1) as derived in (11).

C. Robot Body and Orientation Estimation

Points that are at most r distance away from pi locate inPi⊕B2(r). If ‖pi − pj‖2 = rij , the marker nominal positionpj ∈ Pi⊕B2(rij). Thereby, the set refinement by rigid bodyconstrains in (12) preserves the property that pj ∈ Pj(k+1).Finally, given the assumption of the robot body being in theconvex hull of the markers, Pxy in (13) over-bounds theentire robot body Pxy i.e. Pxy ⊂ Pxy . The conclusion forthe robot nominal orientation that pθ ∈ Pθ follows similarlyas B.1.

REFERENCES

[1] S. Thrun, W. Burgard, and D. Fox, Probabilistic robotics. MIT Press,2005.

[2] M. Boban, A. Kousaridas, K. Manolakis, J. Eichinger, and W. Xu,“Connected roads of the future: Use cases, requirements, and designconsiderations for vehicle-to-everything communications,” IEEE Ve-hicular Technology Magazine, vol. 13, no. 3, pp. 110–123, 2018.

[3] S. Thrun, “Simultaneous localization and mapping,” in Robotics andCognitive Approaches to Spatial Mapping. Springer, 2007, pp. 13–41.

[4] T. Teixeira, D. Jung, and A. Savvides, “Tasking networked cctv cam-eras and mobile phones to identify and localize multiple people,” inProceedings of the 12th ACM International Conference on UbiquitousComputing, 2010, pp. 213–222.

[5] N. Ceccarelli, M. Di Marco, A. Garulli, and A. Giannitrapani, “A settheoretic approach to path planning for mobile robots,” in 43rd IEEEConference on Decision and Control, vol. 1, 2004, pp. 147–152.

[6] M. Di Marco, A. Garulli, A. Giannitrapani, and A. Vicino, “Aset theoretic approach to dynamic robot localization and mapping,”Autonomous Robots, vol. 16, no. 1, pp. 23–47, 2004.

[7] B. Song, “Cooperative lateral vehicle control for autonomous valetparking,” International Journal of Automotive Technology, vol. 14,no. 4, pp. 633–640, 2013.

[8] K. Sung, J. Choi, and D. Kwak, “Vehicle control system for automaticvalet parking with infrastructure sensors,” in IEEE International Con-ference on Consumer Electronics (ICCE), 2011, pp. 567–568.

[9] M. Khalid, K. Wang, N. Aslam, Y. Cao, N. Ahmad, and M. K. Khan,“From smart parking towards autonomous valet parking: A survey,challenges and future works,” Journal of Network and ComputerApplications, p. 102935, 2020.

[10] T. D. Barfoot, State estimation for robotics. Cambridge UniversityPress, 2017.

[11] A. Smith, Sequential Monte Carlo methods in practice. SpringerScience & Business Media, 2013.

[12] A. Barrau and S. Bonnabel, “Invariant kalman filtering,” AnnualReview of Control, Robotics, and Autonomous Systems, vol. 1, pp.237–257, 2018.

[13] P. Moutarlier and R. Chatila, “An experimental system for incrementalenvironment modelling by an autonomous mobile robot,” in Experi-mental Robotics I. Springer, 1990, pp. 327–346.

[14] P. Moutarilier, “Stochastic multisensory data fusion for mobile robotlocation and environment modeling,” Proceedings of InternationalSymposium on Robotics Research, 1989.

[15] M. Montemerlo, S. Thrun, D. Koller, B. Wegbreit et al., “Fastslam:A factored solution to the simultaneous localization and mappingproblem,” AAAI Innovative Applications of Artificial Intelligence, vol.593598, 2002.

[16] G. Grisetti, R. Kummerle, C. Stachniss, and W. Burgard, “A tutorial ongraph-based slam,” IEEE Intelligent Transportation Systems Magazine,vol. 2, no. 4, pp. 31–43, 2010.

[17] M. Kaess, A. Ranganathan, and F. Dellaert, “isam: Incrementalsmoothing and mapping,” IEEE Transactions on Robotics, vol. 24,no. 6, pp. 1365–1378, 2008.

[18] F. Dellaert, “Factor graphs and gtsam: A hands-on introduction,”Georgia Institute of Technology, Tech. Rep., 2012.

[19] R. Mur-Artal, J. M. M. Montiel, and J. D. Tardos, “Orb-slam: aversatile and accurate monocular slam system,” IEEE Transactionson Robotics, vol. 31, no. 5, pp. 1147–1163, 2015.

[20] J. Engel, T. Schops, and D. Cremers, “Lsd-slam: Large-scale di-rect monocular slam,” in European Conference on Computer Vision.Springer, 2014, pp. 834–849.

[21] A. J. Davison, I. D. Reid, N. D. Molton, and O. Stasse, “Monoslam:Real-time single camera slam,” IEEE Transactions on Pattern Analysisand Machine Intelligence, vol. 29, no. 6, pp. 1052–1067, 2007.

[22] T. Whelan, M. Kaess, H. Johannsson, M. Fallon, J. J. Leonard, andJ. McDonald, “Real-time large-scale dense rgb-d slam with volumetricfusion,” The International Journal of Robotics Research, vol. 34, no.4-5, pp. 598–626, 2015.

[23] S. Lowry, N. Sunderhauf, P. Newman, J. J. Leonard, D. Cox, P. Corke,and M. J. Milford, “Visual place recognition: A survey,” IEEE Trans-actions on Robotics, vol. 32, no. 1, pp. 1–19, 2016.

[24] T. Naseer, G. L. Oliveira, T. Brox, and W. Burgard, “Semantics-aware visual localization under challenging perceptual conditions,” inIEEE International Conference on Robotics and Automation, 2017,pp. 2614–2620.

[25] K. Pirker, M. Ruther, and H. Bischof, “Cd slam-continuous localiza-tion and mapping in a dynamic world,” in IEEE/RSJ InternationalConference on Intelligent Robots and Systems, 2011, pp. 3990–3997.

[26] R. Arandjelovic, P. Gronat, A. Torii, T. Pajdla, and J. Sivic,“NetVLAD: CNN architecture for weakly supervised place recogni-tion,” in IEEE Conference on Computer Vision and Pattern Recogni-tion, 2016.

[27] U. D. Hanebeck and G. Schmidt, “Set theoretic localization of fastmobile robots using an angle measurement technique,” in Proceedingsof IEEE International Conference on Robotics and Automation, vol. 2,1996, pp. 1387–1394.

[28] T. Alamo, J. M. Bravo, and E. F. Camacho, “Guaranteed stateestimation by zonotopes,” Automatica, vol. 41, no. 6, pp. 1035–1043,2005.

[29] D. Merhy, C. Stoica Maniu, T. Alamo, E. F. Camacho, S. Ben Cha-bane, T. Chevet, M. Makarov, and I. Hinostroza, “Guaranteed set-membership state estimation of an octorotor’s position for radarapplications,” International Journal of Control, vol. 93, no. 11, pp.2760–2770, 2020.

[30] M. Di Marco, “Set-membership estimation techniques for mobilerobotics applications,” Ph.D. dissertation, Bologna University, 2001.

[31] H. Wang, I. Kolmanovsky, and J. Sun, “Zonotope-based recursiveestimation of the feasible solution set for linear static systems withadditive and multiplicative uncertainties,” Automatica, vol. 95, pp.236–245, 2018.

[32] I. Kolmanovsky, I. Sivergina, and J. Sun, “Simultaneous input andparameter estimation with input observers and set-membership param-eter bounding: Theory and an automotive application,” InternationalJournal of Adaptive Control and Signal Processing, vol. 20, no. 5, pp.225–246, 2006.

[33] E. Rublee, V. Rabaud, K. Konolige, and G. Bradski, “Orb: An efficientalternative to sift or surf,” in IEEE International Conference onComputer Vision, 2011, pp. 2564–2571.

[34] D. G. Lowe, “Object recognition from local scale-invariant features,”in Proceedings of the 7th IEEE International Conference on ComputerVision, vol. 2, 1999, pp. 1150–1157.

[35] C. Papageorgiou and T. Poggio, “A trainable system for objectdetection,” International Journal of Computer Vision, vol. 38, no. 1,pp. 15–33, 2000.

[36] O. Achler and M. M. Trivedi, “Camera based vehicle detection,tracking, and wheel baseline estimation approach,” in Proceedings ofthe 7th International IEEE Conference on Intelligent TransportationSystems, 2004, pp. 743–748.

[37] ——, “Vehicle wheel detector using 2d filter banks,” in IEEE Intelli-gent Vehicles Symposium, 2004, pp. 25–30.

[38] M. Althoff, “An introduction to cora 2015,” in Proceedings of theWorkshop on Applied Verification for Continuous and Hybrid Systems,2015.

[39] M. Althoff and D. Grebenyuk, “Implementation of interval arithmeticin cora 2016,” in Proceedings of the 3rd International Workshop onApplied Verification for Continuous and Hybrid Systems, 2016.

[40] M. Althoff, D. Grebenyuk, and N. Kochdumper, “Implementation oftaylor models in cora 2018,” in Proceedings of the 5th InternationalWorkshop on Applied Verification for Continuous and Hybrid Systems,2018.