Closed-Form Algorithm for the Least-squares Trilateration Problem

Robotica http://journals.cambridge.org/ROB  Additional services for Robotica: Email alerts: Click here Subscriptions: Click here Commercial reprints: Click here Terms of use : Click here

A closed-form algorithm for the least-squares trilateration problem Yu Zhou Robotica / Volume 29 / Issue 03 / May 2011, pp 375 - 389 DOI: 10.1017/S0263574710000196, Published online: 20 May 2010

Link to this article: http://journals.cambridge.org/abstract_S0263574710000196 How to cite this article: Yu Zhou (2011). A closed-form algorithm for the least-squares trilateration problem. Robotica, 29, pp 375-389 doi:10.1017/ S0263574710000196 Request Permissions : Click here

Downloaded from http://journals.cambridge.org/ROB, http://journals.cambridge.org/ROB, IP address: 131.155.2.68 on 20 Feb 201 4

Robotica (2011) volume 29, pp. 375–389. © Cambridge University Press 2010 doi:10.1017/S0263574710000196

A closed-form algorithm for the least-squares trilateration problem Yu Zhou∗  Department of Mechanical Engineering, State University U niversity of New York at Stony Brook, Stony Brook, NY 11794, USA (Received in Final Form: April 16, 2010. First published online: May 20, 2010)

SUMMARY Trilat rilatera erati tion on is the the most most adop adopted ted exte extern rnal al refer referen ence ce-base based d local localiz izat atio ion n techn techniq ique ue for for mobi mobile le robo robots ts,, give given n the correspondence correspondence of external external references. The nonlinear nonlinear least-squares trilateration formulation provides an optimal position estimate from a general number (greater than or equal equal to the dimens dimension ion of the envir environm onment ent)) of referen reference ce points points and corresp correspond onding ing distan distance ce measur measureme ements nts.. This This paper paper presen presents ts a novel novel closed closed-for -form m soluti solution on to the nonlin nonlinear ear leastleast-squ squares ares trilat trilaterat eration ion proble problem. m. The perform performanc ancee of  the propos proposed ed algori algorithm thm in dealin dealing g with with erroneo erroneous us inputs inputs of referen reference ce points points and distan distance ce measur measureme ements nts has been been analyzed analyzed through through representati representative ve examples. examples. The proposed proposed trilateration trilateration algorithm algorithm has low computation computational al complexit complexity y, high operational robustness, and reduced systematic error and uncertainty in position estimation. The effectiveness of  the proposed algorithm has been further verified through an experimental test. KEYWORDS: Localization; Trilateration; Mobile robot.

1. Introduction Introduction This This pape paperr pres presen ents ts a nove novell close closedd-fo form rm tril trilate atera rati tion on algorithm, which facilitates the self-localization of autonomous mous mobile mobile robots robots in two-dim two-dimens ension ional al (2D) (2D) and threethreedimensional (3D) environments. 1.1. Concept Concept of trilateration trilateration Trilat rilatera erati tion on refer referss to posi positi tion onin ing g an obje object ct base based d on the measur measured ed distan distances ces betwee between n the object object and multip multiple le 1 reference points at known positions. (People tend to call it “multilateration” when more than three reference points are used to position the object. However, “multilateration” has has been been used used to name name anot anothe herr proc proces esss of posi positio tion n esti estima mati tion on based on the measured differences differences in the distances between the object and three or more reference points.2 ) Given the corresponde correspondence nce of external external references, references, trilateration trilateration is the most adopted adopted external external reference-bas reference-based ed localization localization technique technique for mobile robots. A number of trilateration systems can be found in literature, such as those in refs. [3–12]. In principle, trilateration locates an object by solving a system of equations in the form

(pi

T 

 − p ) 0

(pi

2 i

 − p ) = r  , 0

(1)

* Corresponding author. E-mail: [email protected]

http://journals.cambridge.org

Downloaded: 20 Feb 2014

where p where p 0  denotes the unknown position of the object, p object,  pi the known position of the  i th reference point, and  r i  the known distance between p0 and pi . Solving a system of Eq. (1) is equivalent to finding the intersection point/points of a set of  circles in R2 or spheres in R3 : (1) In R2 , Eq. (1) defines a circle centered at the reference point pi  with  with a radi radius us of ri . With two referen reference ce points points,, two circl circles es defin defined ed by Eq. Eq. (1) (1) in gene generalinte ralinters rsec ectt at two two poin points ts symmetric to the base line – the straight line connecting the two reference points (Fig. 1). The object is located at one of the two intersection points. In particular, when the object is collinear with the two reference points, points, the two circles are tangent to each other, and the object is located at the point of tangency. With three or more reference points, three or more circles defined by Eq. (1) intersect at one unique point where the object is located (Fig. 2). (2) In R3 , Eq. (1) defines a sphere centered at the reference  r i . With three reference points, point p point  p i  with a radius of  r  threesphe threesphere ress defin defined ed by Eq. Eq. (1) (1) in gene genera rall inte inters rsec ectt at two two points points symmet symmetric ric to the base base plane plane – the plane plane determ determine ined d by the three three referen reference ce points points (Fig. (Fig. 3). The object object is locate located d at one of the two intersection points. In particular, when the object is coplanar with the three reference points, the three spheres intersect at one unique unique point where the obje object ct is loca locate ted. d. With ith four four or more more refere referenc ncee poin points ts,, four four or more more sphe spheres res defin defined ed by Eq. Eq. (1) (1) inter interse sect ct at one one uniq unique ue point where the object is located located (Fig. 4). Whene Whenever ver obtain obtaining ing two inters intersect ection ion points points,, one must must decide decide which which of them them the the obje object ct is actua actuall lly y loca locate ted d at. at. In many many case cases, s, one of the two intersection intersection points naturally distinguishes distinguishes itself as the only reasonable choice based on some simple criteri criteria, a, e.g., e.g., a mobile mobile robot robot movin moving g in an indoor indoor envir environm onment ent and localizing itself with respect to landmarks on the ceiling must be located under the ceiling instead of above the ceiling (where the ceiling is the base plane). In reality, positioning error arises due to the inaccuracy in measuring distances and mapping reference points, and is largely affected by the geometrical arrangement of the reference points and the object.13 As a result, the involved circles or spheres may not intersect at the actual position of  the the objec object, t, or even even may may not not inte inters rsec ectt at all. all. It is thus thus neces necessa sary ry to determine a “best approximation” of the object position, in particular when no intersection point is available (i.e., no real solution exists for the system of Eq. (1)).

IP address: 131.155.2.68

376

A closed-form algorithm for the least-squares trilateration problem

Fig. 1. Two-dimensional trilateration with two reference points.

Fig. 2. Two-dimensional trilateration with three reference points.

Fig. 3. Three-dimensional trilateration with three reference points.

Fig. 4. Three-dimensional trilateration with four reference points.

1.2. Some existing existing trilateration algorithms Though straightforward in concept, the trilateration problem is far from trivial to solve due to the nonlinearity of Eq. (1) and the errors in measuring distances and mapping reference points. A number of algorithms have been proposed in order to effectively solve the trilateration problem, including both closed-form and numerical solutions. Fang14 provided a closed-form solution for determining the 3D navig navigati ation on positi position on of a movin moving g object object based based on the distance measurements from three reference points by referencing the navigation position to the base plane defined by the three reference points. A similar formulation was presented by Ziegert and Mize,15 applied to measuring the tool position in a machine tool using a linear displacement measuring device – the laser ball bar. In order to use those

http://journals.cambridge.org

Downloaded: 20 Feb 2014

formulas, however, the positions of reference points must be defined in or transformed into the “base” frame. Moreover, the error in position estimation is correlated to the chosen frame. Independent of the choice of any particular frame of  reference, Manolakis13 derived a more general closed-form solution for estimating the 3D position of an object based on the distance measurements from three reference points. His work shows that the positioning error is affected by the ranging errors, the geometrical arrangement of the object and reference points, and the nonlinearity of the algorithm. A few typos in ref. [13] were fixed by Rao.16 Recently, Thomas Thomas and Ros17 proposed proposed an alternativ alternativee closed-form closed-form solution for locating an object based on the simultaneous distance measurements from three reference points, using the formulation of Cayley–Menger determinants which are relat related ed to the the geom geometr etry y of the the tetr tetrah ahed edra ra forme formed d by the the obje object ct and three reference points. Closely related to trilateration, Coope18 presented a closed-form solution for determining  n  spheres in Rn based on Gaussian the intersection points of  n elimin eliminati ation. on. A more more robust robust,, though though not closed closed-fo -form, rm, soluti solution on based on the orthogonal decomposition was also presented in ref. [18]. In general, closed-form solutions have low computational comp comple lexi xity ty when when the the solu solutio tion n of Eq. Eq. (1) (1) exis exists ts.. They They also also facili facilitate tate the theore theoretic tical al analys analysis is of the algori algorithm thm performance. performance. Closed-form Closed-form expressions expressions of the relationship relationship between the error in position estimation and that in distance meas measur urem emen entt can be foun found d in refs. refs. [13, [13, 17]. 17]. Howe Howeve verr, existi existing ng closed closed-fo -form rm soluti solutions ons do not accomm accommoda odate te the situation that the involved circles or spheres do not intersect, i.e., no real solution exists for Eq. (1). Moreover, existing closed-form solutions only solve for the intersection points of  n  spheres in Rn . They do not apply to determining the intersection point of  N   N  > n  >  n spheres in Rn , where small errors in distan distance ce measur measuring ing and referenc referencee point point mappin mapping g can easily easily cause the involved spheres to fail to intersect at one point. In order to determine the physically existing location of the target object, even if no intersection point exists, it is always necess necessary ary to determ determine ine an optima optimall estima estimate te which which minimi minimizes zes the residuals of Eq. (1) in some appropriate form. Numerical methods are in general necessary in order to provide such an estimate, as indicated in refs. [17, 18]. Foy19 presented presented a numerical numerical algorithm algorithm called Taylor-series aylor-series estimation which solves the simultaneous set of algebraic posi positi tion on equa equati tion onss by itera iterati tive vely ly impr improv ovin ing g an init initia iall guess with local linear least-sum-squared-error corrections. Incorp Incorpora oratin ting g the distan distance ce measur measureme ement nt errors, errors, Nadiv Nadivii 20 et al. compared three statistical methods for estimating the 3D position of a point via trilateration, a linear least-squares estimator, an iteratively reweighted least-squares estimator, and a nonlinear least-squares technique, and showed that, in general general,, the nonlin nonlinear ear least-s least-squa quares res method method perform performss 21 the the best best.. Hu and and Tang ang gave gave a geomet geometric ric expla explanat nation ion of the optima optimall result result in least-sq least-squar uares-b es-based ased trilat trilaterat eration ion,, which is the point of tangency between the hyperellipsoid, determ determine ined d by the standa standard rd devia deviatio tion n of the positi positioni oning ng error, and the intersection of the hypersurfaces, determined by the constraints among the measurements. Coope 18 also suggested suggested a nonlinear nonlinear least-squares least-squares method to obtain the approx approxima imate te soluti solution, on, which which minimi minimizes zes the sum of the

IP address: 131.155.2.68

 A closed-form algorithm for the least-squares trilateration problem

difference between the measured and estimated distances. In another work, to locate mobile stations (MS) in a 2D GSM cellular communication network, Pent  et al.22 defined a probabilistic model of the MS-BTS (base station) distance measurement error and used the extended Kalman filtering (EKF) to solve the trilateration problem, where the dynamic system was the MS, and the observations were the distance measurements. In general, general, numerical numerical methods are availabl availablee to provide provide an optimal estimation of the position of a target object, in particular when no real solution exists for Eq. (1). Moreover, numeri numerical cal method methodss are in generalnot generalnot limite limited d to dealin dealing g with n spheres in Rn . In fact, higher certainty in position estimation is expec expected ted as the number number of invo involv lved ed referen reference ce points points increases. However, compared with closed-form solutions, numerical numerical methods methods in general general have have higher higher computation computational al comple complexit xity y, and closed closed-fo -form rm perform performanc ancee analys analysis is is in general general not availabl available. e. Many numerical methods involve involve a “local optimization” optimization” searching process, process, such as Newton’ Newton’ss method and the steepest descent method, which iteratively improv improves es an initia initiall guess guess towar toward d a conver converged ged positi position on estimate.18–20 However, most of these search algorithms are sensitive to the choice of the initial guess, and in general converge to a local minimum in the vicinity of the initial guess. guess. Though Though “global “global optimizatio optimization” n” search methods are available, such as the simulated annealing method 23,24 and the genetic algorithm,23,25 the lack of a general, systematic way to tune tune the vario various us algori algorithm thm-sp -speci ecific fic parame parameters ters and decision decision criteria criteria cause inconvenienc inconveniencee in autonomous autonomous applic applicatio ations, ns, and the relati relative vely ly high high time time comple complexit xity y of these these methods makes them not suitable for real-time applications. 1.3. Overview of the proposed proposed trilateration algorithm Built on and extending our recent work 26 (based on “An efficie efficient nt least-sq least-square uaress trilat trilaterat eration ion algori algorithm thm for mobile mobile robot robot locali localizati zation” on” by Yu Zhou, Zhou, which which appeare appeared d in the Proceedings of 2009 IEEE/RSJ International Conference on c  2009 IEEE), we propose Intelligent Robots and Systems.  a novel novel closed-form closed-form trilateration trilateration algorithm algorithm which estimates the the posi positi tion on of a targ target et obje object ct,, e.g. e.g.,, a mobi mobile le robo robot, t, based based on the simult simultane aneous ous distan distance ce measur measureme ements nts from from multiple multiple reference points, points, by solving solving the nonlinear leastsquares trilateration problem using standard linear algebra techniques. The proposed trilateration algorithm intends to combine the merits of existing closed-form and numerical solutions. It has the following salient features:

377

will derive and explain the proposed trilateration algorithm in detai detail. l. Sect Sectio ion n 3 will will perfo perform rm the the erro errorr anal analys ysis is of  the proposed trilateration algorithm through representative examples. Section 4 will present an experimental test of the proposed trilateration algorithm. Section 5 will summarize this work.

2. Proposed Proposed Trilateration Trilateration Algorithm Algorithm 2.1. Nonlinear least-squares least-squares formulation The goal goal of the propos proposed ed trilate trilaterati ration on algori algorithm thm is to estima estimate te the position of a target object based on the simultaneous distance distance measurement measurementss from multiple reference points at known known positi positions ons.. In order order to obtain obtain an optima optimall positi position on estima estimate te of the target target object object from from potent potential ially ly inaccu inaccurat ratee distan distance ce measur measureme ements nts and referen reference ce positi positions ons,, we target target our algorithm to solve the nonlinear least-squares trilateration problem. That is, we define an optimal approximation of the object object position position in Rn (n  2 or 3 corresponding to a 2D or 3D environment, with the global frame of reference attached to the environment) as

 =

p0

 = arg min S (p

0,est ),

(2)

p0,est 

N 

where S (p0,est )  [(pi p0,est )T  (pi p0,est ) ri2 ] 2 , i =1 [(p p0,est   denotes an estimate of the object position p0 , pi the premap premapped ped positio position n of the ith referenc referencee point, point, ri  the measured measured distance between p between  p 0 and p and  p i , and  N  the number of reference points used to determine p determine  p0 . Here, we are not constrained to the case of  N   N  n. Instead, we are going to give a solution to  N  n  ( N  Z + ). the general case of  N 

=



=

 −

≥

 −

−

 ∈  ∈

2.2. Algorithm Algorithm derivation derivation Targeting to derive an efficient algebraic solution, we notice that, given pi and  r i , solving Eq. (2) for  p 0  is equivalent to solving ∂ S (p0 ) ∂ p0

T  0 0

T  0 0

T  0 0 0

= a + Bp + [2p [2p p  + (p  p )I]c − p p  p  = 0, 0

(3) where 0   deno denote tess the the n-dimen -dimensio sional nal zero vecto vector, r, I N  1 T  denotes denotes the n n   identity identity matrix, matrix, a i =1 (pi pi pi N 

×

= B=

 − =

 − −

N  1 T  ri2 pi ) (n-dimen -dimensio sional nal vecto vector), r), i 1 [ 2pi pi N  N  1 ri2 I]  I ] ( n n symmetric matrix), and c (pT  and c i pi )I i 1 pi N 

+

×

(n-dimensional vector). By introducing a linear transform

=

=

(1) In the closed closed form form and using standa standard rd linear linear algebra algebra techniques, the proposed trilateration algorithm has low computation computational al complexity complexity and facilitates facilitates closed-form closed-form performance analysis. (2) Solvin Solving g the least-s least-squa quares res trilat trilaterat eration ion proble problem, m, the prop propos osed ed algo algori rith thm m prov provid ides es an opti optima mall or at least least near near opti optima mall estim estimat atee of the the inte inters rsect ectio ion n poin pointt of  spheres constru constructed cted from from erroneo erroneous us distan distance ce N  n   spheres n measurements and reference positions in R (where  n 2 or 3), and is not limited to solving for the intersection points of exactly  n  spheres in Rn .

where q   is an n-dimensional vector with the k th th element denoted as  qk , we obtain from Eq. (3) an equation containing  q no quadratic term of  q

This This paper paper focuses focuses on the deriv derivati ation on and perform performanc ancee analysis of the proposed trilateration algorithm. Section 2

By defining f  defining f  a Bc 2ccT  c (n-dimensional vector with the k th th element denoted as f k ) and D B 2ccT  (cT  c)I

≥

http://journals.cambridge.org

=

Downloaded: 20 Feb 2014

p0

 = q + c,

(a

T 

T 

+ Bc + 2cc c) +{ Bq+ [2cc [2cc +  =  = + +

(4)

cT  c I]q

 

T 

} − qq q = 0.

= +

(5)

+

IP address: 131.155.2.68

A closed-form algorithm for the least-squares trilateration problem

378 (n

× n symmetric matrix), we rewrite Eq. (5) as f  + [D − (q q)I]q = 0. T 

(6)

In order to solve Eq. (6) for q for  q  efficiently, we introduce a simplification. First, we have D

T 

− (q

q)I

T 

cT  c I

c)T  (p0

 − − −  − −   +  = + +  − − − = B + 2cc +

[(p [(p0

N 

2pi pT  i

pT  i pi

H q

 = f  + hq ,

ri2 I

I

=

cT  c I

c)T  (p0

[(p [(p0

)]I. (7) c)]I

Next, we notice that, with perfect pi and ri , we have from Eq. (1) N 

N 

 = ri2

i 1

(pi

 − p )

i 1

=

=

0

T 

 − p ),

(pi

(8)

0

i 1

(pi

 − p )

i 1

=

=

0

T 

(pi

 − p ).

 =

−

−

− × − −  = − −

=

 =

−

qk = q   =

det Hkh

   

det Hk

k

+

det(H det(H )

det(H det(H )

qn ,

∀k ∈ {1, . . . , n − 1},

(9)

0

q)I

N 

1

≈ N 

 −

2pi pT  i

i 1

=

T 

−

pT  i pi I

   −  + −

T 

[(p  − p ) (p )]I 2cc + [(p p )]I + c c I − [(p [(p − c) (p − c)]I )]I, 0

i

T 

   =− 2

N 

0

i

0

T 

0

i 1

=

pi pT  i

T 

+ 2cc

,

 

(10)

+ Hq = 0.

 

=

Equation (11) is a linear system of  n   equations of the unknown  n -dimensional vector q vector  q.. If  H  H  has full rank (and is hence invertible), q  can be calculated easily as q as  q H−1 f  or using numerical methods such as Gaussian elimination. 27 However, it may happen that H   does not have full rank. In fact, we have verified by symbolic computation that the H   constructed from an arbitrary set of  N  n  independent reference points pi in R2 (where n  2) or R3 (where  n 3) has a rank of  n the  H constructed  constructed from N > n  n 1, though the H pi  in general has a rank of  n. Moreover, when all pi   have the same value of  x , y, or z  coordinate, H  will have a zero row and a zero column and hence a rank of  n 1. In these  n  independent cases, Eq. (11) does not represent a system of  n linear equations, and hence we cannot uniquely determine q determine  q from only Eq. (11). Instead, additional constraints need to

=−

−

 =

1

T 

q q

 

 =

Downloaded: 20 Feb 2014

N 

= − N 

q,



1

pT  i pi

 + N 

i 1

=

T 

− (q

q)I

(14)

= H as

N 



ri2

i 1

=

T 

+c

c.

 

(15)

Substituting Eq. (13) into Eq. (14), we obtain n 1

−

f 

  +  

det Hkh qn ]2

[det Hk

k 1

=

2 T 

2 2 n

det(H ) q  = det(H det(H ) q + det(H

q,

(16)

which which is a quadra quadratic tic equati equation on of qn . Writteninto Writteninto the canoni canonical cal form, Eq. (16) becomes aq n2

 + bq + c = 0,

−

http://journals.cambridge.org

T 

 = q

where q where  qT  q can be obtained from D from  D

(11)

=

=

qk2

k 1

+

f 

−

n



N 

whic which h is an n n symmetric symmetric matrix containing containing no q. Definin Defining g N  2 T  T  2cc and denoting the (k,  l ) element H i =1 pi pi N   H  as  H k,l of  H as k, l , we obtain from Eq. (6)

×

f 

where “det” means the matrix determinant, H determinant,  Hk  is the matrix formed by replacing the  k th th column of  H  with  f  , and H and  H kh is  H   with f  the matrix formed by replacing the  k th th column of  H  with h .  H  with h In particular, because H because H , constructed from the H the  H with  with a rank    n  or  n 1, has a full rank of  n  n 1, det(H of  n det(H ) 0. Equatio Equation n (13) (13) defines defines qk in term termss of qn , whichmean whichmeanss that that q can be completely determined upon the determination of  q  qn . To solve for  qn , one more independent equation is needed. In fact, one valid constraint is

Substituting Eqs. (9) into (7), we obtain

=−

−

(13)

ri2



(12)

N 

 ≈ − (q

 

n

where q where q  is an n 1 dimensional vector with the k th th element   as q k qk , H  is an (n 1) (n 1) matrix with the (k,l)  H k,l element element as H  k,l 1 dimensiona dimensionall k,l k,l –H n,l n,l , f    is an n vector with the k th th element as f  k (f k f n ), and h is an n 1 dimensional vector with the k th th element as h k (H k,n k,n H n,n n,n ). Next, from Cramer’s rule28 and the definition of matrix determinant, we obtain f 

while, with realistic measurements of  pi and ri , we have a good approximation: N 

−

−

)]I c)]I

i 1

2ccT 

D

−

N 

1

T 

be found to construct a complete system of  n  independent equations so that the specific solution can be obtained. Here we propose a unified solution procedure for H for  H  with a rank of either n  (full-rank) or  n 1. First, we construct a degenerated system of  n 1 linear equa equati tion onss from from Eq. Eq. (11) (11) by subt subtrac racti ting ng the the last last equa equati tion on from from the other  n 1 equations

where

a f 

 =

n 1  det(Hkh )2 det(H det(H )2 , k 1  det(H f  n 1 c  det(Hk )2 k 1  det(H



[det(H [det(Hk ) det( det(H Hkh )],

 

n

− =

=



+

− =



b

(17)

=2

n 1 k 1 2 T 



− det(H det(H ) q

IP address: 131.155.2.68

− =

q.

 A closed-form algorithm for the least-squares trilateration problem

Equation (17) is well-known solvable in the closed form as

−b ± q  = n

√ 

b2

2a

− 4ac .

 

(18)

In particular, since det(H det(H ) 0, a 0. Then, by substituting the resulting qn  into Eq. (13), we obtain  qk , k {1,  . . .  ,  n 1}. Furthermore, Furthermore, by substitutin substituting g the resulting resulting q  q  into Eq. (4), we obtain p obtain  p0 . The above process generally results in two candidates of  p0 , due to the duality of Eq. (18). However, only one of   p 0 . Therefore, those two candidates is the valid estimate of  p Therefore, a judgment needs to be made to pick the correct one. The  judging criterion is usually very simple, such as that p0 is known on one specific side of the base plane (or base line) defined by the reference points, or that current estimate of  p  p0 should be close enough to the last one.

= −

∀  ∈

=

2.3. Algorithm Algorithm summary summary Followin Following g the above above derivati derivation, on, the proposed proposed trilateration trilateration algorithm is summarized as Algorithm 1. Since each step of  calculation in Algorithm 1 has its closed-form expression, the proposed algorithm, as whole, indeed provides a closedform solution to the trilateration problem. This facilitates the closed-form closed-form performance performance analysis analysis in Section Section 3. Solving the least-squares trilateration problem, the proposed algorithm provides an optimal or at least near-optimal estimation of  the the posi positi tion on of the the targ target et obje object ct base based d on its its dist distan ance cess from N   referenc referencee points points,, where where N   can can be any any inte intege gerr great reater er than than or equ equal to 2 in a 2D envi enviro ron nment ment or 3 in a 3D envi enviro ronm nmen ent. t. Usin Using g stan standa dard rd line linear ar alge algebr braa techni technique ques, s, the propos proposed ed algori algorithm thm is highly highly tractab tractable le and convenient to implement, and has low computational complexit complexity y. Without Without depending on the techniques techniques which tend tend to be affe affect cted ed by algeb algebrai raicc sing singul ulari ariti ties es,, such such as matrix inversi inversion, on, the proposed proposed algorithm algorithm has high operational operational robustness.

Algorithm 1:  Nonlinear Least-Squares Trilateration in (n {2,3})

379

positions. The input to the algorithm includes the mapped positions of the reference points, p points,  p i , which are defined in a global frame of reference attached to the environment, and the measur measured ed distan distances ces between between the object object and referen reference ce points, ri . In practice, errors arise in pi   due to inaccurate mapping of the reference points, and errors arise in r i  due to imperfect distance measurement of the range sensors. These input input errors errors will will cause cause output output errors in the estima estimatio tion n of  the object position p0 . This section discusses the influence of thes thesee inpu inputt erro errors rs on the the accu accurac racy y of the the esti estima mati tion on output. 3.1. Performance indices We define an (n 1) N -dimen -dimensio sional nal input input vector vector x T  T  [ pi ] which consists of all p all  pi and r i . Thus p Thus  p0  is ri a function of  x, i.e., p0 p0 (x). Denoting the actual value and random error of  x as x¯ and δx   respectively, we have x x¯ δx  for the measurement of  x.  x . Correspondingly, the actual value and output estimate of  p0  are defined by p0 (x¯ ) and p0 (x) p0 (x¯ δx) respec respecti tivel vely y, and the estima estimatio tion n error error  p0  is  δ p0 (x) p0 (x¯ δx) p0 (x¯ ). of  p From the Taylor expansion, we obtain

··· ··· ··· =

δp0 (x)

≤ ≤

∀  ∈

−

3. Performance Performance Analysis The proposed trilateration algorithm estimates the position of a targ target et obje object ct base based d on the the simu simult ltan aneo eous us dist distan ance ce measur measureme ements nts from multip multiple le referen reference ce points points at known known

http://journals.cambridge.org

Downloaded: 20 Feb 2014

+

−

0

0

∞

 = k 1

=

1 ∂ k p0 (x¯ ) k!

 k ∂ xT 

[k ]

δx

→

×

.   (19)

×

k1 k2

k th t h Krone Kroneck cker er powe powerr of δx, i.e. i.e.,, δx[k] δx δx δx. We assume that the random errors of the input quantities have zero mean values, i.e., E(δx) 0. Neglecting the highorde orderr term termss in Eq. Eq. (19) (19),, we canobtain canobtain,, base based d on the the defin definit itio ion n of mean and variance of random variables, the mean vector E(δp0 ) and variance matrix var(δp0 ) of the output error  δ p0 as

= ⊗ ⊗···⊗

=

2

E [δp0 (x)]

≤ ≤  ∈

(1) Calculate a Calculate a,,  B,  B ,  c,  c,  H,  H , f ,  H  ,  h  (2) Calculate q Calculate qT  q from Eq. (15). (3) Calculate q n  from Eq. (18). (4) Calculate q k , k {1,  . . .  , n 1}, from Eq. (13). (5) Calculate p Calculate p0  from Eq. (4). (6) Choose one of the two candidates of  p  p0 . (7) Return p Return p0.

=

+

Here we follow the notation of matrix calculus.29 The matrix F derivative ∂∂M  of a function F function  F::  M n×m Fp×q   is represented as an n m block matrix whose (k1 ,k2 ) entry is a  p q matrix ∂F , where where M k1 k2  is  is the (k 1 ,k 2 ) entr entry y of M. δx[k] denote denotess the ∂M 

n

f  ,  f  ,  Hkh and H and H k .

 =

= p (x¯ + δx) − p (x¯ )

∈

 ∈

=

= +

R

 N  reference Input: A set of  N   reference points {pi | i Z + , n i N }, and the corres correspon pondin ding g set of distan distances ces betwee between n unknown position p0 — {ri | i Z + , pi   and the unknown n i N }. Output: p0 .

+

var[δp0 (x)]

≈ 12 ∂ p (x¯ ) vec[var(δx)], 0

 2

∂ xT 

≈

∂ p0 (x¯ ) ∂ p0 (x¯ )T  ,   var(δx) ∂ xT  ∂x

 

(20)

 

(21)

where vec(M) denotes the vector created from a matrix M matrix M by  by stacking its columns, [

···

∂ 2 p0 T  ∂ pT  i  ∂ pj 

···

∂ 2 p0 ∂ pT  i  ∂ rj 

∂ p0 ∂ xT 

∂ p0 ∂ pT  i ∂ 2 p0 ∂r i ∂r j 

∂ p0 ∂r i

∂ 2 p0  2 ∂ xT 

= [· · · · · · · · ·], and  = Equations (20) ··· · · · · · ·]. Equations ∂ 2 p0 ∂r i ∂ pT  j 

and (21) agree with the formulation in ref. [13]. They show that the mean and variance of the output error are directly related to the variance of the input error. Adopting the scheme of error analysis in refs. [13, 17], we evalu evaluate ate the impact impact of the position position error of  pi , δpi , and and the the impa impact ct of the the dist distan ance ce error error of  ri , δri , on δp0 separately. To evaluate the impact of  δpi on δp0 , we assume that the measur measureme ement nt errors errors in mappin mapping g the referenc referencee points points are zero-mea zero-mean n random random varia variable bless and uncorre uncorrelat lated ed from from one one anot anothe herr with with the the same same stan standa dard rd devi deviat atio ion n σp for

IP address: 131.155.2.68

A closed-form algorithm for the least-squares trilateration problem

380

each coordinate. coordinate. We define two performance performance indices, the normalized total bias Bp , which represents the systematic estimation error, and the normalized total standard deviation error S p , whic which h repre represe sent ntss the the unce uncert rtain ainty ty of posi positi tion on estimation Bp

∂ 2 p0

|E(δp )| = 1 (p ) = 0

0

2 ∂ pT  2

σ2p

 =    N 

1

S p (p0 )

∂ 2 p0

T  2 i 1 ∂ pi

2



vec(InN )

vec(In ) ,

=

[var(δp0 )] Tr [var(

=

    

σp

Tr 

=

(22)

∂ p0 ∂ pT  0 ∂ pT  ∂ p



,   (23)

where p [ pT  ]T  is an nN -dimensiona -dimensionall vector vector i consisting of all p all  p i , |v| denotes the norm of a vector v, and Tr (M) denotes the trace of a matrix M. We notice that Bp  σ p . and  S p  are independent of  σ Similarly, to evaluate the impact of  δ assume  δ ri on  δ p0 , we assume that the errors in measuring distances are zero-mean random variables and uncorrelated from one another with the same standard deviation σr . We define two performance indices, the normal normalize ized d total total bias bias Br  and the normal normalized ized total total standa standard rd deviation error  S r as

= ··· ···

Br (p0 )

S r (p0 )

= |E(σδp )| = 12 0

2 r

=

 

 

Tr [var( [var(δp0 )] σr

2

∂ p0  2 ∂ rT 

=

 = 

vec(IN )

   Tr 

N 

1 2

∂ p0 ∂ pT  0 ∂ rT  ∂ r

   i 1

=

,

2

∂ p0 ∂ ri2

 

,

(24)  

(25)

where r [ ri ]T  is an N-dime N-dimensi nsiona onall vecto vectorr consis consistin ting g  σ r . of all  r i . We notice that  B r and S r  are independent of  σ The differential terms of  p0   relevant relevant to Eqs. (22)–(25) ∂ p0 ∂ p0 ∂ 2 p0 ∂ 2 p0 include ∂ pT  , ∂r , T  2 and ∂r 2 . They are derived and listed

= ··· ··· i

i

∂ pi

i

in the Appendix. As discussed before, due to the duality of Eq. (18), we generally obtain two candidates of  p0   from computation computation.. ∂p Correspondingly, we can generally obtain two ∂ xT 0   and two ∂ 2 p0

, and hence two values for each of  Bp ,  S p , Br , and S r . In order to distinguish between them, we denote the values b2 4ac in for Bp , S p , Br , and S r   corresponding corresponding to Eq. (18) as Bp+ ,  S p+ , Br + , and  S r +  respectively, and those b2 4ac as Bp− , S p− , Br − , and S r − corresponding to respectively. As a result, in practice the prediction of the estimation error will, in general, depend on whether p0 is b2 4ac  or b2 4ac . corresponding to An error analysis on the proposed trilateration algorithm has been conducted, and the results of the above-defined performance indices are reported in the following sections. Without loss of generality, our error analysis focuses on R3 , with an understanding that the trilateration in R2 is indeed a downgrade of the trilateration in R3 . Through representative examples, we test the proposed trilateration algorithm with  2

∂ xT 

 −

√ 

 +

−

−

√  + −

http://journals.cambridge.org

√ 

√  − −

Downloaded: 20 Feb 2014

three reference points at first and then with four reference points. 3.2. Trilateration Trilateration in R3 with three reference points Follo Followin wing g the represe representa ntati tive ve exampl examples es in refs. refs. [13, [13, 17], 17], 3 we examine a three-reference case in R in which the XY  coordinates of the three reference points form an equilateral triangle inscribed in a circle centered at the origin of the frame frame of referen reference ce with with a radius radius of 1000. 1000. The referen reference ce T  points are located at p1 [ 500 3 500 0 ] , p2 [ 0 1000 0 ]T  and p3 [ 500 3 500 0 ]T  . We also {p0 define define two square square data data acquis acquisiti ition on region regionss as R+ T  [ x , y, z]  z  z 8000, 4000  x , y 4000} and R− {p0 [x , y , z]T  z 8000, 4000 x, y 4000}, which are symmetric to the base plane defined by the reference points and corresponding to b2 4ac  and b2 4ac in Eq. (18) respectively. As a result, we obtain Bp+ , S p+ , Br + , and  S r +  across  R + , and  Bp− ,  S p− ,  B r − , and  S r −  across R− . Analyzing the effect of  δpi on δp0 , we notice that Bp+ and Bp−  are symmetric to the base plane, so are S p+ and S p− , i.e., Bp (p0 Bp+ (p0 [x,y, z]T  ) [x , y , z]T  ) [x,y, z]T  ) and S p (p0 [x , y , z]T  ) Bp− (p0 S p+ (p0 [x , y , z]T  ) [x,y, z]T  ). Moreover Moreover,, S p− (p0 Bp and  S p  are symmetric symmetric to the orthographic orthographic projections projections of  the medians of the base triangle onto the data acquisition plan planes es.. For For exam exampl ple, e, one one of the the medi median anss is the the y axis axis,, T  and correspondi correspondingly ngly Bp (p0 [x,y, z] ) Bp (p0 T  T  S p (p0 [ x , y , z] ) and S p (p0 [x,y, z] ) T  [ x , y , z] ). The resulting trends of  Bp and S p   are shown in Fig. 5. Both Bp  (systematic error) and S p   (estimation uncertainty) increase as p0   moves away from the centroid of the base  B p and  S p  are obtained at triangle. The minimum values of  B the centersof centersof the data data acquis acquisiti ition on region regionss p0 [0, [0, 0, 8000]T  which correspond to the centroid of the base triangle, where Bp,min 0.0055 and  S p,min 9.3277. p,min Compared with the results reported in ref. [13, 17] which were generat generated ed from the exactl exactly y same same examp example, le, the propos proposed ed algorithm has lower  S p  values (Figure 4a in ref. [17] reports that S p2 170 (S p  13.04) when p0  [0, 0, 8000]T  , and S p2 192 (S p  13.86) when p when  p 0  [ 4000, 4000, 8000] T  , which are consistent with the results in ref. [13]. Since only reportedin tedin ref. ref. [17] [17],, we only only make make a comp compari ariso son n in S p S p was repor here.), which means that the proposed trilateration algorithm has a reduced uncertainty in position estimation when using imperfectly mapped reference points. Analyz Analyzing ing the effect effect of δri  on δp0 , we noti notice ce that that simi similar lar to Bp and S p ,  B r +  and  B r −  are symmetric to the base plane, so are S r +  and S r − ; Br and S r   are are symmet symmetric ric to the orthog orthograp raphic hic projec projectio tions ns of the medians medians of the base triang triangle le onto onto the data acquisition acquisition planes; planes; Br and S r   increase as p0   moves away from the centroid of the base triangle. The minimum values of  Br and S r   are obtained at the centers of the data acquisition acquisition regions, regions, where Br,min   0.0054 0.0054 and S r,min r,min 9.3277. Moreover, the values of  Br and S r   are very close, if not equal, to those of  Bp and S p   respectively (Table I). For this reason, we do not present the figures for  B r and  S r specifically.

=

 = − √ √  −  =  = − = | = − ≤ ≤ = | =− − ≤ ≤ √  √  + − − −

 =

 =  =

− −

 = −

±

 =  =  = ±  = −

 =

 = =

± ±

± ±

= =

=

=

 ≈

 ≈

 = =

 = =

±

=

 =

 =

 =  = −

=

IP address: 131.155.2.68

=

 A closed-form algorithm for the least-squares trilateration problem Normalized Norm alized B ias 4000

3000

  9   0  0    5   0.   0  8   0   0.    8    0    0  .    0

 6 5  0. 0 0

0  . 0  0   6  5  

     y

0

0     . 0     0     7     5    

0  . 0  0   6  

    7     0     0  .     0       5       7       7       0       0       0       0 .  .       0       0

0  . 0  0   0    8  5   . 0    0    9    0    . 0    0    8   

0 .0  0 7  

2000

1000

S p  and  B r / S  Sr   for the Table I. Comparison between  B p / S  three-reference case.

B p 

0 . 0 0  07     5  7

  7 5  0. 0 0  7  0. 0 0

0       . 0       0       7      

     6      0      0  .      0

       5       6       0       0  .       0

p0

(0,0,8000)

( 4000 4000,, 4000 4000,, 8000)

 Bp  Br S p S r

0.0055 0.0054 9.3277 9.3277

0.0102 0.0101 12.7608 12.7608

−

( 2000, 2000, 1600, 1600, 8000)

(2000, (2000, 3200, 3200, 8000)

0.0064 0.0062 10.0348 10.0348

0.0074 0.0073 10.8435 10.8435

−

 Bp / S  Sp  and  B r / S  S r  are compared at 4 representative  p0 .

       5       6       0       0  .       0

   6    0    0  .    0

 –1000  –100 0

381

     5      7      0      0  .      0

0 .0 0  06      6 0  .0      7 0      0 . 0    0  .0   0  6     0 . 0    5   0     0  5 7    7     6 0    .0    5     0. 0 0 0    8     –3000  –300 0 0     5 . 0   0  .0   0. 007  0  7   0  0  8   5   0  9 0   0    .  0. 0   0  0 9    8  5      8   0 . 0 . 0 0    0 07     5  7 .   0   0 5    0  –4000  –400 0  –4000  –400 0 –300 –3000 0 –200 –2000 0 –100 –1000 0 0 1000 2000 3000 4000

S p  of the three-reference case Table II. Comparison between  B p / S  and those of the four-reference case.

 –2000  –200 0

x 

3000

2000

11

   2   1

10 1    1   

   0    1

1000

     y

       5   .        0         1

0

1 0  .5  

 5  5  1 0 .

  1  1

    5  .    1    1

1  2  

 9. 5

1      0      

      1       1

1000  – 1000

3000  – 3000

1    1    . 5    

1  1  

1  0   .5  

1  2   

   5  .    0    1

  1  0

1 0 

  1  1

10 .5

11 4000  – 4000 4000 – 3000 3000 – 2000 2000 – 1000 0  – 4000

   5 .   1  1

  1  2 1000

2000

3000

4000

x 

Fig. 5. Normalized total bias Bp   and normalized normalized total standard standard deviation error  S p  obtained from the three-reference example.

Comp Compar ared ed with with the the resul results ts repor reported ted in refs. refs. [13, [13, 17], 17], the proposed proposed algorithm has significantl significantly y lower lower Br   values (Ref. [17] reports that the maximum Br  on the edge of the same data acquisition region is about 0.03 which agrees with the result in ref. [13], while our result is about 0.01.), which means means that that the propos proposed ed trilate trilaterati ration on algori algorithm thm has a reduce reduced d system systemati aticc error error in positi position on estima estimatio tion n when when using using erroneo erroneous us distance measurements. 3.3. Trilateration Trilateration in R3 with four reference points To test test the the perfo perform rman ance ce of the the prop propos osed ed tril trilate atera rati tion on N > n  reference points, we examine a fouralgorithm algorithm with N > reference reference case in R3 in which the  XY  coordinates   coordinates of the four reference points form a square inscribed in the same circle

http://journals.cambridge.org

S p,max p ,max

0.0055 0.0070

0.0102 0.0101

9.3277 8.0780

12.7608 11.0059

=±

 −

= =±

 −

=

 = ±

±

as in the three-reference example (centered at the origin of  the frame of reference with a radius of 1000). The reference points points are locatedat locatedat p1 [ 500 2 500 2 0 ]T  , p2 [ 500 2 500 2 0 ]T  , p3 [ 500 2 500 2 0 ]T  ,

1      0      . 5     

    5  . 9

2000  – 2000

S p,min p ,min

1     1     . 5    

        0          1

9     . 5    

Bp,max

In the three-reference case,  Bp,min  and S  and  Sp,min  are obtained at p at  p0  p [0, 0, 8000], and  Bp,max and  S p,max  are obtained at p at [ 4000, 0 p ,max 4000, 8000]; In the four-reference case,  B p,min  is obtained at  p 0 [0, [0, 0, 8000], Bp,max  is is obtain obtained ed at p0 [4000, [4000, 4000, 4000, 8000] 8000] and [ 4000, 4000, 8000], S 8000],  S p,min  is obtained at  p 0  [0, 0, p0 8000], and  S p,max at  p0 [ 4000, 4000, 8000]. p ,max  is obtained at p

± ± = ±  = − ±

Normalized Standard Deviation S p  4000

Three-reference Four-reference

 Bp,min

Downloaded: 20 Feb 2014

√  √   =  = − − √  √  √  √   = − √  √  and p  = [ 500 2 −500 2 0 ] . Usin Using g the the same same two two T 

4

square data acquisition regions as the three-reference case, we calculate Bp+ , S p+ , Br + , and S r +   across R+ , and Bp− , S p− ,  Br − , and  S r −  across  R− .  B p+ and  B p−  are shown in Fig. 6. The resulting trends of  B Different from the three-reference case, Bp+ and Bp− are not symmetric to the base plane, i.e., in general Bp+ (p0 [x , y , z]T  ) Bp− (p0 [x,y, z]T  ). Instead, Instead,  B p+ and  B p− are mutually symmetric to the diagonals of the base square, i.e.,  B p+ (p0 [x , y , z]T  ) Bp− (p0 [ x, y, z]T  ). S p+ and S p−  are still symmetric to the base plane, i.e., S p (p0 S p+ (p0 S p− (p0 [x,y, z]T  ) [x , y , z]T  ) T  [x,y, z] ). Moreover Moreover,, S p   is radial radially ly symmet symmetric ric to the verti vertica call axis axis which which pass passes es thro throug ugh h the the cent center erss of the the base base square square and data data acquis acquisiti ition on region regions, s, i.e., i.e., S p (p0 [x,y, z]T  ) [ x,y, z]T  ) S p (p0 S p (p0 T  T   S p [x, y, z] ) S p (p0 [ x, y, z] ). The trend of  S  is shown in Fig. 7. Both Bp  and S p  increas  increasee as p0 move movess away away from from the the cent center er of the base square. The minimum values of  Bp and S p are obtained at the centers of the data acquisition regions, where Bp,min Bp+,min Bp−,min 0.0070 and  S p,min S p+,min p,min S p−,min 8.0780. Compar Compared ed with with the threethree-refe referenc rencee case case (Tabl (Tablee II), we notice notice that there there is an increa increase se in the minimum minimum Bp due to the additio addition n of anothe anotherr imperfe imperfectly ctly mapped mapped referen reference ce point (Bp,min   0.0070 for the four-reference case versus 0.0055 for the three-reference case). However, the Bp,min maximum Bp  decreases slightly ( Bp,max   0.0101 for the

= =

=

 =

=

 = −

±

± − ±

= =

=

 =

=− − −  =  =

 =

=− ±  = − − ±

=

=

 =

−

 = =

=

=

 =

=

 =

=

IP address: 131.155.2.68

A closed-form algorithm for the least-squares trilateration problem

382 Normalized Bias 4000

3000

.

  8   0  0   0   0.

0    . 0    0    7     5    

1000 0        .  0        0        7        5       

0

0    . 0    0    9    0    . 0    0    8    5   

0  . 0  0   8  

0 . 0 0  07     5  7 5 

   7  5   0  0   0   0.

2000

     y

Table III. Comparison between  S p  and  S r  for the four-reference case.

B p +

0     . 0     0     8    

p0

(0,0,8000)

( 4000 4000,, 4000 4000,, 8000)

S p S r

8.0780 8.0780

11.0059 11.0059

−

( 2000, 2000, 1600, 1600, 8000)

(2000, (2000, 3200, 3200, 8000)

8.6836 8.6836

9.3884 9.3884

−

S p  and  S r  are compared at 4 representative  p0 .

Normalized Standard Deviation 4000

 –1000  –100 0      5      7     0     0  .     0

0  . 0  0   7    5   

 –2000  –200 0 0    . 0    0    8   

   5   0 .       8       0       0  . 0

 –3000  –300 0

 9. 5

3000

  1  0

2000

    5  .    9

  9

1000

2000

3000

4000

8        .  5       

     5  .     8

 –2000  –200 0 9    . 5    

2000

 –3000  –300 0

0     . 0     0     7     5    

1000 0       . 0       0       0 8      

0  . 0  0   8  

 9

 9. 5

9 . 5  0

1000

2000

   5 .   1  0 3000

4000

x 

Fig. 7. Normalized total standard deviation error  S p  obtained from the four-reference example.

0  .0   0  7   5  

0  . 0  0   8  5    0  . 0  0   9  

4000  – 4000 4000 – 3000 3000 – 2000 2000 – 1000  – 4000

 5  0. 0 0 7  8 0. 0 0

0 . 0 0  08      8

0

1000

2000

3000

4000

x 

Fig. 6. Normalized total bias  B p +  and  B p −  obtained from the four reference example.

four-reference case versus Bp,max   0.0102 for the threereference case). Moreover, a comparison between Figs. 5 and 6 exposes that the values of  Bp   at the corners of the data acquisition regions in the four-reference case are in general lower than those in the three-reference case. This means that, as p0  moves away from the center of the data acquis acquisiti ition on region region,, the increas increasee rate of  Bp   for the the four four-reference case is less than that of the three-reference case, and hence hence the system systemati aticc estima estimatio tion n error error with with four four referen reference ce points tends to be lower than that with three reference points when the object is located farther away. We also notice (from Table II and Figs. 5 and 7) that  S p  of the four-reference case is significantly lower than that of the three-reference case across the data acquisition regions, which means that the uncertainty in position estimation, when using imperfectly

=

http://journals.cambridge.org

   1

1  0   

1  0   .5   

        5          7         0          0    .         0 

1000  – 1000

2000  – 2000

8. 5

9  

 –4000  –4000  –4000  –400 0 –300 –3000 0 –200 –2000 0 –100 –1000 0

0      . 0      0      7      5     

     5  .     9

    9

0  . 0  0   8  

0 .0  0 7   7  5 

   7  5  . 0  0   0   0

9     . 5     

        9 

0

 –1000  –100 0     8    0    0  .    0

9    

      5  .

     y

B p  – 

4000

3000  – 3000

8  .5  

1000 0

Normalized Norma lized Bias

     y

1     0    

   5   8.

x 

3000

1  0   .5   

9 .5  9 

0. 007 5

 –4000  –4000  –4000  –400 0 –300 –3000 0 –200 –2000 0 –100 –1000 0

S p 

Downloaded: 20 Feb 2014

mapped reference points, will decrease by referring to more reference points. Similar Similar to the threethree-refe referen rence ce case, case, Br + and Br − are symm symmet etricto ricto the the base base plan plane, e, so are are S r + and S r − . Moreover Moreover,, Br and S r  are  are radiall radially y symmet symmetric ric to the vertic vertical al axis axis which which passes passes through the centers of the base square and data acquisition regions. Both  Br and S r  increase as p as p0  moves away from the centroid of the base square. The minimum values of  B r and S r  are obtained at the centers of the data acquisition regions, where  B r,min 0.0040 and  S r,min 8.0780. The trend of  B  Br r,min is shown in Fig. 8. We also notice that the values of  S r are very close, if not equal, to those of  S p  (Table III). For this reason, we do not present the figure for S r  specifically. Compar Compared ed with the value valuess in the three-r three-refer eferenc encee case case (from (from Table able IV and and Figs Figs.. 5, 7, and and 8), 8), both both Br and S r in the four-r four-refer eferenc encee case case are signifi significan cantly tly lower lower across across the data data acquis acquisiti ition on region regions, s, which which means means that that both both the systematic error and the uncertainty in position estimation, when using erroneous distance measurements, will decrease by combining more distance measurements.

=

=

3.4. Approximation Approximation performance of the the error indices In the above above error error analys analysis, is, the Bp , S p , Br and S r are calculated calculated directly from Eqs. (22)–(25). (22)–(25). In order to validate validate

IP address: 131.155.2.68

 A closed-form algorithm for the least-squares trilateration problem S r  of the three-reference case Table IV. IV. Comparison between  B r / S  and those of the four-reference case.

Three-reference Four-reference

 Br,min

Br ,max

S r,min r ,min

S r,max r ,max

0.0054 0.0040

0.0101 0.0075

9.3277 8.0780

12.7608 11.0059

In the three-reference case,  B r,min  and  Sr,min  are obtained at p at  p 0 [0, 0, 8000], and  B r,max  and  S r ,max are obtained at p at  p0 [ 4000, 4000, 8000]; In the four-reference case, Br,min and S r,min r ,min are obtained at p at  p0 [0, 0, 8000], and  Br,max  and  S r,max  are obtained r ,max at p at  p0 [ 4000, 4000, 8000].

±  ± = ±  = ± ± ±

 =

=±

Normalized Nor malized Bi as

B r 

4000

   5    0  6 3000    0  .

   6    0    0  .    0     5 2000     5     0     0  .     0

1000

     y

0

      5       5       0       0  .       0

 5 5  0. 0 0

0 .0 0  05     5  5

 0 5  0. 0

0 .0  0 5  5 

0    . 0    0    6    5    0     . 

0     0     0     . 0     0     5      5     

 0. 0 0 4 5

      5      0      0  .      0

0     . 0     0     4     5    

      5       4       0       0  .       0

0       . 0       0       5      

 –1000  –100 0

 –2000  –200 0

0    . 0    0    5   

0    0     . 0    . 0     0    5     0     5     6    

   5   0 4   0 .   0

0 . 0  0 4  5  

 –3000  –300 0 . 0    0    6    5   

0.005

0 .0 0  05     5  5 5   –4000  –4000  –4000  –400 0 –300 –3000 0 –200 –2000 0 –1000 –1000

0

   5    0    0    0 .

 0 5 5  0. 0 1000

2000

     5      5      0      0  .      0

   0    0  .    0

   5    0  6    0  .    0 3000

4000

x 

Fig. 8. Normalized total bias  B r   obtained from the four-reference example.

those closed-form closed-form formulas formulas in approximati approximating ng the relationship relationship between the input and output errors, the results calculated from Eqs. (22)–(25) are compared with those obtained from numerical simulations. In the numerical simulations, to evaluate the impact of  assume me that that the the comp compon onen ents ts of  δpi are δpi on δp0 , we assu uncorre uncorrelat lated ed from from one anothe anotherr and follo follow w the Gaussi Gaussian an dist distrib ribut utio ion n with with the the zero zero mean mean and and the the same same stan standa dard rd deviation  σ p . Given the correct position of the target object p0 , the correct positions of the reference points pi   and the correct distances between the object and reference points ri , we generate a number of erroneous pi  according to the random distribution of  δ  δ pi , substitute them into Algorithm 1 to calculate corresponding erroneous p erroneous  p0, obtain the statistics of  δp0 , and then calcul calculate ate Bp and S p   from E(δp0 ) and var(δp0 ) respectively. To evaluate the impact of  δri on δp0 , we assume that δri   are uncorrelated from one another and follow the Gaussian distribution with the zero mean and the same standard deviation σr . Given the correct p0 , pi and ri , we generate a number of erroneous ri   according to the random distribution of  δ ri , substitute them into Algorithm 1

http://journals.cambridge.org

Downloaded: 20 Feb 2014

383

to calculate corresponding erroneous p erroneous  p0, obtain the statistics of δp0 , and then then calcul calculate ate Br  and S r  from  from E(δp0 ) and and var( ar(δp0 ) respectively. Differe Different nt from from the closed closed-fo -form rm calcula calculatio tion n using using Eqs. Eqs.  σ p and  σ r , the numerical (22)–(25) which is independent of  σ numerical simulation must be carried out based on specified σp and σr . We set σp and σr  with different values ( σp, σr {10, 20, 30, 40, 50, 60, 70, 80, 90, 100 }), run the simulation with 10,000 samples for each value, and calculate Bp , S p , Br , and S r  across the data acquisition regions. It turns out that the resulting Bp , S p , Br , and S r   are consistent across the range of  σp and σr , which verifies that Bp ,  S p ,  B r , and  S r are largely independent of  σp and σr , as indicated in Eqs. (22)–(25). The comparison between the  B p ,  S p ,  B r , and  S r  obtained from Eqs. (22)–(25) and those obtained from the numerical simula simulatio tions ns verifie verifiess that that Eqs. Eqs. (22)–( (22)–(25) 25) provid providee a good good closed-form approximation of the effect of  δpi and δri on δp0  (Table V).

 ∈

4. Experimental Experimental Test Test The effectiveness of the proposed trilateration algorithm has also been verified through an experiment of trilaterating an onboard onboard stereo stereovis vision ion unit. unit. Focus Focusing ing on testin testing g the estima estimatio tion n error under the combined effect of realistic input errors and the execution speed of the proposed trilateration trilateration algorithm, algorithm, we used used the stereo stereovis vision ion system system only only for obtain obtaining ing the distan distance ce measur measureme ements nts between between the unit unit and referen reference ce points. The measured distances were input to the proposed trilateration algorithm to position the unit in 3D space. The experiment system consisted of a reference pattern and an onboard stereovision unit: 1. Two Two referenc referencee patter patterns ns were used used for three-r three-refer eferenc encee and four-reference trilateration respectively, where bright LEDs were adopted to represent reference points. The three-reference three-reference pattern pattern contains contains three symmetric symmetric reference reference points which define an equilateral base triangle, while the four-reference pattern contains four symmetric reference points which define a base square, both inscribed in a circle with a radius of 550 mm. Since they were built in the same way, we only show the four-reference pattern here (Fig. 9a). For either three-reference or four-reference trilateration, the corresponding pattern was hung under

Fig. 9. Reference pattern used in the experiment.

IP address: 131.155.2.68

A closed-form algorithm for the least-squares trilateration problem

384

Table V. Comparison between  B p ,  S p ,  B r , and  S r  calculated from Eqs. (22)–(25) and those obtained from numerical simulations. p0

(0, 0, 8000)

( 4000 4000,, 4000 4000,, 8000 8000))

( 2000, 2000, 1600, 1600, 8000) 8000)

(2000, (2000, 3200, 3200, 8000) 8000)

0.0055 0.0057 9.3277 9.3609 0.0054 0.0054 9.3277 9.3059 0.0070 0.0077 8.0780 8.0725 0.0040 0.0040 8.0780 8.1271

0.0102 0.0107 12.7608 12.8730 0.0101 0.0111 12.7608 12.7783 0.0085 0.0090 11.0059 11.0442 0.0075 0.0077 11.0059 11.0947

0.0064 0.0066 10.0348 10.0896 0.0062 0.0066 10.0348 10.0170 0.0073 0.0078 8.6836 8.6908 0.0047 0.0047 8.6836 8.7389

0.0074 0.0078 10.8435 10.8947 0.0073 0.0076 10.8435 10.8358 0.0083 0.0087 9.3884 9.4088 0.0054 0.0057 9.3884 9.4609

 Bp Bp,sim S p S p,sim p ,sim Br Br,sim S r S r,sim r,sim Bp Bp,sim S p S p,sim p ,sim Br Br,sim S r S r,sim r,sim

−

−

The closed-form and simulation results of  B p ,  S p ,  B r , and  S r  are compared at 4 representative  p 0 . The subscript “sim” denotes the results from the numerical simulations. The presented  B p,sim  and S p,sim 70, while the presented  B r,sim  and  S r,sim 70. p ,sim  are obtained with  σ p r,sim  are obtained with  σ r

 =

 =

the ceilin ceiling g of an indoor indoor experi experimen mentt envir environm onment ent with the base plane parallel to the flat floor. We used the base plan planee as the the XY plan planee of the the glob global al frame frame of refer referen ence ce,, and and the center of the base triangle or square as the origin of  the frame. 2. A stereo stereovis vision ion unit, unit, consis consistin ting g of two Matrix Matrix Vision ision BlueFox USB 640   480 CCD cameras with Kowa 12 mm lenses, was installed on an Activmedia Pioneer3-DX mobile robot (Fig. 10). This stereovision set is supported by a Direc Directe ted d Perce Percept ptio ion n PTU-D PTU-D46 46-1 -17 7 cont contro roll llab able le pan/tilt unit which enables the stereovision unit to orient towar toward d the referen reference ce pattern pattern.. With the mobile mobile robot robot moving in the flat planar indoor environment, the cameras were were kept kept on a plan planee paral paralle lell to the base base plan planee ( XY  plane of the global frame) at a distance of 1600 mm. The apertu apertures res of both both the cameras cameras were intent intention ionally ally minimi minimized zed such such that, that, with with regula regularr ambien ambientt lighti lighting, ng, the the LEDs LEDs were were show shown n as brig bright ht spot spotss outs outsta tand ndin ing g from from the the dark background in the images (Fig. 9b), which provide conveni convenience ence for segmentin segmenting g the reference reference points points from the images.

 ×

The cameras cameras were calibra calibrated ted using using a camera camera calibra calibratio tion n toolbox toolbox for Matlab Matlab availabl availablee at http://www http://www.visio .vision.calt n.caltech. ech. edu/bougue edu/bouguetj/cal tj/calib ib doc/ and a printed printed black/white black/white checkerboa erboard rd patt pattern ern to find find out out the the intr intrin insi sicc param paramet eter erss of  the the came camera ras, s, such such as foca focall leng length th,, imag imagee cent center er and and distor distortio tion n coeffi coefficie cients nts,, and the transfo transforma rmatio tion n between between the the two two came cameras ras.. The The cali calibr brati ation on resul results ts are are list listed ed as follows: 1. For the left camera, we have the focal length (pixels) f  (pixels)  f  [1623.4699; 1626.0442] [1.129 [1.1299; 9; 1.1164 1.1164], ], image image center center (pixels) c   [329.6443; [329.6443; 248.5307] [1.9019; [1.9019; 1.6757], 1.6757],

=

http://journals.cambridge.org

±

 =  =

±

Downloaded: 20 Feb 2014

Fig. 10. Onboard experimental system.

and distortion coefficients k coefficients k [ 0.3546; 0.3416; 0.0013; 0.0020] [0.0115; 0.1950; 0.00019; 0.00018]. 2. For the right camera, we have the focal length (pixels) f  (pixels) f  [1627.7598; 1629.5029] [1.147 [1.1475; 5; 1.1091 1.1091], ], image image center center (pixels) c   [334.0203; [334.0203; 246.5809] [1.9384; [1.9384; 1.5751], 1.5751], and distortion coefficients k coefficients k [ 0.3364; 0.2858; 0.0006; 0.0011] [0.0094; 0.1669; 0.00031; 0.00014]. 3. For the transformation between the two cameras, we have the rotation rotation vector vector (radians) (radians) vR [ 0.0129; 0.0129; 0.2389; 0.2389; 0. 0016] 0016] [0.004 [0.0045; 5; 0.0064 0.0064;; 0.0004 0.0004]] from which which the corresponding rotation matrix can be calculated using the Rodrigues’ rotation formula, and translation vector (mm) [ 289.0918; 3.2027; 32.6389] [0.1915; 0.1109; vT  1.3917].

=−

±

= ±

− −

±

= −

=

±

=−

±

= − ±

In each of the above above calibra calibratio tion n results results,, the numbers numbers in the first “[]” are the calibrated values, while the numbers in the the seco second nd “[]” “[]” are the the nume numeric rical al error errorss whic which, h, as

IP address: 131.155.2.68

 A closed-form algorithm for the least-squares trilateration problem Table VI. Camera position estimation errors. Thre Threee-re refe fere renc ncee case case

Four Four-r -ref efer eren ence ce case case

 Rh

 E (δp0 )| | E 

|STD(δp0 )|

 E (δp0 )| | E 

|STD(δp0 )|

2000 2500 3000 3500 4000

2.6391 11.8517 14.7132 20.4381 28.3571

20.7778 24.9709 28.2164 32.3926 49.2009

1.7574 11.3892 13.4792 18.2349 26.1342

14.8373 20.1474 25.2913 27.7020 46.9997

In this table,  E (.) (.) denotes mean, STD(.) denotes standard deviation, |.| |.| deno denote tess the the norm norm of the the vect vector or,, and and the the unit unit of leng length thss is millimeter.

indicated by the toolbox, are approximately three times the standard deviations. Since the transformation between the two cameras is referenced to the left camera, the position of  the stereovision unit is represented by the position of the left camera. For data acquisition, we moved the mobile robot to a few differe different nt distan distances ces from from the referen reference ce patter pattern n (repres (represent ented ed by the horizontal horizontal distance distance Rh from  from the Z -axis - axis of the the glob global al frame frame which passes through the center of the base triangle/square), and at each distance we acquired data from a number of   Rh . locations on the circle about the Z -axis -axis with the radius of  R At each location, the stereovision unit was oriented toward the the refer referen ence ce patt patter ern, n, a pair pair of imag images es of the the refer referen ence ce patt pattern ern was taken by the stereovision unit, and the actual position of  the the left left camera camera was was meas measur ured ed manu manual ally ly.. The The data data were were stor stored ed in an onboard laptop computer. The The coll collec ecte ted d imag images es were were proc proces esse sed d after after the the data data acquisition acquisition process. Focusing Focusing on evaluati evaluating ng the proposed proposed trilateration algorithm, we segmented the reference points from from each each pair pair of images images manual manually ly (after (after correct correcting ing the image image distor distortio tions) ns),, ran a stereo stereovis vision ion progra program m to estima estimate te the distances from the left camera to the reference points based on the camera calibration calibration results, and then implemented implemented the proposed proposed trilateration trilateration algorithm algorithm to estimate estimate the 3D position position of the left camera. camera. The estimatio estimation n error error was calculate calculated d by compar comparing ing the estima estimated ted positi position on with the measur measured ed one, which was affected by all the above-mentioned input errors, including the reference positioning errors, distance measur measureme ement nt errors errors (caused (caused by camera camera calibra calibratio tion n errors errors and image segmentation errors), as well as the manual camera position measurement errors. As the the resul result, t, we obtai obtaine ned d a profi profile le of the the posi positi tion on estimation error under the experimental settings (Table VI). We noti notice ce that that,, even with with the the abo above less less accu accura rate te measurements, the experimental system achieved an average 3D positioning accuracy of  <  <30 mm in both three-reference and four-reference cases at an observation distance (between the the came camera rass and and patt patter ern) n) of 4.3 4.3 m (cor (corre resp spon ondi ding ng to  Rh 4 m), m), whic which, h, as we belie believe ve,, is suffi sufficie cient nt for for many many mobi mobile le robot robot applica applicatio tions. ns. The result resultss also also show show some some impro improvem vement entss in both both the estima estimatio tion n accurac accuracy y and uncerta uncertaint inty y by using using more more reference points (four reference points versus three reference points in our experiment).

385

Moreo Moreover ver,, due to its closed closed-for -form m nature nature,, the propos proposed ed trilateration algorithm can be implemented at a high speed in practice. We tested both a Matlab program and a compiled standalone executable (exe file) of the algorithm respectively on a lapt lapto op PC with with a 1.66 .66 G Hz Inte Intell Core Core 2 CPU. CPU. Given the reference positions and distances, for the threereference trilateration, the Matlab version of the proposed trilateration trilateration algorithm algorithm takes 3.8310 3.8310 10−4 s in avera average, ge, while while the standalone version takes 3.7006 10−4 s in average; for the four-referen four-reference ce trilateration trilateration,, the Matlab program takes − 4 3.9732 10 s in average, while the standalone executable takes 3.9333 10−4 s. It means that the proposed trilateration algori algorithm thm is highly highly applic applicabl ablee to real-ti real-time me applica applicatio tions. ns. More Moreov over er,, requi requiri ring ng only only the the refere referenc ncee posi positi tion onss and and corres correspon pondin ding g distan distance ce measur measureme ements nts as the input, input, the proposed algorithm is ready to be integrated into various ranging-based trilateration systems with different ways of  distance measurement, for instance, those systems based on ultrasound6,10,30,31 and radio frequency signals.7,8,11

× ×

×

×

5. Conclusio Conclusion n This This pape paperr pres presen ents ts a nove novell close closedd-fo form rm tril trilate atera rati tion on algorithm which estimates the position of a target object, such as a mobile robot, based on the simultaneous distance measurements from multiple reference points. Solving the nonlinear least-squares trilateration problem, the proposed algorithm provides a near optimal position estimate of the intersection point of  N  n   spheres in Rn (n   2 for 2D environments and n  3 for 3D environments), not limited to solving for the intersection intersection points of exactly exactly n  spheres in n R . Using standard linear algebra techniques, the proposed algori algorithm thm has low low comput computati ationa onall comple complexit xity y. Witho Without ut depending on the techniques which tend to be affected by algebraic algebraic singularitie singularities, s, such as matrix inversion inversion,, the proposed proposed algorithm has high operational robustness. The performance analysis shows that the algorithm is highly effective, with lower lower system systemati aticc bias bias and estima estimatio tion n uncerta uncertaint inty y than than repres represent entati ative ve closed closed-fo -form rm method methods, s, when when dealin dealing g with with erroneous erroneous inputs of distance distance measurement measurementss and reference points. The results have also verified that introducing more reference points and corresponding distance measurements into into the trilate trilaterat ration ion proces processs will in general general reduce the estimation uncertainty. The experimental test shows further that the proposed trilateration algorithm is highly applicable to real-time applications. Though targeting the applications in mobile robotics, the proposed trilateration algorithm is readily applicable to any ranging-based object localization tasks in various environments and scenarios.

 =

 ≥  ≥

 =

=

http://journals.cambridge.org

Downloaded: 20 Feb 2014

Appendix In this appendix, we provide the differential terms of  p0 relevant to Eqs. (22)–(25), including

∂ p0 ∂ p0 ∂ 2 p0 , ∂r i , T  2 ∂ pT  ∂ pi i

and

IP address: 131.155.2.68

∂ 2 p0 . ∂r i2

A closed-form algorithm for the least-squares trilateration problem

386 ∂ p0

∂q

∂c

∂ pT  i

∂ pT  i

∂ pT  i

∂ qk

1

∂ pT  i

det(H det(H )

  = +           +   = + + −  ∀  ∈ { − } √      − ± − = − ± √  − − −  −          = +            = +          = − −           =           =           =    =  ⊗ − + −  = + ⊗ + +  ⊗   +         = ⊗ + − =− ⊗ + + ⊗  =   =  −   ,

f 

∂  det Hkh

∂  det Hk ∂ pT  i

qn

∂ pT  i

det

f 

det Hkh qn ∂  det(H  det(H )

det Hk

∂ qn

Hkh

∂ pT  i

k

∂ qn

1

∂b

∂ pT  i

2a

∂ pT  i

1

b2

n 1

∂a

−

2

∂ pT  i

det

Hkh

det

Hkh

det

f  Hk

k 1 n 1

∂b

= −

2

∂ pT  i

= −

2

∂ pT  i

∂ pT  i

∂  det Hk

σ

∈P 

ss ss

∂  det Hk

T 

2q

∂ pT  i

−

l, σ(l )

σ

∈P 

ss ss

f 

σ

∂H

2

∂ pT  i

N 

∂ f 

∂a

∂ pT  i

∂ pT  i

∂a

N 

∂c

1

∂ pT  i T 

N 

∂ (q q)

2

∂ pT  i

N 

The summation

∈

(In ∂ pT  i

1

N 

2

B

2pi pT  i

cT  c

∂B

2

∂ pT  i

N 

N 

ri2 ,

∂ p0 ∂r i

∂q

= ∂r

,

i

 = −   =    =

cT 

σ

∈P 

ss ss

in

h ∂ det(H ) ∂ det(Hk ) , T  T  ∂ pi ∂ pi

det(H )

∂r i

∂r i

k 1

=

f 

∂  det Hk ∂ ri

∂r i

2a

∂ ri

1

b2

f 

det Hk

∈P 

ss ss

4ac

∂  det

f  Hk

∂ ri

b

∂b

∂r i

∂ det(Hk ) ∂ pT  i

N 

ccT  , pT  i

In

pi vec(In )T 

is

pT  i

In ,

of  SS . sgn(σ) denotes the signature of the permutation σ, where sgn(σ) 1 if  σ  σ  is an even permutation, and sgn( σ) 1 if it is odd.

f 

−

l 1

http://journals.cambridge.org

=

2a

f  H k l, σ(l )

l SS,l l 

∈

=

∂c

∂r i

=−

∂ qn ∂r i

∂b

,

∂r i

, k

1, . . . , n

f 

n 1

2

−

1 ,

det Hkh

k 1

=

∂ det Hk ∂r i

,

T 

∂ (q q)

∂H k l  ,σ(l  ) ∂r i

=+

   ∀  ∈ { − }  =       

det Hkh

det(H )2

n 1

sgn(σ)

σ

f 

∂  det Hk

∂r i

 2

(A1)

f 

and

   = +  ± √  − −     −   1

∂b

∂c

,

pT  i .

∂ qk

1

−

∂ pT  i

In ,

∂q n

n 1

4

In

computed over all the permutations of the set of numbers SS   { 1,2,. . .,n}.  P ss ss  denotes the set of all n! permutations

 =  =

q)

pi )vec(In )T  ,

(c

c)

pT  i pi

In

=

pT  i

∂B

1

∂ pT  i

cT 

In

T 

∂ (q det(H det(H )2

,

∂ pT  i

l SS,l l 

=

ss ss

,

,

∂ H k l  ,σ(l  )

f  H k l, σ(l )

l 1

∈P 

∂ pT  i

2a 2

f 

−

sgn(σ)

∂ pT  i

4ac ∂ a

,

∂ pT  i

 l l ∈SS,l =

n 1

∂  det Hk

∂ pT  i

∂ pT  i

∂ H khl  ,σ(l  )

H khl, σ(l )

l  =1

b2

b

∂  det(H  det(H )  q det(H det(H )

∂ pT  i

 l l ∈SS,l =

−

sgn(σ)

∂ pT  i

∂c

,

∂ pT  i

∂ H l ,σ(l  )

H 

n 1

∂  det Hkh

∂ pT  i

∂  det Hkh

f  Hk

det

∂ pT  i

l  =1

2a

1 ,

∂ pT  i

n 1

sgn(σ)

∂a

1, . . . , n

,

∂  det(H  det(H )  , 2 det( det(H H)

f 

=

∂ pT  i

2c

∂ pT  i

∂  det Hkh

k 1

 det(H ) ∂  det(H

4ac

∂b

f 

k 1 n 1

∂c

b

∂ pT  i

det(H det(H )2

∂ ri

 

,

∂ f  ∂r i

T 

∂B ∂a ∂B ∂ (q q) 2 2 2 c, = ∂∂ra + ∂r = − rp, = rI , = r. ∂r N  ∂r N  ∂r N 

Downloaded: 20 Feb 2014

i

i

i

i

i

i n

i

i

i

IP address: 131.155.2.68

      

(A2)

 A closed-form algorithm for the least-squares trilateration problem

∂ 2 p0 ∂ pT  i

2

=

∂ 2q ∂ pT  i

2

387

 ,

              + = +  ⊗ + ⊗ +             +    − − + +  ⊗            + − + +  +     ∀  ∈ { − } + ⊗         = −  ± √  ⊗ +  − ⊗ −  − ⊗ − −       ∓ √  − − ⊗ − − − √      − ± − − ⊗ − ± √  − − − −     √    − ± − + − − ± √  − − + ⊗ −              ⊗ +    = ⊗ + +                      ⊗ + ⊗ + = +             ⊗   ⊗ + − = +   − − ⊗ −            ⊗ = +                 ⊗ = +                 ⊗ = +        = − { ⊗ ⊗ + ⊗ } = − f 

∂ 2 det Hk

2

∂ qk

1

2 ∂ pT  i

det( H )

∂q n

In

∂ pT  i

2 ∂ pT  i

1

∂ det(H )

det(H )2

∂ pT  i

f 

In

∂ pT  i

f 

2

∂ 2 qn

∂ pT  i

2

det Hkh qn ∂ det(H )

det Hk

1

∂2b

2a

∂ pT  i

1

2

4ac

∂a

1

2 ∂ pT  i

∂ pT  i

∂  det Hkh

n 1

−

 2

2

k 1

=

=

2qT  q det(H )

sgn(σ)

2

σ

∈P 

ss ss

2

σ

ss ss

f 

σ

∂ 2H

∂ 2B 2 ∂ pT  i

ss ss

2

2 ∂ pT  i

2 ∂ pT  i

2 det( det(H )

∂  det(H )

H l, σ(l )

 l l  ∈SS,l  =

 1

−

= =

∂2a 2 ∂ pT  i

2 N 

 +

f 

H k l, σ(l )

pT  i



2 ∂ pT  i

 [In

In ](In

⊗ (I ⊗ c)] + N 1 ∂∂pB U n

T  i

= − N 2 {U

n

n,1 n,1 [vec(In )

http://journals.cambridge.org

T 

n,n (In

T  i

T 

⊗ I + I ⊗ p + p vec(I ) n

Un,n )

i

n



n

n,n

b2

f  Hk

∂ pT  i

2

∂ (qT  q)

b2

4ac ∂ 2 a

2a 2

∂ pT  i

∂ pT  i

∂ pT  i

In

∂ pT  i

vec(In )T  ,

2 det( det(H )

n

∂ pT  i

∂ (qT  q)

qT  q

∂ pT  i

2

∂ det(H )

H l, σ(l )

In

∂ pT  i

∂ 2 H l ,σ(l  ) ∂ pT  i

 l l ∈SS,l =

H khl, σ(l )

∂ pT  i

∂ 2 H khl  ,σ(l  ) ∂ pT  i

2

n

T 

Downloaded: 20 Feb 2014

n

∂ pT  i

∂ 2 H k l  ,σ(l  ) ∂ pT  i

 l l∈SS,l =

∂ 2 (qT  q)

2

 1

2 ∂ pT  i

N 

N 

T 

2

2

n

n

T 

,

∂ pT  i

,

,

n

T 

2

∂ det(H )

1 vec(In )T  ,

T 

n

∂ pT  i

f 

f 

H k l, σ(l )

2



,

2

 l l∈SS,l =

∂H k l  ,σ(l )

T  i

∂ pT  i

,

∂H khl  ,σ(l )

1,n

2

∂ 2 det Hkh

f 

⊗ I )(I ⊗ U ) + N 1 ∂∂pB + N 4 [c ⊗ I + I ⊗ c + cvec(I ) n

∂ pT  i

∂ 2 det(H )

det Hk

f 

∂H k l  ,σ(l )

∂2c

,

∂ pT  i

∂  det Hkh

In

 ∂ 2 (qT  q)

∂ pT  i

In

∂ pT  i

∂a

In

∂ pT  i

∂H l ,σ(l  )

In

2a

2

∂ det(H )

In

2 det( H )

det( H )2

∂c

In

∂ pT  i

∂ pT  i

∂ pT  i

∂ det

∂a

4ac ∂a

∂  det(H )

2

2

2

a3

,

n

∂ pT  i

∂ pT  i

⊗ I ](I ⊗ U ) + I ⊗ vec(I ) + vec(I ) ⊗ I }. n

∂2a

b

2 ∂ pT  i

In

2

∂ pT  i

b

f  Hk

∂ pT  i

 l  ,l= l  l ∈SS,l =

∂ pT  i

∂c

∂c

∂ pT  i

f 

−

∂ 2B

2a

∂ pT  i

∂c

∂H khl  ,σ(l )

 l  ,l= l  l ∈SS,l =

 l l  ∈SS,l  =

∂a

∂ 2 det Hk

∂ pT  i

H khl, σ(l )

T  1 Un,1 n,1 [vec(In )

N  N 

2a

∂ pT  i

∂H l ,σ(l  )

 l  ,l= l  l ∈SS,l =

 l l  ∈SS,l  =

∂a

∂ pT  i

In

∂ pT  i

−

l  =1

∈P 

∂ 2a

det

∂ pT  i

2

2c

2c

∂ pT  i

2 ∂ pT  i

f  Hk

2

1,

f 

∂  det Hk

n 1

sgn(σ)

2

2 ∂ pT  i

In

∂ pT  i

l  =1

∈P 

∂ 2 f 

∂ pT  i

∂ pT  i

 

det Hkh qn ∂ 2 det(H )

det(H )2

∂a

In

∂ pT  i

∂ pT  i

∂ 2 det

det Hkh

∂ pT  i

1, . . . , n

∂ 2 det Hkh

det Hkh

f  Hk

∂c

∂b

2a

∂ pT  i

n 1

sgn(σ)

∂ 2 det Hk ∂ pT  i

∂a

2c

∂ pT  i

∂ pT  i

∂ 2 det(H )

l  =1

∂ 2 det Hkh ∂ pT  i

4ac

∂b

2

2c

∂ pT  i

n 1

∂ 2 det(H ) ∂ pT  i

b

∂b

det Hkh

∂ pT  i

det Hk

∂q n

Hkh

k

2

b

f 

∂ pT  i

k 1

b

4ac

∂ pT  i

In

∂ pT  i

∂ 2 qn

∂ pT  i

∂ 2b

In

∂ pT  i

∂  det Hkh

f 

−

 2

2 ∂ pT  i

b2

∂  det

∂ det Hk

n 1

∂2c

b2

In

∂ pT  i

∂c

1

In

∂ pT  i

∂ pT  i

2a

∂ pT  i

1

∂ det Hkh

=

∂2b

∂ pT  i

∂b

k 1

∂a

∂ pT  i

2a2

−

2c

∂ pT  i

∂b

1

n 1

 2

∂b

In

2a 2 ∂ pT  i

∂2a

b

3

b

det

,

∂ pT  i

∂b

In

T  4ac ∂ pi

b2

1

b2

∂b

∂  det(H )

In

∂ pT  i

det(H )3

∂q n

∂q n

det Hkh

qn

∂ pT  i

∂  det Hkh

2 ∂ pT  i

qn

∂ pT  i

∂ det Hkh

∂ 2 det Hkh

f 

∂ det Hkh

∂ pT  i

f 

det(H )2

qn

∂ pT  i

∂  det Hk

∂  det Hk

1

∂  det Hkh

                                              

],

IP address: 131.155.2.68

,

(A3)

A closed-form algorithm for the least-squares trilateration problem

388 k

(k l )

l

×

(k l )

(k l )

×

×

Here Uk,l [Eij  ]T  , where Eij  i =1 j =1  Eij  is the  k l  matrix with the (i,j) element as 1 and all the other elements as zero.

×

 =

 

∂ 2 p0 ∂ ri2

2 i

, f 

      = + ∀  ∈ { − }       = − ± √  + − ∓ √  − − −        =            − = +              = +    ∂ 2 det Hk

1

∂ ri2

det(H det(H ) 1

∂ 2b

∂ ri2

2a

∂r i2

∂ 2c ∂r i2

f  det Hk ∂ ri2

∂ 2 f 

∂r i2

 2

b2

 2

∂r i

4ac

∂ 2 qn

,

∂r i2 b

1, . . . , n

k

∂ 2b

2a

∂ ri2

det

∂ 2c

f  2

∂ ri2

∂ 2 det Hk

f  Hk

det H

∂ ri2

n 1

sgn σ

−

l  =1

∈P 

ss ss

2

= ∂∂ ra + ∂∂rB c, 2 i

2 i

f 

H k l, σ(l )

 l l  ∈SS,l  =

∂ 2a ∂ ri2

 l ,l= l l ∈SS,l =

= − N 2 p , i

∂ 2B ∂ ri2

= N 2 I ,

Acknowledgments Special thanks are given to Mr. Xionghui Lu and Mr. Xu Zhong for their assistance in the process of experimental data acquisition. References 1. J. Borenstein, Borenstein, H. R. Everett, Everett, L. Feng and D. Wehe, Wehe, “Mobile robot positioning positioning:: Sensors Sensors and techniques, techniques,””   J. Robot. Robot. Syst. Syst. 14(4), 14(4), 231–249 (1997). 2. “Multil “Multilate aterat ration ion,,” Wikim Wikimedi ediaa Founda Foundatio tion, n, Inc. Inc. [Onlin [Online]. e]. Available http://en.wikipedia.org/wiki/Multila http://en.wikipedia.org/wiki/Multilateration. teration. 3. A. El-Rabbany El-Rabbany,,  Introduction to GPS: The Global Positioning System  (Artech House, Norwood, MA, 2002). 4. V. Ashkenazi, Ashkenazi, D. Park and M. Dumville, Dumville, “Robot positioning positioning  27 (6), and the global navigation satellite system,”  Ind. Robot  27(6), 419–426 (2000). 5. F. Folster and H. Rohling, Rohling, “Data association association and tracking for automotive radar networks,”  IEEE Trans. Intell. Transp. Syst. 6(4), 370–377 (2005). 6. A. Ward, Ward, A. Jones and A. Hopper, “A “A new location technique for the activ activee office office,,”  IEEE Pers. Commun. 4(5), (5), 42–47 42–47 (1997) (1997).. 7. P. Bahl and V. V. N. Padmanabhan, “User Location and Tracking in an In-b In-bui uild ldin ing g Radi Radio o Netw Networ ork, k,””   Microsoft Microsoft Research Research Technical Report MSR-TR-99-12 , Redmond, WA, Microsoft Corporation (1999). 8. J. High Highto towe werr, G. Borr Borrie iell llo o and and R. Want, ant, “Spo “SpotO tON: N: An Indoor 3D Location Sensing Technology Based on RF Signal Strength,”  UW CSE Technical Report 2000-02-02  (University of Washington, Seattle, WA, 2000). 9. N. B. Priy Priyan anth tha, a, A. Chak Chakra rabo bort rty y and and H. Bala Balakr kris ishn hnan an,, “The Cricket Cricket Location-Su Location-Support pport System, System,””   Sixth ACM/IEEE  ACM/IEEE   International Conference on Mobile Computing and   Networking, Boston, MA (2000), pp. 32–43. 10. A. Harter, Harter, A. Hopper, Hopper, P. P. Steggles, Steggles, A. Ward and P. Webster, ebster, “The anatomy of a context-aware application,”  Wirel. Netw.  1,  1, 1–16 (2001). 11. F. F. Ahmad Ahmad andM. G. Amin, Amin, “Nonco “Noncoher herent ent approa approach ch to throug throughhTrans. Aerosp. Aerosp. Electron Electron.. the-wall radar localization,” localization,”   IEEE Trans. Syst.  42(4),  42(4), 1405–1419 (2006).

http://journals.cambridge.org

b2

f 

det

∂r i

=

2

1

4ac

3

b

∂b

∂r i

2a

2

∂c

∂ ri

,

,

∂ ri2

∂  det Hk

k 1

σ

1 ,

f 

∂ 2 det Hk

Hkh

k 1

= n−1

∂b 2

1

n 1

−

det Hkh

∂r i2

∂ 2 qn

∂ ri2

∂

= ∂r

∂ 2 qk

∂ 2b

2

∂ 2q

⊗

Downloaded: 20 Feb 2014

n

2∂

2

(qT  q)

∂r i2

,

f  ∂ H k l  ,σ(l  )

f  ∂ H k l  ,σ(l  )

∂ ri

∂ ri

∂ 2 qT  q ∂r i2

= N 2 .

f 

∂ 2 H 

f 

H k l, σ(l )

 l l ∈SS,l =

                  

k l  ,σ l 

∂ ri2

(A4)

,

12. Y. Zhou, W. Liu and P. P. Huang, “Laser-Activated RFID-Based Indoor Localization System for Mobile Robots,”  Proceedings of 2007 2007 IEEE IEEE Intern Internati ationa onall Confer Conferenc encee on Roboti Robotics cs and   Automation, Roma, Italy (2007) pp. 4600–4605. 13. D. E. Manolakis, “Efficient solution and performance analysis of 3-D positi position on estima estimatio tion n by trilat trilatera eratio tion, n,””   IEEE Trans. Trans.  Aerosp. Electron. Syst.  32(4), (4), 1239–1248 (1996).  32 14. B. Fang, Fang, “Trilater “Trilateration ation and extension extension to global global positionin positioning g system navigation,” navigation,”  J. Guid.  9(6),  9(6), 715–717 (1986). 15. 15. J. Zieg Zieger ertt and and C. D. Mize Mize,, “The “The lase laserr ball ball bar: bar: a new new inst instru rume ment nt Precis. Eng. 16(4) for machin machinee tool tool metrol metrology ogy,,”   Precis. 259– 16(4),, 259– 267 (1994). 16. S. K. Rao, “Comments on “efficient solution and performance analysis analysis of 3-D position position estimation estimation by trilaterati trilateration, on,””   IEEE  Trans. Aerosp. Electron. Syst.  34(2),  34(2), 681 (1998). 17. F. Thomas Thomas and L. Ros, Ros, “Revis “Revisiti iting ng trilat trilatera eratio tion n for robot robot  21(1), 93–101 (2005). localization,”  IEEE Trans. Robot.  21(1), 18. I. D. Coope, Coope, “Relia “Reliable ble comput computati ation on of the points points of inters intersect ection ion 42(E), of  n   spheres in R n ,”   Aust. N.Z. Ind. Appl. Math. J. 42(E), C461–C477 (2000). 19. W. H. Foy Foy, “Posit “Position ion-lo -locat cation ion soluti solutions ons by Taylor aylor-se -serie riess estimation,”   IEEE Trans. Aerosp. Electron. Syst.  AES-12(2),  AES-12(2), 187–194 (1976). 20. W. Navidi, Navidi, W. S. Murphy Murphy Jr. Jr. and W. Hereman, “Statistical “Statistical methods methods in surveying surveying by trilaterati trilateration, on,””   Comput. Stat. Data  Anal.  27,  27 , 209–217 (1998). 21. W. C. Hu andW.H. Tang, ang, “Automa Automatedleast tedleast-sq -squar uares es adjust adjustmen mentt of triangulation-trilateration figures,”  J. Surv. Eng.   133–142 (2001). 22. 22. M. M. Pent Pent,, M. A. Spir Spirit ito o and and E. Turc urco, “Me “Method thod for posi positi tion onin ing g GSM GSM mobi mobile le stat statio ions ns usin using g abso absolu lute te time time Electron. Lett. 33(24), delay measurement measurements, s,””   Electron. 2019–2020 33(24), 2019–2020 (1997). 23. 23. J. C.Spall, C.Spall, Introduction to Stochastic Search and Optimization: Hoboken, n, NJ (John (John Wile Wiley y  Estimation, Simulation, and Control. Hoboke & Sons, 2005). 24. S. Kirkpatrick, Kirkpatrick, C. D. Gelatt and M. P. Vecchi Vecchi,, “Optimization “Optimization by simula simulated ted anneal annealing ing,,”   Science 220(45 98),, 671–68 671–680 0 220(4598) (1983).

IP address: 131.155.2.68

 A closed-form algorithm for the least-squares trilateration problem Introduc ductio tion n to Geneti Geneticc Algori Algorithm thmss for  25. 25. D. A. Cole Coley y,   An Intro Scientists and Engineers  (World Scientific, Singapore, 1999). 26. Y. Zhou, “An efficient Least-Squares Trilateration Trilateration Algorithm Procee eedi ding ngss of 2009 2009 for Mobile Mobile Robot Robot Locali Localizat zation ion,,”   Proc  IEEE/RSJ International Conference on Intelligent Robots and  Systems  (St. Louis, MO, 2009). 27. G. H. Golub and C. F. Van Van Loan,   Matrix Computations , 3rd ed. (The Johns Hopkins University University Press, Baltimore, Baltimore, MD, 1996). Engineering Mathematics Mathematics, 8th 28. E. Kreysz Kreyszig, ig,   Advanced Engineering 8th ed. ed. (John Wiley & Sons, New York, 1999).

http://journals.cambridge.org

Downloaded: 20 Feb 2014

389

Uncerta tain in Mode Models ls and and Robu Robust st Cont Contrrol 29. A. Weinman einmann, n,   Uncer (Springer-Verlag, (Springer-Verlag, Wien, New York, 1991). 30. J. Urena, Urena, A. Hernan Hernandez dez,, A. Jimene Jimenez, z, J. M. Villada illadango ngos, s, M. Mazo, azo, J. C. Garc Garciia, J. J. Garc Garcia ia,, F. J. Alvar lvarez ez,, C. De Marz Marzia iani ni,, M. C. Pere Perez, z, J. A. Jime Jimene nez, z, A. R. Jime Jimene nezz and and F. Seco Seco,, “Adva Advanc nced ed sens sensor oria iall syst system em for for an acoust acoustic ic LPS, LPS,”   Microprocess. 393–401 1 Microprocess. Microsyst. 31, 31, 393–40 (2007). 31. X. Cheng, H. Shu, Q. Liang and D. H. Du, “Silent “Silent positioning positioning Veh. in underwate underwaterr acoustic acoustic sensor sensor networks, networks,””   IEEE Trans. Veh. Technol.  57(3),  57(3), 1756–1766 (2008).

IP address: 131.155.2.68