Beruflich Dokumente
Kultur Dokumente
I. I NTRODUCTION
Localization is an essential service in many sensor network
applications, where it is desirable to accurately estimate the
position of mobile wireless nodes [1]. Localization applications range from autonomous mobile robots to vehicular
networks, and from personnel localization to industrial and
commercial asset tracking. In several cases, such as indoor or
dense urban environments, the coverage or accuracy provided
by commonly used global navigation satellite systems is not
sufficient. Therefore, in recent years, research has been focused on alternative methods, particularly for wireless indoor
localization [2].
Localization techniques based on Pulse-based UltraWideband (UWB) are of particular interest. The main feature that makes pulse-based UWB suitable for localization
is the fine time resolution, which allows for a theoretical
ranging accuracy of the order of centimeters, when used in
conjunction with time-of-arrival ranging methods [3]. Other
potential benefits of the adoption of this technique are the
resilience to multipath propagation effects and low-power
device implementation. However, the fundamental limitations
of UWB localization techniques can be overcome by a fusion
with other source of information, such as speed and absolute
orientation information that can be measured by on-board
units.
In this paper, we propose a new fusion method based on
a Pareto optimization that uses two heterogeneous sources of
information. We consider the scenario in which a mobile node
y
Slave 2
Slave 3
x3 , y 3 !
x2 , y 2 !
r2 k !
Slave 1
x1 , y1 !
r3 k !
V k!
r1 k !
" k!
Master
x ( k ), y ( k ) !
r4 k !
Slave 4
x4 , y 4 !
x
Fig. 1. Model of the localization system. The slave nodes are placed at fixed
and known positions. At time k, the unknown-position master node measures
its distance ri with respect to each slave. Speed V and orientation of the
master are measured using on-board sensors.
x
v (k + 1) = x
(k | k) + V (k)T cos((k))
yv (k + 1) = y (k | k) + V (k)T sin((k))
,
(3)
(4)
(k)
the measurement of the orientation and T is the sampling
time interval. These measurements are expressed as
V (k) = V (k) + wV (k)
(k)
= (k) + w (k)
where the noise terms wV and w are zero-mean Gaussian
random variables with known variances V2 and 2 , respectively. The estimates provided by the dead-reckoning block
have a low variance in the estimation error but are biased.
This is due to that the orientation measurement appears as the
argument of a cosinus. It follows that the estimators of Eqs. (3)
and (4) are biased and so are the estimates (1) and (2).
We can model our estimator (1) as
x
(k + 1 | k + 1) , x(k + 1) + wx (k + 1) ,
(5)
(6)
y (k + 1 | k + 1)]T ,
(7)
x
(k + 1 | k + 1) = (1 x,k )
xr (k + 1) + x,k x
v (k + 1) ,
(1)
y (k + 1 | k + 1) = (1 y,k )
yr (k + 1) + y,k yv (k + 1) ,
(2)
r (k + 1) = [
where z
xr (k + 1) yr (k + 1)]T is the position
v (k +
estimate based only on the ranging measurements, and z
1) = [
xv (k+1) yv (k+1)]T is the position estimate based on
min
x,k
s.t.
(9)
V (k) cos((k))
x,k T V (k) cos((k)) ,
(10)
s=
r (k + 1) = [
z
xr (k + 1)
yr (k + 1)] R .
s = H
zr ,
H=
2 (x1 xn )
2 (x2 xn )
..
.
2 (y1 yn )
2 (y2 yn )
..
.
2 (xn1 xn ) 2 (yn1 yn )
(12)
ri = ri + wri .
(13)
r2i
(14)
By substituting (13)
rn2 r12
..
s =
.
+a
2
rn2 rn1
rn2 r12
..
=
+a
.
2
rn2 rn1
..
+
.
2
2
wrn + 2rn wrn wrn1 2rn1 wrn1
=s + ws ,
(15)
where
wr2n + 2rn wrn wr21 2r1 wr1
..
.
ws ,
wr2n
+ 2rn wrn
wr2n1
2rn1 wrn1
(16)
with
2
rn1
+ a,
A. Ranging estimate
rn2
a=
,
..
.
2
2
2
2
xn1 xn + yn1 yn
rn2 r12
..
.
(11)
(17)
We have that
..
E {ws } =
2 .
E wrn + 2rn E {wrn }
..
n
o
E wr2n1 + 2rn1 E wrn1
2
2
w
w
r1
rn
..
(19)
=
.
.
2
2
w
wr
rn
n1
Here we used that the noises wri affecting the range measurements have zero-mean. Finally, the expression of the
covariance matrix R is obtained by substituting Eqs. (16)
and (19) in (17). Tedious computations give that the diagonal
elements Rll , l = 1, . . . , n 1 are given by
2
4
4
2
Rll = 4rn2 w
+ 2w
+ 2w
+ 4rl2 w
rn
rn
r
r
l
R =D + p11T
=D I + G11T = DM ,
(18)
i = Dii + (n 1)p 6= 0, i = 0 . . . n 1 ,
whereby the we see that the eigenvalues of R are all non-zero.
We can now turn our attention to derive the expression of the
inverse.
(20)
n1
iT
= z + E {wr } ,
whereby the lemma follows.
In the following, we derive E wr2 . We start by taking the
square of Eq. (13):
ri2 = ri2 + wr2i + 2ri wri ,
(21)
rn2 r12
2
2
r
r
n
2
1 T
T
r = H JH
z
H J
+ Bk + a , (22)
..
2
rn2 rn1
where
Bk =
. (23)
1
HT JBk .
(24)
4
2
2
2
2
2
2
=3w
w
2 + 4rn2 w
w
w
.
w
+ w
rn
rn wr
rn
rn
r
r
r
j
E {
x (k + 1 | k + 1)} =
= (1 x,k ) x(k + 1) + (1 x,k ) E {wr (k + 1)}
2
ak = E wr2 (k + 1) + E wx2 (k)
We have the following results for the first and second order
moments of the dead-reckoning estimate:
2
o
n
V (k) cos((k))
= V (k) cos((k))e 2 .
(26)
and
2
1 1
+ cos (2(k)) e2
2 2
2
V. S IMULATION R ESULTS
x,k
(x,k ) = max (1, min (, 1)) ,
(30)
3.5
x 10
with
2 (1 x,k ) v2r (k + 1) 2x,k E {wr (k + 1)}
,
2 (1 x,k ) + 2x, k 2
where
,v2r (k + 1) + v2x (k) + v2v (k) ,
and
, E {wr (k + 1)} + E {wx (k)}
2
2
+ T V (k) cos((k)) e
1 .
Note that the optimal value of x,k in (30) depends on x,k .
The best value of x,k is found by building the Pareto trade-off
curve and selecting the knee-point on this curve [11]. Thus,
(31)
(x,k )) P12 x,k
(x,k ) ,
min P2 (x,k
x,k
x,k
(x,k ). This problem is highly non-linear. Simple numerical procedures, based on a discrimination of x,k [11], can be
employed to approximate the optimal Pareto coefficient x,k .
Finally, the values of the optimal y,k and y,k yielding the
y component of the position estimate are obtained similarly to
what done so far for the x component.
3.4
k = 0
2
Mean2: P1(k(k)) [m2]
3.3
3.2
3.1
k = 0.81
2.2
2.4
2.6
2.8
3.2
3.4
4
x 10
4
2
yposition [m]
yposition [m]
0
1
0
2
3
xposition [m]
5
0
ranging error
dead reckoning error
sensor fusion error
0.3
0.1
0
10
20
30
40
50
30
40
50
Time [s]
0.5
ranging error
dead reckoning error
sensor fusion error
0.4
0.3
0.2
0.1
0
10
20
Time [s]
Empirical CDF
1
0.8
0.6
0.4
Ranging, RMSE = 0.120 m
Deadreckoning, RMSE = 0.184 m
Sensor fusion, RMSE = 0.040 m
0.2
0
2
3
xposition [m]
A. Performance Comparison
0.2
0.5
0.4
0.1
0.2
0.3
Absolute position error [m]
0.4
0.5
TABLE I
AVERAGE EXECUTION TIME PER ITERATION [ S ].
EKF
[16] with WLS
LC-KF
UKF
Pareto Opt.
1.3 104 1.7 104 1.9 104 4.2 104 4.5 104
VII. ACKNOWLEDGEMENTS
included in the state space formulation, the Kalman approaches
might be able to provide better performance.
Comparisons for two selected simulation configurations are
provided in Fig. 7 and 8, as obtained in the two scenarios
with different values of T . The proposed Pareto method
outperforms the other methods, while the KF-based methods
have similar performance with respect to each other.
0.5
RMSE [m]
0.4
0.3
0.2
0.1
0
2
10
10
Speed [m/s]
10
Fig. 7. RMSE vs speed of the different methods for Scenario A with T = 0.5 s.
0.25
RMSE [m]
0.2
0.15
0.1
0.05
0
10
10
10
2