Single Beacon-Based Localization With Constraints and...

IEEE TRANSACTIONS ON INDUSTRIAL ELECTRONICS, VOL. 63, NO. 4, APRIL 2016 2229

Single Beacon-Based LocalizationWith Constraints and Unknown Initial Poses

Sen Wang, Dongbing Gu, Senior Member, IEEE , Ling Chen, Member, IEEE ,and Huosheng Hu, Senior Member, IEEE

Abstract—This paper studies a single beacon-basedthree-dimensional multirobot localization (MRL) problem.Unlike most of existing localization algorithms which useextended Kalman filter or maximum a posteriori, movinghorizon estimation (MHE), and convex optimization arenovelly designed to perform MRL with constraints andunknown initial poses. The main contribution of this paperis three-fold: 1) a constrained MHE-based localization algo-rithm, which can bound localization error, impose variousconstraints and compromise between computational com-plexity and estimator accuracy, is proposed to estimaterobot poses; 2) constrained optimization is examined inthe perspective of Fisher information matrix to analyze whyand how multirobot information and constraints are able toreduce uncertainties; 3) a semidefinite programming-basedinitial pose estimation, which can efficiently converge toglobal optimum, is developed by using convex relaxation.Simulations and experiments are conducted to verify theeffectiveness of the proposed methods.

Index Terms—Extended Kalman filter (EKF), localization,semidefinite programming (SDP), state estimation.

I. INTRODUCTION

M ULTIROBOT systems are being widely studied dueto their increasingly powerful capability to accomplish

various tasks [1]. As prerequisites for multirobot cooperation,robot poses are one of the most critical information that needto be acquired [2], [3]. Multirobot localization (MRL) is to pro-vide pose estimation for the robot group and, more importantly,improve it by sharing information [4].

In general, filter-based and optimization-based state estima-tion methods can be adopted to address the MRL problem. Forunconstrained and linear systems subject to Gaussian noises,Kalman filter (KF)[5]–[7] can be used as an optimal estima-tor. However, since the MRL problem is nonlinear, extended

Manuscript received March 23, 2015; revised June 29, 2015 andSeptember 1, 2015; accepted October 21, 2015. Date of publica-tion November 10, 2015; date of current version March 8, 2016.This work was supported in part by EU FP7 under the ResearchGrant “ECROBOT—European and Chinese Platform for Robotics andApplications” and in part by the National Natural Science Foundation ofChina under Grant 61165016.

S. Wang is with Department of Computer Science, University ofOxford, Oxford, OX1 3QD, U.K. (e-mail: [email protected]).

D. Gu and H. Hu are with School of Computer Science and ElectronicEngineering, University of Essex, Colchester, CO4 3SQ, U.K. (e-mail:[email protected]; [email protected]).

L. Chen is with School of Mechatronics Engineering andAutomation, Shanghai University, Shanghai 200072, China (e-mail:[email protected]).

Color versions of one or more of the figures in this paper are availableonline at http://ieeexplore.ieee.org.

Digital Object Identifier 10.1109/TIE.2015.2499253

Kalman filter (EKF) is more applicable. In [4], a distributedEKF algorithm which decomposes the estimation to differentrobots is proposed. In [8], all robots exchange their states witheach other to derive a global estimate by EKF. However, sinceEKF suffers from linearization errors, it may cause estima-tor inconsistency or big linearization errors [9]. Particle filter(PF) which does not need linearization can be employed [10].But its computational complexity could be high to localizemultiple robots in a high frequency.

Optimization-based methods can handle nonlinear systemsby iterative convergence and improve the accuracy of entirestate estimates by using all information up to now [11]. In[12], maximum likelihood estimation (MLE) is designed forthe MRL problem with nonlinear models. However, priorknowledge which is of importance for robot localization isnot considered. Maximum a posteriori (MAP)-based approachintroduces prior probability and improves the localization accu-racy by regularization [13]. Unfortunately, it is difficult toapply MAP for real-time or long-term uses because its com-putational complexity increases unboundedly over time whenincorporating all previous information. To overcome this limi-tation, a sliding window method is proposed in [14] to boundcomputational complexity by marginalization. Moving horizonestimation (MHE), which can bound complexity, is utilized in[15] for multirobot relative localization.

Many real systems have physical insights (e.g., nonnegativedistance) or practical limitations (e.g., safety requirements) onstates and disturbances [16], [17], which can be considered asconstraints. However, none of the above algorithms considersconstraints on states and noises. Although some modified filteralgorithms can introduce system constraints [18], optimization-based method can achieve this in a more natural manner. In thefield of estimation, it has been acknowledged that incorporatingconstraints can improve the results [19]. Therefore, it is naturalto ask why and how the constraints are able to function. In thispaper, we examine constraints and answer these questions inthe context of MRL by using Fisher information matrix (FIM).

Sometimes robots have to work in unknown environments,which means it is difficult to obtain their initial poses withrespect to a global coordinate system. An easy solution is toestimate them by using the previous optimization-based meth-ods. However, since these optimization problems are nonlinearprogramming (NLP) for MRL, their initial guesses largelydetermine whether global minima can be reached. Moreover,NLP as a nonconvex optimization problem is typically diffi-cult to be solved. In this paper, the NLP problem of initialpose estimation is reformulated as a semidefinite programming

0278-0046 © 2015 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission.See http://www.ieee.org/publications_standards/publications/rights/index.html for more information.

2230 IEEE TRANSACTIONS ON INDUSTRIAL ELECTRONICS, VOL. 63, NO. 4, APRIL 2016

Fig. 1. Single beacon-based MRL in three dimension.

(SDP) problem by using SDP relaxation [20]. SDP, as convexoptimization, can find the global minima efficiently without ini-tial guesses. It has been successfully used for localization ofstatic sensor network [21], target tracking problem [22], etc.However, to the best of our knowledge, there is no work onSDP-based MRL, especially including nonconvex transforma-tion matrices.

The main contribution of this work is three-fold: 1) A novelMHE algorithm is proposed for single beacon-based three-dimensional (3-D) MRL. Since it is an optimization-basedmethod, it can incorporate system constraints easily. 2) Aninterpretation of constrained optimization-based MRL is givenin the perspective of FIM. It intuitively shows why and howMRL and constraints improve the estimation results. 3) AnSDP-based initial pose estimation algorithm is proposed forMRL problem by applying convex relaxation.

This paper is organized as follows. Section II proposes MHE-based MRL and FIM-based analysis. In Section III, SDP-basedinitial pose estimation is presented. Sections IV and V verify thealgorithms by simulation and experiment, respectively. Finally,conclusion is given in Section VI.

II. MHE-BASED MRL

In this section, both MRL problem and MHE-based MRLalgorithm are described with an analysis on MRL and optimiza-tion constraints.

A. Problem Description

In this MRL system, several robots and a single mobile bea-con (SMB) work with each other, see Fig. 1. Each robot isequipped with proprioceptive sensors to perceive linear veloc-ity. An altitude sensor, an inertial measurement unit (IMU) anda wireless sensor network (WSN) node are installed as extero-ceptive devices. The SMB provides reference position for therobots. Each robot can obtain ranges to the other robots andSMB via WSN.

1) System State: Suppose the state of N robots workingtogether at time t is

xt =[xT1t . . . x

Tit. . . xT

Nt

]T ∈ R6N (1)

where xit = [pTit,ψT

it]T = [xit , yit , zit , φit , θit , ψit ]

T ∈ R6 is

the pose (including x-, y-, z-position p and roll, pitch, yaw ori-entation ψ) of the ith robot at time t. Throughout this paper,

let a be the estimate of a true state a. The neighborhood setof robot i at time t, which includes all robots and SMB thatcan be directly communicated, is represented as Nit with sizenit . 0 and I denote the zero and identity matrices of compatibledimensions, respectively.

2) Process Model: The system models, including processmodel and measurement model, are briefly introduced here.

The process model of the ith robot is

xit+1= f(xit , uit ,wit) (2)

where uit is the motion vector of the robot between time tand t+ 1 with respect to its local frame. The control inputis affected by an additive Gaussian noise wit ∼ N (0,Qc

it).

Let Fit and Git be the Jacobian matrices with respect to xit

and wit , respectively. Since there is a rotation matrix withtrigonometric functions in (2), the process model of the robotis nonlinear. The model of the robot team is

xt+1 = f(xt, ut,wt) (3)

where wt ∼ N (0,Qct). Similarly, denote Ft and Gt as its

Jacobian matrices with respect to xt and wt, respectively.3) Measurement Model: The measurement model for

robot i at time t with three different types of measurements is

zit = h(xit , . . . ,xqt , . . .) + vit , vit ∼ N (0,Rit). (4)

a) Range measurement: The range measurement ofrobot i to robot q (or SMB xbt ) is

ziqr,t = hr(xit ,xqt) + viqr,t = ‖pit − pqt‖2 + viqr,t, q ∈ Nit

(5)

where ‖ · ‖2 is 2-norm and viqr,t ∼ N (0, Riqr,t). We denote iHiq

r,t

and qHiqr,t as the Jacobian matrices of this equation with respect

to xit and xqt , respectively.b) Attitude measurement: The IMU observation

which has been fused with magnetometer is

zia,t = Hia,txit + vi

a,t =[0 I]xit + vi

a,t, via,t ∼ N (0,Ri

a,t).(6)

where zia,t includes roll, pitch, and yaw measurements of theith robot.

c) Altitude measurement: The altitude measured byaltimeter is

zid,t = Hid,txit + vid,t =

[0 0 1 0

]xit + vid,t,

vid,t ∼ N (0, Rid,t). (7)

The SMB-based MRL problem is how to simultaneously local-ize multiple robots modeled as (2) with the measurementmodel (4).

B. MHE-Based MRL

The proposed MRL algorithm consists of two parts: 1) EKF-based dead-reckoning (DR); and 2) MHE-based range update.Since solving a constrained optimization problem is relatively

WANG et al.: SINGLE BEACON-BASED LOCALIZATION 2231

slow, EKF instead of MHE is adopted for DR to fuse high-frequency IMU readings. DR calculates traveled displacementand changed orientation with respect to last pose by fusing theprediction with attitude zia,t and altitude zid,t. Then, the statesevolved by DR are updated by MHE-based range update withziqr,t. Due to the simplicity of DR, it is omitted here.

When a range measurement is obtained, the MHE-basedupdate is used to fuse it with the predicted state. Underthe assumption that evolution of the state is a discrete-timeMarkov process and the poses are independent, the poses canbe obtained by solving the following MAP problem:

maximize{xt}k

0

p(x0,x1, . . . ,xk|zr,0, zr,1, . . . , zr,k)

= minimize{xt}k

0

Φr

({xt}k0)+Φp

({xt}k0)+Φ0

({xt}k0)

(8)where

Φr({xt}k0) =k∑

t=0

N∑i=1

∑q∈Nit

‖ziqr,t − hr(xit ,xqt)‖2Riqr,t

Φp({xt}k0) =k−1∑t=0

N∑i=1

‖xit+1− f(xit , uit)‖2Qit

Φ0({xt}k0) =N∑i=1

‖xi0 − xi0‖2Πi0(9)

‖p‖2A = pTA−1p, Qit = GitQcitGT

it, N (xi0 ,Πi0) is prior

knowledge on the initial state of robot i, zr,i is all the rangemeasurements at time t, and {·}ba denotes a set of quantitiesfrom time a to b. The aim of (8) is to optimally estimate allrobot poses up to time k given all the control inputs {ut}k−1

0

and the range measurements {zr,t}k0 . However, its computa-tional complexity increases over time as more states and rangemeasurements are involved. This hinders its use for real-timeor long-term application. To overcome this problem, the MHEstrategy [23] is adopted. According to (8), MHE-based MRLwith constraints is formulated as

minimize{xt}k

W

Φr({xt}kW ) +Φp({xt}kW ) +Φw({xt}kW )

subject to gl(x,u,w) � dl, l = 1, . . . ,m(10)

where

Φr({xt}kW ) =

k∑t=W

N∑i=1

∑q∈Nit

‖ziqr,t − hr(xit ,xqt)‖2Riqr,t

Φp({xt}kW ) =

k−1∑t=W

N∑i=1

‖xit+1− f(xit , uit)‖2Qit

Φw({xt}kW ) = ZW (xW )(11)

where W = k −M , gl(x,u,w) � dl are generalized kl-dimensional inequality constraints on states and noises, M isthe window size of MHE, and ZW (xW ) is the arrival costof MHE. The advantage of this MHE is that it converts theoriginally unbounded problem (8) into a fixed size problem by

summarizing information acquired before time windowW ontothe arrival cost. In order to compute ZW (xW ) efficiently, anEKF-based approximate algebraic expression is exploited [23]

ZW (xW ) = ‖xW − xW ‖2PW(12)

where

PW = GW−1QW−1GTW−1 + FW−1

(PW−1 −PW−1H

TW−1

(RW−1 +HW−1PW−1HTW−1)

−1HW−1PW−1

)FT

W−1

(13)

is the covariance calculated by EKF. In fact, choosing differentwindow size M enables MHE to scale from MAP to EKF, bal-ancing computational complexity and estimation accuracy [24].Current estimate xk can be yielded after solving (10) by usinginterior-point algorithm.

C. Further Analysis

According to information theory, uncertainty reduces ifinformation increases. Then, the optimization method can beexamined by this to analyze how MRL and constraints boostthe amount of information.

We start our analysis from time k ≤M , in which the MHEproblem (10) is identical to the MAP problem (8). When theoptimization problem (8) is iteratively solved, in each iterationwe have the following problem after linearization:

minimize{Δxt}k

0

Φ({Δxt}k0)

subject to GΔx0:k � d(14)

where

Φ({Δxt}k0)

=

k∑t=0

N∑i=1

∑q∈Nit

∥∥∥ciqr,t −i Hiqr,tΔxit −q Hiq

r,tΔxqt

∥∥∥2Riq

r,t

+k−1∑t=0

N∑i=1

∥∥Δxit+1− FitΔxit − ait+1

∥∥2Qit

+

N∑i=1

‖Δxi0 − ai0‖2Πi0(15)

and

G =

⎡⎢⎣∇gT0...

∇gTm

⎤⎥⎦ , d =

⎡⎢⎣ d0 − g0(x,u,w)...

dm − gm(x,u,w)

⎤⎥⎦ (16)

where

ciqr,t = ziqr,t − hr(xit ,xqt),ait+1= f(xit , uit)− xit+1

,

ai0 = xi0 − xi0 . (17)

Because ‖p‖2A = ‖A− 12 p‖22 if A ∈ S

n++ where S

n++ is the set

of positive definite matrices, Φ({Δxt}k0) can be rewritten as

Φ({Δxt}k0) = ‖C(AΔx0:k − b)‖22 (18)


where

––––––––

––

––

–

––

––

–

C =

[Q− 1

2

R− 12

], A =

[F– – –H

], b =

[a– –c

]Q− 1

2 = diag(Π

− 12

10, . . . ,Π

− 12

N0,Q

− 12

10, . . . ,Q

− 12

Nk−1

)R− 1

2 = diag(R1b

r,0

− 12 ,R12

r,0− 1

2 , . . . ,R(N−1)Nr,k

− 12

)F = I+

[0(6·N)×(6·N ·k) 0(6·N)×(6·N)

– – – – – – – – – – – – – – – – – – – – – – – – – – –diag

(−F10 , . . . ,−FNk−1

)0(6·N ·k)×(6·N)

]H = diag (H0, . . . ,Ht, . . . ,Hk)

Ht =

⎡⎢⎢⎢⎣1H1b

r,t1H12

r,t2H12

r,t

· · · · · · iHiqr,t · · · qHiq

r,t...

...

⎤⎥⎥⎥⎦(∑N

i=1 nit)×(6·N)

a =[aT0 · · · aTk

]Tat =

[aT1t aT2t · · · aTNt

]Tc =

[cT0 · · · cTk

]Tct =

[c1br,t

Tc12r,t

T · · · cjqr,tT · · ·

]T(19)

where diag(·) is a function to map vectors to a block diago-nal matrix. To solve this constrained problem, the primal-dualinterior point method [25] can be applied. The Lagrangian of(14) is

L(Δx0:k,λ) =1

2Φ({Δxt}k0) + λT (GΔx0:k − d) (20)

where λ ∈ RK ,K =

∑ml=0 kl, is the vector of Lagrange mul-

tipliers. According to the block matrices defined in (18), theKarush–Kuhn–Tucker (KKT) conditions of the optimizationproblem (14) are given by

∇Δx0:kL = (FTQ−1F+HTR−1H)Δx0:k

− (HTR−1c+ FTQ−1a) + GTλ = 0

diag(λ)(GΔx0:k − d) = 0.

(21)

In primal-dual interior point methods, the KKT conditions aresolved by Newton’s method. Assume the current point andthe Newton step are (Δx0:k,λ) and (δx0:k, δλ), respectively.Therefore, Newton step for solving (21) is given byApplying Schur Complement to (22), as shown at the bottom ofthe page, the coefficient of δx0:k can be derived

D = (FTQ−1F+HTR−1H)︸︷︷︸FIM of unconstrained problem

+ GT (diag(d− GΔx0:k))−1diag(λ)G︸︷︷︸

extra part

= FTQ−1F

+

[HG

]T [R−1 00 (diag(d− GΔx0:k))

−1diag(λ)

][HG

]= FTQ−1F+ HT R−1H. (23)

[(FTQ−1F+HTR−1H) GT

diag(λ)G −diag(d−GΔx0:k)

][δx0:k

δλ

]=

[−(FTQ−1F+HTR−1H)Δx0:k + (HTR−1c+ FTQ−1a)− GTλ−diag(λ)(GΔx0:k−d)

](22)

Fig. 2. Robot team at time 0, 1, and 2 with partial range measurements.

Since 0 � d− GΔx0:k [according to (14)], 0 � λ (KKT con-ditions [25]) and R is positive definite, it can be derived that R ispositive definite. By examining (23) in the perspective of FIM,D includes the FIM of the unconstrained optimization problem[14], [26] and an extra part which comes from the informationof constraints. Then, the amount of information increases byincluding HTR−1H and the extra part. In fact, H represents thestructure of the MRL, while the extra part is related with theconstraints.

1) MRL: It is known that MRL is superior to single robot-based localization [27]. According to (23), some insights intothis can be derived.

Assume there are three different situations: each robot canmeasure ranges to (a) the SMB only; (b) some robots and theSMB with partial ranges; (c) all robots and the SMB with allranges. Consider time period from 0 to 2 and the situation (b) inFig. 2. H and D of these situations are shown in Fig. 3. Byconcentrating on the differences between Fig. 3(d)–(f), it canbe seen that the information on diagonal blocks increases. Thismeans that as more ranges between robots are used, the infor-mation on robot positions increases. Moreover, there are someoff-diagonal blocks [e.g., cov(x10 ,x20)] in D that appear orbecome denser, i.e., correlation between robots is introducedor increased. Note that the existing off-diagonal blocks [e.g.,cov(x10 ,x11)] in Fig. 3(d)–(f) are from the process model, i.e.,FTQ−1F, while the new ones are from the ranges betweenrobots. These two types of off-diagonal blocks are different inthat the new ones represent the relationships between differentrobots in the same time slot (dashed square area in Fig. 3 indi-cates a time slot), while the existing ones imply the correlationof the same robot in different times. These two kinds of infor-mation ensure that the robots are connected both spatially andtemporally.

The significance of the information on the diagonal is obvi-ous. For the off-diagonal elements, assume that a random vectorx ∼ N (μ,P) can be partitioned into two parts

x =

[xa

xb

], μ =

[μa

μb

], P =

[Paa Pab

Pba Pbb

]. (24)

Then, the conditional density p(xa|xb) is given by

p(xa|xb) ∼ N (μa +PabP−1bb (xb − μb),Paa −PabP

−1bb Pba).

(25)


Fig. 3. H and D in different situations when time is 2. The upper row is H, while the lower row is D. The dashed square indicates a time slot.(a) SMB only. (b) Partial ranges. (c) All ranges. (d) SMB only. (e) Partial ranges. (f) All ranges.

In contrast, the case without off-diagonal elements is simplyp(xa|xb) ∼ N (μa,Paa). It is obvious that covariance can bereduced in the presence of off-diagonal elements.

2) Constraints: In practice, many systems are subject toconstraints, which normally exist as prior information. Becausethe capability to take these constraints into consideration is crit-ical, our MRL method (10) incorporates them as generalizedinequalities. Although constraints are of significance for stateestimation, it has not been explicitly clear why they are use-ful and how they play a role in improving MRL algorithms. Bystudying (23), these problems can be explored.

Fig. 4 shows D with various constraints, i.e., different extraparts in (23). The scenario where no constraint is imposed isemployed as a standard of reference, see Fig. 4(a). Two typesof constraints are investigated here. First is the constraints onthe noises of a robot. For example, if its maximum veloc-ity is known, then its maximum displacement and orientationchanges in a certain time can be limited by constraining thecontrol input noises. When this type of constraints on robot 1is in action, its D is given in Fig. 4(b). Comparing Fig. 4(b)with Fig. 4(a) shows that the states of robot 1 in different timeslots can be constrained, e.g., cov(x10 ,x11) increases. Thiscontributed off-diagonal elements can also reduce the uncer-tainties. Second is the constraints on the states of a robot. Forexample, Fig. 4(c) is a result with the constraints on the statesof robot 2. The changes from Fig. 4(a)–(c) show that only thestate estimates of robot 2 are improved. This constraint, forinstance, can be used when there is prior knowledge on theoperating field. By combining these two types of constraints,

more flexible information can be provided to reduce the uncer-tainty as shown in Fig. 4(d). In fact, the extra part in (23) alsoindicates that the constraints can be conceived as supplementary“sensors,” which supply additional information.

When time k > M , all the results are preserved. Comparing(10) with (8), it can be seen that the same equation structureis maintained, and the only changes to (14) are that the timeinterval is shifted and the prior uncertainty Πi0 becomes theuncertainty of arrival cost PW .

III. INITIAL POSE ESTIMATION USING SDP

When initial poses of robots are unavailable, the systemneeds to be carefully initialized. For filter-based methods, suchas EKF, N (xi0 ,Πi0) can be set to be large. Unfortunately,the single beacon-based MRL system without bearing mea-surement only receives limited information. This means that itmay take a long time for filters to coverage to true values. Inthis section, an SDP-based method is proposed to estimate theunknown initial poses efficiently.

Assume several ranges are collected at the beginning toestimate the unknown poses by using the MAP (8), whichis nonconvex. To convert it to a convex representation, (1) isrearranged as

Xt =[

ETt ΨT

t

]T=[

x1t . . . . . . xNt

] ∈ R6×N (26)

where Et ∈ R3×N is the position vector. Assume Yt = ET

t Et.Then, the equivalent objective function of Φr({xt}k0) can berewritten as follows by introducing new variables:


Fig. 4. D with different constraints. Changes between different figures are highlighted by dashed squares. (a) No constraints. (b) Constraints onrobot 1 only. (c) Constraints on robot 2 only. (d) Both previous constraints.∥∥∥ziqr,t − hr(xit ,xqt)

∥∥∥2Riq

r,t

� Riqr,t

−1(αiqt − 2ziqr,tε

iqt

)∀q ∈ Nit ∧ q �= b∥∥zibr,t − hr(xit ,xbt)

∥∥2Rib

r,t

� Ribr,t

−1(βibt − 2zibr,tη

ibt

) ∀b ∈ Nit

(27)where

αiqt = pT

itpit − 2pTitpqt + pT

qtpqt

= Yiit − 2Yiq

t +Yqqt

βibt = pT

itpit − 2pTitpbt + pT

btpbt

= Yiit − 2pT

itpbt + pTbtpbt

εiqt ≥ 0, αiqt = εiqt

2

ηibt ≥ 0, βibt = ηibt

2.

(28)

Then, the convex form of Φr({xt}k0) is

Φr(α, ε,β,η) =k∑

t=0

N∑i=1

∑q∈Nitq �=b

Riqr,t

−1(αiqt − 2ziqr,tε

iqt

)

+k∑

t=0

N∑i=1

∑b∈Nit

Ribr,t

−1 (βibt − 2zibr,tη

ibt

).

(29)

Furthermore, by using epigraph, Φp({xt}k0) and Φ0({xt}k0)can be written as

Φp(τ ) =

k−1∑t=0

N∑i=1

τit , where τit = ‖xit+1− f(xit , uit)‖2Qit

Φ0(ρ) =

N∑i=1

ρi0 , where ρi0 = ‖xi0 − xi0‖2Πi0.

(30)

Finally, the equivalent problem of (8) is

.


In (31), the constraints highlighted are still nonconvex. Tofinally formulate a convex optimization, the Schur complementand rank relaxation are applied. Then, (31) is reformulated as

minimize{Xt}k

0 ,Yα,ε,β,η,τ ,ρ

Φr(α, ε,β,η) + Φp(τ ) + Φ0(ρ)

subject to

t=0,...,ki=1,...,N

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

αiqt = Yii

t − 2Yiqt +Yqq

t

εiqt ≥ 0,

[I εiqtεiqt αiq

t

] 0 ∀q ∈ Nit ∧ q �= b

βibt = Yii

t − 2pTitpbt + pT

btpbt

ηibt ≥ 0,

[I ηibtηibt βib

t

] 0 ∀b ∈ Nit[

I Et

ETt Yt

] 0

(32a)

t=0,...,k−1i=1,...,N

{[Qit fitfTit τit

] 0 (32b)

i = 1, . . . , N

{[Πi0 xi0 − xi0

(xi0 − xi0)T ρi0

] 0 (32c)

where fit = xit+1− f(xit , uit). Now only (32b) is nonconvex

since fit involves rotation matrix with trigonometric functions.The semidefinite description of convex hull of rotation matri-ces [28] is adopted to handle this. Specifically, for a rotationmatrix Θit ∈ SO(3), where SO(3) is special orthogonal group,its convex hull conv (SO(3)) in (33), as shown at the bottom ofthe page, is used to approximate Θit . Then, the convex problemof (8) is

minimize{Xt}k

0 ,Y,Γα,ε,β,η,τ ,ρ

Φr(α, ε,β,η) + Φp(τ ) + Φ0(ρ)

subject to (32a)

t=0,...,k−1i=1,...,N

⎧⎨⎩[Qit fit(xit+1

,xit ,Γit)

fit(xit+1,xit ,Γit)

T τit

] 0

Γit ∈ conv (SO(3))(32c)

(34)

where fit(xit+1,xit ,Γit) = xit+1

− xit − diag(Γit , I)uit isthe convex relaxation of fit . More precisely, (34) is an SDPproblem which has many merits [20]. Note that by applyingthe previous strategy to (10) and accordingly adjusting the timeinterval, the convex relaxation of (10) can also be derived.

Since accuracy may be degenerated by high-rank propertyof SDP relaxations [21], the SDP is only used to estimate ini-tial poses and its results are further refined by the algorithmproposed in Section II-B.

conv (SO(3))

=

⎧⎪⎪⎨⎪⎪⎩Γit ∈ R3×3 :

⎡⎢⎢⎣1− Γ11

it− Γ22

it+ Γ33

itΓ13it

+ Γ31it

Γ12it

− Γ21it

Γ23it

+ Γ32it∗ 1 + Γ11

it− Γ22

it− Γ33

itΓ23it

− Γ32it

Γ12it

+ Γ21it∗ ∗ 1 + Γ11

it+ Γ22

it+ Γ33

itΓ31it

− Γ13it∗ ∗ ∗ 1− Γ11

it+ Γ22

it− Γ33

it

⎤⎥⎥⎦ 0

⎫⎪⎪⎬⎪⎪⎭(33)

Fig. 5. Simulator and simulation environment in ROS.

IV. SIMULATION RESULTS

In this section, performances of the proposed MHE-basedMRL algorithm and the SDP-based initial pose estimation areevaluated through robot operating system (ROS)-based simu-lation. As shown in Fig. 5, three flying robots and one groundrobot serve as robots and SMB, respectively. The size of oper-ating field is about 50 m × 30 m × 20 m. The frequencyof range update of each robot is set to be 1 Hz and Riq

r,t =(0.5 m)2. The frequencies of IMU and altimeter are 100 Hzwith Ri

a,t = (0.36rad)2I and Rid,t = (1 m)2 respectively. The

window size of MHE is set to be 5.

A. Robots With Known Initial Poses

In this part, the localization results of DR, EKF and MHEare compared under the assumption that the initial poses of therobots are available. Constraints are also discussed.

1) Localization Results: The top-down view of localiza-tion trajectories of DR, EKF and MHE in Fig. 6 shows thatthe estimated positions of DR (magenta solid line with circles)gradually diverges from the ground truth (red solid line), whilethe results of EKF (green solid line with squares) and MHE(blue solid line with stars) follow the ground truth with boundederror owing to the range measurements from SMB.

It is interesting to compare EKF with MHE. To make surethe EKF is well designed as a great standard for comparison,its performance needs to be studied. Specifically, as shown inFig. 7, the EKF innovation sequences of the three robots havezero mean and most of the innovations are within the ±2σ con-fidence bounds (CBs). As expected, their innovation means alsoconverge to 1 and gradually falls within the ±2σ CBs. Theseverify the EKF is well-designed and consistent. Fig. 8(a)–(c) presents the localization errors on x, y, z and root mean


Fig. 6. Top-down view of localization trajectories of DR, EKF, and MHE.

Fig. 7. EKF innovation and 2σ CBs. (a) Innovation and CBs of robot 1.(b) Innovation and CBs of robot 2. (c) Innovation and CBs of robot 3.

square of EKF and MHE. It verifies that MHE can estimate thestates with a higher degree of accuracy and robustness. Evenwhen both methods provide similar results, the localizationerror of MHE is still smoother than that of EKF, see Fig. 8(b).The reason lies in that the proposed MHE is a smoother andits optimization-based property can handle the nonlinear modelmore elegantly than EKF. Therefore, although the EKF is well

designed, its performance in terms of localization accuracy isstill inferior to that of MHE.

The localization errors of MHE against the ±3σ confi-dence intervals (CIs) are shown in Fig. 8(d)–(f). It can be seenthat all the errors are within their lower and upper CBs withextra allowance, which indicates that the estimation of thisMHE-based localization is reliable.

2) Constraints: Two examples are demonstrated toexplain how constraints can improve the results. Meanwhile,these verify the advantages of the proposed MHE algorithm.

a) Constraints on states: Constraints on system statescan be introduced if there is prior knowledge on the regionof feasible values. As shown in Fig. 6, the reasonable posesestimated should be within the boundary. However, withoutincorporating the constraints on states, the trajectories of bothEKF and MHE may cross the boundary occasionally, see theenlargement in Fig. 9(a). If the constraints on states are intro-duced into MHE algorithm, all the estimates of MHE can beconfined within the operating field, as shown in the zoomed partin Fig. 9(b). However, the results of EKF cannot be confined inthe operating area.

b) Constraints on noises: In practice, the measure-ments may suffer from sudden interference, which coulddegrade the estimation accuracy or even result in the failure ofestimator. Suppose one range between robot 2 and 3 is seriouslycorrupted by noise, see Fig. 9(c). Then, the estimation accuracyof EKF is dramatically decreased while that of MHE is slightlyinfluenced. This is because the EKF fuses this inferior mea-surement as a regular observation within Gaussian distribution.However, the MHE smooths the estimate, and more importantlythe constraints imposed on control input noises restrain the esti-mate from sudden change. These constraints, for example, canbe derived from the maximum speed of the robot. By employingthe constraints on noises, the noises can be elegantly modeled.

B. Robots With Unknown Initial Poses

In this section, the initial poses of the robots are assumedto be unknown. To analyze the SDP-based initial pose estima-tion, it is tested with different field sizes, initial guesses andrange measurement noises in Table I. Since orientation can beobserved from IMU, the initial pose initialization is mainlyposition estimation. The robots measure the ranges to two posesof the SMB during the initialization. Note that for each trial, thealgorithm is run 50 times.

Fig. 10 presents the results of the proposed SDP method.As shown in Fig. 10(a), the estimated initial poses are veryclose to their ground truth for the different parameter com-binations. Fig. 10(b) shows that even for the situation where

pi0 = [100, 100, 100]T and√Riq

r,t = 1 m, the estimation errorsof the initial poses are less than 0.8 m.

To test the MHE-based MRL algorithm with SDP-based ini-tial pose estimation, they are combined to perform a MRL withunknown initial poses. The scenario where pi0 = [25, 15, 10]T

and√Riq

r,t = 1 m is considered. Since once the estimator con-verges the remaining localization results should be similar to


Fig. 8. Localization errors of EKF and MHE, and CIs of MHE. (a)–(c) Localization errors. (d)–(f) Corresponding errors and 3σ CIs of MHE-basedmethod. (a) Errors of robot 1. (b) Errors of robot 2. (c) Errors of robot 3. (d) 3σ CIs of robot 1. (e) 3σ CIs of robot 2. (f)3σ CIs of robot 3.

Fig. 9. Localization results without and with constraints on states, and with constraints on noises. The whole scale of the figures is identical to thatof Fig. 6. (a) Without constraints on states. (b) With constraints on states. (c) With constraints on noises.

TABLE IVARIOUS PARAMETERS FOR TESTING INITIAL POSE ESTIMATION

the localization results given in Section IV-A, only the first sev-eral steps are presented for clarity. The localization results ofthe first ten steps of the EKF and the MHE are presented inFig. 11. Fig. 11(a) shows that the proposed localization algo-rithm can converge to the ground truth efficiently, while thetrajectory of EKF changes significantly and it is hard to estimatethe true poses. However, as shown in Fig. 11(b), the innovationof the EKF has converged and been within its ±2σ CBs. Thismeans that EKF cannot converge to the ground truth efficientlywithout a good initial guess.

It is interesting to compare the proposed SDP algorithmwith global optimization. The global solver Bmibnb [29] is

adopted to directly solve the original NLP problem. However,it is found that even for the scenario where pi0 = [25, 15, 10]T

and√Riq

r,t = 0.05 m, Bmibnb failed to find a reasonable valueafter 100 iterations in 191.9 s. Although the maximum numberof iterations can be further increased, it is not a good idea tospend more than 200 s to initialize a MRL system. In contrast,by using the same computer, it only takes about 0.2 s for theSDP method to obtain the optimal values by using the SeDuMisolver [30].

V. EXPERIMENTAL RESULTS

The proposed localization algorithm is further evaluated inexperiment. The experimental environment in Fig. 12(a) showsthat two flying robots and one mobile robot are used in ourexperiment as robots and SMB, respectively. They operatein a rectangular area tracked by a Vicon system [31], whichhas eight tracking cameras and provides ground truth of therobots in submillimeter and subdegree accuracy. As shown


Fig. 10. Initial poses calculated by SDP. (a) Estimated initial posesof the robots in three dimension (initial poses estimated by SDP).(b) Corresponding estimation errors for the different scenarios (estima-tion errors of SDP).

Fig. 11. Localization results of first 10 steps of MHE with SDP-basedinitial poses estimation and EKF when the initial positions are unknownand guessed to be pi0 = [25, 15, 10]T . (a) Localization results of EKFand MHE. (b) EKF innovation.

Fig. 12. Experiment environment, flying robot, and system architecture.(a) Experimental field and robots. (b) ArDrone flying robot with usedsensors. (c) System architecture.

in Fig. 12(b), a downward-looking camera, an altimeter andan IMU with magnetometer are equipped on the flying robotto obtain the linear velocities, altitude, and attitude, respec-tively. Optical flow technique is utilized to estimate the linearvelocities. The ranges between robots are obtained by addingGaussian noises onto their real distances, which is a simplifi-cation of noises in real world. Fig. 12(c) presents the systemarchitecture where the computer communicates with the Viconsystem via Ethernet connection and with the beacon and therobots via wireless connection. The localization algorithm andcommunication software are run on a computer. Control com-mands and sensor readings are sent to and returned from therobots by wireless connection, respectively. Since this workfocuses on localization problem rather than control problem,the robots are controlled manually.

Fig. 13 shows the localization trajectories of the EKF andthe MHE. The 3-D trajectories of the ground truth, the EKFand the MHE of the robots are given in Fig. 13(a). It can beseen that the robots fly at the height between 2 to 2.5 m andboth the EKF and the MHE can estimate the z-positions accu-rately. This is because the altimeter is adopted to measure the


Fig. 13. Localization trajectories of EKF and MHE. (a) Localizationtrajectories in 3-D. (b) Localization trajectories on x− y.

altitude within a certain uncertainty. In order to further evalu-ate the localization results, the localization trajectories of theEKF and the MHE on x− y axes are presented in Fig. 13(b).For clarity, the trajectory of beacon and the result of DR arenot plotted. It can be seen that although both methods producelocalization results with bounded errors, the positions estimatedby MHE are more accurate than those of EKF. Especially, theMHE reduces the significant localization errors at the cornerand the accuracy of the whole trajectory is improved. This alsoagrees with the aforementioned simulation results.

Fig. 14 shows the localization errors of EKF and MHE ofrobot 1 on x-, y-, and z-axes in detail. It clearly presents that thelocalization errors of MHE on x and y tend to be smaller thanEKF’s, especially on x-axis. Note that the localization errors onz-axis are small (mostly smaller than 0.1 m) and both results ofthe EKF and the MHE are similar. This verifies that the heightof the robot can be accurately estimated by fusing the altitudemeasurements from the altimeter.

The orientation estimation errors of the EKF and the MHE ofrobot 1 on roll, pitch and yaw rotations are given in Fig. 15. Asexpected, the errors on these three rotations are small (mostly

Fig. 14. Localization errors on position of EKF and MHE.

Fig. 15. Localization errors on orientation of EKF and MHE.

smaller than 0.1 rad) and the change patterns of the EKF andthe MHE are almost identical. This means both methods canestimate the orientation of the robots with high accuracy. In fact,the attitude measurements from the IMU are efficiently fused inthe algorithm to produce orientation estimation.

In order to analyze the localization results quantitatively, theaverage localization errors of the two robots on both positionand orientation are presented in Table II. It can be seen that theaverage errors of the EKF and the MHE on the z-position andorientations are below 0.05m and 0.1rad, respectively. Thanksto the introduction of the altimeter and the IMU, these errorsare small and the accuracies of the two methods are very simi-lar by fusing the sensor readings of these two sensors. However,the localization errors on x- and y-positions appear relativelybig differences. Especially, the average errors of the EKF andthe MHE of the robot 1 on x-position are 0.2105 and 0.0714,respectively. Since the positions on x- and y-axes need to beestimated by fusing all the information, particularly the rangemeasurements, their estimate accuracy highly depends on the


TABLE IIAVERAGE LOCALIZATION ERRORS OF TWO ROBOTS

design of the localization algorithm. Table II verifies that theMHE-based localization algorithm can not only efficiently fusethe data from the altimeter and the IMU to compute the z-position and orientation on roll, pitch and yaw, but also estimatethe positions on x- and y-axes accurately. These experimen-tal results are similar to the ones in the simulation and furthervalidate that the proposed localization algorithm can achievehigh accuracy by using historical information and iterativelyconverging to the optimum.

VI. CONCLUSION

Optimization-based MRL has been investigated in this paper.A MHE-based MRL algorithm was proposed to estimate theposes of robots. It can bound the localization errors and incor-porate constraints. Moreover, the MRL and constraints are bothanalyzed in the perspective of FIM to explain how they effect toattenuate uncertainties. In addition, an SDP-based initial poseestimation method is developed to efficiently find the globalminima for unknown initial poses even with nonconvex rotationmatrices. Compared with EKF-based MRL, the effectivenessof the proposed algorithm is validated by both simulation andexperiment.

However, the proposed algorithm needs to work properly inpractice under the assumptions that the environment is line-of-sight and the robot team is time-synchronized. Our future workwill focus on alleviating these practical problems and studyingthe effects of different types of sensor noises. Both outdoor andindoor applications of the method will also be conducted.

REFERENCES

[1] I. Palunko, P. Cruz, and R. Fierro, “Agile load transportation: Safe andefficient load manipulation with aerial robots,” IEEE Robot. Autom. Mag.,vol. 19, no. 3, pp. 69–79, Sep. 2012.

[2] R. C. Luo and C. C. Lai, “Multisensor fusion-based concurrent envi-ronment mapping and moving object detection for intelligent servicerobotics,” IEEE Trans. Ind. Electron., vol. 61, no. 8, pp. 4043–4051, Aug.2014.

[3] A. T. Rashid, M. Frasca, A. A. Ali, A. Rizzo, and L. Fortuna, “Multi-robotlocalization and orientation estimation using robotic cluster matchingalgorithm,” Robot. Auton. Syst., vol. 63, pp. 108–121, 2015.

[4] S. I. Roumeliotis and G. A. Bekey, “Distributed multirobot localization,”IEEE Trans. Robot. Autom., vol. 18, no. 5, pp. 781–795, Oct. 2002.

[5] F. Auger, M. Hilairet, J. M. Guerrero, E. Monmasson, T. Orlowska-Kowalska, and S. Katsura, “Industrial applications of the Kalman filter:A review,” IEEE Trans. Ind. Electron., vol. 60, no. 12, pp. 5458–5471,Dec. 2013.

[6] E. DiGiampaolo and F. Martinelli, “Mobile robot localization using thephase of passive UHF RFID signals,” IEEE Trans. Ind. Electron., vol. 61,no. 1, pp. 365–376, Jan. 2014.

[7] O. De Silva, G. K. Mann, and R. G. Gosine, “Efficient distributed multi-robot localization: A target tracking inspired design,” in Proc. IEEE Int.Conf. Robot. Autom., 2015, pp. 434–439.

[8] N. Karam, F. Chausse, R. Aufrere, and R. Chapuis, “Localization of agroup of communicating vehicles by state exchange,” in Proc. IEEE/RSJInt. Conf. Intell. Robots Syst., Oct. 2006, pp. 519–524.

[9] G. P. Huang, N. Trawny, A. I. Mourikis, and S. I. Roumeliotis,“Observability-based consistent EKF estimators for multi-robot coopera-tive localization,” Auton. Robots., vol. 30, no. 1, pp. 99–122, Sep. 2010.

[10] D. Fox, W. Burgard, H. Kruppa, and S. Thrun, “A probabilistic approachto collaborative multi-robot localization,” Auton. Robots., vol. 8, pp. 325–344, 2000.

[11] A. Y. Aravkin, J. V. Burke, and G. Pillonetto, “Optimization viewpoint onKalman smoothing with applications to robust and sparse estimation,” inCompressed Sensing & Sparse Filtering. New York, NY, USA: Springer,2014, pp. 237–280.

[12] A. Howard, M. Matark, and G. Sukhatme, “Localization for mobile robotteams using maximum likelihood estimation,” in Proc. IEEE/RSJ Int.Conf. Intell. Robots Syst., 2002, pp. 434–439.

[13] E. D. Nerurkar, S. I. Roumeliotis, and A. Martinelli, “Distributed maxi-mum a posteriori estimation for multi-robot cooperative localization,” inProc. IEEE Int. Conf. Robot. Autom., May 2009, pp. 1402–1409.

[14] G. Sibley, L. Matthies, and G. Sukhatme, “Sliding window filter withapplication to planetary landing,” J. Field Robot., vol. 27, no. 5, pp. 587–608, 2010.

[15] M. W. Mehrez, G. K. Mann, and R. G. Gosine, “Nonlinear moving hori-zon state estimation for multi-robot relative localization,” in Proc. Can.Conf. Elect. Comput. Eng., 2014, pp. 1–5.

[16] H. Gao and T. Chen, “H∞ estimation for uncertain systems with limitedcommunication capacity,” IEEE Trans. Autom. Control, vol. 52, no. 11,pp. 2070–2084, Nov. 2007.

[17] S. Yin, H. Luo, and S. X. Ding, “Real-time implementation of fault-tolerant control systems with performance optimization,” IEEE Trans.Ind. Electron., vol. 61, no. 5, pp. 2402–2411, May 2014.

[18] D. Simon, “Kalman filtering with state constraints: A survey of linear andnonlinear algorithms,” IET Control Theory Appl., vol. 4, no. 8, pp. 1303–1318, 2010.

[19] D. G. Robertson and J. H. Lee, “On the use of constraints in least squaresestimation and control,” Automatica, vol. 38, no. 7, pp. 1113–1123, 2002.

[20] Z. Luo, W. Ma, A. So, Y. Ye, and S. Zhang, “Semidefinite relaxation ofquadratic optimization problems,” IEEE Signal Process. Mag., vol. 27,no. 3, pp. 20–34, 2010.

[21] P. Biswas, T.-C. Lian, T.-C. Wang, and Y. Ye, “Semidefinite programmingbased algorithms for sensor network localization,” ACM Trans. Sens.Netw., vol. 2, no. 2, pp. 188–220, 2006.

[22] A. Simonetto, D. Balzaretti, and T. Keviczky, “A distributed moving hori-zon estimator for mobile robot localization problems,” in Proc. 18th IFACWorld Congr., 2011, vol. 18, no. 1, pp. 8902–8907.

[23] C. Rao, J. Rawlings, and D. Mayne, “Constrained state estimation fornonlinear discrete-time systems: Stability and moving horizon approxi-mations,” IEEE Trans. Autom. Control, vol. 48, no. 2, pp. 246–258, Feb.2003.

[24] S. Wang, L. Chen, D. Gu, and H. Hu, “An optimization based movinghorizon estimation with application to localization of autonomous under-water vehicles,” Robot. Auton. Syst., vol. 62, no. 10, pp. 1581–1596,2014.

[25] S. Boyd and L. Vandenberghe, Convex Optimization. Cambridge, U.K.:Cambridge Univ. press, 2009.

[26] T.-C. Dong-Si and A. I. Mourikis, “Motion tracking with fixed-lagsmoothing: Algorithm and consistency analysis,” in Proc. IEEE Int. Conf.Robot. Autom., 2011, pp. 5655–5662.

[27] A. I. Mourikis and S. I. Roumeliotis, “Performance analysis of multirobotcooperative localization,” IEEE Trans. Robot., vol. 22, no. 4, pp. 666–681, Aug. 2006.

[28] J. Saunderson, P. A. Parrilo, and A. S. Willsky, “Semidefinite descriptionsof the convex hull of rotation matrices,” arXiv preprint arXiv:1403.4914,2014.


[29] J. Lofberg, “Yalmip: A toolbox for modeling and optimization inMATLAB,” in Proc. IEEE Int. Symp. Comput. Aided Control Syst. Des.,2004, pp. 284–289.

[30] J. F. Sturm, “Using SeDuMi 1.02, A MATLAB toolbox for optimizationover symmetric cones,” Optim. Method. Softw., vol. 11, nos. 1–4, pp. 625–653, 1999.

[31] Vicon. (2015). Vicon Motion Capture Systems [Online]. Available:http://www.vicon.com/, accessed on Mar. 2, 2015.

Sen Wang received the M.Eng. degree in controlscience and engineering from Harbin Instituteof Technology, Harbin, China, in 2011, and thePh.D. degree in computer science from theUniversity of Essex, Colchester, U.K., in 2015.

Currently, he is a Research Assistant withthe Department of Computer Science, Universityof Oxford, Oxford, U.K. His research interestsinclude robot localization, simultaneous local-ization and mapping, visual inertial navigation,experience-based navigation, and multiple sen-

sor fusion.

Dongbing Gu (SM’04) received the B.Sc. andM.Sc. degrees in control engineering fromBeijing Institute of Technology, Beijing, China,and the Ph.D. degree in robotics from theUniversity of Essex, Colchester, U.K.

From 1996 to 1997, he was an AcademicVisiting Scholar with the Department ofEngineering Science, University of Oxford,Oxford, U.K. In 2000, he joined, as a Lecturer,the University of Essex. Currently, he is aProfessor with the School of Computer Science

and Electronic Engineering, University of Essex. His research interestsinclude multiagent systems, wireless sensor networks, distributedcontrol algorithms, distributed information fusion, cooperative control,reinforcement learning, fuzzy logic and neural network-based motioncontrol, and model predictive control.

Ling Chen (M’15) received the M.Eng. degreein control science and engineering from CentralSouth University, Changsha, China, in 2010, andthe Ph.D. degree in computing and electron-ics engineering from the University of Essex,Colchester, U.K., in 2014.

Currently, he is a Lecturer with the Schoolof Mechatronics Engineering and Automation,Shanghai University, Shanghai, China. Hisresearch interests include underwater simulta-neous localization and mapping.

Huosheng Hu (SM’01) received the M.Sc.degree in industrial automation from CentralSouth University, Changsha, China, in 1982, andthe Ph.D. degree in robotics from the Universityof Oxford, Oxford, U.K., in 1993.

Currently, he is a Professor with the School ofComputer Science and Electronic Engineering,University of Essex, Colchester, U.K., leadingthe Human-Centred Robotics Group. He hasauthored more than 370 research articles pub-lished in journals, books, and conference pro-

ceedings. His research interests include autonomous robots, human–robot interaction, multirobot collaboration, embedded systems, perva-sive computing, sensor integration, intelligent control, cognitive robotics,and networked robots.

Dr. Hu is a Fellow of the Institute of Engineering and Technology,U.K., a Fellow of the Institution of Measurement and Control, a SeniorMember of the Association for Computing Machinery, and a CharteredEngineer in the U.K. Currently, he is Editor-in-Chief of the InternationalJournal of Automation and Computing, founding Editor-in-Chief of theRobotics Journal , and an Executive Editor for the International Journalof Mechatronics and Automation.

Single Beacon-Based Localization With Constraints and...

Documents

Transcript of Single Beacon-Based Localization With Constraints and...