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, IP address: 131.155.2.68 on 20 Feb 2014
http://journals.cambridge.org Downloaded: 20 Feb 2014 IP address: 131.155.2.68
Robotica (2011) volume 29, pp. 375389. 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 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
Trilateration is the most adopted external reference-
based localization technique for mobile robots, given
the correspondence of external references. The nonlinear
least-squares trilateration formulation provides an optimal
position estimate from a general number (greater than or
equal to the dimension of the environment) of reference
points and corresponding distance measurements. This paper
presents a novel closed-form solution to the nonlinear
least-squares trilateration problem. The performance of
the proposed algorithm in dealing with erroneous inputs
of reference points and distance measurements has been
analyzed through representative examples. The proposed
trilateration algorithm has low computational complexity,
high operational robustness, and reduced systematic error
and uncertainty in position estimation. The effectiveness of
the proposed algorithm has been further veried through an
experimental test.
KEYWORDS: Localization; Trilateration; Mobile robot.
1. Introduction
This paper presents a novel closed-form trilateration
algorithm, which facilitates the self-localization of autono-
mous mobile robots in two-dimensional (2D) and three-
dimensional (3D) environments.
1.1. Concept of trilateration
Trilateration refers to positioning an object based on
the measured distances between the object and multiple
reference points at known positions.
1
(People tend to call
it multilateration when more than three reference points
are used to position the object. However, multilateration
has been used to name another process of position estimation
based on the measured differences in the distances between
the object and three or more reference points.
2
) Given the
correspondence of external references, trilateration is the
most adopted external reference-based localization technique
for mobile robots. A number of trilateration systems can be
found in literature, such as those in refs. [312].
In principle, trilateration locates an object by solving a
system of equations in the form
(p
i
p
0
)
T
(p
i
p
0
) = r
2
i
, (1)
* Corresponding author. E-mail: yuzhou@notes.cc.sunysb.edu
where p
0
denotes the unknown position of the object, p
i
the
known position of the ith reference point, and r
i
the known
distance between p
0
and p
i
. Solving a system of Eq. (1) is
equivalent to nding the intersection point/points of a set of
circles in R
2
or spheres in R
3
:
(1) In R
2
, Eq. (1) denes a circle centered at the reference
point p
i
with a radius of r
i
. With two reference points, two
circles dened by Eq. (1) in general intersect at two points
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, 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 dened by Eq. (1) intersect
at one unique point where the object is located (Fig. 2).
(2) In R
3
, Eq. (1) denes a sphere centered at the reference
point p
i
with a radius of r
i
. With three reference points,
three spheres dened by Eq. (1) in general intersect at two
points symmetric to the base plane the plane determined
by the three reference points (Fig. 3). The object is located
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 point where the
object is located. With four or more reference points, four
or more spheres dened by Eq. (1) intersect at one unique
point where the object is located (Fig. 4).
Whenever obtaining two intersection points, one must decide
which of themthe object is actually located at. In many cases,
one of the two intersection points naturally distinguishes
itself as the only reasonable choice based on some simple
criteria, e.g., a mobile robot moving in an indoor environment
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 object, or even may not intersect at all. It is thus necessary
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)).
http://journals.cambridge.org Downloaded: 20 Feb 2014 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 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.
Fang
14
provided a closed-form solution for determining
the 3D navigation position of a moving object based on
the distance measurements from three reference points by
referencing the navigation position to the base plane dened
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
formulas, however, the positions of reference points must be
dened 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, Manolakis
13
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 xed by Rao.
16
Recently,
Thomas and Ros
17
proposed an alternative closed-form
solution for locating an object based on the simultaneous
distance measurements from three reference points, using
the formulation of CayleyMenger determinants which are
related to the geometry of the tetrahedra formed by the object
and three reference points. Closely related to trilateration,
Coope
18
presented a closed-form solution for determining
the intersection points of n spheres in R
n
based on Gaussian
elimination. Amore robust, though not closed-form, solution
based on the orthogonal decomposition was also presented
in ref. [18].
In general, closed-form solutions have low computational
complexity when the solution of Eq. (1) exists. They
also facilitate the theoretical analysis of the algorithm
performance. Closed-form expressions of the relationship
between the error in position estimation and that in distance
measurement can be found in refs. [13, 17]. However,
existing closed-form solutions do not accommodate 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 R
n
. They do not apply to determining the
intersection point of N > n spheres in R
n
, where small errors
in distance measuring and reference point mapping can 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
necessary to determine an optimal estimate which minimizes
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].
Foy
19
presented a numerical algorithmcalled Taylor-series
estimation which solves the simultaneous set of algebraic
position equations by iteratively improving an initial
guess with local linear least-sum-squared-error corrections.
Incorporating the distance measurement errors, Nadivi
et al.
20
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, the nonlinear least-squares method performs
the best. Hu and Tang
21
gave a geometric explanation
of the optimal result in least-squares-based trilateration,
which is the point of tangency between the hyperellipsoid,
determined by the standard deviation of the positioning
error, and the intersection of the hypersurfaces, determined
by the constraints among the measurements. Coope
18
also
suggested a nonlinear least-squares method to obtain the
approximate solution, which minimizes the sum of the
http://journals.cambridge.org Downloaded: 20 Feb 2014 IP address: 131.155.2.68
A closed-form algorithm for the least-squares trilateration problem 377
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
dened
a probabilistic model of the MS-BTS (base station) distance
measurement error and used the extended Kalman ltering
(EKF) to solve the trilateration problem, where the dynamic
system was the MS, and the observations were the distance
measurements.
In general, numerical methods are available to provide
an optimal estimation of the position of a target object, in
particular when no real solution exists for Eq. (1). Moreover,
numerical methods are in general not limited to dealing with n
spheres in R
n
. In fact, higher certainty in position estimation
is expected as the number of involved reference points
increases. However, compared with closed-form solutions,
numerical methods in general have higher computational
complexity, and closed-form performance analysis is in
general not available. Many numerical methods involve a
local optimization searching process, such as Newtons
method and the steepest descent method, which iteratively
improves an initial guess toward a converged position
estimate.
1820
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. Though global optimization 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 the various algorithm-specic parameters
and decision criteria cause inconvenience in autonomous
applications, and the relatively high time complexity of these
methods makes them not suitable for real-time applications.
1.3. Overview of the proposed trilateration algorithm
Built on and extending our recent work
26
(based on An
efcient least-squares trilateration algorithm for mobile
robot localization by Yu Zhou, which appeared in the
Proceedings of 2009 IEEE/RSJ International Conference on
Intelligent Robots and Systems. c 2009 IEEE), we propose
a novel closed-form trilateration algorithm which estimates
the position of a target object, e.g., a mobile robot,
based on the simultaneous distance measurements from
multiple reference points, by solving the nonlinear least-
squares 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:
(1) In the closed form and using standard linear algebra
techniques, the proposed trilateration algorithm has low
computational complexity and facilitates closed-form
performance analysis.
(2) Solving the least-squares trilateration problem, the
proposed algorithm provides an optimal or at least
near optimal estimate of the intersection point of
N n spheres constructed from erroneous distance
measurements and reference positions in R
n
(where n =
2 or 3), and is not limited to solving for the intersection
points of exactly n spheres in R
n
.
This paper focuses on the derivation and performance
analysis of the proposed trilateration algorithm. Section 2
will derive and explain the proposed trilateration algorithm
in detail. Section 3 will perform the error analysis 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 Trilateration Algorithm
2.1. Nonlinear least-squares formulation
The goal of the proposed trilateration algorithmis to estimate
the position of a target object based on the simultaneous
distance measurements from multiple reference points at
known positions. In order to obtain an optimal position
estimate of the target object from potentially inaccurate
distance measurements and reference positions, we target our
algorithm to solve the nonlinear least-squares trilateration
problem. That is, we dene an optimal approximation of the
object position in R
n
(n = 2 or 3 corresponding to a 2D or
3D environment, with the global frame of reference attached
to the environment) as
p
0
= arg min
p
0,est
S(p
0,est
), (2)
where S(p
0,est
) =

N
i=1
[(p
i
p
0,est
)
T
(p
i
p
0,est
) r
2
i
]
2
,
p
0,est
denotes an estimate of the object position p
0
, p
i
the
premapped position of the ith reference point, r
i
the measured
distance between p
0
and p
i
, and N the number of reference
points used to determine p
0
. Here, we are not constrained to
the case of N =n. Instead, we are going to give a solution to
the general case of N n (N Z
+
).
2.2. Algorithm derivation
Targeting to derive an efcient algebraic solution, we notice
that, given p
i
and r
i
, solving Eq. (2) for p
0
is equivalent to
solving
S(p
0
)
p
0
= a +Bp
0
+[2p
0
p
T
0
+(p
T
0
p
0
)I]c p
0
p
T
0
p
0
= 0,
(3)
where 0 denotes the n-dimensional zero vector, I
denotes the n n identity matrix, a =
1
N

N
i=1
(p
i
p
T
i
p
i

r
2
i
p
i
) (n-dimensional vector), B =
1
N

N
i=1
[2p
i
p
T
i

(p
T
i
p
i
)I +r
2
i
I] (nn symmetric matrix), and c =
1
N

N
i=1
p
i
(n-dimensional vector).
By introducing a linear transform
p
0
= q +c, (4)
where q is an n-dimensional vector with the kth element
denoted as q
k
, we obtain from Eq. (3) an equation containing
no quadratic term of q
(a +Bc +2cc
T
c)+{Bq+[2cc
T
+
_
c
T
c
_
I]q} qq
T
q = 0.
(5)
By dening f = a +Bc +2cc
T
c (n-dimensional vector with
the kth element denoted as f
k
) and D = B +2cc
T
+(c
T
c)I
http://journals.cambridge.org Downloaded: 20 Feb 2014 IP address: 131.155.2.68
378 A closed-form algorithm for the least-squares trilateration problem
(n n symmetric matrix), we rewrite Eq. (5) as
f +[D (q
T
q)I]q = 0. (6)
In order to solve Eq. (6) for q efciently, we introduce a
simplication. First, we have
D (q
T
q)I = B +2cc
T
+
_
c
T
c
_
I [(p
0
c)
T
(p
0
c)]I
=
1
N
N

i=1
_
2p
i
p
T
i

_
p
T
i
p
i
_
I +r
2
i
I
_
+2cc
T
+
_
c
T
c
_
I [(p
0
c)
T
(p
0
c)]I. (7)
Next, we notice that, with perfect p
i
and r
i
, we have from
Eq. (1)
N

i=1
r
2
i
=
N

i=1
(p
i
p
0
)
T
(p
i
p
0
), (8)
while, with realistic measurements of p
i
and r
i
, we have a
good approximation:
N

i=1
r
2
i

N

i=1
(p
i
p
0
)
T
(p
i
p
0
). (9)
Substituting Eqs. (9) into (7), we obtain
D (q
T
q)I
1
N
N

i=1
_
2p
i
p
T
i

_
p
T
i
p
i
_
I
+[(p
i
p
0
)
T
(p
i
p
0
)]I
_
+2cc
T
+
_
c
T
c
_
I [(p
0
c)
T
(p
0
c)]I,
=
2
N
N

i=1
p
i
p
T
i
+2cc
T
, (10)
which is an n nsymmetric matrix containing no q. Dening
H =
2
N

N
i=1
p
i
p
T
i
+2cc
T
and denoting the (k, l) element
of H as H
k,l
, we obtain from Eq. (6)
f +Hq = 0. (11)
Equation (11) is a linear system of n equations of the
unknown n-dimensional vector q. If H has full rank (and is
hence invertible), q can be calculated easily 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 veried by symbolic computation that the
H constructed from an arbitrary set of N =n independent
reference points p
i
in R
2
(where n = 2) or R
3
(where n =
3) has a rank of n 1, though the H constructed from N >n
p
i
in general has a rank of n. Moreover, when all p
i
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
cases, Eq. (11) does not represent a system of n independent
linear equations, and hence we cannot uniquely determine q
from only Eq. (11). Instead, additional constraints need to
be found to construct a complete system of n independent
equations so that the specic solution can be obtained.
Here we propose a unied solution procedure for H with
a rank of either n (full-rank) or n 1.
First, we construct a degenerated system of n 1 linear
equations fromEq. (11) by subtracting the last equation from
the other n 1 equations
H

= f

+h

q
n
, (12)
where q

is an n 1 dimensional vector with the kth element

as q

k
= q
k
, H

is an (n 1)(n 1) matrix with the (k,l)

element as H

k,l
= H
k,l
H
n,l
, f

is an n 1 dimensional
vector with the kth element as f

k
= (f
k
f
n
), and h

is
an n 1 dimensional vector with the kth element as h

k
=
(H
k,n
H
n,n
).
Next, from Cramers rule
28
and the denition of matrix
determinant, we obtain
q
k
= q

k
=
det
_
H
f
k
_
det(H

)
+
det
_
H
h
k
_
det(H

)
q
n
, k {1, . . . , n 1},
(13)
where det means the matrix determinant, H
f
k
is the matrix
formed by replacing the kth column of H

with f

, and H
h
k
is
the matrix formed by replacing the kth column of H

with h

.
In particular, because H

, constructed from the H with a rank

of n or n 1, has a full rank of n 1, det(H

) =0.
Equation (13) denes q
k
in terms of q
n
, which means that q
can be completely determined upon the determination of q
n
.
To solve for q
n
, one more independent equation is needed. In
fact, one valid constraint is
n

k=1
q
2
k
= q
T
q, (14)
where q
T
q can be obtained from D (q
T
q)I = H as
q
T
q =
1
N
N

i=1
p
T
i
p
i
+
1
N
N

i=1
r
2
i
+c
T
c. (15)
Substituting Eq. (13) into Eq. (14), we obtain
n1

k=1
[det
_
H
f
k
_
+det
_
H
h
k
_
q
n
]
2
+det(H

)
2
q
2
n
= det(H

)
2
q
T
q,
(16)
which is a quadratic equation of q
n
. Written into the canonical
form, Eq. (16) becomes
aq
2
n
+bq
n
+c = 0, (17)
where a =

n1
k=1
det(H
h
k
)
2
+det(H

)
2
, b = 2

n1
k=1
[det(H
f
k
) det(H
h
k
)], c =

n1
k=1
det(H
f
k
)
2
det(H

)
2
q
T
q.
http://journals.cambridge.org Downloaded: 20 Feb 2014 IP address: 131.155.2.68
A closed-form algorithm for the least-squares trilateration problem 379
Equation (17) is well-known solvable in the closed form as
q
n
=
b

b
2
4ac
2a
. (18)
In particular, since det(H

) =0, a =0.
Then, by substituting the resulting q
n
into Eq. (13), we
obtain q
k
, k {1, . . . , n 1}.
Furthermore, by substituting the resulting q into Eq. (4),
we obtain p
0
.
The above process generally results in two candidates of
p
0
, due to the duality of Eq. (18). However, only one of
those two candidates is the valid estimate of p
0
. Therefore,
a judgment needs to be made to pick the correct one. The
judging criterion is usually very simple, such as that p
0
is
known on one specic side of the base plane (or base line)
dened by the reference points, or that current estimate of p
0
should be close enough to the last one.
2.3. Algorithm summary
Following the above derivation, the proposed 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 closed-
form solution to the trilateration problem. This facilitates the
closed-form performance analysis in Section 3. Solving the
least-squares trilateration problem, the proposed algorithm
provides an optimal or at least near-optimal estimation of
the position of the target object based on its distances
from N reference points, where N can be any integer
greater than or equal to 2 in a 2D environment or
3 in a 3D environment. Using standard linear algebra
techniques, the proposed algorithm is highly tractable
and convenient to implement, and has low computational
complexity. Without depending on the techniques which
tend to be affected by algebraic singularities, such as
matrix inversion, the proposed algorithmhas high operational
robustness.
Algorithm 1: Nonlinear Least-Squares Trilateration in R
n
(n {2,3})
Input: A set of N reference points {p
i
| i Z
+
, n i N},
and the corresponding set of distances between
p
i
and the unknown position p
0
{r
i
| i Z
+
,
n i N}.
Output: p
0
.
(1) Calculate a, B, c, H, f, H

, h

, f

, H
h
k
and H
f
k
.
(2) Calculate q
T
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
0
from Eq. (4).
(6) Choose one of the two candidates of p
0
.
(7) Return p
0.
3. Performance Analysis
The proposed trilateration algorithm estimates the position
of a target object based on the simultaneous distance
measurements from multiple reference points at known
positions. The input to the algorithm includes the mapped
positions of the reference points, p
i
, which are dened in a
global frame of reference attached to the environment, and
the measured distances between the object and reference
points, r
i
. In practice, errors arise in p
i
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 errors will cause output errors in the estimation of
the object position p
0
. This section discusses the inuence
of these input errors on the accuracy of the estimation
output.
3.1. Performance indices
We dene an (n+1)N-dimensional input vector x =
[ p
T
i
r
i
]
T
which consists of all p
i
and r
i
. Thus p
0
is
a function of x, i.e., p
0
= p
0
(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. Correspondingly, the
actual value and output estimate of p
0
are dened by p
0
( x)
and p
0
(x) = p
0
( x +x) respectively, and the estimation error
of p
0
is p
0
(x) = p
0
( x +x) p
0
( x).
From the Taylor expansion, we obtain
p
0
(x) = p
0
( x +x) p
0
( x) =

k=1
1
k!

k
p
0
( x)
x
T
k
x
[k]
. (19)
Here we follow the notation of matrix calculus.
29
The matrix
derivative
F
M
of a function F: M
nm
F
pq
is represented
as an nm block matrix whose (k
1
,k
2
) entry is a pq matrix
F
M
k
1
k
2
, where M
k
1
k
2
is the (k
1
,k
2
) entry of M. x
[k]
denotes the
kth Kronecker power of x, 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 high-
order terms in Eq. (19), we can obtain, based on the denition
of mean and variance of random variables, the mean vector
E(p
0
) and variance matrix var(p
0
) of the output error p
0
as
E[p
0
(x)]
1
2

2
p
0
( x)
x
T
2
vec[var(x)], (20)
var[p
0
(x)]
p
0
( x)
x
T
var(x)
p
0
( x)
T
x
, (21)
where vec(M) denotes the vector created from a matrix M by
stacking its columns,
p
0
x
T
= [
p
0
p
T
i

p
0
r
i
], and

2
p
0
x
T
2
=
[

2
p
0
p
T
i
p
T
j


2
p
0
p
T
i
r
j


2
p
0
r
i
p
T
j


2
p
0
r
i
r
j
]. Equations (20)
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 evaluate the impact of the position error of p
i
, p
i
,
and the impact of the distance error of r
i
, r
i
, on p
0
separately.
To evaluate the impact of p
i
on p
0
, we assume that
the measurement errors in mapping the reference points
are zero-mean random variables and uncorrelated from
one another with the same standard deviation
p
for
http://journals.cambridge.org Downloaded: 20 Feb 2014 IP address: 131.155.2.68
380 A closed-form algorithm for the least-squares trilateration problem
each coordinate. We dene two performance indices, the
normalized total bias B
p
, which represents the systematic
estimation error, and the normalized total standard deviation
error S
p
, which represents the uncertainty of position
estimation
B
p
(p
0
) =
|E(p
0
)|

2
p
=
1
2

2
p
0
p
T
2
vec(I
nN
)

=
1
2

i=1

2
p
0
p
T
i
2
vec(I
n
)

,
(22)
S
p
(p
0
) =
_
Tr[var(p
0
)]

p
=
_
Tr
_
p
0
p
T
p
T
0
p
_
, (23)
where p = [ p
T
i
]
T
is an nN-dimensional vector
consisting of all p
i
, |v| denotes the norm of a vector v, and
Tr(M) denotes the trace of a matrix M. We notice that B
p
and S
p
are independent of
p
.
Similarly, to evaluate the impact of r
i
on p
0
, 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 dene two performance indices,
the normalized total bias B
r
and the normalized total standard
deviation error S
r
as
B
r
(p
0
) =
|E(p
0
)|

2
r
=
1
2

2
p
0
r
T
2
vec(I
N
)

=
1
2

i=1

2
p
0
r
2
i

,
(24)
S
r
(p
0
) =
_
Tr[var(p
0
)]

r
=
_
Tr
_
p
0
r
T
p
T
0
r
_
, (25)
where r = [ r
i
]
T
is an N-dimensional vector consisting
of all r
i
. We notice that B
r
and S
r
are independent of
r
.
The differential terms of p
0
relevant to Eqs. (22)(25)
include
p
0
p
T
i
,
p
0
r
i
,

2
p
0
p
T
i
2
and

2
p
0
r
2
i
. They are derived and listed
in the Appendix.
As discussed before, due to the duality of Eq. (18), we
generally obtain two candidates of p
0
from computation.
Correspondingly, we can generally obtain two
p
0
x
T
and two

2
p
0
x
T
2
, and hence two values for each of B
p
, S
p
, B
r
, and S
r
.
In order to distinguish between them, we denote the values
for B
p
, S
p
, B
r
, and S
r
corresponding to +

b
2
4ac in
Eq. (18) as B
p+
, S
p+
, B
r+
, and S
r+
respectively, and those
corresponding to

b
2
4ac as B
p
, S
p
, B
r
, and S
r
respectively. As a result, in practice the prediction of the
estimation error will, in general, depend on whether p
0
is
corresponding to +

b
2
4ac or

b
2
4ac.
An error analysis on the proposed trilateration algorithm
has been conducted, and the results of the above-dened
performance indices are reported in the following sections.
Without loss of generality, our error analysis focuses on R
3
,
with an understanding that the trilateration in R
2
is indeed a
downgrade of the trilateration in R
3
. Through representative
examples, we test the proposed trilateration algorithm with
three reference points at rst and then with four reference
points.
3.2. Trilateration in R
3
with three reference points
Following the representative examples in refs. [13, 17],
we examine a three-reference case in R
3
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 of reference with a radius of 1000. The reference
points are located at p
1
= [ 500

3 500 0]
T
, p
2
=
[ 0 1000 0]
T
and p
3
= [ 500

3 500 0]
T
. We also
dene two square data acquisition regions as R
+
= {p
0
= [x, y, z]
T
|z = 8000, 4000 x, y 4000} and R

=
{p
0
= [x, y, z]
T
|z = 8000, 4000 x, y 4000}, which
are symmetric to the base plane dened by the reference
points and corresponding to +

b
2
4ac and

b
2
4ac
in Eq. (18) respectively. As a result, we obtain B
p+
, S
p+
,
B
r+
, and S
r+
across R
+
, and B
p
, S
p
, B
r
, and S
r
across
R

.
Analyzing the effect of p
i
on p
0
, we notice that B
p+
and B
p
are symmetric to the base plane, so are S
p+
and
S
p
, i.e., B
p
(p
0
= [x, y, z]
T
) = B
p+
(p
0
= [x, y, z]
T
) =
B
p
(p
0
= [x, y, z]
T
) and S
p
(p
0
= [x, y, z]
T
) =
S
p+
(p
0
= [x, y, z]
T
) = S
p
(p
0
= [x, y, z]
T
). Moreover,
B
p
and S
p
are symmetric to the orthographic projections of
the medians of the base triangle onto the data acquisition
planes. For example, one of the medians is the y axis,
and correspondingly B
p
(p
0
= [x, y, z]
T
) = B
p
(p
0
=
[x, y, z]
T
) and S
p
(p
0
= [x, y, z]
T
) = S
p
(p
0
=
[x, y, z]
T
).
The resulting trends of B
p
and S
p
are shown in Fig. 5.
Both B
p
(systematic error) and S
p
(estimation uncertainty)
increase as p
0
moves away from the centroid of the base
triangle. The minimum values of B
p
and S
p
are obtained at
the centers of the data acquisition regions p
0
=[0, 0, 8000]
T
which correspond to the centroid of the base triangle, where
B
p,min
= 0.0055 and S
p,min
= 9.3277.
Compared with the results reported in ref. [13, 17] which
were generated fromthe exactly same example, the proposed
algorithm has lower S
p
values (Figure 4a in ref. [17] reports
that S
2
p
170 (S
p
= 13.04) when p
0
= [0, 0, 8000]
T
, and
S
2
p
192 (S
p
= 13.86) when p
0
= [4000, 4000, 8000]
T
,
which are consistent with the results in ref. [13]. Since only
S
p
was reported in ref. [17], we only make a comparison in S
p
here.), which means that the proposed trilateration algorithm
has a reduced uncertainty in position estimation when using
imperfectly mapped reference points.
Analyzing the effect of r
i
on p
0
, we notice that similar to
B
p
and S
p
, B
r+
and B
r
are symmetric to the base plane, so
are S
r+
and S
r
; B
r
and S
r
are symmetric to the orthographic
projections of the medians of the base triangle onto the
data acquisition planes; B
r
and S
r
increase as p
0
moves
away from the centroid of the base triangle. The minimum
values of B
r
and S
r
are obtained at the centers of the data
acquisition regions, where B
r,min
= 0.0054 and S
r,min
=
9.3277. Moreover, the values of B
r
and S
r
are very close,
if not equal, to those of B
p
and S
p
respectively (Table I).
For this reason, we do not present the gures for B
r
and S
r
specically.
http://journals.cambridge.org Downloaded: 20 Feb 2014 IP address: 131.155.2.68
A closed-form algorithm for the least-squares trilateration problem 381
0
.
0
0
6
0
.0
0
6
0
.
0
0
6
0
.
0
0
6
0
.
0
0
6
5
0
.
0
0
6
5
0
.0
0
6
5
0
.
0
0
6
5
0
.
0
0
6
5
0
.0
0
6
5
0
.
0
0
7
0
.
0
0
7
0.007
0
.
0
0
7
0
.
0
0
7
0
.0
0
7
0.007
0
.
0
0
7
0
.
0
0
7
5
0
.
0
0
7
5
0.0075
0
.0
0
7
5
0
.
0
0
7
5
0
.
0
0
7
5
0
.0
0
7
5
0
.0
0
7
5
0
.
0
0
8
0
.
0
0
8
0
.
0
0
8
0
.
0
0
8
0
.
0
0
8
5
0
.
0
0
8
5
0
.
0
0
8
5
0
.
0
0
8
5
0
.
0
0
9
0
.
0
0
9
0
.
0
0
9
0
.
0
0
9
Normalized Bias B
p
x
y
4000 3000 2000 1000 0 1000 2000 3000 4000
4000
3000
2000
1000
0
1000
2000
3000
4000
9
.
5 9
.
5
9
.5
1
0
1
0
1
0
1
0
10
1
0
1
0
.
5
1
0
.
5
10.5
1
0
.
5
1
0
.
5
1
0
.5
1
0
.5
11
1
1
1
1
1
1
11
1
1
1
1
1
1
11
1
1
.
5
1
1
.
5
1
1
.
5
1
1
.
5
1
2
1
2
1
2
1
2
Normalized Standard Deviation S
p
x
y
4000 3000 2000 1000 0 1000 2000 3000 4000
4000
3000
2000
1000
0
1000
2000
3000
4000
Fig. 5. Normalized total bias B
p
and normalized total standard
deviation error S
p
obtained from the three-reference example.
Compared with the results reported in refs. [13, 17],
the proposed algorithm has signicantly lower B
r
values
(Ref. [17] reports that the maximum B
r
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 that the proposed trilateration algorithmhas a reduced
systematic error in position estimation when using erroneous
distance measurements.
3.3. Trilateration in R
3
with four reference points
To test the performance of the proposed trilateration
algorithm with N>n reference points, we examine a four-
reference case in R
3
in which the XY coordinates of the four
reference points form a square inscribed in the same circle
Table I. Comparison between B
p
/S
p
and B
r
/S
r
for the
three-reference case.
(4000, 4000, (2000, 1600, (2000, 3200,
p
0
(0,0,8000) 8000) 8000) 8000)
B
p
0.0055 0.0102 0.0064 0.0074
B
r
0.0054 0.0101 0.0062 0.0073
S
p
9.3277 12.7608 10.0348 10.8435
S
r
9.3277 12.7608 10.0348 10.8435
B
p
/S
p
and B
r
/S
r
are compared at 4 representative p
0
.
Table II. Comparison between B
p
/S
p
of the three-reference case
and those of the four-reference case.
B
p,min
B
p,max
S
p,min
S
p,max
Three-reference 0.0055 0.0102 9.3277 12.7608
Four-reference 0.0070 0.0101 8.0780 11.0059
In the three-reference case, B
p,min
and S
p,min
are obtained at p
0
=
[0, 0, 8000], and B
p,max
and S
p,max
are obtained at p
0
=[4000,
4000, 8000]; In the four-reference case, B
p,min
is obtained at p
0
=[0, 0, 8000], B
p,max
is obtained at p
0
=[4000, 4000, 8000] and
p
0
= [4000, 4000, 8000], S
p,min
is obtained at p
0
= [0, 0,
8000], and S
p,max
is obtained at p
0
=[4000, 4000, 8000].
as in the three-reference example (centered at the origin of
the frame of reference with a radius of 1000). The reference
points are located at p
1
= [ 500

2 500

2 0]
T
, p
2
=
[ 500

2 500

2 0]
T
, p
3
= [ 500

2 500

2 0]
T
,
and p
4
= [ 500

2 500

2 0]
T
. Using the same two
square data acquisition regions as the three-reference case,
we calculate B
p+
, S
p+
, B
r+
, and S
r+
across R
+
, and B
p
,
S
p
, B
r
, and S
r
across R

.
The resulting trends of B
p+
and B
p
are shown in Fig. 6.
Different from the three-reference case, B
p+
and B
p
are
not symmetric to the base plane, i.e., in general B
p+
(p
0
=
[x, y, z]
T
) =B
p
(p
0
= [x, y, z]
T
). Instead, B
p+
and B
p
are mutually symmetric to the diagonals of the base square,
i.e., B
p+
(p
0
= [x, y, z]
T
) = B
p
(p
0
= [x, y, z]
T
).
S
p+
and S
p
are still symmetric to the base plane, i.e.,
S
p
(p
0
= [x, y, z]
T
) = S
p+
(p
0
= [x, y, z]
T
) = S
p
(p
0
=
[x, y, z]
T
). Moreover, S
p
is radially symmetric to the
vertical axis which passes through the centers of the
base square and data acquisition regions, i.e., S
p
(p
0
=
[x, y, z]
T
) = S
p
(p
0
= [x, y, z]
T
) = S
p
(p
0
=
[x, y, z]
T
) = S
p
(p
0
= [x, y, z]
T
). The trend of S
p
is shown in Fig. 7.
Both B
p
and S
p
increase as p
0
moves away fromthe center
of the base square. The minimum values of B
p
and S
p
are
obtained at the centers of the data acquisition regions, where
B
p,min
= B
p+,min
= B
p,min
= 0.0070 and S
p,min
= S
p+,min
= S
p,min
= 8.0780.
Compared with the three-reference case (Table II), we
notice that there is an increase in the minimum B
p
due
to the addition of another imperfectly mapped reference
point (B
p,min
= 0.0070 for the four-reference case versus
B
p,min
= 0.0055 for the three-reference case). However, the
maximum B
p
decreases slightly (B
p,max
= 0.0101 for the
http://journals.cambridge.org Downloaded: 20 Feb 2014 IP address: 131.155.2.68
382 A closed-form algorithm for the least-squares trilateration problem
0
.
0
0
7
5
0
.
0
0
7
5
0.0075
0
.
0
0
7
5
0
.
0
0
7
5
0
.0
0
7
5
0
.
0
0
7
5
0
.
0
0
8
0
.
0
0
8
0
.
0
0
8
0
.
0
0
8
0.008
0
.
0
0
8
0
.
0
0
8
5
0
.
0
0
9
Normalized Bias B
p+
x
y
4000 3000 2000 1000 0 1000 2000 3000 4000
4000
3000
2000
1000
0
1000
2000
3000
4000
0
.
0
0
7
5
0
.0
0
7
5
0
.
0
0
7
5
0
.
0
0
7
5
0
.
0
0
7
5
0.0075
0
.
0
0
7
5
0
.
0
0
8
0
.
0
0
8
0
.
0
0
8
0.008
0
.0
08
0
.
0
0
8
0
.
0
0
8
5
0
.
0
0
9
Normalized Bias B
p
x
y
4000 3000 2000 1000 0 1000 2000 3000 4000
4000
3000
2000
1000
0
1000
2000
3000
4000
Fig. 6. Normalized total bias B
p+
and B
p
obtained from the four
reference example.
four-reference case versus B
p,max
= 0.0102 for the three-
reference case). Moreover, a comparison between Figs. 5
and 6 exposes that the values of B
p
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 p
0
moves away from the center of the data
acquisition region, the increase rate of B
p
for the four-
reference case is less than that of the three-reference case,
and hence the systematic estimation error with four reference
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 signicantly lower than that of the three-reference case
across the data acquisition regions, which means that the
uncertainty in position estimation, when using imperfectly
Table III. Comparison between S
p
and S
r
for the four-reference
case.
(4000, 4000, (2000, 1600, (2000, 3200,
p
0
(0,0,8000) 8000) 8000) 8000)
S
p
8.0780 11.0059 8.6836 9.3884
S
r
8.0780 11.0059 8.6836 9.3884
S
p
and S
r
are compared at 4 representative p
0
.
8
.
5
8.5
8
.
5
8
.5
8
.
5
9
9
9
9
9
9
9
9
.
5
9
.
5
9.5
9
.5
9
.
5
9
.
5
9
.5
9.5
9
.
5
1
0
1
0
1
0
1
0
1
0
.
5
1
0
.
5
1
0
.
5
1
0
.
5
Normalized Standard Deviation S
p
x
y
4000 3000 2000 1000 0 1000 2000 3000 4000
4000
3000
2000
1000
0
1000
2000
3000
4000
Fig. 7. Normalized total standard deviation error S
p
obtained from
the four-reference example.
mapped reference points, will decrease by referring to more
reference points.
Similar to the three-reference case, B
r+
and B
r
are
symmetric to the base plane, so are S
r+
and S
r
. Moreover, B
r
and S
r
are radially symmetric to the vertical axis which passes
through the centers of the base square and data acquisition
regions. Both B
r
and S
r
increase as p
0
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
r
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 gure for S
r
specically.
Compared with the values in the three-reference case
(from Table IV and Figs. 5, 7, and 8), both B
r
and S
r
in the four-reference case are signicantly lower across
the data acquisition regions, which means that 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 performance of the error indices
In the above error analysis, the B
p
, S
p
, B
r
and S
r
are
calculated directly from Eqs. (22)(25). In order to validate
http://journals.cambridge.org Downloaded: 20 Feb 2014 IP address: 131.155.2.68
A closed-form algorithm for the least-squares trilateration problem 383
Table IV. Comparison between B
r
/S
r
of the three-reference case
and those of the four-reference case.
B
r ,min
B
r ,max
S
r ,min
S
r ,max
Three-reference 0.0054 0.0101 9.3277 12.7608
Four-reference 0.0040 0.0075 8.0780 11.0059
In the three-reference case, B
r ,min
and S
r ,min
are obtained at p
0
=
[0, 0, 8000], and B
r ,max
and S
r ,max
are obtained at p
0
=[4000,
4000, 8000]; In the four-reference case, B
r ,min
and S
r ,min
are
obtained at p
0
=[0, 0, 8000], and B
r ,max
and S
r ,max
are obtained
at p
0
= [4000, 4000, 8000].
0
.
0
0
4
5
0
.0
0
4
5
0
.
0
0
4
5
0
.
0
0
4
5
0.0045
0
.
0
0
5
0
.
0
0
5
0.005
0
.
0
0
5
0
.
0
0
5
0
.0
0
5
0
.0
0
5
0
.
0
0
5
5
0
.
0
0
5
5
0.0055
0
.0
0
5
5
0
.
0
0
5
5
0
.
0
0
5
5
0
.0
0
5
5 0
.0
0
5
5
0
.
0
0
5
5
0
.
0
0
6
0
.
0
0
6
0
.
0
0
6
0
.
0
0
6
0
.
0
0
6
5
0
.
0
0
6
5
0
.
0
0
6
5
0
.
0
0
6
5
Normalized Bias B
r
x
y
4000 3000 2000 1000 0 1000 2000 3000 4000
4000
3000
2000
1000
0
1000
2000
3000
4000
Fig. 8. Normalized total bias B
r
obtained from the four-reference
example.
those closed-formformulas in approximating the 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
p
i
on p
0
, we assume that the components of p
i
are
uncorrelated from one another and follow the Gaussian
distribution with the zero mean and the same standard
deviation
p
. Given the correct position of the target object
p
0
, the correct positions of the reference points p
i
and the
correct distances between the object and reference points
r
i
, we generate a number of erroneous p
i
according to the
random distribution of p
i
, substitute them into Algorithm 1
to calculate corresponding erroneous p
0
, obtain the statistics
of p
0
, and then calculate B
p
and S
p
from E(p
0
) and
var(p
0
) respectively. To evaluate the impact of r
i
on p
0
,
we assume that r
i
are uncorrelated from one another and
follow the Gaussian distribution with the zero mean and the
same standard deviation
r
. Given the correct p
0
, p
i
and
r
i
, we generate a number of erroneous r
i
according to the
random distribution of r
i
, substitute them into Algorithm 1
to calculate corresponding erroneous p
0
, obtain the statistics
of p
0
, and then calculate B
r
and S
r
fromE(p
0
) and var(p
0
)
respectively.
Different from the closed-form calculation using Eqs.
(22)(25) which is independent of
p
and
r
, the numerical
simulation must be carried out based on specied
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 B
p
, S
p
, B
r
,
and S
r
across the data acquisition regions. It turns out that
the resulting B
p
, S
p
, B
r
, and S
r
are consistent across the
range of
p
and
r
, which veries that B
p
, 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
simulations veries that Eqs. (22)(25) provide a good
closed-form approximation of the effect of p
i
and r
i
on
p
0
(Table V).
4. Experimental Test
The effectiveness of the proposed trilateration algorithm has
also been veried through an experiment of trilaterating an
onboard stereovision unit. Focusing on testing the estimation
error under the combined effect of realistic input errors and
the execution speed of the proposed trilateration algorithm,
we used the stereovision system only for obtaining the
distance measurements between the unit and reference
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 reference patterns were used for three-reference
and four-reference trilateration respectively, where bright
LEDs were adopted to represent reference points. The
three-reference pattern contains three symmetric reference
points which dene an equilateral base triangle, while the
four-reference pattern contains four symmetric reference
points which dene 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.
http://journals.cambridge.org Downloaded: 20 Feb 2014 IP address: 131.155.2.68
384 A closed-form algorithm for the least-squares trilateration problem
Table V. Comparison between B
p
, S
p
, B
r
, and S
r
calculated from Eqs. (22)(25) and those obtained
from numerical simulations.
p
0
(0, 0, 8000) (4000, 4000, 8000) (2000, 1600, 8000) (2000, 3200, 8000)
B
p
0.0055 0.0102 0.0064 0.0074
B
p,sim
0.0057 0.0107 0.0066 0.0078
S
p
9.3277 12.7608 10.0348 10.8435
S
p,sim
9.3609 12.8730 10.0896 10.8947
B
r
0.0054 0.0101 0.0062 0.0073
B
r,sim
0.0054 0.0111 0.0066 0.0076
S
r
9.3277 12.7608 10.0348 10.8435
S
r,sim
9.3059 12.7783 10.0170 10.8358
B
p
0.0070 0.0085 0.0073 0.0083
B
p,sim
0.0077 0.0090 0.0078 0.0087
S
p
8.0780 11.0059 8.6836 9.3884
S
p,sim
8.0725 11.0442 8.6908 9.4088
B
r
0.0040 0.0075 0.0047 0.0054
B
r,sim
0.0040 0.0077 0.0047 0.0057
S
r
8.0780 11.0059 8.6836 9.3884
S
r,sim
8.1271 11.0947 8.7389 9.4609
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
are obtained with
p
= 70, while the presented B
r,sim
and S
r,sim
are obtained with
r
= 70.
the ceiling of an indoor experiment environment with
the base plane parallel to the at oor. We used the base
plane as the XY plane of the global frame of reference, and
the center of the base triangle or square as the origin of
the frame.
2. A stereovision unit, consisting of two Matrix Vision
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 Directed Perception PTU-D46-17 controllable
pan/tilt unit which enables the stereovision unit to orient
toward the reference pattern. With the mobile robot
moving in the at planar indoor environment, the cameras
were kept on a plane parallel to the base plane (XY
plane of the global frame) at a distance of 1600 mm.
The apertures of both the cameras were intentionally
minimized such that, with regular ambient lighting,
the LEDs were shown as bright spots outstanding fromthe
dark background in the images (Fig. 9b), which provide
convenience for segmenting the reference points from
the images.
The cameras were calibrated using a camera calibration
toolbox for Matlab available at http://www.vision.caltech.
edu/bouguetj/calib doc/ and a printed black/white check-
erboard pattern to nd out the intrinsic parameters of
the cameras, such as focal length, image center and
distortion coefcients, and the transformation between
the two cameras. The calibration results are listed as
follows:
1. For the left camera, we have the focal length (pixels) f =
[1623.4699; 1626.0442] [1.1299; 1.1164], image center
(pixels) c = [329.6443; 248.5307] [1.9019; 1.6757],
Fig. 10. Onboard experimental system.
and distortion coefcients 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 =
[1627.7598; 1629.5029] [1.1475; 1.1091], image center
(pixels) c = [334.0203; 246.5809] [1.9384; 1.5751],
and distortion coefcients 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 vector (radians) v
R
= [0.0129; 0.2389;
0. 0016] [0.0045; 0.0064; 0.0004] from which the
corresponding rotation matrix can be calculated using the
Rodrigues rotation formula, and translation vector (mm)
v
T
= [289.0918; 3.2027; 32.6389] [0.1915; 0.1109;
1.3917].
In each of the above calibration results, the numbers in
the rst [] are the calibrated values, while the numbers
in the second [] are the numerical errors which, as
http://journals.cambridge.org Downloaded: 20 Feb 2014 IP address: 131.155.2.68
A closed-form algorithm for the least-squares trilateration problem 385
Table VI. Camera position estimation errors.
Three-reference case Four-reference case
R
h
|E(p
0
)| |STD(p
0
)| |E(p
0
)| |STD(p
0
)|
2000 2.6391 20.7778 1.7574 14.8373
2500 11.8517 24.9709 11.3892 20.1474
3000 14.7132 28.2164 13.4792 25.2913
3500 20.4381 32.3926 18.2349 27.7020
4000 28.3571 49.2009 26.1342 46.9997
In this table, E(.) denotes mean, STD(.) denotes standard deviation,
|.| denotes the norm of the vector, and the unit of lengths 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
different distances fromthe reference pattern (represented by
the horizontal distance R
h
fromthe Z-axis of the global frame
which passes through the center of the base triangle/square),
and at each distance we acquired data from a number of
locations on the circle about the Z-axis with the radius of R
h
.
At each location, the stereovision unit was oriented toward
the reference pattern, a pair of images of the reference pattern
was taken by the stereovision unit, and the actual position of
the left camera was measured manually. The data were stored
in an onboard laptop computer.
The collected images were processed after the data
acquisition process. Focusing on evaluating the proposed
trilateration algorithm, we segmented the reference points
from each pair of images manually (after correcting the
image distortions), ran a stereovision programto estimate the
distances from the left camera to the reference points based
on the camera calibration results, and then implemented the
proposed trilateration algorithm to estimate the 3D position
of the left camera. The estimation error was calculated
by comparing the estimated position with the measured
one, which was affected by all the above-mentioned input
errors, including the reference positioning errors, distance
measurement errors (caused by camera calibration errors and
image segmentation errors), as well as the manual camera
position measurement errors.
As the result, we obtained a prole of the position
estimation error under the experimental settings (Table VI).
We notice that, even with the above less accurate
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 cameras and pattern) of 4.3 m (corresponding to
R
h
=4 m), which, as we believe, is sufcient for many mobile
robot applications. The results also showsome improvements
in both the estimation accuracy and uncertainty by using more
reference points (four reference points versus three reference
points in our experiment).
Moreover, due to its closed-form nature, the proposed
trilateration algorithm can be implemented at a high speed in
practice. We tested both a Matlab program and a compiled
standalone executable (exe le) of the algorithm respectively
on a laptop PC with a 1.66 GHz Intel Core 2 CPU.
Given the reference positions and distances, for the three-
reference trilateration, the Matlab version of the proposed
trilateration algorithmtakes 3.831010
4
s in average, while
the standalone version takes 3.7006 10
4
s in average; for
the four-reference trilateration, the Matlab program takes
3.973210
4
s in average, while the standalone executable
takes 3.933310
4
s. It means that the proposed trilateration
algorithm is highly applicable to real-time applications.
Moreover, requiring only the reference positions and
corresponding distance measurements as the 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
ultrasound
6,10,30,31
and radio frequency signals.
7,8,11
5. Conclusion
This paper presents a novel closed-form trilateration
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 R
n
(n = 2 for 2D
environments and n = 3 for 3D environments), not limited
to solving for the intersection points of exactly n spheres in
R
n
. Using standard linear algebra techniques, the proposed
algorithm has low computational complexity. Without
depending on the techniques which tend to be affected by
algebraic singularities, such as matrix inversion, the proposed
algorithm has high operational robustness. The performance
analysis shows that the algorithm is highly effective, with
lower systematic bias and estimation uncertainty than
representative closed-form methods, when dealing with
erroneous inputs of distance measurements and reference
points. The results have also veried that introducing more
reference points and corresponding distance measurements
into the trilateration process will in 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.
Appendix
In this appendix, we provide the differential terms of p
0
relevant to Eqs. (22)(25), including
p
0
p
T
i
,
p
0
r
i
,

2
p
0
p
T
i
2
and

2
p
0
r
2
i
.
http://journals.cambridge.org Downloaded: 20 Feb 2014 IP address: 131.155.2.68
386 A closed-form algorithm for the least-squares trilateration problem
The summation

P
ss
in
det(H

)
p
T
i
,
det(H
h
k
)
p
T
i
and
det(H
f
k
)
p
T
i
is
computed over all the permutations of the set of numbers
SS = {1,2,. . .,n}. P
ss
denotes the set of all n! permutations
p
0
p
T
i
=
q
p
T
i
+
c
p
T
i
,
q
k
p
T
i
=
1
det(H

)
_
det
_
H
f
k
_
p
T
i
+
det
_
H
h
k
_
p
T
i
q
n
+det
_
H
h
k
_
q
n
p
T
i
_

det
_
H
f
k
_
+det
_
H
h
k
_
q
n
det(H

)
2
det(H

)
p
T
i
,
k {1, . . . , n 1},
q
n
p
T
i
=
1
2a
_

b
p
T
i

b
2
4ac
_
b
b
p
T
i
2c
a
p
T
i
2a
c
p
T
i
__

b
2
4ac
2a
2
a
p
T
i
,
a
p
T
i
= 2
n1

k=1
_
det
_
H
h
k
_ det
_
H
h
k
_
p
T
i
_
+2 det(H

)
det(H

)
p
T
i
,
b
p
T
i
= 2
n1

k=1
_
det
_
H
h
k
_ det
_
H
f
k
_
p
T
i
+det
_
H
f
k
_ det
_
H
h
k
_
p
T
i
_
,
c
p
T
i
= 2
n1

k=1
_
det
_
H
f
k
_ det
_
H
f
k
_
p
T
i
_
2q
T
qdet(H

)
det(H

)
p
T
i
det(H

)
2
(q
T
q)
p
T
i
,
det(H

)
p
T
i
=

P
ss
sgn()
n1

=1
_
_

lSS,l=l

l,(l)
H

,(l

)
p
T
i
_
_
,
det
_
H
h
k
_
p
T
i
=

P
ss
sgn()
n1

=1
_
_

lSS,l=l

H
h
k l,(l)
H
h
k l

,(l

)
p
T
i
_
_
,
det
_
H
f
k
_
p
T
i
=

P
ss
sgn()
n1

=1
_
_

lSS,l=l

H
f
k l,(l)
H
f
k l

,(l

)
p
T
i
_
_
,
H
p
T
i
=
2
N
_
I
n

_
c
T
p
T
i
_
+(c p
i
)vec(I
n
)
T
_
,
f
p
T
i
=
a
p
T
i
+
B
p
T
i
(I
n
c) +
1
N
B +
2
N
_
I
n

_
c
T
c
__
+
4
N
cc
T
,
a
p
T
i
=
1
N
_
I
n

_
p
T
i
p
i
_
+2p
i
p
T
i
r
2
i
_
,
B
p
T
i
=
2
N
_
I
n
p
T
i
+p
i
vec(I
n
)
T
+p
T
i
I
n
_
,
c
p
T
i
=
1
N
I
n
,
(q
T
q)
p
T
i
=
2
N
_
c
T
p
T
i
_
.
_

_
(A1)
of SS. sgn() denotes the signature of the permutation ,
where sgn() = +1 if is an even permutation, and sgn()
= 1 if it is odd.
p
0
r
i
=
q
r
i
,
q
k
r
i
=
1
det(H

)
_
det
_
H
f
k
_
r
i
+det
_
H
h
k
_ q
n
r
i
_
, k {1, . . . , n 1},
q
n
r
i
=
1
2a
_

b
r
i

b
2
4ac
_
b
b
r
i
2a
c
r
i
__
,
b
r
i
= 2
n1

k=1
_
det
_
H
h
k
_ det
_
H
f
k
_
r
i
_
,
c
r
i
= 2
n1

k=1
_
det
_
H
f
k
_ det
_
H
f
k
_
r
i
_
det(H

)
2
(q
T
q)
r
i
det
_
H
f
k
_
r
i
=

P
ss
sgn()
n1

=1
_
_

lSS,l=l

H
f
k l,(l)
H
f
k l

,(l

)
r
i
_
_
,
f
r
i
=
a
r
i
+
B
r
i
c,
a
r
i
=
2
N
r
i
p
i
,
B
r
i
=
2
N
r
i
I
n
,
(q
T
q)
r
i
=
2
N
r
i
.
_

_
(A2)
http://journals.cambridge.org Downloaded: 20 Feb 2014 IP address: 131.155.2.68
A closed-form algorithm for the least-squares trilateration problem 387

2
p
0
p
T
i
2
=

2
q
p
T
i
2
,

2
q
k
p
T
i
2
=
1
det(H

)
_
_
_

2
det
_
H
f
k
_
p
T
i
2
+
q
n
p
T
i
_
_
I
n

det
_
H
h
k
_
p
T
i
_
_
+q
n

2
det
_
H
h
k
_
p
T
i
2
+
det
_
H
h
k
_
p
T
i
_
I
n

q
n
p
T
i
_
+det
_
H
h
k
_

2
q
n
p
T
i
2
_
_
_

1
det(H

)
2
det(H

)
p
T
i
_
_
_
I
n

_
_
det
_
H
f
k
_
p
T
i
+
det
_
H
h
k
_
p
T
i
q
n
+det
_
H
h
k
_
q
n
p
T
i
_
_
_
_
_

det
_
H
f
k
_
+det
_
H
h
k
_
q
n
det(H

)
2

2
det(H

)
p
T
i
2
+
_

1
det(H

)
2
_
_
det
_
H
f
k
_
p
T
i
+
det
_
H
h
k
_
p
T
i
q
n
+det
_
H
h
k
_
q
n
p
T
i
_
_
+2
det
_
H
f
k
_
+det
_
H
h
k
_
q
n
det(H

)
3
det(H

)
p
T
i
_
_
I
n

det(H

)
p
T
i
_
, k {1, . . . , n 1},

2
q
n
p
T
i
2
=
1
2a
_

2
b
p
T
i
2

1

b
2
4ac
_
b
p
T
i
_
I
n

b
p
T
i
_
+b

2
b
p
T
i
2
2
c
p
T
i
_
I
n

a
p
T
i
_
2c

2
a
p
T
i
2
2
a
p
T
i
_
I
n

c
p
T
i
_
2a

2
c
p
T
i
2
_

b
2
4ac
3
_
b
b
p
T
i
2c
a
p
T
i
2a
c
p
T
i
__
I
n

_
b
b
p
T
i
2c
a
p
T
i
2a
c
p
T
i
__
_

1
2a
2
a
p
T
i
_
I
n

b
p
T
i

b
2
4ac
_
b
b
p
T
i
2c
a
p
T
i
2a
c
p
T
i
___

b
2
4ac
2a
2

2
a
p
T
i
2
+
_

1
2a
2
_

b
p
T
i

b
2
4ac
_
b
b
p
T
i
2c
a
p
T
i
2a
c
p
T
i
__
+
b

b
2
4ac
a
3
a
p
T
i
_
_
I
n

a
p
T
i
_
,

2
a
p
T
i
2
= 2
n1

k=1
_
_
_
det
_
H
h
k
_
p
T
i
_
_
I
n

det
_
H
h
k
_
p
T
i
_
_
+det
_
H
h
k
_
2
det
_
H
h
k
_
p
T
i
2
_
_
_
+2
det(H

)
p
T
i
_
I
n

det(H

)
p
T
i
_
+2 det(H

2
det(H

)
p
T
i
2
,

2
b
p
T
i
2
= 2
n1

k=1
_
_
det
_
H
h
k
_
p
T
i
_
_
I
n

det
_
H
f
k
_
p
T
i
_
_
+det
_
H
h
k
_
2
det
_
H
f
k
_
p
T
i
2
+
det
_
H
f
k
_
p
T
i
_
_
I
n

det
_
H
h
k
_
p
T
i
_
_
+det
_
H
f
k
_
2
det
_
H
h
k
_
p
T
i
2
_
_
,

2
c
p
T
i
2
= 2
n1

k=1
_
_
det
_
H
f
k
_
p
T
i
_
_
I
n

det
_
H
f
k
_
p
T
i
_
_
+det
_
H
f
k
_
2
det
_
H
f
k
_
p
T
i
2
_
_
2
_
det(H

)
(q
T
q)
p
T
i
+q
T
q
det(H

)
p
T
i
_ _
I
n

det(H

)
p
T
i
_
2q
T
qdet(H

2
det(H

)
p
T
i
2
2 det(H

)
det(H

)
p
T
i
_
I
n

(q
T
q)
p
T
i
_
det(H

)
2

2
(q
T
q)
p
T
i
2
,

2
det(H

)
p
T
i
2
=

Pss
sgn()
n1

=1
_
_
_

SS,l

=l

_
_

lSS,l=l

,l=l

l,(l)
H

,(l

)
p
T
i
_
_
_
I
n

,(l

)
p
T
i
_
+

lSS,l=l

l,(l)

2
H

,(l

)
p
T
i
2
_
_
_
,

2
det
_
H
h
k
_
p
T
i
2
=

Pss
sgn()
n1

=1
_
_
_

SS,l

=l

_
_

lSS,l=l

,l=l

H
h
k l,(l)
H
h
k l

,(l

)
p
T
i
_
_
_
I
n

H
h
k l

,(l

)
p
T
i
_
+

lSS,l=l

H
h
k l,(l)

2
H
h
k l

,(l

)
p
T
i
2
_
_
_
,

2
det
_
H
f
k
_
p
T
i
2
=

Pss
sgn()
n1

=1
_
_
_

SS,l

=l

_
_

lSS,l=l

,l=l

H
f
k l,(l)
H
f
k l

,(l

)
p
T
i
_
_
_
I
n

H
f
k l

,(l

)
p
T
i
_
+

lSS,l=l

H
f
k l,(l)

2
H
f
k l

,(l

)
p
T
i
2
_
_
_
,

2
H
p
T
i
2
=
2
N
_
1
N
1
_
{U
n,1
[vec(I
n
)
T
I
n
](I
n
U
n,n
) +I
n
vec(I
n
)
T
},

2
(q
T
q)
p
T
i
2
=
2
N
_
1
N
1
_
vec(I
n
)
T
,

2
f
p
T
i
2
=

2
a
p
T
i
2
+

2
B
p
T
i
2
[I
n
(I
n
c)] +
1
N
B
p
T
i
U
n,n
(I
n
I
n
)(I
n
U
1,n
) +
1
N
B
p
T
i
+
4
N
2
[c
T
I
n
+I
n
c
T
+cvec(I
n
)
T
],

2
a
p
T
i
2
=
2
N
_
p
T
i
I
n
+I
n
p
T
i
+p
i
vec(I
n
)
T
_
,

2
B
p
T
i
2
=
2
N
{U
n,1
[vec(I
n
)
T
I
n
](I
n
U
n,n
) +I
n
vec(I
n
)
T
+vec(I
n
)
T
I
n
}.
_

_
(A3)
http://journals.cambridge.org Downloaded: 20 Feb 2014 IP address: 131.155.2.68
388 A closed-form algorithm for the least-squares trilateration problem
Here U
k,l
=

k
i=1

l
j=1
E
(kl)
ij
[E
(kl)
ij
]
T
, where E
(kl)
ij
is the kl matrix with the (i,j) element as 1 and all the other
elements as zero.
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, H. R. Everett, L. Feng and D. Wehe, Mobile
robot positioning: Sensors and techniques, J. Robot. Syst.
14(4), 231249 (1997).
2. Multilateration, Wikimedia Foundation, Inc. [Online].
Available http://en.wikipedia.org/wiki/Multilateration.
3. A. El-Rabbany, Introduction to GPS: The Global Positioning
System (Artech House, Norwood, MA, 2002).
4. V. Ashkenazi, D. Park and M. Dumville, Robot positioning
and the global navigation satellite system, Ind. Robot 27(6),
419426 (2000).
5. F. Folster and H. Rohling, Data association and tracking for
automotive radar networks, IEEE Trans. Intell. Transp. Syst.
6(4), 370377 (2005).
6. A. Ward, A. Jones and A. Hopper, A new location technique
for the active ofce, IEEE Pers. Commun. 4(5), 4247 (1997).
7. P. Bahl and V. N. Padmanabhan, User Location and Tracking
in an In-building Radio Network, Microsoft Research
Technical Report MSR-TR-99-12, Redmond, WA, Microsoft
Corporation (1999).
8. J. Hightower, G. Borriello and R. Want, SpotON: 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. Priyantha, A. Chakraborty and H. Balakrishnan,
The Cricket Location-Support System, Sixth ACM/IEEE
International Conference on Mobile Computing and
Networking, Boston, MA (2000), pp. 3243.
10. A. Harter, A. Hopper, P. Steggles, A. Ward and P. Webster,
The anatomy of a context-aware application, Wirel. Netw. 1,
116 (2001).
11. F. Ahmad and M. G. Amin, Noncoherent approach to through-
the-wall radar localization, IEEE Trans. Aerosp. Electron.
Syst. 42(4), 14051419 (2006).

2
p
0
r
2
i
=

2
q
r
2
i
,

2
q
k
r
2
i
=
1
det(H

)
_

2
det
_
H
f
k
_
r
2
i
+det
_
H
h
k
_
2
q
n
r
2
i
_
, k {1, . . . , n 1},

2
q
n
r
2
i
=
1
2a
_

2
b
r
2
i

b
2
4ac
_
b
r
i
2
+b

2
b
r
2
i
2a

2
c
r
2
i
_

b
2
4ac
3
_
b
b
r
i
2a
c
r
i
_
2
_
,

2
b
r
2
i
= 2
n1

k=1
_
det
_
H
h
k
_
2
det
_
H
f
k
_
r
2
i
_
,

2
c
r
2
i
= 2
n1

k=1
_
_
det
_
H
f
k
_
r
i
2
+det
_
H
f
k
_
2
det
_
H
f
k
_
r
2
i
_
_
det
_
H

_
2
2
(q
T
q)
r
2
i
,

2
det
_
H
f
k
_
r
2
i
=

P
ss
sgn
_

_
n1

=1
_

SS,l

=l

_
_

lSS,l=l

,l=l

H
f
k l,(l)
H
f
k l

,(l

)
r
i
_
_
H
f
k l

,(l

)
r
i
+

lSS,l=l

H
f
k l,(l)

2
H
f
k l

,
_
l

_
r
2
i
_

_
,

2
f
r
2
i
=

2
a
r
2
i
+

2
B
r
2
i
c,

2
a
r
2
i
=
2
N
p
i
,

2
B
r
2
i
=
2
N
I
n
,

2
_
q
T
q
_
r
2
i
=
2
N
.
_

_
(A4)
12. Y. Zhou, W. Liu and P. Huang, Laser-Activated RFID-Based
Indoor Localization System for Mobile Robots, Proceedings
of 2007 IEEE International Conference on Robotics and
Automation, Roma, Italy (2007) pp. 46004605.
13. D. E. Manolakis, Efcient solution and performance analysis
of 3-D position estimation by trilateration, IEEE Trans.
Aerosp. Electron. Syst. 32(4), 12391248 (1996).
14. B. Fang, Trilateration and extension to global positioning
system navigation, J. Guid. 9(6), 715717 (1986).
15. J. Ziegert and C. D. Mize, The laser ball bar: a newinstrument
for machine tool metrology, Precis. Eng. 16(4), 259
267 (1994).
16. S. K. Rao, Comments on efcient solution and performance
analysis of 3-D position estimation by trilateration, IEEE
Trans. Aerosp. Electron. Syst. 34(2), 681 (1998).
17. F. Thomas and L. Ros, Revisiting trilateration for robot
localization, IEEE Trans. Robot. 21(1), 93101 (2005).
18. I. D. Coope, Reliable computation of the points of intersection
of n spheres in R
n
, Aust. N.Z. Ind. Appl. Math. J. 42(E),
C461C477 (2000).
19. W. H. Foy, Position-location solutions by Taylor-series
estimation, IEEE Trans. Aerosp. Electron. Syst. AES-12(2),
187194 (1976).
20. W. Navidi, W. S. Murphy Jr. and W. Hereman, Statistical
methods in surveying by trilateration, Comput. Stat. Data
Anal. 27, 209217 (1998).
21. W. C. Hu and W. H. Tang, Automated least-squares adjustment
of triangulation-trilateration gures, J. Surv. Eng. 133142
(2001).
22. M. Pent, M. A. Spirito and E. Turco, Method for
positioning GSM mobile stations using absolute time
delay measurements, Electron. Lett. 33(24), 20192020
(1997).
23. J. C. Spall, Introduction to Stochastic Search and Optimization:
Estimation, Simulation, and Control. Hoboken, NJ (John Wiley
& Sons, 2005).
24. S. Kirkpatrick, C. D. Gelatt and M. P. Vecchi, Optimization
by simulated annealing, Science 220(4598), 671680
(1983).
http://journals.cambridge.org Downloaded: 20 Feb 2014 IP address: 131.155.2.68
A closed-form algorithm for the least-squares trilateration problem 389
25. D. A. Coley, An Introduction to Genetic Algorithms for
Scientists and Engineers (World Scientic, Singapore, 1999).
26. Y. Zhou, An efcient Least-Squares Trilateration Algorithm
for Mobile Robot Localization, Proceedings of 2009
IEEE/RSJ International Conference on Intelligent Robots and
Systems (St. Louis, MO, 2009).
27. G. H. Golub and C. F. Van Loan, Matrix Computations, 3rd
ed. (The Johns Hopkins University Press, Baltimore, MD,
1996).
28. E. Kreyszig, Advanced Engineering Mathematics, 8th ed.
(John Wiley & Sons, New York, 1999).
29. A. Weinmann, Uncertain Models and Robust Control
(Springer-Verlag, Wien, New York, 1991).
30. J. Urena, A. Hernandez, A. Jimenez, J. M. Villadangos,
M. Mazo, J. C. Garcia, J. J. Garcia, F. J. Alvarez,
C. De Marziani, M. C. Perez, J. A. Jimenez, A. R.
Jimenez and F. Seco, Advanced sensorial system for
an acoustic LPS, Microprocess. Microsyst. 31, 393401
(2007).
31. X. Cheng, H. Shu, Q. Liang and D. H. Du, Silent positioning
in underwater acoustic sensor networks, IEEE Trans. Veh.
Technol. 57(3), 17561766 (2008).